Fix CW turns

wheelerrobotics · Dec 6, 2022 · b44d5e0 · b44d5e0
1 parent 6f6a0c9
commit b44d5e0
Show file tree

Hide file tree

Showing 2 changed files with 13 additions and 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
@@ -297,12 +297,14 @@ public final class TurnAction implements Action {
 
         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 boolean loop(TelemetryPacket p) {
             }
 
             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
@@ -317,12 +317,14 @@ public final class TurnAction implements Action {
 
         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 boolean loop(TelemetryPacket p) {
             }
 
             DualNum<Time> x = profile.get(t);
+            if (reversed) {
+                x = x.unaryMinus();
+            }
 
             Twist2d robotVelRobot = updatePoseEstimateAndGetActualVel();