Merge remote-tracking branch 'origin/branch-cooper' into branch-cooper

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

@@ -94,11 +94,13 @@ 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;
+    @Override
     public void runOpMode() {
 
         // Initialize the drive system variables
@@ -146,10 +148,8 @@ 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);
+        raiseArm(10);
-        arm.setPower(0.001);
+
-        sleep(10000);
-        //driveForward(2);
 //        driveForward(28);
 //        {
 //            int blueleft = readColorLeft();
@@ -223,6 +223,7 @@ public class Autonomoustest extends LinearOpMode {
         rightDrive.setDirection(DcMotor.Direction.FORWARD);
         backrightDrive.setDirection(DcMotor.Direction.REVERSE);
         backleftDrive.setDirection(DcMotor.Direction.REVERSE);
+        arm.setPower(0);
         encoderDrive(DRIVE_SPEED,  distance,  distance, LONG_TIMEOUT);  // S1: Forward 47 Inches with 5 Sec timeout
     }
     public void straightLeft(double distance)
@@ -231,6 +232,7 @@ public class Autonomoustest extends LinearOpMode {
         rightDrive.setDirection(DcMotor.Direction.FORWARD);
         backrightDrive.setDirection(DcMotor.Direction.FORWARD);
         backleftDrive.setDirection(DcMotor.Direction.REVERSE);
+        arm.setPower(0);
         encoderDrive(DRIVE_SPEED,  distance,  distance, LONG_TIMEOUT);
     }
     public void straightRight(double distance)
@@ -239,6 +241,7 @@ public class Autonomoustest extends LinearOpMode {
         rightDrive.setDirection(DcMotor.Direction.REVERSE);
         backrightDrive.setDirection(DcMotor.Direction.REVERSE);
         backleftDrive.setDirection(DcMotor.Direction.FORWARD);
+        arm.setPower(0);
         encoderDrive(DRIVE_SPEED,  distance,  distance, LONG_TIMEOUT);
     }
     public void turnLeft(double degrees)
@@ -247,6 +250,7 @@ public class Autonomoustest extends LinearOpMode {
         rightDrive.setDirection(DcMotor.Direction.FORWARD);
         backrightDrive.setDirection(DcMotor.Direction.REVERSE);
         backleftDrive.setDirection(DcMotor.Direction.FORWARD);
+        arm.setPower(0);
         double turning_distance = degrees * DEGREE_TOO_DISTANCE;
         encoderDrive(DRIVE_SPEED,  turning_distance,  turning_distance, LONG_TIMEOUT);
     }
@@ -256,6 +260,7 @@ public class Autonomoustest extends LinearOpMode {
         rightDrive.setDirection(DcMotor.Direction.REVERSE);
         backrightDrive.setDirection(DcMotor.Direction.FORWARD);
         backleftDrive.setDirection(DcMotor.Direction.REVERSE);
+        arm.setPower(0);
         double turning_distance = degrees * DEGREE_TOO_DISTANCE;
         encoderDrive(DRIVE_SPEED,  turning_distance,  turning_distance, LONG_TIMEOUT);
     }
@@ -281,13 +286,22 @@ public class Autonomoustest extends LinearOpMode {
 
 
     }
-
+    public void raiseArm(double distance)
-    public void raisearm(long seconds)
     {
-        arm.setPower(.1);
+        leftDrive.setPower(0);
-        sleep(seconds * 1000);
+        rightDrive.setPower(0);
+        backleftDrive.setPower(0);
+        backrightDrive.setPower(0);
+        arm.setDirection(DcMotorSimple.Direction.REVERSE);
+        encoderDrive(DRIVE_SPEED, distance,distance,LONG_TIMEOUT );
     }
 
+//    public void raisearm(long seconds)
+//    {
+//        arm.setPower(.1);
+//        sleep(seconds * 1000);
+//    }
+
 
 
     /*
@@ -306,6 +320,7 @@ public class Autonomoustest extends LinearOpMode {
         int newLeftTarget;
         int newRightTarget;
         int newBackLeftTarget;
+        int newArmTarget;
                 int newbackRightTarget;
 
         if (opModeIsActive()) {
@@ -315,16 +330,19 @@ public class Autonomoustest extends LinearOpMode {
             newRightTarget = rightDrive.getCurrentPosition() + (int)(rightInches * COUNTS_PER_INCH);
             newBackLeftTarget = backleftDrive.getCurrentPosition() + (int)(rightInches * COUNTS_PER_INCH);
             newbackRightTarget = backrightDrive.getCurrentPosition() + (int)(rightInches * COUNTS_PER_INCH);
+            newArmTarget = backrightDrive.getCurrentPosition() + (int)(rightInches * COUNTS_PER_ARM_INCH);
             leftDrive.setTargetPosition(newLeftTarget);
             rightDrive.setTargetPosition(newRightTarget);
             backrightDrive.setTargetPosition(newbackRightTarget);
             backleftDrive.setTargetPosition(newBackLeftTarget);
+            arm.setTargetPosition(newArmTarget);
 
             // 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);
+            arm.setMode(DcMotor.RunMode.RUN_TO_POSITION);
 
             // reset the timeout time and start motion.
             runtime.reset();
@@ -332,6 +350,7 @@ public class Autonomoustest extends LinearOpMode {
             rightDrive.setPower(Math.abs(speed));
             backrightDrive.setPower(Math.abs(speed));
             backleftDrive.setPower(Math.abs(speed));
+            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
@@ -341,7 +360,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() && arm.isBusy())) {
 
                 // Display it for the driver.
                 telemetry.addData("Running to",  " %7d :%7d", newLeftTarget,  newRightTarget);
@@ -356,6 +375,7 @@ public class Autonomoustest extends LinearOpMode {
                 rightDrive.setPower(0);
                 backrightDrive.setPower(0);
                 backleftDrive.setPower(0);
+                arm.setPower(0);
 
 
             // Turn off RUN_TO_POSITION
@@ -363,6 +383,7 @@ public class Autonomoustest extends LinearOpMode {
             rightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
             backleftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
             backrightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+            arm.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
 
             sleep(250);   // optional pause after each move.
         }