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();
+        }
+
 
 
     }
@@ -154,6 +153,16 @@ public class CometBotAutoDevelopment {
             dualSlides.toHighBucketPosition();
         }
     }
+    private void robotUp(){
+        if(currentGamepad1.dpad_up && !previousGamepad1.dpad_up){
+
+        }
+    }
+    private void robotDown(){
+        if(currentGamepad1.dpad_down && !previousGamepad1.dpad_down){
+
+        }
+    }
     private void dualSlidesToFloorPosition() {
         if (currentGamepad2.dpad_down && !previousGamepad2.dpad_down) {
             dualSlides.toFloorPosition();

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,59 @@
+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 setPower(double power){
+        hang.setPower(power);
+    }
+    public void stopMotor(){
+        hang.setPower(0);
+
+
+    }
+    public void toHangPosition(int Position){
+        //write in limits for protection
+        hang.setPower(Position);
+    }
+
+}

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

@@ -58,6 +58,7 @@ public class LiftArmWrist extends LinearOpMode {
         WristSubsystem wrist = new WristSubsystem(hardwareMap);
         ArmSubsystem arm = new ArmSubsystem(hardwareMap);
         ClawSubsystem claw = new ClawSubsystem(hardwareMap);
+        HangMotorSubsystem hang = new HangMotorSubsystem(hardwareMap);
 
 
         /*
@@ -117,15 +118,23 @@ public class LiftArmWrist extends LinearOpMode {
                 lift.toLowBucketPosition();
 
             }
+            if(currentGamepad1.left_trigger > 0){
+                hang.setPower((double) currentGamepad1.left_trigger);
+            }
+            if(currentGamepad1.right_trigger > 0){
+                hang.setPower((double) currentGamepad1.right_trigger);
+            }
 
             lift.update();
 
             // Show the elapsed game time and wheel power.
+
             telemetry.addData("Status", "Run Time: " + runtime.toString());
             telemetry.addData("Lift Drive Position", lift.getFixedPosition());
             telemetry.addData("Wrist Position", wrist.getPosition());
             telemetry.addData("Arm Position", arm.getPosition());
             telemetry.addData("Slide Ticks", lift.getCurrentPosition());
+            telemetry.addData("Hook Ticks", hang.getCurrentPosition());
             telemetry.update();
         }
     }