diff --git a/TeamCode/build.gradle b/TeamCode/build.gradle index afb8482..b0c4a83 100644 --- a/TeamCode/build.gradle +++ b/TeamCode/build.gradle @@ -33,6 +33,6 @@ dependencies { implementation project(':FtcRobotController') annotationProcessor files('lib/OpModeAnnotationProcessor.jar') - implementation "com.acmerobotics.roadrunner:ftc:0.1.12" + implementation "com.acmerobotics.roadrunner:ftc:0.1.13" implementation "com.acmerobotics.dashboard:dashboard:0.4.14" } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java index d3a8f1b..26576a9 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java @@ -39,9 +39,10 @@ import com.qualcomm.robotcore.hardware.HardwareMap; import com.qualcomm.robotcore.hardware.VoltageSensor; import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles; import org.firstinspires.ftc.teamcode.messages.DriveCommandMessage; import org.firstinspires.ftc.teamcode.messages.MecanumCommandMessage; -import org.firstinspires.ftc.teamcode.messages.MecanumEncodersMessage; +import org.firstinspires.ftc.teamcode.messages.MecanumLocalizerInputsMessage; import org.firstinspires.ftc.teamcode.messages.PoseMessage; import java.lang.Math; @@ -125,6 +126,7 @@ public final class MecanumDrive { private int lastLeftFrontPos, lastLeftBackPos, lastRightBackPos, lastRightFrontPos; private Rotation2d lastHeading; + private boolean initialized; public DriveLocalizer() { leftFront = new OverflowEncoder(new RawEncoder(MecanumDrive.this.leftFront)); @@ -134,13 +136,6 @@ public final class MecanumDrive { // TODO: reverse encoders if needed // leftFront.setDirection(DcMotorSimple.Direction.REVERSE); - - lastLeftFrontPos = leftFront.getPositionAndVelocity().position; - lastLeftBackPos = leftBack.getPositionAndVelocity().position; - lastRightBackPos = rightBack.getPositionAndVelocity().position; - lastRightFrontPos = rightFront.getPositionAndVelocity().position; - - lastHeading = Rotation2d.exp(imu.get().getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS)); } @Override @@ -150,12 +145,30 @@ public final class MecanumDrive { PositionVelocityPair rightBackPosVel = rightBack.getPositionAndVelocity(); PositionVelocityPair rightFrontPosVel = rightFront.getPositionAndVelocity(); - FlightRecorder.write("MECANUM_ENCODERS", new MecanumEncodersMessage( - leftFrontPosVel, leftBackPosVel, rightBackPosVel, rightFrontPosVel)); + YawPitchRollAngles angles = imu.get().getRobotYawPitchRollAngles(); + + FlightRecorder.write("MECANUM_LOCALIZER_INPUTS", new MecanumLocalizerInputsMessage( + leftFrontPosVel, leftBackPosVel, rightBackPosVel, rightFrontPosVel, angles)); + + Rotation2d heading = Rotation2d.exp(angles.getYaw(AngleUnit.RADIANS)); + + if (!initialized) { + initialized = true; + + lastLeftFrontPos = leftFrontPosVel.position; + lastLeftBackPos = leftBackPosVel.position; + lastRightBackPos = rightBackPosVel.position; + lastRightFrontPos = rightFrontPosVel.position; + + lastHeading = heading; + + return new Twist2dDual<>( + Vector2dDual.constant(new Vector2d(0.0, 0.0), 2), + DualNum.constant(0.0, 2) + ); + } - Rotation2d heading = Rotation2d.exp(imu.get().getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS)); double headingDelta = heading.minus(lastHeading); - Twist2dDual