Add localization test

Also shorten some longer method names.
wheelerrobotics · May 21, 2023 · 1ae2be3 · 1ae2be3
1 parent 491bb42
commit 1ae2be3
Show file tree

Hide file tree

Showing 7 changed files with 75 additions and 14 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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -4,5 +4,5 @@
 import com.acmerobotics.roadrunner.Twist2dIncrDual;
 
 public interface Localizer {
-    Twist2dIncrDual<Time> updateAndGetIncr();
+    Twist2dIncrDual<Time> update();
 }