arm encoders made, tested, and implmented into main code

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

@@ -79,8 +79,6 @@ public class Autonomoustest extends LinearOpMode {
     private DcMotor arm = null;
 
 
-
-
     private ElapsedTime runtime = new ElapsedTime();
 
     // Calculate the COUNTS_PER_INCH for your specific drive train.
@@ -94,11 +92,16 @@ public class Autonomoustest extends LinearOpMode {
     static final double WHEEL_DIAMETER_INCHES = 3.77953;     // For figuring circumference
     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 TURN_SPEED = 0.4;
 
     static final double LONG_TIMEOUT = 1000;
-    static final double     DEGREE_TOO_DISTANCE = 0.21944444444;   @Override
+    static final double DEGREE_TOO_DISTANCE = 0.21944444444;
+    static final double ARM_SPEED = .1;
+    static final double TICKS_TO_DEGREES = 0.07462686567;
+
+    @Override
     public void runOpMode() {
 
         // Initialize the drive system variables
@@ -118,7 +121,7 @@ public class Autonomoustest extends LinearOpMode {
         rightDrive.setDirection(DcMotor.Direction.FORWARD);
         backrightDrive.setDirection(DcMotor.Direction.REVERSE);
         backleftDrive.setDirection(DcMotor.Direction.REVERSE);
-        arm.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);
@@ -146,68 +149,57 @@ public class Autonomoustest extends LinearOpMode {
 
         // Step through each leg of the path,
         // Note: Reverse movement is obtained by setting a negative distance (not speed)
-        raisearm(1);
-        arm.setPower(0.001);
-        sleep(10000);
-        //driveForward(2);
-//        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);
-//               turnLeft(180);
-//               driveForward(19);
-//               straightLeft(7);
-//               //use arm to place pixel
-//               straightRight(34);
-//               driveForward(11);
-//               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);
-//               //use arm to place pixel
-//               straightRight(20);
-//               driveForward(11);
-//               terminateOpModeNow();
-//
-//
-//           }
-//           else
-//               telemetry.addData("position","center");
-//           driveForward(11);
-//            driveForward(-17);
-//            turnRight(90);
-//            driveForward(39);
-//            straightRight(3.5);
-//            //use arm to place pixel
-//            straightRight(23.5);
-//            driveForward(11);
-//
-//           telemetry.update();
-//            sleep(250);
-//
-//        }
 
+        {
+            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
@@ -217,32 +209,31 @@ public class Autonomoustest extends LinearOpMode {
         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);
         backleftDrive.setDirection(DcMotor.Direction.REVERSE);
         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);
         backleftDrive.setDirection(DcMotor.Direction.REVERSE);
         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);
         backleftDrive.setDirection(DcMotor.Direction.FORWARD);
         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);
@@ -250,8 +241,8 @@ public class Autonomoustest extends LinearOpMode {
         double turning_distance = degrees * DEGREE_TOO_DISTANCE;
         encoderDrive(DRIVE_SPEED, turning_distance, turning_distance, LONG_TIMEOUT);
     }
-    public void turnRight(double degrees)
+
-    {
+    public void turnRight(double degrees) {
         leftDrive.setDirection(DcMotor.Direction.REVERSE);
         rightDrive.setDirection(DcMotor.Direction.REVERSE);
         backrightDrive.setDirection(DcMotor.Direction.FORWARD);
@@ -259,8 +250,8 @@ public class Autonomoustest extends LinearOpMode {
         double turning_distance = degrees * DEGREE_TOO_DISTANCE;
         encoderDrive(DRIVE_SPEED, turning_distance, turning_distance, LONG_TIMEOUT);
     }
-    public int readColorRight()
+
-    {
+    public int readColorRight() {
         telemetry.addData("Clear", colorRight.alpha());
         telemetry.addData("Red  ", colorRight.red());
         telemetry.addData("Green", colorRight.green());
@@ -268,8 +259,8 @@ public class Autonomoustest extends LinearOpMode {
         int bluenumber = colorRight.blue();
         return bluenumber;
     }
-    public int readColorLeft()
+
-    {
+    public int readColorLeft() {
         telemetry.addData("Clear Left", colorLeft.alpha());
         telemetry.addData("Red left ", colorLeft.red());
         telemetry.addData("Green left", colorLeft.green());
@@ -279,14 +270,14 @@ public class Autonomoustest extends LinearOpMode {
         return bluenumber;
 
 
+    }
+
+    public void raisearm(int degrees) {
+        armEncoder(ARM_SPEED, degrees*TICKS_TO_DEGREES, LONG_TIMEOUT);
 
     }
 
-    public void raisearm(long seconds)
+
-    {
-        arm.setPower(.1);
-        sleep(seconds * 1000);
-    }
 
 
 
@@ -308,6 +299,7 @@ public class Autonomoustest extends LinearOpMode {
         int newBackLeftTarget;
         int newbackRightTarget;
 
+
         if (opModeIsActive()) {
 
             // Determine new target position, and pass to motor controller
@@ -341,7 +333,7 @@ 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() && backrightDrive.isBusy())) {
 
                 // Display it for the driver.
                 telemetry.addData("Running to", " %7d :%7d", newLeftTarget, newRightTarget);
@@ -351,7 +343,6 @@ public class Autonomoustest extends LinearOpMode {
             }
 
 
-
             leftDrive.setPower(0);
             rightDrive.setPower(0);
             backrightDrive.setPower(0);
@@ -367,4 +358,48 @@ public class Autonomoustest extends LinearOpMode {
             sleep(250);   // optional pause after each move.
         }
     }
+
+    public void armEncoder(double speed,
+                           double Inches, double timeoutS) {
+        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);
+
+            // Turn On RUN_TO_POSITION
+            arm.setMode(DcMotor.RunMode.RUN_TO_POSITION);
+
+            // reset the timeout time and start motion.
+            runtime.reset();
+            arm.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
+            // its target position, the motion will stop.  This is "safer" in the event that the robot will
+            // always end the motion as soon as possible.
+            // However, if you require that BOTH motors have finished their moves before the robot continues
+            // onto the next step, use (isBusy() || isBusy()) in the loop test.
+            while (opModeIsActive() &&
+                    (runtime.seconds() < timeoutS) &&
+                    (arm.isBusy())) {
+
+                // Display it for the driver.
+                telemetry.addData("Running to", " %7d", newarmTarget);
+                telemetry.addData("Currently at", " at %7d",
+                        arm.getCurrentPosition());
+                telemetry.update();
+            }
+
+
+            arm.setPower(0);
+
+
+            // Turn off RUN_TO_POSITION
+            arm.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+        }
+    }
 }