From 089ca0cf3db37bc0a63e77969aa05807ed9b3731 Mon Sep 17 00:00:00 2001
From: robotics2 <robotics2@scds.org>
Date: Tue, 31 Oct 2023 17:10:06 -0700
Subject: [PATCH] manual control complete, and states started again

---
 .../firstinspires/ftc/teamcode/ArmStates.java | 28 +++++++++++++++----
 1 file changed, 22 insertions(+), 6 deletions(-)

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ArmStates.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ArmStates.java
index 96f3123..19d1010 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ArmStates.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ArmStates.java
@@ -16,9 +16,10 @@ public class
 ArmStates extends OpMode {
 
     DcMotor arm;
+    Servo wrist;
     static final double TICKS_TO_DEGREES = 0.07462686567;
     static final double COUNTS_PER_MOTOR_REV = 537.6;
-    static final double DRIVE_GEAR_REDUCTION = 75;
+    static final double DRIVE_GEAR_REDUCTION = 60;
     static final double COUNTS_PER_ARM_INCH = (COUNTS_PER_MOTOR_REV * DRIVE_GEAR_REDUCTION) / (2.75 * Math.PI);
 
 
@@ -53,6 +54,7 @@ ArmStates extends OpMode {
         telemetry.addData("Status", "In Init()");
         telemetry.update();
         arm = hardwareMap.dcMotor.get("arm raise");
+        wrist = hardwareMap.servo.get("wrist");
         arm.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
 
     }
@@ -109,6 +111,10 @@ ArmStates extends OpMode {
         if (gamepad2.b) {
             armState = 3;
         }
+        if (gamepad2.a)
+        {
+            armState = 0;
+        }
         if (gamepad2.right_bumper) {
             armState += 1;
         }
@@ -140,10 +146,9 @@ ArmStates extends OpMode {
             telemetry.addData("Currently at", " %7d", arm.getCurrentPosition());
             telemetry.update();
         }
-
+        arm.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
         arm.setPower(0);
 
-
         // Turn off RUN_TO_POSITION
         arm.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
 
@@ -152,28 +157,39 @@ ArmStates extends OpMode {
 
     public void raisearm(int degrees) {
         arm.setDirection(DcMotor.Direction.REVERSE);
-        armDriveWithEncoder(.1, degrees);
+        armDriveWithEncoder(.05, degrees);
     }
 
     public void updateState() {
         if (armState == 0) {
             // arm state 0 means on the ground with gripper down
+            wrist.setPosition(.5);
             raisearm(0);
             telemetry.addData("state", 0);
         }
+        arm.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+        arm.setPower(0);
         if (armState == 1) {
             // arm state 1 is for driving
             raisearm(200);
             telemetry.addData("state", 1);
         }
+        arm.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+        arm.setPower(0);
         if (armState == 2) {
-            raisearm(4400);
+            raisearm(697);
+            wrist.setPosition(0);
+            raisearm(697);
             telemetry.addData("state", 2);
         }
+        arm.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+        arm.setPower(.0);
         if (armState == 3) {
-            raisearm(4800);
+            raisearm(780);
             telemetry.addData("state", 3);
             telemetry.addData("arm pos", arm.getCurrentPosition());
         }
+        arm.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+        arm.setPower(0);
     }
 }