Remove mecanum forward kinematics rotation factors

wheelerrobotics · Sep 10, 2023 · dc1b43e · dc1b43e
1 parent b12b121
commit dc1b43e
Showing 1 changed file with 4 additions and 4 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
@@ -136,19 +136,19 @@ public Twist2dDual<Time> update() {
 
             Twist2dDual<Time> twist = kinematics.forward(new MecanumKinematics.WheelIncrements<>(
                     new DualNum<Time>(new double[]{
-                            (leftFrontPosVel.position - lastLeftFrontPos) + kinematics.trackWidth * headingDelta,
+                            (leftFrontPosVel.position - lastLeftFrontPos),
                             leftFrontPosVel.velocity,
                     }).times(PARAMS.inPerTick),
                     new DualNum<Time>(new double[]{
-                            (leftRearPosVel.position - lastLeftRearPos) + kinematics.trackWidth * headingDelta,
+                            (leftRearPosVel.position - lastLeftRearPos),
                             leftRearPosVel.velocity,
                     }).times(PARAMS.inPerTick),
                     new DualNum<Time>(new double[]{
-                            (rightRearPosVel.position - lastRightRearPos) - kinematics.trackWidth * headingDelta,
+                            (rightRearPosVel.position - lastRightRearPos),
                             rightRearPosVel.velocity,
                     }).times(PARAMS.inPerTick),
                     new DualNum<Time>(new double[]{
-                            (rightFrontPosVel.position - lastRightFrontPos) - kinematics.trackWidth * headingDelta,
+                            (rightFrontPosVel.position - lastRightFrontPos),
                             rightFrontPosVel.velocity,
                     }).times(PARAMS.inPerTick)
             ));