diff --git a/simgui-ds.json b/simgui-ds.json index 580c1475..68c7f8aa 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -96,8 +96,7 @@ ], "robotJoysticks": [ { - "guid": "78696e70757401000000000000000000", - "useGamepad": true + "guid": "Keyboard0" }, { "guid": "Keyboard1" diff --git a/simgui.json b/simgui.json index ebe09c9d..836e5566 100644 --- a/simgui.json +++ b/simgui.json @@ -1,5 +1,10 @@ { "HALProvider": { + "DIO": { + "window": { + "visible": true + } + }, "Other Devices": { "window": { "visible": false @@ -108,8 +113,5 @@ "open": true } } - }, - "NetworkTables View": { - "visible": false } } diff --git a/src/main/java/org/sciborgs1155/robot/Constants.java b/src/main/java/org/sciborgs1155/robot/Constants.java index bad8ba5c..1dff5609 100644 --- a/src/main/java/org/sciborgs1155/robot/Constants.java +++ b/src/main/java/org/sciborgs1155/robot/Constants.java @@ -143,7 +143,7 @@ public static final class Elbow { public static final double ELBOW_OFFSET = -1.248660; - public static final double ERROR_SHUTDOWN_THRESHOLD = 1; + public static final double FAILING_DEBOUNCE_TIME = 0.2; } } diff --git a/src/main/java/org/sciborgs1155/robot/RobotContainer.java b/src/main/java/org/sciborgs1155/robot/RobotContainer.java index f0236ab6..255cbe2a 100644 --- a/src/main/java/org/sciborgs1155/robot/RobotContainer.java +++ b/src/main/java/org/sciborgs1155/robot/RobotContainer.java @@ -277,8 +277,7 @@ private void configureBindings() { // MANUAL OVERRIDE FOR STOPPING // operator.leftTrigger().onTrue(placement.setStopped(true)); - new Trigger(elevator::stalling).onTrue(placement.setStopped(true)); - new Trigger(arm::elbowFailing).onTrue(placement.setStopped(true)); + arm.onElbowFailing().onTrue(placement.setStopped(true)); } /** A command to run when the robot is enabled */ diff --git a/src/main/java/org/sciborgs1155/robot/subsystems/Arm.java b/src/main/java/org/sciborgs1155/robot/subsystems/Arm.java index ef2bfdaa..f5d9aafe 100644 --- a/src/main/java/org/sciborgs1155/robot/subsystems/Arm.java +++ b/src/main/java/org/sciborgs1155/robot/subsystems/Arm.java @@ -11,6 +11,7 @@ import com.revrobotics.SparkMaxAbsoluteEncoder.Type; import edu.wpi.first.math.controller.ArmFeedforward; import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.filter.Debouncer.DebounceType; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.math.trajectory.TrapezoidProfile; @@ -19,6 +20,7 @@ import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.button.Trigger; import io.github.oblarg.oblog.Loggable; import io.github.oblarg.oblog.annotations.Log; import org.sciborgs1155.lib.Derivative; @@ -93,6 +95,8 @@ public class Arm extends SubsystemBase implements Loggable, AutoCloseable { private final Visualizer setpointVisualizer; @Log private boolean stopped = false; + // 'tis but a scratch (see: match 42) (the elbow is unplugged) + @Log private boolean butAScratch = false; @Log private boolean wristLimp = false; public Arm(Visualizer positionVisualizer, Visualizer setpointVisualizer) { @@ -149,24 +153,22 @@ public Rotation2d getAbsoluteWristPosition() { /** * If elbow is far enough from setpoint (we always use trapezoid profiling or trajectories), it is - * dangerous + * dangerous. This method returns a debounced trigger for when the elbow encoder is likely + * failing/not plugged in. */ - @Log(name = "elbow failing") - public boolean elbowFailing() { - return Math.abs(elbowFeedback.getPositionError()) > Elbow.ERROR_SHUTDOWN_THRESHOLD; + public Trigger onElbowFailing() { + return new Trigger(() -> butAScratch).debounce(Elbow.FAILING_DEBOUNCE_TIME, DebounceType.kBoth); } /** - * If wrist is not working, we cannot pass over. It's better to check if encoder is 0 because our - * wrist cannot be at 0 (hardware) + * If wrist is not working, we cannot pass over. The wrist not working is determined in {@link + * #periodic()} */ @Log public boolean allowPassOver() { return !wristLimp; } - /** If wrist is far enough from setpoint (it is dangerous, although not as much as elbow) */ - /** Sets the position setpoints for the elbow and wrist, in radians */ public Command setSetpoints(Rotation2d elbowAngle, Rotation2d wristAngle) { return runOnce( @@ -265,7 +267,14 @@ public Command setStopped(boolean stopped) { @Override public void periodic() { // SAFETY CHECKS - wristLimp = wristEncoder.getPosition() == 0; + wristLimp = + wristEncoder.getPosition() == 0 + && wristEncoder.getVelocity() == 0 + && wristSetpoint.position() != 0; + butAScratch = + elbowEncoder.getPosition() == 0 + && elbowEncoder.getVelocity() == 0 + && elbowSetpoint.position() != Elbow.ELBOW_OFFSET; double elbowFB = elbowFeedback.calculate(getElbowPosition().getRadians(), elbowSetpoint.position());