stop running intake after we intake

Choate-Robotics · Apr 18, 2024 · 34e4ea5 · 34e4ea5
1 parent 01b89b8
commit 34e4ea5
Show file tree

Hide file tree

Showing 6 changed files with 17 additions and 33 deletions.
diff --git a/autonomous/routines/AMP_SKIP_2/auto.py b/autonomous/routines/AMP_SKIP_2/auto.py
@@ -177,10 +177,7 @@
                 path_2,
                 path_3
             ),
-            SequentialCommandGroup(
-                IntakeStageNote(Robot.wrist, Robot.intake),
-                AimWrist(Robot.wrist, Field.calculations)
-            )
+            IntakeThenAim(Robot.intake, Robot.wrist, Field.calculations)
         ),
 
         # Shoot second note
@@ -194,10 +191,7 @@
                 path_4,
                 path_5
             ),
-            SequentialCommandGroup(
-                IntakeStageNote(Robot.wrist, Robot.intake),
-                AimWrist(Robot.wrist, Field.calculations)
-            )
+            IntakeThenAim(Robot.intake, Robot.wrist, Field.calculations)
         ),
 
         # Shoot third note

diff --git a/autonomous/routines/AMP_SKIP_2_2/auto.py b/autonomous/routines/AMP_SKIP_2_2/auto.py
@@ -177,10 +177,7 @@
                 path_2,
                 path_3
             ),
-            SequentialCommandGroup(
-                IntakeStageNote(Robot.wrist, Robot.intake),
-                AimWrist(Robot.wrist, Field.calculations)
-            )
+            IntakeThenAim(Robot.intake, Robot.wrist, Field.calculations)
         ),
 
         # Shoot second note
@@ -194,10 +191,7 @@
                 path_4,
                 path_5
             ),
-            SequentialCommandGroup(
-                IntakeStageNote(Robot.wrist, Robot.intake),
-                AimWrist(Robot.wrist, Field.calculations)
-            )
+            IntakeThenAim(Robot.intake, Robot.wrist, Field.calculations)
         ),
 
         # Shoot third note

diff --git a/autonomous/routines/MIDLINE_NOTES/auto.py b/autonomous/routines/MIDLINE_NOTES/auto.py
@@ -215,10 +215,7 @@
                 # ),
                 path_3
             ),
-            SequentialCommandGroup(
-                IntakeStageNote(Robot.wrist, Robot.intake),
-                AimWrist(Robot.wrist, Field.calculations)
-            )
+            IntakeThenAim(Robot.intake, Robot.wrist, Field.calculations)
         ),
 
         # shoot second note
@@ -242,10 +239,7 @@
                 # ),
                 path_8
             ),
-            SequentialCommandGroup(
-                IntakeStageNote(Robot.wrist, Robot.intake),
-                AimWrist(Robot.wrist, Field.calculations)
-            )
+            IntakeThenAim(Robot.intake, Robot.wrist, Field.calculations)
         ),
 
         # drive to shot zone

diff --git a/autonomous/routines/MIDLINE_NOTES/auto_2.py b/autonomous/routines/MIDLINE_NOTES/auto_2.py
@@ -215,10 +215,7 @@
                 # ),
                 path_3
             ),
-            SequentialCommandGroup(
-                IntakeStageNote(Robot.wrist, Robot.intake),
-                AimWrist(Robot.wrist, Field.calculations)
-            )
+            IntakeThenAim(Robot.intake, Robot.wrist, Field.calculations)
         ),
 
         # shoot second note
@@ -242,10 +239,7 @@
                 # ),
                 path_8
             ),
-            SequentialCommandGroup(
-                IntakeStageNote(Robot.wrist, Robot.intake),
-                AimWrist(Robot.wrist, Field.calculations)
-            )
+            IntakeThenAim(Robot.intake, Robot.wrist, Field.calculations)
         ),
 
         # drive to shot zone

diff --git a/command/__init__.py b/command/__init__.py
@@ -4,4 +4,4 @@
 from command.wrist import SetWrist, FeedIn, FeedOut, ZeroWrist, PassNote, AimWrist, SetWristIdle
 from command.flywheel import SetFlywheelLinearVelocity, SetFlywheelVelocityIndependent, SetFlywheelShootSpeaker, SetFlywheelShootFeeder
 from command.controller import Giraffe, StageNote, AimWristSpeaker, Shoot, EnableClimb, UndoClimb, EmergencyManuver, IntakeStageNote, IntakeStageIdle, Amp, ScoreTrap, ClimbDown,\
-    ShootAuto, ControllerRumble, ControllerRumbleTimeout, IntakeStageNoteAuto, PathUntilIntake, AutoPickupNote, PathIntakeAim
+    ShootAuto, ControllerRumble, ControllerRumbleTimeout, IntakeStageNoteAuto, PathUntilIntake, AutoPickupNote, PathIntakeAim, IntakeThenAim
diff --git a/command/controller.py b/command/controller.py
@@ -247,6 +247,14 @@ def __init__(self, wrist: Wrist, intake: Intake):
             WaitUntilCommand(lambda: wrist.note_in_feeder()),
         )
 
+class IntakeThenAim(SequentialCommandGroup):
+    def __init__(self, intake: Intake, wrist: Wrist, calculations: TrajectoryCalculator):
+        super().__init__(
+            IntakeStageNote(wrist, intake),
+            IntakeIdle(intake),
+            AimWrist(wrist, calculations)
+        )
+
 
 class PathUntilIntake(ParallelRaceGroup):
     def __init__(self, path: Command, wrist: Wrist, intake: Intake,