add a simple checkDevices() method to each mechanism for RevA. add a …

…CheckDevices procedure that checks each mechanism and signals whether or not the checks passed.

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

@@ -380,6 +380,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

@@ -197,6 +197,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/mechanisms/Climber.java

@@ -130,6 +130,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

@@ -30,6 +30,12 @@ public GrayScaleCamera getCamera() {
         return camera;
     }
 
+    public boolean checkDevices() {
+        boolean check = ((camera != null) && camera.isConnected());
+        log("ForwardApriltagCamera: " + check);
+        return check;
+    }
+
     public void run() {
         if (tagId == -1) {
             Optional<Alliance> alliance = DriverStation.getAlliance();

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

@@ -59,6 +59,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,26 @@ 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;
+            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.
+            boolean check = sparkMax.getMotorTemperature() <= 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

@@ -80,6 +80,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) < 1;
     }
 
+    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();
+        }
+    }
+}