diff --git a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/Robot.java b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/Robot.java index ecb6aea..df52009 100644 --- a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/Robot.java +++ b/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); diff --git a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/autonomous/Rotate.java b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/autonomous/Rotate.java index 4bfd70a..db11705 100644 --- a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/autonomous/Rotate.java +++ b/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; }