fixed elbow encoder failsafe

simgui-ds.json

@@ -96,8 +96,7 @@
   ],
   "robotJoysticks": [
     {
-      "guid": "78696e70757401000000000000000000",
-      "useGamepad": true
+      "guid": "Keyboard0"
     },
     {
       "guid": "Keyboard1"

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
   }
 }

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;
     }
   }
 

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 */

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());