From 839837c844e196e86cbdf4c8e03b929dd7f7a41b Mon Sep 17 00:00:00 2001
From: robotics3 <Chris@scds.org>
Date: Thu, 23 Jan 2025 10:52:56 -0800
Subject: [PATCH] Commit Hang Subsystem

---
 .../cometbots/CometBotAutoDevelopment.java    | 11 ++--
 .../ftc/teamcode/configs/RobotConstants.java  |  3 +-
 .../subsystem/HangMotorSubsystem.java         | 51 +++++++++++++++++++
 3 files changed, 58 insertions(+), 7 deletions(-)
 create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/HangMotorSubsystem.java

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/CometBotAutoDevelopment.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/CometBotAutoDevelopment.java
index e82cfe3..fa11d60 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/cometbots/CometBotAutoDevelopment.java
+++ b/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();
+        }
+
 
 
     }
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/configs/RobotConstants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/configs/RobotConstants.java
index 9e2bf89..488dde1 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/configs/RobotConstants.java
+++ b/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;
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/HangMotorSubsystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/HangMotorSubsystem.java
new file mode 100644
index 0000000..7a107ea
--- /dev/null
+++ b/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);
+    }
+
+}