add a simple method to check devices (mainly motors) for each mechanism

src/main/java/com/team766/robot/common/mechanisms/Drive.java

@@ -379,6 +379,14 @@ private static Translation2d getPositionForWheel(
                 relativeLocation.getX() * halfDistance, relativeLocation.getY() * halfDistance);
     }
 
+    public boolean checkDevices() {
+        // TODO: also check pigeon.
+        return (swerveFR.checkDevices()
+                && swerveFL.checkDevices()
+                && swerveBR.checkDevices()
+                && swerveBL.checkDevices());
+    }
+
     // Odometry
     @Override
     public void run() {

src/main/java/com/team766/robot/common/mechanisms/SwerveModule.java

@@ -199,6 +199,12 @@ public SwerveModuleState getModuleState() {
                         steer.getSensorPosition() / ENCODER_CONVERSION_FACTOR - offset));
     }
 
+    public boolean checkDevices() {
+        // TODO: check CANcoder too.  perhaps check magnet health?
+        return MotorUtil.checkMotor("[SWERVE] " + modulePlacement + " steer", steer)
+                && MotorUtil.checkMotor("[SWERVE] " + modulePlacement + " drive", drive);
+    }
+
     public void dashboardCurrentUsage() {
         // SmartDashboard.putNumber(
         //         "[" + modulePlacement + "]" + " steer supply current",

src/main/java/com/team766/robot/reva/DebugOI.java

@@ -8,6 +8,7 @@
 import com.team766.robot.reva.mechanisms.Intake;
 import com.team766.robot.reva.mechanisms.Shooter;
 import com.team766.robot.reva.mechanisms.Shoulder;
+import com.team766.robot.reva.procedures.CheckDevices;
 
 /**
  * Programmer-centric controls to test each of our (non-drive) mechanisms.
@@ -147,5 +148,9 @@ protected void handleOI(Context context) {
             shooter.stop();
             context.releaseOwnership(shooter);
         }
+
+        if (macropad.getButtonPressed(InputConstants.QUICKTEST)) {
+            new CheckDevices().run(context);
+        }
     }
 }

src/main/java/com/team766/robot/reva/constants/InputConstants.java

@@ -23,7 +23,7 @@ public final class InputConstants {
     public static final int NUDGE_DOWN = 12;
     public static final int MACROPAD_PRESET_1 = 13;
     public static final int MACROPAD_PRESET_2 = 14;
-    public static final int MACROPAD_PRESET_3 = 15;
+    public static final int QUICKTEST = 15;
     public static final int MACROPAD_PRESET_4 = 16;
 
     // Joystick buttons

src/main/java/com/team766/robot/reva/mechanisms/Climber.java

@@ -129,6 +129,11 @@ public boolean isRightAtBottom() {
         return Math.abs(getHeightRight()) < POSITION_LOCATION_THRESHOLD;
     }
 
+    public boolean checkDevices() {
+        return (MotorUtil.checkMotor("[CLIMBER] left", leftMotor)
+                && MotorUtil.checkMotor("[CLIMBER] right", rightMotor));
+    }
+
     @Override
     public void run() {
         // SmartDashboard.putNumber("[CLIMBER] Left Rotations", leftMotor.getSensorPosition());

src/main/java/com/team766/robot/reva/mechanisms/ForwardApriltagCamera.java

@@ -37,6 +37,12 @@ public GrayScaleCamera getCamera() {
         return camera;
     }
 
+    public boolean checkDevices() {
+        boolean check = ((camera != null) && camera.isConnected());
+        log("ForwardApriltagCamera: " + check);
+        return check;
+    }
+
     public void run() {
 
         try {

src/main/java/com/team766/robot/reva/mechanisms/Intake.java

@@ -97,6 +97,10 @@ public void nudgeDown() {
         intakeMotor.set(intakePower);
     }
 
+    public boolean checkDevices() {
+        return MotorUtil.checkMotor("[INTAKE] motor", intakeMotor);
+    }
+
     public void run() {
         // SmartDashboard.putString("[INTAKE]", state.toString());
         // SmartDashboard.putNumber("[INTAKE POWER]", intakePower);

src/main/java/com/team766/robot/reva/mechanisms/Lights.java

@@ -71,6 +71,18 @@ public boolean signalNoNoteInIntakeYet() {
         return handleErrorCode(e);
     }
 
+    // Green
+    public boolean signalDeviceCheckHealthy() {
+        ErrorCode e = m_candle.setLEDs(0, 100, 0);
+        return handleErrorCode(e);
+    }
+
+    // Red
+    public boolean signalDeviceCheckUnhealthy() {
+        ErrorCode e = m_candle.setLEDs(100, 0, 0);
+        return handleErrorCode(e);
+    }
+
     public boolean isDoingShootingProcedure() {
         ErrorCode e = m_candle.setLEDs(0, 227, 197);
         return handleErrorCode(e);

src/main/java/com/team766/robot/reva/mechanisms/MotorUtil.java

@@ -124,4 +124,34 @@ public static void enableSoftLimits(MotorController motor, boolean enable) {
         limitSwitchConfigs.ReverseSoftLimitEnable = enable;
         applyLimitSwitchConfigs(talonFX, limitSwitchConfigs);
     }
+
+    public static boolean checkMotor(String label, MotorController motor) {
+        if (motor instanceof TalonFX) {
+            TalonFX talonFX = (TalonFX) motor;
+            // TODO: check that this correct detects missing CAN bus and missing power.
+            boolean check = talonFX.isAlive();
+            Logger.get(Category.MECHANISMS).logData(Severity.INFO, "{0}: {1}", label, check);
+            // TODO: also check for faults, sticky faults?
+            return check;
+        }
+
+        if (motor instanceof CANSparkMax) {
+            CANSparkMax sparkMax = (CANSparkMax) motor;
+            // CANSparkMax doesn't have an "isAlive()" method.  Use something else that depends on
+            // being able to communicate over the CAN bus.
+            // getMotorTemperature() still returns a value - in limited empirical tests, half of
+            // what it
+            // normally returns - when the CAN bus is disconnected.
+            // getBusVoltage() returns 0 if the CAN bus is disconnected.  It still does return the
+            // expected
+            // value if the fuse for the motor is removed.
+            // TODO: see if we want to use a combination of these things.
+            boolean check = sparkMax.getBusVoltage() > 0;
+            Logger.get(Category.MECHANISMS).logData(Severity.INFO, "{0}: {1}", label, check);
+            // TODO: also check for faults?
+            return check;
+        }
+        Logger.get(Category.MECHANISMS).logData(Severity.INFO, "{0}: unknown", label);
+        return false;
+    }
 }

src/main/java/com/team766/robot/reva/mechanisms/NoteCamera.java

@@ -14,4 +14,10 @@ public NoteCamera() {
     public ColorCamera getCamera() {
         return camera;
     }
+
+    public boolean checkDevices() {
+        boolean check = ((camera != null) && camera.isConnected());
+        log("NoteCamera: " + check);
+        return check;
+    }
 }

src/main/java/com/team766/robot/reva/mechanisms/Shooter.java

@@ -86,6 +86,11 @@ public void nudgeDown() {
         shoot(Math.max(targetSpeed - NUDGE_INCREMENT, MIN_SPEED));
     }
 
+    public boolean checkDevices() {
+        return MotorUtil.checkMotor("[SHOOTER] top motor", shooterMotorTop)
+                && MotorUtil.checkMotor("[SHOOTER] bottom motor", shooterMotorBottom);
+    }
+
     public void run() {
         if (speedUpdated || rateLimiter.next()) {
             // SmartDashboard.putNumber("[SHOOTER TARGET SPEED]", shouldRun ? targetSpeed : 0.0);

src/main/java/com/team766/robot/reva/mechanisms/Shoulder.java

@@ -140,6 +140,14 @@ public boolean isFinished() {
         return Math.abs(getAngle() - targetAngle) < 2.5;
     }
 
+    public boolean checkDevices() {
+        boolean checkEncoder = absoluteEncoder.isConnected();
+        log("[SHOULDER] encoder: " + checkEncoder);
+        return checkEncoder
+                && MotorUtil.checkMotor("[SHOULDER] left motor", leftMotor)
+                && MotorUtil.checkMotor("[SHOULDER] right motor", rightMotor);
+    }
+
     @Override
     public void run() {
         // encoder takes some time to settle.

src/main/java/com/team766/robot/reva/procedures/CheckDevices.java

@@ -0,0 +1,45 @@
+package com.team766.robot.reva.procedures;
+
+import com.team766.framework.Context;
+import com.team766.framework.Procedure;
+import com.team766.robot.reva.Robot;
+
+public class CheckDevices extends Procedure {
+    @Override
+    public void run(Context context) {
+        boolean check = true;
+        context.takeOwnership(Robot.drive);
+        check &= Robot.drive.checkDevices();
+        context.releaseOwnership(Robot.drive);
+
+        context.takeOwnership(Robot.climber);
+        check &= Robot.climber.checkDevices();
+        context.releaseOwnership(Robot.climber);
+
+        context.takeOwnership(Robot.forwardApriltagCamera);
+        check &= Robot.forwardApriltagCamera.checkDevices();
+        context.releaseOwnership(Robot.forwardApriltagCamera);
+
+        context.takeOwnership(Robot.intake);
+        check &= Robot.intake.checkDevices();
+        context.releaseOwnership(Robot.intake);
+
+        context.takeOwnership(Robot.noteCamera);
+        check &= Robot.noteCamera.checkDevices();
+        context.releaseOwnership(Robot.noteCamera);
+
+        context.takeOwnership(Robot.shooter);
+        check &= Robot.shooter.checkDevices();
+        context.releaseOwnership(Robot.shooter);
+
+        context.takeOwnership(Robot.shoulder);
+        check &= Robot.shoulder.checkDevices();
+        context.releaseOwnership(Robot.shoulder);
+
+        if (check) {
+            Robot.lights.signalDeviceCheckHealthy();
+        } else {
+            Robot.lights.signalDeviceCheckUnhealthy();
+        }
+    }
+}