From 432ed9a734c3099e4b90c3883c975b8cc27a03a7 Mon Sep 17 00:00:00 2001
From: Sean Chan <sean.chan3@student.education.wa.edu.au>
Date: Sun, 4 Feb 2024 16:00:36 +0800
Subject: [PATCH] .

---
 src/main/cpp/Auto.cpp            | 272 +++++++++++++++----------------
 src/main/cpp/IntakeBehaviour.cpp |   2 +-
 src/main/cpp/Robot.cpp           |  29 +++-
 src/main/include/Auto.h          |  12 +-
 src/main/include/Robot.h         |  28 +++-
 5 files changed, 192 insertions(+), 151 deletions(-)

diff --git a/src/main/cpp/Auto.cpp b/src/main/cpp/Auto.cpp
index 2007a923..ac3f4f8c 100644
--- a/src/main/cpp/Auto.cpp
+++ b/src/main/cpp/Auto.cpp
@@ -1,31 +1,31 @@
 #include "Auto.h"
 
-std::shared_ptr<behaviour::Behaviour> Taxi(wom::drivetrain::SwerveDrive _driveBase, frc::Timer* timer, frc::Field2d* field, Shooter _shooter, Intake _intake, AlphaArm _alphaArm) {
-  return behaviour::make<ArmToSetPoint>(&_alphaArm, 0 * 1_deg, 0.2);
-    behaviour::make<AutoShoot>(&_shooter, 8_V);
-    behaviour::make<ArmToSetPoint>(&_alphaArm, 0 * 1_deg, 0.2);
-    behaviour::make<wom::drivetrain::behaviours::AutoSwerveDrive>(&_driveBase, timer, field);
+std::shared_ptr<behaviour::Behaviour> autos::Taxi(wom::drivetrain::SwerveDrive* _driveBase, frc::Timer* timer, frc::Field2d* field, Shooter* _shooter, Intake* _intake, AlphaArm* _alphaArm) {
+  return behaviour::make<ArmToSetPoint>(_alphaArm, 0 * 1_deg, 0.2);
+    behaviour::make<AutoShoot>(_shooter, 8_V);
+    behaviour::make<ArmToSetPoint>(_alphaArm, 0 * 1_deg, 0.2);
+    behaviour::make<wom::drivetrain::behaviours::AutoSwerveDrive>(_driveBase, timer, field);
 }
 // Shoots starting note then moves out of starting position.
 
-std::shared_ptr<behaviour::Behaviour> QuadrupleClose(wom::drivetrain::SwerveDrive _driveBase, frc::Timer* timer, frc::Field2d* field, Shooter _shooter, Intake _intake, AlphaArm _alphaArm) {
-  return behaviour::make<ArmToSetPoint>(&_alphaArm, 0 * 1_deg, 0.2);
-    behaviour::make<AutoShoot>(&_shooter, 8_V);
-    behaviour::make<wom::drivetrain::behaviours::AutoSwerveDrive>(&_driveBase, timer, field);
-    behaviour::make<ArmToSetPoint>(&_alphaArm, 0 * 1_deg, 0.2);
-    behaviour::make<AutoIntake>(&_intake, 8_V);
-    behaviour::make<ArmToSetPoint>(&_alphaArm, 0 * 1_deg, 0.2);
-    behaviour::make<AutoShoot>(&_shooter, 8_V);
-    behaviour::make<wom::drivetrain::behaviours::AutoSwerveDrive>(&_driveBase, timer, field);
-    behaviour::make<ArmToSetPoint>(&_alphaArm, 0 * 1_deg, 0.2);
-    behaviour::make<AutoIntake>(&_intake, 8_V);
-    behaviour::make<ArmToSetPoint>(&_alphaArm, 0 * 1_deg, 0.2);
-    behaviour::make<AutoShoot>(&_shooter, 8_V);
-    behaviour::make<wom::drivetrain::behaviours::AutoSwerveDrive>(&_driveBase, timer, field);
-    behaviour::make<ArmToSetPoint>(&_alphaArm, 0 * 1_deg, 0.2);
-    behaviour::make<AutoIntake>(&_intake, 8_V);
-    behaviour::make<ArmToSetPoint>(&_alphaArm, 0 * 1_deg, 0.2);
-    behaviour::make<AutoShoot>(&_shooter, 8_V);
+std::shared_ptr<behaviour::Behaviour> autos::QuadrupleClose(wom::drivetrain::SwerveDrive* _driveBase, frc::Timer* timer, frc::Field2d* field, Shooter* _shooter, Intake* _intake, AlphaArm* _alphaArm) {
+  return behaviour::make<ArmToSetPoint>(_alphaArm, 0 * 1_deg, 0.2);
+    behaviour::make<AutoShoot>(_shooter, 8_V);
+    behaviour::make<wom::drivetrain::behaviours::AutoSwerveDrive>(_driveBase, timer, field);
+    behaviour::make<ArmToSetPoint>(_alphaArm, 0 * 1_deg, 0.2);
+    behaviour::make<AutoIntake>(_intake, 8_V);
+    behaviour::make<ArmToSetPoint>(_alphaArm, 0 * 1_deg, 0.2);
+    behaviour::make<AutoShoot>(_shooter, 8_V);
+    behaviour::make<wom::drivetrain::behaviours::AutoSwerveDrive>(_driveBase, timer, field);
+    behaviour::make<ArmToSetPoint>(_alphaArm, 0 * 1_deg, 0.2);
+    behaviour::make<AutoIntake>(_intake, 8_V);
+    behaviour::make<ArmToSetPoint>(_alphaArm, 0 * 1_deg, 0.2);
+    behaviour::make<AutoShoot>(_shooter, 8_V);
+    behaviour::make<wom::drivetrain::behaviours::AutoSwerveDrive>(_driveBase, timer, field);
+    behaviour::make<ArmToSetPoint>(_alphaArm, 0 * 1_deg, 0.2);
+    behaviour::make<AutoIntake>(_intake, 8_V);
+    behaviour::make<ArmToSetPoint>(_alphaArm, 0 * 1_deg, 0.2);
+    behaviour::make<AutoShoot>(_shooter, 8_V);
 
      /*
       4N Close
@@ -39,37 +39,37 @@ std::shared_ptr<behaviour::Behaviour> QuadrupleClose(wom::drivetrain::SwerveDriv
     */
 }
 
-std::shared_ptr<behaviour::Behaviour> QuadrupleFar(wom::drivetrain::SwerveDrive _driveBase, frc::Timer* timer, frc::Field2d* field, Shooter _shooter, Intake _intake, AlphaArm _alphaArm) {
-  return behaviour::make<ArmToSetPoint>(&_alphaArm, 0 * 1_deg, 0.2);
-    behaviour::make<AutoShoot>(&_shooter, 8_V);
-    behaviour::make<ArmToSetPoint>(&_alphaArm, 0 * 1_deg, 0.2);
-    behaviour::make<wom::drivetrain::behaviours::AutoSwerveDrive>(&_driveBase, timer, field);
-    behaviour::make<ArmToSetPoint>(&_alphaArm, 0 * 1_deg, 0.2);
-    behaviour::make<AutoIntake>(&_intake, 8_V);
-    behaviour::make<ArmToSetPoint>(&_alphaArm, 0 * 1_deg, 0.2);
-    behaviour::make<wom::drivetrain::behaviours::AutoSwerveDrive>(&_driveBase, timer, field);
-    behaviour::make<ArmToSetPoint>(&_alphaArm, 0 * 1_deg, 0.2);
-    behaviour::make<AutoShoot>(&_shooter, 8_V);
-    behaviour::make<ArmToSetPoint>(&_alphaArm, 0 * 1_deg, 0.2);
-    behaviour::make<wom::drivetrain::behaviours::AutoSwerveDrive>(&_driveBase, timer, field);
-    behaviour::make<ArmToSetPoint>(&_alphaArm, 0 * 1_deg, 0.2);
-    behaviour::make<AutoIntake>(&_intake, 8_V);
-    behaviour::make<ArmToSetPoint>(&_alphaArm, 0 * 1_deg, 0.2);
-    behaviour::make<wom::drivetrain::behaviours::AutoSwerveDrive>(&_driveBase, timer, field);
-    behaviour::make<ArmToSetPoint>(&_alphaArm, 0 * 1_deg, 0.2);
-    behaviour::make<AutoShoot>(&_shooter, 8_V);
-    behaviour::make<ArmToSetPoint>(&_alphaArm, 0 * 1_deg, 0.2);
-    behaviour::make<wom::drivetrain::behaviours::AutoSwerveDrive>(&_driveBase, timer, field);
-    behaviour::make<ArmToSetPoint>(&_alphaArm, 0 * 1_deg, 0.2);
-    behaviour::make<AutoIntake>(&_intake, 8_V);
-    behaviour::make<ArmToSetPoint>(&_alphaArm, 0 * 1_deg, 0.2);
-    behaviour::make<wom::drivetrain::behaviours::AutoSwerveDrive>(&_driveBase, timer, field);
-    behaviour::make<ArmToSetPoint>(&_alphaArm, 0 * 1_deg, 0.2);
-    behaviour::make<AutoShoot>(&_shooter, 8_V);
-    behaviour::make<ArmToSetPoint>(&_alphaArm, 0 * 1_deg, 0.2);
-    behaviour::make<wom::drivetrain::behaviours::AutoSwerveDrive>(&_driveBase, timer, field);   // do this if possible
-    behaviour::make<ArmToSetPoint>(&_alphaArm, 0 * 1_deg, 0.2);
-    behaviour::make<AutoIntake>(&_intake, 8_V);
+std::shared_ptr<behaviour::Behaviour> autos::QuadrupleFar(wom::drivetrain::SwerveDrive* _driveBase, frc::Timer* timer, frc::Field2d* field, Shooter* _shooter, Intake* _intake, AlphaArm* _alphaArm) {
+  return behaviour::make<ArmToSetPoint>(_alphaArm, 0 * 1_deg, 0.2);
+    behaviour::make<AutoShoot>(_shooter, 8_V);
+    behaviour::make<ArmToSetPoint>(_alphaArm, 0 * 1_deg, 0.2);
+    behaviour::make<wom::drivetrain::behaviours::AutoSwerveDrive>(_driveBase, timer, field);
+    behaviour::make<ArmToSetPoint>(_alphaArm, 0 * 1_deg, 0.2);
+    behaviour::make<AutoIntake>(_intake, 8_V);
+    behaviour::make<ArmToSetPoint>(_alphaArm, 0 * 1_deg, 0.2);
+    behaviour::make<wom::drivetrain::behaviours::AutoSwerveDrive>(_driveBase, timer, field);
+    behaviour::make<ArmToSetPoint>(_alphaArm, 0 * 1_deg, 0.2);
+    behaviour::make<AutoShoot>(_shooter, 8_V);
+    behaviour::make<ArmToSetPoint>(_alphaArm, 0 * 1_deg, 0.2);
+    behaviour::make<wom::drivetrain::behaviours::AutoSwerveDrive>(_driveBase, timer, field);
+    behaviour::make<ArmToSetPoint>(_alphaArm, 0 * 1_deg, 0.2);
+    behaviour::make<AutoIntake>(_intake, 8_V);
+    behaviour::make<ArmToSetPoint>(_alphaArm, 0 * 1_deg, 0.2);
+    behaviour::make<wom::drivetrain::behaviours::AutoSwerveDrive>(_driveBase, timer, field);
+    behaviour::make<ArmToSetPoint>(_alphaArm, 0 * 1_deg, 0.2);
+    behaviour::make<AutoShoot>(_shooter, 8_V);
+    behaviour::make<ArmToSetPoint>(_alphaArm, 0 * 1_deg, 0.2);
+    behaviour::make<wom::drivetrain::behaviours::AutoSwerveDrive>(_driveBase, timer, field);
+    behaviour::make<ArmToSetPoint>(_alphaArm, 0 * 1_deg, 0.2);
+    behaviour::make<AutoIntake>(_intake, 8_V);
+    behaviour::make<ArmToSetPoint>(_alphaArm, 0 * 1_deg, 0.2);
+    behaviour::make<wom::drivetrain::behaviours::AutoSwerveDrive>(_driveBase, timer, field);
+    behaviour::make<ArmToSetPoint>(_alphaArm, 0 * 1_deg, 0.2);
+    behaviour::make<AutoShoot>(_shooter, 8_V);
+    behaviour::make<ArmToSetPoint>(_alphaArm, 0 * 1_deg, 0.2);
+    behaviour::make<wom::drivetrain::behaviours::AutoSwerveDrive>(_driveBase, timer, field);   // do this if possible
+    behaviour::make<ArmToSetPoint>(_alphaArm, 0 * 1_deg, 0.2);
+    behaviour::make<AutoIntake>(_intake, 8_V);
 
     /*
       4N Far
@@ -90,55 +90,55 @@ std::shared_ptr<behaviour::Behaviour> QuadrupleFar(wom::drivetrain::SwerveDrive
     */
 }
 
-std::shared_ptr<behaviour::Behaviour> QuadrupleCloseDoubleFar(wom::drivetrain::SwerveDrive _driveBase, frc::Timer* timer, frc::Field2d* field, Shooter _shooter, Intake _intake, AlphaArm _alphaArm) {
-  return behaviour::make<ArmToSetPoint>(&_alphaArm, 0 * 1_deg, 0.2);
-    behaviour::make<AutoShoot>(&_shooter, 8_V);
-    behaviour::make<ArmToSetPoint>(&_alphaArm, 0 * 1_deg, 0.2);
-    behaviour::make<wom::drivetrain::behaviours::AutoSwerveDrive>(&_driveBase, timer, field);
-    behaviour::make<ArmToSetPoint>(&_alphaArm, 0 * 1_deg, 0.2);
-    behaviour::make<AutoIntake>(&_intake, 8_V);
-    behaviour::make<ArmToSetPoint>(&_alphaArm, 0 * 1_deg, 0.2);
-    behaviour::make<wom::drivetrain::behaviours::AutoSwerveDrive>(&_driveBase, timer, field);
-    behaviour::make<ArmToSetPoint>(&_alphaArm, 0 * 1_deg, 0.2);
-    behaviour::make<AutoShoot>(&_shooter, 8_V);
-    behaviour::make<ArmToSetPoint>(&_alphaArm, 0 * 1_deg, 0.2);
-    behaviour::make<wom::drivetrain::behaviours::AutoSwerveDrive>(&_driveBase, timer, field);
-    behaviour::make<ArmToSetPoint>(&_alphaArm, 0 * 1_deg, 0.2);
-    behaviour::make<AutoIntake>(&_intake, 8_V);
-    behaviour::make<ArmToSetPoint>(&_alphaArm, 0 * 1_deg, 0.2);
-    behaviour::make<wom::drivetrain::behaviours::AutoSwerveDrive>(&_driveBase, timer, field);
-    behaviour::make<ArmToSetPoint>(&_alphaArm, 0 * 1_deg, 0.2);
-    behaviour::make<AutoShoot>(&_shooter, 8_V);
-    behaviour::make<ArmToSetPoint>(&_alphaArm, 0 * 1_deg, 0.2);
-    behaviour::make<wom::drivetrain::behaviours::AutoSwerveDrive>(&_driveBase, timer, field);
-    behaviour::make<ArmToSetPoint>(&_alphaArm, 0 * 1_deg, 0.2);
-    behaviour::make<AutoIntake>(&_intake, 8_V);
-    behaviour::make<ArmToSetPoint>(&_alphaArm, 0 * 1_deg, 0.2);
-    behaviour::make<wom::drivetrain::behaviours::AutoSwerveDrive>(&_driveBase, timer, field);
-    behaviour::make<ArmToSetPoint>(&_alphaArm, 0 * 1_deg, 0.2);
-    behaviour::make<AutoShoot>(&_shooter, 8_V);
-    behaviour::make<ArmToSetPoint>(&_alphaArm, 0 * 1_deg, 0.2);
-    behaviour::make<wom::drivetrain::behaviours::AutoSwerveDrive>(&_driveBase, timer, field);
-    behaviour::make<ArmToSetPoint>(&_alphaArm, 0 * 1_deg, 0.2);
-    behaviour::make<AutoIntake>(&_intake, 8_V);
-    behaviour::make<ArmToSetPoint>(&_alphaArm, 0 * 1_deg, 0.2);
-    behaviour::make<wom::drivetrain::behaviours::AutoSwerveDrive>(&_driveBase, timer, field);
-    behaviour::make<ArmToSetPoint>(&_alphaArm, 0 * 1_deg, 0.2);
-    behaviour::make<AutoShoot>(&_shooter, 8_V);
-    behaviour::make<ArmToSetPoint>(&_alphaArm, 0 * 1_deg, 0.2);
-    behaviour::make<wom::drivetrain::behaviours::AutoSwerveDrive>(&_driveBase, timer, field);
-    behaviour::make<ArmToSetPoint>(&_alphaArm, 0 * 1_deg, 0.2);
-    behaviour::make<AutoIntake>(&_intake, 8_V);
-    behaviour::make<ArmToSetPoint>(&_alphaArm, 0 * 1_deg, 0.2);
-    behaviour::make<wom::drivetrain::behaviours::AutoSwerveDrive>(&_driveBase, timer, field);
-    behaviour::make<ArmToSetPoint>(&_alphaArm, 0 * 1_deg, 0.2);
-    behaviour::make<AutoShoot>(&_shooter, 8_V);
-    behaviour::make<ArmToSetPoint>(&_alphaArm, 0 * 1_deg, 0.2);
-    behaviour::make<wom::drivetrain::behaviours::AutoSwerveDrive>(&_driveBase, timer, field);
-    behaviour::make<ArmToSetPoint>(&_alphaArm, 0 * 1_deg, 0.2);
-    behaviour::make<AutoIntake>(&_intake, 8_V);
-    behaviour::make<ArmToSetPoint>(&_alphaArm, 0 * 1_deg, 0.2);
-    behaviour::make<AutoShoot>(&_shooter, 8_V);
+std::shared_ptr<behaviour::Behaviour> autos::QuadrupleCloseDoubleFar(wom::drivetrain::SwerveDrive* _driveBase, frc::Timer* timer, frc::Field2d* field, Shooter* _shooter, Intake* _intake, AlphaArm* _alphaArm) {
+  return behaviour::make<ArmToSetPoint>(_alphaArm, 0 * 1_deg, 0.2);
+    behaviour::make<AutoShoot>(_shooter, 8_V);
+    behaviour::make<ArmToSetPoint>(_alphaArm, 0 * 1_deg, 0.2);
+    behaviour::make<wom::drivetrain::behaviours::AutoSwerveDrive>(_driveBase, timer, field);
+    behaviour::make<ArmToSetPoint>(_alphaArm, 0 * 1_deg, 0.2);
+    behaviour::make<AutoIntake>(_intake, 8_V);
+    behaviour::make<ArmToSetPoint>(_alphaArm, 0 * 1_deg, 0.2);
+    behaviour::make<wom::drivetrain::behaviours::AutoSwerveDrive>(_driveBase, timer, field);
+    behaviour::make<ArmToSetPoint>(_alphaArm, 0 * 1_deg, 0.2);
+    behaviour::make<AutoShoot>(_shooter, 8_V);
+    behaviour::make<ArmToSetPoint>(_alphaArm, 0 * 1_deg, 0.2);
+    behaviour::make<wom::drivetrain::behaviours::AutoSwerveDrive>(_driveBase, timer, field);
+    behaviour::make<ArmToSetPoint>(_alphaArm, 0 * 1_deg, 0.2);
+    behaviour::make<AutoIntake>(_intake, 8_V);
+    behaviour::make<ArmToSetPoint>(_alphaArm, 0 * 1_deg, 0.2);
+    behaviour::make<wom::drivetrain::behaviours::AutoSwerveDrive>(_driveBase, timer, field);
+    behaviour::make<ArmToSetPoint>(_alphaArm, 0 * 1_deg, 0.2);
+    behaviour::make<AutoShoot>(_shooter, 8_V);
+    behaviour::make<ArmToSetPoint>(_alphaArm, 0 * 1_deg, 0.2);
+    behaviour::make<wom::drivetrain::behaviours::AutoSwerveDrive>(_driveBase, timer, field);
+    behaviour::make<ArmToSetPoint>(_alphaArm, 0 * 1_deg, 0.2);
+    behaviour::make<AutoIntake>(_intake, 8_V);
+    behaviour::make<ArmToSetPoint>(_alphaArm, 0 * 1_deg, 0.2);
+    behaviour::make<wom::drivetrain::behaviours::AutoSwerveDrive>(_driveBase, timer, field);
+    behaviour::make<ArmToSetPoint>(_alphaArm, 0 * 1_deg, 0.2);
+    behaviour::make<AutoShoot>(_shooter, 8_V);
+    behaviour::make<ArmToSetPoint>(_alphaArm, 0 * 1_deg, 0.2);
+    behaviour::make<wom::drivetrain::behaviours::AutoSwerveDrive>(_driveBase, timer, field);
+    behaviour::make<ArmToSetPoint>(_alphaArm, 0 * 1_deg, 0.2);
+    behaviour::make<AutoIntake>(_intake, 8_V);
+    behaviour::make<ArmToSetPoint>(_alphaArm, 0 * 1_deg, 0.2);
+    behaviour::make<wom::drivetrain::behaviours::AutoSwerveDrive>(_driveBase, timer, field);
+    behaviour::make<ArmToSetPoint>(_alphaArm, 0 * 1_deg, 0.2);
+    behaviour::make<AutoShoot>(_shooter, 8_V);
+    behaviour::make<ArmToSetPoint>(_alphaArm, 0 * 1_deg, 0.2);
+    behaviour::make<wom::drivetrain::behaviours::AutoSwerveDrive>(_driveBase, timer, field);
+    behaviour::make<ArmToSetPoint>(_alphaArm, 0 * 1_deg, 0.2);
+    behaviour::make<AutoIntake>(_intake, 8_V);
+    behaviour::make<ArmToSetPoint>(_alphaArm, 0 * 1_deg, 0.2);
+    behaviour::make<wom::drivetrain::behaviours::AutoSwerveDrive>(_driveBase, timer, field);
+    behaviour::make<ArmToSetPoint>(_alphaArm, 0 * 1_deg, 0.2);
+    behaviour::make<AutoShoot>(_shooter, 8_V);
+    behaviour::make<ArmToSetPoint>(_alphaArm, 0 * 1_deg, 0.2);
+    behaviour::make<wom::drivetrain::behaviours::AutoSwerveDrive>(_driveBase, timer, field);
+    behaviour::make<ArmToSetPoint>(_alphaArm, 0 * 1_deg, 0.2);
+    behaviour::make<AutoIntake>(_intake, 8_V);
+    behaviour::make<ArmToSetPoint>(_alphaArm, 0 * 1_deg, 0.2);
+    behaviour::make<AutoShoot>(_shooter, 8_V);
     
     //   4N Close 2N Far
     // 1. Shoot note
@@ -162,34 +162,34 @@ std::shared_ptr<behaviour::Behaviour> QuadrupleCloseDoubleFar(wom::drivetrain::S
     
 }
 
-std::shared_ptr<behaviour::Behaviour> QuadrupleCloseSingleFar(wom::drivetrain::SwerveDrive _driveBase, frc::Timer* timer, frc::Field2d* field, Shooter _shooter, Intake _intake, AlphaArm _alphaArm) {
-  return behaviour::make<ArmToSetPoint>(&_alphaArm, 0 * 1_deg, 0.2);
-    behaviour::make<AutoShoot>(&_shooter, 8_V);
-    behaviour::make<ArmToSetPoint>(&_alphaArm, 0 * 1_deg, 0.2);
-    behaviour::make<wom::drivetrain::behaviours::AutoSwerveDrive>(&_driveBase, timer, field);
-    behaviour::make<ArmToSetPoint>(&_alphaArm, 0 * 1_deg, 0.2);
-    behaviour::make<AutoIntake>(&_intake, 8_V);
-    behaviour::make<ArmToSetPoint>(&_alphaArm, 0 * 1_deg, 0.2);
-    behaviour::make<wom::drivetrain::behaviours::AutoSwerveDrive>(&_driveBase, timer, field);
-    behaviour::make<ArmToSetPoint>(&_alphaArm, 0 * 1_deg, 0.2);
-    behaviour::make<AutoShoot>(&_shooter, 8_V);
-    behaviour::make<ArmToSetPoint>(&_alphaArm, 0 * 1_deg, 0.2);
-    behaviour::make<wom::drivetrain::behaviours::AutoSwerveDrive>(&_driveBase, timer, field);
-    behaviour::make<ArmToSetPoint>(&_alphaArm, 0 * 1_deg, 0.2);
-    behaviour::make<AutoIntake>(&_intake, 8_V);
-    behaviour::make<ArmToSetPoint>(&_alphaArm, 0 * 1_deg, 0.2);
-    behaviour::make<wom::drivetrain::behaviours::AutoSwerveDrive>(&_driveBase, timer, field);
-    behaviour::make<ArmToSetPoint>(&_alphaArm, 0 * 1_deg, 0.2);
-    behaviour::make<AutoShoot>(&_shooter, 8_V);
-    behaviour::make<ArmToSetPoint>(&_alphaArm, 0 * 1_deg, 0.2);
-    behaviour::make<wom::drivetrain::behaviours::AutoSwerveDrive>(&_driveBase, timer, field);
-    behaviour::make<ArmToSetPoint>(&_alphaArm, 0 * 1_deg, 0.2);
-    behaviour::make<AutoIntake>(&_intake, 8_V); 
-    behaviour::make<ArmToSetPoint>(&_alphaArm, 0 * 1_deg, 0.2);
-    behaviour::make<wom::drivetrain::behaviours::AutoSwerveDrive>(&_driveBase, timer, field);
-    behaviour::make<ArmToSetPoint>(&_alphaArm, 0 * 1_deg, 0.2);
-    behaviour::make<AutoShoot>(&_shooter, 8_V);
-    behaviour::make<ArmToSetPoint>(&_alphaArm, 0 * 1_deg, 0.2);
+std::shared_ptr<behaviour::Behaviour> autos::QuadrupleCloseSingleFar(wom::drivetrain::SwerveDrive* _driveBase, frc::Timer* timer, frc::Field2d* field, Shooter* _shooter, Intake* _intake, AlphaArm* _alphaArm) {
+  return behaviour::make<ArmToSetPoint>(_alphaArm, 0 * 1_deg, 0.2);
+    behaviour::make<AutoShoot>(_shooter, 8_V);
+    behaviour::make<ArmToSetPoint>(_alphaArm, 0 * 1_deg, 0.2);
+    behaviour::make<wom::drivetrain::behaviours::AutoSwerveDrive>(_driveBase, timer, field);
+    behaviour::make<ArmToSetPoint>(_alphaArm, 0 * 1_deg, 0.2);
+    behaviour::make<AutoIntake>(_intake, 8_V);
+    behaviour::make<ArmToSetPoint>(_alphaArm, 0 * 1_deg, 0.2);
+    behaviour::make<wom::drivetrain::behaviours::AutoSwerveDrive>(_driveBase, timer, field);
+    behaviour::make<ArmToSetPoint>(_alphaArm, 0 * 1_deg, 0.2);
+    behaviour::make<AutoShoot>(_shooter, 8_V);
+    behaviour::make<ArmToSetPoint>(_alphaArm, 0 * 1_deg, 0.2);
+    behaviour::make<wom::drivetrain::behaviours::AutoSwerveDrive>(_driveBase, timer, field);
+    behaviour::make<ArmToSetPoint>(_alphaArm, 0 * 1_deg, 0.2);
+    behaviour::make<AutoIntake>(_intake, 8_V);
+    behaviour::make<ArmToSetPoint>(_alphaArm, 0 * 1_deg, 0.2);
+    behaviour::make<wom::drivetrain::behaviours::AutoSwerveDrive>(_driveBase, timer, field);
+    behaviour::make<ArmToSetPoint>(_alphaArm, 0 * 1_deg, 0.2);
+    behaviour::make<AutoShoot>(_shooter, 8_V);
+    behaviour::make<ArmToSetPoint>(_alphaArm, 0 * 1_deg, 0.2);
+    behaviour::make<wom::drivetrain::behaviours::AutoSwerveDrive>(_driveBase, timer, field);
+    behaviour::make<ArmToSetPoint>(_alphaArm, 0 * 1_deg, 0.2);
+    behaviour::make<AutoIntake>(_intake, 8_V); 
+    behaviour::make<ArmToSetPoint>(_alphaArm, 0 * 1_deg, 0.2);
+    behaviour::make<wom::drivetrain::behaviours::AutoSwerveDrive>(_driveBase, timer, field);
+    behaviour::make<ArmToSetPoint>(_alphaArm, 0 * 1_deg, 0.2);
+    behaviour::make<AutoShoot>(_shooter, 8_V);
+    behaviour::make<ArmToSetPoint>(_alphaArm, 0 * 1_deg, 0.2);
 }
     
     // 4N Close 1N Far
@@ -232,9 +232,9 @@ std::shared_ptr<behaviour::Behaviour> QuadrupleCloseSingleFar(wom::drivetrain::S
 //     */
 // }
 
-std::shared_ptr<behaviour::Behaviour> autos::AutoTest(wom::drivetrain::SwerveDrive _driveBase, frc::Timer* timer, frc::Field2d* field, Shooter _shooter, Intake _intake, AlphaArm _alphaArm) {
-  return behaviour::make<ArmToSetPoint>(&_alphaArm, 0 * 1_deg, 0.2);
-    behaviour::make<wom::drivetrain::behaviours::AutoSwerveDrive>(&_driveBase, timer, field);
-    behaviour::make<AutoShoot>(&_shooter, 8_V);
-    behaviour::make<AutoIntake>(&_intake, 8_V);
+std::shared_ptr<behaviour::Behaviour> autos::AutoTest(wom::drivetrain::SwerveDrive* _driveBase, frc::Timer* timer, frc::Field2d* field, Shooter* _shooter, Intake* _intake, AlphaArm* _alphaArm) {
+  return behaviour::make<ArmToSetPoint>(_alphaArm, 0 * 1_deg, 0.2);
+    behaviour::make<wom::drivetrain::behaviours::AutoSwerveDrive>(_driveBase, timer, field);
+    behaviour::make<AutoShoot>(_shooter, 8_V);
+    behaviour::make<AutoIntake>(_intake, 8_V);
 } // This auto is a test for auto to see if all things work. 
\ No newline at end of file
diff --git a/src/main/cpp/IntakeBehaviour.cpp b/src/main/cpp/IntakeBehaviour.cpp
index 31ea9fb5..3e7d55c5 100644
--- a/src/main/cpp/IntakeBehaviour.cpp
+++ b/src/main/cpp/IntakeBehaviour.cpp
@@ -54,4 +54,4 @@ void AutoIntake::OnTick(units::second_t dt) {
   // } else if (_intake->GetConfig().magSensor->Get() == 0) {
   //   _intake->setState(IntakeState::kIdle);
   // }
-}
+} 
\ No newline at end of file
diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp
index 888245e6..9ea002ad 100644
--- a/src/main/cpp/Robot.cpp
+++ b/src/main/cpp/Robot.cpp
@@ -20,6 +20,8 @@
 
 #include "behaviour/HasBehaviour.h"
 
+#include "Auto.h"
+
 static units::second_t lastPeriodic;
 
 void Robot::RobotInit() {
@@ -29,9 +31,13 @@ void Robot::RobotInit() {
       [this]() { return wom::make<ShooterManualControl>(shooter, &robotmap.controllers.codriver); });
 
   sched = wom::BehaviourScheduler::GetInstance();
-  m_chooser.SetDefaultOption("Default Auto", "Default Auto");
+  m_chooser.SetDefaultOption("kTaxi", "kTaxi");
+
+  for (auto &option : autoOptions) {
+    m_chooser.AddOption(option, option);
+  }
 
-  // frc::SmartDashboard::PutData("Auto Selector", &m_chooser);
+  frc::SmartDashboard::PutData("Auto Modes", &m_chooser);
 
   // m_path_chooser.SetDefaultOption("Path1", "paths/output/Path1.wpilib.json");
 
@@ -42,7 +48,7 @@ void Robot::RobotInit() {
 
   // frc::SmartDashboard::PutData("Field", &m_field);
 
-  // simulation_timer = frc::Timer();
+  timer = frc::Timer();
 
   // robotmap.swerveBase.gyro->Reset();
 
@@ -105,11 +111,28 @@ void Robot::RobotPeriodic() {
   _swerveDrive->OnUpdate(dt);
   alphaArm->OnUpdate(dt);
   intake->OnUpdate(dt);
+
 }
 
 void Robot::AutonomousInit() {
   loop.Clear();
   sched->InterruptAll();
+
+  m_autoSelected = m_chooser.GetSelected();
+  fmt::print("Auto selected: {}\n", m_autoSelected);
+  if (m_autoSelected == "Taxi") {
+    sched->Schedule(autos::Taxi(_swerveDrive, &timer, &field, shooter, intake, alphaArm));
+  } else if (m_autoSelected == "Auto Test") {
+    sched->Schedule(autos::AutoTest(_swerveDrive, &timer, &field, shooter, intake, alphaArm));
+  } else if (m_autoSelected == "Quadruple Close") {
+    sched->Schedule(autos::QuadrupleClose(_swerveDrive, &timer, &field, shooter, intake, alphaArm));
+  } else if (m_autoSelected == "Quadruple Far") {
+    sched->Schedule(autos::QuadrupleFar(_swerveDrive, &timer, &field, shooter, intake, alphaArm));
+  } else if (m_autoSelected == "Quadruple Close Double Far") {
+    sched->Schedule(autos::QuadrupleCloseDoubleFar(_swerveDrive, &timer, &field, shooter, intake, alphaArm));
+  } else if (m_autoSelected == "Quadruple Close Single Far") {
+    sched->Schedule(autos::QuadrupleCloseSingleFar(_swerveDrive, &timer, &field, shooter, intake, alphaArm));
+  }
 }
 void Robot::AutonomousPeriodic() {}
 
diff --git a/src/main/include/Auto.h b/src/main/include/Auto.h
index de439b35..bd1a62f8 100644
--- a/src/main/include/Auto.h
+++ b/src/main/include/Auto.h
@@ -8,15 +8,15 @@
 
 
 namespace autos {
-  std::shared_ptr<behaviour::Behaviour> AutoTest(wom::drivetrain::SwerveDrive _swerveDrive, frc::Timer* timer, frc::Field2d* field, Shooter _shooter, Intake _intake, AlphaArm _alphaArm);
+  std::shared_ptr<behaviour::Behaviour> AutoTest(wom::drivetrain::SwerveDrive* _swerveDrive, frc::Timer* timer, frc::Field2d* field, Shooter* _shooter, Intake* _intake, AlphaArm* _alphaArm);
 
-  std::shared_ptr<behaviour::Behaviour> Taxi(wom::drivetrain::SwerveDrive _swerveDrive, frc::Timer* timer, frc::Field2d* field, Shooter _shooter, Intake _intake, AlphaArm _alphaArm);
+  std::shared_ptr<behaviour::Behaviour> Taxi(wom::drivetrain::SwerveDrive* _swerveDrive, frc::Timer* timer, frc::Field2d* field, Shooter* _shooter, Intake* _intake, AlphaArm* _alphaArm);
 
-  std::shared_ptr<behaviour::Behaviour> QuadrupleClose(wom::drivetrain::SwerveDrive _swerveDrive, frc::Timer* timer, frc::Field2d* field, Shooter _shooter, Intake _intake, AlphaArm _alphaArm);
+  std::shared_ptr<behaviour::Behaviour> QuadrupleClose(wom::drivetrain::SwerveDrive* _swerveDrive, frc::Timer* timer, frc::Field2d* field, Shooter* _shooter, Intake* _intake, AlphaArm* _alphaArm);
 
-  std::shared_ptr<behaviour::Behaviour> QuadrupleFar(wom::drivetrain::SwerveDrive _swerveDrive, frc::Timer* timer, frc::Field2d* field, Shooter _shooter, Intake _intake, AlphaArm _alphaArm);
+  std::shared_ptr<behaviour::Behaviour> QuadrupleFar(wom::drivetrain::SwerveDrive* _swerveDrive, frc::Timer* timer, frc::Field2d* field, Shooter* _shooter, Intake* _intake, AlphaArm* _alphaArm);
 
-  std::shared_ptr<behaviour::Behaviour> QuadrupleCloseDoubleFar(wom::drivetrain::SwerveDrive _swerveDrive, frc::Timer* timer, frc::Field2d* field, Shooter _shooter, Intake _intake, AlphaArm _alphaArm);
+  std::shared_ptr<behaviour::Behaviour> QuadrupleCloseDoubleFar(wom::drivetrain::SwerveDrive* _swerveDrive, frc::Timer* timer, frc::Field2d* field, Shooter* _shooter, Intake* _intake, AlphaArm* _alphaArm);
 
-  std::shared_ptr<behaviour::Behaviour> QuadrupleCloseSingleFar(wom::drivetrain::SwerveDrive _swerveDrive, frc::Timer* timer, frc::Field2d* field, Shooter _shooter, Intake _intake, AlphaArm _alphaArm);
+  std::shared_ptr<behaviour::Behaviour> QuadrupleCloseSingleFar(wom::drivetrain::SwerveDrive* _swerveDrive, frc::Timer* timer, frc::Field2d* field, Shooter* _shooter, Intake* _intake, AlphaArm* _alphaArm);
 }
\ No newline at end of file
diff --git a/src/main/include/Robot.h b/src/main/include/Robot.h
index 158a6fa6..52a7b12f 100644
--- a/src/main/include/Robot.h
+++ b/src/main/include/Robot.h
@@ -44,17 +44,35 @@ class Robot : public frc::TimedRobot {
   RobotMap robotmap;
   wom::BehaviourScheduler* sched;
   frc::EventLoop loop;
-  Shooter* shooter;
 
-  Intake* intake;
-  frc::SendableChooser<std::string> m_chooser;
-  frc::Field2d m_field;
-  frc::Timer simulation_timer;
+  frc::Field2d field;
+  frc::Timer timer;
   frc::SendableChooser<std::string> m_path_chooser;
 
   wom::SwerveDrive* _swerveDrive;
 
   AlphaArm* alphaArm;
+  Intake* intake;
+  Shooter* shooter;
+
+  frc::SendableChooser<std::string> m_chooser;
+  const std::string kTaxi = "kTaxi";
+  const std::string kAutoTest = "kAutoTest";
+  const std::string kQuadrupleClose = "kQuadrupleClose";
+  const std::string kQuadrupleFar = "kQuadrupleFar";
+  const std::string kQuadrupleCloseDoubleFar = "kQuadrupleCloseDoubleFar";
+  const std::string kQuadrupleCloseSingleFar = "kQuadrupleCloseSingleFar";
+  std::string m_autoSelected;
+
+  std::string defaultAuto = "kTaxi";
+  std::vector<std::string> autoOptions = {
+    kTaxi,
+    kAutoTest,
+    kQuadrupleClose,
+    kQuadrupleFar,
+    kQuadrupleCloseDoubleFar,
+    kQuadrupleCloseSingleFar,
+  };
 
   // ctre::phoenix6::hardware::TalonFX *frontLeft;
   // ctre::phoenix6::hardware::TalonFX *frontRight;