From 2a1513a2baf1a2e532c4341cec28965d1eb0d6e0 Mon Sep 17 00:00:00 2001
From: robotics3 <Chris@scds.org>
Date: Mon, 20 Jan 2025 09:39:50 -0800
Subject: [PATCH] Commit Init Positions

---
 .../cometbots/CometBotAutoDevelopment.java     |  6 +++---
 .../ftc/teamcode/subsystem/ArmSubsystem.java   | 13 +++++++++++--
 .../ftc/teamcode/subsystem/WristSubsystem.java | 18 +++++++++++++-----
 3 files changed, 27 insertions(+), 10 deletions(-)

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 adc9dbb..c9ecdb2 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
@@ -61,8 +61,8 @@ public class CometBotAutoDevelopment {
     public void init() {
         dualSlides.init();
         claw.init();
-        wrist.init();
-        arm.init();
+        wrist.initTeleOp();
+        arm.initTeleOp();
         follower.setMaxPower(MAX_POWER);
         follower.startTeleopDrive();
     }
@@ -145,7 +145,7 @@ public class CometBotAutoDevelopment {
     private void armToParkPosition(){
         if(currentGamepad2.x && !previousGamepad2.x){
             arm.toParkPosition();
-
+            wrist.toFloorPosition();
         }
     }
 
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/ArmSubsystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/ArmSubsystem.java
index 03a4d5a..5af501c 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/ArmSubsystem.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/ArmSubsystem.java
@@ -5,6 +5,7 @@ import static org.firstinspires.ftc.teamcode.configs.RobotConstants.armBucket;
 import static org.firstinspires.ftc.teamcode.configs.RobotConstants.armFloor;
 import static org.firstinspires.ftc.teamcode.configs.RobotConstants.armGrabBlueberrySkyhook;
 import static org.firstinspires.ftc.teamcode.configs.RobotConstants.armHangBlueberrySkyhook;
+import static org.firstinspires.ftc.teamcode.configs.RobotConstants.armInit;
 import static org.firstinspires.ftc.teamcode.configs.RobotConstants.armPark;
 
 import com.qualcomm.robotcore.hardware.HardwareMap;
@@ -13,7 +14,7 @@ import com.qualcomm.robotcore.hardware.ServoImplEx;
 public class ArmSubsystem {
 
     public enum ArmState {
-        PARK, FLOOR, BUCKET, SUBMARINE, GRAB_BLUEBERRY_SKYHOOK, HANG_BLUEBERRY_SKYHOOK
+        PARK, FLOOR, BUCKET, SUBMARINE, GRAB_BLUEBERRY_SKYHOOK, HANG_BLUEBERRY_SKYHOOK, INIT
     }
 
     private ServoImplEx arm;
@@ -45,6 +46,10 @@ public class ArmSubsystem {
         arm.setPosition(armBucket);
         setState(ArmState.BUCKET);
     }
+    public void toInitPosition(){
+        arm.setPosition(armInit);
+        setState(ArmState.INIT);
+    }
 
     public void setState(ArmState armState) {
         this.state = armState;
@@ -54,10 +59,14 @@ public class ArmSubsystem {
         return this.state;
     }
 
-    public void init() {
+    public void initTeleOp() {
         arm.resetDeviceConfigurationForOpMode();
         toParkPosition();
     }
+    public void initAuto(){
+        arm.resetDeviceConfigurationForOpMode();
+        toInitPosition();
+    }
 
     public double getPosition() {
         return this.arm.getPosition();
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/WristSubsystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/WristSubsystem.java
index a67dfad..a8dbf15 100644
--- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/WristSubsystem.java
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystem/WristSubsystem.java
@@ -5,6 +5,7 @@ import static org.firstinspires.ftc.teamcode.configs.RobotConstants.wristBucket;
 import static org.firstinspires.ftc.teamcode.configs.RobotConstants.wristFloor;
 import static org.firstinspires.ftc.teamcode.configs.RobotConstants.wristGrabBlueberrySkyhook;
 import static org.firstinspires.ftc.teamcode.configs.RobotConstants.wristHangBlueberrySkyhook;
+import static org.firstinspires.ftc.teamcode.configs.RobotConstants.wristInit;
 import static org.firstinspires.ftc.teamcode.configs.RobotConstants.wristPickup;
 import static org.firstinspires.ftc.teamcode.configs.RobotConstants.wristSpeciemen;
 
@@ -19,7 +20,7 @@ import com.qualcomm.robotcore.hardware.ServoImplEx;
 public class WristSubsystem {
 
     public enum WristState {
-        FLOOR, BUCKET, PICKUP, RUNG, SPECIMEN, GRAB_BLUEBERRY_SKYHOOK, HANG_BLUEBERRY_SKYHOOK
+        FLOOR, BUCKET, PICKUP, RUNG, SPECIMEN, GRAB_BLUEBERRY_SKYHOOK, HANG_BLUEBERRY_SKYHOOK, INIT
     }
 
     public ServoImplEx wrist;
@@ -51,9 +52,7 @@ public class WristSubsystem {
         wrist.setPosition(wristGrabBlueberrySkyhook);
     }
 
-    /**
-     * This function goes after grabBlueberrySkyhook
-     */
+
     public void hangBlueberrySkyhook(){
         setState(WristState.HANG_BLUEBERRY_SKYHOOK);
         wrist.setPosition(wristHangBlueberrySkyhook);
@@ -72,6 +71,15 @@ public class WristSubsystem {
         setState(WristState.PICKUP);
         wrist.setPosition(wristPickup);
     }
+    public void toInitPosition(){
+        setState(WristState.INIT);
+        wrist.setPosition(wristInit);
+    }
+    public void InitAuto(){
+        wrist.resetDeviceConfigurationForOpMode();
+        toInitPosition();
+
+    }
 
     public void setState(WristState wristState) {
         this.state = wristState;
@@ -89,7 +97,7 @@ public class WristSubsystem {
         return this.state;
     }
 
-    public void init() {
+    public void initTeleOp() {
         wrist.resetDeviceConfigurationForOpMode();
         toPickupPosition();
     }