Fix/wrist

command/wrist.py

@@ -128,6 +128,31 @@ def end(self, interrupted) -> None:
         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
+
+
+    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]):
     """

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

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: