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 3dd1698..aa0eaf4 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoMain.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoMain.java
@@ -1,120 +1,120 @@
-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.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 {
- // Global Variables to store Game Specific Information
- AutoMode autoMode = AutoMode.AUTO_MODE_NOT_SELECTED; // store autonomous mode selected
- private AutoDrive drive;
- private AutoTurn turn;
- private CommonAutoMethods autoMethods;
-
- // 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;
-
-
- telemetry.addData("Mode", "calibrating imu....");
- telemetry.update();
- autoMethods.setUpIMU(RevHubOrientationOnRobot.LogoFacingDirection.BACKWARD,
- RevHubOrientationOnRobot.UsbFacingDirection.DOWN);
-
- // 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 = autoMethods.SelectAutoMode();
- // parkLocation = SelectParkLoc();
- delayTimeMilliseconds = autoMethods.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();
-
- autoMethods.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() {
- drive.driveForward(10.0, AutoConstants.quarterPower);
- }
-
- // Possible Autonomous Modes
- public enum AutoMode {
- AUTO_MODE_NOT_SELECTED,
- AUTO_MODE_DEFAULT,
- }
-}
+//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.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 {
+// // Global Variables to store Game Specific Information
+// AutoMode autoMode = AutoMode.AUTO_MODE_NOT_SELECTED; // store autonomous mode selected
+// private AutoDrive drive;
+// private AutoTurn turn;
+// private CommonAutoMethods autoMethods;
+//
+// // 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;
+//
+//
+// telemetry.addData("Mode", "calibrating imu....");
+// telemetry.update();
+// autoMethods.setUpIMU(RevHubOrientationOnRobot.LogoFacingDirection.BACKWARD,
+// RevHubOrientationOnRobot.UsbFacingDirection.DOWN);
+//
+// // 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 = autoMethods.SelectAutoMode();
+// // parkLocation = SelectParkLoc();
+// delayTimeMilliseconds = autoMethods.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();
+//
+// autoMethods.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() {
+// drive.driveForward(10.0, AutoConstants.quarterPower);
+// }
+//
+// // Possible Autonomous Modes
+// public enum AutoMode {
+// AUTO_MODE_NOT_SELECTED,
+// AUTO_MODE_DEFAULT,
+// }
+//}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/QuickAutoBlue.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/QuickAutoBlue.java
deleted file mode 100644
index 9f40074..0000000
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/QuickAutoBlue.java
+++ /dev/null
@@ -1,92 +0,0 @@
-package org.firstinspires.ftc.teamcode;
-
-import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
-import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
-import com.qualcomm.robotcore.hardware.Servo;
-
-import org.firstinspires.ftc.teamcode.Utils_13233.AutoDrive;
-import org.firstinspires.ftc.teamcode.Utils_13233.AutoTurn;
-import org.firstinspires.ftc.teamcode.Utils_13233.DriveControls;
-import org.firstinspires.ftc.teamcode.Utils_13233.LaunchControls;
-import org.firstinspires.ftc.teamcode.Utils_13233.MotorConstructor;
-import org.firstinspires.ftc.teamcode.Utils_13233.SorterControls;
-
-//@Autonomous(name = "QuickAutoBlue", group = "Autonomous")
-public class QuickAutoBlue extends LinearOpMode {
- // Constructors for the utils classes
- private DriveControls drive;
- private LaunchControls launch;
- private SorterControls sorter;
- private MotorConstructor motors;
-
- @Override
- public void runOpMode() {
- telemetry.addData("Status", "Initialized");
- telemetry.update();
-
- waitForStart();
-
- if (opModeIsActive()) {
- // Create the utils classes
- drive = new DriveControls(hardwareMap);
- launch = new LaunchControls(hardwareMap);
- motors = new MotorConstructor(hardwareMap);
- sorter = new SorterControls(motors);
-
-
- //autoDrive.driveForward(10.0, AutoConstants.quarterPower);
- //autoTurn.rotate(90, AutoConstants.quarterPower);
-
-
- //Jacob's not so useful code
-
- drive.setDrivePower(-1.0f);
- sleep(1600);
- drive.setDriveMotorZeroPowerBehavior(true);
- drive.setDrivePower(0);//brakes the reverse to ensure no major fouls are incurred
- sorter.moveSorterToPos(SorterControls.sorterModes.LAUNCH, 1);
- //sets position to ensure the ball is correctly lined up
- launch.setLaunchPower(true, 0.9f);
-
- sleep(2500);//wait state to wait for launcher to spin up
- motors.Flipper.setDirection(Servo.Direction.FORWARD);//runs servo forward, shooting 1st ball
- motors.Flipper.setPosition(0.0);
- sleep(1000);//runs flipper servo into motor
- motors.Flipper.setDirection(Servo.Direction.REVERSE);
- motors.Flipper.setPosition(0.15);
- sleep(2000);
-
- //2nd ball code
- sleep(750);//waits to spin turntable
- sorter.moveSorterToPos(SorterControls.sorterModes.LAUNCH, 2);
- sleep(1500);//waits while turntable spins
- motors.Flipper.setDirection(Servo.Direction.FORWARD);//runs servo forward, shooting 2nd ball
- motors.Flipper.setPosition(0.0);
- sleep(1000);
- motors.Flipper.setDirection(Servo.Direction.REVERSE);
- motors.Flipper.setPosition(0.15);//brings servo back up to avoid turn table collision
- sleep(2000);
-
- //3rd ball code
- sleep(1000);
- sorter.moveSorterToPos(SorterControls.sorterModes.LAUNCH, 3);
- sleep(1000);//turns off ramp
- motors.Flipper.setDirection(Servo.Direction.FORWARD);//runs servo forward, shooting 3rd ball
- motors.Flipper.setPosition(0.0);
- sleep(1000);// sets servo back to init pos
- motors.Flipper.setDirection(Servo.Direction.REVERSE);//runs servo forward, shooting 1st ball
- motors.Flipper.setPosition(0.15);
- sleep(1000);
- sorter.moveSorterToPos(SorterControls.sorterModes.LAUNCH, 1);//brings sorter back to init
- launch.setLaunchPower(false);//turns off launcher and ramp
-
- //strafing section
- drive.setDrivePower(0, 1.0f, 0, 0);
- sleep(1000);
- drive.setDrivePower(1.0f);//strafes over to balls, ensuring ally has space to shoot, and moves off line
- sleep(1000);
-
-
- }
- }
-}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/QuickAutoRed.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/QuickAutoRed.java
deleted file mode 100644
index dc7342e..0000000
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/QuickAutoRed.java
+++ /dev/null
@@ -1,97 +0,0 @@
-package org.firstinspires.ftc.teamcode;
-
-import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
-import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
-import com.qualcomm.robotcore.hardware.Servo;
-
-import org.firstinspires.ftc.teamcode.Utils_13233.AutoDrive;
-import org.firstinspires.ftc.teamcode.Utils_13233.AutoTurn;
-import org.firstinspires.ftc.teamcode.Utils_13233.DriveControls;
-import org.firstinspires.ftc.teamcode.Utils_13233.LaunchControls;
-import org.firstinspires.ftc.teamcode.Utils_13233.MotorConstructor;
-import org.firstinspires.ftc.teamcode.Utils_13233.SorterControls;
-
-//@Autonomous(name = "QuickAutoRed", group = "Autonomous")
-public class QuickAutoRed extends LinearOpMode {
-
- // Constructors for the utils classes
- private DriveControls drive;
- private LaunchControls launch;
- private SorterControls sorter;
- private MotorConstructor motors;
-
- @Override
- public void runOpMode() {
-
-
- telemetry.addData("Status", "Initialized");
- telemetry.update();
-
- waitForStart();
-
- if (opModeIsActive()) {
- // Create the utils classes
- drive = new DriveControls(hardwareMap);
- launch = new LaunchControls(hardwareMap);
- motors = new MotorConstructor(hardwareMap);
- sorter = new SorterControls(motors);
-
-
- //autoDrive.driveForward(10.0, AutoConstants.quarterPower);
- //autoTurn.rotate(90, AutoConstants.quarterPower);
-
- drive.setDrivePower(-1.0f);
- sleep(1200);
- drive.setDriveMotorZeroPowerBehavior(true);
- drive.setDrivePower(0);//brakes the reverse to ensure no major fouls are incurred
- sorter.moveSorterToPos(SorterControls.sorterModes.LAUNCH, 1);
- //sets position to ensure the ball is correctly lined up
- launch.setLaunchPower(true);
-
- sleep(2500);//wait state to wait for launcher to spin up
- motors.Flipper.setDirection(Servo.Direction.FORWARD);//runs servo forward, shooting 1st ball
- motors.Flipper.setPosition(0.0);
- sleep(1000);//runs flipper servo into motor
- motors.Flipper.setDirection(Servo.Direction.REVERSE);
- motors.Flipper.setPosition(0.15);
-
- //2nd ball code
- sleep(750);//waits to spin turntable
- sorter.moveSorterToPos(SorterControls.sorterModes.LAUNCH, 2);
- sleep(1500);//waits while turntable spins
- motors.Flipper.setDirection(Servo.Direction.FORWARD);//runs servo forward, shooting 2nd ball
- motors.Flipper.setPosition(0.0);
- sleep(1000);
- motors.Flipper.setDirection(Servo.Direction.REVERSE);
- motors.Flipper.setPosition(0.15);//brings servo back up to avoid turn table collision
-
- //3rd ball code
- sleep(1000);
- sorter.moveSorterToPos(SorterControls.sorterModes.LAUNCH, 3);
- sleep(1000);//turns off ramp
- motors.Flipper.setDirection(Servo.Direction.FORWARD);//runs servo forward, shooting 3rd ball
- motors.Flipper.setPosition(0.0);
- sleep(1000);// sets servo back to init pos
- motors.Flipper.setDirection(Servo.Direction.REVERSE);//runs servo forward, shooting 1st ball
- motors.Flipper.setPosition(0.15);
- sleep(1000);
- sorter.moveSorterToPos(SorterControls.sorterModes.LAUNCH, 1);//brings sorter back to init
- launch.setLaunchPower(false);//turns off launcher and ramp
-
- //strafing section
- drive.setDrivePower(1.0f, 0, 0, 0);
- sleep(1000);
- drive.setDrivePower(1.0f);//strafes over to balls, ensuring ally has space to shoot, and moves off line
- sleep(1000);
-
-
- }
- }
-}
-// 傑弗裡，你是說要運行它嗎？傑弗裡，你是說要運行它嗎？傑弗裡，你是說要運行它嗎？傑弗裡,傑弗裡，你是說要運行它嗎？
-//傑弗裡，你是說要運行它嗎？傑弗裡，你是說要運行它嗎？傑弗裡，你是說要運行它嗎？傑弗裡，你是說要運行它嗎？傑弗裡，
-//傑弗裡，你是說要運行它嗎？傑弗裡，你是說要運行它嗎？傑弗裡，你是說要運行它嗎？傑弗裡，你是說要運行它嗎？傑弗裡，
-//傑弗裡，你是說要運行它嗎？傑弗裡，你是說要運行它嗎？傑弗裡，你是說要運行它嗎？傑弗裡，你是說要運行它嗎？傑弗裡，
-//傑弗裡，你是說要運行它嗎？傑弗裡，你是說要運行它嗎？傑弗裡，你是說要運行它嗎？傑弗裡，你是說要運行它嗎？傑弗裡
-//傑弗裡，你是說要運行它嗎？傑弗裡，你是說要運行它嗎？傑弗裡，你是說要運行它嗎？傑弗裡，你是說要運行它嗎？傑弗裡
-//傑弗裡，你是說要運行它嗎？傑弗裡，你是說要運行它嗎？傑弗裡，你是說要運行它嗎？傑弗裡，你是說要運行它嗎？傑弗裡
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
deleted file mode 100644
index 64ef8c4..0000000
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/AutoConstants.java
+++ /dev/null
@@ -1,34 +0,0 @@
-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
- 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
- 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
deleted file mode 100644
index 8056298..0000000
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/AutoDrive.java
+++ /dev/null
@@ -1,109 +0,0 @@
-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 class AutoDrive {
- private final MotorConstructor motors;
- CommonAutoMethods autoMethods;
- private final LinearOpMode opMode;
-
- public AutoDrive(LinearOpMode opMode, HardwareMap hardwareMap) {
- this.opMode = opMode;
- this.motors = new MotorConstructor(hardwareMap);
- this.autoMethods = new CommonAutoMethods(opMode, 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.
- */
- public 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: 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
- * 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 (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.
- 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);
- 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
deleted file mode 100644
index 1645f2e..0000000
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/AutoTurn.java
+++ /dev/null
@@ -1,63 +0,0 @@
-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.AngleUnit;
-import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles;
-
-public class AutoTurn {
- private final MotorConstructor motors;
- CommonAutoMethods autoMethods;
- AutoConstants autoConst;
- private final LinearOpMode opMode;
-
- public AutoTurn(LinearOpMode opMode, HardwareMap hardwareMap) {
- this.motors = new MotorConstructor(hardwareMap);
- this.autoMethods = new CommonAutoMethods(opMode, hardwareMap);
- this.autoConst = new AutoConstants();
- this.opMode = opMode;
- }
-
- /**
- * 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
- */
- public 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();
-
- 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
deleted file mode 100644
index 0bfb26a..0000000
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/CommonAutoMethods.java
+++ /dev/null
@@ -1,239 +0,0 @@
-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;
-import org.firstinspires.ftc.teamcode.AutoMain;
-
-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;
- }
-
- public 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);
- }
-
- 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);
- 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);
- }
-
- /**
- * 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.
- *
- * @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;
- }
-
- 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();
- }
- }
-
- /**
- * 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
-
- /**
- * 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
-}
-