diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Autonomoustest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Autonomoustest.java index 3ad5165..fe8fb02 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Autonomoustest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Autonomoustest.java @@ -70,8 +70,8 @@ public class Autonomoustest extends LinearOpMode { /* Declare OpMode members. */ private DcMotor leftDrive = null; private DcMotor rightDrive = null; - private DcMotor backrightDrive = null; - private DcMotor backleftDrive = null; + private DcMotor backRightDrive = null; + private DcMotor backLeftDrive = null; private ColorSensor colorRight = null; private ColorSensor colorLeft = null; private Servo wrist = null; @@ -112,8 +112,8 @@ public class Autonomoustest extends LinearOpMode { /* telemetry.addData("Starting at", "%7d :%7d", leftDrive.getCurrentPosition(), rightDrive.getCurrentPosition(), - backleftDrive.getCurrentPosition(), - backrightDrive.getCurrentPosition());*/ + backLeftDrive.getCurrentPosition(), + backRightDrive.getCurrentPosition());*/ telemetry.update(); @@ -137,8 +137,8 @@ public class Autonomoustest extends LinearOpMode { { leftDrive.setDirection(DcMotor.Direction.REVERSE); rightDrive.setDirection(DcMotor.Direction.FORWARD); - backrightDrive.setDirection(DcMotor.Direction.REVERSE); - backleftDrive.setDirection(DcMotor.Direction.REVERSE); + backRightDrive.setDirection(DcMotor.Direction.REVERSE); + backLeftDrive.setDirection(DcMotor.Direction.REVERSE); encoderDrive(DRIVE_SPEED, distance, distance, LONG_TIMEOUT); // S1: Forward 47 Inches with 5 Sec timeout } @@ -146,8 +146,8 @@ public class Autonomoustest extends LinearOpMode { { leftDrive.setDirection(DcMotor.Direction.FORWARD); rightDrive.setDirection(DcMotor.Direction.FORWARD); - backrightDrive.setDirection(DcMotor.Direction.FORWARD); - backleftDrive.setDirection(DcMotor.Direction.REVERSE); + backRightDrive.setDirection(DcMotor.Direction.FORWARD); + backLeftDrive.setDirection(DcMotor.Direction.REVERSE); encoderDrive(DRIVE_SPEED, distance, distance, LONG_TIMEOUT); } @@ -155,8 +155,8 @@ public class Autonomoustest extends LinearOpMode { { leftDrive.setDirection(DcMotor.Direction.REVERSE); rightDrive.setDirection(DcMotor.Direction.REVERSE); - backrightDrive.setDirection(DcMotor.Direction.REVERSE); - backleftDrive.setDirection(DcMotor.Direction.FORWARD); + backRightDrive.setDirection(DcMotor.Direction.REVERSE); + backLeftDrive.setDirection(DcMotor.Direction.FORWARD); encoderDrive(DRIVE_SPEED, distance, distance, LONG_TIMEOUT); } @@ -164,8 +164,8 @@ public class Autonomoustest extends LinearOpMode { { leftDrive.setDirection(DcMotor.Direction.FORWARD); rightDrive.setDirection(DcMotor.Direction.FORWARD); - backrightDrive.setDirection(DcMotor.Direction.REVERSE); - backleftDrive.setDirection(DcMotor.Direction.FORWARD); + backRightDrive.setDirection(DcMotor.Direction.REVERSE); + backLeftDrive.setDirection(DcMotor.Direction.FORWARD); double turning_distance = degrees * DEGREE_TOO_DISTANCE; encoderDrive(DRIVE_SPEED, turning_distance, turning_distance, LONG_TIMEOUT); } @@ -173,8 +173,8 @@ public class Autonomoustest extends LinearOpMode { public void turnRight(double degrees) { leftDrive.setDirection(DcMotor.Direction.REVERSE); rightDrive.setDirection(DcMotor.Direction.REVERSE); - backrightDrive.setDirection(DcMotor.Direction.FORWARD); - backleftDrive.setDirection(DcMotor.Direction.REVERSE); + backRightDrive.setDirection(DcMotor.Direction.FORWARD); + backLeftDrive.setDirection(DcMotor.Direction.REVERSE); double turning_distance = degrees * DEGREE_TOO_DISTANCE; encoderDrive(DRIVE_SPEED, turning_distance, turning_distance, LONG_TIMEOUT); } @@ -209,8 +209,8 @@ public class Autonomoustest extends LinearOpMode { { leftDrive = hardwareMap.get(DcMotor.class, "Drive front lt"); rightDrive = hardwareMap.get(DcMotor.class, "Drive front rt"); - backleftDrive = hardwareMap.get(DcMotor.class, "Drive back lt"); - backrightDrive = hardwareMap.get(DcMotor.class, "Drive back rt"); + backLeftDrive = hardwareMap.get(DcMotor.class, "Drive back lt"); + backRightDrive = hardwareMap.get(DcMotor.class, "Drive back rt"); colorRight = hardwareMap.get(ColorSensor.class, "color right"); colorLeft = hardwareMap.get(ColorSensor.class, "color left"); gripper = hardwareMap.get(Servo.class, "gripper"); @@ -223,21 +223,21 @@ public class Autonomoustest extends LinearOpMode { // Note: The settings here assume direct drive on left and right wheels. Gear Reduction or 90 Deg drives may require direction flips leftDrive.setDirection(DcMotor.Direction.REVERSE); rightDrive.setDirection(DcMotor.Direction.FORWARD); - backrightDrive.setDirection(DcMotor.Direction.REVERSE); - backleftDrive.setDirection(DcMotor.Direction.REVERSE); + backRightDrive.setDirection(DcMotor.Direction.REVERSE); + backLeftDrive.setDirection(DcMotor.Direction.REVERSE); arm.setDirection(DcMotor.Direction.REVERSE); leftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); rightDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); - backleftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); - backrightDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + backLeftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + backRightDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); arm.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); leftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); rightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); - backrightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); - backleftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + backRightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + backLeftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); arm.setMode(DcMotor.RunMode.RUN_USING_ENCODER); } public void testWrist() @@ -370,25 +370,25 @@ public class Autonomoustest extends LinearOpMode { // Determine new target position, and pass to motor controller newLeftTarget = leftDrive.getCurrentPosition() + (int) (leftInches * COUNTS_PER_INCH); newRightTarget = rightDrive.getCurrentPosition() + (int) (rightInches * COUNTS_PER_INCH); - newBackLeftTarget = backleftDrive.getCurrentPosition() + (int) (leftInches * COUNTS_PER_INCH); - newBackRightTarget = backrightDrive.getCurrentPosition() + (int) (rightInches * COUNTS_PER_INCH); + newBackLeftTarget = backLeftDrive.getCurrentPosition() + (int) (leftInches * COUNTS_PER_INCH); + newBackRightTarget = backRightDrive.getCurrentPosition() + (int) (rightInches * COUNTS_PER_INCH); leftDrive.setTargetPosition(newLeftTarget); rightDrive.setTargetPosition(newRightTarget); - backrightDrive.setTargetPosition(newBackRightTarget); - backleftDrive.setTargetPosition(newBackLeftTarget); + backRightDrive.setTargetPosition(newBackRightTarget); + backLeftDrive.setTargetPosition(newBackLeftTarget); // Turn On RUN_TO_POSITION leftDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION); rightDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION); - backrightDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION); - backleftDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION); + backRightDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION); + backLeftDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION); // reset the timeout time and start motion. runtime.reset(); leftDrive.setPower(Math.abs(speed)); rightDrive.setPower(Math.abs(speed)); - backrightDrive.setPower(Math.abs(speed)); - backleftDrive.setPower(Math.abs(speed)); + backRightDrive.setPower(Math.abs(speed)); + backLeftDrive.setPower(Math.abs(speed)); // keep looping while we are still active, and there is time left, and both motors are running. // Note: We use (isBusy() && isBusy()) in the loop test, which means that when EITHER motor hits @@ -398,27 +398,27 @@ public class Autonomoustest extends LinearOpMode { // onto the next step, use (isBusy() || isBusy()) in the loop test. while (opModeIsActive() && (runtime.seconds() < timeoutS) && - (leftDrive.isBusy() && rightDrive.isBusy() && backleftDrive.isBusy() && backrightDrive.isBusy())) { + (leftDrive.isBusy() && rightDrive.isBusy() && backLeftDrive.isBusy() && backRightDrive.isBusy())) { // Display it for the driver. telemetry.addData("Running to", " %7d :%7d", newLeftTarget, newRightTarget); telemetry.addData("Currently at", " at %7d :%7d", - leftDrive.getCurrentPosition(), rightDrive.getCurrentPosition(), backrightDrive.getCurrentPosition(), backleftDrive.getCurrentPosition()); + leftDrive.getCurrentPosition(), rightDrive.getCurrentPosition(), backRightDrive.getCurrentPosition(), backLeftDrive.getCurrentPosition()); telemetry.update(); } leftDrive.setPower(0); rightDrive.setPower(0); - backrightDrive.setPower(0); - backleftDrive.setPower(0); + backRightDrive.setPower(0); + backLeftDrive.setPower(0); // Turn off RUN_TO_POSITION leftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); rightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); - backleftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); - backrightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + backLeftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + backRightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); sleep(250); // optional pause after each move. } @@ -426,14 +426,14 @@ public class Autonomoustest extends LinearOpMode { public void armEncoder(double speed, double Inches, double timeoutS) { - int newarmTarget; + int newArmTarget; if (opModeIsActive()) { // Determine new target position, and pass to motor controller - newarmTarget = arm.getCurrentPosition() + (int) (Inches * COUNTS_PER_ARM_INCH); - arm.setTargetPosition(newarmTarget); + newArmTarget = arm.getCurrentPosition() + (int) (Inches * COUNTS_PER_ARM_INCH); + arm.setTargetPosition(newArmTarget); // Turn On RUN_TO_POSITION arm.setMode(DcMotor.RunMode.RUN_TO_POSITION); @@ -453,7 +453,7 @@ public class Autonomoustest extends LinearOpMode { (arm.isBusy())) { // Display it for the driver. - telemetry.addData("Running to", " %7d", newarmTarget); + telemetry.addData("Running to", " %7d", newArmTarget); telemetry.addData("Currently at", " at %7d", arm.getCurrentPosition()); telemetry.update();