From b44d5e0ca5e0118127868cdab0361c792203062f Mon Sep 17 00:00:00 2001
From: Ryan Brott <rcbrott@gmail.com>
Date: Mon, 5 Dec 2022 22:00:23 -0800
Subject: [PATCH] Fix CW turns

---
 .../java/org/firstinspires/ftc/teamcode/MecanumDrive.java | 8 +++++++-
 .../java/org/firstinspires/ftc/teamcode/TankDrive.java    | 7 ++++++-
 2 files changed, 13 insertions(+), 2 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 30f26b7..4354cb3 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java
@@ -297,12 +297,14 @@ public final class MecanumDrive {
 
         private final TimeProfile profile;
         private double beginTs;
+        private final boolean reversed;
 
         private boolean active;
 
         public TurnAction(Pose2d beginPose, double angle) {
             this.beginPose = beginPose;
-            profile = new TimeProfile(Profiles.constantProfile(angle, 0, MAX_ANG_VEL, -MAX_ANG_ACCEL, MAX_ANG_ACCEL).baseProfile);
+            profile = new TimeProfile(Profiles.constantProfile(Math.abs(angle), 0, MAX_ANG_VEL, -MAX_ANG_ACCEL, MAX_ANG_ACCEL).baseProfile);
+            reversed = angle < 0;
         }
 
         @Override
@@ -327,6 +329,10 @@ public final class MecanumDrive {
             }
 
             DualNum<Time> x = profile.get(t);
+            if (reversed) {
+                x = x.unaryMinus();
+            }
+
             Pose2dDual<Time> txWorldTarget = Rotation2dDual.exp(x).times(beginPose);
 
             Twist2d robotVelRobot = updatePoseEstimateAndGetActualVel();
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TankDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TankDrive.java
index d81dbf2..57a2df1 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TankDrive.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TankDrive.java
@@ -317,12 +317,14 @@ public final class TankDrive {
 
         private final TimeProfile profile;
         private double beginTs;
+        private final boolean reversed;
 
         private boolean active;
 
         public TurnAction(Pose2d beginPose, double angle) {
             this.beginPose = beginPose;
-            profile = new TimeProfile(Profiles.constantProfile(angle, 0, MAX_ANG_VEL, -MAX_ANG_ACCEL, MAX_ANG_ACCEL).baseProfile);
+            profile = new TimeProfile(Profiles.constantProfile(Math.abs(angle), 0, MAX_ANG_VEL, -MAX_ANG_ACCEL, MAX_ANG_ACCEL).baseProfile);
+            reversed = angle < 0;
         }
 
         @Override
@@ -349,6 +351,9 @@ public final class TankDrive {
             }
 
             DualNum<Time> x = profile.get(t);
+            if (reversed) {
+                x = x.unaryMinus();
+            }
 
             Twist2d robotVelRobot = updatePoseEstimateAndGetActualVel();