From d4ed79614e9e9d448e04467317f96fa336e62ef1 Mon Sep 17 00:00:00 2001 From: Debajit Ghosh Date: Mon, 25 Mar 2024 19:25:09 -0700 Subject: [PATCH 1/5] 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. --- .../robot/common/mechanisms/Drive.java | 8 ++++ .../robot/common/mechanisms/SwerveModule.java | 6 +++ .../robot/reva/mechanisms/Climber.java | 5 +++ .../mechanisms/ForwardApriltagCamera.java | 6 +++ .../team766/robot/reva/mechanisms/Intake.java | 4 ++ .../team766/robot/reva/mechanisms/Lights.java | 12 +++++ .../robot/reva/mechanisms/MotorUtil.java | 22 +++++++++ .../robot/reva/mechanisms/NoteCamera.java | 6 +++ .../robot/reva/mechanisms/Shooter.java | 5 +++ .../robot/reva/mechanisms/Shoulder.java | 8 ++++ .../robot/reva/procedures/CheckDevices.java | 45 +++++++++++++++++++ 11 files changed, 127 insertions(+) create mode 100644 src/main/java/com/team766/robot/reva/procedures/CheckDevices.java 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/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 89fc356ff..a8165a716 100644 --- a/src/main/java/com/team766/robot/reva/mechanisms/ForwardApriltagCamera.java +++ b/src/main/java/com/team766/robot/reva/mechanisms/ForwardApriltagCamera.java @@ -36,6 +36,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 = DriverStation.getAlliance(); 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 4868d1a5a..3f72cd310 100644 --- a/src/main/java/com/team766/robot/reva/mechanisms/Intake.java +++ b/src/main/java/com/team766/robot/reva/mechanisms/Intake.java @@ -96,6 +96,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..10784b178 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,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; + } } 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 dc624f5e9..3fcbe2e61 100644 --- a/src/main/java/com/team766/robot/reva/mechanisms/Shooter.java +++ b/src/main/java/com/team766/robot/reva/mechanisms/Shooter.java @@ -81,6 +81,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 e3be71083..d4d4c9955 100644 --- a/src/main/java/com/team766/robot/reva/mechanisms/Shoulder.java +++ b/src/main/java/com/team766/robot/reva/mechanisms/Shoulder.java @@ -139,6 +139,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. 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(); + } + } +} From 49be0965bf6bc4e7b0b9106300786a932ab9d9ec Mon Sep 17 00:00:00 2001 From: Debajit Ghosh Date: Mon, 25 Mar 2024 19:53:36 -0700 Subject: [PATCH 2/5] flip --- src/main/java/com/team766/robot/reva/mechanisms/MotorUtil.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 10784b178..5dc57c81b 100644 --- a/src/main/java/com/team766/robot/reva/mechanisms/MotorUtil.java +++ b/src/main/java/com/team766/robot/reva/mechanisms/MotorUtil.java @@ -138,7 +138,7 @@ public static boolean checkMotor(String label, MotorController motor) { 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; + boolean check = sparkMax.getMotorTemperature() > 0; Logger.get(Category.MECHANISMS).logData(Severity.INFO, "{0}: {1}", label, check); // TODO: also check for faults? return check; From 02164ecf730769ededb24795c0314d6a8c8b0a34 Mon Sep 17 00:00:00 2001 From: Debajit Ghosh Date: Fri, 29 Mar 2024 08:24:40 -0700 Subject: [PATCH 3/5] update motor check for SparkMax --- .../com/team766/robot/reva/mechanisms/MotorUtil.java | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) 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 5dc57c81b..da3ab21c4 100644 --- a/src/main/java/com/team766/robot/reva/mechanisms/MotorUtil.java +++ b/src/main/java/com/team766/robot/reva/mechanisms/MotorUtil.java @@ -128,6 +128,7 @@ public static void enableSoftLimits(MotorController motor, boolean enable) { 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? @@ -138,7 +139,14 @@ public static boolean checkMotor(String label, MotorController motor) { 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; + // 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; From 956510e42dbce5e23a3972efe3aa79be09ff73ad Mon Sep 17 00:00:00 2001 From: Debajit Ghosh Date: Fri, 29 Mar 2024 19:02:51 -0700 Subject: [PATCH 4/5] add debugoi control for running CheckDevices --- src/main/java/com/team766/robot/reva/DebugOI.java | 5 +++++ .../com/team766/robot/reva/constants/InputConstants.java | 2 +- 2 files changed, 6 insertions(+), 1 deletion(-) 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 From ec16a721e81b067fed14d496a72621d198f12f1b Mon Sep 17 00:00:00 2001 From: Debajit Ghosh Date: Thu, 4 Apr 2024 23:14:09 -0700 Subject: [PATCH 5/5] Merge remote-tracking branch 'origin/main' into device-quicktest