diff --git a/command/wrist.py b/command/wrist.py index d9faef6..fc74a4c 100644 --- a/command/wrist.py +++ b/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]): """ 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: