diff --git a/src/main/java/com/team1816/lib/BaseRobotState.java b/src/main/java/com/team1816/lib/BaseRobotState.java index 96430a90..75e28745 100644 --- a/src/main/java/com/team1816/lib/BaseRobotState.java +++ b/src/main/java/com/team1816/lib/BaseRobotState.java @@ -8,19 +8,19 @@ public class BaseRobotState { * The pose of the robot on the field estimated from odometry and vision data, measured at the * robot's center. */ - public static Pose2d robotPose = Pose2d.kZero; + public static volatile Pose2d robotPose = Pose2d.kZero; /** * The robot-centric speeds of the robot. */ - public static ChassisSpeeds robotSpeeds = new ChassisSpeeds(0.0, 0.0, 0.0); + public static volatile ChassisSpeeds robotSpeeds = new ChassisSpeeds(0.0, 0.0, 0.0); /** * The tilt of the robot relative to the field, in radians. Measured as the angle between a * vector pointing up in the field reference frame and a vector pointing up in the robot * reference frame. */ - public static double robotTiltRadians = 0.0; + public static volatile double robotTiltRadians = 0.0; /** * If the {@link #robotPose} is believed to be an accurate estimate of the robot's pose on the @@ -29,7 +29,7 @@ public class BaseRobotState { * its pose. Other cases might be if we leave the ground while climbing, or if we had to use a * default auto without the correct starting pose. */ - public static boolean hasAccuratePoseEstimate = true; + public static volatile boolean hasAccuratePoseEstimate = true; /** * If you are trying to get the pose of the robot on the field, you probably want to use the @@ -42,5 +42,5 @@ public class BaseRobotState { * as the source of truth during simulation, while the raw odometry position is just supposed * to be the pre-vision position estimate of the real robot. */ - public static Pose2d simActualOrRawOdometryPose = Pose2d.kZero; + public static volatile Pose2d simActualOrRawOdometryPose = Pose2d.kZero; } diff --git a/src/main/java/com/team1816/lib/hardware/components/motor/TalonFXImpl.java b/src/main/java/com/team1816/lib/hardware/components/motor/TalonFXImpl.java index 6d81e8c6..1e2a213f 100644 --- a/src/main/java/com/team1816/lib/hardware/components/motor/TalonFXImpl.java +++ b/src/main/java/com/team1816/lib/hardware/components/motor/TalonFXImpl.java @@ -70,17 +70,17 @@ public void zeroMotorPosition() { @Override public StatusCode setSimRotorVelocity(double rps) { - return simState.setRotorVelocity(rps); + return (simState == null) ? StatusCode.OK : simState.setRotorVelocity(rps); } @Override public StatusCode setSimRotorPosition(double rotations) { - return simState.setRawRotorPosition(rotations); + return (simState == null) ? StatusCode.OK : simState.setRawRotorPosition(rotations); } @Override public double getSimMotorVoltage() { - return simState.getMotorVoltage(); + return (simState == null) ? 12 : simState.getMotorVoltage(); } @Override @@ -90,7 +90,7 @@ public boolean isGhost() { @Override public StatusCode setSimSupplyVoltage(double volts) { - return simState.setSupplyVoltage(volts); + return (simState == null) ? StatusCode.OK : simState.setSupplyVoltage(volts); } } diff --git a/src/main/java/com/team1816/lib/hardware/components/motor/TalonFXSImpl.java b/src/main/java/com/team1816/lib/hardware/components/motor/TalonFXSImpl.java index 49199b73..81bde516 100644 --- a/src/main/java/com/team1816/lib/hardware/components/motor/TalonFXSImpl.java +++ b/src/main/java/com/team1816/lib/hardware/components/motor/TalonFXSImpl.java @@ -73,12 +73,12 @@ public void zeroMotorPosition() { @Override public StatusCode setSimRotorVelocity(double rps) { - return simState.setRotorVelocity(rps); + return (simState == null) ? StatusCode.OK : simState.setRotorVelocity(rps); } @Override public double getSimMotorVoltage() { - return simState.getMotorVoltage(); + return (simState == null) ? 12 : simState.getMotorVoltage(); } @Override @@ -88,7 +88,7 @@ public boolean isGhost() { @Override public StatusCode setSimRotorPosition(double rotations) { - return simState.setRawRotorPosition(rotations); + return (simState == null) ? StatusCode.OK : simState.setRawRotorPosition(rotations); } @Override diff --git a/src/main/java/com/team1816/season/InTheZoneCommand.java b/src/main/java/com/team1816/season/InTheZoneCommand.java index 726e11ed..40c2c23b 100644 --- a/src/main/java/com/team1816/season/InTheZoneCommand.java +++ b/src/main/java/com/team1816/season/InTheZoneCommand.java @@ -7,11 +7,18 @@ public class InTheZoneCommand extends GreenCommand { private final LedManager.RobotLEDStateEvent robotStateEvent = pubsub.GetEvent(LedManager.RobotLEDStateEvent.class); + @Override public void initialize() { robotStateEvent.Publish(LedManager.LEDControlState.BLINK); } + @Override public void end(boolean interrupted) { robotStateEvent.Publish(LedManager.LEDControlState.SOLID); } + + @Override + public boolean isFinished() { + return true; + } } diff --git a/src/main/java/com/team1816/season/subsystems/Gatekeeper.java b/src/main/java/com/team1816/season/subsystems/Gatekeeper.java index eab937bb..20f9559d 100644 --- a/src/main/java/com/team1816/season/subsystems/Gatekeeper.java +++ b/src/main/java/com/team1816/season/subsystems/Gatekeeper.java @@ -69,14 +69,6 @@ public enum GatekeeperState { this.bottomMotorValue = bottomMotorValue; } - private void adjustTopMotorValue(double adjustValue) { - this.topMotorValue = adjustValue; - } - - private void adjustBottomMotorValue(double adjustValue) { - this.bottomMotorValue += adjustValue; - } - public double getTopMotorValue() { return topMotorValue; } diff --git a/src/main/java/com/team1816/season/subsystems/Intake.java b/src/main/java/com/team1816/season/subsystems/Intake.java index 1dfeef8c..d8d12cc3 100644 --- a/src/main/java/com/team1816/season/subsystems/Intake.java +++ b/src/main/java/com/team1816/season/subsystems/Intake.java @@ -150,10 +150,6 @@ public void setWantedState(IntakeState state) { this.wantedState = state; } - public boolean isIntaking() { return (wantedState == IntakeState.STOW); } - - public boolean isOutaking() { return (wantedState == IntakeState.INTAKE); } - /** * The position of the flipper. This is not just a number, since we are doing a custom current * based control, so the behavior is completely different for going in and out.