Apply heading velocity correction in localizer

wheelerrobotics · Sep 24, 2023 · 7cce1c4 · 7cce1c4
1 parent 629c103
commit 7cce1c4
Showing 1 changed file with 13 additions and 1 deletion.
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TwoDeadWheelLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TwoDeadWheelLocalizer.java
@@ -34,6 +34,8 @@ public static class Params {
 
     private final double inPerTick;
 
+    private double lastRawHeadingVel, headingVelOffset;
+
     public TwoDeadWheelLocalizer(HardwareMap hardwareMap, IMU imu, double inPerTick) {
         par = new OverflowEncoder(new RawEncoder(hardwareMap.get(DcMotorEx.class, "par")));
         perp = new OverflowEncoder(new RawEncoder(hardwareMap.get(DcMotorEx.class, "perp")));
@@ -48,6 +50,16 @@ public TwoDeadWheelLocalizer(HardwareMap hardwareMap, IMU imu, double inPerTick)
         FlightRecorder.write("TWO_DEAD_WHEEL_PARAMS", PARAMS);
     }
 
+    // see https://github.com/FIRST-Tech-Challenge/FtcRobotController/issues/617
+    private double getHeadingVelocity() {
+        double rawHeadingVel = imu.getRobotAngularVelocity(AngleUnit.RADIANS).zRotationRate;
+        if (Math.abs(rawHeadingVel - lastRawHeadingVel) > Math.PI) {
+            headingVelOffset -= Math.signum(rawHeadingVel) * 2 * Math.PI;
+        }
+        lastRawHeadingVel = rawHeadingVel;
+        return headingVelOffset + rawHeadingVel;
+    }
+
     public Twist2dDual<Time> update() {
         PositionVelocityPair parPosVel = par.getPositionAndVelocity();
         PositionVelocityPair perpPosVel = perp.getPositionAndVelocity();
@@ -57,7 +69,7 @@ public Twist2dDual<Time> update() {
         int perpPosDelta = perpPosVel.position - lastPerpPos;
         double headingDelta = heading.minus(lastHeading);
 
-        double headingVel = imu.getRobotAngularVelocity(AngleUnit.RADIANS).zRotationRate;
+        double headingVel = getHeadingVelocity();
 
         Twist2dDual<Time> twist = new Twist2dDual<>(
                 new Vector2dDual<>(