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,