fixed odometry

TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionEngines/Competition2BlueBackStage.java

@@ -11,7 +11,7 @@ import org.timecrafters.CenterStage.Common.CompetitionRobotV1;
 
 import dev.cyberarm.engine.V2.CyberarmEngine;
 
-@Autonomous(name = "BURNSVILLE blue audience")
+@Autonomous(name = "old BURNSVILLE blue audience")
 
 public class Competition2BlueBackStage extends CyberarmEngine {
 
@@ -22,7 +22,9 @@ public class Competition2BlueBackStage extends CyberarmEngine {
     public void init() {
         super.init();
         robot.clawArm.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
-        robot.clawArm.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+        robot.clawArm.setTargetPosition(0);
+        robot.clawArm.setPower(0);
+        robot.clawArm.setMode(DcMotor.RunMode.RUN_TO_POSITION);
         robot.imu.resetYaw();
         robot.leftClaw.setPosition(0.25);
         robot.rightClaw.setPosition(0.6);
@@ -32,7 +34,7 @@ public class Competition2BlueBackStage extends CyberarmEngine {
     public void setup() {
         this.robot = new CompetitionRobotV1("burnsville audience blue");
         this.robot.setup();
-        addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "0-00-0"));
+//        addState(new DriveToCoordinatesState(robot,"burnsville audience blue", "0-00-0"));
 
         addState(new ClawArmState(robot,"burnsville audience blue", "0-01-0"));
 

TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionEngines/CompetitionBurnsvilleAudienceBlue.java

@@ -0,0 +1,79 @@
+package org.timecrafters.CenterStage.Autonomous.CompetitionEngines;
+
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.hardware.DcMotor;
+
+import org.timecrafters.CenterStage.Autonomous.CompetitionStates.ClawArmControlTask;
+import org.timecrafters.CenterStage.Autonomous.CompetitionStates.ClawArmState;
+import org.timecrafters.CenterStage.Autonomous.CompetitionStates.ClawFingerState;
+import org.timecrafters.CenterStage.Autonomous.CompetitionStates.DriveToCoordinatesState;
+import org.timecrafters.CenterStage.Autonomous.CompetitionStates.DriveToCoordinatesTask;
+import org.timecrafters.CenterStage.Autonomous.CompetitionStates.OdometryLocalizerTask;
+import org.timecrafters.CenterStage.Common.CompetitionRobotV1;
+
+import dev.cyberarm.engine.V2.CyberarmEngine;
+
+@Autonomous(name = "Burnsville blue audience")
+
+public class CompetitionBurnsvilleAudienceBlue extends CyberarmEngine {
+
+    CompetitionRobotV1 robot;
+
+
+    @Override
+    public void init() {
+        super.init();
+        robot.clawArm.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        robot.clawArm.setTargetPosition(0);
+        robot.clawArm.setPower(0);
+        robot.clawArm.setMode(DcMotor.RunMode.RUN_TO_POSITION);
+        robot.imu.resetYaw();
+        robot.leftClaw.setPosition(0.25);
+        robot.rightClaw.setPosition(0.6);
+    }
+
+    @Override
+    public void setup() {
+        this.robot = new CompetitionRobotV1("Burnsville audience blue");
+        addTask(new DriveToCoordinatesTask(robot));
+        addTask(new OdometryLocalizerTask(robot));
+//        addTask(new ClawArmControlTask(robot));
+
+        this.robot.setup();
+        addState(new ClawArmState(robot,"Burnsville audience blue", "0-01-0"));
+
+//        addState(new CameraVisionState(robot));
+
+        addState(new ClawArmState(robot,"Burnsville audience blue", "0-01-1"));
+
+        // drive to the left, center, or right spike mark
+//        addState(new DriveToCoordinatesState(robot,"Burnsville audience blue", "3-02-0"));
+//        addState(new DriveToCoordinatesState(robot,"Burnsville audience blue", "2-02-0"));
+        addState(new DriveToCoordinatesState(robot,"Burnsville audience blue", "1-02-0"));
+
+        // place pixel
+        addState(new ClawFingerState(robot,"Burnsville audience blue", "0-02-1"));
+
+        // drive to search pos
+        addState(new ClawArmState(robot,"Burnsville audience blue", "0-02-2"));
+
+        // close right finger
+        addState(new ClawFingerState(robot,"Burnsville audience blue", "0-02-3"));
+
+//        // drive back and away from the spike mark (x,y) (1050, 1000) H = 0
+////        addState(new DriveToCoordinatesState(robot,"Burnsville audience blue", "3-03-0"));
+////        addState(new DriveToCoordinatesState(robot,"Burnsville audience blue", "2-03-0"));
+//        addState(new DriveToCoordinatesState(robot,"Burnsville audience blue", "1-03-0"));
+//
+//
+//        // drive to the middle truss (right version) (1130,980) H = -90
+////        addState(new DriveToCoordinatesState(robot,"Burnsville audience blue", "3-04-0"));
+////        addState(new DriveToCoordinatesState(robot,"Burnsville audience blue", "2-04-0"));
+//        addState(new DriveToCoordinatesState(robot,"Burnsville audience blue", "1-04-0"));
+//
+//        // drive to hover mode to clear the middle truss
+//        addState(new ClawArmState(robot,"Burnsville audience blue", "0-04-1"));
+//
+    }
+
+}

TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionEngines/CompetitionBurnsvilleBackDropBlue.java

@@ -0,0 +1,78 @@
+package org.timecrafters.CenterStage.Autonomous.CompetitionEngines;
+
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.hardware.DcMotor;
+
+import org.timecrafters.CenterStage.Autonomous.CompetitionStates.ClawArmState;
+import org.timecrafters.CenterStage.Autonomous.CompetitionStates.ClawFingerState;
+import org.timecrafters.CenterStage.Autonomous.CompetitionStates.DriveToCoordinatesState;
+import org.timecrafters.CenterStage.Autonomous.CompetitionStates.DriveToCoordinatesTask;
+import org.timecrafters.CenterStage.Autonomous.CompetitionStates.OdometryLocalizerTask;
+import org.timecrafters.CenterStage.Common.CompetitionRobotV1;
+
+import dev.cyberarm.engine.V2.CyberarmEngine;
+
+@Autonomous(name = "Burnsville blue audience")
+
+public class CompetitionBurnsvilleBackDropBlue extends CyberarmEngine {
+
+    CompetitionRobotV1 robot;
+
+
+    @Override
+    public void init() {
+        super.init();
+        robot.clawArm.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        robot.clawArm.setTargetPosition(0);
+        robot.clawArm.setPower(0);
+        robot.clawArm.setMode(DcMotor.RunMode.RUN_TO_POSITION);
+        robot.imu.resetYaw();
+        robot.leftClaw.setPosition(0.25);
+        robot.rightClaw.setPosition(0.6);
+    }
+
+    @Override
+    public void setup() {
+        this.robot = new CompetitionRobotV1("Burnsville audience blue");
+        addTask(new DriveToCoordinatesTask(robot));
+        addTask(new OdometryLocalizerTask(robot));
+//        addTask(new ClawArmControlTask(robot));
+
+        this.robot.setup();
+        addState(new ClawArmState(robot,"Burnsville audience blue", "0-01-0"));
+
+//        addState(new CameraVisionState(robot));
+
+        addState(new ClawArmState(robot,"Burnsville audience blue", "0-01-1"));
+
+        // drive to the left, center, or right spike mark
+//        addState(new DriveToCoordinatesState(robot,"Burnsville audience blue", "3-02-0"));
+//        addState(new DriveToCoordinatesState(robot,"Burnsville audience blue", "2-02-0"));
+        addState(new DriveToCoordinatesState(robot,"Burnsville audience blue", "1-02-0"));
+
+        // place pixel
+        addState(new ClawFingerState(robot,"Burnsville audience blue", "0-02-1"));
+
+        // drive to search pos
+        addState(new ClawArmState(robot,"Burnsville audience blue", "0-02-2"));
+
+        // close right finger
+        addState(new ClawFingerState(robot,"Burnsville audience blue", "0-02-3"));
+
+//        // drive back and away from the spike mark (x,y) (1050, 1000) H = 0
+////        addState(new DriveToCoordinatesState(robot,"Burnsville audience blue", "3-03-0"));
+////        addState(new DriveToCoordinatesState(robot,"Burnsville audience blue", "2-03-0"));
+//        addState(new DriveToCoordinatesState(robot,"Burnsville audience blue", "1-03-0"));
+//
+//
+//        // drive to the middle truss (right version) (1130,980) H = -90
+////        addState(new DriveToCoordinatesState(robot,"Burnsville audience blue", "3-04-0"));
+////        addState(new DriveToCoordinatesState(robot,"Burnsville audience blue", "2-04-0"));
+//        addState(new DriveToCoordinatesState(robot,"Burnsville audience blue", "1-04-0"));
+//
+//        // drive to hover mode to clear the middle truss
+//        addState(new ClawArmState(robot,"Burnsville audience blue", "0-04-1"));
+//
+    }
+
+}

TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionStates/ClawArmControlTask.java

@@ -0,0 +1,21 @@
+package org.timecrafters.CenterStage.Autonomous.CompetitionStates;
+
+import com.acmerobotics.dashboard.config.Config;
+
+import org.timecrafters.CenterStage.Common.CompetitionRobotV1;
+
+import dev.cyberarm.engine.V2.CyberarmState;
+import dev.cyberarm.minibots.red_crab.RedCrabMinibot;
+
+@Config
+public class ClawArmControlTask extends CyberarmState {
+
+    CompetitionRobotV1 robot;
+
+    public ClawArmControlTask(CompetitionRobotV1 robot) {
+        this.robot = robot;
+    }
+
+    @Override
+    public void exec() {robot.clawArmControl();}
+}

TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionStates/ClawArmState.java

@@ -29,6 +29,7 @@ public class ClawArmState extends CyberarmState {
 
     @Override
     public void exec() {
+        robot.armTime = System.currentTimeMillis() - initTime;
         // odometry driving ALWAYS
         robot.DriveToCoordinates();
         robot.OdometryLocalizer();

TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionStates/DriveToCoordinatesState.java

@@ -1,11 +1,11 @@
 package org.timecrafters.CenterStage.Autonomous.CompetitionStates;
 
+import android.util.Log;
+
 import com.acmerobotics.dashboard.config.Config;
-import com.arcrobotics.ftclib.controller.PIDController;
 
 import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
 import org.timecrafters.CenterStage.Common.CompetitionRobotV1;
-import org.timecrafters.CenterStage.Common.PrototypeRobot;
 
 import dev.cyberarm.engine.V2.CyberarmState;
 @Config
@@ -21,8 +21,11 @@ public class DriveToCoordinatesState extends CyberarmState {
     public boolean posSpecific;
     public double maxXPower;
     public double maxYPower;
+    private String actionName;
 
     public DriveToCoordinatesState(CompetitionRobotV1 robot, String groupName, String actionName) {
+        this.actionName = actionName;
+
         this.robot = robot;
         this.xTarget = robot.configuration.variable(groupName, actionName, "xTarget").value();
         this.yTarget = robot.configuration.variable(groupName, actionName, "yTarget").value();
@@ -43,6 +46,9 @@ public class DriveToCoordinatesState extends CyberarmState {
         robot.yMaxPower = maxYPower;
         robot.xMaxPower = maxXPower;
 
+        Log.d("TTT?", ""  + actionName + " CURRENT POSITION: x: " + robot.positionX + " Y: " + robot.positionY + "h: " + robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.DEGREES));
+        Log.d("TTT?", ""  + actionName + " TARGET POSITION: x: " + robot.xTarget + " Y: " + robot.yTarget + "h: " + robot.hTarget);
+
     }
 
     @Override
@@ -51,29 +57,29 @@ public class DriveToCoordinatesState extends CyberarmState {
             if (objectPos != robot.objectPos) {
                 // enter the ending loop
                 setHasFinished(true);
+            } else {
+
+                if (armDrive) {
+                    robot.clawArmControl();
+                }
+
+                if (Math.abs(robot.positionX - robot.xTarget) < 5
+                        && Math.abs(robot.positionY - robot.yTarget) < 5) {
+                    setHasFinished(true);
+                }
+            }
+        } else {
+                if (armDrive) {
+                    robot.clawArmControl();
+                }
+
+                if (Math.abs(robot.positionX - robot.xTarget) < 5
+                        && Math.abs(robot.positionY - robot.yTarget) < 5) {
+                    setHasFinished(true);
+                }
             }
         }
 
-        if (armDrive) {
-            robot.clawArmControl();
-        }
-
-        robot.OdometryLocalizer();
-        robot.XDrivePowerModifier();
-        robot.YDrivePowerModifier();
-        robot.DriveToCoordinates();
-        robot.OdometryLocalizer();
-
-        if ((Math.abs(robot.backLeftPower) < 0.15 &&
-                Math.abs(robot.backRightPower) < 0.15 &&
-                Math.abs(robot.frontLeftPower) < .15 &&
-                Math.abs(robot.frontRightPower) < 0.15)){
-                setHasFinished(true);
-        }
-    }
-
-
-
     @Override
     public void telemetry() {
         engine.telemetry.addData("x pos", robot.positionX);

TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionStates/DriveToCoordinatesTask.java

@@ -0,0 +1,23 @@
+package org.timecrafters.CenterStage.Autonomous.CompetitionStates;
+
+import android.util.Log;
+
+import com.acmerobotics.dashboard.config.Config;
+
+import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
+import org.timecrafters.CenterStage.Common.CompetitionRobotV1;
+
+import dev.cyberarm.engine.V2.CyberarmState;
+
+@Config
+public class DriveToCoordinatesTask extends CyberarmState {
+
+    CompetitionRobotV1 robot;
+    public DriveToCoordinatesTask(CompetitionRobotV1 robot) {this.robot = robot;}
+    @Override
+    public void exec() {
+        robot.XDrivePowerModifier();
+        robot.YDrivePowerModifier();
+        robot.DriveToCoordinates();
+    }
+}

TeamCode/src/main/java/org/timecrafters/CenterStage/Autonomous/CompetitionStates/OdometryLocalizerTask.java

@@ -0,0 +1,18 @@
+package org.timecrafters.CenterStage.Autonomous.CompetitionStates;
+
+import com.acmerobotics.dashboard.config.Config;
+
+import org.timecrafters.CenterStage.Common.CompetitionRobotV1;
+
+import dev.cyberarm.engine.V2.CyberarmState;
+
+@Config
+public class OdometryLocalizerTask extends CyberarmState {
+
+    CompetitionRobotV1 robot;
+    public OdometryLocalizerTask(CompetitionRobotV1 robot) {
+        this.robot = robot;
+    }
+    @Override
+    public void exec() {robot.OdometryLocalizer();}
+}

TeamCode/src/main/java/org/timecrafters/CenterStage/Common/CompetitionRobotV1.java

@@ -5,10 +5,12 @@ import com.arcrobotics.ftclib.controller.PIDController;
 import com.qualcomm.hardware.rev.Rev2mDistanceSensor;
 import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
 import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.DcMotorEx;
 import com.qualcomm.robotcore.hardware.DcMotorSimple;
 import com.qualcomm.robotcore.hardware.DistanceSensor;
 import com.qualcomm.robotcore.hardware.HardwareMap;
 import com.qualcomm.robotcore.hardware.IMU;
+import com.qualcomm.robotcore.hardware.PIDFCoefficients;
 import com.qualcomm.robotcore.hardware.Servo;
 import com.qualcomm.robotcore.hardware.TouchSensor;
 import com.qualcomm.robotcore.util.ElapsedTime;
@@ -38,10 +40,11 @@ public class CompetitionRobotV1 extends Robot {
     // ------------------------------------------------------------------------------------------------------------------ HardwareMap setup:
     public OpenCvWebcam webcam1 = null;
     public WebcamName webCamName;
-    public DcMotor frontLeft, frontRight, backLeft, backRight, lift, clawArm, chinUp;
+    public DcMotor frontLeft, frontRight, backLeft, backRight, lift, chinUp;
+    public DcMotorEx clawArm;
 
     public IMU imu;
-    public Servo shoulder, elbow, leftClaw, rightClaw, chinUpServo;
+    public Servo shoulder, elbow, leftClaw, rightClaw, chinUpServo, shootServo;
     public DistanceSensor customObject;
     public TouchSensor touchLeftArm, touchRightArm;
 
@@ -97,14 +100,16 @@ public class CompetitionRobotV1 extends Robot {
     PIDController pidController;
     public double power;
     public String armPos;
+    public long armTime;
+
 
     public int target;
-    public  double p = 0.007, i = 0,  d = 0.0001, f = 0;
-    public  double shoulderCollect = 0.38;
-    public  double shoulderDeposit = 0.36;
-    public  double shoulderPassive = 0.8;
-    public  double elbowCollect = 0.02;
-    public  double elbowDeposit = 0;
+    public double p = 0.007, i = 0,  d = 0.0001, f = 0;
+    public double shoulderCollect = 1;
+    public double shoulderDeposit = 1;
+    public double shoulderPassive = 1;
+    public double elbowCollect = 0.02;
+    public double elbowDeposit = 0;
 
 
     private HardwareMap hardwareMap;
@@ -140,7 +145,7 @@ public class CompetitionRobotV1 extends Robot {
         backLeft = engine.hardwareMap.dcMotor.get("backLeft");
         chinUp = engine.hardwareMap.dcMotor.get("chinUp");
         lift = engine.hardwareMap.dcMotor.get("Lift");
-        clawArm = engine.hardwareMap.dcMotor.get("clawArm");
+        clawArm = (DcMotorEx) engine.hardwareMap.dcMotor.get("clawArm");
 
         frontRight.setDirection(DcMotorSimple.Direction.FORWARD);
         backRight.setDirection(DcMotorSimple.Direction.REVERSE);
@@ -160,9 +165,6 @@ public class CompetitionRobotV1 extends Robot {
         backLeft.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
         frontRight.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
         chinUp.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
-        clawArm.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
-
-
 
         frontRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
         backRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
@@ -198,7 +200,7 @@ public class CompetitionRobotV1 extends Robot {
         leftClaw = hardwareMap.servo.get("leftClaw");
         rightClaw = hardwareMap.servo.get("rightClaw");
         chinUpServo = hardwareMap.servo.get("chinUpServo");
-
+        shootServo = hardwareMap.servo.get("shoot");
 
         elbow.setDirection(Servo.Direction.REVERSE);
 
@@ -256,7 +258,7 @@ public class CompetitionRobotV1 extends Robot {
     public double HeadingPIDControl(double reference, double current){
         double error = angleWrap(current - reference);
         headingIntegralSum += error * headingTimer.seconds();
-        double derivative = (headingLastError - error) / headingTimer.seconds();
+        double derivative = (error - headingLastError) / headingTimer.seconds();
 
         headingTimer.reset();
 
@@ -365,24 +367,27 @@ public class CompetitionRobotV1 extends Robot {
             if (lift.getCurrentPosition() >= 20) {
                 lift.setPower(-0.6);
             } else {
-                shoulder.setPosition(0.38);
+                shoulder.setPosition(shoulderCollect);
                 target = 120;
             }
 
         }
         if (armPos.equals("search")) {
-            shoulder.setPosition(0.15);
-            target = 570;
+            shoulder.setPosition(0.48);
+            if (armTime > 400){
+                target = 570;
+            }
 
         }
 
-        pidController.setPID(p, i, d);
-        int armPos = clawArm.getCurrentPosition();
-        double pid = pidController.calculate(armPos, target);
+//        pidController.setPID(p, i, d);
+//        int armPos = clawArm.getCurrentPosition();
+//        double pid = pidController.calculate(armPos, target);
+//
+//        power = pid;
 
-        power = pid;
-
-        clawArm.setPower(power);
+        clawArm.setTargetPosition(target);
+        clawArm.setPower(0.4);
 
     }
 }

TeamCode/src/main/java/org/timecrafters/CenterStage/TeleOp/States/CompetitionTeleOpState.java

@@ -20,9 +20,13 @@ public class CompetitionTeleOpState extends CyberarmState {
     private CompetitionRobotV1 robot;
     // ------------------------------------------------------------------------------------------------------------robot claw arm variables:
     private PIDController pidController;
-    public  double p = 0.007, i = 0,  d = 0.0001, f = 0;
+    public  double p = 0.005, i = 0,  d = 0.0001, f = 0;
     public int target = 0;
 
+    // ---------------------------------------------------------------------------------------------------------------------shoot variables:
+    public static double releasePos = 0.95;
+    public static double holdPos = 0.55                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                    ;
+
 
     // ------------------------------------------------------------------------------------------------------------- Heading lock variables:
     public double integralSum = 0;
@@ -111,19 +115,16 @@ public class CompetitionTeleOpState extends CyberarmState {
         boolean lbs1 = engine.gamepad1.left_stick_button;
         if (lbs1 && !lbsVar1) {
             if (drivePower == 1) {
-                drivePower = 0.5;
+                drivePower = 0.35;
             } else {
                 drivePower = 1;
             }
         }
         lbsVar1 = lbs1;
 
-        if (engine.gamepad1.left_stick_x != 0 || engine.gamepad1.left_stick_y != 0){
-            boost = engine.gamepad1.right_trigger + 1;
-        }
 
-        double x = -((engine.gamepad1.left_stick_x * 0.5)  * boost);
-        double y = ((engine.gamepad1.left_stick_y * 0.5)  * boost);
+        double x = -(engine.gamepad1.left_stick_x);
+        double y = (engine.gamepad1.left_stick_y);
         double denominator = Math.max(Math.abs(y) + Math.abs(x) + Math.abs(rx), 1);
 
         // angle math to make things field oriented
@@ -139,10 +140,10 @@ public class CompetitionTeleOpState extends CyberarmState {
 
         // setting each power determined previously from the math above
         // as well as multiplying it by a drive power that can be changed.
-        robot.backLeft.setPower(backLeftPower);
-        robot.backRight.setPower(-backRightPower);
-        robot.frontLeft.setPower(frontLeftPower);
-        robot.frontRight.setPower(frontRightPower);
+        robot.backLeft.setPower(backLeftPower * drivePower);
+        robot.backRight.setPower(-backRightPower * drivePower);
+        robot.frontLeft.setPower(frontLeftPower * drivePower);
+        robot.frontRight.setPower(frontRightPower * drivePower);
     }
 
     public double angleWrap(double radians) {
@@ -211,6 +212,9 @@ public class CompetitionTeleOpState extends CyberarmState {
         } else if (engine.gamepad2.b) {
             armPos = "hover";
             depositMode = true;
+        } else if (engine.gamepad2.x) {
+            armPos = "passive";
+            depositMode = true;
         } else if (engine.gamepad2.dpad_left) {
             armPos = "lift up";
             depositMode = true;
@@ -230,7 +234,7 @@ public class CompetitionTeleOpState extends CyberarmState {
                 robot.shoulder.setPosition(robot.shoulderCollect);
                 robot.elbow.setPosition(robot.elbowCollect);
                 robot.chinUpServo.setPosition(chinUpServoDown);
-                target = 30;
+                target = 0;
 
             }
         }
@@ -251,10 +255,20 @@ public class CompetitionTeleOpState extends CyberarmState {
                 robot.elbow.setPosition(robot.elbowCollect);
                 target = 120;
             }
-
         }
+
+        if (Objects.equals(armPos, "passive")) {
+            if (robot.lift.getCurrentPosition() >= 20) {
+                robot.chinUpServo.setPosition(chinUpServoDown);
+                robot.lift.setPower(-0.6);
+            } else {
+                robot.shoulder.setPosition(robot.shoulderPassive);
+                target = 570;
+            }
+        }
+
         if (Objects.equals(armPos, "lift up")) {
-            robot.shoulder.setPosition(robot.shoulderDeposit);
+            robot.shoulder.setPosition(robot.shoulderCollect);
             robot.elbow.setPosition(robot.elbowDeposit);
             target = 120;
             robot.chinUpServo.setPosition(chinUpServoUp);
@@ -268,16 +282,16 @@ public class CompetitionTeleOpState extends CyberarmState {
                 robot.lift.setPower(0);
                 robot.chinUpServo.setPosition(chinUpServoDown);
                 robot.shoulder.setPosition(robot.shoulderCollect);
-                robot.elbow.setPosition(robot.elbowCollect);
-                target = 850;
+                target = 120;
             }
         }
 
         if (armPos.equals("reset")) {
             robot.shoulder.setPosition(robot.shoulderPassive);
+            target = -10000;
             if (robot.touchLeftArm.isPressed() || robot.touchRightArm.isPressed()) {
                 robot.clawArm.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
-                robot.clawArm.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+                robot.clawArm.setMode(DcMotor.RunMode.RUN_TO_POSITION);
                 armPos = "collect";
             }
         }
@@ -290,11 +304,22 @@ public class CompetitionTeleOpState extends CyberarmState {
             pidController = new PIDController(p, i, d);
             robot.imu.resetYaw();
 
+        robot.clawArm.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        robot.clawArm.setTargetPosition(0);
+        robot.clawArm.setPower(0);
+        robot.clawArm.setMode(DcMotor.RunMode.RUN_TO_POSITION);
+
     }
 
     @Override
     public void exec() {
 
+        if (engine.gamepad2.start && engine.gamepad2.x){
+            robot.shootServo.setPosition(releasePos);
+        } else {
+            robot.shootServo.setPosition(holdPos);
+        }
+
 
         if (engine.gamepad2.dpad_up) {
             robot.chinUp.setPower(-1);
@@ -334,16 +359,16 @@ public class CompetitionTeleOpState extends CyberarmState {
         DriveTrainTeleOp();
 
         // ---------------------------------------------------------------------------------------- Game Pad 2, arms, claw, drone, and lift:
-        pidController.setPID(p, i, d);
-        int armCurrentPos = robot.clawArm.getCurrentPosition();
-        double pid = pidController.calculate(armCurrentPos, target);
+//        pidController.setPID(p, i, d);
+//        int armCurrentPos = robot.clawArm.getCurrentPosition();
+//        double pid = pidController.calculate(armCurrentPos, target);
 
         if (armPos.equals("reset")){
-            armPower = -0.2;
+            robot.clawArm.setPower(armPower);
         } else {
-            armPower = pid;
+            armPower = 0.4; // pid
         }
-
+        robot.clawArm.setTargetPosition(target);
         robot.clawArm.setPower(armPower);
 
         // ------------------------------------------------------------------------------------------------------------------- Lift Control: