Commit Hang Subsystem

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

@@ -114,12 +114,9 @@ public class CometBotAutoDevelopment {
         }
     }
     private void armAndWristToFloor(){
-        if(currentGamepad2.a && !previousGamepad2.a){
-            wrist.switchState();
-        }
         if(currentGamepad2.a && !previousGamepad2.a){
 
-            double increment = 0.5 - arm.getPosition();
+            double increment = 0.7 - arm.getPosition();
             for(int i = 0; i < 3; i ++){
                 arm.setPosition(arm.getPosition() + increment);
                 try {
@@ -129,9 +126,11 @@ public class CometBotAutoDevelopment {
                 }
             }
             arm.toFloorPosition();
-
-
         }
+        if(currentGamepad2.a && !previousGamepad2.a ){
+            wrist.switchState();
+        }
+
 
 
     }

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

@@ -30,7 +30,8 @@ public class RobotConstants {
 
     public final static double wristSpeciemen = 0.1;
 
-
+    public final static int toBar = 500;
+    public final static int toFloor = 0;
     public final static int liftToFloorPos = 350;
     public final static int liftToSubmarinePos = 350;
     public final static int liftToLowBucketPos = 1750;

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

@@ -0,0 +1,51 @@
+package org.firstinspires.ftc.teamcode.subsystem;
+
+import static org.firstinspires.ftc.teamcode.PedroConstants.LIFT_SLIDE_LEFT_MOTOR;
+import static org.firstinspires.ftc.teamcode.PedroConstants.LIFT_SLIDE_RIGHT_MOTOR;
+
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.DcMotorEx;
+import com.qualcomm.robotcore.hardware.DcMotorSimple;
+import com.qualcomm.robotcore.hardware.HardwareMap;
+import static org.firstinspires.ftc.teamcode.configs.RobotConstants.toBar;
+import static org.firstinspires.ftc.teamcode.configs.RobotConstants.toFloor;
+
+public class HangMotorSubsystem {
+    private DcMotorEx Hang;
+
+    public HangMotorSubsystem(HardwareMap hardwareMap) {
+
+        Hang = hardwareMap.get(DcMotorEx.class, "skyhook");
+
+
+    }
+
+    public int getCurrentPosition(){
+        int Return = Hang.getCurrentPosition();
+        return Return;
+    }
+    public void init(){
+        Hang.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        Hang.setDirection(DcMotorSimple.Direction.FORWARD);
+        Hang.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+        Hang.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+
+    }
+    public void hangRobot(){
+        Hang.setTargetPosition(toBar);
+
+    }
+    public void robotToFloor(){
+        Hang.setTargetPosition(toFloor);
+    }
+    public void disableMotor(){
+        Hang.setPower(0);
+        Hang.setMotorDisable();
+
+    }
+    public void toHangPosition(int Position){
+        //write in limits for protection
+        Hang.setPower(Position);
+    }
+
+}