starighten added, center added, tweakes too both backstage code made but undtestested for some

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Blue.java

@@ -100,6 +100,8 @@ public class Blue extends LinearOpMode {
             (WHEEL_DIAMETER_INCHES * Math.PI);
     static final double COUNTS_PER_ARM_INCH = (COUNTS_PER_MOTOR_REV * DRIVE_GEAR_REDUCTION) / (2.7 * Math.PI);
     static final double DRIVE_SPEED = 0.3;
+
+    static final double DRIVE_SPEED_SLOW = 0.2;
     static final double TURN_SPEED = 0.4;
 
     static final double LONG_TIMEOUT = 1000;
@@ -143,7 +145,7 @@ public class Blue extends LinearOpMode {
         rightDrive.setDirection(DcMotor.Direction.FORWARD);
         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
+        encoderDrive(DRIVE_SPEED, distance, distance, LONG_TIMEOUT, false);  // S1: Forward 47 Inches with 5 Sec timeout
     }
 
     public void straightLeft(double distance)
@@ -152,16 +154,28 @@ public class Blue extends LinearOpMode {
         rightDrive.setDirection(DcMotor.Direction.FORWARD);
         backrightDrive.setDirection(DcMotor.Direction.FORWARD);
         backleftDrive.setDirection(DcMotor.Direction.REVERSE);
-        encoderDrive(DRIVE_SPEED, distance, distance, LONG_TIMEOUT);
+        encoderDrive(DRIVE_SPEED, distance, distance, LONG_TIMEOUT, true);
     }
 
+    public void straightLeftOnPower(double speed) {
+        leftDrive.setDirection(DcMotor.Direction.FORWARD);
+        rightDrive.setDirection(DcMotor.Direction.FORWARD);
+        backrightDrive.setDirection(DcMotor.Direction.FORWARD);
+        backleftDrive.setDirection(DcMotor.Direction.REVERSE);
+        leftDrive.setPower(speed * 1.05);
+        rightDrive.setPower(speed * 1.05);
+        backrightDrive.setPower(speed);
+        backleftDrive.setPower(speed);
+    }
+
+
     public void straightRight(double distance)
     {
         leftDrive.setDirection(DcMotor.Direction.REVERSE);
         rightDrive.setDirection(DcMotor.Direction.REVERSE);
         backrightDrive.setDirection(DcMotor.Direction.REVERSE);
         backleftDrive.setDirection(DcMotor.Direction.FORWARD);
-        encoderDrive(DRIVE_SPEED, distance, distance, LONG_TIMEOUT);
+        encoderDrive(DRIVE_SPEED, distance, distance, LONG_TIMEOUT, true);
     }
 
     public void turnLeft(double degrees)
@@ -171,7 +185,7 @@ public class Blue extends LinearOpMode {
         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);
+        encoderDrive(DRIVE_SPEED, turning_distance, turning_distance, LONG_TIMEOUT, false);
     }
 
     public void turnRight(double degrees) {
@@ -180,9 +194,36 @@ public class Blue extends LinearOpMode {
         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);
+        encoderDrive(DRIVE_SPEED, turning_distance, turning_distance, LONG_TIMEOUT, false);
     }
 
+    public void straighten(Double distance)
+
+    {
+        driveForward(0);
+        double D1 = distanceLeft.getDistance(DistanceUnit.INCH);
+        driveForward(distance);
+        double D2 = distanceLeft.getDistance(DistanceUnit.INCH);
+        double rad = Math.atan2(D1 - D2, distance);
+        double degrees = Math.toDegrees(rad);
+        turnRight(degrees);
+        telemetry.addData("d1", D1);
+        telemetry.addData("d2", D2);
+        telemetry.addData("Calibration deg", degrees);
+        telemetry.update();
+        sleep(1000);
+    }
+
+    public void centerLeft()
+    {
+        double leftDistance = distanceLeft.getDistance(DistanceUnit.INCH);
+        straightLeft(leftDistance - 3);
+        telemetry.addData("leftDistance",leftDistance);
+        telemetry.addData("moving left x inches",leftDistance - 3);
+        telemetry.update();
+        straightLeft(0.0);
+        sleep(1000);
+    }
 
     public void raisearm(int degrees) {
         armEncoder(ARM_SPEED, degrees*TICKS_TO_DEGREES, LONG_TIMEOUT);
@@ -238,61 +279,109 @@ public class Blue extends LinearOpMode {
     @SuppressLint("SuspiciousIndentation")
     public void executeAuto()
     {
-
+        while (true)
-        arm.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
-        driveForward(26);
-        sleep(500);
-
-        int distanceleft = (int)distanceLeft.getDistance(DistanceUnit.INCH);
-        int distanceright = (int)distanceRight.getDistance(DistanceUnit.INCH);
-        telemetry.addData("color left sensor",distanceleft);
-        telemetry.addData("color right sensor",distanceright);
-        telemetry.update();
-
-        if (distanceleft < 7)
         {
-            telemetry.addData("postion","left");
+            int distright = (int)distanceRight.getDistance(DistanceUnit.INCH);
+            telemetry.addData("right dist", distright);
             telemetry.update();
-            turnLeft(90);
-            straightLeft(2);
-            driveForward(6.5);
-            raisearm(45);
-            arm.setPower(0);
-            driveForward(-8);
-            terminateOpModeNow();
-
-
-
-
         }
-        if (distanceright < 7)
+
-        {
+//        arm.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
-            telemetry.addData("postion", "right");
+//        driveForward(26);
-            telemetry.update();
+//        sleep(500);
-            straightRight(12);
+//
-            raisearm(80);
+//        int distanceleft = (int)distanceLeft.getDistance(DistanceUnit.INCH);
-            arm.setPower(0);
+//        int distanceright = (int)distanceRight.getDistance(DistanceUnit.INCH);
-            driveForward(-10);
+//        telemetry.addData("color left sensor",distanceleft);
-            terminateOpModeNow();
+//        telemetry.addData("color right sensor",distanceright);
+//        telemetry.update();
 
 
-        }
+//        if (distanceleft < 7)
-        else
+//        {
-            telemetry.addData("postion","center");
+//            telemetry.addData("postion","left");
-        telemetry.update();
+//            telemetry.update();
-        driveForward(3.5);
+//            turnLeft(90);
-        raisearm(80);
+//            straightLeft(2);
-        arm.setPower(0);
+//            driveForward(6.5);
-        driveForward(-8);
+//            raisearm(45);
-        terminateOpModeNow();
+//            arm.setPower(0);
+//            sleep(500);
+//            driveForward(-20);
+//
+//            do {
+//                straightLeftOnPower(DRIVE_SPEED_SLOW);
+//                distanceleft = (int) distanceLeft.getDistance(DistanceUnit.INCH);
+//
+//            } while (distanceleft >= 4);
+//            straighten(12.0);
+//            centerLeft();
+//            driveForward(88);
+//            sleep(1000);
+//            wrist.setPosition(.465);
+//            gripper.setPosition(1);
+//            sleep(1000);
+//            driveForward(-3);
+//            terminateOpModeNow();
+//
+//
+//
+//
+//        }
+//        if (distanceright < 7)
+//        {
+//            telemetry.addData("postion", "right");
+//            telemetry.update();
+//            straightRight(12);
+//            raisearm(80);
+//            arm.setPower(0);
+//            driveForward(-22);
+//            turnLeft(90);
+//            do {
+//                straightLeftOnPower(DRIVE_SPEED_SLOW);
+//                distanceleft = (int) distanceLeft.getDistance(DistanceUnit.INCH);
+//
+//            } while (distanceleft >= 6);
+//            straighten(12.0);
+//            centerLeft();
+//            driveForward(98);
+//            sleep(1000);
+//            wrist.setPosition(.465);
+//            gripper.setPosition(1);
+//            sleep(1000);
+//            driveForward(-3);
+//            terminateOpModeNow();
+//
+//
+//        }
+//        else
+//            telemetry.addData("postion","center");
+//        telemetry.update();
+//        driveForward(3.5);
+//        raisearm(80);
+//        arm.setPower(0);
+//        driveForward(-8);
+//        turnLeft(90);
+//        driveForward(-12);
+//        do {
+//            straightLeftOnPower(DRIVE_SPEED_SLOW);
+//            distanceleft = (int) distanceLeft.getDistance(DistanceUnit.INCH);
+//
+//        } while (distanceleft >= 6);
+//        straighten(12.0);
+//        centerLeft();
+//        driveForward(88);
+//        sleep(1000);
+//        wrist.setPosition(.465);
+//        gripper.setPosition(1);
+//        sleep(1000);
+//        driveForward(-3);
+//        terminateOpModeNow();
 
 
 
         //Values were created from robot with wheel issues 9/28/23
-
+        // pause to display final telemetry message.
-        telemetry.addData("Path", "Complete");
-        telemetry.update();
-//        sleep(1000);  // pause to display final telemetry message.
     }
 
 
@@ -311,7 +400,7 @@ public class Blue extends LinearOpMode {
 
     public void encoderDrive(double speed,
                              double leftInches, double rightInches,
-                             double timeoutS) {
+                             double timeoutS, boolean addJuice) {
         int newLeftTarget;
         int newRightTarget;
         int newBackLeftTarget;
@@ -338,8 +427,13 @@ public class Blue extends LinearOpMode {
 
             // reset the timeout time and start motion.
             runtime.reset();
+            if(addJuice) {
+                leftDrive.setPower(Math.abs(speed * 1.05));
+                rightDrive.setPower(Math.abs(speed * 1.05));
+            } else {
                 leftDrive.setPower(Math.abs(speed));
                 rightDrive.setPower(Math.abs(speed));
+            }
             backrightDrive.setPower(Math.abs(speed));
             backleftDrive.setPower(Math.abs(speed));
 

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BlueBackStage.java

@@ -94,7 +94,9 @@ public class BlueBackStage extends LinearOpMode {
     static final double COUNTS_PER_INCH = (COUNTS_PER_MOTOR_REV * DRIVE_GEAR_REDUCTION) /
             (WHEEL_DIAMETER_INCHES * Math.PI);
     static final double COUNTS_PER_ARM_INCH = (COUNTS_PER_MOTOR_REV * DRIVE_GEAR_REDUCTION) / (2.7 * Math.PI);
-    static final double DRIVE_SPEED = 0.3;
+    double DRIVE_SPEED = 0.5;
+
+    static final double DRIVE_SPEED_SLOW = .25;
     static final double TURN_SPEED = 0.4;
 
     static final double LONG_TIMEOUT = 1000;
@@ -103,8 +105,7 @@ public class BlueBackStage extends LinearOpMode {
     static final double TICKS_TO_DEGREES = 0.07462686567;
 
     @Override
-    public void runOpMode()
+    public void runOpMode() {
-    {
         hardwareinit();
 
         // Send telemetry message to indicate successful Encoder reset
@@ -129,54 +130,47 @@ public class BlueBackStage extends LinearOpMode {
 
     }
 
-
 //
 
-    public void driveForward(double distance)
+    public void driveForward(double distance) {
-    {
         leftDrive.setDirection(DcMotor.Direction.REVERSE);
         rightDrive.setDirection(DcMotor.Direction.FORWARD);
         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
+        encoderDrive(DRIVE_SPEED, distance, distance, LONG_TIMEOUT, false);  // S1: Forward 47 Inches with 5 Sec timeout
     }
 
-    public void straightLeft(double distance)
+    public void driveForwardSpeed(double distance, double speed) {
-    {
+        leftDrive.setDirection(DcMotor.Direction.REVERSE);
+        rightDrive.setDirection(DcMotor.Direction.FORWARD);
+        backrightDrive.setDirection(DcMotor.Direction.REVERSE);
+        backleftDrive.setDirection(DcMotor.Direction.REVERSE);
+        encoderDrive(speed, distance, distance, LONG_TIMEOUT, false);  // S1: Forward 47 Inches with 5 Sec timeout
+    }
+
+    public void straightLeft(double distance) {
         leftDrive.setDirection(DcMotor.Direction.FORWARD);
         rightDrive.setDirection(DcMotor.Direction.FORWARD);
         backrightDrive.setDirection(DcMotor.Direction.FORWARD);
         backleftDrive.setDirection(DcMotor.Direction.REVERSE);
-        encoderDrive(DRIVE_SPEED, distance, distance, LONG_TIMEOUT);
+        encoderDrive(DRIVE_SPEED, distance, distance, LONG_TIMEOUT, true);
     }
 
-    public void straightRight(double distance)
+    public void straightRight(double distance) {
-    {
         leftDrive.setDirection(DcMotor.Direction.REVERSE);
         rightDrive.setDirection(DcMotor.Direction.REVERSE);
         backrightDrive.setDirection(DcMotor.Direction.REVERSE);
         backleftDrive.setDirection(DcMotor.Direction.FORWARD);
-        encoderDrive(DRIVE_SPEED, distance, distance, LONG_TIMEOUT);
+        encoderDrive(DRIVE_SPEED, distance, distance, LONG_TIMEOUT, true);
     }
 
-    public void turnLeft(double degrees)
+    public void turnRight(double degrees) {
-    {
-        leftDrive.setDirection(DcMotor.Direction.FORWARD);
-        rightDrive.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);
-    }
-
-    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);
         double turning_distance = degrees * DEGREE_TOO_DISTANCE;
-        encoderDrive(DRIVE_SPEED, turning_distance, turning_distance, LONG_TIMEOUT);
+        encoderDrive(DRIVE_SPEED, turning_distance, turning_distance, LONG_TIMEOUT, false);
     }
 
 
@@ -184,8 +178,8 @@ public class BlueBackStage extends LinearOpMode {
         armEncoder(ARM_SPEED, degrees * TICKS_TO_DEGREES, LONG_TIMEOUT);
 
     }
-    public void hardwareinit()
+
-    {
+    public void hardwareinit() {
         leftDrive = hardwareMap.get(DcMotor.class, "Drive front lt");
         rightDrive = hardwareMap.get(DcMotor.class, "Drive front rt");
         backleftDrive = hardwareMap.get(DcMotor.class, "Drive back lt");
@@ -204,6 +198,10 @@ public class BlueBackStage extends LinearOpMode {
         backrightDrive.setDirection(DcMotor.Direction.REVERSE);
         backleftDrive.setDirection(DcMotor.Direction.REVERSE);
         arm.setDirection(DcMotor.Direction.REVERSE);
+        leftDrive.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+        rightDrive.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+        backleftDrive.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+        backrightDrive.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
 
         leftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
         rightDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
@@ -218,30 +216,28 @@ public class BlueBackStage extends LinearOpMode {
         backleftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
         arm.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
     }
-    public void testWrist()
+
-    {
+    public void testWrist() {
         wrist.setPosition(0);
         sleep(3000);
         wrist.setPosition(1);
         sleep(3000);
     }
-    public void testGripper()
+
-    {
+    public void testGripper() {
         gripper.setPosition(0);
         sleep(3000);
         gripper.setPosition(1);
         sleep(3000);
     }
 
-    public void executeAuto()
+    public void executeAuto() {
-   {
         arm.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
         driveForward(26);
         sleep(500);
         int distanceleft = (int) distanceLeft.getDistance(DistanceUnit.INCH);
         int distanceright = (int) distanceRight.getDistance(DistanceUnit.INCH);
-        if (distanceleft < 7)
+        if (distanceleft < 7) {
-        {
             telemetry.addData("position", "left");
             telemetry.update();
             straightLeft(13.5);
@@ -251,7 +247,8 @@ public class BlueBackStage extends LinearOpMode {
             turnRight(90);
             straightRight(15);
             driveForward(-18);
-            straightLeft(20.25);
+            DRIVE_SPEED = .25;
+            straightLeft(22.25);
             driveForward(-.30);
             raisearm(80);
             wrist.setPosition(0);
@@ -262,16 +259,13 @@ public class BlueBackStage extends LinearOpMode {
             raisearm(-50);
             wrist.setPosition(1);
             raisearm(-70);
-            straightLeft(29);
+            straightLeft(35);
-            driveForward(-10);
+            driveForward(-22);
             terminateOpModeNow();
 
 
-
-
         }
-        if (distanceright < 7)
+        if (distanceright < 7) {
-        {
             telemetry.addData("position", "right");
             telemetry.update();
             turnRight(88);
@@ -293,12 +287,11 @@ public class BlueBackStage extends LinearOpMode {
             terminateOpModeNow();
 
 
-        }
+        } else {
-        else
             telemetry.addData("position", "center");
             telemetry.update();
-        driveForward(3.5);
+            driveForward(5);
-        raisearm(80);;
+            raisearm(80);
             arm.setPower(0);
             driveForward(-8);
             straightLeft(11.5);
@@ -306,6 +299,7 @@ public class BlueBackStage extends LinearOpMode {
             turnRight(90);
             straightRight(15);
             driveForward(-18);
+            DRIVE_SPEED = .25;
             straightLeft(29);
             driveForward(-2);
             raisearm(80);
@@ -316,11 +310,10 @@ public class BlueBackStage extends LinearOpMode {
             raisearm(-50);
             wrist.setPosition(1);
             raisearm(-70);
-       straightLeft(19);
+            straightLeft(28);
-       driveForward(-10);
+            driveForward(-25);
             terminateOpModeNow();
-
+        }
-
 
 
         //Values were created from robot with wheel issues 9/28/23
@@ -343,16 +336,28 @@ public class BlueBackStage extends LinearOpMode {
      *  3) Driver stops the opmode running.
 
      */
+//    public void encoderDrive(double speed, double leftInches, double rightInches, double timeout, boolean addJuice) {
+//
+//        if(leftInches < 4) {
+//            encodedDriver(speed, leftInches, rightInches, timeout, addJuice);
+//
+//        } else {
+//            // first n-4 is 50% speed (regional tournament speed)
+//            encodedDriver(DRIVE_SPEED, leftInches-10, rightInches-4, timeout, addJuice);
+//            // trailing result is 30% speed (base speed)
+//            encodedDriver(.3, 10, 10, timeout, addJuice);
+//
+//        }
+//}
 
     public void encoderDrive(double speed,
                              double leftInches, double rightInches,
-                             double timeoutS) {
+                             double timeoutS, boolean addJuice) {
         int newLeftTarget;
         int newRightTarget;
         int newBackLeftTarget;
         int newbackRightTarget;
 
-
         if (opModeIsActive()) {
 
             // Determine new target position, and pass to motor controller
@@ -373,8 +378,13 @@ public class BlueBackStage extends LinearOpMode {
 
             // reset the timeout time and start motion.
             runtime.reset();
+            if(addJuice) {
+                leftDrive.setPower(Math.abs(speed * 1.05));
+                rightDrive.setPower(Math.abs(speed * 1.05));
+            } else {
                 leftDrive.setPower(Math.abs(speed));
                 rightDrive.setPower(Math.abs(speed));
+            }
             backrightDrive.setPower(Math.abs(speed));
             backleftDrive.setPower(Math.abs(speed));
 
@@ -395,7 +405,6 @@ public class BlueBackStage extends LinearOpMode {
                 telemetry.update();
             }
 
-
             leftDrive.setPower(0);
             rightDrive.setPower(0);
             backrightDrive.setPower(0);
@@ -408,7 +417,7 @@ public class BlueBackStage extends LinearOpMode {
             backleftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
             backrightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
 
-            sleep(250);   // optional pause after each move.
+//            sleep(250);   // optional pause after each move.
         }
     }
 

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Red.java

@@ -97,6 +97,8 @@ public class Red extends LinearOpMode {
             (WHEEL_DIAMETER_INCHES * Math.PI);
     static final double COUNTS_PER_ARM_INCH = (COUNTS_PER_MOTOR_REV * DRIVE_GEAR_REDUCTION) / (2.7 * Math.PI);
     static final double DRIVE_SPEED = 0.3;
+
+    static final double DRIVE_SPEED_SLOW = 0.25;
     static final double TURN_SPEED = 0.4;
 
     static final double LONG_TIMEOUT = 1000;
@@ -219,6 +221,47 @@ public class Red extends LinearOpMode {
         backleftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
         arm.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
     }
+
+    public void straightRightOnPower(double speed) {
+        speed *= -1;
+        leftDrive.setDirection(DcMotor.Direction.FORWARD);
+        rightDrive.setDirection(DcMotor.Direction.FORWARD);
+        backrightDrive.setDirection(DcMotor.Direction.FORWARD);
+        backleftDrive.setDirection(DcMotor.Direction.REVERSE);
+        leftDrive.setPower(speed * 1.05);
+        rightDrive.setPower(speed * 1.05);
+        backrightDrive.setPower(speed);
+        backleftDrive.setPower(speed);
+    }
+
+    public void straighten(Double distance)
+
+    {
+        driveForward(0);
+        double D1 = distanceRight.getDistance(DistanceUnit.INCH);
+        driveForward(distance);
+        double D2 = distanceRight.getDistance(DistanceUnit.INCH);
+        double rad = Math.atan2(D1 - D2, distance);
+        double degrees = Math.toDegrees(rad);
+        turnRight(-degrees);
+        telemetry.addData("d1", D1);
+        telemetry.addData("d2", D2);
+        telemetry.addData("Calibration deg", degrees);
+        telemetry.update();
+        sleep(3000);
+    }
+
+    public void centerRight()
+    {
+        double rightDistance = distanceRight.getDistance(DistanceUnit.INCH);
+        straightRight(rightDistance - 3);
+        telemetry.addData("rightDistance",rightDistance);
+        telemetry.addData("moving left x inches",rightDistance - 3);
+        telemetry.update();
+        straightRight(0.0);
+        sleep(3000);
+    }
+
     public void testWrist()
     {
         wrist.setPosition(0);
@@ -235,6 +278,13 @@ public class Red extends LinearOpMode {
     }
     public void executeAuto()
     {
+//        while (true)
+//        {
+//            int distanceright = (int)distanceRight.getDistance(DistanceUnit.INCH);
+//            telemetry.addData("right", distanceright);
+//            telemetry.update();
+//            sleep(500);
+//        }
         arm.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
         driveForward(26);
         sleep(500);
@@ -247,14 +297,27 @@ public class Red extends LinearOpMode {
             straightLeft(12);
             raisearm(80);
             arm.setPower(0);
+            sleep(500);
+            turnRight(90);
             driveForward(-10);
+            do {
+                straightRightOnPower(DRIVE_SPEED_SLOW);
+                distanceright = (int) distanceRight.getDistance(DistanceUnit.INCH);
+
+            } while (distanceright >= 4);
+            straighten(12.0);
+            centerRight();
+            driveForward(88);
+            sleep(1000);
+            wrist.setPosition(.465);
+            gripper.setPosition(1);
+            sleep(1000);
+            driveForward(-3);
             terminateOpModeNow();
 
 
-
-
         }
-        if (distanceright < 7)
+        if (distanceright < 7) //right
         {
             telemetry.addData("position","right");
             telemetry.update();
@@ -263,7 +326,21 @@ public class Red extends LinearOpMode {
             driveForward(6.5);
             raisearm(45);
             arm.setPower(0);
-            driveForward(-10);
+            sleep(500);
+            driveForward(-6);
+            do {
+                straightRightOnPower(DRIVE_SPEED_SLOW);
+                distanceright = (int) distanceRight.getDistance(DistanceUnit.INCH);
+
+            } while (distanceright >= 6);
+            straighten(12.0);
+            centerRight();
+            driveForward(88);
+            sleep(1000);
+            wrist.setPosition(.465);
+            gripper.setPosition(1);
+            sleep(1000);
+            driveForward(-3);
             terminateOpModeNow();
 
 
@@ -275,11 +352,28 @@ public class Red extends LinearOpMode {
         raisearm(80);
         arm.setPower(0);
         driveForward(-8);
+        turnRight(90);
+        driveForward(-12);
+        do {
+            straightRightOnPower(DRIVE_SPEED_SLOW);
+            distanceright = (int) distanceRight.getDistance(DistanceUnit.INCH);
+
+        } while (distanceright >= 6);
+        straighten(12.0);
+        centerRight();
+        //straightLeft(3);
+        driveForward(88);
+        sleep(1000);
+        wrist.setPosition(.465);
+        gripper.setPosition(1);
+        sleep(1000);
+        driveForward(-3);
         terminateOpModeNow();
 
 
 
 
+
         //Values were created from robot with wheel issues 9/28/23
 
         telemetry.addData("Path", "Complete");

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RedBackStage.java

@@ -97,7 +97,7 @@ public class RedBackStage extends LinearOpMode {
     static final double COUNTS_PER_INCH = (COUNTS_PER_MOTOR_REV * DRIVE_GEAR_REDUCTION) /
             (WHEEL_DIAMETER_INCHES * Math.PI);
     static final double COUNTS_PER_ARM_INCH = (COUNTS_PER_MOTOR_REV * DRIVE_GEAR_REDUCTION) / (2.7 * Math.PI);
-    static final double DRIVE_SPEED = 0.3;
+    double DRIVE_SPEED = 0.5;
     static final double TURN_SPEED = 0.4;
 
     static final double LONG_TIMEOUT = 1000;
@@ -234,7 +234,10 @@ public class RedBackStage extends LinearOpMode {
     @SuppressLint("SuspiciousIndentation")
     public void executeAuto()
     {
-
+        backrightDrive.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+        backleftDrive.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+        leftDrive.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+        rightDrive.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
         arm.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
         driveForward(26);
         sleep(500);
@@ -257,6 +260,7 @@ public class RedBackStage extends LinearOpMode {
             driveForward(-21);
             straightLeft(34);
             driveForward(-10);
+            DRIVE_SPEED = .3;
             straightRight(35);
             driveForward(-5);
             raisearm(80);
@@ -267,6 +271,11 @@ public class RedBackStage extends LinearOpMode {
             driveForward(4.5);
             sleep(500);
             driveForward(1.5);
+            raisearm(-50);
+            sleep(500);
+            wrist.setPosition(1);
+            raisearm(-70);
+            driveForward(-6);
             terminateOpModeNow();
 
 
@@ -284,13 +293,20 @@ public class RedBackStage extends LinearOpMode {
             turnLeft(90);
             straightLeft(15);
             driveForward(-20.5);
+            DRIVE_SPEED = .3;
             straightRight(19);
             driveForward(-1.5);
             raisearm(80);
             wrist.setPosition(0);
             raisearm(100);
             gripper.setPosition(1);
+            sleep(500);
             driveForward(8.5);
+            raisearm(-50);
+            sleep(500);
+            wrist.setPosition(1);
+            raisearm(-70);
+            driveForward(-6);
             terminateOpModeNow();
 
 
@@ -307,14 +323,20 @@ else
             turnLeft(90);
             straightLeft(15);
             driveForward(-18);
+            DRIVE_SPEED = .3;
             straightRight(29);
-            turnRight(10);
+            turnRight(5);
             raisearm(80);
             wrist.setPosition(0);
             raisearm(100);
             gripper.setPosition(1);
             sleep(500);
             driveForward(5);
+            raisearm(-50);
+            sleep(500);
+            wrist.setPosition(1);
+            raisearm(-70);
+            driveForward(-6);
             terminateOpModeNow();