From dc1b43e13f1d0377030ef082f04597f13fcbd2cd Mon Sep 17 00:00:00 2001
From: Ryan Brott <rcbrott@gmail.com>
Date: Sun, 10 Sep 2023 09:12:50 -0700
Subject: [PATCH] Remove mecanum forward kinematics rotation factors

---
 .../java/org/firstinspires/ftc/teamcode/MecanumDrive.java | 8 ++++----
 1 file changed, 4 insertions(+), 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
index 8622389a5b2a..d972a915bc88 100644
--- 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)
             ));