Copy from 9.0.1 to here

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

@@ -93,7 +93,7 @@ public class Autonomoustest 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.2;
+    static final double DRIVE_SPEED = 0.3;
     static final double TURN_SPEED = 0.4;
 
     static final double LONG_TIMEOUT = 1000;
@@ -105,6 +105,8 @@ public class Autonomoustest extends LinearOpMode {
     public void runOpMode()
     {
         hardwareinit();
+        gripper.setPosition(1);
+        sleep(1000);
 
         // Send telemetry message to indicate successful Encoder reset
        /* telemetry.addData("Starting at",  "%7d :%7d",
@@ -204,7 +206,7 @@ public class Autonomoustest extends LinearOpMode {
 
     }
     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");
@@ -214,6 +216,8 @@ public class Autonomoustest extends LinearOpMode {
         gripper = hardwareMap.get(Servo.class, "gripper");
         arm = hardwareMap.get(DcMotor.class, "arm raise");
         wrist = hardwareMap.get(Servo.class, "wrist");
+        wrist.setPosition(1);
+        sleep(1000);
         // To drive forward, most robots need the motor on one side to be reversed, because the axles point in opposite directions.
         // When run, this OpMode should start both motors driving forward. So adjust these two lines based on your first test drive.
         // Note: The settings here assume direct drive on left and right wheels.  Gear Reduction or 90 Deg drives may require direction flips
@@ -249,91 +253,93 @@ public class Autonomoustest extends LinearOpMode {
 
     }
     public void executeAuto()
+    {
+        arm.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+        driveForward(26);
+        int blueleft = readColorLeft();
+        int blueright = readColorRight();
+        double backboard = 29;
+        if (blueleft > 50 )
         {
-            arm.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+            //telemetry.addData("color sensor","left");
-            gripper.setPosition(1);
+            if(blueleft > blueright)
-            sleep(1000);
+                telemetry.addData("color sensor","left");
-            driveForward(28);
+            turnLeft(90);
-            int blueleft = readColorLeft();
+            straightLeft(2);
-            int blueright = readColorRight();
+            driveForward(6.5);
-            double backboard = 29;
+            raisearm(80);
-           if (blueleft > 75)
-           {
-               //telemetry.addData("color sensor","left");
-               if(blueleft > blueright)
-                   telemetry.addData("color sensor","left");
-               turnLeft(90);
-               straightLeft(2);
-               driveForward(6.5);
-               raisearm(80);
-               arm.setPower(0);
-               driveForward(-23);
-               straightLeft(32);
-               turnLeft(10);
-               driveForward(18);
-               driveForward(-31);
-               straightRight(31.5);
-               raisearm(80);
-               wrist.setPosition(0);
-               raisearm(100);
-               driveForward(-1);
-               gripper.setPosition(0.25);
-               terminateOpModeNow();
-
-
-
-
-           }
-           if (blueright > 175)
-           {
-               if(blueleft < blueright)
-                   telemetry.addData("color sensor","right");
-               straightRight(11);
-               raisearm(80);
-               arm.setPower(0);
-               driveForward(-13.5);
-               turnLeft(90);
-               straightLeft(15);
-               driveForward(8);
-               driveForward(-26);
-               straightRight(22.5);
-               raisearm(80);
-               wrist.setPosition(0);
-               raisearm(100);
-               driveForward(-5);
-               gripper.setPosition(.25);
-               terminateOpModeNow();
-
-
-           }
-           else
-               telemetry.addData("position","center");
-           driveForward(4);
-           raisearm(80);
             arm.setPower(0);
-            driveForward(-8);
+            driveForward(-23);
-            straightRight(11.5);
+            straightLeft(32);
-            driveForward(-15);
+            turnLeft(10);
+            driveForward(18);
+            driveForward(-40);
+//               straightRight(31.5);
+//               raisearm(80);
+//               wrist.setPosition(0);
+//               raisearm(100);
+//               driveForward(-1);
+//               gripper.setPosition(0.25);
+            terminateOpModeNow();
+
+
+
+
+        }
+        if (blueright > 50)
+        {
+            if(blueleft < blueright)
+                telemetry.addData("color sensor","right");
+            straightRight(11);
+            raisearm(80);
+            arm.setPower(0);
+            driveForward(-15.5);
             turnLeft(90);
             straightLeft(15);
             driveForward(8);
-            driveForward(-26);
+            driveForward(-38);
-            straightRight(29);
+//               straightRight(14.5);
-            raisearm(80);
+//               raisearm(80);
-            wrist.setPosition(0);
+//               wrist.setPosition(0);
-            raisearm(100);
+//               raisearm(100);
-            gripper.setPosition(.25);
+//               gripper.setPosition(.25);
-           telemetry.update();
+//               driveForward(5);
-            sleep(250);
+//               raisearm(-200);
+            terminateOpModeNow();
+
+
+        }
+        else
+            telemetry.addData("position","center");
+        driveForward(2.5);
+        raisearm(80);
+        arm.setPower(0);
+        driveForward(-8);
+        straightRight(11.5);
+        driveForward(-15);
+        turnLeft(90);
+        straightLeft(15);
+        driveForward(8);
+        driveForward(-26);
+        straightRight(29);
+        raisearm(80);
+        wrist.setPosition(0);
+        raisearm(100);
+        gripper.setPosition(.25);
+        sleep(500);
+        driveForward(5);
+        raisearm(-270);
+        telemetry.update();
+        sleep(250);
 
 
 
 
-            //Values were created from robot with wheel issues 9/28/23
+        //Values were created from robot with wheel issues 9/28/23
 
-            telemetry.addData("Path", "Complete");
+        telemetry.addData("Path", "Complete");
-            telemetry.update();
+        telemetry.update();
-            sleep(1000);  // pause to display final telemetry message.
+        sleep(1000);  // pause to display final telemetry message.
     }