arm state machine controls, needs large motor adjustments (see comment in code)

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

@@ -12,18 +12,19 @@ import com.qualcomm.robotcore.util.ElapsedTime;
 
 
 @TeleOp( name = "ArmState")
-public class ArmStates extends OpMode {
+public class
+ArmStates extends OpMode {
 
     DcMotor arm;
-    DcMotor armThroughBoreEncoder;
     static final double TICKS_TO_DEGREES = 0.07462686567;
     static final double COUNTS_PER_MOTOR_REV = 537.6;
-    static final double DRIVE_GEAR_REDUCTION = 1.0;
+    static final double DRIVE_GEAR_REDUCTION = 75;
-    static final double COUNTS_PER_ARM_INCH = (COUNTS_PER_MOTOR_REV * DRIVE_GEAR_REDUCTION) / (2.7 * Math.PI);
+    static final double COUNTS_PER_ARM_INCH = (COUNTS_PER_MOTOR_REV * DRIVE_GEAR_REDUCTION) / (2.75 * Math.PI);
 
 
     /**
      * this function takes a long milliseconds parameter and sleeps
+     *
      * @param millis milliseconds to sleep
      */
     public void sleepmillis(long millis) {
@@ -40,6 +41,7 @@ public class ArmStates extends OpMode {
         arm.setPower(0);
 
     }
+
     /**
      * User defined init method
      * This method will be called once when the INIT button is pressed.
@@ -51,7 +53,7 @@ public class ArmStates extends OpMode {
         telemetry.addData("Status", "In Init()");
         telemetry.update();
         arm = hardwareMap.dcMotor.get("arm raise");
-        armThroughBoreEncoder = hardwareMap.dcMotor.get("armThroughBoreEncoder");
+        arm.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
 
     }
 
@@ -65,6 +67,7 @@ public class ArmStates extends OpMode {
         telemetry.addData("Status", "Initialized");
         telemetry.update();
     }
+
     /**
      * User defined start method.
      * This method will be called once when the PLAY button is first pressed.
@@ -88,85 +91,41 @@ public class ArmStates extends OpMode {
      * User defined loop method.
      * This method will be called repeatedly in a loop while this op mode is running
      */
+    public int armState = 0;
+
     public void loop() {
-if (gamepad1.right_bumper)
+        int currentArmState = armState;
-{
-    raisearm(200);
-}
-
-
-//        int armState = 0;
-//        int armPos = 0;
-//        if(gamepad2.x)
-//        {
-//            armState = 1;
-//          raisearm(0 - armPos);
-//          armPos = 0;
-//        }
-//        if(gamepad2.y)
-//        {
-//             armState = 2;
-//            raisearm(1000);
-//            armPos = 30;
-//        }
-//        if(gamepad2.b)
-//        {
-//             armState = 2;
-//            raisearm(200 - armPos);
-//            armPos = 200;
-//        }
-//        if(gamepad2.right_bumper)
-//        {
-//            armState =+ 1;
-//        }
-//        if(gamepad2.left_bumper)
-//        {
-//            armState =- 1;
-//        }
-//        if(armState == 1)
-//        {
-//            raisearm(0 - armPos);
-//            armPos = 0;
-//        }
-//        if(armState == 2)
-//        {
-//            raisearm(30 - armPos);
-//            armPos = 30;
-//        }
-//        if(armState == 3)
-//        {
-//            raisearm(200 - armPos);
-//            armPos = 200;
-//
-//        }
-
-
-
-
-
-//         Show the elapsed game time and wheel power
-//        telemetry.addData("Status", "Run Time: " + runtime.toString());
-//        telemetry.addData("Front left, Right", "%4.2f, %4.2f", leftFrontPower, rightFrontPower);
-//        telemetry.addData("Back  left, Right", "%4.2f, %4.2f", leftBackPower, rightBackPower);
-//        telemetry.addData("Speed", speed);
-
 
+        telemetry.addData("state", armState);
+        telemetry.addData("arm pos", arm.getCurrentPosition());
         telemetry.update();
+        //Something is wrong, the arm is going the wrong way. Maybe it's just my robot, but you should look at it
+        if (gamepad2.x) {
+            armState = 1;
+        }
+        if (gamepad2.y) {
+            armState = 2;
+        }
+        if (gamepad2.b) {
+            armState = 3;
+        }
+        if (gamepad2.right_bumper) {
+            armState += 1;
+        }
+        if (gamepad2.left_bumper) {
+            armState -= 1;
+        }
+        if (armState != currentArmState) {
+            updateState();
+        }
     }
-    public void raisearm(int degrees)
-    {
-        arm.setDirection(DcMotor.Direction.REVERSE);
-        armDriveWithEncoder(.2, degrees);
 
-    }
+
     public void armDriveWithEncoder(double speed,
-                                    double Inches) {
+                                    int ticks) {
-        int newarmTarget;
-
 
         // Determine new target position, and pass to motor controller
-        newarmTarget = armThroughBoreEncoder.getCurrentPosition() + (int) (Inches * COUNTS_PER_ARM_INCH);
+        arm.setTargetPosition(ticks);
-        armThroughBoreEncoder.setTargetPosition(newarmTarget);
 
         // Turn On RUN_TO_POSITION
         arm.setMode(DcMotor.RunMode.RUN_TO_POSITION);
@@ -174,13 +133,47 @@ if (gamepad1.right_bumper)
         // reset the timeout time and start motion.
         arm.setPower(Math.abs(speed));
 
+        while //(opModeIsActive() &&
+            //(runtime.seconds() < timeoutS) &&
+        (arm.isBusy()) {
+            telemetry.addData("Running to", " %7d ", ticks);
+            telemetry.addData("Currently at", " %7d", arm.getCurrentPosition());
+            telemetry.update();
+        }
 
         arm.setPower(0);
 
 
         // Turn off RUN_TO_POSITION
-        armThroughBoreEncoder.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+        arm.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
 
     }
+
+
+    public void raisearm(int degrees) {
+        arm.setDirection(DcMotor.Direction.REVERSE);
+        armDriveWithEncoder(.1, degrees);
     }
 
+    public void updateState() {
+        if (armState == 0) {
+            // arm state 0 means on the ground with gripper down
+            raisearm(0);
+            telemetry.addData("state", 0);
+        }
+        if (armState == 1) {
+            // arm state 1 is for driving
+            raisearm(200);
+            telemetry.addData("state", 1);
+        }
+        if (armState == 2) {
+            raisearm(4400);
+            telemetry.addData("state", 2);
+        }
+        if (armState == 3) {
+            raisearm(4800);
+            telemetry.addData("state", 3);
+            telemetry.addData("arm pos", arm.getCurrentPosition());
+        }
+    }
+}