From 34e4ea5355b3e896952cf1ab879f5a92f7608ece Mon Sep 17 00:00:00 2001
From: e-bauman <ebauman26@choate.edu>
Date: Thu, 18 Apr 2024 00:36:48 -0400
Subject: [PATCH] stop running intake after we intake

---
 autonomous/routines/AMP_SKIP_2/auto.py      | 10 ++--------
 autonomous/routines/AMP_SKIP_2_2/auto.py    | 10 ++--------
 autonomous/routines/MIDLINE_NOTES/auto.py   | 10 ++--------
 autonomous/routines/MIDLINE_NOTES/auto_2.py | 10 ++--------
 command/__init__.py                         |  2 +-
 command/controller.py                       |  8 ++++++++
 6 files changed, 17 insertions(+), 33 deletions(-)

diff --git a/autonomous/routines/AMP_SKIP_2/auto.py b/autonomous/routines/AMP_SKIP_2/auto.py
index fa277afe..ddb5d026 100644
--- 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
index fc708ab9..fe0c9730 100644
--- 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
index 69e1151c..b1bca333 100644
--- 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
index ab99f9f4..6864e4c7 100644
--- 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
index c2ee84a8..278326c5 100644
--- 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
index 32ad63fc..a5664e15 100644
--- 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,