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 5ec518c79fb7..ee3b388a1f08 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java @@ -103,20 +103,20 @@ kinematics.new WheelVelConstraint(PARAMS.maxWheelVel), private final LinkedList poseHistory = new LinkedList<>(); public class DriveLocalizer implements Localizer { - public final Encoder leftFront, leftRear, rightRear, rightFront; + public final Encoder leftFront, leftBack, rightBack, rightFront; - private int lastLeftFrontPos, lastLeftRearPos, lastRightRearPos, lastRightFrontPos; + private int lastLeftFrontPos, lastLeftBackPos, lastRightBackPos, lastRightFrontPos; private Rotation2d lastHeading; public DriveLocalizer() { leftFront = new OverflowEncoder(new RawEncoder(MecanumDrive.this.leftFront)); - leftRear = new OverflowEncoder(new RawEncoder(MecanumDrive.this.leftBack)); - rightRear = new OverflowEncoder(new RawEncoder(MecanumDrive.this.rightBack)); + leftBack = new OverflowEncoder(new RawEncoder(MecanumDrive.this.leftBack)); + rightBack = new OverflowEncoder(new RawEncoder(MecanumDrive.this.rightBack)); rightFront = new OverflowEncoder(new RawEncoder(MecanumDrive.this.rightFront)); lastLeftFrontPos = leftFront.getPositionAndVelocity().position; - lastLeftRearPos = leftRear.getPositionAndVelocity().position; - lastRightRearPos = rightRear.getPositionAndVelocity().position; + lastLeftBackPos = leftBack.getPositionAndVelocity().position; + lastRightBackPos = rightBack.getPositionAndVelocity().position; lastRightFrontPos = rightFront.getPositionAndVelocity().position; lastHeading = Rotation2d.exp(imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS)); @@ -125,8 +125,8 @@ public DriveLocalizer() { @Override public Twist2dDual