From 80e360f2ebec1bf1640c3f7a9f66d3235db0b2b5 Mon Sep 17 00:00:00 2001
From: alexl1239 <aluo27@choate.edu>
Date: Thu, 20 Feb 2025 17:16:23 -0500
Subject: [PATCH 1/2] added the feed out for distance command

---
 command/wrist.py   | 20 ++++++++++++++++++++
 config.py          |  2 ++
 subsystem/wrist.py |  4 ++++
 3 files changed, 26 insertions(+)

diff --git a/command/wrist.py b/command/wrist.py
index d9faef6..8d84cc4 100644
--- a/command/wrist.py
+++ b/command/wrist.py
@@ -127,8 +127,28 @@ def end(self, interrupted) -> None:
         self.subsystem.coral_in_feed = False
         self.subsystem.feed_stop()
         self.subsystem.wrist_ejecting = False
+class FeedOutForDistance(SubsystemCommand[Wrist]):
+    def __init__(self, subsystem: Wrist):
+        super().__init__(subsystem)
+        self.subsystem = subsystem
+        
+
+    def initialize(self) -> None:
+        self.subsystem.feed_out()
+        self.subsystem.wrist_ejecting = True
+        self.subsystem.cummulative_extake_distance = self.subsystem.feed_motor.get_sensor_position()
+        
 
+    def execute(self) -> None:
+        pass
+
+    def isFinished(self) -> bool:
+        return self.subsystem.feed_motor.get_sensor_position()  - self.subsystem.cummulative_extake_distance < config.wrist_extake_distance
 
+    def end(self, interrupted) -> None:
+        self.subsystem.feed_stop()
+        self.subsystem.wrist_ejecting = False
+        self.subsystem.cummulative_extake_distance = self.subsystem.feed_motor.get_sensor_position()
 class WristAlgaeIn(SubsystemCommand[Wrist]):
     """
     run the feed motor to get the algae
diff --git a/config.py b/config.py
index 4383a0e..419cea3 100644
--- a/config.py
+++ b/config.py
@@ -106,6 +106,8 @@
 wrist_cancoder_id = 22
 wrist_encoder_zero = 0.781
 
+wrist_extake_distance = 50 #rotations
+
 wrist_intake_speed = 0.35
 wrist_extake_speed = -0.25
 wrist_algae_speed = 0.25
diff --git a/subsystem/wrist.py b/subsystem/wrist.py
index a0683a9..e404e02 100644
--- a/subsystem/wrist.py
+++ b/subsystem/wrist.py
@@ -39,6 +39,8 @@ def __init__(self):
         self.wrist_zeroed: bool = False
         self.table = None
 
+        self.cummulative_extake_distance = 0
+
         self.algae_in_wrist: bool = False
         self.algae_running_in: bool = False
         self.algae_running_out: bool = False
@@ -180,6 +182,8 @@ def update_table(self) -> None:
             / constants.wrist_encoder_gear_ratio
             * 2
             * math.pi)-self.get_wrist_angle())
+        
+        self.table.putNumber("wrist cummulative distance", self.cummulative_extake_distance)
 
     def periodic(self) -> None:
         if config.NT_WRIST:

From 3f704e88f8b0c4eb7ec3f19d26a7bed29001739d Mon Sep 17 00:00:00 2001
From: alexl1239 <aluo27@choate.edu>
Date: Thu, 20 Feb 2025 17:17:08 -0500
Subject: [PATCH 2/2] added docstring

---
 command/wrist.py | 5 +++++
 1 file changed, 5 insertions(+)

diff --git a/command/wrist.py b/command/wrist.py
index 8d84cc4..fc74a4c 100644
--- a/command/wrist.py
+++ b/command/wrist.py
@@ -127,7 +127,11 @@ def end(self, interrupted) -> None:
         self.subsystem.coral_in_feed = False
         self.subsystem.feed_stop()
         self.subsystem.wrist_ejecting = False
+
 class FeedOutForDistance(SubsystemCommand[Wrist]):
+    """
+    run out the feed motor for a set amount of rotations
+    """
     def __init__(self, subsystem: Wrist):
         super().__init__(subsystem)
         self.subsystem = subsystem
@@ -149,6 +153,7 @@ def end(self, interrupted) -> None:
         self.subsystem.feed_stop()
         self.subsystem.wrist_ejecting = False
         self.subsystem.cummulative_extake_distance = self.subsystem.feed_motor.get_sensor_position()
+
 class WristAlgaeIn(SubsystemCommand[Wrist]):
     """
     run the feed motor to get the algae