From f6fc8377efeda60cb14ce58c5552cbbef0a5a5de Mon Sep 17 00:00:00 2001
From: Isaac Turner <spacey-sooty@proton.me>
Date: Sun, 26 May 2024 13:08:02 +0800
Subject: [PATCH] Add example OneNote Auto Routine

Resolves https://github.com/CurtinFRC/2024-Offseason/issues/20
---
 src/main/java/frc/robot/Robot.java         |  9 ++++---
 src/main/java/frc/robot/autos/OneNote.java | 30 ++++++++++++++++++++++
 2 files changed, 36 insertions(+), 3 deletions(-)
 create mode 100644 src/main/java/frc/robot/autos/OneNote.java

diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index 8a1e6e0..f200595 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -14,6 +14,7 @@
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
+import frc.robot.autos.OneNote;
 import frc.robot.generated.TunerConstants;
 import frc.robot.subsystems.Climber;
 import frc.robot.subsystems.CommandSwerveDrivetrain;
@@ -26,11 +27,13 @@ public class Robot extends TimedRobot {
   private CommandXboxController m_codriver;
   private Shooter m_shooter;
   private Climber m_climber;
-  private SendableChooser<Command> m_chooser = new SendableChooser<>();
+  private SendableChooser<Auto> m_chooser = new SendableChooser<>();
   private Intake m_intake;
 
   private Command getAutonomousCommand() {
-    return m_chooser.getSelected();
+    Auto auto = m_chooser.getSelected();
+    auto.configureBindings();
+    return auto.followTrajectory();
   }
 
   private double MaxSpeed =
@@ -103,7 +106,7 @@ public Robot() {
         new Intake(new CANSparkMax(Constants.intakePort, CANSparkMaxLowLevel.MotorType.kBrushless));
     CommandScheduler.getInstance().registerSubsystem(m_intake);
 
-    m_chooser.setDefaultOption("Simple Auto", m_shooter.spinup(1));
+    m_chooser.setDefaultOption("One Note", new OneNote(m_shooter, m_intake));
     SmartDashboard.putData(m_chooser);
 
     configureBindings();
diff --git a/src/main/java/frc/robot/autos/OneNote.java b/src/main/java/frc/robot/autos/OneNote.java
new file mode 100644
index 0000000..2da8585
--- /dev/null
+++ b/src/main/java/frc/robot/autos/OneNote.java
@@ -0,0 +1,30 @@
+package frc.robot.autos;
+
+import com.choreo.lib.Choreo;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
+import frc.robot.Auto;
+import frc.robot.subsystems.Intake;
+import frc.robot.subsystems.Shooter;
+
+public class OneNote implements Auto {
+  Shooter m_shooter;
+  Intake m_intake;
+
+  public OneNote(Shooter shooter, Intake intake) {
+    m_shooter = shooter;
+    m_intake = intake;
+  }
+
+  public Command followTrajectory() {
+    return Commands.print("No path to follow");
+  }
+
+  public void configureBindings() {
+    Choreo.event("shoot")
+        .onTrue(
+            m_shooter
+                .spinup(300)
+                .andThen(Commands.parallel(m_intake.pass(), m_shooter.maintain())));
+  }
+}