Use IMU heading in mecanum drive localizer (fixes FIRST-Tech-Challeng…

…e#215)
wheelerrobotics · Mar 2, 2023 · b4af817 · b4af817
1 parent 77dbd7f
commit b4af817
Showing 1 changed file with 20 additions and 10 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
@@ -5,9 +5,7 @@
 import com.acmerobotics.dashboard.canvas.Canvas;
 import com.acmerobotics.dashboard.config.Config;
 import com.acmerobotics.dashboard.telemetry.TelemetryPacket;
-import com.acmerobotics.roadrunner.AccelConstraint;
-import com.acmerobotics.roadrunner.Action;
-import com.acmerobotics.roadrunner.Actions;
+import com.acmerobotics.roadrunner.*;
 import com.acmerobotics.roadrunner.AngularVelConstraint;
 import com.acmerobotics.roadrunner.DualNum;
 import com.acmerobotics.roadrunner.HolonomicController;
@@ -35,6 +33,7 @@
 import com.qualcomm.robotcore.hardware.IMU;
 import com.qualcomm.robotcore.hardware.VoltageSensor;
 
+import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
 import org.firstinspires.ftc.teamcode.util.LogFiles;
 import org.firstinspires.ftc.teamcode.util.Encoder;
 import org.firstinspires.ftc.teamcode.util.Localizer;
@@ -109,6 +108,7 @@ public class DriveLocalizer implements Localizer {
         public final Encoder leftFront, leftRear, rightRear, rightFront;
 
         private int lastLeftFrontPos, lastLeftRearPos, lastRightRearPos, lastRightFrontPos;
+        private Rotation2d lastHeading;
 
         public DriveLocalizer() {
             leftFront = new OverflowEncoder(new RawEncoder(MecanumDrive.this.leftFront));
@@ -120,6 +120,8 @@ public DriveLocalizer() {
             lastLeftRearPos = leftRear.getPositionAndVelocity().position;
             lastRightRearPos = rightRear.getPositionAndVelocity().position;
             lastRightFrontPos = rightFront.getPositionAndVelocity().position;
+
+            lastHeading = Rotation2d.exp(imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS));
         }
 
         @Override
@@ -129,31 +131,39 @@ public Twist2dIncrDual<Time> updateAndGetIncr() {
             Encoder.PositionVelocityPair rightRearPosVel = rightRear.getPositionAndVelocity();
             Encoder.PositionVelocityPair rightFrontPosVel = rightFront.getPositionAndVelocity();
 
-            MecanumKinematics.WheelIncrements<Time> incrs = new MecanumKinematics.WheelIncrements<>(
+            Rotation2d heading = Rotation2d.exp(imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS));
+            double headingDelta = heading.minus(lastHeading);
+
+            Twist2dIncrDual<Time> twist = kinematics.forward(new MecanumKinematics.WheelIncrements<>(
                     new DualNum<Time>(new double[]{
-                            leftFrontPosVel.position - lastLeftFrontPos,
+                            (leftFrontPosVel.position - lastLeftFrontPos) + kinematics.trackWidth * headingDelta,
                             leftFrontPosVel.velocity,
                     }).times(inPerTick),
                     new DualNum<Time>(new double[]{
-                            leftRearPosVel.position - lastLeftRearPos,
+                            (leftRearPosVel.position - lastLeftRearPos) + kinematics.trackWidth * headingDelta,
                             leftRearPosVel.velocity,
                     }).times(inPerTick),
                     new DualNum<Time>(new double[]{
-                            rightRearPosVel.position - lastRightRearPos,
+                            (rightRearPosVel.position - lastRightRearPos) - kinematics.trackWidth * headingDelta,
                             rightRearPosVel.velocity,
                     }).times(inPerTick),
                     new DualNum<Time>(new double[]{
-                            rightFrontPosVel.position - lastRightFrontPos,
+                            (rightFrontPosVel.position - lastRightFrontPos) - kinematics.trackWidth * headingDelta,
                             rightFrontPosVel.velocity,
                     }).times(inPerTick)
-            );
+            ));
 
             lastLeftFrontPos = leftFrontPosVel.position;
             lastLeftRearPos = leftRearPosVel.position;
             lastRightRearPos = rightRearPosVel.position;
             lastRightFrontPos = rightFrontPosVel.position;
 
-            return kinematics.forward(incrs);
+            lastHeading = heading;
+
+            return new Twist2dIncrDual<>(
+                    twist.transIncr,
+                    twist.rotIncr.drop(1).addFront(headingDelta)
+            );
         }
     }