Correct velocity in ManualFeedforwardTuner

wheelerrobotics · Mar 6, 2023 · 47151cb · 47151cb
1 parent 98cbe1e
commit 47151cb
Show file tree

Hide file tree

Showing 2 changed files with 22 additions and 2 deletions.
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/DriveView.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/DriveView.java
@@ -37,6 +37,8 @@ final class DriveView {
     public final List<RawEncoder> leftEncs, rightEncs, parEncs, perpEncs;
     public final List<RawEncoder> forwardEncs;
 
+    public final List<Encoder> forwardEncsWrapped;
+
     public final IMU imu;
 
     public final VoltageSensor voltageSensor;
@@ -88,35 +90,53 @@ public DriveView(HardwareMap hardwareMap) {
             throw new AssertionError();
         }
 
+        forwardEncsWrapped = new ArrayList<>();
+
         if (localizer instanceof TwoDeadWheelLocalizer) {
             TwoDeadWheelLocalizer l2 = (TwoDeadWheelLocalizer) localizer;
             parEncs = Collections.singletonList(unwrap(l2.par));
             perpEncs = Collections.singletonList(unwrap(l2.perp));
             leftEncs = Collections.emptyList();
             rightEncs = Collections.emptyList();
+
+            forwardEncsWrapped.add(l2.par);
+            forwardEncsWrapped.add(l2.perp);
         } else if (localizer instanceof ThreeDeadWheelLocalizer) {
             ThreeDeadWheelLocalizer l3 = (ThreeDeadWheelLocalizer) localizer;
             parEncs = Arrays.asList(unwrap(l3.par0), unwrap(l3.par1));
             perpEncs = Collections.singletonList(unwrap(l3.perp));
             leftEncs = Collections.emptyList();
             rightEncs = Collections.emptyList();
+
+            forwardEncsWrapped.add(l3.par0);
+            forwardEncsWrapped.add(l3.par1);
+            forwardEncsWrapped.add(l3.perp);
         } else if (localizer instanceof MecanumDrive.DriveLocalizer) {
             MecanumDrive.DriveLocalizer dl = (MecanumDrive.DriveLocalizer) localizer;
             parEncs = Collections.emptyList();
             perpEncs = Collections.emptyList();
             leftEncs = Arrays.asList(unwrap(dl.leftFront), unwrap(dl.leftRear));
             rightEncs = Arrays.asList(unwrap(dl.rightFront), unwrap(dl.rightRear));
+
+            forwardEncsWrapped.add(dl.leftFront);
+            forwardEncsWrapped.add(dl.leftRear);
+            forwardEncsWrapped.add(dl.rightFront);
+            forwardEncsWrapped.add(dl.rightRear);
         } else if (localizer instanceof TankDrive.DriveLocalizer) {
             TankDrive.DriveLocalizer dl = (TankDrive.DriveLocalizer) localizer;
             parEncs = Collections.emptyList();
             perpEncs = Collections.emptyList();
             leftEncs = new ArrayList<>();
             for (Encoder e : dl.leftEncs) {
                 leftEncs.add(unwrap(e));
+
+                forwardEncsWrapped.add(e);
             }
             rightEncs = new ArrayList<>();
             for (Encoder e : dl.rightEncs) {
                 rightEncs.add(unwrap(e));
+
+                forwardEncsWrapped.add(e);
             }
         } else {
             throw new AssertionError();

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/ManualFeedforwardTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/ManualFeedforwardTuner.java
@@ -53,8 +53,8 @@ public void runOpMode() {
                         mode = Mode.DRIVER_MODE;
                     }
 
-                    for (int i = 0; i < view.forwardEncs.size(); i++) {
-                        double v = view.forwardEncs.get(i).getPositionAndVelocity().velocity;
+                    for (int i = 0; i < view.forwardEncsWrapped.size(); i++) {
+                        double v = view.forwardEncsWrapped.get(i).getPositionAndVelocity().velocity;
                         telemetry.addData("v" + i, view.inPerTick * v);
                     }