Reconcile telemetry data and moved all actions to gamepad 1

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/configs/RobotConstants.java

@@ -5,11 +5,11 @@ import com.acmerobotics.dashboard.config.Config;
 @Config
 public class RobotConstants {
     public static double clawClose = 1.00;
-    public static double clawOpen = 0.25;
+    public static double clawOpen = 0.0625;
 
-    public static double armFloor = 0.5;
+    public static double armFloor = 0.375;
-    public static double armPark = 0.125;
+    public static double armPark = 0.0;
-    public static double armBucket = 0.175;
+    public static double armBucket = 0.15;
 
     public static double wristFloor = 0.625;
     public static double wristBucket = 0.215;
@@ -17,5 +17,5 @@ public class RobotConstants {
     public static int liftToFloatPos = 150;
     public static int liftToLowBucketPos = 2250;
     public static int liftToHighBucketPos = 3850;
-    public static double liftPower = .45;
+    public static double liftPower = .375;
 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/runmodes/DevTeleopRunMode.java

@@ -12,6 +12,7 @@ import org.firstinspires.ftc.teamcode.subsystem.MotorsSubsystem;
 import org.firstinspires.ftc.teamcode.subsystem.WristSubsystem;
 import org.firstinspires.ftc.teamcode.util.action.Actions;
 import org.firstinspires.ftc.teamcode.util.action.SequentialAction;
+import org.firstinspires.ftc.teamcode.util.action.SleepAction;
 
 public class DevTeleopRunMode {
 
@@ -33,16 +34,18 @@ public class DevTeleopRunMode {
     public Gamepad previousGP1;
     public Gamepad currentGP2;
     public Gamepad previousGP2;
+    private Telemetry telemetry;
     public FieldStates fieldStates;
 
     public DevTeleopRunMode(HardwareMap hardwareMap, Telemetry telemetry, Gamepad gp1, Gamepad gp2) {
-        this.motors = new MotorsSubsystem(hardwareMap, telemetry);
+        this.motors = new MotorsSubsystem(hardwareMap, telemetry, .55);
         this.claw = new ClawSubsystem(hardwareMap, telemetry);
         this.arm = new ArmSubsystem(hardwareMap, telemetry);
         this.wrist = new WristSubsystem(hardwareMap, telemetry);
         this.lift = new LiftSubsystem(hardwareMap, telemetry);
         this.GP1 = gp1;
         this.GP2 = gp2;
+        this.telemetry = telemetry;
         this.currentGP1 = new Gamepad();
         this.currentGP2 = new Gamepad();
         this.previousGP1 = new Gamepad();
@@ -64,11 +67,19 @@ public class DevTeleopRunMode {
         this.previousGP2.copy(currentGP2);
         this.currentGP2.copy(this.GP2);
         this.theDrop();
-        this.theLift();
         this.thePickup();
         this.theTravel();
         this.theLowBucketScore();
         this.theHighBucketScore();
+        this.toHold();
+        this.motors.calculateTrajectory(this.GP1);
+        this.telemetry.addData("Field State", this.fieldStates.getFieldLocation());
+        this.telemetry.addData("Claw State", this.claw.getState());
+        this.telemetry.addData("Claw Position", this.claw.getPosition());
+        this.telemetry.addData("Wrist State", this.wrist.getState());
+        this.telemetry.addData("Arm State", this.arm.getState());
+        this.telemetry.addData("Lift State", this.lift.getState());
+        this.telemetry.addData("Lift Position", this.lift.getPosition());
     }
 
     /*
@@ -77,11 +88,13 @@ public class DevTeleopRunMode {
     Action: On button press, Arm hovers the floor with wrist parallel to arm
      */
     public void theDrop() {
-        if (this.currentGP1.a && !this.previousGP1.a) {
+        if (this.currentGP1.dpad_down && !this.previousGP1.dpad_down) {
-            if (fieldStates.getFieldLocation() == FieldStates.FieldLocation.FIELD) {
+            if (fieldStates.getFieldLocation() == FieldStates.FieldLocation.FIELD &&
+                    this.lift.getPosition() < 40) {
                 Actions.runBlocking(new SequentialAction(
-                        this.arm.toFloorPosition,
+                        this.wrist.toFloorPosition,
-                        this.wrist.toFloorPosition
+                        new SleepAction(.75),
+                        this.arm.toFloorPosition
                 ));
             }
         }
@@ -99,16 +112,16 @@ public class DevTeleopRunMode {
     }
 
     /*
-    Controller: 1
+   Controller: 1
-    Button: B
+   Button: Right Bumper
-    Action: On button press, lift arm and prepare wrist for bucket
+   Action: On button press, open and closes claw
-     */
+    */
-    public void theLift() {
+    public void toHold() {
-        if (this.currentGP1.b && !this.previousGP1.b) {
+        if (this.currentGP1.left_bumper && !this.previousGP1.left_bumper) {
             if (fieldStates.getFieldLocation() == FieldStates.FieldLocation.FIELD) {
                 Actions.runBlocking(new SequentialAction(
-                        this.arm.toParkPosition,
+                        arm.toParkPosition,
-                        this.wrist.toBucketPosition
+                        wrist.toFloorPosition
                 ));
             }
         }
@@ -121,7 +134,7 @@ public class DevTeleopRunMode {
             arm to bucket position, wrist to bucket position
      */
     public void theLowBucketScore() {
-        if (this.currentGP2.y && !this.previousGP2.y) {
+        if (this.currentGP1.a && !this.previousGP1.a) {
             fieldStates.setFieldLocation(FieldStates.FieldLocation.BUCKET);
             Actions.runBlocking(new SequentialAction(
                     lift.toLowBucket,
@@ -138,7 +151,7 @@ public class DevTeleopRunMode {
             arm to bucket position, wrist to bucket position
      */
     public void theHighBucketScore() {
-        if (this.currentGP2.a && !this.previousGP2.a) {
+        if (this.currentGP1.b && !this.previousGP1.b) {
             fieldStates.setFieldLocation(FieldStates.FieldLocation.BUCKET);
             Actions.runBlocking(new SequentialAction(
                     lift.toHighBucket,
@@ -155,7 +168,7 @@ public class DevTeleopRunMode {
             arm to bucket position, wrist to floor position
      */
     public void theTravel(){
-        if (this.currentGP2.dpad_down && !this.previousGP2.dpad_down){
+        if (this.currentGP1.dpad_right && !this.previousGP1.dpad_right){
             if (fieldStates.getFieldLocation() == FieldStates.FieldLocation.BUCKET) {
                 Actions.runBlocking(new SequentialAction(
                         lift.toFloor,

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/ArmSubsystem.java

@@ -39,7 +39,6 @@ public class ArmSubsystem {
             this.arm.setPosition(armBucket);
         }
         this.state = armState;
-        this.telemetry.addData("Arm State", this.getState());
     }
 
     public void toFloorPosition() {

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/ClawSubsystem.java

@@ -35,7 +35,6 @@ public class ClawSubsystem {
             claw.setPosition(clawOpen);
         }
         this.state = clawState;
-        this.telemetry.addData("Claw State", this.getState());
     }
 
     public ClawState getState() {
@@ -66,4 +65,8 @@ public class ClawSubsystem {
         Actions.runBlocking(closeClaw);
     }
 
+    public double getPosition() {
+        return this.claw.getPosition();
+    }
+
 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/LiftSubsystem.java

@@ -38,12 +38,11 @@ public class LiftSubsystem {
     public void setTarget(int b) {
         lift.setTargetPosition(b);
         lift.setMode(DcMotor.RunMode.RUN_TO_POSITION);
-        this.telemetry.addData("Lift State", this.getState());
     }
 
     public void switchState() {
         if (this.liftState == LiftState.FLOOR) {
-            this.toFloor();
+            this.toFloat();
         } else if (this.liftState == LiftState.FLOAT) {
             this.toLowBucket();
         } else if (this.liftState == LiftState.LOW_BUCKET) {
@@ -54,23 +53,23 @@ public class LiftSubsystem {
     }
 
     public void toFloor() {
-        this.setTarget(liftToFloorPos);
         this.setState(LiftState.FLOOR);
+        this.setTarget(liftToFloorPos);
     }
 
     public void toFloat() {
-        this.setTarget(liftToFloatPos);
         this.setState(LiftState.FLOAT);
+        this.setTarget(liftToFloatPos);
     }
 
     public void toLowBucket() {
-        this.setTarget(liftToLowBucketPos);
         this.setState(LiftState.LOW_BUCKET);
+        this.setTarget(liftToLowBucketPos);
     }
 
     public void toHighBucket() {
-        this.setTarget(liftToHighBucketPos);
         this.setState(LiftState.HIGH_BUCKET);
+        this.setTarget(liftToHighBucketPos);
     }
 
     public void init() {
@@ -83,7 +82,7 @@ public class LiftSubsystem {
         this.liftState = liftState;
     }
 
-    private LiftState getState() { return this.liftState; }
+    public LiftState getState() { return this.liftState; }
 
     public int getPosition() {
         return lift.getCurrentPosition();

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/WristSubsystem.java

@@ -35,7 +35,6 @@ public class WristSubsystem {
             wrist.setPosition(wristBucket);
         }
         this.state = wristState;
-        this.telemetry.addData("Wrist State", this.getState());
     }
 
     public void toFloorPosition() {