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 new file mode 100644 index 0000000..f21140f --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/diagnostics/rev_hub_test_suite/RevHubTestSuiteEngine.java @@ -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)); + } +} 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 new file mode 100644 index 0000000..19b57b4 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/diagnostics/rev_hub_test_suite/RevHubTestSuiteRobot.java @@ -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 controlHubMotors, expansionHubMotors; + public ArrayList controlHubServos, expansionHubServos; + public ArrayList controlHubAnalogSensors, expansionHubAnalogSensors; + public ArrayList controlHubDigitalSensors, expansionHubDigitalSensors; + public ArrayList 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")); + } +} diff --git a/TeamCode/src/main/java/org/timecrafters/diagnostics/rev_hub_test_suite/states/RevHubTestSuiteAnalogTestsState.java b/TeamCode/src/main/java/org/timecrafters/diagnostics/rev_hub_test_suite/states/RevHubTestSuiteAnalogTestsState.java new file mode 100644 index 0000000..f3f5140 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/diagnostics/rev_hub_test_suite/states/RevHubTestSuiteAnalogTestsState.java @@ -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(); + } +} diff --git a/TeamCode/src/main/java/org/timecrafters/diagnostics/rev_hub_test_suite/states/RevHubTestSuiteDigitalTestsState.java b/TeamCode/src/main/java/org/timecrafters/diagnostics/rev_hub_test_suite/states/RevHubTestSuiteDigitalTestsState.java new file mode 100644 index 0000000..efd732b --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/diagnostics/rev_hub_test_suite/states/RevHubTestSuiteDigitalTestsState.java @@ -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(); + } +} diff --git a/TeamCode/src/main/java/org/timecrafters/diagnostics/rev_hub_test_suite/states/RevHubTestSuiteI2CTestsState.java b/TeamCode/src/main/java/org/timecrafters/diagnostics/rev_hub_test_suite/states/RevHubTestSuiteI2CTestsState.java new file mode 100644 index 0000000..83e31bc --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/diagnostics/rev_hub_test_suite/states/RevHubTestSuiteI2CTestsState.java @@ -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(); + } +} 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 new file mode 100644 index 0000000..363ac09 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/diagnostics/rev_hub_test_suite/states/RevHubTestSuiteMotorTestsState.java @@ -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 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(); + } +} 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 new file mode 100644 index 0000000..b692129 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/diagnostics/rev_hub_test_suite/states/RevHubTestSuiteServoTestsState.java @@ -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 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(); + } +} 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 new file mode 100644 index 0000000..ed6308d --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/diagnostics/rev_hub_test_suite/states/RevTestSuiteTestState.java @@ -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 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); + } + } +}