From 10ce6ee5c8d95e43c3e91d6fcaedb38b6f75cc94 Mon Sep 17 00:00:00 2001 From: Cyberarm Date: Fri, 22 Sep 2023 13:22:50 -0500 Subject: [PATCH] WIP: RevHubTestSuite: Added current (amp) and ticks/s test for motors, refactored to use an enum for test stages, added a state to control which hub is tested, added bulk sensor reading support. --- .../RevHubTestSuiteEngine.java | 9 ++ .../RevHubTestSuiteRobot.java | 32 ++++ .../RevHubTestSuiteHubSelectionState.java | 42 ++++++ .../RevHubTestSuiteMotorTestsState.java | 142 +++++++++++++++--- .../RevHubTestSuiteServoTestsState.java | 9 +- .../states/RevTestSuiteTestState.java | 25 ++- 6 files changed, 231 insertions(+), 28 deletions(-) create mode 100644 TeamCode/src/main/java/org/timecrafters/diagnostics/rev_hub_test_suite/states/RevHubTestSuiteHubSelectionState.java diff --git a/TeamCode/src/main/java/org/timecrafters/diagnostics/rev_hub_test_suite/RevHubTestSuiteEngine.java b/TeamCode/src/main/java/org/timecrafters/diagnostics/rev_hub_test_suite/RevHubTestSuiteEngine.java index f21140f..ee512a5 100644 --- a/TeamCode/src/main/java/org/timecrafters/diagnostics/rev_hub_test_suite/RevHubTestSuiteEngine.java +++ b/TeamCode/src/main/java/org/timecrafters/diagnostics/rev_hub_test_suite/RevHubTestSuiteEngine.java @@ -4,6 +4,7 @@ import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import org.timecrafters.diagnostics.rev_hub_test_suite.states.RevHubTestSuiteAnalogTestsState; import org.timecrafters.diagnostics.rev_hub_test_suite.states.RevHubTestSuiteDigitalTestsState; +import org.timecrafters.diagnostics.rev_hub_test_suite.states.RevHubTestSuiteHubSelectionState; import org.timecrafters.diagnostics.rev_hub_test_suite.states.RevHubTestSuiteI2CTestsState; import org.timecrafters.diagnostics.rev_hub_test_suite.states.RevHubTestSuiteMotorTestsState; import org.timecrafters.diagnostics.rev_hub_test_suite.states.RevHubTestSuiteServoTestsState; @@ -20,10 +21,18 @@ public class RevHubTestSuiteEngine extends CyberarmEngine { public void setup() { this.robot = new RevHubTestSuiteRobot(); + addState(new RevHubTestSuiteHubSelectionState(robot)); addState(new RevHubTestSuiteMotorTestsState(robot)); addState(new RevHubTestSuiteServoTestsState(robot)); addState(new RevHubTestSuiteAnalogTestsState(robot)); addState(new RevHubTestSuiteDigitalTestsState(robot)); addState(new RevHubTestSuiteI2CTestsState(robot)); } + + @Override + public void loop() { + robot.clearStaleData(); + + super.loop(); + } } diff --git a/TeamCode/src/main/java/org/timecrafters/diagnostics/rev_hub_test_suite/RevHubTestSuiteRobot.java b/TeamCode/src/main/java/org/timecrafters/diagnostics/rev_hub_test_suite/RevHubTestSuiteRobot.java index 19b57b4..45ef883 100644 --- a/TeamCode/src/main/java/org/timecrafters/diagnostics/rev_hub_test_suite/RevHubTestSuiteRobot.java +++ b/TeamCode/src/main/java/org/timecrafters/diagnostics/rev_hub_test_suite/RevHubTestSuiteRobot.java @@ -3,8 +3,10 @@ package org.timecrafters.diagnostics.rev_hub_test_suite; import com.arcrobotics.ftclib.hardware.ServoEx; import com.arcrobotics.ftclib.hardware.SimpleServo; import com.arcrobotics.ftclib.hardware.motors.MotorEx; +import com.qualcomm.hardware.lynx.LynxModule; import com.qualcomm.hardware.rev.Rev2mDistanceSensor; import com.qualcomm.robotcore.hardware.AnalogInput; +import com.qualcomm.robotcore.hardware.DcMotorSimple; import com.qualcomm.robotcore.hardware.DigitalChannel; import com.qualcomm.robotcore.hardware.HardwareMap; @@ -25,10 +27,14 @@ public class RevHubTestSuiteRobot extends Robot { public ArrayList controlHubAnalogSensors, expansionHubAnalogSensors; public ArrayList controlHubDigitalSensors, expansionHubDigitalSensors; public ArrayList controlHubI2cSensors, expansionHubI2cSensors; + + public ArrayList lynxModules; @Override public void setup() { this.hardwareMap = CyberarmEngine.instance.hardwareMap; + this.lynxModules = new ArrayList<>(hardwareMap.getAll(LynxModule.class)); + /* ------------------------------------------------ Control Hub Devices ------------------------------------------------ */ // MOTORS controlHubMotors.add(new MotorEx(hardwareMap, "c_motor_0")); @@ -102,5 +108,31 @@ public class RevHubTestSuiteRobot extends Robot { expansionHubI2cSensors.add(hardwareMap.get(Rev2mDistanceSensor.class, "x_i2c_1")); expansionHubI2cSensors.add(hardwareMap.get(Rev2mDistanceSensor.class, "x_i2c_2")); expansionHubI2cSensors.add(hardwareMap.get(Rev2mDistanceSensor.class, "x_i2c_3")); + + /* ------------------------------------------------ Hub Sensor Reading Optimization ------------------------------------------------ */ + for (LynxModule hub : lynxModules) { + hub.setBulkCachingMode(LynxModule.BulkCachingMode.MANUAL); + } + + /* ------------------------------------------------ Motor Configuration ------------------------------------------------ */ + configureMotors(); + } + + private void configureMotors() { + for(MotorEx motor : controlHubMotors) { + motor.motorEx.setDirection(DcMotorSimple.Direction.FORWARD); + motor.stopAndResetEncoder(); + } + + for(MotorEx motor : expansionHubMotors) { + motor.motorEx.setDirection(DcMotorSimple.Direction.FORWARD); + motor.stopAndResetEncoder(); + } + } + + protected void clearStaleData() { + for (LynxModule hub : lynxModules) { + hub.clearBulkCache(); + } } } diff --git a/TeamCode/src/main/java/org/timecrafters/diagnostics/rev_hub_test_suite/states/RevHubTestSuiteHubSelectionState.java b/TeamCode/src/main/java/org/timecrafters/diagnostics/rev_hub_test_suite/states/RevHubTestSuiteHubSelectionState.java new file mode 100644 index 0000000..49265a0 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/diagnostics/rev_hub_test_suite/states/RevHubTestSuiteHubSelectionState.java @@ -0,0 +1,42 @@ +package org.timecrafters.diagnostics.rev_hub_test_suite.states; + +import com.qualcomm.robotcore.hardware.Gamepad; + +import org.timecrafters.diagnostics.rev_hub_test_suite.RevHubTestSuiteRobot; + +import dev.cyberarm.engine.V2.CyberarmState; + +public class RevHubTestSuiteHubSelectionState extends CyberarmState { + private RevHubTestSuiteRobot robot; + public RevHubTestSuiteHubSelectionState(RevHubTestSuiteRobot robot) { + this.robot = robot; + } + + @Override + public void exec() { + } + + @Override + public void telemetry() { + engine.telemetry.addLine("SELECT HUB TO BE TESTED:"); + engine.telemetry.addLine(); + engine.telemetry.addLine("PRESS 'LEFT BUMPER' TO TEST CONTROL HUB"); + engine.telemetry.addLine("PRESS 'RIGHT BUMPER' TO TEST EXPANSION HUB"); + engine.telemetry.addLine(); + + super.telemetry(); + } + + @Override + public void buttonUp(Gamepad gamepad, String button) { + if (button.equals("left_bumper")) { + robot.testingControlHub = true; + setHasFinished(true); + } + + if (button.equals("right_bumper")) { + robot.testingControlHub = false; + setHasFinished(true); + } + } +} diff --git a/TeamCode/src/main/java/org/timecrafters/diagnostics/rev_hub_test_suite/states/RevHubTestSuiteMotorTestsState.java b/TeamCode/src/main/java/org/timecrafters/diagnostics/rev_hub_test_suite/states/RevHubTestSuiteMotorTestsState.java index 363ac09..0d2b3ea 100644 --- a/TeamCode/src/main/java/org/timecrafters/diagnostics/rev_hub_test_suite/states/RevHubTestSuiteMotorTestsState.java +++ b/TeamCode/src/main/java/org/timecrafters/diagnostics/rev_hub_test_suite/states/RevHubTestSuiteMotorTestsState.java @@ -4,21 +4,22 @@ import com.arcrobotics.ftclib.hardware.motors.Motor; import com.arcrobotics.ftclib.hardware.motors.MotorEx; import com.qualcomm.robotcore.hardware.Gamepad; +import org.firstinspires.ftc.robotcore.external.navigation.CurrentUnit; import org.timecrafters.diagnostics.rev_hub_test_suite.RevHubTestSuiteRobot; import java.util.ArrayList; +import java.util.Locale; public class RevHubTestSuiteMotorTestsState extends RevTestSuiteTestState { - private final int STAGE_ENCODER_STEADY = 0; // Encoder value doesn't change by more than 2 while motor is off. - private final int STAGE_ENCODER_CHANGES = 1; // Encoder value is changes when motor is running - private final int STAGE_MOTOR_BRAKING = 2; // Motor brake mode works (manual) - private int motor_index = 0; private final ArrayList motors; - private double lastMonitorTime; - private double automaticInterval = 500.0; // milliseconds + private double lastMonitorTime, lastSampleMonitorTime; + private final double automaticInterval = 500.0; // milliseconds + private final double automaticSampleInterval = 3_000.0; // milliseconds - test motor current and ticks per second + private final ArrayList sampleAmpsList = new ArrayList<>(); + private final ArrayList sampleTicksList = new ArrayList<>(); private int initialValue, lastValue; private boolean setInitialValue = true; public RevHubTestSuiteMotorTestsState(RevHubTestSuiteRobot robot) { @@ -33,25 +34,34 @@ public class RevHubTestSuiteMotorTestsState extends RevTestSuiteTestState { super.exec(); switch (stage) { - case STAGE_ENCODER_STEADY: { + case MOTOR_ENCODER_STEADY: { test_encoder_steady(); } - case STAGE_ENCODER_CHANGES: { - test_encoder_changes(); + case MOTOR_ENCODER_FORWARD: { + test_encoder_changes(true); } - case STAGE_MOTOR_BRAKING: { + case MOTOR_ENCODER_REVERSE: { + test_encoder_changes(false); + } + case MOTOR_50_PERCENT_SPEED: { + test_motor_current_and_ticks_per_second(0.5); + } + case MOTOR_100_PERCENT_SPEED: { + test_motor_current_and_ticks_per_second(1.0); + } + case MOTOR_BRAKING_MODE: { test_motor_braking(); } } - if (stage > STAGE_MOTOR_BRAKING) { + if (stage.ordinal() > STAGE.MOTOR_BRAKING_MODE.ordinal()) { testComplete = true; } } protected void test_encoder_steady() { if (motor_index >= motors.size()) { - stage += 1; + nextStage(); motor_index = 0; setInitialValue = true; return; @@ -70,16 +80,17 @@ public class RevHubTestSuiteMotorTestsState extends RevTestSuiteTestState { if (lastValue <= 2) { report("PASSED: Motor Encoder " + motor_index + " STEADY"); - this.motor_index++; + motor_index++; + setInitialValue = true; } else { report("FAILED: Motor Encoder " + motor_index + " STEADY (" + lastValue + " > 2)"); } } } - protected void test_encoder_changes() { + protected void test_encoder_changes(boolean forward) { if (motor_index >= motors.size()) { - stage += 1; + nextStage(); motor_index = 0; setInitialValue = true; return; @@ -92,25 +103,101 @@ public class RevHubTestSuiteMotorTestsState extends RevTestSuiteTestState { initialValue = motor.getCurrentPosition(); lastMonitorTime = runTime(); - motor.setVelocity(0.25); + motor.setVelocity(forward ? 0.5 : -0.5); } if (runTime() - lastMonitorTime >= automaticInterval) { - lastValue = Math.abs(initialValue - motor.getCurrentPosition()); + lastValue = initialValue - motor.getCurrentPosition(); - if (lastValue >= 100) { + String pass = forward ? "CHANGED; FORWARD." : "CHANGED; REVERSE."; + String fail = forward ? "NO CHANGE; FORWARD." : "NO CHANGE; REVERSE."; + + if (Math.abs(lastValue) >= 100) { motor.stopMotor(); report("PASSED: Motor Encoder `" + motor_index + "` CHANGE"); - this.motor_index++; + motor_index++; + setInitialValue = true; } else { report("FAILED: Motor Encoder `" + motor_index + "` CHANGE (" + lastValue + " < 100)"); } } } + protected void test_motor_current_and_ticks_per_second(double speed) { + if (motor_index >= motors.size()) { + nextStage(); + motor_index = 0; + setInitialValue = true; + return; + } + + MotorEx motor = motors.get(motor_index); + + if (setInitialValue) { + setInitialValue = false; + + // Reset samples + sampleAmpsList.clear(); + sampleTicksList.clear(); + + initialValue = 0; + lastMonitorTime = runTime(); + lastSampleMonitorTime = runTime(); + + // Start "warming up" motor + motor.motorEx.setPower(speed); + } + + + if (runTime() - lastMonitorTime >= automaticSampleInterval) { + if (initialValue == 0) { + initialValue = 1; + lastMonitorTime = runTime(); + + // FTCLib function, motor doesn't need to be "stopped." + motor.resetEncoder(); + } + } + + if (initialValue == 1) { + // Collect stats + if (runTime() - lastSampleMonitorTime >= 100) { + lastSampleMonitorTime = runTime(); + + motor.motorEx.getCurrent(CurrentUnit.AMPS); + motor.motorEx.getCurrentPosition(); + } + + /* --- Generate report --- */ + if (runTime() - lastMonitorTime >= automaticSampleInterval) { + double average_amps = 0.0; + double average_ticks = 0.0; // NOTE: needs to be converted into PER SECOND "space" + double total_amps = 0.0, total_ticks = 0.0; + + for (double amps : sampleAmpsList) { + total_amps += amps; + } + + for (double ticks : sampleTicksList) { + total_ticks += ticks; + } + + average_amps = total_amps / sampleAmpsList.size(); + average_ticks = total_ticks / sampleTicksList.size(); + average_ticks = average_ticks * (1.0 / (automaticSampleInterval * 0.001)); // Convert to PER SECOND + + report(String.format(Locale.US, "RESULT: Motor %d: Sample Period: %.2fms, Average AMPS: %.4f, Average TICKS/s: %d", motor_index, average_amps, average_ticks)); + + motor_index++; + motor.motorEx.setPower(0.0); + setInitialValue = true; + } + } + } + protected void test_motor_braking() { if (motor_index >= motors.size()) { - stage += 1; + nextStage(); motor_index = 0; setInitialValue = true; return; @@ -123,7 +210,7 @@ public class RevHubTestSuiteMotorTestsState extends RevTestSuiteTestState { @Override public void buttonUp(Gamepad gamepad, String button) { - if (stage != STAGE_MOTOR_BRAKING) { + if (stage != STAGE.MOTOR_BRAKING_MODE) { return; } @@ -137,6 +224,7 @@ public class RevHubTestSuiteMotorTestsState extends RevTestSuiteTestState { } catch (IndexOutOfBoundsException e) { /* no op */ } motor_index++; + setInitialValue = true; } else if (button.equals("y")) { report("FAILED: Motor " + motor_index + " does NOT BRAKE"); @@ -146,13 +234,23 @@ public class RevHubTestSuiteMotorTestsState extends RevTestSuiteTestState { } catch (IndexOutOfBoundsException e) { /* no op */ } motor_index++; + setInitialValue = true; } } } @Override public void telemetry() { - if (stage == STAGE_MOTOR_BRAKING) { + engine.telemetry.addLine("MOTOR CONTROLLER TESTING"); + engine.telemetry.addLine(); + if (motor_index < 4) { + MotorEx motor = motors.get(motor_index); + + engine.telemetry.addData("Motor", "Port: %d: Power: %.2f, Velocity: %.2f, Ticks: %d", motor_index, motor.motorEx.getPower(), motor.getVelocity(), motor.getCurrentPosition()); + engine.telemetry.addLine(); + } + + if (stage == STAGE.MOTOR_BRAKING_MODE) { engine.telemetry.addLine("MANUAL TEST"); engine.telemetry.addLine("PRESS `A` if Motor " + motor_index + " is BRAKING."); engine.telemetry.addLine(); diff --git a/TeamCode/src/main/java/org/timecrafters/diagnostics/rev_hub_test_suite/states/RevHubTestSuiteServoTestsState.java b/TeamCode/src/main/java/org/timecrafters/diagnostics/rev_hub_test_suite/states/RevHubTestSuiteServoTestsState.java index b692129..93c064e 100644 --- a/TeamCode/src/main/java/org/timecrafters/diagnostics/rev_hub_test_suite/states/RevHubTestSuiteServoTestsState.java +++ b/TeamCode/src/main/java/org/timecrafters/diagnostics/rev_hub_test_suite/states/RevHubTestSuiteServoTestsState.java @@ -8,7 +8,6 @@ import org.timecrafters.diagnostics.rev_hub_test_suite.RevHubTestSuiteRobot; import java.util.ArrayList; public class RevHubTestSuiteServoTestsState extends RevTestSuiteTestState { - private final int STAGE_SERVO_ROTATING = 0; private int servo_index = 0; private final ArrayList servos; private double lastMonitorTime; @@ -27,14 +26,14 @@ public class RevHubTestSuiteServoTestsState extends RevTestSuiteTestState { test_servos(); - if (stage > STAGE_SERVO_ROTATING) { + if (stage.ordinal() > STAGE.SERVO_SWEEP.ordinal()) { testComplete = true; } } private void test_servos() { if (servo_index >= servos.size()) { - stage += 1; + nextStage(); return; } @@ -55,7 +54,7 @@ public class RevHubTestSuiteServoTestsState extends RevTestSuiteTestState { } public void buttonUp(Gamepad gamepad, String button) { - if (stage != STAGE_SERVO_ROTATING) { + if (stage != STAGE.SERVO_SWEEP) { return; } @@ -75,7 +74,7 @@ public class RevHubTestSuiteServoTestsState extends RevTestSuiteTestState { } public void telemetry() { - if (stage == STAGE_SERVO_ROTATING) { + if (stage == STAGE.SERVO_SWEEP) { engine.telemetry.addLine("MANUAL TEST"); engine.telemetry.addLine("PRESS `A` if Servo " + servo_index + " is ROTATING."); engine.telemetry.addLine(); diff --git a/TeamCode/src/main/java/org/timecrafters/diagnostics/rev_hub_test_suite/states/RevTestSuiteTestState.java b/TeamCode/src/main/java/org/timecrafters/diagnostics/rev_hub_test_suite/states/RevTestSuiteTestState.java index ed6308d..aba73f2 100644 --- a/TeamCode/src/main/java/org/timecrafters/diagnostics/rev_hub_test_suite/states/RevTestSuiteTestState.java +++ b/TeamCode/src/main/java/org/timecrafters/diagnostics/rev_hub_test_suite/states/RevTestSuiteTestState.java @@ -9,9 +9,25 @@ import java.util.ArrayList; import dev.cyberarm.engine.V2.CyberarmState; public class RevTestSuiteTestState extends CyberarmState { + public enum STAGE { + NONE, + MOTOR_ENCODER_STEADY, /* Encoder value doesn't change by more than 2 ticks while motor is off. */ + MOTOR_ENCODER_FORWARD, /* Encoder value is increasing while motor is running */ + MOTOR_ENCODER_REVERSE, /* Encoder value is decreasing while motor is running */ + MOTOR_50_PERCENT_SPEED, /* Measure motor Amps and ticks per second at 50% speed, after a 3 second warm up */ + MOTOR_100_PERCENT_SPEED, /* Measure motor Amps and ticks per second at 50% speed, after a 3 second warm up */ + MOTOR_BRAKING_MODE, /* MANUAL: Test */ + SERVO_SWEEP, + ANALOG_SENSOR, + DIGITAL_SENSOR, + I2C_SENSOR, + + COMPLETE + } + final String TAG = "RevTestSuite|State"; RevHubTestSuiteRobot robot; - protected int stage = 0; + protected STAGE stage = STAGE.NONE; protected ArrayList reports; protected boolean testComplete = false; public RevTestSuiteTestState(RevHubTestSuiteRobot robot) { @@ -31,6 +47,13 @@ public class RevTestSuiteTestState extends CyberarmState { reports.add(reason); } + public void nextStage() { + if (stage == STAGE.COMPLETE) + return; + + stage = STAGE.values()[stage.ordinal() + 1]; + } + @Override public void telemetry() { if (testComplete) {