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.

2025-12-15 16:52:35 +00:00 · 2023-09-22 13:22:50 -05:00
parent b2e1aaa852
commit 10ce6ee5c8
6 changed files with 231 additions and 28 deletions
--- 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();
+    }
 }
--- 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<AnalogInput> controlHubAnalogSensors, expansionHubAnalogSensors;
    public ArrayList<DigitalChannel> controlHubDigitalSensors, expansionHubDigitalSensors;
    public ArrayList<Rev2mDistanceSensor> controlHubI2cSensors, expansionHubI2cSensors;
+
+    public ArrayList<LynxModule> 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();
+        }
    }
 }
--- 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
@@ -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);
+        }
+    }
+}
--- 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<MotorEx> 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<Double> sampleAmpsList = new ArrayList<>();
+    private final ArrayList<Integer> 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();
--- 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<ServoEx> 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();
--- 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<String> 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) {