- * 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); + autoMethods = new CommonAutoMethods(this, hardwareMap); 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); - + autoMethods.setUpIMU(RevHubOrientationOnRobot.LogoFacingDirection.BACKWARD, + RevHubOrientationOnRobot.UsbFacingDirection.DOWN); // Create local variable to store selected autonomous mode // and amount of delay time @@ -258,9 +67,9 @@ public void runOpMode() throws InterruptedException { /* Select Autonomous Mode, Parking Location and Delay time */ - autoMode = SelectAutoMode(); + autoMode = autoMethods.SelectAutoMode(); // parkLocation = SelectParkLoc(); - delayTimeMilliseconds = SelectDelayTime(); + delayTimeMilliseconds = autoMethods.SelectDelayTime(); /* All required data entered. Autonomous is initialized. */ @@ -278,7 +87,7 @@ public void runOpMode() throws InterruptedException { telemetry.addData("Mode", "running"); telemetry.update(); - resetEncoders(); + autoMethods.resetEncoders(); // Delay start if needed if (delayTimeMilliseconds > 0) { @@ -292,7 +101,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 @@ -309,215 +117,6 @@ public void runOpMode() throws InterruptedException { } 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; + drive.driveForward(10.0, AutoConstants.quarterPower); } - - /** - * 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/TeleOpMain.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOpMain.java index f183239..e995556 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOpMain.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TeleOpMain.java @@ -3,10 +3,8 @@ 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; @@ -39,6 +37,7 @@ public void runOpMode() { telemetry.update(); // Set the power to the launch motors based while the x button is being pressed + launch.setLaunchPower(gamepad1.x); //Add option to enable brakes when sharbell holds a 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..64ef8c4 --- /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 + 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 new file mode 100644 index 0000000..8056298 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/AutoDrive.java @@ -0,0 +1,109 @@ +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 new file mode 100644 index 0000000..1645f2e --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/AutoTurn.java @@ -0,0 +1,63 @@ +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 new file mode 100644 index 0000000..0bfb26a --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/CommonAutoMethods.java @@ -0,0 +1,239 @@ +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
+}
+
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/MotorConstructor.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Utils_13233/MotorConstructor.java
index ff7e62e..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
@@ -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;
+ public VoltageSensor VoltSens;
+ public IMU imu;
+
public MotorConstructor(HardwareMap hardwareMap) {
// Map main Drive Motors
leftFront = hardwareMap.get(DcMotor.class, "leftFront");
@@ -67,11 +72,19 @@ 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");
+
// Set direction of the main drive motors
leftFront.setDirection(DcMotorSimple.Direction.FORWARD);
rightFront.setDirection(DcMotorSimple.Direction.REVERSE);
@@ -82,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/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 fdf6ab7..6c3c416 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