From 6fcce3f1e48f19bf25380c82c86cc2f0240d3768 Mon Sep 17 00:00:00 2001 From: Jeffrey Mello Date: Sat, 15 Nov 2025 22:06:03 -0500 Subject: [PATCH] Refactor to use MotorConstructor and DriveControls Replaced CommonControls with MotorConstructor in AutoMain and DriveControls in QuickAuto for improved hardware abstraction. Updated motor references and initialization to use new constructors, enhancing modularity and maintainability. --- .../firstinspires/ftc/teamcode/AutoMain.java | 1041 +++++++++-------- .../firstinspires/ftc/teamcode/QuickAuto.java | 52 +- 2 files changed, 553 insertions(+), 540 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 6f0dc7d..8c732b7 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoMain.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoMain.java @@ -1,518 +1,523 @@ -//package org.firstinspires.ftc.teamcode; -// -//import com.qualcomm.robotcore.eventloop.opmode.Autonomous; -//import com.qualcomm.robotcore.hardware.IMU; -//import com.qualcomm.hardware.rev.RevHubOrientationOnRobot; -//import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; -// -// -//// Telemetry - print on driver hub -//// -// -// -//// Robot Hardware Classes -//// -//import com.qualcomm.robotcore.hardware.DcMotor; -//import com.qualcomm.robotcore.hardware.DcMotorSimple; -//import com.qualcomm.robotcore.hardware.VoltageSensor; -// -// -//// 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.CommonControls; -// -// -//@Autonomous(name = "AutoMain", group = "Auto") -// -//public class AutoMain extends LinearOpMode { -// CommonControls CommonControls = new CommonControls(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 -// public enum AutoMode { -// AUTO_MODE_NOT_SELECTED, -// AUTO_MODE_DEFAULT, -// } -// -// -// // 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 -// -// -// /** -// * 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 -// -// /** -// * 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 { -// -// 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 -// * 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 -// CommonControls.leftFront.setDirection(DcMotorSimple.Direction.REVERSE); -// CommonControls.rightFront.setDirection(DcMotorSimple.Direction.FORWARD); -// CommonControls.leftBack.setDirection(DcMotorSimple.Direction.REVERSE); -// CommonControls.rightBack.setDirection(DcMotorSimple.Direction.FORWARD); -// -// -// // Create local variable to store selected autonomous mode -// // and amount of delay time -// int delayTimeMilliseconds = 0; -// -// telemetry.setAutoClear(false); -// telemetry.addData("Status", "Initializing"); -// telemetry.update(); -// -// /* Select Autonomous Mode, Parking Location and Delay time */ -// -// autoMode = SelectAutoMode(); -// // parkLocation = SelectParkLoc(); -// delayTimeMilliseconds = SelectDelayTime(); -// -// -// /* All required data entered. Autonomous is initialized. */ -// -// telemetry.addData("Status", "Initialized"); -// telemetry.addData("mode", "waiting for start"); -// telemetry.addData(">", "Press Play to start op mode"); -// telemetry.update(); -// -// /* Wait for start of the match */ -// waitForStart(); -// -// telemetry.clearAll(); // clear display messages -// -// telemetry.addData("Mode", "running"); -// telemetry.update(); -// -// resetEncoders(); -// -// // Delay start if needed -// if (delayTimeMilliseconds > 0) { -// // display delay status -// telemetry.addData("Status", "Delaying"); -// telemetry.update(); -// -// // wait selected amount of time -// sleep(delayTimeMilliseconds); -// -// // display delay over and autonomous code is running -// telemetry.addData("Status", "Running"); -// telemetry.update(); -// -// } -// -// telemetry.clearAll(); // clear display messages -// -// // Determine which autonomous code to run -// switch (autoMode) { -// case AUTO_MODE_DEFAULT: -// defaultAuto(); -// break; -// case AUTO_MODE_NOT_SELECTED: -// // This one should not happen if it does do nothing -// break; -// } -// } -// -// private void defaultAuto() { -// driveForward(10.0, quarterPower); -// } -// -// -// // Functions needed for driving auto -// private void resetEncoders() { -// CommonControls.rightFront.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); -// CommonControls.rightBack.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); -// CommonControls.leftFront.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); -// CommonControls.leftBack.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); -// } -// -// private void runUsingEncoders() { -// CommonControls.rightFront.setMode(DcMotor.RunMode.RUN_USING_ENCODER); -// CommonControls.rightBack.setMode(DcMotor.RunMode.RUN_USING_ENCODER); -// CommonControls.leftFront.setMode(DcMotor.RunMode.RUN_USING_ENCODER); -// CommonControls.leftBack.setMode(DcMotor.RunMode.RUN_USING_ENCODER); -// } -// -// private void setDrivePower(double leftFrontPower, double rightFrontPower, -// double leftBackPower, double rightBackPower) { -// CommonControls.rightFront.setPower(rightFrontPower); -// CommonControls.rightBack.setPower(rightBackPower); -// CommonControls.leftFront.setPower(leftFrontPower); -// CommonControls.leftBack.setPower(leftBackPower); -// } -// -// /** -// * 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: 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(CommonControls.leftFront.getCurrentPosition()) + -// Math.abs(CommonControls.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. -// * -// * @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; -// } -// -// /** -// * 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. -// * -// * @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(); -// } -// -//} +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.hardware.HardwareMap; +import com.qualcomm.robotcore.hardware.IMU; +import com.qualcomm.hardware.rev.RevHubOrientationOnRobot; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; + + +// Telemetry - print on driver hub +// + + +// Robot Hardware Classes +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorSimple; +import com.qualcomm.robotcore.hardware.VoltageSensor; + + +// 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.MotorConstructor; + + +@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 + public enum AutoMode { + AUTO_MODE_NOT_SELECTED, + AUTO_MODE_DEFAULT, + } + + + // 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 + + + /** + * 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 + + /** + * 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 { + + 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 + * 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); + + + // Create local variable to store selected autonomous mode + // and amount of delay time + int delayTimeMilliseconds = 0; + + telemetry.setAutoClear(false); + telemetry.addData("Status", "Initializing"); + telemetry.update(); + + /* Select Autonomous Mode, Parking Location and Delay time */ + + autoMode = SelectAutoMode(); + // parkLocation = SelectParkLoc(); + delayTimeMilliseconds = SelectDelayTime(); + + + /* All required data entered. Autonomous is initialized. */ + + telemetry.addData("Status", "Initialized"); + telemetry.addData("mode", "waiting for start"); + telemetry.addData(">", "Press Play to start op mode"); + telemetry.update(); + + /* Wait for start of the match */ + waitForStart(); + + telemetry.clearAll(); // clear display messages + + telemetry.addData("Mode", "running"); + telemetry.update(); + + resetEncoders(); + + // Delay start if needed + if (delayTimeMilliseconds > 0) { + // display delay status + telemetry.addData("Status", "Delaying"); + telemetry.update(); + + // wait selected amount of time + sleep(delayTimeMilliseconds); + + // display delay over and autonomous code is running + telemetry.addData("Status", "Running"); + telemetry.update(); + + } + + telemetry.clearAll(); // clear display messages + + // Determine which autonomous code to run + switch (autoMode) { + case AUTO_MODE_DEFAULT: + defaultAuto(); + break; + case AUTO_MODE_NOT_SELECTED: + // This one should not happen if it does do nothing + break; + } + } + + private void defaultAuto() { + driveForward(10.0, quarterPower); + } + + + // 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 + *

+ * 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: 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. + * + * @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; + } + + /** + * 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. + * + * @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/QuickAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/QuickAuto.java index 73afa16..335fa71 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/QuickAuto.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/QuickAuto.java @@ -1,22 +1,30 @@ -//package org.firstinspires.ftc.teamcode; -// -//import com.qualcomm.robotcore.eventloop.opmode.*; -// -//@Autonomous(name = "QuickAuto") -//public class QuickAuto extends LinearOpMode { -// @Override -// public void runOpMode() { -// telemetry.addData("Status", "Initialized"); -// telemetry.update(); -// // Create a new CommonControls object -// CommonControls CommonControls = new CommonControls(hardwareMap); -// -// //CommonControls.setDriveBrake(); -// waitForStart(); -// if (opModeIsActive()) {// OpMode loop -// CommonControls.setDrivePower(-1.0f); -// sleep(500); -// CommonControls.setDrivePower(0); -// } -// } -//} +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.robotcore.eventloop.opmode.*; +import com.qualcomm.robotcore.hardware.HardwareMap; + +import org.firstinspires.ftc.teamcode.Utils_13233.DriveControls; +import org.firstinspires.ftc.teamcode.Utils_13233.MotorConstructor; + +@Autonomous(name = "QuickAuto") +public class QuickAuto extends LinearOpMode { + private final DriveControls drive; + + public QuickAuto(HardwareMap hardwareMap) { + this.drive = new DriveControls(hardwareMap); + } + + @Override + public void runOpMode() { + telemetry.addData("Status", "Initialized"); + telemetry.update(); + + //CommonControls.setDriveBrake(); + waitForStart(); + if (opModeIsActive()) {// OpMode loop + drive.setDrivePower(-1.0f); + sleep(500); + drive.setDrivePower(0); + } + } +}