From 8bec2268f01f29d11701061848ca51ea96a9b1c1 Mon Sep 17 00:00:00 2001 From: Cyberarm Date: Sat, 23 Sep 2023 13:09:26 -0500 Subject: [PATCH] WIP: RevHubTestSuite: Motor testing control hub functional --- .../RevHubTestSuiteEngine.java | 1 + .../RevHubTestSuiteRobot.java | 160 +++++++++--------- .../RevHubTestSuiteMotorTestsState.java | 41 +++-- .../states/RevTestSuiteTestState.java | 15 +- 4 files changed, 120 insertions(+), 97 deletions(-) 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 1aa5374..c549592 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 @@ -20,6 +20,7 @@ public class RevHubTestSuiteEngine extends CyberarmEngine { @Override public void setup() { this.robot = new RevHubTestSuiteRobot(); + this.robot.setup(); addState(new RevHubTestSuiteHubSelectionState(robot)); 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 45ef883..da3c27b 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 @@ -22,13 +22,13 @@ public class RevHubTestSuiteRobot extends Robot { private HardwareMap hardwareMap; public boolean testingControlHub = true; - public ArrayList controlHubMotors, expansionHubMotors; - public ArrayList controlHubServos, expansionHubServos; - public ArrayList controlHubAnalogSensors, expansionHubAnalogSensors; - public ArrayList controlHubDigitalSensors, expansionHubDigitalSensors; - public ArrayList controlHubI2cSensors, expansionHubI2cSensors; + public ArrayList controlHubMotors = new ArrayList<>(), expansionHubMotors = new ArrayList<>(); + public ArrayList controlHubServos = new ArrayList<>(), expansionHubServos = new ArrayList<>(); + public ArrayList controlHubAnalogSensors = new ArrayList<>(), expansionHubAnalogSensors = new ArrayList<>(); + public ArrayList controlHubDigitalSensors = new ArrayList<>(), expansionHubDigitalSensors = new ArrayList<>(); + public ArrayList controlHubI2cSensors = new ArrayList<>(), expansionHubI2cSensors = new ArrayList<>(); - public ArrayList lynxModules; + public ArrayList lynxModules = new ArrayList<>(); @Override public void setup() { this.hardwareMap = CyberarmEngine.instance.hardwareMap; @@ -42,77 +42,77 @@ public class RevHubTestSuiteRobot extends Robot { controlHubMotors.add(new MotorEx(hardwareMap, "c_motor_2")); controlHubMotors.add(new MotorEx(hardwareMap, "c_motor_3")); - // SERVOS - controlHubServos.add(new SimpleServo(hardwareMap, "c_servo_0", 0.0, 180.0, AngleUnit.DEGREES)); - controlHubServos.add(new SimpleServo(hardwareMap, "c_servo_1", 0.0, 180.0, AngleUnit.DEGREES)); - controlHubServos.add(new SimpleServo(hardwareMap, "c_servo_2", 0.0, 180.0, AngleUnit.DEGREES)); - controlHubServos.add(new SimpleServo(hardwareMap, "c_servo_3", 0.0, 180.0, AngleUnit.DEGREES)); - controlHubServos.add(new SimpleServo(hardwareMap, "c_servo_4", 0.0, 180.0, AngleUnit.DEGREES)); - controlHubServos.add(new SimpleServo(hardwareMap, "c_servo_5", 0.0, 180.0, AngleUnit.DEGREES)); - - // ANALOG SENSORS - controlHubAnalogSensors.add(hardwareMap.analogInput.get("c_analog_0")); - controlHubAnalogSensors.add(hardwareMap.analogInput.get("c_analog_1")); - controlHubAnalogSensors.add(hardwareMap.analogInput.get("c_analog_2")); - controlHubAnalogSensors.add(hardwareMap.analogInput.get("c_analog_3")); - - // DIGITAL SENSORS - controlHubDigitalSensors.add(hardwareMap.digitalChannel.get("c_digital_0")); - controlHubDigitalSensors.add(hardwareMap.digitalChannel.get("c_digital_1")); - controlHubDigitalSensors.add(hardwareMap.digitalChannel.get("c_digital_2")); - controlHubDigitalSensors.add(hardwareMap.digitalChannel.get("c_digital_3")); - controlHubDigitalSensors.add(hardwareMap.digitalChannel.get("c_digital_4")); - controlHubDigitalSensors.add(hardwareMap.digitalChannel.get("c_digital_5")); - controlHubDigitalSensors.add(hardwareMap.digitalChannel.get("c_digital_6")); - controlHubDigitalSensors.add(hardwareMap.digitalChannel.get("c_digital_7")); - - // I2C SENSORS - controlHubI2cSensors.add(hardwareMap.get(Rev2mDistanceSensor.class, "c_i2c_0")); - controlHubI2cSensors.add(hardwareMap.get(Rev2mDistanceSensor.class, "c_i2c_1")); - controlHubI2cSensors.add(hardwareMap.get(Rev2mDistanceSensor.class, "c_i2c_2")); - controlHubI2cSensors.add(hardwareMap.get(Rev2mDistanceSensor.class, "c_i2c_3")); - - /* ------------------------------------------------ Expansion Hub Devices ------------------------------------------------ */ - // MOTORS - expansionHubMotors.add(new MotorEx(hardwareMap, "x_motor_0")); - expansionHubMotors.add(new MotorEx(hardwareMap, "x_motor_1")); - expansionHubMotors.add(new MotorEx(hardwareMap, "x_motor_2")); - expansionHubMotors.add(new MotorEx(hardwareMap, "x_motor_3")); - - // SERVOS - expansionHubServos.add(new SimpleServo(hardwareMap, "x_servo_0", 0.0, 180.0, AngleUnit.DEGREES)); - expansionHubServos.add(new SimpleServo(hardwareMap, "x_servo_1", 0.0, 180.0, AngleUnit.DEGREES)); - expansionHubServos.add(new SimpleServo(hardwareMap, "x_servo_2", 0.0, 180.0, AngleUnit.DEGREES)); - expansionHubServos.add(new SimpleServo(hardwareMap, "x_servo_3", 0.0, 180.0, AngleUnit.DEGREES)); - expansionHubServos.add(new SimpleServo(hardwareMap, "x_servo_4", 0.0, 180.0, AngleUnit.DEGREES)); - expansionHubServos.add(new SimpleServo(hardwareMap, "x_servo_5", 0.0, 180.0, AngleUnit.DEGREES)); - - // ANALOG SENSORS - expansionHubAnalogSensors.add(hardwareMap.analogInput.get("x_analog_0")); - expansionHubAnalogSensors.add(hardwareMap.analogInput.get("x_analog_1")); - expansionHubAnalogSensors.add(hardwareMap.analogInput.get("x_analog_2")); - expansionHubAnalogSensors.add(hardwareMap.analogInput.get("x_analog_3")); - - // DIGITAL SENSORS - expansionHubDigitalSensors.add(hardwareMap.digitalChannel.get("x_digital_0")); - expansionHubDigitalSensors.add(hardwareMap.digitalChannel.get("x_digital_1")); - expansionHubDigitalSensors.add(hardwareMap.digitalChannel.get("x_digital_2")); - expansionHubDigitalSensors.add(hardwareMap.digitalChannel.get("x_digital_3")); - expansionHubDigitalSensors.add(hardwareMap.digitalChannel.get("x_digital_4")); - expansionHubDigitalSensors.add(hardwareMap.digitalChannel.get("x_digital_5")); - expansionHubDigitalSensors.add(hardwareMap.digitalChannel.get("x_digital_6")); - expansionHubDigitalSensors.add(hardwareMap.digitalChannel.get("x_digital_7")); - - // I2C SENSORS - expansionHubI2cSensors.add(hardwareMap.get(Rev2mDistanceSensor.class, "x_i2c_0")); - 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); - } +// // SERVOS +// controlHubServos.add(new SimpleServo(hardwareMap, "c_servo_0", 0.0, 180.0, AngleUnit.DEGREES)); +// controlHubServos.add(new SimpleServo(hardwareMap, "c_servo_1", 0.0, 180.0, AngleUnit.DEGREES)); +// controlHubServos.add(new SimpleServo(hardwareMap, "c_servo_2", 0.0, 180.0, AngleUnit.DEGREES)); +// controlHubServos.add(new SimpleServo(hardwareMap, "c_servo_3", 0.0, 180.0, AngleUnit.DEGREES)); +// controlHubServos.add(new SimpleServo(hardwareMap, "c_servo_4", 0.0, 180.0, AngleUnit.DEGREES)); +// controlHubServos.add(new SimpleServo(hardwareMap, "c_servo_5", 0.0, 180.0, AngleUnit.DEGREES)); +// +// // ANALOG SENSORS +// controlHubAnalogSensors.add(hardwareMap.analogInput.get("c_analog_0")); +// controlHubAnalogSensors.add(hardwareMap.analogInput.get("c_analog_1")); +// controlHubAnalogSensors.add(hardwareMap.analogInput.get("c_analog_2")); +// controlHubAnalogSensors.add(hardwareMap.analogInput.get("c_analog_3")); +// +// // DIGITAL SENSORS +// controlHubDigitalSensors.add(hardwareMap.digitalChannel.get("c_digital_0")); +// controlHubDigitalSensors.add(hardwareMap.digitalChannel.get("c_digital_1")); +// controlHubDigitalSensors.add(hardwareMap.digitalChannel.get("c_digital_2")); +// controlHubDigitalSensors.add(hardwareMap.digitalChannel.get("c_digital_3")); +// controlHubDigitalSensors.add(hardwareMap.digitalChannel.get("c_digital_4")); +// controlHubDigitalSensors.add(hardwareMap.digitalChannel.get("c_digital_5")); +// controlHubDigitalSensors.add(hardwareMap.digitalChannel.get("c_digital_6")); +// controlHubDigitalSensors.add(hardwareMap.digitalChannel.get("c_digital_7")); +// +// // I2C SENSORS +// controlHubI2cSensors.add(hardwareMap.get(Rev2mDistanceSensor.class, "c_i2c_0")); +// controlHubI2cSensors.add(hardwareMap.get(Rev2mDistanceSensor.class, "c_i2c_1")); +// controlHubI2cSensors.add(hardwareMap.get(Rev2mDistanceSensor.class, "c_i2c_2")); +// controlHubI2cSensors.add(hardwareMap.get(Rev2mDistanceSensor.class, "c_i2c_3")); +// +// /* ------------------------------------------------ Expansion Hub Devices ------------------------------------------------ */ +// // MOTORS +// expansionHubMotors.add(new MotorEx(hardwareMap, "x_motor_0")); +// expansionHubMotors.add(new MotorEx(hardwareMap, "x_motor_1")); +// expansionHubMotors.add(new MotorEx(hardwareMap, "x_motor_2")); +// expansionHubMotors.add(new MotorEx(hardwareMap, "x_motor_3")); +// +// // SERVOS +// expansionHubServos.add(new SimpleServo(hardwareMap, "x_servo_0", 0.0, 180.0, AngleUnit.DEGREES)); +// expansionHubServos.add(new SimpleServo(hardwareMap, "x_servo_1", 0.0, 180.0, AngleUnit.DEGREES)); +// expansionHubServos.add(new SimpleServo(hardwareMap, "x_servo_2", 0.0, 180.0, AngleUnit.DEGREES)); +// expansionHubServos.add(new SimpleServo(hardwareMap, "x_servo_3", 0.0, 180.0, AngleUnit.DEGREES)); +// expansionHubServos.add(new SimpleServo(hardwareMap, "x_servo_4", 0.0, 180.0, AngleUnit.DEGREES)); +// expansionHubServos.add(new SimpleServo(hardwareMap, "x_servo_5", 0.0, 180.0, AngleUnit.DEGREES)); +// +// // ANALOG SENSORS +// expansionHubAnalogSensors.add(hardwareMap.analogInput.get("x_analog_0")); +// expansionHubAnalogSensors.add(hardwareMap.analogInput.get("x_analog_1")); +// expansionHubAnalogSensors.add(hardwareMap.analogInput.get("x_analog_2")); +// expansionHubAnalogSensors.add(hardwareMap.analogInput.get("x_analog_3")); +// +// // DIGITAL SENSORS +// expansionHubDigitalSensors.add(hardwareMap.digitalChannel.get("x_digital_0")); +// expansionHubDigitalSensors.add(hardwareMap.digitalChannel.get("x_digital_1")); +// expansionHubDigitalSensors.add(hardwareMap.digitalChannel.get("x_digital_2")); +// expansionHubDigitalSensors.add(hardwareMap.digitalChannel.get("x_digital_3")); +// expansionHubDigitalSensors.add(hardwareMap.digitalChannel.get("x_digital_4")); +// expansionHubDigitalSensors.add(hardwareMap.digitalChannel.get("x_digital_5")); +// expansionHubDigitalSensors.add(hardwareMap.digitalChannel.get("x_digital_6")); +// expansionHubDigitalSensors.add(hardwareMap.digitalChannel.get("x_digital_7")); +// +// // I2C SENSORS +// expansionHubI2cSensors.add(hardwareMap.get(Rev2mDistanceSensor.class, "x_i2c_0")); +// 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(); @@ -131,8 +131,8 @@ public class RevHubTestSuiteRobot extends Robot { } protected void clearStaleData() { - for (LynxModule hub : lynxModules) { - hub.clearBulkCache(); - } +// for (LynxModule hub : lynxModules) { +// hub.clearBulkCache(); +// } } } 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 d61e9b9..da974e6 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 @@ -15,7 +15,7 @@ public class RevHubTestSuiteMotorTestsState extends RevTestSuiteTestState { private final ArrayList motors; private double lastMonitorTime, lastSampleMonitorTime; - private final double automaticInterval = 500.0; // milliseconds + private final double automaticInterval = 3_000; // milliseconds private final double automaticSampleInterval = 3_000.0; // milliseconds - test motor current and ticks per second private final ArrayList sampleAmpsList = new ArrayList<>(); @@ -29,6 +29,13 @@ public class RevHubTestSuiteMotorTestsState extends RevTestSuiteTestState { lastMonitorTime = runTime(); } + @Override + public void start() { + super.start(); + + nextStage(); + } + @Override public void exec() { super.exec(); @@ -39,21 +46,27 @@ public class RevHubTestSuiteMotorTestsState extends RevTestSuiteTestState { switch (stage) { case MOTOR_ENCODER_STEADY: { test_encoder_steady(); + break; } case MOTOR_ENCODER_FORWARD: { test_encoder_changes(true); + break; } case MOTOR_ENCODER_REVERSE: { test_encoder_changes(false); + break; } case MOTOR_50_PERCENT_SPEED: { test_motor_current_and_ticks_per_second(0.5); + break; } case MOTOR_100_PERCENT_SPEED: { test_motor_current_and_ticks_per_second(1.0); + break; } case MOTOR_BRAKING_MODE: { test_motor_braking(); + break; } } @@ -76,6 +89,7 @@ public class RevHubTestSuiteMotorTestsState extends RevTestSuiteTestState { setInitialValue = false; initialValue = motor.getCurrentPosition(); lastMonitorTime = runTime(); + motor.resetEncoder(); } if (runTime() - lastMonitorTime >= automaticInterval) { @@ -83,11 +97,12 @@ public class RevHubTestSuiteMotorTestsState extends RevTestSuiteTestState { if (lastValue <= 2) { report("PASSED: Motor Encoder " + motor_index + " STEADY"); - motor_index++; - setInitialValue = true; } else { report("FAILED: Motor Encoder " + motor_index + " STEADY (" + lastValue + " > 2)"); } + + motor_index++; + setInitialValue = true; } } @@ -106,7 +121,8 @@ public class RevHubTestSuiteMotorTestsState extends RevTestSuiteTestState { initialValue = motor.getCurrentPosition(); lastMonitorTime = runTime(); - motor.setVelocity(forward ? 0.5 : -0.5); + motor.resetEncoder(); + motor.motorEx.setPower(forward ? 1.0 : -1.0); } if (runTime() - lastMonitorTime >= automaticInterval) { @@ -116,13 +132,14 @@ public class RevHubTestSuiteMotorTestsState extends RevTestSuiteTestState { String fail = forward ? "NO CHANGE; FORWARD." : "NO CHANGE; REVERSE."; if (Math.abs(lastValue) >= 100) { - motor.stopMotor(); - report("PASSED: Motor Encoder `" + motor_index + "` CHANGE"); - motor_index++; - setInitialValue = true; + report("PASSED: Motor Encoder `" + motor_index + "` " + pass); } else { - report("FAILED: Motor Encoder `" + motor_index + "` CHANGE (" + lastValue + " < 100)"); + report("FAILED: Motor Encoder `" + motor_index + "` " + fail + " (" + lastValue + " < 100)"); } + + motor.stopMotor(); + motor_index++; + setInitialValue = true; } } @@ -167,8 +184,8 @@ public class RevHubTestSuiteMotorTestsState extends RevTestSuiteTestState { if (runTime() - lastSampleMonitorTime >= 100) { lastSampleMonitorTime = runTime(); - motor.motorEx.getCurrent(CurrentUnit.AMPS); - motor.motorEx.getCurrentPosition(); + sampleAmpsList.add(motor.motorEx.getCurrent(CurrentUnit.AMPS)); + sampleTicksList.add(motor.motorEx.getCurrentPosition()); } /* --- Generate report --- */ @@ -189,7 +206,7 @@ public class RevHubTestSuiteMotorTestsState extends RevTestSuiteTestState { 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)); + report(String.format(Locale.US, "RESULT: Motor %d: Speed: %.2f, Average AMPS: %.4f, Average TICKS/s: %d", motor_index, motor.motorEx.getPower(), average_amps, (int)average_ticks)); motor_index++; motor.motorEx.setPower(0.0); 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 aba73f2..9b7aa3c 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 @@ -28,7 +28,7 @@ public class RevTestSuiteTestState extends CyberarmState { final String TAG = "RevTestSuite|State"; RevHubTestSuiteRobot robot; protected STAGE stage = STAGE.NONE; - protected ArrayList reports; + protected final ArrayList reports = new ArrayList<>(); protected boolean testComplete = false; public RevTestSuiteTestState(RevHubTestSuiteRobot robot) { super(); @@ -43,8 +43,10 @@ public class RevTestSuiteTestState extends CyberarmState { } } - public void report(String reason) { - reports.add(reason); + void report(String reason) { + synchronized (reports) { + reports.add(reason); + } } public void nextStage() { @@ -65,8 +67,11 @@ public class RevTestSuiteTestState extends CyberarmState { engine.telemetry.addData("STAGE", stage); engine.telemetry.addLine(); engine.telemetry.addLine("REPORTS"); - for (String report : reports) { - engine.telemetry.addLine(report); + + synchronized (reports) { + for (String report : reports) { + engine.telemetry.addLine(report); + } } } }