Move drive encoder reversal before first get

Changing the direction of an encoder after reading it the first time will create a potentially massive delta in the first odometry update.
OpticYeetYT · Dec 9, 2023 · 02d8ea7 · 02d8ea7
1 parent 01453c2
commit 02d8ea7
Show file tree

Hide file tree

Showing 2 changed files with 13 additions and 7 deletions.
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java
@@ -133,15 +133,15 @@ public DriveLocalizer() {
             rightBack = new OverflowEncoder(new RawEncoder(MecanumDrive.this.rightBack));
             rightFront = new OverflowEncoder(new RawEncoder(MecanumDrive.this.rightFront));
 
+            // 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.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS));
-
-            // TODO: reverse encoders if needed
-            // leftFront.setDirection(DcMotorSimple.Direction.REVERSE);
         }
 
         @Override

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TankDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TankDrive.java
@@ -136,9 +136,7 @@ public DriveLocalizer() {
                 for (DcMotorEx m : leftMotors) {
                     Encoder e = new OverflowEncoder(new RawEncoder(m));
                     leftEncs.add(e);
-                    lastLeftPos += e.getPositionAndVelocity().position;
                 }
-                lastLeftPos /= leftEncs.size();
                 this.leftEncs = Collections.unmodifiableList(leftEncs);
             }
 
@@ -147,14 +145,22 @@ public DriveLocalizer() {
                 for (DcMotorEx m : rightMotors) {
                     Encoder e = new OverflowEncoder(new RawEncoder(m));
                     rightEncs.add(e);
-                    lastRightPos += e.getPositionAndVelocity().position;
                 }
-                lastRightPos /= rightEncs.size();
                 this.rightEncs = Collections.unmodifiableList(rightEncs);
             }
 
             // TODO: reverse encoder directions if needed
             // leftEncs.get(0).setDirection(DcMotorSimple.Direction.REVERSE);
+
+            for (Encoder e : leftEncs) {
+                lastLeftPos += e.getPositionAndVelocity().position;
+            }
+            lastLeftPos /= leftEncs.size();
+
+            for (Encoder e : rightEncs) {
+                lastRightPos += e.getPositionAndVelocity().position;
+            }
+            lastRightPos /= rightEncs.size();
         }
 
         @Override