added center auto -- also for Bardoe

autos/dealgae_center.py

@@ -13,7 +13,7 @@
 from commands2 import SequentialCommandGroup, InstantCommand, ParallelCommandGroup, ParallelDeadlineGroup, WaitCommand, ConditionalCommand
 
 path_name = "Center L4 2 Barge Algae"
-paths = [PathPlannerPath.fromChoreoTrajectory(path_name, i) for i in range(5)]
+paths = [PathPlannerPath.fromChoreoTrajectory(path_name, i) for i in range(7)]
 
 command = SequentialCommandGroup(
     InstantCommand(lambda: Robot.wrist.set_coral(True)),
@@ -31,22 +31,38 @@
     FeedOut(Robot.wrist).withTimeout(.3),
     ParallelCommandGroup(
         Target(config.target_positions["DEALGAE_LOW"], Robot.wrist, Robot.elevator),
-        # Move back a bit and de-algae low
+        # Move back a bit and pick up algae low
         AutoBuilder.followPath(paths[2]).andThen(InstantCommand(lambda: Robot.drivetrain.set_driver_centric((0, 0), 0))),
     ),
+    WristAlgaeIn(Robot.wrist),
     ParallelCommandGroup(
+        # Move to barge
+        AutoBuilder.followPath(paths[3]).andThen(InstantCommand(lambda: Robot.drivetrain.set_driver_centric((0, 0), 0))),
         SequentialCommandGroup(
             WaitCommand(0.2),
-            Target(config.target_positions["IDLE"], Robot.wrist, Robot.elevator),
-        ),
-        # Move back
-        AutoBuilder.followPath(paths[3])
+            Target(config.target_positions["SCORE_BARGE"], Robot.wrist, Robot.elevator),
+        )
     ),
+    WristAlgaeOut(Robot.wrist).withTimeout(0.3),
     ParallelCommandGroup(
-        Target(config.target_positions["DEALGAE_HIGH"], Robot.wrist, Robot.elevator),
-        # Move back a bit and de-algae low
+        # Move to the algae high
         AutoBuilder.followPath(paths[4]).andThen(InstantCommand(lambda: Robot.drivetrain.set_driver_centric((0, 0), 0))),
-    )
-    )
+        SequentialCommandGroup(
+            WaitCommand(0.2),
+            Target(config.target_positions["DEALGAE_HIGH"], Robot.wrist, Robot.elevator),
+        )
+    ),
+    WristAlgaeIn(Robot.wrist),
+    ParallelCommandGroup(
+        # Move to barge
+        AutoBuilder.followPath(paths[5]).andThen(InstantCommand(lambda: Robot.drivetrain.set_driver_centric((0, 0), 0))),
+        SequentialCommandGroup(
+            WaitCommand(0.2),
+            Target(config.target_positions["SCORE_BARGE"], Robot.wrist, Robot.elevator),
+        )
+    ),  
+    WristAlgaeOut(Robot.wrist).withTimeout(0.3),
+    AutoBuilder.followPath(paths[6])
+)
 
 auto = AutoRoutine(command, paths[0].getStartingHolonomicPose())

command/wrist.py

@@ -146,24 +146,25 @@ def initialize(self) -> None:
         self.subsystem.algae_in()
         self.subsystem.algae_running_in = True
 
-        # self.debouncer = Debouncer(
-        #     config.current_time_threshold, Debouncer.DebounceType.kRising
-        # )
+        self.debouncer = Debouncer(
+            config.current_time_threshold, Debouncer.DebounceType.kRising
+        )
 
     def execute(self) -> None:
         pass
 
     def isFinished(self) -> bool:
-        # return self.debouncer.calculate(
-        #     self.subsystem.algae_motor.get_motor_current()
-        #     > config.algae_current_threshold
-        # )
-        return False
+        return self.debouncer.calculate(
+            self.subsystem.algae_motor.get_motor_current()
+        > config.algae_current_threshold
+        )
 
     def end(self, interrupted) -> None:
         if interrupted:
             log.warn("Algae in command interrupted")
-        self.subsystem.algae_stop()
+            self.subsystem.algae_stop()
+        if not interrupted:
+            self.subsystem.hold_algae()
         self.subsystem.algae_in_wrist = True
         self.subsystem.algae_running_in = False