diff --git a/src/main/java/com/team766/robot/common/mechanisms/Drive.java b/src/main/java/com/team766/robot/common/mechanisms/Drive.java index 761e7edc8..22f68ce35 100644 --- a/src/main/java/com/team766/robot/common/mechanisms/Drive.java +++ b/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() { diff --git a/src/main/java/com/team766/robot/common/mechanisms/SwerveModule.java b/src/main/java/com/team766/robot/common/mechanisms/SwerveModule.java index a777f9d48..c2a86808e 100644 --- a/src/main/java/com/team766/robot/common/mechanisms/SwerveModule.java +++ b/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", diff --git a/src/main/java/com/team766/robot/reva/DebugOI.java b/src/main/java/com/team766/robot/reva/DebugOI.java index 7931d7dd9..7dbad2ae3 100644 --- a/src/main/java/com/team766/robot/reva/DebugOI.java +++ b/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); + } } } diff --git a/src/main/java/com/team766/robot/reva/constants/InputConstants.java b/src/main/java/com/team766/robot/reva/constants/InputConstants.java index 07c2ad75a..d9ce4cdc5 100644 --- a/src/main/java/com/team766/robot/reva/constants/InputConstants.java +++ b/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 diff --git a/src/main/java/com/team766/robot/reva/mechanisms/Climber.java b/src/main/java/com/team766/robot/reva/mechanisms/Climber.java index 998352d1a..ec234ec39 100644 --- a/src/main/java/com/team766/robot/reva/mechanisms/Climber.java +++ b/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()); diff --git a/src/main/java/com/team766/robot/reva/mechanisms/ForwardApriltagCamera.java b/src/main/java/com/team766/robot/reva/mechanisms/ForwardApriltagCamera.java index 7caa39d3f..14775bcaa 100644 --- a/src/main/java/com/team766/robot/reva/mechanisms/ForwardApriltagCamera.java +++ b/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 { diff --git a/src/main/java/com/team766/robot/reva/mechanisms/Intake.java b/src/main/java/com/team766/robot/reva/mechanisms/Intake.java index 691a6dd49..07f1b2bf3 100644 --- a/src/main/java/com/team766/robot/reva/mechanisms/Intake.java +++ b/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); diff --git a/src/main/java/com/team766/robot/reva/mechanisms/Lights.java b/src/main/java/com/team766/robot/reva/mechanisms/Lights.java index 2f348a726..3cfc50f25 100644 --- a/src/main/java/com/team766/robot/reva/mechanisms/Lights.java +++ b/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); diff --git a/src/main/java/com/team766/robot/reva/mechanisms/MotorUtil.java b/src/main/java/com/team766/robot/reva/mechanisms/MotorUtil.java index 5a3506310..da3ab21c4 100644 --- a/src/main/java/com/team766/robot/reva/mechanisms/MotorUtil.java +++ b/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; + } } diff --git a/src/main/java/com/team766/robot/reva/mechanisms/NoteCamera.java b/src/main/java/com/team766/robot/reva/mechanisms/NoteCamera.java index 25161dc8a..1292cf5d1 100644 --- a/src/main/java/com/team766/robot/reva/mechanisms/NoteCamera.java +++ b/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; + } } diff --git a/src/main/java/com/team766/robot/reva/mechanisms/Shooter.java b/src/main/java/com/team766/robot/reva/mechanisms/Shooter.java index 953bb5b16..c131832fa 100644 --- a/src/main/java/com/team766/robot/reva/mechanisms/Shooter.java +++ b/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); diff --git a/src/main/java/com/team766/robot/reva/mechanisms/Shoulder.java b/src/main/java/com/team766/robot/reva/mechanisms/Shoulder.java index 536781ffa..7d24cb1fb 100644 --- a/src/main/java/com/team766/robot/reva/mechanisms/Shoulder.java +++ b/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. diff --git a/src/main/java/com/team766/robot/reva/procedures/CheckDevices.java b/src/main/java/com/team766/robot/reva/procedures/CheckDevices.java new file mode 100644 index 000000000..d4a282e16 --- /dev/null +++ b/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(); + } + } +}