mirror of
https://github.com/TimeCrafters/CenterStage
synced 2025-12-15 16:52:35 +00:00
WIP: Initial work on semi-automated Rev *Hub Test Suite
This commit is contained in:
@@ -0,0 +1,29 @@
|
||||
package org.timecrafters.diagnostics.rev_hub_test_suite;
|
||||
|
||||
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.RevHubTestSuiteI2CTestsState;
|
||||
import org.timecrafters.diagnostics.rev_hub_test_suite.states.RevHubTestSuiteMotorTestsState;
|
||||
import org.timecrafters.diagnostics.rev_hub_test_suite.states.RevHubTestSuiteServoTestsState;
|
||||
|
||||
import dev.cyberarm.engine.V2.CyberarmEngine;
|
||||
|
||||
//@Disabled
|
||||
@TeleOp(name = "Rev Hub Test Suite", group = "diagnostics")
|
||||
public class RevHubTestSuiteEngine extends CyberarmEngine {
|
||||
final String TAG = "RevTestSuite|Engine";
|
||||
RevHubTestSuiteRobot robot;
|
||||
|
||||
@Override
|
||||
public void setup() {
|
||||
this.robot = new RevHubTestSuiteRobot();
|
||||
|
||||
addState(new RevHubTestSuiteMotorTestsState(robot));
|
||||
addState(new RevHubTestSuiteServoTestsState(robot));
|
||||
addState(new RevHubTestSuiteAnalogTestsState(robot));
|
||||
addState(new RevHubTestSuiteDigitalTestsState(robot));
|
||||
addState(new RevHubTestSuiteI2CTestsState(robot));
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,106 @@
|
||||
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.rev.Rev2mDistanceSensor;
|
||||
import com.qualcomm.robotcore.hardware.AnalogInput;
|
||||
import com.qualcomm.robotcore.hardware.DigitalChannel;
|
||||
import com.qualcomm.robotcore.hardware.HardwareMap;
|
||||
|
||||
import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
|
||||
import org.timecrafters.Library.Robot;
|
||||
|
||||
import java.util.ArrayList;
|
||||
|
||||
import dev.cyberarm.engine.V2.CyberarmEngine;
|
||||
|
||||
public class RevHubTestSuiteRobot extends Robot {
|
||||
final String TAG = "RevTestSuite|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;
|
||||
@Override
|
||||
public void setup() {
|
||||
this.hardwareMap = CyberarmEngine.instance.hardwareMap;
|
||||
|
||||
/* ------------------------------------------------ Control Hub Devices ------------------------------------------------ */
|
||||
// MOTORS
|
||||
controlHubMotors.add(new MotorEx(hardwareMap, "c_motor_0"));
|
||||
controlHubMotors.add(new MotorEx(hardwareMap, "c_motor_1"));
|
||||
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"));
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,14 @@
|
||||
package org.timecrafters.diagnostics.rev_hub_test_suite.states;
|
||||
|
||||
import org.timecrafters.diagnostics.rev_hub_test_suite.RevHubTestSuiteRobot;
|
||||
|
||||
public class RevHubTestSuiteAnalogTestsState extends RevTestSuiteTestState {
|
||||
public RevHubTestSuiteAnalogTestsState(RevHubTestSuiteRobot robot) {
|
||||
super(robot);
|
||||
}
|
||||
|
||||
@Override
|
||||
public void exec() {
|
||||
super.exec();
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,14 @@
|
||||
package org.timecrafters.diagnostics.rev_hub_test_suite.states;
|
||||
|
||||
import org.timecrafters.diagnostics.rev_hub_test_suite.RevHubTestSuiteRobot;
|
||||
|
||||
public class RevHubTestSuiteDigitalTestsState extends RevTestSuiteTestState {
|
||||
public RevHubTestSuiteDigitalTestsState(RevHubTestSuiteRobot robot) {
|
||||
super(robot);
|
||||
}
|
||||
|
||||
@Override
|
||||
public void exec() {
|
||||
super.exec();
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,16 @@
|
||||
package org.timecrafters.diagnostics.rev_hub_test_suite.states;
|
||||
|
||||
import org.timecrafters.diagnostics.rev_hub_test_suite.RevHubTestSuiteRobot;
|
||||
|
||||
import dev.cyberarm.engine.V2.CyberarmState;
|
||||
|
||||
public class RevHubTestSuiteI2CTestsState extends RevTestSuiteTestState {
|
||||
public RevHubTestSuiteI2CTestsState(RevHubTestSuiteRobot robot) {
|
||||
super(robot);
|
||||
}
|
||||
|
||||
@Override
|
||||
public void exec() {
|
||||
super.exec();
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,165 @@
|
||||
package org.timecrafters.diagnostics.rev_hub_test_suite.states;
|
||||
|
||||
import com.arcrobotics.ftclib.hardware.motors.Motor;
|
||||
import com.arcrobotics.ftclib.hardware.motors.MotorEx;
|
||||
import com.qualcomm.robotcore.hardware.Gamepad;
|
||||
|
||||
import org.timecrafters.diagnostics.rev_hub_test_suite.RevHubTestSuiteRobot;
|
||||
|
||||
import java.util.ArrayList;
|
||||
|
||||
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 int initialValue, lastValue;
|
||||
private boolean setInitialValue = true;
|
||||
public RevHubTestSuiteMotorTestsState(RevHubTestSuiteRobot robot) {
|
||||
super(robot);
|
||||
|
||||
motors = robot.testingControlHub ? robot.controlHubMotors : robot.expansionHubMotors;
|
||||
lastMonitorTime = runTime();
|
||||
}
|
||||
|
||||
@Override
|
||||
public void exec() {
|
||||
super.exec();
|
||||
|
||||
switch (stage) {
|
||||
case STAGE_ENCODER_STEADY: {
|
||||
test_encoder_steady();
|
||||
}
|
||||
case STAGE_ENCODER_CHANGES: {
|
||||
test_encoder_changes();
|
||||
}
|
||||
case STAGE_MOTOR_BRAKING: {
|
||||
test_motor_braking();
|
||||
}
|
||||
}
|
||||
|
||||
if (stage > STAGE_MOTOR_BRAKING) {
|
||||
testComplete = true;
|
||||
}
|
||||
}
|
||||
|
||||
protected void test_encoder_steady() {
|
||||
if (motor_index >= motors.size()) {
|
||||
stage += 1;
|
||||
motor_index = 0;
|
||||
setInitialValue = true;
|
||||
return;
|
||||
}
|
||||
|
||||
MotorEx motor = motors.get(motor_index);
|
||||
|
||||
if (setInitialValue) {
|
||||
setInitialValue = false;
|
||||
initialValue = motor.getCurrentPosition();
|
||||
lastMonitorTime = runTime();
|
||||
}
|
||||
|
||||
if (runTime() - lastMonitorTime >= automaticInterval) {
|
||||
lastValue = Math.abs(initialValue - motor.getCurrentPosition());
|
||||
|
||||
if (lastValue <= 2) {
|
||||
report("PASSED: Motor Encoder " + motor_index + " STEADY");
|
||||
this.motor_index++;
|
||||
} else {
|
||||
report("FAILED: Motor Encoder " + motor_index + " STEADY (" + lastValue + " > 2)");
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
protected void test_encoder_changes() {
|
||||
if (motor_index >= motors.size()) {
|
||||
stage += 1;
|
||||
motor_index = 0;
|
||||
setInitialValue = true;
|
||||
return;
|
||||
}
|
||||
|
||||
MotorEx motor = motors.get(motor_index);
|
||||
|
||||
if (setInitialValue) {
|
||||
setInitialValue = false;
|
||||
initialValue = motor.getCurrentPosition();
|
||||
lastMonitorTime = runTime();
|
||||
|
||||
motor.setVelocity(0.25);
|
||||
}
|
||||
|
||||
if (runTime() - lastMonitorTime >= automaticInterval) {
|
||||
lastValue = Math.abs(initialValue - motor.getCurrentPosition());
|
||||
|
||||
if (lastValue >= 100) {
|
||||
motor.stopMotor();
|
||||
report("PASSED: Motor Encoder `" + motor_index + "` CHANGE");
|
||||
this.motor_index++;
|
||||
} else {
|
||||
report("FAILED: Motor Encoder `" + motor_index + "` CHANGE (" + lastValue + " < 100)");
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
protected void test_motor_braking() {
|
||||
if (motor_index >= motors.size()) {
|
||||
stage += 1;
|
||||
motor_index = 0;
|
||||
setInitialValue = true;
|
||||
return;
|
||||
}
|
||||
|
||||
MotorEx motor = motors.get(motor_index);
|
||||
|
||||
motor.setZeroPowerBehavior(Motor.ZeroPowerBehavior.BRAKE);
|
||||
}
|
||||
|
||||
@Override
|
||||
public void buttonUp(Gamepad gamepad, String button) {
|
||||
if (stage != STAGE_MOTOR_BRAKING) {
|
||||
return;
|
||||
}
|
||||
|
||||
if (engine.gamepad1 == gamepad) {
|
||||
if (button.equals("a")) {
|
||||
report("PASSED: Motor " + motor_index + " BRAKES");
|
||||
|
||||
try {
|
||||
MotorEx motor = motors.get(motor_index);
|
||||
motor.setZeroPowerBehavior(Motor.ZeroPowerBehavior.FLOAT);
|
||||
} catch (IndexOutOfBoundsException e) { /* no op */ }
|
||||
|
||||
motor_index++;
|
||||
} else if (button.equals("y")) {
|
||||
report("FAILED: Motor " + motor_index + " does NOT BRAKE");
|
||||
|
||||
try {
|
||||
MotorEx motor = motors.get(motor_index);
|
||||
motor.setZeroPowerBehavior(Motor.ZeroPowerBehavior.FLOAT);
|
||||
} catch (IndexOutOfBoundsException e) { /* no op */ }
|
||||
|
||||
motor_index++;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@Override
|
||||
public void telemetry() {
|
||||
if (stage == STAGE_MOTOR_BRAKING) {
|
||||
engine.telemetry.addLine("MANUAL TEST");
|
||||
engine.telemetry.addLine("PRESS `A` if Motor " + motor_index + " is BRAKING.");
|
||||
engine.telemetry.addLine();
|
||||
engine.telemetry.addLine("PRESS `Y` if Motor " + motor_index + " is NOT BRAKING.");
|
||||
engine.telemetry.addLine();
|
||||
}
|
||||
|
||||
super.telemetry();
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,88 @@
|
||||
package org.timecrafters.diagnostics.rev_hub_test_suite.states;
|
||||
|
||||
import com.arcrobotics.ftclib.hardware.ServoEx;
|
||||
import com.qualcomm.robotcore.hardware.Gamepad;
|
||||
|
||||
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;
|
||||
private double automaticInterval = 1500.0; // milliseconds
|
||||
private boolean setInitialValue = true;
|
||||
public RevHubTestSuiteServoTestsState(RevHubTestSuiteRobot robot) {
|
||||
super(robot);
|
||||
|
||||
servos = robot.testingControlHub ? robot.controlHubServos : robot.expansionHubServos;
|
||||
lastMonitorTime = runTime();
|
||||
}
|
||||
|
||||
@Override
|
||||
public void exec() {
|
||||
super.exec();
|
||||
|
||||
test_servos();
|
||||
|
||||
if (stage > STAGE_SERVO_ROTATING) {
|
||||
testComplete = true;
|
||||
}
|
||||
}
|
||||
|
||||
private void test_servos() {
|
||||
if (servo_index >= servos.size()) {
|
||||
stage += 1;
|
||||
return;
|
||||
}
|
||||
|
||||
ServoEx servo = servos.get(servo_index);
|
||||
|
||||
if (setInitialValue) {
|
||||
setInitialValue = false;
|
||||
lastMonitorTime = runTime();
|
||||
|
||||
servo.setPosition(0);
|
||||
}
|
||||
|
||||
if (runTime() - lastMonitorTime >= automaticInterval) {
|
||||
lastMonitorTime = runTime();
|
||||
|
||||
servo.setPosition(servo.getPosition() == 0 ? 1 : 0);
|
||||
}
|
||||
}
|
||||
|
||||
public void buttonUp(Gamepad gamepad, String button) {
|
||||
if (stage != STAGE_SERVO_ROTATING) {
|
||||
return;
|
||||
}
|
||||
|
||||
if (engine.gamepad1 == gamepad) {
|
||||
if (button.equals("a")) {
|
||||
report("PASSED: Servo " + servo_index + " ROTATES");
|
||||
|
||||
setInitialValue = true;
|
||||
servo_index++;
|
||||
} else if (button.equals("y")) {
|
||||
report("FAILED: Servo " + servo_index + " does NOT ROTATE");
|
||||
|
||||
setInitialValue = true;
|
||||
servo_index++;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
public void telemetry() {
|
||||
if (stage == STAGE_SERVO_ROTATING) {
|
||||
engine.telemetry.addLine("MANUAL TEST");
|
||||
engine.telemetry.addLine("PRESS `A` if Servo " + servo_index + " is ROTATING.");
|
||||
engine.telemetry.addLine();
|
||||
engine.telemetry.addLine("PRESS `Y` if Servo " + servo_index + " is NOT ROTATING.");
|
||||
engine.telemetry.addLine();
|
||||
}
|
||||
|
||||
super.telemetry();
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,49 @@
|
||||
package org.timecrafters.diagnostics.rev_hub_test_suite.states;
|
||||
|
||||
import android.util.Log;
|
||||
|
||||
import org.timecrafters.diagnostics.rev_hub_test_suite.RevHubTestSuiteRobot;
|
||||
|
||||
import java.util.ArrayList;
|
||||
|
||||
import dev.cyberarm.engine.V2.CyberarmState;
|
||||
|
||||
public class RevTestSuiteTestState extends CyberarmState {
|
||||
final String TAG = "RevTestSuite|State";
|
||||
RevHubTestSuiteRobot robot;
|
||||
protected int stage = 0;
|
||||
protected ArrayList<String> reports;
|
||||
protected boolean testComplete = false;
|
||||
public RevTestSuiteTestState(RevHubTestSuiteRobot robot) {
|
||||
super();
|
||||
|
||||
this.robot = robot;
|
||||
}
|
||||
|
||||
@Override
|
||||
public void exec() {
|
||||
if (testComplete && engine.gamepad1.guide) {
|
||||
setHasFinished(true);
|
||||
}
|
||||
}
|
||||
|
||||
public void report(String reason) {
|
||||
reports.add(reason);
|
||||
}
|
||||
|
||||
@Override
|
||||
public void telemetry() {
|
||||
if (testComplete) {
|
||||
engine.telemetry.addLine("TESTING");
|
||||
} else {
|
||||
engine.telemetry.addLine("PRESS `GUIDE` TO CONTINUE");
|
||||
}
|
||||
engine.telemetry.addLine();
|
||||
engine.telemetry.addData("STAGE", stage);
|
||||
engine.telemetry.addLine();
|
||||
engine.telemetry.addLine("REPORTS");
|
||||
for (String report : reports) {
|
||||
engine.telemetry.addLine(report);
|
||||
}
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user