Hang Successful

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

@@ -67,17 +67,16 @@ public class SpecimenAuto extends OpMode {
                 state = 2;
                  break;
              case 2:
-                 lift.toSpecimanHangHeight();
                  new SleepAction(5);
                  state = 3;
                  break;
              case 3:
-                 wrist.toSpecimenHang();
+
                  new SleepAction(5);
                  state = 4;
                  break;
              case 4:
-                 lift.toSpecimanReleaseHeight();
+
                  new SleepAction(5);
                  state = 5;
                  break;

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/CometBotTeleOpDevelopment.java

@@ -1,5 +1,6 @@
 package org.firstinspires.ftc.teamcode.cometbots;
 
+import static org.firstinspires.ftc.teamcode.PedroConstants.FRONT_LEFT_MOTOR;
 import static org.firstinspires.ftc.teamcode.PedroConstants.MAX_POWER;
 
 import com.qualcomm.robotcore.hardware.Gamepad;
@@ -31,6 +32,7 @@ public class CometBotTeleOpDevelopment {
     public Gamepad currentGamepad2;
     public Gamepad previousGamepad1;
     public Gamepad previousGamepad2;
+    public boolean grabBlock;
 
 
     private Follower follower;
@@ -41,7 +43,7 @@ public class CometBotTeleOpDevelopment {
         arm = new ArmSubsystem(hardwareMap);
         wrist = new WristSubsystem(hardwareMap);
         skyhook = new HangMotorSubsystem(hardwareMap);
-
+        grabBlock = false;
         this.gamepad1 = gamepad1;
         this.gamepad2 = gamepad2;
         currentGamepad1 = new Gamepad();
@@ -104,18 +106,14 @@ public class CometBotTeleOpDevelopment {
     private void armAndWristToFloor() {
         if (currentGamepad2.a && !previousGamepad2.a) {
             arm.toFloorPositionTeleOp();
-            if (wrist.getState() != WristSubsystem.WristState.FLOOR) {
+            if (arm.getState() == ArmSubsystem.ArmState.FLOOR) {
-                wrist.toFloorPositionTeleop();
+                wrist.switchState();
-            } else {
+            } else if (arm.getState() != ArmSubsystem.ArmState.FLOOR) {
                 wrist.toPickupPosition();
+            } else {
+                wrist.toFloorPosition();
             }
         }
-
-        if (currentGamepad2.a && !previousGamepad2.a) {
-            wrist.switchState();
-        }
-
-
     }
 
     private void armToBucketPosition() {
@@ -129,7 +127,7 @@ public class CometBotTeleOpDevelopment {
     private void armToParkPosition() {
         if (currentGamepad2.x && !previousGamepad2.x) {
             arm.toParkPosition();
-            wrist.toFloorPositionTeleop();
+            wrist.toFloorPosition();
         }
     }
 
@@ -153,7 +151,7 @@ public class CometBotTeleOpDevelopment {
     private void hang(){
         if (gamepad1.a) {
             claw.grabBlueberry();
-            arm.setPosition(0.15);
+            arm.setPosition(0.13);
             arm.grabBlueberrySkyhook();
 
             //claw Open small amount
@@ -163,35 +161,50 @@ public class CometBotTeleOpDevelopment {
         if(gamepad1.b) {
             //confirm grab
             claw.closeClaw();
-
+            grabBlock = true;
         }
 
-        if (gamepad1.x) {
+        if (gamepad1.x && grabBlock) {
             //now slap on bar, first wrist, then arm, then claw then driver must drive away
-            dualSlides.toFixedPosition(500);
+            dualSlides.toFixedPosition(300);
             dualSlides.update();
         }
         if (gamepad1.y) {
             arm.hangBlueberrySkyhook();
             wrist.hangBlueberrySkyhook();
             try {
-                Thread.sleep(500);
+                Thread.sleep(1500);
             } catch (InterruptedException e) {
                 throw new RuntimeException(e);
             }
-            dualSlides.toFixedPosition(1800);
+            dualSlides.toFixedPosition(2100);
             dualSlides.update();
         }
         if (gamepad1.right_bumper) {
             claw.openClaw();
-        }
+            try {
-        if (gamepad1.dpad_up && claw.getState() == ClawSubsystem.ClawState.OPEN) {
+                Thread.sleep(1000);
-            skyhook.hangRobot();
+            } catch (InterruptedException e) {
+                throw new RuntimeException(e);
+            }
+            dualSlides.toFloorPosition();
+            claw.closeClaw();
+            arm.toParkPosition();
+            wrist.toFloorPosition();
         }
         skyhook.setPower(0);
-        if (gamepad1.dpad_down) {
+
+        /*if (gamepad1.dpad_down) {
             skyhook.disableMotor();
             skyhook.setPower(1);
+        }*/
+        if(gamepad1.right_trigger  > 0){
+            skyhook.setPower(true, 1);
+
+        }
+        if(gamepad1.left_trigger  > 0){
+            skyhook.setPower(false, 1);
+
         }
     }
 

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/tests/DualMotorSliderTest.java

@@ -19,7 +19,7 @@ public class DualMotorSliderTest extends LinearOpMode {
 
     private DcMotorEx liftSlideLeft;
     private DcMotorEx liftSlideRight;
-    public static double kp = 0.0015, ki = 0, kd = 0;
+    public static double kp = 0.002, ki = 0, kd = 0;
     private double lastError = 0;
     private double integralSum = 0;
     public static int targetPosition = 0;

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

@@ -7,8 +7,6 @@ public class RobotConstants {
     public final static double clawClose = 1;
     public final static double clawOpen = 0.05;
 
-
-
     public final static double armFloor = 0.7;
     public final static double armSubmarine = 0.55;
     public final static double armReverseBucket = 0.08;
@@ -29,7 +27,7 @@ public class RobotConstants {
     public final static double armSpecimen = 0.155;
     public final static double armInit = 0.13;
     public final static double wristInit = 0.125;
-    public final static double wristPickup = 0.475;
+    public final static double wristPickup = 0.425;
     public final static double wristBucket = 0.56;
     public final static double wristSpecimenPrep = 0.63;
     public final static double wristSpecimenHang = 0.53;

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

@@ -56,7 +56,7 @@ public class DualMotorSliderSubsystem {
     It's the only value we set because the variable ki and kd deal with how to handle when we're off the path.
     Since we're going straight, we don't need to worry about.
     */
-    public final static double kp = 0.0015, ki = 0, kd = 0;
+    public final static double kp = 0.002, ki = 0, kd = 0;
 
     /*
     lastError/integralSum/timer - These 3 variables are placeholders in determining how much

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

@@ -55,5 +55,12 @@ public class HangMotorSubsystem {
         //write in limits for protection
         hang.setPower(Position);
     }
+    public void setPower(boolean forward, double power){
+        if(forward)
+        hang.setDirection(DcMotorSimple.Direction.FORWARD);
+        else if(!forward)
+            hang.setDirection(DcMotorSimple.Direction.REVERSE);
+        hang.setPower(power);
+    }
 
 }

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

@@ -88,6 +88,7 @@ public class HookTest extends OpMode {
         }
 
 
+
     }
 
 }