From 02a066d2b63e5760573b54c3734dcfec5da9760b Mon Sep 17 00:00:00 2001 From: Cyberarm Date: Fri, 3 Feb 2023 09:51:11 -0600 Subject: [PATCH] Allow Rotate states created after robot has moved and rotated to work correctly by using the robot's initial facing angle. --- .../org/timecrafters/minibots/cyberarm/chiron/Robot.java | 9 ++++++++- .../cyberarm/chiron/states/autonomous/Rotate.java | 2 +- 2 files changed, 9 insertions(+), 2 deletions(-) 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; }