From 47151cbebd7936c3e73cf336123f7fee492a9cec Mon Sep 17 00:00:00 2001 From: Ryan Brott Date: Sun, 5 Mar 2023 18:32:16 -0800 Subject: [PATCH] Correct velocity in ManualFeedforwardTuner --- .../ftc/teamcode/tuning/DriveView.java | 20 +++++++++++++++++++ .../tuning/ManualFeedforwardTuner.java | 4 ++-- 2 files changed, 22 insertions(+), 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 index 14ef1ef..54fdfb1 100644 --- 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 leftEncs, rightEncs, parEncs, perpEncs; public final List forwardEncs; + public final List forwardEncsWrapped; + public final IMU imu; public final VoltageSensor voltageSensor; @@ -88,24 +90,38 @@ final class DriveView { 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(); @@ -113,10 +129,14 @@ final class DriveView { 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 index 5578bc9..742bacd 100644 --- 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 class ManualFeedforwardTuner extends LinearOpMode { 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); }