Allow Rotate states created after robot has moved and rotated to work correctly by using the robot's initial facing angle.

TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/Robot.java

@@ -43,7 +43,7 @@ public class Robot {
     public final ColorSensor indicatorA, indicatorB;
     public LynxModule expansionHub;
 
-    public final double imuAngleOffset;
+    public final double imuAngleOffset, initialFacing;
     public boolean wristManuallyControlled = false, armManuallyControlled = false;
     public boolean automaticAntiTipActive = false;
     public boolean hardwareFault = false;
@@ -191,6 +191,9 @@ public class Robot {
 
         imu.initialize(parameters);
 
+        // Preserve our "initial" facing, since we transform it from zero.
+        initialFacing = facing();
+
         // BulkRead from Hubs
         for (LynxModule hub : engine.hardwareMap.getAll(LynxModule.class)) {
             hub.setBulkCachingMode(LynxModule.BulkCachingMode.MANUAL);
@@ -725,6 +728,10 @@ public class Robot {
         arm.setPower(tuningConfig("arm_automatic_power").value());
     }
 
+    public double initialFacing() {
+        return initialFacing;
+    }
+
     public double facing() {
         double imuDegrees = -imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES);
 

TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/autonomous/Rotate.java

@@ -32,7 +32,7 @@ public class Rotate extends CyberarmState {
 
         stateDisabled = !robot.getConfiguration().action(groupName, actionName).enabled;
 
-        facing = (robot.facing() + targetFacing + 360.0) % 360.0;
+        facing = (robot.initialFacing() + targetFacing + 360.0) % 360.0;
 
         velocity = targetVelocity;
     }