From 031561b9f2bf28cecc55627abf1d21e84f6015e8 Mon Sep 17 00:00:00 2001 From: Jeffrey Mello Date: Tue, 18 Nov 2025 15:40:28 -0500 Subject: [PATCH 1/8] Refactor autonomous code into modular utility classes Moved constants, drive, turn, and common auto methods from AutoMain into new utility classes (AutoConstants, AutoDrive, AutoTurn, CommonAutoMethods) for better code organization and reuse. Updated MotorConstructor to include IMU and VoltageSensor initialization. Simplified AutoMain by removing duplicated logic and delegating hardware and movement control to utility classes. --- .../firstinspires/ftc/teamcode/AutoMain.java | 165 ------------------ .../teamcode/Utils_13233/AutoConstants.java | 34 ++++ .../ftc/teamcode/Utils_13233/AutoDrive.java | 97 ++++++++++ .../ftc/teamcode/Utils_13233/AutoTurn.java | 23 +++ .../Utils_13233/CommonAutoMethods.java | 57 ++++++ .../Utils_13233/MotorConstructor.java | 10 ++ 6 files changed, 221 insertions(+), 165 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/AutoConstants.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/AutoDrive.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/AutoTurn.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/CommonAutoMethods.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoMain.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoMain.java index 8c732b7..c80b218 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoMain.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoMain.java @@ -26,23 +26,6 @@ @Autonomous(name = "AutoMain", group = "Auto") public class AutoMain extends LinearOpMode { - private final MotorConstructor motors; - - public AutoMain(HardwareMap hardwareMap) { - this.motors = new MotorConstructor(hardwareMap); - } - - private IMU imu; - - private VoltageSensor VoltSens; - - - // Variables for imu - double globalAngle, correction; - YawPitchRollAngles lastAngles; - - double voltage = 0; - double batteryConst = 13.5; // Possible Autonomous Modes @@ -52,24 +35,6 @@ public enum AutoMode { } - // Variables for driving with Encoders - private static final double wheelCircumference = 4.0 * Math.PI; - private static final double gearRatio = 18.9; // Rev HD Hex 20:1 - private static final double countsPerRotation = 560.0; // Rev HD Hex 20:1 - private static final double scaleFactor = 9.0; // need to find scale factor!!! - private static final double countsPerInch = countsPerRotation / wheelCircumference - / gearRatio * scaleFactor; - - - // Global Variables - private final static double noPower = 0.0; - private final static double quarterPower = 0.25; - private final static double oneThirdPower = 0.34; - private final static double halfPower = 0.5; - private final static double threeQuartPower = 0.75; - private final static double fullPower = 1.0; - - // Global Variables to store Game Specific Information AutoMode autoMode = AutoMode.AUTO_MODE_NOT_SELECTED; // store autonomous mode selected @@ -199,16 +164,11 @@ public void runOpMode() throws InterruptedException { double tgtPower = 0; - VoltSens = hardwareMap.voltageSensor.get("Control Hub"); - /* Setup IMU parameters */ - // Retrieve and initialize the IMU. The IMU should be attached to - // IC2 port 0 on a Core Device Interface Module - imu = hardwareMap.get(IMU.class, "imu"); /* The next two lines define Hub orientation. * The Default Orientation (shown) is when a hub is mounted horizontally with the printed @@ -314,27 +274,7 @@ private void defaultAuto() { // Functions needed for driving auto - private void resetEncoders() { - motors.rightFront.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); - motors.rightBack.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); - motors.leftFront.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); - motors.leftBack.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); - } - private void runUsingEncoders() { - motors.rightFront.setMode(DcMotor.RunMode.RUN_USING_ENCODER); - motors.rightBack.setMode(DcMotor.RunMode.RUN_USING_ENCODER); - motors.leftFront.setMode(DcMotor.RunMode.RUN_USING_ENCODER); - motors.leftBack.setMode(DcMotor.RunMode.RUN_USING_ENCODER); - } - - private void setDrivePower(double leftFrontPower, double rightFrontPower, - double leftBackPower, double rightBackPower) { - motors.rightFront.setPower(rightFrontPower); - motors.rightBack.setPower(rightBackPower); - motors.leftFront.setPower(leftFrontPower); - motors.leftBack.setPower(leftBackPower); - } /** * Function: strafeRight @@ -346,94 +286,11 @@ private void strafeRight(double inches, double power) { driveInches(inches, power, -power, -power, power); } - /** - * Function: strafeLeft - *

- * This function is called to have the robot move sideways - * in a left direction - */ - private void strafeLeft(double inches, double power) { - driveInches(inches, -power, power, power, -power); - } - - - /** - * Function: driveForward - *

- * This function is called to have the robot move forward. - * The robot speed is passed in and that value is used for - * all wheels. - */ - private void driveForward(double inches, double power) { - driveInches(inches, power, power, power, power); - } - - - /** - * Function: driveBackward - *

- * This function is called to have the robot move in reverse. - * The robot speed is passed in and that value is used for - * all wheels. - */ - private void driveBackward(double inches, double power) { - driveInches(inches, -power, -power, -power, -power); - } - - - /** - * Function: driveInches - * This function is called to have the robot move straight - * in a forward or reverse direction. - *

- * Strafe Forward = negative front wheels, positive back - * wheels - */ - private void driveInches(double inches, double leftFrontPower, double rightFrontPower, double leftBackPower, double rightBackPower) { - double counts = inches * countsPerInch; - - resetEncoders(); - runUsingEncoders(); - - voltage = VoltSens.getVoltage(); // read current battery voltage - - double leftFrontPowerCont = ((batteryConst * leftFrontPower) / voltage); - double rightFrontPowerCont = ((batteryConst * rightFrontPower) / voltage); - double leftBackPowerCont = ((batteryConst * leftBackPower) / voltage); - double rightBackPowerCont = ((batteryConst * rightBackPower) / voltage); - - setDrivePower(leftFrontPowerCont, rightFrontPowerCont, leftBackPowerCont, rightBackPowerCont); - - while (opModeIsActive() && - (Math.abs(motors.leftFront.getCurrentPosition()) + - Math.abs(motors.rightFront.getCurrentPosition()) / 2.0) < Math.abs(counts)) { - // Use gyro to drive in a straight line. - correction = checkDirection(); - - // telemetry.addData("1 imu heading", lastAngles.firstAngle); - // telemetry.addData("2 global heading", globalAngle); - // telemetry.addData("3 correction", correction); - // telemetry.update(); - - setDrivePower(leftFrontPower - correction, - rightFrontPower + correction, leftBackPower - correction, - rightBackPower + correction); - idle(); - } - - setDrivePower(noPower, noPower, noPower, noPower); // Stop all motors - } // Functions for turning and checking robot angle for correction /* Resets the cumulative angle tracking to zero. */ - private void resetAngle() { - imu.resetYaw(); - lastAngles = imu.getRobotYawPitchRollAngles(); - - globalAngle = 0; - } /** * Get current cumulative angle rotation from last reset. @@ -463,28 +320,6 @@ else if (deltaAngle > 180) return globalAngle; } - /** - * See if we are moving in a straight line and if not return a power correction value. - * - * @return Power adjustment, + is adjust left - is adjust right. - */ - private double checkDirection() { - // The gain value determines how sensitive the correction is to direction changes. - // You will have to experiment with your robot to get small smooth direction changes - // to stay on a straight line. - double correction, angle, gain = .05; - - angle = getAngle(); - - if (angle == 0) - correction = 0; // no adjustment. - else - correction = -angle; // reverse sign of angle for correction. - - correction = correction * gain; - - return correction; - } /** * Rotate left or right the number of degrees. Does not support turning more than 180 degrees. diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/AutoConstants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/AutoConstants.java new file mode 100644 index 0000000..d4d782c --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/AutoConstants.java @@ -0,0 +1,34 @@ +package org.firstinspires.ftc.teamcode.Utils_13233; + +import com.qualcomm.robotcore.hardware.HardwareMap; +import com.qualcomm.robotcore.hardware.IMU; +import com.qualcomm.robotcore.hardware.VoltageSensor; + +import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles; + +public class AutoConstants { + // Global Variables + protected final static double noPower = 0.0; + protected final static double quarterPower = 0.25; + protected final static double oneThirdPower = 0.34; + protected final static double halfPower = 0.5; + protected final static double threeQuartPower = 0.75; + protected final static double fullPower = 1.0; + + + // Variables for imu + protected double globalAngle, correction; + protected YawPitchRollAngles lastAngles; + + protected double voltage = 0; + protected static double batteryConst = 13.5; + + // Variables for driving with Encoders + protected static final double wheelCircumference = 4.0 * Math.PI; + protected static final double gearRatio = 18.9; // Rev HD Hex 20:1 + protected static final double countsPerRotation = 560.0; // Rev HD Hex 20:1 + protected static final double scaleFactor = 9.0; // need to find scale factor!!! + protected static final double countsPerInch = countsPerRotation / wheelCircumference + / gearRatio * scaleFactor; + +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/AutoDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/AutoDrive.java new file mode 100644 index 0000000..6e10517 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/AutoDrive.java @@ -0,0 +1,97 @@ +package org.firstinspires.ftc.teamcode.Utils_13233; + + +import static org.firstinspires.ftc.robotcore.external.BlocksOpModeCompanion.hardwareMap; + +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; + +import com.qualcomm.robotcore.hardware.HardwareMap; +import com.qualcomm.robotcore.hardware.VoltageSensor; + + +public abstract class AutoDrive extends LinearOpMode { + private final MotorConstructor motors; + CommonAutoMethods autoMethods; + + private AutoDrive(HardwareMap hardwareMap) { + this.motors = new MotorConstructor(hardwareMap); + this.autoMethods = new CommonAutoMethods(hardwareMap); + } + + + /** + * Function: driveForward + *

+ * This function is called to have the robot move forward. + * The robot speed is passed in and that value is used for + * all wheels. + */ + private void driveForward(double inches, double power) { + driveInches(inches, power, power, power, power); + } + + + /** + * Function: driveBackward + *

+ * This function is called to have the robot move in reverse. + * The robot speed is passed in and that value is used for + * all wheels. + */ + private void driveBackward(double inches, double power) { + driveInches(inches, -power, -power, -power, -power); + } + + /** + * Function: strafeLeft + *

+ * This function is called to have the robot move sideways + * in a left direction + */ + private void strafeLeft(double inches, double power) { + driveInches(inches, -power, power, power, -power); + } + + /** + * Function: driveInches + * This function is called to have the robot move straight + * in a forward or reverse direction. + *

+ * Strafe Forward = negative front wheels, positive back + * wheels + */ + private void driveInches(double inches, double leftFrontPower, double rightFrontPower, double leftBackPower, double rightBackPower) { + double counts = inches * AutoConstants.countsPerInch; + + autoMethods.resetEncoders(); + autoMethods.runUsingEncoders(); + + double voltage = motors.VoltSens.getVoltage(); // read current battery voltage + + double leftFrontPowerCont = ((AutoConstants.batteryConst * leftFrontPower) / voltage); + double rightFrontPowerCont = ((AutoConstants.batteryConst * rightFrontPower) / voltage); + double leftBackPowerCont = ((AutoConstants.batteryConst * leftBackPower) / voltage); + double rightBackPowerCont = ((AutoConstants.batteryConst * rightBackPower) / voltage); + + autoMethods.setDrivePower(leftFrontPowerCont, rightFrontPowerCont, leftBackPowerCont, rightBackPowerCont); + + while (opModeIsActive() && + (Math.abs(motors.leftFront.getCurrentPosition()) + + Math.abs(motors.rightFront.getCurrentPosition()) / 2.0) < Math.abs(counts)) { + // Use gyro to drive in a straight line. + double correction = autoMethods.checkDirection(); + + // telemetry.addData("1 imu heading", lastAngles.firstAngle); + // telemetry.addData("2 global heading", globalAngle); + // telemetry.addData("3 correction", correction); + // telemetry.update(); + + autoMethods.setDrivePower(leftFrontPower - correction, + rightFrontPower + correction, leftBackPower - correction, + rightBackPower + correction); + idle(); + } + + autoMethods.setDrivePower(AutoConstants.noPower, AutoConstants.noPower, AutoConstants.noPower, AutoConstants.noPower); // Stop all motors + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/AutoTurn.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/AutoTurn.java new file mode 100644 index 0000000..e45fc80 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/AutoTurn.java @@ -0,0 +1,23 @@ +package org.firstinspires.ftc.teamcode.Utils_13233; + +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.hardware.HardwareMap; + +import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles; + +public abstract class AutoTurn extends LinearOpMode { + private final MotorConstructor motors; + CommonAutoMethods autoMethods; + + private AutoTurn(HardwareMap hardwareMap) { + this.motors = new MotorConstructor(hardwareMap); + this.autoMethods = new CommonAutoMethods(hardwareMap); + } + + private void resetAngle() { + motors.imu.resetYaw(); + YawPitchRollAngles lastAngles = motors.imu.getRobotYawPitchRollAngles(); + + int globalAngle = 0; + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/CommonAutoMethods.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/CommonAutoMethods.java new file mode 100644 index 0000000..62612f3 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/CommonAutoMethods.java @@ -0,0 +1,57 @@ +package org.firstinspires.ftc.teamcode.Utils_13233; + +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.HardwareMap; + +public class CommonAutoMethods { + private final MotorConstructor motors; + + public CommonAutoMethods(HardwareMap hardwareMap) { + this.motors = new MotorConstructor(hardwareMap); + } + + protected void setDrivePower(double leftFrontPower, double rightFrontPower, + double leftBackPower, double rightBackPower) { + motors.rightFront.setPower(rightFrontPower); + motors.rightBack.setPower(rightBackPower); + motors.leftFront.setPower(leftFrontPower); + motors.leftBack.setPower(leftBackPower); + } + + protected void resetEncoders() { + motors.rightFront.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + motors.rightBack.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + motors.leftFront.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + motors.leftBack.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + } + + protected void runUsingEncoders() { + motors.rightFront.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + motors.rightBack.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + motors.leftFront.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + motors.leftBack.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + } + + /** + * See if we are moving in a straight line and if not return a power correction value. + * + * @return Power adjustment, + is adjust left - is adjust right. + */ + protected double checkDirection() { + // The gain value determines how sensitive the correction is to direction changes. + // You will have to experiment with your robot to get small smooth direction changes + // to stay on a straight line. + double correction, angle, gain = .05; + + angle = getAngle(); + + if (angle == 0) + correction = 0; // no adjustment. + else + correction = -angle; // reverse sign of angle for correction. + + correction = correction * gain; + + return correction; + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/MotorConstructor.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/MotorConstructor.java index ff7e62e..7899dcd 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/MotorConstructor.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/MotorConstructor.java @@ -4,6 +4,8 @@ import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.DcMotorSimple; import com.qualcomm.robotcore.hardware.HardwareMap; +import com.qualcomm.robotcore.hardware.IMU; +import com.qualcomm.robotcore.hardware.VoltageSensor; /** * Class holds motors and servos that are used throughout the code @@ -53,6 +55,9 @@ public class MotorConstructor { public CRServo rampServo3; public CRServo rampServo4; + VoltageSensor VoltSens; + protected IMU imu; + public MotorConstructor(HardwareMap hardwareMap) { // Map main Drive Motors leftFront = hardwareMap.get(DcMotor.class, "leftFront"); @@ -72,6 +77,11 @@ public MotorConstructor(HardwareMap hardwareMap) { rampServo3 = hardwareMap.get(CRServo.class, "rampServo3"); rampServo4 = hardwareMap.get(CRServo.class, "rampServo4"); + VoltSens = hardwareMap.voltageSensor.get("Control Hub"); + + // IC2 port 0 on a Core Device Interface Module + imu = hardwareMap.get(IMU.class, "imu"); + // Set direction of the main drive motors leftFront.setDirection(DcMotorSimple.Direction.FORWARD); rightFront.setDirection(DcMotorSimple.Direction.REVERSE); From 916d545f0cedc777a0ace099ac43cd64c80ba6d2 Mon Sep 17 00:00:00 2001 From: Jeffrey Mello Date: Tue, 18 Nov 2025 21:19:48 -0500 Subject: [PATCH 2/8] Refactor autonomous code into modular classes Moved drive, turn, and common autonomous methods from AutoMain into separate utility classes (AutoDrive, AutoTurn, CommonAutoMethods) for better modularity and maintainability. Updated AutoConstants to use public constants. Adjusted method access and signatures to support new structure and improved IMU setup and encoder reset handling. --- .../firstinspires/ftc/teamcode/AutoMain.java | 143 ++---------------- .../teamcode/Utils_13233/AutoConstants.java | 12 +- .../ftc/teamcode/Utils_13233/AutoDrive.java | 23 ++- .../ftc/teamcode/Utils_13233/AutoTurn.java | 47 +++++- .../Utils_13233/CommonAutoMethods.java | 65 +++++++- 5 files changed, 147 insertions(+), 143 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoMain.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoMain.java index c80b218..40e6bb7 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoMain.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoMain.java @@ -20,12 +20,20 @@ // import for IMU (gyroscope) import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles; +import org.firstinspires.ftc.teamcode.Utils_13233.AutoConstants; +import org.firstinspires.ftc.teamcode.Utils_13233.AutoDrive; +import org.firstinspires.ftc.teamcode.Utils_13233.AutoTurn; +import org.firstinspires.ftc.teamcode.Utils_13233.CommonAutoMethods; import org.firstinspires.ftc.teamcode.Utils_13233.MotorConstructor; @Autonomous(name = "AutoMain", group = "Auto") -public class AutoMain extends LinearOpMode { +public class AutoMain extends LinearOpMode{ + private AutoDrive drive; + private AutoTurn turn; + private CommonAutoMethods autoMethods; + // Possible Autonomous Modes @@ -161,52 +169,17 @@ private Integer SelectDelayTime() { // OpMode for autonomous code @Override public void runOpMode() throws InterruptedException { + drive = new AutoDrive(this, hardwareMap); + turn = new AutoTurn(this, hardwareMap); + autoMethods = new CommonAutoMethods(this, hardwareMap); double tgtPower = 0; - - - - - - /* The next two lines define Hub orientation. - * The Default Orientation (shown) is when a hub is mounted horizontally with the printed - * logo pointing UP and the USB port pointing FORWARD. - * - * To Do: EDIT these two lines to match YOUR mounting configuration. - */ - RevHubOrientationOnRobot.LogoFacingDirection logoDirection - = RevHubOrientationOnRobot.LogoFacingDirection.BACKWARD; - RevHubOrientationOnRobot.UsbFacingDirection usbDirection - = RevHubOrientationOnRobot.UsbFacingDirection.DOWN; - telemetry.addData("Mode", "calibrating imu...."); telemetry.update(); - - try { - RevHubOrientationOnRobot orientationOnRobot = - new RevHubOrientationOnRobot(logoDirection, usbDirection); - imu.initialize(new IMU.Parameters(orientationOnRobot)); - - telemetry.addData("imu calib status", "calibrated"); - telemetry.update(); - - // initialize imu global variables after calibrating imu - resetAngle(); - - } catch (IllegalArgumentException e) { - telemetry.addData("imu calib status", "failed - try again"); - telemetry.update(); - } - - - // set direction of motors - motors.leftFront.setDirection(DcMotorSimple.Direction.REVERSE); - motors.rightFront.setDirection(DcMotorSimple.Direction.FORWARD); - motors.leftBack.setDirection(DcMotorSimple.Direction.REVERSE); - motors.rightBack.setDirection(DcMotorSimple.Direction.FORWARD); - + autoMethods.setUpIMU(RevHubOrientationOnRobot.LogoFacingDirection.BACKWARD, + RevHubOrientationOnRobot.UsbFacingDirection.DOWN); // Create local variable to store selected autonomous mode // and amount of delay time @@ -238,7 +211,7 @@ public void runOpMode() throws InterruptedException { telemetry.addData("Mode", "running"); telemetry.update(); - resetEncoders(); + autoMethods.resetEncoders(); // Delay start if needed if (delayTimeMilliseconds > 0) { @@ -269,90 +242,6 @@ public void runOpMode() throws InterruptedException { } private void defaultAuto() { - driveForward(10.0, quarterPower); + drive.driveForward(10.0, AutoConstants.quarterPower); } - - - // Functions needed for driving auto - - - /** - * Function: strafeRight - *

- * This function is called to have the robot move sideways - * in a right direction - */ - private void strafeRight(double inches, double power) { - driveInches(inches, power, -power, -power, power); - } - - - // Functions for turning and checking robot angle for correction - - /* Resets the cumulative angle tracking to zero. */ - - - /** - * Get current cumulative angle rotation from last reset. - * - * @return Angle in degrees. + = left, - = right. - */ - private double getAngle() { - // We experimentally determined the Z axis is the axis we want to use for heading angle. - // We have to process the angle because the imu works in euler angles so the Z axis is - // returned as 0 to +180 or 0 to -180 rolling back to -179 or +179 when rotation passes - // 180 degrees. We detect this transition and track the total cumulative angle of rotation. - - // Orientation angles = imu.getAngularOrientation(AxesReference.INTRINSIC, AxesOrder.ZYX, AngleUnit.DEGREES); - YawPitchRollAngles orientation = imu.getRobotYawPitchRollAngles(); - - double deltaAngle = orientation.getYaw(AngleUnit.DEGREES) - lastAngles.getYaw(AngleUnit.DEGREES); - - if (deltaAngle < -180) - deltaAngle += 360; - else if (deltaAngle > 180) - deltaAngle -= 360; - - globalAngle += deltaAngle; - - lastAngles = orientation; - - return globalAngle; - } - - - /** - * Rotate left or right the number of degrees. Does not support turning more than 180 degrees. - * - * @param degrees Degrees to turn, + is left - is right - */ - private void rotate(int degrees, double power) { - double leftPower, rightPower; - - // restart imu movement tracking. - resetAngle(); - - // getAngle() returns + when rotating counter clockwise (left) and - when rotating - // clockwise (right). - if (degrees < 0) { // turn right. - leftPower = power; - rightPower = -power; - } else if (degrees > 0) { // turn left. - leftPower = -power; - rightPower = power; - } else return; - - // set power to rotate. - setDrivePower(leftPower, rightPower, leftPower, rightPower); - - // turn the motors off. - setDrivePower(0.0, 0.0, 0.0, 0.0); - - // wait for rotation to stop. - sleep(500); - - // reset angle tracking on new heading. - resetAngle(); - } - } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/AutoConstants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/AutoConstants.java index d4d782c..64ef8c4 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/AutoConstants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/AutoConstants.java @@ -8,12 +8,12 @@ public class AutoConstants { // Global Variables - protected final static double noPower = 0.0; - protected final static double quarterPower = 0.25; - protected final static double oneThirdPower = 0.34; - protected final static double halfPower = 0.5; - protected final static double threeQuartPower = 0.75; - protected final static double fullPower = 1.0; + public final static double noPower = 0.0; + public final static double quarterPower = 0.25; + public final static double oneThirdPower = 0.34; + public final static double halfPower = 0.5; + public final static double threeQuartPower = 0.75; + public final static double fullPower = 1.0; // Variables for imu diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/AutoDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/AutoDrive.java index 6e10517..b9ab4d4 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/AutoDrive.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/AutoDrive.java @@ -9,16 +9,19 @@ import com.qualcomm.robotcore.hardware.VoltageSensor; -public abstract class AutoDrive extends LinearOpMode { +public class AutoDrive { private final MotorConstructor motors; CommonAutoMethods autoMethods; + private final LinearOpMode opMode; - private AutoDrive(HardwareMap hardwareMap) { + public AutoDrive(LinearOpMode opMode, HardwareMap hardwareMap) { + this.opMode = opMode; this.motors = new MotorConstructor(hardwareMap); this.autoMethods = new CommonAutoMethods(hardwareMap); } + /** * Function: driveForward *

@@ -26,7 +29,7 @@ private AutoDrive(HardwareMap hardwareMap) { * The robot speed is passed in and that value is used for * all wheels. */ - private void driveForward(double inches, double power) { + public void driveForward(double inches, double power) { driveInches(inches, power, power, power, power); } @@ -52,6 +55,16 @@ private void strafeLeft(double inches, double power) { driveInches(inches, -power, power, power, -power); } + /** + * Function: strafeRight + *

+ * This function is called to have the robot move sideways + * in a right direction + */ + private void strafeRight(double inches, double power) { + driveInches(inches, power, -power, -power, power); + } + /** * Function: driveInches * This function is called to have the robot move straight @@ -75,7 +88,7 @@ private void driveInches(double inches, double leftFrontPower, double rightFront autoMethods.setDrivePower(leftFrontPowerCont, rightFrontPowerCont, leftBackPowerCont, rightBackPowerCont); - while (opModeIsActive() && + while (opMode.opModeIsActive() && (Math.abs(motors.leftFront.getCurrentPosition()) + Math.abs(motors.rightFront.getCurrentPosition()) / 2.0) < Math.abs(counts)) { // Use gyro to drive in a straight line. @@ -89,7 +102,7 @@ private void driveInches(double inches, double leftFrontPower, double rightFront autoMethods.setDrivePower(leftFrontPower - correction, rightFrontPower + correction, leftBackPower - correction, rightBackPower + correction); - idle(); + opMode.idle(); } autoMethods.setDrivePower(AutoConstants.noPower, AutoConstants.noPower, AutoConstants.noPower, AutoConstants.noPower); // Stop all motors diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/AutoTurn.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/AutoTurn.java index e45fc80..aad14bc 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/AutoTurn.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/AutoTurn.java @@ -3,18 +3,59 @@ import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.hardware.HardwareMap; +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles; -public abstract class AutoTurn extends LinearOpMode { +public class AutoTurn { private final MotorConstructor motors; CommonAutoMethods autoMethods; + AutoConstants autoConst; + private final LinearOpMode opMode; - private AutoTurn(HardwareMap hardwareMap) { + public AutoTurn(LinearOpMode opMode, HardwareMap hardwareMap) { this.motors = new MotorConstructor(hardwareMap); this.autoMethods = new CommonAutoMethods(hardwareMap); + this.autoConst = new AutoConstants(); + this.opMode = opMode; } - private void resetAngle() { + /** + * Rotate left or right the number of degrees. Does not support turning more than 180 degrees. + * + * @param degrees Degrees to turn, + is left - is right + */ + private void rotate(int degrees, double power) { + double leftPower, rightPower; + + // restart imu movement tracking. + resetAngle(); + + // getAngle() returns + when rotating counter clockwise (left) and - when rotating + // clockwise (right). + if (degrees < 0) { // turn right. + leftPower = power; + rightPower = -power; + } else if (degrees > 0) { // turn left. + leftPower = -power; + rightPower = power; + } else return; + + // set power to rotate. + autoMethods.setDrivePower(leftPower, rightPower, leftPower, rightPower); + + // turn the motors off. + autoMethods.setDrivePower(0.0, 0.0, 0.0, 0.0); + + // wait for rotation to stop. + opMode.sleep(500); + + // reset angle tracking on new heading. + resetAngle(); + } + + + + public void resetAngle() { motors.imu.resetYaw(); YawPitchRollAngles lastAngles = motors.imu.getRobotYawPitchRollAngles(); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/CommonAutoMethods.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/CommonAutoMethods.java index 62612f3..cea8008 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/CommonAutoMethods.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/CommonAutoMethods.java @@ -1,13 +1,25 @@ package org.firstinspires.ftc.teamcode.Utils_13233; +import static org.firstinspires.ftc.robotcore.external.BlocksOpModeCompanion.telemetry; + +import com.qualcomm.hardware.rev.RevHubOrientationOnRobot; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.HardwareMap; +import com.qualcomm.robotcore.hardware.IMU; + +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles; public class CommonAutoMethods { private final MotorConstructor motors; + AutoConstants autoConst; + AutoTurn autoTurn; - public CommonAutoMethods(HardwareMap hardwareMap) { + public CommonAutoMethods(LinearOpMode opMode, HardwareMap hardwareMap) { this.motors = new MotorConstructor(hardwareMap); + this.autoConst = new AutoConstants(); + this.autoTurn = new AutoTurn(opMode, hardwareMap); } protected void setDrivePower(double leftFrontPower, double rightFrontPower, @@ -18,7 +30,7 @@ protected void setDrivePower(double leftFrontPower, double rightFrontPower, motors.leftBack.setPower(leftBackPower); } - protected void resetEncoders() { + public void resetEncoders() { motors.rightFront.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); motors.rightBack.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); motors.leftFront.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); @@ -32,6 +44,34 @@ protected void runUsingEncoders() { motors.leftBack.setMode(DcMotor.RunMode.RUN_USING_ENCODER); } + /** + * Get current cumulative angle rotation from last reset. + * + * @return Angle in degrees. + = left, - = right. + */ + private double getAngle() { + // We experimentally determined the Z axis is the axis we want to use for heading angle. + // We have to process the angle because the imu works in euler angles so the Z axis is + // returned as 0 to +180 or 0 to -180 rolling back to -179 or +179 when rotation passes + // 180 degrees. We detect this transition and track the total cumulative angle of rotation. + + // Orientation angles = imu.getAngularOrientation(AxesReference.INTRINSIC, AxesOrder.ZYX, AngleUnit.DEGREES); + YawPitchRollAngles orientation = motors.imu.getRobotYawPitchRollAngles(); + + double deltaAngle = orientation.getYaw(AngleUnit.DEGREES) - autoConst.lastAngles.getYaw(AngleUnit.DEGREES); + + if (deltaAngle < -180) + deltaAngle += 360; + else if (deltaAngle > 180) + deltaAngle -= 360; + + autoConst.globalAngle += deltaAngle; + + autoConst.lastAngles = orientation; + + return autoConst.globalAngle; + } + /** * See if we are moving in a straight line and if not return a power correction value. * @@ -54,4 +94,25 @@ protected double checkDirection() { return correction; } + + public void setUpIMU(RevHubOrientationOnRobot.LogoFacingDirection logoDirection, + RevHubOrientationOnRobot.UsbFacingDirection usbDirection) { + try { + RevHubOrientationOnRobot orientationOnRobot = + new RevHubOrientationOnRobot(logoDirection, usbDirection); + motors.imu.initialize(new IMU.Parameters(orientationOnRobot)); + + telemetry.addData("imu calib status", "calibrated"); + telemetry.update(); + + // initialize imu global variables after calibrating imu + autoTurn.resetAngle(); + + } catch ( + IllegalArgumentException e) { + telemetry.addData("imu calib status", "failed - try again"); + telemetry.update(); + } + } } + From ce09dceb393cdd400544718d65326b950a645eee Mon Sep 17 00:00:00 2001 From: Jeffrey Mello Date: Wed, 19 Nov 2025 08:50:33 -0500 Subject: [PATCH 3/8] Refactor delay selection and add TeleOp common methods Moved SelectDelayTime from AutoMain to CommonAutoMethods for reuse and cleaner structure. Updated constructors in AutoDrive and AutoTurn to pass LinearOpMode. Added TeleOpCommonMethods class for button toggling and integrated it into TeleOpMain for improved launch control logic. --- .../firstinspires/ftc/teamcode/AutoMain.java | 87 +------------------ .../ftc/teamcode/TeleOpMain.java | 10 ++- .../ftc/teamcode/Utils_13233/AutoDrive.java | 3 +- .../ftc/teamcode/Utils_13233/AutoTurn.java | 3 +- .../Utils_13233/CommonAutoMethods.java | 81 +++++++++++++++++ .../teamcode/Utils_13233/LaunchControls.java | 1 + .../Utils_13233/TeleOpCommonMethods.java | 29 +++++++ 7 files changed, 125 insertions(+), 89 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/TeleOpCommonMethods.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoMain.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoMain.java index 40e6bb7..f325a5c 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoMain.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoMain.java @@ -29,13 +29,12 @@ @Autonomous(name = "AutoMain", group = "Auto") -public class AutoMain extends LinearOpMode{ +public class AutoMain extends LinearOpMode { private AutoDrive drive; private AutoTurn turn; private CommonAutoMethods autoMethods; - // Possible Autonomous Modes public enum AutoMode { AUTO_MODE_NOT_SELECTED, @@ -86,91 +85,12 @@ private AutoMode SelectAutoMode() { } // end SelectAutoMode - /** - * Function: SelectDelayTime - *

- * This function is use to select how long to delay the start of the autonomous code. - * Game pad 1 is used and the following controls are used for selection: - * left bumper - decrease delay time by 1000 milliseconds (1 second) - * right bumper - increase delay time by 1000 milliseconds (1 second) - * a button - set selected time - * note: if no delay time is needed, just select the a button. The default for the delay time - * is 0. - * - * @return Delay Time in milliseconds - */ - private Integer SelectDelayTime() { - int delayTimeMilliseconds = 0; // Initialize delay to be 0 seconds - - // display delay time not set - telemetry.addData("Delay", "%d (Not Set)", delayTimeMilliseconds); - telemetry.addData("Left Bumper -", " decrease delay time by (1 second)"); - telemetry.addData("Right bumper -", " increase delay time by (1 second)"); - telemetry.addData("A", "No delay time is needed"); - telemetry.update(); - - - /* Select Delay time. - - Select 'a' button without hitting bumpers if no delay needed - - Use Left Bumper to decrease delay time - - Use Right bumper to increase delay time - - Note: After entering delay time, use "a" button to set selected time - */ - - while (!isStopRequested() && !gamepad1.a) { - if (gamepad1.left_bumper) { - delayTimeMilliseconds -= 1000; - - // ensure delay time does not go negative - if (delayTimeMilliseconds < 0) { - delayTimeMilliseconds = 0; - } - - // Wait for the bumper to be released - while (gamepad1.left_bumper) { - idle(); - } - - telemetry.addData("Delay", "%d (decrease)", delayTimeMilliseconds); - telemetry.update(); - } - - if (gamepad1.right_bumper) { - delayTimeMilliseconds += 1000; - - // ensure delay time is not greater than 10 seconds - if (delayTimeMilliseconds > 10000) { - delayTimeMilliseconds = 10000; - } - - while (gamepad1.right_bumper) { - idle(); - } - telemetry.addData("Delay", "%d (increase)", delayTimeMilliseconds); - telemetry.update(); - } - } - - // Wait for user to release the a button - while (!isStopRequested() && gamepad1.a) { - idle(); - } - - - // Display selected delay time - telemetry.addData("Delay Time SET", delayTimeMilliseconds); - telemetry.update(); - return delayTimeMilliseconds; // returns selected delay time - - } // end SelectDelayTime - // OpMode for autonomous code @Override public void runOpMode() throws InterruptedException { drive = new AutoDrive(this, hardwareMap); - turn = new AutoTurn(this, hardwareMap); + turn = new AutoTurn(this, hardwareMap); autoMethods = new CommonAutoMethods(this, hardwareMap); double tgtPower = 0; @@ -193,7 +113,7 @@ public void runOpMode() throws InterruptedException { autoMode = SelectAutoMode(); // parkLocation = SelectParkLoc(); - delayTimeMilliseconds = SelectDelayTime(); + delayTimeMilliseconds = autoMethods.SelectDelayTime(); /* All required data entered. Autonomous is initialized. */ @@ -225,7 +145,6 @@ public void runOpMode() throws InterruptedException { // display delay over and autonomous code is running telemetry.addData("Status", "Running"); telemetry.update(); - } telemetry.clearAll(); // clear display messages diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOpMain.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOpMain.java index f183239..64c0962 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOpMain.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOpMain.java @@ -1,6 +1,8 @@ // Import libraries package org.firstinspires.ftc.teamcode; +import static org.firstinspires.ftc.robotcore.external.BlocksOpModeCompanion.opMode; + import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import com.qualcomm.robotcore.hardware.HardwareMap; @@ -9,6 +11,7 @@ import org.firstinspires.ftc.teamcode.Utils_13233.MotorConstructor; import org.firstinspires.ftc.teamcode.Utils_13233.RampControls; import org.firstinspires.ftc.teamcode.Utils_13233.LaunchControls; +import org.firstinspires.ftc.teamcode.Utils_13233.TeleOpCommonMethods; @TeleOp(name = "TeleOpMain", group = "Drive") @@ -18,6 +21,7 @@ public class TeleOpMain extends LinearOpMode { private DriveControls drive; private LaunchControls launch; private RampControls ramp; + private TeleOpCommonMethods commonMethods; //This function is executed when this Op Mode is selected from the Driver Station @@ -27,6 +31,8 @@ public void runOpMode() { drive = new DriveControls(hardwareMap); launch = new LaunchControls(hardwareMap); ramp = new RampControls(hardwareMap); + commonMethods = new TeleOpCommonMethods(this); + // The waitForStart() function will wait for the start button to begin // DON'T WRITE ANY CODE AFTER THE WAIT FOR START UNTIL THE "while (opModIsActive())" @@ -39,7 +45,9 @@ public void runOpMode() { telemetry.update(); // Set the power to the launch motors based while the x button is being pressed - launch.setLaunchPower(gamepad1.x); + + launch.setLaunchPower(commonMethods.toggleButton("launch", gamepad1.x, + false)); //Add option to enable brakes when sharbell holds a drive.setDriveMotorZeroPowerBehavior(gamepad1.a); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/AutoDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/AutoDrive.java index b9ab4d4..8056298 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/AutoDrive.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/AutoDrive.java @@ -17,11 +17,10 @@ public class AutoDrive { public AutoDrive(LinearOpMode opMode, HardwareMap hardwareMap) { this.opMode = opMode; this.motors = new MotorConstructor(hardwareMap); - this.autoMethods = new CommonAutoMethods(hardwareMap); + this.autoMethods = new CommonAutoMethods(opMode, hardwareMap); } - /** * Function: driveForward *

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/AutoTurn.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/AutoTurn.java index aad14bc..8d7ef89 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/AutoTurn.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/AutoTurn.java @@ -14,7 +14,7 @@ public class AutoTurn { public AutoTurn(LinearOpMode opMode, HardwareMap hardwareMap) { this.motors = new MotorConstructor(hardwareMap); - this.autoMethods = new CommonAutoMethods(hardwareMap); + this.autoMethods = new CommonAutoMethods(opMode, hardwareMap); this.autoConst = new AutoConstants(); this.opMode = opMode; } @@ -54,7 +54,6 @@ private void rotate(int degrees, double power) { } - public void resetAngle() { motors.imu.resetYaw(); YawPitchRollAngles lastAngles = motors.imu.getRobotYawPitchRollAngles(); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/CommonAutoMethods.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/CommonAutoMethods.java index cea8008..3517726 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/CommonAutoMethods.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/CommonAutoMethods.java @@ -15,11 +15,13 @@ public class CommonAutoMethods { private final MotorConstructor motors; AutoConstants autoConst; AutoTurn autoTurn; + LinearOpMode opMode; public CommonAutoMethods(LinearOpMode opMode, HardwareMap hardwareMap) { this.motors = new MotorConstructor(hardwareMap); this.autoConst = new AutoConstants(); this.autoTurn = new AutoTurn(opMode, hardwareMap); + this.opMode = opMode; } protected void setDrivePower(double leftFrontPower, double rightFrontPower, @@ -114,5 +116,84 @@ public void setUpIMU(RevHubOrientationOnRobot.LogoFacingDirection logoDirection, telemetry.update(); } } + + /** + * Function: SelectDelayTime + *

+ * This function is use to select how long to delay the start of the autonomous code. + * Game pad 1 is used and the following controls are used for selection: + * left bumper - decrease delay time by 1000 milliseconds (1 second) + * right bumper - increase delay time by 1000 milliseconds (1 second) + * a button - set selected time + * note: if no delay time is needed, just select the a button. The default for the delay time + * is 0. + * + * @return Delay Time in milliseconds + */ + public Integer SelectDelayTime() { + int delayTimeMilliseconds = 0; // Initialize delay to be 0 seconds + + // display delay time not set + telemetry.addData("Delay", "%d (Not Set)", delayTimeMilliseconds); + telemetry.addData("Left Bumper -", " decrease delay time by (1 second)"); + telemetry.addData("Right bumper -", " increase delay time by (1 second)"); + telemetry.addData("A", "No delay time is needed"); + telemetry.update(); + + + /* Select Delay time. + - Select 'a' button without hitting bumpers if no delay needed + - Use Left Bumper to decrease delay time + - Use Right bumper to increase delay time + + Note: After entering delay time, use "a" button to set selected time + */ + + while (!opMode.isStopRequested() && !opMode.gamepad1.a) { + if (opMode.gamepad1.left_bumper) { + delayTimeMilliseconds -= 1000; + + // ensure delay time does not go negative + if (delayTimeMilliseconds < 0) { + delayTimeMilliseconds = 0; + } + + // Wait for the bumper to be released + while (opMode.gamepad1.left_bumper) { + opMode.idle(); + } + + telemetry.addData("Delay", "%d (decrease)", delayTimeMilliseconds); + telemetry.update(); + } + + if (opMode.gamepad1.right_bumper) { + delayTimeMilliseconds += 1000; + + // ensure delay time is not greater than 10 seconds + if (delayTimeMilliseconds > 10000) { + delayTimeMilliseconds = 10000; + } + + while (opMode.gamepad1.right_bumper) { + opMode.idle(); + } + telemetry.addData("Delay", "%d (increase)", delayTimeMilliseconds); + telemetry.update(); + } + } + + // Wait for user to release the a button + while (!opMode.isStopRequested() && opMode.gamepad1.a) { + opMode.idle(); + } + + + // Display selected delay time + telemetry.addData("Delay Time SET", delayTimeMilliseconds); + telemetry.update(); + return delayTimeMilliseconds; // returns selected delay time + + } // end SelectDelayTime } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/LaunchControls.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/LaunchControls.java index 36f4da2..b5a5533 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/LaunchControls.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/LaunchControls.java @@ -21,4 +21,5 @@ public void setLaunchPower(boolean launchInput) { motors.Launcher.setPower(-power); motors.Launcher2.setPower(power); } + } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/TeleOpCommonMethods.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/TeleOpCommonMethods.java new file mode 100644 index 0000000..94f4c21 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/TeleOpCommonMethods.java @@ -0,0 +1,29 @@ +package org.firstinspires.ftc.teamcode.Utils_13233; + +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.hardware.HardwareMap; + +import java.util.HashMap; +import java.util.Map; + +public class TeleOpCommonMethods { + public Map prev = new HashMap<>(); + LinearOpMode opMode; + + public TeleOpCommonMethods(LinearOpMode opMode) { + this.opMode = opMode; + } + + + public boolean toggleButton(String name, boolean button, boolean toggle) { + boolean previous = prev.getOrDefault(name, false); + + if (button && !previous) { + toggle = !toggle; + } + + prev.put(name, button); + return toggle; + } + +} From 0faea3213bf125c750683f58de787c5c1f8ddda0 Mon Sep 17 00:00:00 2001 From: Jeffrey Mello Date: Tue, 18 Nov 2025 15:40:28 -0500 Subject: [PATCH 4/8] Refactor autonomous code into modular utility classes Moved constants, drive, turn, and common auto methods from AutoMain into new utility classes (AutoConstants, AutoDrive, AutoTurn, CommonAutoMethods) for better code organization and reuse. Updated MotorConstructor to include IMU and VoltageSensor initialization. Simplified AutoMain by removing duplicated logic and delegating hardware and movement control to utility classes. --- .../firstinspires/ftc/teamcode/AutoMain.java | 165 ------------------ .../teamcode/Utils_13233/AutoConstants.java | 34 ++++ .../ftc/teamcode/Utils_13233/AutoDrive.java | 97 ++++++++++ .../ftc/teamcode/Utils_13233/AutoTurn.java | 23 +++ .../Utils_13233/CommonAutoMethods.java | 57 ++++++ .../Utils_13233/MotorConstructor.java | 10 ++ 6 files changed, 221 insertions(+), 165 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/AutoConstants.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/AutoDrive.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/AutoTurn.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/CommonAutoMethods.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoMain.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoMain.java index 8c732b7..c80b218 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoMain.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoMain.java @@ -26,23 +26,6 @@ @Autonomous(name = "AutoMain", group = "Auto") public class AutoMain extends LinearOpMode { - private final MotorConstructor motors; - - public AutoMain(HardwareMap hardwareMap) { - this.motors = new MotorConstructor(hardwareMap); - } - - private IMU imu; - - private VoltageSensor VoltSens; - - - // Variables for imu - double globalAngle, correction; - YawPitchRollAngles lastAngles; - - double voltage = 0; - double batteryConst = 13.5; // Possible Autonomous Modes @@ -52,24 +35,6 @@ public enum AutoMode { } - // Variables for driving with Encoders - private static final double wheelCircumference = 4.0 * Math.PI; - private static final double gearRatio = 18.9; // Rev HD Hex 20:1 - private static final double countsPerRotation = 560.0; // Rev HD Hex 20:1 - private static final double scaleFactor = 9.0; // need to find scale factor!!! - private static final double countsPerInch = countsPerRotation / wheelCircumference - / gearRatio * scaleFactor; - - - // Global Variables - private final static double noPower = 0.0; - private final static double quarterPower = 0.25; - private final static double oneThirdPower = 0.34; - private final static double halfPower = 0.5; - private final static double threeQuartPower = 0.75; - private final static double fullPower = 1.0; - - // Global Variables to store Game Specific Information AutoMode autoMode = AutoMode.AUTO_MODE_NOT_SELECTED; // store autonomous mode selected @@ -199,16 +164,11 @@ public void runOpMode() throws InterruptedException { double tgtPower = 0; - VoltSens = hardwareMap.voltageSensor.get("Control Hub"); - /* Setup IMU parameters */ - // Retrieve and initialize the IMU. The IMU should be attached to - // IC2 port 0 on a Core Device Interface Module - imu = hardwareMap.get(IMU.class, "imu"); /* The next two lines define Hub orientation. * The Default Orientation (shown) is when a hub is mounted horizontally with the printed @@ -314,27 +274,7 @@ private void defaultAuto() { // Functions needed for driving auto - private void resetEncoders() { - motors.rightFront.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); - motors.rightBack.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); - motors.leftFront.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); - motors.leftBack.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); - } - private void runUsingEncoders() { - motors.rightFront.setMode(DcMotor.RunMode.RUN_USING_ENCODER); - motors.rightBack.setMode(DcMotor.RunMode.RUN_USING_ENCODER); - motors.leftFront.setMode(DcMotor.RunMode.RUN_USING_ENCODER); - motors.leftBack.setMode(DcMotor.RunMode.RUN_USING_ENCODER); - } - - private void setDrivePower(double leftFrontPower, double rightFrontPower, - double leftBackPower, double rightBackPower) { - motors.rightFront.setPower(rightFrontPower); - motors.rightBack.setPower(rightBackPower); - motors.leftFront.setPower(leftFrontPower); - motors.leftBack.setPower(leftBackPower); - } /** * Function: strafeRight @@ -346,94 +286,11 @@ private void strafeRight(double inches, double power) { driveInches(inches, power, -power, -power, power); } - /** - * Function: strafeLeft - *

- * This function is called to have the robot move sideways - * in a left direction - */ - private void strafeLeft(double inches, double power) { - driveInches(inches, -power, power, power, -power); - } - - - /** - * Function: driveForward - *

- * This function is called to have the robot move forward. - * The robot speed is passed in and that value is used for - * all wheels. - */ - private void driveForward(double inches, double power) { - driveInches(inches, power, power, power, power); - } - - - /** - * Function: driveBackward - *

- * This function is called to have the robot move in reverse. - * The robot speed is passed in and that value is used for - * all wheels. - */ - private void driveBackward(double inches, double power) { - driveInches(inches, -power, -power, -power, -power); - } - - - /** - * Function: driveInches - * This function is called to have the robot move straight - * in a forward or reverse direction. - *

- * Strafe Forward = negative front wheels, positive back - * wheels - */ - private void driveInches(double inches, double leftFrontPower, double rightFrontPower, double leftBackPower, double rightBackPower) { - double counts = inches * countsPerInch; - - resetEncoders(); - runUsingEncoders(); - - voltage = VoltSens.getVoltage(); // read current battery voltage - - double leftFrontPowerCont = ((batteryConst * leftFrontPower) / voltage); - double rightFrontPowerCont = ((batteryConst * rightFrontPower) / voltage); - double leftBackPowerCont = ((batteryConst * leftBackPower) / voltage); - double rightBackPowerCont = ((batteryConst * rightBackPower) / voltage); - - setDrivePower(leftFrontPowerCont, rightFrontPowerCont, leftBackPowerCont, rightBackPowerCont); - - while (opModeIsActive() && - (Math.abs(motors.leftFront.getCurrentPosition()) + - Math.abs(motors.rightFront.getCurrentPosition()) / 2.0) < Math.abs(counts)) { - // Use gyro to drive in a straight line. - correction = checkDirection(); - - // telemetry.addData("1 imu heading", lastAngles.firstAngle); - // telemetry.addData("2 global heading", globalAngle); - // telemetry.addData("3 correction", correction); - // telemetry.update(); - - setDrivePower(leftFrontPower - correction, - rightFrontPower + correction, leftBackPower - correction, - rightBackPower + correction); - idle(); - } - - setDrivePower(noPower, noPower, noPower, noPower); // Stop all motors - } // Functions for turning and checking robot angle for correction /* Resets the cumulative angle tracking to zero. */ - private void resetAngle() { - imu.resetYaw(); - lastAngles = imu.getRobotYawPitchRollAngles(); - - globalAngle = 0; - } /** * Get current cumulative angle rotation from last reset. @@ -463,28 +320,6 @@ else if (deltaAngle > 180) return globalAngle; } - /** - * See if we are moving in a straight line and if not return a power correction value. - * - * @return Power adjustment, + is adjust left - is adjust right. - */ - private double checkDirection() { - // The gain value determines how sensitive the correction is to direction changes. - // You will have to experiment with your robot to get small smooth direction changes - // to stay on a straight line. - double correction, angle, gain = .05; - - angle = getAngle(); - - if (angle == 0) - correction = 0; // no adjustment. - else - correction = -angle; // reverse sign of angle for correction. - - correction = correction * gain; - - return correction; - } /** * Rotate left or right the number of degrees. Does not support turning more than 180 degrees. diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/AutoConstants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/AutoConstants.java new file mode 100644 index 0000000..d4d782c --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/AutoConstants.java @@ -0,0 +1,34 @@ +package org.firstinspires.ftc.teamcode.Utils_13233; + +import com.qualcomm.robotcore.hardware.HardwareMap; +import com.qualcomm.robotcore.hardware.IMU; +import com.qualcomm.robotcore.hardware.VoltageSensor; + +import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles; + +public class AutoConstants { + // Global Variables + protected final static double noPower = 0.0; + protected final static double quarterPower = 0.25; + protected final static double oneThirdPower = 0.34; + protected final static double halfPower = 0.5; + protected final static double threeQuartPower = 0.75; + protected final static double fullPower = 1.0; + + + // Variables for imu + protected double globalAngle, correction; + protected YawPitchRollAngles lastAngles; + + protected double voltage = 0; + protected static double batteryConst = 13.5; + + // Variables for driving with Encoders + protected static final double wheelCircumference = 4.0 * Math.PI; + protected static final double gearRatio = 18.9; // Rev HD Hex 20:1 + protected static final double countsPerRotation = 560.0; // Rev HD Hex 20:1 + protected static final double scaleFactor = 9.0; // need to find scale factor!!! + protected static final double countsPerInch = countsPerRotation / wheelCircumference + / gearRatio * scaleFactor; + +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/AutoDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/AutoDrive.java new file mode 100644 index 0000000..6e10517 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/AutoDrive.java @@ -0,0 +1,97 @@ +package org.firstinspires.ftc.teamcode.Utils_13233; + + +import static org.firstinspires.ftc.robotcore.external.BlocksOpModeCompanion.hardwareMap; + +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; + +import com.qualcomm.robotcore.hardware.HardwareMap; +import com.qualcomm.robotcore.hardware.VoltageSensor; + + +public abstract class AutoDrive extends LinearOpMode { + private final MotorConstructor motors; + CommonAutoMethods autoMethods; + + private AutoDrive(HardwareMap hardwareMap) { + this.motors = new MotorConstructor(hardwareMap); + this.autoMethods = new CommonAutoMethods(hardwareMap); + } + + + /** + * Function: driveForward + *

+ * This function is called to have the robot move forward. + * The robot speed is passed in and that value is used for + * all wheels. + */ + private void driveForward(double inches, double power) { + driveInches(inches, power, power, power, power); + } + + + /** + * Function: driveBackward + *

+ * This function is called to have the robot move in reverse. + * The robot speed is passed in and that value is used for + * all wheels. + */ + private void driveBackward(double inches, double power) { + driveInches(inches, -power, -power, -power, -power); + } + + /** + * Function: strafeLeft + *

+ * This function is called to have the robot move sideways + * in a left direction + */ + private void strafeLeft(double inches, double power) { + driveInches(inches, -power, power, power, -power); + } + + /** + * Function: driveInches + * This function is called to have the robot move straight + * in a forward or reverse direction. + *

+ * Strafe Forward = negative front wheels, positive back + * wheels + */ + private void driveInches(double inches, double leftFrontPower, double rightFrontPower, double leftBackPower, double rightBackPower) { + double counts = inches * AutoConstants.countsPerInch; + + autoMethods.resetEncoders(); + autoMethods.runUsingEncoders(); + + double voltage = motors.VoltSens.getVoltage(); // read current battery voltage + + double leftFrontPowerCont = ((AutoConstants.batteryConst * leftFrontPower) / voltage); + double rightFrontPowerCont = ((AutoConstants.batteryConst * rightFrontPower) / voltage); + double leftBackPowerCont = ((AutoConstants.batteryConst * leftBackPower) / voltage); + double rightBackPowerCont = ((AutoConstants.batteryConst * rightBackPower) / voltage); + + autoMethods.setDrivePower(leftFrontPowerCont, rightFrontPowerCont, leftBackPowerCont, rightBackPowerCont); + + while (opModeIsActive() && + (Math.abs(motors.leftFront.getCurrentPosition()) + + Math.abs(motors.rightFront.getCurrentPosition()) / 2.0) < Math.abs(counts)) { + // Use gyro to drive in a straight line. + double correction = autoMethods.checkDirection(); + + // telemetry.addData("1 imu heading", lastAngles.firstAngle); + // telemetry.addData("2 global heading", globalAngle); + // telemetry.addData("3 correction", correction); + // telemetry.update(); + + autoMethods.setDrivePower(leftFrontPower - correction, + rightFrontPower + correction, leftBackPower - correction, + rightBackPower + correction); + idle(); + } + + autoMethods.setDrivePower(AutoConstants.noPower, AutoConstants.noPower, AutoConstants.noPower, AutoConstants.noPower); // Stop all motors + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/AutoTurn.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/AutoTurn.java new file mode 100644 index 0000000..e45fc80 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/AutoTurn.java @@ -0,0 +1,23 @@ +package org.firstinspires.ftc.teamcode.Utils_13233; + +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.hardware.HardwareMap; + +import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles; + +public abstract class AutoTurn extends LinearOpMode { + private final MotorConstructor motors; + CommonAutoMethods autoMethods; + + private AutoTurn(HardwareMap hardwareMap) { + this.motors = new MotorConstructor(hardwareMap); + this.autoMethods = new CommonAutoMethods(hardwareMap); + } + + private void resetAngle() { + motors.imu.resetYaw(); + YawPitchRollAngles lastAngles = motors.imu.getRobotYawPitchRollAngles(); + + int globalAngle = 0; + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/CommonAutoMethods.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/CommonAutoMethods.java new file mode 100644 index 0000000..62612f3 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/CommonAutoMethods.java @@ -0,0 +1,57 @@ +package org.firstinspires.ftc.teamcode.Utils_13233; + +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.HardwareMap; + +public class CommonAutoMethods { + private final MotorConstructor motors; + + public CommonAutoMethods(HardwareMap hardwareMap) { + this.motors = new MotorConstructor(hardwareMap); + } + + protected void setDrivePower(double leftFrontPower, double rightFrontPower, + double leftBackPower, double rightBackPower) { + motors.rightFront.setPower(rightFrontPower); + motors.rightBack.setPower(rightBackPower); + motors.leftFront.setPower(leftFrontPower); + motors.leftBack.setPower(leftBackPower); + } + + protected void resetEncoders() { + motors.rightFront.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + motors.rightBack.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + motors.leftFront.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + motors.leftBack.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + } + + protected void runUsingEncoders() { + motors.rightFront.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + motors.rightBack.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + motors.leftFront.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + motors.leftBack.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + } + + /** + * See if we are moving in a straight line and if not return a power correction value. + * + * @return Power adjustment, + is adjust left - is adjust right. + */ + protected double checkDirection() { + // The gain value determines how sensitive the correction is to direction changes. + // You will have to experiment with your robot to get small smooth direction changes + // to stay on a straight line. + double correction, angle, gain = .05; + + angle = getAngle(); + + if (angle == 0) + correction = 0; // no adjustment. + else + correction = -angle; // reverse sign of angle for correction. + + correction = correction * gain; + + return correction; + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/MotorConstructor.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/MotorConstructor.java index ff7e62e..7899dcd 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/MotorConstructor.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/MotorConstructor.java @@ -4,6 +4,8 @@ import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.DcMotorSimple; import com.qualcomm.robotcore.hardware.HardwareMap; +import com.qualcomm.robotcore.hardware.IMU; +import com.qualcomm.robotcore.hardware.VoltageSensor; /** * Class holds motors and servos that are used throughout the code @@ -53,6 +55,9 @@ public class MotorConstructor { public CRServo rampServo3; public CRServo rampServo4; + VoltageSensor VoltSens; + protected IMU imu; + public MotorConstructor(HardwareMap hardwareMap) { // Map main Drive Motors leftFront = hardwareMap.get(DcMotor.class, "leftFront"); @@ -72,6 +77,11 @@ public MotorConstructor(HardwareMap hardwareMap) { rampServo3 = hardwareMap.get(CRServo.class, "rampServo3"); rampServo4 = hardwareMap.get(CRServo.class, "rampServo4"); + VoltSens = hardwareMap.voltageSensor.get("Control Hub"); + + // IC2 port 0 on a Core Device Interface Module + imu = hardwareMap.get(IMU.class, "imu"); + // Set direction of the main drive motors leftFront.setDirection(DcMotorSimple.Direction.FORWARD); rightFront.setDirection(DcMotorSimple.Direction.REVERSE); From 8965cbbc133ff98151e6cb76f2ea395f33502908 Mon Sep 17 00:00:00 2001 From: Jeffrey Mello Date: Tue, 18 Nov 2025 21:19:48 -0500 Subject: [PATCH 5/8] Refactor autonomous code into modular classes Moved drive, turn, and common autonomous methods from AutoMain into separate utility classes (AutoDrive, AutoTurn, CommonAutoMethods) for better modularity and maintainability. Updated AutoConstants to use public constants. Adjusted method access and signatures to support new structure and improved IMU setup and encoder reset handling. --- .../firstinspires/ftc/teamcode/AutoMain.java | 143 ++---------------- .../teamcode/Utils_13233/AutoConstants.java | 12 +- .../ftc/teamcode/Utils_13233/AutoDrive.java | 23 ++- .../ftc/teamcode/Utils_13233/AutoTurn.java | 47 +++++- .../Utils_13233/CommonAutoMethods.java | 65 +++++++- 5 files changed, 147 insertions(+), 143 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoMain.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoMain.java index c80b218..40e6bb7 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoMain.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoMain.java @@ -20,12 +20,20 @@ // import for IMU (gyroscope) import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles; +import org.firstinspires.ftc.teamcode.Utils_13233.AutoConstants; +import org.firstinspires.ftc.teamcode.Utils_13233.AutoDrive; +import org.firstinspires.ftc.teamcode.Utils_13233.AutoTurn; +import org.firstinspires.ftc.teamcode.Utils_13233.CommonAutoMethods; import org.firstinspires.ftc.teamcode.Utils_13233.MotorConstructor; @Autonomous(name = "AutoMain", group = "Auto") -public class AutoMain extends LinearOpMode { +public class AutoMain extends LinearOpMode{ + private AutoDrive drive; + private AutoTurn turn; + private CommonAutoMethods autoMethods; + // Possible Autonomous Modes @@ -161,52 +169,17 @@ private Integer SelectDelayTime() { // OpMode for autonomous code @Override public void runOpMode() throws InterruptedException { + drive = new AutoDrive(this, hardwareMap); + turn = new AutoTurn(this, hardwareMap); + autoMethods = new CommonAutoMethods(this, hardwareMap); double tgtPower = 0; - - - - - - /* The next two lines define Hub orientation. - * The Default Orientation (shown) is when a hub is mounted horizontally with the printed - * logo pointing UP and the USB port pointing FORWARD. - * - * To Do: EDIT these two lines to match YOUR mounting configuration. - */ - RevHubOrientationOnRobot.LogoFacingDirection logoDirection - = RevHubOrientationOnRobot.LogoFacingDirection.BACKWARD; - RevHubOrientationOnRobot.UsbFacingDirection usbDirection - = RevHubOrientationOnRobot.UsbFacingDirection.DOWN; - telemetry.addData("Mode", "calibrating imu...."); telemetry.update(); - - try { - RevHubOrientationOnRobot orientationOnRobot = - new RevHubOrientationOnRobot(logoDirection, usbDirection); - imu.initialize(new IMU.Parameters(orientationOnRobot)); - - telemetry.addData("imu calib status", "calibrated"); - telemetry.update(); - - // initialize imu global variables after calibrating imu - resetAngle(); - - } catch (IllegalArgumentException e) { - telemetry.addData("imu calib status", "failed - try again"); - telemetry.update(); - } - - - // set direction of motors - motors.leftFront.setDirection(DcMotorSimple.Direction.REVERSE); - motors.rightFront.setDirection(DcMotorSimple.Direction.FORWARD); - motors.leftBack.setDirection(DcMotorSimple.Direction.REVERSE); - motors.rightBack.setDirection(DcMotorSimple.Direction.FORWARD); - + autoMethods.setUpIMU(RevHubOrientationOnRobot.LogoFacingDirection.BACKWARD, + RevHubOrientationOnRobot.UsbFacingDirection.DOWN); // Create local variable to store selected autonomous mode // and amount of delay time @@ -238,7 +211,7 @@ public void runOpMode() throws InterruptedException { telemetry.addData("Mode", "running"); telemetry.update(); - resetEncoders(); + autoMethods.resetEncoders(); // Delay start if needed if (delayTimeMilliseconds > 0) { @@ -269,90 +242,6 @@ public void runOpMode() throws InterruptedException { } private void defaultAuto() { - driveForward(10.0, quarterPower); + drive.driveForward(10.0, AutoConstants.quarterPower); } - - - // Functions needed for driving auto - - - /** - * Function: strafeRight - *

- * This function is called to have the robot move sideways - * in a right direction - */ - private void strafeRight(double inches, double power) { - driveInches(inches, power, -power, -power, power); - } - - - // Functions for turning and checking robot angle for correction - - /* Resets the cumulative angle tracking to zero. */ - - - /** - * Get current cumulative angle rotation from last reset. - * - * @return Angle in degrees. + = left, - = right. - */ - private double getAngle() { - // We experimentally determined the Z axis is the axis we want to use for heading angle. - // We have to process the angle because the imu works in euler angles so the Z axis is - // returned as 0 to +180 or 0 to -180 rolling back to -179 or +179 when rotation passes - // 180 degrees. We detect this transition and track the total cumulative angle of rotation. - - // Orientation angles = imu.getAngularOrientation(AxesReference.INTRINSIC, AxesOrder.ZYX, AngleUnit.DEGREES); - YawPitchRollAngles orientation = imu.getRobotYawPitchRollAngles(); - - double deltaAngle = orientation.getYaw(AngleUnit.DEGREES) - lastAngles.getYaw(AngleUnit.DEGREES); - - if (deltaAngle < -180) - deltaAngle += 360; - else if (deltaAngle > 180) - deltaAngle -= 360; - - globalAngle += deltaAngle; - - lastAngles = orientation; - - return globalAngle; - } - - - /** - * Rotate left or right the number of degrees. Does not support turning more than 180 degrees. - * - * @param degrees Degrees to turn, + is left - is right - */ - private void rotate(int degrees, double power) { - double leftPower, rightPower; - - // restart imu movement tracking. - resetAngle(); - - // getAngle() returns + when rotating counter clockwise (left) and - when rotating - // clockwise (right). - if (degrees < 0) { // turn right. - leftPower = power; - rightPower = -power; - } else if (degrees > 0) { // turn left. - leftPower = -power; - rightPower = power; - } else return; - - // set power to rotate. - setDrivePower(leftPower, rightPower, leftPower, rightPower); - - // turn the motors off. - setDrivePower(0.0, 0.0, 0.0, 0.0); - - // wait for rotation to stop. - sleep(500); - - // reset angle tracking on new heading. - resetAngle(); - } - } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/AutoConstants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/AutoConstants.java index d4d782c..64ef8c4 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/AutoConstants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/AutoConstants.java @@ -8,12 +8,12 @@ public class AutoConstants { // Global Variables - protected final static double noPower = 0.0; - protected final static double quarterPower = 0.25; - protected final static double oneThirdPower = 0.34; - protected final static double halfPower = 0.5; - protected final static double threeQuartPower = 0.75; - protected final static double fullPower = 1.0; + public final static double noPower = 0.0; + public final static double quarterPower = 0.25; + public final static double oneThirdPower = 0.34; + public final static double halfPower = 0.5; + public final static double threeQuartPower = 0.75; + public final static double fullPower = 1.0; // Variables for imu diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/AutoDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/AutoDrive.java index 6e10517..b9ab4d4 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/AutoDrive.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/AutoDrive.java @@ -9,16 +9,19 @@ import com.qualcomm.robotcore.hardware.VoltageSensor; -public abstract class AutoDrive extends LinearOpMode { +public class AutoDrive { private final MotorConstructor motors; CommonAutoMethods autoMethods; + private final LinearOpMode opMode; - private AutoDrive(HardwareMap hardwareMap) { + public AutoDrive(LinearOpMode opMode, HardwareMap hardwareMap) { + this.opMode = opMode; this.motors = new MotorConstructor(hardwareMap); this.autoMethods = new CommonAutoMethods(hardwareMap); } + /** * Function: driveForward *

@@ -26,7 +29,7 @@ private AutoDrive(HardwareMap hardwareMap) { * The robot speed is passed in and that value is used for * all wheels. */ - private void driveForward(double inches, double power) { + public void driveForward(double inches, double power) { driveInches(inches, power, power, power, power); } @@ -52,6 +55,16 @@ private void strafeLeft(double inches, double power) { driveInches(inches, -power, power, power, -power); } + /** + * Function: strafeRight + *

+ * This function is called to have the robot move sideways + * in a right direction + */ + private void strafeRight(double inches, double power) { + driveInches(inches, power, -power, -power, power); + } + /** * Function: driveInches * This function is called to have the robot move straight @@ -75,7 +88,7 @@ private void driveInches(double inches, double leftFrontPower, double rightFront autoMethods.setDrivePower(leftFrontPowerCont, rightFrontPowerCont, leftBackPowerCont, rightBackPowerCont); - while (opModeIsActive() && + while (opMode.opModeIsActive() && (Math.abs(motors.leftFront.getCurrentPosition()) + Math.abs(motors.rightFront.getCurrentPosition()) / 2.0) < Math.abs(counts)) { // Use gyro to drive in a straight line. @@ -89,7 +102,7 @@ private void driveInches(double inches, double leftFrontPower, double rightFront autoMethods.setDrivePower(leftFrontPower - correction, rightFrontPower + correction, leftBackPower - correction, rightBackPower + correction); - idle(); + opMode.idle(); } autoMethods.setDrivePower(AutoConstants.noPower, AutoConstants.noPower, AutoConstants.noPower, AutoConstants.noPower); // Stop all motors diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/AutoTurn.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/AutoTurn.java index e45fc80..aad14bc 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/AutoTurn.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/AutoTurn.java @@ -3,18 +3,59 @@ import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.hardware.HardwareMap; +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles; -public abstract class AutoTurn extends LinearOpMode { +public class AutoTurn { private final MotorConstructor motors; CommonAutoMethods autoMethods; + AutoConstants autoConst; + private final LinearOpMode opMode; - private AutoTurn(HardwareMap hardwareMap) { + public AutoTurn(LinearOpMode opMode, HardwareMap hardwareMap) { this.motors = new MotorConstructor(hardwareMap); this.autoMethods = new CommonAutoMethods(hardwareMap); + this.autoConst = new AutoConstants(); + this.opMode = opMode; } - private void resetAngle() { + /** + * Rotate left or right the number of degrees. Does not support turning more than 180 degrees. + * + * @param degrees Degrees to turn, + is left - is right + */ + private void rotate(int degrees, double power) { + double leftPower, rightPower; + + // restart imu movement tracking. + resetAngle(); + + // getAngle() returns + when rotating counter clockwise (left) and - when rotating + // clockwise (right). + if (degrees < 0) { // turn right. + leftPower = power; + rightPower = -power; + } else if (degrees > 0) { // turn left. + leftPower = -power; + rightPower = power; + } else return; + + // set power to rotate. + autoMethods.setDrivePower(leftPower, rightPower, leftPower, rightPower); + + // turn the motors off. + autoMethods.setDrivePower(0.0, 0.0, 0.0, 0.0); + + // wait for rotation to stop. + opMode.sleep(500); + + // reset angle tracking on new heading. + resetAngle(); + } + + + + public void resetAngle() { motors.imu.resetYaw(); YawPitchRollAngles lastAngles = motors.imu.getRobotYawPitchRollAngles(); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/CommonAutoMethods.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/CommonAutoMethods.java index 62612f3..cea8008 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/CommonAutoMethods.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/CommonAutoMethods.java @@ -1,13 +1,25 @@ package org.firstinspires.ftc.teamcode.Utils_13233; +import static org.firstinspires.ftc.robotcore.external.BlocksOpModeCompanion.telemetry; + +import com.qualcomm.hardware.rev.RevHubOrientationOnRobot; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.HardwareMap; +import com.qualcomm.robotcore.hardware.IMU; + +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles; public class CommonAutoMethods { private final MotorConstructor motors; + AutoConstants autoConst; + AutoTurn autoTurn; - public CommonAutoMethods(HardwareMap hardwareMap) { + public CommonAutoMethods(LinearOpMode opMode, HardwareMap hardwareMap) { this.motors = new MotorConstructor(hardwareMap); + this.autoConst = new AutoConstants(); + this.autoTurn = new AutoTurn(opMode, hardwareMap); } protected void setDrivePower(double leftFrontPower, double rightFrontPower, @@ -18,7 +30,7 @@ protected void setDrivePower(double leftFrontPower, double rightFrontPower, motors.leftBack.setPower(leftBackPower); } - protected void resetEncoders() { + public void resetEncoders() { motors.rightFront.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); motors.rightBack.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); motors.leftFront.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); @@ -32,6 +44,34 @@ protected void runUsingEncoders() { motors.leftBack.setMode(DcMotor.RunMode.RUN_USING_ENCODER); } + /** + * Get current cumulative angle rotation from last reset. + * + * @return Angle in degrees. + = left, - = right. + */ + private double getAngle() { + // We experimentally determined the Z axis is the axis we want to use for heading angle. + // We have to process the angle because the imu works in euler angles so the Z axis is + // returned as 0 to +180 or 0 to -180 rolling back to -179 or +179 when rotation passes + // 180 degrees. We detect this transition and track the total cumulative angle of rotation. + + // Orientation angles = imu.getAngularOrientation(AxesReference.INTRINSIC, AxesOrder.ZYX, AngleUnit.DEGREES); + YawPitchRollAngles orientation = motors.imu.getRobotYawPitchRollAngles(); + + double deltaAngle = orientation.getYaw(AngleUnit.DEGREES) - autoConst.lastAngles.getYaw(AngleUnit.DEGREES); + + if (deltaAngle < -180) + deltaAngle += 360; + else if (deltaAngle > 180) + deltaAngle -= 360; + + autoConst.globalAngle += deltaAngle; + + autoConst.lastAngles = orientation; + + return autoConst.globalAngle; + } + /** * See if we are moving in a straight line and if not return a power correction value. * @@ -54,4 +94,25 @@ protected double checkDirection() { return correction; } + + public void setUpIMU(RevHubOrientationOnRobot.LogoFacingDirection logoDirection, + RevHubOrientationOnRobot.UsbFacingDirection usbDirection) { + try { + RevHubOrientationOnRobot orientationOnRobot = + new RevHubOrientationOnRobot(logoDirection, usbDirection); + motors.imu.initialize(new IMU.Parameters(orientationOnRobot)); + + telemetry.addData("imu calib status", "calibrated"); + telemetry.update(); + + // initialize imu global variables after calibrating imu + autoTurn.resetAngle(); + + } catch ( + IllegalArgumentException e) { + telemetry.addData("imu calib status", "failed - try again"); + telemetry.update(); + } + } } + From 7fa65a59f512aea707147137c2446a60e9bb4e10 Mon Sep 17 00:00:00 2001 From: Jeffrey Mello Date: Wed, 19 Nov 2025 08:50:33 -0500 Subject: [PATCH 6/8] Refactor delay selection and add TeleOp common methods Moved SelectDelayTime from AutoMain to CommonAutoMethods for reuse and cleaner structure. Updated constructors in AutoDrive and AutoTurn to pass LinearOpMode. Added TeleOpCommonMethods class for button toggling and integrated it into TeleOpMain for improved launch control logic. --- .../firstinspires/ftc/teamcode/AutoMain.java | 87 +------------------ .../ftc/teamcode/TeleOpMain.java | 10 ++- .../ftc/teamcode/Utils_13233/AutoDrive.java | 3 +- .../ftc/teamcode/Utils_13233/AutoTurn.java | 3 +- .../Utils_13233/CommonAutoMethods.java | 81 +++++++++++++++++ .../teamcode/Utils_13233/LaunchControls.java | 1 + .../Utils_13233/TeleOpCommonMethods.java | 29 +++++++ 7 files changed, 125 insertions(+), 89 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/TeleOpCommonMethods.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoMain.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoMain.java index 40e6bb7..f325a5c 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoMain.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoMain.java @@ -29,13 +29,12 @@ @Autonomous(name = "AutoMain", group = "Auto") -public class AutoMain extends LinearOpMode{ +public class AutoMain extends LinearOpMode { private AutoDrive drive; private AutoTurn turn; private CommonAutoMethods autoMethods; - // Possible Autonomous Modes public enum AutoMode { AUTO_MODE_NOT_SELECTED, @@ -86,91 +85,12 @@ private AutoMode SelectAutoMode() { } // end SelectAutoMode - /** - * Function: SelectDelayTime - *

- * This function is use to select how long to delay the start of the autonomous code. - * Game pad 1 is used and the following controls are used for selection: - * left bumper - decrease delay time by 1000 milliseconds (1 second) - * right bumper - increase delay time by 1000 milliseconds (1 second) - * a button - set selected time - * note: if no delay time is needed, just select the a button. The default for the delay time - * is 0. - * - * @return Delay Time in milliseconds - */ - private Integer SelectDelayTime() { - int delayTimeMilliseconds = 0; // Initialize delay to be 0 seconds - - // display delay time not set - telemetry.addData("Delay", "%d (Not Set)", delayTimeMilliseconds); - telemetry.addData("Left Bumper -", " decrease delay time by (1 second)"); - telemetry.addData("Right bumper -", " increase delay time by (1 second)"); - telemetry.addData("A", "No delay time is needed"); - telemetry.update(); - - - /* Select Delay time. - - Select 'a' button without hitting bumpers if no delay needed - - Use Left Bumper to decrease delay time - - Use Right bumper to increase delay time - - Note: After entering delay time, use "a" button to set selected time - */ - - while (!isStopRequested() && !gamepad1.a) { - if (gamepad1.left_bumper) { - delayTimeMilliseconds -= 1000; - - // ensure delay time does not go negative - if (delayTimeMilliseconds < 0) { - delayTimeMilliseconds = 0; - } - - // Wait for the bumper to be released - while (gamepad1.left_bumper) { - idle(); - } - - telemetry.addData("Delay", "%d (decrease)", delayTimeMilliseconds); - telemetry.update(); - } - - if (gamepad1.right_bumper) { - delayTimeMilliseconds += 1000; - - // ensure delay time is not greater than 10 seconds - if (delayTimeMilliseconds > 10000) { - delayTimeMilliseconds = 10000; - } - - while (gamepad1.right_bumper) { - idle(); - } - telemetry.addData("Delay", "%d (increase)", delayTimeMilliseconds); - telemetry.update(); - } - } - - // Wait for user to release the a button - while (!isStopRequested() && gamepad1.a) { - idle(); - } - - - // Display selected delay time - telemetry.addData("Delay Time SET", delayTimeMilliseconds); - telemetry.update(); - return delayTimeMilliseconds; // returns selected delay time - - } // end SelectDelayTime - // OpMode for autonomous code @Override public void runOpMode() throws InterruptedException { drive = new AutoDrive(this, hardwareMap); - turn = new AutoTurn(this, hardwareMap); + turn = new AutoTurn(this, hardwareMap); autoMethods = new CommonAutoMethods(this, hardwareMap); double tgtPower = 0; @@ -193,7 +113,7 @@ public void runOpMode() throws InterruptedException { autoMode = SelectAutoMode(); // parkLocation = SelectParkLoc(); - delayTimeMilliseconds = SelectDelayTime(); + delayTimeMilliseconds = autoMethods.SelectDelayTime(); /* All required data entered. Autonomous is initialized. */ @@ -225,7 +145,6 @@ public void runOpMode() throws InterruptedException { // display delay over and autonomous code is running telemetry.addData("Status", "Running"); telemetry.update(); - } telemetry.clearAll(); // clear display messages diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOpMain.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOpMain.java index f183239..64c0962 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOpMain.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOpMain.java @@ -1,6 +1,8 @@ // Import libraries package org.firstinspires.ftc.teamcode; +import static org.firstinspires.ftc.robotcore.external.BlocksOpModeCompanion.opMode; + import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import com.qualcomm.robotcore.hardware.HardwareMap; @@ -9,6 +11,7 @@ import org.firstinspires.ftc.teamcode.Utils_13233.MotorConstructor; import org.firstinspires.ftc.teamcode.Utils_13233.RampControls; import org.firstinspires.ftc.teamcode.Utils_13233.LaunchControls; +import org.firstinspires.ftc.teamcode.Utils_13233.TeleOpCommonMethods; @TeleOp(name = "TeleOpMain", group = "Drive") @@ -18,6 +21,7 @@ public class TeleOpMain extends LinearOpMode { private DriveControls drive; private LaunchControls launch; private RampControls ramp; + private TeleOpCommonMethods commonMethods; //This function is executed when this Op Mode is selected from the Driver Station @@ -27,6 +31,8 @@ public void runOpMode() { drive = new DriveControls(hardwareMap); launch = new LaunchControls(hardwareMap); ramp = new RampControls(hardwareMap); + commonMethods = new TeleOpCommonMethods(this); + // The waitForStart() function will wait for the start button to begin // DON'T WRITE ANY CODE AFTER THE WAIT FOR START UNTIL THE "while (opModIsActive())" @@ -39,7 +45,9 @@ public void runOpMode() { telemetry.update(); // Set the power to the launch motors based while the x button is being pressed - launch.setLaunchPower(gamepad1.x); + + launch.setLaunchPower(commonMethods.toggleButton("launch", gamepad1.x, + false)); //Add option to enable brakes when sharbell holds a drive.setDriveMotorZeroPowerBehavior(gamepad1.a); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/AutoDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/AutoDrive.java index b9ab4d4..8056298 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/AutoDrive.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/AutoDrive.java @@ -17,11 +17,10 @@ public class AutoDrive { public AutoDrive(LinearOpMode opMode, HardwareMap hardwareMap) { this.opMode = opMode; this.motors = new MotorConstructor(hardwareMap); - this.autoMethods = new CommonAutoMethods(hardwareMap); + this.autoMethods = new CommonAutoMethods(opMode, hardwareMap); } - /** * Function: driveForward *

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/AutoTurn.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/AutoTurn.java index aad14bc..8d7ef89 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/AutoTurn.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/AutoTurn.java @@ -14,7 +14,7 @@ public class AutoTurn { public AutoTurn(LinearOpMode opMode, HardwareMap hardwareMap) { this.motors = new MotorConstructor(hardwareMap); - this.autoMethods = new CommonAutoMethods(hardwareMap); + this.autoMethods = new CommonAutoMethods(opMode, hardwareMap); this.autoConst = new AutoConstants(); this.opMode = opMode; } @@ -54,7 +54,6 @@ private void rotate(int degrees, double power) { } - public void resetAngle() { motors.imu.resetYaw(); YawPitchRollAngles lastAngles = motors.imu.getRobotYawPitchRollAngles(); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/CommonAutoMethods.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/CommonAutoMethods.java index cea8008..3517726 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/CommonAutoMethods.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/CommonAutoMethods.java @@ -15,11 +15,13 @@ public class CommonAutoMethods { private final MotorConstructor motors; AutoConstants autoConst; AutoTurn autoTurn; + LinearOpMode opMode; public CommonAutoMethods(LinearOpMode opMode, HardwareMap hardwareMap) { this.motors = new MotorConstructor(hardwareMap); this.autoConst = new AutoConstants(); this.autoTurn = new AutoTurn(opMode, hardwareMap); + this.opMode = opMode; } protected void setDrivePower(double leftFrontPower, double rightFrontPower, @@ -114,5 +116,84 @@ public void setUpIMU(RevHubOrientationOnRobot.LogoFacingDirection logoDirection, telemetry.update(); } } + + /** + * Function: SelectDelayTime + *

+ * This function is use to select how long to delay the start of the autonomous code. + * Game pad 1 is used and the following controls are used for selection: + * left bumper - decrease delay time by 1000 milliseconds (1 second) + * right bumper - increase delay time by 1000 milliseconds (1 second) + * a button - set selected time + * note: if no delay time is needed, just select the a button. The default for the delay time + * is 0. + * + * @return Delay Time in milliseconds + */ + public Integer SelectDelayTime() { + int delayTimeMilliseconds = 0; // Initialize delay to be 0 seconds + + // display delay time not set + telemetry.addData("Delay", "%d (Not Set)", delayTimeMilliseconds); + telemetry.addData("Left Bumper -", " decrease delay time by (1 second)"); + telemetry.addData("Right bumper -", " increase delay time by (1 second)"); + telemetry.addData("A", "No delay time is needed"); + telemetry.update(); + + + /* Select Delay time. + - Select 'a' button without hitting bumpers if no delay needed + - Use Left Bumper to decrease delay time + - Use Right bumper to increase delay time + + Note: After entering delay time, use "a" button to set selected time + */ + + while (!opMode.isStopRequested() && !opMode.gamepad1.a) { + if (opMode.gamepad1.left_bumper) { + delayTimeMilliseconds -= 1000; + + // ensure delay time does not go negative + if (delayTimeMilliseconds < 0) { + delayTimeMilliseconds = 0; + } + + // Wait for the bumper to be released + while (opMode.gamepad1.left_bumper) { + opMode.idle(); + } + + telemetry.addData("Delay", "%d (decrease)", delayTimeMilliseconds); + telemetry.update(); + } + + if (opMode.gamepad1.right_bumper) { + delayTimeMilliseconds += 1000; + + // ensure delay time is not greater than 10 seconds + if (delayTimeMilliseconds > 10000) { + delayTimeMilliseconds = 10000; + } + + while (opMode.gamepad1.right_bumper) { + opMode.idle(); + } + telemetry.addData("Delay", "%d (increase)", delayTimeMilliseconds); + telemetry.update(); + } + } + + // Wait for user to release the a button + while (!opMode.isStopRequested() && opMode.gamepad1.a) { + opMode.idle(); + } + + + // Display selected delay time + telemetry.addData("Delay Time SET", delayTimeMilliseconds); + telemetry.update(); + return delayTimeMilliseconds; // returns selected delay time + + } // end SelectDelayTime } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/LaunchControls.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/LaunchControls.java index 36f4da2..b5a5533 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/LaunchControls.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/LaunchControls.java @@ -21,4 +21,5 @@ public void setLaunchPower(boolean launchInput) { motors.Launcher.setPower(-power); motors.Launcher2.setPower(power); } + } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/TeleOpCommonMethods.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/TeleOpCommonMethods.java new file mode 100644 index 0000000..94f4c21 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/TeleOpCommonMethods.java @@ -0,0 +1,29 @@ +package org.firstinspires.ftc.teamcode.Utils_13233; + +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.hardware.HardwareMap; + +import java.util.HashMap; +import java.util.Map; + +public class TeleOpCommonMethods { + public Map prev = new HashMap<>(); + LinearOpMode opMode; + + public TeleOpCommonMethods(LinearOpMode opMode) { + this.opMode = opMode; + } + + + public boolean toggleButton(String name, boolean button, boolean toggle) { + boolean previous = prev.getOrDefault(name, false); + + if (button && !previous) { + toggle = !toggle; + } + + prev.put(name, button); + return toggle; + } + +} From ae1c4411aa976ddaf05e7839a627e2f9142a6f34 Mon Sep 17 00:00:00 2001 From: Jeffrey Mello Date: Wed, 19 Nov 2025 10:31:18 -0500 Subject: [PATCH 7/8] Refactor SelectAutoMode and add TeleOp tests Moved SelectAutoMode logic from AutoMain to CommonAutoMethods for better code reuse. Updated AutoMain to use the new method. Enhanced MockMotorUtil with additional hardware mocks and setup. Added initial TeleOpCommonMethodsTest for unit testing TeleOp methods. --- .../firstinspires/ftc/teamcode/AutoMain.java | 46 +------------------ .../Utils_13233/CommonAutoMethods.java | 40 ++++++++++++++++ .../ftc/teamcode/MockMotorUtil.java | 31 +++++++++++++ .../ftc/teamcode/TeleOpCommonMethodsTest.java | 20 ++++++++ 4 files changed, 92 insertions(+), 45 deletions(-) create mode 100644 TeamCode/src/test/java/org/firstinspires/ftc/teamcode/TeleOpCommonMethodsTest.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoMain.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoMain.java index f325a5c..7bc1c93 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoMain.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoMain.java @@ -26,66 +26,22 @@ import org.firstinspires.ftc.teamcode.Utils_13233.CommonAutoMethods; import org.firstinspires.ftc.teamcode.Utils_13233.MotorConstructor; - @Autonomous(name = "AutoMain", group = "Auto") - public class AutoMain extends LinearOpMode { private AutoDrive drive; private AutoTurn turn; private CommonAutoMethods autoMethods; - // Possible Autonomous Modes public enum AutoMode { AUTO_MODE_NOT_SELECTED, AUTO_MODE_DEFAULT, } - // Global Variables to store Game Specific Information AutoMode autoMode = AutoMode.AUTO_MODE_NOT_SELECTED; // store autonomous mode selected - /** - * SelectAutoMode - * This function is use to select the autonomous code to be executed for this match - * Game pad 1 is used and the following buttons are used for selection: - * a - Red alliance Sample - * b - Blue alliance Sample - * x - Red alliance Specimen - * y - Blue alliance Specimen - * - * @return Selected mode - */ - private AutoMode SelectAutoMode() { - // Local variable to store selected autonomous mode - - AutoMode autoMode = AutoMode.AUTO_MODE_NOT_SELECTED; - // Display autonomous mode not selected yet - telemetry.addData("AutoMode", "Not Selected"); - telemetry.update(); - - - // Loop until autonomous mode is selected - while (!isStopRequested() && autoMode == AutoMode.AUTO_MODE_NOT_SELECTED) { - autoMode = AutoMode.AUTO_MODE_DEFAULT; - idle(); - } - - // Display selected autonomous mode - telemetry.addData("Autonomous Mode", autoMode.toString()); - telemetry.update(); - - // Wait for the user to release the button - while (!isStopRequested() && (gamepad1.a || gamepad1.b || gamepad1.x || gamepad1.y)) { - idle(); - } - - return autoMode; - - } // end SelectAutoMode - - // OpMode for autonomous code @Override public void runOpMode() throws InterruptedException { @@ -111,7 +67,7 @@ public void runOpMode() throws InterruptedException { /* Select Autonomous Mode, Parking Location and Delay time */ - autoMode = SelectAutoMode(); + autoMode = autoMethods.SelectAutoMode(); // parkLocation = SelectParkLoc(); delayTimeMilliseconds = autoMethods.SelectDelayTime(); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/CommonAutoMethods.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/CommonAutoMethods.java index 3517726..b9acbae 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/CommonAutoMethods.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/CommonAutoMethods.java @@ -10,6 +10,7 @@ import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles; +import org.firstinspires.ftc.teamcode.AutoMain; public class CommonAutoMethods { private final MotorConstructor motors; @@ -195,5 +196,44 @@ public Integer SelectDelayTime() { return delayTimeMilliseconds; // returns selected delay time } // end SelectDelayTime + + /** + * SelectAutoMode + * This function is use to select the autonomous code to be executed for this match + * Game pad 1 is used and the following buttons are used for selection: + * a - Red alliance Sample + * b - Blue alliance Sample + * x - Red alliance Specimen + * y - Blue alliance Specimen + * + * @return Selected mode + */ + public AutoMain.AutoMode SelectAutoMode() { + // Local variable to store selected autonomous mode + + AutoMain.AutoMode autoMode = AutoMain.AutoMode.AUTO_MODE_NOT_SELECTED; + // Display autonomous mode not selected yet + telemetry.addData("AutoMode", "Not Selected"); + telemetry.update(); + + + // Loop until autonomous mode is selected + while (!opMode.isStopRequested() && autoMode == AutoMain.AutoMode.AUTO_MODE_NOT_SELECTED) { + autoMode = AutoMain.AutoMode.AUTO_MODE_DEFAULT; + opMode.idle(); + } + + // Display selected autonomous mode + telemetry.addData("Autonomous Mode", autoMode.toString()); + telemetry.update(); + + // Wait for the user to release the button + while (!opMode.isStopRequested() && (opMode.gamepad1.a || opMode.gamepad1.b || opMode.gamepad1.x || opMode.gamepad1.y)) { + opMode.idle(); + } + + return autoMode; + + } // end SelectAutoMode } diff --git a/TeamCode/src/test/java/org/firstinspires/ftc/teamcode/MockMotorUtil.java b/TeamCode/src/test/java/org/firstinspires/ftc/teamcode/MockMotorUtil.java index fdf6ab7..7e80e1d 100644 --- a/TeamCode/src/test/java/org/firstinspires/ftc/teamcode/MockMotorUtil.java +++ b/TeamCode/src/test/java/org/firstinspires/ftc/teamcode/MockMotorUtil.java @@ -5,19 +5,31 @@ import com.qualcomm.robotcore.hardware.CRServo; import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorSimple; import com.qualcomm.robotcore.hardware.HardwareMap; +import com.qualcomm.robotcore.hardware.IMU; +import com.qualcomm.robotcore.hardware.VoltageSensor; import org.firstinspires.ftc.teamcode.Utils_13233.LaunchControls; import org.junit.jupiter.api.BeforeEach; +import org.mockito.Mock; import org.mockito.MockitoAnnotations; public class MockMotorUtil { public DcMotor launcher, launcher2, intake, rightFront, leftFront, leftBack, rightBack; public CRServo rampServo1, rampServo2, rampServo3, rampServo4; public HardwareMap hardwareMap; + public IMU imu; + + @Mock + HardwareMap.DeviceMapping voltageSensorMap; + + @Mock + VoltageSensor VoltSens; public void setUp() { + MockitoAnnotations.openMocks(this); hardwareMap = mock(HardwareMap.class); @@ -37,6 +49,12 @@ public void setUp() { rampServo3 = mock(CRServo.class); rampServo4 = mock(CRServo.class); + VoltSens = mock(VoltageSensor.class); + imu = mock(IMU.class); + + + // Mock the motors + when(hardwareMap.get(DcMotor.class, "intake")).thenReturn(intake); @@ -52,5 +70,18 @@ public void setUp() { when(hardwareMap.get(CRServo.class, "rampServo2")).thenReturn(rampServo2); when(hardwareMap.get(CRServo.class, "rampServo3")).thenReturn(rampServo3); when(hardwareMap.get(CRServo.class, "rampServo4")).thenReturn(rampServo4); + when(hardwareMap.get(VoltageSensor.class, "Control Hub")).thenReturn(VoltSens); + + when(hardwareMap.voltageSensor).thenReturn(voltageSensorMap); + when(voltageSensorMap.get("Control Hub")).thenReturn(VoltSens); + + // Mock the IMU (gyroscope) + when(hardwareMap.get(IMU.class, "imu")).thenReturn(imu); + + // Set direction of the main drive motors + leftFront.setDirection(DcMotorSimple.Direction.FORWARD); + + // IC2 port 0 on a Core Device Interface Module + imu = hardwareMap.get(IMU.class, "imu"); } } diff --git a/TeamCode/src/test/java/org/firstinspires/ftc/teamcode/TeleOpCommonMethodsTest.java b/TeamCode/src/test/java/org/firstinspires/ftc/teamcode/TeleOpCommonMethodsTest.java new file mode 100644 index 0000000..7353d9e --- /dev/null +++ b/TeamCode/src/test/java/org/firstinspires/ftc/teamcode/TeleOpCommonMethodsTest.java @@ -0,0 +1,20 @@ +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; + +import org.firstinspires.ftc.teamcode.Utils_13233.RampControls; +import org.firstinspires.ftc.teamcode.Utils_13233.TeleOpCommonMethods; +import org.junit.jupiter.api.BeforeEach; + +public class TeleOpCommonMethodsTest { + public TeleOpCommonMethods teleOpMethods; + + MockMotorUtil mockMotor = new MockMotorUtil(); + + @BeforeEach + void motorSetup() { + LinearOpMode opMode = null; + mockMotor.setUp(); + teleOpMethods = new TeleOpCommonMethods(null); + } +} From 8f00026788b4b0f4209fb52b1311a4e4d81baea3 Mon Sep 17 00:00:00 2001 From: Jeffrey Mello Date: Mon, 1 Dec 2025 10:09:27 -0500 Subject: [PATCH 8/8] Remove TeleOpCommonMethods and update related code Deleted TeleOpCommonMethods and its test, refactored TeleOpMain to remove its usage, and updated launch control logic to use gamepad input directly. Also made rotate and setDrivePower methods public, and updated MotorConstructor to expose VoltSens and imu, as well as set launcher motors to use encoders. --- .../ftc/teamcode/TeleOpMain.java | 11 +------ .../ftc/teamcode/Utils_13233/AutoTurn.java | 2 +- .../Utils_13233/CommonAutoMethods.java | 4 +-- .../Utils_13233/MotorConstructor.java | 13 +++++++-- .../Utils_13233/TeleOpCommonMethods.java | 29 ------------------- .../ftc/teamcode/DriveControlsTest.java | 2 -- .../ftc/teamcode/MockMotorUtil.java | 4 +-- .../ftc/teamcode/TeleOpCommonMethodsTest.java | 20 ------------- 8 files changed, 15 insertions(+), 70 deletions(-) delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/TeleOpCommonMethods.java delete mode 100644 TeamCode/src/test/java/org/firstinspires/ftc/teamcode/TeleOpCommonMethodsTest.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOpMain.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOpMain.java index 64c0962..e995556 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOpMain.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOpMain.java @@ -1,17 +1,12 @@ // Import libraries package org.firstinspires.ftc.teamcode; -import static org.firstinspires.ftc.robotcore.external.BlocksOpModeCompanion.opMode; - import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; -import com.qualcomm.robotcore.hardware.HardwareMap; import org.firstinspires.ftc.teamcode.Utils_13233.DriveControls; -import org.firstinspires.ftc.teamcode.Utils_13233.MotorConstructor; import org.firstinspires.ftc.teamcode.Utils_13233.RampControls; import org.firstinspires.ftc.teamcode.Utils_13233.LaunchControls; -import org.firstinspires.ftc.teamcode.Utils_13233.TeleOpCommonMethods; @TeleOp(name = "TeleOpMain", group = "Drive") @@ -21,7 +16,6 @@ public class TeleOpMain extends LinearOpMode { private DriveControls drive; private LaunchControls launch; private RampControls ramp; - private TeleOpCommonMethods commonMethods; //This function is executed when this Op Mode is selected from the Driver Station @@ -31,8 +25,6 @@ public void runOpMode() { drive = new DriveControls(hardwareMap); launch = new LaunchControls(hardwareMap); ramp = new RampControls(hardwareMap); - commonMethods = new TeleOpCommonMethods(this); - // The waitForStart() function will wait for the start button to begin // DON'T WRITE ANY CODE AFTER THE WAIT FOR START UNTIL THE "while (opModIsActive())" @@ -46,8 +38,7 @@ public void runOpMode() { // Set the power to the launch motors based while the x button is being pressed - launch.setLaunchPower(commonMethods.toggleButton("launch", gamepad1.x, - false)); + launch.setLaunchPower(gamepad1.x); //Add option to enable brakes when sharbell holds a drive.setDriveMotorZeroPowerBehavior(gamepad1.a); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/AutoTurn.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/AutoTurn.java index 8d7ef89..1645f2e 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/AutoTurn.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/AutoTurn.java @@ -24,7 +24,7 @@ public AutoTurn(LinearOpMode opMode, HardwareMap hardwareMap) { * * @param degrees Degrees to turn, + is left - is right */ - private void rotate(int degrees, double power) { + public void rotate(int degrees, double power) { double leftPower, rightPower; // restart imu movement tracking. diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/CommonAutoMethods.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/CommonAutoMethods.java index b9acbae..0bfb26a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/CommonAutoMethods.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/CommonAutoMethods.java @@ -25,8 +25,8 @@ public CommonAutoMethods(LinearOpMode opMode, HardwareMap hardwareMap) { this.opMode = opMode; } - protected void setDrivePower(double leftFrontPower, double rightFrontPower, - double leftBackPower, double rightBackPower) { + public void setDrivePower(double leftFrontPower, double rightFrontPower, + double leftBackPower, double rightBackPower) { motors.rightFront.setPower(rightFrontPower); motors.rightBack.setPower(rightBackPower); motors.leftFront.setPower(leftFrontPower); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/MotorConstructor.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/MotorConstructor.java index 7899dcd..d4796ea 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/MotorConstructor.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/MotorConstructor.java @@ -55,8 +55,8 @@ public class MotorConstructor { public CRServo rampServo3; public CRServo rampServo4; - VoltageSensor VoltSens; - protected IMU imu; + public VoltageSensor VoltSens; + public IMU imu; public MotorConstructor(HardwareMap hardwareMap) { // Map main Drive Motors @@ -72,13 +72,16 @@ public MotorConstructor(HardwareMap hardwareMap) { Launcher = hardwareMap.get(DcMotor.class, "Launcher"); Launcher2 = hardwareMap.get(DcMotor.class, "Launcher2"); + // Map the ramp servos rampServo1 = hardwareMap.get(CRServo.class, "rampServo1"); rampServo2 = hardwareMap.get(CRServo.class, "rampServo2"); rampServo3 = hardwareMap.get(CRServo.class, "rampServo3"); rampServo4 = hardwareMap.get(CRServo.class, "rampServo4"); + // Control hub voltage sensor + // Used to move at a constant speed in auto regardless of battery voltage VoltSens = hardwareMap.voltageSensor.get("Control Hub"); - + // IC2 port 0 on a Core Device Interface Module imu = hardwareMap.get(IMU.class, "imu"); @@ -92,6 +95,10 @@ public MotorConstructor(HardwareMap hardwareMap) { Launcher.setDirection(DcMotorSimple.Direction.REVERSE); Launcher2.setDirection(DcMotorSimple.Direction.REVERSE); + // Set the launch motors to run using encoders + Launcher.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + Launcher2.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + // Set the direction of the intake motor intake.setDirection(DcMotorSimple.Direction.REVERSE); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/TeleOpCommonMethods.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/TeleOpCommonMethods.java deleted file mode 100644 index 94f4c21..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/TeleOpCommonMethods.java +++ /dev/null @@ -1,29 +0,0 @@ -package org.firstinspires.ftc.teamcode.Utils_13233; - -import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; -import com.qualcomm.robotcore.hardware.HardwareMap; - -import java.util.HashMap; -import java.util.Map; - -public class TeleOpCommonMethods { - public Map prev = new HashMap<>(); - LinearOpMode opMode; - - public TeleOpCommonMethods(LinearOpMode opMode) { - this.opMode = opMode; - } - - - public boolean toggleButton(String name, boolean button, boolean toggle) { - boolean previous = prev.getOrDefault(name, false); - - if (button && !previous) { - toggle = !toggle; - } - - prev.put(name, button); - return toggle; - } - -} diff --git a/TeamCode/src/test/java/org/firstinspires/ftc/teamcode/DriveControlsTest.java b/TeamCode/src/test/java/org/firstinspires/ftc/teamcode/DriveControlsTest.java index 8df60b5..37cb925 100644 --- a/TeamCode/src/test/java/org/firstinspires/ftc/teamcode/DriveControlsTest.java +++ b/TeamCode/src/test/java/org/firstinspires/ftc/teamcode/DriveControlsTest.java @@ -1,7 +1,5 @@ package org.firstinspires.ftc.teamcode; -import static org.junit.jupiter.api.Assertions.assertEquals; -import static org.mockito.Mockito.mock; import static org.mockito.Mockito.verify; import com.qualcomm.robotcore.hardware.DcMotor; diff --git a/TeamCode/src/test/java/org/firstinspires/ftc/teamcode/MockMotorUtil.java b/TeamCode/src/test/java/org/firstinspires/ftc/teamcode/MockMotorUtil.java index 7e80e1d..6c3c416 100644 --- a/TeamCode/src/test/java/org/firstinspires/ftc/teamcode/MockMotorUtil.java +++ b/TeamCode/src/test/java/org/firstinspires/ftc/teamcode/MockMotorUtil.java @@ -54,8 +54,6 @@ public void setUp() { // Mock the motors - - when(hardwareMap.get(DcMotor.class, "intake")).thenReturn(intake); when(hardwareMap.get(DcMotor.class, "leftFront")).thenReturn(leftFront); @@ -72,7 +70,7 @@ public void setUp() { when(hardwareMap.get(CRServo.class, "rampServo4")).thenReturn(rampServo4); when(hardwareMap.get(VoltageSensor.class, "Control Hub")).thenReturn(VoltSens); - when(hardwareMap.voltageSensor).thenReturn(voltageSensorMap); + hardwareMap.voltageSensor = voltageSensorMap; when(voltageSensorMap.get("Control Hub")).thenReturn(VoltSens); // Mock the IMU (gyroscope) diff --git a/TeamCode/src/test/java/org/firstinspires/ftc/teamcode/TeleOpCommonMethodsTest.java b/TeamCode/src/test/java/org/firstinspires/ftc/teamcode/TeleOpCommonMethodsTest.java deleted file mode 100644 index 7353d9e..0000000 --- a/TeamCode/src/test/java/org/firstinspires/ftc/teamcode/TeleOpCommonMethodsTest.java +++ /dev/null @@ -1,20 +0,0 @@ -package org.firstinspires.ftc.teamcode; - -import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; - -import org.firstinspires.ftc.teamcode.Utils_13233.RampControls; -import org.firstinspires.ftc.teamcode.Utils_13233.TeleOpCommonMethods; -import org.junit.jupiter.api.BeforeEach; - -public class TeleOpCommonMethodsTest { - public TeleOpCommonMethods teleOpMethods; - - MockMotorUtil mockMotor = new MockMotorUtil(); - - @BeforeEach - void motorSetup() { - LinearOpMode opMode = null; - mockMotor.setUp(); - teleOpMethods = new TeleOpCommonMethods(null); - } -}