From 1ae2be3dcfb385d6a7d92a81e1d0e258404053c0 Mon Sep 17 00:00:00 2001
From: Ryan Brott <rcbrott@gmail.com>
Date: Sat, 20 May 2023 22:59:50 -0700
Subject: [PATCH] Add localization test

Also shorten some longer method names.
---
 .../ftc/teamcode/MecanumDrive.java            | 10 ++--
 .../firstinspires/ftc/teamcode/TankDrive.java | 10 ++--
 .../ftc/teamcode/ThreeDeadWheelLocalizer.java |  2 +-
 .../ftc/teamcode/TwoDeadWheelLocalizer.java   |  2 +-
 .../ftc/teamcode/tuning/LocalizationTest.java | 60 +++++++++++++++++++
 .../ftc/teamcode/tuning/TuningOpModes.java    |  3 +-
 .../ftc/teamcode/util/Localizer.java          |  2 +-
 7 files changed, 75 insertions(+), 14 deletions(-)
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/LocalizationTest.java

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 f7bb23a71c23..27e0c19f2b15 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java
@@ -126,7 +126,7 @@ public DriveLocalizer() {
         }
 
         @Override
-        public Twist2dIncrDual<Time> updateAndGetIncr() {
+        public Twist2dIncrDual<Time> update() {
             Encoder.PositionVelocityPair leftFrontPosVel = leftFront.getPositionAndVelocity();
             Encoder.PositionVelocityPair leftRearPosVel = leftRear.getPositionAndVelocity();
             Encoder.PositionVelocityPair rightRearPosVel = rightRear.getPositionAndVelocity();
@@ -255,7 +255,7 @@ public boolean run(@NonNull TelemetryPacket p) {
 
             Pose2dDual<Time> txWorldTarget = timeTrajectory.get(t);
 
-            Twist2d robotVelRobot = updatePoseEstimateAndGetActualVel();
+            Twist2d robotVelRobot = updatePoseEstimate();
 
             Twist2dDual<Time> command = new HolonomicController(
                     AXIAL_GAIN, LATERAL_GAIN, HEADING_GAIN,
@@ -336,7 +336,7 @@ public boolean run(@NonNull TelemetryPacket p) {
 
             Pose2dDual<Time> txWorldTarget = turn.get(t);
 
-            Twist2d robotVelRobot = updatePoseEstimateAndGetActualVel();
+            Twist2d robotVelRobot = updatePoseEstimate();
 
             Twist2dDual<Time> command = new HolonomicController(
                     AXIAL_GAIN, LATERAL_GAIN, HEADING_GAIN,
@@ -375,8 +375,8 @@ public void preview(Canvas c) {
         }
     }
 
-    private Twist2d updatePoseEstimateAndGetActualVel() {
-        Twist2dIncrDual<Time> incr = localizer.updateAndGetIncr();
+    public Twist2d updatePoseEstimate() {
+        Twist2dIncrDual<Time> incr = localizer.update();
         pose = pose.plus(incr.value());
 
         poseHistory.add(pose);
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TankDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TankDrive.java
index b6082ac7db21..dd89d9c9adbd 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TankDrive.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TankDrive.java
@@ -135,7 +135,7 @@ public DriveLocalizer() {
         }
 
         @Override
-        public Twist2dIncrDual<Time> updateAndGetIncr() {
+        public Twist2dIncrDual<Time> update() {
             double meanLeftPos = 0.0, meanLeftVel = 0.0;
             for (Encoder e : leftEncs) {
                 Encoder.PositionVelocityPair p = e.getPositionAndVelocity();
@@ -265,7 +265,7 @@ public boolean run(@NonNull TelemetryPacket p) {
 
             Pose2dDual<Arclength> txWorldTarget = timeTrajectory.path.get(x.value(), 3);
 
-            updatePoseEstimateAndGetActualVel();
+            updatePoseEstimate();
 
             Twist2dDual<Time> command = new RamseteController(kinematics.trackWidth, RAMSETE_ZETA, RAMSETE_BBAR)
                     .compute(x, txWorldTarget, pose);
@@ -347,7 +347,7 @@ public boolean run(@NonNull TelemetryPacket p) {
 
             Pose2dDual<Time> txWorldTarget = turn.get(t);
 
-            Twist2d robotVelRobot = updatePoseEstimateAndGetActualVel();
+            Twist2d robotVelRobot = updatePoseEstimate();
 
             Twist2dDual<Time> command = new Twist2dDual<>(
                     Vector2dDual.constant(new Vector2d(0, 0), 3),
@@ -390,8 +390,8 @@ public void preview(Canvas c) {
         }
     }
 
-    public Twist2d updatePoseEstimateAndGetActualVel() {
-        Twist2dIncrDual<Time> incr = localizer.updateAndGetIncr();
+    public Twist2d updatePoseEstimate() {
+        Twist2dIncrDual<Time> incr = localizer.update();
         pose = pose.plus(incr.value());
 
         poseHistory.add(pose);
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ThreeDeadWheelLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ThreeDeadWheelLocalizer.java
index 2b36e97340c9..407dee9a6db7 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ThreeDeadWheelLocalizer.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ThreeDeadWheelLocalizer.java
@@ -36,7 +36,7 @@ public ThreeDeadWheelLocalizer(HardwareMap hardwareMap, double inPerTick) {
         this.inPerTick = inPerTick;
     }
 
-    public Twist2dIncrDual<Time> updateAndGetIncr() {
+    public Twist2dIncrDual<Time> update() {
         Encoder.PositionVelocityPair par0PosVel = par0.getPositionAndVelocity();
         Encoder.PositionVelocityPair par1PosVel = par1.getPositionAndVelocity();
         Encoder.PositionVelocityPair perpPosVel = perp.getPositionAndVelocity();
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TwoDeadWheelLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TwoDeadWheelLocalizer.java
index 5c89659b5433..c9c9cfcfcb27 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TwoDeadWheelLocalizer.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TwoDeadWheelLocalizer.java
@@ -40,7 +40,7 @@ public TwoDeadWheelLocalizer(HardwareMap hardwareMap, IMU imu, double inPerTick)
         this.inPerTick = inPerTick;
     }
 
-    public Twist2dIncrDual<Time> updateAndGetIncr() {
+    public Twist2dIncrDual<Time> update() {
         Encoder.PositionVelocityPair parPosVel = par.getPositionAndVelocity();
         Encoder.PositionVelocityPair perpPosVel = perp.getPositionAndVelocity();
         Rotation2d heading = Rotation2d.exp(imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS));
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/LocalizationTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/LocalizationTest.java
new file mode 100644
index 000000000000..131dde83872c
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/LocalizationTest.java
@@ -0,0 +1,60 @@
+package org.firstinspires.ftc.teamcode.tuning;
+
+import com.acmerobotics.roadrunner.Pose2d;
+import com.acmerobotics.roadrunner.Twist2d;
+import com.acmerobotics.roadrunner.Vector2d;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+
+import org.firstinspires.ftc.teamcode.MecanumDrive;
+import org.firstinspires.ftc.teamcode.TankDrive;
+
+public class LocalizationTest extends LinearOpMode {
+    @Override
+    public void runOpMode() throws InterruptedException {
+        if (TuningOpModes.DRIVE_CLASS.equals(MecanumDrive.class)) {
+            MecanumDrive drive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, 0));
+
+            waitForStart();
+
+            while (opModeIsActive()) {
+                drive.setDrivePowers(new Twist2d(
+                        new Vector2d(
+                                -gamepad1.left_stick_y,
+                                -gamepad1.left_stick_x
+                        ),
+                        -gamepad1.right_stick_x
+                ));
+
+                drive.updatePoseEstimate();
+
+                telemetry.addData("x", drive.pose.trans.x);
+                telemetry.addData("y", drive.pose.trans.y);
+                telemetry.addData("heading", drive.pose.rot);
+                telemetry.update();
+            }
+        } else if (TuningOpModes.DRIVE_CLASS.equals(TankDrive.class)) {
+            TankDrive drive = new TankDrive(hardwareMap, new Pose2d(0, 0, 0));
+
+            waitForStart();
+
+            while (opModeIsActive()) {
+                drive.setDrivePowers(new Twist2d(
+                        new Vector2d(
+                                -gamepad1.left_stick_y,
+                                0.0
+                        ),
+                        -gamepad1.right_stick_x
+                ));
+
+                drive.updatePoseEstimate();
+
+                telemetry.addData("x", drive.pose.trans.x);
+                telemetry.addData("y", drive.pose.trans.y);
+                telemetry.addData("heading", drive.pose.rot);
+                telemetry.update();
+            }
+        } else {
+            throw new AssertionError();
+        }
+    }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/TuningOpModes.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/TuningOpModes.java
index 8f6858944c79..b706b864a669 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/TuningOpModes.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/TuningOpModes.java
@@ -32,7 +32,8 @@ public static void register(OpModeManager manager) {
                 ManualFeedbackTuner.class,
                 ManualFeedforwardTuner.class,
                 SplineTest.class,
-                MecanumMotorDirectionDebugger.class
+                MecanumMotorDirectionDebugger.class,
+                LocalizationTest.class
         );
 
         for (Class<? extends OpMode> o : opModes) {
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Localizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Localizer.java
index adfebba7414f..69a9750e0051 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Localizer.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Localizer.java
@@ -4,5 +4,5 @@
 import com.acmerobotics.roadrunner.Twist2dIncrDual;
 
 public interface Localizer {
-    Twist2dIncrDual<Time> updateAndGetIncr();
+    Twist2dIncrDual<Time> update();
 }