arm encoders made, tested, and implmented into main code

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

@@ -102,39 +102,10 @@ public class Autonomoustest extends LinearOpMode {
     static final double TICKS_TO_DEGREES = 0.07462686567;
 
     @Override
-    public void runOpMode() {
+    public void runOpMode()
+    {
+        hardwareinit();
 
-        // Initialize the drive system variables
-        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");
-        colorRight = hardwareMap.get(ColorSensor.class, "color right");
-        colorLeft = hardwareMap.get(ColorSensor.class, "color left");
-        gripper = hardwareMap.get(Servo.class, "gripper");
-        arm = hardwareMap.get(DcMotor.class, "arm raise");
-        wrist = hardwareMap.get(Servo.class, "wrist");
-        // 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
-        leftDrive.setDirection(DcMotor.Direction.REVERSE);
-        rightDrive.setDirection(DcMotor.Direction.FORWARD);
-        backrightDrive.setDirection(DcMotor.Direction.REVERSE);
-        backleftDrive.setDirection(DcMotor.Direction.REVERSE);
-        arm.setDirection(DcMotor.Direction.FORWARD);
-
-        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);
-        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);
-        arm.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
         // Send telemetry message to indicate successful Encoder reset
        /* telemetry.addData("Starting at",  "%7d :%7d",
                           leftDrive.getCurrentPosition(),
@@ -146,70 +117,23 @@ public class Autonomoustest extends LinearOpMode {
 
         // Wait for the game to start (driver presses PLAY)
         waitForStart();
+        {
+            //executeAuto();
+            testGripper();
+
+        }
 
         // Step through each leg of the path,
         // Note: Reverse movement is obtained by setting a negative distance (not speed)
 
-        {
-            raisearm(100);
-            int blueleft = readColorLeft();
-            int blueright = readColorRight();
-           if (blueleft > 75)
-           {
-               //telemetry.addData("color sensor","left");
-               if(blueleft > blueright)
-                   telemetry.addData("color sensor","left");
-               turnLeft(90);
-               straightLeft(2);
-               driveForward(11);
-               driveForward(-30);
-               terminateOpModeNow();
 
-
-
-
-           }
-           if (blueright > 75)
-           {
-               //telemetry.addData("color sensor", "right");
-               if(blueleft < blueright)
-                   telemetry.addData("color sensor","right");
-               straightRight(11);
-               driveForward(-17);
-               turnRight(90);
-               straightLeft(8);
-               driveForward(-28.5);
-               raisearm(80);
-               straightRight(22);
-               driveForward(-11);
-               terminateOpModeNow();
-
-
-           }
-           else
-               telemetry.addData("position","center");
-           driveForward(7);
-            driveForward(-13);
-            turnRight(90);
-            driveForward(-35);
-            raisearm(80);
-            straightRight(22.5);
-            driveForward(-11);
-
-           telemetry.update();
-            sleep(250);
-
-        }
-
-
-        //Values were created from robot with wheel issues 9/28/23
-
-        telemetry.addData("Path", "Complete");
-        telemetry.update();
-        sleep(1000);  // pause to display final telemetry message.
     }
 
-    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);
@@ -217,7 +141,8 @@ public class Autonomoustest extends LinearOpMode {
         encoderDrive(DRIVE_SPEED, distance, distance, LONG_TIMEOUT);  // S1: Forward 47 Inches with 5 Sec timeout
     }
 
-    public void straightLeft(double distance) {
+    public void straightLeft(double distance)
+    {
         leftDrive.setDirection(DcMotor.Direction.FORWARD);
         rightDrive.setDirection(DcMotor.Direction.FORWARD);
         backrightDrive.setDirection(DcMotor.Direction.FORWARD);
@@ -225,7 +150,8 @@ public class Autonomoustest extends LinearOpMode {
         encoderDrive(DRIVE_SPEED, distance, distance, LONG_TIMEOUT);
     }
 
-    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);
@@ -233,7 +159,8 @@ public class Autonomoustest extends LinearOpMode {
         encoderDrive(DRIVE_SPEED, distance, distance, LONG_TIMEOUT);
     }
 
-    public void turnLeft(double degrees) {
+    public void turnLeft(double degrees)
+    {
         leftDrive.setDirection(DcMotor.Direction.FORWARD);
         rightDrive.setDirection(DcMotor.Direction.FORWARD);
         backrightDrive.setDirection(DcMotor.Direction.REVERSE);
@@ -276,6 +203,132 @@ public class Autonomoustest extends LinearOpMode {
         armEncoder(ARM_SPEED, degrees*TICKS_TO_DEGREES, LONG_TIMEOUT);
 
     }
+    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");
+        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");
+        arm = hardwareMap.get(DcMotor.class, "arm raise");
+        wrist = hardwareMap.get(Servo.class, "wrist");
+        // 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
+        leftDrive.setDirection(DcMotor.Direction.REVERSE);
+        rightDrive.setDirection(DcMotor.Direction.FORWARD);
+        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);
+        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);
+        arm.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+    }
+    public void testWrist()
+    {
+        wrist.setPosition(0);
+        sleep(3000);
+        wrist.setPosition(1);
+        sleep(3000);
+    }
+    public void testGripper()
+    {
+        gripper.setPosition(0);
+        sleep(3000);
+        gripper.setPosition(1);
+        sleep(3000);
+    }
+    public void executeAuto()
+        {
+
+            raisearm(100);
+            wrist.setPosition(0);
+            driveForward(28);
+            int blueleft = readColorLeft();
+            int blueright = readColorRight();
+           if (blueleft > 75)
+           {
+               //telemetry.addData("color sensor","left");
+               if(blueleft > blueright)
+                   telemetry.addData("color sensor","left");
+               turnLeft(90);
+               straightLeft(2);
+               driveForward(11);
+               driveForward(-30);
+               straightLeft(32);
+               turnLeft(10);
+               driveForward(18);
+               driveForward(-31);
+               double backboard = 34;
+               straightRight(backboard);
+               raisearm(80);
+               driveForward(-1);
+               gripper.setPosition(0);
+               terminateOpModeNow();
+
+
+
+
+           }
+           if (blueright > 75)
+           {
+               //telemetry.addData("color sensor", "right");
+               if(blueleft < blueright)
+                   telemetry.addData("color sensor","right");
+               straightRight(11.5);
+               driveForward(-17);
+               turnLeft(90);
+               straightLeft(15);
+               driveForward(8);
+               driveForward(-26);
+               double backboard = 22.5;
+               straightRight(backboard);
+               raisearm(80);
+               driveForward(-5);
+               gripper.setPosition(1);
+               terminateOpModeNow();
+
+
+           }
+           else
+               telemetry.addData("position","center");
+           driveForward(7);
+            driveForward(-7);
+            straightRight(11.5);
+            driveForward(-17);
+            turnLeft(90);
+            straightLeft(15);
+            driveForward(8);
+            driveForward(-26);
+            double backboard = 29;
+            straightRight(backboard);
+            raisearm(80);
+            driveForward(-5);
+            gripper.setPosition(1);
+           telemetry.update();
+            sleep(250);
+
+
+
+
+            //Values were created from robot with wheel issues 9/28/23
+
+            telemetry.addData("Path", "Complete");
+            telemetry.update();
+            sleep(1000);  // pause to display final telemetry message.
+    }
 
 
 
@@ -400,6 +453,7 @@ public class Autonomoustest extends LinearOpMode {
 
             // Turn off RUN_TO_POSITION
             arm.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+
         }
     }
 }