TimeCrafters/CenterStage
1
0
Fork 0
mirror of https://github.com/TimeCrafters/CenterStage synced 2025-12-15 16:52:35 +00:00
Code Issues Packages Projects Releases Wiki Activity

WIP: RevHubTestSuite: Motor testing control hub functional

Browse Source
This commit is contained in:
Cyberarm
2023-09-23 13:09:26 -05:00
parent 352b3d0f57
commit 8bec2268f0
4 changed files with 120 additions and 97 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
TeamCode/src/main/java/org/timecrafters/diagnostics/rev_hub_test_suite/RevHubTestSuiteEngine.java
View File

@@ -20,6 +20,7 @@ public class RevHubTestSuiteEngine extends CyberarmEngine {
@Override
public void setup() {
this.robot = new RevHubTestSuiteRobot();
this.robot.setup();
addState(new RevHubTestSuiteHubSelectionState(robot));

160
TeamCode/src/main/java/org/timecrafters/diagnostics/rev_hub_test_suite/RevHubTestSuiteRobot.java
View File

@@ -22,13 +22,13 @@ public class RevHubTestSuiteRobot extends Robot {
private HardwareMap hardwareMap;
public boolean testingControlHub = true;
public ArrayList<MotorEx> controlHubMotors, expansionHubMotors;
public ArrayList<ServoEx> controlHubServos, expansionHubServos;
public ArrayList<AnalogInput> controlHubAnalogSensors, expansionHubAnalogSensors;
public ArrayList<DigitalChannel> controlHubDigitalSensors, expansionHubDigitalSensors;
public ArrayList<Rev2mDistanceSensor> controlHubI2cSensors, expansionHubI2cSensors;
public ArrayList<MotorEx> controlHubMotors = new ArrayList<>(), expansionHubMotors = new ArrayList<>();
public ArrayList<ServoEx> controlHubServos = new ArrayList<>(), expansionHubServos = new ArrayList<>();
public ArrayList<AnalogInput> controlHubAnalogSensors = new ArrayList<>(), expansionHubAnalogSensors = new ArrayList<>();
public ArrayList<DigitalChannel> controlHubDigitalSensors = new ArrayList<>(), expansionHubDigitalSensors = new ArrayList<>();
public ArrayList<Rev2mDistanceSensor> controlHubI2cSensors = new ArrayList<>(), expansionHubI2cSensors = new ArrayList<>();
public ArrayList<LynxModule> lynxModules;
public ArrayList<LynxModule> 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();
// }
}
}

41
TeamCode/src/main/java/org/timecrafters/diagnostics/rev_hub_test_suite/states/RevHubTestSuiteMotorTestsState.java
View File

@@ -15,7 +15,7 @@ public class RevHubTestSuiteMotorTestsState extends RevTestSuiteTestState {
private final ArrayList<MotorEx> 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<Double> 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);

15
TeamCode/src/main/java/org/timecrafters/diagnostics/rev_hub_test_suite/states/RevTestSuiteTestState.java
View File

@@ -28,7 +28,7 @@ public class RevTestSuiteTestState extends CyberarmState {
final String TAG = "RevTestSuite|State";
RevHubTestSuiteRobot robot;
protected STAGE stage = STAGE.NONE;
protected ArrayList<String> reports;
protected final ArrayList<String> 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);
}
}
}
}