From 9d7918e2150e4504d949801f72ca99992c6b606f Mon Sep 17 00:00:00 2001
From: Kingsley Wong <kingsley.wong@student.education.wa.edu.au>
Date: Fri, 5 Jan 2024 11:56:15 +0800
Subject: [PATCH 001/207] First version Tank Drive

---
 src/main/cpp/Robot.cpp           | 37 +++++++++++++++--
 src/main/cpp/arm.cpp             | 28 +++++++++++++
 src/main/cpp/armBehaviour.cpp    | 17 ++++++++
 src/main/cpp/tank.cpp            | 25 ++++++++++++
 src/main/cpp/tankBehaviour.cpp   | 13 ++++++
 src/main/include/Robot.h         | 12 +++++-
 src/main/include/RobotMap.h      | 69 ++++++++++++++++++++++++++++++++
 src/main/include/arm.h           | 31 ++++++++++++++
 src/main/include/armBehaviour.h  | 15 +++++++
 src/main/include/tank.h          | 32 +++++++++++++++
 src/main/include/tankBehaviour.h | 18 +++++++++
 11 files changed, 293 insertions(+), 4 deletions(-)
 create mode 100644 src/main/cpp/arm.cpp
 create mode 100644 src/main/cpp/armBehaviour.cpp
 create mode 100644 src/main/cpp/tank.cpp
 create mode 100644 src/main/cpp/tankBehaviour.cpp
 create mode 100644 src/main/include/RobotMap.h
 create mode 100644 src/main/include/arm.h
 create mode 100644 src/main/include/armBehaviour.h
 create mode 100644 src/main/include/tank.h
 create mode 100644 src/main/include/tankBehaviour.h

diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp
index 244353d4..13a3c494 100644
--- a/src/main/cpp/Robot.cpp
+++ b/src/main/cpp/Robot.cpp
@@ -4,13 +4,44 @@
 
 #include "Robot.h"
 
-void Robot::RobotInit() {}
-void Robot::RobotPeriodic() {}
+static units::second_t lastPeriodic;
+
+void Robot::RobotInit() {
+  lastPeriodic = wom::now();
+
+  tank = new TankDrive(map.tankSystem.tankConfig);
+  wom::BehaviourScheduler::GetInstance()->Register(tank);
+  arm = new Arm(map.armSystem.config);
+  wom::BehaviourScheduler::GetInstance()->Register(arm);
+
+  arm->SetDefaultBehaviour([this]() {
+    return wom::make<ArmManualControl>(arm, map.driver);
+  });
+
+  tank->SetDefaultBehaviour([this]() {
+    return wom::make<TankManualControl>(tank, map.driver);
+  });
+}
+void Robot::RobotPeriodic() {
+  units::second_t dt = wom::now() - lastPeriodic;
+  lastPeriodic = wom::now();
+
+  loop.Poll();
+  wom::BehaviourScheduler::GetInstance()->Tick();
+
+  arm->OnUpdate(dt);
+  tank->OnUpdate(dt);
+}
+
 
 void Robot::AutonomousInit() {}
 void Robot::AutonomousPeriodic() {}
 
-void Robot::TeleopInit() {}
+void Robot::TeleopInit() {
+  loop.Clear();
+  wom::BehaviourScheduler *scheduler = wom::BehaviourScheduler::GetInstance();
+  scheduler->InterruptAll();
+}
 void Robot::TeleopPeriodic() {}
 
 void Robot::DisabledInit() {}
diff --git a/src/main/cpp/arm.cpp b/src/main/cpp/arm.cpp
new file mode 100644
index 00000000..5cc4f8d6
--- /dev/null
+++ b/src/main/cpp/arm.cpp
@@ -0,0 +1,28 @@
+#include "arm.h"
+
+Arm::Arm(ArmConfig config) : _config(config) {}
+
+void Arm::OnUpdate(units::second_t dt) {
+  
+  switch (_state) {
+    case ArmState::kIdle:
+    break;
+    case ArmState::kRaw:
+    break;
+    case ArmState::kAngle:
+    break;
+    default:
+      std::cout << "Error arm in invalid state" << std::endl;
+    break;
+  }
+}
+
+void Arm::setState(ArmState state) {
+  _state = state;
+}
+void Arm::setAngle(units::degree_t angle) {
+  _angle = angle;
+}
+void Arm::setRaw(units::volt_t voltage) {
+  _voltage = voltage;
+}
\ No newline at end of file
diff --git a/src/main/cpp/armBehaviour.cpp b/src/main/cpp/armBehaviour.cpp
new file mode 100644
index 00000000..92b8bbe4
--- /dev/null
+++ b/src/main/cpp/armBehaviour.cpp
@@ -0,0 +1,17 @@
+#include "armBehaviour.h"
+
+ArmManualControl::ArmManualControl(Arm *arm, frc::XboxController &codriver) : _arm(arm), _codriver(codriver) {
+  Controls(arm);
+}
+
+void ArmManualControl::OnTick(units::second_t dt) {
+
+  if(_codriver.GetRightBumper()) {
+    _arm->setRaw(5_V);
+  } else if (_codriver.GetLeftBumper()) {
+    _arm->setRaw(-5_V);
+  } else {
+    _arm->setRaw(0_V);
+  }
+
+}
\ No newline at end of file
diff --git a/src/main/cpp/tank.cpp b/src/main/cpp/tank.cpp
new file mode 100644
index 00000000..f45ba10b
--- /dev/null
+++ b/src/main/cpp/tank.cpp
@@ -0,0 +1,25 @@
+#include "tank.h"
+
+TankDrive::TankDrive(TankConfig config) : _config(config) {}
+
+void TankDrive::OnUpdate(units::second_t dt) {
+
+    switch (_state) {
+        case TankState::kIdle:
+        break;
+        case TankState::kRaw:
+        break;
+        default:
+          std::cout <<"Error: Tank Drive in INVALID STATE." << std::endl;
+        break;
+    }
+}
+
+void TankDrive::setState(TankState state) {
+  _state = state;
+}
+void TankDrive::setRaw(units::volt_t voltL, units::volt_t voltR) {
+//voltL and voltR stands for Voltage Left and Right.
+  _voltL = voltL;
+  _voltR = voltR;
+}
\ No newline at end of file
diff --git a/src/main/cpp/tankBehaviour.cpp b/src/main/cpp/tankBehaviour.cpp
new file mode 100644
index 00000000..4fc55878
--- /dev/null
+++ b/src/main/cpp/tankBehaviour.cpp
@@ -0,0 +1,13 @@
+#include "tankBehaviour.h"
+#include <frc/XboxController.h>
+
+TankManualControl::TankManualControl(TankDrive *tank, frc::XboxController &driver) : _tank(tank), _driver(driver) {
+  Controls(tank);
+};
+
+void TankManualControl::OnTick(units::second_t dt) {
+  _leftStick = (_driver.GetLeftY()>0.05?_driver.GetLeftY():0) * 8_V;
+  _rightStick = (_driver.GetRightY()>0.05?_driver.GetRightY():0) * 8_V;
+
+  _tank->setRaw(_leftStick, _rightStick);
+}
\ No newline at end of file
diff --git a/src/main/include/Robot.h b/src/main/include/Robot.h
index a564a3b7..5303af48 100644
--- a/src/main/include/Robot.h
+++ b/src/main/include/Robot.h
@@ -4,7 +4,12 @@
 
 #pragma once
 
+#include "tankBehaviour.h"
+#include "armBehaviour.h"
+#include "Wombat.h"
+#include "RobotMap.h"
 #include <frc/TimedRobot.h>
+#include <frc/event/EventLoop.h>
 
 class Robot : public frc::TimedRobot {
  public:
@@ -20,4 +25,9 @@ class Robot : public frc::TimedRobot {
   void TestPeriodic() override;
 
  private:
-};
+  frc::EventLoop loop;
+
+  RobotMap map;  
+  Arm *arm;
+  TankDrive *tank;
+};
\ No newline at end of file
diff --git a/src/main/include/RobotMap.h b/src/main/include/RobotMap.h
new file mode 100644
index 00000000..05a60c00
--- /dev/null
+++ b/src/main/include/RobotMap.h
@@ -0,0 +1,69 @@
+#pragma once
+
+#include "Wombat.h"
+#include "tank.h"
+#include <frc/XboxController.h>
+
+struct RobotMap {
+  frc::XboxController driver{0};
+
+  struct ArmSystem {
+    rev::CANSparkMax ArmMotor{98, rev::CANSparkMax::MotorType::kBrushless};
+    wom::VoltageController ArmMotorGroup = wom::VoltageController::Group(ArmMotor);
+
+    wom::Gearbox ArmGearbox {
+      &ArmMotorGroup,
+      nullptr,
+      wom::DCMotor::NEO(1).WithReduction(1)
+    };
+
+    ArmConfig config {
+      ArmGearbox,
+    };
+
+  }; ArmSystem armSystem;
+  
+  struct TankSystem {
+    wom::VoltageController tankDriveMotorFrontLeft{ new WPI_TalonSRX(99)};
+    wom::VoltageController tankDriveMotorFrontRight{ new WPI_TalonSRX(99)};
+    wom::VoltageController tankDriveMotorBackLeft{ new WPI_TalonSRX(99)};
+    wom::VoltageController tankDriveMotorBackRight{ new WPI_TalonSRX(99)};
+
+    //wom::VoltageController tankDriveMotorGroupLeft = wom::VoltageController::Group(new WPI_TalonSRX(99), new WPI_TalonSRX(99));
+   // wom::VoltageController tankDriveMotorGroupRight = wom::VoltageController::Group(tankDriveMotorBackRight, tankDriveMotorFrontRight);
+
+    wom::Gearbox tankFrontLeftGearbox {
+      &tankDriveMotorFrontLeft,
+      nullptr,
+      wom::DCMotor::CIM(1).WithReduction(1)
+    };
+
+    wom::Gearbox tankBackLeftGearbox {
+      &tankDriveMotorBackLeft,
+      nullptr,
+      wom::DCMotor::CIM(1).WithReduction(1)
+    };
+
+    wom::Gearbox tankFrontRightGearbox {
+      &tankDriveMotorFrontRight,
+      nullptr,
+      wom::DCMotor::CIM(1).WithReduction(1)
+    };
+    
+    wom::Gearbox tankBackRightGearbox {
+      &tankDriveMotorBackRight,
+      nullptr,
+      wom::DCMotor::CIM(1).WithReduction(1)
+    };
+
+    TankConfig tankConfig {
+      tankFrontRightGearbox,
+      tankBackRightGearbox,
+      tankFrontLeftGearbox,
+      tankBackLeftGearbox
+    };
+  }; TankSystem tankSystem;
+
+  //port to be filled later.
+
+};
\ No newline at end of file
diff --git a/src/main/include/arm.h b/src/main/include/arm.h
new file mode 100644
index 00000000..8460d5c2
--- /dev/null
+++ b/src/main/include/arm.h
@@ -0,0 +1,31 @@
+#pragma once
+
+#include "Wombat.h"
+
+struct ArmConfig {
+ wom::Gearbox armGearbox;
+};
+
+enum class ArmState {
+  kIdle,
+  kRaw,
+  kAngle
+};
+
+class Arm : public behaviour::HasBehaviour {
+ public:
+  Arm(ArmConfig config);
+
+  void OnUpdate(units::second_t dt);
+
+  void setState(ArmState state);
+  void setAngle(units::degree_t angle);
+  void setRaw(units::volt_t voltage);
+  
+ private:
+  ArmConfig _config;
+  ArmState _state = ArmState::kIdle;
+
+  units::degree_t _angle;
+  units::volt_t _voltage;
+};
\ No newline at end of file
diff --git a/src/main/include/armBehaviour.h b/src/main/include/armBehaviour.h
new file mode 100644
index 00000000..f5e5dc88
--- /dev/null
+++ b/src/main/include/armBehaviour.h
@@ -0,0 +1,15 @@
+#pragma once
+
+#include "Wombat.h"
+#include <frc/XboxController.h>
+#include "arm.h"
+
+class ArmManualControl : public behaviour::Behaviour {
+ public:
+  ArmManualControl(Arm *arm, frc::XboxController &codriver);
+
+  void OnTick(units::second_t dt) override;
+ private:
+  Arm *_arm;
+  frc::XboxController &_codriver;
+};
\ No newline at end of file
diff --git a/src/main/include/tank.h b/src/main/include/tank.h
new file mode 100644
index 00000000..9725c4f3
--- /dev/null
+++ b/src/main/include/tank.h
@@ -0,0 +1,32 @@
+#pragma once
+
+#include "wombat.h"
+
+struct TankConfig {
+  wom::Gearbox tankFrontLeftGearbox;
+  wom::Gearbox tankFrontRightGearbox;
+  wom::Gearbox tankBackLeftGearbox;
+  wom::Gearbox tankBackRightGearbox;
+};
+
+enum class TankState {
+  kIdle,
+  kRaw, 
+};
+
+class TankDrive : public behaviour::HasBehaviour {
+ public:
+  TankDrive(TankConfig config);
+
+  void OnUpdate(units::second_t dt);
+
+  void setState(TankState state);
+  void setRaw(units::volt_t voltL, units::volt_t voltR);
+  
+ private:
+  TankConfig _config;
+  TankState _state = TankState::kIdle;
+
+  units::volt_t _voltL;
+  units::volt_t _voltR;
+};
\ No newline at end of file
diff --git a/src/main/include/tankBehaviour.h b/src/main/include/tankBehaviour.h
new file mode 100644
index 00000000..e6f13203
--- /dev/null
+++ b/src/main/include/tankBehaviour.h
@@ -0,0 +1,18 @@
+#pragma once
+
+#include "Wombat.h"
+#include <frc/XboxController.h>
+#include "tank.h"
+
+class TankManualControl : public behaviour::Behaviour {
+ public:
+  TankManualControl(TankDrive *tank, frc::XboxController &driver);
+
+  void OnTick(units::second_t dt) override;
+ private:
+  TankDrive *_tank;
+  frc::XboxController &_driver;
+
+  units::volt_t _leftStick;
+  units::volt_t _rightStick;
+};
\ No newline at end of file

From 699f26ab4a1e68a064bf24a8101246c85ab74c67 Mon Sep 17 00:00:00 2001
From: Sean Chan <sean.chan3@student.education.wa.edu.au>
Date: Thu, 11 Jan 2024 16:33:37 +0800
Subject: [PATCH 002/207] Started implementing magazine :)

---
 src/main/cpp/Mag.cpp                       | 48 ++++++++++++++++++++++
 src/main/cpp/Robot.cpp                     | 28 +++++++++++--
 src/main/cpp/behaviours/MagBehaviour.cpp   | 38 +++++++++++++++++
 src/main/include/Mag.h                     | 37 +++++++++++++++++
 src/main/include/Robot.h                   |  7 ++++
 src/main/include/RobotMap.h                | 33 +++++++++++++++
 src/main/include/behaviours/MagBehaviour.h | 24 +++++++++++
 7 files changed, 212 insertions(+), 3 deletions(-)
 create mode 100644 src/main/cpp/Mag.cpp
 create mode 100644 src/main/cpp/behaviours/MagBehaviour.cpp
 create mode 100644 src/main/include/Mag.h
 create mode 100644 src/main/include/RobotMap.h
 create mode 100644 src/main/include/behaviours/MagBehaviour.h

diff --git a/src/main/cpp/Mag.cpp b/src/main/cpp/Mag.cpp
new file mode 100644
index 00000000..af650b84
--- /dev/null
+++ b/src/main/cpp/Mag.cpp
@@ -0,0 +1,48 @@
+#include "mag.h"
+
+Mag::Mag(MagConfig config) : _config(config) {}
+
+void Mag::OnUpdate(units::second_t dt) {
+//work on later 
+  switch (_state) {
+    case MagState::kIdle: 
+    {
+      // if (_config.intakeSensor.Get()) {
+      //   setState(MagState::kHold);
+      // }
+    }
+    break;
+    case MagState::kHold:
+    break;
+    case MagState::kEject:
+    break;
+    case MagState::kPass:
+    break;
+    default:
+      std::cout << "Error magazine in invalid state" << std::endl;
+    break;
+  }
+}
+void Mag::setState(MagState state) {
+  _state = state;
+}
+void Mag::setRaw(units::volt_t voltage) {
+  _voltage = voltage;
+}
+MagState Mag::getState() {
+  return _state;
+}
+
+// void Mag::setPass(units::volt_t voltage) {
+//   _voltage = voltage;
+// }
+// void Mag::setHold(units::volt_t voltage) {
+//   _voltage = voltage;
+// }
+// void Mag::setEject(units::volt_t voltage) {
+//   _voltage = voltage;
+// }
+
+
+
+
diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp
index 244353d4..3dfdf963 100644
--- a/src/main/cpp/Robot.cpp
+++ b/src/main/cpp/Robot.cpp
@@ -4,13 +4,34 @@
 
 #include "Robot.h"
 
-void Robot::RobotInit() {}
-void Robot::RobotPeriodic() {}
+static units::second_t lastPeriodic;
+
+void Robot::RobotInit() {
+  lastPeriodic = wom::now();
+
+  mag = new Mag(map.magSystem.config);
+  wom::BehaviourScheduler::GetInstance()->Register(mag);
+  mag->SetDefaultBehaviour([this]() {
+    return wom::make<MagManualControl>(mag, map.codriver);
+  });
+}
+
+void Robot::RobotPeriodic() {
+  units::second_t dt = wom::now() - lastPeriodic;
+  lastPeriodic = wom::now();
+
+  loop.Poll();
+  wom::BehaviourScheduler::GetInstance()->Tick();
+}
 
 void Robot::AutonomousInit() {}
 void Robot::AutonomousPeriodic() {}
 
-void Robot::TeleopInit() {}
+void Robot::TeleopInit() {
+  loop.Clear();
+  wom::BehaviourScheduler *scheduler = wom::BehaviourScheduler::GetInstance();
+  scheduler->InterruptAll();
+}
 void Robot::TeleopPeriodic() {}
 
 void Robot::DisabledInit() {}
@@ -18,3 +39,4 @@ void Robot::DisabledPeriodic() {}
 
 void Robot::TestInit() {}
 void Robot::TestPeriodic() {}
+
diff --git a/src/main/cpp/behaviours/MagBehaviour.cpp b/src/main/cpp/behaviours/MagBehaviour.cpp
new file mode 100644
index 00000000..467608a0
--- /dev/null
+++ b/src/main/cpp/behaviours/MagBehaviour.cpp
@@ -0,0 +1,38 @@
+#include "behaviours/MagBehaviour.h"
+
+MagManualControl::MagManualControl(Mag *mag, frc::XboxController &codriver) : _mag(mag), _codriver(codriver) {
+  Controls(mag);
+}
+MagAutoControl::MagAutoControl(Mag *mag, units::volt_t voltage) {
+  Controls(mag);
+}
+
+void MagManualControl::OnTick(units::second_t dt) {
+  //manual control
+  if (_codriver.GetAButton()) {
+    //adjust voltage later
+    _mag->setRaw(5_V);
+  } else if (_codriver.GetBButton()) {
+    _mag->setRaw(-5_V);
+  } else {
+    _mag->setRaw(0_V);
+  }
+}
+
+
+void MagAutoControl::OnTick(units::second_t dt) {
+  MagState _current = _mag->getState();
+  //Auto control.
+  //adjust voltage later.
+  if (_current == MagState::kHold) {
+    //-1_V is for keeping the note in the magazine during transit.
+    _mag->setRaw(-1_V);
+  } else if (_current == MagState::kEject) {
+    _mag->setRaw(-5_V);
+  } else if (_current == MagState::kPass) {
+    _mag->setRaw(5_V);
+  } else {
+    _mag->setRaw(0_V);
+  };
+}
+
diff --git a/src/main/include/Mag.h b/src/main/include/Mag.h
new file mode 100644
index 00000000..3a73f331
--- /dev/null
+++ b/src/main/include/Mag.h
@@ -0,0 +1,37 @@
+#pragma once
+#include "wombat.h"
+#include <frc/DigitalInput.h>
+
+struct MagConfig {
+  wom::Gearbox magGearbox;
+  // frc::DigitalInput intakeSensor;
+  // frc::DigitalInput magSensor;
+};
+
+enum class MagState {
+  kIdle,
+  kHold,
+  kPass,
+  kEject
+};
+
+class Mag : public behaviour::HasBehaviour {
+  public:
+    Mag(MagConfig config);
+
+    void OnUpdate(units::second_t dt);
+    void setState(MagState state);
+    // void setReceive(units::volt_t voltage)
+    // void setHold(units::volt_t voltage)
+    // void setPass(units::volt_t voltage)
+    // void setLeave(units::volt_t voltage)
+    void setRaw(units::volt_t voltage);
+    MagState getState();
+  private:
+    MagConfig _config;
+    MagState _state;
+    frc::DigitalInput _intakeSensor {0};
+    frc::DigitalInput _magSensor {1};
+
+    units::volt_t _voltage; 
+};
\ No newline at end of file
diff --git a/src/main/include/Robot.h b/src/main/include/Robot.h
index a564a3b7..6db9e9dc 100644
--- a/src/main/include/Robot.h
+++ b/src/main/include/Robot.h
@@ -4,7 +4,10 @@
 
 #pragma once
 
+#include "RobotMap.h"
+#include "behaviours/MagBehaviour.h"
 #include <frc/TimedRobot.h>
+#include <frc/event/Eventloop.h>
 
 class Robot : public frc::TimedRobot {
  public:
@@ -20,4 +23,8 @@ class Robot : public frc::TimedRobot {
   void TestPeriodic() override;
 
  private:
+  frc::EventLoop loop;
+
+  RobotMap map;
+  Mag *mag;
 };
diff --git a/src/main/include/RobotMap.h b/src/main/include/RobotMap.h
new file mode 100644
index 00000000..24089b25
--- /dev/null
+++ b/src/main/include/RobotMap.h
@@ -0,0 +1,33 @@
+#pragma once
+#include "mag.h"
+#include "Wombat.h"
+
+#include <frc/XboxController.h>
+#include <frc/DigitalInput.h>
+
+struct RobotMap {
+  frc::XboxController driver{0};
+  frc::XboxController codriver{1};
+
+  struct Mag {
+    rev::CANSparkMax magMotor{99, rev::CANSparkMax::MotorType::kBrushless};
+    wom::VoltageController magMotorGroup = wom::VoltageController::Group(magMotor);
+    // frc::DigitalInput _switch;
+    // wom::CANSparkMaxEncoder magEncoder(&magMotor, 100);
+    frc::DigitalInput intakeSensor {0};
+    frc::DigitalInput magSensor {1};
+
+    wom::Gearbox magGearbox {
+      &magMotorGroup,
+      // &magEncoder,
+      nullptr,
+      wom::DCMotor::NEO(1).WithReduction(1)
+    };
+
+    MagConfig config {
+      magGearbox,
+      // intakeSensor,
+      // magSensor
+    };
+  }; Mag magSystem;
+};
\ No newline at end of file
diff --git a/src/main/include/behaviours/MagBehaviour.h b/src/main/include/behaviours/MagBehaviour.h
new file mode 100644
index 00000000..0ba0eefc
--- /dev/null
+++ b/src/main/include/behaviours/MagBehaviour.h
@@ -0,0 +1,24 @@
+#pragma once
+ 
+#include "wombat.h"
+#include "mag.h"
+#include <frc/XboxController.h>
+ 
+class MagManualControl : public behaviour::Behaviour {
+ public:
+  MagManualControl(Mag *mag, frc::XboxController &codriver);
+   
+  void OnTick(units::second_t dt) override;
+ private:
+  Mag *_mag;
+  frc::XboxController &_codriver;
+};
+
+class MagAutoControl : public behaviour::Behaviour {
+ public:
+  MagAutoControl(Mag *mag, units::volt_t voltage);
+   
+  void OnTick(units::second_t dt) override;
+ private:
+  Mag *_mag;
+};
\ No newline at end of file

From fdb7b39b8aec3b6eb904fb1396c3ab62b3fee4c7 Mon Sep 17 00:00:00 2001
From: Sean Chan <sean.chan3@student.education.wa.edu.au>
Date: Sun, 14 Jan 2024 15:38:53 +0800
Subject: [PATCH 003/207] Finished Magazine

---
 src/main/cpp/Mag.cpp                       | 35 +++++++++--
 src/main/cpp/behaviours/MagBehaviour.cpp   | 73 ++++++++++++++--------
 src/main/include/Mag.h                     | 15 +++--
 src/main/include/Robot.h                   |  6 +-
 src/main/include/RobotMap.h                | 15 ++---
 src/main/include/behaviours/MagBehaviour.h |  1 +
 6 files changed, 95 insertions(+), 50 deletions(-)

diff --git a/src/main/cpp/Mag.cpp b/src/main/cpp/Mag.cpp
index af650b84..ef62520a 100644
--- a/src/main/cpp/Mag.cpp
+++ b/src/main/cpp/Mag.cpp
@@ -1,28 +1,53 @@
-#include "mag.h"
+#include "Mag.h"
 
 Mag::Mag(MagConfig config) : _config(config) {}
 
 void Mag::OnUpdate(units::second_t dt) {
-//work on later 
+//Work on later. 
   switch (_state) {
     case MagState::kIdle: 
     {
-      // if (_config.intakeSensor.Get()) {
-      //   setState(MagState::kHold);
-      // }
+      //set to 0 volts
+      if (_config.intakeSensor->Get()) {
+        setState(MagState::kHold);
+      }
     }
     break;
+
     case MagState::kHold:
+    {
+      if (_config.magSensor->Get() == 0) {
+        setState(MagState::kIdle);
+      }
+    }
     break;
+
     case MagState::kEject:
+    {
+      if (_config.magSensor->Get() == 0) {
+        setState(MagState::kIdle);
+      }
+    }
+    break;
+
+    case MagState::kRaw:
     break;
+
     case MagState::kPass:
+    {
+      if (_config.shooterSensor->Get()) {
+        setState(MagState::kIdle);
+      }
+    }
     break;
+
     default:
       std::cout << "Error magazine in invalid state" << std::endl;
     break;
   }
 }
+
+
 void Mag::setState(MagState state) {
   _state = state;
 }
diff --git a/src/main/cpp/behaviours/MagBehaviour.cpp b/src/main/cpp/behaviours/MagBehaviour.cpp
index 467608a0..9986c9ea 100644
--- a/src/main/cpp/behaviours/MagBehaviour.cpp
+++ b/src/main/cpp/behaviours/MagBehaviour.cpp
@@ -3,36 +3,55 @@
 MagManualControl::MagManualControl(Mag *mag, frc::XboxController &codriver) : _mag(mag), _codriver(codriver) {
   Controls(mag);
 }
-MagAutoControl::MagAutoControl(Mag *mag, units::volt_t voltage) {
-  Controls(mag);
-}
 
 void MagManualControl::OnTick(units::second_t dt) {
-  //manual control
-  if (_codriver.GetAButton()) {
-    //adjust voltage later
-    _mag->setRaw(5_V);
-  } else if (_codriver.GetBButton()) {
-    _mag->setRaw(-5_V);
-  } else {
-    _mag->setRaw(0_V);
-  }
-}
 
+  if (_codriver.GetAButtonPressed()) {
+    if (_rawControl == true) {
+      _rawControl = false;
+    } else {
+      _rawControl = true;
+    }
+  }
+  
+  if (_rawControl) {
+    // Manual control, right bumper for manual override.
+    if (_codriver.GetLeftBumper()) {
+      // Adjust voltage later.
+      _mag->setRaw(5_V);
+      _mag->setState(MagState::kRaw);
+    } else if (_codriver.GetRightBumper()) {
+      _mag->setRaw(-5_V);
+      _mag->setState(MagState::kRaw);
 
-void MagAutoControl::OnTick(units::second_t dt) {
-  MagState _current = _mag->getState();
-  //Auto control.
-  //adjust voltage later.
-  if (_current == MagState::kHold) {
-    //-1_V is for keeping the note in the magazine during transit.
-    _mag->setRaw(-1_V);
-  } else if (_current == MagState::kEject) {
-    _mag->setRaw(-5_V);
-  } else if (_current == MagState::kPass) {
-    _mag->setRaw(5_V);
+    } else {
+      _mag->setRaw(0_V);
+      // _mag->setState(MagState::kIdle);
+    }
+    
   } else {
-    _mag->setRaw(0_V);
-  };
+    _mag->setState(MagState::kIdle);
+    if (_codriver.GetLeftBumper()) {
+      _mag->setState(MagState::kPass);
+      
+    }
+  }
 }
-
+    
+    MagAutoControl::MagAutoControl(Mag *mag, units::volt_t voltage) {}
+    
+    void MagAutoControl::OnTick(units::second_t dt) {  
+      MagState _current = _mag->getState();
+      //Auto control.
+      //adjust voltage later.
+      if (_current == MagState::kHold) {
+        //-1_V is for keeping the note in the magazine during transit.
+        _mag->setRaw(-1_V);
+      } else if (_current == MagState::kEject) {
+        _mag->setRaw(-5_V);
+      } else if (_current == MagState::kPass) {
+        _mag->setRaw(5_V);
+      } else {
+        _mag->setRaw(0_V);
+      };
+    }
diff --git a/src/main/include/Mag.h b/src/main/include/Mag.h
index 3a73f331..e8f3ad8f 100644
--- a/src/main/include/Mag.h
+++ b/src/main/include/Mag.h
@@ -4,15 +4,17 @@
 
 struct MagConfig {
   wom::Gearbox magGearbox;
-  // frc::DigitalInput intakeSensor;
-  // frc::DigitalInput magSensor;
+  frc::DigitalInput* intakeSensor;
+  frc::DigitalInput* magSensor;
+  frc::DigitalInput* shooterSensor;
 };
 
 enum class MagState {
   kIdle,
   kHold,
-  kPass,
-  kEject
+  kEject,
+  kRaw,
+  kPass
 };
 
 class Mag : public behaviour::HasBehaviour {
@@ -30,8 +32,5 @@ class Mag : public behaviour::HasBehaviour {
   private:
     MagConfig _config;
     MagState _state;
-    frc::DigitalInput _intakeSensor {0};
-    frc::DigitalInput _magSensor {1};
-
-    units::volt_t _voltage; 
+    units::volt_t _voltage;
 };
\ No newline at end of file
diff --git a/src/main/include/Robot.h b/src/main/include/Robot.h
index 6db9e9dc..b161b6f5 100644
--- a/src/main/include/Robot.h
+++ b/src/main/include/Robot.h
@@ -15,9 +15,9 @@ class Robot : public frc::TimedRobot {
   void RobotPeriodic() override;
   void AutonomousInit() override;
   void AutonomousPeriodic() override;
-  void TeleopInit() override;
-  void TeleopPeriodic() override;
-  void DisabledInit() override;
+  void TeleopInit() override;//skibidi
+  void TeleopPeriodic() override;//bum
+  void DisabledInit() override;//poo
   void DisabledPeriodic() override;
   void TestInit() override;
   void TestPeriodic() override;
diff --git a/src/main/include/RobotMap.h b/src/main/include/RobotMap.h
index 24089b25..108f8b7b 100644
--- a/src/main/include/RobotMap.h
+++ b/src/main/include/RobotMap.h
@@ -14,8 +14,8 @@ struct RobotMap {
     wom::VoltageController magMotorGroup = wom::VoltageController::Group(magMotor);
     // frc::DigitalInput _switch;
     // wom::CANSparkMaxEncoder magEncoder(&magMotor, 100);
-    frc::DigitalInput intakeSensor {0};
-    frc::DigitalInput magSensor {1};
+    frc::DigitalInput intakeSensor{0};
+    frc::DigitalInput magSensor{1};
 
     wom::Gearbox magGearbox {
       &magMotorGroup,
@@ -25,9 +25,10 @@ struct RobotMap {
     };
 
     MagConfig config {
-      magGearbox,
-      // intakeSensor,
-      // magSensor
-    };
-  }; Mag magSystem;
+       magGearbox,
+       &intakeSensor,
+       &magSensor
+     };
+  }; 
+  Mag magSystem;
 };
\ No newline at end of file
diff --git a/src/main/include/behaviours/MagBehaviour.h b/src/main/include/behaviours/MagBehaviour.h
index 0ba0eefc..fc63a56f 100644
--- a/src/main/include/behaviours/MagBehaviour.h
+++ b/src/main/include/behaviours/MagBehaviour.h
@@ -12,6 +12,7 @@ class MagManualControl : public behaviour::Behaviour {
  private:
   Mag *_mag;
   frc::XboxController &_codriver;
+  bool _rawControl; // Default of Bool is False.
 };
 
 class MagAutoControl : public behaviour::Behaviour {

From d6e2f1c007dee9f3ed4b1cde2ea0997e3b7d07ca Mon Sep 17 00:00:00 2001
From: Sean Chan <sean.chan3@student.education.wa.edu.au>
Date: Sun, 14 Jan 2024 15:58:18 +0800
Subject: [PATCH 004/207] test

---
 src/main/cpp/Climber.cpp                      |  0
 src/main/cpp/Mag.cpp                          |  1 +
 src/main/cpp/behaviours/ClimberBehaviour.cpp  |  0
 src/main/include/Climber.h                    | 22 +++++++++++++++++++
 .../include/behaviours/ClimberBehaviour.h     |  0
 5 files changed, 23 insertions(+)
 create mode 100644 src/main/cpp/Climber.cpp
 create mode 100644 src/main/cpp/behaviours/ClimberBehaviour.cpp
 create mode 100644 src/main/include/Climber.h
 create mode 100644 src/main/include/behaviours/ClimberBehaviour.h

diff --git a/src/main/cpp/Climber.cpp b/src/main/cpp/Climber.cpp
new file mode 100644
index 00000000..e69de29b
diff --git a/src/main/cpp/Mag.cpp b/src/main/cpp/Mag.cpp
index ef62520a..6f0e51b9 100644
--- a/src/main/cpp/Mag.cpp
+++ b/src/main/cpp/Mag.cpp
@@ -51,6 +51,7 @@ void Mag::OnUpdate(units::second_t dt) {
 void Mag::setState(MagState state) {
   _state = state;
 }
+
 void Mag::setRaw(units::volt_t voltage) {
   _voltage = voltage;
 }
diff --git a/src/main/cpp/behaviours/ClimberBehaviour.cpp b/src/main/cpp/behaviours/ClimberBehaviour.cpp
new file mode 100644
index 00000000..e69de29b
diff --git a/src/main/include/Climber.h b/src/main/include/Climber.h
new file mode 100644
index 00000000..0f0e78be
--- /dev/null
+++ b/src/main/include/Climber.h
@@ -0,0 +1,22 @@
+#pragma once
+
+#include "wombat.h"
+
+struct ClimberConfig {
+  wom::Gearbox climberGearBox;
+};
+
+enum class ClimberState {
+  kIdle,
+  kLift,
+  kHang
+};
+
+class Climber : public behaviour:HasBehaviour {
+  public:
+    Climber(ClimberConfig config);
+
+    void OnUpdate(units::second_t dt);
+    void setState(ClimberState state);
+    
+}
\ No newline at end of file
diff --git a/src/main/include/behaviours/ClimberBehaviour.h b/src/main/include/behaviours/ClimberBehaviour.h
new file mode 100644
index 00000000..e69de29b

From 2cbcf1014e0837e05d9f07d485027e378f120436 Mon Sep 17 00:00:00 2001
From: Sean Chan <sean.chan3@student.education.wa.edu.au>
Date: Mon, 15 Jan 2024 10:59:15 +0800
Subject: [PATCH 005/207] Worked on more of the magazine

---
 src/main/cpp/Mag.cpp                     | 16 ++++------------
 src/main/cpp/Robot.cpp                   |  4 +++-
 src/main/cpp/behaviours/MagBehaviour.cpp |  3 ---
 src/main/include/Climber.h               | 12 +++++++++++-
 src/main/include/Mag.h                   |  4 ----
 src/main/include/Robot.h                 |  6 +++---
 src/main/include/RobotMap.h              |  4 +---
 7 files changed, 22 insertions(+), 27 deletions(-)

diff --git a/src/main/cpp/Mag.cpp b/src/main/cpp/Mag.cpp
index 6f0e51b9..85329d2f 100644
--- a/src/main/cpp/Mag.cpp
+++ b/src/main/cpp/Mag.cpp
@@ -3,11 +3,9 @@
 Mag::Mag(MagConfig config) : _config(config) {}
 
 void Mag::OnUpdate(units::second_t dt) {
-//Work on later. 
   switch (_state) {
     case MagState::kIdle: 
     {
-      //set to 0 volts
       if (_config.intakeSensor->Get()) {
         setState(MagState::kHold);
       }
@@ -19,6 +17,7 @@ void Mag::OnUpdate(units::second_t dt) {
       if (_config.magSensor->Get() == 0) {
         setState(MagState::kIdle);
       }
+      _config.magGearbox.transmission->SetVoltage(_voltage);
     }
     break;
 
@@ -27,10 +26,12 @@ void Mag::OnUpdate(units::second_t dt) {
       if (_config.magSensor->Get() == 0) {
         setState(MagState::kIdle);
       }
+      _config.magGearbox.transmission->SetVoltage(_voltage);
     }
     break;
 
     case MagState::kRaw:
+      _config.magGearbox.transmission->SetVoltage(_voltage);
     break;
 
     case MagState::kPass:
@@ -38,6 +39,7 @@ void Mag::OnUpdate(units::second_t dt) {
       if (_config.shooterSensor->Get()) {
         setState(MagState::kIdle);
       }
+      _config.magGearbox.transmission->SetVoltage(_voltage);
     }
     break;
 
@@ -59,16 +61,6 @@ MagState Mag::getState() {
   return _state;
 }
 
-// void Mag::setPass(units::volt_t voltage) {
-//   _voltage = voltage;
-// }
-// void Mag::setHold(units::volt_t voltage) {
-//   _voltage = voltage;
-// }
-// void Mag::setEject(units::volt_t voltage) {
-//   _voltage = voltage;
-// }
-
 
 
 
diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp
index 3dfdf963..f0e7b64c 100644
--- a/src/main/cpp/Robot.cpp
+++ b/src/main/cpp/Robot.cpp
@@ -19,9 +19,11 @@ void Robot::RobotInit() {
 void Robot::RobotPeriodic() {
   units::second_t dt = wom::now() - lastPeriodic;
   lastPeriodic = wom::now();
-
+  
   loop.Poll();
   wom::BehaviourScheduler::GetInstance()->Tick();
+
+  mag->OnUpdate(dt);
 }
 
 void Robot::AutonomousInit() {}
diff --git a/src/main/cpp/behaviours/MagBehaviour.cpp b/src/main/cpp/behaviours/MagBehaviour.cpp
index 9986c9ea..f9a9d1c7 100644
--- a/src/main/cpp/behaviours/MagBehaviour.cpp
+++ b/src/main/cpp/behaviours/MagBehaviour.cpp
@@ -17,7 +17,6 @@ void MagManualControl::OnTick(units::second_t dt) {
   if (_rawControl) {
     // Manual control, right bumper for manual override.
     if (_codriver.GetLeftBumper()) {
-      // Adjust voltage later.
       _mag->setRaw(5_V);
       _mag->setState(MagState::kRaw);
     } else if (_codriver.GetRightBumper()) {
@@ -26,7 +25,6 @@ void MagManualControl::OnTick(units::second_t dt) {
 
     } else {
       _mag->setRaw(0_V);
-      // _mag->setState(MagState::kIdle);
     }
     
   } else {
@@ -43,7 +41,6 @@ void MagManualControl::OnTick(units::second_t dt) {
     void MagAutoControl::OnTick(units::second_t dt) {  
       MagState _current = _mag->getState();
       //Auto control.
-      //adjust voltage later.
       if (_current == MagState::kHold) {
         //-1_V is for keeping the note in the magazine during transit.
         _mag->setRaw(-1_V);
diff --git a/src/main/include/Climber.h b/src/main/include/Climber.h
index 0f0e78be..3760f65e 100644
--- a/src/main/include/Climber.h
+++ b/src/main/include/Climber.h
@@ -18,5 +18,15 @@ class Climber : public behaviour:HasBehaviour {
 
     void OnUpdate(units::second_t dt);
     void setState(ClimberState state);
-    
+    // void setReceive(units::volt_t voltage)
+    // void setHold(units::volt_t voltage)
+    // void setPass(units::volt_t voltage)
+    // void setLeave(units::volt_t voltage)
+    void setRaw(units::volt_t voltage);
+    ClimberState getState();
+  private:
+    ClimberConfig _config;
+    ClimberState _state;
+    units::volt_t _voltage;
+
 }
\ No newline at end of file
diff --git a/src/main/include/Mag.h b/src/main/include/Mag.h
index e8f3ad8f..9787c2b6 100644
--- a/src/main/include/Mag.h
+++ b/src/main/include/Mag.h
@@ -23,10 +23,6 @@ class Mag : public behaviour::HasBehaviour {
 
     void OnUpdate(units::second_t dt);
     void setState(MagState state);
-    // void setReceive(units::volt_t voltage)
-    // void setHold(units::volt_t voltage)
-    // void setPass(units::volt_t voltage)
-    // void setLeave(units::volt_t voltage)
     void setRaw(units::volt_t voltage);
     MagState getState();
   private:
diff --git a/src/main/include/Robot.h b/src/main/include/Robot.h
index b161b6f5..6db9e9dc 100644
--- a/src/main/include/Robot.h
+++ b/src/main/include/Robot.h
@@ -15,9 +15,9 @@ class Robot : public frc::TimedRobot {
   void RobotPeriodic() override;
   void AutonomousInit() override;
   void AutonomousPeriodic() override;
-  void TeleopInit() override;//skibidi
-  void TeleopPeriodic() override;//bum
-  void DisabledInit() override;//poo
+  void TeleopInit() override;
+  void TeleopPeriodic() override;
+  void DisabledInit() override;
   void DisabledPeriodic() override;
   void TestInit() override;
   void TestPeriodic() override;
diff --git a/src/main/include/RobotMap.h b/src/main/include/RobotMap.h
index 108f8b7b..65e83f87 100644
--- a/src/main/include/RobotMap.h
+++ b/src/main/include/RobotMap.h
@@ -12,14 +12,12 @@ struct RobotMap {
   struct Mag {
     rev::CANSparkMax magMotor{99, rev::CANSparkMax::MotorType::kBrushless};
     wom::VoltageController magMotorGroup = wom::VoltageController::Group(magMotor);
-    // frc::DigitalInput _switch;
-    // wom::CANSparkMaxEncoder magEncoder(&magMotor, 100);
+    //wom::CANSparkMaxEncoder magEncoder(&magMotor, 100);
     frc::DigitalInput intakeSensor{0};
     frc::DigitalInput magSensor{1};
 
     wom::Gearbox magGearbox {
       &magMotorGroup,
-      // &magEncoder,
       nullptr,
       wom::DCMotor::NEO(1).WithReduction(1)
     };

From 7f3b88200de731b95979511075cecf13653b3233 Mon Sep 17 00:00:00 2001
From: Kingsley Wong <kingsley.wong@student.education.wa.edu.au>
Date: Mon, 15 Jan 2024 11:31:48 +0800
Subject: [PATCH 006/207] Intake draft

---
 src/main/cpp/Intake.cpp            | 25 ++++++++++++++++++++++++
 src/main/cpp/IntakeBehaviour.cpp   | 16 +++++++++++++++
 src/main/cpp/Robot.cpp             |  8 +++++++-
 src/main/include/Intake.h          | 31 ++++++++++++++++++++++++++++++
 src/main/include/IntakeBehaviour.h | 17 ++++++++++++++++
 src/main/include/Robot.h           |  3 +++
 src/main/include/RobotMap.h        | 22 +++++++++++++++++++++
 7 files changed, 121 insertions(+), 1 deletion(-)
 create mode 100644 src/main/cpp/Intake.cpp
 create mode 100644 src/main/cpp/IntakeBehaviour.cpp
 create mode 100644 src/main/include/Intake.h
 create mode 100644 src/main/include/IntakeBehaviour.h

diff --git a/src/main/cpp/Intake.cpp b/src/main/cpp/Intake.cpp
new file mode 100644
index 00000000..20a25bc4
--- /dev/null
+++ b/src/main/cpp/Intake.cpp
@@ -0,0 +1,25 @@
+#include "Intake.h"
+
+Intake::Intake(IntakeConfig config) : _config(config) {
+}
+
+void Intake::OnUpdate(units::second_t dt) {
+
+  switch (_state) {
+      case IntakeState::kIdle:
+      break;
+      case IntakeState::kRaw:
+      break;
+      default:
+        std::cout <<"Error: Intake in INVALID STATE." << std::endl;
+      break;
+  }
+  _config.IntakeMotor.transmission->SetVoltage(_voltage);
+}
+
+void Intake::setState(IntakeState state) {
+  _state = state;
+}
+void Intake::setRaw(units::volt_t voltage) {
+  _voltage = voltage;
+}
\ No newline at end of file
diff --git a/src/main/cpp/IntakeBehaviour.cpp b/src/main/cpp/IntakeBehaviour.cpp
new file mode 100644
index 00000000..9b5cd596
--- /dev/null
+++ b/src/main/cpp/IntakeBehaviour.cpp
@@ -0,0 +1,16 @@
+#include "IntakeBehaviour.h"
+#include <frc/XboxController.h>
+
+IntakeManualControl::IntakeManualControl(Intake *intake, frc::XboxController &codriver) : _intake(intake), _codriver(codriver) {
+  Controls(intake);
+};
+
+void IntakeManualControl::OnTick(units::second_t dt) {
+  if (_codriver.GetBButtonPressed()) {
+    _intake->setRaw(8_V);
+  } else {
+    _intake->setRaw(0_V);
+  }
+
+  //_intake->setRaw(_button);
+}
\ No newline at end of file
diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp
index 13a3c494..a1c6d52e 100644
--- a/src/main/cpp/Robot.cpp
+++ b/src/main/cpp/Robot.cpp
@@ -13,6 +13,8 @@ void Robot::RobotInit() {
   wom::BehaviourScheduler::GetInstance()->Register(tank);
   arm = new Arm(map.armSystem.config);
   wom::BehaviourScheduler::GetInstance()->Register(arm);
+  intake = new Intake(map.intakeSystem.config);
+  wom::BehaviourScheduler::GetInstance()->Register(intake);
 
   arm->SetDefaultBehaviour([this]() {
     return wom::make<ArmManualControl>(arm, map.driver);
@@ -21,6 +23,10 @@ void Robot::RobotInit() {
   tank->SetDefaultBehaviour([this]() {
     return wom::make<TankManualControl>(tank, map.driver);
   });
+
+  intake->SetDefaultBehaviour([this]() {
+    return wom::make<IntakeManualControl>(intake, map.driver);
+  });
 }
 void Robot::RobotPeriodic() {
   units::second_t dt = wom::now() - lastPeriodic;
@@ -31,9 +37,9 @@ void Robot::RobotPeriodic() {
 
   arm->OnUpdate(dt);
   tank->OnUpdate(dt);
+  intake->OnUpdate(dt);
 }
 
-
 void Robot::AutonomousInit() {}
 void Robot::AutonomousPeriodic() {}
 
diff --git a/src/main/include/Intake.h b/src/main/include/Intake.h
new file mode 100644
index 00000000..27b095c2
--- /dev/null
+++ b/src/main/include/Intake.h
@@ -0,0 +1,31 @@
+
+#pragma once
+
+#include "wombat.h"
+#include <frc/DigitalInput.h>
+
+struct IntakeConfig {
+  wom::Gearbox IntakeMotor;
+  frc::DigitalInput* intakeSensor;
+};
+
+enum class IntakeState {
+  kIdle,
+  kRaw
+};
+
+class Intake : public behaviour::HasBehaviour {
+ public:
+  Intake(IntakeConfig config);
+
+  void OnUpdate(units::second_t dt);
+  
+  void setState(IntakeState state);
+  void setRaw(units::volt_t voltage);
+
+ private:
+  IntakeConfig _config;
+  IntakeState _state = IntakeState::kIdle;
+
+  units::volt_t _voltage;
+}; 
\ No newline at end of file
diff --git a/src/main/include/IntakeBehaviour.h b/src/main/include/IntakeBehaviour.h
new file mode 100644
index 00000000..6d3b9cdc
--- /dev/null
+++ b/src/main/include/IntakeBehaviour.h
@@ -0,0 +1,17 @@
+#pragma once
+
+#include "Wombat.h"
+#include <frc/XboxController.h>
+#include "Intake.h"
+
+class IntakeManualControl : public behaviour::Behaviour {
+ public:
+  IntakeManualControl(Intake *intake, frc::XboxController &codriver);
+
+  void OnTick(units::second_t dt) override;
+ private:
+  Intake *_intake;
+  frc::XboxController &_codriver;
+
+  units::volt_t _button;
+}; 
\ No newline at end of file
diff --git a/src/main/include/Robot.h b/src/main/include/Robot.h
index 5303af48..3ebd2cff 100644
--- a/src/main/include/Robot.h
+++ b/src/main/include/Robot.h
@@ -6,6 +6,8 @@
 
 #include "tankBehaviour.h"
 #include "armBehaviour.h"
+#include "Intake.h"
+#include "IntakeBehaviour.h"
 #include "Wombat.h"
 #include "RobotMap.h"
 #include <frc/TimedRobot.h>
@@ -30,4 +32,5 @@ class Robot : public frc::TimedRobot {
   RobotMap map;  
   Arm *arm;
   TankDrive *tank;
+  Intake *intake;
 };
\ No newline at end of file
diff --git a/src/main/include/RobotMap.h b/src/main/include/RobotMap.h
index 05a60c00..821314f5 100644
--- a/src/main/include/RobotMap.h
+++ b/src/main/include/RobotMap.h
@@ -2,7 +2,10 @@
 
 #include "Wombat.h"
 #include "tank.h"
+#include "Intake.h"
+#include "IntakeBehaviour.h"
 #include <frc/XboxController.h>
+#include "Intake.h"
 
 struct RobotMap {
   frc::XboxController driver{0};
@@ -29,6 +32,7 @@ struct RobotMap {
     wom::VoltageController tankDriveMotorBackLeft{ new WPI_TalonSRX(99)};
     wom::VoltageController tankDriveMotorBackRight{ new WPI_TalonSRX(99)};
 
+
     //wom::VoltageController tankDriveMotorGroupLeft = wom::VoltageController::Group(new WPI_TalonSRX(99), new WPI_TalonSRX(99));
    // wom::VoltageController tankDriveMotorGroupRight = wom::VoltageController::Group(tankDriveMotorBackRight, tankDriveMotorFrontRight);
 
@@ -64,6 +68,24 @@ struct RobotMap {
     };
   }; TankSystem tankSystem;
 
+  struct IntakeSystem {
+
+    //rev::CANSparkMax IntakeController{99,rev::CANSparkMax::MotorType::kBrushless};
+    wom::VoltageController IntakeMotor{new rev::CANSparkMax(99, rev::CANSparkMax::MotorType::kBrushless) };
+
+    //wom::VoltageController IntakeMotor{ IntakeController NEO(99)};
+
+    wom::Gearbox IntakeGearbox {
+      &IntakeMotor,
+      nullptr,
+      wom::DCMotor::NEO(1).WithReduction(1)
+    };
+
+    IntakeConfig config {
+      IntakeGearbox,
+    };
+  }; IntakeSystem intakeSystem;
+
   //port to be filled later.
 
 };
\ No newline at end of file

From 7b2cf7b9ee1faf73bfcf8898ba704f8319624e64 Mon Sep 17 00:00:00 2001
From: Kingsley Wong <kingsley.wong@student.education.wa.edu.au>
Date: Mon, 15 Jan 2024 12:43:18 +0800
Subject: [PATCH 007/207] Intake draft fixed errors 1

---
 src/main/cpp/Intake.cpp            |  8 +++-
 src/main/cpp/IntakeBehaviour.cpp   |  2 +
 src/main/cpp/Robot.cpp             | 15 --------
 src/main/cpp/arm.cpp               | 28 --------------
 src/main/cpp/armBehaviour.cpp      | 17 ---------
 src/main/cpp/tank.cpp              | 25 ------------
 src/main/cpp/tankBehaviour.cpp     | 13 -------
 src/main/include/IntakeBehaviour.h |  2 +-
 src/main/include/Robot.h           |  5 +--
 src/main/include/RobotMap.h        | 61 +-----------------------------
 src/main/include/arm.h             | 31 ---------------
 src/main/include/armBehaviour.h    | 15 --------
 src/main/include/tank.h            | 32 ----------------
 src/main/include/tankBehaviour.h   | 18 ---------
 14 files changed, 12 insertions(+), 260 deletions(-)
 delete mode 100644 src/main/cpp/arm.cpp
 delete mode 100644 src/main/cpp/armBehaviour.cpp
 delete mode 100644 src/main/cpp/tank.cpp
 delete mode 100644 src/main/cpp/tankBehaviour.cpp
 delete mode 100644 src/main/include/arm.h
 delete mode 100644 src/main/include/armBehaviour.h
 delete mode 100644 src/main/include/tank.h
 delete mode 100644 src/main/include/tankBehaviour.h

diff --git a/src/main/cpp/Intake.cpp b/src/main/cpp/Intake.cpp
index 20a25bc4..b2ecf84c 100644
--- a/src/main/cpp/Intake.cpp
+++ b/src/main/cpp/Intake.cpp
@@ -7,14 +7,20 @@ void Intake::OnUpdate(units::second_t dt) {
 
   switch (_state) {
       case IntakeState::kIdle:
+      {
+        _config.IntakeMotor.transmission->SetVoltage(0_V);
+      }
       break;
       case IntakeState::kRaw:
+      {
+        _config.IntakeMotor.transmission->SetVoltage(_voltage);
+      }
       break;
       default:
         std::cout <<"Error: Intake in INVALID STATE." << std::endl;
       break;
   }
-  _config.IntakeMotor.transmission->SetVoltage(_voltage);
+  
 }
 
 void Intake::setState(IntakeState state) {
diff --git a/src/main/cpp/IntakeBehaviour.cpp b/src/main/cpp/IntakeBehaviour.cpp
index 9b5cd596..7eb0d565 100644
--- a/src/main/cpp/IntakeBehaviour.cpp
+++ b/src/main/cpp/IntakeBehaviour.cpp
@@ -8,8 +8,10 @@ IntakeManualControl::IntakeManualControl(Intake *intake, frc::XboxController &co
 void IntakeManualControl::OnTick(units::second_t dt) {
   if (_codriver.GetBButtonPressed()) {
     _intake->setRaw(8_V);
+    _intake->setState(IntakeState::kRaw)
   } else {
     _intake->setRaw(0_V);
+    _intake->setState(IntakeState::kIdle)
   }
 
   //_intake->setRaw(_button);
diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp
index a1c6d52e..038385f4 100644
--- a/src/main/cpp/Robot.cpp
+++ b/src/main/cpp/Robot.cpp
@@ -8,22 +8,9 @@ static units::second_t lastPeriodic;
 
 void Robot::RobotInit() {
   lastPeriodic = wom::now();
-
-  tank = new TankDrive(map.tankSystem.tankConfig);
-  wom::BehaviourScheduler::GetInstance()->Register(tank);
-  arm = new Arm(map.armSystem.config);
-  wom::BehaviourScheduler::GetInstance()->Register(arm);
   intake = new Intake(map.intakeSystem.config);
   wom::BehaviourScheduler::GetInstance()->Register(intake);
 
-  arm->SetDefaultBehaviour([this]() {
-    return wom::make<ArmManualControl>(arm, map.driver);
-  });
-
-  tank->SetDefaultBehaviour([this]() {
-    return wom::make<TankManualControl>(tank, map.driver);
-  });
-
   intake->SetDefaultBehaviour([this]() {
     return wom::make<IntakeManualControl>(intake, map.driver);
   });
@@ -35,8 +22,6 @@ void Robot::RobotPeriodic() {
   loop.Poll();
   wom::BehaviourScheduler::GetInstance()->Tick();
 
-  arm->OnUpdate(dt);
-  tank->OnUpdate(dt);
   intake->OnUpdate(dt);
 }
 
diff --git a/src/main/cpp/arm.cpp b/src/main/cpp/arm.cpp
deleted file mode 100644
index 5cc4f8d6..00000000
--- a/src/main/cpp/arm.cpp
+++ /dev/null
@@ -1,28 +0,0 @@
-#include "arm.h"
-
-Arm::Arm(ArmConfig config) : _config(config) {}
-
-void Arm::OnUpdate(units::second_t dt) {
-  
-  switch (_state) {
-    case ArmState::kIdle:
-    break;
-    case ArmState::kRaw:
-    break;
-    case ArmState::kAngle:
-    break;
-    default:
-      std::cout << "Error arm in invalid state" << std::endl;
-    break;
-  }
-}
-
-void Arm::setState(ArmState state) {
-  _state = state;
-}
-void Arm::setAngle(units::degree_t angle) {
-  _angle = angle;
-}
-void Arm::setRaw(units::volt_t voltage) {
-  _voltage = voltage;
-}
\ No newline at end of file
diff --git a/src/main/cpp/armBehaviour.cpp b/src/main/cpp/armBehaviour.cpp
deleted file mode 100644
index 92b8bbe4..00000000
--- a/src/main/cpp/armBehaviour.cpp
+++ /dev/null
@@ -1,17 +0,0 @@
-#include "armBehaviour.h"
-
-ArmManualControl::ArmManualControl(Arm *arm, frc::XboxController &codriver) : _arm(arm), _codriver(codriver) {
-  Controls(arm);
-}
-
-void ArmManualControl::OnTick(units::second_t dt) {
-
-  if(_codriver.GetRightBumper()) {
-    _arm->setRaw(5_V);
-  } else if (_codriver.GetLeftBumper()) {
-    _arm->setRaw(-5_V);
-  } else {
-    _arm->setRaw(0_V);
-  }
-
-}
\ No newline at end of file
diff --git a/src/main/cpp/tank.cpp b/src/main/cpp/tank.cpp
deleted file mode 100644
index f45ba10b..00000000
--- a/src/main/cpp/tank.cpp
+++ /dev/null
@@ -1,25 +0,0 @@
-#include "tank.h"
-
-TankDrive::TankDrive(TankConfig config) : _config(config) {}
-
-void TankDrive::OnUpdate(units::second_t dt) {
-
-    switch (_state) {
-        case TankState::kIdle:
-        break;
-        case TankState::kRaw:
-        break;
-        default:
-          std::cout <<"Error: Tank Drive in INVALID STATE." << std::endl;
-        break;
-    }
-}
-
-void TankDrive::setState(TankState state) {
-  _state = state;
-}
-void TankDrive::setRaw(units::volt_t voltL, units::volt_t voltR) {
-//voltL and voltR stands for Voltage Left and Right.
-  _voltL = voltL;
-  _voltR = voltR;
-}
\ No newline at end of file
diff --git a/src/main/cpp/tankBehaviour.cpp b/src/main/cpp/tankBehaviour.cpp
deleted file mode 100644
index 4fc55878..00000000
--- a/src/main/cpp/tankBehaviour.cpp
+++ /dev/null
@@ -1,13 +0,0 @@
-#include "tankBehaviour.h"
-#include <frc/XboxController.h>
-
-TankManualControl::TankManualControl(TankDrive *tank, frc::XboxController &driver) : _tank(tank), _driver(driver) {
-  Controls(tank);
-};
-
-void TankManualControl::OnTick(units::second_t dt) {
-  _leftStick = (_driver.GetLeftY()>0.05?_driver.GetLeftY():0) * 8_V;
-  _rightStick = (_driver.GetRightY()>0.05?_driver.GetRightY():0) * 8_V;
-
-  _tank->setRaw(_leftStick, _rightStick);
-}
\ No newline at end of file
diff --git a/src/main/include/IntakeBehaviour.h b/src/main/include/IntakeBehaviour.h
index 6d3b9cdc..a760cd21 100644
--- a/src/main/include/IntakeBehaviour.h
+++ b/src/main/include/IntakeBehaviour.h
@@ -13,5 +13,5 @@ class IntakeManualControl : public behaviour::Behaviour {
   Intake *_intake;
   frc::XboxController &_codriver;
 
-  units::volt_t _button;
+  units::volt_t _voltage;
 }; 
\ No newline at end of file
diff --git a/src/main/include/Robot.h b/src/main/include/Robot.h
index 3ebd2cff..2cf97084 100644
--- a/src/main/include/Robot.h
+++ b/src/main/include/Robot.h
@@ -4,8 +4,7 @@
 
 #pragma once
 
-#include "tankBehaviour.h"
-#include "armBehaviour.h"
+
 #include "Intake.h"
 #include "IntakeBehaviour.h"
 #include "Wombat.h"
@@ -30,7 +29,5 @@ class Robot : public frc::TimedRobot {
   frc::EventLoop loop;
 
   RobotMap map;  
-  Arm *arm;
-  TankDrive *tank;
   Intake *intake;
 };
\ No newline at end of file
diff --git a/src/main/include/RobotMap.h b/src/main/include/RobotMap.h
index 821314f5..8a527402 100644
--- a/src/main/include/RobotMap.h
+++ b/src/main/include/RobotMap.h
@@ -1,77 +1,18 @@
 #pragma once
 
 #include "Wombat.h"
-#include "tank.h"
 #include "Intake.h"
 #include "IntakeBehaviour.h"
 #include <frc/XboxController.h>
-#include "Intake.h"
 
 struct RobotMap {
   frc::XboxController driver{0};
 
-  struct ArmSystem {
-    rev::CANSparkMax ArmMotor{98, rev::CANSparkMax::MotorType::kBrushless};
-    wom::VoltageController ArmMotorGroup = wom::VoltageController::Group(ArmMotor);
-
-    wom::Gearbox ArmGearbox {
-      &ArmMotorGroup,
-      nullptr,
-      wom::DCMotor::NEO(1).WithReduction(1)
-    };
-
-    ArmConfig config {
-      ArmGearbox,
-    };
-
-  }; ArmSystem armSystem;
-  
-  struct TankSystem {
-    wom::VoltageController tankDriveMotorFrontLeft{ new WPI_TalonSRX(99)};
-    wom::VoltageController tankDriveMotorFrontRight{ new WPI_TalonSRX(99)};
-    wom::VoltageController tankDriveMotorBackLeft{ new WPI_TalonSRX(99)};
-    wom::VoltageController tankDriveMotorBackRight{ new WPI_TalonSRX(99)};
-
-
-    //wom::VoltageController tankDriveMotorGroupLeft = wom::VoltageController::Group(new WPI_TalonSRX(99), new WPI_TalonSRX(99));
-   // wom::VoltageController tankDriveMotorGroupRight = wom::VoltageController::Group(tankDriveMotorBackRight, tankDriveMotorFrontRight);
-
-    wom::Gearbox tankFrontLeftGearbox {
-      &tankDriveMotorFrontLeft,
-      nullptr,
-      wom::DCMotor::CIM(1).WithReduction(1)
-    };
-
-    wom::Gearbox tankBackLeftGearbox {
-      &tankDriveMotorBackLeft,
-      nullptr,
-      wom::DCMotor::CIM(1).WithReduction(1)
-    };
-
-    wom::Gearbox tankFrontRightGearbox {
-      &tankDriveMotorFrontRight,
-      nullptr,
-      wom::DCMotor::CIM(1).WithReduction(1)
-    };
-    
-    wom::Gearbox tankBackRightGearbox {
-      &tankDriveMotorBackRight,
-      nullptr,
-      wom::DCMotor::CIM(1).WithReduction(1)
-    };
-
-    TankConfig tankConfig {
-      tankFrontRightGearbox,
-      tankBackRightGearbox,
-      tankFrontLeftGearbox,
-      tankBackLeftGearbox
-    };
-  }; TankSystem tankSystem;
-
   struct IntakeSystem {
 
     //rev::CANSparkMax IntakeController{99,rev::CANSparkMax::MotorType::kBrushless};
     wom::VoltageController IntakeMotor{new rev::CANSparkMax(99, rev::CANSparkMax::MotorType::kBrushless) };
+    frc::DigitalInput intakeSensor(0);
 
     //wom::VoltageController IntakeMotor{ IntakeController NEO(99)};
 
diff --git a/src/main/include/arm.h b/src/main/include/arm.h
deleted file mode 100644
index 8460d5c2..00000000
--- a/src/main/include/arm.h
+++ /dev/null
@@ -1,31 +0,0 @@
-#pragma once
-
-#include "Wombat.h"
-
-struct ArmConfig {
- wom::Gearbox armGearbox;
-};
-
-enum class ArmState {
-  kIdle,
-  kRaw,
-  kAngle
-};
-
-class Arm : public behaviour::HasBehaviour {
- public:
-  Arm(ArmConfig config);
-
-  void OnUpdate(units::second_t dt);
-
-  void setState(ArmState state);
-  void setAngle(units::degree_t angle);
-  void setRaw(units::volt_t voltage);
-  
- private:
-  ArmConfig _config;
-  ArmState _state = ArmState::kIdle;
-
-  units::degree_t _angle;
-  units::volt_t _voltage;
-};
\ No newline at end of file
diff --git a/src/main/include/armBehaviour.h b/src/main/include/armBehaviour.h
deleted file mode 100644
index f5e5dc88..00000000
--- a/src/main/include/armBehaviour.h
+++ /dev/null
@@ -1,15 +0,0 @@
-#pragma once
-
-#include "Wombat.h"
-#include <frc/XboxController.h>
-#include "arm.h"
-
-class ArmManualControl : public behaviour::Behaviour {
- public:
-  ArmManualControl(Arm *arm, frc::XboxController &codriver);
-
-  void OnTick(units::second_t dt) override;
- private:
-  Arm *_arm;
-  frc::XboxController &_codriver;
-};
\ No newline at end of file
diff --git a/src/main/include/tank.h b/src/main/include/tank.h
deleted file mode 100644
index 9725c4f3..00000000
--- a/src/main/include/tank.h
+++ /dev/null
@@ -1,32 +0,0 @@
-#pragma once
-
-#include "wombat.h"
-
-struct TankConfig {
-  wom::Gearbox tankFrontLeftGearbox;
-  wom::Gearbox tankFrontRightGearbox;
-  wom::Gearbox tankBackLeftGearbox;
-  wom::Gearbox tankBackRightGearbox;
-};
-
-enum class TankState {
-  kIdle,
-  kRaw, 
-};
-
-class TankDrive : public behaviour::HasBehaviour {
- public:
-  TankDrive(TankConfig config);
-
-  void OnUpdate(units::second_t dt);
-
-  void setState(TankState state);
-  void setRaw(units::volt_t voltL, units::volt_t voltR);
-  
- private:
-  TankConfig _config;
-  TankState _state = TankState::kIdle;
-
-  units::volt_t _voltL;
-  units::volt_t _voltR;
-};
\ No newline at end of file
diff --git a/src/main/include/tankBehaviour.h b/src/main/include/tankBehaviour.h
deleted file mode 100644
index e6f13203..00000000
--- a/src/main/include/tankBehaviour.h
+++ /dev/null
@@ -1,18 +0,0 @@
-#pragma once
-
-#include "Wombat.h"
-#include <frc/XboxController.h>
-#include "tank.h"
-
-class TankManualControl : public behaviour::Behaviour {
- public:
-  TankManualControl(TankDrive *tank, frc::XboxController &driver);
-
-  void OnTick(units::second_t dt) override;
- private:
-  TankDrive *_tank;
-  frc::XboxController &_driver;
-
-  units::volt_t _leftStick;
-  units::volt_t _rightStick;
-};
\ No newline at end of file

From aacb4a6afa8b85068ca7f16ae0e1db14d3a132d4 Mon Sep 17 00:00:00 2001
From: Sean Chan <sean.chan3@student.education.wa.edu.au>
Date: Mon, 15 Jan 2024 12:55:01 +0800
Subject: [PATCH 008/207] Added/fixed auto code

---
 src/main/cpp/Mag.cpp                       |  8 ++++----
 src/main/cpp/behaviours/MagBehaviour.cpp   | 23 +++++++++-------------
 src/main/include/RobotMap.h                |  8 +++++---
 src/main/include/behaviours/MagBehaviour.h | 15 +++++++++++---
 4 files changed, 30 insertions(+), 24 deletions(-)

diff --git a/src/main/cpp/Mag.cpp b/src/main/cpp/Mag.cpp
index 85329d2f..796c03d9 100644
--- a/src/main/cpp/Mag.cpp
+++ b/src/main/cpp/Mag.cpp
@@ -17,16 +17,15 @@ void Mag::OnUpdate(units::second_t dt) {
       if (_config.magSensor->Get() == 0) {
         setState(MagState::kIdle);
       }
-      _config.magGearbox.transmission->SetVoltage(_voltage);
     }
     break;
 
     case MagState::kEject:
     {
-      if (_config.magSensor->Get() == 0) {
+      if (_config.magSensor->Get() && _config.intakeSensor->Get()) {
         setState(MagState::kIdle);
       }
-      _config.magGearbox.transmission->SetVoltage(_voltage);
+      _config.magGearbox.transmission->SetVoltage(-5_V);
     }
     break;
 
@@ -38,8 +37,9 @@ void Mag::OnUpdate(units::second_t dt) {
     {
       if (_config.shooterSensor->Get()) {
         setState(MagState::kIdle);
+      } else {
+        _config.magGearbox.transmission->SetVoltage(5_V);
       }
-      _config.magGearbox.transmission->SetVoltage(_voltage);
     }
     break;
 
diff --git a/src/main/cpp/behaviours/MagBehaviour.cpp b/src/main/cpp/behaviours/MagBehaviour.cpp
index f9a9d1c7..fe451107 100644
--- a/src/main/cpp/behaviours/MagBehaviour.cpp
+++ b/src/main/cpp/behaviours/MagBehaviour.cpp
@@ -36,19 +36,14 @@ void MagManualControl::OnTick(units::second_t dt) {
   }
 }
     
-    MagAutoControl::MagAutoControl(Mag *mag, units::volt_t voltage) {}
+    MagAutoPass::MagAutoPass(Mag *mag) {}
     
-    void MagAutoControl::OnTick(units::second_t dt) {  
-      MagState _current = _mag->getState();
-      //Auto control.
-      if (_current == MagState::kHold) {
-        //-1_V is for keeping the note in the magazine during transit.
-        _mag->setRaw(-1_V);
-      } else if (_current == MagState::kEject) {
-        _mag->setRaw(-5_V);
-      } else if (_current == MagState::kPass) {
-        _mag->setRaw(5_V);
-      } else {
-        _mag->setRaw(0_V);
-      };
+    void MagAutoPass::OnTick(units::second_t dt) {  
+      _mag->setState(MagState::kPass);
+    }
+
+    MagAutoHold::MagAutoHold(Mag *mag) {}
+    
+    void MagAutoHold::OnTick(units::second_t dt) {  
+      _mag->setState(MagState::kHold);
     }
diff --git a/src/main/include/RobotMap.h b/src/main/include/RobotMap.h
index 65e83f87..08cdb541 100644
--- a/src/main/include/RobotMap.h
+++ b/src/main/include/RobotMap.h
@@ -12,20 +12,22 @@ struct RobotMap {
   struct Mag {
     rev::CANSparkMax magMotor{99, rev::CANSparkMax::MotorType::kBrushless};
     wom::VoltageController magMotorGroup = wom::VoltageController::Group(magMotor);
-    //wom::CANSparkMaxEncoder magEncoder(&magMotor, 100);
+    wom::CANSparkMaxEncoder magEncoder(&magMotor, 100);
     frc::DigitalInput intakeSensor{0};
     frc::DigitalInput magSensor{1};
+    frc::DigitalInput shooterSensor{1};
 
     wom::Gearbox magGearbox {
       &magMotorGroup,
-      nullptr,
+      &magEncoder,
       wom::DCMotor::NEO(1).WithReduction(1)
     };
 
     MagConfig config {
        magGearbox,
        &intakeSensor,
-       &magSensor
+       &magSensor,
+       &shooterSensor
      };
   }; 
   Mag magSystem;
diff --git a/src/main/include/behaviours/MagBehaviour.h b/src/main/include/behaviours/MagBehaviour.h
index fc63a56f..34c12e9d 100644
--- a/src/main/include/behaviours/MagBehaviour.h
+++ b/src/main/include/behaviours/MagBehaviour.h
@@ -15,11 +15,20 @@ class MagManualControl : public behaviour::Behaviour {
   bool _rawControl; // Default of Bool is False.
 };
 
-class MagAutoControl : public behaviour::Behaviour {
+class MagAutoPass : public behaviour::Behaviour {
  public:
-  MagAutoControl(Mag *mag, units::volt_t voltage);
+  MagAutoPass(Mag *mag);
    
   void OnTick(units::second_t dt) override;
  private:
   Mag *_mag;
-};
\ No newline at end of file
+};
+
+class MagAutoHold : public behaviour::Behaviour {
+ public:
+  MagAutoHold(Mag *mag);
+   
+  void OnTick(units::second_t dt) override;
+ private:
+  Mag *_mag;
+};

From c86f054b1bf682d0b13185ea517cf4a4ee98f8b7 Mon Sep 17 00:00:00 2001
From: Sean Chan <sean.chan3@student.education.wa.edu.au>
Date: Mon, 15 Jan 2024 14:17:04 +0800
Subject: [PATCH 009/207] Made more fixes to magazine

---
 src/main/cpp/Mag.cpp        | 4 +++-
 src/main/include/RobotMap.h | 2 +-
 2 files changed, 4 insertions(+), 2 deletions(-)

diff --git a/src/main/cpp/Mag.cpp b/src/main/cpp/Mag.cpp
index 796c03d9..b87259e8 100644
--- a/src/main/cpp/Mag.cpp
+++ b/src/main/cpp/Mag.cpp
@@ -9,6 +9,7 @@ void Mag::OnUpdate(units::second_t dt) {
       if (_config.intakeSensor->Get()) {
         setState(MagState::kHold);
       }
+      _config.magGearbox.transmission->SetVoltage(0_V);
     }
     break;
 
@@ -17,12 +18,13 @@ void Mag::OnUpdate(units::second_t dt) {
       if (_config.magSensor->Get() == 0) {
         setState(MagState::kIdle);
       }
+      _config.magGearbox.transmission->SetVoltage(0_V);
     }
     break;
 
     case MagState::kEject:
     {
-      if (_config.magSensor->Get() && _config.intakeSensor->Get()) {
+      if (_config.magSensor->Get() == 0 && _config.intakeSensor->Get() ==  0) {
         setState(MagState::kIdle);
       }
       _config.magGearbox.transmission->SetVoltage(-5_V);
diff --git a/src/main/include/RobotMap.h b/src/main/include/RobotMap.h
index 08cdb541..2e77d8ac 100644
--- a/src/main/include/RobotMap.h
+++ b/src/main/include/RobotMap.h
@@ -12,7 +12,7 @@ struct RobotMap {
   struct Mag {
     rev::CANSparkMax magMotor{99, rev::CANSparkMax::MotorType::kBrushless};
     wom::VoltageController magMotorGroup = wom::VoltageController::Group(magMotor);
-    wom::CANSparkMaxEncoder magEncoder(&magMotor, 100);
+    wom::CANSparkMaxEncoder magEncoder(&magMotor, 42);
     frc::DigitalInput intakeSensor{0};
     frc::DigitalInput magSensor{1};
     frc::DigitalInput shooterSensor{1};

From b52b244b2e1558a3cfafb3f86eabfd67b98118ff Mon Sep 17 00:00:00 2001
From: Sean Chan <sean.chan3@student.education.wa.edu.au>
Date: Mon, 15 Jan 2024 14:59:29 +0800
Subject: [PATCH 010/207] Made more fixes to Magazine, previous had errors.

---
 src/main/include/RobotMap.h | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/main/include/RobotMap.h b/src/main/include/RobotMap.h
index 2e77d8ac..07e9f022 100644
--- a/src/main/include/RobotMap.h
+++ b/src/main/include/RobotMap.h
@@ -12,7 +12,7 @@ struct RobotMap {
   struct Mag {
     rev::CANSparkMax magMotor{99, rev::CANSparkMax::MotorType::kBrushless};
     wom::VoltageController magMotorGroup = wom::VoltageController::Group(magMotor);
-    wom::CANSparkMaxEncoder magEncoder(&magMotor, 42);
+    wom::CANSparkMaxEncoder magEncoder{&magMotor, 42};
     frc::DigitalInput intakeSensor{0};
     frc::DigitalInput magSensor{1};
     frc::DigitalInput shooterSensor{1};
@@ -27,7 +27,7 @@ struct RobotMap {
        magGearbox,
        &intakeSensor,
        &magSensor,
-       &shooterSensor
+       &shooterSensor,
      };
   }; 
   Mag magSystem;

From 5bffbb7761a6521a9fd2068b485907d6e89644ac Mon Sep 17 00:00:00 2001
From: Sean Chan <sean.chan3@student.education.wa.edu.au>
Date: Mon, 15 Jan 2024 18:37:01 +0800
Subject: [PATCH 011/207] Fixed Capatalisation errors in commands, magazine
 finally finished.

---
 src/main/cpp/Mag.cpp                     | 16 +++++++++-------
 src/main/cpp/behaviours/MagBehaviour.cpp | 18 +++++++++---------
 src/main/include/Climber.h               |  6 +++---
 src/main/include/Mag.h                   |  6 +++---
 4 files changed, 24 insertions(+), 22 deletions(-)

diff --git a/src/main/cpp/Mag.cpp b/src/main/cpp/Mag.cpp
index b87259e8..b934c7c3 100644
--- a/src/main/cpp/Mag.cpp
+++ b/src/main/cpp/Mag.cpp
@@ -7,7 +7,9 @@ void Mag::OnUpdate(units::second_t dt) {
     case MagState::kIdle: 
     {
       if (_config.intakeSensor->Get()) {
-        setState(MagState::kHold);
+        SetState(MagState::kHold);
+      } else if (_config.intakeSensor->Get()){
+        SetState(MagState::kHold);
       }
       _config.magGearbox.transmission->SetVoltage(0_V);
     }
@@ -16,7 +18,7 @@ void Mag::OnUpdate(units::second_t dt) {
     case MagState::kHold:
     {
       if (_config.magSensor->Get() == 0) {
-        setState(MagState::kIdle);
+        SetState(MagState::kIdle);
       }
       _config.magGearbox.transmission->SetVoltage(0_V);
     }
@@ -25,7 +27,7 @@ void Mag::OnUpdate(units::second_t dt) {
     case MagState::kEject:
     {
       if (_config.magSensor->Get() == 0 && _config.intakeSensor->Get() ==  0) {
-        setState(MagState::kIdle);
+        SetState(MagState::kIdle);
       }
       _config.magGearbox.transmission->SetVoltage(-5_V);
     }
@@ -38,7 +40,7 @@ void Mag::OnUpdate(units::second_t dt) {
     case MagState::kPass:
     {
       if (_config.shooterSensor->Get()) {
-        setState(MagState::kIdle);
+        SetState(MagState::kIdle);
       } else {
         _config.magGearbox.transmission->SetVoltage(5_V);
       }
@@ -52,14 +54,14 @@ void Mag::OnUpdate(units::second_t dt) {
 }
 
 
-void Mag::setState(MagState state) {
+void Mag::SetState(MagState state) {
   _state = state;
 }
 
-void Mag::setRaw(units::volt_t voltage) {
+void Mag::SetRaw(units::volt_t voltage) {
   _voltage = voltage;
 }
-MagState Mag::getState() {
+MagState Mag::GetState() {
   return _state;
 }
 
diff --git a/src/main/cpp/behaviours/MagBehaviour.cpp b/src/main/cpp/behaviours/MagBehaviour.cpp
index fe451107..af37920a 100644
--- a/src/main/cpp/behaviours/MagBehaviour.cpp
+++ b/src/main/cpp/behaviours/MagBehaviour.cpp
@@ -17,20 +17,20 @@ void MagManualControl::OnTick(units::second_t dt) {
   if (_rawControl) {
     // Manual control, right bumper for manual override.
     if (_codriver.GetLeftBumper()) {
-      _mag->setRaw(5_V);
-      _mag->setState(MagState::kRaw);
+      _mag->SetRaw(5_V);
+      _mag->SetState(MagState::kRaw);
     } else if (_codriver.GetRightBumper()) {
-      _mag->setRaw(-5_V);
-      _mag->setState(MagState::kRaw);
+      _mag->SetRaw(-5_V);
+      _mag->SetState(MagState::kRaw);
 
     } else {
-      _mag->setRaw(0_V);
+      _mag->SetRaw(0_V);
     }
     
   } else {
-    _mag->setState(MagState::kIdle);
+    _mag->SetState(MagState::kIdle);
     if (_codriver.GetLeftBumper()) {
-      _mag->setState(MagState::kPass);
+      _mag->SetState(MagState::kPass);
       
     }
   }
@@ -39,11 +39,11 @@ void MagManualControl::OnTick(units::second_t dt) {
     MagAutoPass::MagAutoPass(Mag *mag) {}
     
     void MagAutoPass::OnTick(units::second_t dt) {  
-      _mag->setState(MagState::kPass);
+      _mag->SetState(MagState::kPass);
     }
 
     MagAutoHold::MagAutoHold(Mag *mag) {}
     
     void MagAutoHold::OnTick(units::second_t dt) {  
-      _mag->setState(MagState::kHold);
+      _mag->SetState(MagState::kHold);
     }
diff --git a/src/main/include/Climber.h b/src/main/include/Climber.h
index 3760f65e..a8b2bae9 100644
--- a/src/main/include/Climber.h
+++ b/src/main/include/Climber.h
@@ -17,13 +17,13 @@ class Climber : public behaviour:HasBehaviour {
     Climber(ClimberConfig config);
 
     void OnUpdate(units::second_t dt);
-    void setState(ClimberState state);
+    void SetState(ClimberState state);
     // void setReceive(units::volt_t voltage)
     // void setHold(units::volt_t voltage)
     // void setPass(units::volt_t voltage)
     // void setLeave(units::volt_t voltage)
-    void setRaw(units::volt_t voltage);
-    ClimberState getState();
+    void SetRaw(units::volt_t voltage);
+    ClimberState GetState();
   private:
     ClimberConfig _config;
     ClimberState _state;
diff --git a/src/main/include/Mag.h b/src/main/include/Mag.h
index 9787c2b6..d25153b8 100644
--- a/src/main/include/Mag.h
+++ b/src/main/include/Mag.h
@@ -22,9 +22,9 @@ class Mag : public behaviour::HasBehaviour {
     Mag(MagConfig config);
 
     void OnUpdate(units::second_t dt);
-    void setState(MagState state);
-    void setRaw(units::volt_t voltage);
-    MagState getState();
+    void SetState(MagState state);
+    void SetRaw(units::volt_t voltage);
+    MagState GetState();
   private:
     MagConfig _config;
     MagState _state;

From 2a252e3d0483e84c5cf2433e038ba1f2601beee5 Mon Sep 17 00:00:00 2001
From: Kingsley Wong <kingsley.wong@student.education.wa.edu.au>
Date: Tue, 16 Jan 2024 15:09:15 +0800
Subject: [PATCH 012/207] Intake draft fixed errors 2

---
 src/main/cpp/Intake.cpp          | 25 +++++++++++++++++++++++--
 src/main/cpp/IntakeBehaviour.cpp | 10 +++++-----
 src/main/include/Intake.h        | 10 +++++++---
 src/main/include/RobotMap.h      | 10 +++++-----
 4 files changed, 40 insertions(+), 15 deletions(-)

diff --git a/src/main/cpp/Intake.cpp b/src/main/cpp/Intake.cpp
index b2ecf84c..94468a55 100644
--- a/src/main/cpp/Intake.cpp
+++ b/src/main/cpp/Intake.cpp
@@ -9,11 +9,32 @@ void Intake::OnUpdate(units::second_t dt) {
       case IntakeState::kIdle:
       {
         _config.IntakeMotor.transmission->SetVoltage(0_V);
+        if (_config.intakeSensor->Get()) {
+          setState(IntakeState::kHold);
+        }
       }
       break;
       case IntakeState::kRaw:
       {
-        _config.IntakeMotor.transmission->SetVoltage(_voltage);
+        _config.IntakeMotor.transmission->SetVoltage(_rawVoltage);
+      }
+      break;
+      case IntakeState::kEject:
+      {
+        _config.IntakeMotor.transmission->SetVoltage(-5_V);
+        if (_config.intakeSensor->Get() == 0 && _config.magSensor->Get() == 0) {
+          setState(IntakeState::kIdle);
+        }
+      }
+      break;
+      case IntakeState::kHold:
+      {
+        _config.IntakeMotor.transmission->SetVoltage(0_V);
+      }
+      break;
+      case IntakeState::kIntake:
+      {
+        _config.IntakeMotor.transmission->SetVoltage(5_V);
       }
       break;
       default:
@@ -27,5 +48,5 @@ void Intake::setState(IntakeState state) {
   _state = state;
 }
 void Intake::setRaw(units::volt_t voltage) {
-  _voltage = voltage;
+  voltage = voltage;
 }
\ No newline at end of file
diff --git a/src/main/cpp/IntakeBehaviour.cpp b/src/main/cpp/IntakeBehaviour.cpp
index 7eb0d565..bebe8e63 100644
--- a/src/main/cpp/IntakeBehaviour.cpp
+++ b/src/main/cpp/IntakeBehaviour.cpp
@@ -6,13 +6,13 @@ IntakeManualControl::IntakeManualControl(Intake *intake, frc::XboxController &co
 };
 
 void IntakeManualControl::OnTick(units::second_t dt) {
-  if (_codriver.GetBButtonPressed()) {
+  if (_codriver.GetBButton()) {
     _intake->setRaw(8_V);
-    _intake->setState(IntakeState::kRaw)
+    _intake->setState(IntakeState::kRaw);
   } else {
     _intake->setRaw(0_V);
-    _intake->setState(IntakeState::kIdle)
-  }
+    _intake->setState(IntakeState::kIdle);
+  };
 
   //_intake->setRaw(_button);
-}
\ No newline at end of file
+};
\ No newline at end of file
diff --git a/src/main/include/Intake.h b/src/main/include/Intake.h
index 27b095c2..2e22efef 100644
--- a/src/main/include/Intake.h
+++ b/src/main/include/Intake.h
@@ -7,17 +7,21 @@
 struct IntakeConfig {
   wom::Gearbox IntakeMotor;
   frc::DigitalInput* intakeSensor;
+  frc::DigitalInput* magSensor;
 };
 
 enum class IntakeState {
   kIdle,
-  kRaw
+  kRaw,
+  kHold,
+  kEject,
+  kIntake
 };
 
 class Intake : public behaviour::HasBehaviour {
  public:
   Intake(IntakeConfig config);
-
+  
   void OnUpdate(units::second_t dt);
   
   void setState(IntakeState state);
@@ -27,5 +31,5 @@ class Intake : public behaviour::HasBehaviour {
   IntakeConfig _config;
   IntakeState _state = IntakeState::kIdle;
 
-  units::volt_t _voltage;
+  units::volt_t _rawVoltage;
 }; 
\ No newline at end of file
diff --git a/src/main/include/RobotMap.h b/src/main/include/RobotMap.h
index 8a527402..3bde8655 100644
--- a/src/main/include/RobotMap.h
+++ b/src/main/include/RobotMap.h
@@ -10,13 +10,11 @@ struct RobotMap {
 
   struct IntakeSystem {
 
-    //rev::CANSparkMax IntakeController{99,rev::CANSparkMax::MotorType::kBrushless};
     wom::VoltageController IntakeMotor{new rev::CANSparkMax(99, rev::CANSparkMax::MotorType::kBrushless) };
-    frc::DigitalInput intakeSensor(0);
+    frc::DigitalInput intakeSensor {0};
+    frc::DigitalInput magSensor {0};
 
-    //wom::VoltageController IntakeMotor{ IntakeController NEO(99)};
-
-    wom::Gearbox IntakeGearbox {
+    wom::Gearbox IntakeGearbox { 
       &IntakeMotor,
       nullptr,
       wom::DCMotor::NEO(1).WithReduction(1)
@@ -24,6 +22,8 @@ struct RobotMap {
 
     IntakeConfig config {
       IntakeGearbox,
+      &intakeSensor,
+      &magSensor
     };
   }; IntakeSystem intakeSystem;
 

From 9f6a060948284b0355d57751ac064e89265d543b Mon Sep 17 00:00:00 2001
From: Sean Chan <sean.chan3@student.education.wa.edu.au>
Date: Tue, 16 Jan 2024 16:35:23 +0800
Subject: [PATCH 013/207] removed climber stuff

---
 src/main/cpp/Climber.cpp                      |  0
 src/main/cpp/behaviours/ClimberBehaviour.cpp  |  0
 src/main/include/Climber.h                    | 32 -------------------
 src/main/include/RobotMap.h                   |  1 +
 .../include/behaviours/ClimberBehaviour.h     |  0
 5 files changed, 1 insertion(+), 32 deletions(-)
 delete mode 100644 src/main/cpp/Climber.cpp
 delete mode 100644 src/main/cpp/behaviours/ClimberBehaviour.cpp
 delete mode 100644 src/main/include/Climber.h
 delete mode 100644 src/main/include/behaviours/ClimberBehaviour.h

diff --git a/src/main/cpp/Climber.cpp b/src/main/cpp/Climber.cpp
deleted file mode 100644
index e69de29b..00000000
diff --git a/src/main/cpp/behaviours/ClimberBehaviour.cpp b/src/main/cpp/behaviours/ClimberBehaviour.cpp
deleted file mode 100644
index e69de29b..00000000
diff --git a/src/main/include/Climber.h b/src/main/include/Climber.h
deleted file mode 100644
index a8b2bae9..00000000
--- a/src/main/include/Climber.h
+++ /dev/null
@@ -1,32 +0,0 @@
-#pragma once
-
-#include "wombat.h"
-
-struct ClimberConfig {
-  wom::Gearbox climberGearBox;
-};
-
-enum class ClimberState {
-  kIdle,
-  kLift,
-  kHang
-};
-
-class Climber : public behaviour:HasBehaviour {
-  public:
-    Climber(ClimberConfig config);
-
-    void OnUpdate(units::second_t dt);
-    void SetState(ClimberState state);
-    // void setReceive(units::volt_t voltage)
-    // void setHold(units::volt_t voltage)
-    // void setPass(units::volt_t voltage)
-    // void setLeave(units::volt_t voltage)
-    void SetRaw(units::volt_t voltage);
-    ClimberState GetState();
-  private:
-    ClimberConfig _config;
-    ClimberState _state;
-    units::volt_t _voltage;
-
-}
\ No newline at end of file
diff --git a/src/main/include/RobotMap.h b/src/main/include/RobotMap.h
index 07e9f022..6172ab77 100644
--- a/src/main/include/RobotMap.h
+++ b/src/main/include/RobotMap.h
@@ -31,4 +31,5 @@ struct RobotMap {
      };
   }; 
   Mag magSystem;
+
 };
\ No newline at end of file
diff --git a/src/main/include/behaviours/ClimberBehaviour.h b/src/main/include/behaviours/ClimberBehaviour.h
deleted file mode 100644
index e69de29b..00000000

From d816e8235f99b0bc60cf42ffcd72363742f94ead Mon Sep 17 00:00:00 2001
From: Kingsley Wong <kingsley.wong@student.education.wa.edu.au>
Date: Tue, 16 Jan 2024 19:43:16 +0800
Subject: [PATCH 014/207] Intake draft fixed errors 3

---
 src/main/cpp/Intake.cpp            |  7 ++++++-
 src/main/cpp/IntakeBehaviour.cpp   | 29 ++++++++++++++++++++---------
 src/main/include/Intake.h          |  3 ++-
 src/main/include/IntakeBehaviour.h |  1 +
 src/main/include/RobotMap.h        | 11 +++++------
 5 files changed, 34 insertions(+), 17 deletions(-)

diff --git a/src/main/cpp/Intake.cpp b/src/main/cpp/Intake.cpp
index 94468a55..a36922e8 100644
--- a/src/main/cpp/Intake.cpp
+++ b/src/main/cpp/Intake.cpp
@@ -37,6 +37,11 @@ void Intake::OnUpdate(units::second_t dt) {
         _config.IntakeMotor.transmission->SetVoltage(5_V);
       }
       break;
+      case IntakeState::kPass:
+      {
+        _config.IntakeMotor.transmission->SetVoltage(5_V);
+      }
+      break;
       default:
         std::cout <<"Error: Intake in INVALID STATE." << std::endl;
       break;
@@ -48,5 +53,5 @@ void Intake::setState(IntakeState state) {
   _state = state;
 }
 void Intake::setRaw(units::volt_t voltage) {
-  voltage = voltage;
+  _rawVoltage = voltage;
 }
\ No newline at end of file
diff --git a/src/main/cpp/IntakeBehaviour.cpp b/src/main/cpp/IntakeBehaviour.cpp
index bebe8e63..30405deb 100644
--- a/src/main/cpp/IntakeBehaviour.cpp
+++ b/src/main/cpp/IntakeBehaviour.cpp
@@ -6,13 +6,24 @@ IntakeManualControl::IntakeManualControl(Intake *intake, frc::XboxController &co
 };
 
 void IntakeManualControl::OnTick(units::second_t dt) {
-  if (_codriver.GetBButton()) {
-    _intake->setRaw(8_V);
-    _intake->setState(IntakeState::kRaw);
-  } else {
-    _intake->setRaw(0_V);
-    _intake->setState(IntakeState::kIdle);
-  };
 
-  //_intake->setRaw(_button);
-};
\ No newline at end of file
+  if (_codriver.GetBButtonPressed()) {
+    if(_rawControl == true) {
+      _rawControl = false;
+    } else {
+      _rawControl = true;
+    }
+  }
+
+  if (_rawControl == true) {
+      if (_codriver.GetBButton()) {
+        _intake->setRaw(8_V);
+        _intake->setState(IntakeState::kRaw);
+      } else {
+        _intake->setRaw(0_V);
+        _intake->setState(IntakeState::kIdle);
+      };
+  } else {
+    // intake auto control
+  }
+}
diff --git a/src/main/include/Intake.h b/src/main/include/Intake.h
index 2e22efef..c28688d4 100644
--- a/src/main/include/Intake.h
+++ b/src/main/include/Intake.h
@@ -15,7 +15,8 @@ enum class IntakeState {
   kRaw,
   kHold,
   kEject,
-  kIntake
+  kIntake,
+  kPass
 };
 
 class Intake : public behaviour::HasBehaviour {
diff --git a/src/main/include/IntakeBehaviour.h b/src/main/include/IntakeBehaviour.h
index a760cd21..78b49af6 100644
--- a/src/main/include/IntakeBehaviour.h
+++ b/src/main/include/IntakeBehaviour.h
@@ -14,4 +14,5 @@ class IntakeManualControl : public behaviour::Behaviour {
   frc::XboxController &_codriver;
 
   units::volt_t _voltage;
+  bool _rawControl;
 }; 
\ No newline at end of file
diff --git a/src/main/include/RobotMap.h b/src/main/include/RobotMap.h
index 3bde8655..4b57164f 100644
--- a/src/main/include/RobotMap.h
+++ b/src/main/include/RobotMap.h
@@ -9,14 +9,15 @@ struct RobotMap {
   frc::XboxController driver{0};
 
   struct IntakeSystem {
-
-    wom::VoltageController IntakeMotor{new rev::CANSparkMax(99, rev::CANSparkMax::MotorType::kBrushless) };
+    rev::CANSparkMax intakeMotor{99, rev::CANSparkMax::MotorType::kBrushless};
+    wom::VoltageController intakeMotorGroup = wom::VoltageController::Group(intakeMotor);
+    wom::CANSparkMaxEncoder intakeEncoder{&intakeMotor, 42};
     frc::DigitalInput intakeSensor {0};
     frc::DigitalInput magSensor {0};
 
     wom::Gearbox IntakeGearbox { 
-      &IntakeMotor,
-      nullptr,
+      &intakeMotorGroup,
+      &intakeEncoder,
       wom::DCMotor::NEO(1).WithReduction(1)
     };
 
@@ -27,6 +28,4 @@ struct RobotMap {
     };
   }; IntakeSystem intakeSystem;
 
-  //port to be filled later.
-
 };
\ No newline at end of file

From 645cedb3a415f76c3599f6a09d7d0d439ba631d7 Mon Sep 17 00:00:00 2001
From: Sean Chan <sean.chan3@student.education.wa.edu.au>
Date: Wed, 17 Jan 2024 15:48:54 +0800
Subject: [PATCH 015/207] Made a start to climber code

---
 src/main/cpp/Behaviours/ClimberBehaviour.cpp  | 12 +++++++++
 src/main/cpp/Climber.cpp                      | 27 +++++++++++++++++++
 src/main/cpp/Main.cpp                         |  2 +-
 src/main/cpp/Robot.cpp                        | 15 ++++++++++-
 .../include/Behaviours/ClimberBehaviour.h     | 15 +++++++++++
 src/main/include/Climber.h                    | 27 +++++++++++++++++++
 src/main/include/Robot.h                      |  5 +++-
 src/main/include/RobotMap.h                   | 22 ++++++++++++++-
 8 files changed, 121 insertions(+), 4 deletions(-)
 create mode 100644 src/main/cpp/Behaviours/ClimberBehaviour.cpp
 create mode 100644 src/main/cpp/Climber.cpp
 create mode 100644 src/main/include/Behaviours/ClimberBehaviour.h
 create mode 100644 src/main/include/Climber.h

diff --git a/src/main/cpp/Behaviours/ClimberBehaviour.cpp b/src/main/cpp/Behaviours/ClimberBehaviour.cpp
new file mode 100644
index 00000000..b7627240
--- /dev/null
+++ b/src/main/cpp/Behaviours/ClimberBehaviour.cpp
@@ -0,0 +1,12 @@
+#include "behaviours/ClimberBehaviour.h"
+
+ClimberManualControl::ClimberManualControl(Climber *climber, frc::XboxController *codriver) : _climber(climber), _codriver(codriver) {
+  Controls(climber); 
+}
+
+void ClimberManualControl::OnTick(units::second_t dt) {
+
+  if (_codriver->GetXButtonPressed()) {
+    //start climbing, use arrow later for doing GetButton.
+  }
+}
\ No newline at end of file
diff --git a/src/main/cpp/Climber.cpp b/src/main/cpp/Climber.cpp
new file mode 100644
index 00000000..828a2604
--- /dev/null
+++ b/src/main/cpp/Climber.cpp
@@ -0,0 +1,27 @@
+#include "Climber.h"
+
+Climber::Climber(ClimberConfig config) : _config(config) {}
+
+void Climber::OnUpdate(units::second_t dt) {
+  switch (_state) {
+    case ClimberState::kIdle: 
+    break;
+    case ClimberState::kClimb:
+    break;
+    case ClimberState::kHang:
+    break;
+    case ClimberState::kRaw:
+    break;
+    default:
+       std::cout << "Error magazine in invalid state" << std::endl;
+    break;
+  }
+}
+
+void Climber::SetState(ClimberState state) {
+  _state = state;
+}
+
+void Climber::SetRaw(units::volt_t voltage) {
+  _voltage = voltage;
+}
diff --git a/src/main/cpp/Main.cpp b/src/main/cpp/Main.cpp
index e27c9c50..3bd80491 100644
--- a/src/main/cpp/Main.cpp
+++ b/src/main/cpp/Main.cpp
@@ -10,4 +10,4 @@
 #include "Robot.h"
 #include "Wombat.h"
 
-WOMBAT_ROBOT_MAIN(Robot);
+WOMBAT_ROBOT_MAIN(Robot);
\ No newline at end of file
diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp
index 1e8fdf88..ea84a17c 100644
--- a/src/main/cpp/Robot.cpp
+++ b/src/main/cpp/Robot.cpp
@@ -44,18 +44,27 @@ void Robot::RobotInit() {
                                            &robotmap.controllers.driver);
   });
 
+  lastPeriodic = wom::now();
+
+  _climber = new Climber(robotmap.climberSystem.config);
+  wom::BehaviourScheduler::GetInstance()->Register(_climber);
+  _climber->SetDefaultBehaviour([this]() {
+    return wom::make<ClimberManualControl>(_climber, &robotmap.controllers.coDriver);
+  });
   // m_driveSim = new wom::TempSimSwerveDrive(&simulation_timer, &m_field);
   // m_driveSim = wom::TempSimSwerveDrive();
 }
 
 void Robot::RobotPeriodic() {
-  auto dt = wom::now() - lastPeriodic;
+  units::second_t dt = wom::now() - lastPeriodic;
   lastPeriodic = wom::now();
 
   loop.Poll();
   wom::BehaviourScheduler::GetInstance()->Tick();
 
   _swerveDrive->OnUpdate(dt);
+
+  _climber->OnUpdate(dt);
 }
 
 void Robot::AutonomousInit() {
@@ -70,9 +79,13 @@ void Robot::AutonomousPeriodic() {
 }
 
 void Robot::TeleopInit() {
+  loop.Clear();
+  wom::BehaviourScheduler *scheduler = wom::BehaviourScheduler::GetInstance();
+  scheduler->InterruptAll();
   // _swerveDrive->OnStart();
   // sched->InterruptAll();
 }
+
 void Robot::TeleopPeriodic() {}
 
 void Robot::DisabledInit() {}
diff --git a/src/main/include/Behaviours/ClimberBehaviour.h b/src/main/include/Behaviours/ClimberBehaviour.h
new file mode 100644
index 00000000..771f1142
--- /dev/null
+++ b/src/main/include/Behaviours/ClimberBehaviour.h
@@ -0,0 +1,15 @@
+#pragma once
+
+#include "wombat.h"
+#include "climber.h"
+#include <frc/XboxController.h>
+
+class ClimberManualControl : public behaviour::Behaviour {
+  public:
+    ClimberManualControl(Climber *climber, frc::XboxController *codriver);
+    void OnTick(units::second_t dt) override;
+  private:
+    Climber *_climber;
+    frc::XboxController *_codriver;
+};
+
diff --git a/src/main/include/Climber.h b/src/main/include/Climber.h
new file mode 100644
index 00000000..71fd74a6
--- /dev/null
+++ b/src/main/include/Climber.h
@@ -0,0 +1,27 @@
+#pragma once
+
+#include "wombat.h"
+
+struct ClimberConfig {
+  wom::Gearbox climberGearbox;
+};
+
+enum class ClimberState {
+  kIdle,
+  kClimb,
+  kHang,
+  kRaw
+};
+
+class Climber : public behaviour::HasBehaviour {
+  public:
+    Climber(ClimberConfig config);
+
+    void OnUpdate(units::second_t dt);
+    void SetState(ClimberState state);
+    void SetRaw(units::volt_t voltage);
+  private:
+    ClimberConfig _config;
+    ClimberState _state;
+    units::volt_t _voltage;
+};
\ No newline at end of file
diff --git a/src/main/include/Robot.h b/src/main/include/Robot.h
index bc98df2a..e2e0d817 100644
--- a/src/main/include/Robot.h
+++ b/src/main/include/Robot.h
@@ -16,6 +16,8 @@
 
 #include <string>
 
+#include "Behaviours/ClimberBehaviour.h"
+
 #include "RobotMap.h"
 #include "Wombat.h"
 
@@ -48,4 +50,5 @@ class Robot : public frc::TimedRobot {
   frc::SendableChooser<std::string> m_path_chooser;
 
   wom::SwerveDrive* _swerveDrive;
-};
+  Climber* _climber;
+};
\ No newline at end of file
diff --git a/src/main/include/RobotMap.h b/src/main/include/RobotMap.h
index cdf67aff..997ebca1 100644
--- a/src/main/include/RobotMap.h
+++ b/src/main/include/RobotMap.h
@@ -18,6 +18,7 @@
 #include <ctre/phoenix6/TalonFX.hpp>
 
 #include "Wombat.h"
+#include "Climber.h"
 
 struct RobotMap {
   struct Controllers {
@@ -142,4 +143,23 @@ struct RobotMap {
     //}
   };
   SwerveBase swerveBase;
-};
+
+  struct Climber {
+    // rev::CANSparkMax magMotor{99, rev::CANSparkMax::MotorType::kBrushless};
+    // wom::VoltageController magMotorGroup = wom::VoltageController::Group(magMotor);
+    // wom::CANSparkMaxEncoder magEncoder{&magMotor, 42};
+  
+    wom::Gearbox climberGearbox {
+        // &climberMotorGroup,
+        // &climberEncoder,
+        // wom::DCMotor::NEO(1).WithReduction(1)
+
+    };
+
+    ClimberConfig config {
+      climberGearbox,
+    };
+
+  };
+  Climber climberSystem;
+};
\ No newline at end of file

From 0a4d6dc633ac9f652dc59129a9f9a03c0d1514c3 Mon Sep 17 00:00:00 2001
From: Kingsley Wong <kingsley.wong@student.education.wa.edu.au>
Date: Wed, 17 Jan 2024 15:50:36 +0800
Subject: [PATCH 016/207] Intake draft fixed errors 4

---
 src/main/cpp/Intake.cpp            | 12 ++++++++++--
 src/main/cpp/IntakeBehaviour.cpp   | 30 ++++++++++++++++++++----------
 src/main/include/Intake.h          |  2 ++
 src/main/include/IntakeBehaviour.h | 11 ++++++++++-
 src/main/include/RobotMap.h        |  4 +++-
 5 files changed, 45 insertions(+), 14 deletions(-)

diff --git a/src/main/cpp/Intake.cpp b/src/main/cpp/Intake.cpp
index a36922e8..4233c4b3 100644
--- a/src/main/cpp/Intake.cpp
+++ b/src/main/cpp/Intake.cpp
@@ -1,6 +1,11 @@
 #include "Intake.h"
 
 Intake::Intake(IntakeConfig config) : _config(config) {
+
+}
+
+IntakeConfig Intake::getConfig() {
+  return _config;
 }
 
 void Intake::OnUpdate(units::second_t dt) {
@@ -35,18 +40,21 @@ void Intake::OnUpdate(units::second_t dt) {
       case IntakeState::kIntake:
       {
         _config.IntakeMotor.transmission->SetVoltage(5_V);
-      }
+      } 
       break;
       case IntakeState::kPass:
       {
         _config.IntakeMotor.transmission->SetVoltage(5_V);
+        if (_config.shooterSensor->Get()) {
+          setState(IntakeState::kIdle);
       }
       break;
       default:
         std::cout <<"Error: Intake in INVALID STATE." << std::endl;
       break;
+    }
+    
   }
-  
 }
 
 void Intake::setState(IntakeState state) {
diff --git a/src/main/cpp/IntakeBehaviour.cpp b/src/main/cpp/IntakeBehaviour.cpp
index 30405deb..0bbad085 100644
--- a/src/main/cpp/IntakeBehaviour.cpp
+++ b/src/main/cpp/IntakeBehaviour.cpp
@@ -16,14 +16,24 @@ void IntakeManualControl::OnTick(units::second_t dt) {
   }
 
   if (_rawControl == true) {
-      if (_codriver.GetBButton()) {
-        _intake->setRaw(8_V);
-        _intake->setState(IntakeState::kRaw);
-      } else {
-        _intake->setRaw(0_V);
-        _intake->setState(IntakeState::kIdle);
-      };
-  } else {
-    // intake auto control
-  }
+    if (_codriver.GetBButton()) {
+      _intake->setRaw(8_V);
+      _intake->setState(IntakeState::kRaw);
+    } else {
+      _intake->setRaw(0_V);
+      _intake->setState(IntakeState::kIdle);
+    };
+  }; //else {
+    
+  // }
 }
+
+IntakeAutoControl::IntakeAutoControl(Intake *intake) : _intake(intake) {};
+
+void IntakeAutoControl::OnTick(units::second_t dt) {
+  if (_intake->getConfig().intakeSensor->Get() == 1) {
+    _intake->setState(IntakeState::kPass);
+  } else if (_intake->getConfig().magSensor->Get() == 0) {
+    _intake->setState(IntakeState::kIdle);
+  };
+}
\ No newline at end of file
diff --git a/src/main/include/Intake.h b/src/main/include/Intake.h
index c28688d4..72fcad2f 100644
--- a/src/main/include/Intake.h
+++ b/src/main/include/Intake.h
@@ -8,6 +8,7 @@ struct IntakeConfig {
   wom::Gearbox IntakeMotor;
   frc::DigitalInput* intakeSensor;
   frc::DigitalInput* magSensor;
+  frc::DigitalInput* shooterSensor;
 };
 
 enum class IntakeState {
@@ -27,6 +28,7 @@ class Intake : public behaviour::HasBehaviour {
   
   void setState(IntakeState state);
   void setRaw(units::volt_t voltage);
+  IntakeConfig getConfig();
 
  private:
   IntakeConfig _config;
diff --git a/src/main/include/IntakeBehaviour.h b/src/main/include/IntakeBehaviour.h
index 78b49af6..3f2e9ad3 100644
--- a/src/main/include/IntakeBehaviour.h
+++ b/src/main/include/IntakeBehaviour.h
@@ -13,6 +13,15 @@ class IntakeManualControl : public behaviour::Behaviour {
   Intake *_intake;
   frc::XboxController &_codriver;
 
-  units::volt_t _voltage;
+  units::volt_t _rawVoltage;
   bool _rawControl;
+}; 
+
+class IntakeAutoControl : public behaviour::Behaviour {
+ public:
+  IntakeAutoControl(Intake *intake);
+
+  void OnTick(units::second_t dt) override;
+ private:
+  Intake *_intake;
 }; 
\ No newline at end of file
diff --git a/src/main/include/RobotMap.h b/src/main/include/RobotMap.h
index 4b57164f..e52eb133 100644
--- a/src/main/include/RobotMap.h
+++ b/src/main/include/RobotMap.h
@@ -14,6 +14,7 @@ struct RobotMap {
     wom::CANSparkMaxEncoder intakeEncoder{&intakeMotor, 42};
     frc::DigitalInput intakeSensor {0};
     frc::DigitalInput magSensor {0};
+    frc::DigitalInput shooterSensor {0};
 
     wom::Gearbox IntakeGearbox { 
       &intakeMotorGroup,
@@ -24,7 +25,8 @@ struct RobotMap {
     IntakeConfig config {
       IntakeGearbox,
       &intakeSensor,
-      &magSensor
+      &magSensor,
+      &shooterSensor
     };
   }; IntakeSystem intakeSystem;
 

From bc2904473115b4ef203f12771aac51f8286b3255 Mon Sep 17 00:00:00 2001
From: Kingsley Wong <kingsley.wong@student.education.wa.edu.au>
Date: Wed, 17 Jan 2024 16:27:10 +0800
Subject: [PATCH 017/207] Intake draft fixed errors 5

---
 src/main/cpp/Intake.cpp          |  2 +-
 src/main/cpp/IntakeBehaviour.cpp | 29 +++++++++++++++--------------
 src/main/include/Intake.h        |  2 +-
 3 files changed, 17 insertions(+), 16 deletions(-)

diff --git a/src/main/cpp/Intake.cpp b/src/main/cpp/Intake.cpp
index 4233c4b3..1aeb8aa4 100644
--- a/src/main/cpp/Intake.cpp
+++ b/src/main/cpp/Intake.cpp
@@ -4,7 +4,7 @@ Intake::Intake(IntakeConfig config) : _config(config) {
 
 }
 
-IntakeConfig Intake::getConfig() {
+IntakeConfig Intake::GetConfig() {
   return _config;
 }
 
diff --git a/src/main/cpp/IntakeBehaviour.cpp b/src/main/cpp/IntakeBehaviour.cpp
index 0bbad085..17d18484 100644
--- a/src/main/cpp/IntakeBehaviour.cpp
+++ b/src/main/cpp/IntakeBehaviour.cpp
@@ -15,25 +15,26 @@ void IntakeManualControl::OnTick(units::second_t dt) {
     }
   }
 
-  if (_rawControl == true) {
-    if (_codriver.GetBButton()) {
-      _intake->setRaw(8_V);
-      _intake->setState(IntakeState::kRaw);
-    } else {
-      _intake->setRaw(0_V);
-      _intake->setState(IntakeState::kIdle);
-    };
-  }; //else {
-    
-  // }
-}
+  if (_rawControl) {
+    _intake->setState(IntakeState::kRaw);
+    _intake->setRaw(_codriver.GetLeftY() * 5_V);
+  } else {
+    if (_codriver.GetYButtonPressed()) {
+      _intake->setState(IntakeState::kIntake);
+    }
+    if (_codriver.GetAButtonPressed()) {
+      _intake->setState(IntakeState::kPass);
+    }
+  };
+}; //else {}
+
 
 IntakeAutoControl::IntakeAutoControl(Intake *intake) : _intake(intake) {};
 
 void IntakeAutoControl::OnTick(units::second_t dt) {
-  if (_intake->getConfig().intakeSensor->Get() == 1) {
+  if (_intake->GetConfig().intakeSensor->Get() == 1) {
     _intake->setState(IntakeState::kPass);
-  } else if (_intake->getConfig().magSensor->Get() == 0) {
+  } else if (_intake->GetConfig().magSensor->Get() == 0) {
     _intake->setState(IntakeState::kIdle);
   };
 }
\ No newline at end of file
diff --git a/src/main/include/Intake.h b/src/main/include/Intake.h
index 72fcad2f..f51d7487 100644
--- a/src/main/include/Intake.h
+++ b/src/main/include/Intake.h
@@ -28,7 +28,7 @@ class Intake : public behaviour::HasBehaviour {
   
   void setState(IntakeState state);
   void setRaw(units::volt_t voltage);
-  IntakeConfig getConfig();
+  IntakeConfig GetConfig();
 
  private:
   IntakeConfig _config;

From 3595fb1c7cdc1cf8bf32bee8b52798727fde6da5 Mon Sep 17 00:00:00 2001
From: Sean Chan <sean.chan3@student.education.wa.edu.au>
Date: Wed, 17 Jan 2024 19:48:08 +0800
Subject: [PATCH 018/207] Added logic and motors

---
 src/main/cpp/Behaviours/ClimberBehaviour.cpp |  9 ++++++++-
 src/main/cpp/Climber.cpp                     | 14 ++++++++------
 src/main/cpp/Robot.cpp                       | 12 ++++++------
 src/main/include/Climber.h                   |  6 ++----
 src/main/include/Robot.h                     |  2 +-
 src/main/include/RobotMap.h                  | 14 +++++++-------
 6 files changed, 32 insertions(+), 25 deletions(-)

diff --git a/src/main/cpp/Behaviours/ClimberBehaviour.cpp b/src/main/cpp/Behaviours/ClimberBehaviour.cpp
index b7627240..f89bc6ed 100644
--- a/src/main/cpp/Behaviours/ClimberBehaviour.cpp
+++ b/src/main/cpp/Behaviours/ClimberBehaviour.cpp
@@ -7,6 +7,13 @@ ClimberManualControl::ClimberManualControl(Climber *climber, frc::XboxController
 void ClimberManualControl::OnTick(units::second_t dt) {
 
   if (_codriver->GetXButtonPressed()) {
-    //start climbing, use arrow later for doing GetButton.
+    _climber->SetRaw(8_V);
+    _climber->SetState(ClimberState::kRaw);
+  } else if (_codriver->GetYButtonPressed()){
+    _climber->SetRaw(-8_V);
+    _climber->SetState(ClimberState::kRaw);
+  } else {
+    _climber->SetRaw(8_V);
+    _climber->SetState(ClimberState::kIdle);
   }
 }
\ No newline at end of file
diff --git a/src/main/cpp/Climber.cpp b/src/main/cpp/Climber.cpp
index 828a2604..d13df990 100644
--- a/src/main/cpp/Climber.cpp
+++ b/src/main/cpp/Climber.cpp
@@ -4,13 +4,15 @@ Climber::Climber(ClimberConfig config) : _config(config) {}
 
 void Climber::OnUpdate(units::second_t dt) {
   switch (_state) {
-    case ClimberState::kIdle: 
-    break;
-    case ClimberState::kClimb:
-    break;
-    case ClimberState::kHang:
+    case ClimberState::kIdle:
+    {
+      _config.climberGearbox.motorController->Set(0);
+    }
     break;
     case ClimberState::kRaw:
+    {
+      _config.climberGearbox.motorController->Set(_rawVoltage.value());
+    }
     break;
     default:
        std::cout << "Error magazine in invalid state" << std::endl;
@@ -23,5 +25,5 @@ void Climber::SetState(ClimberState state) {
 }
 
 void Climber::SetRaw(units::volt_t voltage) {
-  _voltage = voltage;
+  _rawVoltage = voltage;
 }
diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp
index ea84a17c..1e3ff764 100644
--- a/src/main/cpp/Robot.cpp
+++ b/src/main/cpp/Robot.cpp
@@ -46,11 +46,11 @@ void Robot::RobotInit() {
 
   lastPeriodic = wom::now();
 
-  _climber = new Climber(robotmap.climberSystem.config);
-  wom::BehaviourScheduler::GetInstance()->Register(_climber);
-  _climber->SetDefaultBehaviour([this]() {
-    return wom::make<ClimberManualControl>(_climber, &robotmap.controllers.coDriver);
-  });
+  // climber = new Climber(robotmap.climberSystem.config);
+  // wom::BehaviourScheduler::GetInstance()->Register(climber);
+  // climber->SetDefaultBehaviour([this]() {
+  //   return wom::make<ClimberManualControl>(climber, &robotmap.controllers.coDriver);
+  // });
   // m_driveSim = new wom::TempSimSwerveDrive(&simulation_timer, &m_field);
   // m_driveSim = wom::TempSimSwerveDrive();
 }
@@ -64,7 +64,7 @@ void Robot::RobotPeriodic() {
 
   _swerveDrive->OnUpdate(dt);
 
-  _climber->OnUpdate(dt);
+  // climber->OnUpdate(dt);
 }
 
 void Robot::AutonomousInit() {
diff --git a/src/main/include/Climber.h b/src/main/include/Climber.h
index 71fd74a6..ef6d4cef 100644
--- a/src/main/include/Climber.h
+++ b/src/main/include/Climber.h
@@ -8,8 +8,6 @@ struct ClimberConfig {
 
 enum class ClimberState {
   kIdle,
-  kClimb,
-  kHang,
   kRaw
 };
 
@@ -22,6 +20,6 @@ class Climber : public behaviour::HasBehaviour {
     void SetRaw(units::volt_t voltage);
   private:
     ClimberConfig _config;
-    ClimberState _state;
-    units::volt_t _voltage;
+    ClimberState _state = ClimberState::kIdle;
+    units::volt_t _rawVoltage;
 };
\ No newline at end of file
diff --git a/src/main/include/Robot.h b/src/main/include/Robot.h
index e2e0d817..b4aed9de 100644
--- a/src/main/include/Robot.h
+++ b/src/main/include/Robot.h
@@ -50,5 +50,5 @@ class Robot : public frc::TimedRobot {
   frc::SendableChooser<std::string> m_path_chooser;
 
   wom::SwerveDrive* _swerveDrive;
-  Climber* _climber;
+  Climber* climber;
 };
\ No newline at end of file
diff --git a/src/main/include/RobotMap.h b/src/main/include/RobotMap.h
index 997ebca1..92f3af1a 100644
--- a/src/main/include/RobotMap.h
+++ b/src/main/include/RobotMap.h
@@ -10,6 +10,8 @@
 #include <frc/system/plant/DCMotor.h>
 #include <units/angle.h>
 #include <units/length.h>
+#include <frc/motorcontrol/MotorControllerGroup.h>
+#include <frc/system/plant/DCMotor.h>
 
 #include <string>
 
@@ -145,15 +147,13 @@ struct RobotMap {
   SwerveBase swerveBase;
 
   struct Climber {
-    // rev::CANSparkMax magMotor{99, rev::CANSparkMax::MotorType::kBrushless};
-    // wom::VoltageController magMotorGroup = wom::VoltageController::Group(magMotor);
-    // wom::CANSparkMaxEncoder magEncoder{&magMotor, 42};
+    rev::CANSparkMax *climberMotor = new rev::CANSparkMax{99, rev::CANSparkMax::MotorType::kBrushless};
+    wom::CANSparkMaxEncoder climberEncoder{climberMotor, 0.1_m};
   
     wom::Gearbox climberGearbox {
-        // &climberMotorGroup,
-        // &climberEncoder,
-        // wom::DCMotor::NEO(1).WithReduction(1)
-
+        climberMotor,
+        &climberEncoder,
+        frc::DCMotor::NEO(1)
     };
 
     ClimberConfig config {

From 0d909f190452afebce4937790075416b5ea628bd Mon Sep 17 00:00:00 2001
From: Sean Chan <sean.chan3@student.education.wa.edu.au>
Date: Thu, 18 Jan 2024 12:47:04 +0800
Subject: [PATCH 019/207] Network Tables and states added

---
 gradlew.bat                                   |  4 +-
 src/main/cpp/Behaviours/ClimberBehaviour.cpp  | 37 +++++++++++--------
 src/main/cpp/Climber.cpp                      | 26 ++++++++++++-
 src/main/cpp/Robot.cpp                        | 14 ++++---
 .../include/Behaviours/ClimberBehaviour.h     |  1 +
 src/main/include/Climber.h                    |  7 +++-
 src/main/include/Robot.h                      | 10 +++++
 7 files changed, 73 insertions(+), 26 deletions(-)

diff --git a/gradlew.bat b/gradlew.bat
index 6689b85b..6471a59a 100644
--- a/gradlew.bat
+++ b/gradlew.bat
@@ -7,7 +7,7 @@
 @rem
 @rem      https://www.apache.org/licenses/LICENSE-2.0
 @rem
-@rem Unless required by applicable law or agreed to in writing, software
+@rem Unless required by applicable law or agreed to in writing, software``
 @rem distributed under the License is distributed on an "AS IS" BASIS,
 @rem WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 @rem See the License for the specific language governing permissions and
@@ -22,7 +22,7 @@
 @rem ##########################################################################
 
 @rem Set local scope for the variables with windows NT shell
-if "%OS%"=="Windows_NT" setlocal
+if "%OS%"=="Windows_NT" setlocal    
 
 set DIRNAME=%~dp0
 if "%DIRNAME%"=="" set DIRNAME=.
diff --git a/src/main/cpp/Behaviours/ClimberBehaviour.cpp b/src/main/cpp/Behaviours/ClimberBehaviour.cpp
index f89bc6ed..796f1ebf 100644
--- a/src/main/cpp/Behaviours/ClimberBehaviour.cpp
+++ b/src/main/cpp/Behaviours/ClimberBehaviour.cpp
@@ -1,19 +1,24 @@
-#include "behaviours/ClimberBehaviour.h"
+  #include "behaviours/ClimberBehaviour.h"
 
-ClimberManualControl::ClimberManualControl(Climber *climber, frc::XboxController *codriver) : _climber(climber), _codriver(codriver) {
-  Controls(climber); 
-}
+  ClimberManualControl::ClimberManualControl(Climber *climber, frc::XboxController *codriver) : _climber(climber), _codriver(codriver) {
+    Controls(climber); 
+  }
 
-void ClimberManualControl::OnTick(units::second_t dt) {
+  void ClimberManualControl::OnTick(units::second_t dt) {
 
-  if (_codriver->GetXButtonPressed()) {
-    _climber->SetRaw(8_V);
-    _climber->SetState(ClimberState::kRaw);
-  } else if (_codriver->GetYButtonPressed()){
-    _climber->SetRaw(-8_V);
-    _climber->SetState(ClimberState::kRaw);
-  } else {
-    _climber->SetRaw(8_V);
-    _climber->SetState(ClimberState::kIdle);
-  }
-}
\ No newline at end of file
+    if (_codriver->GetXButtonPressed()) {
+      if (_rawControl == false) {
+        _rawControl == true;
+      } else {
+        _rawControl == false;
+      }
+    }
+    if (_rawControl) {
+      _climber->SetRaw(_codriver->GetRightY() * 8_V);
+      _climber->SetState(ClimberState::kRaw);
+    } else {
+      if (_codriver->GetYButtonPressed()) {
+        _climber->SetState(ClimberState::kClimb);
+      }
+    }
+  } 
\ No newline at end of file
diff --git a/src/main/cpp/Climber.cpp b/src/main/cpp/Climber.cpp
index d13df990..50da705c 100644
--- a/src/main/cpp/Climber.cpp
+++ b/src/main/cpp/Climber.cpp
@@ -6,18 +6,40 @@ void Climber::OnUpdate(units::second_t dt) {
   switch (_state) {
     case ClimberState::kIdle:
     {
-      _config.climberGearbox.motorController->Set(0);
+      _stringStateName = "Idle";
+      _setVoltage = 0_V;
     }
     break;
+
+    case ClimberState::kClimb:
+    {
+      _stringStateName = "Climb";
+      _setVoltage = 8_V;
+    }
+    break;
+
+    case ClimberState::kHang:
+    {
+      _stringStateName = "Hang";
+      _setVoltage = -8_V;
+    }
+    break;
+
     case ClimberState::kRaw:
     {
-      _config.climberGearbox.motorController->Set(_rawVoltage.value());
+      _stringStateName = "Raw";
+      _setVoltage = _rawVoltage;
     }
     break;
     default:
        std::cout << "Error magazine in invalid state" << std::endl;
     break;
   }
+  _config.climberGearbox.motorController->SetVoltage(_setVoltage);
+
+  _table->GetEntry("State: ").SetString(_stringStateName);
+  _table->GetEntry("Motor Voltage: ").SetDouble(_setVoltage.value());
+  
 }
 
 void Climber::SetState(ClimberState state) {
diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp
index 1e3ff764..9a96e6d8 100644
--- a/src/main/cpp/Robot.cpp
+++ b/src/main/cpp/Robot.cpp
@@ -46,15 +46,16 @@ void Robot::RobotInit() {
 
   lastPeriodic = wom::now();
 
-  // climber = new Climber(robotmap.climberSystem.config);
-  // wom::BehaviourScheduler::GetInstance()->Register(climber);
-  // climber->SetDefaultBehaviour([this]() {
-  //   return wom::make<ClimberManualControl>(climber, &robotmap.controllers.coDriver);
-  // });
+  climber = new Climber(robotmap.climberSystem.config);
+  wom::BehaviourScheduler::GetInstance()->Register(climber);
+  climber->SetDefaultBehaviour([this]() {
+    return wom::make<ClimberManualControl>(climber, &robotmap.controllers.coDriver);
+  });
   // m_driveSim = new wom::TempSimSwerveDrive(&simulation_timer, &m_field);
   // m_driveSim = wom::TempSimSwerveDrive();
 }
 
+
 void Robot::RobotPeriodic() {
   units::second_t dt = wom::now() - lastPeriodic;
   lastPeriodic = wom::now();
@@ -97,3 +98,6 @@ void Robot::TestPeriodic() {}
 void Robot::SimulationInit() {}
 
 void Robot::SimulationPeriodic() {}
+
+
+
diff --git a/src/main/include/Behaviours/ClimberBehaviour.h b/src/main/include/Behaviours/ClimberBehaviour.h
index 771f1142..399ed843 100644
--- a/src/main/include/Behaviours/ClimberBehaviour.h
+++ b/src/main/include/Behaviours/ClimberBehaviour.h
@@ -11,5 +11,6 @@ class ClimberManualControl : public behaviour::Behaviour {
   private:
     Climber *_climber;
     frc::XboxController *_codriver;
+    bool _rawControl;
 };
 
diff --git a/src/main/include/Climber.h b/src/main/include/Climber.h
index ef6d4cef..a430d1dd 100644
--- a/src/main/include/Climber.h
+++ b/src/main/include/Climber.h
@@ -8,7 +8,9 @@ struct ClimberConfig {
 
 enum class ClimberState {
   kIdle,
-  kRaw
+  kRaw,
+  kClimb,
+  kHang
 };
 
 class Climber : public behaviour::HasBehaviour {
@@ -22,4 +24,7 @@ class Climber : public behaviour::HasBehaviour {
     ClimberConfig _config;
     ClimberState _state = ClimberState::kIdle;
     units::volt_t _rawVoltage;
+    std::string _stringStateName;
+    units::volt_t _setVoltage;
+    std::shared_ptr<nt::NetworkTable> _table = nt::NetworkTableInstance::GetDefault().GetTable("Climber");
 };
\ No newline at end of file
diff --git a/src/main/include/Robot.h b/src/main/include/Robot.h
index b4aed9de..541ad2df 100644
--- a/src/main/include/Robot.h
+++ b/src/main/include/Robot.h
@@ -1,3 +1,8 @@
+
+
+
+
+
 // Copyright (c) 2023-2024 CurtinFRC
 // Open Source Software, you can modify it according to the terms
 // of the MIT License at the root of this project
@@ -14,6 +19,11 @@
 #include <frc/smartdashboard/SendableChooser.h>
 #include <frc/smartdashboard/SmartDashboard.h>
 
+#include <frc/TimedRobot.h>
+#include <networktables/DoubleTopic.h>
+#include <networktables/NetworkTable.h>
+#include <networktables/NetworkTableInstance.h>
+
 #include <string>
 
 #include "Behaviours/ClimberBehaviour.h"

From 6d13c3006354aa9cbf5bf92dd57ebb8b15e2f9c4 Mon Sep 17 00:00:00 2001
From: Sean Chan <sean.chan3@student.education.wa.edu.au>
Date: Thu, 18 Jan 2024 12:51:32 +0800
Subject: [PATCH 020/207] Changed voltage controller to new version

---
 src/main/include/RobotMap.h | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/main/include/RobotMap.h b/src/main/include/RobotMap.h
index 6172ab77..8d57d330 100644
--- a/src/main/include/RobotMap.h
+++ b/src/main/include/RobotMap.h
@@ -11,14 +11,14 @@ struct RobotMap {
 
   struct Mag {
     rev::CANSparkMax magMotor{99, rev::CANSparkMax::MotorType::kBrushless};
-    wom::VoltageController magMotorGroup = wom::VoltageController::Group(magMotor);
+    //wom::VoltageController magMotorGroup = wom::VoltageController::Group(magMotor);
     wom::CANSparkMaxEncoder magEncoder{&magMotor, 42};
     frc::DigitalInput intakeSensor{0};
     frc::DigitalInput magSensor{1};
     frc::DigitalInput shooterSensor{1};
 
     wom::Gearbox magGearbox {
-      &magMotorGroup,
+      &magMotor,
       &magEncoder,
       wom::DCMotor::NEO(1).WithReduction(1)
     };

From 1f99406bccae951ac50c430990d6d6fe08bbd3d5 Mon Sep 17 00:00:00 2001
From: Kingsley Wong <kingsley.wong@student.education.wa.edu.au>
Date: Thu, 18 Jan 2024 14:47:06 +0800
Subject: [PATCH 021/207] Network files 1

---
 src/main/cpp/Intake.cpp          | 29 +++++++++----
 src/main/cpp/IntakeBehaviour.cpp |  2 +-
 src/main/cpp/Main.cpp            |  2 +-
 src/main/cpp/Robot.cpp           | 19 +++++----
 src/main/include/Intake.h        |  6 ++-
 src/main/include/Robot.h         |  2 +-
 src/main/include/RobotMap.h      | 71 ++++++--------------------------
 7 files changed, 52 insertions(+), 79 deletions(-)

diff --git a/src/main/cpp/Intake.cpp b/src/main/cpp/Intake.cpp
index 1aeb8aa4..99515938 100644
--- a/src/main/cpp/Intake.cpp
+++ b/src/main/cpp/Intake.cpp
@@ -13,48 +13,63 @@ void Intake::OnUpdate(units::second_t dt) {
   switch (_state) {
       case IntakeState::kIdle:
       {
-        _config.IntakeMotor.transmission->SetVoltage(0_V);
+        _config.IntakeMotor.motorController->SetVoltage(0_V);
         if (_config.intakeSensor->Get()) {
           setState(IntakeState::kHold);
         }
+        _stringStateName = "Idle";
+        _setVoltage = 0_V;
       }
       break;
       case IntakeState::kRaw:
       {
-        _config.IntakeMotor.transmission->SetVoltage(_rawVoltage);
+        _config.IntakeMotor.motorController->SetVoltage(_rawVoltage);
+        _stringStateName = "Raw";
+        _setVoltage = _rawVoltage;
       }
       break;
       case IntakeState::kEject:
       {
-        _config.IntakeMotor.transmission->SetVoltage(-5_V);
+        _config.IntakeMotor.motorController->SetVoltage(-5_V);
         if (_config.intakeSensor->Get() == 0 && _config.magSensor->Get() == 0) {
           setState(IntakeState::kIdle);
         }
+        _stringStateName = "Eject";
+        _setVoltage = -5_V;
       }
       break;
       case IntakeState::kHold:
       {
-        _config.IntakeMotor.transmission->SetVoltage(0_V);
+        _config.IntakeMotor.motorController->SetVoltage(0_V);
+        _stringStateName = "Hold";
+        _setVoltage = 0_V;
       }
       break;
       case IntakeState::kIntake:
       {
-        _config.IntakeMotor.transmission->SetVoltage(5_V);
+        _config.IntakeMotor.motorController->SetVoltage(5_V);
+        _stringStateName = "Intake";
+        _setVoltage = 5_V;
       } 
       break;
       case IntakeState::kPass:
       {
-        _config.IntakeMotor.transmission->SetVoltage(5_V);
+        _config.IntakeMotor.motorController->SetVoltage(5_V);
         if (_config.shooterSensor->Get()) {
           setState(IntakeState::kIdle);
+          _stringStateName = "Pass";
+          _setVoltage = 5_V;
       }
       break;
       default:
         std::cout <<"Error: Intake in INVALID STATE." << std::endl;
       break;
     }
-    
   }
+
+  _table->GetEntry("State: ").SetString(_stringStateName);
+  _table->GetEntry("Motor Voltage: ").SetDouble(_setVoltage.value());
+
 }
 
 void Intake::setState(IntakeState state) {
diff --git a/src/main/cpp/IntakeBehaviour.cpp b/src/main/cpp/IntakeBehaviour.cpp
index 17d18484..ec18d5a1 100644
--- a/src/main/cpp/IntakeBehaviour.cpp
+++ b/src/main/cpp/IntakeBehaviour.cpp
@@ -26,7 +26,7 @@ void IntakeManualControl::OnTick(units::second_t dt) {
       _intake->setState(IntakeState::kPass);
     }
   };
-}; //else {}
+};
 
 
 IntakeAutoControl::IntakeAutoControl(Intake *intake) : _intake(intake) {};
diff --git a/src/main/cpp/Main.cpp b/src/main/cpp/Main.cpp
index e27c9c50..3bd80491 100644
--- a/src/main/cpp/Main.cpp
+++ b/src/main/cpp/Main.cpp
@@ -10,4 +10,4 @@
 #include "Robot.h"
 #include "Wombat.h"
 
-WOMBAT_ROBOT_MAIN(Robot);
+WOMBAT_ROBOT_MAIN(Robot);
\ No newline at end of file
diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp
index 8c887e0e..3f9881e2 100644
--- a/src/main/cpp/Robot.cpp
+++ b/src/main/cpp/Robot.cpp
@@ -4,7 +4,6 @@
 
 #include "Robot.h"
 
-// include units
 #include <units/velocity.h>
 #include <units/acceleration.h>
 #include <units/length.h>
@@ -16,6 +15,11 @@
 #include <frc/controller/RamseteController.h>
 #include <frc/Timer.h>
 
+#include <frc/TimedRobot.h>
+#include <networktables/DoubleTopic.h>
+#include <networktables/NetworkTable.h>
+#include <networktables/NetworkTableInstance.h>
+
 static units::second_t lastPeriodic;
 
 void Robot::RobotInit() {
@@ -48,11 +52,9 @@ void Robot::RobotInit() {
   wom::BehaviourScheduler::GetInstance()->Register(intake);
 
   intake->SetDefaultBehaviour([this]() {
-    return wom::make<IntakeManualControl>(intake, robotmap.driver);
+    return wom::make<IntakeManualControl>(intake, robotmap.controllers.driver);
   });
 
-  // m_driveSim = new wom::TempSimSwerveDrive(&simulation_timer, &m_field);
-  // m_driveSim = wom::TempSimSwerveDrive();
 }
 
 void Robot::RobotPeriodic() {
@@ -66,14 +68,12 @@ void Robot::RobotPeriodic() {
 }
 
 void Robot::AutonomousInit() {
-  // m_driveSim->SetPath(m_path_chooser.GetSelected());
 
   loop.Clear();
   sched->InterruptAll();
-  // _swerveDrive->OnStart();
 }
+
 void Robot::AutonomousPeriodic() {
-  // m_driveSim->OnUpdate();
 }
 
 void Robot::TeleopInit() {
@@ -81,7 +81,10 @@ void Robot::TeleopInit() {
   wom::BehaviourScheduler *scheduler = wom::BehaviourScheduler::GetInstance();
   scheduler->InterruptAll();
 }
-void Robot::TeleopPeriodic() {}
+
+void Robot::TeleopPeriodic() {
+  
+}
 
 void Robot::DisabledInit() {}
 void Robot::DisabledPeriodic() {}
diff --git a/src/main/include/Intake.h b/src/main/include/Intake.h
index f51d7487..ec237ac8 100644
--- a/src/main/include/Intake.h
+++ b/src/main/include/Intake.h
@@ -1,4 +1,3 @@
-
 #pragma once
 
 #include "wombat.h"
@@ -35,4 +34,7 @@ class Intake : public behaviour::HasBehaviour {
   IntakeState _state = IntakeState::kIdle;
 
   units::volt_t _rawVoltage;
-}; 
\ No newline at end of file
+  std::string _stringStateName;
+  units::volt_t _setVoltage;
+  std::shared_ptr<nt::NetworkTable> _table = nt::NetworkTableInstance::GetDefault().GetTable("Intake");
+}; 
diff --git a/src/main/include/Robot.h b/src/main/include/Robot.h
index 90b4e3ba..87fe1181 100644
--- a/src/main/include/Robot.h
+++ b/src/main/include/Robot.h
@@ -55,4 +55,4 @@ class Robot : public frc::TimedRobot {
   wom::SwerveDrive* _swerveDrive;
 
   Intake *intake;
-};
\ No newline at end of file
+};
diff --git a/src/main/include/RobotMap.h b/src/main/include/RobotMap.h
index 5783ee48..f7d39759 100644
--- a/src/main/include/RobotMap.h
+++ b/src/main/include/RobotMap.h
@@ -22,25 +22,19 @@
 #include "IntakeBehaviour.h"
 
 struct RobotMap {
-  struct Controllers {
-    frc::XboxController driver = frc::XboxController(0);
-    frc::XboxController coDriver = frc::XboxController(1);
-    frc::XboxController testController = frc::XboxController(2);
-  };
-  Controllers controllers;
 
   struct IntakeSystem {
+
     rev::CANSparkMax intakeMotor{99, rev::CANSparkMax::MotorType::kBrushless};
-    wom::VoltageController intakeMotorGroup = wom::VoltageController::Group(intakeMotor);
-    wom::CANSparkMaxEncoder intakeEncoder{&intakeMotor, 42};
+    wom::CANSparkMaxEncoder intakeEncoder{&intakeMotor, 0.1_m};
     frc::DigitalInput intakeSensor {0};
     frc::DigitalInput magSensor {0};
     frc::DigitalInput shooterSensor {0};
 
     wom::Gearbox IntakeGearbox { 
-      &intakeMotorGroup,
+      &intakeMotor,
       &intakeEncoder,
-      wom::DCMotor::NEO(1).WithReduction(1)
+      frc::DCMotor::NEO(1)
     };
 
     IntakeConfig config {
@@ -51,7 +45,11 @@ struct RobotMap {
     };
   }; IntakeSystem intakeSystem;
 
-
+  struct Controllers {
+    frc::XboxController driver = frc::XboxController(0);
+    frc::XboxController codriver = frc::XboxController(1);
+  };
+  Controllers controllers;
 
   struct SwerveBase {
     ctre::phoenix6::hardware::CANcoder frontLeftCancoder{19};
@@ -73,7 +71,6 @@ struct RobotMap {
 
     wpi::array<wom::SwerveModuleConfig, 4> moduleConfigs{
         wom::SwerveModuleConfig{
-            // front left module
             frc::Translation2d(10.761_in, 9.455_in),
             wom::Gearbox{driveMotors[0], new wom::TalonFXEncoder(driveMotors[0], 0.0445_m, 6.75),
                          frc::DCMotor::Falcon500(1).WithReduction(6.75)},
@@ -81,7 +78,6 @@ struct RobotMap {
                          frc::DCMotor::Falcon500(1).WithReduction(12.8)},
             &frontLeftCancoder, 4_in / 2},
         wom::SwerveModuleConfig{
-            // front right module
             frc::Translation2d(10.761_in, -9.455_in),
             wom::Gearbox{driveMotors[1], new wom::TalonFXEncoder(driveMotors[1], 0.0445_m, 6.75),
                          frc::DCMotor::Falcon500(1).WithReduction(6.75)},
@@ -89,7 +85,6 @@ struct RobotMap {
                          frc::DCMotor::Falcon500(1).WithReduction(12.8)},
             &frontRightCancoder, 4_in / 2},
         wom::SwerveModuleConfig{
-            // back left module
             frc::Translation2d(-10.761_in, 9.455_in),
             wom::Gearbox{driveMotors[2], new wom::TalonFXEncoder(driveMotors[2], 0.0445_m, 6.75),
                          frc::DCMotor::Falcon500(1).WithReduction(6.75)},
@@ -97,7 +92,6 @@ struct RobotMap {
                          frc::DCMotor::Falcon500(1).WithReduction(12.8)},
             &backRightCancoder, 4_in / 2},
         wom::SwerveModuleConfig{
-            // back right module
             frc::Translation2d(-10.761_in, -9.455_in),
             wom::Gearbox{driveMotors[3], new wom::TalonFXEncoder(driveMotors[3], 0.0445_m, 6.75),
                          frc::DCMotor::Falcon500(1).WithReduction(6.75)},
@@ -106,14 +100,11 @@ struct RobotMap {
             &backLeftCancoder, 4_in / 2},
     };
 
-    // Setting the PID path and values to be used for SwerveDrive and
-    // SwerveModules
     wom::SwerveModule::angle_pid_conf_t anglePID{
         "/drivetrain/pid/angle/config", 2_V / 360_deg, 0.0_V / (100_deg * 1_s),
         0_V / (100_deg / 1_s),          1_deg,         0.5_deg / 2_s};
     wom::SwerveModule::velocity_pid_conf_t velocityPID{
         "/drivetrain/pid/velocity/config",
-        //  12_V / 4_mps // webers per metre
     };
     wom::SwerveDriveConfig::pose_angle_conf_t poseAnglePID{
         "/drivetrain/pid/pose/angle/config",
@@ -131,28 +122,17 @@ struct RobotMap {
         10_cm / 1_s,
         10_cm};
 
-    // the config for the whole swerve drive
     wom::SwerveDriveConfig config{"/drivetrain",
                                   anglePID,
                                   velocityPID,
-                                  moduleConfigs,  // each module
+                                  moduleConfigs, 
                                   gyro,
                                   poseAnglePID,
                                   posePositionPID,
-                                  60_kg,  // robot mass (estimate rn)
+                                  60_kg, 
                                   {0.1, 0.1, 0.1},
                                   {0.9, 0.9, 0.9}};
 
-    // current limiting and setting idle mode of modules to brake mode
-    // SwerveBase() {
-    //  for (size_t i = 0; i < 4; i++) {
-    //    turnMotors[i]->ConfigSupplyCurrentLimit(
-    //        SupplyCurrentLimitConfiguration(true, 15, 15, 0));
-    //    driveMotors[i]->SetNeutralMode(NeutralMode::Brake);
-    //    turnMotors[i]->SetNeutralMode(NeutralMode::Brake);
-    //    driveMotors[i]->SetInverted(true);
-    //  }
-    //}
   };
   SwerveBase swerveBase;
 };
@@ -161,31 +141,4 @@ struct RobotMap {
 #include "Wombat.h"
 #include "Intake.h"
 #include "IntakeBehaviour.h"
-#include <frc/XboxController.h>
-
-struct RobotMap {
-  frc::XboxController driver{0};
-
-  struct IntakeSystem {
-    rev::CANSparkMax intakeMotor{99, rev::CANSparkMax::MotorType::kBrushless};
-    wom::VoltageController intakeMotorGroup = wom::VoltageController::Group(intakeMotor);
-    wom::CANSparkMaxEncoder intakeEncoder{&intakeMotor, 42};
-    frc::DigitalInput intakeSensor {0};
-    frc::DigitalInput magSensor {0};
-    frc::DigitalInput shooterSensor {0};
-
-    wom::Gearbox IntakeGearbox { 
-      &intakeMotorGroup,
-      &intakeEncoder,
-      wom::DCMotor::NEO(1).WithReduction(1)
-    };
-
-    IntakeConfig config {
-      IntakeGearbox,
-      &intakeSensor,
-      &magSensor,
-      &shooterSensor
-    };
-  }; IntakeSystem intakeSystem;
-
-};
\ No newline at end of file
+#include <frc/XboxController.h>
\ No newline at end of file

From c6b9be5162ed2f5dfc7cfea6fd5230f353e5e80b Mon Sep 17 00:00:00 2001
From: Kingsley Wong <kingsley.wong@student.education.wa.edu.au>
Date: Thu, 18 Jan 2024 15:03:38 +0800
Subject: [PATCH 022/207] Network files 2

---
 src/main/cpp/Intake.cpp | 5 +++--
 1 file changed, 3 insertions(+), 2 deletions(-)

diff --git a/src/main/cpp/Intake.cpp b/src/main/cpp/Intake.cpp
index 99515938..68e36072 100644
--- a/src/main/cpp/Intake.cpp
+++ b/src/main/cpp/Intake.cpp
@@ -66,10 +66,11 @@ void Intake::OnUpdate(units::second_t dt) {
       break;
     }
   }
-
   _table->GetEntry("State: ").SetString(_stringStateName);
   _table->GetEntry("Motor Voltage: ").SetDouble(_setVoltage.value());
-
+  _table->GetEntry("Intake Sensor: ").SetDouble(_config.intakeSensor->Get());
+  _table->GetEntry("Shooter Sensor: ").SetDouble(_config.shooterSensor->Get());
+  _table->GetEntry("Magazine Sensor: ").SetDouble(_config.magSensor->Get());
 }
 
 void Intake::setState(IntakeState state) {

From cf5488e581257987954800d0ba4845619b79e295 Mon Sep 17 00:00:00 2001
From: Sean Chan <sean.chan3@student.education.wa.edu.au>
Date: Thu, 18 Jan 2024 16:09:03 +0800
Subject: [PATCH 023/207] Climber added more fixes

---
 gradlew.bat                                   |  3 +-
 src/main/cpp/Behaviours/ClimberBehaviour.cpp  | 38 +++++++++----------
 src/main/cpp/Robot.cpp                        |  2 +-
 .../include/Behaviours/ClimberBehaviour.h     |  2 +-
 src/main/include/Climber.h                    |  2 +-
 5 files changed, 24 insertions(+), 23 deletions(-)

diff --git a/gradlew.bat b/gradlew.bat
index 6471a59a..567381b5 100644
--- a/gradlew.bat
+++ b/gradlew.bat
@@ -7,7 +7,8 @@
 @rem
 @rem      https://www.apache.org/licenses/LICENSE-2.0
 @rem
-@rem Unless required by applicable law or agreed to in writing, software``
+@rem Unless required by applicable law or agreed to in writing, software
+
 @rem distributed under the License is distributed on an "AS IS" BASIS,
 @rem WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 @rem See the License for the specific language governing permissions and
diff --git a/src/main/cpp/Behaviours/ClimberBehaviour.cpp b/src/main/cpp/Behaviours/ClimberBehaviour.cpp
index 796f1ebf..46414b22 100644
--- a/src/main/cpp/Behaviours/ClimberBehaviour.cpp
+++ b/src/main/cpp/Behaviours/ClimberBehaviour.cpp
@@ -1,24 +1,24 @@
-  #include "behaviours/ClimberBehaviour.h"
+#include "behaviours/ClimberBehaviour.h"
 
-  ClimberManualControl::ClimberManualControl(Climber *climber, frc::XboxController *codriver) : _climber(climber), _codriver(codriver) {
-    Controls(climber); 
-  }
+ClimberManualControl::ClimberManualControl(Climber *climber, frc::XboxController *codriver) : _climber(climber), _codriver(codriver) {
+  Controls(climber); 
+}
 
-  void ClimberManualControl::OnTick(units::second_t dt) {
+void ClimberManualControl::OnTick(units::second_t dt) {
 
-    if (_codriver->GetXButtonPressed()) {
-      if (_rawControl == false) {
-        _rawControl == true;
-      } else {
-        _rawControl == false;
-      }
-    }
-    if (_rawControl) {
-      _climber->SetRaw(_codriver->GetRightY() * 8_V);
-      _climber->SetState(ClimberState::kRaw);
+  if (_codriver->GetXButtonPressed()) {
+    if (_rawControl == false) {
+      _rawControl == true;
     } else {
-      if (_codriver->GetYButtonPressed()) {
-        _climber->SetState(ClimberState::kClimb);
-      }
+      _rawControl == false;
     }
-  } 
\ No newline at end of file
+  }
+  if (_rawControl) {
+    _climber->SetRaw(_codriver->GetRightY() * 8_V);
+    _climber->SetState(ClimberState::kRaw);
+  } else {
+    if (_codriver->GetYButtonPressed()) {
+      _climber->SetState(ClimberState::kClimb);
+    }
+  }
+} 
\ No newline at end of file
diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp
index 9a96e6d8..9ee8050a 100644
--- a/src/main/cpp/Robot.cpp
+++ b/src/main/cpp/Robot.cpp
@@ -65,7 +65,7 @@ void Robot::RobotPeriodic() {
 
   _swerveDrive->OnUpdate(dt);
 
-  // climber->OnUpdate(dt);
+  climber->OnUpdate(dt);
 }
 
 void Robot::AutonomousInit() {
diff --git a/src/main/include/Behaviours/ClimberBehaviour.h b/src/main/include/Behaviours/ClimberBehaviour.h
index 399ed843..adab25e5 100644
--- a/src/main/include/Behaviours/ClimberBehaviour.h
+++ b/src/main/include/Behaviours/ClimberBehaviour.h
@@ -11,6 +11,6 @@ class ClimberManualControl : public behaviour::Behaviour {
   private:
     Climber *_climber;
     frc::XboxController *_codriver;
-    bool _rawControl;
+    bool _rawControl = false;
 };
 
diff --git a/src/main/include/Climber.h b/src/main/include/Climber.h
index a430d1dd..a0f01505 100644
--- a/src/main/include/Climber.h
+++ b/src/main/include/Climber.h
@@ -23,7 +23,7 @@ class Climber : public behaviour::HasBehaviour {
   private:
     ClimberConfig _config;
     ClimberState _state = ClimberState::kIdle;
-    units::volt_t _rawVoltage;
+    units::volt_t _rawVoltage = 0_V;
     std::string _stringStateName;
     units::volt_t _setVoltage;
     std::shared_ptr<nt::NetworkTable> _table = nt::NetworkTableInstance::GetDefault().GetTable("Climber");

From a5940a9fbc9ad7eeb6fab861609ed614cadc6dbb Mon Sep 17 00:00:00 2001
From: Sean Chan <sean.chan3@student.education.wa.edu.au>
Date: Thu, 18 Jan 2024 16:13:35 +0800
Subject: [PATCH 024/207] Magazine added more fixes

---
 src/main/include/Mag.h | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/main/include/Mag.h b/src/main/include/Mag.h
index b02ff876..df0564e9 100644
--- a/src/main/include/Mag.h
+++ b/src/main/include/Mag.h
@@ -29,7 +29,7 @@ class Mag : public behaviour::HasBehaviour {
     MagConfig _config;
     MagState _state;
     units::volt_t _voltage;
-    std::string _stringStateName;
-    units::volt_t _setVoltage;
+    std::string _stringStateName = "No State";
+    units::volt_t _setVoltage = 0_V;
     std::shared_ptr<nt::NetworkTable> _table = nt::NetworkTableInstance::GetDefault().GetTable("Magazine");
 };
\ No newline at end of file

From a3eaf45a9d9d6f76e6fe0a4eb951ea0bd90417c4 Mon Sep 17 00:00:00 2001
From: Kingsley Wong <kingsley.wong@student.education.wa.edu.au>
Date: Thu, 18 Jan 2024 16:22:25 +0800
Subject: [PATCH 025/207] Network files 3

---
 src/main/cpp/Intake.cpp     | 22 ++++++++++++----------
 src/main/cpp/Robot.cpp      |  1 +
 src/main/include/Intake.h   |  6 +++---
 src/main/include/RobotMap.h | 17 ++++++-----------
 4 files changed, 22 insertions(+), 24 deletions(-)

diff --git a/src/main/cpp/Intake.cpp b/src/main/cpp/Intake.cpp
index 68e36072..07287a47 100644
--- a/src/main/cpp/Intake.cpp
+++ b/src/main/cpp/Intake.cpp
@@ -13,7 +13,7 @@ void Intake::OnUpdate(units::second_t dt) {
   switch (_state) {
       case IntakeState::kIdle:
       {
-        _config.IntakeMotor.motorController->SetVoltage(0_V);
+        //_config.IntakeMotor.motorController->SetVoltage(0_V);
         if (_config.intakeSensor->Get()) {
           setState(IntakeState::kHold);
         }
@@ -23,14 +23,14 @@ void Intake::OnUpdate(units::second_t dt) {
       break;
       case IntakeState::kRaw:
       {
-        _config.IntakeMotor.motorController->SetVoltage(_rawVoltage);
+        //_config.IntakeMotor.motorController->SetVoltage(_rawVoltage);
         _stringStateName = "Raw";
         _setVoltage = _rawVoltage;
       }
       break;
       case IntakeState::kEject:
       {
-        _config.IntakeMotor.motorController->SetVoltage(-5_V);
+        //_config.IntakeMotor.motorController->SetVoltage(-5_V);
         if (_config.intakeSensor->Get() == 0 && _config.magSensor->Get() == 0) {
           setState(IntakeState::kIdle);
         }
@@ -40,21 +40,21 @@ void Intake::OnUpdate(units::second_t dt) {
       break;
       case IntakeState::kHold:
       {
-        _config.IntakeMotor.motorController->SetVoltage(0_V);
+        //_config.IntakeMotor.motorController->SetVoltage(0_V);
         _stringStateName = "Hold";
         _setVoltage = 0_V;
       }
       break;
       case IntakeState::kIntake:
       {
-        _config.IntakeMotor.motorController->SetVoltage(5_V);
+        //_config.IntakeMotor.motorController->SetVoltage(5_V);
         _stringStateName = "Intake";
         _setVoltage = 5_V;
       } 
       break;
       case IntakeState::kPass:
       {
-        _config.IntakeMotor.motorController->SetVoltage(5_V);
+        //_config.IntakeMotor.motorController->SetVoltage(5_V);
         if (_config.shooterSensor->Get()) {
           setState(IntakeState::kIdle);
           _stringStateName = "Pass";
@@ -67,10 +67,12 @@ void Intake::OnUpdate(units::second_t dt) {
     }
   }
   _table->GetEntry("State: ").SetString(_stringStateName);
-  _table->GetEntry("Motor Voltage: ").SetDouble(_setVoltage.value());
-  _table->GetEntry("Intake Sensor: ").SetDouble(_config.intakeSensor->Get());
-  _table->GetEntry("Shooter Sensor: ").SetDouble(_config.shooterSensor->Get());
-  _table->GetEntry("Magazine Sensor: ").SetDouble(_config.magSensor->Get());
+  _table->GetEntry("Motor Voltage: ").SetBoolean(_setVoltage.value());
+  _table->GetEntry("Intake Sensor: ").SetBoolean(_config.intakeSensor->Get());
+  _table->GetEntry("Shooter Sensor: ").SetBoolean(_config.shooterSensor->Get());
+  _table->GetEntry("Magazine Sensor: ").SetBoolean(_config.magSensor->Get());
+
+  _config.IntakeMotor.motorController->SetVoltage(5_V);
 }
 
 void Intake::setState(IntakeState state) {
diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp
index 3f9881e2..b906ece6 100644
--- a/src/main/cpp/Robot.cpp
+++ b/src/main/cpp/Robot.cpp
@@ -65,6 +65,7 @@ void Robot::RobotPeriodic() {
   wom::BehaviourScheduler::GetInstance()->Tick();
 
   _swerveDrive->OnUpdate(dt);
+  intake->OnUpdate(dt);
 }
 
 void Robot::AutonomousInit() {
diff --git a/src/main/include/Intake.h b/src/main/include/Intake.h
index ec237ac8..a9571136 100644
--- a/src/main/include/Intake.h
+++ b/src/main/include/Intake.h
@@ -33,8 +33,8 @@ class Intake : public behaviour::HasBehaviour {
   IntakeConfig _config;
   IntakeState _state = IntakeState::kIdle;
 
-  units::volt_t _rawVoltage;
-  std::string _stringStateName;
-  units::volt_t _setVoltage;
+  units::volt_t _rawVoltage = 0_V;
+  std::string _stringStateName = "error";
+  units::volt_t _setVoltage = 0_V;
   std::shared_ptr<nt::NetworkTable> _table = nt::NetworkTableInstance::GetDefault().GetTable("Intake");
 }; 
diff --git a/src/main/include/RobotMap.h b/src/main/include/RobotMap.h
index f7d39759..47245b08 100644
--- a/src/main/include/RobotMap.h
+++ b/src/main/include/RobotMap.h
@@ -23,6 +23,11 @@
 
 struct RobotMap {
 
+  struct Controllers {
+    frc::XboxController driver = frc::XboxController(0);
+    frc::XboxController codriver = frc::XboxController(1);
+  };
+
   struct IntakeSystem {
 
     rev::CANSparkMax intakeMotor{99, rev::CANSparkMax::MotorType::kBrushless};
@@ -45,10 +50,6 @@ struct RobotMap {
     };
   }; IntakeSystem intakeSystem;
 
-  struct Controllers {
-    frc::XboxController driver = frc::XboxController(0);
-    frc::XboxController codriver = frc::XboxController(1);
-  };
   Controllers controllers;
 
   struct SwerveBase {
@@ -135,10 +136,4 @@ struct RobotMap {
 
   };
   SwerveBase swerveBase;
-};
-#pragma once
-
-#include "Wombat.h"
-#include "Intake.h"
-#include "IntakeBehaviour.h"
-#include <frc/XboxController.h>
\ No newline at end of file
+};
\ No newline at end of file

From 8a725189929fca93a945e845e73059f8a09231c8 Mon Sep 17 00:00:00 2001
From: Sean Chan <sean.chan3@student.education.wa.edu.au>
Date: Sun, 21 Jan 2024 10:44:50 +0800
Subject: [PATCH 026/207] Climber added more fixes

---
 src/main/cpp/Behaviours/ClimberBehaviour.cpp | 6 +++---
 src/main/include/Climber.h                   | 2 +-
 2 files changed, 4 insertions(+), 4 deletions(-)

diff --git a/src/main/cpp/Behaviours/ClimberBehaviour.cpp b/src/main/cpp/Behaviours/ClimberBehaviour.cpp
index 46414b22..0befa293 100644
--- a/src/main/cpp/Behaviours/ClimberBehaviour.cpp
+++ b/src/main/cpp/Behaviours/ClimberBehaviour.cpp
@@ -8,13 +8,13 @@ void ClimberManualControl::OnTick(units::second_t dt) {
 
   if (_codriver->GetXButtonPressed()) {
     if (_rawControl == false) {
-      _rawControl == true;
+      _rawControl = true;
     } else {
-      _rawControl == false;
+      _rawControl = false;
     }
   }
   if (_rawControl) {
-    _climber->SetRaw(_codriver->GetRightY() * 8_V);
+    _climber->SetRaw(wom::deadzone(_codriver->GetRightY()) * 8_V);
     _climber->SetState(ClimberState::kRaw);
   } else {
     if (_codriver->GetYButtonPressed()) {
diff --git a/src/main/include/Climber.h b/src/main/include/Climber.h
index a0f01505..7499a839 100644
--- a/src/main/include/Climber.h
+++ b/src/main/include/Climber.h
@@ -24,7 +24,7 @@ class Climber : public behaviour::HasBehaviour {
     ClimberConfig _config;
     ClimberState _state = ClimberState::kIdle;
     units::volt_t _rawVoltage = 0_V;
-    std::string _stringStateName;
+    std::string _stringStateName = "error";
     units::volt_t _setVoltage;
     std::shared_ptr<nt::NetworkTable> _table = nt::NetworkTableInstance::GetDefault().GetTable("Climber");
 };
\ No newline at end of file

From 2542159252c28572ef4b121b102cb4549ee649ec Mon Sep 17 00:00:00 2001
From: Sean Chan <sean.chan3@student.education.wa.edu.au>
Date: Sun, 21 Jan 2024 10:50:28 +0800
Subject: [PATCH 027/207] Magazine changed _voltage to _rawVoltage to aviod
 confusion

---
 src/main/cpp/Mag.cpp   | 4 ++--
 src/main/include/Mag.h | 2 +-
 2 files changed, 3 insertions(+), 3 deletions(-)

diff --git a/src/main/cpp/Mag.cpp b/src/main/cpp/Mag.cpp
index 43d2c234..2b279651 100644
--- a/src/main/cpp/Mag.cpp
+++ b/src/main/cpp/Mag.cpp
@@ -37,7 +37,7 @@ void Mag::OnUpdate(units::second_t dt) {
     break;
 
     case MagState::kRaw:
-      _setVoltage = _voltage;
+      _setVoltage = _rawVoltage;
       _stringStateName = "Raw";
     break;
 
@@ -70,7 +70,7 @@ void Mag::SetState(MagState state) {
 }
 
 void Mag::SetRaw(units::volt_t voltage) {
-  _voltage = voltage;
+  _rawVoltage = voltage;
 }
 MagState Mag::GetState() {
   return _state;
diff --git a/src/main/include/Mag.h b/src/main/include/Mag.h
index df0564e9..a79a1ddd 100644
--- a/src/main/include/Mag.h
+++ b/src/main/include/Mag.h
@@ -28,7 +28,7 @@ class Mag : public behaviour::HasBehaviour {
   private:
     MagConfig _config;
     MagState _state;
-    units::volt_t _voltage;
+    units::volt_t _rawVoltage = 0_V;
     std::string _stringStateName = "No State";
     units::volt_t _setVoltage = 0_V;
     std::shared_ptr<nt::NetworkTable> _table = nt::NetworkTableInstance::GetDefault().GetTable("Magazine");

From 6b858252818b3f7c973462eea3e409d3da5651df Mon Sep 17 00:00:00 2001
From: Kingsley Wong <kingsley.wong@student.education.wa.edu.au>
Date: Sun, 21 Jan 2024 10:54:15 +0800
Subject: [PATCH 028/207] Almost done i think

---
 src/main/cpp/Intake.cpp | 12 ++++++------
 1 file changed, 6 insertions(+), 6 deletions(-)

diff --git a/src/main/cpp/Intake.cpp b/src/main/cpp/Intake.cpp
index 07287a47..55b277dd 100644
--- a/src/main/cpp/Intake.cpp
+++ b/src/main/cpp/Intake.cpp
@@ -13,7 +13,7 @@ void Intake::OnUpdate(units::second_t dt) {
   switch (_state) {
       case IntakeState::kIdle:
       {
-        //_config.IntakeMotor.motorController->SetVoltage(0_V);
+        _config.IntakeMotor.motorController->SetVoltage(0_V);
         if (_config.intakeSensor->Get()) {
           setState(IntakeState::kHold);
         }
@@ -23,14 +23,14 @@ void Intake::OnUpdate(units::second_t dt) {
       break;
       case IntakeState::kRaw:
       {
-        //_config.IntakeMotor.motorController->SetVoltage(_rawVoltage);
+        _config.IntakeMotor.motorController->SetVoltage(_rawVoltage);
         _stringStateName = "Raw";
         _setVoltage = _rawVoltage;
       }
       break;
       case IntakeState::kEject:
       {
-        //_config.IntakeMotor.motorController->SetVoltage(-5_V);
+        _config.IntakeMotor.motorController->SetVoltage(-5_V);
         if (_config.intakeSensor->Get() == 0 && _config.magSensor->Get() == 0) {
           setState(IntakeState::kIdle);
         }
@@ -40,7 +40,7 @@ void Intake::OnUpdate(units::second_t dt) {
       break;
       case IntakeState::kHold:
       {
-        //_config.IntakeMotor.motorController->SetVoltage(0_V);
+        _config.IntakeMotor.motorController->SetVoltage(0_V);
         _stringStateName = "Hold";
         _setVoltage = 0_V;
       }
@@ -54,7 +54,7 @@ void Intake::OnUpdate(units::second_t dt) {
       break;
       case IntakeState::kPass:
       {
-        //_config.IntakeMotor.motorController->SetVoltage(5_V);
+        _config.IntakeMotor.motorController->SetVoltage(5_V);
         if (_config.shooterSensor->Get()) {
           setState(IntakeState::kIdle);
           _stringStateName = "Pass";
@@ -72,7 +72,7 @@ void Intake::OnUpdate(units::second_t dt) {
   _table->GetEntry("Shooter Sensor: ").SetBoolean(_config.shooterSensor->Get());
   _table->GetEntry("Magazine Sensor: ").SetBoolean(_config.magSensor->Get());
 
-  _config.IntakeMotor.motorController->SetVoltage(5_V);
+  _config.IntakeMotor.motorController->SetVoltage(_setVoltage);
 }
 
 void Intake::setState(IntakeState state) {

From 5a7c91ef20e990db94fbdbc67084f4e177447148 Mon Sep 17 00:00:00 2001
From: Sean Chan <sean.chan3@student.education.wa.edu.au>
Date: Sun, 21 Jan 2024 11:10:14 +0800
Subject: [PATCH 029/207] Fixed build errors

---
 src/main/include/Mag.h                     | 5 ++++-
 src/main/include/behaviours/MagBehaviour.h | 2 +-
 2 files changed, 5 insertions(+), 2 deletions(-)

diff --git a/src/main/include/Mag.h b/src/main/include/Mag.h
index a79a1ddd..fa971106 100644
--- a/src/main/include/Mag.h
+++ b/src/main/include/Mag.h
@@ -1,6 +1,9 @@
 #pragma once
 #include "wombat.h"
 #include <frc/DigitalInput.h>
+#include <string>
+#include <memory>
+
 
 struct MagConfig {
   wom::Gearbox magGearbox;
@@ -19,7 +22,7 @@ enum class MagState {
 
 class Mag : public behaviour::HasBehaviour {
   public:
-    Mag(MagConfig config);
+    explicit Mag(MagConfig config);
 
     void OnUpdate(units::second_t dt);
     void SetState(MagState state);
diff --git a/src/main/include/behaviours/MagBehaviour.h b/src/main/include/behaviours/MagBehaviour.h
index d42d9271..df8c0e91 100644
--- a/src/main/include/behaviours/MagBehaviour.h
+++ b/src/main/include/behaviours/MagBehaviour.h
@@ -26,7 +26,7 @@ class MagAutoPass : public behaviour::Behaviour {
 
 class MagAutoHold : public behaviour::Behaviour {
  public:
-  MagAutoHold(Mag *mag);
+  explicit MagAutoHold(Mag *mag);
    
   void OnTick(units::second_t dt) override;
  private:

From 477e949224a227fae05735270a3561a041dd2a54 Mon Sep 17 00:00:00 2001
From: Sean Chan <sean.chan3@student.education.wa.edu.au>
Date: Sun, 21 Jan 2024 11:16:48 +0800
Subject: [PATCH 030/207] Fixed build errors

---
 src/main/include/Behaviours/ClimberBehaviour.h | 2 +-
 src/main/include/Climber.h                     | 4 +++-
 2 files changed, 4 insertions(+), 2 deletions(-)

diff --git a/src/main/include/Behaviours/ClimberBehaviour.h b/src/main/include/Behaviours/ClimberBehaviour.h
index adab25e5..f1611ddf 100644
--- a/src/main/include/Behaviours/ClimberBehaviour.h
+++ b/src/main/include/Behaviours/ClimberBehaviour.h
@@ -6,7 +6,7 @@
 
 class ClimberManualControl : public behaviour::Behaviour {
   public:
-    ClimberManualControl(Climber *climber, frc::XboxController *codriver);
+    explicit ClimberManualControl(Climber *climber, frc::XboxController *codriver);
     void OnTick(units::second_t dt) override;
   private:
     Climber *_climber;
diff --git a/src/main/include/Climber.h b/src/main/include/Climber.h
index 7499a839..f22db2cb 100644
--- a/src/main/include/Climber.h
+++ b/src/main/include/Climber.h
@@ -1,6 +1,8 @@
 #pragma once
 
 #include "wombat.h"
+#include <string>
+#include <memory>
 
 struct ClimberConfig {
   wom::Gearbox climberGearbox;
@@ -15,7 +17,7 @@ enum class ClimberState {
 
 class Climber : public behaviour::HasBehaviour {
   public:
-    Climber(ClimberConfig config);
+    explicit Climber(ClimberConfig config);
 
     void OnUpdate(units::second_t dt);
     void SetState(ClimberState state);

From fb329742b04733dcdc94f8596aacc32f530512a7 Mon Sep 17 00:00:00 2001
From: Kingsley Wong <kingsley.wong@student.education.wa.edu.au>
Date: Sun, 21 Jan 2024 11:17:10 +0800
Subject: [PATCH 031/207] finished wpiformat errors

---
 src/main/cpp/Intake.cpp            | 2 +-
 src/main/cpp/IntakeBehaviour.cpp   | 1 -
 src/main/include/Intake.h          | 6 ++++--
 src/main/include/IntakeBehaviour.h | 4 ++--
 4 files changed, 7 insertions(+), 6 deletions(-)

diff --git a/src/main/cpp/Intake.cpp b/src/main/cpp/Intake.cpp
index 55b277dd..43361ab3 100644
--- a/src/main/cpp/Intake.cpp
+++ b/src/main/cpp/Intake.cpp
@@ -47,7 +47,7 @@ void Intake::OnUpdate(units::second_t dt) {
       break;
       case IntakeState::kIntake:
       {
-        //_config.IntakeMotor.motorController->SetVoltage(5_V);
+        _config.IntakeMotor.motorController->SetVoltage(5_V);
         _stringStateName = "Intake";
         _setVoltage = 5_V;
       } 
diff --git a/src/main/cpp/IntakeBehaviour.cpp b/src/main/cpp/IntakeBehaviour.cpp
index ec18d5a1..cdaff469 100644
--- a/src/main/cpp/IntakeBehaviour.cpp
+++ b/src/main/cpp/IntakeBehaviour.cpp
@@ -28,7 +28,6 @@ void IntakeManualControl::OnTick(units::second_t dt) {
   };
 };
 
-
 IntakeAutoControl::IntakeAutoControl(Intake *intake) : _intake(intake) {};
 
 void IntakeAutoControl::OnTick(units::second_t dt) {
diff --git a/src/main/include/Intake.h b/src/main/include/Intake.h
index a9571136..2b788ba3 100644
--- a/src/main/include/Intake.h
+++ b/src/main/include/Intake.h
@@ -2,6 +2,8 @@
 
 #include "wombat.h"
 #include <frc/DigitalInput.h>
+#include <string>
+#include <memory>
 
 struct IntakeConfig {
   wom::Gearbox IntakeMotor;
@@ -21,7 +23,7 @@ enum class IntakeState {
 
 class Intake : public behaviour::HasBehaviour {
  public:
-  Intake(IntakeConfig config);
+  explicit Intake(IntakeConfig config);
   
   void OnUpdate(units::second_t dt);
   
@@ -37,4 +39,4 @@ class Intake : public behaviour::HasBehaviour {
   std::string _stringStateName = "error";
   units::volt_t _setVoltage = 0_V;
   std::shared_ptr<nt::NetworkTable> _table = nt::NetworkTableInstance::GetDefault().GetTable("Intake");
-}; 
+};
\ No newline at end of file
diff --git a/src/main/include/IntakeBehaviour.h b/src/main/include/IntakeBehaviour.h
index 3f2e9ad3..c432e987 100644
--- a/src/main/include/IntakeBehaviour.h
+++ b/src/main/include/IntakeBehaviour.h
@@ -6,7 +6,7 @@
 
 class IntakeManualControl : public behaviour::Behaviour {
  public:
-  IntakeManualControl(Intake *intake, frc::XboxController &codriver);
+  explicit IntakeManualControl(Intake *intake, frc::XboxController &codriver);
 
   void OnTick(units::second_t dt) override;
  private:
@@ -19,7 +19,7 @@ class IntakeManualControl : public behaviour::Behaviour {
 
 class IntakeAutoControl : public behaviour::Behaviour {
  public:
-  IntakeAutoControl(Intake *intake);
+  explicit IntakeAutoControl(Intake *intake);
 
   void OnTick(units::second_t dt) override;
  private:

From 5f9d9f07823f89262d8a86f14dc81102f397f442 Mon Sep 17 00:00:00 2001
From: Sean Chan <sean.chan3@student.education.wa.edu.au>
Date: Sun, 21 Jan 2024 11:20:19 +0800
Subject: [PATCH 032/207] Fixed more build errors

---
 src/main/include/behaviours/MagBehaviour.h | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/main/include/behaviours/MagBehaviour.h b/src/main/include/behaviours/MagBehaviour.h
index df8c0e91..1e312686 100644
--- a/src/main/include/behaviours/MagBehaviour.h
+++ b/src/main/include/behaviours/MagBehaviour.h
@@ -6,7 +6,7 @@
  
 class MagManualControl : public behaviour::Behaviour {
  public:
-  MagManualControl(Mag *mag, frc::XboxController *codriver);
+  explicit MagManualControl(Mag *mag, frc::XboxController *codriver);
    
   void OnTick(units::second_t dt) override;
  private:
@@ -17,7 +17,7 @@ class MagManualControl : public behaviour::Behaviour {
 
 class MagAutoPass : public behaviour::Behaviour {
  public:
-  MagAutoPass(Mag *mag);
+  explicit MagAutoPass(Mag *mag);
    
   void OnTick(units::second_t dt) override;
  private:

From 6d66b24984fa517ca7582a2bb5ed9f27e1a2c2a2 Mon Sep 17 00:00:00 2001
From: Kingsley Wong <kingsley.wong@student.education.wa.edu.au>
Date: Sun, 21 Jan 2024 11:26:08 +0800
Subject: [PATCH 033/207] finished wpiformat errors 2

---
 src/main/cpp/IntakeBehaviour.cpp | 8 ++++----
 1 file changed, 4 insertions(+), 4 deletions(-)

diff --git a/src/main/cpp/IntakeBehaviour.cpp b/src/main/cpp/IntakeBehaviour.cpp
index cdaff469..2e3dd932 100644
--- a/src/main/cpp/IntakeBehaviour.cpp
+++ b/src/main/cpp/IntakeBehaviour.cpp
@@ -3,7 +3,7 @@
 
 IntakeManualControl::IntakeManualControl(Intake *intake, frc::XboxController &codriver) : _intake(intake), _codriver(codriver) {
   Controls(intake);
-};
+}
 
 void IntakeManualControl::OnTick(units::second_t dt) {
 
@@ -25,8 +25,8 @@ void IntakeManualControl::OnTick(units::second_t dt) {
     if (_codriver.GetAButtonPressed()) {
       _intake->setState(IntakeState::kPass);
     }
-  };
-};
+  }
+}
 
 IntakeAutoControl::IntakeAutoControl(Intake *intake) : _intake(intake) {};
 
@@ -35,5 +35,5 @@ void IntakeAutoControl::OnTick(units::second_t dt) {
     _intake->setState(IntakeState::kPass);
   } else if (_intake->GetConfig().magSensor->Get() == 0) {
     _intake->setState(IntakeState::kIdle);
-  };
+  }
 }
\ No newline at end of file

From 535f7c59661af816b383a182518bdb4dfbfe839b Mon Sep 17 00:00:00 2001
From: Kingsley Wong <kingsley.wong@student.education.wa.edu.au>
Date: Sun, 21 Jan 2024 11:30:23 +0800
Subject: [PATCH 034/207] finished wpiformat errors 3

---
 src/main/cpp/IntakeBehaviour.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/cpp/IntakeBehaviour.cpp b/src/main/cpp/IntakeBehaviour.cpp
index 2e3dd932..1642ef22 100644
--- a/src/main/cpp/IntakeBehaviour.cpp
+++ b/src/main/cpp/IntakeBehaviour.cpp
@@ -28,7 +28,7 @@ void IntakeManualControl::OnTick(units::second_t dt) {
   }
 }
 
-IntakeAutoControl::IntakeAutoControl(Intake *intake) : _intake(intake) {};
+IntakeAutoControl::IntakeAutoControl(Intake *intake) : _intake(intake) {}
 
 void IntakeAutoControl::OnTick(units::second_t dt) {
   if (_intake->GetConfig().intakeSensor->Get() == 1) {

From 22736c595975fb8e6a1f4ee99931b2064473ac93 Mon Sep 17 00:00:00 2001
From: Sean Chan <sean.chan3@student.education.wa.edu.au>
Date: Sun, 21 Jan 2024 11:33:34 +0800
Subject: [PATCH 035/207] Fixed more build errors

---
 src/main/cpp/Climber.cpp                       | 2 +-
 src/main/cpp/Robot.cpp                         | 5 +----
 src/main/include/Behaviours/ClimberBehaviour.h | 7 +++----
 src/main/include/Climber.h                     | 2 +-
 4 files changed, 6 insertions(+), 10 deletions(-)

diff --git a/src/main/cpp/Climber.cpp b/src/main/cpp/Climber.cpp
index 50da705c..c66f58a8 100644
--- a/src/main/cpp/Climber.cpp
+++ b/src/main/cpp/Climber.cpp
@@ -48,4 +48,4 @@ void Climber::SetState(ClimberState state) {
 
 void Climber::SetRaw(units::volt_t voltage) {
   _rawVoltage = voltage;
-}
+}
\ No newline at end of file
diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp
index 9ee8050a..38ae6eba 100644
--- a/src/main/cpp/Robot.cpp
+++ b/src/main/cpp/Robot.cpp
@@ -97,7 +97,4 @@ void Robot::TestPeriodic() {}
 
 void Robot::SimulationInit() {}
 
-void Robot::SimulationPeriodic() {}
-
-
-
+void Robot::SimulationPeriodic() {}
\ No newline at end of file
diff --git a/src/main/include/Behaviours/ClimberBehaviour.h b/src/main/include/Behaviours/ClimberBehaviour.h
index f1611ddf..8435b5ef 100644
--- a/src/main/include/Behaviours/ClimberBehaviour.h
+++ b/src/main/include/Behaviours/ClimberBehaviour.h
@@ -1,7 +1,7 @@
 #pragma once
 
-#include "wombat.h"
-#include "climber.h"
+#include "Wombat.h"
+#include "Climber.h"
 #include <frc/XboxController.h>
 
 class ClimberManualControl : public behaviour::Behaviour {
@@ -12,5 +12,4 @@ class ClimberManualControl : public behaviour::Behaviour {
     Climber *_climber;
     frc::XboxController *_codriver;
     bool _rawControl = false;
-};
-
+};
\ No newline at end of file
diff --git a/src/main/include/Climber.h b/src/main/include/Climber.h
index f22db2cb..738896a9 100644
--- a/src/main/include/Climber.h
+++ b/src/main/include/Climber.h
@@ -1,6 +1,6 @@
 #pragma once
 
-#include "wombat.h"
+#include "Wombat.h"
 #include <string>
 #include <memory>
 

From b80c7606731ead3440db0513e4d809f61cf88652 Mon Sep 17 00:00:00 2001
From: Kingsley Wong <kingsley.wong@student.education.wa.edu.au>
Date: Sun, 21 Jan 2024 11:34:16 +0800
Subject: [PATCH 036/207] finished wpiformat errors 4

---
 src/main/cpp/Robot.cpp   | 2 +-
 src/main/include/Robot.h | 2 +-
 2 files changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp
index b906ece6..16d85cff 100644
--- a/src/main/cpp/Robot.cpp
+++ b/src/main/cpp/Robot.cpp
@@ -95,4 +95,4 @@ void Robot::TestPeriodic() {}
 
 void Robot::SimulationInit() {}
 
-void Robot::SimulationPeriodic() {}
+void Robot::SimulationPeriodic() {}
\ No newline at end of file
diff --git a/src/main/include/Robot.h b/src/main/include/Robot.h
index 87fe1181..90b4e3ba 100644
--- a/src/main/include/Robot.h
+++ b/src/main/include/Robot.h
@@ -55,4 +55,4 @@ class Robot : public frc::TimedRobot {
   wom::SwerveDrive* _swerveDrive;
 
   Intake *intake;
-};
+};
\ No newline at end of file

From ac6ce9b762c99f3b3e3e37ae3b0d3b0f89f157c9 Mon Sep 17 00:00:00 2001
From: Isaac Turner <spacey_sooty@outlook.com>
Date: Sun, 21 Jan 2024 11:42:28 +0800
Subject: [PATCH 037/207] ran wpiformat

---
 src/main/cpp/Mag.cpp                       | 41 ++++++++----------
 src/main/cpp/Robot.cpp                     |  9 ++--
 src/main/cpp/behaviours/MagBehaviour.cpp   | 34 +++++++--------
 src/main/include/Mag.h                     | 48 +++++++++++-----------
 src/main/include/Robot.h                   | 10 ++---
 src/main/include/RobotMap.h                | 38 ++++++++---------
 src/main/include/behaviours/MagBehaviour.h | 38 ++++++++++-------
 7 files changed, 106 insertions(+), 112 deletions(-)

diff --git a/src/main/cpp/Mag.cpp b/src/main/cpp/Mag.cpp
index 2b279651..1bde78f0 100644
--- a/src/main/cpp/Mag.cpp
+++ b/src/main/cpp/Mag.cpp
@@ -1,60 +1,56 @@
+// Copyright (c) 2023-2024 CurtinFRC
+// Open Source Software, you can modify it according to the terms
+// of the MIT License at the root of this project
+
 #include "Mag.h"
 
 Mag::Mag(MagConfig config) : _config(config) {}
 
 void Mag::OnUpdate(units::second_t dt) {
   switch (_state) {
-    case MagState::kIdle: 
-    {
+    case MagState::kIdle: {
       if (_config.intakeSensor->Get()) {
         SetState(MagState::kHold);
-      } else if (_config.intakeSensor->Get()){
+      } else if (_config.intakeSensor->Get()) {
         SetState(MagState::kHold);
       }
       _setVoltage = 0_V;
       _stringStateName = "Idle";
-    }
-    break;
+    } break;
 
-    case MagState::kHold:
-    {
+    case MagState::kHold: {
       if (_config.magSensor->Get() == 0) {
         SetState(MagState::kIdle);
       }
       _setVoltage = 0_V;
       _stringStateName = "Hold";
-    }
-    break;
+    } break;
 
-    case MagState::kEject:
-    {
-      if (_config.magSensor->Get() == 0 && _config.intakeSensor->Get() ==  0) {
+    case MagState::kEject: {
+      if (_config.magSensor->Get() == 0 && _config.intakeSensor->Get() == 0) {
         SetState(MagState::kIdle);
       }
       _setVoltage = -5_V;
       _stringStateName = "Eject";
-    }
-    break;
+    } break;
 
     case MagState::kRaw:
       _setVoltage = _rawVoltage;
       _stringStateName = "Raw";
-    break;
+      break;
 
-    case MagState::kPass:
-    {
+    case MagState::kPass: {
       if (_config.shooterSensor->Get()) {
         SetState(MagState::kIdle);
       } else {
         _setVoltage = 5_V;
         _stringStateName = "Pass";
       }
-    }
-    break;
+    } break;
 
     default:
       std::cout << "Error magazine in invalid state" << std::endl;
-    break;
+      break;
   }
   _config.magGearbox.motorController->SetVoltage(_setVoltage);
   _table->GetEntry("State: ").SetString(_stringStateName);
@@ -64,7 +60,6 @@ void Mag::OnUpdate(units::second_t dt) {
   _table->GetEntry("Magazine Sensor: ").SetDouble(_config.magSensor->Get());
 }
 
-
 void Mag::SetState(MagState state) {
   _state = state;
 }
@@ -75,7 +70,3 @@ void Mag::SetRaw(units::volt_t voltage) {
 MagState Mag::GetState() {
   return _state;
 }
-
-
-
-
diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp
index 6a3dae5b..526b3a72 100644
--- a/src/main/cpp/Robot.cpp
+++ b/src/main/cpp/Robot.cpp
@@ -23,15 +23,14 @@ void Robot::RobotInit() {
 
   mag = new Mag(robotmap.magSystem.config);
   wom::BehaviourScheduler::GetInstance()->Register(mag);
-  mag->SetDefaultBehaviour([this]() {
-    return wom::make<MagManualControl>(mag, &robotmap.controllers.coDriver);
-  });
+  mag->SetDefaultBehaviour(
+      [this]() { return wom::make<MagManualControl>(mag, &robotmap.controllers.coDriver); });
 }
 
 void Robot::RobotPeriodic() {
   auto dt = wom::now() - lastPeriodic;
   lastPeriodic = wom::now();
-  
+
   loop.Poll();
   wom::BehaviourScheduler::GetInstance()->Tick();
 
@@ -41,7 +40,7 @@ void Robot::RobotPeriodic() {
 
 void Robot::TeleopInit() {
   loop.Clear();
-  wom::BehaviourScheduler *scheduler = wom::BehaviourScheduler::GetInstance();
+  wom::BehaviourScheduler* scheduler = wom::BehaviourScheduler::GetInstance();
   scheduler->InterruptAll();
 
   m_chooser.SetDefaultOption("Default Auto", "Default Auto");
diff --git a/src/main/cpp/behaviours/MagBehaviour.cpp b/src/main/cpp/behaviours/MagBehaviour.cpp
index 9f568ee5..8977eb4b 100644
--- a/src/main/cpp/behaviours/MagBehaviour.cpp
+++ b/src/main/cpp/behaviours/MagBehaviour.cpp
@@ -1,11 +1,14 @@
+// Copyright (c) 2023-2024 CurtinFRC
+// Open Source Software, you can modify it according to the terms
+// of the MIT License at the root of this project
+
 #include "behaviours/MagBehaviour.h"
 
-MagManualControl::MagManualControl(Mag *mag, frc::XboxController *codriver) : _mag(mag), _codriver(codriver) {
+MagManualControl::MagManualControl(Mag* mag, frc::XboxController* codriver) : _mag(mag), _codriver(codriver) {
   Controls(mag);
 }
 
 void MagManualControl::OnTick(units::second_t dt) {
-
   if (_codriver->GetAButtonPressed()) {
     if (_rawControl == true) {
       _rawControl = false;
@@ -13,7 +16,7 @@ void MagManualControl::OnTick(units::second_t dt) {
       _rawControl = true;
     }
   }
-  
+
   if (_rawControl) {
     // Manual control, right bumper for manual override.
     if (_codriver->GetLeftBumper()) {
@@ -26,24 +29,23 @@ void MagManualControl::OnTick(units::second_t dt) {
     } else {
       _mag->SetRaw(0_V);
     }
-    
+
   } else {
     _mag->SetState(MagState::kIdle);
     if (_codriver->GetLeftBumper()) {
       _mag->SetState(MagState::kPass);
-      
     }
   }
 }
-    
-    MagAutoPass::MagAutoPass(Mag *mag) {}
-    
-    void MagAutoPass::OnTick(units::second_t dt) {  
-      _mag->SetState(MagState::kPass);
-    }
 
-    MagAutoHold::MagAutoHold(Mag *mag) {}
-    
-    void MagAutoHold::OnTick(units::second_t dt) {  
-      _mag->SetState(MagState::kHold);
-    }
+MagAutoPass::MagAutoPass(Mag* mag) {}
+
+void MagAutoPass::OnTick(units::second_t dt) {
+  _mag->SetState(MagState::kPass);
+}
+
+MagAutoHold::MagAutoHold(Mag* mag) {}
+
+void MagAutoHold::OnTick(units::second_t dt) {
+  _mag->SetState(MagState::kHold);
+}
diff --git a/src/main/include/Mag.h b/src/main/include/Mag.h
index fa971106..9f66971b 100644
--- a/src/main/include/Mag.h
+++ b/src/main/include/Mag.h
@@ -1,9 +1,14 @@
+// Copyright (c) 2023-2024 CurtinFRC
+// Open Source Software, you can modify it according to the terms
+// of the MIT License at the root of this project
+
 #pragma once
-#include "wombat.h"
 #include <frc/DigitalInput.h>
-#include <string>
+
 #include <memory>
+#include <string>
 
+#include "wombat.h"
 
 struct MagConfig {
   wom::Gearbox magGearbox;
@@ -12,27 +17,22 @@ struct MagConfig {
   frc::DigitalInput* shooterSensor;
 };
 
-enum class MagState {
-  kIdle,
-  kHold,
-  kEject,
-  kRaw,
-  kPass
-};
+enum class MagState { kIdle, kHold, kEject, kRaw, kPass };
 
 class Mag : public behaviour::HasBehaviour {
-  public:
-    explicit Mag(MagConfig config);
-
-    void OnUpdate(units::second_t dt);
-    void SetState(MagState state);
-    void SetRaw(units::volt_t voltage);
-    MagState GetState();
-  private:
-    MagConfig _config;
-    MagState _state;
-    units::volt_t _rawVoltage = 0_V;
-    std::string _stringStateName = "No State";
-    units::volt_t _setVoltage = 0_V;
-    std::shared_ptr<nt::NetworkTable> _table = nt::NetworkTableInstance::GetDefault().GetTable("Magazine");
-};
\ No newline at end of file
+ public:
+  explicit Mag(MagConfig config);
+
+  void OnUpdate(units::second_t dt);
+  void SetState(MagState state);
+  void SetRaw(units::volt_t voltage);
+  MagState GetState();
+
+ private:
+  MagConfig _config;
+  MagState _state;
+  units::volt_t _rawVoltage = 0_V;
+  std::string _stringStateName = "No State";
+  units::volt_t _setVoltage = 0_V;
+  std::shared_ptr<nt::NetworkTable> _table = nt::NetworkTableInstance::GetDefault().GetTable("Magazine");
+};
diff --git a/src/main/include/Robot.h b/src/main/include/Robot.h
index ff3f485e..fea7e9ec 100644
--- a/src/main/include/Robot.h
+++ b/src/main/include/Robot.h
@@ -4,12 +4,11 @@
 
 #pragma once
 
-#include "behaviours/MagBehaviour.h"
-#include <frc/TimedRobot.h>
-#include <frc/event/Eventloop.h>
 #include <frc/Encoder.h>
+#include <frc/TimedRobot.h>
 #include <frc/Timer.h>
 #include <frc/event/EventLoop.h>
+#include <frc/event/Eventloop.h>
 #include <frc/simulation/DifferentialDrivetrainSim.h>
 #include <frc/simulation/EncoderSim.h>
 #include <frc/smartdashboard/Field2d.h>
@@ -20,6 +19,7 @@
 
 #include "RobotMap.h"
 #include "Wombat.h"
+#include "behaviours/MagBehaviour.h"
 
 class Robot : public frc::TimedRobot {
  public:
@@ -38,7 +38,7 @@ class Robot : public frc::TimedRobot {
 
  private:
   behaviour::BehaviourScheduler* sched;
-  
+
   RobotMap robotmap;
 
   frc::EventLoop loop;
@@ -48,5 +48,5 @@ class Robot : public frc::TimedRobot {
   frc::SendableChooser<std::string> m_path_chooser;
 
   wom::SwerveDrive* _swerveDrive;
-  Mag *mag;
+  Mag* mag;
 };
diff --git a/src/main/include/RobotMap.h b/src/main/include/RobotMap.h
index 9f4523b9..71618244 100644
--- a/src/main/include/RobotMap.h
+++ b/src/main/include/RobotMap.h
@@ -4,14 +4,10 @@
 
 #pragma once
 
-#include "mag.h"
-#include "Wombat.h"
-
-#include <frc/XboxController.h>
-#include <frc/DigitalInput.h>
-
 #include <frc/Compressor.h>
+#include <frc/DigitalInput.h>
 #include <frc/DoubleSolenoid.h>
+#include <frc/XboxController.h>
 #include <frc/system/plant/DCMotor.h>
 #include <units/angle.h>
 #include <units/length.h>
@@ -22,6 +18,9 @@
 #include <ctre/phoenix6/Pigeon2.hpp>
 #include <ctre/phoenix6/TalonFX.hpp>
 
+#include "Wombat.h"
+#include "mag.h"
+
 struct RobotMap {
   struct Controllers {
     frc::XboxController driver = frc::XboxController(0);
@@ -29,7 +28,7 @@ struct RobotMap {
     frc::XboxController testController = frc::XboxController(2);
   };
   Controllers controllers;
-  
+
   struct SwerveBase {
     ctre::phoenix6::hardware::CANcoder frontLeftCancoder{19};
     ctre::phoenix6::hardware::CANcoder frontRightCancoder{17};
@@ -134,26 +133,21 @@ struct RobotMap {
   SwerveBase swerveBase;
 
   struct Mag {
-    rev::CANSparkMax *magMotor = new rev::CANSparkMax{99, rev::CANSparkMax::MotorType::kBrushless};
-    //wom::VoltageController magMotorGroup = wom::VoltageController::Group(magMotor);
+    rev::CANSparkMax* magMotor = new rev::CANSparkMax{99, rev::CANSparkMax::MotorType::kBrushless};
+    // wom::VoltageController magMotorGroup = wom::VoltageController::Group(magMotor);
     wom::CANSparkMaxEncoder magEncoder{magMotor, 0.1_m};
     frc::DigitalInput intakeSensor{0};
     frc::DigitalInput magSensor{1};
     frc::DigitalInput shooterSensor{1};
 
-    wom::Gearbox magGearbox {
-      magMotor,
-      &magEncoder,
-      frc::DCMotor::NEO(1)
-    };
+    wom::Gearbox magGearbox{magMotor, &magEncoder, frc::DCMotor::NEO(1)};
 
-    MagConfig config {
-       magGearbox,
-       &intakeSensor,
-       &magSensor,
-       &shooterSensor,
-     };
-  }; 
+    MagConfig config{
+        magGearbox,
+        &intakeSensor,
+        &magSensor,
+        &shooterSensor,
+    };
+  };
   Mag magSystem;
-
 };
diff --git a/src/main/include/behaviours/MagBehaviour.h b/src/main/include/behaviours/MagBehaviour.h
index 1e312686..7f0b29a6 100644
--- a/src/main/include/behaviours/MagBehaviour.h
+++ b/src/main/include/behaviours/MagBehaviour.h
@@ -1,34 +1,42 @@
+// Copyright (c) 2023-2024 CurtinFRC
+// Open Source Software, you can modify it according to the terms
+// of the MIT License at the root of this project
+
 #pragma once
- 
-#include "wombat.h"
-#include "mag.h"
+
 #include <frc/XboxController.h>
- 
+
+#include "mag.h"
+#include "wombat.h"
+
 class MagManualControl : public behaviour::Behaviour {
  public:
-  explicit MagManualControl(Mag *mag, frc::XboxController *codriver);
-   
+  explicit MagManualControl(Mag* mag, frc::XboxController* codriver);
+
   void OnTick(units::second_t dt) override;
+
  private:
-  Mag *_mag;
-  frc::XboxController *_codriver;
-  bool _rawControl; // Default of Bool is False.
+  Mag* _mag;
+  frc::XboxController* _codriver;
+  bool _rawControl;  // Default of Bool is False.
 };
 
 class MagAutoPass : public behaviour::Behaviour {
  public:
-  explicit MagAutoPass(Mag *mag);
-   
+  explicit MagAutoPass(Mag* mag);
+
   void OnTick(units::second_t dt) override;
+
  private:
-  Mag *_mag;
+  Mag* _mag;
 };
 
 class MagAutoHold : public behaviour::Behaviour {
  public:
-  explicit MagAutoHold(Mag *mag);
-   
+  explicit MagAutoHold(Mag* mag);
+
   void OnTick(units::second_t dt) override;
+
  private:
-  Mag *_mag;
+  Mag* _mag;
 };

From 79a0eeba33eeddfb5ab58c32ecf1ea695ffaaf82 Mon Sep 17 00:00:00 2001
From: Isaac Turner <spacey_sooty@outlook.com>
Date: Sun, 21 Jan 2024 11:53:58 +0800
Subject: [PATCH 038/207] ran wpiformat

---
 src/main/cpp/Intake.cpp            | 94 +++++++++++++-----------------
 src/main/cpp/IntakeBehaviour.cpp   | 15 +++--
 src/main/cpp/Main.cpp              |  2 +-
 src/main/cpp/Robot.cpp             | 39 +++++--------
 src/main/include/Intake.h          | 25 ++++----
 src/main/include/IntakeBehaviour.h | 23 +++++---
 src/main/include/Robot.h           | 11 ++--
 src/main/include/RobotMap.h        | 42 ++++---------
 8 files changed, 111 insertions(+), 140 deletions(-)

diff --git a/src/main/cpp/Intake.cpp b/src/main/cpp/Intake.cpp
index 43361ab3..7025fdc8 100644
--- a/src/main/cpp/Intake.cpp
+++ b/src/main/cpp/Intake.cpp
@@ -1,69 +1,59 @@
-#include "Intake.h"
+// Copyright (c) 2023-2024 CurtinFRC
+// Open Source Software, you can modify it according to the terms
+// of the MIT License at the root of this project
 
-Intake::Intake(IntakeConfig config) : _config(config) {
+#include "Intake.h"
 
-}
+Intake::Intake(IntakeConfig config) : _config(config) {}
 
 IntakeConfig Intake::GetConfig() {
   return _config;
 }
 
 void Intake::OnUpdate(units::second_t dt) {
-
   switch (_state) {
-      case IntakeState::kIdle:
-      {
-        _config.IntakeMotor.motorController->SetVoltage(0_V);
-        if (_config.intakeSensor->Get()) {
-          setState(IntakeState::kHold);
-        }
-        _stringStateName = "Idle";
-        _setVoltage = 0_V;
+    case IntakeState::kIdle: {
+      _config.IntakeMotor.motorController->SetVoltage(0_V);
+      if (_config.intakeSensor->Get()) {
+        setState(IntakeState::kHold);
       }
-      break;
-      case IntakeState::kRaw:
-      {
-        _config.IntakeMotor.motorController->SetVoltage(_rawVoltage);
-        _stringStateName = "Raw";
-        _setVoltage = _rawVoltage;
-      }
-      break;
-      case IntakeState::kEject:
-      {
-        _config.IntakeMotor.motorController->SetVoltage(-5_V);
-        if (_config.intakeSensor->Get() == 0 && _config.magSensor->Get() == 0) {
-          setState(IntakeState::kIdle);
-        }
-        _stringStateName = "Eject";
-        _setVoltage = -5_V;
+      _stringStateName = "Idle";
+      _setVoltage = 0_V;
+    } break;
+    case IntakeState::kRaw: {
+      _config.IntakeMotor.motorController->SetVoltage(_rawVoltage);
+      _stringStateName = "Raw";
+      _setVoltage = _rawVoltage;
+    } break;
+    case IntakeState::kEject: {
+      _config.IntakeMotor.motorController->SetVoltage(-5_V);
+      if (_config.intakeSensor->Get() == 0 && _config.magSensor->Get() == 0) {
+        setState(IntakeState::kIdle);
       }
-      break;
-      case IntakeState::kHold:
-      {
-        _config.IntakeMotor.motorController->SetVoltage(0_V);
-        _stringStateName = "Hold";
-        _setVoltage = 0_V;
-      }
-      break;
-      case IntakeState::kIntake:
-      {
-        _config.IntakeMotor.motorController->SetVoltage(5_V);
-        _stringStateName = "Intake";
+      _stringStateName = "Eject";
+      _setVoltage = -5_V;
+    } break;
+    case IntakeState::kHold: {
+      _config.IntakeMotor.motorController->SetVoltage(0_V);
+      _stringStateName = "Hold";
+      _setVoltage = 0_V;
+    } break;
+    case IntakeState::kIntake: {
+      _config.IntakeMotor.motorController->SetVoltage(5_V);
+      _stringStateName = "Intake";
+      _setVoltage = 5_V;
+    } break;
+    case IntakeState::kPass: {
+      _config.IntakeMotor.motorController->SetVoltage(5_V);
+      if (_config.shooterSensor->Get()) {
+        setState(IntakeState::kIdle);
+        _stringStateName = "Pass";
         _setVoltage = 5_V;
-      } 
-      break;
-      case IntakeState::kPass:
-      {
-        _config.IntakeMotor.motorController->SetVoltage(5_V);
-        if (_config.shooterSensor->Get()) {
-          setState(IntakeState::kIdle);
-          _stringStateName = "Pass";
-          _setVoltage = 5_V;
       }
       break;
       default:
-        std::cout <<"Error: Intake in INVALID STATE." << std::endl;
-      break;
+        std::cout << "Error: Intake in INVALID STATE." << std::endl;
+        break;
     }
   }
   _table->GetEntry("State: ").SetString(_stringStateName);
@@ -80,4 +70,4 @@ void Intake::setState(IntakeState state) {
 }
 void Intake::setRaw(units::volt_t voltage) {
   _rawVoltage = voltage;
-}
\ No newline at end of file
+}
diff --git a/src/main/cpp/IntakeBehaviour.cpp b/src/main/cpp/IntakeBehaviour.cpp
index 1642ef22..aaf3eb64 100644
--- a/src/main/cpp/IntakeBehaviour.cpp
+++ b/src/main/cpp/IntakeBehaviour.cpp
@@ -1,14 +1,19 @@
+// Copyright (c) 2023-2024 CurtinFRC
+// Open Source Software, you can modify it according to the terms
+// of the MIT License at the root of this project
+
 #include "IntakeBehaviour.h"
+
 #include <frc/XboxController.h>
 
-IntakeManualControl::IntakeManualControl(Intake *intake, frc::XboxController &codriver) : _intake(intake), _codriver(codriver) {
+IntakeManualControl::IntakeManualControl(Intake* intake, frc::XboxController& codriver)
+    : _intake(intake), _codriver(codriver) {
   Controls(intake);
 }
 
 void IntakeManualControl::OnTick(units::second_t dt) {
-
   if (_codriver.GetBButtonPressed()) {
-    if(_rawControl == true) {
+    if (_rawControl == true) {
       _rawControl = false;
     } else {
       _rawControl = true;
@@ -28,7 +33,7 @@ void IntakeManualControl::OnTick(units::second_t dt) {
   }
 }
 
-IntakeAutoControl::IntakeAutoControl(Intake *intake) : _intake(intake) {}
+IntakeAutoControl::IntakeAutoControl(Intake* intake) : _intake(intake) {}
 
 void IntakeAutoControl::OnTick(units::second_t dt) {
   if (_intake->GetConfig().intakeSensor->Get() == 1) {
@@ -36,4 +41,4 @@ void IntakeAutoControl::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/Main.cpp b/src/main/cpp/Main.cpp
index 3bd80491..e27c9c50 100644
--- a/src/main/cpp/Main.cpp
+++ b/src/main/cpp/Main.cpp
@@ -10,4 +10,4 @@
 #include "Robot.h"
 #include "Wombat.h"
 
-WOMBAT_ROBOT_MAIN(Robot);
\ No newline at end of file
+WOMBAT_ROBOT_MAIN(Robot);
diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp
index 16d85cff..da4a6461 100644
--- a/src/main/cpp/Robot.cpp
+++ b/src/main/cpp/Robot.cpp
@@ -4,26 +4,23 @@
 
 #include "Robot.h"
 
-#include <units/velocity.h>
-#include <units/acceleration.h>
-#include <units/length.h>
-#include <units/angle.h>
-#include <units/time.h>
-#include <units/voltage.h>
-
-#include <frc/kinematics/DifferentialDriveKinematics.h>
-#include <frc/controller/RamseteController.h>
-#include <frc/Timer.h>
-
 #include <frc/TimedRobot.h>
+#include <frc/Timer.h>
+#include <frc/controller/RamseteController.h>
+#include <frc/kinematics/DifferentialDriveKinematics.h>
 #include <networktables/DoubleTopic.h>
 #include <networktables/NetworkTable.h>
 #include <networktables/NetworkTableInstance.h>
+#include <units/acceleration.h>
+#include <units/angle.h>
+#include <units/length.h>
+#include <units/time.h>
+#include <units/velocity.h>
+#include <units/voltage.h>
 
 static units::second_t lastPeriodic;
 
 void Robot::RobotInit() {
-
   lastPeriodic = wom::now();
 
   m_chooser.SetDefaultOption("Default Auto", "Default Auto");
@@ -51,10 +48,8 @@ void Robot::RobotInit() {
   intake = new Intake(robotmap.intakeSystem.config);
   wom::BehaviourScheduler::GetInstance()->Register(intake);
 
-  intake->SetDefaultBehaviour([this]() {
-    return wom::make<IntakeManualControl>(intake, robotmap.controllers.driver);
-  });
-
+  intake->SetDefaultBehaviour(
+      [this]() { return wom::make<IntakeManualControl>(intake, robotmap.controllers.driver); });
 }
 
 void Robot::RobotPeriodic() {
@@ -69,23 +64,19 @@ void Robot::RobotPeriodic() {
 }
 
 void Robot::AutonomousInit() {
-
   loop.Clear();
   sched->InterruptAll();
 }
 
-void Robot::AutonomousPeriodic() {
-}
+void Robot::AutonomousPeriodic() {}
 
 void Robot::TeleopInit() {
   loop.Clear();
-  wom::BehaviourScheduler *scheduler = wom::BehaviourScheduler::GetInstance();
+  wom::BehaviourScheduler* scheduler = wom::BehaviourScheduler::GetInstance();
   scheduler->InterruptAll();
 }
 
-void Robot::TeleopPeriodic() {
-  
-}
+void Robot::TeleopPeriodic() {}
 
 void Robot::DisabledInit() {}
 void Robot::DisabledPeriodic() {}
@@ -95,4 +86,4 @@ void Robot::TestPeriodic() {}
 
 void Robot::SimulationInit() {}
 
-void Robot::SimulationPeriodic() {}
\ No newline at end of file
+void Robot::SimulationPeriodic() {}
diff --git a/src/main/include/Intake.h b/src/main/include/Intake.h
index 2b788ba3..68703474 100644
--- a/src/main/include/Intake.h
+++ b/src/main/include/Intake.h
@@ -1,9 +1,15 @@
+// Copyright (c) 2023-2024 CurtinFRC
+// Open Source Software, you can modify it according to the terms
+// of the MIT License at the root of this project
+
 #pragma once
 
-#include "wombat.h"
 #include <frc/DigitalInput.h>
-#include <string>
+
 #include <memory>
+#include <string>
+
+#include "wombat.h"
 
 struct IntakeConfig {
   wom::Gearbox IntakeMotor;
@@ -12,21 +18,14 @@ struct IntakeConfig {
   frc::DigitalInput* shooterSensor;
 };
 
-enum class IntakeState {
-  kIdle,
-  kRaw,
-  kHold,
-  kEject,
-  kIntake,
-  kPass
-};
+enum class IntakeState { kIdle, kRaw, kHold, kEject, kIntake, kPass };
 
 class Intake : public behaviour::HasBehaviour {
  public:
   explicit Intake(IntakeConfig config);
-  
+
   void OnUpdate(units::second_t dt);
-  
+
   void setState(IntakeState state);
   void setRaw(units::volt_t voltage);
   IntakeConfig GetConfig();
@@ -39,4 +38,4 @@ class Intake : public behaviour::HasBehaviour {
   std::string _stringStateName = "error";
   units::volt_t _setVoltage = 0_V;
   std::shared_ptr<nt::NetworkTable> _table = nt::NetworkTableInstance::GetDefault().GetTable("Intake");
-};
\ No newline at end of file
+};
diff --git a/src/main/include/IntakeBehaviour.h b/src/main/include/IntakeBehaviour.h
index c432e987..c84c6d32 100644
--- a/src/main/include/IntakeBehaviour.h
+++ b/src/main/include/IntakeBehaviour.h
@@ -1,27 +1,34 @@
+// Copyright (c) 2023-2024 CurtinFRC
+// Open Source Software, you can modify it according to the terms
+// of the MIT License at the root of this project
+
 #pragma once
 
-#include "Wombat.h"
 #include <frc/XboxController.h>
+
 #include "Intake.h"
+#include "Wombat.h"
 
 class IntakeManualControl : public behaviour::Behaviour {
  public:
-  explicit IntakeManualControl(Intake *intake, frc::XboxController &codriver);
+  explicit IntakeManualControl(Intake* intake, frc::XboxController& codriver);
 
   void OnTick(units::second_t dt) override;
+
  private:
-  Intake *_intake;
-  frc::XboxController &_codriver;
+  Intake* _intake;
+  frc::XboxController& _codriver;
 
   units::volt_t _rawVoltage;
   bool _rawControl;
-}; 
+};
 
 class IntakeAutoControl : public behaviour::Behaviour {
  public:
-  explicit IntakeAutoControl(Intake *intake);
+  explicit IntakeAutoControl(Intake* intake);
 
   void OnTick(units::second_t dt) override;
+
  private:
-  Intake *_intake;
-}; 
\ No newline at end of file
+  Intake* _intake;
+};
diff --git a/src/main/include/Robot.h b/src/main/include/Robot.h
index 90b4e3ba..fa8ae00a 100644
--- a/src/main/include/Robot.h
+++ b/src/main/include/Robot.h
@@ -5,11 +5,6 @@
 #pragma once
 
 #include <frc/Encoder.h>
-
-#include "Intake.h"
-#include "IntakeBehaviour.h"
-#include "Wombat.h"
-#include "RobotMap.h"
 #include <frc/TimedRobot.h>
 #include <frc/Timer.h>
 #include <frc/event/EventLoop.h>
@@ -21,6 +16,8 @@
 
 #include <string>
 
+#include "Intake.h"
+#include "IntakeBehaviour.h"
 #include "RobotMap.h"
 #include "Wombat.h"
 
@@ -54,5 +51,5 @@ class Robot : public frc::TimedRobot {
 
   wom::SwerveDrive* _swerveDrive;
 
-  Intake *intake;
-};
\ No newline at end of file
+  Intake* intake;
+};
diff --git a/src/main/include/RobotMap.h b/src/main/include/RobotMap.h
index 47245b08..bccc0fb2 100644
--- a/src/main/include/RobotMap.h
+++ b/src/main/include/RobotMap.h
@@ -17,38 +17,28 @@
 #include <ctre/phoenix6/Pigeon2.hpp>
 #include <ctre/phoenix6/TalonFX.hpp>
 
-#include "Wombat.h"
 #include "Intake.h"
 #include "IntakeBehaviour.h"
+#include "Wombat.h"
 
 struct RobotMap {
-
   struct Controllers {
     frc::XboxController driver = frc::XboxController(0);
     frc::XboxController codriver = frc::XboxController(1);
   };
 
   struct IntakeSystem {
-
     rev::CANSparkMax intakeMotor{99, rev::CANSparkMax::MotorType::kBrushless};
     wom::CANSparkMaxEncoder intakeEncoder{&intakeMotor, 0.1_m};
-    frc::DigitalInput intakeSensor {0};
-    frc::DigitalInput magSensor {0};
-    frc::DigitalInput shooterSensor {0};
+    frc::DigitalInput intakeSensor{0};
+    frc::DigitalInput magSensor{0};
+    frc::DigitalInput shooterSensor{0};
 
-    wom::Gearbox IntakeGearbox { 
-      &intakeMotor,
-      &intakeEncoder,
-      frc::DCMotor::NEO(1)
-    };
+    wom::Gearbox IntakeGearbox{&intakeMotor, &intakeEncoder, frc::DCMotor::NEO(1)};
 
-    IntakeConfig config {
-      IntakeGearbox,
-      &intakeSensor,
-      &magSensor,
-      &shooterSensor
-    };
-  }; IntakeSystem intakeSystem;
+    IntakeConfig config{IntakeGearbox, &intakeSensor, &magSensor, &shooterSensor};
+  };
+  IntakeSystem intakeSystem;
 
   Controllers controllers;
 
@@ -123,17 +113,9 @@ struct RobotMap {
         10_cm / 1_s,
         10_cm};
 
-    wom::SwerveDriveConfig config{"/drivetrain",
-                                  anglePID,
-                                  velocityPID,
-                                  moduleConfigs, 
-                                  gyro,
-                                  poseAnglePID,
-                                  posePositionPID,
-                                  60_kg, 
-                                  {0.1, 0.1, 0.1},
-                                  {0.9, 0.9, 0.9}};
-
+    wom::SwerveDriveConfig config{
+        "/drivetrain", anglePID,        velocityPID, moduleConfigs,   gyro,
+        poseAnglePID,  posePositionPID, 60_kg,       {0.1, 0.1, 0.1}, {0.9, 0.9, 0.9}};
   };
   SwerveBase swerveBase;
-};
\ No newline at end of file
+};

From 621efce2b94bb70fa070d2a954981f3cdc6b5a1d Mon Sep 17 00:00:00 2001
From: Isaac Turner <spacey_sooty@outlook.com>
Date: Sun, 21 Jan 2024 11:56:30 +0800
Subject: [PATCH 039/207] ran wpiformat

---
 src/main/cpp/Behaviours/ClimberBehaviour.cpp  | 13 ++++--
 src/main/cpp/Climber.cpp                      | 35 ++++++--------
 src/main/cpp/Main.cpp                         |  2 +-
 src/main/cpp/Robot.cpp                        |  8 ++--
 .../include/Behaviours/ClimberBehaviour.h     | 27 +++++++----
 src/main/include/Climber.h                    | 46 ++++++++++---------
 src/main/include/Robot.h                      | 10 +---
 src/main/include/RobotMap.h                   | 24 ++++------
 8 files changed, 81 insertions(+), 84 deletions(-)

diff --git a/src/main/cpp/Behaviours/ClimberBehaviour.cpp b/src/main/cpp/Behaviours/ClimberBehaviour.cpp
index 0befa293..c857263c 100644
--- a/src/main/cpp/Behaviours/ClimberBehaviour.cpp
+++ b/src/main/cpp/Behaviours/ClimberBehaviour.cpp
@@ -1,11 +1,16 @@
+// Copyright (c) 2023-2024 CurtinFRC
+// Open Source Software, you can modify it according to the terms
+// of the MIT License at the root of this project
+
 #include "behaviours/ClimberBehaviour.h"
 
-ClimberManualControl::ClimberManualControl(Climber *climber, frc::XboxController *codriver) : _climber(climber), _codriver(codriver) {
-  Controls(climber); 
+ClimberManualControl::ClimberManualControl(Climber* climber,
+                                           frc::XboxController* codriver)
+    : _climber(climber), _codriver(codriver) {
+  Controls(climber);
 }
 
 void ClimberManualControl::OnTick(units::second_t dt) {
-
   if (_codriver->GetXButtonPressed()) {
     if (_rawControl == false) {
       _rawControl = true;
@@ -21,4 +26,4 @@ void ClimberManualControl::OnTick(units::second_t dt) {
       _climber->SetState(ClimberState::kClimb);
     }
   }
-} 
\ No newline at end of file
+}
diff --git a/src/main/cpp/Climber.cpp b/src/main/cpp/Climber.cpp
index c66f58a8..3d71ae5a 100644
--- a/src/main/cpp/Climber.cpp
+++ b/src/main/cpp/Climber.cpp
@@ -1,45 +1,40 @@
+// Copyright (c) 2023-2024 CurtinFRC
+// Open Source Software, you can modify it according to the terms
+// of the MIT License at the root of this project
+
 #include "Climber.h"
 
 Climber::Climber(ClimberConfig config) : _config(config) {}
 
 void Climber::OnUpdate(units::second_t dt) {
   switch (_state) {
-    case ClimberState::kIdle:
-    {
+    case ClimberState::kIdle: {
       _stringStateName = "Idle";
       _setVoltage = 0_V;
-    }
-    break;
+    } break;
 
-    case ClimberState::kClimb:
-    {
+    case ClimberState::kClimb: {
       _stringStateName = "Climb";
       _setVoltage = 8_V;
-    }
-    break;
+    } break;
 
-    case ClimberState::kHang:
-    {
+    case ClimberState::kHang: {
       _stringStateName = "Hang";
       _setVoltage = -8_V;
-    }
-    break;
+    } break;
 
-    case ClimberState::kRaw:
-    {
+    case ClimberState::kRaw: {
       _stringStateName = "Raw";
       _setVoltage = _rawVoltage;
-    }
-    break;
+    } break;
     default:
-       std::cout << "Error magazine in invalid state" << std::endl;
-    break;
+      std::cout << "Error magazine in invalid state" << std::endl;
+      break;
   }
   _config.climberGearbox.motorController->SetVoltage(_setVoltage);
 
   _table->GetEntry("State: ").SetString(_stringStateName);
   _table->GetEntry("Motor Voltage: ").SetDouble(_setVoltage.value());
-  
 }
 
 void Climber::SetState(ClimberState state) {
@@ -48,4 +43,4 @@ void Climber::SetState(ClimberState state) {
 
 void Climber::SetRaw(units::volt_t voltage) {
   _rawVoltage = voltage;
-}
\ No newline at end of file
+}
diff --git a/src/main/cpp/Main.cpp b/src/main/cpp/Main.cpp
index 3bd80491..e27c9c50 100644
--- a/src/main/cpp/Main.cpp
+++ b/src/main/cpp/Main.cpp
@@ -10,4 +10,4 @@
 #include "Robot.h"
 #include "Wombat.h"
 
-WOMBAT_ROBOT_MAIN(Robot);
\ No newline at end of file
+WOMBAT_ROBOT_MAIN(Robot);
diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp
index 38ae6eba..56a3059b 100644
--- a/src/main/cpp/Robot.cpp
+++ b/src/main/cpp/Robot.cpp
@@ -49,13 +49,13 @@ void Robot::RobotInit() {
   climber = new Climber(robotmap.climberSystem.config);
   wom::BehaviourScheduler::GetInstance()->Register(climber);
   climber->SetDefaultBehaviour([this]() {
-    return wom::make<ClimberManualControl>(climber, &robotmap.controllers.coDriver);
+    return wom::make<ClimberManualControl>(climber,
+                                           &robotmap.controllers.coDriver);
   });
   // m_driveSim = new wom::TempSimSwerveDrive(&simulation_timer, &m_field);
   // m_driveSim = wom::TempSimSwerveDrive();
 }
 
-
 void Robot::RobotPeriodic() {
   units::second_t dt = wom::now() - lastPeriodic;
   lastPeriodic = wom::now();
@@ -81,7 +81,7 @@ void Robot::AutonomousPeriodic() {
 
 void Robot::TeleopInit() {
   loop.Clear();
-  wom::BehaviourScheduler *scheduler = wom::BehaviourScheduler::GetInstance();
+  wom::BehaviourScheduler* scheduler = wom::BehaviourScheduler::GetInstance();
   scheduler->InterruptAll();
   // _swerveDrive->OnStart();
   // sched->InterruptAll();
@@ -97,4 +97,4 @@ void Robot::TestPeriodic() {}
 
 void Robot::SimulationInit() {}
 
-void Robot::SimulationPeriodic() {}
\ No newline at end of file
+void Robot::SimulationPeriodic() {}
diff --git a/src/main/include/Behaviours/ClimberBehaviour.h b/src/main/include/Behaviours/ClimberBehaviour.h
index 8435b5ef..e7b64a86 100644
--- a/src/main/include/Behaviours/ClimberBehaviour.h
+++ b/src/main/include/Behaviours/ClimberBehaviour.h
@@ -1,15 +1,22 @@
+// Copyright (c) 2023-2024 CurtinFRC
+// Open Source Software, you can modify it according to the terms
+// of the MIT License at the root of this project
+
 #pragma once
 
-#include "Wombat.h"
-#include "Climber.h"
 #include <frc/XboxController.h>
 
+#include "Climber.h"
+#include "Wombat.h"
+
 class ClimberManualControl : public behaviour::Behaviour {
-  public:
-    explicit ClimberManualControl(Climber *climber, frc::XboxController *codriver);
-    void OnTick(units::second_t dt) override;
-  private:
-    Climber *_climber;
-    frc::XboxController *_codriver;
-    bool _rawControl = false;
-};
\ No newline at end of file
+ public:
+  explicit ClimberManualControl(Climber* climber,
+                                frc::XboxController* codriver);
+  void OnTick(units::second_t dt) override;
+
+ private:
+  Climber* _climber;
+  frc::XboxController* _codriver;
+  bool _rawControl = false;
+};
diff --git a/src/main/include/Climber.h b/src/main/include/Climber.h
index 738896a9..8342810c 100644
--- a/src/main/include/Climber.h
+++ b/src/main/include/Climber.h
@@ -1,32 +1,34 @@
+// Copyright (c) 2023-2024 CurtinFRC
+// Open Source Software, you can modify it according to the terms
+// of the MIT License at the root of this project
+
 #pragma once
 
-#include "Wombat.h"
-#include <string>
 #include <memory>
+#include <string>
+
+#include "Wombat.h"
 
 struct ClimberConfig {
   wom::Gearbox climberGearbox;
 };
 
-enum class ClimberState {
-  kIdle,
-  kRaw,
-  kClimb,
-  kHang
-};
+enum class ClimberState { kIdle, kRaw, kClimb, kHang };
 
 class Climber : public behaviour::HasBehaviour {
-  public:
-    explicit Climber(ClimberConfig config);
-
-    void OnUpdate(units::second_t dt);
-    void SetState(ClimberState state);
-    void SetRaw(units::volt_t voltage);
-  private:
-    ClimberConfig _config;
-    ClimberState _state = ClimberState::kIdle;
-    units::volt_t _rawVoltage = 0_V;
-    std::string _stringStateName = "error";
-    units::volt_t _setVoltage;
-    std::shared_ptr<nt::NetworkTable> _table = nt::NetworkTableInstance::GetDefault().GetTable("Climber");
-};
\ No newline at end of file
+ public:
+  explicit Climber(ClimberConfig config);
+
+  void OnUpdate(units::second_t dt);
+  void SetState(ClimberState state);
+  void SetRaw(units::volt_t voltage);
+
+ private:
+  ClimberConfig _config;
+  ClimberState _state = ClimberState::kIdle;
+  units::volt_t _rawVoltage = 0_V;
+  std::string _stringStateName = "error";
+  units::volt_t _setVoltage;
+  std::shared_ptr<nt::NetworkTable> _table =
+      nt::NetworkTableInstance::GetDefault().GetTable("Climber");
+};
diff --git a/src/main/include/Robot.h b/src/main/include/Robot.h
index 541ad2df..ffee60bf 100644
--- a/src/main/include/Robot.h
+++ b/src/main/include/Robot.h
@@ -1,8 +1,3 @@
-
-
-
-
-
 // Copyright (c) 2023-2024 CurtinFRC
 // Open Source Software, you can modify it according to the terms
 // of the MIT License at the root of this project
@@ -18,8 +13,6 @@
 #include <frc/smartdashboard/Field2d.h>
 #include <frc/smartdashboard/SendableChooser.h>
 #include <frc/smartdashboard/SmartDashboard.h>
-
-#include <frc/TimedRobot.h>
 #include <networktables/DoubleTopic.h>
 #include <networktables/NetworkTable.h>
 #include <networktables/NetworkTableInstance.h>
@@ -27,7 +20,6 @@
 #include <string>
 
 #include "Behaviours/ClimberBehaviour.h"
-
 #include "RobotMap.h"
 #include "Wombat.h"
 
@@ -61,4 +53,4 @@ class Robot : public frc::TimedRobot {
 
   wom::SwerveDrive* _swerveDrive;
   Climber* climber;
-};
\ No newline at end of file
+};
diff --git a/src/main/include/RobotMap.h b/src/main/include/RobotMap.h
index 92f3af1a..9625f069 100644
--- a/src/main/include/RobotMap.h
+++ b/src/main/include/RobotMap.h
@@ -7,11 +7,10 @@
 #include <frc/Compressor.h>
 #include <frc/DoubleSolenoid.h>
 #include <frc/XboxController.h>
+#include <frc/motorcontrol/MotorControllerGroup.h>
 #include <frc/system/plant/DCMotor.h>
 #include <units/angle.h>
 #include <units/length.h>
-#include <frc/motorcontrol/MotorControllerGroup.h>
-#include <frc/system/plant/DCMotor.h>
 
 #include <string>
 
@@ -19,8 +18,8 @@
 #include <ctre/phoenix6/Pigeon2.hpp>
 #include <ctre/phoenix6/TalonFX.hpp>
 
-#include "Wombat.h"
 #include "Climber.h"
+#include "Wombat.h"
 
 struct RobotMap {
   struct Controllers {
@@ -147,19 +146,16 @@ struct RobotMap {
   SwerveBase swerveBase;
 
   struct Climber {
-    rev::CANSparkMax *climberMotor = new rev::CANSparkMax{99, rev::CANSparkMax::MotorType::kBrushless};
+    rev::CANSparkMax* climberMotor =
+        new rev::CANSparkMax{99, rev::CANSparkMax::MotorType::kBrushless};
     wom::CANSparkMaxEncoder climberEncoder{climberMotor, 0.1_m};
-  
-    wom::Gearbox climberGearbox {
-        climberMotor,
-        &climberEncoder,
-        frc::DCMotor::NEO(1)
-    };
 
-    ClimberConfig config {
-      climberGearbox,
-    };
+    wom::Gearbox climberGearbox{climberMotor, &climberEncoder,
+                                frc::DCMotor::NEO(1)};
 
+    ClimberConfig config{
+        climberGearbox,
+    };
   };
   Climber climberSystem;
-};
\ No newline at end of file
+};

From 4623dc858b64b5cf65502c802fdbf3115cd675fb Mon Sep 17 00:00:00 2001
From: Isaac Turner <spacey_sooty@outlook.com>
Date: Sun, 21 Jan 2024 12:02:12 +0800
Subject: [PATCH 040/207] actually ran it

---
 src/main/cpp/Behaviours/ClimberBehaviour.cpp   | 3 +--
 src/main/cpp/Robot.cpp                         | 6 ++----
 src/main/include/Behaviours/ClimberBehaviour.h | 3 +--
 src/main/include/Climber.h                     | 3 +--
 src/main/include/RobotMap.h                    | 6 ++----
 5 files changed, 7 insertions(+), 14 deletions(-)

diff --git a/src/main/cpp/Behaviours/ClimberBehaviour.cpp b/src/main/cpp/Behaviours/ClimberBehaviour.cpp
index c857263c..e6422a17 100644
--- a/src/main/cpp/Behaviours/ClimberBehaviour.cpp
+++ b/src/main/cpp/Behaviours/ClimberBehaviour.cpp
@@ -4,8 +4,7 @@
 
 #include "behaviours/ClimberBehaviour.h"
 
-ClimberManualControl::ClimberManualControl(Climber* climber,
-                                           frc::XboxController* codriver)
+ClimberManualControl::ClimberManualControl(Climber* climber, frc::XboxController* codriver)
     : _climber(climber), _codriver(codriver) {
   Controls(climber);
 }
diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp
index d658729f..5c40b767 100644
--- a/src/main/cpp/Robot.cpp
+++ b/src/main/cpp/Robot.cpp
@@ -45,10 +45,8 @@ void Robot::RobotInit() {
 
   climber = new Climber(robotmap.climberSystem.config);
   wom::BehaviourScheduler::GetInstance()->Register(climber);
-  climber->SetDefaultBehaviour([this]() {
-    return wom::make<ClimberManualControl>(climber,
-                                           &robotmap.controllers.coDriver);
-  });
+  climber->SetDefaultBehaviour(
+      [this]() { return wom::make<ClimberManualControl>(climber, &robotmap.controllers.coDriver); });
   // m_driveSim = new wom::TempSimSwerveDrive(&simulation_timer, &m_field);
   // m_driveSim = wom::TempSimSwerveDrive();
 }
diff --git a/src/main/include/Behaviours/ClimberBehaviour.h b/src/main/include/Behaviours/ClimberBehaviour.h
index e7b64a86..15b9db7e 100644
--- a/src/main/include/Behaviours/ClimberBehaviour.h
+++ b/src/main/include/Behaviours/ClimberBehaviour.h
@@ -11,8 +11,7 @@
 
 class ClimberManualControl : public behaviour::Behaviour {
  public:
-  explicit ClimberManualControl(Climber* climber,
-                                frc::XboxController* codriver);
+  explicit ClimberManualControl(Climber* climber, frc::XboxController* codriver);
   void OnTick(units::second_t dt) override;
 
  private:
diff --git a/src/main/include/Climber.h b/src/main/include/Climber.h
index 8342810c..8d9d6763 100644
--- a/src/main/include/Climber.h
+++ b/src/main/include/Climber.h
@@ -29,6 +29,5 @@ class Climber : public behaviour::HasBehaviour {
   units::volt_t _rawVoltage = 0_V;
   std::string _stringStateName = "error";
   units::volt_t _setVoltage;
-  std::shared_ptr<nt::NetworkTable> _table =
-      nt::NetworkTableInstance::GetDefault().GetTable("Climber");
+  std::shared_ptr<nt::NetworkTable> _table = nt::NetworkTableInstance::GetDefault().GetTable("Climber");
 };
diff --git a/src/main/include/RobotMap.h b/src/main/include/RobotMap.h
index fb188f75..a0caac6d 100644
--- a/src/main/include/RobotMap.h
+++ b/src/main/include/RobotMap.h
@@ -133,12 +133,10 @@ struct RobotMap {
   SwerveBase swerveBase;
 
   struct Climber {
-    rev::CANSparkMax* climberMotor =
-        new rev::CANSparkMax{99, rev::CANSparkMax::MotorType::kBrushless};
+    rev::CANSparkMax* climberMotor = new rev::CANSparkMax{99, rev::CANSparkMax::MotorType::kBrushless};
     wom::CANSparkMaxEncoder climberEncoder{climberMotor, 0.1_m};
 
-    wom::Gearbox climberGearbox{climberMotor, &climberEncoder,
-                                frc::DCMotor::NEO(1)};
+    wom::Gearbox climberGearbox{climberMotor, &climberEncoder, frc::DCMotor::NEO(1)};
 
     ClimberConfig config{
         climberGearbox,

From d91b5e0c02d71ad1d942a25c49de270905afcceb Mon Sep 17 00:00:00 2001
From: Sean Chan <sean.chan3@student.education.wa.edu.au>
Date: Sun, 21 Jan 2024 12:22:08 +0800
Subject: [PATCH 041/207] Fixed more build errors

---
 src/main/include/Mag.h                     | 2 +-
 src/main/include/Robot.h                   | 1 -
 src/main/include/behaviours/MagBehaviour.h | 4 ++--
 wombat/src/main/cpp/utils/Encoder.cpp      | 2 +-
 4 files changed, 4 insertions(+), 5 deletions(-)

diff --git a/src/main/include/Mag.h b/src/main/include/Mag.h
index fa971106..7cbdea1b 100644
--- a/src/main/include/Mag.h
+++ b/src/main/include/Mag.h
@@ -1,5 +1,5 @@
 #pragma once
-#include "wombat.h"
+#include "Wombat.h"
 #include <frc/DigitalInput.h>
 #include <string>
 #include <memory>
diff --git a/src/main/include/Robot.h b/src/main/include/Robot.h
index ff3f485e..d5b87e56 100644
--- a/src/main/include/Robot.h
+++ b/src/main/include/Robot.h
@@ -6,7 +6,6 @@
 
 #include "behaviours/MagBehaviour.h"
 #include <frc/TimedRobot.h>
-#include <frc/event/Eventloop.h>
 #include <frc/Encoder.h>
 #include <frc/Timer.h>
 #include <frc/event/EventLoop.h>
diff --git a/src/main/include/behaviours/MagBehaviour.h b/src/main/include/behaviours/MagBehaviour.h
index 1e312686..a0fcb4a7 100644
--- a/src/main/include/behaviours/MagBehaviour.h
+++ b/src/main/include/behaviours/MagBehaviour.h
@@ -1,7 +1,7 @@
 #pragma once
  
-#include "wombat.h"
-#include "mag.h"
+#include "Wombat.h"
+#include "Mag.h"
 #include <frc/XboxController.h>
  
 class MagManualControl : public behaviour::Behaviour {
diff --git a/wombat/src/main/cpp/utils/Encoder.cpp b/wombat/src/main/cpp/utils/Encoder.cpp
index 3935d82c..2d59c781 100644
--- a/wombat/src/main/cpp/utils/Encoder.cpp
+++ b/wombat/src/main/cpp/utils/Encoder.cpp
@@ -56,7 +56,7 @@ units::radian_t wom::utils::Encoder::GetEncoderPosition() {
 }
 
 double wom::utils::Encoder::GetEncoderDistance() {
-  return GetEncoderTicks() * (2 * M_PI) * _wheelRadius.value();
+  return GetEncoderTicks() * (2 * 3.14159265358979) * _wheelRadius.value();
 }
 
 units::radians_per_second_t wom::utils::Encoder::GetEncoderAngularVelocity() {

From ded8cbda92a1fbbfb79f9cf270e6d800371d7e82 Mon Sep 17 00:00:00 2001
From: Kingsley Wong <kingsley.wong@student.education.wa.edu.au>
Date: Sun, 21 Jan 2024 12:22:27 +0800
Subject: [PATCH 042/207] finished wpiformat errors 5

---
 src/main/cpp/Intake.cpp               | 4 ++--
 wombat/src/main/cpp/utils/Encoder.cpp | 2 +-
 2 files changed, 3 insertions(+), 3 deletions(-)

diff --git a/src/main/cpp/Intake.cpp b/src/main/cpp/Intake.cpp
index 43361ab3..d8418a99 100644
--- a/src/main/cpp/Intake.cpp
+++ b/src/main/cpp/Intake.cpp
@@ -77,7 +77,7 @@ void Intake::OnUpdate(units::second_t dt) {
 
 void Intake::setState(IntakeState state) {
   _state = state;
-}
+};
 void Intake::setRaw(units::volt_t voltage) {
   _rawVoltage = voltage;
-}
\ No newline at end of file
+};
\ No newline at end of file
diff --git a/wombat/src/main/cpp/utils/Encoder.cpp b/wombat/src/main/cpp/utils/Encoder.cpp
index 3935d82c..2d59c781 100644
--- a/wombat/src/main/cpp/utils/Encoder.cpp
+++ b/wombat/src/main/cpp/utils/Encoder.cpp
@@ -56,7 +56,7 @@ units::radian_t wom::utils::Encoder::GetEncoderPosition() {
 }
 
 double wom::utils::Encoder::GetEncoderDistance() {
-  return GetEncoderTicks() * (2 * M_PI) * _wheelRadius.value();
+  return GetEncoderTicks() * (2 * 3.14159265358979) * _wheelRadius.value();
 }
 
 units::radians_per_second_t wom::utils::Encoder::GetEncoderAngularVelocity() {

From 4a067353bd3fb4a0fdc39864566f568850d5d9d4 Mon Sep 17 00:00:00 2001
From: Kingsley Wong <kingsley.wong@student.education.wa.edu.au>
Date: Sun, 21 Jan 2024 14:17:50 +0800
Subject: [PATCH 043/207] finished wpiformat errors 8

---
 src/main/include/Intake.h | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/include/Intake.h b/src/main/include/Intake.h
index 68703474..a84b41b1 100644
--- a/src/main/include/Intake.h
+++ b/src/main/include/Intake.h
@@ -9,7 +9,7 @@
 #include <memory>
 #include <string>
 
-#include "wombat.h"
+#include "Wombat.h"
 
 struct IntakeConfig {
   wom::Gearbox IntakeMotor;

From 9daf5b5e77e0e17c43a4e4f7816b17f2c1af649e Mon Sep 17 00:00:00 2001
From: Kingsley Wong <kingsley.wong@student.education.wa.edu.au>
Date: Sun, 21 Jan 2024 14:21:05 +0800
Subject: [PATCH 044/207] finished wpiformat errors 9

---
 src/main/cpp/Intake.cpp | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/main/cpp/Intake.cpp b/src/main/cpp/Intake.cpp
index df7dda8e..b8c2a818 100644
--- a/src/main/cpp/Intake.cpp
+++ b/src/main/cpp/Intake.cpp
@@ -67,7 +67,7 @@ void Intake::OnUpdate(units::second_t dt) {
 
 void Intake::setState(IntakeState state) {
   _state = state;
-};
+}
 void Intake::setRaw(units::volt_t voltage) {
   _rawVoltage = voltage;
-};
\ No newline at end of file
+}
\ No newline at end of file

From 9bdd7dbe781245d922b6cf779a56560baf56331e Mon Sep 17 00:00:00 2001
From: Kingsley Wong <kingsley.wong@student.education.wa.edu.au>
Date: Sun, 21 Jan 2024 14:24:27 +0800
Subject: [PATCH 045/207] finished wpiformat errors 10

---
 src/main/cpp/Intake.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/cpp/Intake.cpp b/src/main/cpp/Intake.cpp
index b8c2a818..7025fdc8 100644
--- a/src/main/cpp/Intake.cpp
+++ b/src/main/cpp/Intake.cpp
@@ -70,4 +70,4 @@ void Intake::setState(IntakeState state) {
 }
 void Intake::setRaw(units::volt_t voltage) {
   _rawVoltage = voltage;
-}
\ No newline at end of file
+}

From 7cc74a77c4b6e5c8b090d14d6a1c6eb3a72d40d8 Mon Sep 17 00:00:00 2001
From: Sean Chan <sean.chan3@student.education.wa.edu.au>
Date: Mon, 22 Jan 2024 10:56:46 +0800
Subject: [PATCH 046/207] fixed some github errors

---
 src/main/cpp/behaviours/MagBehaviour.cpp   | 4 ++--
 src/main/include/RobotMap.h                | 2 +-
 src/main/include/behaviours/MagBehaviour.h | 6 +++---
 3 files changed, 6 insertions(+), 6 deletions(-)

diff --git a/src/main/cpp/behaviours/MagBehaviour.cpp b/src/main/cpp/behaviours/MagBehaviour.cpp
index 8977eb4b..5cc0312c 100644
--- a/src/main/cpp/behaviours/MagBehaviour.cpp
+++ b/src/main/cpp/behaviours/MagBehaviour.cpp
@@ -9,7 +9,7 @@ MagManualControl::MagManualControl(Mag* mag, frc::XboxController* codriver) : _m
 }
 
 void MagManualControl::OnTick(units::second_t dt) {
-  if (_codriver->GetAButtonPressed()) {
+  if (_codriver->GetXButtonPressed()) {
     if (_rawControl == true) {
       _rawControl = false;
     } else {
@@ -48,4 +48,4 @@ MagAutoHold::MagAutoHold(Mag* mag) {}
 
 void MagAutoHold::OnTick(units::second_t dt) {
   _mag->SetState(MagState::kHold);
-}
+}
\ No newline at end of file
diff --git a/src/main/include/RobotMap.h b/src/main/include/RobotMap.h
index 71618244..7ca69796 100644
--- a/src/main/include/RobotMap.h
+++ b/src/main/include/RobotMap.h
@@ -19,7 +19,7 @@
 #include <ctre/phoenix6/TalonFX.hpp>
 
 #include "Wombat.h"
-#include "mag.h"
+#include "Mag.h"
 
 struct RobotMap {
   struct Controllers {
diff --git a/src/main/include/behaviours/MagBehaviour.h b/src/main/include/behaviours/MagBehaviour.h
index bc497496..e6cd62ab 100644
--- a/src/main/include/behaviours/MagBehaviour.h
+++ b/src/main/include/behaviours/MagBehaviour.h
@@ -3,12 +3,12 @@
 // of the MIT License at the root of this project
 
 #pragma once
- 
-#include "Wombat.h"
-#include "Mag.h"
 
 #include <frc/XboxController.h>
 
+#include "Mag.h"
+#include "Wombat.h"
+
 class MagManualControl : public behaviour::Behaviour {
  public:
   explicit MagManualControl(Mag* mag, frc::XboxController* codriver);

From 1f6f19fad8b0b16b80c1f88d5ff1f69c8d3d2b07 Mon Sep 17 00:00:00 2001
From: Sean Chan <sean.chan3@student.education.wa.edu.au>
Date: Mon, 22 Jan 2024 11:05:36 +0800
Subject: [PATCH 047/207] Finished github build errors

---
 src/main/cpp/behaviours/MagBehaviour.cpp | 2 +-
 src/main/include/RobotMap.h              | 3 ++-
 2 files changed, 3 insertions(+), 2 deletions(-)

diff --git a/src/main/cpp/behaviours/MagBehaviour.cpp b/src/main/cpp/behaviours/MagBehaviour.cpp
index 5cc0312c..f07dd1f5 100644
--- a/src/main/cpp/behaviours/MagBehaviour.cpp
+++ b/src/main/cpp/behaviours/MagBehaviour.cpp
@@ -48,4 +48,4 @@ MagAutoHold::MagAutoHold(Mag* mag) {}
 
 void MagAutoHold::OnTick(units::second_t dt) {
   _mag->SetState(MagState::kHold);
-}
\ No newline at end of file
+}
diff --git a/src/main/include/RobotMap.h b/src/main/include/RobotMap.h
index 7ca69796..6e1175bc 100644
--- a/src/main/include/RobotMap.h
+++ b/src/main/include/RobotMap.h
@@ -18,8 +18,9 @@
 #include <ctre/phoenix6/Pigeon2.hpp>
 #include <ctre/phoenix6/TalonFX.hpp>
 
-#include "Wombat.h"
 #include "Mag.h"
+#include "Wombat.h"
+
 
 struct RobotMap {
   struct Controllers {

From 78bb6d9d9686e1285f1c7d4069b80e42f79510be Mon Sep 17 00:00:00 2001
From: Sean Chan <sean.chan3@student.education.wa.edu.au>
Date: Mon, 22 Jan 2024 11:17:14 +0800
Subject: [PATCH 048/207] Fixed github errers vorsion 2

---
 src/main/include/RobotMap.h | 1 -
 1 file changed, 1 deletion(-)

diff --git a/src/main/include/RobotMap.h b/src/main/include/RobotMap.h
index 6e1175bc..2aca3cef 100644
--- a/src/main/include/RobotMap.h
+++ b/src/main/include/RobotMap.h
@@ -21,7 +21,6 @@
 #include "Mag.h"
 #include "Wombat.h"
 
-
 struct RobotMap {
   struct Controllers {
     frc::XboxController driver = frc::XboxController(0);

From 1bbe21ce3e9cf81e932ea0ce5b754a48fa2aecee Mon Sep 17 00:00:00 2001
From: Sean Chan <sean.chan3@student.education.wa.edu.au>
Date: Mon, 22 Jan 2024 11:45:19 +0800
Subject: [PATCH 049/207] Fixed github build errors

---
 src/main/cpp/Behaviours/ClimberBehaviour.cpp | 2 +-
 src/main/include/RobotMap.h                  | 2 +-
 wombat/src/main/cpp/utils/Encoder.cpp        | 2 +-
 3 files changed, 3 insertions(+), 3 deletions(-)

diff --git a/src/main/cpp/Behaviours/ClimberBehaviour.cpp b/src/main/cpp/Behaviours/ClimberBehaviour.cpp
index 0befa293..baeeccd9 100644
--- a/src/main/cpp/Behaviours/ClimberBehaviour.cpp
+++ b/src/main/cpp/Behaviours/ClimberBehaviour.cpp
@@ -1,4 +1,4 @@
-#include "behaviours/ClimberBehaviour.h"
+#include "Behaviours/ClimberBehaviour.h"
 
 ClimberManualControl::ClimberManualControl(Climber *climber, frc::XboxController *codriver) : _climber(climber), _codriver(codriver) {
   Controls(climber); 
diff --git a/src/main/include/RobotMap.h b/src/main/include/RobotMap.h
index 92f3af1a..91681b78 100644
--- a/src/main/include/RobotMap.h
+++ b/src/main/include/RobotMap.h
@@ -162,4 +162,4 @@ struct RobotMap {
 
   };
   Climber climberSystem;
-};
\ No newline at end of file
+};
diff --git a/wombat/src/main/cpp/utils/Encoder.cpp b/wombat/src/main/cpp/utils/Encoder.cpp
index 5f27044e..7e6a6892 100644
--- a/wombat/src/main/cpp/utils/Encoder.cpp
+++ b/wombat/src/main/cpp/utils/Encoder.cpp
@@ -56,7 +56,7 @@ units::radian_t wom::utils::Encoder::GetEncoderPosition() {
 }
 
 double wom::utils::Encoder::GetEncoderDistance() {
-  return GetEncoderTicks() * (2 * M_PI) * _wheelRadius.value();
+  return GetEncoderTicks() * (2 * 3.141592653589) * _wheelRadius.value();
 }
 
 units::radians_per_second_t wom::utils::Encoder::GetEncoderAngularVelocity() {

From b1cb5777ab306467bfb244535f8b9fe322edad72 Mon Sep 17 00:00:00 2001
From: JoystickMaster-race <foxlocks179@gmail.com>
Date: Mon, 22 Jan 2024 11:57:32 +0800
Subject: [PATCH 050/207] Alpha Arm vers 1

---
 src/main/cpp/AlphaArm.cpp             | 29 ++++++++++++++++++++++++
 src/main/cpp/AlphaArmBehaviour.cpp    | 29 ++++++++++++++++++++++++
 src/main/cpp/Robot.cpp                |  8 ++++++-
 src/main/include/AlphaArm.h           | 32 +++++++++++++++++++++++++++
 src/main/include/AlphaArmBehaviour.h  | 18 +++++++++++++++
 src/main/include/Robot.h              |  4 ++++
 src/main/include/RobotMap.h           | 23 +++++++++++++++++--
 wombat/src/main/cpp/utils/Encoder.cpp |  2 +-
 8 files changed, 141 insertions(+), 4 deletions(-)
 create mode 100644 src/main/cpp/AlphaArm.cpp
 create mode 100644 src/main/cpp/AlphaArmBehaviour.cpp
 create mode 100644 src/main/include/AlphaArm.h
 create mode 100644 src/main/include/AlphaArmBehaviour.h

diff --git a/src/main/cpp/AlphaArm.cpp b/src/main/cpp/AlphaArm.cpp
new file mode 100644
index 00000000..3e7fa1e7
--- /dev/null
+++ b/src/main/cpp/AlphaArm.cpp
@@ -0,0 +1,29 @@
+#include "AlphaArm.h"
+
+AlphaArm::AlphaArm(AlphaArmConfig config) : _config(config) {}
+
+void AlphaArm::OnUpdate(units::second_t dt){
+        switch(_state){
+            case AlphaArmState::kIdle:
+            //transmission translate
+            _config.armGearBox.motorController->SetVoltage(0_V);
+            break;
+            case AlphaArmState::kRaw:
+            setAlphaArmVoltage = _armVoltage;
+            break;
+            default:
+            std::cout << "oops, wrong state" << std::endl;
+            break;
+            
+        }
+        //transmission translate
+        _config.armGearBox.motorController->SetVoltage(setAlphaArmVoltage);
+}
+
+void AlphaArm::SetRaw(units::volt_t voltR){
+    _voltR = voltR;
+}
+
+void AlphaArm::SetState(AlphaArmState state){
+    _state = state;
+}
\ No newline at end of file
diff --git a/src/main/cpp/AlphaArmBehaviour.cpp b/src/main/cpp/AlphaArmBehaviour.cpp
new file mode 100644
index 00000000..dd1b483b
--- /dev/null
+++ b/src/main/cpp/AlphaArmBehaviour.cpp
@@ -0,0 +1,29 @@
+#include "AlphaArmBehaviour.h"
+#include <frc/XboxController.h>
+
+AlphaArmManualControl::AlphaArmManualControl(AlphaArm *alphaArm, frc::XboxController* codriver) : _alphaArm(alphaArm), _codriver(codriver){
+    Controls(alphaArm);
+}
+
+void AlphaArmManualControl::OnTick(units::second_t dt){
+    if (_codriver->GetXButtonPressed()){
+        if(_rawControl == true) {
+            _rawControl = false;
+        } else {
+            _rawControl = true;
+        }
+    }
+
+
+if (_rawControl) {
+    _alphaArm->SetState(AlphaArmState::kRaw);
+    _alphaArm->SetRaw(_rightStick);
+ }
+}
+
+// void AlphaArmManualControl::OnTick(units::second_t dt){
+     
+
+//     _alphaArm->SetState(AlphaArmState::kRaw); 
+//     _alphaArm->SetRaw(_rightStick);
+// }
\ No newline at end of file
diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp
index 80f253d5..54df2652 100644
--- a/src/main/cpp/Robot.cpp
+++ b/src/main/cpp/Robot.cpp
@@ -43,7 +43,13 @@ void Robot::RobotInit() {
 
   // m_driveSim = new wom::TempSimSwerveDrive(&simulation_timer, &m_field);
   // m_driveSim = wom::TempSimSwerveDrive();
-}
+
+  alphaArm = new AlphaArm(robotmap.alphaArmSystem.config);
+  wom::BehaviourScheduler::GetInstance()->Register(alphaArm);
+  alphaArm->SetDefaultBehaviour([this]()
+     {return wom::make<AlphaArmManualControl>(alphaArm, &robotmap.controllers.codriver); });
+  }
+
 
 void Robot::RobotPeriodic() {
   auto dt = wom::now() - lastPeriodic;
diff --git a/src/main/include/AlphaArm.h b/src/main/include/AlphaArm.h
new file mode 100644
index 00000000..4f804ddf
--- /dev/null
+++ b/src/main/include/AlphaArm.h
@@ -0,0 +1,32 @@
+#pragma once
+#include "Wombat.h"
+#include <frc/DigitalInput.h>
+
+struct AlphaArmConfig {
+    wom::Gearbox armGearBox;
+
+};
+
+enum class AlphaArmState {
+    kIdle,
+    kIntakeAngle,
+    kAmpAngle,
+    kSpeakerAngle,
+    kRaw
+};
+
+class AlphaArm : public::behaviour::HasBehaviour{
+    public:
+     AlphaArm(AlphaArmConfig config);
+
+    void OnUpdate(units::second_t dt);
+    void SetRaw(units::volt_t volt);
+    void SetState(AlphaArmState state);
+
+    private:
+    AlphaArmConfig _config;
+    AlphaArmState _state = AlphaArmState::kIdle;
+    units::volt_t setAlphaArmVoltage;
+    units::volt_t _armVoltage;
+    units::volt_t _voltR;
+};
\ No newline at end of file
diff --git a/src/main/include/AlphaArmBehaviour.h b/src/main/include/AlphaArmBehaviour.h
new file mode 100644
index 00000000..f191a323
--- /dev/null
+++ b/src/main/include/AlphaArmBehaviour.h
@@ -0,0 +1,18 @@
+#pragma once
+
+#pragma once
+#include "Wombat.h"
+#include "AlphaArm.h"
+#include <frc/XboxController.h>
+
+class AlphaArmManualControl : public behaviour::Behaviour {
+    public:
+        explicit AlphaArmManualControl(AlphaArm* alphaArm, frc::XboxController* codriver);
+        void OnTick(units::second_t dt);
+
+    private:
+    AlphaArm *_alphaArm;
+    frc::XboxController* _codriver;
+    units::volt_t _rightStick = ((_codriver->GetRightY()>0.05 || _codriver->GetRightY() < -0.05 )?_codriver->GetRightY():0) * 4_V;
+    bool _rawControl;
+};
\ No newline at end of file
diff --git a/src/main/include/Robot.h b/src/main/include/Robot.h
index bc98df2a..9ded52a6 100644
--- a/src/main/include/Robot.h
+++ b/src/main/include/Robot.h
@@ -13,6 +13,8 @@
 #include <frc/smartdashboard/Field2d.h>
 #include <frc/smartdashboard/SendableChooser.h>
 #include <frc/smartdashboard/SmartDashboard.h>
+#include "AlphaArm.h"
+#include "AlphaArmBehaviour.h"
 
 #include <string>
 
@@ -48,4 +50,6 @@ class Robot : public frc::TimedRobot {
   frc::SendableChooser<std::string> m_path_chooser;
 
   wom::SwerveDrive* _swerveDrive;
+
+  AlphaArm* alphaArm;
 };
diff --git a/src/main/include/RobotMap.h b/src/main/include/RobotMap.h
index d5efd27a..e32bf24c 100644
--- a/src/main/include/RobotMap.h
+++ b/src/main/include/RobotMap.h
@@ -18,11 +18,13 @@
 #include <ctre/phoenix6/TalonFX.hpp>
 
 #include "Wombat.h"
+#include "AlphaArm.h"
+#include "AlphaArmBehaviour.h"
 
 struct RobotMap {
   struct Controllers {
     frc::XboxController driver = frc::XboxController(0);
-    frc::XboxController coDriver = frc::XboxController(1);
+    frc::XboxController codriver = frc::XboxController(1);
     frc::XboxController testController = frc::XboxController(2);
   };
   Controllers controllers;
@@ -127,6 +129,23 @@ struct RobotMap {
     //    driveMotors[i]->SetInverted(true);
     //  }
     //}
+  }; SwerveBase swerveBase;
+
+  struct AlphaArmSystem{
+    //wom::VoltageController armMotor{new rev::CANSparkMax(99, rev::CANSparkMax::MotorType::kBrushless) };
+    //rev::CANSparkMax* armMotor = new rev::CANSparkMax{99, rev::CANSparkMax::MotorType::kBrushless};
+    rev::CANSparkMax alphaArmMotor{99, rev::CANSparkMax::MotorType::kBrushless};
+    //rev::CANSparkMax* armMotor = new rev::CANSparkMax{99, rev::CANSparkMax::MotorType::kBrushless};
+    wom::Gearbox alphaArmGearbox{
+    &alphaArmMotor,
+    nullptr,
+    frc::DCMotor::NEO(1)
+  };
+
+  AlphaArmConfig config {
+    alphaArmGearbox
   };
-  SwerveBase swerveBase;
+  
+  
+ }; AlphaArmSystem alphaArmSystem;
 };
diff --git a/wombat/src/main/cpp/utils/Encoder.cpp b/wombat/src/main/cpp/utils/Encoder.cpp
index 3935d82c..a593c33f 100644
--- a/wombat/src/main/cpp/utils/Encoder.cpp
+++ b/wombat/src/main/cpp/utils/Encoder.cpp
@@ -56,7 +56,7 @@ units::radian_t wom::utils::Encoder::GetEncoderPosition() {
 }
 
 double wom::utils::Encoder::GetEncoderDistance() {
-  return GetEncoderTicks() * (2 * M_PI) * _wheelRadius.value();
+  return GetEncoderTicks() * (2 * 3.14159) * _wheelRadius.value();
 }
 
 units::radians_per_second_t wom::utils::Encoder::GetEncoderAngularVelocity() {

From 09a6511df0b7e8dda8b5ec1fd849070c05a8668d Mon Sep 17 00:00:00 2001
From: JoystickMaster-race <foxlocks179@gmail.com>
Date: Mon, 22 Jan 2024 12:03:00 +0800
Subject: [PATCH 051/207] Alpha Arm ver 1 w/ explicit

---
 src/main/include/AlphaArm.h | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/include/AlphaArm.h b/src/main/include/AlphaArm.h
index 4f804ddf..e8cdbed1 100644
--- a/src/main/include/AlphaArm.h
+++ b/src/main/include/AlphaArm.h
@@ -17,7 +17,7 @@ enum class AlphaArmState {
 
 class AlphaArm : public::behaviour::HasBehaviour{
     public:
-     AlphaArm(AlphaArmConfig config);
+     explicit AlphaArm(AlphaArmConfig config);
 
     void OnUpdate(units::second_t dt);
     void SetRaw(units::volt_t volt);

From 9e6314ae022ae80a21257307f08e95061dedf71a Mon Sep 17 00:00:00 2001
From: JoystickMaster-race <foxlocks179@gmail.com>
Date: Mon, 22 Jan 2024 12:36:04 +0800
Subject: [PATCH 052/207] Alpha arm v1 formatting

---
 src/main/include/RobotMap.h | 26 +++++++++-----------------
 1 file changed, 9 insertions(+), 17 deletions(-)

diff --git a/src/main/include/RobotMap.h b/src/main/include/RobotMap.h
index e32bf24c..c4628bf0 100644
--- a/src/main/include/RobotMap.h
+++ b/src/main/include/RobotMap.h
@@ -29,6 +29,15 @@ struct RobotMap {
   };
   Controllers controllers;
 
+  struct AlphaArmSystem {
+    rev::CANSparkMax alphaArmMotor{99, rev::CANSparkMax::MotorType::kBrushless};
+
+    wom::Gearbox alphaArmGearbox{&alphaArmMotor, nullptr, frc::DCMotor::NEO(1)};
+
+    AlphaArmConfig config{alphaArmGearbox};
+ }; 
+ AlphaArmSystem alphaArmSystem;
+
   struct SwerveBase {
     ctre::phoenix6::hardware::CANcoder frontLeftCancoder{19};
     ctre::phoenix6::hardware::CANcoder frontRightCancoder{17};
@@ -131,21 +140,4 @@ struct RobotMap {
     //}
   }; SwerveBase swerveBase;
 
-  struct AlphaArmSystem{
-    //wom::VoltageController armMotor{new rev::CANSparkMax(99, rev::CANSparkMax::MotorType::kBrushless) };
-    //rev::CANSparkMax* armMotor = new rev::CANSparkMax{99, rev::CANSparkMax::MotorType::kBrushless};
-    rev::CANSparkMax alphaArmMotor{99, rev::CANSparkMax::MotorType::kBrushless};
-    //rev::CANSparkMax* armMotor = new rev::CANSparkMax{99, rev::CANSparkMax::MotorType::kBrushless};
-    wom::Gearbox alphaArmGearbox{
-    &alphaArmMotor,
-    nullptr,
-    frc::DCMotor::NEO(1)
-  };
-
-  AlphaArmConfig config {
-    alphaArmGearbox
-  };
-  
-  
- }; AlphaArmSystem alphaArmSystem;
 };

From b7accf13a2e91c04b1d96aa7be31d7560bb3b0b6 Mon Sep 17 00:00:00 2001
From: Kingsley Wong <kingsley.wong@student.education.wa.edu.au>
Date: Mon, 22 Jan 2024 15:09:15 +0800
Subject: [PATCH 053/207] Tested on robot

---
 src/main/cpp/Intake.cpp          | 46 +++++++--------
 src/main/cpp/IntakeBehaviour.cpp |  3 +-
 src/main/cpp/Robot.cpp           | 29 +---------
 src/main/include/Robot.h         | 10 ----
 src/main/include/RobotMap.h      | 96 +++-----------------------------
 5 files changed, 38 insertions(+), 146 deletions(-)

diff --git a/src/main/cpp/Intake.cpp b/src/main/cpp/Intake.cpp
index 7025fdc8..384b39d8 100644
--- a/src/main/cpp/Intake.cpp
+++ b/src/main/cpp/Intake.cpp
@@ -13,54 +13,56 @@ IntakeConfig Intake::GetConfig() {
 void Intake::OnUpdate(units::second_t dt) {
   switch (_state) {
     case IntakeState::kIdle: {
-      _config.IntakeMotor.motorController->SetVoltage(0_V);
-      if (_config.intakeSensor->Get()) {
-        setState(IntakeState::kHold);
-      }
+      // _config.IntakeMotor.motorController->SetVoltage(0_V);
+      // if (_config.intakeSensor->Get()) {
+      //   setState(IntakeState::kHold);
+      // }
       _stringStateName = "Idle";
       _setVoltage = 0_V;
     } break;
     case IntakeState::kRaw: {
-      _config.IntakeMotor.motorController->SetVoltage(_rawVoltage);
+      // _config.IntakeMotor.motorController->SetVoltage(_rawVoltage);
       _stringStateName = "Raw";
       _setVoltage = _rawVoltage;
     } break;
     case IntakeState::kEject: {
-      _config.IntakeMotor.motorController->SetVoltage(-5_V);
-      if (_config.intakeSensor->Get() == 0 && _config.magSensor->Get() == 0) {
-        setState(IntakeState::kIdle);
-      }
+      // _config.IntakeMotor.motorController->SetVoltage(-5_V);
+      // if (_config.intakeSensor->Get() == 0 && _config.magSensor->Get() == 0) {
+      //   setState(IntakeState::kIdle);
+      // }
       _stringStateName = "Eject";
       _setVoltage = -5_V;
     } break;
     case IntakeState::kHold: {
-      _config.IntakeMotor.motorController->SetVoltage(0_V);
+      // _config.IntakeMotor.motorController->SetVoltage(0_V);
       _stringStateName = "Hold";
       _setVoltage = 0_V;
     } break;
     case IntakeState::kIntake: {
-      _config.IntakeMotor.motorController->SetVoltage(5_V);
+      // _config.IntakeMotor.motorController->SetVoltage(5_V);
       _stringStateName = "Intake";
       _setVoltage = 5_V;
     } break;
     case IntakeState::kPass: {
-      _config.IntakeMotor.motorController->SetVoltage(5_V);
-      if (_config.shooterSensor->Get()) {
-        setState(IntakeState::kIdle);
-        _stringStateName = "Pass";
-        _setVoltage = 5_V;
-      }
+      // _config.IntakeMotor.motorController->SetVoltage(5_V);
+      // if (_config.shooterSensor->Get()) {
+      //   setState(IntakeState::kIdle);
+      //   _stringStateName = "Pass";
+      // }
+      _setVoltage = 5_V;
       break;
       default:
         std::cout << "Error: Intake in INVALID STATE." << std::endl;
-        break;
+      break;
     }
   }
   _table->GetEntry("State: ").SetString(_stringStateName);
-  _table->GetEntry("Motor Voltage: ").SetBoolean(_setVoltage.value());
-  _table->GetEntry("Intake Sensor: ").SetBoolean(_config.intakeSensor->Get());
-  _table->GetEntry("Shooter Sensor: ").SetBoolean(_config.shooterSensor->Get());
-  _table->GetEntry("Magazine Sensor: ").SetBoolean(_config.magSensor->Get());
+   _table->GetEntry("Motor Voltage: ").SetDouble(_setVoltage.value());
+  // _table->GetEntry("Intake Sensor: ").SetBoolean(_config.intakeSensor->Get());
+  // _table->GetEntry("Shooter Sensor: ").SetBoolean(_config.shooterSensor->Get());
+  // _table->GetEntry("Magazine Sensor: ").SetBoolean(_config.magSensor->Get());
+
+  std::cout <<_setVoltage.value()<< std::endl;
 
   _config.IntakeMotor.motorController->SetVoltage(_setVoltage);
 }
diff --git a/src/main/cpp/IntakeBehaviour.cpp b/src/main/cpp/IntakeBehaviour.cpp
index aaf3eb64..925f7027 100644
--- a/src/main/cpp/IntakeBehaviour.cpp
+++ b/src/main/cpp/IntakeBehaviour.cpp
@@ -22,7 +22,8 @@ void IntakeManualControl::OnTick(units::second_t dt) {
 
   if (_rawControl) {
     _intake->setState(IntakeState::kRaw);
-    _intake->setRaw(_codriver.GetLeftY() * 5_V);
+    _intake->setRaw(_codriver.GetLeftY() * 10_V);
+    std::cout <<"Raw"<< std::endl;
   } else {
     if (_codriver.GetYButtonPressed()) {
       _intake->setState(IntakeState::kIntake);
diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp
index da4a6461..da91c705 100644
--- a/src/main/cpp/Robot.cpp
+++ b/src/main/cpp/Robot.cpp
@@ -23,33 +23,10 @@ static units::second_t lastPeriodic;
 void Robot::RobotInit() {
   lastPeriodic = wom::now();
 
-  m_chooser.SetDefaultOption("Default Auto", "Default Auto");
-
-  frc::SmartDashboard::PutData("Auto Selector", &m_chooser);
-
-  m_path_chooser.SetDefaultOption("Path1", "paths/output/Path1.wpilib.json");
-
-  m_path_chooser.AddOption("Path1", "paths/output/Path1.wpilib.json");
-  m_path_chooser.AddOption("Path2", "paths/output/Path2.wpilib.json");
-
-  frc::SmartDashboard::PutData("Path Selector", &m_path_chooser);
-
-  frc::SmartDashboard::PutData("Field", &m_field);
-
-  simulation_timer = frc::Timer();
-
-  robotmap.swerveBase.gyro->Reset();
-
-  _swerveDrive = new wom::SwerveDrive(robotmap.swerveBase.config, frc::Pose2d());
-  wom::BehaviourScheduler::GetInstance()->Register(_swerveDrive);
-  _swerveDrive->SetDefaultBehaviour(
-      [this]() { return wom::make<wom::ManualDrivebase>(_swerveDrive, &robotmap.controllers.driver); });
-
   intake = new Intake(robotmap.intakeSystem.config);
   wom::BehaviourScheduler::GetInstance()->Register(intake);
-
   intake->SetDefaultBehaviour(
-      [this]() { return wom::make<IntakeManualControl>(intake, robotmap.controllers.driver); });
+      [this]() { return wom::make<IntakeManualControl>(intake, robotmap.controllers.codriver); });
 }
 
 void Robot::RobotPeriodic() {
@@ -59,13 +36,13 @@ void Robot::RobotPeriodic() {
   loop.Poll();
   wom::BehaviourScheduler::GetInstance()->Tick();
 
-  _swerveDrive->OnUpdate(dt);
   intake->OnUpdate(dt);
 }
 
 void Robot::AutonomousInit() {
   loop.Clear();
-  sched->InterruptAll();
+  wom::BehaviourScheduler* scheduler = wom::BehaviourScheduler::GetInstance();
+  scheduler->InterruptAll();
 }
 
 void Robot::AutonomousPeriodic() {}
diff --git a/src/main/include/Robot.h b/src/main/include/Robot.h
index fa8ae00a..e70912da 100644
--- a/src/main/include/Robot.h
+++ b/src/main/include/Robot.h
@@ -41,15 +41,5 @@ class Robot : public frc::TimedRobot {
   RobotMap robotmap;
   frc::EventLoop loop;
 
-  frc::SendableChooser<std::string> m_chooser;
-
-  frc::Field2d m_field;
-
-  frc::Timer simulation_timer;
-
-  frc::SendableChooser<std::string> m_path_chooser;
-
-  wom::SwerveDrive* _swerveDrive;
-
   Intake* intake;
 };
diff --git a/src/main/include/RobotMap.h b/src/main/include/RobotMap.h
index bccc0fb2..670228a6 100644
--- a/src/main/include/RobotMap.h
+++ b/src/main/include/RobotMap.h
@@ -25,97 +25,19 @@ struct RobotMap {
   struct Controllers {
     frc::XboxController driver = frc::XboxController(0);
     frc::XboxController codriver = frc::XboxController(1);
-  };
+  }; Controllers controllers;
 
   struct IntakeSystem {
-    rev::CANSparkMax intakeMotor{99, rev::CANSparkMax::MotorType::kBrushless};
-    wom::CANSparkMaxEncoder intakeEncoder{&intakeMotor, 0.1_m};
-    frc::DigitalInput intakeSensor{0};
-    frc::DigitalInput magSensor{0};
-    frc::DigitalInput shooterSensor{0};
+    rev::CANSparkMax intakeMotor{2, rev::CANSparkMax::MotorType::kBrushed};
+    // wom::CANSparkMaxEncoder intakeEncoder{&intakeMotor, 0.1_m};
+    //frc::DigitalInput intakeSensor{0};
+    //frc::DigitalInput magSensor{0};
+    //frc::DigitalInput shooterSensor{0};
 
-    wom::Gearbox IntakeGearbox{&intakeMotor, &intakeEncoder, frc::DCMotor::NEO(1)};
+    wom::Gearbox IntakeGearbox{&intakeMotor, nullptr, frc::DCMotor::CIM(1)};
 
-    IntakeConfig config{IntakeGearbox, &intakeSensor, &magSensor, &shooterSensor};
+    IntakeConfig config{IntakeGearbox/*, &intakeSensor, &magSensor, &shooterSensor*/};
   };
   IntakeSystem intakeSystem;
 
-  Controllers controllers;
-
-  struct SwerveBase {
-    ctre::phoenix6::hardware::CANcoder frontLeftCancoder{19};
-    ctre::phoenix6::hardware::CANcoder frontRightCancoder{17};
-    ctre::phoenix6::hardware::CANcoder backLeftCancoder{16};
-    ctre::phoenix6::hardware::CANcoder backRightCancoder{18};
-
-    ctre::phoenix6::hardware::Pigeon2* gyro = new ctre::phoenix6::hardware::Pigeon2(20, "Drivebase");
-    wpi::array<ctre::phoenix6::hardware::TalonFX*, 4> turnMotors{
-        new ctre::phoenix6::hardware::TalonFX(7, "Drivebase"),
-        new ctre::phoenix6::hardware::TalonFX(5, "Drivebase"),
-        new ctre::phoenix6::hardware::TalonFX(1, "Drivebase"),
-        new ctre::phoenix6::hardware::TalonFX(3, "Drivebase")};
-    wpi::array<ctre::phoenix6::hardware::TalonFX*, 4> driveMotors{
-        new ctre::phoenix6::hardware::TalonFX(9, "Drivebase"),
-        new ctre::phoenix6::hardware::TalonFX(6, "Drivebase"),
-        new ctre::phoenix6::hardware::TalonFX(2, "Drivebase"),
-        new ctre::phoenix6::hardware::TalonFX(4, "Drivebase")};
-
-    wpi::array<wom::SwerveModuleConfig, 4> moduleConfigs{
-        wom::SwerveModuleConfig{
-            frc::Translation2d(10.761_in, 9.455_in),
-            wom::Gearbox{driveMotors[0], new wom::TalonFXEncoder(driveMotors[0], 0.0445_m, 6.75),
-                         frc::DCMotor::Falcon500(1).WithReduction(6.75)},
-            wom::Gearbox{turnMotors[0], new wom::CanEncoder(19, 0.0445_m, 4096, 12.8),
-                         frc::DCMotor::Falcon500(1).WithReduction(12.8)},
-            &frontLeftCancoder, 4_in / 2},
-        wom::SwerveModuleConfig{
-            frc::Translation2d(10.761_in, -9.455_in),
-            wom::Gearbox{driveMotors[1], new wom::TalonFXEncoder(driveMotors[1], 0.0445_m, 6.75),
-                         frc::DCMotor::Falcon500(1).WithReduction(6.75)},
-            wom::Gearbox{turnMotors[1], new wom::CanEncoder(17, 0.0445_m, 4096, 12.8),
-                         frc::DCMotor::Falcon500(1).WithReduction(12.8)},
-            &frontRightCancoder, 4_in / 2},
-        wom::SwerveModuleConfig{
-            frc::Translation2d(-10.761_in, 9.455_in),
-            wom::Gearbox{driveMotors[2], new wom::TalonFXEncoder(driveMotors[2], 0.0445_m, 6.75),
-                         frc::DCMotor::Falcon500(1).WithReduction(6.75)},
-            wom::Gearbox{turnMotors[2], new wom::CanEncoder(16, 0.0445_m, 4096, 12.8),
-                         frc::DCMotor::Falcon500(1).WithReduction(12.8)},
-            &backRightCancoder, 4_in / 2},
-        wom::SwerveModuleConfig{
-            frc::Translation2d(-10.761_in, -9.455_in),
-            wom::Gearbox{driveMotors[3], new wom::TalonFXEncoder(driveMotors[3], 0.0445_m, 6.75),
-                         frc::DCMotor::Falcon500(1).WithReduction(6.75)},
-            wom::Gearbox{turnMotors[3], new wom::CanEncoder(18, 0.0445_m, 4096, 12.8),
-                         frc::DCMotor::Falcon500(1).WithReduction(12.8)},
-            &backLeftCancoder, 4_in / 2},
-    };
-
-    wom::SwerveModule::angle_pid_conf_t anglePID{
-        "/drivetrain/pid/angle/config", 2_V / 360_deg, 0.0_V / (100_deg * 1_s),
-        0_V / (100_deg / 1_s),          1_deg,         0.5_deg / 2_s};
-    wom::SwerveModule::velocity_pid_conf_t velocityPID{
-        "/drivetrain/pid/velocity/config",
-    };
-    wom::SwerveDriveConfig::pose_angle_conf_t poseAnglePID{
-        "/drivetrain/pid/pose/angle/config",
-        180_deg / 1_s / 45_deg,
-        wom::SwerveDriveConfig::pose_angle_conf_t::ki_t{0.1},
-        0_deg / 1_deg,
-        10_deg,
-        10_deg / 1_s};
-    wom::SwerveDriveConfig::pose_position_conf_t posePositionPID{
-        "/drivetrain/pid/pose/position/config",
-        3_mps / 1_m,
-        wom::SwerveDriveConfig::pose_position_conf_t::ki_t{0.15},
-        0_m / 1_m,
-        20_cm,
-        10_cm / 1_s,
-        10_cm};
-
-    wom::SwerveDriveConfig config{
-        "/drivetrain", anglePID,        velocityPID, moduleConfigs,   gyro,
-        poseAnglePID,  posePositionPID, 60_kg,       {0.1, 0.1, 0.1}, {0.9, 0.9, 0.9}};
-  };
-  SwerveBase swerveBase;
-};
+};
\ No newline at end of file

From f0a7a9ff43218d41af499fa2b20f03d716bf5e9b Mon Sep 17 00:00:00 2001
From: Sean Chan <sean.chan3@student.education.wa.edu.au>
Date: Mon, 22 Jan 2024 15:24:19 +0800
Subject: [PATCH 054/207] started basic auto

---
 src/main/cpp/Auto.cpp   | 25 +++++++++++++++++++++++++
 src/main/include/Auto.h |  7 +++++++
 2 files changed, 32 insertions(+)
 create mode 100644 src/main/cpp/Auto.cpp
 create mode 100644 src/main/include/Auto.h

diff --git a/src/main/cpp/Auto.cpp b/src/main/cpp/Auto.cpp
new file mode 100644
index 00000000..303946c3
--- /dev/null
+++ b/src/main/cpp/Auto.cpp
@@ -0,0 +1,25 @@
+#include "Auto.h"
+
+std::shared_ptr<Behaviour> QuadrupleClose(/*Drivebase drivebase, Shooter shooter, Mag mag, Intake intake*/) {
+  return
+    << make<Shoot>()
+    << make<Drive>()
+    << make<Intake>()
+    << make<Shoot>()
+    << make<Drive>()
+    << make<Intake>()
+    << make<Shoot>()
+    << make<Drive>()
+    << make<Intake>()
+    << make<Shoot>()
+    /* 
+    1. Shoot starting note into speaker
+    2. Intake note from close note
+    3. Shoot note into speaker
+    4. Intake note from close floor note
+    5. Shoot note into speaker
+    6. Intake not from close floor
+    7. Shoot note
+    */
+}
+
diff --git a/src/main/include/Auto.h b/src/main/include/Auto.h
new file mode 100644
index 00000000..67d0dca1
--- /dev/null
+++ b/src/main/include/Auto.h
@@ -0,0 +1,7 @@
+#pragma once
+
+#include "Wombat.h"
+
+namespace autos {
+  std::shared_ptr<behaviour::Behaviour> QuadrupleClose(/*Drivebase drivebase, Shooter shooter, Mag mag, Intake intake*/);
+} // namespace autos

From 64731b69100e0636eb15c36316f4d4b364a2e406 Mon Sep 17 00:00:00 2001
From: JoystickMaster-race <foxlocks179@gmail.com>
Date: Mon, 22 Jan 2024 15:25:55 +0800
Subject: [PATCH 055/207] Alpha arm w/ wrist v1

---
 src/main/cpp/AlphaArm.cpp            | 16 +++++++++++-----
 src/main/cpp/AlphaArmBehaviour.cpp   | 13 +++----------
 src/main/include/AlphaArm.h          |  7 +++++--
 src/main/include/AlphaArmBehaviour.h |  2 +-
 src/main/include/RobotMap.h          |  4 +++-
 5 files changed, 23 insertions(+), 19 deletions(-)

diff --git a/src/main/cpp/AlphaArm.cpp b/src/main/cpp/AlphaArm.cpp
index 3e7fa1e7..a4c0fa3b 100644
--- a/src/main/cpp/AlphaArm.cpp
+++ b/src/main/cpp/AlphaArm.cpp
@@ -6,7 +6,9 @@ void AlphaArm::OnUpdate(units::second_t dt){
         switch(_state){
             case AlphaArmState::kIdle:
             //transmission translate
-            _config.armGearBox.motorController->SetVoltage(0_V);
+            _config.alphaArmGearbox.motorController->SetVoltage(0_V);
+            _config.wristGearbox.motorController->SetVoltage(0_V);
+
             break;
             case AlphaArmState::kRaw:
             setAlphaArmVoltage = _armVoltage;
@@ -17,13 +19,17 @@ void AlphaArm::OnUpdate(units::second_t dt){
             
         }
         //transmission translate
-        _config.armGearBox.motorController->SetVoltage(setAlphaArmVoltage);
+       // _config.armGearBox.motorController->SetVoltage(setAlphaArmVoltage);
+       _config.alphaArmGearbox.motorController->SetVoltage(setAlphaArmVoltage);
+       _config.wristGearbox.motorController->SetVoltage(setWristVoltage);
+        
+}
+
+void AlphaArm::SetState(AlphaArmState state){
+    _state = state;
 }
 
 void AlphaArm::SetRaw(units::volt_t voltR){
     _voltR = voltR;
 }
 
-void AlphaArm::SetState(AlphaArmState state){
-    _state = state;
-}
\ No newline at end of file
diff --git a/src/main/cpp/AlphaArmBehaviour.cpp b/src/main/cpp/AlphaArmBehaviour.cpp
index dd1b483b..c7286b52 100644
--- a/src/main/cpp/AlphaArmBehaviour.cpp
+++ b/src/main/cpp/AlphaArmBehaviour.cpp
@@ -14,16 +14,9 @@ void AlphaArmManualControl::OnTick(units::second_t dt){
         }
     }
 
-
 if (_rawControl) {
     _alphaArm->SetState(AlphaArmState::kRaw);
-    _alphaArm->SetRaw(_rightStick);
- }
+    //mess around with _rightStick later
+    _alphaArm->SetRaw(_codriver->GetRightY() * 2_V);
+  }
 }
-
-// void AlphaArmManualControl::OnTick(units::second_t dt){
-     
-
-//     _alphaArm->SetState(AlphaArmState::kRaw); 
-//     _alphaArm->SetRaw(_rightStick);
-// }
\ No newline at end of file
diff --git a/src/main/include/AlphaArm.h b/src/main/include/AlphaArm.h
index e8cdbed1..13545f12 100644
--- a/src/main/include/AlphaArm.h
+++ b/src/main/include/AlphaArm.h
@@ -3,7 +3,9 @@
 #include <frc/DigitalInput.h>
 
 struct AlphaArmConfig {
-    wom::Gearbox armGearBox;
+    //wom::Gearbox armGearBox;
+    wom::Gearbox alphaArmGearbox;
+    wom::Gearbox wristGearbox;
 
 };
 
@@ -26,7 +28,8 @@ class AlphaArm : public::behaviour::HasBehaviour{
     private:
     AlphaArmConfig _config;
     AlphaArmState _state = AlphaArmState::kIdle;
-    units::volt_t setAlphaArmVoltage;
+    units::volt_t setAlphaArmVoltage = 0_V;
+    units::volt_t setWristVoltage = 0_V;
     units::volt_t _armVoltage;
     units::volt_t _voltR;
 };
\ No newline at end of file
diff --git a/src/main/include/AlphaArmBehaviour.h b/src/main/include/AlphaArmBehaviour.h
index f191a323..56902a02 100644
--- a/src/main/include/AlphaArmBehaviour.h
+++ b/src/main/include/AlphaArmBehaviour.h
@@ -13,6 +13,6 @@ class AlphaArmManualControl : public behaviour::Behaviour {
     private:
     AlphaArm *_alphaArm;
     frc::XboxController* _codriver;
-    units::volt_t _rightStick = ((_codriver->GetRightY()>0.05 || _codriver->GetRightY() < -0.05 )?_codriver->GetRightY():0) * 4_V;
+    //units::volt_t _rightStick = ((_codriver->GetRightY()>0.05 || _codriver->GetRightY() < -0.05 )?_codriver->GetRightY():0) * 2_V;
     bool _rawControl;
 };
\ No newline at end of file
diff --git a/src/main/include/RobotMap.h b/src/main/include/RobotMap.h
index c4628bf0..5392492b 100644
--- a/src/main/include/RobotMap.h
+++ b/src/main/include/RobotMap.h
@@ -31,10 +31,12 @@ struct RobotMap {
 
   struct AlphaArmSystem {
     rev::CANSparkMax alphaArmMotor{99, rev::CANSparkMax::MotorType::kBrushless};
+    rev::CANSparkMax wristMotor{99, rev::CANSparkMax::MotorType::kBrushless};
 
     wom::Gearbox alphaArmGearbox{&alphaArmMotor, nullptr, frc::DCMotor::NEO(1)};
+    wom::Gearbox wristGearbox{&wristMotor, nullptr, frc::DCMotor::NEO(1)};
 
-    AlphaArmConfig config{alphaArmGearbox};
+    AlphaArmConfig config{alphaArmGearbox, wristGearbox};
  }; 
  AlphaArmSystem alphaArmSystem;
 

From 03c73ab9928f6b77636f52b5b27f23eab45ecb24 Mon Sep 17 00:00:00 2001
From: Isaac Turner <spacey_sooty@outlook.com>
Date: Mon, 22 Jan 2024 15:27:53 +0800
Subject: [PATCH 056/207] formatted

---
 src/main/cpp/AlphaArm.cpp            | 51 ++++++++++++++--------------
 src/main/cpp/AlphaArmBehaviour.cpp   | 28 +++++++++------
 src/main/cpp/Robot.cpp               |  7 ++--
 src/main/include/AlphaArm.h          | 50 +++++++++++++--------------
 src/main/include/AlphaArmBehaviour.h | 28 +++++++++------
 src/main/include/Robot.h             |  4 +--
 src/main/include/RobotMap.h          | 10 +++---
 7 files changed, 94 insertions(+), 84 deletions(-)

diff --git a/src/main/cpp/AlphaArm.cpp b/src/main/cpp/AlphaArm.cpp
index a4c0fa3b..a0066022 100644
--- a/src/main/cpp/AlphaArm.cpp
+++ b/src/main/cpp/AlphaArm.cpp
@@ -1,35 +1,36 @@
+// Copyright (c) 2023-2024 CurtinFRC
+// Open Source Software, you can modify it according to the terms
+// of the MIT License at the root of this project
+
 #include "AlphaArm.h"
 
 AlphaArm::AlphaArm(AlphaArmConfig config) : _config(config) {}
 
-void AlphaArm::OnUpdate(units::second_t dt){
-        switch(_state){
-            case AlphaArmState::kIdle:
-            //transmission translate
-            _config.alphaArmGearbox.motorController->SetVoltage(0_V);
-            _config.wristGearbox.motorController->SetVoltage(0_V);
+void AlphaArm::OnUpdate(units::second_t dt) {
+  switch (_state) {
+    case AlphaArmState::kIdle:
+      // transmission translate
+      _config.alphaArmGearbox.motorController->SetVoltage(0_V);
+      _config.wristGearbox.motorController->SetVoltage(0_V);
 
-            break;
-            case AlphaArmState::kRaw:
-            setAlphaArmVoltage = _armVoltage;
-            break;
-            default:
-            std::cout << "oops, wrong state" << std::endl;
-            break;
-            
-        }
-        //transmission translate
-       // _config.armGearBox.motorController->SetVoltage(setAlphaArmVoltage);
-       _config.alphaArmGearbox.motorController->SetVoltage(setAlphaArmVoltage);
-       _config.wristGearbox.motorController->SetVoltage(setWristVoltage);
-        
+      break;
+    case AlphaArmState::kRaw:
+      setAlphaArmVoltage = _armVoltage;
+      break;
+    default:
+      std::cout << "oops, wrong state" << std::endl;
+      break;
+  }
+  // transmission translate
+  // _config.armGearBox.motorController->SetVoltage(setAlphaArmVoltage);
+  _config.alphaArmGearbox.motorController->SetVoltage(setAlphaArmVoltage);
+  _config.wristGearbox.motorController->SetVoltage(setWristVoltage);
 }
 
-void AlphaArm::SetState(AlphaArmState state){
-    _state = state;
+void AlphaArm::SetState(AlphaArmState state) {
+  _state = state;
 }
 
-void AlphaArm::SetRaw(units::volt_t voltR){
-    _voltR = voltR;
+void AlphaArm::SetRaw(units::volt_t voltR) {
+  _voltR = voltR;
 }
-
diff --git a/src/main/cpp/AlphaArmBehaviour.cpp b/src/main/cpp/AlphaArmBehaviour.cpp
index c7286b52..f8a3793c 100644
--- a/src/main/cpp/AlphaArmBehaviour.cpp
+++ b/src/main/cpp/AlphaArmBehaviour.cpp
@@ -1,22 +1,28 @@
+// Copyright (c) 2023-2024 CurtinFRC
+// Open Source Software, you can modify it according to the terms
+// of the MIT License at the root of this project
+
 #include "AlphaArmBehaviour.h"
+
 #include <frc/XboxController.h>
 
-AlphaArmManualControl::AlphaArmManualControl(AlphaArm *alphaArm, frc::XboxController* codriver) : _alphaArm(alphaArm), _codriver(codriver){
-    Controls(alphaArm);
+AlphaArmManualControl::AlphaArmManualControl(AlphaArm* alphaArm, frc::XboxController* codriver)
+    : _alphaArm(alphaArm), _codriver(codriver) {
+  Controls(alphaArm);
 }
 
-void AlphaArmManualControl::OnTick(units::second_t dt){
-    if (_codriver->GetXButtonPressed()){
-        if(_rawControl == true) {
-            _rawControl = false;
-        } else {
-            _rawControl = true;
-        }
+void AlphaArmManualControl::OnTick(units::second_t dt) {
+  if (_codriver->GetXButtonPressed()) {
+    if (_rawControl == true) {
+      _rawControl = false;
+    } else {
+      _rawControl = true;
     }
+  }
 
-if (_rawControl) {
+  if (_rawControl) {
     _alphaArm->SetState(AlphaArmState::kRaw);
-    //mess around with _rightStick later
+    // mess around with _rightStick later
     _alphaArm->SetRaw(_codriver->GetRightY() * 2_V);
   }
 }
diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp
index 54df2652..0a6bd6b4 100644
--- a/src/main/cpp/Robot.cpp
+++ b/src/main/cpp/Robot.cpp
@@ -46,10 +46,9 @@ void Robot::RobotInit() {
 
   alphaArm = new AlphaArm(robotmap.alphaArmSystem.config);
   wom::BehaviourScheduler::GetInstance()->Register(alphaArm);
-  alphaArm->SetDefaultBehaviour([this]()
-     {return wom::make<AlphaArmManualControl>(alphaArm, &robotmap.controllers.codriver); });
-  }
-
+  alphaArm->SetDefaultBehaviour(
+      [this]() { return wom::make<AlphaArmManualControl>(alphaArm, &robotmap.controllers.codriver); });
+}
 
 void Robot::RobotPeriodic() {
   auto dt = wom::now() - lastPeriodic;
diff --git a/src/main/include/AlphaArm.h b/src/main/include/AlphaArm.h
index 13545f12..2232b7fb 100644
--- a/src/main/include/AlphaArm.h
+++ b/src/main/include/AlphaArm.h
@@ -1,35 +1,33 @@
+// Copyright (c) 2023-2024 CurtinFRC
+// Open Source Software, you can modify it according to the terms
+// of the MIT License at the root of this project
+
 #pragma once
-#include "Wombat.h"
 #include <frc/DigitalInput.h>
 
-struct AlphaArmConfig {
-    //wom::Gearbox armGearBox;
-    wom::Gearbox alphaArmGearbox;
-    wom::Gearbox wristGearbox;
+#include "Wombat.h"
 
+struct AlphaArmConfig {
+  // wom::Gearbox armGearBox;
+  wom::Gearbox alphaArmGearbox;
+  wom::Gearbox wristGearbox;
 };
 
-enum class AlphaArmState {
-    kIdle,
-    kIntakeAngle,
-    kAmpAngle,
-    kSpeakerAngle,
-    kRaw
-};
+enum class AlphaArmState { kIdle, kIntakeAngle, kAmpAngle, kSpeakerAngle, kRaw };
 
-class AlphaArm : public::behaviour::HasBehaviour{
-    public:
-     explicit AlphaArm(AlphaArmConfig config);
+class AlphaArm : public ::behaviour::HasBehaviour {
+ public:
+  explicit AlphaArm(AlphaArmConfig config);
 
-    void OnUpdate(units::second_t dt);
-    void SetRaw(units::volt_t volt);
-    void SetState(AlphaArmState state);
+  void OnUpdate(units::second_t dt);
+  void SetRaw(units::volt_t volt);
+  void SetState(AlphaArmState state);
 
-    private:
-    AlphaArmConfig _config;
-    AlphaArmState _state = AlphaArmState::kIdle;
-    units::volt_t setAlphaArmVoltage = 0_V;
-    units::volt_t setWristVoltage = 0_V;
-    units::volt_t _armVoltage;
-    units::volt_t _voltR;
-};
\ No newline at end of file
+ private:
+  AlphaArmConfig _config;
+  AlphaArmState _state = AlphaArmState::kIdle;
+  units::volt_t setAlphaArmVoltage = 0_V;
+  units::volt_t setWristVoltage = 0_V;
+  units::volt_t _armVoltage;
+  units::volt_t _voltR;
+};
diff --git a/src/main/include/AlphaArmBehaviour.h b/src/main/include/AlphaArmBehaviour.h
index 56902a02..4c2d03fb 100644
--- a/src/main/include/AlphaArmBehaviour.h
+++ b/src/main/include/AlphaArmBehaviour.h
@@ -1,18 +1,24 @@
+// Copyright (c) 2023-2024 CurtinFRC
+// Open Source Software, you can modify it according to the terms
+// of the MIT License at the root of this project
+
 #pragma once
 
 #pragma once
-#include "Wombat.h"
-#include "AlphaArm.h"
 #include <frc/XboxController.h>
 
+#include "AlphaArm.h"
+#include "Wombat.h"
+
 class AlphaArmManualControl : public behaviour::Behaviour {
-    public:
-        explicit AlphaArmManualControl(AlphaArm* alphaArm, frc::XboxController* codriver);
-        void OnTick(units::second_t dt);
+ public:
+  explicit AlphaArmManualControl(AlphaArm* alphaArm, frc::XboxController* codriver);
+  void OnTick(units::second_t dt);
 
-    private:
-    AlphaArm *_alphaArm;
-    frc::XboxController* _codriver;
-    //units::volt_t _rightStick = ((_codriver->GetRightY()>0.05 || _codriver->GetRightY() < -0.05 )?_codriver->GetRightY():0) * 2_V;
-    bool _rawControl;
-};
\ No newline at end of file
+ private:
+  AlphaArm* _alphaArm;
+  frc::XboxController* _codriver;
+  // units::volt_t _rightStick = ((_codriver->GetRightY()>0.05 || _codriver->GetRightY() < -0.05
+  // )?_codriver->GetRightY():0) * 2_V;
+  bool _rawControl;
+};
diff --git a/src/main/include/Robot.h b/src/main/include/Robot.h
index 9ded52a6..15833879 100644
--- a/src/main/include/Robot.h
+++ b/src/main/include/Robot.h
@@ -13,11 +13,11 @@
 #include <frc/smartdashboard/Field2d.h>
 #include <frc/smartdashboard/SendableChooser.h>
 #include <frc/smartdashboard/SmartDashboard.h>
-#include "AlphaArm.h"
-#include "AlphaArmBehaviour.h"
 
 #include <string>
 
+#include "AlphaArm.h"
+#include "AlphaArmBehaviour.h"
 #include "RobotMap.h"
 #include "Wombat.h"
 
diff --git a/src/main/include/RobotMap.h b/src/main/include/RobotMap.h
index 5392492b..10a58844 100644
--- a/src/main/include/RobotMap.h
+++ b/src/main/include/RobotMap.h
@@ -17,9 +17,9 @@
 #include <ctre/phoenix6/Pigeon2.hpp>
 #include <ctre/phoenix6/TalonFX.hpp>
 
-#include "Wombat.h"
 #include "AlphaArm.h"
 #include "AlphaArmBehaviour.h"
+#include "Wombat.h"
 
 struct RobotMap {
   struct Controllers {
@@ -37,8 +37,8 @@ struct RobotMap {
     wom::Gearbox wristGearbox{&wristMotor, nullptr, frc::DCMotor::NEO(1)};
 
     AlphaArmConfig config{alphaArmGearbox, wristGearbox};
- }; 
- AlphaArmSystem alphaArmSystem;
+  };
+  AlphaArmSystem alphaArmSystem;
 
   struct SwerveBase {
     ctre::phoenix6::hardware::CANcoder frontLeftCancoder{19};
@@ -140,6 +140,6 @@ struct RobotMap {
     //    driveMotors[i]->SetInverted(true);
     //  }
     //}
-  }; SwerveBase swerveBase;
-
+  };
+  SwerveBase swerveBase;
 };

From dcb7b2102401aea9cb1b12d395f92972ddd1edac Mon Sep 17 00:00:00 2001
From: Kingsley Wong <kingsley.wong@student.education.wa.edu.au>
Date: Mon, 22 Jan 2024 15:32:45 +0800
Subject: [PATCH 057/207] Fixed github build errors after modifying during
 robot tests

---
 src/main/cpp/Intake.cpp          | 13 ++++++-------
 src/main/cpp/IntakeBehaviour.cpp |  2 +-
 src/main/include/RobotMap.h      | 13 +++++++------
 3 files changed, 14 insertions(+), 14 deletions(-)

diff --git a/src/main/cpp/Intake.cpp b/src/main/cpp/Intake.cpp
index 384b39d8..810b5ef3 100644
--- a/src/main/cpp/Intake.cpp
+++ b/src/main/cpp/Intake.cpp
@@ -50,19 +50,18 @@ void Intake::OnUpdate(units::second_t dt) {
       //   _stringStateName = "Pass";
       // }
       _setVoltage = 5_V;
-      break;
-      default:
-        std::cout << "Error: Intake in INVALID STATE." << std::endl;
-      break;
-    }
+    } break;
+    default:
+      std::cout << "Error: Intake in INVALID STATE." << std::endl;
+    break;
   }
   _table->GetEntry("State: ").SetString(_stringStateName);
-   _table->GetEntry("Motor Voltage: ").SetDouble(_setVoltage.value());
+  _table->GetEntry("Motor Voltage: ").SetDouble(_setVoltage.value());
   // _table->GetEntry("Intake Sensor: ").SetBoolean(_config.intakeSensor->Get());
   // _table->GetEntry("Shooter Sensor: ").SetBoolean(_config.shooterSensor->Get());
   // _table->GetEntry("Magazine Sensor: ").SetBoolean(_config.magSensor->Get());
 
-  std::cout <<_setVoltage.value()<< std::endl;
+  std::cout << _setVoltage.value() << std::endl;
 
   _config.IntakeMotor.motorController->SetVoltage(_setVoltage);
 }
diff --git a/src/main/cpp/IntakeBehaviour.cpp b/src/main/cpp/IntakeBehaviour.cpp
index 925f7027..52570649 100644
--- a/src/main/cpp/IntakeBehaviour.cpp
+++ b/src/main/cpp/IntakeBehaviour.cpp
@@ -23,7 +23,7 @@ void IntakeManualControl::OnTick(units::second_t dt) {
   if (_rawControl) {
     _intake->setState(IntakeState::kRaw);
     _intake->setRaw(_codriver.GetLeftY() * 10_V);
-    std::cout <<"Raw"<< std::endl;
+    std::cout << "Raw" << std::endl;
   } else {
     if (_codriver.GetYButtonPressed()) {
       _intake->setState(IntakeState::kIntake);
diff --git a/src/main/include/RobotMap.h b/src/main/include/RobotMap.h
index 670228a6..895fc813 100644
--- a/src/main/include/RobotMap.h
+++ b/src/main/include/RobotMap.h
@@ -25,19 +25,20 @@ struct RobotMap {
   struct Controllers {
     frc::XboxController driver = frc::XboxController(0);
     frc::XboxController codriver = frc::XboxController(1);
-  }; Controllers controllers;
+  };
+  Controllers controllers;
 
   struct IntakeSystem {
     rev::CANSparkMax intakeMotor{2, rev::CANSparkMax::MotorType::kBrushed};
     // wom::CANSparkMaxEncoder intakeEncoder{&intakeMotor, 0.1_m};
-    //frc::DigitalInput intakeSensor{0};
-    //frc::DigitalInput magSensor{0};
-    //frc::DigitalInput shooterSensor{0};
+    // frc::DigitalInput intakeSensor{0};
+    // frc::DigitalInput magSensor{0};
+    // frc::DigitalInput shooterSensor{0};
 
     wom::Gearbox IntakeGearbox{&intakeMotor, nullptr, frc::DCMotor::CIM(1)};
 
-    IntakeConfig config{IntakeGearbox/*, &intakeSensor, &magSensor, &shooterSensor*/};
+    IntakeConfig config{IntakeGearbox /*, &intakeSensor, &magSensor, &shooterSensor*/};
   };
   IntakeSystem intakeSystem;
 
-};
\ No newline at end of file
+};

From 3ea6206caff85eea0157c56300d196ebb6d07387 Mon Sep 17 00:00:00 2001
From: Kingsley Wong <kingsley.wong@student.education.wa.edu.au>
Date: Mon, 22 Jan 2024 15:40:25 +0800
Subject: [PATCH 058/207] fixed more github build errors 2

---
 src/main/cpp/Intake.cpp | 36 ++++++++++++++++++++++++------------
 1 file changed, 24 insertions(+), 12 deletions(-)

diff --git a/src/main/cpp/Intake.cpp b/src/main/cpp/Intake.cpp
index 810b5ef3..f1201602 100644
--- a/src/main/cpp/Intake.cpp
+++ b/src/main/cpp/Intake.cpp
@@ -12,45 +12,57 @@ IntakeConfig Intake::GetConfig() {
 
 void Intake::OnUpdate(units::second_t dt) {
   switch (_state) {
-    case IntakeState::kIdle: {
+    case IntakeState::kIdle:
+    {
       // _config.IntakeMotor.motorController->SetVoltage(0_V);
       // if (_config.intakeSensor->Get()) {
       //   setState(IntakeState::kHold);
       // }
       _stringStateName = "Idle";
       _setVoltage = 0_V;
-    } break;
-    case IntakeState::kRaw: {
+    }
+    break;
+    case IntakeState::kRaw:
+    {
       // _config.IntakeMotor.motorController->SetVoltage(_rawVoltage);
       _stringStateName = "Raw";
       _setVoltage = _rawVoltage;
-    } break;
-    case IntakeState::kEject: {
+    }
+    break;
+    case IntakeState::kEject:
+    {
       // _config.IntakeMotor.motorController->SetVoltage(-5_V);
       // if (_config.intakeSensor->Get() == 0 && _config.magSensor->Get() == 0) {
       //   setState(IntakeState::kIdle);
       // }
       _stringStateName = "Eject";
       _setVoltage = -5_V;
-    } break;
-    case IntakeState::kHold: {
+    }
+    break;
+    case IntakeState::kHold:
+    {
       // _config.IntakeMotor.motorController->SetVoltage(0_V);
       _stringStateName = "Hold";
       _setVoltage = 0_V;
-    } break;
-    case IntakeState::kIntake: {
+    }
+    break;
+    case IntakeState::kIntake:
+    {
       // _config.IntakeMotor.motorController->SetVoltage(5_V);
       _stringStateName = "Intake";
       _setVoltage = 5_V;
-    } break;
-    case IntakeState::kPass: {
+    } 
+    break;
+    case IntakeState::kPass:
+    {
       // _config.IntakeMotor.motorController->SetVoltage(5_V);
       // if (_config.shooterSensor->Get()) {
       //   setState(IntakeState::kIdle);
       //   _stringStateName = "Pass";
       // }
       _setVoltage = 5_V;
-    } break;
+    } 
+    break;
     default:
       std::cout << "Error: Intake in INVALID STATE." << std::endl;
     break;

From ad6bc870bf5d83570462dc5aba2f03ac6a86d6d8 Mon Sep 17 00:00:00 2001
From: Kingsley Wong <kingsley.wong@student.education.wa.edu.au>
Date: Mon, 22 Jan 2024 15:44:58 +0800
Subject: [PATCH 059/207] fixed more github build errors 3

---
 src/main/cpp/Intake.cpp     | 38 +++++++++++++------------------------
 src/main/include/RobotMap.h |  1 -
 2 files changed, 13 insertions(+), 26 deletions(-)

diff --git a/src/main/cpp/Intake.cpp b/src/main/cpp/Intake.cpp
index f1201602..75839209 100644
--- a/src/main/cpp/Intake.cpp
+++ b/src/main/cpp/Intake.cpp
@@ -12,60 +12,48 @@ IntakeConfig Intake::GetConfig() {
 
 void Intake::OnUpdate(units::second_t dt) {
   switch (_state) {
-    case IntakeState::kIdle:
-    {
+    case IntakeState::kIdle: {
       // _config.IntakeMotor.motorController->SetVoltage(0_V);
       // if (_config.intakeSensor->Get()) {
       //   setState(IntakeState::kHold);
       // }
       _stringStateName = "Idle";
       _setVoltage = 0_V;
-    }
-    break;
-    case IntakeState::kRaw:
-    {
+    } break;
+    case IntakeState::kRaw: {
       // _config.IntakeMotor.motorController->SetVoltage(_rawVoltage);
       _stringStateName = "Raw";
       _setVoltage = _rawVoltage;
-    }
-    break;
-    case IntakeState::kEject:
-    {
+    } break;
+    case IntakeState::kEject: {
       // _config.IntakeMotor.motorController->SetVoltage(-5_V);
       // if (_config.intakeSensor->Get() == 0 && _config.magSensor->Get() == 0) {
       //   setState(IntakeState::kIdle);
       // }
       _stringStateName = "Eject";
       _setVoltage = -5_V;
-    }
-    break;
-    case IntakeState::kHold:
-    {
+    } break;
+    case IntakeState::kHold: {
       // _config.IntakeMotor.motorController->SetVoltage(0_V);
       _stringStateName = "Hold";
       _setVoltage = 0_V;
-    }
-    break;
-    case IntakeState::kIntake:
-    {
+    } break;
+    case IntakeState::kIntake: {
       // _config.IntakeMotor.motorController->SetVoltage(5_V);
       _stringStateName = "Intake";
       _setVoltage = 5_V;
-    } 
-    break;
-    case IntakeState::kPass:
-    {
+    } break;
+    case IntakeState::kPass: {
       // _config.IntakeMotor.motorController->SetVoltage(5_V);
       // if (_config.shooterSensor->Get()) {
       //   setState(IntakeState::kIdle);
       //   _stringStateName = "Pass";
       // }
       _setVoltage = 5_V;
-    } 
-    break;
+    } break;
     default:
       std::cout << "Error: Intake in INVALID STATE." << std::endl;
-    break;
+      break;
   }
   _table->GetEntry("State: ").SetString(_stringStateName);
   _table->GetEntry("Motor Voltage: ").SetDouble(_setVoltage.value());
diff --git a/src/main/include/RobotMap.h b/src/main/include/RobotMap.h
index 895fc813..3010c36b 100644
--- a/src/main/include/RobotMap.h
+++ b/src/main/include/RobotMap.h
@@ -40,5 +40,4 @@ struct RobotMap {
     IntakeConfig config{IntakeGearbox /*, &intakeSensor, &magSensor, &shooterSensor*/};
   };
   IntakeSystem intakeSystem;
-
 };

From a248769c19081a6fb9604d3210e8bd33f020fd3e Mon Sep 17 00:00:00 2001
From: Sean Chan <sean.chan3@student.education.wa.edu.au>
Date: Tue, 23 Jan 2024 18:17:50 +0800
Subject: [PATCH 060/207] Started auto

---
 src/main/cpp/Auto.cpp   | 248 ++++++++++++++++++++++++++++++++++++++--
 src/main/include/Auto.h |  14 ++-
 2 files changed, 249 insertions(+), 13 deletions(-)

diff --git a/src/main/cpp/Auto.cpp b/src/main/cpp/Auto.cpp
index 303946c3..426ba50b 100644
--- a/src/main/cpp/Auto.cpp
+++ b/src/main/cpp/Auto.cpp
@@ -1,18 +1,36 @@
 #include "Auto.h"
 
-std::shared_ptr<Behaviour> QuadrupleClose(/*Drivebase drivebase, Shooter shooter, Mag mag, Intake intake*/) {
+std::shared_ptr<Behaviour> Taxi(DriveBase _driveBase, Shooter _shooter, Mag _mag, Intake _intake, Arm _arm) {
+  return 
+    <<make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+    <<make<AutoShoot>(_shooter, shooterVolt)
+    <<make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+    <<make<DriveToLocation>(_driveBase, raw, distance)
+  //Shoots starting note then moves out of starting position.
+}
+
+std::shared_ptr<Behaviour> QuadrupleClose(DriveBase _driveBase, Shooter _shooter, Mag _mag, Intake _intake, Arm _arm) {
   return
-    << make<Shoot>()
-    << make<Drive>()
-    << make<Intake>()
-    << make<Shoot>()
-    << make<Drive>()
-    << make<Intake>()
-    << make<Shoot>()
-    << make<Drive>()
-    << make<Intake>()
-    << make<Shoot>()
+    << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+    << make<AutoShoot>(_shooter, shooterVolt)
+    << make<DriveToLocation>(_driveBase, raw, distance)
+    << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+    << make<AutoIntake>(_intake, intakeVolt)
+    << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+    << make<AutoShoot>(_shooter, shooterVolt)
+    << make<DriveToLocation>(_driveBase, raw, distance)
+    << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+    << make<AuttoIntake>(_intake, intakeVolt)
+    << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+    << make<AutoShoot>(_shooter, shooterVolt)
+    << make<DriveToLocation>(_driveBase, raw, distance)
+    << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+    << make<AutoIntake>(_intake, intakeVolt)
+    << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+    << make<AutoShoo>(_shooter, shooterVolt)
+
     /* 
+      4N Close
     1. Shoot starting note into speaker
     2. Intake note from close note
     3. Shoot note into speaker
@@ -23,3 +41,211 @@ std::shared_ptr<Behaviour> QuadrupleClose(/*Drivebase drivebase, Shooter shooter
     */
 }
 
+std::shared_ptr<Behaviour> QuadrupleFar(DriveBase _driveBase, Shooter _shooter, Mag _mag, Intake _intake, Arm _arm) {
+  return
+    << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+    << make<AutoShoot>(_shooter, shooterVolt)
+    << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+    << make<DriveToLocation>(_driveBase, raw, distance)
+    << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+    << make<AutoIntake>(_intake, intakeVolt)
+    //<< make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+    //<< make<DriveToLocation>(_driveBase, raw, distance)
+    << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+    << make<AutoShoot>(_shooter, shooterVolt)
+    << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+    << make<DriveToLocation>(_driveBase, raw, distance)
+    << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+    << make<AutoIntake>(_intake, intakeVolt)
+    //<< make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+    //<< make<DriveToLocation>(_driveBase, raw, distance)
+    << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+    << make<AutoShoot>(_shooter, shooterVolt)
+    << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+    << make<DriveToLocation>(_driveBase, raw, distance)
+    << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+    << make<AutoIntake>(_intake, intakeVolt)
+    //<< make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+    //<< make<DriveToLocation>(_driveBase, raw, distance)
+    << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+    << make<AutoShoot>(_shooter, shooterVolt)
+    // << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+    // << make<DriveToLocation>(_driveBase, raw, distance)   do this if possible
+    // << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+    // << make<AutoIntake>(_intake, intakeVolt)
+
+    /*
+      4N Far
+    1. Shoot start note in speaker
+    2. Drive to far note
+    3. Intake note
+    4. Drive back to shooting line
+    5. Shoot note into speaker
+    6. Drive to note
+    7. Intake note
+    8. Drive to shooting line
+    9. Shoot note
+    10. Drive to note
+    11. Intake note
+    12. Drive to shooting line
+    13. Shoot note
+    14. Drive to intake note (if possible)
+    */
+}
+
+std::shared_ptr<Behaviour> QuadrupleCloseDoubleFar(DriveBase _driveBase, Shooter _shooter, Mag _mag, Intake _intake, Arm _arm) {
+  return
+    << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+    << make<AutoShoot>(_shooter, shooterVolt)
+    << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+    << make<DriveToLocation>(_driveBase, raw, distance)
+    << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+    << make<AutoIntake>(_intake, intakeVolt)
+    //<< make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+    //<< make<DriveToLocation>(_driveBase, raw, distance)
+    << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+    << make<AutoShoot>(_shooter, shooterVolt)
+    << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+    << make<DriveToLocation>(_driveBase, raw, distance)
+    << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+    << make<AutoIntake>(_intake, intakeVolt)
+    //<< make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+    //<< make<DriveToLocation>(_driveBase, raw, distance)
+    << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+    << make<AutoShoot>(_shooter, shooterVolt)
+    << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+    << make<DriveToLocation>(_driveBase, raw, distance)
+    << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+    << make<AutoIntake>(_intake, intakeVolt)
+    //<< make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+    //<< make<DriveToLocation>(_driveBase, raw, distance)
+    << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+    << make<AutoShoot>(_shooter, shooterVolt)
+    << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+    << make<DriveToLocation>(_driveBase, raw, distance)
+    << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+    << make<AutoIntake>(_intake, intakeVolt)
+    << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+    << make<DriveToLocation>(_driveBase, raw, distance)
+    << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+    << make<AutoShoot>(_shooter, shooterVolt)
+    << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+    << make<DriveToLocation>(_driveBase, raw, distance)
+    << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+    << make<AutoIntake>(_intake, intakeVolt)
+    << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+    << make<DriveToLocation>(_driveBase, raw, distance)
+    << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+    << make<AutoShoot>(_shooter, shooterVolt)
+    << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+    << make<DriveToLocation>(_driveBase, raw, distance)
+    << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+    << make<AutoIntake>(_intake, intakeVolt)
+    << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+    << make<AutoShoot>(_shooter, shooterVolt)
+    /*
+      4N Close 2N Far
+    1. Shoot note
+    2. Drive to close note
+    3. Intake note
+    4. Shoot note
+    5. Drive to close note
+    6. Intake note
+    7. Shoot note
+    8. Drive to close note
+    9. Intake note
+    10. Shoot note
+    11. Drive to far note
+    12. Intake note
+    13. Drive to shooting line
+    14. Shoot note
+    15. Drive to far note
+    16. Intake note
+    17. Drive to shooting line
+    18. Shoot note
+    */
+}
+
+std::shared_ptr<Behaviour> QuadrupleCloseSingleFar(DriveBase _driveBase, Shooter _shooter, Mag _mag, Intake _intake, Arm _arm) {
+  return
+    << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+    << make<AutoShoot>(_shooter, shooterVolt)
+    << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+    << make<DriveToLocation>(_driveBase, raw, distance)
+    << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+    << make<AutoIntake>(_intake, intakeVolt)
+    << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+    << make<DriveToLocation>(_driveBase, raw, distance)
+    << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+    << make<AutoShoot>(_shooter, shooterVolt)
+    << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+    << make<DriveToLocation>(_driveBase, raw, distance)
+    << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+    << make<AutoIntake>(_intake, intakeVolt)
+    << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+    << make<DriveToLocation>(_driveBase, raw, distance)
+    << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+    << make<AutoShoot>(_shooter, shooterVolt)
+    << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+    << make<DriveToLocation>(_driveBase, raw, distance)
+    << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+    << make<AutoIntake>(_intake, intakeVolt)
+    << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+    << make<DriveToLocation>(_driveBase, raw, distance)
+    << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+    << make<AutoShoot>(_shooter, shooterVolt)
+    << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+
+    /*
+    4N Close 1N Far
+    1. Shoot note
+    2. Drive to close note
+    3. Intake note
+    4. Drive to speaker
+    5. Shoot note
+    6. Drive to close note
+    7. Intake note
+    8. Drive to speaker
+    9. Shoot note
+    10. Drive to close note
+    11. Intake note
+    12. Drive to speaker
+    13. Shoot note
+    14. Drive to far note
+    15. Intake note
+    15. Drive to speaker
+    16. shoot 
+    */
+}
+
+std::shared_ptr<Behaviour> TrapAuto(DriveBase _driveBase, Shooter _shooter, Mag _mag, Intake _intake, Arm _arm) {
+  return 
+    << make<DriveToLocation>(_driveBase, raw, distance)
+    << make<>
+    << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+    << make<AutoShoot>(_shooter, shooterVolt)
+    << make<>
+    << make<DriveToLocation>(_driveBase, raw, distance)
+    << make<AutoIntake>(_intake, intakeVolt)
+    << make<DriveToLocation>(_driveBase, raw, distance)
+    
+    /* 
+    TRAP AUTO
+    1. Drive to trap
+    2. Climb up
+    3. Shoot note
+    4. Climb down
+    5. Drive to far note
+    6. Intake note
+    7. Drive to trap
+    8. Climb up
+    9. Shoot note
+    10. Climb down
+    11. Drive to far note
+    12. Intake
+    13. Drive to trap 
+    14. Climb up
+    15. Shoot note
+    16. Climb down
+    */
+}
\ No newline at end of file
diff --git a/src/main/include/Auto.h b/src/main/include/Auto.h
index 67d0dca1..4d8839ae 100644
--- a/src/main/include/Auto.h
+++ b/src/main/include/Auto.h
@@ -3,5 +3,15 @@
 #include "Wombat.h"
 
 namespace autos {
-  std::shared_ptr<behaviour::Behaviour> QuadrupleClose(/*Drivebase drivebase, Shooter shooter, Mag mag, Intake intake*/);
-} // namespace autos
+  std::shared_ptr<behaviour::Behaviour> Taxi(Drivebase driveBase, Shooter shooter, Mag mag, Intake intake, Arm _arm);
+
+  std::shared_ptr<behaviour::Behaviour> QuadrupleClose(Drivebase driveBase, Shooter shooter, Mag mag, Intake intake, Arm _arm);
+
+  std::shared_ptr<behaviour::Behaviour> QuadrupleFar(Drivebase driveBase, Shooter shooter, Mag mag, Intake intake, Arm _arm);
+
+  std::shared_ptr<behaviour::Behaviour> QuadrupleCloseDoubleFar(Drivebase driveBase, Shooter shooter, Mag mag, Intake intake, Arm _arm);
+
+  std::shared_ptr<behaviour::Behaviour> QuadrupleCloseSingleFar(Drivebase driveBase, Shooter shooter, Mag mag, Intake intake, Arm _arm);
+}
+
+

From 0c6bfbefcf94b723602e9b45ceefd0efc416b0d5 Mon Sep 17 00:00:00 2001
From: Sean Chan <sean.chan3@student.education.wa.edu.au>
Date: Tue, 23 Jan 2024 18:58:01 +0800
Subject: [PATCH 061/207] merged mag into auto code and fixed errors 2

---
 src/main/cpp/Auto.cpp   | 482 ++++++++++++++++++++--------------------
 src/main/cpp/Robot.cpp  |  35 +--
 src/main/include/Auto.h |  18 +-
 3 files changed, 258 insertions(+), 277 deletions(-)

diff --git a/src/main/cpp/Auto.cpp b/src/main/cpp/Auto.cpp
index 426ba50b..3c2da0cc 100644
--- a/src/main/cpp/Auto.cpp
+++ b/src/main/cpp/Auto.cpp
@@ -1,251 +1,251 @@
-#include "Auto.h"
+// #include "Auto.h"
 
-std::shared_ptr<Behaviour> Taxi(DriveBase _driveBase, Shooter _shooter, Mag _mag, Intake _intake, Arm _arm) {
-  return 
-    <<make<ArmToSetPoint>(_arm, armAngle, wristAngle)
-    <<make<AutoShoot>(_shooter, shooterVolt)
-    <<make<ArmToSetPoint>(_arm, armAngle, wristAngle)
-    <<make<DriveToLocation>(_driveBase, raw, distance)
-  //Shoots starting note then moves out of starting position.
-}
+// std::shared_ptr<Behaviour> Taxi(DriveBase _driveBase, Shooter _shooter, Mag _mag, Intake _intake, Arm _arm) {
+//   return 
+//     <<make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+//     <<make<AutoShoot>(_shooter, shooterVolt)
+//     <<make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+//     <<make<DriveToLocation>(_driveBase, raw, distance)
+//   //Shoots starting note then moves out of starting position.
+// }
 
-std::shared_ptr<Behaviour> QuadrupleClose(DriveBase _driveBase, Shooter _shooter, Mag _mag, Intake _intake, Arm _arm) {
-  return
-    << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
-    << make<AutoShoot>(_shooter, shooterVolt)
-    << make<DriveToLocation>(_driveBase, raw, distance)
-    << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
-    << make<AutoIntake>(_intake, intakeVolt)
-    << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
-    << make<AutoShoot>(_shooter, shooterVolt)
-    << make<DriveToLocation>(_driveBase, raw, distance)
-    << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
-    << make<AuttoIntake>(_intake, intakeVolt)
-    << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
-    << make<AutoShoot>(_shooter, shooterVolt)
-    << make<DriveToLocation>(_driveBase, raw, distance)
-    << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
-    << make<AutoIntake>(_intake, intakeVolt)
-    << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
-    << make<AutoShoo>(_shooter, shooterVolt)
+// std::shared_ptr<Behaviour> QuadrupleClose(DriveBase _driveBase, Shooter _shooter, Mag _mag, Intake _intake, Arm _arm) {
+//   return
+//     << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+//     << make<AutoShoot>(_shooter, shooterVolt)
+//     << make<DriveToLocation>(_driveBase, raw, distance)
+//     << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+//     << make<AutoIntake>(_intake, intakeVolt)
+//     << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+//     << make<AutoShoot>(_shooter, shooterVolt)
+//     << make<DriveToLocation>(_driveBase, raw, distance)
+//     << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+//     << make<AuttoIntake>(_intake, intakeVolt)
+//     << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+//     << make<AutoShoot>(_shooter, shooterVolt)
+//     << make<DriveToLocation>(_driveBase, raw, distance)
+//     << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+//     << make<AutoIntake>(_intake, intakeVolt)
+//     << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+//     << make<AutoShoo>(_shooter, shooterVolt)
 
-    /* 
-      4N Close
-    1. Shoot starting note into speaker
-    2. Intake note from close note
-    3. Shoot note into speaker
-    4. Intake note from close floor note
-    5. Shoot note into speaker
-    6. Intake not from close floor
-    7. Shoot note
-    */
-}
+//     /* 
+//       4N Close
+//     1. Shoot starting note into speaker
+//     2. Intake note from close note
+//     3. Shoot note into speaker
+//     4. Intake note from close floor note
+//     5. Shoot note into speaker
+//     6. Intake not from close floor
+//     7. Shoot note
+//     */
+// }
 
-std::shared_ptr<Behaviour> QuadrupleFar(DriveBase _driveBase, Shooter _shooter, Mag _mag, Intake _intake, Arm _arm) {
-  return
-    << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
-    << make<AutoShoot>(_shooter, shooterVolt)
-    << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
-    << make<DriveToLocation>(_driveBase, raw, distance)
-    << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
-    << make<AutoIntake>(_intake, intakeVolt)
-    //<< make<ArmToSetPoint>(_arm, armAngle, wristAngle)
-    //<< make<DriveToLocation>(_driveBase, raw, distance)
-    << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
-    << make<AutoShoot>(_shooter, shooterVolt)
-    << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
-    << make<DriveToLocation>(_driveBase, raw, distance)
-    << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
-    << make<AutoIntake>(_intake, intakeVolt)
-    //<< make<ArmToSetPoint>(_arm, armAngle, wristAngle)
-    //<< make<DriveToLocation>(_driveBase, raw, distance)
-    << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
-    << make<AutoShoot>(_shooter, shooterVolt)
-    << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
-    << make<DriveToLocation>(_driveBase, raw, distance)
-    << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
-    << make<AutoIntake>(_intake, intakeVolt)
-    //<< make<ArmToSetPoint>(_arm, armAngle, wristAngle)
-    //<< make<DriveToLocation>(_driveBase, raw, distance)
-    << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
-    << make<AutoShoot>(_shooter, shooterVolt)
-    // << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
-    // << make<DriveToLocation>(_driveBase, raw, distance)   do this if possible
-    // << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
-    // << make<AutoIntake>(_intake, intakeVolt)
+// std::shared_ptr<Behaviour> QuadrupleFar(DriveBase _driveBase, Shooter _shooter, Mag _mag, Intake _intake, Arm _arm) {
+//   return
+//     << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+//     << make<AutoShoot>(_shooter, shooterVolt)
+//     << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+//     << make<DriveToLocation>(_driveBase, raw, distance)
+//     << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+//     << make<AutoIntake>(_intake, intakeVolt)
+//     //<< make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+//     //<< make<DriveToLocation>(_driveBase, raw, distance)
+//     << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+//     << make<AutoShoot>(_shooter, shooterVolt)
+//     << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+//     << make<DriveToLocation>(_driveBase, raw, distance)
+//     << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+//     << make<AutoIntake>(_intake, intakeVolt)
+//     //<< make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+//     //<< make<DriveToLocation>(_driveBase, raw, distance)
+//     << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+//     << make<AutoShoot>(_shooter, shooterVolt)
+//     << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+//     << make<DriveToLocation>(_driveBase, raw, distance)
+//     << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+//     << make<AutoIntake>(_intake, intakeVolt)
+//     //<< make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+//     //<< make<DriveToLocation>(_driveBase, raw, distance)
+//     << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+//     << make<AutoShoot>(_shooter, shooterVolt)
+//     // << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+//     // << make<DriveToLocation>(_driveBase, raw, distance)   do this if possible
+//     // << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+//     // << make<AutoIntake>(_intake, intakeVolt)
 
-    /*
-      4N Far
-    1. Shoot start note in speaker
-    2. Drive to far note
-    3. Intake note
-    4. Drive back to shooting line
-    5. Shoot note into speaker
-    6. Drive to note
-    7. Intake note
-    8. Drive to shooting line
-    9. Shoot note
-    10. Drive to note
-    11. Intake note
-    12. Drive to shooting line
-    13. Shoot note
-    14. Drive to intake note (if possible)
-    */
-}
+//     /*
+//       4N Far
+//     1. Shoot start note in speaker
+//     2. Drive to far note
+//     3. Intake note
+//     4. Drive back to shooting line
+//     5. Shoot note into speaker
+//     6. Drive to note
+//     7. Intake note
+//     8. Drive to shooting line
+//     9. Shoot note
+//     10. Drive to note
+//     11. Intake note
+//     12. Drive to shooting line
+//     13. Shoot note
+//     14. Drive to intake note (if possible)
+//     */
+// }
 
-std::shared_ptr<Behaviour> QuadrupleCloseDoubleFar(DriveBase _driveBase, Shooter _shooter, Mag _mag, Intake _intake, Arm _arm) {
-  return
-    << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
-    << make<AutoShoot>(_shooter, shooterVolt)
-    << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
-    << make<DriveToLocation>(_driveBase, raw, distance)
-    << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
-    << make<AutoIntake>(_intake, intakeVolt)
-    //<< make<ArmToSetPoint>(_arm, armAngle, wristAngle)
-    //<< make<DriveToLocation>(_driveBase, raw, distance)
-    << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
-    << make<AutoShoot>(_shooter, shooterVolt)
-    << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
-    << make<DriveToLocation>(_driveBase, raw, distance)
-    << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
-    << make<AutoIntake>(_intake, intakeVolt)
-    //<< make<ArmToSetPoint>(_arm, armAngle, wristAngle)
-    //<< make<DriveToLocation>(_driveBase, raw, distance)
-    << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
-    << make<AutoShoot>(_shooter, shooterVolt)
-    << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
-    << make<DriveToLocation>(_driveBase, raw, distance)
-    << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
-    << make<AutoIntake>(_intake, intakeVolt)
-    //<< make<ArmToSetPoint>(_arm, armAngle, wristAngle)
-    //<< make<DriveToLocation>(_driveBase, raw, distance)
-    << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
-    << make<AutoShoot>(_shooter, shooterVolt)
-    << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
-    << make<DriveToLocation>(_driveBase, raw, distance)
-    << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
-    << make<AutoIntake>(_intake, intakeVolt)
-    << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
-    << make<DriveToLocation>(_driveBase, raw, distance)
-    << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
-    << make<AutoShoot>(_shooter, shooterVolt)
-    << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
-    << make<DriveToLocation>(_driveBase, raw, distance)
-    << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
-    << make<AutoIntake>(_intake, intakeVolt)
-    << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
-    << make<DriveToLocation>(_driveBase, raw, distance)
-    << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
-    << make<AutoShoot>(_shooter, shooterVolt)
-    << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
-    << make<DriveToLocation>(_driveBase, raw, distance)
-    << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
-    << make<AutoIntake>(_intake, intakeVolt)
-    << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
-    << make<AutoShoot>(_shooter, shooterVolt)
-    /*
-      4N Close 2N Far
-    1. Shoot note
-    2. Drive to close note
-    3. Intake note
-    4. Shoot note
-    5. Drive to close note
-    6. Intake note
-    7. Shoot note
-    8. Drive to close note
-    9. Intake note
-    10. Shoot note
-    11. Drive to far note
-    12. Intake note
-    13. Drive to shooting line
-    14. Shoot note
-    15. Drive to far note
-    16. Intake note
-    17. Drive to shooting line
-    18. Shoot note
-    */
-}
+// std::shared_ptr<Behaviour> QuadrupleCloseDoubleFar(DriveBase _driveBase, Shooter _shooter, Mag _mag, Intake _intake, Arm _arm) {
+//   return
+//     << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+//     << make<AutoShoot>(_shooter, shooterVolt)
+//     << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+//     << make<DriveToLocation>(_driveBase, raw, distance)
+//     << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+//     << make<AutoIntake>(_intake, intakeVolt)
+//     //<< make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+//     //<< make<DriveToLocation>(_driveBase, raw, distance)
+//     << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+//     << make<AutoShoot>(_shooter, shooterVolt)
+//     << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+//     << make<DriveToLocation>(_driveBase, raw, distance)
+//     << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+//     << make<AutoIntake>(_intake, intakeVolt)
+//     //<< make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+//     //<< make<DriveToLocation>(_driveBase, raw, distance)
+//     << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+//     << make<AutoShoot>(_shooter, shooterVolt)
+//     << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+//     << make<DriveToLocation>(_driveBase, raw, distance)
+//     << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+//     << make<AutoIntake>(_intake, intakeVolt)
+//     //<< make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+//     //<< make<DriveToLocation>(_driveBase, raw, distance)
+//     << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+//     << make<AutoShoot>(_shooter, shooterVolt)
+//     << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+//     << make<DriveToLocation>(_driveBase, raw, distance)
+//     << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+//     << make<AutoIntake>(_intake, intakeVolt)
+//     << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+//     << make<DriveToLocation>(_driveBase, raw, distance)
+//     << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+//     << make<AutoShoot>(_shooter, shooterVolt)
+//     << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+//     << make<DriveToLocation>(_driveBase, raw, distance)
+//     << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+//     << make<AutoIntake>(_intake, intakeVolt)
+//     << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+//     << make<DriveToLocation>(_driveBase, raw, distance)
+//     << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+//     << make<AutoShoot>(_shooter, shooterVolt)
+//     << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+//     << make<DriveToLocation>(_driveBase, raw, distance)
+//     << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+//     << make<AutoIntake>(_intake, intakeVolt)
+//     << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+//     << make<AutoShoot>(_shooter, shooterVolt)
+//     /*
+//       4N Close 2N Far
+//     1. Shoot note
+//     2. Drive to close note
+//     3. Intake note
+//     4. Shoot note
+//     5. Drive to close note
+//     6. Intake note
+//     7. Shoot note
+//     8. Drive to close note
+//     9. Intake note
+//     10. Shoot note
+//     11. Drive to far note
+//     12. Intake note
+//     13. Drive to shooting line
+//     14. Shoot note
+//     15. Drive to far note
+//     16. Intake note
+//     17. Drive to shooting line
+//     18. Shoot note
+//     */
+// }
 
-std::shared_ptr<Behaviour> QuadrupleCloseSingleFar(DriveBase _driveBase, Shooter _shooter, Mag _mag, Intake _intake, Arm _arm) {
-  return
-    << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
-    << make<AutoShoot>(_shooter, shooterVolt)
-    << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
-    << make<DriveToLocation>(_driveBase, raw, distance)
-    << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
-    << make<AutoIntake>(_intake, intakeVolt)
-    << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
-    << make<DriveToLocation>(_driveBase, raw, distance)
-    << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
-    << make<AutoShoot>(_shooter, shooterVolt)
-    << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
-    << make<DriveToLocation>(_driveBase, raw, distance)
-    << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
-    << make<AutoIntake>(_intake, intakeVolt)
-    << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
-    << make<DriveToLocation>(_driveBase, raw, distance)
-    << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
-    << make<AutoShoot>(_shooter, shooterVolt)
-    << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
-    << make<DriveToLocation>(_driveBase, raw, distance)
-    << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
-    << make<AutoIntake>(_intake, intakeVolt)
-    << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
-    << make<DriveToLocation>(_driveBase, raw, distance)
-    << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
-    << make<AutoShoot>(_shooter, shooterVolt)
-    << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+// std::shared_ptr<Behaviour> QuadrupleCloseSingleFar(DriveBase _driveBase, Shooter _shooter, Mag _mag, Intake _intake, Arm _arm) {
+//   return
+//     << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+//     << make<AutoShoot>(_shooter, shooterVolt)
+//     << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+//     << make<DriveToLocation>(_driveBase, raw, distance)
+//     << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+//     << make<AutoIntake>(_intake, intakeVolt)
+//     << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+//     << make<DriveToLocation>(_driveBase, raw, distance)
+//     << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+//     << make<AutoShoot>(_shooter, shooterVolt)
+//     << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+//     << make<DriveToLocation>(_driveBase, raw, distance)
+//     << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+//     << make<AutoIntake>(_intake, intakeVolt)
+//     << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+//     << make<DriveToLocation>(_driveBase, raw, distance)
+//     << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+//     << make<AutoShoot>(_shooter, shooterVolt)
+//     << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+//     << make<DriveToLocation>(_driveBase, raw, distance)
+//     << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+//     << make<AutoIntake>(_intake, intakeVolt)
+//     << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+//     << make<DriveToLocation>(_driveBase, raw, distance)
+//     << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+//     << make<AutoShoot>(_shooter, shooterVolt)
+//     << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
 
-    /*
-    4N Close 1N Far
-    1. Shoot note
-    2. Drive to close note
-    3. Intake note
-    4. Drive to speaker
-    5. Shoot note
-    6. Drive to close note
-    7. Intake note
-    8. Drive to speaker
-    9. Shoot note
-    10. Drive to close note
-    11. Intake note
-    12. Drive to speaker
-    13. Shoot note
-    14. Drive to far note
-    15. Intake note
-    15. Drive to speaker
-    16. shoot 
-    */
-}
+//     /*
+//     4N Close 1N Far
+//     1. Shoot note
+//     2. Drive to close note
+//     3. Intake note
+//     4. Drive to speaker
+//     5. Shoot note
+//     6. Drive to close note
+//     7. Intake note
+//     8. Drive to speaker
+//     9. Shoot note
+//     10. Drive to close note
+//     11. Intake note
+//     12. Drive to speaker
+//     13. Shoot note
+//     14. Drive to far note
+//     15. Intake note
+//     15. Drive to speaker
+//     16. shoot 
+//     */
+// }
 
-std::shared_ptr<Behaviour> TrapAuto(DriveBase _driveBase, Shooter _shooter, Mag _mag, Intake _intake, Arm _arm) {
-  return 
-    << make<DriveToLocation>(_driveBase, raw, distance)
-    << make<>
-    << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
-    << make<AutoShoot>(_shooter, shooterVolt)
-    << make<>
-    << make<DriveToLocation>(_driveBase, raw, distance)
-    << make<AutoIntake>(_intake, intakeVolt)
-    << make<DriveToLocation>(_driveBase, raw, distance)
+// std::shared_ptr<Behaviour> TrapAuto(DriveBase _driveBase, Shooter _shooter, Mag _mag, Intake _intake, Arm _arm) {
+//   return 
+//     << make<DriveToLocation>(_driveBase, raw, distance)
+//     << make<>
+//     << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+//     << make<AutoShoot>(_shooter, shooterVolt)
+//     << make<>
+//     << make<DriveToLocation>(_driveBase, raw, distance)
+//     << make<AutoIntake>(_intake, intakeVolt)
+//     << make<DriveToLocation>(_driveBase, raw, distance)
     
-    /* 
-    TRAP AUTO
-    1. Drive to trap
-    2. Climb up
-    3. Shoot note
-    4. Climb down
-    5. Drive to far note
-    6. Intake note
-    7. Drive to trap
-    8. Climb up
-    9. Shoot note
-    10. Climb down
-    11. Drive to far note
-    12. Intake
-    13. Drive to trap 
-    14. Climb up
-    15. Shoot note
-    16. Climb down
-    */
-}
\ No newline at end of file
+//     /* 
+//     TRAP AUTO
+//     1. Drive to trap
+//     2. Climb up
+//     3. Shoot note
+//     4. Climb down
+//     5. Drive to far note
+//     6. Intake note
+//     7. Drive to trap
+//     8. Climb up
+//     9. Shoot note
+//     10. Climb down
+//     11. Drive to far note
+//     12. Intake
+//     13. Drive to trap 
+//     14. Climb up
+//     15. Shoot note
+//     16. Climb down
+//     */
+// }
\ No newline at end of file
diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp
index db0f6dfe..ca066fbd 100644
--- a/src/main/cpp/Robot.cpp
+++ b/src/main/cpp/Robot.cpp
@@ -20,11 +20,6 @@ static units::second_t lastPeriodic;
 
 void Robot::RobotInit() {
   lastPeriodic = wom::now();
-
-  mag = new Mag(robotmap.magSystem.config);
-  wom::BehaviourScheduler::GetInstance()->Register(mag);
-  mag->SetDefaultBehaviour(
-      [this]() { return wom::make<MagManualControl>(mag, &robotmap.controllers.coDriver); });
 }
 
 void Robot::RobotPeriodic() {
@@ -36,6 +31,7 @@ void Robot::RobotPeriodic() {
 
   _swerveDrive->OnUpdate(dt);
   mag->OnUpdate(dt);
+  climber->OnUpdate(dt);
 }
 
 void Robot::TeleopInit() {
@@ -60,33 +56,26 @@ void Robot::TeleopInit() {
 
   robotmap.swerveBase.gyro->Reset();
 
+  lastPeriodic = wom::now();
+
   _swerveDrive = new wom::SwerveDrive(robotmap.swerveBase.config, frc::Pose2d());
   wom::BehaviourScheduler::GetInstance()->Register(_swerveDrive);
   _swerveDrive->SetDefaultBehaviour(
       [this]() { return wom::make<wom::ManualDrivebase>(_swerveDrive, &robotmap.controllers.driver); });
 
-  lastPeriodic = wom::now();
-
   climber = new Climber(robotmap.climberSystem.config);
   wom::BehaviourScheduler::GetInstance()->Register(climber);
   climber->SetDefaultBehaviour(
       [this]() { return wom::make<ClimberManualControl>(climber, &robotmap.controllers.coDriver); });
+
+  mag = new Mag(robotmap.magSystem.config);
+  wom::BehaviourScheduler::GetInstance()->Register(mag);
+  mag->SetDefaultBehaviour(
+      [this]() { return wom::make<MagManualControl>(mag, &robotmap.controllers.coDriver); });
   // m_driveSim = new wom::TempSimSwerveDrive(&simulation_timer, &m_field);
   // m_driveSim = wom::TempSimSwerveDrive();
 }
 
-void Robot::RobotPeriodic() {
-  units::second_t dt = wom::now() - lastPeriodic;
-  lastPeriodic = wom::now();
-
-  loop.Poll();
-  wom::BehaviourScheduler::GetInstance()->Tick();
-
-  _swerveDrive->OnUpdate(dt);
-
-  climber->OnUpdate(dt);
-}
-
 void Robot::AutonomousInit() {
   // m_driveSim->SetPath(m_path_chooser.GetSelected());
 
@@ -98,14 +87,6 @@ void Robot::AutonomousPeriodic() {
   // m_driveSim->OnUpdate();
 }
 
-void Robot::TeleopInit() {
-  loop.Clear();
-  wom::BehaviourScheduler* scheduler = wom::BehaviourScheduler::GetInstance();
-  scheduler->InterruptAll();
-  // _swerveDrive->OnStart();
-  // sched->InterruptAll();
-}
-
 void Robot::TeleopPeriodic() {}
 
 void Robot::DisabledInit() {}
diff --git a/src/main/include/Auto.h b/src/main/include/Auto.h
index 4d8839ae..55f6664d 100644
--- a/src/main/include/Auto.h
+++ b/src/main/include/Auto.h
@@ -1,17 +1,17 @@
-#pragma once
+// #pragma once
 
-#include "Wombat.h"
+// #include "Wombat.h"
 
-namespace autos {
-  std::shared_ptr<behaviour::Behaviour> Taxi(Drivebase driveBase, Shooter shooter, Mag mag, Intake intake, Arm _arm);
+// namespace autos {
+//   std::shared_ptr<behaviour::Behaviour> Taxi(Drivebase driveBase, Shooter shooter, Mag mag, Intake intake, Arm _arm);
 
-  std::shared_ptr<behaviour::Behaviour> QuadrupleClose(Drivebase driveBase, Shooter shooter, Mag mag, Intake intake, Arm _arm);
+//   std::shared_ptr<behaviour::Behaviour> QuadrupleClose(Drivebase driveBase, Shooter shooter, Mag mag, Intake intake, Arm _arm);
 
-  std::shared_ptr<behaviour::Behaviour> QuadrupleFar(Drivebase driveBase, Shooter shooter, Mag mag, Intake intake, Arm _arm);
+//   std::shared_ptr<behaviour::Behaviour> QuadrupleFar(Drivebase driveBase, Shooter shooter, Mag mag, Intake intake, Arm _arm);
 
-  std::shared_ptr<behaviour::Behaviour> QuadrupleCloseDoubleFar(Drivebase driveBase, Shooter shooter, Mag mag, Intake intake, Arm _arm);
+//   std::shared_ptr<behaviour::Behaviour> QuadrupleCloseDoubleFar(Drivebase driveBase, Shooter shooter, Mag mag, Intake intake, Arm _arm);
 
-  std::shared_ptr<behaviour::Behaviour> QuadrupleCloseSingleFar(Drivebase driveBase, Shooter shooter, Mag mag, Intake intake, Arm _arm);
-}
+//   std::shared_ptr<behaviour::Behaviour> QuadrupleCloseSingleFar(Drivebase driveBase, Shooter shooter, Mag mag, Intake intake, Arm _arm);
+// }
 
 

From 33d85deae42429148984eed0104e8129bda263e0 Mon Sep 17 00:00:00 2001
From: Kingsley Wong <kingsley.wong@student.education.wa.edu.au>
Date: Tue, 23 Jan 2024 20:18:21 +0800
Subject: [PATCH 062/207] Tested on robot and works

---
 src/main/cpp/IntakeBehaviour.cpp | 42 ++++++++++++++++++++------------
 1 file changed, 26 insertions(+), 16 deletions(-)

diff --git a/src/main/cpp/IntakeBehaviour.cpp b/src/main/cpp/IntakeBehaviour.cpp
index 52570649..90867045 100644
--- a/src/main/cpp/IntakeBehaviour.cpp
+++ b/src/main/cpp/IntakeBehaviour.cpp
@@ -22,24 +22,34 @@ void IntakeManualControl::OnTick(units::second_t dt) {
 
   if (_rawControl) {
     _intake->setState(IntakeState::kRaw);
-    _intake->setRaw(_codriver.GetLeftY() * 10_V);
-    std::cout << "Raw" << std::endl;
-  } else {
-    if (_codriver.GetYButtonPressed()) {
-      _intake->setState(IntakeState::kIntake);
-    }
-    if (_codriver.GetAButtonPressed()) {
-      _intake->setState(IntakeState::kPass);
+    if (_codriver.GetLeftBumper()) {
+      _intake->setRaw(10_V);
+    } else if (_codriver.GetRightBumper()) {
+      _intake->setRaw(-10_V);
+    } else {
+      _intake->setRaw(0_V);
     }
+
+    // _intake->setRaw(_codriver.GetLeftBumper() * 10_V);
+    // _intake->setRaw(_codriver.GetRightBumper() * -10_V);
+    std::cout << "Raw" << std::endl;
   }
+  // } else {
+  //   if (_codriver.GetYButtonPressed()) {
+  //     _intake->setState(IntakeState::kIntake);
+  //   }
+  //   if (_codriver.GetAButtonPressed()) {
+  //     _intake->setState(IntakeState::kPass);
+  //   }
+  // }
 }
 
-IntakeAutoControl::IntakeAutoControl(Intake* intake) : _intake(intake) {}
+// IntakeAutoControl::IntakeAutoControl(Intake* intake) : _intake(intake) {}
 
-void IntakeAutoControl::OnTick(units::second_t dt) {
-  if (_intake->GetConfig().intakeSensor->Get() == 1) {
-    _intake->setState(IntakeState::kPass);
-  } else if (_intake->GetConfig().magSensor->Get() == 0) {
-    _intake->setState(IntakeState::kIdle);
-  }
-}
+// void IntakeAutoControl::OnTick(units::second_t dt) {
+//   if (_intake->GetConfig().intakeSensor->Get() == 1) {
+//     _intake->setState(IntakeState::kPass);
+//   } else if (_intake->GetConfig().magSensor->Get() == 0) {
+//     _intake->setState(IntakeState::kIdle);
+//   }
+// }

From 52eba455c9a71e218c49b684f9a1b5e7f5b8f62c Mon Sep 17 00:00:00 2001
From: Anna Pedersen <59428185+goanna247@users.noreply.github.com>
Date: Tue, 23 Jan 2024 20:21:33 +0800
Subject: [PATCH 063/207] Swerve fix (#106)

* fixed swerve

* wrapping function
---
 .clang-format                                 |   2 +-
 .editorconfig                                 |   5 +
 .github/workflows/format.yml                  |   2 +-
 build.gradle                                  |   2 +-
 gradlew                                       |   0
 gradlew.bat                                   | 184 +++++++--------
 init.ps1                                      |   2 +
 init.sh                                       |   5 +
 src/main/cpp/Robot.cpp                        |  52 ++++-
 src/main/include/Robot.h                      |  10 +-
 src/main/include/RobotMap.h                   | 100 +++++----
 wombat/LICENSE                                |  21 ++
 wombat/README.md                              |   1 +
 wombat/settings.gradle                        |   1 +
 wombat/src/main/cpp/behaviour/Behaviour.cpp   |  23 +-
 .../main/cpp/behaviour/BehaviourScheduler.cpp |   9 +-
 .../src/main/cpp/behaviour/HasBehaviour.cpp   |   3 +-
 wombat/src/main/cpp/drivetrain/Drivetrain.cpp |  15 +-
 .../src/main/cpp/drivetrain/SwerveDrive.cpp   | 209 ++++++++++++------
 .../behaviours/SwerveBehaviours.cpp           | 169 +++++++-------
 wombat/src/main/cpp/subsystems/Arm.cpp        |  39 ++--
 wombat/src/main/cpp/subsystems/Elevator.cpp   |  34 +--
 wombat/src/main/cpp/subsystems/Shooter.cpp    |  24 +-
 wombat/src/main/cpp/utils/Encoder.cpp         |  84 ++++---
 wombat/src/main/cpp/utils/Util.cpp            |  27 ++-
 wombat/src/main/cpp/vision/Limelight.cpp      |  24 +-
 wombat/src/main/include/behaviour/Behaviour.h |  42 ++--
 .../src/main/include/behaviour/HasBehaviour.h |   3 +-
 .../src/main/include/drivetrain/SwerveDrive.h |  41 ++--
 .../drivetrain/behaviours/SwerveBehaviours.h  |  17 +-
 wombat/src/main/include/subsystems/Arm.h      |   3 +-
 wombat/src/main/include/subsystems/Shooter.h  |   3 +-
 wombat/src/main/include/utils/Encoder.h       |  38 ++--
 wombat/src/main/include/utils/PID.h           |  82 ++++---
 wombat/src/main/include/utils/Util.h          |  55 +++--
 wombat/src/main/include/vision/Limelight.h    |  22 +-
 36 files changed, 838 insertions(+), 515 deletions(-)
 create mode 100644 .editorconfig
 mode change 100755 => 100644 gradlew
 create mode 100644 init.ps1
 create mode 100644 init.sh
 create mode 100644 wombat/LICENSE
 create mode 100644 wombat/README.md
 create mode 100644 wombat/settings.gradle

diff --git a/.clang-format b/.clang-format
index a879dec4..f69bef0c 100644
--- a/.clang-format
+++ b/.clang-format
@@ -81,7 +81,7 @@ BreakBeforeTernaryOperators: true
 BreakConstructorInitializers: BeforeColon
 BreakInheritanceList: BeforeColon
 BreakStringLiterals: true
-ColumnLimit:     110
+ColumnLimit:     80
 CommentPragmas:  '^ IWYU pragma:'
 CompactNamespaces: false
 ConstructorInitializerIndentWidth: 4
diff --git a/.editorconfig b/.editorconfig
new file mode 100644
index 00000000..0020fc03
--- /dev/null
+++ b/.editorconfig
@@ -0,0 +1,5 @@
+root = true
+
+[*]
+indent_style = space
+indent_size = 2
diff --git a/.github/workflows/format.yml b/.github/workflows/format.yml
index f57ddeb4..c6fced20 100644
--- a/.github/workflows/format.yml
+++ b/.github/workflows/format.yml
@@ -21,7 +21,7 @@ jobs:
         with:
           python-version: 3.8
       - name: Install wpiformat
-        run: pip3 install wpiformat==2024.31
+        run: pip3 install wpiformat==2023.36
       - name: Run
         run: wpiformat
       - name: Check output
diff --git a/build.gradle b/build.gradle
index 84ccc0cb..91b6acb4 100644
--- a/build.gradle
+++ b/build.gradle
@@ -1,7 +1,7 @@
 plugins {
     id "cpp"
     id "google-test-test-suite"
-    id "edu.wpi.first.GradleRIO" version "2024.2.1"
+    id "edu.wpi.first.GradleRIO" version "2024.1.1"
 }
 
 // Define my targets (RoboRIO) and artifacts (deployable files)
diff --git a/gradlew b/gradlew
old mode 100755
new mode 100644
diff --git a/gradlew.bat b/gradlew.bat
index 6689b85b..93e3f59f 100644
--- a/gradlew.bat
+++ b/gradlew.bat
@@ -1,92 +1,92 @@
-@rem
-@rem Copyright 2015 the original author or authors.
-@rem
-@rem Licensed under the Apache License, Version 2.0 (the "License");
-@rem you may not use this file except in compliance with the License.
-@rem You may obtain a copy of the License at
-@rem
-@rem      https://www.apache.org/licenses/LICENSE-2.0
-@rem
-@rem Unless required by applicable law or agreed to in writing, software
-@rem distributed under the License is distributed on an "AS IS" BASIS,
-@rem WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-@rem See the License for the specific language governing permissions and
-@rem limitations under the License.
-@rem
-
-@if "%DEBUG%"=="" @echo off
-@rem ##########################################################################
-@rem
-@rem  Gradle startup script for Windows
-@rem
-@rem ##########################################################################
-
-@rem Set local scope for the variables with windows NT shell
-if "%OS%"=="Windows_NT" setlocal
-
-set DIRNAME=%~dp0
-if "%DIRNAME%"=="" set DIRNAME=.
-@rem This is normally unused
-set APP_BASE_NAME=%~n0
-set APP_HOME=%DIRNAME%
-
-@rem Resolve any "." and ".." in APP_HOME to make it shorter.
-for %%i in ("%APP_HOME%") do set APP_HOME=%%~fi
-
-@rem Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script.
-set DEFAULT_JVM_OPTS="-Xmx64m" "-Xms64m"
-
-@rem Find java.exe
-if defined JAVA_HOME goto findJavaFromJavaHome
-
-set JAVA_EXE=java.exe
-%JAVA_EXE% -version >NUL 2>&1
-if %ERRORLEVEL% equ 0 goto execute
-
-echo.
-echo ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH.
-echo.
-echo Please set the JAVA_HOME variable in your environment to match the
-echo location of your Java installation.
-
-goto fail
-
-:findJavaFromJavaHome
-set JAVA_HOME=%JAVA_HOME:"=%
-set JAVA_EXE=%JAVA_HOME%/bin/java.exe
-
-if exist "%JAVA_EXE%" goto execute
-
-echo.
-echo ERROR: JAVA_HOME is set to an invalid directory: %JAVA_HOME%
-echo.
-echo Please set the JAVA_HOME variable in your environment to match the
-echo location of your Java installation.
-
-goto fail
-
-:execute
-@rem Setup the command line
-
-set CLASSPATH=%APP_HOME%\gradle\wrapper\gradle-wrapper.jar
-
-
-@rem Execute Gradle
-"%JAVA_EXE%" %DEFAULT_JVM_OPTS% %JAVA_OPTS% %GRADLE_OPTS% "-Dorg.gradle.appname=%APP_BASE_NAME%" -classpath "%CLASSPATH%" org.gradle.wrapper.GradleWrapperMain %*
-
-:end
-@rem End local scope for the variables with windows NT shell
-if %ERRORLEVEL% equ 0 goto mainEnd
-
-:fail
-rem Set variable GRADLE_EXIT_CONSOLE if you need the _script_ return code instead of
-rem the _cmd.exe /c_ return code!
-set EXIT_CODE=%ERRORLEVEL%
-if %EXIT_CODE% equ 0 set EXIT_CODE=1
-if not ""=="%GRADLE_EXIT_CONSOLE%" exit %EXIT_CODE%
-exit /b %EXIT_CODE%
-
-:mainEnd
-if "%OS%"=="Windows_NT" endlocal
-
-:omega
+@rem
+@rem Copyright 2015 the original author or authors.
+@rem
+@rem Licensed under the Apache License, Version 2.0 (the "License");
+@rem you may not use this file except in compliance with the License.
+@rem You may obtain a copy of the License at
+@rem
+@rem      https://www.apache.org/licenses/LICENSE-2.0
+@rem
+@rem Unless required by applicable law or agreed to in writing, software
+@rem distributed under the License is distributed on an "AS IS" BASIS,
+@rem WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+@rem See the License for the specific language governing permissions and
+@rem limitations under the License.
+@rem
+
+@if "%DEBUG%"=="" @echo off
+@rem ##########################################################################
+@rem
+@rem  Gradle startup script for Windows
+@rem
+@rem ##########################################################################
+
+@rem Set local scope for the variables with windows NT shell
+if "%OS%"=="Windows_NT" setlocal
+
+set DIRNAME=%~dp0
+if "%DIRNAME%"=="" set DIRNAME=.
+@rem This is normally unused
+set APP_BASE_NAME=%~n0
+set APP_HOME=%DIRNAME%
+
+@rem Resolve any "." and ".." in APP_HOME to make it shorter.
+for %%i in ("%APP_HOME%") do set APP_HOME=%%~fi
+
+@rem Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script.
+set DEFAULT_JVM_OPTS="-Xmx64m" "-Xms64m"
+
+@rem Find java.exe
+if defined JAVA_HOME goto findJavaFromJavaHome
+
+set JAVA_EXE=java.exe
+%JAVA_EXE% -version >NUL 2>&1
+if %ERRORLEVEL% equ 0 goto execute
+
+echo.
+echo ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH.
+echo.
+echo Please set the JAVA_HOME variable in your environment to match the
+echo location of your Java installation.
+
+goto fail
+
+:findJavaFromJavaHome
+set JAVA_HOME=%JAVA_HOME:"=%
+set JAVA_EXE=%JAVA_HOME%/bin/java.exe
+
+if exist "%JAVA_EXE%" goto execute
+
+echo.
+echo ERROR: JAVA_HOME is set to an invalid directory: %JAVA_HOME%
+echo.
+echo Please set the JAVA_HOME variable in your environment to match the
+echo location of your Java installation.
+
+goto fail
+
+:execute
+@rem Setup the command line
+
+set CLASSPATH=%APP_HOME%\gradle\wrapper\gradle-wrapper.jar
+
+
+@rem Execute Gradle
+"%JAVA_EXE%" %DEFAULT_JVM_OPTS% %JAVA_OPTS% %GRADLE_OPTS% "-Dorg.gradle.appname=%APP_BASE_NAME%" -classpath "%CLASSPATH%" org.gradle.wrapper.GradleWrapperMain %*
+
+:end
+@rem End local scope for the variables with windows NT shell
+if %ERRORLEVEL% equ 0 goto mainEnd
+
+:fail
+rem Set variable GRADLE_EXIT_CONSOLE if you need the _script_ return code instead of
+rem the _cmd.exe /c_ return code!
+set EXIT_CODE=%ERRORLEVEL%
+if %EXIT_CODE% equ 0 set EXIT_CODE=1
+if not ""=="%GRADLE_EXIT_CONSOLE%" exit %EXIT_CODE%
+exit /b %EXIT_CODE%
+
+:mainEnd
+if "%OS%"=="Windows_NT" endlocal
+
+:omega
diff --git a/init.ps1 b/init.ps1
new file mode 100644
index 00000000..fa83b9a6
--- /dev/null
+++ b/init.ps1
@@ -0,0 +1,2 @@
+./gradlew installRoborioToolchain
+./gradlew build
diff --git a/init.sh b/init.sh
new file mode 100644
index 00000000..6a932ed6
--- /dev/null
+++ b/init.sh
@@ -0,0 +1,5 @@
+#!/usr/bin/sh
+sudo apt update
+chmod +x gradlew
+./gradlew installRoborioToolchain
+./gradlew build
diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp
index 80f253d5..ddccb14d 100644
--- a/src/main/cpp/Robot.cpp
+++ b/src/main/cpp/Robot.cpp
@@ -15,10 +15,12 @@
 #include <frc/kinematics/DifferentialDriveKinematics.h>
 #include <frc/controller/RamseteController.h>
 #include <frc/Timer.h>
+#include "behaviour/HasBehaviour.h"
 
 static units::second_t lastPeriodic;
 
 void Robot::RobotInit() {
+  sched = wom::BehaviourScheduler::GetInstance();
   m_chooser.SetDefaultOption("Default Auto", "Default Auto");
 
   frc::SmartDashboard::PutData("Auto Selector", &m_chooser);
@@ -36,13 +38,34 @@ void Robot::RobotInit() {
 
   robotmap.swerveBase.gyro->Reset();
 
-  _swerveDrive = new wom::SwerveDrive(robotmap.swerveBase.config, frc::Pose2d());
+  _swerveDrive =
+      new wom::SwerveDrive(robotmap.swerveBase.config, frc::Pose2d());
   wom::BehaviourScheduler::GetInstance()->Register(_swerveDrive);
-  _swerveDrive->SetDefaultBehaviour(
-      [this]() { return wom::make<wom::ManualDrivebase>(_swerveDrive, &robotmap.controllers.driver); });
+  _swerveDrive->SetDefaultBehaviour([this]() {
+    return wom::make<wom::ManualDrivebase>(_swerveDrive,
+                                           &robotmap.controllers.driver);
+  });
 
+  
   // m_driveSim = new wom::TempSimSwerveDrive(&simulation_timer, &m_field);
   // m_driveSim = wom::TempSimSwerveDrive();
+
+  robotmap.swerveBase.moduleConfigs[0].turnMotor.encoder->SetEncoderOffset(0_rad);
+  robotmap.swerveBase.moduleConfigs[1].turnMotor.encoder->SetEncoderOffset(0_rad);
+  robotmap.swerveBase.moduleConfigs[2].turnMotor.encoder->SetEncoderOffset(0_rad);
+  robotmap.swerveBase.moduleConfigs[3].turnMotor.encoder->SetEncoderOffset(0_rad);
+
+
+
+
+  // frontLeft = new ctre::phoenix6::hardware::TalonFX(7, "Drivebase");  // front left
+  // frontRight = new ctre::phoenix6::hardware::TalonFX(2, "Drivebase");   // front right
+  // backLeft = new ctre::phoenix6::hardware::TalonFX(6, "Drivebase");   // back left
+  // backRight = new ctre::phoenix6::hardware::TalonFX(4, "Drivebase");  // back right
+  // frontLeft = new ctre::phoenix6::hardware::TalonFX(9, "Drivebase");   // front left
+  // frontRight = new ctre::phoenix6::hardware::TalonFX(1, "Drivebase");   // front right
+  // backLeft = new ctre::phoenix6::hardware::TalonFX(5, "Drivebase");   // back left
+  // backRight = new ctre::phoenix6::hardware::TalonFX(3, "Drivebase");
 }
 
 void Robot::RobotPeriodic() {
@@ -50,7 +73,12 @@ void Robot::RobotPeriodic() {
   lastPeriodic = wom::now();
 
   loop.Poll();
-  wom::BehaviourScheduler::GetInstance()->Tick();
+  sched->Tick();
+
+  robotmap.swerveTable.swerveDriveTable->GetEntry("frontLeftEncoder").SetDouble(robotmap.swerveBase.moduleConfigs[0].turnMotor.encoder->GetEncoderPosition().value());
+  robotmap.swerveTable.swerveDriveTable->GetEntry("frontRightEncoder").SetDouble(robotmap.swerveBase.moduleConfigs[1].turnMotor.encoder->GetEncoderPosition().value());
+  robotmap.swerveTable.swerveDriveTable->GetEntry("backLeftEncoder").SetDouble(robotmap.swerveBase.moduleConfigs[2].turnMotor.encoder->GetEncoderPosition().value());
+  robotmap.swerveTable.swerveDriveTable->GetEntry("backRightEncoder").SetDouble(robotmap.swerveBase.moduleConfigs[3].turnMotor.encoder->GetEncoderPosition().value());
 
   _swerveDrive->OnUpdate(dt);
 }
@@ -60,15 +88,19 @@ void Robot::AutonomousInit() {
 
   loop.Clear();
   sched->InterruptAll();
-  // _swerveDrive->OnStart();
 }
 void Robot::AutonomousPeriodic() {
   // m_driveSim->OnUpdate();
 }
 
 void Robot::TeleopInit() {
-  // _swerveDrive->OnStart();
-  // sched->InterruptAll();
+  loop.Clear();
+  sched->InterruptAll();
+
+  // frontLeft->SetVoltage(4_V);
+  // frontRight->SetVoltage(4_V);
+  // backLeft->SetVoltage(4_V);
+  // backRight->SetVoltage(4_V);
 }
 void Robot::TeleopPeriodic() {}
 
@@ -76,8 +108,4 @@ void Robot::DisabledInit() {}
 void Robot::DisabledPeriodic() {}
 
 void Robot::TestInit() {}
-void Robot::TestPeriodic() {}
-
-void Robot::SimulationInit() {}
-
-void Robot::SimulationPeriodic() {}
+void Robot::TestPeriodic() {}
\ No newline at end of file
diff --git a/src/main/include/Robot.h b/src/main/include/Robot.h
index bc98df2a..9cef2cb1 100644
--- a/src/main/include/Robot.h
+++ b/src/main/include/Robot.h
@@ -31,12 +31,9 @@ class Robot : public frc::TimedRobot {
   void DisabledPeriodic() override;
   void TestInit() override;
   void TestPeriodic() override;
-  void SimulationInit() override;
-  void SimulationPeriodic() override;
-
  private:
-  behaviour::BehaviourScheduler* sched;
   RobotMap robotmap;
+  wom::BehaviourScheduler* sched;
   frc::EventLoop loop;
 
   frc::SendableChooser<std::string> m_chooser;
@@ -48,4 +45,9 @@ class Robot : public frc::TimedRobot {
   frc::SendableChooser<std::string> m_path_chooser;
 
   wom::SwerveDrive* _swerveDrive;
+
+  // ctre::phoenix6::hardware::TalonFX *frontLeft;
+  // ctre::phoenix6::hardware::TalonFX *frontRight;
+  // ctre::phoenix6::hardware::TalonFX *backLeft;
+  // ctre::phoenix6::hardware::TalonFX *backRight;
 };
diff --git a/src/main/include/RobotMap.h b/src/main/include/RobotMap.h
index d5efd27a..6e64d5e8 100644
--- a/src/main/include/RobotMap.h
+++ b/src/main/include/RobotMap.h
@@ -1,7 +1,6 @@
 // Copyright (c) 2023-2024 CurtinFRC
 // Open Source Software, you can modify it according to the terms
 // of the MIT License at the root of this project
-
 #pragma once
 
 #include <frc/Compressor.h>
@@ -28,90 +27,99 @@ struct RobotMap {
   Controllers controllers;
 
   struct SwerveBase {
-    ctre::phoenix6::hardware::CANcoder frontLeftCancoder{19};
-    ctre::phoenix6::hardware::CANcoder frontRightCancoder{17};
-    ctre::phoenix6::hardware::CANcoder backLeftCancoder{16};
-    ctre::phoenix6::hardware::CANcoder backRightCancoder{18};
+    ctre::phoenix6::hardware::CANcoder frontLeftCancoder{18, "Drivebase"};
+    ctre::phoenix6::hardware::CANcoder frontRightCancoder{19, "Drivebase"};
+    ctre::phoenix6::hardware::CANcoder backLeftCancoder{16, "Drivebase"};
+    ctre::phoenix6::hardware::CANcoder backRightCancoder{17, "Drivebase"};
 
-    ctre::phoenix6::hardware::Pigeon2* gyro = new ctre::phoenix6::hardware::Pigeon2(20, "Drivebase");
+    ctre::phoenix6::hardware::Pigeon2* gyro =
+        new ctre::phoenix6::hardware::Pigeon2(20, "Drivebase");
     wpi::array<ctre::phoenix6::hardware::TalonFX*, 4> turnMotors{
-        new ctre::phoenix6::hardware::TalonFX(7, "Drivebase"),
-        new ctre::phoenix6::hardware::TalonFX(5, "Drivebase"),
-        new ctre::phoenix6::hardware::TalonFX(1, "Drivebase"),
-        new ctre::phoenix6::hardware::TalonFX(3, "Drivebase")};
+        new ctre::phoenix6::hardware::TalonFX(7, "Drivebase"),   // front left
+        new ctre::phoenix6::hardware::TalonFX(2, "Drivebase"),   // front right
+        new ctre::phoenix6::hardware::TalonFX(6, "Drivebase"),   // back left
+        new ctre::phoenix6::hardware::TalonFX(4, "Drivebase")};  // back right
     wpi::array<ctre::phoenix6::hardware::TalonFX*, 4> driveMotors{
-        new ctre::phoenix6::hardware::TalonFX(9, "Drivebase"),
-        new ctre::phoenix6::hardware::TalonFX(6, "Drivebase"),
-        new ctre::phoenix6::hardware::TalonFX(2, "Drivebase"),
-        new ctre::phoenix6::hardware::TalonFX(4, "Drivebase")};
+        new ctre::phoenix6::hardware::TalonFX(9, "Drivebase"),   // front left
+        new ctre::phoenix6::hardware::TalonFX(1, "Drivebase"),   // front right
+        new ctre::phoenix6::hardware::TalonFX(5, "Drivebase"),   // back left
+        new ctre::phoenix6::hardware::TalonFX(3, "Drivebase")};  // back right
 
     wpi::array<wom::SwerveModuleConfig, 4> moduleConfigs{
-        wom::SwerveModuleConfig{
+        wom::SwerveModuleConfig{ //CORRECT
             // front left module
             frc::Translation2d(10.761_in, 9.455_in),
-            wom::Gearbox{driveMotors[0], new wom::TalonFXEncoder(driveMotors[0], 0.0445_m, 6.75),
-                         frc::DCMotor::Falcon500(1).WithReduction(6.75)},
-            wom::Gearbox{turnMotors[0], new wom::CanEncoder(19, 0.0445_m, 4096, 12.8),
+            wom::Gearbox{
+                driveMotors[0],
+                new wom::TalonFXEncoder(driveMotors[0], 0.0445_m, 6.75),
+                frc::DCMotor::Falcon500(1).WithReduction(6.75)},
+            wom::Gearbox{turnMotors[0],
+                         new wom::CanEncoder(18, 0.0445_m, 4096, 12.8),
                          frc::DCMotor::Falcon500(1).WithReduction(12.8)},
             &frontLeftCancoder, 4_in / 2},
-        wom::SwerveModuleConfig{
+        wom::SwerveModuleConfig{ //CORRECT
             // front right module
             frc::Translation2d(10.761_in, -9.455_in),
-            wom::Gearbox{driveMotors[1], new wom::TalonFXEncoder(driveMotors[1], 0.0445_m, 6.75),
-                         frc::DCMotor::Falcon500(1).WithReduction(6.75)},
-            wom::Gearbox{turnMotors[1], new wom::CanEncoder(17, 0.0445_m, 4096, 12.8),
+            wom::Gearbox{
+                driveMotors[1],
+                new wom::TalonFXEncoder(driveMotors[1], 0.0445_m, 6.75),
+                frc::DCMotor::Falcon500(1).WithReduction(6.75)},
+            wom::Gearbox{turnMotors[1],
+                         new wom::CanEncoder(19, 0.0445_m, 4096, 12.8),
                          frc::DCMotor::Falcon500(1).WithReduction(12.8)},
             &frontRightCancoder, 4_in / 2},
         wom::SwerveModuleConfig{
             // back left module
             frc::Translation2d(-10.761_in, 9.455_in),
-            wom::Gearbox{driveMotors[2], new wom::TalonFXEncoder(driveMotors[2], 0.0445_m, 6.75),
-                         frc::DCMotor::Falcon500(1).WithReduction(6.75)},
-            wom::Gearbox{turnMotors[2], new wom::CanEncoder(16, 0.0445_m, 4096, 12.8),
+            wom::Gearbox{
+                driveMotors[2],
+                new wom::TalonFXEncoder(driveMotors[2], 0.0445_m, 6.75),
+                frc::DCMotor::Falcon500(1).WithReduction(6.75)},
+            wom::Gearbox{turnMotors[2],
+                         new wom::CanEncoder(16, 0.0445_m, 4096, 12.8),
                          frc::DCMotor::Falcon500(1).WithReduction(12.8)},
             &backRightCancoder, 4_in / 2},
         wom::SwerveModuleConfig{
             // back right module
             frc::Translation2d(-10.761_in, -9.455_in),
-            wom::Gearbox{driveMotors[3], new wom::TalonFXEncoder(driveMotors[3], 0.0445_m, 6.75),
-                         frc::DCMotor::Falcon500(1).WithReduction(6.75)},
-            wom::Gearbox{turnMotors[3], new wom::CanEncoder(18, 0.0445_m, 4096, 12.8),
+            wom::Gearbox{
+                driveMotors[3],
+                new wom::TalonFXEncoder(driveMotors[3], 0.0445_m, 6.75),
+                frc::DCMotor::Falcon500(1).WithReduction(6.75)},
+            wom::Gearbox{turnMotors[3],
+                         new wom::CanEncoder(17, 0.0445_m, 4096, 12.8),
                          frc::DCMotor::Falcon500(1).WithReduction(12.8)},
             &backLeftCancoder, 4_in / 2},
     };
 
     // Setting the PID path and values to be used for SwerveDrive and
     // SwerveModules
-    wom::SwerveModule::angle_pid_conf_t anglePID{
-        "/drivetrain/pid/angle/config", 2_V / 360_deg, 0.0_V / (100_deg * 1_s),
-        0_V / (100_deg / 1_s),          1_deg,         0.5_deg / 2_s};
+    /*wom::SwerveModule::angle_pid_conf_t anglePID{
+        "/drivetrain/pid/angle/config", 90_V / 360_deg, 0.0_V / (100_deg * 1_s),
+        0_V / (100_deg / 1_s)};*/
     wom::SwerveModule::velocity_pid_conf_t velocityPID{
         "/drivetrain/pid/velocity/config",
         //  12_V / 4_mps // webers per metre
     };
-    wom::SwerveDriveConfig::pose_angle_conf_t poseAnglePID{
+    /*wom::SwerveDriveConfig::pose_angle_conf_t poseAnglePID{
         "/drivetrain/pid/pose/angle/config",
-        180_deg / 1_s / 45_deg,
-        wom::SwerveDriveConfig::pose_angle_conf_t::ki_t{0.1},
-        0_deg / 1_deg,
-        10_deg,
-        10_deg / 1_s};
+        0_deg / 1_s / 45_deg,
+        wom::SwerveDriveConfig::pose_angle_conf_t::ki_t{0},
+        0_deg / 1_deg};*/
     wom::SwerveDriveConfig::pose_position_conf_t posePositionPID{
         "/drivetrain/pid/pose/position/config",
-        3_mps / 1_m,
+        0_mps / 1_m,
         wom::SwerveDriveConfig::pose_position_conf_t::ki_t{0.15},
         0_m / 1_m,
-        20_cm,
-        10_cm / 1_s,
-        10_cm};
+        0_cm};
 
     // the config for the whole swerve drive
     wom::SwerveDriveConfig config{"/drivetrain",
-                                  anglePID,
+                                  //anglePID,
                                   velocityPID,
                                   moduleConfigs,  // each module
                                   gyro,
-                                  poseAnglePID,
+                                  //poseAnglePID,
                                   posePositionPID,
                                   60_kg,  // robot mass (estimate rn)
                                   {0.1, 0.1, 0.1},
@@ -129,4 +137,8 @@ struct RobotMap {
     //}
   };
   SwerveBase swerveBase;
-};
+
+  struct SwerveTable {
+    std::shared_ptr<nt::NetworkTable> swerveDriveTable = nt::NetworkTableInstance::GetDefault().GetTable("swerve");
+  }; SwerveTable swerveTable;
+};
\ No newline at end of file
diff --git a/wombat/LICENSE b/wombat/LICENSE
new file mode 100644
index 00000000..b1561bab
--- /dev/null
+++ b/wombat/LICENSE
@@ -0,0 +1,21 @@
+MIT License
+
+Copyright (c) 2023 CurtinFRC
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in all
+copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+SOFTWARE.
diff --git a/wombat/README.md b/wombat/README.md
new file mode 100644
index 00000000..0595c6cb
--- /dev/null
+++ b/wombat/README.md
@@ -0,0 +1 @@
+# Wombat
diff --git a/wombat/settings.gradle b/wombat/settings.gradle
new file mode 100644
index 00000000..8b137891
--- /dev/null
+++ b/wombat/settings.gradle
@@ -0,0 +1 @@
+
diff --git a/wombat/src/main/cpp/behaviour/Behaviour.cpp b/wombat/src/main/cpp/behaviour/Behaviour.cpp
index 7312c922..d21d9e57 100644
--- a/wombat/src/main/cpp/behaviour/Behaviour.cpp
+++ b/wombat/src/main/cpp/behaviour/Behaviour.cpp
@@ -10,7 +10,9 @@ using namespace behaviour;
 
 // Behaviour
 Behaviour::Behaviour(std::string name, units::time::second_t period)
-    : _bhvr_name(name), _bhvr_period(period), _bhvr_state(BehaviourState::INITIALISED) {}
+    : _bhvr_name(name),
+      _bhvr_period(period),
+      _bhvr_state(BehaviourState::INITIALISED) {}
 Behaviour::~Behaviour() {
   if (!IsFinished())
     Interrupt();
@@ -80,7 +82,8 @@ bool Behaviour::Tick() {
 
     if (dt > 2 * _bhvr_period) {
       std::cerr << "Behaviour missed deadline. Reduce Period. Dt=" << dt.value()
-                << " Dt(deadline)=" << (2 * _bhvr_period).value() << ". Bhvr: " << GetName() << std::endl;
+                << " Dt(deadline)=" << (2 * _bhvr_period).value()
+                << ". Bhvr: " << GetName() << std::endl;
     }
 
     if (_bhvr_timeout.value() > 0 && _bhvr_timer > _bhvr_timeout)
@@ -97,7 +100,8 @@ bool Behaviour::IsRunning() const {
 }
 
 bool Behaviour::IsFinished() const {
-  return _bhvr_state != BehaviourState::INITIALISED && _bhvr_state != BehaviourState::RUNNING;
+  return _bhvr_state != BehaviourState::INITIALISED &&
+         _bhvr_state != BehaviourState::RUNNING;
 }
 
 void Behaviour::Stop(BehaviourState new_state) {
@@ -169,7 +173,8 @@ void ConcurrentBehaviour::Add(Behaviour::ptr behaviour) {
 }
 
 std::string ConcurrentBehaviour::GetName() const {
-  std::string msg = (_reducer == ConcurrentBehaviourReducer::ALL ? "ALL { " : "RACE {");
+  std::string msg =
+      (_reducer == ConcurrentBehaviourReducer::ALL ? "ALL { " : "RACE {");
   for (auto b : _children)
     msg += b->GetName() + ", ";
   msg += "}";
@@ -185,8 +190,8 @@ void ConcurrentBehaviour::OnStart() {
         using namespace std::chrono_literals;
 
         b->Tick();
-        std::this_thread::sleep_for(
-            std::chrono::milliseconds(static_cast<int64_t>(b->GetPeriod().value() * 1000)));
+        std::this_thread::sleep_for(std::chrono::milliseconds(
+            static_cast<int64_t>(b->GetPeriod().value() * 1000)));
       }
 
       if (IsFinished() && !b->IsFinished())
@@ -268,8 +273,10 @@ void WaitFor::OnTick(units::time::second_t dt) {
 }
 
 // WaitTime
-WaitTime::WaitTime(units::time::second_t time) : WaitTime([time]() { return time; }) {}
-WaitTime::WaitTime(std::function<units::time::second_t()> time_fn) : _time_fn(time_fn) {}
+WaitTime::WaitTime(units::time::second_t time)
+    : WaitTime([time]() { return time; }) {}
+WaitTime::WaitTime(std::function<units::time::second_t()> time_fn)
+    : _time_fn(time_fn) {}
 
 void WaitTime::OnStart() {
   _time = _time_fn();
diff --git a/wombat/src/main/cpp/behaviour/BehaviourScheduler.cpp b/wombat/src/main/cpp/behaviour/BehaviourScheduler.cpp
index d34bf4aa..26863c79 100644
--- a/wombat/src/main/cpp/behaviour/BehaviourScheduler.cpp
+++ b/wombat/src/main/cpp/behaviour/BehaviourScheduler.cpp
@@ -10,7 +10,7 @@ BehaviourScheduler::BehaviourScheduler() {}
 
 BehaviourScheduler::~BehaviourScheduler() {
   for (HasBehaviour* sys : _systems) {
-    if (sys->_active_behaviour)
+    if (sys->_active_behaviour != nullptr)
       sys->_active_behaviour->Interrupt();
   }
 
@@ -52,8 +52,8 @@ void BehaviourScheduler::Schedule(Behaviour::ptr behaviour) {
         std::lock_guard<std::recursive_mutex> lk(_active_mtx);
         behaviour->Tick();
       }
-      std::this_thread::sleep_for(
-          std::chrono::milliseconds(static_cast<int64_t>(behaviour->GetPeriod().value() * 1000)));
+      std::this_thread::sleep_for(std::chrono::milliseconds(
+          static_cast<int64_t>(behaviour->GetPeriod().value() * 1000)));
     }
   });
 }
@@ -78,7 +78,8 @@ void BehaviourScheduler::Tick() {
 void BehaviourScheduler::InterruptAll() {
   std::lock_guard<std::recursive_mutex> lk(_active_mtx);
   for (HasBehaviour* sys : _systems) {
-    if (sys->_active_behaviour)
+    if (sys->_active_behaviour != nullptr) {
       sys->_active_behaviour->Interrupt();
+    }
   }
 }
diff --git a/wombat/src/main/cpp/behaviour/HasBehaviour.cpp b/wombat/src/main/cpp/behaviour/HasBehaviour.cpp
index f2f557c5..964522c6 100644
--- a/wombat/src/main/cpp/behaviour/HasBehaviour.cpp
+++ b/wombat/src/main/cpp/behaviour/HasBehaviour.cpp
@@ -8,7 +8,8 @@
 
 using namespace behaviour;
 
-void HasBehaviour::SetDefaultBehaviour(std::function<std::shared_ptr<Behaviour>(void)> fn) {
+void HasBehaviour::SetDefaultBehaviour(
+    std::function<std::shared_ptr<Behaviour>(void)> fn) {
   _default_behaviour_producer = fn;
 }
 
diff --git a/wombat/src/main/cpp/drivetrain/Drivetrain.cpp b/wombat/src/main/cpp/drivetrain/Drivetrain.cpp
index 4a5cb744..e516e7d7 100644
--- a/wombat/src/main/cpp/drivetrain/Drivetrain.cpp
+++ b/wombat/src/main/cpp/drivetrain/Drivetrain.cpp
@@ -11,7 +11,8 @@
 using namespace frc;
 using namespace units;
 
-wom::drivetrain::Drivetrain::Drivetrain(DrivetrainConfig* config, XboxController& driver)
+wom::drivetrain::Drivetrain::Drivetrain(DrivetrainConfig* config,
+                                        XboxController& driver)
     : _config(config), _driver(driver) {}
 wom::drivetrain::Drivetrain::~Drivetrain() {}
 
@@ -37,12 +38,12 @@ void wom::drivetrain::Drivetrain::OnUpdate(second_t dt) {
     case DrivetrainState::kTank: {
       double rightSpeed = wom::utils::deadzone(_driver.GetRightY());
       double leftSpeed = wom::utils::deadzone(_driver.GetLeftY());
-      _config->left1.motorController->SetVoltage(leftSpeed * maxVolts);
-      _config->left2.motorController->SetVoltage(leftSpeed * maxVolts);
-      _config->left3.motorController->SetVoltage(leftSpeed * maxVolts);
-      _config->right1.motorController->SetVoltage(rightSpeed * maxVolts);
-      _config->right2.motorController->SetVoltage(rightSpeed * maxVolts);
-      _config->right3.motorController->SetVoltage(rightSpeed * maxVolts);
+      // _config->left1.motorController->SetVoltage(leftSpeed * maxVolts);
+      // _config->left2.motorController->SetVoltage(leftSpeed * maxVolts);
+      // _config->left3.motorController->SetVoltage(leftSpeed * maxVolts);
+      // _config->right1.motorController->SetVoltage(rightSpeed * maxVolts);
+      // _config->right2.motorController->SetVoltage(rightSpeed * maxVolts);
+      // _config->right3.motorController->SetVoltage(rightSpeed * maxVolts);
       break;
     }
     case DrivetrainState::kAuto:
diff --git a/wombat/src/main/cpp/drivetrain/SwerveDrive.cpp b/wombat/src/main/cpp/drivetrain/SwerveDrive.cpp
index 7af1ff39..eb33efb1 100644
--- a/wombat/src/main/cpp/drivetrain/SwerveDrive.cpp
+++ b/wombat/src/main/cpp/drivetrain/SwerveDrive.cpp
@@ -6,6 +6,8 @@
 
 #include <networktables/NetworkTableInstance.h>
 #include <units/math.h>
+#include <units/voltage.h>
+#include <units/angle.h>
 
 #include <iostream>
 
@@ -14,24 +16,28 @@
 namespace wom {
 namespace drivetrain {
 
-void SwerveModuleConfig::WriteNT(std::shared_ptr<nt::NetworkTable> table) const {
+void SwerveModuleConfig::WriteNT(
+    std::shared_ptr<nt::NetworkTable> table) const {
   std::array<double, 2> pos{position.X().value(), position.Y().value()};
   table->GetEntry("position").SetDoubleArray(std::span(pos));
   table->GetEntry("wheelRadius").SetDouble(wheelRadius.value());
 }
 
 SwerveModule::SwerveModule(std::string path, SwerveModuleConfig config,
-                           SwerveModule::angle_pid_conf_t anglePID,
+                           //SwerveModule::angle_pid_conf_t anglePID,
                            SwerveModule::velocity_pid_conf_t velocityPID)
-    : _config(config),
-      _anglePIDController(path + "/pid/angle", anglePID),
-      _velocityPIDController(path + "/pid/velocity", velocityPID),
+    : //_anglePIDController(path + "/pid/angle", anglePID),
+      _anglePIDController{frc::PIDController(4, 0, 0, 0.005_s)},
+      _config(config),
+      _velocityPIDController(frc::PIDController(1.2, 0, 0, 0.005_s)),
       _table(nt::NetworkTableInstance::GetDefault().GetTable(path)) {
-  _anglePIDController.SetWrap(360_deg);
+  // _anglePIDController.(2 * 3.1415);
+  _anglePIDController.EnableContinuousInput(0, (2 * 3.1415));
 }
 
 void SwerveModule::OnStart() {
   // _offset = offset;
+  _config.canEncoder->SetPosition(units::turn_t{0});
   _anglePIDController.Reset();
   _velocityPIDController.Reset();
 }
@@ -40,6 +46,8 @@ void SwerveModule::OnUpdate(units::second_t dt) {
   units::volt_t driveVoltage{0};
   units::volt_t turnVoltage{0};
 
+
+
   switch (_state) {
     case SwerveModuleState::kIdle:
       driveVoltage = 0_V;
@@ -47,38 +55,60 @@ void SwerveModule::OnUpdate(units::second_t dt) {
       break;
     case SwerveModuleState::kPID: {
       auto feedforward = _config.driveMotor.motor.Voltage(
-          0_Nm,
-          units::radians_per_second_t{(_velocityPIDController.GetSetpoint() / _config.wheelRadius).value()});
-      driveVoltage = _velocityPIDController.Calculate(GetSpeed(), dt, feedforward);
-      turnVoltage = _anglePIDController.Calculate(_config.turnMotor.encoder->GetEncoderPosition(), dt);
+          0_Nm, units::radians_per_second_t{
+                    _velocityPIDController.GetSetpoint() / _config.wheelRadius.value()});
+      //std::cout << "GetSpeed()" << GetSpeed().value() << std::endl;
+      // units::radian_t input = units::math::fmod(_config.turnMotor.encoder->GetEncoderPosition(), (2_rad * 3.1415));
+      double input = -_config.turnMotor.encoder->GetEncoderPosition().value();
+      _table->GetEntry("/testing/GetEncoderPos").SetDouble(input);
+      // _velocityPIDController.SetSetpoint(3);
+      driveVoltage = units::volt_t{_velocityPIDController.Calculate(GetSpeed().value())};
+      std::cout << "Turn angle input: " << input << std::endl;
+      turnVoltage = units::volt_t{_anglePIDController.Calculate(input)};
+
+      _table->GetEntry("Input angle").SetDouble(input);
+      _table->GetEntry("Setpoint angle").SetDouble(_anglePIDController.GetSetpoint());
     } break;
+    case wom::drivetrain::SwerveModuleState::kZeroing: {
+    } break;
+    default:
+      std::cerr << "Case not handled" << std::endl;
   }
 
-  units::newton_meter_t torqueLimit = 50_kg / 4 * _config.wheelRadius * _currentAccelerationLimit;
-  units::volt_t voltageMax =
-      _config.driveMotor.motor.Voltage(torqueLimit, _config.driveMotor.encoder->GetEncoderAngularVelocity());
-  units::volt_t voltageMin =
-      _config.driveMotor.motor.Voltage(-torqueLimit, _config.driveMotor.encoder->GetEncoderAngularVelocity());
+  units::newton_meter_t torqueLimit =
+      50_kg / 4 * _config.wheelRadius * _currentAccelerationLimit;
+  // units::volt_t voltageMax = _config.driveMotor.motor.Voltage(
+  //     torqueLimit, _config.driveMotor.encoder->GetEncoderAngularVelocity());
+  // units::volt_t voltageMin = _config.driveMotor.motor.Voltage(
+  //     -torqueLimit, _config.driveMotor.encoder->GetEncoderAngularVelocity());
 
-  driveVoltage = units::math::max(units::math::min(driveVoltage, voltageMax), voltageMin);
+  // driveVoltage =
+  //     units::math::max(units::math::min(driveVoltage, voltageMax), voltageMin);
 
   // driveVoltage = units::math::min(driveVoltage, 10_V);
   turnVoltage = units::math::min(turnVoltage, 7_V);
 
-  driveVoltage = units::math::min(units::math::max(driveVoltage, -_driveModuleVoltageLimit),
-                                  _driveModuleVoltageLimit);  // was originally 10_V
-  std::cout << "drive-voltage: " << driveVoltage.value() << std::endl;
+  // driveVoltage = units::math::min(
+  //     units::math::max(driveVoltage, -_driveModuleVoltageLimit),
+  //     _driveModuleVoltageLimit);  // was originally 10_V
   units::volt_t turnVoltageMax = 7_V - (driveVoltage * (7_V / 10_V));
-  turnVoltage = units::math::min(units::math::max(turnVoltage, -turnVoltageMax), turnVoltageMax);
+  turnVoltage = units::math::min(units::math::max(turnVoltage, -turnVoltageMax),
+                                 turnVoltageMax);
   // turnVoltage = units::math::min(units::math::max(turnVoltage, -7_V), 7_V);
   // std::cout << "turn-voltage-max: " << turnVoltageMax.value() << std::endl;
 
+  _table->GetEntry("TurnVoltage").SetDouble(turnVoltage.value());
+  _table->GetEntry("TurnSetpoint").SetDouble(_anglePIDController.GetSetpoint());
+  _table->GetEntry("Error").SetDouble(_anglePIDController.GetPositionError());
+
   _config.driveMotor.motorController->SetVoltage(driveVoltage);
   _config.turnMotor.motorController->SetVoltage(turnVoltage);
 
   _table->GetEntry("speed").SetDouble(GetSpeed().value());
   _table->GetEntry("angle").SetDouble(
-      _config.turnMotor.encoder->GetEncoderPosition().convert<units::degree>().value());
+      _config.turnMotor.encoder->GetEncoderPosition()
+          .convert<units::degree>()
+          .value());
   _config.WriteNT(_table->GetSubTable("config"));
 }
 
@@ -86,11 +116,13 @@ void SwerveModule::OnUpdate(units::second_t dt) {
 //   return (_config.turnMotor.encoder->GetEncoderPosition().value());
 // }
 
-void SwerveModule::SetAccelerationLimit(units::meters_per_second_squared_t limit) {
+void SwerveModule::SetAccelerationLimit(
+    units::meters_per_second_squared_t limit) {
   _currentAccelerationLimit = limit;
 }
 
-void SwerveDrive::SetAccelerationLimit(units::meters_per_second_squared_t limit) {
+void SwerveDrive::SetAccelerationLimit(
+    units::meters_per_second_squared_t limit) {
   for (int motorNumber = 0; motorNumber < 4; motorNumber++) {
     _modules[motorNumber].SetAccelerationLimit(limit);
   }
@@ -109,46 +141,63 @@ void SwerveModule::SetZero(units::second_t dt) {
   _state = SwerveModuleState::kPID;
 }
 
-void SwerveModule::SetPID(units::radian_t angle, units::meters_per_second_t speed, units::second_t dt) {
+void SwerveModule::SetPID(units::radian_t angle,
+                          units::meters_per_second_t speed,
+                          units::second_t dt) {
   _state = SwerveModuleState::kPID;
   // @liam start added
-  double diff = std::fmod((_anglePIDController.GetSetpoint() - angle).convert<units::degree>().value(), 360);
-  if (std::abs(diff) >= 90) {
+  // std::cout << "angle Setpoint: " << _anglePIDController.GetSetpoint() << std::endl;
+  // std::cout << "angle value: " << angle.value() << std::endl;
+
+  double diff = std::fmod((_anglePIDController.GetSetpoint() - angle.value()),
+                          (2 * 3.1415));
+  std::cout << "DIFF value: " << diff << std::endl;
+  _table->GetEntry("/Differential value:").SetDouble(diff);
+  if (std::abs(diff) >= 3.1415) {
     speed *= -1;
-    angle += 180_deg;
+    angle -= 3.1415_rad;
   }
   // @liam end added
 
-  _anglePIDController.SetSetpoint(angle);
-  _velocityPIDController.SetSetpoint(speed);
+  _anglePIDController.SetSetpoint(angle.value());
+  _velocityPIDController.SetSetpoint(speed.value());
 }
 
 void SwerveModule::ModuleVectorHandler(frc::ChassisSpeeds speeds) {
   units::meters_per_second_t xVelocityComponent =
-      1_mps * (speeds.vx.value() + speeds.omega.value() * _config.position.X().value());
+      1_mps *
+      (speeds.vx.value() + speeds.omega.value() * _config.position.X().value());
   units::meters_per_second_t yVelocityComponent =
-      1_mps * (speeds.vy.value() + speeds.omega.value() * _config.position.Y().value());
+      1_mps *
+      (speeds.vy.value() + speeds.omega.value() * _config.position.Y().value());
 
   units::meters_per_second_t velocity =
-      1_mps * std::sqrt(std::pow(xVelocityComponent.value(), 2) + std::pow(yVelocityComponent.value(), 2));
-  units::degree_t angle = 1_rad * std::atan2(yVelocityComponent.value(), xVelocityComponent.value());
+      1_mps * std::sqrt(std::pow(xVelocityComponent.value(), 2) +
+                        std::pow(yVelocityComponent.value(), 2));
+  units::degree_t angle = 1_rad * std::atan2(yVelocityComponent.value(),
+                                             xVelocityComponent.value());
 
-  _anglePIDController.SetSetpoint(angle);
-  _velocityPIDController.SetSetpoint(velocity);
+  _anglePIDController.SetSetpoint(angle.value());
+  _velocityPIDController.SetSetpoint(velocity.value());
 }
 
 units::meters_per_second_t SwerveModule::GetSpeed() const {
-  return units::meters_per_second_t{_config.driveMotor.encoder->GetEncoderAngularVelocity().value() *
-                                    _config.wheelRadius.value()};
+  units::meters_per_second_t returnVal{
+    _config.driveMotor.encoder->GetVelocityValue() *
+    _config.wheelRadius.value()};
+    // std::cout << returnVal.value() << std::endl;
+    return returnVal;
 }
 
 units::meter_t SwerveModule::GetDistance() const {
-  return units::meter_t{_config.driveMotor.encoder->GetEncoderPosition().value() *
-                        _config.wheelRadius.value()};
+  return units::meter_t{
+      _config.driveMotor.encoder->GetEncoderPosition().value() *
+      _config.wheelRadius.value()};
 }
 
 frc::SwerveModulePosition SwerveModule::GetPosition() const {
-  return frc::SwerveModulePosition{GetDistance(), _config.turnMotor.encoder->GetEncoderPosition()};
+  return frc::SwerveModulePosition{
+      GetDistance(), _config.turnMotor.encoder->GetEncoderPosition()};
 }
 
 const SwerveModuleConfig& SwerveModule::GetConfig() const {
@@ -161,36 +210,40 @@ void SwerveDriveConfig::WriteNT(std::shared_ptr<nt::NetworkTable> table) {
 
 SwerveDrive::SwerveDrive(SwerveDriveConfig config, frc::Pose2d initialPose)
     : _config(config),
-      _kinematics(_config.modules[0].position, _config.modules[1].position, _config.modules[2].position,
-                  _config.modules[3].position),
+      _kinematics(_config.modules[0].position, _config.modules[1].position,
+                  _config.modules[2].position, _config.modules[3].position),
       _poseEstimator(
           _kinematics, frc::Rotation2d(0_deg),
-          wpi::array<frc::SwerveModulePosition, 4>{frc::SwerveModulePosition{0_m, frc::Rotation2d{0_deg}},
-                                                   frc::SwerveModulePosition{0_m, frc::Rotation2d{0_deg}},
-                                                   frc::SwerveModulePosition{0_m, frc::Rotation2d{0_deg}},
-                                                   frc::SwerveModulePosition{0_m, frc::Rotation2d{0_deg}}},
+          wpi::array<frc::SwerveModulePosition, 4>{
+              frc::SwerveModulePosition{0_m, frc::Rotation2d{0_deg}},
+              frc::SwerveModulePosition{0_m, frc::Rotation2d{0_deg}},
+              frc::SwerveModulePosition{0_m, frc::Rotation2d{0_deg}},
+              frc::SwerveModulePosition{0_m, frc::Rotation2d{0_deg}}},
           initialPose, _config.stateStdDevs, _config.visionMeasurementStdDevs),
-      _anglePIDController(config.path + "/pid/heading", _config.poseAnglePID),
+      _anglePIDController{frc::PIDController(1, 0, 0)},
       _xPIDController(config.path + "/pid/x", _config.posePositionPID),
       _yPIDController(config.path + "/pid/y", _config.posePositionPID),
       _table(nt::NetworkTableInstance::GetDefault().GetTable(_config.path)) {
-  _anglePIDController.SetWrap(360_deg);
+  _anglePIDController.SetTolerance(360);
 
   int i = 1;
   for (auto cfg : _config.modules) {
-    _modules.emplace_back(config.path + "/modules/" + std::to_string(i), cfg, config.anglePID,
-                          config.velocityPID);
+    _modules.emplace_back(config.path + "/modules/" + std::to_string(i), cfg,
+                          /*config.anglePID,*/ config.velocityPID);
     i++;
   }
 
   ResetPose(initialPose);
 }
 
-frc::ChassisSpeeds FieldRelativeSpeeds::ToChassisSpeeds(const units::radian_t robotHeading) {
-  return frc::ChassisSpeeds::FromFieldRelativeSpeeds(vx, vy, omega, frc::Rotation2d{robotHeading});
+frc::ChassisSpeeds FieldRelativeSpeeds::ToChassisSpeeds(
+    const units::radian_t robotHeading) {
+  return frc::ChassisSpeeds::FromFieldRelativeSpeeds(
+      vx, vy, omega, frc::Rotation2d{robotHeading});
 }
 
 void SwerveDrive::OnUpdate(units::second_t dt) {
+  //std::cout << _config.gyro->GetYaw().GetValue().value() << std::endl;
   switch (_state) {
     case SwerveDriveState::kZeroing:
       for (auto mod = _modules.begin(); mod < _modules.end(); mod++) {
@@ -205,13 +258,14 @@ void SwerveDrive::OnUpdate(units::second_t dt) {
     case SwerveDriveState::kPose: {
       _target_fr_speeds.vx = _xPIDController.Calculate(GetPose().X(), dt);
       _target_fr_speeds.vy = _yPIDController.Calculate(GetPose().Y(), dt);
-      _target_fr_speeds.omega = _anglePIDController.Calculate(GetPose().Rotation().Radians(), dt);
+      _target_fr_speeds.omega = units::radians_per_second_t{_anglePIDController.Calculate(GetPose().Rotation().Radians().value())};
     }
       [[fallthrough]];
     case SwerveDriveState::kFieldRelativeVelocity:
-      _target_speed = _target_fr_speeds.ToChassisSpeeds(GetPose().Rotation().Radians());
+      _target_speed =
+          _target_fr_speeds.ToChassisSpeeds(GetPose().Rotation().Radians());
       if (isRotateToMatchJoystick) {
-        _target_speed.omega = _anglePIDController.Calculate(GetPose().Rotation().Radians(), dt);
+        _target_speed.omega = units::radians_per_second_t{_anglePIDController.Calculate(GetPose().Rotation().Radians().value())};
       }
       // std::cout << "vx = " << _target_speed.vx.value() << " vy = " <<
       // _target_fr_speeds.vy.value() << std::endl;
@@ -219,7 +273,8 @@ void SwerveDrive::OnUpdate(units::second_t dt) {
     case SwerveDriveState::kVelocity: {
       auto target_states = _kinematics.ToSwerveModuleStates(_target_speed);
       for (size_t i = 0; i < _modules.size(); i++) {
-        _modules[i].SetPID(target_states[i].angle.Radians(), target_states[i].speed, dt);
+        _modules[i].SetPID(target_states[i].angle.Radians(),
+                           target_states[i].speed, dt);
         // std::cout << "module " << i << ": " <<
         // target_states[i].angle.Radians().value() << std::endl;
       }
@@ -242,12 +297,13 @@ void SwerveDrive::OnUpdate(units::second_t dt) {
     case SwerveDriveState::kFRVelocityRotationLock:
       _target_speed.vx = _xPIDController.Calculate(GetPose().X(), dt);
       _target_speed.vy = _yPIDController.Calculate(GetPose().Y(), dt);
-      _target_speed.omega = _anglePIDController.Calculate(GetPose().Rotation().Radians(), dt);
-      _target_speed = _requestedSpeeds.ToChassisSpeeds(GetPose().Rotation().Radians());
+      _target_speed.omega = units::radians_per_second_t{_anglePIDController.Calculate(GetPose().Rotation().Radians().value())};
+      _target_speed =
+          _requestedSpeeds.ToChassisSpeeds(GetPose().Rotation().Radians());
       auto target_states = _kinematics.ToSwerveModuleStates(_target_speed);
       for (size_t i = 0; i < _modules.size(); i++) {
-        std::cout << "Speeds :" << target_states[i].speed.value() << std::endl;
-        _modules[i].SetPID(target_states[i].angle.Radians(), target_states[i].speed, dt);
+        _modules[i].SetPID(target_states[i].angle.Radians(),
+                           target_states[i].speed, dt);
       }
       break;
   }
@@ -258,10 +314,12 @@ void SwerveDrive::OnUpdate(units::second_t dt) {
 
   _poseEstimator.Update(
       _config.gyro->GetRotation2d(),
-      wpi::array<frc::SwerveModulePosition, 4>{_modules[0].GetPosition(), _modules[1].GetPosition(),
-                                               _modules[2].GetPosition(), _modules[3].GetPosition()});
+      wpi::array<frc::SwerveModulePosition, 4>{
+          _modules[0].GetPosition(), _modules[1].GetPosition(),
+          _modules[2].GetPosition(), _modules[3].GetPosition()});
 
-  utils::WritePose2NT(_table->GetSubTable("estimatedPose"), _poseEstimator.GetEstimatedPosition());
+  utils::WritePose2NT(_table->GetSubTable("estimatedPose"),
+                      _poseEstimator.GetEstimatedPosition());
   _config.WriteNT(_table->GetSubTable("config"));
 }
 
@@ -298,16 +356,16 @@ void SwerveDrive::OnResetMode() {
   _xPIDController.Reset();
   _yPIDController.Reset();
   _anglePIDController.Reset();
-  std::cout << "reset" << std::endl;
 }
 
-void SwerveDrive::RotateMatchJoystick(units::radian_t joystickAngle, FieldRelativeSpeeds speeds) {
+void SwerveDrive::RotateMatchJoystick(units::radian_t joystickAngle,
+                                      FieldRelativeSpeeds speeds) {
   // _state = SwerveDriveState::kFRVelocityRotationLock;
   // _anglePIDController.SetSetpoint(joystickAngle);
   // _target_fr_speeds = speeds;
   _state = SwerveDriveState::kFieldRelativeVelocity;
   isRotateToMatchJoystick = true;
-  _anglePIDController.SetSetpoint(joystickAngle);
+  _anglePIDController.SetSetpoint(joystickAngle.value());
   _target_fr_speeds = speeds;
 }
 
@@ -327,14 +385,16 @@ bool SwerveDrive::GetIsFieldRelative() {
   return _isFieldRelative;
 }
 
-void SwerveDrive::SetIndividualTuning(int mod, units::radian_t angle, units::meters_per_second_t speed) {
+void SwerveDrive::SetIndividualTuning(int mod, units::radian_t angle,
+                                      units::meters_per_second_t speed) {
   _mod = mod;
   _angle = angle;
   _speed = speed;
   _state = SwerveDriveState::kIndividualTuning;
 }
 
-void SwerveDrive::SetTuning(units::radian_t angle, units::meters_per_second_t speed) {
+void SwerveDrive::SetTuning(units::radian_t angle,
+                            units::meters_per_second_t speed) {
   _angle = angle;
   _speed = speed;
   _state = SwerveDriveState::kTuning;
@@ -348,20 +408,22 @@ void SwerveDrive::SetFieldRelativeVelocity(FieldRelativeSpeeds speeds) {
 
 void SwerveDrive::SetPose(frc::Pose2d pose) {
   _state = SwerveDriveState::kPose;
-  _anglePIDController.SetSetpoint(pose.Rotation().Radians());
+  _anglePIDController.SetSetpoint(pose.Rotation().Radians().value());
   _xPIDController.SetSetpoint(pose.X());
   _yPIDController.SetSetpoint(pose.Y());
 }
 
 bool SwerveDrive::IsAtSetPose() {
-  return _anglePIDController.IsStable() && _xPIDController.IsStable() && _yPIDController.IsStable(0.05_m);
+  return /*_anglePIDController.IsStable()*/ true && _xPIDController.IsStable() &&
+         _yPIDController.IsStable(0.05_m);
 }
 
 void SwerveDrive::ResetPose(frc::Pose2d pose) {
   _poseEstimator.ResetPosition(
       _config.gyro->GetRotation2d(),
-      wpi::array<frc::SwerveModulePosition, 4>{_modules[0].GetPosition(), _modules[1].GetPosition(),
-                                               _modules[2].GetPosition(), _modules[3].GetPosition()},
+      wpi::array<frc::SwerveModulePosition, 4>{
+          _modules[0].GetPosition(), _modules[1].GetPosition(),
+          _modules[2].GetPosition(), _modules[3].GetPosition()},
       pose);
 }
 
@@ -369,7 +431,8 @@ frc::Pose2d SwerveDrive::GetPose() {
   return _poseEstimator.GetEstimatedPosition();
 }
 
-void SwerveDrive::AddVisionMeasurement(frc::Pose2d pose, units::second_t timestamp) {
+void SwerveDrive::AddVisionMeasurement(frc::Pose2d pose,
+                                       units::second_t timestamp) {
   _poseEstimator.AddVisionMeasurement(pose, timestamp);
 }
 }  // namespace drivetrain
diff --git a/wombat/src/main/cpp/drivetrain/behaviours/SwerveBehaviours.cpp b/wombat/src/main/cpp/drivetrain/behaviours/SwerveBehaviours.cpp
index 18d88f88..a82c84d8 100644
--- a/wombat/src/main/cpp/drivetrain/behaviours/SwerveBehaviours.cpp
+++ b/wombat/src/main/cpp/drivetrain/behaviours/SwerveBehaviours.cpp
@@ -29,61 +29,58 @@ ManualDrivebase::ManualDrivebase(wom::drivetrain::SwerveDrive* swerveDrivebase,
 void ManualDrivebase::OnStart() {
   _swerveDrivebase->OnStart();
   _swerveDrivebase->SetAccelerationLimit(6_mps_sq);
-  std::cout << "Manual Drivebase Start" << std::endl;
 }
 
 void ManualDrivebase::OnTick(units::second_t deltaTime) {
-  if (_driverController->GetXButtonPressed()) {
-    ResetMode();
-    if (isRotateMatch) {
-      isRotateMatch = false;
-    } else {
-      isRotateMatch = true;
-    }
-  }
-
-  if (_driverController->GetYButton()) {
-    std::cout << "RESETING POSE" << std::endl;
-    _swerveDrivebase->ResetPose(frc::Pose2d());
-  }
-
-  /*   HOLD SOLUTION   */
-  if (_driverController->GetLeftBumperPressed()) {
-    maxMovementMagnitude = lowSensitivityDriveSpeed;
-    maxRotationMagnitude = lowSensitivityRotateSpeed;
-  } else if (_driverController->GetLeftBumperReleased() && !_driverController->GetRightBumper()) {
-    maxMovementMagnitude = defaultDriveSpeed;
-    maxRotationMagnitude = defaultRotateSpeed;
-    _swerveDrivebase->SetAccelerationLimit(6_mps_sq);
-    _swerveDrivebase->SetVoltageLimit(10_V);
-  }
-  if (_driverController->GetRightBumperPressed()) {
-    maxMovementMagnitude = highSensitivityDriveSpeed;
-    maxRotationMagnitude = highSensitivityRotateSpeed;
-    _swerveDrivebase->SetAccelerationLimit(12_mps_sq);
-    _swerveDrivebase->SetVoltageLimit(14_V);
-
-  } else if (_driverController->GetRightBumperReleased() && !_driverController->GetLeftBumper()) {
-    maxMovementMagnitude = defaultDriveSpeed;
-    maxRotationMagnitude = defaultRotateSpeed;
-    _swerveDrivebase->SetAccelerationLimit(6_mps_sq);
-    _swerveDrivebase->SetVoltageLimit(10_V);
-  }
-
-  if (_driverController->GetAButtonReleased()) {
-    isZero = !isZero;
-  }
-
-  if (isZero) {
-    _swerveDrivebase->SetZeroing();
-  } else {
-    double xVelocity = wom::utils::spow2(
-        -wom::utils::deadzone(_driverController->GetLeftY(),
-                              driverDeadzone));  // GetLeftY due to x being where y should be on field
-    double yVelocity =
-        wom::utils::spow2(-wom::utils::deadzone(_driverController->GetLeftX(), driverDeadzone));
-
-    double r_x = wom::utils::spow2(-wom::utils::deadzone(_driverController->GetRightX(), turningDeadzone));
+  // if (_driverController->GetXButtonPressed()) {
+  //   ResetMode();
+  //   isRotateMatch = !isRotateMatch;
+  // }
+
+  // if (_driverController->GetYButton()) {
+  //   std::cout << "RESETING POSE" << std::endl;
+  //   _swerveDrivebase->ResetPose(frc::Pose2d());
+  // }
+
+  // if (_driverController->GetLeftBumperPressed()) {
+  //   maxMovementMagnitude = lowSensitivityDriveSpeed;
+  //   maxRotationMagnitude = lowSensitivityRotateSpeed;
+  // } else if (_driverController->GetLeftBumperReleased() &&
+  //            !_driverController->GetRightBumper()) {
+  //   maxMovementMagnitude = defaultDriveSpeed;
+  //   maxRotationMagnitude = defaultRotateSpeed;
+  //   _swerveDrivebase->SetAccelerationLimit(6_mps_sq);
+  //   _swerveDrivebase->SetVoltageLimit(10_V);
+  // }
+  // if (_driverController->GetRightBumperPressed()) {
+  //   maxMovementMagnitude = highSensitivityDriveSpeed;
+  //   maxRotationMagnitude = highSensitivityRotateSpeed;
+  //   _swerveDrivebase->SetAccelerationLimit(12_mps_sq);
+  //   _swerveDrivebase->SetVoltageLimit(14_V);
+
+  // } else if (_driverController->GetRightBumperReleased() &&
+  //            !_driverController->GetLeftBumper()) {
+  //   maxMovementMagnitude = defaultDriveSpeed;
+  //   maxRotationMagnitude = defaultRotateSpeed;
+  //   _swerveDrivebase->SetAccelerationLimit(6_mps_sq);
+  //   _swerveDrivebase->SetVoltageLimit(10_V);
+  // }
+
+  // if (_driverController->GetAButtonReleased()) {
+  //   isZero = !isZero;
+  // }
+
+  // if (isZero) {
+  //   _swerveDrivebase->SetZeroing();
+  // } else {
+    double xVelocity = wom::utils::spow2(-wom::utils::deadzone(
+        _driverController->GetLeftY(),
+        driverDeadzone));  // GetLeftY due to x being where y should be on field
+    double yVelocity = wom::utils::spow2(
+        -wom::utils::deadzone(_driverController->GetLeftX(), driverDeadzone));
+
+    double r_x = wom::utils::spow2(
+        -wom::utils::deadzone(_driverController->GetRightX(), turningDeadzone));
 
     double turnX = _driverController->GetRightX();
     double turnY = _driverController->GetRightY();
@@ -93,20 +90,26 @@ void ManualDrivebase::OnTick(units::second_t deltaTime) {
       turnY = 0;
     }
 
-    if (isRotateMatch) {
-      units::degree_t currentAngle = _swerveDrivebase->GetPose().Rotation().Degrees();
-      CalculateRequestedAngle(turnX, turnY, currentAngle);
-      _swerveDriveTable->GetEntry("RotateMatch").SetDouble(_requestedAngle.value());
-      _swerveDrivebase->RotateMatchJoystick(
-          _requestedAngle,
-          wom::drivetrain::FieldRelativeSpeeds{// also field relative
-                                               xVelocity * maxMovementMagnitude,
-                                               yVelocity * maxMovementMagnitude, r_x * maxRotationMagnitude});
-    } else {
-      _swerveDrivebase->SetFieldRelativeVelocity(wom::drivetrain::FieldRelativeSpeeds{
-          xVelocity * maxMovementMagnitude, yVelocity * maxMovementMagnitude, r_x * maxRotationMagnitude});
-    }
-  }
+    // if (isRotateMatch) {
+    //   units::degree_t currentAngle =
+    //       _swerveDrivebase->GetPose().Rotation().Degrees();
+    //   CalculateRequestedAngle(turnX, turnY, currentAngle);
+    //   _swerveDriveTable->GetEntry("RotateMatch")
+    //       .SetDouble(_requestedAngle.value());
+    //   _swerveDrivebase->RotateMatchJoystick(
+    //       _requestedAngle,
+    //       wom::drivetrain::FieldRelativeSpeeds{// also field relative
+    //                                            xVelocity * maxMovementMagnitude,
+    //                                            yVelocity * maxMovementMagnitude,
+    //                                            r_x * maxRotationMagnitude});
+    // } else {
+      _swerveDrivebase->SetFieldRelativeVelocity(
+          wom::drivetrain::FieldRelativeSpeeds{xVelocity * maxMovementMagnitude,
+                                               yVelocity * maxMovementMagnitude,
+                                               r_x * maxRotationMagnitude});
+  //   }
+  // }
+  // _swerveDrivebase->SetTuning(100_deg, 1_mps);
 }
 
 void ManualDrivebase::ResetMode() {
@@ -114,16 +117,18 @@ void ManualDrivebase::ResetMode() {
   resetMode = false;
 }
 
-void ManualDrivebase::CalculateRequestedAngle(double joystickX, double joystickY,
+void ManualDrivebase::CalculateRequestedAngle(double joystickX,
+                                              double joystickY,
                                               units::degree_t defaultAngle) {
   _requestedAngle = (1_rad * std::atan2(joystickY, -joystickX)) + 90_deg;
-  if (joystickX == 0 & joystickY == 0) {
+  if (wom::utils::deadzone(joystickX) == 0 && wom::utils::deadzone(joystickY) == 0) {
     _requestedAngle = _swerveDrivebase->GetPose().Rotation().Radians();
   }
 }
 
 // Code for x-ing the wheels on the drivebase
-XDrivebase::XDrivebase(wom::drivetrain::SwerveDrive* swerveDrivebase) : _swerveDrivebase(swerveDrivebase) {
+XDrivebase::XDrivebase(wom::drivetrain::SwerveDrive* swerveDrivebase)
+    : _swerveDrivebase(swerveDrivebase) {
   Controls(swerveDrivebase);
 }
 void XDrivebase::OnTick(units::second_t deltaTime) {
@@ -165,17 +170,20 @@ void XDrivebase::OnTick(units::second_t deltaTime) {
 //   m_timer.Start();
 // }
 
-wom::drivetrain::behaviours::TempSimSwerveDrive::TempSimSwerveDrive(frc::Timer* timer, frc::Field2d* field)
+wom::drivetrain::behaviours::TempSimSwerveDrive::TempSimSwerveDrive(
+    frc::Timer* timer, frc::Field2d* field)
     : m_timer(timer), m_field(field) {}
 
 void wom::drivetrain::behaviours::TempSimSwerveDrive::OnUpdate() {
   m_field->SetRobotPose(m_driveSim.GetPose());
 
   // get the current trajectory state
-  frc::Trajectory::State desired_state = current_trajectory.Sample(m_timer->Get());
+  frc::Trajectory::State desired_state =
+      current_trajectory.Sample(m_timer->Get());
 
   // get the current wheel speeds
-  wom::utils::WriteTrajectoryState(current_trajectory_state_table, desired_state);
+  wom::utils::WriteTrajectoryState(current_trajectory_state_table,
+                                   desired_state);
 
   // move drivebase position to the desired state
   m_driveSim.SetPose(wom::utils::TrajectoryStateToPose2d(desired_state));
@@ -193,16 +201,19 @@ frc::Pose2d wom::drivetrain::behaviours::TempSimSwerveDrive::GetPose2d() {
   return m_driveSim.GetPose();
 }
 
-void wom::drivetrain::behaviours::TempSimSwerveDrive::SetPath(std::string path) {
+void wom::drivetrain::behaviours::TempSimSwerveDrive::SetPath(
+    std::string path) {
   nt::NetworkTableInstance inst = nt::NetworkTableInstance::GetDefault();
   std::shared_ptr<nt::NetworkTable> table = inst.GetTable("FMSInfo");
 
   // create a netowrk table for the trajectory
   std::shared_ptr<nt::NetworkTable> trajectory_table =
       nt::NetworkTableInstance::GetDefault().GetTable("trajectory_path");
-  current_trajectory_table = nt::NetworkTableInstance::GetDefault().GetTable("current_trajectory");
+  current_trajectory_table =
+      nt::NetworkTableInstance::GetDefault().GetTable("current_trajectory");
   current_trajectory_state_table =
-      nt::NetworkTableInstance::GetDefault().GetTable("current_trajectory_state");
+      nt::NetworkTableInstance::GetDefault().GetTable(
+          "current_trajectory_state");
 
   current_trajectory = m_pathplanner.getTrajectory(path);
   m_driveSim.SetPose(current_trajectory.Sample(0_s).pose);
@@ -210,10 +221,12 @@ void wom::drivetrain::behaviours::TempSimSwerveDrive::SetPath(std::string path)
   m_timer->Start();
 }
 
-wom::drivetrain::behaviours::AutoSwerveDrive::AutoSwerveDrive(wom::drivetrain::SwerveDrive* swerve,
-                                                              frc::Timer* timer, frc::Field2d* field)
+wom::drivetrain::behaviours::AutoSwerveDrive::AutoSwerveDrive(
+    wom::drivetrain::SwerveDrive* swerve, frc::Timer* timer,
+    frc::Field2d* field)
     : _swerve(swerve), m_timer(timer), m_field(field) {
-  _simSwerveDrive = new wom::drivetrain::behaviours::TempSimSwerveDrive(timer, field);
+  _simSwerveDrive =
+      new wom::drivetrain::behaviours::TempSimSwerveDrive(timer, field);
 }
 
 void wom::drivetrain::behaviours::AutoSwerveDrive::OnUpdate() {
diff --git a/wombat/src/main/cpp/subsystems/Arm.cpp b/wombat/src/main/cpp/subsystems/Arm.cpp
index 942003c4..9390601a 100644
--- a/wombat/src/main/cpp/subsystems/Arm.cpp
+++ b/wombat/src/main/cpp/subsystems/Arm.cpp
@@ -10,14 +10,19 @@ using namespace frc;
 using namespace wom;
 
 // creates network table instatnce on shuffleboard
-void wom::subsystems::ArmConfig::WriteNT(std::shared_ptr<nt::NetworkTable> table) {
+void wom::subsystems::ArmConfig::WriteNT(
+    std::shared_ptr<nt::NetworkTable> table) {
   table->GetEntry("armMass").SetDouble(armMass.value());
   table->GetEntry("loadMass").SetDouble(loadMass.value());
   table->GetEntry("armLength").SetDouble(armLength.value());
-  table->GetEntry("minAngle").SetDouble(minAngle.convert<units::degree>().value());
-  table->GetEntry("maxAngle").SetDouble(maxAngle.convert<units::degree>().value());
-  table->GetEntry("initialAngle").SetDouble(initialAngle.convert<units::degree>().value());
-  table->GetEntry("angleOffset").SetDouble(initialAngle.convert<units::degree>().value());
+  table->GetEntry("minAngle")
+      .SetDouble(minAngle.convert<units::degree>().value());
+  table->GetEntry("maxAngle")
+      .SetDouble(maxAngle.convert<units::degree>().value());
+  table->GetEntry("initialAngle")
+      .SetDouble(initialAngle.convert<units::degree>().value());
+  table->GetEntry("angleOffset")
+      .SetDouble(initialAngle.convert<units::degree>().value());
 }
 
 // arm config is used
@@ -38,12 +43,14 @@ void wom::subsystems::Arm::OnUpdate(units::second_t dt) {
     case ArmState::kIdle:
       break;
     case ArmState::kVelocity: {
-      units::newton_meter_t torque = 9.81_m / 1_s / 1_s * _config.armLength *
-                                     units::math::cos(angle + _config.angleOffset) *
-                                     (0.5 * _config.armMass + _config.loadMass);
+      units::newton_meter_t torque =
+          9.81_m / 1_s / 1_s * _config.armLength *
+          units::math::cos(angle + _config.angleOffset) *
+          (0.5 * _config.armMass + _config.loadMass);
       // units::volt_t feedforward = _config.leftGearbox.motor.Voltage(torque,
       // 0_rad/1_s);
-      units::volt_t feedforward = _config.leftGearbox.motor.Voltage(torque, _velocityPID.GetSetpoint());
+      units::volt_t feedforward =
+          _config.leftGearbox.motor.Voltage(torque, _velocityPID.GetSetpoint());
       // feedforward = 3.5_V;
       // std::cout << "feedforward" << feedforward.value() << std::endl;
       voltage = _velocityPID.Calculate(GetArmVelocity(), dt, feedforward);
@@ -51,10 +58,12 @@ void wom::subsystems::Arm::OnUpdate(units::second_t dt) {
       // std::endl; voltage = 0_V;
     } break;
     case ArmState::kAngle: {
-      units::newton_meter_t torque = 9.81_m / 1_s / 1_s * _config.armLength *
-                                     units::math::cos(angle + _config.angleOffset) *
-                                     (0.5 * _config.armMass + _config.loadMass);
-      units::volt_t feedforward = _config.leftGearbox.motor.Voltage(torque, 0_rad / 1_s);
+      units::newton_meter_t torque =
+          9.81_m / 1_s / 1_s * _config.armLength *
+          units::math::cos(angle + _config.angleOffset) *
+          (0.5 * _config.armMass + _config.loadMass);
+      units::volt_t feedforward =
+          _config.leftGearbox.motor.Voltage(torque, 0_rad / 1_s);
       // std::cout << "feedforward" << feedforward.value() << std::endl;
       voltage = _pid.Calculate(angle, dt, feedforward);
     } break;
@@ -79,8 +88,8 @@ void wom::subsystems::Arm::OnUpdate(units::second_t dt) {
 
   // std::cout << "voltage: " << voltage.value() << std::endl;
 
-  _config.leftGearbox.motorController->SetVoltage(voltage);
-  _config.rightGearbox.motorController->SetVoltage(voltage);
+  // _config.leftGearbox.motorController->SetVoltage(voltage);
+  // _config.rightGearbox.motorController->SetVoltage(voltage);
 
   // creates network table instances for the angle and config of the arm
   _table->GetEntry("angle").SetDouble(angle.convert<units::degree>().value());
diff --git a/wombat/src/main/cpp/subsystems/Elevator.cpp b/wombat/src/main/cpp/subsystems/Elevator.cpp
index 6e0267e7..d6b59f8a 100644
--- a/wombat/src/main/cpp/subsystems/Elevator.cpp
+++ b/wombat/src/main/cpp/subsystems/Elevator.cpp
@@ -7,7 +7,8 @@
 #include <networktables/NetworkTableInstance.h>
 #include <units/acceleration.h>
 
-void wom::subsystems::ElevatorConfig::WriteNT(std::shared_ptr<nt::NetworkTable> table) {
+void wom::subsystems::ElevatorConfig::WriteNT(
+    std::shared_ptr<nt::NetworkTable> table) {
   table->GetEntry("radius").SetDouble(radius.value());
   table->GetEntry("mass").SetDouble(mass.value());
   table->GetEntry("maxHeight").SetDouble(maxHeight.value());
@@ -19,7 +20,8 @@ wom::subsystems::Elevator::Elevator(ElevatorConfig config)
       _pid{config.path + "/pid", config.pid},
       _velocityPID{config.path + "/velocityPID", config.velocityPID},
       _table(nt::NetworkTableInstance::GetDefault().GetTable(config.path)) {
-  _config.leftGearbox.encoder->SetEncoderPosition(_config.initialHeight / _config.radius * 1_rad);
+  _config.leftGearbox.encoder->SetEncoderPosition(_config.initialHeight /
+                                                  _config.radius * 1_rad);
 }
 
 void wom::subsystems::Elevator::OnUpdate(units::second_t dt) {
@@ -37,7 +39,8 @@ void wom::subsystems::Elevator::OnUpdate(units::second_t dt) {
     case ElevatorState::kVelocity: {
       units::volt_t feedforward = _config.rightGearbox.motor.Voltage(
           (_config.mass * 9.81_mps_sq) * _config.radius,
-          _velocityPID.GetSetpoint() / (14.0 / 60.0 * 2.0 * 3.1415 * 0.02225 * 1_m) * 1_rad);
+          _velocityPID.GetSetpoint() /
+              (14.0 / 60.0 * 2.0 * 3.1415 * 0.02225 * 1_m) * 1_rad);
       // units::volt_t feedforward = _config.rightGearbox.motor.Voltage(0_Nm,
       // _velocityPID.GetSetpoint() / (14.0/60.0 * 2.0 * 3.1415 * 0.02225 * 1_m)
       // * 1_rad);
@@ -50,8 +53,8 @@ void wom::subsystems::Elevator::OnUpdate(units::second_t dt) {
       // voltage = 0_V;
     } break;
     case ElevatorState::kPID: {
-      units::volt_t feedforward =
-          _config.rightGearbox.motor.Voltage((_config.mass * 9.81_mps_sq) * _config.radius, 0_rad_per_s);
+      units::volt_t feedforward = _config.rightGearbox.motor.Voltage(
+          (_config.mass * 9.81_mps_sq) * _config.radius, 0_rad_per_s);
       // std::cout << "feed forward" << feedforward.value() << std::endl;
       feedforward = 1.2_V;
       // voltage = _pid.Calculate(height, dt, feedforward);
@@ -84,8 +87,8 @@ void wom::subsystems::Elevator::OnUpdate(units::second_t dt) {
 
   // Set voltage to motors...
   voltage *= speedLimit;
-  _config.leftGearbox.motorController->SetVoltage(voltage);
-  _config.rightGearbox.motorController->SetVoltage(voltage);
+  // _config.leftGearbox.motorController->SetVoltage(voltage);
+  // _config.rightGearbox.motorController->SetVoltage(voltage);
 }
 
 void wom::subsystems::Elevator::SetManual(units::volt_t voltage) {
@@ -96,7 +99,8 @@ void wom::subsystems::Elevator::SetPID(units::meter_t height) {
   _pid.SetSetpoint(height);
 }
 
-void wom::subsystems::Elevator::SetVelocity(units::meters_per_second_t velocity) {
+void wom::subsystems::Elevator::SetVelocity(
+    units::meters_per_second_t velocity) {
   _velocityPID.SetSetpoint(velocity);
 }
 
@@ -128,14 +132,18 @@ units::meter_t wom::subsystems::Elevator::GetHeight() const {
   // std::cout << "elevator position"<<
   // _config.rightGearbox.encoder->GetEncoderTicks() << std::endl; return
   // _config.rightGearbox.encoder->GetEncoderDistance() * 1_m;
-  return _config.elevatorEncoder.GetPosition() * 14 / 60 * 2 * 3.1415 * 0.02225 * 1_m;
+  return _config.elevatorEncoder.GetPosition() * 14 / 60 * 2 * 3.1415 *
+         0.02225 * 1_m;
 }
 
-units::meters_per_second_t wom::subsystems::Elevator::GetElevatorVelocity() const {
-  return _config.elevatorEncoder.GetVelocity() / 60_s * 14 / 60 * 2 * 3.1415 * 0.02225 * 1_m;
+units::meters_per_second_t wom::subsystems::Elevator::GetElevatorVelocity()
+    const {
+  return _config.elevatorEncoder.GetVelocity() / 60_s * 14 / 60 * 2 * 3.1415 *
+         0.02225 * 1_m;
 }
 
 units::meters_per_second_t wom::subsystems::Elevator::MaxSpeed() const {
-  return _config.leftGearbox.motor.Speed((_config.mass * 9.81_mps_sq) * _config.radius, 12_V) / 1_rad *
-         _config.radius;
+  return _config.leftGearbox.motor.Speed(
+             (_config.mass * 9.81_mps_sq) * _config.radius, 12_V) /
+         1_rad * _config.radius;
 }
diff --git a/wombat/src/main/cpp/subsystems/Shooter.cpp b/wombat/src/main/cpp/subsystems/Shooter.cpp
index b289c179..2fede2f3 100644
--- a/wombat/src/main/cpp/subsystems/Shooter.cpp
+++ b/wombat/src/main/cpp/subsystems/Shooter.cpp
@@ -14,14 +14,16 @@ wom::subsystems::Shooter::Shooter(std::string path, ShooterParams params)
 
 void wom::subsystems::Shooter::OnUpdate(units::second_t dt) {
   units::volt_t voltage{0};
-  units::revolutions_per_minute_t currentSpeed = _params.gearbox.encoder->GetEncoderAngularVelocity();
+  units::revolutions_per_minute_t currentSpeed =
+      _params.gearbox.encoder->GetEncoderAngularVelocity();
 
   switch (_state) {
     case ShooterState::kManual:
       voltage = _setpointManual;
       break;
     case ShooterState::kPID: {
-      auto feedforward = _params.gearbox.motor.Voltage(0_Nm, _pid.GetSetpoint());
+      auto feedforward =
+          _params.gearbox.motor.Voltage(0_Nm, _pid.GetSetpoint());
       voltage = _pid.Calculate(currentSpeed, dt, feedforward);
     } break;
     case ShooterState::kIdle:
@@ -29,17 +31,20 @@ void wom::subsystems::Shooter::OnUpdate(units::second_t dt) {
       break;
   }
 
-  units::newton_meter_t max_torque_at_current_limit = _params.gearbox.motor.Torque(_params.currentLimit);
+  units::newton_meter_t max_torque_at_current_limit =
+      _params.gearbox.motor.Torque(_params.currentLimit);
   units::volt_t max_voltage_for_current_limit =
       _params.gearbox.motor.Voltage(max_torque_at_current_limit, currentSpeed);
 
-  voltage = 1_V * std::min(voltage.value(), max_voltage_for_current_limit.value());
+  voltage =
+      1_V * std::min(voltage.value(), max_voltage_for_current_limit.value());
 
-  _params.gearbox.motorController->SetVoltage(voltage);
+  // _params.gearbox.motorController->SetVoltage(voltage);
 
   _table->GetEntry("output_volts").SetDouble(voltage.value());
   _table->GetEntry("speed_rpm").SetDouble(currentSpeed.value());
-  _table->GetEntry("setpoint_rpm").SetDouble(units::revolutions_per_minute_t{_pid.GetSetpoint()}.value());
+  _table->GetEntry("setpoint_rpm")
+      .SetDouble(units::revolutions_per_minute_t{_pid.GetSetpoint()}.value());
   _table->GetEntry("stable").SetBoolean(_pid.IsStable());
 }
 
@@ -62,7 +67,8 @@ bool wom::subsystems::Shooter::IsStable() const {
 
 // Shooter Manual Set
 
-wom::subsystems::ShooterConstant::ShooterConstant(Shooter* s, units::volt_t setpoint)
+wom::subsystems::ShooterConstant::ShooterConstant(Shooter* s,
+                                                  units::volt_t setpoint)
     : _shooter(s), _setpoint(setpoint) {
   Controls(_shooter);
 }
@@ -73,7 +79,9 @@ void wom::subsystems::ShooterConstant::OnTick(units::second_t dt) {
 
 // ShooterSpinup
 
-wom::subsystems::ShooterSpinup::ShooterSpinup(Shooter* s, units::radians_per_second_t speed, bool hold)
+wom::subsystems::ShooterSpinup::ShooterSpinup(Shooter* s,
+                                              units::radians_per_second_t speed,
+                                              bool hold)
     : _shooter(s), _speed(speed), _hold(hold) {
   Controls(_shooter);
 }
diff --git a/wombat/src/main/cpp/utils/Encoder.cpp b/wombat/src/main/cpp/utils/Encoder.cpp
index 3935d82c..e5d6ab2b 100644
--- a/wombat/src/main/cpp/utils/Encoder.cpp
+++ b/wombat/src/main/cpp/utils/Encoder.cpp
@@ -6,8 +6,8 @@
 
 #include <rev/SparkRelativeEncoder.h>
 
-wom::utils::Encoder::Encoder(double encoderTicksPerRotation, int type, units::meter_t wheelRadius,
-                             double reduction)
+wom::utils::Encoder::Encoder(double encoderTicksPerRotation, int type,
+                             units::meter_t wheelRadius, double reduction)
     : _reduction(reduction),
       _encoderTicksPerRotation(encoderTicksPerRotation),
       _type(type),
@@ -43,41 +43,55 @@ void wom::utils::Encoder::SetReduction(double reduction) {
 }
 
 units::radian_t wom::utils::Encoder::GetEncoderPosition() {
-  if (_type == 0) {
-    units::turn_t n_turns{GetEncoderTicks() / GetEncoderTicksPerRotation()};
-    return n_turns;
-  } else if (_type == 2) {
-    units::degree_t pos = GetEncoderTicks() * 1_deg;
-    return pos - _offset;
-  } else {
-    units::degree_t pos = GetEncoderTicks() * 1_deg;
-    return pos - _offset;
-  }
+  //if (_type == 0) {
+  //  units::turn_t n_turns{GetEncoderTicks() / GetEncoderTicksPerRotation()};
+  //  return n_turns;
+  //} else if (_type == 2) {
+  //  units::degree_t pos = GetEncoderTicks() * 1_deg;
+  //  return pos;
+  //} else {
+  //  units::degree_t pos = GetEncoderTicks() * 1_deg;
+  //  return pos - _offset;
+  //}
+  return GetEncoderTicks() * 1_rad;
 }
 
 double wom::utils::Encoder::GetEncoderDistance() {
-  return GetEncoderTicks() * (2 * M_PI) * _wheelRadius.value();
+  return GetEncoderTicks() * (2 * 3.141592) * _wheelRadius.value();
 }
 
 units::radians_per_second_t wom::utils::Encoder::GetEncoderAngularVelocity() {
   // return GetEncoderTickVelocity() / (double)GetEncoderTicksPerRotation() * 2
   // * 3.1415926;
-  units::turns_per_second_t n_turns_per_s{GetEncoderTickVelocity() / GetEncoderTicksPerRotation()};
+  units::turns_per_second_t n_turns_per_s{GetEncoderTickVelocity() /
+                                          GetEncoderTicksPerRotation()};
   return n_turns_per_s;
 }
 
+double wom::utils::Encoder::GetVelocityValue() const {
+  // std::cout << "GET VELOCITY: " << GetVelocity() << std::endl;
+  return GetVelocity();
+  // return 0;
+}
+
 double wom::utils::DigitalEncoder::GetEncoderRawTicks() const {
   return _nativeEncoder.Get();
 }
 
+double wom::utils::DigitalEncoder::GetVelocity() const {
+  return 0;
+}
+
 double wom::utils::DigitalEncoder::GetEncoderTickVelocity() const {
   return _nativeEncoder.GetRate();
 }
 
-wom::utils::CANSparkMaxEncoder::CANSparkMaxEncoder(rev::CANSparkMax* controller, units::meter_t wheelRadius,
+wom::utils::CANSparkMaxEncoder::CANSparkMaxEncoder(rev::CANSparkMax* controller,
+                                                   units::meter_t wheelRadius,
                                                    double reduction)
     : wom::utils::Encoder(42, reduction, wheelRadius, 2),
-      _encoder(controller->GetEncoder(rev::SparkRelativeEncoder::Type::kQuadrature)) {}
+      _encoder(controller->GetEncoder(
+          rev::SparkRelativeEncoder::Type::kQuadrature)) {}
 
 double wom::utils::CANSparkMaxEncoder::GetEncoderRawTicks() const {
   return _encoder.GetPosition() * _reduction;
@@ -87,6 +101,18 @@ double wom::utils::CANSparkMaxEncoder::GetEncoderTickVelocity() const {
   return _encoder.GetVelocity() * GetEncoderTicksPerRotation() / 60;
 }
 
+double wom::utils::TalonFXEncoder::GetVelocity() const {
+  return _controller->GetVelocity().GetValue().value();
+}
+
+double wom::utils::CanEncoder::GetVelocity() const {
+  return _canEncoder->GetVelocity().GetValue().value();
+}
+
+double wom::utils::DutyCycleEncoder::GetVelocity() const {
+  return 0;
+}
+
 double wom::utils::CANSparkMaxEncoder::GetPosition() const {
   return _encoder.GetPosition();
 }
@@ -95,9 +121,11 @@ double wom::utils::CANSparkMaxEncoder::GetVelocity() const {
   return _encoder.GetVelocity();
 }
 
-wom::utils::TalonFXEncoder::TalonFXEncoder(ctre::phoenix6::hardware::TalonFX* controller,
-                                           units::meter_t wheelRadius, double reduction)
-    : utils::Encoder(2048, reduction, wheelRadius, 0), _controller(controller) {}
+wom::utils::TalonFXEncoder::TalonFXEncoder(
+    ctre::phoenix6::hardware::TalonFX* controller, units::meter_t wheelRadius,
+    double reduction)
+    : utils::Encoder(2048, reduction, wheelRadius, 0),
+      _controller(controller) {}
 
 double wom::utils::TalonFXEncoder::GetEncoderRawTicks() const {
   return _controller->GetPosition().GetValue().value();
@@ -107,9 +135,12 @@ double wom::utils::TalonFXEncoder::GetEncoderTickVelocity() const {
   return _controller->Get() * 10;
 }
 
-wom::utils::DutyCycleEncoder::DutyCycleEncoder(int channel, units::meter_t wheelRadius,
-                                               double ticksPerRotation, double reduction)
-    : wom::utils::Encoder(ticksPerRotation, reduction, wheelRadius, 0), _dutyCycleEncoder(channel) {}
+wom::utils::DutyCycleEncoder::DutyCycleEncoder(int channel,
+                                               units::meter_t wheelRadius,
+                                               double ticksPerRotation,
+                                               double reduction)
+    : wom::utils::Encoder(ticksPerRotation, reduction, wheelRadius, 0),
+      _dutyCycleEncoder(channel) {}
 
 double wom::utils::DutyCycleEncoder::GetEncoderRawTicks() const {
   return _dutyCycleEncoder.Get().value();
@@ -119,14 +150,15 @@ double wom::utils::DutyCycleEncoder::GetEncoderTickVelocity() const {
   return 0;
 }
 
-wom::utils::CanEncoder::CanEncoder(int deviceNumber, units::meter_t wheelRadius, double ticksPerRotation,
-                                   double reduction, std::string name)
-    : wom::utils::Encoder(ticksPerRotation, reduction, wheelRadius, 1) {
+wom::utils::CanEncoder::CanEncoder(int deviceNumber, units::meter_t wheelRadius,
+                                   double ticksPerRotation, double reduction,
+                                   std::string name)
+    : wom::utils::Encoder(ticksPerRotation, 2, wheelRadius, reduction) {
   _canEncoder = new ctre::phoenix6::hardware::CANcoder(deviceNumber, name);
 }
 
 double wom::utils::CanEncoder::GetEncoderRawTicks() const {
-  return _canEncoder->GetAbsolutePosition().GetValue().value();
+  return _canEncoder->GetPosition().GetValue().value() * 2 * 3.14;
 }
 
 double wom::utils::CanEncoder::GetEncoderTickVelocity() const {
diff --git a/wombat/src/main/cpp/utils/Util.cpp b/wombat/src/main/cpp/utils/Util.cpp
index dbdbb642..e529e245 100644
--- a/wombat/src/main/cpp/utils/Util.cpp
+++ b/wombat/src/main/cpp/utils/Util.cpp
@@ -13,18 +13,21 @@ units::second_t wom::utils::now() {
   return static_cast<double>(now) / 1000000 * 1_s;
 }
 
-void wom::utils::WritePose2NT(std::shared_ptr<nt::NetworkTable> table, frc::Pose2d pose) {
+void wom::utils::WritePose2NT(std::shared_ptr<nt::NetworkTable> table,
+                              frc::Pose2d pose) {
   table->GetEntry("x").SetDouble(pose.X().value());
   table->GetEntry("y").SetDouble(pose.Y().value());
   table->GetEntry("angle").SetDouble(pose.Rotation().Degrees().value());
 }
 
-void wom::utils::WritePose3NT(std::shared_ptr<nt::NetworkTable> table, frc::Pose3d pose) {
+void wom::utils::WritePose3NT(std::shared_ptr<nt::NetworkTable> table,
+                              frc::Pose3d pose) {
   table->GetEntry("x").SetDouble(pose.X().value());
   table->GetEntry("y").SetDouble(pose.Y().value());
   table->GetEntry("z").SetDouble(pose.Z().value());
 
-  table->GetEntry("angle").SetDouble(pose.Rotation().Z().convert<units::degree>().value());
+  table->GetEntry("angle").SetDouble(
+      pose.Rotation().Z().convert<units::degree>().value());
 }
 
 double wom::utils::deadzone(double val, double deadzone) {
@@ -47,24 +50,32 @@ units::volt_t wom::utils::GetVoltage(frc::MotorController* controller) {
   return controller->Get() * frc::RobotController::GetBatteryVoltage();
 }
 
-void wom::utils::WriteTrajectory(std::shared_ptr<nt::NetworkTable> table, frc::Trajectory trajectory) {
+void wom::utils::WriteTrajectory(std::shared_ptr<nt::NetworkTable> table,
+                                 frc::Trajectory trajectory) {
   table->GetEntry("length").SetDouble(trajectory.TotalTime().value());
 
   // write the trajectory to the network table
   int i = 0;
   for (auto state : trajectory.States()) {
-    table->GetSubTable(std::to_string(i))->GetEntry("x").SetDouble(state.pose.X().value());
-    table->GetSubTable(std::to_string(i))->GetEntry("y").SetDouble(state.pose.Y().value());
+    table->GetSubTable(std::to_string(i))
+        ->GetEntry("x")
+        .SetDouble(state.pose.X().value());
+    table->GetSubTable(std::to_string(i))
+        ->GetEntry("y")
+        .SetDouble(state.pose.Y().value());
     table->GetSubTable(std::to_string(i))
         ->GetEntry("angle")
         .SetDouble(state.pose.Rotation().Degrees().value());
-    table->GetSubTable(std::to_string(i))->GetEntry("time").SetDouble(state.t.value());
+    table->GetSubTable(std::to_string(i))
+        ->GetEntry("time")
+        .SetDouble(state.t.value());
 
     i++;
   }
 }
 
-void wom::utils::WriteTrajectoryState(std::shared_ptr<nt::NetworkTable> table, frc::Trajectory::State state) {
+void wom::utils::WriteTrajectoryState(std::shared_ptr<nt::NetworkTable> table,
+                                      frc::Trajectory::State state) {
   table->GetEntry("x").SetDouble(state.pose.X().value());
   table->GetEntry("y").SetDouble(state.pose.Y().value());
   table->GetEntry("angle").SetDouble(state.pose.Rotation().Degrees().value());
diff --git a/wombat/src/main/cpp/vision/Limelight.cpp b/wombat/src/main/cpp/vision/Limelight.cpp
index 906f92cb..b37148ce 100644
--- a/wombat/src/main/cpp/vision/Limelight.cpp
+++ b/wombat/src/main/cpp/vision/Limelight.cpp
@@ -8,7 +8,8 @@
 
 #include "utils/Util.h"
 
-wom::vision::Limelight::Limelight(std::string limelightName) : _limelightName(limelightName) {}
+wom::vision::Limelight::Limelight(std::string limelightName)
+    : _limelightName(limelightName) {}
 
 std::string wom::vision::Limelight::GetName() {
   return _limelightName;
@@ -23,7 +24,8 @@ std::pair<double, double> wom::vision::Limelight::GetOffset() {
   return offset;
 }
 
-std::vector<double> wom::vision::Limelight::GetAprilTagData(LimelightAprilTagData dataType) {
+std::vector<double> wom::vision::Limelight::GetAprilTagData(
+    LimelightAprilTagData dataType) {
   std::string dataName;
 
   switch (dataType) {
@@ -67,7 +69,8 @@ std::vector<double> wom::vision::Limelight::GetAprilTagData(LimelightAprilTagDat
   return table->GetNumberArray(dataName, std::vector<double>(6));
 }
 
-double wom::vision::Limelight::GetTargetingData(LimelightTargetingData dataType, double defaultValue) {
+double wom::vision::Limelight::GetTargetingData(LimelightTargetingData dataType,
+                                                double defaultValue) {
   std::string dataName;
 
   switch (dataType) {
@@ -155,8 +158,8 @@ void wom::vision::Limelight::SetCrop(std::array<double, 4> crop) {
   table->PutNumberArray("camtran", crop);
 }
 
-units::meters_per_second_t wom::vision::Limelight::GetSpeed(frc::Pose3d pose1, frc::Pose3d pose2,
-                                                            units::second_t dt) {
+units::meters_per_second_t wom::vision::Limelight::GetSpeed(
+    frc::Pose3d pose1, frc::Pose3d pose2, units::second_t dt) {
   frc::Transform3d dPose{pose1, pose2};
   frc::Translation3d dTranslation = dPose.Translation();
 
@@ -169,8 +172,9 @@ units::meters_per_second_t wom::vision::Limelight::GetSpeed(frc::Pose3d pose1, f
 
 frc::Pose3d wom::vision::Limelight::GetPose() {
   std::vector<double> pose = GetAprilTagData(LimelightAprilTagData::kBotpose);
-  return frc::Pose3d(pose[1] * 1_m, 1_m * pose[2], 1_m * pose[3],
-                     frc::Rotation3d(1_deg * (pose[4]), 1_deg * (pose[5]), 1_deg * (pose[6])));
+  return frc::Pose3d(
+      pose[1] * 1_m, 1_m * pose[2], 1_m * pose[3],
+      frc::Rotation3d(1_deg * (pose[4]), 1_deg * (pose[5]), 1_deg * (pose[6])));
 }
 
 void wom::vision::Limelight::OnStart() {
@@ -181,9 +185,11 @@ void wom::vision::Limelight::OnUpdate(units::time::second_t dt) {
   wom::utils::WritePose3NT(table, GetPose());
 }
 
-bool wom::vision::Limelight::IsAtSetPoseVision(frc::Pose3d pose, units::second_t dt) {
+bool wom::vision::Limelight::IsAtSetPoseVision(frc::Pose3d pose,
+                                               units::second_t dt) {
   frc::Pose3d actualPose = GetPose();
   frc::Pose3d relativePose = actualPose.RelativeTo(pose);
-  return (units::math::fabs(relativePose.X()) < 0.01_m && units::math::fabs(relativePose.Y()) < 0.01_m &&
+  return (units::math::fabs(relativePose.X()) < 0.01_m &&
+          units::math::fabs(relativePose.Y()) < 0.01_m &&
           GetSpeed(pose, GetPose(), dt) < 1_m / 1_s);
 }
diff --git a/wombat/src/main/include/behaviour/Behaviour.h b/wombat/src/main/include/behaviour/Behaviour.h
index 84ac8689..4f2e357e 100644
--- a/wombat/src/main/include/behaviour/Behaviour.h
+++ b/wombat/src/main/include/behaviour/Behaviour.h
@@ -22,7 +22,13 @@
 #include "HasBehaviour.h"
 
 namespace behaviour {
-enum class BehaviourState { INITIALISED, RUNNING, DONE, TIMED_OUT, INTERRUPTED };
+enum class BehaviourState {
+  INITIALISED,
+  RUNNING,
+  DONE,
+  TIMED_OUT,
+  INTERRUPTED
+};
 
 class SequentialBehaviour;
 
@@ -40,7 +46,8 @@ class Behaviour : public std::enable_shared_from_this<Behaviour> {
  public:
   using ptr = std::shared_ptr<Behaviour>;
 
-  explicit Behaviour(std::string name = "<unnamed behaviour>", units::time::second_t period = 20_ms);
+  Behaviour(std::string name = "<unnamed behaviour>",
+            units::time::second_t period = 20_ms);
   ~Behaviour();
 
   /**
@@ -186,15 +193,16 @@ class SequentialBehaviour : public Behaviour {
   std::deque<ptr> _queue;
 };
 
-inline std::shared_ptr<SequentialBehaviour> operator<<(Behaviour::ptr a, Behaviour::ptr b) {
+inline std::shared_ptr<SequentialBehaviour> operator<<(Behaviour::ptr a,
+                                                       Behaviour::ptr b) {
   auto seq = std::make_shared<SequentialBehaviour>();
   seq->Add(a);
   seq->Add(b);
   return seq;
 }
 
-inline std::shared_ptr<SequentialBehaviour> operator<<(std::shared_ptr<SequentialBehaviour> a,
-                                                       Behaviour::ptr b) {
+inline std::shared_ptr<SequentialBehaviour> operator<<(
+    std::shared_ptr<SequentialBehaviour> a, Behaviour::ptr b) {
   a->Add(b);
   return a;
 }
@@ -239,8 +247,10 @@ class ConcurrentBehaviour : public Behaviour {
  * Create a concurrent behaviour group, waiting for all behaviours
  * to finish before moving on.
  */
-inline std::shared_ptr<ConcurrentBehaviour> operator&(Behaviour::ptr a, Behaviour::ptr b) {
-  auto conc = std::make_shared<ConcurrentBehaviour>(ConcurrentBehaviourReducer::ALL);
+inline std::shared_ptr<ConcurrentBehaviour> operator&(Behaviour::ptr a,
+                                                      Behaviour::ptr b) {
+  auto conc =
+      std::make_shared<ConcurrentBehaviour>(ConcurrentBehaviourReducer::ALL);
   conc->Add(a);
   conc->Add(b);
   return conc;
@@ -251,8 +261,10 @@ inline std::shared_ptr<ConcurrentBehaviour> operator&(Behaviour::ptr a, Behaviou
  * be interrupted as soon as any members of the group are finished (the
  * behaviours are 'raced' against each other).
  */
-inline std::shared_ptr<ConcurrentBehaviour> operator|(Behaviour::ptr a, Behaviour::ptr b) {
-  auto conc = std::make_shared<ConcurrentBehaviour>(ConcurrentBehaviourReducer::ANY);
+inline std::shared_ptr<ConcurrentBehaviour> operator|(Behaviour::ptr a,
+                                                      Behaviour::ptr b) {
+  auto conc =
+      std::make_shared<ConcurrentBehaviour>(ConcurrentBehaviourReducer::ANY);
   conc->Add(a);
   conc->Add(b);
   return conc;
@@ -320,7 +332,8 @@ struct Switch : public Behaviour {
    * @param condition The function yielding true if this is the correct option
    * @param b The behaviour to call if this option is provided.
    */
-  std::shared_ptr<Switch> When(std::function<bool(T&)> condition, Behaviour::ptr b) {
+  std::shared_ptr<Switch> When(std::function<bool(T&)> condition,
+                               Behaviour::ptr b) {
     _options.push_back(std::pair(condition, b));
     Inherit(*b);
     return std::reinterpret_pointer_cast<Switch<T>>(shared_from_this());
@@ -376,7 +389,8 @@ struct Switch : public Behaviour {
 
  private:
   std::function<T()> _fn;
-  wpi::SmallVector<std::pair<std::function<bool(T&)>, Behaviour::ptr>, 4> _options;
+  wpi::SmallVector<std::pair<std::function<bool(T&)>, Behaviour::ptr>, 4>
+      _options;
   Behaviour::ptr _locked = nullptr;
 };
 
@@ -393,8 +407,10 @@ struct Decide : public Switch<std::monostate> {
    * @param condition The function yielding true if this is the correct option
    * @param b The behaviour to call if this option is provided.
    */
-  std::shared_ptr<Decide> When(std::function<bool()> condition, Behaviour::ptr b) {
-    return std::reinterpret_pointer_cast<Decide>(Switch::When([condition](auto) { return condition(); }, b));
+  std::shared_ptr<Decide> When(std::function<bool()> condition,
+                               Behaviour::ptr b) {
+    return std::reinterpret_pointer_cast<Decide>(
+        Switch::When([condition](auto) { return condition(); }, b));
   }
 };
 
diff --git a/wombat/src/main/include/behaviour/HasBehaviour.h b/wombat/src/main/include/behaviour/HasBehaviour.h
index bae5ac05..c9b35b2e 100644
--- a/wombat/src/main/include/behaviour/HasBehaviour.h
+++ b/wombat/src/main/include/behaviour/HasBehaviour.h
@@ -30,7 +30,8 @@ class HasBehaviour {
 
  protected:
   std::shared_ptr<Behaviour> _active_behaviour{nullptr};
-  std::function<std::shared_ptr<Behaviour>(void)> _default_behaviour_producer{nullptr};
+  std::function<std::shared_ptr<Behaviour>(void)> _default_behaviour_producer{
+      nullptr};
 
  private:
   friend class BehaviourScheduler;
diff --git a/wombat/src/main/include/drivetrain/SwerveDrive.h b/wombat/src/main/include/drivetrain/SwerveDrive.h
index 661a2943..b48db5a5 100644
--- a/wombat/src/main/include/drivetrain/SwerveDrive.h
+++ b/wombat/src/main/include/drivetrain/SwerveDrive.h
@@ -6,6 +6,7 @@
 
 #include <frc/estimator/SwerveDrivePoseEstimator.h>
 #include <frc/kinematics/SwerveDriveKinematics.h>
+#include <frc/controller/PIDController.h>
 #include <networktables/DoubleTopic.h>
 #include <networktables/NetworkTable.h>
 #include <networktables/NetworkTableInstance.h>
@@ -47,11 +48,12 @@ struct SwerveModuleConfig {
 
 class SwerveModule {
  public:
-  using angle_pid_conf_t = utils::PIDConfig<units::radian, units::volt>;
-  using velocity_pid_conf_t = utils::PIDConfig<units::meters_per_second, units::volt>;
+  //using angle_pid_conf_t = utils::PIDConfig<units::radian, units::volt>;
+  using velocity_pid_conf_t =
+      utils::PIDConfig<units::meters_per_second, units::volt>;
 
-  SwerveModule(std::string path, SwerveModuleConfig config, angle_pid_conf_t anglePID,
-               velocity_pid_conf_t velocityPID);
+  SwerveModule(std::string path, SwerveModuleConfig config,
+               /*angle_pid_conf_t anglePID,*/ velocity_pid_conf_t velocityPID);
   void OnUpdate(units::second_t dt);
   void OnStart();
 
@@ -65,7 +67,8 @@ class SwerveModule {
 
   void SetZero(units::second_t dt);
   void SetIdle();
-  void SetPID(units::radian_t angle, units::meters_per_second_t speed, units::second_t dt);
+  void SetPID(units::radian_t angle, units::meters_per_second_t speed,
+              units::second_t dt);
   void SetZero();
   void SetVoltageLimit(units::volt_t driveModuleVoltageLimit);
 
@@ -81,7 +84,8 @@ class SwerveModule {
 
   const SwerveModuleConfig& GetConfig() const;
 
-  utils::PIDController<units::radians, units::volt> _anglePIDController;
+  //utils::PIDController<units::radians, units::volt> _anglePIDController;
+  frc::PIDController _anglePIDController;
 
  private:
   SwerveModuleConfig _config;
@@ -91,7 +95,7 @@ class SwerveModule {
   bool _hasZeroedEncoder = false;
   bool _hasZeroed = false;
 
-  utils::PIDController<units::meters_per_second, units::volt> _velocityPIDController;
+  frc::PIDController _velocityPIDController;
 
   std::shared_ptr<nt::NetworkTable> _table;
 
@@ -102,19 +106,22 @@ class SwerveModule {
 };
 
 struct SwerveDriveConfig {
-  using pose_angle_conf_t = utils::PIDConfig<units::radian, units::radians_per_second>;
-  using pose_position_conf_t = utils::PIDConfig<units::meter, units::meters_per_second>;
-  using balance_conf_t = utils::PIDConfig<units::degree, units::meters_per_second>;
+  /*using pose_angle_conf_t =
+      utils::PIDConfig<units::radian, units::radians_per_second>;*/
+  using pose_position_conf_t =
+      utils::PIDConfig<units::meter, units::meters_per_second>;
+  using balance_conf_t =
+      utils::PIDConfig<units::degree, units::meters_per_second>;
 
   std::string path;
-  SwerveModule::angle_pid_conf_t anglePID;
+  //SwerveModule::angle_pid_conf_t anglePID;
   SwerveModule::velocity_pid_conf_t velocityPID;
 
   wpi::array<SwerveModuleConfig, 4> modules;
 
   ctre::phoenix6::hardware::Pigeon2* gyro;
 
-  pose_angle_conf_t poseAnglePID;
+  //pose_angle_conf_t poseAnglePID;
   pose_position_conf_t posePositionPID;
 
   units::kilogram_t mass;
@@ -168,7 +175,8 @@ class SwerveDrive : public behaviour::HasBehaviour {
    * @brief This function switches the state to handle the robot's rotation
    * matching that of the joystick
    */
-  void RotateMatchJoystick(units::radian_t joystickAngle, FieldRelativeSpeeds speeds);
+  void RotateMatchJoystick(units::radian_t joystickAngle,
+                           FieldRelativeSpeeds speeds);
 
   void SetIdle();
 
@@ -178,7 +186,8 @@ class SwerveDrive : public behaviour::HasBehaviour {
   void SetFieldRelativeVelocity(FieldRelativeSpeeds speeds);
   void SetPose(frc::Pose2d pose);
   bool IsAtSetPose();
-  void SetIndividualTuning(int mod, units::radian_t angle, units::meters_per_second_t speed);
+  void SetIndividualTuning(int mod, units::radian_t angle,
+                           units::meters_per_second_t speed);
   void SetTuning(units::radian_t angle, units::meters_per_second_t speed);
   void SetZero();
   void SetVoltageLimit(units::volt_t driveVoltageLimit);
@@ -213,7 +222,9 @@ class SwerveDrive : public behaviour::HasBehaviour {
   frc::SwerveDriveKinematics<4> _kinematics;
   frc::SwerveDrivePoseEstimator<4> _poseEstimator;
 
-  utils::PIDController<units::radian, units::radians_per_second> _anglePIDController;
+  /*utils::PIDController<units::radian, units::radians_per_second>
+      _anglePIDController;*/
+  frc::PIDController _anglePIDController;
   utils::PIDController<units::meter, units::meters_per_second> _xPIDController;
   utils::PIDController<units::meter, units::meters_per_second> _yPIDController;
 
diff --git a/wombat/src/main/include/drivetrain/behaviours/SwerveBehaviours.h b/wombat/src/main/include/drivetrain/behaviours/SwerveBehaviours.h
index 11400c4a..5aa1972f 100644
--- a/wombat/src/main/include/drivetrain/behaviours/SwerveBehaviours.h
+++ b/wombat/src/main/include/drivetrain/behaviours/SwerveBehaviours.h
@@ -37,14 +37,16 @@ class ManualDrivebase : public behaviour::Behaviour {
    * A pointer to the controller that the driver has been allocated (the
    * allocated memory address that stores the "driver controller" object)
    */
-  ManualDrivebase(wom::drivetrain::SwerveDrive* swerveDrivebase, frc::XboxController* driverController);
+  ManualDrivebase(wom::drivetrain::SwerveDrive* swerveDrivebase,
+                  frc::XboxController* driverController);
 
   void OnTick(units::second_t deltaTime) override;
   /**
    * @brief This function handles all of the logic behind the tangent function,
    * to be able to calculate an angle between 0 andd 360 degrees, inclusively
    */
-  void CalculateRequestedAngle(double joystickX, double joystickY, units::degree_t defaultAngle);
+  void CalculateRequestedAngle(double joystickX, double joystickY,
+                               units::degree_t defaultAngle);
   void OnStart() override;
   void ResetMode();
 
@@ -70,8 +72,8 @@ class ManualDrivebase : public behaviour::Behaviour {
   const double turningDeadzone = 0.2;
 
   // Variables for solution to Anti-tip
-  double prevJoystickX, prevJoystickY, prevPrevJoystickX, prevPrevJoystickY, usingJoystickXPos,
-      usingJoystickYPos;
+  double prevJoystickX, prevJoystickY, prevPrevJoystickX, prevPrevJoystickY,
+      usingJoystickXPos, usingJoystickYPos;
   // The speed that the joystick must travel to activate averaging over previous
   // 3 joystick positions
   const double smoothingThreshold = 1;
@@ -124,8 +126,8 @@ class GoToPose : public behaviour::Behaviour {
 
 class FollowTrajectory : public behaviour::Behaviour {
  public:
-  FollowTrajectory(wom::drivetrain::SwerveDrive* swerve, wom::utils::Pathplanner* pathplanner,
-                   std::string path);
+  FollowTrajectory(wom::drivetrain::SwerveDrive* swerve,
+                   wom::utils::Pathplanner* pathplanner, std::string path);
 
   void OnTick(units::second_t dt) override;
 
@@ -182,7 +184,8 @@ class TempSimSwerveDrive {
 
 class AutoSwerveDrive {
  public:
-  AutoSwerveDrive(wom::drivetrain::SwerveDrive* swerve, frc::Timer* timer, frc::Field2d* field);
+  AutoSwerveDrive(wom::drivetrain::SwerveDrive* swerve, frc::Timer* timer,
+                  frc::Field2d* field);
 
   void OnUpdate();
 
diff --git a/wombat/src/main/include/subsystems/Arm.h b/wombat/src/main/include/subsystems/Arm.h
index 8b91be72..155fd4cd 100644
--- a/wombat/src/main/include/subsystems/Arm.h
+++ b/wombat/src/main/include/subsystems/Arm.h
@@ -68,7 +68,8 @@ class Arm : public behaviour::HasBehaviour {
   ArmConfig _config;
   ArmState _state = ArmState::kIdle;
   wom::utils::PIDController<units::radian, units::volt> _pid;
-  wom::utils::PIDController<units::radians_per_second, units::volt> _velocityPID;
+  wom::utils::PIDController<units::radians_per_second, units::volt>
+      _velocityPID;
 
   std::shared_ptr<nt::NetworkTable> _table;
 
diff --git a/wombat/src/main/include/subsystems/Shooter.h b/wombat/src/main/include/subsystems/Shooter.h
index e5ac219d..2bbf6f5e 100644
--- a/wombat/src/main/include/subsystems/Shooter.h
+++ b/wombat/src/main/include/subsystems/Shooter.h
@@ -62,7 +62,8 @@ class ShooterConstant : public behaviour::Behaviour {
 
 class ShooterSpinup : public behaviour::Behaviour {
  public:
-  ShooterSpinup(Shooter* s, units::radians_per_second_t speed, bool hold = false);
+  ShooterSpinup(Shooter* s, units::radians_per_second_t speed,
+                bool hold = false);
 
   void OnTick(units::second_t dt) override;
 
diff --git a/wombat/src/main/include/utils/Encoder.h b/wombat/src/main/include/utils/Encoder.h
index 62ccc28f..b504e1c0 100644
--- a/wombat/src/main/include/utils/Encoder.h
+++ b/wombat/src/main/include/utils/Encoder.h
@@ -20,7 +20,8 @@ namespace wom {
 namespace utils {
 class Encoder {
  public:
-  Encoder(double encoderTicksPerRotation, int type, units::meter_t wheelRadius, double reduction = 1.0);
+  Encoder(double encoderTicksPerRotation, int type, units::meter_t wheelRadius,
+          double reduction = 1.0);
 
   virtual double GetEncoderRawTicks() const = 0;
   virtual double GetEncoderTickVelocity() const = 0;  // ticks/s
@@ -41,6 +42,9 @@ class Encoder {
   int encoderType = 0;
   double _reduction;
 
+  virtual double GetVelocity() const = 0;
+  double GetVelocityValue() const;
+
  private:
   double _encoderTicksPerRotation;
   units::radian_t _offset = 0_rad;
@@ -50,30 +54,30 @@ class Encoder {
 
 class DigitalEncoder : public Encoder {
  public:
-  DigitalEncoder(int channelA, int channelB, double ticksPerRotation, units::meter_t wheelRadius,
-                 double reduction = 1)
-      : Encoder(ticksPerRotation, reduction, wheelRadius, 0), _nativeEncoder(channelA, channelB) {}
+  DigitalEncoder(int channelA, int channelB, double ticksPerRotation,
+                 units::meter_t wheelRadius, double reduction = 1)
+      : Encoder(ticksPerRotation, reduction, wheelRadius, 0),
+        _nativeEncoder(channelA, channelB) {}
 
   double GetEncoderRawTicks() const override;
   double GetEncoderTickVelocity() const override;
 
   double GetPosition() const;
-  double GetVelocity() const;
-
+  double GetVelocity() const override;
  private:
   frc::Encoder _nativeEncoder;
 };
 
-class SimCANSparkMaxEncoder;
 class CANSparkMaxEncoder : public Encoder {
  public:
-  explicit CANSparkMaxEncoder(rev::CANSparkMax* controller, units::meter_t wheelRadius, double reduction = 1);
+  explicit CANSparkMaxEncoder(rev::CANSparkMax* controller,
+                              units::meter_t wheelRadius, double reduction = 1);
 
   double GetEncoderRawTicks() const override;
   double GetEncoderTickVelocity() const override;
 
   double GetPosition() const;
-  double GetVelocity() const;
+  double GetVelocity() const override;
 
  protected:
   rev::SparkRelativeEncoder _encoder;
@@ -82,11 +86,12 @@ class CANSparkMaxEncoder : public Encoder {
 
 class TalonFXEncoder : public Encoder {
  public:
-  TalonFXEncoder(ctre::phoenix6::hardware::TalonFX* controller, units::meter_t wheelRadius,
-                 double reduction = 1);
+  TalonFXEncoder(ctre::phoenix6::hardware::TalonFX* controller,
+                 units::meter_t wheelRadius, double reduction = 1);
 
   double GetEncoderRawTicks() const override;
   double GetEncoderTickVelocity() const override;
+  double GetVelocity() const override;
 
  private:
   ctre::phoenix6::hardware::TalonFX* _controller;
@@ -94,11 +99,12 @@ class TalonFXEncoder : public Encoder {
 
 class DutyCycleEncoder : public Encoder {
  public:
-  DutyCycleEncoder(int channel, units::meter_t wheelRadius, double ticksPerRotation = 1,
-                   double reduction = 1);
+  DutyCycleEncoder(int channel, units::meter_t wheelRadius,
+                   double ticksPerRotation = 1, double reduction = 1);
 
   double GetEncoderRawTicks() const override;
   double GetEncoderTickVelocity() const override;
+  double GetVelocity() const override;
 
  private:
   frc::DutyCycleEncoder _dutyCycleEncoder;
@@ -106,12 +112,14 @@ class DutyCycleEncoder : public Encoder {
 
 class CanEncoder : public Encoder {
  public:
-  CanEncoder(int deviceNumber, units::meter_t wheelRadius, double ticksPerRotation = 4095,
-             double reduction = 1, std::string name = "Drivebase");
+  CanEncoder(int deviceNumber, units::meter_t wheelRadius,
+             double ticksPerRotation = 4095, double reduction = 6.75,
+             std::string name = "Drivebase");
 
   double GetEncoderRawTicks() const override;
   double GetEncoderTickVelocity() const override;
   double GetAbsoluteEncoderPosition();
+  double GetVelocity() const override;
 
   const double constantValue = 0.0;
 
diff --git a/wombat/src/main/include/utils/PID.h b/wombat/src/main/include/utils/PID.h
index a0f2eaf9..2ce378ec 100644
--- a/wombat/src/main/include/utils/PID.h
+++ b/wombat/src/main/include/utils/PID.h
@@ -9,6 +9,7 @@
 #include <units/base.h>
 #include <units/time.h>
 
+#include <cmath>
 #include <memory>
 #include <optional>
 #include <string>
@@ -23,15 +24,19 @@ struct PIDConfig {
   using in_t = units::unit_t<IN>;
 
   using kp_t = units::unit_t<units::compound_unit<OUT, units::inverse<IN>>>;
-  using ki_t = units::unit_t<units::compound_unit<OUT, units::inverse<IN>, units::inverse<units::second>>>;
-  using kd_t = units::unit_t<units::compound_unit<OUT, units::inverse<IN>, units::second>>;
+  using ki_t =
+      units::unit_t<units::compound_unit<OUT, units::inverse<IN>,
+                                         units::inverse<units::second>>>;
+  using kd_t = units::unit_t<
+      units::compound_unit<OUT, units::inverse<IN>, units::second>>;
 
   using error_t = units::unit_t<IN>;
-  using deriv_t = units::unit_t<units::compound_unit<IN, units::inverse<units::second>>>;
+  using deriv_t =
+      units::unit_t<units::compound_unit<IN, units::inverse<units::second>>>;
 
-  PIDConfig(std::string path, kp_t kp = kp_t{0}, ki_t ki = ki_t{0}, kd_t kd = kd_t{0},
-            error_t stableThresh = error_t{-1}, deriv_t stableDerivThresh = deriv_t{-1},
-            in_t izone = in_t{-1})
+  PIDConfig(std::string path, kp_t kp = kp_t{0}, ki_t ki = ki_t{0},
+            kd_t kd = kd_t{0}, error_t stableThresh = error_t{-1},
+            deriv_t stableDerivThresh = deriv_t{-1}, in_t izone = in_t{-1})
       : path(path),
         kp(kp),
         ki(ki),
@@ -59,14 +64,23 @@ struct PIDConfig {
  public:
   void RegisterNT() {
     auto table = nt::NetworkTableInstance::GetDefault().GetTable(path);
-    _nt_bindings.emplace_back(std::make_shared<NTBoundUnit<typename kp_t::unit_type>>(table, "kP", kp));
-    _nt_bindings.emplace_back(std::make_shared<NTBoundUnit<typename ki_t::unit_type>>(table, "kI", ki));
-    _nt_bindings.emplace_back(std::make_shared<NTBoundUnit<typename kd_t::unit_type>>(table, "kD", kd));
     _nt_bindings.emplace_back(
-        std::make_shared<NTBoundUnit<typename error_t::unit_type>>(table, "stableThresh", stableThresh));
-    _nt_bindings.emplace_back(std::make_shared<NTBoundUnit<typename deriv_t::unit_type>>(
-        table, "stableThreshVelocity", stableDerivThresh));
-    _nt_bindings.emplace_back(std::make_shared<NTBoundUnit<IN>>(table, "izone", izone));
+        std::make_shared<NTBoundUnit<typename kp_t::unit_type>>(table, "kP",
+                                                                kp));
+    _nt_bindings.emplace_back(
+        std::make_shared<NTBoundUnit<typename ki_t::unit_type>>(table, "kI",
+                                                                ki));
+    _nt_bindings.emplace_back(
+        std::make_shared<NTBoundUnit<typename kd_t::unit_type>>(table, "kD",
+                                                                kd));
+    _nt_bindings.emplace_back(
+        std::make_shared<NTBoundUnit<typename error_t::unit_type>>(
+            table, "stableThresh", stableThresh));
+    _nt_bindings.emplace_back(
+        std::make_shared<NTBoundUnit<typename deriv_t::unit_type>>(
+            table, "stableThreshVelocity", stableDerivThresh));
+    _nt_bindings.emplace_back(
+        std::make_shared<NTBoundUnit<IN>>(table, "izone", izone));
   }
 };
 
@@ -80,15 +94,19 @@ class PIDController {
 
   config_t config;
 
-  PIDController(std::string path, config_t initialGains, in_t setpoint = in_t{0})
+  PIDController(std::string path, config_t initialGains,
+                in_t setpoint = in_t{0})
       : config(initialGains),
         _setpoint(setpoint),
-        _posFilter(frc::LinearFilter<typename config_t::error_t>::MovingAverage(20)),
-        _velFilter(frc::LinearFilter<typename config_t::deriv_t>::MovingAverage(20)),
+        _posFilter(
+            frc::LinearFilter<typename config_t::error_t>::MovingAverage(20)),
+        _velFilter(
+            frc::LinearFilter<typename config_t::deriv_t>::MovingAverage(20)),
         _table(nt::NetworkTableInstance::GetDefault().GetTable(path)) {}
 
   void SetSetpoint(in_t setpoint) {
-    if (std::abs(setpoint.value() - _setpoint.value()) > std::abs(0.1 * _setpoint.value())) {
+    if (std::abs(setpoint.value() - _setpoint.value()) >
+        std::abs(0.1 * _setpoint.value())) {
       _iterations = 0;
     }
     _setpoint = setpoint;
@@ -103,9 +121,13 @@ class PIDController {
   void Reset() { _integralSum = sum_t{0}; }
 
   out_t Calculate(in_t pv, units::second_t dt, out_t feedforward = out_t{0}) {
+    pv = units::math::fabs(pv);
     auto error = do_wrap(_setpoint - pv);
+    error = units::math::fabs(error);
     _integralSum += error * dt;
-    if (config.izone.value() > 0 && (error > config.izone || error < -config.izone))
+    _integralSum = units::math::fabs(_integralSum);
+    if (config.izone.value() > 0 &&
+        (error > config.izone || error < -config.izone))
       _integralSum = sum_t{0};
 
     typename config_t::deriv_t deriv{0};
@@ -116,16 +138,8 @@ class PIDController {
     _stablePos = _posFilter.Calculate(error);
     _stableVel = _velFilter.Calculate(deriv);
 
-    auto out = config.kp * error + config.ki * _integralSum + config.kd * deriv + feedforward;
-    // std::cout << "Out value" << out.value() << std::endl;
-
-    _table->GetEntry("pv").SetDouble(pv.value());
-    _table->GetEntry("dt").SetDouble(dt.value());
-    _table->GetEntry("setpoint").SetDouble(_setpoint.value());
-    _table->GetEntry("error").SetDouble(error.value());
-    _table->GetEntry("integralSum").SetDouble(_integralSum.value());
-    _table->GetEntry("stable").SetBoolean(IsStable());
-    _table->GetEntry("demand").SetDouble(out.value());
+    auto out = config.kp * error + config.ki * _integralSum +
+               config.kd * deriv + feedforward;
 
     _last_pv = pv;
     _last_error = error;
@@ -133,8 +147,10 @@ class PIDController {
     return out;
   }
 
-  bool IsStable(std::optional<typename config_t::error_t> stableThreshOverride = {},
-                std::optional<typename config_t::deriv_t> velocityThreshOverride = {}) const {
+  bool IsStable(
+      std::optional<typename config_t::error_t> stableThreshOverride = {},
+      std::optional<typename config_t::deriv_t> velocityThreshOverride = {})
+      const {
     auto stableThresh = config.stableThresh;
     auto stableDerivThresh = config.stableDerivThresh;
 
@@ -143,8 +159,10 @@ class PIDController {
     if (velocityThreshOverride.has_value())
       stableDerivThresh = velocityThreshOverride.value();
 
-    return _iterations > 20 && std::abs(_stablePos.value()) <= std::abs(stableThresh.value()) &&
-           (stableDerivThresh.value() < 0 || std::abs(_stableVel.value()) <= stableDerivThresh.value());
+    return _iterations > 20 &&
+           std::abs(_stablePos.value()) <= std::abs(stableThresh.value()) &&
+           (stableDerivThresh.value() < 0 ||
+            std::abs(_stableVel.value()) <= stableDerivThresh.value());
   }
 
  private:
diff --git a/wombat/src/main/include/utils/Util.h b/wombat/src/main/include/utils/Util.h
index d624e408..665c85ec 100644
--- a/wombat/src/main/include/utils/Util.h
+++ b/wombat/src/main/include/utils/Util.h
@@ -28,30 +28,39 @@ units::second_t now();
 
 class NTBound {
  public:
-  NTBound(std::shared_ptr<nt::NetworkTable> table, std::string name, const nt::Value& value,
+  NTBound(std::shared_ptr<nt::NetworkTable> table, std::string name,
+          const nt::Value& value,
           std::function<void(const nt::Value&)> onUpdateFn)
-      : _table(table), _entry(table->GetEntry(name)), _onUpdate(onUpdateFn), _name(name) {
+      : _table(table),
+        _entry(table->GetEntry(name)),
+        _onUpdate(onUpdateFn),
+        _name(name) {
     _entry.SetValue(value);
     // _listener = table->AddListener(name, , ([=](const nt::EntryNotification
     // &evt) {
     //   this->_onUpdate(evt.value);
     // }, NT_NOTIFY_UPDATE);
-    _listener =
-        table->AddListener(name, nt::EventFlags::kValueAll,
-                           ([this](nt::NetworkTable* table, std::string_view key, const nt::Event& event) {
-                             std::cout << "NT UPDATE" << std::endl;
-                             this->_onUpdate(event.GetValueEventData()->value);
-                           }));
+    _listener = table->AddListener(
+        name, nt::EventFlags::kValueAll,
+        ([this](nt::NetworkTable* table, std::string_view key,
+                const nt::Event& event) {
+          std::cout << "NT UPDATE" << std::endl;
+          this->_onUpdate(event.GetValueEventData()->value);
+        }));
   }
 
   NTBound(const NTBound& other)
-      : _table(other._table), _entry(other._entry), _onUpdate(other._onUpdate), _name(other._name) {
-    _listener =
-        _table->AddListener(_name, nt::EventFlags::kValueAll,
-                            ([this](nt::NetworkTable* table, std::string_view key, const nt::Event& event) {
-                              std::cout << "NT UPDATE" << std::endl;
-                              this->_onUpdate(event.GetValueEventData()->value);
-                            }));
+      : _table(other._table),
+        _entry(other._entry),
+        _onUpdate(other._onUpdate),
+        _name(other._name) {
+    _listener = _table->AddListener(
+        _name, nt::EventFlags::kValueAll,
+        ([this](nt::NetworkTable* table, std::string_view key,
+                const nt::Event& event) {
+          std::cout << "NT UPDATE" << std::endl;
+          this->_onUpdate(event.GetValueEventData()->value);
+        }));
   }
 
   ~NTBound() { _table->RemoveListener(_listener); }
@@ -66,7 +75,8 @@ class NTBound {
 
 class NTBoundDouble : public NTBound {
  public:
-  NTBoundDouble(std::shared_ptr<nt::NetworkTable> table, std::string name, double& val)
+  NTBoundDouble(std::shared_ptr<nt::NetworkTable> table, std::string name,
+                double& val)
       : NTBound(table, name, nt::Value::MakeDouble(val),
                 [&val](const nt::Value& v) { val = v.GetDouble(); }) {}
 };
@@ -74,15 +84,20 @@ class NTBoundDouble : public NTBound {
 template <typename T>
 class NTBoundUnit : public NTBound {
  public:
-  NTBoundUnit(std::shared_ptr<nt::NetworkTable> table, std::string name, units::unit_t<T>& val)
+  NTBoundUnit(std::shared_ptr<nt::NetworkTable> table, std::string name,
+              units::unit_t<T>& val)
       : NTBound(table, name, nt::Value::MakeDouble(val.value()),
-                [&val](const nt::Value& v) { val = units::unit_t<T>{v.GetDouble()}; }) {}
+                [&val](const nt::Value& v) {
+                  val = units::unit_t<T>{v.GetDouble()};
+                }) {}
 };
 
 void WritePose2NT(std::shared_ptr<nt::NetworkTable> table, frc::Pose2d pose);
 void WritePose3NT(std::shared_ptr<nt::NetworkTable> table, frc::Pose3d pose);
-void WriteTrajectory(std::shared_ptr<nt::NetworkTable> table, frc::Trajectory trajectory);
-void WriteTrajectoryState(std::shared_ptr<nt::NetworkTable> table, frc::Trajectory::State state);
+void WriteTrajectory(std::shared_ptr<nt::NetworkTable> table,
+                     frc::Trajectory trajectory);
+void WriteTrajectoryState(std::shared_ptr<nt::NetworkTable> table,
+                          frc::Trajectory::State state);
 
 frc::Pose2d TrajectoryStateToPose2d(frc::Trajectory::State state);
 
diff --git a/wombat/src/main/include/vision/Limelight.h b/wombat/src/main/include/vision/Limelight.h
index 5aa6d0b7..ddfb3739 100644
--- a/wombat/src/main/include/vision/Limelight.h
+++ b/wombat/src/main/include/vision/Limelight.h
@@ -26,11 +26,20 @@
 namespace wom {
 namespace vision {
 
-enum class LimelightLEDMode { kPipelineDefault = 0, kForceOff = 1, kForceBlink = 2, kForceOn = 3 };
+enum class LimelightLEDMode {
+  kPipelineDefault = 0,
+  kForceOff = 1,
+  kForceBlink = 2,
+  kForceOn = 3
+};
 
 enum class LimelightCamMode { kVisionProcessor = 0, kDriverCamera = 1 };
 
-enum class LimelightStreamMode { kStandard = 0, kPiPMain = 1, kPiPSecondary = 2 };
+enum class LimelightStreamMode {
+  kStandard = 0,
+  kPiPMain = 1,
+  kPiPSecondary = 2
+};
 
 enum class LimelightSnapshotMode { kReset = 0, kSingle = 1 };
 
@@ -82,12 +91,14 @@ class Limelight {
 
   std::string GetName();
 
-  std::shared_ptr<nt::NetworkTable> table = nt::NetworkTableInstance::GetDefault().GetTable("limelight");
+  std::shared_ptr<nt::NetworkTable> table =
+      nt::NetworkTableInstance::GetDefault().GetTable("limelight");
 
   std::pair<double, double> GetOffset();
 
   std::vector<double> GetAprilTagData(LimelightAprilTagData dataType);
-  double GetTargetingData(LimelightTargetingData dataType, double defaultValue = 0.0);
+  double GetTargetingData(LimelightTargetingData dataType,
+                          double defaultValue = 0.0);
   void SetLEDMode(LimelightLEDMode mode);
   void SetCamMode(LimelightCamMode mode);
   void SetPipeline(LimelightPipeline pipeline);
@@ -100,7 +111,8 @@ class Limelight {
 
   bool IsAtSetPoseVision(frc::Pose3d pose, units::second_t dt);
 
-  units::meters_per_second_t GetSpeed(frc::Pose3d pose1, frc::Pose3d pose2, units::second_t dt);
+  units::meters_per_second_t GetSpeed(frc::Pose3d pose1, frc::Pose3d pose2,
+                                      units::second_t dt);
 
   frc::Pose3d GetPose();
 

From 7533257f4e32441dab1c19ad5ce59f5ce50b9cef Mon Sep 17 00:00:00 2001
From: totallysomeoneyoudontknow
 <155517980+totallysomeoneyoudontknow@users.noreply.github.com>
Date: Tue, 23 Jan 2024 20:34:07 +0800
Subject: [PATCH 064/207] Shooter - Implements Networking tables (#99)

* added the state raw and changed voltage

* test

* added the state raw and changed voltage

* added pid calculate loop, fixed indenting

* added calculate function on a new line, changed the PID to not stable

* Fixed the if condition so shooterSensor is not tripped

* changed the functions so they start with a capital

* changed the value for shooterMotor

* added shuffleboard and networking tables

* added shuffleboard and networking tables

* added network tables

* fixed merge conflicts and merge errors

* fixed merge conflicts and merge errors

* fixed merge conflicts and merge errors

* fixed errors

* ran wpiformat

* fixed merge errors and conflicts

* fixed github errors

* Alpha Bot Shooter

---------

Co-authored-by: Havish12 <havishchopra10@gmail.com>
Co-authored-by: Isaac Turner <spacey_sooty@outlook.com>
---
 src/main/cpp/Robot.cpp              | 21 ++++----
 src/main/cpp/Shooter.cpp            | 79 +++++++++++++++++++++++++++++
 src/main/cpp/ShooterBehaviour.cpp   | 39 ++++++++++++++
 src/main/include/Robot.h            | 13 +++--
 src/main/include/RobotMap.h         | 21 +++++++-
 src/main/include/Shooter.h          | 43 ++++++++++++++++
 src/main/include/ShooterBehaviour.h | 24 +++++++++
 7 files changed, 222 insertions(+), 18 deletions(-)
 create mode 100644 src/main/cpp/Shooter.cpp
 create mode 100644 src/main/cpp/ShooterBehaviour.cpp
 create mode 100644 src/main/include/Shooter.h
 create mode 100644 src/main/include/ShooterBehaviour.h

diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp
index ddccb14d..0dc53d5f 100644
--- a/src/main/cpp/Robot.cpp
+++ b/src/main/cpp/Robot.cpp
@@ -20,6 +20,12 @@
 static units::second_t lastPeriodic;
 
 void Robot::RobotInit() {
+
+  shooter = new Shooter(robotmap.shooterSystem.config);
+  wom::BehaviourScheduler::GetInstance()->Register(shooter);
+  shooter->SetDefaultBehaviour(
+     [this]() {return wom::make<ShooterManualControl>(shooter, &robotmap.controllers.codriver); });
+  
   sched = wom::BehaviourScheduler::GetInstance();
   m_chooser.SetDefaultOption("Default Auto", "Default Auto");
 
@@ -73,6 +79,8 @@ void Robot::RobotPeriodic() {
   lastPeriodic = wom::now();
 
   loop.Poll();
+  wom::BehaviourScheduler::GetInstance()->Tick();
+  shooter->OnUpdate(dt);
   sched->Tick();
 
   robotmap.swerveTable.swerveDriveTable->GetEntry("frontLeftEncoder").SetDouble(robotmap.swerveBase.moduleConfigs[0].turnMotor.encoder->GetEncoderPosition().value());
@@ -84,25 +92,20 @@ void Robot::RobotPeriodic() {
 }
 
 void Robot::AutonomousInit() {
-  // m_driveSim->SetPath(m_path_chooser.GetSelected());
-
   loop.Clear();
   sched->InterruptAll();
 }
 void Robot::AutonomousPeriodic() {
-  // m_driveSim->OnUpdate();
 }
 
 void Robot::TeleopInit() {
   loop.Clear();
+  wom::BehaviourScheduler *sched = wom::BehaviourScheduler::GetInstance();
   sched->InterruptAll();
-
-  // frontLeft->SetVoltage(4_V);
-  // frontRight->SetVoltage(4_V);
-  // backLeft->SetVoltage(4_V);
-  // backRight->SetVoltage(4_V);
 }
-void Robot::TeleopPeriodic() {}
+void Robot::TeleopPeriodic() {
+  
+}
 
 void Robot::DisabledInit() {}
 void Robot::DisabledPeriodic() {}
diff --git a/src/main/cpp/Shooter.cpp b/src/main/cpp/Shooter.cpp
new file mode 100644
index 00000000..613120df
--- /dev/null
+++ b/src/main/cpp/Shooter.cpp
@@ -0,0 +1,79 @@
+// Copyright (c) 2023-2024 CurtinFRC
+// Open Source Software, you can modify it according to the terms
+// of the MIT License at the root of this project
+
+#include "Shooter.h"
+
+Shooter::Shooter(ShooterConfig config) : _config(config)
+// , 
+// _pid{frc::PIDController (1, 0, 0, 0.005_s)}
+{} //config.path + "/pid", config.pidConfig
+void Shooter::OnUpdate(units::second_t dt) {
+  // _pid.SetTolerance(0.1, 1);
+  switch (_state) {
+    case ShooterState::kIdle: {
+      std::cout << "KIdle" << std::endl;
+      _setVoltage = 0_V;
+      // if (_shooterSensor.Get()) {
+      //   _state = ShooterState::kReverse;
+      // }
+    } break;
+    case ShooterState::kSpinUp: {
+      // std::cout << "KSpinUp" << std::endl;
+      // _pid.SetSetpoint(_goal.value());
+      // units::volt_t pidCalculate =
+      //     units::volt_t{_pid.Calculate(_config.ShooterGearbox.encoder->GetEncoderAngularVelocity().value())};
+      // _setVoltage = pidCalculate;
+
+      // if (_pid.AtSetpoint()) {
+      //   SetState(ShooterState::kShooting);
+      // }
+    } break;
+    case ShooterState::kShooting: {
+      // std::cout << "KShooting" << std::endl;
+      // _pid.SetSetpoint(_goal.value());
+      // units::volt_t pidCalculate =
+      //     units::volt_t{_pid.Calculate(_config.ShooterGearbox.encoder->GetEncoderAngularVelocity().value())};
+      // _setVoltage = pidCalculate;
+
+      // if (!_pid.AtSetpoint()) {
+      //   SetState(ShooterState::kSpinUp);
+      // }
+      // if (_shooterSensor.Get()) {
+      //   SetState(ShooterState::kIdle);
+      // }
+    } break;
+
+    case ShooterState::kReverse: {
+      _setVoltage = -5_V;
+      std::cout << "KReverse" << std::endl;
+      // if (!_shooterSensor.Get()) {
+      //   SetState(ShooterState::kIdle);
+      // }
+    } break;
+    case ShooterState::kRaw: {
+      _setVoltage = _rawVoltage;
+      std::cout << "KRaw" << std::endl;
+      // if (_shooterSensor.Get()) {
+      //   SetState(ShooterState::kRaw);
+      // }
+    } break;
+    default: {
+      std::cout << "Error shooter in invalid state" << std::endl;
+    } break;
+  }
+  std::cout << "Voltage:" << _setVoltage.value() << std::endl;
+  _config.ShooterGearbox.motorController->SetVoltage(_setVoltage);
+
+}
+
+void Shooter::SetState(ShooterState state) {
+  _state = state;
+}
+void Shooter::SetRaw(units::volt_t voltage) {
+  _rawVoltage = voltage;
+  _state = ShooterState::kRaw;
+}
+void Shooter::SetPidGoal(units::radians_per_second_t goal) {
+  _goal = goal;
+}
diff --git a/src/main/cpp/ShooterBehaviour.cpp b/src/main/cpp/ShooterBehaviour.cpp
new file mode 100644
index 00000000..1a035ae0
--- /dev/null
+++ b/src/main/cpp/ShooterBehaviour.cpp
@@ -0,0 +1,39 @@
+// Copyright (c) 2023-2024 CurtinFRC
+// Open Source Software, you can modify it according to the terms
+// of the MIT License at the root of this project
+
+#include "ShooterBehaviour.h"
+
+ShooterManualControl::ShooterManualControl(Shooter* shooter, frc::XboxController* codriver)
+    : _shooter(shooter), _codriver(codriver) {
+  Controls(shooter);
+}
+
+void ShooterManualControl::OnTick(units::second_t dt) {
+  _shooter->table->GetEntry("RawControl").SetBoolean(_rawControl);
+
+
+  if (_codriver->GetAButtonReleased()) {
+    if (_rawControl) {
+      _rawControl = false;
+    } else {
+      _rawControl = true;
+    }
+  }
+
+  if (!_rawControl) {
+      if (_codriver->GetYButton()) {
+        _shooter->SetState(ShooterState::kSpinUp);
+        _shooter->SetPidGoal(10_rad_per_s);
+      }
+  } else {
+    if (_codriver->GetRightTriggerAxis() > 0.1) {
+      _shooter->SetRaw(_codriver->GetRightTriggerAxis() * 12_V);
+    } else if (_codriver->GetLeftTriggerAxis() > 0.1) {
+      _shooter->SetRaw(_codriver->GetLeftTriggerAxis() * -12_V);
+    } else {
+      _shooter->SetRaw(0_V);
+    }
+    _shooter->SetState(ShooterState::kRaw);
+  }
+}
diff --git a/src/main/include/Robot.h b/src/main/include/Robot.h
index 9cef2cb1..1806c61c 100644
--- a/src/main/include/Robot.h
+++ b/src/main/include/Robot.h
@@ -1,9 +1,7 @@
 // Copyright (c) 2023-2024 CurtinFRC
 // Open Source Software, you can modify it according to the terms
 // of the MIT License at the root of this project
-
 #pragma once
-
 #include <frc/Encoder.h>
 #include <frc/TimedRobot.h>
 #include <frc/Timer.h>
@@ -13,14 +11,16 @@
 #include <frc/smartdashboard/Field2d.h>
 #include <frc/smartdashboard/SendableChooser.h>
 #include <frc/smartdashboard/SmartDashboard.h>
-
 #include <string>
-
 #include "RobotMap.h"
+#include "Shooter.h"
+#include "ShooterBehaviour.h"
 #include "Wombat.h"
-
+#include "RobotMap.h"
 class Robot : public frc::TimedRobot {
  public:
+  void TestInit() override;
+  void TestPeriodic() override;
   void RobotInit() override;
   void RobotPeriodic() override;
   void AutonomousInit() override;
@@ -29,12 +29,11 @@ class Robot : public frc::TimedRobot {
   void TeleopPeriodic() override;
   void DisabledInit() override;
   void DisabledPeriodic() override;
-  void TestInit() override;
-  void TestPeriodic() override;
  private:
   RobotMap robotmap;
   wom::BehaviourScheduler* sched;
   frc::EventLoop loop;
+  Shooter *shooter;
 
   frc::SendableChooser<std::string> m_chooser;
 
diff --git a/src/main/include/RobotMap.h b/src/main/include/RobotMap.h
index 6e64d5e8..3ae4db10 100644
--- a/src/main/include/RobotMap.h
+++ b/src/main/include/RobotMap.h
@@ -16,16 +16,33 @@
 #include <ctre/phoenix6/Pigeon2.hpp>
 #include <ctre/phoenix6/TalonFX.hpp>
 
+#include "Shooter.h"
 #include "Wombat.h"
 
 struct RobotMap {
   struct Controllers {
     frc::XboxController driver = frc::XboxController(0);
-    frc::XboxController coDriver = frc::XboxController(1);
+    frc::XboxController codriver = frc::XboxController(1);
     frc::XboxController testController = frc::XboxController(2);
   };
   Controllers controllers;
 
+  struct Shooter {
+    rev::CANSparkMax shooterMotor{11, rev::CANSparkMax::MotorType::kBrushless};
+    // frc::DigitalInput shooterSensor{2};
+
+    // wom::VoltageController shooterMotorGroup = wom::VoltageController::Group(shooterMotor);
+    // wom::CANSparkMaxEncoder* shooterEncoder = new wom::CANSparkMaxEncoder(&shooterMotor, 0.01_m);
+    wom::Gearbox shooterGearbox{&shooterMotor, nullptr, frc::DCMotor::NEO(1)};
+
+    ShooterConfig config{
+        "shooterGearbox",
+        shooterGearbox,
+        // &shooterSensor,
+    };
+  };
+  Shooter shooterSystem;
+  
   struct SwerveBase {
     ctre::phoenix6::hardware::CANcoder frontLeftCancoder{18, "Drivebase"};
     ctre::phoenix6::hardware::CANcoder frontRightCancoder{19, "Drivebase"};
@@ -141,4 +158,4 @@ struct RobotMap {
   struct SwerveTable {
     std::shared_ptr<nt::NetworkTable> swerveDriveTable = nt::NetworkTableInstance::GetDefault().GetTable("swerve");
   }; SwerveTable swerveTable;
-};
\ No newline at end of file
+};
diff --git a/src/main/include/Shooter.h b/src/main/include/Shooter.h
new file mode 100644
index 00000000..cf21c3bc
--- /dev/null
+++ b/src/main/include/Shooter.h
@@ -0,0 +1,43 @@
+// Copyright (c) 2023-2024 CurtinFRC
+// Open Source Software, you can modify it according to the terms
+// of the MIT License at the root of this project
+
+#pragma once
+#include <frc/DigitalInput.h>
+#include <frc/controller/PIDController.h>
+#include <units/angular_velocity.h>
+
+#include <memory>
+#include <string>
+
+#include "Wombat.h"
+
+struct ShooterConfig {
+  std::string path;
+  wom::Gearbox ShooterGearbox;
+  // wom::PIDConfig<units::radians_per_second, units::volt> pidConfig;
+  // frc::DigitalInput* shooterSensor;
+};
+
+enum class ShooterState { kIdle, kShooting, kSpinUp, kReverse, kRaw };
+
+class Shooter : public behaviour::HasBehaviour {
+ public:
+  explicit Shooter(ShooterConfig config);
+
+  void OnUpdate(units::second_t dt);
+  void SetState(ShooterState state);
+  void SetRaw(units::volt_t voltage);
+  void SetPidGoal(units::radians_per_second_t);
+  std::shared_ptr<nt::NetworkTable> table = nt::NetworkTableInstance::GetDefault().GetTable("Shooter");
+  ShooterConfig GetConfig() { return _config; }
+
+ private:
+  ShooterConfig _config;
+  ShooterState _state = ShooterState::kRaw;
+  units::volt_t _rawVoltage;
+  units::radians_per_second_t _goal;
+  units::volt_t _setVoltage = 0_V;
+  // frc::PIDController _pid;
+  // frc::DigitalInput _shooterSensor{0};
+};
diff --git a/src/main/include/ShooterBehaviour.h b/src/main/include/ShooterBehaviour.h
new file mode 100644
index 00000000..ff65b1d8
--- /dev/null
+++ b/src/main/include/ShooterBehaviour.h
@@ -0,0 +1,24 @@
+// Copyright (c) 2023-2024 CurtinFRC
+// Open Source Software, you can modify it according to the terms
+// of the MIT License at the root of this project
+
+#pragma once
+
+#include <frc/XboxController.h>
+
+#include "Robot.h"
+#include "Shooter.h"
+#include "Wombat.h"
+
+class ShooterManualControl : public behaviour::Behaviour {
+ public:
+  ShooterManualControl(Shooter* shooter, frc::XboxController* codriver);
+
+  void OnTick(units::second_t dt) override;
+
+ private:
+  Shooter* _shooter;
+  frc::XboxController* _codriver;
+
+  bool _rawControl = true;
+};

From a86a6479a6f712e76258482c8094e97486e93464 Mon Sep 17 00:00:00 2001
From: kill-shots <155516223+kill-shots@users.noreply.github.com>
Date: Tue, 23 Jan 2024 20:47:00 +0800
Subject: [PATCH 065/207] Intake (#100)

* First version Tank Drive

* Intake draft

* Intake draft fixed errors 1

* Intake draft fixed errors 2

* Intake draft fixed errors 3

* Intake draft fixed errors 4

* Intake draft fixed errors 5

* Network files 1

* Network files 2

* Network files 3

* Almost done i think

* finished wpiformat errors

* finished wpiformat errors 2

* finished wpiformat errors 3

* finished wpiformat errors 4

* ran wpiformat

* finished wpiformat errors 5

* finished wpiformat errors 8

* finished wpiformat errors 9

* finished wpiformat errors 10

* Tested on robot

* Fixed github build errors after modifying during robot tests

* fixed more github build errors 2

* fixed more github build errors 3

* Tested on robot and works

---------

Co-authored-by: Isaac Turner <spacey_sooty@outlook.com>
---
 src/main/cpp/Intake.cpp            | 74 ++++++++++++++++++++++++++++++
 src/main/cpp/IntakeBehaviour.cpp   | 55 ++++++++++++++++++++++
 src/main/cpp/Robot.cpp             | 33 ++++++++++---
 src/main/include/Intake.h          | 41 +++++++++++++++++
 src/main/include/IntakeBehaviour.h | 34 ++++++++++++++
 src/main/include/Robot.h           |  4 ++
 src/main/include/RobotMap.h        | 19 ++++++--
 7 files changed, 250 insertions(+), 10 deletions(-)
 create mode 100644 src/main/cpp/Intake.cpp
 create mode 100644 src/main/cpp/IntakeBehaviour.cpp
 create mode 100644 src/main/include/Intake.h
 create mode 100644 src/main/include/IntakeBehaviour.h

diff --git a/src/main/cpp/Intake.cpp b/src/main/cpp/Intake.cpp
new file mode 100644
index 00000000..75839209
--- /dev/null
+++ b/src/main/cpp/Intake.cpp
@@ -0,0 +1,74 @@
+// Copyright (c) 2023-2024 CurtinFRC
+// Open Source Software, you can modify it according to the terms
+// of the MIT License at the root of this project
+
+#include "Intake.h"
+
+Intake::Intake(IntakeConfig config) : _config(config) {}
+
+IntakeConfig Intake::GetConfig() {
+  return _config;
+}
+
+void Intake::OnUpdate(units::second_t dt) {
+  switch (_state) {
+    case IntakeState::kIdle: {
+      // _config.IntakeMotor.motorController->SetVoltage(0_V);
+      // if (_config.intakeSensor->Get()) {
+      //   setState(IntakeState::kHold);
+      // }
+      _stringStateName = "Idle";
+      _setVoltage = 0_V;
+    } break;
+    case IntakeState::kRaw: {
+      // _config.IntakeMotor.motorController->SetVoltage(_rawVoltage);
+      _stringStateName = "Raw";
+      _setVoltage = _rawVoltage;
+    } break;
+    case IntakeState::kEject: {
+      // _config.IntakeMotor.motorController->SetVoltage(-5_V);
+      // if (_config.intakeSensor->Get() == 0 && _config.magSensor->Get() == 0) {
+      //   setState(IntakeState::kIdle);
+      // }
+      _stringStateName = "Eject";
+      _setVoltage = -5_V;
+    } break;
+    case IntakeState::kHold: {
+      // _config.IntakeMotor.motorController->SetVoltage(0_V);
+      _stringStateName = "Hold";
+      _setVoltage = 0_V;
+    } break;
+    case IntakeState::kIntake: {
+      // _config.IntakeMotor.motorController->SetVoltage(5_V);
+      _stringStateName = "Intake";
+      _setVoltage = 5_V;
+    } break;
+    case IntakeState::kPass: {
+      // _config.IntakeMotor.motorController->SetVoltage(5_V);
+      // if (_config.shooterSensor->Get()) {
+      //   setState(IntakeState::kIdle);
+      //   _stringStateName = "Pass";
+      // }
+      _setVoltage = 5_V;
+    } break;
+    default:
+      std::cout << "Error: Intake in INVALID STATE." << std::endl;
+      break;
+  }
+  _table->GetEntry("State: ").SetString(_stringStateName);
+  _table->GetEntry("Motor Voltage: ").SetDouble(_setVoltage.value());
+  // _table->GetEntry("Intake Sensor: ").SetBoolean(_config.intakeSensor->Get());
+  // _table->GetEntry("Shooter Sensor: ").SetBoolean(_config.shooterSensor->Get());
+  // _table->GetEntry("Magazine Sensor: ").SetBoolean(_config.magSensor->Get());
+
+  std::cout << _setVoltage.value() << std::endl;
+
+  _config.IntakeMotor.motorController->SetVoltage(_setVoltage);
+}
+
+void Intake::setState(IntakeState state) {
+  _state = state;
+}
+void Intake::setRaw(units::volt_t voltage) {
+  _rawVoltage = voltage;
+}
diff --git a/src/main/cpp/IntakeBehaviour.cpp b/src/main/cpp/IntakeBehaviour.cpp
new file mode 100644
index 00000000..90867045
--- /dev/null
+++ b/src/main/cpp/IntakeBehaviour.cpp
@@ -0,0 +1,55 @@
+// Copyright (c) 2023-2024 CurtinFRC
+// Open Source Software, you can modify it according to the terms
+// of the MIT License at the root of this project
+
+#include "IntakeBehaviour.h"
+
+#include <frc/XboxController.h>
+
+IntakeManualControl::IntakeManualControl(Intake* intake, frc::XboxController& codriver)
+    : _intake(intake), _codriver(codriver) {
+  Controls(intake);
+}
+
+void IntakeManualControl::OnTick(units::second_t dt) {
+  if (_codriver.GetBButtonPressed()) {
+    if (_rawControl == true) {
+      _rawControl = false;
+    } else {
+      _rawControl = true;
+    }
+  }
+
+  if (_rawControl) {
+    _intake->setState(IntakeState::kRaw);
+    if (_codriver.GetLeftBumper()) {
+      _intake->setRaw(10_V);
+    } else if (_codriver.GetRightBumper()) {
+      _intake->setRaw(-10_V);
+    } else {
+      _intake->setRaw(0_V);
+    }
+
+    // _intake->setRaw(_codriver.GetLeftBumper() * 10_V);
+    // _intake->setRaw(_codriver.GetRightBumper() * -10_V);
+    std::cout << "Raw" << std::endl;
+  }
+  // } else {
+  //   if (_codriver.GetYButtonPressed()) {
+  //     _intake->setState(IntakeState::kIntake);
+  //   }
+  //   if (_codriver.GetAButtonPressed()) {
+  //     _intake->setState(IntakeState::kPass);
+  //   }
+  // }
+}
+
+// IntakeAutoControl::IntakeAutoControl(Intake* intake) : _intake(intake) {}
+
+// void IntakeAutoControl::OnTick(units::second_t dt) {
+//   if (_intake->GetConfig().intakeSensor->Get() == 1) {
+//     _intake->setState(IntakeState::kPass);
+//   } else if (_intake->GetConfig().magSensor->Get() == 0) {
+//     _intake->setState(IntakeState::kIdle);
+//   }
+// }
diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp
index 0dc53d5f..9cef56a6 100644
--- a/src/main/cpp/Robot.cpp
+++ b/src/main/cpp/Robot.cpp
@@ -4,12 +4,18 @@
 
 #include "Robot.h"
 
-// include units
-#include <units/velocity.h>
+#include <frc/TimedRobot.h>
+#include <frc/Timer.h>
+#include <frc/controller/RamseteController.h>
+#include <frc/kinematics/DifferentialDriveKinematics.h>
+#include <networktables/DoubleTopic.h>
+#include <networktables/NetworkTable.h>
+#include <networktables/NetworkTableInstance.h>
 #include <units/acceleration.h>
-#include <units/length.h>
 #include <units/angle.h>
+#include <units/length.h>
 #include <units/time.h>
+#include <units/velocity.h>
 #include <units/voltage.h>
 
 #include <frc/kinematics/DifferentialDriveKinematics.h>
@@ -72,6 +78,12 @@ void Robot::RobotInit() {
   // frontRight = new ctre::phoenix6::hardware::TalonFX(1, "Drivebase");   // front right
   // backLeft = new ctre::phoenix6::hardware::TalonFX(5, "Drivebase");   // back left
   // backRight = new ctre::phoenix6::hardware::TalonFX(3, "Drivebase");
+  lastPeriodic = wom::now();
+
+  intake = new Intake(robotmap.intakeSystem.config);
+  wom::BehaviourScheduler::GetInstance()->Register(intake);
+  intake->SetDefaultBehaviour(
+      [this]() { return wom::make<IntakeManualControl>(intake, robotmap.controllers.codriver); });
 }
 
 void Robot::RobotPeriodic() {
@@ -88,7 +100,7 @@ void Robot::RobotPeriodic() {
   robotmap.swerveTable.swerveDriveTable->GetEntry("backLeftEncoder").SetDouble(robotmap.swerveBase.moduleConfigs[2].turnMotor.encoder->GetEncoderPosition().value());
   robotmap.swerveTable.swerveDriveTable->GetEntry("backRightEncoder").SetDouble(robotmap.swerveBase.moduleConfigs[3].turnMotor.encoder->GetEncoderPosition().value());
 
-  _swerveDrive->OnUpdate(dt);
+  intake->OnUpdate(dt);
 }
 
 void Robot::AutonomousInit() {
@@ -102,10 +114,17 @@ void Robot::TeleopInit() {
   loop.Clear();
   wom::BehaviourScheduler *sched = wom::BehaviourScheduler::GetInstance();
   sched->InterruptAll();
+
+  // frontLeft->SetVoltage(4_V);
+  // frontRight->SetVoltage(4_V);
+  // backLeft->SetVoltage(4_V);
+  // backRight->SetVoltage(4_V);
+  loop.Clear();
+  wom::BehaviourScheduler* scheduler = wom::BehaviourScheduler::GetInstance();
+  scheduler->InterruptAll();
 }
-void Robot::TeleopPeriodic() {
-  
-}
+
+void Robot::TeleopPeriodic() {}
 
 void Robot::DisabledInit() {}
 void Robot::DisabledPeriodic() {}
diff --git a/src/main/include/Intake.h b/src/main/include/Intake.h
new file mode 100644
index 00000000..a84b41b1
--- /dev/null
+++ b/src/main/include/Intake.h
@@ -0,0 +1,41 @@
+// Copyright (c) 2023-2024 CurtinFRC
+// Open Source Software, you can modify it according to the terms
+// of the MIT License at the root of this project
+
+#pragma once
+
+#include <frc/DigitalInput.h>
+
+#include <memory>
+#include <string>
+
+#include "Wombat.h"
+
+struct IntakeConfig {
+  wom::Gearbox IntakeMotor;
+  frc::DigitalInput* intakeSensor;
+  frc::DigitalInput* magSensor;
+  frc::DigitalInput* shooterSensor;
+};
+
+enum class IntakeState { kIdle, kRaw, kHold, kEject, kIntake, kPass };
+
+class Intake : public behaviour::HasBehaviour {
+ public:
+  explicit Intake(IntakeConfig config);
+
+  void OnUpdate(units::second_t dt);
+
+  void setState(IntakeState state);
+  void setRaw(units::volt_t voltage);
+  IntakeConfig GetConfig();
+
+ private:
+  IntakeConfig _config;
+  IntakeState _state = IntakeState::kIdle;
+
+  units::volt_t _rawVoltage = 0_V;
+  std::string _stringStateName = "error";
+  units::volt_t _setVoltage = 0_V;
+  std::shared_ptr<nt::NetworkTable> _table = nt::NetworkTableInstance::GetDefault().GetTable("Intake");
+};
diff --git a/src/main/include/IntakeBehaviour.h b/src/main/include/IntakeBehaviour.h
new file mode 100644
index 00000000..c84c6d32
--- /dev/null
+++ b/src/main/include/IntakeBehaviour.h
@@ -0,0 +1,34 @@
+// Copyright (c) 2023-2024 CurtinFRC
+// Open Source Software, you can modify it according to the terms
+// of the MIT License at the root of this project
+
+#pragma once
+
+#include <frc/XboxController.h>
+
+#include "Intake.h"
+#include "Wombat.h"
+
+class IntakeManualControl : public behaviour::Behaviour {
+ public:
+  explicit IntakeManualControl(Intake* intake, frc::XboxController& codriver);
+
+  void OnTick(units::second_t dt) override;
+
+ private:
+  Intake* _intake;
+  frc::XboxController& _codriver;
+
+  units::volt_t _rawVoltage;
+  bool _rawControl;
+};
+
+class IntakeAutoControl : public behaviour::Behaviour {
+ public:
+  explicit IntakeAutoControl(Intake* intake);
+
+  void OnTick(units::second_t dt) override;
+
+ private:
+  Intake* _intake;
+};
diff --git a/src/main/include/Robot.h b/src/main/include/Robot.h
index 1806c61c..c5a08964 100644
--- a/src/main/include/Robot.h
+++ b/src/main/include/Robot.h
@@ -12,6 +12,9 @@
 #include <frc/smartdashboard/SendableChooser.h>
 #include <frc/smartdashboard/SmartDashboard.h>
 #include <string>
+
+#include "Intake.h"
+#include "IntakeBehaviour.h"
 #include "RobotMap.h"
 #include "Shooter.h"
 #include "ShooterBehaviour.h"
@@ -35,6 +38,7 @@ class Robot : public frc::TimedRobot {
   frc::EventLoop loop;
   Shooter *shooter;
 
+  Intake* intake;
   frc::SendableChooser<std::string> m_chooser;
 
   frc::Field2d m_field;
diff --git a/src/main/include/RobotMap.h b/src/main/include/RobotMap.h
index 3ae4db10..52121f1a 100644
--- a/src/main/include/RobotMap.h
+++ b/src/main/include/RobotMap.h
@@ -16,6 +16,8 @@
 #include <ctre/phoenix6/Pigeon2.hpp>
 #include <ctre/phoenix6/TalonFX.hpp>
 
+#include "Intake.h"
+#include "IntakeBehaviour.h"
 #include "Shooter.h"
 #include "Wombat.h"
 
@@ -23,10 +25,22 @@ struct RobotMap {
   struct Controllers {
     frc::XboxController driver = frc::XboxController(0);
     frc::XboxController codriver = frc::XboxController(1);
-    frc::XboxController testController = frc::XboxController(2);
   };
   Controllers controllers;
 
+  struct IntakeSystem {
+    rev::CANSparkMax intakeMotor{2, rev::CANSparkMax::MotorType::kBrushed};
+    // wom::CANSparkMaxEncoder intakeEncoder{&intakeMotor, 0.1_m};
+    // frc::DigitalInput intakeSensor{0};
+    // frc::DigitalInput magSensor{0};
+    // frc::DigitalInput shooterSensor{0};
+
+    wom::Gearbox IntakeGearbox{&intakeMotor, nullptr, frc::DCMotor::CIM(1)};
+
+    IntakeConfig config{IntakeGearbox /*, &intakeSensor, &magSensor, &shooterSensor*/};
+  }; IntakeSystem intakeSystem;
+
+
   struct Shooter {
     rev::CANSparkMax shooterMotor{11, rev::CANSparkMax::MotorType::kBrushless};
     // frc::DigitalInput shooterSensor{2};
@@ -40,8 +54,7 @@ struct RobotMap {
         shooterGearbox,
         // &shooterSensor,
     };
-  };
-  Shooter shooterSystem;
+  }; Shooter shooterSystem;
   
   struct SwerveBase {
     ctre::phoenix6::hardware::CANcoder frontLeftCancoder{18, "Drivebase"};

From 14a052dd47afe27143b241783607097f214337dd Mon Sep 17 00:00:00 2001
From: JoystickMaster-race
 <97269195+JoystickMaster-race@users.noreply.github.com>
Date: Wed, 24 Jan 2024 09:45:57 +0800
Subject: [PATCH 066/207] Arm (#103)

* Alpha Arm vers 1

* Alpha Arm ver 1 w/ explicit

* Alpha arm v1 formatting

* Alpha arm w/ wrist v1

* formatted

* Alpha arm v1 with ports

* Working alpha arm no encoders

---------

Co-authored-by: Isaac Turner <spacey_sooty@outlook.com>
---
 src/main/cpp/AlphaArm.cpp            | 67 ++++++++++++++++++++++++++++
 src/main/cpp/AlphaArmBehaviour.cpp   | 35 +++++++++++++++
 src/main/cpp/Robot.cpp               | 28 ++++++++----
 src/main/include/AlphaArm.h          | 48 ++++++++++++++++++++
 src/main/include/AlphaArmBehaviour.h | 24 ++++++++++
 src/main/include/Robot.h             |  4 ++
 src/main/include/RobotMap.h          | 14 ++++++
 7 files changed, 212 insertions(+), 8 deletions(-)
 create mode 100644 src/main/cpp/AlphaArm.cpp
 create mode 100644 src/main/cpp/AlphaArmBehaviour.cpp
 create mode 100644 src/main/include/AlphaArm.h
 create mode 100644 src/main/include/AlphaArmBehaviour.h

diff --git a/src/main/cpp/AlphaArm.cpp b/src/main/cpp/AlphaArm.cpp
new file mode 100644
index 00000000..76c48dd9
--- /dev/null
+++ b/src/main/cpp/AlphaArm.cpp
@@ -0,0 +1,67 @@
+// Copyright (c) 2023-2024 CurtinFRC
+// Open Source Software, you can modify it according to the terms
+// of the MIT License at the root of this project
+
+#include "AlphaArm.h"
+
+AlphaArm::AlphaArm(AlphaArmConfig config) : _config(config) {}
+
+AlphaArmConfig AlphaArm::GetConfig(){
+    return _config;
+}
+
+void AlphaArm::OnUpdate(units::second_t dt){
+        switch(_state){
+            case AlphaArmState::kIdle:
+        
+            //_config.alphaArmGearbox.motorController->SetVoltage(0_V);
+            //_config.wristGearbox.motorController->SetVoltage(0_V);
+            _setAlphaArmVoltage = 0_V;
+            _setWristVoltage = 0_V;
+
+            break;
+            case AlphaArmState::kRaw:
+            _setAlphaArmVoltage = _rawArmVoltage;
+            _setWristVoltage = _rawWristVoltage;
+            _config.alphaArmGearbox.motorController->SetVoltage(_rawArmVoltage);
+            _config.wristGearbox.motorController->SetVoltage(_rawWristVoltage);
+            
+            break;
+            case AlphaArmState::kForwardWrist:
+            _config.wristGearbox.motorController->SetVoltage(3_V);
+            _setWristVoltage = 3_V;
+
+            case AlphaArmState::kReverseWrist:
+            _config.wristGearbox.motorController->SetVoltage(-3_V);
+            _setWristVoltage = -3_V;
+            default:
+            std::cout << "oops, wrong state" << std::endl;
+            break;
+            
+        }
+        //transmission translate
+       // _config.armGearBox.motorController->SetVoltage(setAlphaArmVoltage);
+       //_config.alphaArmGearbox.motorController->SetVoltage(setAlphaArmVoltage);
+       //_config.wristGearbox.motorController->SetVoltage(setWristVoltage);
+        _config.alphaArmGearbox.motorController->SetVoltage(_setAlphaArmVoltage);
+        _config.wristGearbox.motorController->SetVoltage(_setWristVoltage);
+        
+        //_config.wristGearbox.motorController->SetVoltage(_setVoltage);
+}
+
+void AlphaArm::SetState(AlphaArmState state) {
+  _state = state;
+}
+
+// void AlphaArm::SetRaw(units::volt_t voltage){
+//     _rawArmVoltage = voltage;
+//     _rawWristVoltage = voltage;
+// }
+
+void AlphaArm::SetArmRaw(units::volt_t voltage){
+    _rawArmVoltage = voltage;
+}
+
+void AlphaArm::setWristRaw(units::volt_t voltage){
+    _rawWristVoltage = voltage;
+}
diff --git a/src/main/cpp/AlphaArmBehaviour.cpp b/src/main/cpp/AlphaArmBehaviour.cpp
new file mode 100644
index 00000000..67f9e059
--- /dev/null
+++ b/src/main/cpp/AlphaArmBehaviour.cpp
@@ -0,0 +1,35 @@
+// Copyright (c) 2023-2024 CurtinFRC
+// Open Source Software, you can modify it according to the terms
+// of the MIT License at the root of this project
+
+#include "AlphaArmBehaviour.h"
+
+#include <frc/XboxController.h>
+
+AlphaArmManualControl::AlphaArmManualControl(AlphaArm* alphaArm, frc::XboxController* codriver)
+    : _alphaArm(alphaArm), _codriver(codriver) {
+  Controls(alphaArm);
+}
+
+void AlphaArmManualControl::OnTick(units::second_t dt) {
+  if (_codriver->GetXButtonPressed()) {
+    if (_rawControl == true) {
+      _rawControl = false;
+    } else {
+      _rawControl = true;
+    }
+  }
+
+  if (_rawControl) {
+    _alphaArm->SetState(AlphaArmState::kRaw);
+    _alphaArm->SetArmRaw(_codriver->GetRightY() * 6_V);
+    _alphaArm->setWristRaw(_codriver->GetLeftY() * -6_V);
+  } else {
+    if (_codriver->GetRightBumperPressed()){
+        _alphaArm->SetState(AlphaArmState::kForwardWrist);
+    }
+    if (_codriver->GetLeftBumperPressed()){
+        _alphaArm->SetState(AlphaArmState::kReverseWrist);
+    }
+  }
+}
diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp
index 9cef56a6..d1b997ca 100644
--- a/src/main/cpp/Robot.cpp
+++ b/src/main/cpp/Robot.cpp
@@ -35,21 +35,25 @@ void Robot::RobotInit() {
   sched = wom::BehaviourScheduler::GetInstance();
   m_chooser.SetDefaultOption("Default Auto", "Default Auto");
 
-  frc::SmartDashboard::PutData("Auto Selector", &m_chooser);
+  // frc::SmartDashboard::PutData("Auto Selector", &m_chooser);
 
-  m_path_chooser.SetDefaultOption("Path1", "paths/output/Path1.wpilib.json");
+  // m_path_chooser.SetDefaultOption("Path1", "paths/output/Path1.wpilib.json");
 
-  m_path_chooser.AddOption("Path1", "paths/output/Path1.wpilib.json");
-  m_path_chooser.AddOption("Path2", "paths/output/Path2.wpilib.json");
+  // m_path_chooser.AddOption("Path1", "paths/output/Path1.wpilib.json");
+  // m_path_chooser.AddOption("Path2", "paths/output/Path2.wpilib.json");
 
-  frc::SmartDashboard::PutData("Path Selector", &m_path_chooser);
+  // frc::SmartDashboard::PutData("Path Selector", &m_path_chooser);
 
-  frc::SmartDashboard::PutData("Field", &m_field);
+  // frc::SmartDashboard::PutData("Field", &m_field);
 
-  simulation_timer = frc::Timer();
+  // simulation_timer = frc::Timer();
 
-  robotmap.swerveBase.gyro->Reset();
+  // robotmap.swerveBase.gyro->Reset();
 
+  // _swerveDrive = new wom::SwerveDrive(robotmap.swerveBase.config, frc::Pose2d());
+  // wom::BehaviourScheduler::GetInstance()->Register(_swerveDrive);
+  // _swerveDrive->SetDefaultBehaviour(
+  //     [this]() { return wom::make<wom::ManualDrivebase>(_swerveDrive, &robotmap.controllers.driver); });
   _swerveDrive =
       new wom::SwerveDrive(robotmap.swerveBase.config, frc::Pose2d());
   wom::BehaviourScheduler::GetInstance()->Register(_swerveDrive);
@@ -62,6 +66,12 @@ void Robot::RobotInit() {
   // m_driveSim = new wom::TempSimSwerveDrive(&simulation_timer, &m_field);
   // m_driveSim = wom::TempSimSwerveDrive();
 
+  alphaArm = 
+      new AlphaArm(robotmap.alphaArmSystem.config);
+  wom::BehaviourScheduler::GetInstance()->Register(alphaArm);
+  alphaArm->SetDefaultBehaviour(
+      [this]() { return wom::make<AlphaArmManualControl>(alphaArm, &robotmap.controllers.codriver); });
+      
   robotmap.swerveBase.moduleConfigs[0].turnMotor.encoder->SetEncoderOffset(0_rad);
   robotmap.swerveBase.moduleConfigs[1].turnMotor.encoder->SetEncoderOffset(0_rad);
   robotmap.swerveBase.moduleConfigs[2].turnMotor.encoder->SetEncoderOffset(0_rad);
@@ -100,6 +110,8 @@ void Robot::RobotPeriodic() {
   robotmap.swerveTable.swerveDriveTable->GetEntry("backLeftEncoder").SetDouble(robotmap.swerveBase.moduleConfigs[2].turnMotor.encoder->GetEncoderPosition().value());
   robotmap.swerveTable.swerveDriveTable->GetEntry("backRightEncoder").SetDouble(robotmap.swerveBase.moduleConfigs[3].turnMotor.encoder->GetEncoderPosition().value());
 
+  _swerveDrive->OnUpdate(dt);
+  alphaArm->OnUpdate(dt);
   intake->OnUpdate(dt);
 }
 
diff --git a/src/main/include/AlphaArm.h b/src/main/include/AlphaArm.h
new file mode 100644
index 00000000..bc9c8d7c
--- /dev/null
+++ b/src/main/include/AlphaArm.h
@@ -0,0 +1,48 @@
+// Copyright (c) 2023-2024 CurtinFRC
+// Open Source Software, you can modify it according to the terms
+// of the MIT License at the root of this project
+
+#pragma once
+#include <frc/DigitalInput.h>
+
+#include "Wombat.h"
+
+struct AlphaArmConfig {
+    wom::Gearbox alphaArmGearbox;
+    wom::Gearbox wristGearbox;
+
+};
+
+enum class AlphaArmState {
+    kIdle,
+    kIntakeAngle,
+    kAmpAngle,
+    kSpeakerAngle,
+    kForwardWrist,
+    kReverseWrist,
+    kRaw
+};
+
+class AlphaArm : public::behaviour::HasBehaviour{
+    public:
+     explicit AlphaArm(AlphaArmConfig config);
+
+    void OnUpdate(units::second_t dt);
+    void SetArmRaw(units::volt_t voltage);
+    void setWristRaw(units::volt_t voltage);
+    void SetState(AlphaArmState state);
+    AlphaArmConfig GetConfig();
+    //void SetRaw(units::volt_t voltage);
+
+    private:
+    AlphaArmConfig _config;
+    AlphaArmState _state = AlphaArmState::kIdle;
+    units::volt_t _setAlphaArmVoltage = 0_V;
+    units::volt_t _setWristVoltage = 0_V;
+
+    units::volt_t _rawArmVoltage = 0_V;
+    units::volt_t _rawWristVoltage = 0_V;
+
+    
+   
+};
diff --git a/src/main/include/AlphaArmBehaviour.h b/src/main/include/AlphaArmBehaviour.h
new file mode 100644
index 00000000..4c2d03fb
--- /dev/null
+++ b/src/main/include/AlphaArmBehaviour.h
@@ -0,0 +1,24 @@
+// Copyright (c) 2023-2024 CurtinFRC
+// Open Source Software, you can modify it according to the terms
+// of the MIT License at the root of this project
+
+#pragma once
+
+#pragma once
+#include <frc/XboxController.h>
+
+#include "AlphaArm.h"
+#include "Wombat.h"
+
+class AlphaArmManualControl : public behaviour::Behaviour {
+ public:
+  explicit AlphaArmManualControl(AlphaArm* alphaArm, frc::XboxController* codriver);
+  void OnTick(units::second_t dt);
+
+ private:
+  AlphaArm* _alphaArm;
+  frc::XboxController* _codriver;
+  // units::volt_t _rightStick = ((_codriver->GetRightY()>0.05 || _codriver->GetRightY() < -0.05
+  // )?_codriver->GetRightY():0) * 2_V;
+  bool _rawControl;
+};
diff --git a/src/main/include/Robot.h b/src/main/include/Robot.h
index c5a08964..5fad6cde 100644
--- a/src/main/include/Robot.h
+++ b/src/main/include/Robot.h
@@ -13,6 +13,8 @@
 #include <frc/smartdashboard/SmartDashboard.h>
 #include <string>
 
+#include "AlphaArm.h"
+#include "AlphaArmBehaviour.h"
 #include "Intake.h"
 #include "IntakeBehaviour.h"
 #include "RobotMap.h"
@@ -49,6 +51,8 @@ class Robot : public frc::TimedRobot {
 
   wom::SwerveDrive* _swerveDrive;
 
+  AlphaArm *alphaArm;
+
   // ctre::phoenix6::hardware::TalonFX *frontLeft;
   // ctre::phoenix6::hardware::TalonFX *frontRight;
   // ctre::phoenix6::hardware::TalonFX *backLeft;
diff --git a/src/main/include/RobotMap.h b/src/main/include/RobotMap.h
index 52121f1a..fb6d2804 100644
--- a/src/main/include/RobotMap.h
+++ b/src/main/include/RobotMap.h
@@ -16,6 +16,8 @@
 #include <ctre/phoenix6/Pigeon2.hpp>
 #include <ctre/phoenix6/TalonFX.hpp>
 
+#include "AlphaArm.h"
+#include "AlphaArmBehaviour.h"
 #include "Intake.h"
 #include "IntakeBehaviour.h"
 #include "Shooter.h"
@@ -25,9 +27,21 @@ struct RobotMap {
   struct Controllers {
     frc::XboxController driver = frc::XboxController(0);
     frc::XboxController codriver = frc::XboxController(1);
+    frc::XboxController testController = frc::XboxController(2);
   };
   Controllers controllers;
 
+  struct AlphaArmSystem {
+    rev::CANSparkMax alphaArmMotor{12, rev::CANSparkMax::MotorType::kBrushless};
+    rev::CANSparkMax wristMotor{15, rev::CANSparkMax::MotorType::kBrushless};
+
+    wom::Gearbox alphaArmGearbox{&alphaArmMotor, nullptr, frc::DCMotor::NEO(1)};
+    wom::Gearbox wristGearbox{&wristMotor, nullptr, frc::DCMotor::NEO(1)};
+
+    AlphaArmConfig config{alphaArmGearbox, wristGearbox};
+  };
+  AlphaArmSystem alphaArmSystem;
+  
   struct IntakeSystem {
     rev::CANSparkMax intakeMotor{2, rev::CANSparkMax::MotorType::kBrushed};
     // wom::CANSparkMaxEncoder intakeEncoder{&intakeMotor, 0.1_m};

From 862393a5af86cd6c2d52015a78fb4392eb986153 Mon Sep 17 00:00:00 2001
From: Isaac Turner <spacey_sooty@outlook.com>
Date: Wed, 24 Jan 2024 14:33:05 +0800
Subject: [PATCH 067/207] fix some formatting

---
 .clang-format                                 |   2 +-
 .editorconfig                                 |   5 -
 .styleguide                                   |   1 -
 build.gradle                                  |   2 +-
 init.ps1                                      |   2 -
 init.sh                                       |   5 -
 src/main/cpp/AlphaArm.cpp                     |  81 +++++-----
 src/main/cpp/AlphaArmBehaviour.cpp            |   8 +-
 src/main/cpp/Robot.cpp                        |  45 +++---
 src/main/cpp/Shooter.cpp                      |   8 +-
 src/main/cpp/ShooterBehaviour.cpp             |   9 +-
 src/main/include/AlphaArm.h                   |  60 ++++----
 src/main/include/Robot.h                      |   9 +-
 src/main/include/RobotMap.h                   |  80 +++++-----
 wombat/src/main/cpp/behaviour/Behaviour.cpp   |  23 +--
 .../main/cpp/behaviour/BehaviourScheduler.cpp |   4 +-
 .../src/main/cpp/behaviour/HasBehaviour.cpp   |   3 +-
 wombat/src/main/cpp/drivetrain/Drivetrain.cpp |   3 +-
 .../src/main/cpp/drivetrain/SwerveDrive.cpp   | 139 +++++++-----------
 .../behaviours/SwerveBehaviours.cpp           |  96 ++++++------
 wombat/src/main/cpp/subsystems/Arm.cpp        |  35 ++---
 wombat/src/main/cpp/subsystems/Elevator.cpp   |  30 ++--
 wombat/src/main/cpp/subsystems/Shooter.cpp    |  22 +--
 wombat/src/main/cpp/utils/Encoder.cpp         |  55 +++----
 wombat/src/main/cpp/utils/Util.cpp            |  27 +---
 wombat/src/main/cpp/vision/Limelight.cpp      |  24 ++-
 wombat/src/main/include/behaviour/Behaviour.h |  42 ++----
 .../src/main/include/behaviour/HasBehaviour.h |   3 +-
 .../src/main/include/drivetrain/SwerveDrive.h |  28 ++--
 .../drivetrain/behaviours/SwerveBehaviours.h  |  17 +--
 wombat/src/main/include/subsystems/Arm.h      |   3 +-
 wombat/src/main/include/subsystems/Shooter.h  |   3 +-
 wombat/src/main/include/utils/Encoder.h       |  27 ++--
 wombat/src/main/include/utils/PID.h           |  69 +++------
 wombat/src/main/include/utils/Util.h          |  55 +++----
 wombat/src/main/include/vision/Limelight.h    |  22 +--
 36 files changed, 413 insertions(+), 634 deletions(-)
 delete mode 100644 .editorconfig
 delete mode 100644 init.ps1
 delete mode 100644 init.sh

diff --git a/.clang-format b/.clang-format
index f69bef0c..a879dec4 100644
--- a/.clang-format
+++ b/.clang-format
@@ -81,7 +81,7 @@ BreakBeforeTernaryOperators: true
 BreakConstructorInitializers: BeforeColon
 BreakInheritanceList: BeforeColon
 BreakStringLiterals: true
-ColumnLimit:     80
+ColumnLimit:     110
 CommentPragmas:  '^ IWYU pragma:'
 CompactNamespaces: false
 ConstructorInitializerIndentWidth: 4
diff --git a/.editorconfig b/.editorconfig
deleted file mode 100644
index 0020fc03..00000000
--- a/.editorconfig
+++ /dev/null
@@ -1,5 +0,0 @@
-root = true
-
-[*]
-indent_style = space
-indent_size = 2
diff --git a/.styleguide b/.styleguide
index be156d3c..82efdeab 100644
--- a/.styleguide
+++ b/.styleguide
@@ -1,4 +1,3 @@
-
 cppHeaderFileInclude {
    \.h$
    \.hpp$
diff --git a/build.gradle b/build.gradle
index 91b6acb4..84ccc0cb 100644
--- a/build.gradle
+++ b/build.gradle
@@ -1,7 +1,7 @@
 plugins {
     id "cpp"
     id "google-test-test-suite"
-    id "edu.wpi.first.GradleRIO" version "2024.1.1"
+    id "edu.wpi.first.GradleRIO" version "2024.2.1"
 }
 
 // Define my targets (RoboRIO) and artifacts (deployable files)
diff --git a/init.ps1 b/init.ps1
deleted file mode 100644
index fa83b9a6..00000000
--- a/init.ps1
+++ /dev/null
@@ -1,2 +0,0 @@
-./gradlew installRoborioToolchain
-./gradlew build
diff --git a/init.sh b/init.sh
deleted file mode 100644
index 6a932ed6..00000000
--- a/init.sh
+++ /dev/null
@@ -1,5 +0,0 @@
-#!/usr/bin/sh
-sudo apt update
-chmod +x gradlew
-./gradlew installRoborioToolchain
-./gradlew build
diff --git a/src/main/cpp/AlphaArm.cpp b/src/main/cpp/AlphaArm.cpp
index 76c48dd9..848ee616 100644
--- a/src/main/cpp/AlphaArm.cpp
+++ b/src/main/cpp/AlphaArm.cpp
@@ -6,47 +6,46 @@
 
 AlphaArm::AlphaArm(AlphaArmConfig config) : _config(config) {}
 
-AlphaArmConfig AlphaArm::GetConfig(){
-    return _config;
+AlphaArmConfig AlphaArm::GetConfig() {
+  return _config;
 }
 
-void AlphaArm::OnUpdate(units::second_t dt){
-        switch(_state){
-            case AlphaArmState::kIdle:
-        
-            //_config.alphaArmGearbox.motorController->SetVoltage(0_V);
-            //_config.wristGearbox.motorController->SetVoltage(0_V);
-            _setAlphaArmVoltage = 0_V;
-            _setWristVoltage = 0_V;
+void AlphaArm::OnUpdate(units::second_t dt) {
+  switch (_state) {
+    case AlphaArmState::kIdle:
 
-            break;
-            case AlphaArmState::kRaw:
-            _setAlphaArmVoltage = _rawArmVoltage;
-            _setWristVoltage = _rawWristVoltage;
-            _config.alphaArmGearbox.motorController->SetVoltage(_rawArmVoltage);
-            _config.wristGearbox.motorController->SetVoltage(_rawWristVoltage);
-            
-            break;
-            case AlphaArmState::kForwardWrist:
-            _config.wristGearbox.motorController->SetVoltage(3_V);
-            _setWristVoltage = 3_V;
+      // _config.alphaArmGearbox.motorController->SetVoltage(0_V);
+      // _config.wristGearbox.motorController->SetVoltage(0_V);
+      _setAlphaArmVoltage = 0_V;
+      _setWristVoltage = 0_V;
 
-            case AlphaArmState::kReverseWrist:
-            _config.wristGearbox.motorController->SetVoltage(-3_V);
-            _setWristVoltage = -3_V;
-            default:
-            std::cout << "oops, wrong state" << std::endl;
-            break;
-            
-        }
-        //transmission translate
-       // _config.armGearBox.motorController->SetVoltage(setAlphaArmVoltage);
-       //_config.alphaArmGearbox.motorController->SetVoltage(setAlphaArmVoltage);
-       //_config.wristGearbox.motorController->SetVoltage(setWristVoltage);
-        _config.alphaArmGearbox.motorController->SetVoltage(_setAlphaArmVoltage);
-        _config.wristGearbox.motorController->SetVoltage(_setWristVoltage);
-        
-        //_config.wristGearbox.motorController->SetVoltage(_setVoltage);
+      break;
+    case AlphaArmState::kRaw:
+      _setAlphaArmVoltage = _rawArmVoltage;
+      _setWristVoltage = _rawWristVoltage;
+      _config.alphaArmGearbox.motorController->SetVoltage(_rawArmVoltage);
+      _config.wristGearbox.motorController->SetVoltage(_rawWristVoltage);
+
+      break;
+    case AlphaArmState::kForwardWrist:
+      _config.wristGearbox.motorController->SetVoltage(3_V);
+      _setWristVoltage = 3_V;
+
+    case AlphaArmState::kReverseWrist:
+      _config.wristGearbox.motorController->SetVoltage(-3_V);
+      _setWristVoltage = -3_V;
+    default:
+      std::cout << "oops, wrong state" << std::endl;
+      break;
+  }
+  // transmission translate
+  // _config.armGearBox.motorController->SetVoltage(setAlphaArmVoltage);
+  // _config.alphaArmGearbox.motorController->SetVoltage(setAlphaArmVoltage);
+  // _config.wristGearbox.motorController->SetVoltage(setWristVoltage);
+  _config.alphaArmGearbox.motorController->SetVoltage(_setAlphaArmVoltage);
+  _config.wristGearbox.motorController->SetVoltage(_setWristVoltage);
+
+  // _config.wristGearbox.motorController->SetVoltage(_setVoltage);
 }
 
 void AlphaArm::SetState(AlphaArmState state) {
@@ -58,10 +57,10 @@ void AlphaArm::SetState(AlphaArmState state) {
 //     _rawWristVoltage = voltage;
 // }
 
-void AlphaArm::SetArmRaw(units::volt_t voltage){
-    _rawArmVoltage = voltage;
+void AlphaArm::SetArmRaw(units::volt_t voltage) {
+  _rawArmVoltage = voltage;
 }
 
-void AlphaArm::setWristRaw(units::volt_t voltage){
-    _rawWristVoltage = voltage;
+void AlphaArm::setWristRaw(units::volt_t voltage) {
+  _rawWristVoltage = voltage;
 }
diff --git a/src/main/cpp/AlphaArmBehaviour.cpp b/src/main/cpp/AlphaArmBehaviour.cpp
index 67f9e059..ece50ebb 100644
--- a/src/main/cpp/AlphaArmBehaviour.cpp
+++ b/src/main/cpp/AlphaArmBehaviour.cpp
@@ -25,11 +25,11 @@ void AlphaArmManualControl::OnTick(units::second_t dt) {
     _alphaArm->SetArmRaw(_codriver->GetRightY() * 6_V);
     _alphaArm->setWristRaw(_codriver->GetLeftY() * -6_V);
   } else {
-    if (_codriver->GetRightBumperPressed()){
-        _alphaArm->SetState(AlphaArmState::kForwardWrist);
+    if (_codriver->GetRightBumperPressed()) {
+      _alphaArm->SetState(AlphaArmState::kForwardWrist);
     }
-    if (_codriver->GetLeftBumperPressed()){
-        _alphaArm->SetState(AlphaArmState::kReverseWrist);
+    if (_codriver->GetLeftBumperPressed()) {
+      _alphaArm->SetState(AlphaArmState::kReverseWrist);
     }
   }
 }
diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp
index d1b997ca..888245e6 100644
--- a/src/main/cpp/Robot.cpp
+++ b/src/main/cpp/Robot.cpp
@@ -18,20 +18,16 @@
 #include <units/velocity.h>
 #include <units/voltage.h>
 
-#include <frc/kinematics/DifferentialDriveKinematics.h>
-#include <frc/controller/RamseteController.h>
-#include <frc/Timer.h>
 #include "behaviour/HasBehaviour.h"
 
 static units::second_t lastPeriodic;
 
 void Robot::RobotInit() {
-
   shooter = new Shooter(robotmap.shooterSystem.config);
   wom::BehaviourScheduler::GetInstance()->Register(shooter);
   shooter->SetDefaultBehaviour(
-     [this]() {return wom::make<ShooterManualControl>(shooter, &robotmap.controllers.codriver); });
-  
+      [this]() { return wom::make<ShooterManualControl>(shooter, &robotmap.controllers.codriver); });
+
   sched = wom::BehaviourScheduler::GetInstance();
   m_chooser.SetDefaultOption("Default Auto", "Default Auto");
 
@@ -54,32 +50,24 @@ void Robot::RobotInit() {
   // wom::BehaviourScheduler::GetInstance()->Register(_swerveDrive);
   // _swerveDrive->SetDefaultBehaviour(
   //     [this]() { return wom::make<wom::ManualDrivebase>(_swerveDrive, &robotmap.controllers.driver); });
-  _swerveDrive =
-      new wom::SwerveDrive(robotmap.swerveBase.config, frc::Pose2d());
+  _swerveDrive = new wom::SwerveDrive(robotmap.swerveBase.config, frc::Pose2d());
   wom::BehaviourScheduler::GetInstance()->Register(_swerveDrive);
-  _swerveDrive->SetDefaultBehaviour([this]() {
-    return wom::make<wom::ManualDrivebase>(_swerveDrive,
-                                           &robotmap.controllers.driver);
-  });
+  _swerveDrive->SetDefaultBehaviour(
+      [this]() { return wom::make<wom::ManualDrivebase>(_swerveDrive, &robotmap.controllers.driver); });
 
-  
   // m_driveSim = new wom::TempSimSwerveDrive(&simulation_timer, &m_field);
   // m_driveSim = wom::TempSimSwerveDrive();
 
-  alphaArm = 
-      new AlphaArm(robotmap.alphaArmSystem.config);
+  alphaArm = new AlphaArm(robotmap.alphaArmSystem.config);
   wom::BehaviourScheduler::GetInstance()->Register(alphaArm);
   alphaArm->SetDefaultBehaviour(
       [this]() { return wom::make<AlphaArmManualControl>(alphaArm, &robotmap.controllers.codriver); });
-      
+
   robotmap.swerveBase.moduleConfigs[0].turnMotor.encoder->SetEncoderOffset(0_rad);
   robotmap.swerveBase.moduleConfigs[1].turnMotor.encoder->SetEncoderOffset(0_rad);
   robotmap.swerveBase.moduleConfigs[2].turnMotor.encoder->SetEncoderOffset(0_rad);
   robotmap.swerveBase.moduleConfigs[3].turnMotor.encoder->SetEncoderOffset(0_rad);
 
-
-
-
   // frontLeft = new ctre::phoenix6::hardware::TalonFX(7, "Drivebase");  // front left
   // frontRight = new ctre::phoenix6::hardware::TalonFX(2, "Drivebase");   // front right
   // backLeft = new ctre::phoenix6::hardware::TalonFX(6, "Drivebase");   // back left
@@ -105,10 +93,14 @@ void Robot::RobotPeriodic() {
   shooter->OnUpdate(dt);
   sched->Tick();
 
-  robotmap.swerveTable.swerveDriveTable->GetEntry("frontLeftEncoder").SetDouble(robotmap.swerveBase.moduleConfigs[0].turnMotor.encoder->GetEncoderPosition().value());
-  robotmap.swerveTable.swerveDriveTable->GetEntry("frontRightEncoder").SetDouble(robotmap.swerveBase.moduleConfigs[1].turnMotor.encoder->GetEncoderPosition().value());
-  robotmap.swerveTable.swerveDriveTable->GetEntry("backLeftEncoder").SetDouble(robotmap.swerveBase.moduleConfigs[2].turnMotor.encoder->GetEncoderPosition().value());
-  robotmap.swerveTable.swerveDriveTable->GetEntry("backRightEncoder").SetDouble(robotmap.swerveBase.moduleConfigs[3].turnMotor.encoder->GetEncoderPosition().value());
+  robotmap.swerveTable.swerveDriveTable->GetEntry("frontLeftEncoder")
+      .SetDouble(robotmap.swerveBase.moduleConfigs[0].turnMotor.encoder->GetEncoderPosition().value());
+  robotmap.swerveTable.swerveDriveTable->GetEntry("frontRightEncoder")
+      .SetDouble(robotmap.swerveBase.moduleConfigs[1].turnMotor.encoder->GetEncoderPosition().value());
+  robotmap.swerveTable.swerveDriveTable->GetEntry("backLeftEncoder")
+      .SetDouble(robotmap.swerveBase.moduleConfigs[2].turnMotor.encoder->GetEncoderPosition().value());
+  robotmap.swerveTable.swerveDriveTable->GetEntry("backRightEncoder")
+      .SetDouble(robotmap.swerveBase.moduleConfigs[3].turnMotor.encoder->GetEncoderPosition().value());
 
   _swerveDrive->OnUpdate(dt);
   alphaArm->OnUpdate(dt);
@@ -119,12 +111,11 @@ void Robot::AutonomousInit() {
   loop.Clear();
   sched->InterruptAll();
 }
-void Robot::AutonomousPeriodic() {
-}
+void Robot::AutonomousPeriodic() {}
 
 void Robot::TeleopInit() {
   loop.Clear();
-  wom::BehaviourScheduler *sched = wom::BehaviourScheduler::GetInstance();
+  wom::BehaviourScheduler* sched = wom::BehaviourScheduler::GetInstance();
   sched->InterruptAll();
 
   // frontLeft->SetVoltage(4_V);
@@ -142,4 +133,4 @@ void Robot::DisabledInit() {}
 void Robot::DisabledPeriodic() {}
 
 void Robot::TestInit() {}
-void Robot::TestPeriodic() {}
\ No newline at end of file
+void Robot::TestPeriodic() {}
diff --git a/src/main/cpp/Shooter.cpp b/src/main/cpp/Shooter.cpp
index 613120df..6499e076 100644
--- a/src/main/cpp/Shooter.cpp
+++ b/src/main/cpp/Shooter.cpp
@@ -4,10 +4,11 @@
 
 #include "Shooter.h"
 
-Shooter::Shooter(ShooterConfig config) : _config(config)
-// , 
+Shooter::Shooter(ShooterConfig config)
+    : _config(config)
+// ,
 // _pid{frc::PIDController (1, 0, 0, 0.005_s)}
-{} //config.path + "/pid", config.pidConfig
+{}  // config.path + "/pid", config.pidConfig
 void Shooter::OnUpdate(units::second_t dt) {
   // _pid.SetTolerance(0.1, 1);
   switch (_state) {
@@ -64,7 +65,6 @@ void Shooter::OnUpdate(units::second_t dt) {
   }
   std::cout << "Voltage:" << _setVoltage.value() << std::endl;
   _config.ShooterGearbox.motorController->SetVoltage(_setVoltage);
-
 }
 
 void Shooter::SetState(ShooterState state) {
diff --git a/src/main/cpp/ShooterBehaviour.cpp b/src/main/cpp/ShooterBehaviour.cpp
index 1a035ae0..341eef15 100644
--- a/src/main/cpp/ShooterBehaviour.cpp
+++ b/src/main/cpp/ShooterBehaviour.cpp
@@ -12,7 +12,6 @@ ShooterManualControl::ShooterManualControl(Shooter* shooter, frc::XboxController
 void ShooterManualControl::OnTick(units::second_t dt) {
   _shooter->table->GetEntry("RawControl").SetBoolean(_rawControl);
 
-
   if (_codriver->GetAButtonReleased()) {
     if (_rawControl) {
       _rawControl = false;
@@ -22,10 +21,10 @@ void ShooterManualControl::OnTick(units::second_t dt) {
   }
 
   if (!_rawControl) {
-      if (_codriver->GetYButton()) {
-        _shooter->SetState(ShooterState::kSpinUp);
-        _shooter->SetPidGoal(10_rad_per_s);
-      }
+    if (_codriver->GetYButton()) {
+      _shooter->SetState(ShooterState::kSpinUp);
+      _shooter->SetPidGoal(10_rad_per_s);
+    }
   } else {
     if (_codriver->GetRightTriggerAxis() > 0.1) {
       _shooter->SetRaw(_codriver->GetRightTriggerAxis() * 12_V);
diff --git a/src/main/include/AlphaArm.h b/src/main/include/AlphaArm.h
index bc9c8d7c..5ac2d7c0 100644
--- a/src/main/include/AlphaArm.h
+++ b/src/main/include/AlphaArm.h
@@ -8,41 +8,37 @@
 #include "Wombat.h"
 
 struct AlphaArmConfig {
-    wom::Gearbox alphaArmGearbox;
-    wom::Gearbox wristGearbox;
-
+  wom::Gearbox alphaArmGearbox;
+  wom::Gearbox wristGearbox;
 };
 
 enum class AlphaArmState {
-    kIdle,
-    kIntakeAngle,
-    kAmpAngle,
-    kSpeakerAngle,
-    kForwardWrist,
-    kReverseWrist,
-    kRaw
+  kIdle,
+  kIntakeAngle,
+  kAmpAngle,
+  kSpeakerAngle,
+  kForwardWrist,
+  kReverseWrist,
+  kRaw
 };
 
-class AlphaArm : public::behaviour::HasBehaviour{
-    public:
-     explicit AlphaArm(AlphaArmConfig config);
-
-    void OnUpdate(units::second_t dt);
-    void SetArmRaw(units::volt_t voltage);
-    void setWristRaw(units::volt_t voltage);
-    void SetState(AlphaArmState state);
-    AlphaArmConfig GetConfig();
-    //void SetRaw(units::volt_t voltage);
-
-    private:
-    AlphaArmConfig _config;
-    AlphaArmState _state = AlphaArmState::kIdle;
-    units::volt_t _setAlphaArmVoltage = 0_V;
-    units::volt_t _setWristVoltage = 0_V;
-
-    units::volt_t _rawArmVoltage = 0_V;
-    units::volt_t _rawWristVoltage = 0_V;
-
-    
-   
+class AlphaArm : public ::behaviour::HasBehaviour {
+ public:
+  explicit AlphaArm(AlphaArmConfig config);
+
+  void OnUpdate(units::second_t dt);
+  void SetArmRaw(units::volt_t voltage);
+  void setWristRaw(units::volt_t voltage);
+  void SetState(AlphaArmState state);
+  AlphaArmConfig GetConfig();
+  // void SetRaw(units::volt_t voltage);
+
+ private:
+  AlphaArmConfig _config;
+  AlphaArmState _state = AlphaArmState::kIdle;
+  units::volt_t _setAlphaArmVoltage = 0_V;
+  units::volt_t _setWristVoltage = 0_V;
+
+  units::volt_t _rawArmVoltage = 0_V;
+  units::volt_t _rawWristVoltage = 0_V;
 };
diff --git a/src/main/include/Robot.h b/src/main/include/Robot.h
index 5fad6cde..c02d71a7 100644
--- a/src/main/include/Robot.h
+++ b/src/main/include/Robot.h
@@ -1,6 +1,7 @@
 // Copyright (c) 2023-2024 CurtinFRC
 // Open Source Software, you can modify it according to the terms
 // of the MIT License at the root of this project
+
 #pragma once
 #include <frc/Encoder.h>
 #include <frc/TimedRobot.h>
@@ -11,6 +12,7 @@
 #include <frc/smartdashboard/Field2d.h>
 #include <frc/smartdashboard/SendableChooser.h>
 #include <frc/smartdashboard/SmartDashboard.h>
+
 #include <string>
 
 #include "AlphaArm.h"
@@ -21,7 +23,7 @@
 #include "Shooter.h"
 #include "ShooterBehaviour.h"
 #include "Wombat.h"
-#include "RobotMap.h"
+
 class Robot : public frc::TimedRobot {
  public:
   void TestInit() override;
@@ -34,11 +36,12 @@ class Robot : public frc::TimedRobot {
   void TeleopPeriodic() override;
   void DisabledInit() override;
   void DisabledPeriodic() override;
+
  private:
   RobotMap robotmap;
   wom::BehaviourScheduler* sched;
   frc::EventLoop loop;
-  Shooter *shooter;
+  Shooter* shooter;
 
   Intake* intake;
   frc::SendableChooser<std::string> m_chooser;
@@ -51,7 +54,7 @@ class Robot : public frc::TimedRobot {
 
   wom::SwerveDrive* _swerveDrive;
 
-  AlphaArm *alphaArm;
+  AlphaArm* alphaArm;
 
   // ctre::phoenix6::hardware::TalonFX *frontLeft;
   // ctre::phoenix6::hardware::TalonFX *frontRight;
diff --git a/src/main/include/RobotMap.h b/src/main/include/RobotMap.h
index fb6d2804..733ab1af 100644
--- a/src/main/include/RobotMap.h
+++ b/src/main/include/RobotMap.h
@@ -1,6 +1,7 @@
 // Copyright (c) 2023-2024 CurtinFRC
 // Open Source Software, you can modify it according to the terms
 // of the MIT License at the root of this project
+
 #pragma once
 
 #include <frc/Compressor.h>
@@ -10,6 +11,7 @@
 #include <units/angle.h>
 #include <units/length.h>
 
+#include <memory>
 #include <string>
 
 #include <ctre/phoenix6/CANcoder.hpp>
@@ -17,9 +19,7 @@
 #include <ctre/phoenix6/TalonFX.hpp>
 
 #include "AlphaArm.h"
-#include "AlphaArmBehaviour.h"
 #include "Intake.h"
-#include "IntakeBehaviour.h"
 #include "Shooter.h"
 #include "Wombat.h"
 
@@ -41,7 +41,7 @@ struct RobotMap {
     AlphaArmConfig config{alphaArmGearbox, wristGearbox};
   };
   AlphaArmSystem alphaArmSystem;
-  
+
   struct IntakeSystem {
     rev::CANSparkMax intakeMotor{2, rev::CANSparkMax::MotorType::kBrushed};
     // wom::CANSparkMaxEncoder intakeEncoder{&intakeMotor, 0.1_m};
@@ -52,8 +52,8 @@ struct RobotMap {
     wom::Gearbox IntakeGearbox{&intakeMotor, nullptr, frc::DCMotor::CIM(1)};
 
     IntakeConfig config{IntakeGearbox /*, &intakeSensor, &magSensor, &shooterSensor*/};
-  }; IntakeSystem intakeSystem;
-
+  };
+  IntakeSystem intakeSystem;
 
   struct Shooter {
     rev::CANSparkMax shooterMotor{11, rev::CANSparkMax::MotorType::kBrushless};
@@ -64,20 +64,19 @@ struct RobotMap {
     wom::Gearbox shooterGearbox{&shooterMotor, nullptr, frc::DCMotor::NEO(1)};
 
     ShooterConfig config{
-        "shooterGearbox",
-        shooterGearbox,
+        "shooterGearbox", shooterGearbox,
         // &shooterSensor,
     };
-  }; Shooter shooterSystem;
-  
+  };
+  Shooter shooterSystem;
+
   struct SwerveBase {
     ctre::phoenix6::hardware::CANcoder frontLeftCancoder{18, "Drivebase"};
     ctre::phoenix6::hardware::CANcoder frontRightCancoder{19, "Drivebase"};
     ctre::phoenix6::hardware::CANcoder backLeftCancoder{16, "Drivebase"};
     ctre::phoenix6::hardware::CANcoder backRightCancoder{17, "Drivebase"};
 
-    ctre::phoenix6::hardware::Pigeon2* gyro =
-        new ctre::phoenix6::hardware::Pigeon2(20, "Drivebase");
+    ctre::phoenix6::hardware::Pigeon2* gyro = new ctre::phoenix6::hardware::Pigeon2(20, "Drivebase");
     wpi::array<ctre::phoenix6::hardware::TalonFX*, 4> turnMotors{
         new ctre::phoenix6::hardware::TalonFX(7, "Drivebase"),   // front left
         new ctre::phoenix6::hardware::TalonFX(2, "Drivebase"),   // front right
@@ -90,48 +89,38 @@ struct RobotMap {
         new ctre::phoenix6::hardware::TalonFX(3, "Drivebase")};  // back right
 
     wpi::array<wom::SwerveModuleConfig, 4> moduleConfigs{
-        wom::SwerveModuleConfig{ //CORRECT
+        wom::SwerveModuleConfig{
+            // CORRECT
             // front left module
             frc::Translation2d(10.761_in, 9.455_in),
-            wom::Gearbox{
-                driveMotors[0],
-                new wom::TalonFXEncoder(driveMotors[0], 0.0445_m, 6.75),
-                frc::DCMotor::Falcon500(1).WithReduction(6.75)},
-            wom::Gearbox{turnMotors[0],
-                         new wom::CanEncoder(18, 0.0445_m, 4096, 12.8),
+            wom::Gearbox{driveMotors[0], new wom::TalonFXEncoder(driveMotors[0], 0.0445_m, 6.75),
+                         frc::DCMotor::Falcon500(1).WithReduction(6.75)},
+            wom::Gearbox{turnMotors[0], new wom::CanEncoder(18, 0.0445_m, 4096, 12.8),
                          frc::DCMotor::Falcon500(1).WithReduction(12.8)},
             &frontLeftCancoder, 4_in / 2},
-        wom::SwerveModuleConfig{ //CORRECT
+        wom::SwerveModuleConfig{
+            // CORRECT
             // front right module
             frc::Translation2d(10.761_in, -9.455_in),
-            wom::Gearbox{
-                driveMotors[1],
-                new wom::TalonFXEncoder(driveMotors[1], 0.0445_m, 6.75),
-                frc::DCMotor::Falcon500(1).WithReduction(6.75)},
-            wom::Gearbox{turnMotors[1],
-                         new wom::CanEncoder(19, 0.0445_m, 4096, 12.8),
+            wom::Gearbox{driveMotors[1], new wom::TalonFXEncoder(driveMotors[1], 0.0445_m, 6.75),
+                         frc::DCMotor::Falcon500(1).WithReduction(6.75)},
+            wom::Gearbox{turnMotors[1], new wom::CanEncoder(19, 0.0445_m, 4096, 12.8),
                          frc::DCMotor::Falcon500(1).WithReduction(12.8)},
             &frontRightCancoder, 4_in / 2},
         wom::SwerveModuleConfig{
             // back left module
             frc::Translation2d(-10.761_in, 9.455_in),
-            wom::Gearbox{
-                driveMotors[2],
-                new wom::TalonFXEncoder(driveMotors[2], 0.0445_m, 6.75),
-                frc::DCMotor::Falcon500(1).WithReduction(6.75)},
-            wom::Gearbox{turnMotors[2],
-                         new wom::CanEncoder(16, 0.0445_m, 4096, 12.8),
+            wom::Gearbox{driveMotors[2], new wom::TalonFXEncoder(driveMotors[2], 0.0445_m, 6.75),
+                         frc::DCMotor::Falcon500(1).WithReduction(6.75)},
+            wom::Gearbox{turnMotors[2], new wom::CanEncoder(16, 0.0445_m, 4096, 12.8),
                          frc::DCMotor::Falcon500(1).WithReduction(12.8)},
             &backRightCancoder, 4_in / 2},
         wom::SwerveModuleConfig{
             // back right module
             frc::Translation2d(-10.761_in, -9.455_in),
-            wom::Gearbox{
-                driveMotors[3],
-                new wom::TalonFXEncoder(driveMotors[3], 0.0445_m, 6.75),
-                frc::DCMotor::Falcon500(1).WithReduction(6.75)},
-            wom::Gearbox{turnMotors[3],
-                         new wom::CanEncoder(17, 0.0445_m, 4096, 12.8),
+            wom::Gearbox{driveMotors[3], new wom::TalonFXEncoder(driveMotors[3], 0.0445_m, 6.75),
+                         frc::DCMotor::Falcon500(1).WithReduction(6.75)},
+            wom::Gearbox{turnMotors[3], new wom::CanEncoder(17, 0.0445_m, 4096, 12.8),
                          frc::DCMotor::Falcon500(1).WithReduction(12.8)},
             &backLeftCancoder, 4_in / 2},
     };
@@ -151,19 +140,16 @@ struct RobotMap {
         wom::SwerveDriveConfig::pose_angle_conf_t::ki_t{0},
         0_deg / 1_deg};*/
     wom::SwerveDriveConfig::pose_position_conf_t posePositionPID{
-        "/drivetrain/pid/pose/position/config",
-        0_mps / 1_m,
-        wom::SwerveDriveConfig::pose_position_conf_t::ki_t{0.15},
-        0_m / 1_m,
-        0_cm};
+        "/drivetrain/pid/pose/position/config", 0_mps / 1_m,
+        wom::SwerveDriveConfig::pose_position_conf_t::ki_t{0.15}, 0_m / 1_m, 0_cm};
 
     // the config for the whole swerve drive
     wom::SwerveDriveConfig config{"/drivetrain",
-                                  //anglePID,
+                                  // anglePID,
                                   velocityPID,
                                   moduleConfigs,  // each module
                                   gyro,
-                                  //poseAnglePID,
+                                  // poseAnglePID,
                                   posePositionPID,
                                   60_kg,  // robot mass (estimate rn)
                                   {0.1, 0.1, 0.1},
@@ -183,6 +169,8 @@ struct RobotMap {
   SwerveBase swerveBase;
 
   struct SwerveTable {
-    std::shared_ptr<nt::NetworkTable> swerveDriveTable = nt::NetworkTableInstance::GetDefault().GetTable("swerve");
-  }; SwerveTable swerveTable;
+    std::shared_ptr<nt::NetworkTable> swerveDriveTable =
+        nt::NetworkTableInstance::GetDefault().GetTable("swerve");
+  };
+  SwerveTable swerveTable;
 };
diff --git a/wombat/src/main/cpp/behaviour/Behaviour.cpp b/wombat/src/main/cpp/behaviour/Behaviour.cpp
index d21d9e57..7312c922 100644
--- a/wombat/src/main/cpp/behaviour/Behaviour.cpp
+++ b/wombat/src/main/cpp/behaviour/Behaviour.cpp
@@ -10,9 +10,7 @@ using namespace behaviour;
 
 // Behaviour
 Behaviour::Behaviour(std::string name, units::time::second_t period)
-    : _bhvr_name(name),
-      _bhvr_period(period),
-      _bhvr_state(BehaviourState::INITIALISED) {}
+    : _bhvr_name(name), _bhvr_period(period), _bhvr_state(BehaviourState::INITIALISED) {}
 Behaviour::~Behaviour() {
   if (!IsFinished())
     Interrupt();
@@ -82,8 +80,7 @@ bool Behaviour::Tick() {
 
     if (dt > 2 * _bhvr_period) {
       std::cerr << "Behaviour missed deadline. Reduce Period. Dt=" << dt.value()
-                << " Dt(deadline)=" << (2 * _bhvr_period).value()
-                << ". Bhvr: " << GetName() << std::endl;
+                << " Dt(deadline)=" << (2 * _bhvr_period).value() << ". Bhvr: " << GetName() << std::endl;
     }
 
     if (_bhvr_timeout.value() > 0 && _bhvr_timer > _bhvr_timeout)
@@ -100,8 +97,7 @@ bool Behaviour::IsRunning() const {
 }
 
 bool Behaviour::IsFinished() const {
-  return _bhvr_state != BehaviourState::INITIALISED &&
-         _bhvr_state != BehaviourState::RUNNING;
+  return _bhvr_state != BehaviourState::INITIALISED && _bhvr_state != BehaviourState::RUNNING;
 }
 
 void Behaviour::Stop(BehaviourState new_state) {
@@ -173,8 +169,7 @@ void ConcurrentBehaviour::Add(Behaviour::ptr behaviour) {
 }
 
 std::string ConcurrentBehaviour::GetName() const {
-  std::string msg =
-      (_reducer == ConcurrentBehaviourReducer::ALL ? "ALL { " : "RACE {");
+  std::string msg = (_reducer == ConcurrentBehaviourReducer::ALL ? "ALL { " : "RACE {");
   for (auto b : _children)
     msg += b->GetName() + ", ";
   msg += "}";
@@ -190,8 +185,8 @@ void ConcurrentBehaviour::OnStart() {
         using namespace std::chrono_literals;
 
         b->Tick();
-        std::this_thread::sleep_for(std::chrono::milliseconds(
-            static_cast<int64_t>(b->GetPeriod().value() * 1000)));
+        std::this_thread::sleep_for(
+            std::chrono::milliseconds(static_cast<int64_t>(b->GetPeriod().value() * 1000)));
       }
 
       if (IsFinished() && !b->IsFinished())
@@ -273,10 +268,8 @@ void WaitFor::OnTick(units::time::second_t dt) {
 }
 
 // WaitTime
-WaitTime::WaitTime(units::time::second_t time)
-    : WaitTime([time]() { return time; }) {}
-WaitTime::WaitTime(std::function<units::time::second_t()> time_fn)
-    : _time_fn(time_fn) {}
+WaitTime::WaitTime(units::time::second_t time) : WaitTime([time]() { return time; }) {}
+WaitTime::WaitTime(std::function<units::time::second_t()> time_fn) : _time_fn(time_fn) {}
 
 void WaitTime::OnStart() {
   _time = _time_fn();
diff --git a/wombat/src/main/cpp/behaviour/BehaviourScheduler.cpp b/wombat/src/main/cpp/behaviour/BehaviourScheduler.cpp
index 26863c79..eb363b3e 100644
--- a/wombat/src/main/cpp/behaviour/BehaviourScheduler.cpp
+++ b/wombat/src/main/cpp/behaviour/BehaviourScheduler.cpp
@@ -52,8 +52,8 @@ void BehaviourScheduler::Schedule(Behaviour::ptr behaviour) {
         std::lock_guard<std::recursive_mutex> lk(_active_mtx);
         behaviour->Tick();
       }
-      std::this_thread::sleep_for(std::chrono::milliseconds(
-          static_cast<int64_t>(behaviour->GetPeriod().value() * 1000)));
+      std::this_thread::sleep_for(
+          std::chrono::milliseconds(static_cast<int64_t>(behaviour->GetPeriod().value() * 1000)));
     }
   });
 }
diff --git a/wombat/src/main/cpp/behaviour/HasBehaviour.cpp b/wombat/src/main/cpp/behaviour/HasBehaviour.cpp
index 964522c6..f2f557c5 100644
--- a/wombat/src/main/cpp/behaviour/HasBehaviour.cpp
+++ b/wombat/src/main/cpp/behaviour/HasBehaviour.cpp
@@ -8,8 +8,7 @@
 
 using namespace behaviour;
 
-void HasBehaviour::SetDefaultBehaviour(
-    std::function<std::shared_ptr<Behaviour>(void)> fn) {
+void HasBehaviour::SetDefaultBehaviour(std::function<std::shared_ptr<Behaviour>(void)> fn) {
   _default_behaviour_producer = fn;
 }
 
diff --git a/wombat/src/main/cpp/drivetrain/Drivetrain.cpp b/wombat/src/main/cpp/drivetrain/Drivetrain.cpp
index e516e7d7..387bbc31 100644
--- a/wombat/src/main/cpp/drivetrain/Drivetrain.cpp
+++ b/wombat/src/main/cpp/drivetrain/Drivetrain.cpp
@@ -11,8 +11,7 @@
 using namespace frc;
 using namespace units;
 
-wom::drivetrain::Drivetrain::Drivetrain(DrivetrainConfig* config,
-                                        XboxController& driver)
+wom::drivetrain::Drivetrain::Drivetrain(DrivetrainConfig* config, XboxController& driver)
     : _config(config), _driver(driver) {}
 wom::drivetrain::Drivetrain::~Drivetrain() {}
 
diff --git a/wombat/src/main/cpp/drivetrain/SwerveDrive.cpp b/wombat/src/main/cpp/drivetrain/SwerveDrive.cpp
index eb33efb1..74fc6d89 100644
--- a/wombat/src/main/cpp/drivetrain/SwerveDrive.cpp
+++ b/wombat/src/main/cpp/drivetrain/SwerveDrive.cpp
@@ -5,9 +5,9 @@
 #include "drivetrain/SwerveDrive.h"
 
 #include <networktables/NetworkTableInstance.h>
+#include <units/angle.h>
 #include <units/math.h>
 #include <units/voltage.h>
-#include <units/angle.h>
 
 #include <iostream>
 
@@ -16,17 +16,16 @@
 namespace wom {
 namespace drivetrain {
 
-void SwerveModuleConfig::WriteNT(
-    std::shared_ptr<nt::NetworkTable> table) const {
+void SwerveModuleConfig::WriteNT(std::shared_ptr<nt::NetworkTable> table) const {
   std::array<double, 2> pos{position.X().value(), position.Y().value()};
   table->GetEntry("position").SetDoubleArray(std::span(pos));
   table->GetEntry("wheelRadius").SetDouble(wheelRadius.value());
 }
 
 SwerveModule::SwerveModule(std::string path, SwerveModuleConfig config,
-                           //SwerveModule::angle_pid_conf_t anglePID,
+                           // SwerveModule::angle_pid_conf_t anglePID,
                            SwerveModule::velocity_pid_conf_t velocityPID)
-    : //_anglePIDController(path + "/pid/angle", anglePID),
+    :  // _anglePIDController(path + "/pid/angle", anglePID),
       _anglePIDController{frc::PIDController(4, 0, 0, 0.005_s)},
       _config(config),
       _velocityPIDController(frc::PIDController(1.2, 0, 0, 0.005_s)),
@@ -46,8 +45,6 @@ void SwerveModule::OnUpdate(units::second_t dt) {
   units::volt_t driveVoltage{0};
   units::volt_t turnVoltage{0};
 
-
-
   switch (_state) {
     case SwerveModuleState::kIdle:
       driveVoltage = 0_V;
@@ -55,10 +52,11 @@ void SwerveModule::OnUpdate(units::second_t dt) {
       break;
     case SwerveModuleState::kPID: {
       auto feedforward = _config.driveMotor.motor.Voltage(
-          0_Nm, units::radians_per_second_t{
-                    _velocityPIDController.GetSetpoint() / _config.wheelRadius.value()});
-      //std::cout << "GetSpeed()" << GetSpeed().value() << std::endl;
-      // units::radian_t input = units::math::fmod(_config.turnMotor.encoder->GetEncoderPosition(), (2_rad * 3.1415));
+          0_Nm,
+          units::radians_per_second_t{_velocityPIDController.GetSetpoint() / _config.wheelRadius.value()});
+      // std::cout << "GetSpeed()" << GetSpeed().value() << std::endl;
+      //  units::radian_t input = units::math::fmod(_config.turnMotor.encoder->GetEncoderPosition(), (2_rad
+      //  * 3.1415));
       double input = -_config.turnMotor.encoder->GetEncoderPosition().value();
       _table->GetEntry("/testing/GetEncoderPos").SetDouble(input);
       // _velocityPIDController.SetSetpoint(3);
@@ -75,8 +73,7 @@ void SwerveModule::OnUpdate(units::second_t dt) {
       std::cerr << "Case not handled" << std::endl;
   }
 
-  units::newton_meter_t torqueLimit =
-      50_kg / 4 * _config.wheelRadius * _currentAccelerationLimit;
+  units::newton_meter_t torqueLimit = 50_kg / 4 * _config.wheelRadius * _currentAccelerationLimit;
   // units::volt_t voltageMax = _config.driveMotor.motor.Voltage(
   //     torqueLimit, _config.driveMotor.encoder->GetEncoderAngularVelocity());
   // units::volt_t voltageMin = _config.driveMotor.motor.Voltage(
@@ -92,8 +89,7 @@ void SwerveModule::OnUpdate(units::second_t dt) {
   //     units::math::max(driveVoltage, -_driveModuleVoltageLimit),
   //     _driveModuleVoltageLimit);  // was originally 10_V
   units::volt_t turnVoltageMax = 7_V - (driveVoltage * (7_V / 10_V));
-  turnVoltage = units::math::min(units::math::max(turnVoltage, -turnVoltageMax),
-                                 turnVoltageMax);
+  turnVoltage = units::math::min(units::math::max(turnVoltage, -turnVoltageMax), turnVoltageMax);
   // turnVoltage = units::math::min(units::math::max(turnVoltage, -7_V), 7_V);
   // std::cout << "turn-voltage-max: " << turnVoltageMax.value() << std::endl;
 
@@ -106,9 +102,7 @@ void SwerveModule::OnUpdate(units::second_t dt) {
 
   _table->GetEntry("speed").SetDouble(GetSpeed().value());
   _table->GetEntry("angle").SetDouble(
-      _config.turnMotor.encoder->GetEncoderPosition()
-          .convert<units::degree>()
-          .value());
+      _config.turnMotor.encoder->GetEncoderPosition().convert<units::degree>().value());
   _config.WriteNT(_table->GetSubTable("config"));
 }
 
@@ -116,13 +110,11 @@ void SwerveModule::OnUpdate(units::second_t dt) {
 //   return (_config.turnMotor.encoder->GetEncoderPosition().value());
 // }
 
-void SwerveModule::SetAccelerationLimit(
-    units::meters_per_second_squared_t limit) {
+void SwerveModule::SetAccelerationLimit(units::meters_per_second_squared_t limit) {
   _currentAccelerationLimit = limit;
 }
 
-void SwerveDrive::SetAccelerationLimit(
-    units::meters_per_second_squared_t limit) {
+void SwerveDrive::SetAccelerationLimit(units::meters_per_second_squared_t limit) {
   for (int motorNumber = 0; motorNumber < 4; motorNumber++) {
     _modules[motorNumber].SetAccelerationLimit(limit);
   }
@@ -141,16 +133,13 @@ void SwerveModule::SetZero(units::second_t dt) {
   _state = SwerveModuleState::kPID;
 }
 
-void SwerveModule::SetPID(units::radian_t angle,
-                          units::meters_per_second_t speed,
-                          units::second_t dt) {
+void SwerveModule::SetPID(units::radian_t angle, units::meters_per_second_t speed, units::second_t dt) {
   _state = SwerveModuleState::kPID;
   // @liam start added
   // std::cout << "angle Setpoint: " << _anglePIDController.GetSetpoint() << std::endl;
   // std::cout << "angle value: " << angle.value() << std::endl;
 
-  double diff = std::fmod((_anglePIDController.GetSetpoint() - angle.value()),
-                          (2 * 3.1415));
+  double diff = std::fmod((_anglePIDController.GetSetpoint() - angle.value()), (2 * 3.1415));
   std::cout << "DIFF value: " << diff << std::endl;
   _table->GetEntry("/Differential value:").SetDouble(diff);
   if (std::abs(diff) >= 3.1415) {
@@ -165,39 +154,32 @@ void SwerveModule::SetPID(units::radian_t angle,
 
 void SwerveModule::ModuleVectorHandler(frc::ChassisSpeeds speeds) {
   units::meters_per_second_t xVelocityComponent =
-      1_mps *
-      (speeds.vx.value() + speeds.omega.value() * _config.position.X().value());
+      1_mps * (speeds.vx.value() + speeds.omega.value() * _config.position.X().value());
   units::meters_per_second_t yVelocityComponent =
-      1_mps *
-      (speeds.vy.value() + speeds.omega.value() * _config.position.Y().value());
+      1_mps * (speeds.vy.value() + speeds.omega.value() * _config.position.Y().value());
 
   units::meters_per_second_t velocity =
-      1_mps * std::sqrt(std::pow(xVelocityComponent.value(), 2) +
-                        std::pow(yVelocityComponent.value(), 2));
-  units::degree_t angle = 1_rad * std::atan2(yVelocityComponent.value(),
-                                             xVelocityComponent.value());
+      1_mps * std::sqrt(std::pow(xVelocityComponent.value(), 2) + std::pow(yVelocityComponent.value(), 2));
+  units::degree_t angle = 1_rad * std::atan2(yVelocityComponent.value(), xVelocityComponent.value());
 
   _anglePIDController.SetSetpoint(angle.value());
   _velocityPIDController.SetSetpoint(velocity.value());
 }
 
 units::meters_per_second_t SwerveModule::GetSpeed() const {
-  units::meters_per_second_t returnVal{
-    _config.driveMotor.encoder->GetVelocityValue() *
-    _config.wheelRadius.value()};
-    // std::cout << returnVal.value() << std::endl;
-    return returnVal;
+  units::meters_per_second_t returnVal{_config.driveMotor.encoder->GetVelocityValue() *
+                                       _config.wheelRadius.value()};
+  // std::cout << returnVal.value() << std::endl;
+  return returnVal;
 }
 
 units::meter_t SwerveModule::GetDistance() const {
-  return units::meter_t{
-      _config.driveMotor.encoder->GetEncoderPosition().value() *
-      _config.wheelRadius.value()};
+  return units::meter_t{_config.driveMotor.encoder->GetEncoderPosition().value() *
+                        _config.wheelRadius.value()};
 }
 
 frc::SwerveModulePosition SwerveModule::GetPosition() const {
-  return frc::SwerveModulePosition{
-      GetDistance(), _config.turnMotor.encoder->GetEncoderPosition()};
+  return frc::SwerveModulePosition{GetDistance(), _config.turnMotor.encoder->GetEncoderPosition()};
 }
 
 const SwerveModuleConfig& SwerveModule::GetConfig() const {
@@ -210,15 +192,14 @@ void SwerveDriveConfig::WriteNT(std::shared_ptr<nt::NetworkTable> table) {
 
 SwerveDrive::SwerveDrive(SwerveDriveConfig config, frc::Pose2d initialPose)
     : _config(config),
-      _kinematics(_config.modules[0].position, _config.modules[1].position,
-                  _config.modules[2].position, _config.modules[3].position),
+      _kinematics(_config.modules[0].position, _config.modules[1].position, _config.modules[2].position,
+                  _config.modules[3].position),
       _poseEstimator(
           _kinematics, frc::Rotation2d(0_deg),
-          wpi::array<frc::SwerveModulePosition, 4>{
-              frc::SwerveModulePosition{0_m, frc::Rotation2d{0_deg}},
-              frc::SwerveModulePosition{0_m, frc::Rotation2d{0_deg}},
-              frc::SwerveModulePosition{0_m, frc::Rotation2d{0_deg}},
-              frc::SwerveModulePosition{0_m, frc::Rotation2d{0_deg}}},
+          wpi::array<frc::SwerveModulePosition, 4>{frc::SwerveModulePosition{0_m, frc::Rotation2d{0_deg}},
+                                                   frc::SwerveModulePosition{0_m, frc::Rotation2d{0_deg}},
+                                                   frc::SwerveModulePosition{0_m, frc::Rotation2d{0_deg}},
+                                                   frc::SwerveModulePosition{0_m, frc::Rotation2d{0_deg}}},
           initialPose, _config.stateStdDevs, _config.visionMeasurementStdDevs),
       _anglePIDController{frc::PIDController(1, 0, 0)},
       _xPIDController(config.path + "/pid/x", _config.posePositionPID),
@@ -236,14 +217,12 @@ SwerveDrive::SwerveDrive(SwerveDriveConfig config, frc::Pose2d initialPose)
   ResetPose(initialPose);
 }
 
-frc::ChassisSpeeds FieldRelativeSpeeds::ToChassisSpeeds(
-    const units::radian_t robotHeading) {
-  return frc::ChassisSpeeds::FromFieldRelativeSpeeds(
-      vx, vy, omega, frc::Rotation2d{robotHeading});
+frc::ChassisSpeeds FieldRelativeSpeeds::ToChassisSpeeds(const units::radian_t robotHeading) {
+  return frc::ChassisSpeeds::FromFieldRelativeSpeeds(vx, vy, omega, frc::Rotation2d{robotHeading});
 }
 
 void SwerveDrive::OnUpdate(units::second_t dt) {
-  //std::cout << _config.gyro->GetYaw().GetValue().value() << std::endl;
+  // std::cout << _config.gyro->GetYaw().GetValue().value() << std::endl;
   switch (_state) {
     case SwerveDriveState::kZeroing:
       for (auto mod = _modules.begin(); mod < _modules.end(); mod++) {
@@ -258,14 +237,15 @@ void SwerveDrive::OnUpdate(units::second_t dt) {
     case SwerveDriveState::kPose: {
       _target_fr_speeds.vx = _xPIDController.Calculate(GetPose().X(), dt);
       _target_fr_speeds.vy = _yPIDController.Calculate(GetPose().Y(), dt);
-      _target_fr_speeds.omega = units::radians_per_second_t{_anglePIDController.Calculate(GetPose().Rotation().Radians().value())};
+      _target_fr_speeds.omega =
+          units::radians_per_second_t{_anglePIDController.Calculate(GetPose().Rotation().Radians().value())};
     }
       [[fallthrough]];
     case SwerveDriveState::kFieldRelativeVelocity:
-      _target_speed =
-          _target_fr_speeds.ToChassisSpeeds(GetPose().Rotation().Radians());
+      _target_speed = _target_fr_speeds.ToChassisSpeeds(GetPose().Rotation().Radians());
       if (isRotateToMatchJoystick) {
-        _target_speed.omega = units::radians_per_second_t{_anglePIDController.Calculate(GetPose().Rotation().Radians().value())};
+        _target_speed.omega = units::radians_per_second_t{
+            _anglePIDController.Calculate(GetPose().Rotation().Radians().value())};
       }
       // std::cout << "vx = " << _target_speed.vx.value() << " vy = " <<
       // _target_fr_speeds.vy.value() << std::endl;
@@ -273,8 +253,7 @@ void SwerveDrive::OnUpdate(units::second_t dt) {
     case SwerveDriveState::kVelocity: {
       auto target_states = _kinematics.ToSwerveModuleStates(_target_speed);
       for (size_t i = 0; i < _modules.size(); i++) {
-        _modules[i].SetPID(target_states[i].angle.Radians(),
-                           target_states[i].speed, dt);
+        _modules[i].SetPID(target_states[i].angle.Radians(), target_states[i].speed, dt);
         // std::cout << "module " << i << ": " <<
         // target_states[i].angle.Radians().value() << std::endl;
       }
@@ -297,13 +276,12 @@ void SwerveDrive::OnUpdate(units::second_t dt) {
     case SwerveDriveState::kFRVelocityRotationLock:
       _target_speed.vx = _xPIDController.Calculate(GetPose().X(), dt);
       _target_speed.vy = _yPIDController.Calculate(GetPose().Y(), dt);
-      _target_speed.omega = units::radians_per_second_t{_anglePIDController.Calculate(GetPose().Rotation().Radians().value())};
-      _target_speed =
-          _requestedSpeeds.ToChassisSpeeds(GetPose().Rotation().Radians());
+      _target_speed.omega =
+          units::radians_per_second_t{_anglePIDController.Calculate(GetPose().Rotation().Radians().value())};
+      _target_speed = _requestedSpeeds.ToChassisSpeeds(GetPose().Rotation().Radians());
       auto target_states = _kinematics.ToSwerveModuleStates(_target_speed);
       for (size_t i = 0; i < _modules.size(); i++) {
-        _modules[i].SetPID(target_states[i].angle.Radians(),
-                           target_states[i].speed, dt);
+        _modules[i].SetPID(target_states[i].angle.Radians(), target_states[i].speed, dt);
       }
       break;
   }
@@ -314,12 +292,10 @@ void SwerveDrive::OnUpdate(units::second_t dt) {
 
   _poseEstimator.Update(
       _config.gyro->GetRotation2d(),
-      wpi::array<frc::SwerveModulePosition, 4>{
-          _modules[0].GetPosition(), _modules[1].GetPosition(),
-          _modules[2].GetPosition(), _modules[3].GetPosition()});
+      wpi::array<frc::SwerveModulePosition, 4>{_modules[0].GetPosition(), _modules[1].GetPosition(),
+                                               _modules[2].GetPosition(), _modules[3].GetPosition()});
 
-  utils::WritePose2NT(_table->GetSubTable("estimatedPose"),
-                      _poseEstimator.GetEstimatedPosition());
+  utils::WritePose2NT(_table->GetSubTable("estimatedPose"), _poseEstimator.GetEstimatedPosition());
   _config.WriteNT(_table->GetSubTable("config"));
 }
 
@@ -358,8 +334,7 @@ void SwerveDrive::OnResetMode() {
   _anglePIDController.Reset();
 }
 
-void SwerveDrive::RotateMatchJoystick(units::radian_t joystickAngle,
-                                      FieldRelativeSpeeds speeds) {
+void SwerveDrive::RotateMatchJoystick(units::radian_t joystickAngle, FieldRelativeSpeeds speeds) {
   // _state = SwerveDriveState::kFRVelocityRotationLock;
   // _anglePIDController.SetSetpoint(joystickAngle);
   // _target_fr_speeds = speeds;
@@ -385,16 +360,14 @@ bool SwerveDrive::GetIsFieldRelative() {
   return _isFieldRelative;
 }
 
-void SwerveDrive::SetIndividualTuning(int mod, units::radian_t angle,
-                                      units::meters_per_second_t speed) {
+void SwerveDrive::SetIndividualTuning(int mod, units::radian_t angle, units::meters_per_second_t speed) {
   _mod = mod;
   _angle = angle;
   _speed = speed;
   _state = SwerveDriveState::kIndividualTuning;
 }
 
-void SwerveDrive::SetTuning(units::radian_t angle,
-                            units::meters_per_second_t speed) {
+void SwerveDrive::SetTuning(units::radian_t angle, units::meters_per_second_t speed) {
   _angle = angle;
   _speed = speed;
   _state = SwerveDriveState::kTuning;
@@ -421,9 +394,8 @@ bool SwerveDrive::IsAtSetPose() {
 void SwerveDrive::ResetPose(frc::Pose2d pose) {
   _poseEstimator.ResetPosition(
       _config.gyro->GetRotation2d(),
-      wpi::array<frc::SwerveModulePosition, 4>{
-          _modules[0].GetPosition(), _modules[1].GetPosition(),
-          _modules[2].GetPosition(), _modules[3].GetPosition()},
+      wpi::array<frc::SwerveModulePosition, 4>{_modules[0].GetPosition(), _modules[1].GetPosition(),
+                                               _modules[2].GetPosition(), _modules[3].GetPosition()},
       pose);
 }
 
@@ -431,8 +403,7 @@ frc::Pose2d SwerveDrive::GetPose() {
   return _poseEstimator.GetEstimatedPosition();
 }
 
-void SwerveDrive::AddVisionMeasurement(frc::Pose2d pose,
-                                       units::second_t timestamp) {
+void SwerveDrive::AddVisionMeasurement(frc::Pose2d pose, units::second_t timestamp) {
   _poseEstimator.AddVisionMeasurement(pose, timestamp);
 }
 }  // namespace drivetrain
diff --git a/wombat/src/main/cpp/drivetrain/behaviours/SwerveBehaviours.cpp b/wombat/src/main/cpp/drivetrain/behaviours/SwerveBehaviours.cpp
index a82c84d8..23baf7fd 100644
--- a/wombat/src/main/cpp/drivetrain/behaviours/SwerveBehaviours.cpp
+++ b/wombat/src/main/cpp/drivetrain/behaviours/SwerveBehaviours.cpp
@@ -73,40 +73,36 @@ void ManualDrivebase::OnTick(units::second_t deltaTime) {
   // if (isZero) {
   //   _swerveDrivebase->SetZeroing();
   // } else {
-    double xVelocity = wom::utils::spow2(-wom::utils::deadzone(
-        _driverController->GetLeftY(),
-        driverDeadzone));  // GetLeftY due to x being where y should be on field
-    double yVelocity = wom::utils::spow2(
-        -wom::utils::deadzone(_driverController->GetLeftX(), driverDeadzone));
-
-    double r_x = wom::utils::spow2(
-        -wom::utils::deadzone(_driverController->GetRightX(), turningDeadzone));
-
-    double turnX = _driverController->GetRightX();
-    double turnY = _driverController->GetRightY();
-    double num = std::sqrt(turnX * turnX + turnY * turnY);
-    if (num < turningDeadzone) {
-      turnX = 0;
-      turnY = 0;
-    }
-
-    // if (isRotateMatch) {
-    //   units::degree_t currentAngle =
-    //       _swerveDrivebase->GetPose().Rotation().Degrees();
-    //   CalculateRequestedAngle(turnX, turnY, currentAngle);
-    //   _swerveDriveTable->GetEntry("RotateMatch")
-    //       .SetDouble(_requestedAngle.value());
-    //   _swerveDrivebase->RotateMatchJoystick(
-    //       _requestedAngle,
-    //       wom::drivetrain::FieldRelativeSpeeds{// also field relative
-    //                                            xVelocity * maxMovementMagnitude,
-    //                                            yVelocity * maxMovementMagnitude,
-    //                                            r_x * maxRotationMagnitude});
-    // } else {
-      _swerveDrivebase->SetFieldRelativeVelocity(
-          wom::drivetrain::FieldRelativeSpeeds{xVelocity * maxMovementMagnitude,
-                                               yVelocity * maxMovementMagnitude,
-                                               r_x * maxRotationMagnitude});
+  double xVelocity = wom::utils::spow2(
+      -wom::utils::deadzone(_driverController->GetLeftY(),
+                            driverDeadzone));  // GetLeftY due to x being where y should be on field
+  double yVelocity = wom::utils::spow2(-wom::utils::deadzone(_driverController->GetLeftX(), driverDeadzone));
+
+  double r_x = wom::utils::spow2(-wom::utils::deadzone(_driverController->GetRightX(), turningDeadzone));
+
+  double turnX = _driverController->GetRightX();
+  double turnY = _driverController->GetRightY();
+  double num = std::sqrt(turnX * turnX + turnY * turnY);
+  if (num < turningDeadzone) {
+    turnX = 0;
+    turnY = 0;
+  }
+
+  // if (isRotateMatch) {
+  //   units::degree_t currentAngle =
+  //       _swerveDrivebase->GetPose().Rotation().Degrees();
+  //   CalculateRequestedAngle(turnX, turnY, currentAngle);
+  //   _swerveDriveTable->GetEntry("RotateMatch")
+  //       .SetDouble(_requestedAngle.value());
+  //   _swerveDrivebase->RotateMatchJoystick(
+  //       _requestedAngle,
+  //       wom::drivetrain::FieldRelativeSpeeds{// also field relative
+  //                                            xVelocity * maxMovementMagnitude,
+  //                                            yVelocity * maxMovementMagnitude,
+  //                                            r_x * maxRotationMagnitude});
+  // } else {
+  _swerveDrivebase->SetFieldRelativeVelocity(wom::drivetrain::FieldRelativeSpeeds{
+      xVelocity * maxMovementMagnitude, yVelocity * maxMovementMagnitude, r_x * maxRotationMagnitude});
   //   }
   // }
   // _swerveDrivebase->SetTuning(100_deg, 1_mps);
@@ -117,8 +113,7 @@ void ManualDrivebase::ResetMode() {
   resetMode = false;
 }
 
-void ManualDrivebase::CalculateRequestedAngle(double joystickX,
-                                              double joystickY,
+void ManualDrivebase::CalculateRequestedAngle(double joystickX, double joystickY,
                                               units::degree_t defaultAngle) {
   _requestedAngle = (1_rad * std::atan2(joystickY, -joystickX)) + 90_deg;
   if (wom::utils::deadzone(joystickX) == 0 && wom::utils::deadzone(joystickY) == 0) {
@@ -127,8 +122,7 @@ void ManualDrivebase::CalculateRequestedAngle(double joystickX,
 }
 
 // Code for x-ing the wheels on the drivebase
-XDrivebase::XDrivebase(wom::drivetrain::SwerveDrive* swerveDrivebase)
-    : _swerveDrivebase(swerveDrivebase) {
+XDrivebase::XDrivebase(wom::drivetrain::SwerveDrive* swerveDrivebase) : _swerveDrivebase(swerveDrivebase) {
   Controls(swerveDrivebase);
 }
 void XDrivebase::OnTick(units::second_t deltaTime) {
@@ -170,20 +164,17 @@ void XDrivebase::OnTick(units::second_t deltaTime) {
 //   m_timer.Start();
 // }
 
-wom::drivetrain::behaviours::TempSimSwerveDrive::TempSimSwerveDrive(
-    frc::Timer* timer, frc::Field2d* field)
+wom::drivetrain::behaviours::TempSimSwerveDrive::TempSimSwerveDrive(frc::Timer* timer, frc::Field2d* field)
     : m_timer(timer), m_field(field) {}
 
 void wom::drivetrain::behaviours::TempSimSwerveDrive::OnUpdate() {
   m_field->SetRobotPose(m_driveSim.GetPose());
 
   // get the current trajectory state
-  frc::Trajectory::State desired_state =
-      current_trajectory.Sample(m_timer->Get());
+  frc::Trajectory::State desired_state = current_trajectory.Sample(m_timer->Get());
 
   // get the current wheel speeds
-  wom::utils::WriteTrajectoryState(current_trajectory_state_table,
-                                   desired_state);
+  wom::utils::WriteTrajectoryState(current_trajectory_state_table, desired_state);
 
   // move drivebase position to the desired state
   m_driveSim.SetPose(wom::utils::TrajectoryStateToPose2d(desired_state));
@@ -201,19 +192,16 @@ frc::Pose2d wom::drivetrain::behaviours::TempSimSwerveDrive::GetPose2d() {
   return m_driveSim.GetPose();
 }
 
-void wom::drivetrain::behaviours::TempSimSwerveDrive::SetPath(
-    std::string path) {
+void wom::drivetrain::behaviours::TempSimSwerveDrive::SetPath(std::string path) {
   nt::NetworkTableInstance inst = nt::NetworkTableInstance::GetDefault();
   std::shared_ptr<nt::NetworkTable> table = inst.GetTable("FMSInfo");
 
   // create a netowrk table for the trajectory
   std::shared_ptr<nt::NetworkTable> trajectory_table =
       nt::NetworkTableInstance::GetDefault().GetTable("trajectory_path");
-  current_trajectory_table =
-      nt::NetworkTableInstance::GetDefault().GetTable("current_trajectory");
+  current_trajectory_table = nt::NetworkTableInstance::GetDefault().GetTable("current_trajectory");
   current_trajectory_state_table =
-      nt::NetworkTableInstance::GetDefault().GetTable(
-          "current_trajectory_state");
+      nt::NetworkTableInstance::GetDefault().GetTable("current_trajectory_state");
 
   current_trajectory = m_pathplanner.getTrajectory(path);
   m_driveSim.SetPose(current_trajectory.Sample(0_s).pose);
@@ -221,12 +209,10 @@ void wom::drivetrain::behaviours::TempSimSwerveDrive::SetPath(
   m_timer->Start();
 }
 
-wom::drivetrain::behaviours::AutoSwerveDrive::AutoSwerveDrive(
-    wom::drivetrain::SwerveDrive* swerve, frc::Timer* timer,
-    frc::Field2d* field)
+wom::drivetrain::behaviours::AutoSwerveDrive::AutoSwerveDrive(wom::drivetrain::SwerveDrive* swerve,
+                                                              frc::Timer* timer, frc::Field2d* field)
     : _swerve(swerve), m_timer(timer), m_field(field) {
-  _simSwerveDrive =
-      new wom::drivetrain::behaviours::TempSimSwerveDrive(timer, field);
+  _simSwerveDrive = new wom::drivetrain::behaviours::TempSimSwerveDrive(timer, field);
 }
 
 void wom::drivetrain::behaviours::AutoSwerveDrive::OnUpdate() {
diff --git a/wombat/src/main/cpp/subsystems/Arm.cpp b/wombat/src/main/cpp/subsystems/Arm.cpp
index 9390601a..148cec43 100644
--- a/wombat/src/main/cpp/subsystems/Arm.cpp
+++ b/wombat/src/main/cpp/subsystems/Arm.cpp
@@ -10,19 +10,14 @@ using namespace frc;
 using namespace wom;
 
 // creates network table instatnce on shuffleboard
-void wom::subsystems::ArmConfig::WriteNT(
-    std::shared_ptr<nt::NetworkTable> table) {
+void wom::subsystems::ArmConfig::WriteNT(std::shared_ptr<nt::NetworkTable> table) {
   table->GetEntry("armMass").SetDouble(armMass.value());
   table->GetEntry("loadMass").SetDouble(loadMass.value());
   table->GetEntry("armLength").SetDouble(armLength.value());
-  table->GetEntry("minAngle")
-      .SetDouble(minAngle.convert<units::degree>().value());
-  table->GetEntry("maxAngle")
-      .SetDouble(maxAngle.convert<units::degree>().value());
-  table->GetEntry("initialAngle")
-      .SetDouble(initialAngle.convert<units::degree>().value());
-  table->GetEntry("angleOffset")
-      .SetDouble(initialAngle.convert<units::degree>().value());
+  table->GetEntry("minAngle").SetDouble(minAngle.convert<units::degree>().value());
+  table->GetEntry("maxAngle").SetDouble(maxAngle.convert<units::degree>().value());
+  table->GetEntry("initialAngle").SetDouble(initialAngle.convert<units::degree>().value());
+  table->GetEntry("angleOffset").SetDouble(initialAngle.convert<units::degree>().value());
 }
 
 // arm config is used
@@ -43,14 +38,12 @@ void wom::subsystems::Arm::OnUpdate(units::second_t dt) {
     case ArmState::kIdle:
       break;
     case ArmState::kVelocity: {
-      units::newton_meter_t torque =
-          9.81_m / 1_s / 1_s * _config.armLength *
-          units::math::cos(angle + _config.angleOffset) *
-          (0.5 * _config.armMass + _config.loadMass);
+      units::newton_meter_t torque = 9.81_m / 1_s / 1_s * _config.armLength *
+                                     units::math::cos(angle + _config.angleOffset) *
+                                     (0.5 * _config.armMass + _config.loadMass);
       // units::volt_t feedforward = _config.leftGearbox.motor.Voltage(torque,
       // 0_rad/1_s);
-      units::volt_t feedforward =
-          _config.leftGearbox.motor.Voltage(torque, _velocityPID.GetSetpoint());
+      units::volt_t feedforward = _config.leftGearbox.motor.Voltage(torque, _velocityPID.GetSetpoint());
       // feedforward = 3.5_V;
       // std::cout << "feedforward" << feedforward.value() << std::endl;
       voltage = _velocityPID.Calculate(GetArmVelocity(), dt, feedforward);
@@ -58,12 +51,10 @@ void wom::subsystems::Arm::OnUpdate(units::second_t dt) {
       // std::endl; voltage = 0_V;
     } break;
     case ArmState::kAngle: {
-      units::newton_meter_t torque =
-          9.81_m / 1_s / 1_s * _config.armLength *
-          units::math::cos(angle + _config.angleOffset) *
-          (0.5 * _config.armMass + _config.loadMass);
-      units::volt_t feedforward =
-          _config.leftGearbox.motor.Voltage(torque, 0_rad / 1_s);
+      units::newton_meter_t torque = 9.81_m / 1_s / 1_s * _config.armLength *
+                                     units::math::cos(angle + _config.angleOffset) *
+                                     (0.5 * _config.armMass + _config.loadMass);
+      units::volt_t feedforward = _config.leftGearbox.motor.Voltage(torque, 0_rad / 1_s);
       // std::cout << "feedforward" << feedforward.value() << std::endl;
       voltage = _pid.Calculate(angle, dt, feedforward);
     } break;
diff --git a/wombat/src/main/cpp/subsystems/Elevator.cpp b/wombat/src/main/cpp/subsystems/Elevator.cpp
index d6b59f8a..d1cf9a8f 100644
--- a/wombat/src/main/cpp/subsystems/Elevator.cpp
+++ b/wombat/src/main/cpp/subsystems/Elevator.cpp
@@ -7,8 +7,7 @@
 #include <networktables/NetworkTableInstance.h>
 #include <units/acceleration.h>
 
-void wom::subsystems::ElevatorConfig::WriteNT(
-    std::shared_ptr<nt::NetworkTable> table) {
+void wom::subsystems::ElevatorConfig::WriteNT(std::shared_ptr<nt::NetworkTable> table) {
   table->GetEntry("radius").SetDouble(radius.value());
   table->GetEntry("mass").SetDouble(mass.value());
   table->GetEntry("maxHeight").SetDouble(maxHeight.value());
@@ -20,8 +19,7 @@ wom::subsystems::Elevator::Elevator(ElevatorConfig config)
       _pid{config.path + "/pid", config.pid},
       _velocityPID{config.path + "/velocityPID", config.velocityPID},
       _table(nt::NetworkTableInstance::GetDefault().GetTable(config.path)) {
-  _config.leftGearbox.encoder->SetEncoderPosition(_config.initialHeight /
-                                                  _config.radius * 1_rad);
+  _config.leftGearbox.encoder->SetEncoderPosition(_config.initialHeight / _config.radius * 1_rad);
 }
 
 void wom::subsystems::Elevator::OnUpdate(units::second_t dt) {
@@ -39,8 +37,7 @@ void wom::subsystems::Elevator::OnUpdate(units::second_t dt) {
     case ElevatorState::kVelocity: {
       units::volt_t feedforward = _config.rightGearbox.motor.Voltage(
           (_config.mass * 9.81_mps_sq) * _config.radius,
-          _velocityPID.GetSetpoint() /
-              (14.0 / 60.0 * 2.0 * 3.1415 * 0.02225 * 1_m) * 1_rad);
+          _velocityPID.GetSetpoint() / (14.0 / 60.0 * 2.0 * 3.1415 * 0.02225 * 1_m) * 1_rad);
       // units::volt_t feedforward = _config.rightGearbox.motor.Voltage(0_Nm,
       // _velocityPID.GetSetpoint() / (14.0/60.0 * 2.0 * 3.1415 * 0.02225 * 1_m)
       // * 1_rad);
@@ -53,8 +50,8 @@ void wom::subsystems::Elevator::OnUpdate(units::second_t dt) {
       // voltage = 0_V;
     } break;
     case ElevatorState::kPID: {
-      units::volt_t feedforward = _config.rightGearbox.motor.Voltage(
-          (_config.mass * 9.81_mps_sq) * _config.radius, 0_rad_per_s);
+      units::volt_t feedforward =
+          _config.rightGearbox.motor.Voltage((_config.mass * 9.81_mps_sq) * _config.radius, 0_rad_per_s);
       // std::cout << "feed forward" << feedforward.value() << std::endl;
       feedforward = 1.2_V;
       // voltage = _pid.Calculate(height, dt, feedforward);
@@ -99,8 +96,7 @@ void wom::subsystems::Elevator::SetPID(units::meter_t height) {
   _pid.SetSetpoint(height);
 }
 
-void wom::subsystems::Elevator::SetVelocity(
-    units::meters_per_second_t velocity) {
+void wom::subsystems::Elevator::SetVelocity(units::meters_per_second_t velocity) {
   _velocityPID.SetSetpoint(velocity);
 }
 
@@ -132,18 +128,14 @@ units::meter_t wom::subsystems::Elevator::GetHeight() const {
   // std::cout << "elevator position"<<
   // _config.rightGearbox.encoder->GetEncoderTicks() << std::endl; return
   // _config.rightGearbox.encoder->GetEncoderDistance() * 1_m;
-  return _config.elevatorEncoder.GetPosition() * 14 / 60 * 2 * 3.1415 *
-         0.02225 * 1_m;
+  return _config.elevatorEncoder.GetPosition() * 14 / 60 * 2 * 3.1415 * 0.02225 * 1_m;
 }
 
-units::meters_per_second_t wom::subsystems::Elevator::GetElevatorVelocity()
-    const {
-  return _config.elevatorEncoder.GetVelocity() / 60_s * 14 / 60 * 2 * 3.1415 *
-         0.02225 * 1_m;
+units::meters_per_second_t wom::subsystems::Elevator::GetElevatorVelocity() const {
+  return _config.elevatorEncoder.GetVelocity() / 60_s * 14 / 60 * 2 * 3.1415 * 0.02225 * 1_m;
 }
 
 units::meters_per_second_t wom::subsystems::Elevator::MaxSpeed() const {
-  return _config.leftGearbox.motor.Speed(
-             (_config.mass * 9.81_mps_sq) * _config.radius, 12_V) /
-         1_rad * _config.radius;
+  return _config.leftGearbox.motor.Speed((_config.mass * 9.81_mps_sq) * _config.radius, 12_V) / 1_rad *
+         _config.radius;
 }
diff --git a/wombat/src/main/cpp/subsystems/Shooter.cpp b/wombat/src/main/cpp/subsystems/Shooter.cpp
index 2fede2f3..c851266b 100644
--- a/wombat/src/main/cpp/subsystems/Shooter.cpp
+++ b/wombat/src/main/cpp/subsystems/Shooter.cpp
@@ -14,16 +14,14 @@ wom::subsystems::Shooter::Shooter(std::string path, ShooterParams params)
 
 void wom::subsystems::Shooter::OnUpdate(units::second_t dt) {
   units::volt_t voltage{0};
-  units::revolutions_per_minute_t currentSpeed =
-      _params.gearbox.encoder->GetEncoderAngularVelocity();
+  units::revolutions_per_minute_t currentSpeed = _params.gearbox.encoder->GetEncoderAngularVelocity();
 
   switch (_state) {
     case ShooterState::kManual:
       voltage = _setpointManual;
       break;
     case ShooterState::kPID: {
-      auto feedforward =
-          _params.gearbox.motor.Voltage(0_Nm, _pid.GetSetpoint());
+      auto feedforward = _params.gearbox.motor.Voltage(0_Nm, _pid.GetSetpoint());
       voltage = _pid.Calculate(currentSpeed, dt, feedforward);
     } break;
     case ShooterState::kIdle:
@@ -31,20 +29,17 @@ void wom::subsystems::Shooter::OnUpdate(units::second_t dt) {
       break;
   }
 
-  units::newton_meter_t max_torque_at_current_limit =
-      _params.gearbox.motor.Torque(_params.currentLimit);
+  units::newton_meter_t max_torque_at_current_limit = _params.gearbox.motor.Torque(_params.currentLimit);
   units::volt_t max_voltage_for_current_limit =
       _params.gearbox.motor.Voltage(max_torque_at_current_limit, currentSpeed);
 
-  voltage =
-      1_V * std::min(voltage.value(), max_voltage_for_current_limit.value());
+  voltage = 1_V * std::min(voltage.value(), max_voltage_for_current_limit.value());
 
   // _params.gearbox.motorController->SetVoltage(voltage);
 
   _table->GetEntry("output_volts").SetDouble(voltage.value());
   _table->GetEntry("speed_rpm").SetDouble(currentSpeed.value());
-  _table->GetEntry("setpoint_rpm")
-      .SetDouble(units::revolutions_per_minute_t{_pid.GetSetpoint()}.value());
+  _table->GetEntry("setpoint_rpm").SetDouble(units::revolutions_per_minute_t{_pid.GetSetpoint()}.value());
   _table->GetEntry("stable").SetBoolean(_pid.IsStable());
 }
 
@@ -67,8 +62,7 @@ bool wom::subsystems::Shooter::IsStable() const {
 
 // Shooter Manual Set
 
-wom::subsystems::ShooterConstant::ShooterConstant(Shooter* s,
-                                                  units::volt_t setpoint)
+wom::subsystems::ShooterConstant::ShooterConstant(Shooter* s, units::volt_t setpoint)
     : _shooter(s), _setpoint(setpoint) {
   Controls(_shooter);
 }
@@ -79,9 +73,7 @@ void wom::subsystems::ShooterConstant::OnTick(units::second_t dt) {
 
 // ShooterSpinup
 
-wom::subsystems::ShooterSpinup::ShooterSpinup(Shooter* s,
-                                              units::radians_per_second_t speed,
-                                              bool hold)
+wom::subsystems::ShooterSpinup::ShooterSpinup(Shooter* s, units::radians_per_second_t speed, bool hold)
     : _shooter(s), _speed(speed), _hold(hold) {
   Controls(_shooter);
 }
diff --git a/wombat/src/main/cpp/utils/Encoder.cpp b/wombat/src/main/cpp/utils/Encoder.cpp
index e5d6ab2b..461a8f31 100644
--- a/wombat/src/main/cpp/utils/Encoder.cpp
+++ b/wombat/src/main/cpp/utils/Encoder.cpp
@@ -6,8 +6,8 @@
 
 #include <rev/SparkRelativeEncoder.h>
 
-wom::utils::Encoder::Encoder(double encoderTicksPerRotation, int type,
-                             units::meter_t wheelRadius, double reduction)
+wom::utils::Encoder::Encoder(double encoderTicksPerRotation, int type, units::meter_t wheelRadius,
+                             double reduction)
     : _reduction(reduction),
       _encoderTicksPerRotation(encoderTicksPerRotation),
       _type(type),
@@ -43,16 +43,16 @@ void wom::utils::Encoder::SetReduction(double reduction) {
 }
 
 units::radian_t wom::utils::Encoder::GetEncoderPosition() {
-  //if (_type == 0) {
-  //  units::turn_t n_turns{GetEncoderTicks() / GetEncoderTicksPerRotation()};
-  //  return n_turns;
-  //} else if (_type == 2) {
-  //  units::degree_t pos = GetEncoderTicks() * 1_deg;
-  //  return pos;
-  //} else {
-  //  units::degree_t pos = GetEncoderTicks() * 1_deg;
-  //  return pos - _offset;
-  //}
+  // if (_type == 0) {
+  //   units::turn_t n_turns{GetEncoderTicks() / GetEncoderTicksPerRotation()};
+  //   return n_turns;
+  // } else if (_type == 2) {
+  //   units::degree_t pos = GetEncoderTicks() * 1_deg;
+  //   return pos;
+  // } else {
+  //   units::degree_t pos = GetEncoderTicks() * 1_deg;
+  //   return pos - _offset;
+  // }
   return GetEncoderTicks() * 1_rad;
 }
 
@@ -63,8 +63,7 @@ double wom::utils::Encoder::GetEncoderDistance() {
 units::radians_per_second_t wom::utils::Encoder::GetEncoderAngularVelocity() {
   // return GetEncoderTickVelocity() / (double)GetEncoderTicksPerRotation() * 2
   // * 3.1415926;
-  units::turns_per_second_t n_turns_per_s{GetEncoderTickVelocity() /
-                                          GetEncoderTicksPerRotation()};
+  units::turns_per_second_t n_turns_per_s{GetEncoderTickVelocity() / GetEncoderTicksPerRotation()};
   return n_turns_per_s;
 }
 
@@ -86,12 +85,10 @@ double wom::utils::DigitalEncoder::GetEncoderTickVelocity() const {
   return _nativeEncoder.GetRate();
 }
 
-wom::utils::CANSparkMaxEncoder::CANSparkMaxEncoder(rev::CANSparkMax* controller,
-                                                   units::meter_t wheelRadius,
+wom::utils::CANSparkMaxEncoder::CANSparkMaxEncoder(rev::CANSparkMax* controller, units::meter_t wheelRadius,
                                                    double reduction)
     : wom::utils::Encoder(42, reduction, wheelRadius, 2),
-      _encoder(controller->GetEncoder(
-          rev::SparkRelativeEncoder::Type::kQuadrature)) {}
+      _encoder(controller->GetEncoder(rev::SparkRelativeEncoder::Type::kQuadrature)) {}
 
 double wom::utils::CANSparkMaxEncoder::GetEncoderRawTicks() const {
   return _encoder.GetPosition() * _reduction;
@@ -121,11 +118,9 @@ double wom::utils::CANSparkMaxEncoder::GetVelocity() const {
   return _encoder.GetVelocity();
 }
 
-wom::utils::TalonFXEncoder::TalonFXEncoder(
-    ctre::phoenix6::hardware::TalonFX* controller, units::meter_t wheelRadius,
-    double reduction)
-    : utils::Encoder(2048, reduction, wheelRadius, 0),
-      _controller(controller) {}
+wom::utils::TalonFXEncoder::TalonFXEncoder(ctre::phoenix6::hardware::TalonFX* controller,
+                                           units::meter_t wheelRadius, double reduction)
+    : utils::Encoder(2048, reduction, wheelRadius, 0), _controller(controller) {}
 
 double wom::utils::TalonFXEncoder::GetEncoderRawTicks() const {
   return _controller->GetPosition().GetValue().value();
@@ -135,12 +130,9 @@ double wom::utils::TalonFXEncoder::GetEncoderTickVelocity() const {
   return _controller->Get() * 10;
 }
 
-wom::utils::DutyCycleEncoder::DutyCycleEncoder(int channel,
-                                               units::meter_t wheelRadius,
-                                               double ticksPerRotation,
-                                               double reduction)
-    : wom::utils::Encoder(ticksPerRotation, reduction, wheelRadius, 0),
-      _dutyCycleEncoder(channel) {}
+wom::utils::DutyCycleEncoder::DutyCycleEncoder(int channel, units::meter_t wheelRadius,
+                                               double ticksPerRotation, double reduction)
+    : wom::utils::Encoder(ticksPerRotation, reduction, wheelRadius, 0), _dutyCycleEncoder(channel) {}
 
 double wom::utils::DutyCycleEncoder::GetEncoderRawTicks() const {
   return _dutyCycleEncoder.Get().value();
@@ -150,9 +142,8 @@ double wom::utils::DutyCycleEncoder::GetEncoderTickVelocity() const {
   return 0;
 }
 
-wom::utils::CanEncoder::CanEncoder(int deviceNumber, units::meter_t wheelRadius,
-                                   double ticksPerRotation, double reduction,
-                                   std::string name)
+wom::utils::CanEncoder::CanEncoder(int deviceNumber, units::meter_t wheelRadius, double ticksPerRotation,
+                                   double reduction, std::string name)
     : wom::utils::Encoder(ticksPerRotation, 2, wheelRadius, reduction) {
   _canEncoder = new ctre::phoenix6::hardware::CANcoder(deviceNumber, name);
 }
diff --git a/wombat/src/main/cpp/utils/Util.cpp b/wombat/src/main/cpp/utils/Util.cpp
index e529e245..dbdbb642 100644
--- a/wombat/src/main/cpp/utils/Util.cpp
+++ b/wombat/src/main/cpp/utils/Util.cpp
@@ -13,21 +13,18 @@ units::second_t wom::utils::now() {
   return static_cast<double>(now) / 1000000 * 1_s;
 }
 
-void wom::utils::WritePose2NT(std::shared_ptr<nt::NetworkTable> table,
-                              frc::Pose2d pose) {
+void wom::utils::WritePose2NT(std::shared_ptr<nt::NetworkTable> table, frc::Pose2d pose) {
   table->GetEntry("x").SetDouble(pose.X().value());
   table->GetEntry("y").SetDouble(pose.Y().value());
   table->GetEntry("angle").SetDouble(pose.Rotation().Degrees().value());
 }
 
-void wom::utils::WritePose3NT(std::shared_ptr<nt::NetworkTable> table,
-                              frc::Pose3d pose) {
+void wom::utils::WritePose3NT(std::shared_ptr<nt::NetworkTable> table, frc::Pose3d pose) {
   table->GetEntry("x").SetDouble(pose.X().value());
   table->GetEntry("y").SetDouble(pose.Y().value());
   table->GetEntry("z").SetDouble(pose.Z().value());
 
-  table->GetEntry("angle").SetDouble(
-      pose.Rotation().Z().convert<units::degree>().value());
+  table->GetEntry("angle").SetDouble(pose.Rotation().Z().convert<units::degree>().value());
 }
 
 double wom::utils::deadzone(double val, double deadzone) {
@@ -50,32 +47,24 @@ units::volt_t wom::utils::GetVoltage(frc::MotorController* controller) {
   return controller->Get() * frc::RobotController::GetBatteryVoltage();
 }
 
-void wom::utils::WriteTrajectory(std::shared_ptr<nt::NetworkTable> table,
-                                 frc::Trajectory trajectory) {
+void wom::utils::WriteTrajectory(std::shared_ptr<nt::NetworkTable> table, frc::Trajectory trajectory) {
   table->GetEntry("length").SetDouble(trajectory.TotalTime().value());
 
   // write the trajectory to the network table
   int i = 0;
   for (auto state : trajectory.States()) {
-    table->GetSubTable(std::to_string(i))
-        ->GetEntry("x")
-        .SetDouble(state.pose.X().value());
-    table->GetSubTable(std::to_string(i))
-        ->GetEntry("y")
-        .SetDouble(state.pose.Y().value());
+    table->GetSubTable(std::to_string(i))->GetEntry("x").SetDouble(state.pose.X().value());
+    table->GetSubTable(std::to_string(i))->GetEntry("y").SetDouble(state.pose.Y().value());
     table->GetSubTable(std::to_string(i))
         ->GetEntry("angle")
         .SetDouble(state.pose.Rotation().Degrees().value());
-    table->GetSubTable(std::to_string(i))
-        ->GetEntry("time")
-        .SetDouble(state.t.value());
+    table->GetSubTable(std::to_string(i))->GetEntry("time").SetDouble(state.t.value());
 
     i++;
   }
 }
 
-void wom::utils::WriteTrajectoryState(std::shared_ptr<nt::NetworkTable> table,
-                                      frc::Trajectory::State state) {
+void wom::utils::WriteTrajectoryState(std::shared_ptr<nt::NetworkTable> table, frc::Trajectory::State state) {
   table->GetEntry("x").SetDouble(state.pose.X().value());
   table->GetEntry("y").SetDouble(state.pose.Y().value());
   table->GetEntry("angle").SetDouble(state.pose.Rotation().Degrees().value());
diff --git a/wombat/src/main/cpp/vision/Limelight.cpp b/wombat/src/main/cpp/vision/Limelight.cpp
index b37148ce..906f92cb 100644
--- a/wombat/src/main/cpp/vision/Limelight.cpp
+++ b/wombat/src/main/cpp/vision/Limelight.cpp
@@ -8,8 +8,7 @@
 
 #include "utils/Util.h"
 
-wom::vision::Limelight::Limelight(std::string limelightName)
-    : _limelightName(limelightName) {}
+wom::vision::Limelight::Limelight(std::string limelightName) : _limelightName(limelightName) {}
 
 std::string wom::vision::Limelight::GetName() {
   return _limelightName;
@@ -24,8 +23,7 @@ std::pair<double, double> wom::vision::Limelight::GetOffset() {
   return offset;
 }
 
-std::vector<double> wom::vision::Limelight::GetAprilTagData(
-    LimelightAprilTagData dataType) {
+std::vector<double> wom::vision::Limelight::GetAprilTagData(LimelightAprilTagData dataType) {
   std::string dataName;
 
   switch (dataType) {
@@ -69,8 +67,7 @@ std::vector<double> wom::vision::Limelight::GetAprilTagData(
   return table->GetNumberArray(dataName, std::vector<double>(6));
 }
 
-double wom::vision::Limelight::GetTargetingData(LimelightTargetingData dataType,
-                                                double defaultValue) {
+double wom::vision::Limelight::GetTargetingData(LimelightTargetingData dataType, double defaultValue) {
   std::string dataName;
 
   switch (dataType) {
@@ -158,8 +155,8 @@ void wom::vision::Limelight::SetCrop(std::array<double, 4> crop) {
   table->PutNumberArray("camtran", crop);
 }
 
-units::meters_per_second_t wom::vision::Limelight::GetSpeed(
-    frc::Pose3d pose1, frc::Pose3d pose2, units::second_t dt) {
+units::meters_per_second_t wom::vision::Limelight::GetSpeed(frc::Pose3d pose1, frc::Pose3d pose2,
+                                                            units::second_t dt) {
   frc::Transform3d dPose{pose1, pose2};
   frc::Translation3d dTranslation = dPose.Translation();
 
@@ -172,9 +169,8 @@ units::meters_per_second_t wom::vision::Limelight::GetSpeed(
 
 frc::Pose3d wom::vision::Limelight::GetPose() {
   std::vector<double> pose = GetAprilTagData(LimelightAprilTagData::kBotpose);
-  return frc::Pose3d(
-      pose[1] * 1_m, 1_m * pose[2], 1_m * pose[3],
-      frc::Rotation3d(1_deg * (pose[4]), 1_deg * (pose[5]), 1_deg * (pose[6])));
+  return frc::Pose3d(pose[1] * 1_m, 1_m * pose[2], 1_m * pose[3],
+                     frc::Rotation3d(1_deg * (pose[4]), 1_deg * (pose[5]), 1_deg * (pose[6])));
 }
 
 void wom::vision::Limelight::OnStart() {
@@ -185,11 +181,9 @@ void wom::vision::Limelight::OnUpdate(units::time::second_t dt) {
   wom::utils::WritePose3NT(table, GetPose());
 }
 
-bool wom::vision::Limelight::IsAtSetPoseVision(frc::Pose3d pose,
-                                               units::second_t dt) {
+bool wom::vision::Limelight::IsAtSetPoseVision(frc::Pose3d pose, units::second_t dt) {
   frc::Pose3d actualPose = GetPose();
   frc::Pose3d relativePose = actualPose.RelativeTo(pose);
-  return (units::math::fabs(relativePose.X()) < 0.01_m &&
-          units::math::fabs(relativePose.Y()) < 0.01_m &&
+  return (units::math::fabs(relativePose.X()) < 0.01_m && units::math::fabs(relativePose.Y()) < 0.01_m &&
           GetSpeed(pose, GetPose(), dt) < 1_m / 1_s);
 }
diff --git a/wombat/src/main/include/behaviour/Behaviour.h b/wombat/src/main/include/behaviour/Behaviour.h
index 4f2e357e..84ac8689 100644
--- a/wombat/src/main/include/behaviour/Behaviour.h
+++ b/wombat/src/main/include/behaviour/Behaviour.h
@@ -22,13 +22,7 @@
 #include "HasBehaviour.h"
 
 namespace behaviour {
-enum class BehaviourState {
-  INITIALISED,
-  RUNNING,
-  DONE,
-  TIMED_OUT,
-  INTERRUPTED
-};
+enum class BehaviourState { INITIALISED, RUNNING, DONE, TIMED_OUT, INTERRUPTED };
 
 class SequentialBehaviour;
 
@@ -46,8 +40,7 @@ class Behaviour : public std::enable_shared_from_this<Behaviour> {
  public:
   using ptr = std::shared_ptr<Behaviour>;
 
-  Behaviour(std::string name = "<unnamed behaviour>",
-            units::time::second_t period = 20_ms);
+  explicit Behaviour(std::string name = "<unnamed behaviour>", units::time::second_t period = 20_ms);
   ~Behaviour();
 
   /**
@@ -193,16 +186,15 @@ class SequentialBehaviour : public Behaviour {
   std::deque<ptr> _queue;
 };
 
-inline std::shared_ptr<SequentialBehaviour> operator<<(Behaviour::ptr a,
-                                                       Behaviour::ptr b) {
+inline std::shared_ptr<SequentialBehaviour> operator<<(Behaviour::ptr a, Behaviour::ptr b) {
   auto seq = std::make_shared<SequentialBehaviour>();
   seq->Add(a);
   seq->Add(b);
   return seq;
 }
 
-inline std::shared_ptr<SequentialBehaviour> operator<<(
-    std::shared_ptr<SequentialBehaviour> a, Behaviour::ptr b) {
+inline std::shared_ptr<SequentialBehaviour> operator<<(std::shared_ptr<SequentialBehaviour> a,
+                                                       Behaviour::ptr b) {
   a->Add(b);
   return a;
 }
@@ -247,10 +239,8 @@ class ConcurrentBehaviour : public Behaviour {
  * Create a concurrent behaviour group, waiting for all behaviours
  * to finish before moving on.
  */
-inline std::shared_ptr<ConcurrentBehaviour> operator&(Behaviour::ptr a,
-                                                      Behaviour::ptr b) {
-  auto conc =
-      std::make_shared<ConcurrentBehaviour>(ConcurrentBehaviourReducer::ALL);
+inline std::shared_ptr<ConcurrentBehaviour> operator&(Behaviour::ptr a, Behaviour::ptr b) {
+  auto conc = std::make_shared<ConcurrentBehaviour>(ConcurrentBehaviourReducer::ALL);
   conc->Add(a);
   conc->Add(b);
   return conc;
@@ -261,10 +251,8 @@ inline std::shared_ptr<ConcurrentBehaviour> operator&(Behaviour::ptr a,
  * be interrupted as soon as any members of the group are finished (the
  * behaviours are 'raced' against each other).
  */
-inline std::shared_ptr<ConcurrentBehaviour> operator|(Behaviour::ptr a,
-                                                      Behaviour::ptr b) {
-  auto conc =
-      std::make_shared<ConcurrentBehaviour>(ConcurrentBehaviourReducer::ANY);
+inline std::shared_ptr<ConcurrentBehaviour> operator|(Behaviour::ptr a, Behaviour::ptr b) {
+  auto conc = std::make_shared<ConcurrentBehaviour>(ConcurrentBehaviourReducer::ANY);
   conc->Add(a);
   conc->Add(b);
   return conc;
@@ -332,8 +320,7 @@ struct Switch : public Behaviour {
    * @param condition The function yielding true if this is the correct option
    * @param b The behaviour to call if this option is provided.
    */
-  std::shared_ptr<Switch> When(std::function<bool(T&)> condition,
-                               Behaviour::ptr b) {
+  std::shared_ptr<Switch> When(std::function<bool(T&)> condition, Behaviour::ptr b) {
     _options.push_back(std::pair(condition, b));
     Inherit(*b);
     return std::reinterpret_pointer_cast<Switch<T>>(shared_from_this());
@@ -389,8 +376,7 @@ struct Switch : public Behaviour {
 
  private:
   std::function<T()> _fn;
-  wpi::SmallVector<std::pair<std::function<bool(T&)>, Behaviour::ptr>, 4>
-      _options;
+  wpi::SmallVector<std::pair<std::function<bool(T&)>, Behaviour::ptr>, 4> _options;
   Behaviour::ptr _locked = nullptr;
 };
 
@@ -407,10 +393,8 @@ struct Decide : public Switch<std::monostate> {
    * @param condition The function yielding true if this is the correct option
    * @param b The behaviour to call if this option is provided.
    */
-  std::shared_ptr<Decide> When(std::function<bool()> condition,
-                               Behaviour::ptr b) {
-    return std::reinterpret_pointer_cast<Decide>(
-        Switch::When([condition](auto) { return condition(); }, b));
+  std::shared_ptr<Decide> When(std::function<bool()> condition, Behaviour::ptr b) {
+    return std::reinterpret_pointer_cast<Decide>(Switch::When([condition](auto) { return condition(); }, b));
   }
 };
 
diff --git a/wombat/src/main/include/behaviour/HasBehaviour.h b/wombat/src/main/include/behaviour/HasBehaviour.h
index c9b35b2e..bae5ac05 100644
--- a/wombat/src/main/include/behaviour/HasBehaviour.h
+++ b/wombat/src/main/include/behaviour/HasBehaviour.h
@@ -30,8 +30,7 @@ class HasBehaviour {
 
  protected:
   std::shared_ptr<Behaviour> _active_behaviour{nullptr};
-  std::function<std::shared_ptr<Behaviour>(void)> _default_behaviour_producer{
-      nullptr};
+  std::function<std::shared_ptr<Behaviour>(void)> _default_behaviour_producer{nullptr};
 
  private:
   friend class BehaviourScheduler;
diff --git a/wombat/src/main/include/drivetrain/SwerveDrive.h b/wombat/src/main/include/drivetrain/SwerveDrive.h
index b48db5a5..be4467b1 100644
--- a/wombat/src/main/include/drivetrain/SwerveDrive.h
+++ b/wombat/src/main/include/drivetrain/SwerveDrive.h
@@ -4,9 +4,9 @@
 
 #pragma once
 
+#include <frc/controller/PIDController.h>
 #include <frc/estimator/SwerveDrivePoseEstimator.h>
 #include <frc/kinematics/SwerveDriveKinematics.h>
-#include <frc/controller/PIDController.h>
 #include <networktables/DoubleTopic.h>
 #include <networktables/NetworkTable.h>
 #include <networktables/NetworkTableInstance.h>
@@ -48,9 +48,8 @@ struct SwerveModuleConfig {
 
 class SwerveModule {
  public:
-  //using angle_pid_conf_t = utils::PIDConfig<units::radian, units::volt>;
-  using velocity_pid_conf_t =
-      utils::PIDConfig<units::meters_per_second, units::volt>;
+  // using angle_pid_conf_t = utils::PIDConfig<units::radian, units::volt>;
+  using velocity_pid_conf_t = utils::PIDConfig<units::meters_per_second, units::volt>;
 
   SwerveModule(std::string path, SwerveModuleConfig config,
                /*angle_pid_conf_t anglePID,*/ velocity_pid_conf_t velocityPID);
@@ -67,8 +66,7 @@ class SwerveModule {
 
   void SetZero(units::second_t dt);
   void SetIdle();
-  void SetPID(units::radian_t angle, units::meters_per_second_t speed,
-              units::second_t dt);
+  void SetPID(units::radian_t angle, units::meters_per_second_t speed, units::second_t dt);
   void SetZero();
   void SetVoltageLimit(units::volt_t driveModuleVoltageLimit);
 
@@ -84,7 +82,7 @@ class SwerveModule {
 
   const SwerveModuleConfig& GetConfig() const;
 
-  //utils::PIDController<units::radians, units::volt> _anglePIDController;
+  // utils::PIDController<units::radians, units::volt> _anglePIDController;
   frc::PIDController _anglePIDController;
 
  private:
@@ -108,20 +106,18 @@ class SwerveModule {
 struct SwerveDriveConfig {
   /*using pose_angle_conf_t =
       utils::PIDConfig<units::radian, units::radians_per_second>;*/
-  using pose_position_conf_t =
-      utils::PIDConfig<units::meter, units::meters_per_second>;
-  using balance_conf_t =
-      utils::PIDConfig<units::degree, units::meters_per_second>;
+  using pose_position_conf_t = utils::PIDConfig<units::meter, units::meters_per_second>;
+  using balance_conf_t = utils::PIDConfig<units::degree, units::meters_per_second>;
 
   std::string path;
-  //SwerveModule::angle_pid_conf_t anglePID;
+  // SwerveModule::angle_pid_conf_t anglePID;
   SwerveModule::velocity_pid_conf_t velocityPID;
 
   wpi::array<SwerveModuleConfig, 4> modules;
 
   ctre::phoenix6::hardware::Pigeon2* gyro;
 
-  //pose_angle_conf_t poseAnglePID;
+  // pose_angle_conf_t poseAnglePID;
   pose_position_conf_t posePositionPID;
 
   units::kilogram_t mass;
@@ -175,8 +171,7 @@ class SwerveDrive : public behaviour::HasBehaviour {
    * @brief This function switches the state to handle the robot's rotation
    * matching that of the joystick
    */
-  void RotateMatchJoystick(units::radian_t joystickAngle,
-                           FieldRelativeSpeeds speeds);
+  void RotateMatchJoystick(units::radian_t joystickAngle, FieldRelativeSpeeds speeds);
 
   void SetIdle();
 
@@ -186,8 +181,7 @@ class SwerveDrive : public behaviour::HasBehaviour {
   void SetFieldRelativeVelocity(FieldRelativeSpeeds speeds);
   void SetPose(frc::Pose2d pose);
   bool IsAtSetPose();
-  void SetIndividualTuning(int mod, units::radian_t angle,
-                           units::meters_per_second_t speed);
+  void SetIndividualTuning(int mod, units::radian_t angle, units::meters_per_second_t speed);
   void SetTuning(units::radian_t angle, units::meters_per_second_t speed);
   void SetZero();
   void SetVoltageLimit(units::volt_t driveVoltageLimit);
diff --git a/wombat/src/main/include/drivetrain/behaviours/SwerveBehaviours.h b/wombat/src/main/include/drivetrain/behaviours/SwerveBehaviours.h
index 5aa1972f..11400c4a 100644
--- a/wombat/src/main/include/drivetrain/behaviours/SwerveBehaviours.h
+++ b/wombat/src/main/include/drivetrain/behaviours/SwerveBehaviours.h
@@ -37,16 +37,14 @@ class ManualDrivebase : public behaviour::Behaviour {
    * A pointer to the controller that the driver has been allocated (the
    * allocated memory address that stores the "driver controller" object)
    */
-  ManualDrivebase(wom::drivetrain::SwerveDrive* swerveDrivebase,
-                  frc::XboxController* driverController);
+  ManualDrivebase(wom::drivetrain::SwerveDrive* swerveDrivebase, frc::XboxController* driverController);
 
   void OnTick(units::second_t deltaTime) override;
   /**
    * @brief This function handles all of the logic behind the tangent function,
    * to be able to calculate an angle between 0 andd 360 degrees, inclusively
    */
-  void CalculateRequestedAngle(double joystickX, double joystickY,
-                               units::degree_t defaultAngle);
+  void CalculateRequestedAngle(double joystickX, double joystickY, units::degree_t defaultAngle);
   void OnStart() override;
   void ResetMode();
 
@@ -72,8 +70,8 @@ class ManualDrivebase : public behaviour::Behaviour {
   const double turningDeadzone = 0.2;
 
   // Variables for solution to Anti-tip
-  double prevJoystickX, prevJoystickY, prevPrevJoystickX, prevPrevJoystickY,
-      usingJoystickXPos, usingJoystickYPos;
+  double prevJoystickX, prevJoystickY, prevPrevJoystickX, prevPrevJoystickY, usingJoystickXPos,
+      usingJoystickYPos;
   // The speed that the joystick must travel to activate averaging over previous
   // 3 joystick positions
   const double smoothingThreshold = 1;
@@ -126,8 +124,8 @@ class GoToPose : public behaviour::Behaviour {
 
 class FollowTrajectory : public behaviour::Behaviour {
  public:
-  FollowTrajectory(wom::drivetrain::SwerveDrive* swerve,
-                   wom::utils::Pathplanner* pathplanner, std::string path);
+  FollowTrajectory(wom::drivetrain::SwerveDrive* swerve, wom::utils::Pathplanner* pathplanner,
+                   std::string path);
 
   void OnTick(units::second_t dt) override;
 
@@ -184,8 +182,7 @@ class TempSimSwerveDrive {
 
 class AutoSwerveDrive {
  public:
-  AutoSwerveDrive(wom::drivetrain::SwerveDrive* swerve, frc::Timer* timer,
-                  frc::Field2d* field);
+  AutoSwerveDrive(wom::drivetrain::SwerveDrive* swerve, frc::Timer* timer, frc::Field2d* field);
 
   void OnUpdate();
 
diff --git a/wombat/src/main/include/subsystems/Arm.h b/wombat/src/main/include/subsystems/Arm.h
index 155fd4cd..8b91be72 100644
--- a/wombat/src/main/include/subsystems/Arm.h
+++ b/wombat/src/main/include/subsystems/Arm.h
@@ -68,8 +68,7 @@ class Arm : public behaviour::HasBehaviour {
   ArmConfig _config;
   ArmState _state = ArmState::kIdle;
   wom::utils::PIDController<units::radian, units::volt> _pid;
-  wom::utils::PIDController<units::radians_per_second, units::volt>
-      _velocityPID;
+  wom::utils::PIDController<units::radians_per_second, units::volt> _velocityPID;
 
   std::shared_ptr<nt::NetworkTable> _table;
 
diff --git a/wombat/src/main/include/subsystems/Shooter.h b/wombat/src/main/include/subsystems/Shooter.h
index 2bbf6f5e..e5ac219d 100644
--- a/wombat/src/main/include/subsystems/Shooter.h
+++ b/wombat/src/main/include/subsystems/Shooter.h
@@ -62,8 +62,7 @@ class ShooterConstant : public behaviour::Behaviour {
 
 class ShooterSpinup : public behaviour::Behaviour {
  public:
-  ShooterSpinup(Shooter* s, units::radians_per_second_t speed,
-                bool hold = false);
+  ShooterSpinup(Shooter* s, units::radians_per_second_t speed, bool hold = false);
 
   void OnTick(units::second_t dt) override;
 
diff --git a/wombat/src/main/include/utils/Encoder.h b/wombat/src/main/include/utils/Encoder.h
index b504e1c0..303b2fda 100644
--- a/wombat/src/main/include/utils/Encoder.h
+++ b/wombat/src/main/include/utils/Encoder.h
@@ -20,8 +20,7 @@ namespace wom {
 namespace utils {
 class Encoder {
  public:
-  Encoder(double encoderTicksPerRotation, int type, units::meter_t wheelRadius,
-          double reduction = 1.0);
+  Encoder(double encoderTicksPerRotation, int type, units::meter_t wheelRadius, double reduction = 1.0);
 
   virtual double GetEncoderRawTicks() const = 0;
   virtual double GetEncoderTickVelocity() const = 0;  // ticks/s
@@ -54,24 +53,23 @@ class Encoder {
 
 class DigitalEncoder : public Encoder {
  public:
-  DigitalEncoder(int channelA, int channelB, double ticksPerRotation,
-                 units::meter_t wheelRadius, double reduction = 1)
-      : Encoder(ticksPerRotation, reduction, wheelRadius, 0),
-        _nativeEncoder(channelA, channelB) {}
+  DigitalEncoder(int channelA, int channelB, double ticksPerRotation, units::meter_t wheelRadius,
+                 double reduction = 1)
+      : Encoder(ticksPerRotation, reduction, wheelRadius, 0), _nativeEncoder(channelA, channelB) {}
 
   double GetEncoderRawTicks() const override;
   double GetEncoderTickVelocity() const override;
 
   double GetPosition() const;
   double GetVelocity() const override;
+
  private:
   frc::Encoder _nativeEncoder;
 };
 
 class CANSparkMaxEncoder : public Encoder {
  public:
-  explicit CANSparkMaxEncoder(rev::CANSparkMax* controller,
-                              units::meter_t wheelRadius, double reduction = 1);
+  explicit CANSparkMaxEncoder(rev::CANSparkMax* controller, units::meter_t wheelRadius, double reduction = 1);
 
   double GetEncoderRawTicks() const override;
   double GetEncoderTickVelocity() const override;
@@ -86,8 +84,8 @@ class CANSparkMaxEncoder : public Encoder {
 
 class TalonFXEncoder : public Encoder {
  public:
-  TalonFXEncoder(ctre::phoenix6::hardware::TalonFX* controller,
-                 units::meter_t wheelRadius, double reduction = 1);
+  TalonFXEncoder(ctre::phoenix6::hardware::TalonFX* controller, units::meter_t wheelRadius,
+                 double reduction = 1);
 
   double GetEncoderRawTicks() const override;
   double GetEncoderTickVelocity() const override;
@@ -99,8 +97,8 @@ class TalonFXEncoder : public Encoder {
 
 class DutyCycleEncoder : public Encoder {
  public:
-  DutyCycleEncoder(int channel, units::meter_t wheelRadius,
-                   double ticksPerRotation = 1, double reduction = 1);
+  DutyCycleEncoder(int channel, units::meter_t wheelRadius, double ticksPerRotation = 1,
+                   double reduction = 1);
 
   double GetEncoderRawTicks() const override;
   double GetEncoderTickVelocity() const override;
@@ -112,9 +110,8 @@ class DutyCycleEncoder : public Encoder {
 
 class CanEncoder : public Encoder {
  public:
-  CanEncoder(int deviceNumber, units::meter_t wheelRadius,
-             double ticksPerRotation = 4095, double reduction = 6.75,
-             std::string name = "Drivebase");
+  CanEncoder(int deviceNumber, units::meter_t wheelRadius, double ticksPerRotation = 4095,
+             double reduction = 6.75, std::string name = "Drivebase");
 
   double GetEncoderRawTicks() const override;
   double GetEncoderTickVelocity() const override;
diff --git a/wombat/src/main/include/utils/PID.h b/wombat/src/main/include/utils/PID.h
index 2ce378ec..cec94b1d 100644
--- a/wombat/src/main/include/utils/PID.h
+++ b/wombat/src/main/include/utils/PID.h
@@ -24,19 +24,15 @@ struct PIDConfig {
   using in_t = units::unit_t<IN>;
 
   using kp_t = units::unit_t<units::compound_unit<OUT, units::inverse<IN>>>;
-  using ki_t =
-      units::unit_t<units::compound_unit<OUT, units::inverse<IN>,
-                                         units::inverse<units::second>>>;
-  using kd_t = units::unit_t<
-      units::compound_unit<OUT, units::inverse<IN>, units::second>>;
+  using ki_t = units::unit_t<units::compound_unit<OUT, units::inverse<IN>, units::inverse<units::second>>>;
+  using kd_t = units::unit_t<units::compound_unit<OUT, units::inverse<IN>, units::second>>;
 
   using error_t = units::unit_t<IN>;
-  using deriv_t =
-      units::unit_t<units::compound_unit<IN, units::inverse<units::second>>>;
+  using deriv_t = units::unit_t<units::compound_unit<IN, units::inverse<units::second>>>;
 
-  PIDConfig(std::string path, kp_t kp = kp_t{0}, ki_t ki = ki_t{0},
-            kd_t kd = kd_t{0}, error_t stableThresh = error_t{-1},
-            deriv_t stableDerivThresh = deriv_t{-1}, in_t izone = in_t{-1})
+  PIDConfig(std::string path, kp_t kp = kp_t{0}, ki_t ki = ki_t{0}, kd_t kd = kd_t{0},
+            error_t stableThresh = error_t{-1}, deriv_t stableDerivThresh = deriv_t{-1},
+            in_t izone = in_t{-1})
       : path(path),
         kp(kp),
         ki(ki),
@@ -64,23 +60,14 @@ struct PIDConfig {
  public:
   void RegisterNT() {
     auto table = nt::NetworkTableInstance::GetDefault().GetTable(path);
+    _nt_bindings.emplace_back(std::make_shared<NTBoundUnit<typename kp_t::unit_type>>(table, "kP", kp));
+    _nt_bindings.emplace_back(std::make_shared<NTBoundUnit<typename ki_t::unit_type>>(table, "kI", ki));
+    _nt_bindings.emplace_back(std::make_shared<NTBoundUnit<typename kd_t::unit_type>>(table, "kD", kd));
     _nt_bindings.emplace_back(
-        std::make_shared<NTBoundUnit<typename kp_t::unit_type>>(table, "kP",
-                                                                kp));
-    _nt_bindings.emplace_back(
-        std::make_shared<NTBoundUnit<typename ki_t::unit_type>>(table, "kI",
-                                                                ki));
-    _nt_bindings.emplace_back(
-        std::make_shared<NTBoundUnit<typename kd_t::unit_type>>(table, "kD",
-                                                                kd));
-    _nt_bindings.emplace_back(
-        std::make_shared<NTBoundUnit<typename error_t::unit_type>>(
-            table, "stableThresh", stableThresh));
-    _nt_bindings.emplace_back(
-        std::make_shared<NTBoundUnit<typename deriv_t::unit_type>>(
-            table, "stableThreshVelocity", stableDerivThresh));
-    _nt_bindings.emplace_back(
-        std::make_shared<NTBoundUnit<IN>>(table, "izone", izone));
+        std::make_shared<NTBoundUnit<typename error_t::unit_type>>(table, "stableThresh", stableThresh));
+    _nt_bindings.emplace_back(std::make_shared<NTBoundUnit<typename deriv_t::unit_type>>(
+        table, "stableThreshVelocity", stableDerivThresh));
+    _nt_bindings.emplace_back(std::make_shared<NTBoundUnit<IN>>(table, "izone", izone));
   }
 };
 
@@ -94,19 +81,15 @@ class PIDController {
 
   config_t config;
 
-  PIDController(std::string path, config_t initialGains,
-                in_t setpoint = in_t{0})
+  PIDController(std::string path, config_t initialGains, in_t setpoint = in_t{0})
       : config(initialGains),
         _setpoint(setpoint),
-        _posFilter(
-            frc::LinearFilter<typename config_t::error_t>::MovingAverage(20)),
-        _velFilter(
-            frc::LinearFilter<typename config_t::deriv_t>::MovingAverage(20)),
+        _posFilter(frc::LinearFilter<typename config_t::error_t>::MovingAverage(20)),
+        _velFilter(frc::LinearFilter<typename config_t::deriv_t>::MovingAverage(20)),
         _table(nt::NetworkTableInstance::GetDefault().GetTable(path)) {}
 
   void SetSetpoint(in_t setpoint) {
-    if (std::abs(setpoint.value() - _setpoint.value()) >
-        std::abs(0.1 * _setpoint.value())) {
+    if (std::abs(setpoint.value() - _setpoint.value()) > std::abs(0.1 * _setpoint.value())) {
       _iterations = 0;
     }
     _setpoint = setpoint;
@@ -126,8 +109,7 @@ class PIDController {
     error = units::math::fabs(error);
     _integralSum += error * dt;
     _integralSum = units::math::fabs(_integralSum);
-    if (config.izone.value() > 0 &&
-        (error > config.izone || error < -config.izone))
+    if (config.izone.value() > 0 && (error > config.izone || error < -config.izone))
       _integralSum = sum_t{0};
 
     typename config_t::deriv_t deriv{0};
@@ -138,8 +120,7 @@ class PIDController {
     _stablePos = _posFilter.Calculate(error);
     _stableVel = _velFilter.Calculate(deriv);
 
-    auto out = config.kp * error + config.ki * _integralSum +
-               config.kd * deriv + feedforward;
+    auto out = config.kp * error + config.ki * _integralSum + config.kd * deriv + feedforward;
 
     _last_pv = pv;
     _last_error = error;
@@ -147,10 +128,8 @@ class PIDController {
     return out;
   }
 
-  bool IsStable(
-      std::optional<typename config_t::error_t> stableThreshOverride = {},
-      std::optional<typename config_t::deriv_t> velocityThreshOverride = {})
-      const {
+  bool IsStable(std::optional<typename config_t::error_t> stableThreshOverride = {},
+                std::optional<typename config_t::deriv_t> velocityThreshOverride = {}) const {
     auto stableThresh = config.stableThresh;
     auto stableDerivThresh = config.stableDerivThresh;
 
@@ -159,10 +138,8 @@ class PIDController {
     if (velocityThreshOverride.has_value())
       stableDerivThresh = velocityThreshOverride.value();
 
-    return _iterations > 20 &&
-           std::abs(_stablePos.value()) <= std::abs(stableThresh.value()) &&
-           (stableDerivThresh.value() < 0 ||
-            std::abs(_stableVel.value()) <= stableDerivThresh.value());
+    return _iterations > 20 && std::abs(_stablePos.value()) <= std::abs(stableThresh.value()) &&
+           (stableDerivThresh.value() < 0 || std::abs(_stableVel.value()) <= stableDerivThresh.value());
   }
 
  private:
diff --git a/wombat/src/main/include/utils/Util.h b/wombat/src/main/include/utils/Util.h
index 665c85ec..d624e408 100644
--- a/wombat/src/main/include/utils/Util.h
+++ b/wombat/src/main/include/utils/Util.h
@@ -28,39 +28,30 @@ units::second_t now();
 
 class NTBound {
  public:
-  NTBound(std::shared_ptr<nt::NetworkTable> table, std::string name,
-          const nt::Value& value,
+  NTBound(std::shared_ptr<nt::NetworkTable> table, std::string name, const nt::Value& value,
           std::function<void(const nt::Value&)> onUpdateFn)
-      : _table(table),
-        _entry(table->GetEntry(name)),
-        _onUpdate(onUpdateFn),
-        _name(name) {
+      : _table(table), _entry(table->GetEntry(name)), _onUpdate(onUpdateFn), _name(name) {
     _entry.SetValue(value);
     // _listener = table->AddListener(name, , ([=](const nt::EntryNotification
     // &evt) {
     //   this->_onUpdate(evt.value);
     // }, NT_NOTIFY_UPDATE);
-    _listener = table->AddListener(
-        name, nt::EventFlags::kValueAll,
-        ([this](nt::NetworkTable* table, std::string_view key,
-                const nt::Event& event) {
-          std::cout << "NT UPDATE" << std::endl;
-          this->_onUpdate(event.GetValueEventData()->value);
-        }));
+    _listener =
+        table->AddListener(name, nt::EventFlags::kValueAll,
+                           ([this](nt::NetworkTable* table, std::string_view key, const nt::Event& event) {
+                             std::cout << "NT UPDATE" << std::endl;
+                             this->_onUpdate(event.GetValueEventData()->value);
+                           }));
   }
 
   NTBound(const NTBound& other)
-      : _table(other._table),
-        _entry(other._entry),
-        _onUpdate(other._onUpdate),
-        _name(other._name) {
-    _listener = _table->AddListener(
-        _name, nt::EventFlags::kValueAll,
-        ([this](nt::NetworkTable* table, std::string_view key,
-                const nt::Event& event) {
-          std::cout << "NT UPDATE" << std::endl;
-          this->_onUpdate(event.GetValueEventData()->value);
-        }));
+      : _table(other._table), _entry(other._entry), _onUpdate(other._onUpdate), _name(other._name) {
+    _listener =
+        _table->AddListener(_name, nt::EventFlags::kValueAll,
+                            ([this](nt::NetworkTable* table, std::string_view key, const nt::Event& event) {
+                              std::cout << "NT UPDATE" << std::endl;
+                              this->_onUpdate(event.GetValueEventData()->value);
+                            }));
   }
 
   ~NTBound() { _table->RemoveListener(_listener); }
@@ -75,8 +66,7 @@ class NTBound {
 
 class NTBoundDouble : public NTBound {
  public:
-  NTBoundDouble(std::shared_ptr<nt::NetworkTable> table, std::string name,
-                double& val)
+  NTBoundDouble(std::shared_ptr<nt::NetworkTable> table, std::string name, double& val)
       : NTBound(table, name, nt::Value::MakeDouble(val),
                 [&val](const nt::Value& v) { val = v.GetDouble(); }) {}
 };
@@ -84,20 +74,15 @@ class NTBoundDouble : public NTBound {
 template <typename T>
 class NTBoundUnit : public NTBound {
  public:
-  NTBoundUnit(std::shared_ptr<nt::NetworkTable> table, std::string name,
-              units::unit_t<T>& val)
+  NTBoundUnit(std::shared_ptr<nt::NetworkTable> table, std::string name, units::unit_t<T>& val)
       : NTBound(table, name, nt::Value::MakeDouble(val.value()),
-                [&val](const nt::Value& v) {
-                  val = units::unit_t<T>{v.GetDouble()};
-                }) {}
+                [&val](const nt::Value& v) { val = units::unit_t<T>{v.GetDouble()}; }) {}
 };
 
 void WritePose2NT(std::shared_ptr<nt::NetworkTable> table, frc::Pose2d pose);
 void WritePose3NT(std::shared_ptr<nt::NetworkTable> table, frc::Pose3d pose);
-void WriteTrajectory(std::shared_ptr<nt::NetworkTable> table,
-                     frc::Trajectory trajectory);
-void WriteTrajectoryState(std::shared_ptr<nt::NetworkTable> table,
-                          frc::Trajectory::State state);
+void WriteTrajectory(std::shared_ptr<nt::NetworkTable> table, frc::Trajectory trajectory);
+void WriteTrajectoryState(std::shared_ptr<nt::NetworkTable> table, frc::Trajectory::State state);
 
 frc::Pose2d TrajectoryStateToPose2d(frc::Trajectory::State state);
 
diff --git a/wombat/src/main/include/vision/Limelight.h b/wombat/src/main/include/vision/Limelight.h
index ddfb3739..5aa6d0b7 100644
--- a/wombat/src/main/include/vision/Limelight.h
+++ b/wombat/src/main/include/vision/Limelight.h
@@ -26,20 +26,11 @@
 namespace wom {
 namespace vision {
 
-enum class LimelightLEDMode {
-  kPipelineDefault = 0,
-  kForceOff = 1,
-  kForceBlink = 2,
-  kForceOn = 3
-};
+enum class LimelightLEDMode { kPipelineDefault = 0, kForceOff = 1, kForceBlink = 2, kForceOn = 3 };
 
 enum class LimelightCamMode { kVisionProcessor = 0, kDriverCamera = 1 };
 
-enum class LimelightStreamMode {
-  kStandard = 0,
-  kPiPMain = 1,
-  kPiPSecondary = 2
-};
+enum class LimelightStreamMode { kStandard = 0, kPiPMain = 1, kPiPSecondary = 2 };
 
 enum class LimelightSnapshotMode { kReset = 0, kSingle = 1 };
 
@@ -91,14 +82,12 @@ class Limelight {
 
   std::string GetName();
 
-  std::shared_ptr<nt::NetworkTable> table =
-      nt::NetworkTableInstance::GetDefault().GetTable("limelight");
+  std::shared_ptr<nt::NetworkTable> table = nt::NetworkTableInstance::GetDefault().GetTable("limelight");
 
   std::pair<double, double> GetOffset();
 
   std::vector<double> GetAprilTagData(LimelightAprilTagData dataType);
-  double GetTargetingData(LimelightTargetingData dataType,
-                          double defaultValue = 0.0);
+  double GetTargetingData(LimelightTargetingData dataType, double defaultValue = 0.0);
   void SetLEDMode(LimelightLEDMode mode);
   void SetCamMode(LimelightCamMode mode);
   void SetPipeline(LimelightPipeline pipeline);
@@ -111,8 +100,7 @@ class Limelight {
 
   bool IsAtSetPoseVision(frc::Pose3d pose, units::second_t dt);
 
-  units::meters_per_second_t GetSpeed(frc::Pose3d pose1, frc::Pose3d pose2,
-                                      units::second_t dt);
+  units::meters_per_second_t GetSpeed(frc::Pose3d pose1, frc::Pose3d pose2, units::second_t dt);
 
   frc::Pose3d GetPose();
 

From cc34736f7a222afc10dcc01efbb15af33d9389a0 Mon Sep 17 00:00:00 2001
From: Isaac Turner <spacey_sooty@outlook.com>
Date: Wed, 24 Jan 2024 14:45:55 +0800
Subject: [PATCH 068/207] fix more stuff

---
 .github/workflows/format.yml |  2 +-
 gradlew                      |  0
 wombat/LICENSE               | 21 ---------------------
 wombat/README.md             |  1 -
 wombat/settings.gradle       |  1 -
 5 files changed, 1 insertion(+), 24 deletions(-)
 mode change 100644 => 100755 gradlew
 delete mode 100644 wombat/LICENSE
 delete mode 100644 wombat/README.md
 delete mode 100644 wombat/settings.gradle

diff --git a/.github/workflows/format.yml b/.github/workflows/format.yml
index c6fced20..f57ddeb4 100644
--- a/.github/workflows/format.yml
+++ b/.github/workflows/format.yml
@@ -21,7 +21,7 @@ jobs:
         with:
           python-version: 3.8
       - name: Install wpiformat
-        run: pip3 install wpiformat==2023.36
+        run: pip3 install wpiformat==2024.31
       - name: Run
         run: wpiformat
       - name: Check output
diff --git a/gradlew b/gradlew
old mode 100644
new mode 100755
diff --git a/wombat/LICENSE b/wombat/LICENSE
deleted file mode 100644
index b1561bab..00000000
--- a/wombat/LICENSE
+++ /dev/null
@@ -1,21 +0,0 @@
-MIT License
-
-Copyright (c) 2023 CurtinFRC
-
-Permission is hereby granted, free of charge, to any person obtaining a copy
-of this software and associated documentation files (the "Software"), to deal
-in the Software without restriction, including without limitation the rights
-to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
-copies of the Software, and to permit persons to whom the Software is
-furnished to do so, subject to the following conditions:
-
-The above copyright notice and this permission notice shall be included in all
-copies or substantial portions of the Software.
-
-THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
-IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
-FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
-AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
-LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
-OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
-SOFTWARE.
diff --git a/wombat/README.md b/wombat/README.md
deleted file mode 100644
index 0595c6cb..00000000
--- a/wombat/README.md
+++ /dev/null
@@ -1 +0,0 @@
-# Wombat
diff --git a/wombat/settings.gradle b/wombat/settings.gradle
deleted file mode 100644
index 8b137891..00000000
--- a/wombat/settings.gradle
+++ /dev/null
@@ -1 +0,0 @@
-

From d8f609fa2145df94639fb925e92caeac2e0056ba Mon Sep 17 00:00:00 2001
From: Sean Chan <sean.chan3@student.education.wa.edu.au>
Date: Wed, 24 Jan 2024 14:48:59 +0800
Subject: [PATCH 069/207] Started to add alpha code

---
 src/main/cpp/AlphaArm.cpp                     | 36 ---------
 src/main/cpp/Auto.cpp                         | 16 ++--
 src/main/cpp/Behaviours/AlphaArmBehaviour.cpp | 28 -------
 src/main/cpp/Behaviours/IntakeBehaviour.cpp   | 43 -----------
 src/main/cpp/Intake.cpp                       | 74 -------------------
 src/main/cpp/Robot.cpp                        | 12 ---
 src/main/include/AlphaArm.h                   | 33 ---------
 src/main/include/Auto.h                       | 21 +++---
 .../include/Behaviours/AlphaArmBehaviour.h    | 24 ------
 src/main/include/Behaviours/IntakeBehaviour.h | 34 ---------
 src/main/include/Intake.h                     | 41 ----------
 src/main/include/Robot.h                      |  6 --
 src/main/include/RobotMap.h                   | 34 +--------
 13 files changed, 20 insertions(+), 382 deletions(-)
 delete mode 100644 src/main/cpp/AlphaArm.cpp
 delete mode 100644 src/main/cpp/Behaviours/AlphaArmBehaviour.cpp
 delete mode 100644 src/main/cpp/Behaviours/IntakeBehaviour.cpp
 delete mode 100644 src/main/cpp/Intake.cpp
 delete mode 100644 src/main/include/AlphaArm.h
 delete mode 100644 src/main/include/Behaviours/AlphaArmBehaviour.h
 delete mode 100644 src/main/include/Behaviours/IntakeBehaviour.h
 delete mode 100644 src/main/include/Intake.h

diff --git a/src/main/cpp/AlphaArm.cpp b/src/main/cpp/AlphaArm.cpp
deleted file mode 100644
index a0066022..00000000
--- a/src/main/cpp/AlphaArm.cpp
+++ /dev/null
@@ -1,36 +0,0 @@
-// Copyright (c) 2023-2024 CurtinFRC
-// Open Source Software, you can modify it according to the terms
-// of the MIT License at the root of this project
-
-#include "AlphaArm.h"
-
-AlphaArm::AlphaArm(AlphaArmConfig config) : _config(config) {}
-
-void AlphaArm::OnUpdate(units::second_t dt) {
-  switch (_state) {
-    case AlphaArmState::kIdle:
-      // transmission translate
-      _config.alphaArmGearbox.motorController->SetVoltage(0_V);
-      _config.wristGearbox.motorController->SetVoltage(0_V);
-
-      break;
-    case AlphaArmState::kRaw:
-      setAlphaArmVoltage = _armVoltage;
-      break;
-    default:
-      std::cout << "oops, wrong state" << std::endl;
-      break;
-  }
-  // transmission translate
-  // _config.armGearBox.motorController->SetVoltage(setAlphaArmVoltage);
-  _config.alphaArmGearbox.motorController->SetVoltage(setAlphaArmVoltage);
-  _config.wristGearbox.motorController->SetVoltage(setWristVoltage);
-}
-
-void AlphaArm::SetState(AlphaArmState state) {
-  _state = state;
-}
-
-void AlphaArm::SetRaw(units::volt_t voltR) {
-  _voltR = voltR;
-}
diff --git a/src/main/cpp/Auto.cpp b/src/main/cpp/Auto.cpp
index 648b48ab..362a49d8 100644
--- a/src/main/cpp/Auto.cpp
+++ b/src/main/cpp/Auto.cpp
@@ -1,4 +1,4 @@
-#include "Auto.h"
+// #include "Auto.h"
 
 // std::shared_ptr<Behaviour> Taxi(DriveBase _driveBase, Shooter _shooter, Mag _mag, Intake _intake, Arm _arm) {
 //   return 
@@ -250,10 +250,10 @@
 //     */
 // }
 
-std::shared_ptr<Behaviour> AutoTest(DriveBase _driveBase, Shooter _shooter, Mag _mag, Intake _intake, Arm _arm) {
-  return
-    <<make<ArmToSetPoint>(_arm, 0, -90)
-    <<make<DriveToLocation>(_driveBase, raw, distance)
-    <<make<AutoShoot>(_shooter, 8_V)
-    <<make<AutoIntake>(_intake, 8_V)
-} // This auto is a test for auto to see if all things work, it does not build as the behaviours are not done.
\ No newline at end of file
+// std::shared_ptr<Behaviour> AutoTest(DriveBase _driveBase, Shooter _shooter, Mag _mag, Intake _intake, Arm _arm) {
+//   return
+//     <<make<ArmToSetPoint>(_arm, 0, -90)
+//     <<make<DriveToLocation>(_driveBase, raw, distance)
+//     <<make<AutoShoot>(_shooter, 8_V)
+//     <<make<AutoIntake>(_intake, 8_V)
+// } // This auto is a test for auto to see if all things work, it does not build as the behaviours are not done.
\ No newline at end of file
diff --git a/src/main/cpp/Behaviours/AlphaArmBehaviour.cpp b/src/main/cpp/Behaviours/AlphaArmBehaviour.cpp
deleted file mode 100644
index 85ee484a..00000000
--- a/src/main/cpp/Behaviours/AlphaArmBehaviour.cpp
+++ /dev/null
@@ -1,28 +0,0 @@
-// Copyright (c) 2023-2024 CurtinFRC
-// Open Source Software, you can modify it according to the terms
-// of the MIT License at the root of this project
-
-#include "Behaviours/AlphaArmBehaviour.h"
-
-#include <frc/XboxController.h>
-
-AlphaArmManualControl::AlphaArmManualControl(AlphaArm* alphaArm, frc::XboxController* codriver)
-    : _alphaArm(alphaArm), _codriver(codriver) {
-  Controls(alphaArm);
-}
-
-void AlphaArmManualControl::OnTick(units::second_t dt) {
-  if (_codriver->GetXButtonPressed()) {
-    if (_rawControl == true) {
-      _rawControl = false;
-    } else {
-      _rawControl = true;
-    }
-  }
-
-  if (_rawControl) {
-    _alphaArm->SetState(AlphaArmState::kRaw);
-    // mess around with _rightStick later
-    _alphaArm->SetRaw(_codriver->GetRightY() * 2_V);
-  }
-}
diff --git a/src/main/cpp/Behaviours/IntakeBehaviour.cpp b/src/main/cpp/Behaviours/IntakeBehaviour.cpp
deleted file mode 100644
index 9fcf777e..00000000
--- a/src/main/cpp/Behaviours/IntakeBehaviour.cpp
+++ /dev/null
@@ -1,43 +0,0 @@
-// Copyright (c) 2023-2024 CurtinFRC
-// Open Source Software, you can modify it according to the terms
-// of the MIT License at the root of this project
-
-#include "Behaviours/IntakeBehaviour.h"
-
-IntakeManualControl::IntakeManualControl(Intake* intake, frc::XboxController* codriver)
-    : _intake(intake), _codriver(codriver) {
-  Controls(intake);
-}
-
-void IntakeManualControl::OnTick(units::second_t dt) {
-  if (_codriver->GetBButtonPressed()) {
-    if (_rawControl == true) {
-      _rawControl = false;
-    } else {
-      _rawControl = true;
-    }
-  }
-
-  if (_rawControl) {
-    _intake->setState(IntakeState::kRaw);
-    _intake->setRaw(_codriver->GetLeftY() * 10_V);
-    std::cout << "Raw" << std::endl;
-  } else {
-    if (_codriver->GetYButtonPressed()) {
-      _intake->setState(IntakeState::kIntake);
-    }
-    if (_codriver->GetAButtonPressed()) {
-      _intake->setState(IntakeState::kPass);
-    }
-  }
-}
-
-IntakeAutoControl::IntakeAutoControl(Intake* intake) : _intake(intake) {}
-
-void IntakeAutoControl::OnTick(units::second_t dt) {
-  if (_intake->GetConfig().intakeSensor->Get() == 1) {
-    _intake->setState(IntakeState::kPass);
-  } else if (_intake->GetConfig().magSensor->Get() == 0) {
-    _intake->setState(IntakeState::kIdle);
-  }
-}
diff --git a/src/main/cpp/Intake.cpp b/src/main/cpp/Intake.cpp
deleted file mode 100644
index 75839209..00000000
--- a/src/main/cpp/Intake.cpp
+++ /dev/null
@@ -1,74 +0,0 @@
-// Copyright (c) 2023-2024 CurtinFRC
-// Open Source Software, you can modify it according to the terms
-// of the MIT License at the root of this project
-
-#include "Intake.h"
-
-Intake::Intake(IntakeConfig config) : _config(config) {}
-
-IntakeConfig Intake::GetConfig() {
-  return _config;
-}
-
-void Intake::OnUpdate(units::second_t dt) {
-  switch (_state) {
-    case IntakeState::kIdle: {
-      // _config.IntakeMotor.motorController->SetVoltage(0_V);
-      // if (_config.intakeSensor->Get()) {
-      //   setState(IntakeState::kHold);
-      // }
-      _stringStateName = "Idle";
-      _setVoltage = 0_V;
-    } break;
-    case IntakeState::kRaw: {
-      // _config.IntakeMotor.motorController->SetVoltage(_rawVoltage);
-      _stringStateName = "Raw";
-      _setVoltage = _rawVoltage;
-    } break;
-    case IntakeState::kEject: {
-      // _config.IntakeMotor.motorController->SetVoltage(-5_V);
-      // if (_config.intakeSensor->Get() == 0 && _config.magSensor->Get() == 0) {
-      //   setState(IntakeState::kIdle);
-      // }
-      _stringStateName = "Eject";
-      _setVoltage = -5_V;
-    } break;
-    case IntakeState::kHold: {
-      // _config.IntakeMotor.motorController->SetVoltage(0_V);
-      _stringStateName = "Hold";
-      _setVoltage = 0_V;
-    } break;
-    case IntakeState::kIntake: {
-      // _config.IntakeMotor.motorController->SetVoltage(5_V);
-      _stringStateName = "Intake";
-      _setVoltage = 5_V;
-    } break;
-    case IntakeState::kPass: {
-      // _config.IntakeMotor.motorController->SetVoltage(5_V);
-      // if (_config.shooterSensor->Get()) {
-      //   setState(IntakeState::kIdle);
-      //   _stringStateName = "Pass";
-      // }
-      _setVoltage = 5_V;
-    } break;
-    default:
-      std::cout << "Error: Intake in INVALID STATE." << std::endl;
-      break;
-  }
-  _table->GetEntry("State: ").SetString(_stringStateName);
-  _table->GetEntry("Motor Voltage: ").SetDouble(_setVoltage.value());
-  // _table->GetEntry("Intake Sensor: ").SetBoolean(_config.intakeSensor->Get());
-  // _table->GetEntry("Shooter Sensor: ").SetBoolean(_config.shooterSensor->Get());
-  // _table->GetEntry("Magazine Sensor: ").SetBoolean(_config.magSensor->Get());
-
-  std::cout << _setVoltage.value() << std::endl;
-
-  _config.IntakeMotor.motorController->SetVoltage(_setVoltage);
-}
-
-void Intake::setState(IntakeState state) {
-  _state = state;
-}
-void Intake::setRaw(units::volt_t voltage) {
-  _rawVoltage = voltage;
-}
diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp
index c577b449..8e3e99f6 100644
--- a/src/main/cpp/Robot.cpp
+++ b/src/main/cpp/Robot.cpp
@@ -34,8 +34,6 @@ void Robot::RobotPeriodic() {
   _swerveDrive->OnUpdate(dt);
   mag->OnUpdate(dt);
   climber->OnUpdate(dt);
-  intake->OnUpdate(dt);
-  alphaArm->OnUpdate(dt);
 }
 
 void Robot::TeleopInit() {
@@ -77,16 +75,6 @@ void Robot::TeleopInit() {
   mag->SetDefaultBehaviour(
       [this]() { return wom::make<MagManualControl>(mag, &robotmap.controllers.codriver); });
 
-  intake = new Intake(robotmap.intakeSystem.config);
-  wom::BehaviourScheduler::GetInstance()->Register(intake);
-  intake->SetDefaultBehaviour(
-      [this]() { return wom::make<IntakeManualControl>(intake, &robotmap.controllers.codriver); });
-
-  alphaArm = new AlphaArm(robotmap.alphaArmSystem.config);
-  wom::BehaviourScheduler::GetInstance()->Register(alphaArm);
-  alphaArm->SetDefaultBehaviour(
-      [this]() { return wom::make<AlphaArmManualControl>(alphaArm, &robotmap.controllers.codriver); });
-
   // m_driveSim = new wom::TempSimSwerveDrive(&simulation_timer, &m_field);
   // m_driveSim = wom::TempSimSwerveDrive();
 }
diff --git a/src/main/include/AlphaArm.h b/src/main/include/AlphaArm.h
deleted file mode 100644
index 2232b7fb..00000000
--- a/src/main/include/AlphaArm.h
+++ /dev/null
@@ -1,33 +0,0 @@
-// Copyright (c) 2023-2024 CurtinFRC
-// Open Source Software, you can modify it according to the terms
-// of the MIT License at the root of this project
-
-#pragma once
-#include <frc/DigitalInput.h>
-
-#include "Wombat.h"
-
-struct AlphaArmConfig {
-  // wom::Gearbox armGearBox;
-  wom::Gearbox alphaArmGearbox;
-  wom::Gearbox wristGearbox;
-};
-
-enum class AlphaArmState { kIdle, kIntakeAngle, kAmpAngle, kSpeakerAngle, kRaw };
-
-class AlphaArm : public ::behaviour::HasBehaviour {
- public:
-  explicit AlphaArm(AlphaArmConfig config);
-
-  void OnUpdate(units::second_t dt);
-  void SetRaw(units::volt_t volt);
-  void SetState(AlphaArmState state);
-
- private:
-  AlphaArmConfig _config;
-  AlphaArmState _state = AlphaArmState::kIdle;
-  units::volt_t setAlphaArmVoltage = 0_V;
-  units::volt_t setWristVoltage = 0_V;
-  units::volt_t _armVoltage;
-  units::volt_t _voltR;
-};
diff --git a/src/main/include/Auto.h b/src/main/include/Auto.h
index bd37e5ea..1bdafc0c 100644
--- a/src/main/include/Auto.h
+++ b/src/main/include/Auto.h
@@ -1,19 +1,20 @@
-#pragma once
 
-#include "Wombat.h"
+// #pragma once
 
-namespace autos {
-  std::shared_ptr<behaviour::Behaviour> AutoTest(Drivebase driveBase, Shooter shooter, Mag mag, Intake intake, Arm _arm);
+// #include "Wombat.h"
 
-  // std::shared_ptr<behaviour::Behaviour> Taxi(Drivebase driveBase, Shooter shooter, Mag mag, Intake intake, Arm _arm);
+// namespace autos {
+//   std::shared_ptr<behaviour::Behaviour> AutoTest(Drivebase driveBase, Shooter shooter, Mag mag, Intake intake, Arm _arm);
 
-  // std::shared_ptr<behaviour::Behaviour> QuadrupleClose(Drivebase driveBase, Shooter shooter, Mag mag, Intake intake, Arm _arm);
+//   // std::shared_ptr<behaviour::Behaviour> Taxi(Drivebase driveBase, Shooter shooter, Mag mag, Intake intake, Arm _arm);
 
-  // std::shared_ptr<behaviour::Behaviour> QuadrupleFar(Drivebase driveBase, Shooter shooter, Mag mag, Intake intake, Arm _arm);
+//   // std::shared_ptr<behaviour::Behaviour> QuadrupleClose(Drivebase driveBase, Shooter shooter, Mag mag, Intake intake, Arm _arm);
 
-  // std::shared_ptr<behaviour::Behaviour> QuadrupleCloseDoubleFar(Drivebase driveBase, Shooter shooter, Mag mag, Intake intake, Arm _arm);
+//   // std::shared_ptr<behaviour::Behaviour> QuadrupleFar(Drivebase driveBase, Shooter shooter, Mag mag, Intake intake, Arm _arm);
 
-  // std::shared_ptr<behaviour::Behaviour> QuadrupleCloseSingleFar(Drivebase driveBase, Shooter shooter, Mag mag, Intake intake, Arm _arm);
-}
+//   // std::shared_ptr<behaviour::Behaviour> QuadrupleCloseDoubleFar(Drivebase driveBase, Shooter shooter, Mag mag, Intake intake, Arm _arm);
+
+//   // std::shared_ptr<behaviour::Behaviour> QuadrupleCloseSingleFar(Drivebase driveBase, Shooter shooter, Mag mag, Intake intake, Arm _arm);
+// }
 
 
diff --git a/src/main/include/Behaviours/AlphaArmBehaviour.h b/src/main/include/Behaviours/AlphaArmBehaviour.h
deleted file mode 100644
index 4c2d03fb..00000000
--- a/src/main/include/Behaviours/AlphaArmBehaviour.h
+++ /dev/null
@@ -1,24 +0,0 @@
-// Copyright (c) 2023-2024 CurtinFRC
-// Open Source Software, you can modify it according to the terms
-// of the MIT License at the root of this project
-
-#pragma once
-
-#pragma once
-#include <frc/XboxController.h>
-
-#include "AlphaArm.h"
-#include "Wombat.h"
-
-class AlphaArmManualControl : public behaviour::Behaviour {
- public:
-  explicit AlphaArmManualControl(AlphaArm* alphaArm, frc::XboxController* codriver);
-  void OnTick(units::second_t dt);
-
- private:
-  AlphaArm* _alphaArm;
-  frc::XboxController* _codriver;
-  // units::volt_t _rightStick = ((_codriver->GetRightY()>0.05 || _codriver->GetRightY() < -0.05
-  // )?_codriver->GetRightY():0) * 2_V;
-  bool _rawControl;
-};
diff --git a/src/main/include/Behaviours/IntakeBehaviour.h b/src/main/include/Behaviours/IntakeBehaviour.h
deleted file mode 100644
index c9b99cc5..00000000
--- a/src/main/include/Behaviours/IntakeBehaviour.h
+++ /dev/null
@@ -1,34 +0,0 @@
-// Copyright (c) 2023-2024 CurtinFRC
-// Open Source Software, you can modify it according to the terms
-// of the MIT License at the root of this project
-
-#pragma once
-
-#include <frc/XboxController.h>
-
-#include "Intake.h"
-#include "Wombat.h"
-
-class IntakeManualControl : public behaviour::Behaviour {
- public:
-  explicit IntakeManualControl(Intake* intake, frc::XboxController* codriver);
-
-  void OnTick(units::second_t dt) override;
-
- private:
-  Intake* _intake;
-  frc::XboxController* _codriver;
-
-  units::volt_t _rawVoltage;
-  bool _rawControl;
-};
-
-class IntakeAutoControl : public behaviour::Behaviour {
- public:
-  explicit IntakeAutoControl(Intake* intake);
-
-  void OnTick(units::second_t dt) override;
-
- private:
-  Intake* _intake;
-};
diff --git a/src/main/include/Intake.h b/src/main/include/Intake.h
deleted file mode 100644
index a84b41b1..00000000
--- a/src/main/include/Intake.h
+++ /dev/null
@@ -1,41 +0,0 @@
-// Copyright (c) 2023-2024 CurtinFRC
-// Open Source Software, you can modify it according to the terms
-// of the MIT License at the root of this project
-
-#pragma once
-
-#include <frc/DigitalInput.h>
-
-#include <memory>
-#include <string>
-
-#include "Wombat.h"
-
-struct IntakeConfig {
-  wom::Gearbox IntakeMotor;
-  frc::DigitalInput* intakeSensor;
-  frc::DigitalInput* magSensor;
-  frc::DigitalInput* shooterSensor;
-};
-
-enum class IntakeState { kIdle, kRaw, kHold, kEject, kIntake, kPass };
-
-class Intake : public behaviour::HasBehaviour {
- public:
-  explicit Intake(IntakeConfig config);
-
-  void OnUpdate(units::second_t dt);
-
-  void setState(IntakeState state);
-  void setRaw(units::volt_t voltage);
-  IntakeConfig GetConfig();
-
- private:
-  IntakeConfig _config;
-  IntakeState _state = IntakeState::kIdle;
-
-  units::volt_t _rawVoltage = 0_V;
-  std::string _stringStateName = "error";
-  units::volt_t _setVoltage = 0_V;
-  std::shared_ptr<nt::NetworkTable> _table = nt::NetworkTableInstance::GetDefault().GetTable("Intake");
-};
diff --git a/src/main/include/Robot.h b/src/main/include/Robot.h
index 35fd683c..b40ce8e8 100644
--- a/src/main/include/Robot.h
+++ b/src/main/include/Robot.h
@@ -19,13 +19,9 @@
 
 #include <string>
 
-#include "AlphaArm.h"
 #include "Climber.h"
-#include "Intake.h"
 #include "Mag.h"
-#include "Behaviours/AlphaArmBehaviour.h"
 #include "Behaviours/ClimberBehaviour.h"
-#include "Behaviours/IntakeBehaviour.h"
 #include "Behaviours/MagBehaviour.h"
 #include "RobotMap.h"
 #include "Wombat.h"
@@ -58,8 +54,6 @@ class Robot : public frc::TimedRobot {
 
   wom::SwerveDrive* _swerveDrive;
 
-  AlphaArm* alphaArm;
   Mag* mag;
   Climber* climber;
-  Intake* intake;
 };
diff --git a/src/main/include/RobotMap.h b/src/main/include/RobotMap.h
index 609862a6..ddf8b441 100644
--- a/src/main/include/RobotMap.h
+++ b/src/main/include/RobotMap.h
@@ -1,4 +1,5 @@
 // Copyright (c) 2023-2024 CurtinFRC
+
 // Open Source Software, you can modify it according to the terms
 // of the MIT License at the root of this project
 
@@ -21,12 +22,8 @@
 
 #include "Climber.h"
 #include "Mag.h"
-#include "AlphaArm.h"
-#include "Intake.h"
-#include "Behaviours/AlphaArmBehaviour.h"
 #include "Behaviours/ClimberBehaviour.h"
 #include "Behaviours/MagBehaviour.h"
-#include "Behaviours/IntakeBehaviour.h"
 #include "Wombat.h"
 
 struct RobotMap {
@@ -37,17 +34,6 @@ struct RobotMap {
   };
   Controllers controllers;
 
-  struct AlphaArmSystem {
-    rev::CANSparkMax alphaArmMotor{99, rev::CANSparkMax::MotorType::kBrushless};
-    rev::CANSparkMax wristMotor{99, rev::CANSparkMax::MotorType::kBrushless};
-
-    wom::Gearbox alphaArmGearbox{&alphaArmMotor, nullptr, frc::DCMotor::NEO(1)};
-    wom::Gearbox wristGearbox{&wristMotor, nullptr, frc::DCMotor::NEO(1)};
-
-    AlphaArmConfig config{alphaArmGearbox, wristGearbox};
-  };
-  AlphaArmSystem alphaArmSystem;
-
   struct SwerveBase {
     ctre::phoenix6::hardware::CANcoder frontLeftCancoder{19};
     ctre::phoenix6::hardware::CANcoder frontRightCancoder{17};
@@ -181,22 +167,4 @@ struct RobotMap {
     };
   };
   Mag magSystem;
-
-  struct IntakeSystem {
-    rev::CANSparkMax intakeMotor{2, rev::CANSparkMax::MotorType::kBrushed};
-    // wom::CANSparkMaxEncoder intakeEncoder{&intakeMotor, 0.1_m};
-    // frc::DigitalInput intakeSensor{0};
-    // frc::DigitalInput magSensor{0};
-    // frc::DigitalInput shooterSensor{0};
-
-    wom::Gearbox IntakeGearbox{&intakeMotor, nullptr, frc::DCMotor::CIM(1)};
-
-    IntakeConfig config {
-      IntakeGearbox,
-      // &intakeSensor,
-      // &magSensor,
-      // &shooterSensor
-    };
-  };
-  IntakeSystem intakeSystem;
 };

From 84af45e17ad1c463a7846ff94cd8a159dbfded3a Mon Sep 17 00:00:00 2001
From: Kingsley Wong <kingsley.wong@student.education.wa.edu.au>
Date: Wed, 24 Jan 2024 20:03:46 +0800
Subject: [PATCH 070/207] added beam break

---
 src/main/cpp/Intake.cpp          |  18 +++---
 src/main/cpp/IntakeBehaviour.cpp |  41 +++++++------
 src/main/cpp/Shooter.cpp         | 102 +++++++++++++++----------------
 src/main/include/RobotMap.h      |   4 +-
 4 files changed, 83 insertions(+), 82 deletions(-)

diff --git a/src/main/cpp/Intake.cpp b/src/main/cpp/Intake.cpp
index 75839209..70614e0e 100644
--- a/src/main/cpp/Intake.cpp
+++ b/src/main/cpp/Intake.cpp
@@ -13,10 +13,10 @@ IntakeConfig Intake::GetConfig() {
 void Intake::OnUpdate(units::second_t dt) {
   switch (_state) {
     case IntakeState::kIdle: {
-      // _config.IntakeMotor.motorController->SetVoltage(0_V);
-      // if (_config.intakeSensor->Get()) {
-      //   setState(IntakeState::kHold);
-      // }
+      _config.IntakeMotor.motorController->SetVoltage(0_V);
+      if (_config.intakeSensor->Get()) {
+        setState(IntakeState::kHold);
+      }
       _stringStateName = "Idle";
       _setVoltage = 0_V;
     } break;
@@ -26,10 +26,10 @@ void Intake::OnUpdate(units::second_t dt) {
       _setVoltage = _rawVoltage;
     } break;
     case IntakeState::kEject: {
-      // _config.IntakeMotor.motorController->SetVoltage(-5_V);
-      // if (_config.intakeSensor->Get() == 0 && _config.magSensor->Get() == 0) {
-      //   setState(IntakeState::kIdle);
-      // }
+      _config.IntakeMotor.motorController->SetVoltage(-5_V);
+      if (_config.intakeSensor->Get() == 0 && _config.magSensor->Get() == 0) {
+        setState(IntakeState::kIdle);
+      }
       _stringStateName = "Eject";
       _setVoltage = -5_V;
     } break;
@@ -57,7 +57,7 @@ void Intake::OnUpdate(units::second_t dt) {
   }
   _table->GetEntry("State: ").SetString(_stringStateName);
   _table->GetEntry("Motor Voltage: ").SetDouble(_setVoltage.value());
-  // _table->GetEntry("Intake Sensor: ").SetBoolean(_config.intakeSensor->Get());
+  _table->GetEntry("Intake Sensor: ").SetBoolean(_config.intakeSensor->Get());
   // _table->GetEntry("Shooter Sensor: ").SetBoolean(_config.shooterSensor->Get());
   // _table->GetEntry("Magazine Sensor: ").SetBoolean(_config.magSensor->Get());
 
diff --git a/src/main/cpp/IntakeBehaviour.cpp b/src/main/cpp/IntakeBehaviour.cpp
index 90867045..a32c2791 100644
--- a/src/main/cpp/IntakeBehaviour.cpp
+++ b/src/main/cpp/IntakeBehaviour.cpp
@@ -19,21 +19,22 @@ void IntakeManualControl::OnTick(units::second_t dt) {
       _rawControl = true;
     }
   }
-
-  if (_rawControl) {
-    _intake->setState(IntakeState::kRaw);
-    if (_codriver.GetLeftBumper()) {
-      _intake->setRaw(10_V);
-    } else if (_codriver.GetRightBumper()) {
-      _intake->setRaw(-10_V);
+  if (_codriver.GetRightTriggerAxis() > 0.1) {
+    _intake->setRaw(_codriver.GetRightTriggerAxis() * 10_V);
+  } else if (_codriver.GetLeftTriggerAxis() > 0.1) {
+    _intake->setRaw(_codriver.GetLeftTriggerAxis() * -10_V);
+  } else if (_rawVoltage != 0_V) {
+    if (_intake->GetConfig().intakeSensor->Get() == 1) {
+      _intake->setRaw(0_V);
     } else {
       _intake->setRaw(0_V);
-    }
-
-    // _intake->setRaw(_codriver.GetLeftBumper() * 10_V);
-    // _intake->setRaw(_codriver.GetRightBumper() * -10_V);
-    std::cout << "Raw" << std::endl;
+    } 
+  } else {
+    _intake->setRaw(0_V);
   }
+    _intake->setState(IntakeState::kRaw);
+}
+
   // } else {
   //   if (_codriver.GetYButtonPressed()) {
   //     _intake->setState(IntakeState::kIntake);
@@ -42,14 +43,14 @@ void IntakeManualControl::OnTick(units::second_t dt) {
   //     _intake->setState(IntakeState::kPass);
   //   }
   // }
-}
+
 
 // IntakeAutoControl::IntakeAutoControl(Intake* intake) : _intake(intake) {}
 
-// void IntakeAutoControl::OnTick(units::second_t dt) {
-//   if (_intake->GetConfig().intakeSensor->Get() == 1) {
-//     _intake->setState(IntakeState::kPass);
-//   } else if (_intake->GetConfig().magSensor->Get() == 0) {
-//     _intake->setState(IntakeState::kIdle);
-//   }
-// }
+void IntakeAutoControl::OnTick(units::second_t dt) {
+  if (_intake->GetConfig().intakeSensor->Get() == 1) {
+    _intake->setState(IntakeState::kPass);
+  } /*else if (_intake->GetConfig().magSensor->Get() == 0) {
+    _intake->setState(IntakeState::kIdle);
+  }*/
+}
diff --git a/src/main/cpp/Shooter.cpp b/src/main/cpp/Shooter.cpp
index 613120df..3f1bef83 100644
--- a/src/main/cpp/Shooter.cpp
+++ b/src/main/cpp/Shooter.cpp
@@ -10,60 +10,60 @@ Shooter::Shooter(ShooterConfig config) : _config(config)
 {} //config.path + "/pid", config.pidConfig
 void Shooter::OnUpdate(units::second_t dt) {
   // _pid.SetTolerance(0.1, 1);
-  switch (_state) {
-    case ShooterState::kIdle: {
-      std::cout << "KIdle" << std::endl;
-      _setVoltage = 0_V;
-      // if (_shooterSensor.Get()) {
-      //   _state = ShooterState::kReverse;
-      // }
-    } break;
-    case ShooterState::kSpinUp: {
-      // std::cout << "KSpinUp" << std::endl;
-      // _pid.SetSetpoint(_goal.value());
-      // units::volt_t pidCalculate =
-      //     units::volt_t{_pid.Calculate(_config.ShooterGearbox.encoder->GetEncoderAngularVelocity().value())};
-      // _setVoltage = pidCalculate;
+  // switch (_state) {
+  //   case ShooterState::kIdle: {
+  //     std::cout << "KIdle" << std::endl;
+  //     _setVoltage = 0_V;
+  //     // if (_shooterSensor.Get()) {
+  //     //   _state = ShooterState::kReverse;
+  //     // }
+  //   } break;
+  //   case ShooterState::kSpinUp: {
+  //     // std::cout << "KSpinUp" << std::endl;
+  //     // _pid.SetSetpoint(_goal.value());
+  //     // units::volt_t pidCalculate =
+  //     //     units::volt_t{_pid.Calculate(_config.ShooterGearbox.encoder->GetEncoderAngularVelocity().value())};
+  //     // _setVoltage = pidCalculate;
 
-      // if (_pid.AtSetpoint()) {
-      //   SetState(ShooterState::kShooting);
-      // }
-    } break;
-    case ShooterState::kShooting: {
-      // std::cout << "KShooting" << std::endl;
-      // _pid.SetSetpoint(_goal.value());
-      // units::volt_t pidCalculate =
-      //     units::volt_t{_pid.Calculate(_config.ShooterGearbox.encoder->GetEncoderAngularVelocity().value())};
-      // _setVoltage = pidCalculate;
+  //     // if (_pid.AtSetpoint()) {
+  //     //   SetState(ShooterState::kShooting);
+  //     // }
+  //   } break;
+  //   case ShooterState::kShooting: {
+  //     // std::cout << "KShooting" << std::endl;
+  //     // _pid.SetSetpoint(_goal.value());
+  //     // units::volt_t pidCalculate =
+  //     //     units::volt_t{_pid.Calculate(_config.ShooterGearbox.encoder->GetEncoderAngularVelocity().value())};
+  //     // _setVoltage = pidCalculate;
 
-      // if (!_pid.AtSetpoint()) {
-      //   SetState(ShooterState::kSpinUp);
-      // }
-      // if (_shooterSensor.Get()) {
-      //   SetState(ShooterState::kIdle);
-      // }
-    } break;
+  //     // if (!_pid.AtSetpoint()) {
+  //     //   SetState(ShooterState::kSpinUp);
+  //     // }
+  //     // if (_shooterSensor.Get()) {
+  //     //   SetState(ShooterState::kIdle);
+  //     // }
+  //   } break;
 
-    case ShooterState::kReverse: {
-      _setVoltage = -5_V;
-      std::cout << "KReverse" << std::endl;
-      // if (!_shooterSensor.Get()) {
-      //   SetState(ShooterState::kIdle);
-      // }
-    } break;
-    case ShooterState::kRaw: {
-      _setVoltage = _rawVoltage;
-      std::cout << "KRaw" << std::endl;
-      // if (_shooterSensor.Get()) {
-      //   SetState(ShooterState::kRaw);
-      // }
-    } break;
-    default: {
-      std::cout << "Error shooter in invalid state" << std::endl;
-    } break;
-  }
-  std::cout << "Voltage:" << _setVoltage.value() << std::endl;
-  _config.ShooterGearbox.motorController->SetVoltage(_setVoltage);
+  //   case ShooterState::kReverse: {
+  //     _setVoltage = -5_V;
+  //     std::cout << "KReverse" << std::endl;
+  //     // if (!_shooterSensor.Get()) {
+  //     //   SetState(ShooterState::kIdle);
+  //     // }
+  //   } break;
+  //   case ShooterState::kRaw: {
+  //     _setVoltage = _rawVoltage;
+  //     std::cout << "KRaw" << std::endl;
+  //     // if (_shooterSensor.Get()) {
+  //     //   SetState(ShooterState::kRaw);
+  //     // }
+  //   } break;
+  //   default: {
+  //     std::cout << "Error shooter in invalid state" << std::endl;
+  //   } break;
+  // }
+  // std::cout << "Voltage:" << _setVoltage.value() << std::endl;
+  // _config.ShooterGearbox.motorController->SetVoltage(_setVoltage);
 
 }
 
diff --git a/src/main/include/RobotMap.h b/src/main/include/RobotMap.h
index 52121f1a..5b0bcdab 100644
--- a/src/main/include/RobotMap.h
+++ b/src/main/include/RobotMap.h
@@ -31,13 +31,13 @@ struct RobotMap {
   struct IntakeSystem {
     rev::CANSparkMax intakeMotor{2, rev::CANSparkMax::MotorType::kBrushed};
     // wom::CANSparkMaxEncoder intakeEncoder{&intakeMotor, 0.1_m};
-    // frc::DigitalInput intakeSensor{0};
+    frc::DigitalInput intakeSensor{4};
     // frc::DigitalInput magSensor{0};
     // frc::DigitalInput shooterSensor{0};
 
     wom::Gearbox IntakeGearbox{&intakeMotor, nullptr, frc::DCMotor::CIM(1)};
 
-    IntakeConfig config{IntakeGearbox /*, &intakeSensor, &magSensor, &shooterSensor*/};
+    IntakeConfig config{IntakeGearbox , &intakeSensor/*, &magSensor, &shooterSensor*/};
   }; IntakeSystem intakeSystem;
 
 

From 7e9b5f95a6dab841acbad5c282d40746cb69e621 Mon Sep 17 00:00:00 2001
From: Sean Chan <sean.chan3@student.education.wa.edu.au>
Date: Sun, 28 Jan 2024 11:50:38 +0800
Subject: [PATCH 071/207] Added auto to intake

---
 src/main/cpp/IntakeBehaviour.cpp | 61 ++++++++++++++++----------------
 1 file changed, 30 insertions(+), 31 deletions(-)

diff --git a/src/main/cpp/IntakeBehaviour.cpp b/src/main/cpp/IntakeBehaviour.cpp
index a32c2791..f8f9b886 100644
--- a/src/main/cpp/IntakeBehaviour.cpp
+++ b/src/main/cpp/IntakeBehaviour.cpp
@@ -6,8 +6,7 @@
 
 #include <frc/XboxController.h>
 
-IntakeManualControl::IntakeManualControl(Intake* intake, frc::XboxController& codriver)
-    : _intake(intake), _codriver(codriver) {
+IntakeManualControl::IntakeManualControl(Intake* intake, frc::XboxController& codriver) : _intake(intake), _codriver(codriver) {
   Controls(intake);
 }
 
@@ -19,38 +18,38 @@ void IntakeManualControl::OnTick(units::second_t dt) {
       _rawControl = true;
     }
   }
-  if (_codriver.GetRightTriggerAxis() > 0.1) {
-    _intake->setRaw(_codriver.GetRightTriggerAxis() * 10_V);
-  } else if (_codriver.GetLeftTriggerAxis() > 0.1) {
-    _intake->setRaw(_codriver.GetLeftTriggerAxis() * -10_V);
-  } else if (_rawVoltage != 0_V) {
-    if (_intake->GetConfig().intakeSensor->Get() == 1) {
-      _intake->setRaw(0_V);
+
+  if (_rawControl) {
+    if (_codriver.GetRightTriggerAxis() > 0.1) {
+      _intake->setRaw(_codriver.GetRightTriggerAxis() * 10_V);
+    } else if (_codriver.GetLeftTriggerAxis() > 0.1) {
+      _intake->setRaw(_codriver.GetLeftTriggerAxis() * -10_V);
+    } else if (_rawVoltage != 0_V) {
+      if (_intake->GetConfig().intakeSensor->Get() == 1) {
+        _intake->setRaw(0_V);
+      } else {
+        _intake->setRaw(10_V);
+      } 
     } else {
       _intake->setRaw(0_V);
-    } 
-  } else {
-    _intake->setRaw(0_V);
-  }
+    }
     _intake->setState(IntakeState::kRaw);
-}
-
-  // } else {
-  //   if (_codriver.GetYButtonPressed()) {
-  //     _intake->setState(IntakeState::kIntake);
-  //   }
-  //   if (_codriver.GetAButtonPressed()) {
-  //     _intake->setState(IntakeState::kPass);
-  //   }
-  // }
 
+  } else {
+     if (_intake->GetConfig().intakeSensor->Get() == 1) {
+        _intake->setState(IntakeState::kHold);
+      } else if (_intake->GetConfig().intakeSensor->Get() == 0) {
+        if (_codriver.GetRightTriggerAxis() > 0.1) {
+          _intake->setState(IntakeState::kIntake);
+        } else {
+          _intake->setState(IntakeState::kIdle);
+        }
+      }
+    }
+  }
 
-// IntakeAutoControl::IntakeAutoControl(Intake* intake) : _intake(intake) {}
-
-void IntakeAutoControl::OnTick(units::second_t dt) {
-  if (_intake->GetConfig().intakeSensor->Get() == 1) {
-    _intake->setState(IntakeState::kPass);
-  } /*else if (_intake->GetConfig().magSensor->Get() == 0) {
-    _intake->setState(IntakeState::kIdle);
-  }*/
+IntakeAutoControl::IntakeAutoControl(Intake* intake) : _intake(intake) {
+  Controls(intake);
 }
+
+void IntakeAutoControl::OnTick(units::second_t dt) {}

From 2df1f037d212dbfa0bb3872844e826eb1609fe06 Mon Sep 17 00:00:00 2001
From: Sean Chan <sean.chan3@student.education.wa.edu.au>
Date: Sun, 28 Jan 2024 16:36:07 +0800
Subject: [PATCH 072/207] Tested on robot

---
 src/main/cpp/Intake.cpp            |  9 ++--
 src/main/cpp/IntakeBehaviour.cpp   | 84 +++++++++++++++++++++++++-----
 src/main/include/Intake.h          |  1 +
 src/main/include/IntakeBehaviour.h |  5 +-
 4 files changed, 81 insertions(+), 18 deletions(-)

diff --git a/src/main/cpp/Intake.cpp b/src/main/cpp/Intake.cpp
index 70614e0e..03d0f1c1 100644
--- a/src/main/cpp/Intake.cpp
+++ b/src/main/cpp/Intake.cpp
@@ -31,7 +31,7 @@ void Intake::OnUpdate(units::second_t dt) {
         setState(IntakeState::kIdle);
       }
       _stringStateName = "Eject";
-      _setVoltage = -5_V;
+      _setVoltage = 7_V;
     } break;
     case IntakeState::kHold: {
       // _config.IntakeMotor.motorController->SetVoltage(0_V);
@@ -41,7 +41,7 @@ void Intake::OnUpdate(units::second_t dt) {
     case IntakeState::kIntake: {
       // _config.IntakeMotor.motorController->SetVoltage(5_V);
       _stringStateName = "Intake";
-      _setVoltage = 5_V;
+      _setVoltage = -7_V;
     } break;
     case IntakeState::kPass: {
       // _config.IntakeMotor.motorController->SetVoltage(5_V);
@@ -49,7 +49,7 @@ void Intake::OnUpdate(units::second_t dt) {
       //   setState(IntakeState::kIdle);
       //   _stringStateName = "Pass";
       // }
-      _setVoltage = 5_V;
+      _setVoltage = -7_V;
     } break;
     default:
       std::cout << "Error: Intake in INVALID STATE." << std::endl;
@@ -72,3 +72,6 @@ void Intake::setState(IntakeState state) {
 void Intake::setRaw(units::volt_t voltage) {
   _rawVoltage = voltage;
 }
+IntakeState Intake::getState() {
+  return _state;
+}
diff --git a/src/main/cpp/IntakeBehaviour.cpp b/src/main/cpp/IntakeBehaviour.cpp
index f8f9b886..bd9e8044 100644
--- a/src/main/cpp/IntakeBehaviour.cpp
+++ b/src/main/cpp/IntakeBehaviour.cpp
@@ -12,44 +12,100 @@ IntakeManualControl::IntakeManualControl(Intake* intake, frc::XboxController& co
 
 void IntakeManualControl::OnTick(units::second_t dt) {
   if (_codriver.GetBButtonPressed()) {
-    if (_rawControl == true) {
+    if (_rawControl) {
       _rawControl = false;
+      _intake->setState(IntakeState::kIdle);
     } else {
       _rawControl = true;
+      _intake->setState(IntakeState::kRaw);
     }
   }
-
+  
   if (_rawControl) {
     if (_codriver.GetRightTriggerAxis() > 0.1) {
       _intake->setRaw(_codriver.GetRightTriggerAxis() * 10_V);
     } else if (_codriver.GetLeftTriggerAxis() > 0.1) {
       _intake->setRaw(_codriver.GetLeftTriggerAxis() * -10_V);
-    } else if (_rawVoltage != 0_V) {
-      if (_intake->GetConfig().intakeSensor->Get() == 1) {
-        _intake->setRaw(0_V);
-      } else {
-        _intake->setRaw(10_V);
-      } 
     } else {
       _intake->setRaw(0_V);
     }
     _intake->setState(IntakeState::kRaw);
 
   } else {
-     if (_intake->GetConfig().intakeSensor->Get() == 1) {
-        _intake->setState(IntakeState::kHold);
-      } else if (_intake->GetConfig().intakeSensor->Get() == 0) {
-        if (_codriver.GetRightTriggerAxis() > 0.1) {
-          _intake->setState(IntakeState::kIntake);
-        } else {
+    if (_codriver.GetRightTriggerAxis() > 0.1) {
+      if (_intaking) {
+        _intaking = false;
+        _intake->setState(IntakeState::kIdle);
+      } else {
+        _intaking = true;
+      }
+    }
+
+    if (_codriver.GetLeftTriggerAxis() > 0.1) {
+      if (_ejecting) {
+        _ejecting = false;
+      } else {
+        _ejecting = true;
+      }
+    }
+
+    if (_intaking) {
+      if (_intake->getState() == IntakeState::kHold || _intake->getState() == IntakeState::kPass) {
+        if (_intake->GetConfig().intakeSensor->Get() == 1) {
           _intake->setState(IntakeState::kIdle);
+          _intaking = false;
+        } else {
+          _intake->setState(IntakeState::kPass);
+        }
+      } else {
+        if (_intake->GetConfig().intakeSensor->Get() == 0) {
+          _intake->setState(IntakeState::kHold);
+          _intaking = false;
+        } else {
+          _intake->setState(IntakeState::kIntake);
         }
       }
     }
+
+    if (_ejecting) {
+      if (_intake->GetConfig().intakeSensor->Get() == 1) {
+        _intake->setState(IntakeState::kIdle);
+        _ejecting = false;
+      } else {
+        _intake->setState(IntakeState::kEject);
+      }
+    }
   }
+}
 
 IntakeAutoControl::IntakeAutoControl(Intake* intake) : _intake(intake) {
   Controls(intake);
 }
 
 void IntakeAutoControl::OnTick(units::second_t dt) {}
+
+  // } else {
+  //   if (_setVoltage != 0_V) { // If the intake wasn't running last tick,
+  //     if (_setVoltage < 0_V && _intake->GetConfig().intakeSensor->Get() == 0) { // If the intake was intaking and there is a note in the intake,
+  //       _intake->setState(IntakeState::kHold); // Hold that note.
+  //     } else if (_setVoltage > 0_V && _intake->GetConfig().intakeSensor->Get() == 1) { // Else if the intake was ejecting and there is noting in the intake,
+  //       _intake->setState(IntakeState::kIdle); // Set the intake to idle mode.
+  //     } 
+
+  //   } else {
+  //     if (_intake->GetConfig().intakeSensor->Get() == 0) { // If there is a note in the intake.
+  //       if (_codriver.GetRightTriggerAxis() > 0.1) { // If the right trigger is pressed,
+  //         _intake->setState(IntakeState::kPass); // Start passing the note into the shooter.
+  //       } else { // Otherwise,
+  //         _intake->setState(IntakeState::kHold); // Keep holding the note.
+  //       }
+  //     } else if (_intake->GetConfig().intakeSensor->Get() == 1) { // If there is nothing in the intake,
+  //       if (_codriver.GetRightTriggerAxis() > 0.1) { // If the right trigger is pressed,
+  //         _intake->setState(IntakeState::kIntake); // Start intaking.
+  //       } else { // Otherwise,
+  //         _intake->setState(IntakeState::kIdle); // Continue to go Idle.
+  //       }
+  //     } else if (_codriver.GetLeftTriggerAxis() > 0.1) { // If the left trigger is pressed,
+  //       _intake->setState(IntakeState::kEject); // Eject the note.
+  //     }
+  //   }
diff --git a/src/main/include/Intake.h b/src/main/include/Intake.h
index a84b41b1..948f3741 100644
--- a/src/main/include/Intake.h
+++ b/src/main/include/Intake.h
@@ -28,6 +28,7 @@ class Intake : public behaviour::HasBehaviour {
 
   void setState(IntakeState state);
   void setRaw(units::volt_t voltage);
+  IntakeState getState();
   IntakeConfig GetConfig();
 
  private:
diff --git a/src/main/include/IntakeBehaviour.h b/src/main/include/IntakeBehaviour.h
index c84c6d32..419b16d8 100644
--- a/src/main/include/IntakeBehaviour.h
+++ b/src/main/include/IntakeBehaviour.h
@@ -20,7 +20,10 @@ class IntakeManualControl : public behaviour::Behaviour {
   frc::XboxController& _codriver;
 
   units::volt_t _rawVoltage;
-  bool _rawControl;
+  units::volt_t _setVoltage;
+  bool _rawControl = true;
+  bool _intaking = false;
+  bool _ejecting = false;
 };
 
 class IntakeAutoControl : public behaviour::Behaviour {

From 7551c8170d93e2a599a451ba2c31ca397d4aba50 Mon Sep 17 00:00:00 2001
From: Sean Chan <sean.chan3@student.education.wa.edu.au>
Date: Sun, 28 Jan 2024 20:14:27 +0800
Subject: [PATCH 073/207] fixed intake manual auto, has not been tested on
 robot

---
 src/main/cpp/IntakeBehaviour.cpp | 11 +++++++----
 1 file changed, 7 insertions(+), 4 deletions(-)

diff --git a/src/main/cpp/IntakeBehaviour.cpp b/src/main/cpp/IntakeBehaviour.cpp
index bd9e8044..235dd880 100644
--- a/src/main/cpp/IntakeBehaviour.cpp
+++ b/src/main/cpp/IntakeBehaviour.cpp
@@ -11,9 +11,11 @@ IntakeManualControl::IntakeManualControl(Intake* intake, frc::XboxController& co
 }
 
 void IntakeManualControl::OnTick(units::second_t dt) {
-  if (_codriver.GetBButtonPressed()) {
+  if (_codriver.GetBButtonReleased()) {
     if (_rawControl) {
       _rawControl = false;
+      _intaking = false;
+      _ejecting = false;
       _intake->setState(IntakeState::kIdle);
     } else {
       _rawControl = true;
@@ -46,19 +48,20 @@ void IntakeManualControl::OnTick(units::second_t dt) {
         _ejecting = false;
       } else {
         _ejecting = true;
+        _intaking = false;
       }
     }
 
     if (_intaking) {
       if (_intake->getState() == IntakeState::kHold || _intake->getState() == IntakeState::kPass) {
-        if (_intake->GetConfig().intakeSensor->Get() == 1) {
+        if (_intake->GetConfig().intakeSensor->Get() == 0) {
           _intake->setState(IntakeState::kIdle);
           _intaking = false;
         } else {
           _intake->setState(IntakeState::kPass);
         }
       } else {
-        if (_intake->GetConfig().intakeSensor->Get() == 0) {
+        if (_intake->GetConfig().intakeSensor->Get() == 1) {
           _intake->setState(IntakeState::kHold);
           _intaking = false;
         } else {
@@ -68,7 +71,7 @@ void IntakeManualControl::OnTick(units::second_t dt) {
     }
 
     if (_ejecting) {
-      if (_intake->GetConfig().intakeSensor->Get() == 1) {
+      if (_intake->GetConfig().intakeSensor->Get() == 0) {
         _intake->setState(IntakeState::kIdle);
         _ejecting = false;
       } else {

From 474ed7eacf07df7ac63fade9318f08ae3e4efa00 Mon Sep 17 00:00:00 2001
From: Sean Chan <sean.chan3@student.education.wa.edu.au>
Date: Sun, 28 Jan 2024 20:36:21 +0800
Subject: [PATCH 074/207] fixed github build errors

---
 src/main/include/RobotMap.h                    | 1 +
 wombat/src/main/cpp/drivetrain/SwerveDrive.cpp | 2 +-
 2 files changed, 2 insertions(+), 1 deletion(-)

diff --git a/src/main/include/RobotMap.h b/src/main/include/RobotMap.h
index 5b0bcdab..b4715e3b 100644
--- a/src/main/include/RobotMap.h
+++ b/src/main/include/RobotMap.h
@@ -11,6 +11,7 @@
 #include <units/length.h>
 
 #include <string>
+#include <memory>
 
 #include <ctre/phoenix6/CANcoder.hpp>
 #include <ctre/phoenix6/Pigeon2.hpp>
diff --git a/wombat/src/main/cpp/drivetrain/SwerveDrive.cpp b/wombat/src/main/cpp/drivetrain/SwerveDrive.cpp
index eb33efb1..00c50e63 100644
--- a/wombat/src/main/cpp/drivetrain/SwerveDrive.cpp
+++ b/wombat/src/main/cpp/drivetrain/SwerveDrive.cpp
@@ -26,7 +26,7 @@ void SwerveModuleConfig::WriteNT(
 SwerveModule::SwerveModule(std::string path, SwerveModuleConfig config,
                            //SwerveModule::angle_pid_conf_t anglePID,
                            SwerveModule::velocity_pid_conf_t velocityPID)
-    : //_anglePIDController(path + "/pid/angle", anglePID),
+    : // _anglePIDController(path + "/pid/angle", anglePID),
       _anglePIDController{frc::PIDController(4, 0, 0, 0.005_s)},
       _config(config),
       _velocityPIDController(frc::PIDController(1.2, 0, 0, 0.005_s)),

From 37f27a9979c1cb01bacc0580a3a783127eb23f1a Mon Sep 17 00:00:00 2001
From: Sean Chan <sean.chan3@student.education.wa.edu.au>
Date: Mon, 29 Jan 2024 15:41:26 +0800
Subject: [PATCH 075/207] Added intake better manual and verfied on robot!

---
 .clang-format                      |  2 +-
 .github/workflows/format.yml       |  2 +-
 build.gradle                       |  2 +-
 src/main/cpp/Intake.cpp            | 38 ++++++++++-------
 src/main/cpp/IntakeBehaviour.cpp   | 67 ++++++++++--------------------
 src/main/include/Intake.h          |  4 ++
 src/main/include/IntakeBehaviour.h |  1 +
 7 files changed, 54 insertions(+), 62 deletions(-)

diff --git a/.clang-format b/.clang-format
index f69bef0c..a879dec4 100644
--- a/.clang-format
+++ b/.clang-format
@@ -81,7 +81,7 @@ BreakBeforeTernaryOperators: true
 BreakConstructorInitializers: BeforeColon
 BreakInheritanceList: BeforeColon
 BreakStringLiterals: true
-ColumnLimit:     80
+ColumnLimit:     110
 CommentPragmas:  '^ IWYU pragma:'
 CompactNamespaces: false
 ConstructorInitializerIndentWidth: 4
diff --git a/.github/workflows/format.yml b/.github/workflows/format.yml
index c6fced20..f57ddeb4 100644
--- a/.github/workflows/format.yml
+++ b/.github/workflows/format.yml
@@ -21,7 +21,7 @@ jobs:
         with:
           python-version: 3.8
       - name: Install wpiformat
-        run: pip3 install wpiformat==2023.36
+        run: pip3 install wpiformat==2024.31
       - name: Run
         run: wpiformat
       - name: Check output
diff --git a/build.gradle b/build.gradle
index 91b6acb4..84ccc0cb 100644
--- a/build.gradle
+++ b/build.gradle
@@ -1,7 +1,7 @@
 plugins {
     id "cpp"
     id "google-test-test-suite"
-    id "edu.wpi.first.GradleRIO" version "2024.1.1"
+    id "edu.wpi.first.GradleRIO" version "2024.2.1"
 }
 
 // Define my targets (RoboRIO) and artifacts (deployable files)
diff --git a/src/main/cpp/Intake.cpp b/src/main/cpp/Intake.cpp
index 03d0f1c1..b56d4b47 100644
--- a/src/main/cpp/Intake.cpp
+++ b/src/main/cpp/Intake.cpp
@@ -13,44 +13,53 @@ IntakeConfig Intake::GetConfig() {
 void Intake::OnUpdate(units::second_t dt) {
   switch (_state) {
     case IntakeState::kIdle: {
-      _config.IntakeMotor.motorController->SetVoltage(0_V);
-      if (_config.intakeSensor->Get()) {
+      if (_config.intakeSensor->Get() == false) {
         setState(IntakeState::kHold);
       }
       _stringStateName = "Idle";
       _setVoltage = 0_V;
     } break;
+
     case IntakeState::kRaw: {
-      // _config.IntakeMotor.motorController->SetVoltage(_rawVoltage);
       _stringStateName = "Raw";
       _setVoltage = _rawVoltage;
     } break;
+
     case IntakeState::kEject: {
-      _config.IntakeMotor.motorController->SetVoltage(-5_V);
-      if (_config.intakeSensor->Get() == 0 && _config.magSensor->Get() == 0) {
-        setState(IntakeState::kIdle);
-      }
       _stringStateName = "Eject";
       _setVoltage = 7_V;
+      if (_config.intakeSensor->Get() == true) {
+        setState(IntakeState::kIdle);
+        _ejecting = false;
+      }
     } break;
+
     case IntakeState::kHold: {
-      // _config.IntakeMotor.motorController->SetVoltage(0_V);
       _stringStateName = "Hold";
       _setVoltage = 0_V;
+      // if (_config.intakeSensor->Get() == false) {
+      //   setState(IntakeState::kHold);
+      // }
     } break;
+
     case IntakeState::kIntake: {
-      // _config.IntakeMotor.motorController->SetVoltage(5_V);
       _stringStateName = "Intake";
       _setVoltage = -7_V;
+      if (_config.intakeSensor->Get() == false) {
+        setState(IntakeState::kHold);
+        _intaking = false;
+      }
     } break;
+
     case IntakeState::kPass: {
-      // _config.IntakeMotor.motorController->SetVoltage(5_V);
-      // if (_config.shooterSensor->Get()) {
-      //   setState(IntakeState::kIdle);
-      //   _stringStateName = "Pass";
-      // }
+      _stringStateName = "Pass";
       _setVoltage = -7_V;
+      if (_config.intakeSensor->Get() == true) {
+        setState(IntakeState::kIdle);
+        _passing = false;
+      }
     } break;
+    
     default:
       std::cout << "Error: Intake in INVALID STATE." << std::endl;
       break;
@@ -59,7 +68,6 @@ void Intake::OnUpdate(units::second_t dt) {
   _table->GetEntry("Motor Voltage: ").SetDouble(_setVoltage.value());
   _table->GetEntry("Intake Sensor: ").SetBoolean(_config.intakeSensor->Get());
   // _table->GetEntry("Shooter Sensor: ").SetBoolean(_config.shooterSensor->Get());
-  // _table->GetEntry("Magazine Sensor: ").SetBoolean(_config.magSensor->Get());
 
   std::cout << _setVoltage.value() << std::endl;
 
diff --git a/src/main/cpp/IntakeBehaviour.cpp b/src/main/cpp/IntakeBehaviour.cpp
index 235dd880..038c5ce3 100644
--- a/src/main/cpp/IntakeBehaviour.cpp
+++ b/src/main/cpp/IntakeBehaviour.cpp
@@ -19,6 +19,8 @@ void IntakeManualControl::OnTick(units::second_t dt) {
       _intake->setState(IntakeState::kIdle);
     } else {
       _rawControl = true;
+      _intaking = false;
+      _ejecting = false;
       _intake->setState(IntakeState::kRaw);
     }
   }
@@ -40,41 +42,44 @@ void IntakeManualControl::OnTick(units::second_t dt) {
         _intake->setState(IntakeState::kIdle);
       } else {
         _intaking = true;
+        _ejecting = false;
       }
     }
 
     if (_codriver.GetLeftTriggerAxis() > 0.1) {
       if (_ejecting) {
         _ejecting = false;
+        _intake->setState(IntakeState::kIdle);
       } else {
         _ejecting = true;
         _intaking = false;
       }
     }
 
-    if (_intaking) {
-      if (_intake->getState() == IntakeState::kHold || _intake->getState() == IntakeState::kPass) {
-        if (_intake->GetConfig().intakeSensor->Get() == 0) {
-          _intake->setState(IntakeState::kIdle);
-          _intaking = false;
-        } else {
-          _intake->setState(IntakeState::kPass);
-        }
+    if (_codriver.GetAButtonPressed()) {
+      if (_passing) {
+        _passing = false;
+        _intake->setState(IntakeState::kIdle);
       } else {
-        if (_intake->GetConfig().intakeSensor->Get() == 1) {
-          _intake->setState(IntakeState::kHold);
-          _intaking = false;
-        } else {
-          _intake->setState(IntakeState::kIntake);
-        }
+        _passing = true;
+        _intaking = false;
+      }
+    }
+
+    if (_intaking) {
+      if (_intake->getState() == IntakeState::kIdle) {
+        _intake->setState(IntakeState::kIntake);
+      }
+    }
+
+    if (_passing) {
+      if (_intake->getState() == IntakeState::kHold) {
+        _intake->setState(IntakeState::kPass);
       }
     }
 
     if (_ejecting) {
-      if (_intake->GetConfig().intakeSensor->Get() == 0) {
-        _intake->setState(IntakeState::kIdle);
-        _ejecting = false;
-      } else {
+      if (_intake->getState() == IntakeState::kIdle || _intake->getState() == IntakeState::kHold) {
         _intake->setState(IntakeState::kEject);
       }
     }
@@ -86,29 +91,3 @@ IntakeAutoControl::IntakeAutoControl(Intake* intake) : _intake(intake) {
 }
 
 void IntakeAutoControl::OnTick(units::second_t dt) {}
-
-  // } else {
-  //   if (_setVoltage != 0_V) { // If the intake wasn't running last tick,
-  //     if (_setVoltage < 0_V && _intake->GetConfig().intakeSensor->Get() == 0) { // If the intake was intaking and there is a note in the intake,
-  //       _intake->setState(IntakeState::kHold); // Hold that note.
-  //     } else if (_setVoltage > 0_V && _intake->GetConfig().intakeSensor->Get() == 1) { // Else if the intake was ejecting and there is noting in the intake,
-  //       _intake->setState(IntakeState::kIdle); // Set the intake to idle mode.
-  //     } 
-
-  //   } else {
-  //     if (_intake->GetConfig().intakeSensor->Get() == 0) { // If there is a note in the intake.
-  //       if (_codriver.GetRightTriggerAxis() > 0.1) { // If the right trigger is pressed,
-  //         _intake->setState(IntakeState::kPass); // Start passing the note into the shooter.
-  //       } else { // Otherwise,
-  //         _intake->setState(IntakeState::kHold); // Keep holding the note.
-  //       }
-  //     } else if (_intake->GetConfig().intakeSensor->Get() == 1) { // If there is nothing in the intake,
-  //       if (_codriver.GetRightTriggerAxis() > 0.1) { // If the right trigger is pressed,
-  //         _intake->setState(IntakeState::kIntake); // Start intaking.
-  //       } else { // Otherwise,
-  //         _intake->setState(IntakeState::kIdle); // Continue to go Idle.
-  //       }
-  //     } else if (_codriver.GetLeftTriggerAxis() > 0.1) { // If the left trigger is pressed,
-  //       _intake->setState(IntakeState::kEject); // Eject the note.
-  //     }
-  //   }
diff --git a/src/main/include/Intake.h b/src/main/include/Intake.h
index 948f3741..7f945702 100644
--- a/src/main/include/Intake.h
+++ b/src/main/include/Intake.h
@@ -38,5 +38,9 @@ class Intake : public behaviour::HasBehaviour {
   units::volt_t _rawVoltage = 0_V;
   std::string _stringStateName = "error";
   units::volt_t _setVoltage = 0_V;
+  bool _intaking;
+  bool _ejecting;
+  bool _passing;
+  
   std::shared_ptr<nt::NetworkTable> _table = nt::NetworkTableInstance::GetDefault().GetTable("Intake");
 };
diff --git a/src/main/include/IntakeBehaviour.h b/src/main/include/IntakeBehaviour.h
index 419b16d8..931273fa 100644
--- a/src/main/include/IntakeBehaviour.h
+++ b/src/main/include/IntakeBehaviour.h
@@ -24,6 +24,7 @@ class IntakeManualControl : public behaviour::Behaviour {
   bool _rawControl = true;
   bool _intaking = false;
   bool _ejecting = false;
+  bool _passing = false;
 };
 
 class IntakeAutoControl : public behaviour::Behaviour {

From 82d02f9e54ae109c79bd4f239e3da4161d821c9c Mon Sep 17 00:00:00 2001
From: Paul Hodges <paul.hodges@student.education.wa.edu.au>
Date: Tue, 30 Jan 2024 13:02:14 +0800
Subject: [PATCH 076/207] Fixed readme

---
 README.md | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/README.md b/README.md
index 630d1fca..7604ddb3 100644
--- a/README.md
+++ b/README.md
@@ -50,11 +50,11 @@ Cleaning removes caches of your compiled code. If you do not understand an error
 Simulation
 ----------
 **Release**
-`./gradlew :nativeSimulation`
+`./gradlew :simulateNative`
 Runs a simulation of your code at highest optimisation.
 
 **Debug**
-`./gradlew :nativeSimulationDebug`
+`./gradlew :simulateNativeDebug`
 Runs a debug simulation of your code, including a variety of debugging tools similar to glass but at lower optimisation.
 
 Documentation

From 4fff1bfdbcf5470c72fe140843a59616c642c5c8 Mon Sep 17 00:00:00 2001
From: totallysomeoneyoudontknow
 <155517980+totallysomeoneyoudontknow@users.noreply.github.com>
Date: Tue, 30 Jan 2024 14:32:03 +0800
Subject: [PATCH 077/207] Shooter pid (#117)

* added the state raw and changed voltage

* test

* added the state raw and changed voltage

* added pid calculate loop, fixed indenting

* added calculate function on a new line, changed the PID to not stable

* Fixed the if condition so shooterSensor is not tripped

* changed the functions so they start with a capital

* changed the value for shooterMotor

* added shuffleboard and networking tables

* added shuffleboard and networking tables

* added network tables

* fixed merge conflicts and merge errors

* fixed merge conflicts and merge errors

* fixed merge conflicts and merge errors

* fixed errors

* ran wpiformat

* fixed merge errors and conflicts

* fixed github errors

* Alpha Bot Shooter

* shooter with pid loop

* Fixed errors

* Fixing pid loop control

* fixing pid control

* pid loop tuning

* fixed PID control

* ran wpiformat

* ran wpiformat again;

* fixed build

* wpiformat

---------

Co-authored-by: Havish12 <havishchopra10@gmail.com>
Co-authored-by: Isaac Turner <spacey_sooty@outlook.com>
---
 src/main/cpp/Robot.cpp                        |  5 +-
 src/main/cpp/Shooter.cpp                      | 80 +++++++++++++------
 src/main/include/RobotMap.h                   | 50 +++++++-----
 src/main/include/Shooter.h                    |  8 +-
 src/main/include/ShooterBehaviour.h           |  7 +-
 .../src/main/cpp/drivetrain/SwerveDrive.cpp   |  7 +-
 wombat/src/main/cpp/utils/Encoder.cpp         |  2 +-
 7 files changed, 104 insertions(+), 55 deletions(-)

diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp
index 888245e6..2278fa1a 100644
--- a/src/main/cpp/Robot.cpp
+++ b/src/main/cpp/Robot.cpp
@@ -93,7 +93,9 @@ void Robot::RobotPeriodic() {
   shooter->OnUpdate(dt);
   sched->Tick();
 
-  robotmap.swerveTable.swerveDriveTable->GetEntry("frontLeftEncoder")
+  robotmap.swerveTable.swerveDriveTable
+      ->GetEntry("frontLeftEncoder")
+
       .SetDouble(robotmap.swerveBase.moduleConfigs[0].turnMotor.encoder->GetEncoderPosition().value());
   robotmap.swerveTable.swerveDriveTable->GetEntry("frontRightEncoder")
       .SetDouble(robotmap.swerveBase.moduleConfigs[1].turnMotor.encoder->GetEncoderPosition().value());
@@ -116,6 +118,7 @@ void Robot::AutonomousPeriodic() {}
 void Robot::TeleopInit() {
   loop.Clear();
   wom::BehaviourScheduler* sched = wom::BehaviourScheduler::GetInstance();
+  shooter->OnStart();
   sched->InterruptAll();
 
   // frontLeft->SetVoltage(4_V);
diff --git a/src/main/cpp/Shooter.cpp b/src/main/cpp/Shooter.cpp
index 6499e076..c3bebfd8 100644
--- a/src/main/cpp/Shooter.cpp
+++ b/src/main/cpp/Shooter.cpp
@@ -4,15 +4,26 @@
 
 #include "Shooter.h"
 
-Shooter::Shooter(ShooterConfig config)
-    : _config(config)
-// ,
-// _pid{frc::PIDController (1, 0, 0, 0.005_s)}
-{}  // config.path + "/pid", config.pidConfig
+Shooter::Shooter(ShooterConfig config) : _config(config), _pid(config.path + "/pid", config.pidConfig) {}
+
+void Shooter::OnStart() {
+  _pid.Reset();
+}
+
 void Shooter::OnUpdate(units::second_t dt) {
-  // _pid.SetTolerance(0.1, 1);
+  // _pid.SetTolerance(0.5, 4);
+  table->GetEntry("Error").SetDouble(_pid.GetError().value());
+  // table->GetEntry("Acceleration Error").SetDouble(_pid.GetVelocityError());
+  table->GetEntry("SetPoint").SetDouble(_pid.GetSetpoint().value());
+  // table->GetEntry("Current
+  // Pos").SetDouble(_config.ShooterGearbox.encoder->GetEncoderAngularVelocity().value());
+  // table->GetEntry("EncoderValue").SetDouble(_config.ShooterGearbox.encoder->GetVelocityValue());
+  table->GetEntry("Shooting").SetString(_statename);
   switch (_state) {
     case ShooterState::kIdle: {
+      _statename = "Idle";
+      _pid.Reset();
+      holdVoltage = 0_V;
       std::cout << "KIdle" << std::endl;
       _setVoltage = 0_V;
       // if (_shooterSensor.Get()) {
@@ -20,32 +31,48 @@ void Shooter::OnUpdate(units::second_t dt) {
       // }
     } break;
     case ShooterState::kSpinUp: {
-      // std::cout << "KSpinUp" << std::endl;
-      // _pid.SetSetpoint(_goal.value());
-      // units::volt_t pidCalculate =
-      //     units::volt_t{_pid.Calculate(_config.ShooterGearbox.encoder->GetEncoderAngularVelocity().value())};
-      // _setVoltage = pidCalculate;
+      _statename = "SpinUp";
+      std::cout << "KSpinUp" << std::endl;
+      _pid.SetSetpoint(_goal);
+      units::volt_t pidCalculate =
+          units::volt_t{_pid.Calculate(_config.ShooterGearbox.encoder->GetEncoderAngularVelocity(), dt, 0_V)};
+      std::cout << "KShooting" << std::endl;
 
-      // if (_pid.AtSetpoint()) {
-      //   SetState(ShooterState::kShooting);
-      // }
+      if (_pid.IsStable()) {
+        holdVoltage = pidCalculate;
+        std::cout << "STABLE" << std::endl;
+        _state = ShooterState::kShooting;
+      }
+
+      if (holdVoltage.value() == 0) {
+        _setVoltage = pidCalculate;
+      } else {
+        _setVoltage = holdVoltage;
+      }
     } break;
     case ShooterState::kShooting: {
-      // std::cout << "KShooting" << std::endl;
-      // _pid.SetSetpoint(_goal.value());
-      // units::volt_t pidCalculate =
-      //     units::volt_t{_pid.Calculate(_config.ShooterGearbox.encoder->GetEncoderAngularVelocity().value())};
-      // _setVoltage = pidCalculate;
+      _statename = "Shooting";
 
-      // if (!_pid.AtSetpoint()) {
-      //   SetState(ShooterState::kSpinUp);
-      // }
-      // if (_shooterSensor.Get()) {
-      //   SetState(ShooterState::kIdle);
-      // }
+      _pid.SetSetpoint(_goal);
+      units::volt_t pidCalculate =
+          units::volt_t{_pid.Calculate(_config.ShooterGearbox.encoder->GetEncoderAngularVelocity(), dt, 0_V)};
+      std::cout << "KShooting" << std::endl;
+
+      if (_pid.IsStable()) {
+        holdVoltage = pidCalculate;
+        std::cout << "STABLE" << std::endl;
+      }
+
+      if (holdVoltage.value() == 0) {
+        _setVoltage = pidCalculate;
+      } else {
+        _setVoltage = holdVoltage;
+      }
     } break;
 
     case ShooterState::kReverse: {
+      _statename = "Reverse";
+      _pid.Reset();
       _setVoltage = -5_V;
       std::cout << "KReverse" << std::endl;
       // if (!_shooterSensor.Get()) {
@@ -53,6 +80,9 @@ void Shooter::OnUpdate(units::second_t dt) {
       // }
     } break;
     case ShooterState::kRaw: {
+      _statename = "Raw";
+      holdVoltage = 0_V;
+      _pid.Reset();
       _setVoltage = _rawVoltage;
       std::cout << "KRaw" << std::endl;
       // if (_shooterSensor.Get()) {
diff --git a/src/main/include/RobotMap.h b/src/main/include/RobotMap.h
index 733ab1af..60c3ed19 100644
--- a/src/main/include/RobotMap.h
+++ b/src/main/include/RobotMap.h
@@ -31,16 +31,26 @@ struct RobotMap {
   };
   Controllers controllers;
 
-  struct AlphaArmSystem {
-    rev::CANSparkMax alphaArmMotor{12, rev::CANSparkMax::MotorType::kBrushless};
-    rev::CANSparkMax wristMotor{15, rev::CANSparkMax::MotorType::kBrushless};
+  struct Shooter {
+    rev::CANSparkMax shooterMotor{11, rev::CANSparkMax::MotorType::kBrushless};  // Port 11
+    // frc::DigitalInput shooterSensor{2};
 
-    wom::Gearbox alphaArmGearbox{&alphaArmMotor, nullptr, frc::DCMotor::NEO(1)};
-    wom::Gearbox wristGearbox{&wristMotor, nullptr, frc::DCMotor::NEO(1)};
+    // wom::VoltageController shooterMotorGroup =
+    // wom::VoltagedController::Group(shooterMotor);
+    wom::CANSparkMaxEncoder* shooterEncoder = new wom::CANSparkMaxEncoder(&shooterMotor, 0.01_m);
+    wom::Gearbox shooterGearbox{&shooterMotor, shooterEncoder, frc::DCMotor::NEO(1)};
 
-    AlphaArmConfig config{alphaArmGearbox, wristGearbox};
+    wom::utils::PIDConfig<units::radians_per_second, units::volts> pidConfigS{
+        "/armavator/arm/velocityPID/config",
+        0.1_V / (360_deg / 1_s),
+        0.03_V / 25_deg,
+        0.001_V / (90_deg / 1_s / 1_s),
+        5_rad_per_s,
+        10_rad_per_s / 1_s};
+
+    ShooterConfig config{"shooterGearbox", shooterGearbox, pidConfigS};
   };
-  AlphaArmSystem alphaArmSystem;
+  Shooter shooterSystem;
 
   struct IntakeSystem {
     rev::CANSparkMax intakeMotor{2, rev::CANSparkMax::MotorType::kBrushed};
@@ -55,21 +65,6 @@ struct RobotMap {
   };
   IntakeSystem intakeSystem;
 
-  struct Shooter {
-    rev::CANSparkMax shooterMotor{11, rev::CANSparkMax::MotorType::kBrushless};
-    // frc::DigitalInput shooterSensor{2};
-
-    // wom::VoltageController shooterMotorGroup = wom::VoltageController::Group(shooterMotor);
-    // wom::CANSparkMaxEncoder* shooterEncoder = new wom::CANSparkMaxEncoder(&shooterMotor, 0.01_m);
-    wom::Gearbox shooterGearbox{&shooterMotor, nullptr, frc::DCMotor::NEO(1)};
-
-    ShooterConfig config{
-        "shooterGearbox", shooterGearbox,
-        // &shooterSensor,
-    };
-  };
-  Shooter shooterSystem;
-
   struct SwerveBase {
     ctre::phoenix6::hardware::CANcoder frontLeftCancoder{18, "Drivebase"};
     ctre::phoenix6::hardware::CANcoder frontRightCancoder{19, "Drivebase"};
@@ -173,4 +168,15 @@ struct RobotMap {
         nt::NetworkTableInstance::GetDefault().GetTable("swerve");
   };
   SwerveTable swerveTable;
+
+  struct AlphaArmSystem {
+    rev::CANSparkMax alphaArmMotor{12, rev::CANSparkMax::MotorType::kBrushless};
+    rev::CANSparkMax wristMotor{15, rev::CANSparkMax::MotorType::kBrushless};
+
+    wom::Gearbox alphaArmGearbox{&alphaArmMotor, nullptr, frc::DCMotor::NEO(1)};
+    wom::Gearbox wristGearbox{&wristMotor, nullptr, frc::DCMotor::NEO(1)};
+
+    AlphaArmConfig config{alphaArmGearbox, wristGearbox};
+  };
+  AlphaArmSystem alphaArmSystem;
 };
diff --git a/src/main/include/Shooter.h b/src/main/include/Shooter.h
index cf21c3bc..6ef8eef4 100644
--- a/src/main/include/Shooter.h
+++ b/src/main/include/Shooter.h
@@ -17,6 +17,7 @@ struct ShooterConfig {
   wom::Gearbox ShooterGearbox;
   // wom::PIDConfig<units::radians_per_second, units::volt> pidConfig;
   // frc::DigitalInput* shooterSensor;
+  wom::PIDConfig<units::radians_per_second, units::volt> pidConfig;
 };
 
 enum class ShooterState { kIdle, kShooting, kSpinUp, kReverse, kRaw };
@@ -25,6 +26,8 @@ class Shooter : public behaviour::HasBehaviour {
  public:
   explicit Shooter(ShooterConfig config);
 
+  void OnStart();
+
   void OnUpdate(units::second_t dt);
   void SetState(ShooterState state);
   void SetRaw(units::volt_t voltage);
@@ -38,6 +41,9 @@ class Shooter : public behaviour::HasBehaviour {
   units::volt_t _rawVoltage;
   units::radians_per_second_t _goal;
   units::volt_t _setVoltage = 0_V;
-  // frc::PIDController _pid;
+  wom::PIDController<units::radians_per_second, units::volt> _pid;
   // frc::DigitalInput _shooterSensor{0};
+
+  units::volt_t holdVoltage = 0_V;
+  std::string _statename = "default";
 };
diff --git a/src/main/include/ShooterBehaviour.h b/src/main/include/ShooterBehaviour.h
index ff65b1d8..c95f78ae 100644
--- a/src/main/include/ShooterBehaviour.h
+++ b/src/main/include/ShooterBehaviour.h
@@ -6,7 +6,8 @@
 
 #include <frc/XboxController.h>
 
-#include "Robot.h"
+#include <memory>
+
 #include "Shooter.h"
 #include "Wombat.h"
 
@@ -18,7 +19,9 @@ class ShooterManualControl : public behaviour::Behaviour {
 
  private:
   Shooter* _shooter;
-  frc::XboxController* _codriver;
 
   bool _rawControl = true;
+  std::shared_ptr<nt::NetworkTable> table =
+      nt::NetworkTableInstance::GetDefault().GetTable("Shooter Behaviour");
+  frc::XboxController* _codriver;
 };
diff --git a/wombat/src/main/cpp/drivetrain/SwerveDrive.cpp b/wombat/src/main/cpp/drivetrain/SwerveDrive.cpp
index 74fc6d89..38919cb6 100644
--- a/wombat/src/main/cpp/drivetrain/SwerveDrive.cpp
+++ b/wombat/src/main/cpp/drivetrain/SwerveDrive.cpp
@@ -80,7 +80,8 @@ void SwerveModule::OnUpdate(units::second_t dt) {
   //     -torqueLimit, _config.driveMotor.encoder->GetEncoderAngularVelocity());
 
   // driveVoltage =
-  //     units::math::max(units::math::min(driveVoltage, voltageMax), voltageMin);
+  //     units::math::max(units::math::min(driveVoltage, voltageMax),
+  //     voltageMin);
 
   // driveVoltage = units::math::min(driveVoltage, 10_V);
   turnVoltage = units::math::min(turnVoltage, 7_V);
@@ -136,8 +137,8 @@ void SwerveModule::SetZero(units::second_t dt) {
 void SwerveModule::SetPID(units::radian_t angle, units::meters_per_second_t speed, units::second_t dt) {
   _state = SwerveModuleState::kPID;
   // @liam start added
-  // std::cout << "angle Setpoint: " << _anglePIDController.GetSetpoint() << std::endl;
-  // std::cout << "angle value: " << angle.value() << std::endl;
+  // std::cout << "angle Setpoint: " << _anglePIDController.GetSetpoint() <<
+  // std::endl; std::cout << "angle value: " << angle.value() << std::endl;
 
   double diff = std::fmod((_anglePIDController.GetSetpoint() - angle.value()), (2 * 3.1415));
   std::cout << "DIFF value: " << diff << std::endl;
diff --git a/wombat/src/main/cpp/utils/Encoder.cpp b/wombat/src/main/cpp/utils/Encoder.cpp
index 461a8f31..1f5a58c4 100644
--- a/wombat/src/main/cpp/utils/Encoder.cpp
+++ b/wombat/src/main/cpp/utils/Encoder.cpp
@@ -115,7 +115,7 @@ double wom::utils::CANSparkMaxEncoder::GetPosition() const {
 }
 
 double wom::utils::CANSparkMaxEncoder::GetVelocity() const {
-  return _encoder.GetVelocity();
+  return _encoder.GetVelocity() * 3.14159265 * 2 / 60;
 }
 
 wom::utils::TalonFXEncoder::TalonFXEncoder(ctre::phoenix6::hardware::TalonFX* controller,

From 87273a0d8cf9478a23fb0ca4e8094d09a43b3cb7 Mon Sep 17 00:00:00 2001
From: Isaac Turner <spacey_sooty@outlook.com>
Date: Tue, 30 Jan 2024 14:34:14 +0800
Subject: [PATCH 078/207] ran wpiformat

---
 src/main/cpp/Intake.cpp                       |   2 +-
 src/main/cpp/IntakeBehaviour.cpp              |   5 +-
 src/main/cpp/Robot.cpp                        |  40 ++---
 src/main/cpp/Shooter.cpp                      |  14 +-
 src/main/cpp/ShooterBehaviour.cpp             |   9 +-
 src/main/include/Intake.h                     |   2 +-
 src/main/include/Robot.h                      |   7 +-
 src/main/include/RobotMap.h                   |  79 +++++-----
 wombat/src/main/cpp/behaviour/Behaviour.cpp   |  23 +--
 .../main/cpp/behaviour/BehaviourScheduler.cpp |   4 +-
 .../src/main/cpp/behaviour/HasBehaviour.cpp   |   3 +-
 wombat/src/main/cpp/drivetrain/Drivetrain.cpp |   3 +-
 .../src/main/cpp/drivetrain/SwerveDrive.cpp   | 139 +++++++-----------
 .../behaviours/SwerveBehaviours.cpp           |  96 ++++++------
 wombat/src/main/cpp/subsystems/Arm.cpp        |  35 ++---
 wombat/src/main/cpp/subsystems/Elevator.cpp   |  30 ++--
 wombat/src/main/cpp/subsystems/Shooter.cpp    |  22 +--
 wombat/src/main/cpp/utils/Encoder.cpp         |  55 +++----
 wombat/src/main/cpp/utils/Util.cpp            |  27 +---
 wombat/src/main/cpp/vision/Limelight.cpp      |  24 ++-
 wombat/src/main/include/behaviour/Behaviour.h |  42 ++----
 .../src/main/include/behaviour/HasBehaviour.h |   3 +-
 .../src/main/include/drivetrain/SwerveDrive.h |  28 ++--
 .../drivetrain/behaviours/SwerveBehaviours.h  |  17 +--
 wombat/src/main/include/subsystems/Arm.h      |   3 +-
 wombat/src/main/include/subsystems/Shooter.h  |   3 +-
 wombat/src/main/include/utils/Encoder.h       |  27 ++--
 wombat/src/main/include/utils/PID.h           |  69 +++------
 wombat/src/main/include/utils/Util.h          |  55 +++----
 wombat/src/main/include/vision/Limelight.h    |  22 +--
 30 files changed, 345 insertions(+), 543 deletions(-)

diff --git a/src/main/cpp/Intake.cpp b/src/main/cpp/Intake.cpp
index b56d4b47..2181902f 100644
--- a/src/main/cpp/Intake.cpp
+++ b/src/main/cpp/Intake.cpp
@@ -59,7 +59,7 @@ void Intake::OnUpdate(units::second_t dt) {
         _passing = false;
       }
     } break;
-    
+
     default:
       std::cout << "Error: Intake in INVALID STATE." << std::endl;
       break;
diff --git a/src/main/cpp/IntakeBehaviour.cpp b/src/main/cpp/IntakeBehaviour.cpp
index 038c5ce3..59558bba 100644
--- a/src/main/cpp/IntakeBehaviour.cpp
+++ b/src/main/cpp/IntakeBehaviour.cpp
@@ -6,7 +6,8 @@
 
 #include <frc/XboxController.h>
 
-IntakeManualControl::IntakeManualControl(Intake* intake, frc::XboxController& codriver) : _intake(intake), _codriver(codriver) {
+IntakeManualControl::IntakeManualControl(Intake* intake, frc::XboxController& codriver)
+    : _intake(intake), _codriver(codriver) {
   Controls(intake);
 }
 
@@ -24,7 +25,7 @@ void IntakeManualControl::OnTick(units::second_t dt) {
       _intake->setState(IntakeState::kRaw);
     }
   }
-  
+
   if (_rawControl) {
     if (_codriver.GetRightTriggerAxis() > 0.1) {
       _intake->setRaw(_codriver.GetRightTriggerAxis() * 10_V);
diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp
index 9cef56a6..9770d6a9 100644
--- a/src/main/cpp/Robot.cpp
+++ b/src/main/cpp/Robot.cpp
@@ -18,20 +18,16 @@
 #include <units/velocity.h>
 #include <units/voltage.h>
 
-#include <frc/kinematics/DifferentialDriveKinematics.h>
-#include <frc/controller/RamseteController.h>
-#include <frc/Timer.h>
 #include "behaviour/HasBehaviour.h"
 
 static units::second_t lastPeriodic;
 
 void Robot::RobotInit() {
-
   shooter = new Shooter(robotmap.shooterSystem.config);
   wom::BehaviourScheduler::GetInstance()->Register(shooter);
   shooter->SetDefaultBehaviour(
-     [this]() {return wom::make<ShooterManualControl>(shooter, &robotmap.controllers.codriver); });
-  
+      [this]() { return wom::make<ShooterManualControl>(shooter, &robotmap.controllers.codriver); });
+
   sched = wom::BehaviourScheduler::GetInstance();
   m_chooser.SetDefaultOption("Default Auto", "Default Auto");
 
@@ -50,15 +46,11 @@ void Robot::RobotInit() {
 
   robotmap.swerveBase.gyro->Reset();
 
-  _swerveDrive =
-      new wom::SwerveDrive(robotmap.swerveBase.config, frc::Pose2d());
+  _swerveDrive = new wom::SwerveDrive(robotmap.swerveBase.config, frc::Pose2d());
   wom::BehaviourScheduler::GetInstance()->Register(_swerveDrive);
-  _swerveDrive->SetDefaultBehaviour([this]() {
-    return wom::make<wom::ManualDrivebase>(_swerveDrive,
-                                           &robotmap.controllers.driver);
-  });
+  _swerveDrive->SetDefaultBehaviour(
+      [this]() { return wom::make<wom::ManualDrivebase>(_swerveDrive, &robotmap.controllers.driver); });
 
-  
   // m_driveSim = new wom::TempSimSwerveDrive(&simulation_timer, &m_field);
   // m_driveSim = wom::TempSimSwerveDrive();
 
@@ -67,9 +59,6 @@ void Robot::RobotInit() {
   robotmap.swerveBase.moduleConfigs[2].turnMotor.encoder->SetEncoderOffset(0_rad);
   robotmap.swerveBase.moduleConfigs[3].turnMotor.encoder->SetEncoderOffset(0_rad);
 
-
-
-
   // frontLeft = new ctre::phoenix6::hardware::TalonFX(7, "Drivebase");  // front left
   // frontRight = new ctre::phoenix6::hardware::TalonFX(2, "Drivebase");   // front right
   // backLeft = new ctre::phoenix6::hardware::TalonFX(6, "Drivebase");   // back left
@@ -95,10 +84,14 @@ void Robot::RobotPeriodic() {
   shooter->OnUpdate(dt);
   sched->Tick();
 
-  robotmap.swerveTable.swerveDriveTable->GetEntry("frontLeftEncoder").SetDouble(robotmap.swerveBase.moduleConfigs[0].turnMotor.encoder->GetEncoderPosition().value());
-  robotmap.swerveTable.swerveDriveTable->GetEntry("frontRightEncoder").SetDouble(robotmap.swerveBase.moduleConfigs[1].turnMotor.encoder->GetEncoderPosition().value());
-  robotmap.swerveTable.swerveDriveTable->GetEntry("backLeftEncoder").SetDouble(robotmap.swerveBase.moduleConfigs[2].turnMotor.encoder->GetEncoderPosition().value());
-  robotmap.swerveTable.swerveDriveTable->GetEntry("backRightEncoder").SetDouble(robotmap.swerveBase.moduleConfigs[3].turnMotor.encoder->GetEncoderPosition().value());
+  robotmap.swerveTable.swerveDriveTable->GetEntry("frontLeftEncoder")
+      .SetDouble(robotmap.swerveBase.moduleConfigs[0].turnMotor.encoder->GetEncoderPosition().value());
+  robotmap.swerveTable.swerveDriveTable->GetEntry("frontRightEncoder")
+      .SetDouble(robotmap.swerveBase.moduleConfigs[1].turnMotor.encoder->GetEncoderPosition().value());
+  robotmap.swerveTable.swerveDriveTable->GetEntry("backLeftEncoder")
+      .SetDouble(robotmap.swerveBase.moduleConfigs[2].turnMotor.encoder->GetEncoderPosition().value());
+  robotmap.swerveTable.swerveDriveTable->GetEntry("backRightEncoder")
+      .SetDouble(robotmap.swerveBase.moduleConfigs[3].turnMotor.encoder->GetEncoderPosition().value());
 
   intake->OnUpdate(dt);
 }
@@ -107,12 +100,11 @@ void Robot::AutonomousInit() {
   loop.Clear();
   sched->InterruptAll();
 }
-void Robot::AutonomousPeriodic() {
-}
+void Robot::AutonomousPeriodic() {}
 
 void Robot::TeleopInit() {
   loop.Clear();
-  wom::BehaviourScheduler *sched = wom::BehaviourScheduler::GetInstance();
+  wom::BehaviourScheduler* sched = wom::BehaviourScheduler::GetInstance();
   sched->InterruptAll();
 
   // frontLeft->SetVoltage(4_V);
@@ -130,4 +122,4 @@ void Robot::DisabledInit() {}
 void Robot::DisabledPeriodic() {}
 
 void Robot::TestInit() {}
-void Robot::TestPeriodic() {}
\ No newline at end of file
+void Robot::TestPeriodic() {}
diff --git a/src/main/cpp/Shooter.cpp b/src/main/cpp/Shooter.cpp
index 3f1bef83..88a217d9 100644
--- a/src/main/cpp/Shooter.cpp
+++ b/src/main/cpp/Shooter.cpp
@@ -4,10 +4,11 @@
 
 #include "Shooter.h"
 
-Shooter::Shooter(ShooterConfig config) : _config(config)
-// , 
+Shooter::Shooter(ShooterConfig config)
+    : _config(config)
+// ,
 // _pid{frc::PIDController (1, 0, 0, 0.005_s)}
-{} //config.path + "/pid", config.pidConfig
+{}  // config.path + "/pid", config.pidConfig
 void Shooter::OnUpdate(units::second_t dt) {
   // _pid.SetTolerance(0.1, 1);
   // switch (_state) {
@@ -22,7 +23,8 @@ void Shooter::OnUpdate(units::second_t dt) {
   //     // std::cout << "KSpinUp" << std::endl;
   //     // _pid.SetSetpoint(_goal.value());
   //     // units::volt_t pidCalculate =
-  //     //     units::volt_t{_pid.Calculate(_config.ShooterGearbox.encoder->GetEncoderAngularVelocity().value())};
+  //     //
+  //     units::volt_t{_pid.Calculate(_config.ShooterGearbox.encoder->GetEncoderAngularVelocity().value())};
   //     // _setVoltage = pidCalculate;
 
   //     // if (_pid.AtSetpoint()) {
@@ -33,7 +35,8 @@ void Shooter::OnUpdate(units::second_t dt) {
   //     // std::cout << "KShooting" << std::endl;
   //     // _pid.SetSetpoint(_goal.value());
   //     // units::volt_t pidCalculate =
-  //     //     units::volt_t{_pid.Calculate(_config.ShooterGearbox.encoder->GetEncoderAngularVelocity().value())};
+  //     //
+  //     units::volt_t{_pid.Calculate(_config.ShooterGearbox.encoder->GetEncoderAngularVelocity().value())};
   //     // _setVoltage = pidCalculate;
 
   //     // if (!_pid.AtSetpoint()) {
@@ -64,7 +67,6 @@ void Shooter::OnUpdate(units::second_t dt) {
   // }
   // std::cout << "Voltage:" << _setVoltage.value() << std::endl;
   // _config.ShooterGearbox.motorController->SetVoltage(_setVoltage);
-
 }
 
 void Shooter::SetState(ShooterState state) {
diff --git a/src/main/cpp/ShooterBehaviour.cpp b/src/main/cpp/ShooterBehaviour.cpp
index 1a035ae0..341eef15 100644
--- a/src/main/cpp/ShooterBehaviour.cpp
+++ b/src/main/cpp/ShooterBehaviour.cpp
@@ -12,7 +12,6 @@ ShooterManualControl::ShooterManualControl(Shooter* shooter, frc::XboxController
 void ShooterManualControl::OnTick(units::second_t dt) {
   _shooter->table->GetEntry("RawControl").SetBoolean(_rawControl);
 
-
   if (_codriver->GetAButtonReleased()) {
     if (_rawControl) {
       _rawControl = false;
@@ -22,10 +21,10 @@ void ShooterManualControl::OnTick(units::second_t dt) {
   }
 
   if (!_rawControl) {
-      if (_codriver->GetYButton()) {
-        _shooter->SetState(ShooterState::kSpinUp);
-        _shooter->SetPidGoal(10_rad_per_s);
-      }
+    if (_codriver->GetYButton()) {
+      _shooter->SetState(ShooterState::kSpinUp);
+      _shooter->SetPidGoal(10_rad_per_s);
+    }
   } else {
     if (_codriver->GetRightTriggerAxis() > 0.1) {
       _shooter->SetRaw(_codriver->GetRightTriggerAxis() * 12_V);
diff --git a/src/main/include/Intake.h b/src/main/include/Intake.h
index 7f945702..8d9c80b5 100644
--- a/src/main/include/Intake.h
+++ b/src/main/include/Intake.h
@@ -41,6 +41,6 @@ class Intake : public behaviour::HasBehaviour {
   bool _intaking;
   bool _ejecting;
   bool _passing;
-  
+
   std::shared_ptr<nt::NetworkTable> _table = nt::NetworkTableInstance::GetDefault().GetTable("Intake");
 };
diff --git a/src/main/include/Robot.h b/src/main/include/Robot.h
index c5a08964..894b6db5 100644
--- a/src/main/include/Robot.h
+++ b/src/main/include/Robot.h
@@ -1,6 +1,7 @@
 // Copyright (c) 2023-2024 CurtinFRC
 // Open Source Software, you can modify it according to the terms
 // of the MIT License at the root of this project
+
 #pragma once
 #include <frc/Encoder.h>
 #include <frc/TimedRobot.h>
@@ -11,6 +12,7 @@
 #include <frc/smartdashboard/Field2d.h>
 #include <frc/smartdashboard/SendableChooser.h>
 #include <frc/smartdashboard/SmartDashboard.h>
+
 #include <string>
 
 #include "Intake.h"
@@ -19,7 +21,7 @@
 #include "Shooter.h"
 #include "ShooterBehaviour.h"
 #include "Wombat.h"
-#include "RobotMap.h"
+
 class Robot : public frc::TimedRobot {
  public:
   void TestInit() override;
@@ -32,11 +34,12 @@ class Robot : public frc::TimedRobot {
   void TeleopPeriodic() override;
   void DisabledInit() override;
   void DisabledPeriodic() override;
+
  private:
   RobotMap robotmap;
   wom::BehaviourScheduler* sched;
   frc::EventLoop loop;
-  Shooter *shooter;
+  Shooter* shooter;
 
   Intake* intake;
   frc::SendableChooser<std::string> m_chooser;
diff --git a/src/main/include/RobotMap.h b/src/main/include/RobotMap.h
index b4715e3b..b66ec127 100644
--- a/src/main/include/RobotMap.h
+++ b/src/main/include/RobotMap.h
@@ -1,6 +1,7 @@
 // Copyright (c) 2023-2024 CurtinFRC
 // Open Source Software, you can modify it according to the terms
 // of the MIT License at the root of this project
+
 #pragma once
 
 #include <frc/Compressor.h>
@@ -10,8 +11,8 @@
 #include <units/angle.h>
 #include <units/length.h>
 
-#include <string>
 #include <memory>
+#include <string>
 
 #include <ctre/phoenix6/CANcoder.hpp>
 #include <ctre/phoenix6/Pigeon2.hpp>
@@ -38,9 +39,9 @@ struct RobotMap {
 
     wom::Gearbox IntakeGearbox{&intakeMotor, nullptr, frc::DCMotor::CIM(1)};
 
-    IntakeConfig config{IntakeGearbox , &intakeSensor/*, &magSensor, &shooterSensor*/};
-  }; IntakeSystem intakeSystem;
-
+    IntakeConfig config{IntakeGearbox, &intakeSensor /*, &magSensor, &shooterSensor*/};
+  };
+  IntakeSystem intakeSystem;
 
   struct Shooter {
     rev::CANSparkMax shooterMotor{11, rev::CANSparkMax::MotorType::kBrushless};
@@ -51,20 +52,19 @@ struct RobotMap {
     wom::Gearbox shooterGearbox{&shooterMotor, nullptr, frc::DCMotor::NEO(1)};
 
     ShooterConfig config{
-        "shooterGearbox",
-        shooterGearbox,
+        "shooterGearbox", shooterGearbox,
         // &shooterSensor,
     };
-  }; Shooter shooterSystem;
-  
+  };
+  Shooter shooterSystem;
+
   struct SwerveBase {
     ctre::phoenix6::hardware::CANcoder frontLeftCancoder{18, "Drivebase"};
     ctre::phoenix6::hardware::CANcoder frontRightCancoder{19, "Drivebase"};
     ctre::phoenix6::hardware::CANcoder backLeftCancoder{16, "Drivebase"};
     ctre::phoenix6::hardware::CANcoder backRightCancoder{17, "Drivebase"};
 
-    ctre::phoenix6::hardware::Pigeon2* gyro =
-        new ctre::phoenix6::hardware::Pigeon2(20, "Drivebase");
+    ctre::phoenix6::hardware::Pigeon2* gyro = new ctre::phoenix6::hardware::Pigeon2(20, "Drivebase");
     wpi::array<ctre::phoenix6::hardware::TalonFX*, 4> turnMotors{
         new ctre::phoenix6::hardware::TalonFX(7, "Drivebase"),   // front left
         new ctre::phoenix6::hardware::TalonFX(2, "Drivebase"),   // front right
@@ -77,48 +77,38 @@ struct RobotMap {
         new ctre::phoenix6::hardware::TalonFX(3, "Drivebase")};  // back right
 
     wpi::array<wom::SwerveModuleConfig, 4> moduleConfigs{
-        wom::SwerveModuleConfig{ //CORRECT
+        wom::SwerveModuleConfig{
+            // CORRECT
             // front left module
             frc::Translation2d(10.761_in, 9.455_in),
-            wom::Gearbox{
-                driveMotors[0],
-                new wom::TalonFXEncoder(driveMotors[0], 0.0445_m, 6.75),
-                frc::DCMotor::Falcon500(1).WithReduction(6.75)},
-            wom::Gearbox{turnMotors[0],
-                         new wom::CanEncoder(18, 0.0445_m, 4096, 12.8),
+            wom::Gearbox{driveMotors[0], new wom::TalonFXEncoder(driveMotors[0], 0.0445_m, 6.75),
+                         frc::DCMotor::Falcon500(1).WithReduction(6.75)},
+            wom::Gearbox{turnMotors[0], new wom::CanEncoder(18, 0.0445_m, 4096, 12.8),
                          frc::DCMotor::Falcon500(1).WithReduction(12.8)},
             &frontLeftCancoder, 4_in / 2},
-        wom::SwerveModuleConfig{ //CORRECT
+        wom::SwerveModuleConfig{
+            // CORRECT
             // front right module
             frc::Translation2d(10.761_in, -9.455_in),
-            wom::Gearbox{
-                driveMotors[1],
-                new wom::TalonFXEncoder(driveMotors[1], 0.0445_m, 6.75),
-                frc::DCMotor::Falcon500(1).WithReduction(6.75)},
-            wom::Gearbox{turnMotors[1],
-                         new wom::CanEncoder(19, 0.0445_m, 4096, 12.8),
+            wom::Gearbox{driveMotors[1], new wom::TalonFXEncoder(driveMotors[1], 0.0445_m, 6.75),
+                         frc::DCMotor::Falcon500(1).WithReduction(6.75)},
+            wom::Gearbox{turnMotors[1], new wom::CanEncoder(19, 0.0445_m, 4096, 12.8),
                          frc::DCMotor::Falcon500(1).WithReduction(12.8)},
             &frontRightCancoder, 4_in / 2},
         wom::SwerveModuleConfig{
             // back left module
             frc::Translation2d(-10.761_in, 9.455_in),
-            wom::Gearbox{
-                driveMotors[2],
-                new wom::TalonFXEncoder(driveMotors[2], 0.0445_m, 6.75),
-                frc::DCMotor::Falcon500(1).WithReduction(6.75)},
-            wom::Gearbox{turnMotors[2],
-                         new wom::CanEncoder(16, 0.0445_m, 4096, 12.8),
+            wom::Gearbox{driveMotors[2], new wom::TalonFXEncoder(driveMotors[2], 0.0445_m, 6.75),
+                         frc::DCMotor::Falcon500(1).WithReduction(6.75)},
+            wom::Gearbox{turnMotors[2], new wom::CanEncoder(16, 0.0445_m, 4096, 12.8),
                          frc::DCMotor::Falcon500(1).WithReduction(12.8)},
             &backRightCancoder, 4_in / 2},
         wom::SwerveModuleConfig{
             // back right module
             frc::Translation2d(-10.761_in, -9.455_in),
-            wom::Gearbox{
-                driveMotors[3],
-                new wom::TalonFXEncoder(driveMotors[3], 0.0445_m, 6.75),
-                frc::DCMotor::Falcon500(1).WithReduction(6.75)},
-            wom::Gearbox{turnMotors[3],
-                         new wom::CanEncoder(17, 0.0445_m, 4096, 12.8),
+            wom::Gearbox{driveMotors[3], new wom::TalonFXEncoder(driveMotors[3], 0.0445_m, 6.75),
+                         frc::DCMotor::Falcon500(1).WithReduction(6.75)},
+            wom::Gearbox{turnMotors[3], new wom::CanEncoder(17, 0.0445_m, 4096, 12.8),
                          frc::DCMotor::Falcon500(1).WithReduction(12.8)},
             &backLeftCancoder, 4_in / 2},
     };
@@ -138,19 +128,16 @@ struct RobotMap {
         wom::SwerveDriveConfig::pose_angle_conf_t::ki_t{0},
         0_deg / 1_deg};*/
     wom::SwerveDriveConfig::pose_position_conf_t posePositionPID{
-        "/drivetrain/pid/pose/position/config",
-        0_mps / 1_m,
-        wom::SwerveDriveConfig::pose_position_conf_t::ki_t{0.15},
-        0_m / 1_m,
-        0_cm};
+        "/drivetrain/pid/pose/position/config", 0_mps / 1_m,
+        wom::SwerveDriveConfig::pose_position_conf_t::ki_t{0.15}, 0_m / 1_m, 0_cm};
 
     // the config for the whole swerve drive
     wom::SwerveDriveConfig config{"/drivetrain",
-                                  //anglePID,
+                                  // anglePID,
                                   velocityPID,
                                   moduleConfigs,  // each module
                                   gyro,
-                                  //poseAnglePID,
+                                  // poseAnglePID,
                                   posePositionPID,
                                   60_kg,  // robot mass (estimate rn)
                                   {0.1, 0.1, 0.1},
@@ -170,6 +157,8 @@ struct RobotMap {
   SwerveBase swerveBase;
 
   struct SwerveTable {
-    std::shared_ptr<nt::NetworkTable> swerveDriveTable = nt::NetworkTableInstance::GetDefault().GetTable("swerve");
-  }; SwerveTable swerveTable;
+    std::shared_ptr<nt::NetworkTable> swerveDriveTable =
+        nt::NetworkTableInstance::GetDefault().GetTable("swerve");
+  };
+  SwerveTable swerveTable;
 };
diff --git a/wombat/src/main/cpp/behaviour/Behaviour.cpp b/wombat/src/main/cpp/behaviour/Behaviour.cpp
index d21d9e57..7312c922 100644
--- a/wombat/src/main/cpp/behaviour/Behaviour.cpp
+++ b/wombat/src/main/cpp/behaviour/Behaviour.cpp
@@ -10,9 +10,7 @@ using namespace behaviour;
 
 // Behaviour
 Behaviour::Behaviour(std::string name, units::time::second_t period)
-    : _bhvr_name(name),
-      _bhvr_period(period),
-      _bhvr_state(BehaviourState::INITIALISED) {}
+    : _bhvr_name(name), _bhvr_period(period), _bhvr_state(BehaviourState::INITIALISED) {}
 Behaviour::~Behaviour() {
   if (!IsFinished())
     Interrupt();
@@ -82,8 +80,7 @@ bool Behaviour::Tick() {
 
     if (dt > 2 * _bhvr_period) {
       std::cerr << "Behaviour missed deadline. Reduce Period. Dt=" << dt.value()
-                << " Dt(deadline)=" << (2 * _bhvr_period).value()
-                << ". Bhvr: " << GetName() << std::endl;
+                << " Dt(deadline)=" << (2 * _bhvr_period).value() << ". Bhvr: " << GetName() << std::endl;
     }
 
     if (_bhvr_timeout.value() > 0 && _bhvr_timer > _bhvr_timeout)
@@ -100,8 +97,7 @@ bool Behaviour::IsRunning() const {
 }
 
 bool Behaviour::IsFinished() const {
-  return _bhvr_state != BehaviourState::INITIALISED &&
-         _bhvr_state != BehaviourState::RUNNING;
+  return _bhvr_state != BehaviourState::INITIALISED && _bhvr_state != BehaviourState::RUNNING;
 }
 
 void Behaviour::Stop(BehaviourState new_state) {
@@ -173,8 +169,7 @@ void ConcurrentBehaviour::Add(Behaviour::ptr behaviour) {
 }
 
 std::string ConcurrentBehaviour::GetName() const {
-  std::string msg =
-      (_reducer == ConcurrentBehaviourReducer::ALL ? "ALL { " : "RACE {");
+  std::string msg = (_reducer == ConcurrentBehaviourReducer::ALL ? "ALL { " : "RACE {");
   for (auto b : _children)
     msg += b->GetName() + ", ";
   msg += "}";
@@ -190,8 +185,8 @@ void ConcurrentBehaviour::OnStart() {
         using namespace std::chrono_literals;
 
         b->Tick();
-        std::this_thread::sleep_for(std::chrono::milliseconds(
-            static_cast<int64_t>(b->GetPeriod().value() * 1000)));
+        std::this_thread::sleep_for(
+            std::chrono::milliseconds(static_cast<int64_t>(b->GetPeriod().value() * 1000)));
       }
 
       if (IsFinished() && !b->IsFinished())
@@ -273,10 +268,8 @@ void WaitFor::OnTick(units::time::second_t dt) {
 }
 
 // WaitTime
-WaitTime::WaitTime(units::time::second_t time)
-    : WaitTime([time]() { return time; }) {}
-WaitTime::WaitTime(std::function<units::time::second_t()> time_fn)
-    : _time_fn(time_fn) {}
+WaitTime::WaitTime(units::time::second_t time) : WaitTime([time]() { return time; }) {}
+WaitTime::WaitTime(std::function<units::time::second_t()> time_fn) : _time_fn(time_fn) {}
 
 void WaitTime::OnStart() {
   _time = _time_fn();
diff --git a/wombat/src/main/cpp/behaviour/BehaviourScheduler.cpp b/wombat/src/main/cpp/behaviour/BehaviourScheduler.cpp
index 26863c79..eb363b3e 100644
--- a/wombat/src/main/cpp/behaviour/BehaviourScheduler.cpp
+++ b/wombat/src/main/cpp/behaviour/BehaviourScheduler.cpp
@@ -52,8 +52,8 @@ void BehaviourScheduler::Schedule(Behaviour::ptr behaviour) {
         std::lock_guard<std::recursive_mutex> lk(_active_mtx);
         behaviour->Tick();
       }
-      std::this_thread::sleep_for(std::chrono::milliseconds(
-          static_cast<int64_t>(behaviour->GetPeriod().value() * 1000)));
+      std::this_thread::sleep_for(
+          std::chrono::milliseconds(static_cast<int64_t>(behaviour->GetPeriod().value() * 1000)));
     }
   });
 }
diff --git a/wombat/src/main/cpp/behaviour/HasBehaviour.cpp b/wombat/src/main/cpp/behaviour/HasBehaviour.cpp
index 964522c6..f2f557c5 100644
--- a/wombat/src/main/cpp/behaviour/HasBehaviour.cpp
+++ b/wombat/src/main/cpp/behaviour/HasBehaviour.cpp
@@ -8,8 +8,7 @@
 
 using namespace behaviour;
 
-void HasBehaviour::SetDefaultBehaviour(
-    std::function<std::shared_ptr<Behaviour>(void)> fn) {
+void HasBehaviour::SetDefaultBehaviour(std::function<std::shared_ptr<Behaviour>(void)> fn) {
   _default_behaviour_producer = fn;
 }
 
diff --git a/wombat/src/main/cpp/drivetrain/Drivetrain.cpp b/wombat/src/main/cpp/drivetrain/Drivetrain.cpp
index e516e7d7..387bbc31 100644
--- a/wombat/src/main/cpp/drivetrain/Drivetrain.cpp
+++ b/wombat/src/main/cpp/drivetrain/Drivetrain.cpp
@@ -11,8 +11,7 @@
 using namespace frc;
 using namespace units;
 
-wom::drivetrain::Drivetrain::Drivetrain(DrivetrainConfig* config,
-                                        XboxController& driver)
+wom::drivetrain::Drivetrain::Drivetrain(DrivetrainConfig* config, XboxController& driver)
     : _config(config), _driver(driver) {}
 wom::drivetrain::Drivetrain::~Drivetrain() {}
 
diff --git a/wombat/src/main/cpp/drivetrain/SwerveDrive.cpp b/wombat/src/main/cpp/drivetrain/SwerveDrive.cpp
index 00c50e63..74fc6d89 100644
--- a/wombat/src/main/cpp/drivetrain/SwerveDrive.cpp
+++ b/wombat/src/main/cpp/drivetrain/SwerveDrive.cpp
@@ -5,9 +5,9 @@
 #include "drivetrain/SwerveDrive.h"
 
 #include <networktables/NetworkTableInstance.h>
+#include <units/angle.h>
 #include <units/math.h>
 #include <units/voltage.h>
-#include <units/angle.h>
 
 #include <iostream>
 
@@ -16,17 +16,16 @@
 namespace wom {
 namespace drivetrain {
 
-void SwerveModuleConfig::WriteNT(
-    std::shared_ptr<nt::NetworkTable> table) const {
+void SwerveModuleConfig::WriteNT(std::shared_ptr<nt::NetworkTable> table) const {
   std::array<double, 2> pos{position.X().value(), position.Y().value()};
   table->GetEntry("position").SetDoubleArray(std::span(pos));
   table->GetEntry("wheelRadius").SetDouble(wheelRadius.value());
 }
 
 SwerveModule::SwerveModule(std::string path, SwerveModuleConfig config,
-                           //SwerveModule::angle_pid_conf_t anglePID,
+                           // SwerveModule::angle_pid_conf_t anglePID,
                            SwerveModule::velocity_pid_conf_t velocityPID)
-    : // _anglePIDController(path + "/pid/angle", anglePID),
+    :  // _anglePIDController(path + "/pid/angle", anglePID),
       _anglePIDController{frc::PIDController(4, 0, 0, 0.005_s)},
       _config(config),
       _velocityPIDController(frc::PIDController(1.2, 0, 0, 0.005_s)),
@@ -46,8 +45,6 @@ void SwerveModule::OnUpdate(units::second_t dt) {
   units::volt_t driveVoltage{0};
   units::volt_t turnVoltage{0};
 
-
-
   switch (_state) {
     case SwerveModuleState::kIdle:
       driveVoltage = 0_V;
@@ -55,10 +52,11 @@ void SwerveModule::OnUpdate(units::second_t dt) {
       break;
     case SwerveModuleState::kPID: {
       auto feedforward = _config.driveMotor.motor.Voltage(
-          0_Nm, units::radians_per_second_t{
-                    _velocityPIDController.GetSetpoint() / _config.wheelRadius.value()});
-      //std::cout << "GetSpeed()" << GetSpeed().value() << std::endl;
-      // units::radian_t input = units::math::fmod(_config.turnMotor.encoder->GetEncoderPosition(), (2_rad * 3.1415));
+          0_Nm,
+          units::radians_per_second_t{_velocityPIDController.GetSetpoint() / _config.wheelRadius.value()});
+      // std::cout << "GetSpeed()" << GetSpeed().value() << std::endl;
+      //  units::radian_t input = units::math::fmod(_config.turnMotor.encoder->GetEncoderPosition(), (2_rad
+      //  * 3.1415));
       double input = -_config.turnMotor.encoder->GetEncoderPosition().value();
       _table->GetEntry("/testing/GetEncoderPos").SetDouble(input);
       // _velocityPIDController.SetSetpoint(3);
@@ -75,8 +73,7 @@ void SwerveModule::OnUpdate(units::second_t dt) {
       std::cerr << "Case not handled" << std::endl;
   }
 
-  units::newton_meter_t torqueLimit =
-      50_kg / 4 * _config.wheelRadius * _currentAccelerationLimit;
+  units::newton_meter_t torqueLimit = 50_kg / 4 * _config.wheelRadius * _currentAccelerationLimit;
   // units::volt_t voltageMax = _config.driveMotor.motor.Voltage(
   //     torqueLimit, _config.driveMotor.encoder->GetEncoderAngularVelocity());
   // units::volt_t voltageMin = _config.driveMotor.motor.Voltage(
@@ -92,8 +89,7 @@ void SwerveModule::OnUpdate(units::second_t dt) {
   //     units::math::max(driveVoltage, -_driveModuleVoltageLimit),
   //     _driveModuleVoltageLimit);  // was originally 10_V
   units::volt_t turnVoltageMax = 7_V - (driveVoltage * (7_V / 10_V));
-  turnVoltage = units::math::min(units::math::max(turnVoltage, -turnVoltageMax),
-                                 turnVoltageMax);
+  turnVoltage = units::math::min(units::math::max(turnVoltage, -turnVoltageMax), turnVoltageMax);
   // turnVoltage = units::math::min(units::math::max(turnVoltage, -7_V), 7_V);
   // std::cout << "turn-voltage-max: " << turnVoltageMax.value() << std::endl;
 
@@ -106,9 +102,7 @@ void SwerveModule::OnUpdate(units::second_t dt) {
 
   _table->GetEntry("speed").SetDouble(GetSpeed().value());
   _table->GetEntry("angle").SetDouble(
-      _config.turnMotor.encoder->GetEncoderPosition()
-          .convert<units::degree>()
-          .value());
+      _config.turnMotor.encoder->GetEncoderPosition().convert<units::degree>().value());
   _config.WriteNT(_table->GetSubTable("config"));
 }
 
@@ -116,13 +110,11 @@ void SwerveModule::OnUpdate(units::second_t dt) {
 //   return (_config.turnMotor.encoder->GetEncoderPosition().value());
 // }
 
-void SwerveModule::SetAccelerationLimit(
-    units::meters_per_second_squared_t limit) {
+void SwerveModule::SetAccelerationLimit(units::meters_per_second_squared_t limit) {
   _currentAccelerationLimit = limit;
 }
 
-void SwerveDrive::SetAccelerationLimit(
-    units::meters_per_second_squared_t limit) {
+void SwerveDrive::SetAccelerationLimit(units::meters_per_second_squared_t limit) {
   for (int motorNumber = 0; motorNumber < 4; motorNumber++) {
     _modules[motorNumber].SetAccelerationLimit(limit);
   }
@@ -141,16 +133,13 @@ void SwerveModule::SetZero(units::second_t dt) {
   _state = SwerveModuleState::kPID;
 }
 
-void SwerveModule::SetPID(units::radian_t angle,
-                          units::meters_per_second_t speed,
-                          units::second_t dt) {
+void SwerveModule::SetPID(units::radian_t angle, units::meters_per_second_t speed, units::second_t dt) {
   _state = SwerveModuleState::kPID;
   // @liam start added
   // std::cout << "angle Setpoint: " << _anglePIDController.GetSetpoint() << std::endl;
   // std::cout << "angle value: " << angle.value() << std::endl;
 
-  double diff = std::fmod((_anglePIDController.GetSetpoint() - angle.value()),
-                          (2 * 3.1415));
+  double diff = std::fmod((_anglePIDController.GetSetpoint() - angle.value()), (2 * 3.1415));
   std::cout << "DIFF value: " << diff << std::endl;
   _table->GetEntry("/Differential value:").SetDouble(diff);
   if (std::abs(diff) >= 3.1415) {
@@ -165,39 +154,32 @@ void SwerveModule::SetPID(units::radian_t angle,
 
 void SwerveModule::ModuleVectorHandler(frc::ChassisSpeeds speeds) {
   units::meters_per_second_t xVelocityComponent =
-      1_mps *
-      (speeds.vx.value() + speeds.omega.value() * _config.position.X().value());
+      1_mps * (speeds.vx.value() + speeds.omega.value() * _config.position.X().value());
   units::meters_per_second_t yVelocityComponent =
-      1_mps *
-      (speeds.vy.value() + speeds.omega.value() * _config.position.Y().value());
+      1_mps * (speeds.vy.value() + speeds.omega.value() * _config.position.Y().value());
 
   units::meters_per_second_t velocity =
-      1_mps * std::sqrt(std::pow(xVelocityComponent.value(), 2) +
-                        std::pow(yVelocityComponent.value(), 2));
-  units::degree_t angle = 1_rad * std::atan2(yVelocityComponent.value(),
-                                             xVelocityComponent.value());
+      1_mps * std::sqrt(std::pow(xVelocityComponent.value(), 2) + std::pow(yVelocityComponent.value(), 2));
+  units::degree_t angle = 1_rad * std::atan2(yVelocityComponent.value(), xVelocityComponent.value());
 
   _anglePIDController.SetSetpoint(angle.value());
   _velocityPIDController.SetSetpoint(velocity.value());
 }
 
 units::meters_per_second_t SwerveModule::GetSpeed() const {
-  units::meters_per_second_t returnVal{
-    _config.driveMotor.encoder->GetVelocityValue() *
-    _config.wheelRadius.value()};
-    // std::cout << returnVal.value() << std::endl;
-    return returnVal;
+  units::meters_per_second_t returnVal{_config.driveMotor.encoder->GetVelocityValue() *
+                                       _config.wheelRadius.value()};
+  // std::cout << returnVal.value() << std::endl;
+  return returnVal;
 }
 
 units::meter_t SwerveModule::GetDistance() const {
-  return units::meter_t{
-      _config.driveMotor.encoder->GetEncoderPosition().value() *
-      _config.wheelRadius.value()};
+  return units::meter_t{_config.driveMotor.encoder->GetEncoderPosition().value() *
+                        _config.wheelRadius.value()};
 }
 
 frc::SwerveModulePosition SwerveModule::GetPosition() const {
-  return frc::SwerveModulePosition{
-      GetDistance(), _config.turnMotor.encoder->GetEncoderPosition()};
+  return frc::SwerveModulePosition{GetDistance(), _config.turnMotor.encoder->GetEncoderPosition()};
 }
 
 const SwerveModuleConfig& SwerveModule::GetConfig() const {
@@ -210,15 +192,14 @@ void SwerveDriveConfig::WriteNT(std::shared_ptr<nt::NetworkTable> table) {
 
 SwerveDrive::SwerveDrive(SwerveDriveConfig config, frc::Pose2d initialPose)
     : _config(config),
-      _kinematics(_config.modules[0].position, _config.modules[1].position,
-                  _config.modules[2].position, _config.modules[3].position),
+      _kinematics(_config.modules[0].position, _config.modules[1].position, _config.modules[2].position,
+                  _config.modules[3].position),
       _poseEstimator(
           _kinematics, frc::Rotation2d(0_deg),
-          wpi::array<frc::SwerveModulePosition, 4>{
-              frc::SwerveModulePosition{0_m, frc::Rotation2d{0_deg}},
-              frc::SwerveModulePosition{0_m, frc::Rotation2d{0_deg}},
-              frc::SwerveModulePosition{0_m, frc::Rotation2d{0_deg}},
-              frc::SwerveModulePosition{0_m, frc::Rotation2d{0_deg}}},
+          wpi::array<frc::SwerveModulePosition, 4>{frc::SwerveModulePosition{0_m, frc::Rotation2d{0_deg}},
+                                                   frc::SwerveModulePosition{0_m, frc::Rotation2d{0_deg}},
+                                                   frc::SwerveModulePosition{0_m, frc::Rotation2d{0_deg}},
+                                                   frc::SwerveModulePosition{0_m, frc::Rotation2d{0_deg}}},
           initialPose, _config.stateStdDevs, _config.visionMeasurementStdDevs),
       _anglePIDController{frc::PIDController(1, 0, 0)},
       _xPIDController(config.path + "/pid/x", _config.posePositionPID),
@@ -236,14 +217,12 @@ SwerveDrive::SwerveDrive(SwerveDriveConfig config, frc::Pose2d initialPose)
   ResetPose(initialPose);
 }
 
-frc::ChassisSpeeds FieldRelativeSpeeds::ToChassisSpeeds(
-    const units::radian_t robotHeading) {
-  return frc::ChassisSpeeds::FromFieldRelativeSpeeds(
-      vx, vy, omega, frc::Rotation2d{robotHeading});
+frc::ChassisSpeeds FieldRelativeSpeeds::ToChassisSpeeds(const units::radian_t robotHeading) {
+  return frc::ChassisSpeeds::FromFieldRelativeSpeeds(vx, vy, omega, frc::Rotation2d{robotHeading});
 }
 
 void SwerveDrive::OnUpdate(units::second_t dt) {
-  //std::cout << _config.gyro->GetYaw().GetValue().value() << std::endl;
+  // std::cout << _config.gyro->GetYaw().GetValue().value() << std::endl;
   switch (_state) {
     case SwerveDriveState::kZeroing:
       for (auto mod = _modules.begin(); mod < _modules.end(); mod++) {
@@ -258,14 +237,15 @@ void SwerveDrive::OnUpdate(units::second_t dt) {
     case SwerveDriveState::kPose: {
       _target_fr_speeds.vx = _xPIDController.Calculate(GetPose().X(), dt);
       _target_fr_speeds.vy = _yPIDController.Calculate(GetPose().Y(), dt);
-      _target_fr_speeds.omega = units::radians_per_second_t{_anglePIDController.Calculate(GetPose().Rotation().Radians().value())};
+      _target_fr_speeds.omega =
+          units::radians_per_second_t{_anglePIDController.Calculate(GetPose().Rotation().Radians().value())};
     }
       [[fallthrough]];
     case SwerveDriveState::kFieldRelativeVelocity:
-      _target_speed =
-          _target_fr_speeds.ToChassisSpeeds(GetPose().Rotation().Radians());
+      _target_speed = _target_fr_speeds.ToChassisSpeeds(GetPose().Rotation().Radians());
       if (isRotateToMatchJoystick) {
-        _target_speed.omega = units::radians_per_second_t{_anglePIDController.Calculate(GetPose().Rotation().Radians().value())};
+        _target_speed.omega = units::radians_per_second_t{
+            _anglePIDController.Calculate(GetPose().Rotation().Radians().value())};
       }
       // std::cout << "vx = " << _target_speed.vx.value() << " vy = " <<
       // _target_fr_speeds.vy.value() << std::endl;
@@ -273,8 +253,7 @@ void SwerveDrive::OnUpdate(units::second_t dt) {
     case SwerveDriveState::kVelocity: {
       auto target_states = _kinematics.ToSwerveModuleStates(_target_speed);
       for (size_t i = 0; i < _modules.size(); i++) {
-        _modules[i].SetPID(target_states[i].angle.Radians(),
-                           target_states[i].speed, dt);
+        _modules[i].SetPID(target_states[i].angle.Radians(), target_states[i].speed, dt);
         // std::cout << "module " << i << ": " <<
         // target_states[i].angle.Radians().value() << std::endl;
       }
@@ -297,13 +276,12 @@ void SwerveDrive::OnUpdate(units::second_t dt) {
     case SwerveDriveState::kFRVelocityRotationLock:
       _target_speed.vx = _xPIDController.Calculate(GetPose().X(), dt);
       _target_speed.vy = _yPIDController.Calculate(GetPose().Y(), dt);
-      _target_speed.omega = units::radians_per_second_t{_anglePIDController.Calculate(GetPose().Rotation().Radians().value())};
-      _target_speed =
-          _requestedSpeeds.ToChassisSpeeds(GetPose().Rotation().Radians());
+      _target_speed.omega =
+          units::radians_per_second_t{_anglePIDController.Calculate(GetPose().Rotation().Radians().value())};
+      _target_speed = _requestedSpeeds.ToChassisSpeeds(GetPose().Rotation().Radians());
       auto target_states = _kinematics.ToSwerveModuleStates(_target_speed);
       for (size_t i = 0; i < _modules.size(); i++) {
-        _modules[i].SetPID(target_states[i].angle.Radians(),
-                           target_states[i].speed, dt);
+        _modules[i].SetPID(target_states[i].angle.Radians(), target_states[i].speed, dt);
       }
       break;
   }
@@ -314,12 +292,10 @@ void SwerveDrive::OnUpdate(units::second_t dt) {
 
   _poseEstimator.Update(
       _config.gyro->GetRotation2d(),
-      wpi::array<frc::SwerveModulePosition, 4>{
-          _modules[0].GetPosition(), _modules[1].GetPosition(),
-          _modules[2].GetPosition(), _modules[3].GetPosition()});
+      wpi::array<frc::SwerveModulePosition, 4>{_modules[0].GetPosition(), _modules[1].GetPosition(),
+                                               _modules[2].GetPosition(), _modules[3].GetPosition()});
 
-  utils::WritePose2NT(_table->GetSubTable("estimatedPose"),
-                      _poseEstimator.GetEstimatedPosition());
+  utils::WritePose2NT(_table->GetSubTable("estimatedPose"), _poseEstimator.GetEstimatedPosition());
   _config.WriteNT(_table->GetSubTable("config"));
 }
 
@@ -358,8 +334,7 @@ void SwerveDrive::OnResetMode() {
   _anglePIDController.Reset();
 }
 
-void SwerveDrive::RotateMatchJoystick(units::radian_t joystickAngle,
-                                      FieldRelativeSpeeds speeds) {
+void SwerveDrive::RotateMatchJoystick(units::radian_t joystickAngle, FieldRelativeSpeeds speeds) {
   // _state = SwerveDriveState::kFRVelocityRotationLock;
   // _anglePIDController.SetSetpoint(joystickAngle);
   // _target_fr_speeds = speeds;
@@ -385,16 +360,14 @@ bool SwerveDrive::GetIsFieldRelative() {
   return _isFieldRelative;
 }
 
-void SwerveDrive::SetIndividualTuning(int mod, units::radian_t angle,
-                                      units::meters_per_second_t speed) {
+void SwerveDrive::SetIndividualTuning(int mod, units::radian_t angle, units::meters_per_second_t speed) {
   _mod = mod;
   _angle = angle;
   _speed = speed;
   _state = SwerveDriveState::kIndividualTuning;
 }
 
-void SwerveDrive::SetTuning(units::radian_t angle,
-                            units::meters_per_second_t speed) {
+void SwerveDrive::SetTuning(units::radian_t angle, units::meters_per_second_t speed) {
   _angle = angle;
   _speed = speed;
   _state = SwerveDriveState::kTuning;
@@ -421,9 +394,8 @@ bool SwerveDrive::IsAtSetPose() {
 void SwerveDrive::ResetPose(frc::Pose2d pose) {
   _poseEstimator.ResetPosition(
       _config.gyro->GetRotation2d(),
-      wpi::array<frc::SwerveModulePosition, 4>{
-          _modules[0].GetPosition(), _modules[1].GetPosition(),
-          _modules[2].GetPosition(), _modules[3].GetPosition()},
+      wpi::array<frc::SwerveModulePosition, 4>{_modules[0].GetPosition(), _modules[1].GetPosition(),
+                                               _modules[2].GetPosition(), _modules[3].GetPosition()},
       pose);
 }
 
@@ -431,8 +403,7 @@ frc::Pose2d SwerveDrive::GetPose() {
   return _poseEstimator.GetEstimatedPosition();
 }
 
-void SwerveDrive::AddVisionMeasurement(frc::Pose2d pose,
-                                       units::second_t timestamp) {
+void SwerveDrive::AddVisionMeasurement(frc::Pose2d pose, units::second_t timestamp) {
   _poseEstimator.AddVisionMeasurement(pose, timestamp);
 }
 }  // namespace drivetrain
diff --git a/wombat/src/main/cpp/drivetrain/behaviours/SwerveBehaviours.cpp b/wombat/src/main/cpp/drivetrain/behaviours/SwerveBehaviours.cpp
index a82c84d8..23baf7fd 100644
--- a/wombat/src/main/cpp/drivetrain/behaviours/SwerveBehaviours.cpp
+++ b/wombat/src/main/cpp/drivetrain/behaviours/SwerveBehaviours.cpp
@@ -73,40 +73,36 @@ void ManualDrivebase::OnTick(units::second_t deltaTime) {
   // if (isZero) {
   //   _swerveDrivebase->SetZeroing();
   // } else {
-    double xVelocity = wom::utils::spow2(-wom::utils::deadzone(
-        _driverController->GetLeftY(),
-        driverDeadzone));  // GetLeftY due to x being where y should be on field
-    double yVelocity = wom::utils::spow2(
-        -wom::utils::deadzone(_driverController->GetLeftX(), driverDeadzone));
-
-    double r_x = wom::utils::spow2(
-        -wom::utils::deadzone(_driverController->GetRightX(), turningDeadzone));
-
-    double turnX = _driverController->GetRightX();
-    double turnY = _driverController->GetRightY();
-    double num = std::sqrt(turnX * turnX + turnY * turnY);
-    if (num < turningDeadzone) {
-      turnX = 0;
-      turnY = 0;
-    }
-
-    // if (isRotateMatch) {
-    //   units::degree_t currentAngle =
-    //       _swerveDrivebase->GetPose().Rotation().Degrees();
-    //   CalculateRequestedAngle(turnX, turnY, currentAngle);
-    //   _swerveDriveTable->GetEntry("RotateMatch")
-    //       .SetDouble(_requestedAngle.value());
-    //   _swerveDrivebase->RotateMatchJoystick(
-    //       _requestedAngle,
-    //       wom::drivetrain::FieldRelativeSpeeds{// also field relative
-    //                                            xVelocity * maxMovementMagnitude,
-    //                                            yVelocity * maxMovementMagnitude,
-    //                                            r_x * maxRotationMagnitude});
-    // } else {
-      _swerveDrivebase->SetFieldRelativeVelocity(
-          wom::drivetrain::FieldRelativeSpeeds{xVelocity * maxMovementMagnitude,
-                                               yVelocity * maxMovementMagnitude,
-                                               r_x * maxRotationMagnitude});
+  double xVelocity = wom::utils::spow2(
+      -wom::utils::deadzone(_driverController->GetLeftY(),
+                            driverDeadzone));  // GetLeftY due to x being where y should be on field
+  double yVelocity = wom::utils::spow2(-wom::utils::deadzone(_driverController->GetLeftX(), driverDeadzone));
+
+  double r_x = wom::utils::spow2(-wom::utils::deadzone(_driverController->GetRightX(), turningDeadzone));
+
+  double turnX = _driverController->GetRightX();
+  double turnY = _driverController->GetRightY();
+  double num = std::sqrt(turnX * turnX + turnY * turnY);
+  if (num < turningDeadzone) {
+    turnX = 0;
+    turnY = 0;
+  }
+
+  // if (isRotateMatch) {
+  //   units::degree_t currentAngle =
+  //       _swerveDrivebase->GetPose().Rotation().Degrees();
+  //   CalculateRequestedAngle(turnX, turnY, currentAngle);
+  //   _swerveDriveTable->GetEntry("RotateMatch")
+  //       .SetDouble(_requestedAngle.value());
+  //   _swerveDrivebase->RotateMatchJoystick(
+  //       _requestedAngle,
+  //       wom::drivetrain::FieldRelativeSpeeds{// also field relative
+  //                                            xVelocity * maxMovementMagnitude,
+  //                                            yVelocity * maxMovementMagnitude,
+  //                                            r_x * maxRotationMagnitude});
+  // } else {
+  _swerveDrivebase->SetFieldRelativeVelocity(wom::drivetrain::FieldRelativeSpeeds{
+      xVelocity * maxMovementMagnitude, yVelocity * maxMovementMagnitude, r_x * maxRotationMagnitude});
   //   }
   // }
   // _swerveDrivebase->SetTuning(100_deg, 1_mps);
@@ -117,8 +113,7 @@ void ManualDrivebase::ResetMode() {
   resetMode = false;
 }
 
-void ManualDrivebase::CalculateRequestedAngle(double joystickX,
-                                              double joystickY,
+void ManualDrivebase::CalculateRequestedAngle(double joystickX, double joystickY,
                                               units::degree_t defaultAngle) {
   _requestedAngle = (1_rad * std::atan2(joystickY, -joystickX)) + 90_deg;
   if (wom::utils::deadzone(joystickX) == 0 && wom::utils::deadzone(joystickY) == 0) {
@@ -127,8 +122,7 @@ void ManualDrivebase::CalculateRequestedAngle(double joystickX,
 }
 
 // Code for x-ing the wheels on the drivebase
-XDrivebase::XDrivebase(wom::drivetrain::SwerveDrive* swerveDrivebase)
-    : _swerveDrivebase(swerveDrivebase) {
+XDrivebase::XDrivebase(wom::drivetrain::SwerveDrive* swerveDrivebase) : _swerveDrivebase(swerveDrivebase) {
   Controls(swerveDrivebase);
 }
 void XDrivebase::OnTick(units::second_t deltaTime) {
@@ -170,20 +164,17 @@ void XDrivebase::OnTick(units::second_t deltaTime) {
 //   m_timer.Start();
 // }
 
-wom::drivetrain::behaviours::TempSimSwerveDrive::TempSimSwerveDrive(
-    frc::Timer* timer, frc::Field2d* field)
+wom::drivetrain::behaviours::TempSimSwerveDrive::TempSimSwerveDrive(frc::Timer* timer, frc::Field2d* field)
     : m_timer(timer), m_field(field) {}
 
 void wom::drivetrain::behaviours::TempSimSwerveDrive::OnUpdate() {
   m_field->SetRobotPose(m_driveSim.GetPose());
 
   // get the current trajectory state
-  frc::Trajectory::State desired_state =
-      current_trajectory.Sample(m_timer->Get());
+  frc::Trajectory::State desired_state = current_trajectory.Sample(m_timer->Get());
 
   // get the current wheel speeds
-  wom::utils::WriteTrajectoryState(current_trajectory_state_table,
-                                   desired_state);
+  wom::utils::WriteTrajectoryState(current_trajectory_state_table, desired_state);
 
   // move drivebase position to the desired state
   m_driveSim.SetPose(wom::utils::TrajectoryStateToPose2d(desired_state));
@@ -201,19 +192,16 @@ frc::Pose2d wom::drivetrain::behaviours::TempSimSwerveDrive::GetPose2d() {
   return m_driveSim.GetPose();
 }
 
-void wom::drivetrain::behaviours::TempSimSwerveDrive::SetPath(
-    std::string path) {
+void wom::drivetrain::behaviours::TempSimSwerveDrive::SetPath(std::string path) {
   nt::NetworkTableInstance inst = nt::NetworkTableInstance::GetDefault();
   std::shared_ptr<nt::NetworkTable> table = inst.GetTable("FMSInfo");
 
   // create a netowrk table for the trajectory
   std::shared_ptr<nt::NetworkTable> trajectory_table =
       nt::NetworkTableInstance::GetDefault().GetTable("trajectory_path");
-  current_trajectory_table =
-      nt::NetworkTableInstance::GetDefault().GetTable("current_trajectory");
+  current_trajectory_table = nt::NetworkTableInstance::GetDefault().GetTable("current_trajectory");
   current_trajectory_state_table =
-      nt::NetworkTableInstance::GetDefault().GetTable(
-          "current_trajectory_state");
+      nt::NetworkTableInstance::GetDefault().GetTable("current_trajectory_state");
 
   current_trajectory = m_pathplanner.getTrajectory(path);
   m_driveSim.SetPose(current_trajectory.Sample(0_s).pose);
@@ -221,12 +209,10 @@ void wom::drivetrain::behaviours::TempSimSwerveDrive::SetPath(
   m_timer->Start();
 }
 
-wom::drivetrain::behaviours::AutoSwerveDrive::AutoSwerveDrive(
-    wom::drivetrain::SwerveDrive* swerve, frc::Timer* timer,
-    frc::Field2d* field)
+wom::drivetrain::behaviours::AutoSwerveDrive::AutoSwerveDrive(wom::drivetrain::SwerveDrive* swerve,
+                                                              frc::Timer* timer, frc::Field2d* field)
     : _swerve(swerve), m_timer(timer), m_field(field) {
-  _simSwerveDrive =
-      new wom::drivetrain::behaviours::TempSimSwerveDrive(timer, field);
+  _simSwerveDrive = new wom::drivetrain::behaviours::TempSimSwerveDrive(timer, field);
 }
 
 void wom::drivetrain::behaviours::AutoSwerveDrive::OnUpdate() {
diff --git a/wombat/src/main/cpp/subsystems/Arm.cpp b/wombat/src/main/cpp/subsystems/Arm.cpp
index 9390601a..148cec43 100644
--- a/wombat/src/main/cpp/subsystems/Arm.cpp
+++ b/wombat/src/main/cpp/subsystems/Arm.cpp
@@ -10,19 +10,14 @@ using namespace frc;
 using namespace wom;
 
 // creates network table instatnce on shuffleboard
-void wom::subsystems::ArmConfig::WriteNT(
-    std::shared_ptr<nt::NetworkTable> table) {
+void wom::subsystems::ArmConfig::WriteNT(std::shared_ptr<nt::NetworkTable> table) {
   table->GetEntry("armMass").SetDouble(armMass.value());
   table->GetEntry("loadMass").SetDouble(loadMass.value());
   table->GetEntry("armLength").SetDouble(armLength.value());
-  table->GetEntry("minAngle")
-      .SetDouble(minAngle.convert<units::degree>().value());
-  table->GetEntry("maxAngle")
-      .SetDouble(maxAngle.convert<units::degree>().value());
-  table->GetEntry("initialAngle")
-      .SetDouble(initialAngle.convert<units::degree>().value());
-  table->GetEntry("angleOffset")
-      .SetDouble(initialAngle.convert<units::degree>().value());
+  table->GetEntry("minAngle").SetDouble(minAngle.convert<units::degree>().value());
+  table->GetEntry("maxAngle").SetDouble(maxAngle.convert<units::degree>().value());
+  table->GetEntry("initialAngle").SetDouble(initialAngle.convert<units::degree>().value());
+  table->GetEntry("angleOffset").SetDouble(initialAngle.convert<units::degree>().value());
 }
 
 // arm config is used
@@ -43,14 +38,12 @@ void wom::subsystems::Arm::OnUpdate(units::second_t dt) {
     case ArmState::kIdle:
       break;
     case ArmState::kVelocity: {
-      units::newton_meter_t torque =
-          9.81_m / 1_s / 1_s * _config.armLength *
-          units::math::cos(angle + _config.angleOffset) *
-          (0.5 * _config.armMass + _config.loadMass);
+      units::newton_meter_t torque = 9.81_m / 1_s / 1_s * _config.armLength *
+                                     units::math::cos(angle + _config.angleOffset) *
+                                     (0.5 * _config.armMass + _config.loadMass);
       // units::volt_t feedforward = _config.leftGearbox.motor.Voltage(torque,
       // 0_rad/1_s);
-      units::volt_t feedforward =
-          _config.leftGearbox.motor.Voltage(torque, _velocityPID.GetSetpoint());
+      units::volt_t feedforward = _config.leftGearbox.motor.Voltage(torque, _velocityPID.GetSetpoint());
       // feedforward = 3.5_V;
       // std::cout << "feedforward" << feedforward.value() << std::endl;
       voltage = _velocityPID.Calculate(GetArmVelocity(), dt, feedforward);
@@ -58,12 +51,10 @@ void wom::subsystems::Arm::OnUpdate(units::second_t dt) {
       // std::endl; voltage = 0_V;
     } break;
     case ArmState::kAngle: {
-      units::newton_meter_t torque =
-          9.81_m / 1_s / 1_s * _config.armLength *
-          units::math::cos(angle + _config.angleOffset) *
-          (0.5 * _config.armMass + _config.loadMass);
-      units::volt_t feedforward =
-          _config.leftGearbox.motor.Voltage(torque, 0_rad / 1_s);
+      units::newton_meter_t torque = 9.81_m / 1_s / 1_s * _config.armLength *
+                                     units::math::cos(angle + _config.angleOffset) *
+                                     (0.5 * _config.armMass + _config.loadMass);
+      units::volt_t feedforward = _config.leftGearbox.motor.Voltage(torque, 0_rad / 1_s);
       // std::cout << "feedforward" << feedforward.value() << std::endl;
       voltage = _pid.Calculate(angle, dt, feedforward);
     } break;
diff --git a/wombat/src/main/cpp/subsystems/Elevator.cpp b/wombat/src/main/cpp/subsystems/Elevator.cpp
index d6b59f8a..d1cf9a8f 100644
--- a/wombat/src/main/cpp/subsystems/Elevator.cpp
+++ b/wombat/src/main/cpp/subsystems/Elevator.cpp
@@ -7,8 +7,7 @@
 #include <networktables/NetworkTableInstance.h>
 #include <units/acceleration.h>
 
-void wom::subsystems::ElevatorConfig::WriteNT(
-    std::shared_ptr<nt::NetworkTable> table) {
+void wom::subsystems::ElevatorConfig::WriteNT(std::shared_ptr<nt::NetworkTable> table) {
   table->GetEntry("radius").SetDouble(radius.value());
   table->GetEntry("mass").SetDouble(mass.value());
   table->GetEntry("maxHeight").SetDouble(maxHeight.value());
@@ -20,8 +19,7 @@ wom::subsystems::Elevator::Elevator(ElevatorConfig config)
       _pid{config.path + "/pid", config.pid},
       _velocityPID{config.path + "/velocityPID", config.velocityPID},
       _table(nt::NetworkTableInstance::GetDefault().GetTable(config.path)) {
-  _config.leftGearbox.encoder->SetEncoderPosition(_config.initialHeight /
-                                                  _config.radius * 1_rad);
+  _config.leftGearbox.encoder->SetEncoderPosition(_config.initialHeight / _config.radius * 1_rad);
 }
 
 void wom::subsystems::Elevator::OnUpdate(units::second_t dt) {
@@ -39,8 +37,7 @@ void wom::subsystems::Elevator::OnUpdate(units::second_t dt) {
     case ElevatorState::kVelocity: {
       units::volt_t feedforward = _config.rightGearbox.motor.Voltage(
           (_config.mass * 9.81_mps_sq) * _config.radius,
-          _velocityPID.GetSetpoint() /
-              (14.0 / 60.0 * 2.0 * 3.1415 * 0.02225 * 1_m) * 1_rad);
+          _velocityPID.GetSetpoint() / (14.0 / 60.0 * 2.0 * 3.1415 * 0.02225 * 1_m) * 1_rad);
       // units::volt_t feedforward = _config.rightGearbox.motor.Voltage(0_Nm,
       // _velocityPID.GetSetpoint() / (14.0/60.0 * 2.0 * 3.1415 * 0.02225 * 1_m)
       // * 1_rad);
@@ -53,8 +50,8 @@ void wom::subsystems::Elevator::OnUpdate(units::second_t dt) {
       // voltage = 0_V;
     } break;
     case ElevatorState::kPID: {
-      units::volt_t feedforward = _config.rightGearbox.motor.Voltage(
-          (_config.mass * 9.81_mps_sq) * _config.radius, 0_rad_per_s);
+      units::volt_t feedforward =
+          _config.rightGearbox.motor.Voltage((_config.mass * 9.81_mps_sq) * _config.radius, 0_rad_per_s);
       // std::cout << "feed forward" << feedforward.value() << std::endl;
       feedforward = 1.2_V;
       // voltage = _pid.Calculate(height, dt, feedforward);
@@ -99,8 +96,7 @@ void wom::subsystems::Elevator::SetPID(units::meter_t height) {
   _pid.SetSetpoint(height);
 }
 
-void wom::subsystems::Elevator::SetVelocity(
-    units::meters_per_second_t velocity) {
+void wom::subsystems::Elevator::SetVelocity(units::meters_per_second_t velocity) {
   _velocityPID.SetSetpoint(velocity);
 }
 
@@ -132,18 +128,14 @@ units::meter_t wom::subsystems::Elevator::GetHeight() const {
   // std::cout << "elevator position"<<
   // _config.rightGearbox.encoder->GetEncoderTicks() << std::endl; return
   // _config.rightGearbox.encoder->GetEncoderDistance() * 1_m;
-  return _config.elevatorEncoder.GetPosition() * 14 / 60 * 2 * 3.1415 *
-         0.02225 * 1_m;
+  return _config.elevatorEncoder.GetPosition() * 14 / 60 * 2 * 3.1415 * 0.02225 * 1_m;
 }
 
-units::meters_per_second_t wom::subsystems::Elevator::GetElevatorVelocity()
-    const {
-  return _config.elevatorEncoder.GetVelocity() / 60_s * 14 / 60 * 2 * 3.1415 *
-         0.02225 * 1_m;
+units::meters_per_second_t wom::subsystems::Elevator::GetElevatorVelocity() const {
+  return _config.elevatorEncoder.GetVelocity() / 60_s * 14 / 60 * 2 * 3.1415 * 0.02225 * 1_m;
 }
 
 units::meters_per_second_t wom::subsystems::Elevator::MaxSpeed() const {
-  return _config.leftGearbox.motor.Speed(
-             (_config.mass * 9.81_mps_sq) * _config.radius, 12_V) /
-         1_rad * _config.radius;
+  return _config.leftGearbox.motor.Speed((_config.mass * 9.81_mps_sq) * _config.radius, 12_V) / 1_rad *
+         _config.radius;
 }
diff --git a/wombat/src/main/cpp/subsystems/Shooter.cpp b/wombat/src/main/cpp/subsystems/Shooter.cpp
index 2fede2f3..c851266b 100644
--- a/wombat/src/main/cpp/subsystems/Shooter.cpp
+++ b/wombat/src/main/cpp/subsystems/Shooter.cpp
@@ -14,16 +14,14 @@ wom::subsystems::Shooter::Shooter(std::string path, ShooterParams params)
 
 void wom::subsystems::Shooter::OnUpdate(units::second_t dt) {
   units::volt_t voltage{0};
-  units::revolutions_per_minute_t currentSpeed =
-      _params.gearbox.encoder->GetEncoderAngularVelocity();
+  units::revolutions_per_minute_t currentSpeed = _params.gearbox.encoder->GetEncoderAngularVelocity();
 
   switch (_state) {
     case ShooterState::kManual:
       voltage = _setpointManual;
       break;
     case ShooterState::kPID: {
-      auto feedforward =
-          _params.gearbox.motor.Voltage(0_Nm, _pid.GetSetpoint());
+      auto feedforward = _params.gearbox.motor.Voltage(0_Nm, _pid.GetSetpoint());
       voltage = _pid.Calculate(currentSpeed, dt, feedforward);
     } break;
     case ShooterState::kIdle:
@@ -31,20 +29,17 @@ void wom::subsystems::Shooter::OnUpdate(units::second_t dt) {
       break;
   }
 
-  units::newton_meter_t max_torque_at_current_limit =
-      _params.gearbox.motor.Torque(_params.currentLimit);
+  units::newton_meter_t max_torque_at_current_limit = _params.gearbox.motor.Torque(_params.currentLimit);
   units::volt_t max_voltage_for_current_limit =
       _params.gearbox.motor.Voltage(max_torque_at_current_limit, currentSpeed);
 
-  voltage =
-      1_V * std::min(voltage.value(), max_voltage_for_current_limit.value());
+  voltage = 1_V * std::min(voltage.value(), max_voltage_for_current_limit.value());
 
   // _params.gearbox.motorController->SetVoltage(voltage);
 
   _table->GetEntry("output_volts").SetDouble(voltage.value());
   _table->GetEntry("speed_rpm").SetDouble(currentSpeed.value());
-  _table->GetEntry("setpoint_rpm")
-      .SetDouble(units::revolutions_per_minute_t{_pid.GetSetpoint()}.value());
+  _table->GetEntry("setpoint_rpm").SetDouble(units::revolutions_per_minute_t{_pid.GetSetpoint()}.value());
   _table->GetEntry("stable").SetBoolean(_pid.IsStable());
 }
 
@@ -67,8 +62,7 @@ bool wom::subsystems::Shooter::IsStable() const {
 
 // Shooter Manual Set
 
-wom::subsystems::ShooterConstant::ShooterConstant(Shooter* s,
-                                                  units::volt_t setpoint)
+wom::subsystems::ShooterConstant::ShooterConstant(Shooter* s, units::volt_t setpoint)
     : _shooter(s), _setpoint(setpoint) {
   Controls(_shooter);
 }
@@ -79,9 +73,7 @@ void wom::subsystems::ShooterConstant::OnTick(units::second_t dt) {
 
 // ShooterSpinup
 
-wom::subsystems::ShooterSpinup::ShooterSpinup(Shooter* s,
-                                              units::radians_per_second_t speed,
-                                              bool hold)
+wom::subsystems::ShooterSpinup::ShooterSpinup(Shooter* s, units::radians_per_second_t speed, bool hold)
     : _shooter(s), _speed(speed), _hold(hold) {
   Controls(_shooter);
 }
diff --git a/wombat/src/main/cpp/utils/Encoder.cpp b/wombat/src/main/cpp/utils/Encoder.cpp
index e5d6ab2b..461a8f31 100644
--- a/wombat/src/main/cpp/utils/Encoder.cpp
+++ b/wombat/src/main/cpp/utils/Encoder.cpp
@@ -6,8 +6,8 @@
 
 #include <rev/SparkRelativeEncoder.h>
 
-wom::utils::Encoder::Encoder(double encoderTicksPerRotation, int type,
-                             units::meter_t wheelRadius, double reduction)
+wom::utils::Encoder::Encoder(double encoderTicksPerRotation, int type, units::meter_t wheelRadius,
+                             double reduction)
     : _reduction(reduction),
       _encoderTicksPerRotation(encoderTicksPerRotation),
       _type(type),
@@ -43,16 +43,16 @@ void wom::utils::Encoder::SetReduction(double reduction) {
 }
 
 units::radian_t wom::utils::Encoder::GetEncoderPosition() {
-  //if (_type == 0) {
-  //  units::turn_t n_turns{GetEncoderTicks() / GetEncoderTicksPerRotation()};
-  //  return n_turns;
-  //} else if (_type == 2) {
-  //  units::degree_t pos = GetEncoderTicks() * 1_deg;
-  //  return pos;
-  //} else {
-  //  units::degree_t pos = GetEncoderTicks() * 1_deg;
-  //  return pos - _offset;
-  //}
+  // if (_type == 0) {
+  //   units::turn_t n_turns{GetEncoderTicks() / GetEncoderTicksPerRotation()};
+  //   return n_turns;
+  // } else if (_type == 2) {
+  //   units::degree_t pos = GetEncoderTicks() * 1_deg;
+  //   return pos;
+  // } else {
+  //   units::degree_t pos = GetEncoderTicks() * 1_deg;
+  //   return pos - _offset;
+  // }
   return GetEncoderTicks() * 1_rad;
 }
 
@@ -63,8 +63,7 @@ double wom::utils::Encoder::GetEncoderDistance() {
 units::radians_per_second_t wom::utils::Encoder::GetEncoderAngularVelocity() {
   // return GetEncoderTickVelocity() / (double)GetEncoderTicksPerRotation() * 2
   // * 3.1415926;
-  units::turns_per_second_t n_turns_per_s{GetEncoderTickVelocity() /
-                                          GetEncoderTicksPerRotation()};
+  units::turns_per_second_t n_turns_per_s{GetEncoderTickVelocity() / GetEncoderTicksPerRotation()};
   return n_turns_per_s;
 }
 
@@ -86,12 +85,10 @@ double wom::utils::DigitalEncoder::GetEncoderTickVelocity() const {
   return _nativeEncoder.GetRate();
 }
 
-wom::utils::CANSparkMaxEncoder::CANSparkMaxEncoder(rev::CANSparkMax* controller,
-                                                   units::meter_t wheelRadius,
+wom::utils::CANSparkMaxEncoder::CANSparkMaxEncoder(rev::CANSparkMax* controller, units::meter_t wheelRadius,
                                                    double reduction)
     : wom::utils::Encoder(42, reduction, wheelRadius, 2),
-      _encoder(controller->GetEncoder(
-          rev::SparkRelativeEncoder::Type::kQuadrature)) {}
+      _encoder(controller->GetEncoder(rev::SparkRelativeEncoder::Type::kQuadrature)) {}
 
 double wom::utils::CANSparkMaxEncoder::GetEncoderRawTicks() const {
   return _encoder.GetPosition() * _reduction;
@@ -121,11 +118,9 @@ double wom::utils::CANSparkMaxEncoder::GetVelocity() const {
   return _encoder.GetVelocity();
 }
 
-wom::utils::TalonFXEncoder::TalonFXEncoder(
-    ctre::phoenix6::hardware::TalonFX* controller, units::meter_t wheelRadius,
-    double reduction)
-    : utils::Encoder(2048, reduction, wheelRadius, 0),
-      _controller(controller) {}
+wom::utils::TalonFXEncoder::TalonFXEncoder(ctre::phoenix6::hardware::TalonFX* controller,
+                                           units::meter_t wheelRadius, double reduction)
+    : utils::Encoder(2048, reduction, wheelRadius, 0), _controller(controller) {}
 
 double wom::utils::TalonFXEncoder::GetEncoderRawTicks() const {
   return _controller->GetPosition().GetValue().value();
@@ -135,12 +130,9 @@ double wom::utils::TalonFXEncoder::GetEncoderTickVelocity() const {
   return _controller->Get() * 10;
 }
 
-wom::utils::DutyCycleEncoder::DutyCycleEncoder(int channel,
-                                               units::meter_t wheelRadius,
-                                               double ticksPerRotation,
-                                               double reduction)
-    : wom::utils::Encoder(ticksPerRotation, reduction, wheelRadius, 0),
-      _dutyCycleEncoder(channel) {}
+wom::utils::DutyCycleEncoder::DutyCycleEncoder(int channel, units::meter_t wheelRadius,
+                                               double ticksPerRotation, double reduction)
+    : wom::utils::Encoder(ticksPerRotation, reduction, wheelRadius, 0), _dutyCycleEncoder(channel) {}
 
 double wom::utils::DutyCycleEncoder::GetEncoderRawTicks() const {
   return _dutyCycleEncoder.Get().value();
@@ -150,9 +142,8 @@ double wom::utils::DutyCycleEncoder::GetEncoderTickVelocity() const {
   return 0;
 }
 
-wom::utils::CanEncoder::CanEncoder(int deviceNumber, units::meter_t wheelRadius,
-                                   double ticksPerRotation, double reduction,
-                                   std::string name)
+wom::utils::CanEncoder::CanEncoder(int deviceNumber, units::meter_t wheelRadius, double ticksPerRotation,
+                                   double reduction, std::string name)
     : wom::utils::Encoder(ticksPerRotation, 2, wheelRadius, reduction) {
   _canEncoder = new ctre::phoenix6::hardware::CANcoder(deviceNumber, name);
 }
diff --git a/wombat/src/main/cpp/utils/Util.cpp b/wombat/src/main/cpp/utils/Util.cpp
index e529e245..dbdbb642 100644
--- a/wombat/src/main/cpp/utils/Util.cpp
+++ b/wombat/src/main/cpp/utils/Util.cpp
@@ -13,21 +13,18 @@ units::second_t wom::utils::now() {
   return static_cast<double>(now) / 1000000 * 1_s;
 }
 
-void wom::utils::WritePose2NT(std::shared_ptr<nt::NetworkTable> table,
-                              frc::Pose2d pose) {
+void wom::utils::WritePose2NT(std::shared_ptr<nt::NetworkTable> table, frc::Pose2d pose) {
   table->GetEntry("x").SetDouble(pose.X().value());
   table->GetEntry("y").SetDouble(pose.Y().value());
   table->GetEntry("angle").SetDouble(pose.Rotation().Degrees().value());
 }
 
-void wom::utils::WritePose3NT(std::shared_ptr<nt::NetworkTable> table,
-                              frc::Pose3d pose) {
+void wom::utils::WritePose3NT(std::shared_ptr<nt::NetworkTable> table, frc::Pose3d pose) {
   table->GetEntry("x").SetDouble(pose.X().value());
   table->GetEntry("y").SetDouble(pose.Y().value());
   table->GetEntry("z").SetDouble(pose.Z().value());
 
-  table->GetEntry("angle").SetDouble(
-      pose.Rotation().Z().convert<units::degree>().value());
+  table->GetEntry("angle").SetDouble(pose.Rotation().Z().convert<units::degree>().value());
 }
 
 double wom::utils::deadzone(double val, double deadzone) {
@@ -50,32 +47,24 @@ units::volt_t wom::utils::GetVoltage(frc::MotorController* controller) {
   return controller->Get() * frc::RobotController::GetBatteryVoltage();
 }
 
-void wom::utils::WriteTrajectory(std::shared_ptr<nt::NetworkTable> table,
-                                 frc::Trajectory trajectory) {
+void wom::utils::WriteTrajectory(std::shared_ptr<nt::NetworkTable> table, frc::Trajectory trajectory) {
   table->GetEntry("length").SetDouble(trajectory.TotalTime().value());
 
   // write the trajectory to the network table
   int i = 0;
   for (auto state : trajectory.States()) {
-    table->GetSubTable(std::to_string(i))
-        ->GetEntry("x")
-        .SetDouble(state.pose.X().value());
-    table->GetSubTable(std::to_string(i))
-        ->GetEntry("y")
-        .SetDouble(state.pose.Y().value());
+    table->GetSubTable(std::to_string(i))->GetEntry("x").SetDouble(state.pose.X().value());
+    table->GetSubTable(std::to_string(i))->GetEntry("y").SetDouble(state.pose.Y().value());
     table->GetSubTable(std::to_string(i))
         ->GetEntry("angle")
         .SetDouble(state.pose.Rotation().Degrees().value());
-    table->GetSubTable(std::to_string(i))
-        ->GetEntry("time")
-        .SetDouble(state.t.value());
+    table->GetSubTable(std::to_string(i))->GetEntry("time").SetDouble(state.t.value());
 
     i++;
   }
 }
 
-void wom::utils::WriteTrajectoryState(std::shared_ptr<nt::NetworkTable> table,
-                                      frc::Trajectory::State state) {
+void wom::utils::WriteTrajectoryState(std::shared_ptr<nt::NetworkTable> table, frc::Trajectory::State state) {
   table->GetEntry("x").SetDouble(state.pose.X().value());
   table->GetEntry("y").SetDouble(state.pose.Y().value());
   table->GetEntry("angle").SetDouble(state.pose.Rotation().Degrees().value());
diff --git a/wombat/src/main/cpp/vision/Limelight.cpp b/wombat/src/main/cpp/vision/Limelight.cpp
index b37148ce..906f92cb 100644
--- a/wombat/src/main/cpp/vision/Limelight.cpp
+++ b/wombat/src/main/cpp/vision/Limelight.cpp
@@ -8,8 +8,7 @@
 
 #include "utils/Util.h"
 
-wom::vision::Limelight::Limelight(std::string limelightName)
-    : _limelightName(limelightName) {}
+wom::vision::Limelight::Limelight(std::string limelightName) : _limelightName(limelightName) {}
 
 std::string wom::vision::Limelight::GetName() {
   return _limelightName;
@@ -24,8 +23,7 @@ std::pair<double, double> wom::vision::Limelight::GetOffset() {
   return offset;
 }
 
-std::vector<double> wom::vision::Limelight::GetAprilTagData(
-    LimelightAprilTagData dataType) {
+std::vector<double> wom::vision::Limelight::GetAprilTagData(LimelightAprilTagData dataType) {
   std::string dataName;
 
   switch (dataType) {
@@ -69,8 +67,7 @@ std::vector<double> wom::vision::Limelight::GetAprilTagData(
   return table->GetNumberArray(dataName, std::vector<double>(6));
 }
 
-double wom::vision::Limelight::GetTargetingData(LimelightTargetingData dataType,
-                                                double defaultValue) {
+double wom::vision::Limelight::GetTargetingData(LimelightTargetingData dataType, double defaultValue) {
   std::string dataName;
 
   switch (dataType) {
@@ -158,8 +155,8 @@ void wom::vision::Limelight::SetCrop(std::array<double, 4> crop) {
   table->PutNumberArray("camtran", crop);
 }
 
-units::meters_per_second_t wom::vision::Limelight::GetSpeed(
-    frc::Pose3d pose1, frc::Pose3d pose2, units::second_t dt) {
+units::meters_per_second_t wom::vision::Limelight::GetSpeed(frc::Pose3d pose1, frc::Pose3d pose2,
+                                                            units::second_t dt) {
   frc::Transform3d dPose{pose1, pose2};
   frc::Translation3d dTranslation = dPose.Translation();
 
@@ -172,9 +169,8 @@ units::meters_per_second_t wom::vision::Limelight::GetSpeed(
 
 frc::Pose3d wom::vision::Limelight::GetPose() {
   std::vector<double> pose = GetAprilTagData(LimelightAprilTagData::kBotpose);
-  return frc::Pose3d(
-      pose[1] * 1_m, 1_m * pose[2], 1_m * pose[3],
-      frc::Rotation3d(1_deg * (pose[4]), 1_deg * (pose[5]), 1_deg * (pose[6])));
+  return frc::Pose3d(pose[1] * 1_m, 1_m * pose[2], 1_m * pose[3],
+                     frc::Rotation3d(1_deg * (pose[4]), 1_deg * (pose[5]), 1_deg * (pose[6])));
 }
 
 void wom::vision::Limelight::OnStart() {
@@ -185,11 +181,9 @@ void wom::vision::Limelight::OnUpdate(units::time::second_t dt) {
   wom::utils::WritePose3NT(table, GetPose());
 }
 
-bool wom::vision::Limelight::IsAtSetPoseVision(frc::Pose3d pose,
-                                               units::second_t dt) {
+bool wom::vision::Limelight::IsAtSetPoseVision(frc::Pose3d pose, units::second_t dt) {
   frc::Pose3d actualPose = GetPose();
   frc::Pose3d relativePose = actualPose.RelativeTo(pose);
-  return (units::math::fabs(relativePose.X()) < 0.01_m &&
-          units::math::fabs(relativePose.Y()) < 0.01_m &&
+  return (units::math::fabs(relativePose.X()) < 0.01_m && units::math::fabs(relativePose.Y()) < 0.01_m &&
           GetSpeed(pose, GetPose(), dt) < 1_m / 1_s);
 }
diff --git a/wombat/src/main/include/behaviour/Behaviour.h b/wombat/src/main/include/behaviour/Behaviour.h
index 4f2e357e..84ac8689 100644
--- a/wombat/src/main/include/behaviour/Behaviour.h
+++ b/wombat/src/main/include/behaviour/Behaviour.h
@@ -22,13 +22,7 @@
 #include "HasBehaviour.h"
 
 namespace behaviour {
-enum class BehaviourState {
-  INITIALISED,
-  RUNNING,
-  DONE,
-  TIMED_OUT,
-  INTERRUPTED
-};
+enum class BehaviourState { INITIALISED, RUNNING, DONE, TIMED_OUT, INTERRUPTED };
 
 class SequentialBehaviour;
 
@@ -46,8 +40,7 @@ class Behaviour : public std::enable_shared_from_this<Behaviour> {
  public:
   using ptr = std::shared_ptr<Behaviour>;
 
-  Behaviour(std::string name = "<unnamed behaviour>",
-            units::time::second_t period = 20_ms);
+  explicit Behaviour(std::string name = "<unnamed behaviour>", units::time::second_t period = 20_ms);
   ~Behaviour();
 
   /**
@@ -193,16 +186,15 @@ class SequentialBehaviour : public Behaviour {
   std::deque<ptr> _queue;
 };
 
-inline std::shared_ptr<SequentialBehaviour> operator<<(Behaviour::ptr a,
-                                                       Behaviour::ptr b) {
+inline std::shared_ptr<SequentialBehaviour> operator<<(Behaviour::ptr a, Behaviour::ptr b) {
   auto seq = std::make_shared<SequentialBehaviour>();
   seq->Add(a);
   seq->Add(b);
   return seq;
 }
 
-inline std::shared_ptr<SequentialBehaviour> operator<<(
-    std::shared_ptr<SequentialBehaviour> a, Behaviour::ptr b) {
+inline std::shared_ptr<SequentialBehaviour> operator<<(std::shared_ptr<SequentialBehaviour> a,
+                                                       Behaviour::ptr b) {
   a->Add(b);
   return a;
 }
@@ -247,10 +239,8 @@ class ConcurrentBehaviour : public Behaviour {
  * Create a concurrent behaviour group, waiting for all behaviours
  * to finish before moving on.
  */
-inline std::shared_ptr<ConcurrentBehaviour> operator&(Behaviour::ptr a,
-                                                      Behaviour::ptr b) {
-  auto conc =
-      std::make_shared<ConcurrentBehaviour>(ConcurrentBehaviourReducer::ALL);
+inline std::shared_ptr<ConcurrentBehaviour> operator&(Behaviour::ptr a, Behaviour::ptr b) {
+  auto conc = std::make_shared<ConcurrentBehaviour>(ConcurrentBehaviourReducer::ALL);
   conc->Add(a);
   conc->Add(b);
   return conc;
@@ -261,10 +251,8 @@ inline std::shared_ptr<ConcurrentBehaviour> operator&(Behaviour::ptr a,
  * be interrupted as soon as any members of the group are finished (the
  * behaviours are 'raced' against each other).
  */
-inline std::shared_ptr<ConcurrentBehaviour> operator|(Behaviour::ptr a,
-                                                      Behaviour::ptr b) {
-  auto conc =
-      std::make_shared<ConcurrentBehaviour>(ConcurrentBehaviourReducer::ANY);
+inline std::shared_ptr<ConcurrentBehaviour> operator|(Behaviour::ptr a, Behaviour::ptr b) {
+  auto conc = std::make_shared<ConcurrentBehaviour>(ConcurrentBehaviourReducer::ANY);
   conc->Add(a);
   conc->Add(b);
   return conc;
@@ -332,8 +320,7 @@ struct Switch : public Behaviour {
    * @param condition The function yielding true if this is the correct option
    * @param b The behaviour to call if this option is provided.
    */
-  std::shared_ptr<Switch> When(std::function<bool(T&)> condition,
-                               Behaviour::ptr b) {
+  std::shared_ptr<Switch> When(std::function<bool(T&)> condition, Behaviour::ptr b) {
     _options.push_back(std::pair(condition, b));
     Inherit(*b);
     return std::reinterpret_pointer_cast<Switch<T>>(shared_from_this());
@@ -389,8 +376,7 @@ struct Switch : public Behaviour {
 
  private:
   std::function<T()> _fn;
-  wpi::SmallVector<std::pair<std::function<bool(T&)>, Behaviour::ptr>, 4>
-      _options;
+  wpi::SmallVector<std::pair<std::function<bool(T&)>, Behaviour::ptr>, 4> _options;
   Behaviour::ptr _locked = nullptr;
 };
 
@@ -407,10 +393,8 @@ struct Decide : public Switch<std::monostate> {
    * @param condition The function yielding true if this is the correct option
    * @param b The behaviour to call if this option is provided.
    */
-  std::shared_ptr<Decide> When(std::function<bool()> condition,
-                               Behaviour::ptr b) {
-    return std::reinterpret_pointer_cast<Decide>(
-        Switch::When([condition](auto) { return condition(); }, b));
+  std::shared_ptr<Decide> When(std::function<bool()> condition, Behaviour::ptr b) {
+    return std::reinterpret_pointer_cast<Decide>(Switch::When([condition](auto) { return condition(); }, b));
   }
 };
 
diff --git a/wombat/src/main/include/behaviour/HasBehaviour.h b/wombat/src/main/include/behaviour/HasBehaviour.h
index c9b35b2e..bae5ac05 100644
--- a/wombat/src/main/include/behaviour/HasBehaviour.h
+++ b/wombat/src/main/include/behaviour/HasBehaviour.h
@@ -30,8 +30,7 @@ class HasBehaviour {
 
  protected:
   std::shared_ptr<Behaviour> _active_behaviour{nullptr};
-  std::function<std::shared_ptr<Behaviour>(void)> _default_behaviour_producer{
-      nullptr};
+  std::function<std::shared_ptr<Behaviour>(void)> _default_behaviour_producer{nullptr};
 
  private:
   friend class BehaviourScheduler;
diff --git a/wombat/src/main/include/drivetrain/SwerveDrive.h b/wombat/src/main/include/drivetrain/SwerveDrive.h
index b48db5a5..be4467b1 100644
--- a/wombat/src/main/include/drivetrain/SwerveDrive.h
+++ b/wombat/src/main/include/drivetrain/SwerveDrive.h
@@ -4,9 +4,9 @@
 
 #pragma once
 
+#include <frc/controller/PIDController.h>
 #include <frc/estimator/SwerveDrivePoseEstimator.h>
 #include <frc/kinematics/SwerveDriveKinematics.h>
-#include <frc/controller/PIDController.h>
 #include <networktables/DoubleTopic.h>
 #include <networktables/NetworkTable.h>
 #include <networktables/NetworkTableInstance.h>
@@ -48,9 +48,8 @@ struct SwerveModuleConfig {
 
 class SwerveModule {
  public:
-  //using angle_pid_conf_t = utils::PIDConfig<units::radian, units::volt>;
-  using velocity_pid_conf_t =
-      utils::PIDConfig<units::meters_per_second, units::volt>;
+  // using angle_pid_conf_t = utils::PIDConfig<units::radian, units::volt>;
+  using velocity_pid_conf_t = utils::PIDConfig<units::meters_per_second, units::volt>;
 
   SwerveModule(std::string path, SwerveModuleConfig config,
                /*angle_pid_conf_t anglePID,*/ velocity_pid_conf_t velocityPID);
@@ -67,8 +66,7 @@ class SwerveModule {
 
   void SetZero(units::second_t dt);
   void SetIdle();
-  void SetPID(units::radian_t angle, units::meters_per_second_t speed,
-              units::second_t dt);
+  void SetPID(units::radian_t angle, units::meters_per_second_t speed, units::second_t dt);
   void SetZero();
   void SetVoltageLimit(units::volt_t driveModuleVoltageLimit);
 
@@ -84,7 +82,7 @@ class SwerveModule {
 
   const SwerveModuleConfig& GetConfig() const;
 
-  //utils::PIDController<units::radians, units::volt> _anglePIDController;
+  // utils::PIDController<units::radians, units::volt> _anglePIDController;
   frc::PIDController _anglePIDController;
 
  private:
@@ -108,20 +106,18 @@ class SwerveModule {
 struct SwerveDriveConfig {
   /*using pose_angle_conf_t =
       utils::PIDConfig<units::radian, units::radians_per_second>;*/
-  using pose_position_conf_t =
-      utils::PIDConfig<units::meter, units::meters_per_second>;
-  using balance_conf_t =
-      utils::PIDConfig<units::degree, units::meters_per_second>;
+  using pose_position_conf_t = utils::PIDConfig<units::meter, units::meters_per_second>;
+  using balance_conf_t = utils::PIDConfig<units::degree, units::meters_per_second>;
 
   std::string path;
-  //SwerveModule::angle_pid_conf_t anglePID;
+  // SwerveModule::angle_pid_conf_t anglePID;
   SwerveModule::velocity_pid_conf_t velocityPID;
 
   wpi::array<SwerveModuleConfig, 4> modules;
 
   ctre::phoenix6::hardware::Pigeon2* gyro;
 
-  //pose_angle_conf_t poseAnglePID;
+  // pose_angle_conf_t poseAnglePID;
   pose_position_conf_t posePositionPID;
 
   units::kilogram_t mass;
@@ -175,8 +171,7 @@ class SwerveDrive : public behaviour::HasBehaviour {
    * @brief This function switches the state to handle the robot's rotation
    * matching that of the joystick
    */
-  void RotateMatchJoystick(units::radian_t joystickAngle,
-                           FieldRelativeSpeeds speeds);
+  void RotateMatchJoystick(units::radian_t joystickAngle, FieldRelativeSpeeds speeds);
 
   void SetIdle();
 
@@ -186,8 +181,7 @@ class SwerveDrive : public behaviour::HasBehaviour {
   void SetFieldRelativeVelocity(FieldRelativeSpeeds speeds);
   void SetPose(frc::Pose2d pose);
   bool IsAtSetPose();
-  void SetIndividualTuning(int mod, units::radian_t angle,
-                           units::meters_per_second_t speed);
+  void SetIndividualTuning(int mod, units::radian_t angle, units::meters_per_second_t speed);
   void SetTuning(units::radian_t angle, units::meters_per_second_t speed);
   void SetZero();
   void SetVoltageLimit(units::volt_t driveVoltageLimit);
diff --git a/wombat/src/main/include/drivetrain/behaviours/SwerveBehaviours.h b/wombat/src/main/include/drivetrain/behaviours/SwerveBehaviours.h
index 5aa1972f..11400c4a 100644
--- a/wombat/src/main/include/drivetrain/behaviours/SwerveBehaviours.h
+++ b/wombat/src/main/include/drivetrain/behaviours/SwerveBehaviours.h
@@ -37,16 +37,14 @@ class ManualDrivebase : public behaviour::Behaviour {
    * A pointer to the controller that the driver has been allocated (the
    * allocated memory address that stores the "driver controller" object)
    */
-  ManualDrivebase(wom::drivetrain::SwerveDrive* swerveDrivebase,
-                  frc::XboxController* driverController);
+  ManualDrivebase(wom::drivetrain::SwerveDrive* swerveDrivebase, frc::XboxController* driverController);
 
   void OnTick(units::second_t deltaTime) override;
   /**
    * @brief This function handles all of the logic behind the tangent function,
    * to be able to calculate an angle between 0 andd 360 degrees, inclusively
    */
-  void CalculateRequestedAngle(double joystickX, double joystickY,
-                               units::degree_t defaultAngle);
+  void CalculateRequestedAngle(double joystickX, double joystickY, units::degree_t defaultAngle);
   void OnStart() override;
   void ResetMode();
 
@@ -72,8 +70,8 @@ class ManualDrivebase : public behaviour::Behaviour {
   const double turningDeadzone = 0.2;
 
   // Variables for solution to Anti-tip
-  double prevJoystickX, prevJoystickY, prevPrevJoystickX, prevPrevJoystickY,
-      usingJoystickXPos, usingJoystickYPos;
+  double prevJoystickX, prevJoystickY, prevPrevJoystickX, prevPrevJoystickY, usingJoystickXPos,
+      usingJoystickYPos;
   // The speed that the joystick must travel to activate averaging over previous
   // 3 joystick positions
   const double smoothingThreshold = 1;
@@ -126,8 +124,8 @@ class GoToPose : public behaviour::Behaviour {
 
 class FollowTrajectory : public behaviour::Behaviour {
  public:
-  FollowTrajectory(wom::drivetrain::SwerveDrive* swerve,
-                   wom::utils::Pathplanner* pathplanner, std::string path);
+  FollowTrajectory(wom::drivetrain::SwerveDrive* swerve, wom::utils::Pathplanner* pathplanner,
+                   std::string path);
 
   void OnTick(units::second_t dt) override;
 
@@ -184,8 +182,7 @@ class TempSimSwerveDrive {
 
 class AutoSwerveDrive {
  public:
-  AutoSwerveDrive(wom::drivetrain::SwerveDrive* swerve, frc::Timer* timer,
-                  frc::Field2d* field);
+  AutoSwerveDrive(wom::drivetrain::SwerveDrive* swerve, frc::Timer* timer, frc::Field2d* field);
 
   void OnUpdate();
 
diff --git a/wombat/src/main/include/subsystems/Arm.h b/wombat/src/main/include/subsystems/Arm.h
index 155fd4cd..8b91be72 100644
--- a/wombat/src/main/include/subsystems/Arm.h
+++ b/wombat/src/main/include/subsystems/Arm.h
@@ -68,8 +68,7 @@ class Arm : public behaviour::HasBehaviour {
   ArmConfig _config;
   ArmState _state = ArmState::kIdle;
   wom::utils::PIDController<units::radian, units::volt> _pid;
-  wom::utils::PIDController<units::radians_per_second, units::volt>
-      _velocityPID;
+  wom::utils::PIDController<units::radians_per_second, units::volt> _velocityPID;
 
   std::shared_ptr<nt::NetworkTable> _table;
 
diff --git a/wombat/src/main/include/subsystems/Shooter.h b/wombat/src/main/include/subsystems/Shooter.h
index 2bbf6f5e..e5ac219d 100644
--- a/wombat/src/main/include/subsystems/Shooter.h
+++ b/wombat/src/main/include/subsystems/Shooter.h
@@ -62,8 +62,7 @@ class ShooterConstant : public behaviour::Behaviour {
 
 class ShooterSpinup : public behaviour::Behaviour {
  public:
-  ShooterSpinup(Shooter* s, units::radians_per_second_t speed,
-                bool hold = false);
+  ShooterSpinup(Shooter* s, units::radians_per_second_t speed, bool hold = false);
 
   void OnTick(units::second_t dt) override;
 
diff --git a/wombat/src/main/include/utils/Encoder.h b/wombat/src/main/include/utils/Encoder.h
index b504e1c0..303b2fda 100644
--- a/wombat/src/main/include/utils/Encoder.h
+++ b/wombat/src/main/include/utils/Encoder.h
@@ -20,8 +20,7 @@ namespace wom {
 namespace utils {
 class Encoder {
  public:
-  Encoder(double encoderTicksPerRotation, int type, units::meter_t wheelRadius,
-          double reduction = 1.0);
+  Encoder(double encoderTicksPerRotation, int type, units::meter_t wheelRadius, double reduction = 1.0);
 
   virtual double GetEncoderRawTicks() const = 0;
   virtual double GetEncoderTickVelocity() const = 0;  // ticks/s
@@ -54,24 +53,23 @@ class Encoder {
 
 class DigitalEncoder : public Encoder {
  public:
-  DigitalEncoder(int channelA, int channelB, double ticksPerRotation,
-                 units::meter_t wheelRadius, double reduction = 1)
-      : Encoder(ticksPerRotation, reduction, wheelRadius, 0),
-        _nativeEncoder(channelA, channelB) {}
+  DigitalEncoder(int channelA, int channelB, double ticksPerRotation, units::meter_t wheelRadius,
+                 double reduction = 1)
+      : Encoder(ticksPerRotation, reduction, wheelRadius, 0), _nativeEncoder(channelA, channelB) {}
 
   double GetEncoderRawTicks() const override;
   double GetEncoderTickVelocity() const override;
 
   double GetPosition() const;
   double GetVelocity() const override;
+
  private:
   frc::Encoder _nativeEncoder;
 };
 
 class CANSparkMaxEncoder : public Encoder {
  public:
-  explicit CANSparkMaxEncoder(rev::CANSparkMax* controller,
-                              units::meter_t wheelRadius, double reduction = 1);
+  explicit CANSparkMaxEncoder(rev::CANSparkMax* controller, units::meter_t wheelRadius, double reduction = 1);
 
   double GetEncoderRawTicks() const override;
   double GetEncoderTickVelocity() const override;
@@ -86,8 +84,8 @@ class CANSparkMaxEncoder : public Encoder {
 
 class TalonFXEncoder : public Encoder {
  public:
-  TalonFXEncoder(ctre::phoenix6::hardware::TalonFX* controller,
-                 units::meter_t wheelRadius, double reduction = 1);
+  TalonFXEncoder(ctre::phoenix6::hardware::TalonFX* controller, units::meter_t wheelRadius,
+                 double reduction = 1);
 
   double GetEncoderRawTicks() const override;
   double GetEncoderTickVelocity() const override;
@@ -99,8 +97,8 @@ class TalonFXEncoder : public Encoder {
 
 class DutyCycleEncoder : public Encoder {
  public:
-  DutyCycleEncoder(int channel, units::meter_t wheelRadius,
-                   double ticksPerRotation = 1, double reduction = 1);
+  DutyCycleEncoder(int channel, units::meter_t wheelRadius, double ticksPerRotation = 1,
+                   double reduction = 1);
 
   double GetEncoderRawTicks() const override;
   double GetEncoderTickVelocity() const override;
@@ -112,9 +110,8 @@ class DutyCycleEncoder : public Encoder {
 
 class CanEncoder : public Encoder {
  public:
-  CanEncoder(int deviceNumber, units::meter_t wheelRadius,
-             double ticksPerRotation = 4095, double reduction = 6.75,
-             std::string name = "Drivebase");
+  CanEncoder(int deviceNumber, units::meter_t wheelRadius, double ticksPerRotation = 4095,
+             double reduction = 6.75, std::string name = "Drivebase");
 
   double GetEncoderRawTicks() const override;
   double GetEncoderTickVelocity() const override;
diff --git a/wombat/src/main/include/utils/PID.h b/wombat/src/main/include/utils/PID.h
index 2ce378ec..cec94b1d 100644
--- a/wombat/src/main/include/utils/PID.h
+++ b/wombat/src/main/include/utils/PID.h
@@ -24,19 +24,15 @@ struct PIDConfig {
   using in_t = units::unit_t<IN>;
 
   using kp_t = units::unit_t<units::compound_unit<OUT, units::inverse<IN>>>;
-  using ki_t =
-      units::unit_t<units::compound_unit<OUT, units::inverse<IN>,
-                                         units::inverse<units::second>>>;
-  using kd_t = units::unit_t<
-      units::compound_unit<OUT, units::inverse<IN>, units::second>>;
+  using ki_t = units::unit_t<units::compound_unit<OUT, units::inverse<IN>, units::inverse<units::second>>>;
+  using kd_t = units::unit_t<units::compound_unit<OUT, units::inverse<IN>, units::second>>;
 
   using error_t = units::unit_t<IN>;
-  using deriv_t =
-      units::unit_t<units::compound_unit<IN, units::inverse<units::second>>>;
+  using deriv_t = units::unit_t<units::compound_unit<IN, units::inverse<units::second>>>;
 
-  PIDConfig(std::string path, kp_t kp = kp_t{0}, ki_t ki = ki_t{0},
-            kd_t kd = kd_t{0}, error_t stableThresh = error_t{-1},
-            deriv_t stableDerivThresh = deriv_t{-1}, in_t izone = in_t{-1})
+  PIDConfig(std::string path, kp_t kp = kp_t{0}, ki_t ki = ki_t{0}, kd_t kd = kd_t{0},
+            error_t stableThresh = error_t{-1}, deriv_t stableDerivThresh = deriv_t{-1},
+            in_t izone = in_t{-1})
       : path(path),
         kp(kp),
         ki(ki),
@@ -64,23 +60,14 @@ struct PIDConfig {
  public:
   void RegisterNT() {
     auto table = nt::NetworkTableInstance::GetDefault().GetTable(path);
+    _nt_bindings.emplace_back(std::make_shared<NTBoundUnit<typename kp_t::unit_type>>(table, "kP", kp));
+    _nt_bindings.emplace_back(std::make_shared<NTBoundUnit<typename ki_t::unit_type>>(table, "kI", ki));
+    _nt_bindings.emplace_back(std::make_shared<NTBoundUnit<typename kd_t::unit_type>>(table, "kD", kd));
     _nt_bindings.emplace_back(
-        std::make_shared<NTBoundUnit<typename kp_t::unit_type>>(table, "kP",
-                                                                kp));
-    _nt_bindings.emplace_back(
-        std::make_shared<NTBoundUnit<typename ki_t::unit_type>>(table, "kI",
-                                                                ki));
-    _nt_bindings.emplace_back(
-        std::make_shared<NTBoundUnit<typename kd_t::unit_type>>(table, "kD",
-                                                                kd));
-    _nt_bindings.emplace_back(
-        std::make_shared<NTBoundUnit<typename error_t::unit_type>>(
-            table, "stableThresh", stableThresh));
-    _nt_bindings.emplace_back(
-        std::make_shared<NTBoundUnit<typename deriv_t::unit_type>>(
-            table, "stableThreshVelocity", stableDerivThresh));
-    _nt_bindings.emplace_back(
-        std::make_shared<NTBoundUnit<IN>>(table, "izone", izone));
+        std::make_shared<NTBoundUnit<typename error_t::unit_type>>(table, "stableThresh", stableThresh));
+    _nt_bindings.emplace_back(std::make_shared<NTBoundUnit<typename deriv_t::unit_type>>(
+        table, "stableThreshVelocity", stableDerivThresh));
+    _nt_bindings.emplace_back(std::make_shared<NTBoundUnit<IN>>(table, "izone", izone));
   }
 };
 
@@ -94,19 +81,15 @@ class PIDController {
 
   config_t config;
 
-  PIDController(std::string path, config_t initialGains,
-                in_t setpoint = in_t{0})
+  PIDController(std::string path, config_t initialGains, in_t setpoint = in_t{0})
       : config(initialGains),
         _setpoint(setpoint),
-        _posFilter(
-            frc::LinearFilter<typename config_t::error_t>::MovingAverage(20)),
-        _velFilter(
-            frc::LinearFilter<typename config_t::deriv_t>::MovingAverage(20)),
+        _posFilter(frc::LinearFilter<typename config_t::error_t>::MovingAverage(20)),
+        _velFilter(frc::LinearFilter<typename config_t::deriv_t>::MovingAverage(20)),
         _table(nt::NetworkTableInstance::GetDefault().GetTable(path)) {}
 
   void SetSetpoint(in_t setpoint) {
-    if (std::abs(setpoint.value() - _setpoint.value()) >
-        std::abs(0.1 * _setpoint.value())) {
+    if (std::abs(setpoint.value() - _setpoint.value()) > std::abs(0.1 * _setpoint.value())) {
       _iterations = 0;
     }
     _setpoint = setpoint;
@@ -126,8 +109,7 @@ class PIDController {
     error = units::math::fabs(error);
     _integralSum += error * dt;
     _integralSum = units::math::fabs(_integralSum);
-    if (config.izone.value() > 0 &&
-        (error > config.izone || error < -config.izone))
+    if (config.izone.value() > 0 && (error > config.izone || error < -config.izone))
       _integralSum = sum_t{0};
 
     typename config_t::deriv_t deriv{0};
@@ -138,8 +120,7 @@ class PIDController {
     _stablePos = _posFilter.Calculate(error);
     _stableVel = _velFilter.Calculate(deriv);
 
-    auto out = config.kp * error + config.ki * _integralSum +
-               config.kd * deriv + feedforward;
+    auto out = config.kp * error + config.ki * _integralSum + config.kd * deriv + feedforward;
 
     _last_pv = pv;
     _last_error = error;
@@ -147,10 +128,8 @@ class PIDController {
     return out;
   }
 
-  bool IsStable(
-      std::optional<typename config_t::error_t> stableThreshOverride = {},
-      std::optional<typename config_t::deriv_t> velocityThreshOverride = {})
-      const {
+  bool IsStable(std::optional<typename config_t::error_t> stableThreshOverride = {},
+                std::optional<typename config_t::deriv_t> velocityThreshOverride = {}) const {
     auto stableThresh = config.stableThresh;
     auto stableDerivThresh = config.stableDerivThresh;
 
@@ -159,10 +138,8 @@ class PIDController {
     if (velocityThreshOverride.has_value())
       stableDerivThresh = velocityThreshOverride.value();
 
-    return _iterations > 20 &&
-           std::abs(_stablePos.value()) <= std::abs(stableThresh.value()) &&
-           (stableDerivThresh.value() < 0 ||
-            std::abs(_stableVel.value()) <= stableDerivThresh.value());
+    return _iterations > 20 && std::abs(_stablePos.value()) <= std::abs(stableThresh.value()) &&
+           (stableDerivThresh.value() < 0 || std::abs(_stableVel.value()) <= stableDerivThresh.value());
   }
 
  private:
diff --git a/wombat/src/main/include/utils/Util.h b/wombat/src/main/include/utils/Util.h
index 665c85ec..d624e408 100644
--- a/wombat/src/main/include/utils/Util.h
+++ b/wombat/src/main/include/utils/Util.h
@@ -28,39 +28,30 @@ units::second_t now();
 
 class NTBound {
  public:
-  NTBound(std::shared_ptr<nt::NetworkTable> table, std::string name,
-          const nt::Value& value,
+  NTBound(std::shared_ptr<nt::NetworkTable> table, std::string name, const nt::Value& value,
           std::function<void(const nt::Value&)> onUpdateFn)
-      : _table(table),
-        _entry(table->GetEntry(name)),
-        _onUpdate(onUpdateFn),
-        _name(name) {
+      : _table(table), _entry(table->GetEntry(name)), _onUpdate(onUpdateFn), _name(name) {
     _entry.SetValue(value);
     // _listener = table->AddListener(name, , ([=](const nt::EntryNotification
     // &evt) {
     //   this->_onUpdate(evt.value);
     // }, NT_NOTIFY_UPDATE);
-    _listener = table->AddListener(
-        name, nt::EventFlags::kValueAll,
-        ([this](nt::NetworkTable* table, std::string_view key,
-                const nt::Event& event) {
-          std::cout << "NT UPDATE" << std::endl;
-          this->_onUpdate(event.GetValueEventData()->value);
-        }));
+    _listener =
+        table->AddListener(name, nt::EventFlags::kValueAll,
+                           ([this](nt::NetworkTable* table, std::string_view key, const nt::Event& event) {
+                             std::cout << "NT UPDATE" << std::endl;
+                             this->_onUpdate(event.GetValueEventData()->value);
+                           }));
   }
 
   NTBound(const NTBound& other)
-      : _table(other._table),
-        _entry(other._entry),
-        _onUpdate(other._onUpdate),
-        _name(other._name) {
-    _listener = _table->AddListener(
-        _name, nt::EventFlags::kValueAll,
-        ([this](nt::NetworkTable* table, std::string_view key,
-                const nt::Event& event) {
-          std::cout << "NT UPDATE" << std::endl;
-          this->_onUpdate(event.GetValueEventData()->value);
-        }));
+      : _table(other._table), _entry(other._entry), _onUpdate(other._onUpdate), _name(other._name) {
+    _listener =
+        _table->AddListener(_name, nt::EventFlags::kValueAll,
+                            ([this](nt::NetworkTable* table, std::string_view key, const nt::Event& event) {
+                              std::cout << "NT UPDATE" << std::endl;
+                              this->_onUpdate(event.GetValueEventData()->value);
+                            }));
   }
 
   ~NTBound() { _table->RemoveListener(_listener); }
@@ -75,8 +66,7 @@ class NTBound {
 
 class NTBoundDouble : public NTBound {
  public:
-  NTBoundDouble(std::shared_ptr<nt::NetworkTable> table, std::string name,
-                double& val)
+  NTBoundDouble(std::shared_ptr<nt::NetworkTable> table, std::string name, double& val)
       : NTBound(table, name, nt::Value::MakeDouble(val),
                 [&val](const nt::Value& v) { val = v.GetDouble(); }) {}
 };
@@ -84,20 +74,15 @@ class NTBoundDouble : public NTBound {
 template <typename T>
 class NTBoundUnit : public NTBound {
  public:
-  NTBoundUnit(std::shared_ptr<nt::NetworkTable> table, std::string name,
-              units::unit_t<T>& val)
+  NTBoundUnit(std::shared_ptr<nt::NetworkTable> table, std::string name, units::unit_t<T>& val)
       : NTBound(table, name, nt::Value::MakeDouble(val.value()),
-                [&val](const nt::Value& v) {
-                  val = units::unit_t<T>{v.GetDouble()};
-                }) {}
+                [&val](const nt::Value& v) { val = units::unit_t<T>{v.GetDouble()}; }) {}
 };
 
 void WritePose2NT(std::shared_ptr<nt::NetworkTable> table, frc::Pose2d pose);
 void WritePose3NT(std::shared_ptr<nt::NetworkTable> table, frc::Pose3d pose);
-void WriteTrajectory(std::shared_ptr<nt::NetworkTable> table,
-                     frc::Trajectory trajectory);
-void WriteTrajectoryState(std::shared_ptr<nt::NetworkTable> table,
-                          frc::Trajectory::State state);
+void WriteTrajectory(std::shared_ptr<nt::NetworkTable> table, frc::Trajectory trajectory);
+void WriteTrajectoryState(std::shared_ptr<nt::NetworkTable> table, frc::Trajectory::State state);
 
 frc::Pose2d TrajectoryStateToPose2d(frc::Trajectory::State state);
 
diff --git a/wombat/src/main/include/vision/Limelight.h b/wombat/src/main/include/vision/Limelight.h
index ddfb3739..5aa6d0b7 100644
--- a/wombat/src/main/include/vision/Limelight.h
+++ b/wombat/src/main/include/vision/Limelight.h
@@ -26,20 +26,11 @@
 namespace wom {
 namespace vision {
 
-enum class LimelightLEDMode {
-  kPipelineDefault = 0,
-  kForceOff = 1,
-  kForceBlink = 2,
-  kForceOn = 3
-};
+enum class LimelightLEDMode { kPipelineDefault = 0, kForceOff = 1, kForceBlink = 2, kForceOn = 3 };
 
 enum class LimelightCamMode { kVisionProcessor = 0, kDriverCamera = 1 };
 
-enum class LimelightStreamMode {
-  kStandard = 0,
-  kPiPMain = 1,
-  kPiPSecondary = 2
-};
+enum class LimelightStreamMode { kStandard = 0, kPiPMain = 1, kPiPSecondary = 2 };
 
 enum class LimelightSnapshotMode { kReset = 0, kSingle = 1 };
 
@@ -91,14 +82,12 @@ class Limelight {
 
   std::string GetName();
 
-  std::shared_ptr<nt::NetworkTable> table =
-      nt::NetworkTableInstance::GetDefault().GetTable("limelight");
+  std::shared_ptr<nt::NetworkTable> table = nt::NetworkTableInstance::GetDefault().GetTable("limelight");
 
   std::pair<double, double> GetOffset();
 
   std::vector<double> GetAprilTagData(LimelightAprilTagData dataType);
-  double GetTargetingData(LimelightTargetingData dataType,
-                          double defaultValue = 0.0);
+  double GetTargetingData(LimelightTargetingData dataType, double defaultValue = 0.0);
   void SetLEDMode(LimelightLEDMode mode);
   void SetCamMode(LimelightCamMode mode);
   void SetPipeline(LimelightPipeline pipeline);
@@ -111,8 +100,7 @@ class Limelight {
 
   bool IsAtSetPoseVision(frc::Pose3d pose, units::second_t dt);
 
-  units::meters_per_second_t GetSpeed(frc::Pose3d pose1, frc::Pose3d pose2,
-                                      units::second_t dt);
+  units::meters_per_second_t GetSpeed(frc::Pose3d pose1, frc::Pose3d pose2, units::second_t dt);
 
   frc::Pose3d GetPose();
 

From 49b76ab549d314aef162b9b1246e51af8f65ce73 Mon Sep 17 00:00:00 2001
From: Isaac Turner <spacey_sooty@outlook.com>
Date: Tue, 30 Jan 2024 14:38:15 +0800
Subject: [PATCH 079/207] fix

---
 .editorconfig          |  5 -----
 gradlew                |  0
 init.ps1               |  2 --
 init.sh                |  5 -----
 wombat/LICENSE         | 21 ---------------------
 wombat/README.md       |  1 -
 wombat/settings.gradle |  1 -
 7 files changed, 35 deletions(-)
 delete mode 100644 .editorconfig
 mode change 100644 => 100755 gradlew
 delete mode 100644 init.ps1
 delete mode 100644 init.sh
 delete mode 100644 wombat/LICENSE
 delete mode 100644 wombat/README.md
 delete mode 100644 wombat/settings.gradle

diff --git a/.editorconfig b/.editorconfig
deleted file mode 100644
index 0020fc03..00000000
--- a/.editorconfig
+++ /dev/null
@@ -1,5 +0,0 @@
-root = true
-
-[*]
-indent_style = space
-indent_size = 2
diff --git a/gradlew b/gradlew
old mode 100644
new mode 100755
diff --git a/init.ps1 b/init.ps1
deleted file mode 100644
index fa83b9a6..00000000
--- a/init.ps1
+++ /dev/null
@@ -1,2 +0,0 @@
-./gradlew installRoborioToolchain
-./gradlew build
diff --git a/init.sh b/init.sh
deleted file mode 100644
index 6a932ed6..00000000
--- a/init.sh
+++ /dev/null
@@ -1,5 +0,0 @@
-#!/usr/bin/sh
-sudo apt update
-chmod +x gradlew
-./gradlew installRoborioToolchain
-./gradlew build
diff --git a/wombat/LICENSE b/wombat/LICENSE
deleted file mode 100644
index b1561bab..00000000
--- a/wombat/LICENSE
+++ /dev/null
@@ -1,21 +0,0 @@
-MIT License
-
-Copyright (c) 2023 CurtinFRC
-
-Permission is hereby granted, free of charge, to any person obtaining a copy
-of this software and associated documentation files (the "Software"), to deal
-in the Software without restriction, including without limitation the rights
-to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
-copies of the Software, and to permit persons to whom the Software is
-furnished to do so, subject to the following conditions:
-
-The above copyright notice and this permission notice shall be included in all
-copies or substantial portions of the Software.
-
-THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
-IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
-FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
-AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
-LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
-OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
-SOFTWARE.
diff --git a/wombat/README.md b/wombat/README.md
deleted file mode 100644
index 0595c6cb..00000000
--- a/wombat/README.md
+++ /dev/null
@@ -1 +0,0 @@
-# Wombat
diff --git a/wombat/settings.gradle b/wombat/settings.gradle
deleted file mode 100644
index 8b137891..00000000
--- a/wombat/settings.gradle
+++ /dev/null
@@ -1 +0,0 @@
-

From 59af7d6584bd4f10538d68d033fd5188346a59e8 Mon Sep 17 00:00:00 2001
From: Isaac Turner <spacey_sooty@outlook.com>
Date: Tue, 30 Jan 2024 14:39:15 +0800
Subject: [PATCH 080/207] fix line endings

---
 .gitattributes | 1 +
 1 file changed, 1 insertion(+)

diff --git a/.gitattributes b/.gitattributes
index 416557ac..df788002 100644
--- a/.gitattributes
+++ b/.gitattributes
@@ -1,5 +1,6 @@
 * text=auto
 *.sh text eol=lf
+*.bat text eol=crlf
 *.gradle text eol=lf
 *.java text eol=lf
 *.json text eol=lf

From a9c619da6e45a3a5627a6ed5e6589ded55a658c3 Mon Sep 17 00:00:00 2001
From: Isaac Turner <spacey_sooty@outlook.com>
Date: Tue, 30 Jan 2024 15:03:07 +0800
Subject: [PATCH 081/207] builds

---
 src/main/cpp/Shooter.cpp           | 66 ------------------------------
 src/main/include/IntakeBehaviour.h |  6 ++-
 src/main/include/RobotMap.h        |  2 +-
 3 files changed, 6 insertions(+), 68 deletions(-)

diff --git a/src/main/cpp/Shooter.cpp b/src/main/cpp/Shooter.cpp
index 3097feb0..c3bebfd8 100644
--- a/src/main/cpp/Shooter.cpp
+++ b/src/main/cpp/Shooter.cpp
@@ -4,71 +4,6 @@
 
 #include "Shooter.h"
 
-<<<<<<< HEAD
-Shooter::Shooter(ShooterConfig config)
-    : _config(config)
-// ,
-// _pid{frc::PIDController (1, 0, 0, 0.005_s)}
-{}  // config.path + "/pid", config.pidConfig
-void Shooter::OnUpdate(units::second_t dt) {
-  // _pid.SetTolerance(0.1, 1);
-  // switch (_state) {
-  //   case ShooterState::kIdle: {
-  //     std::cout << "KIdle" << std::endl;
-  //     _setVoltage = 0_V;
-  //     // if (_shooterSensor.Get()) {
-  //     //   _state = ShooterState::kReverse;
-  //     // }
-  //   } break;
-  //   case ShooterState::kSpinUp: {
-  //     // std::cout << "KSpinUp" << std::endl;
-  //     // _pid.SetSetpoint(_goal.value());
-  //     // units::volt_t pidCalculate =
-  //     //
-  //     units::volt_t{_pid.Calculate(_config.ShooterGearbox.encoder->GetEncoderAngularVelocity().value())};
-  //     // _setVoltage = pidCalculate;
-
-  //     // if (_pid.AtSetpoint()) {
-  //     //   SetState(ShooterState::kShooting);
-  //     // }
-  //   } break;
-  //   case ShooterState::kShooting: {
-  //     // std::cout << "KShooting" << std::endl;
-  //     // _pid.SetSetpoint(_goal.value());
-  //     // units::volt_t pidCalculate =
-  //     //
-  //     units::volt_t{_pid.Calculate(_config.ShooterGearbox.encoder->GetEncoderAngularVelocity().value())};
-  //     // _setVoltage = pidCalculate;
-
-  //     // if (!_pid.AtSetpoint()) {
-  //     //   SetState(ShooterState::kSpinUp);
-  //     // }
-  //     // if (_shooterSensor.Get()) {
-  //     //   SetState(ShooterState::kIdle);
-  //     // }
-  //   } break;
-
-  //   case ShooterState::kReverse: {
-  //     _setVoltage = -5_V;
-  //     std::cout << "KReverse" << std::endl;
-  //     // if (!_shooterSensor.Get()) {
-  //     //   SetState(ShooterState::kIdle);
-  //     // }
-  //   } break;
-  //   case ShooterState::kRaw: {
-  //     _setVoltage = _rawVoltage;
-  //     std::cout << "KRaw" << std::endl;
-  //     // if (_shooterSensor.Get()) {
-  //     //   SetState(ShooterState::kRaw);
-  //     // }
-  //   } break;
-  //   default: {
-  //     std::cout << "Error shooter in invalid state" << std::endl;
-  //   } break;
-  // }
-  // std::cout << "Voltage:" << _setVoltage.value() << std::endl;
-  // _config.ShooterGearbox.motorController->SetVoltage(_setVoltage);
-=======
 Shooter::Shooter(ShooterConfig config) : _config(config), _pid(config.path + "/pid", config.pidConfig) {}
 
 void Shooter::OnStart() {
@@ -160,7 +95,6 @@ void Shooter::OnUpdate(units::second_t dt) {
   }
   std::cout << "Voltage:" << _setVoltage.value() << std::endl;
   _config.ShooterGearbox.motorController->SetVoltage(_setVoltage);
->>>>>>> 4fff1bfdbcf5470c72fe140843a59616c642c5c8
 }
 
 void Shooter::SetState(ShooterState state) {
diff --git a/src/main/include/IntakeBehaviour.h b/src/main/include/IntakeBehaviour.h
index c84c6d32..931273fa 100644
--- a/src/main/include/IntakeBehaviour.h
+++ b/src/main/include/IntakeBehaviour.h
@@ -20,7 +20,11 @@ class IntakeManualControl : public behaviour::Behaviour {
   frc::XboxController& _codriver;
 
   units::volt_t _rawVoltage;
-  bool _rawControl;
+  units::volt_t _setVoltage;
+  bool _rawControl = true;
+  bool _intaking = false;
+  bool _ejecting = false;
+  bool _passing = false;
 };
 
 class IntakeAutoControl : public behaviour::Behaviour {
diff --git a/src/main/include/RobotMap.h b/src/main/include/RobotMap.h
index e8ae64bd..0cadce34 100644
--- a/src/main/include/RobotMap.h
+++ b/src/main/include/RobotMap.h
@@ -18,8 +18,8 @@
 #include <ctre/phoenix6/Pigeon2.hpp>
 #include <ctre/phoenix6/TalonFX.hpp>
 
-#include "Intake.h"
 #include "AlphaArm.h"
+#include "Intake.h"
 #include "Shooter.h"
 #include "Wombat.h"
 

From 0bd9a811f6fb21cadab4349e30d64834f09332bd Mon Sep 17 00:00:00 2001
From: prawny-boy <155516197+prawny-boy@users.noreply.github.com>
Date: Tue, 30 Jan 2024 15:12:55 +0800
Subject: [PATCH 082/207] Intake - Manual/Auto fixes (#114)

* First version Tank Drive

* Intake draft

* Intake draft fixed errors 1

* Intake draft fixed errors 2

* Intake draft fixed errors 3

* Intake draft fixed errors 4

* Intake draft fixed errors 5

* Network files 1

* Network files 2

* Network files 3

* Almost done i think

* finished wpiformat errors

* finished wpiformat errors 2

* finished wpiformat errors 3

* finished wpiformat errors 4

* ran wpiformat

* finished wpiformat errors 5

* finished wpiformat errors 8

* finished wpiformat errors 9

* finished wpiformat errors 10

* Tested on robot

* Fixed github build errors after modifying during robot tests

* fixed more github build errors 2

* fixed more github build errors 3

* Tested on robot and works

* added beam break

* [docs] Update README.md (#107)

* [docs] Update README.md

* Update README.md

* Added auto to intake

* [ci] add comment command action (#111)

* [ci] add comment command action

* fix

* Tested on robot

* fixed intake manual auto, has not been tested on robot

* fixed github build errors

* Added intake better manual and verfied on robot!

* Bump actions/setup-python from 4 to 5 (#116)

Bumps [actions/setup-python](https://github.com/actions/setup-python) from 4 to 5.
- [Release notes](https://github.com/actions/setup-python/releases)
- [Commits](https://github.com/actions/setup-python/compare/v4...v5)

---
updated-dependencies:
- dependency-name: actions/setup-python
  dependency-type: direct:production
  update-type: version-update:semver-major
...

Signed-off-by: dependabot[bot] <support@github.com>
Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com>

* Bump actions/github-script from 6 to 7 (#115)

Bumps [actions/github-script](https://github.com/actions/github-script) from 6 to 7.
- [Release notes](https://github.com/actions/github-script/releases)
- [Commits](https://github.com/actions/github-script/compare/v6...v7)

---
updated-dependencies:
- dependency-name: actions/github-script
  dependency-type: direct:production
  update-type: version-update:semver-major
...

Signed-off-by: dependabot[bot] <support@github.com>
Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com>

* Fixed readme (#118)

* ran wpiformat

* fix

* fix line endings

* builds

---------

Signed-off-by: dependabot[bot] <support@github.com>
Co-authored-by: Kingsley Wong <kingsley.wong@student.education.wa.edu.au>
Co-authored-by: Isaac Turner <spacey_sooty@outlook.com>
Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com>
Co-authored-by: Paul Hodges <111325206+Superbro525Alt@users.noreply.github.com>
---
 .gitattributes                        |  1 +
 .github/workflows/comment-command.yml | 51 +++++++++++++++
 README.md                             |  7 +--
 src/main/cpp/Intake.cpp               | 51 +++++++++------
 src/main/cpp/IntakeBehaviour.cpp      | 91 +++++++++++++++++++--------
 src/main/cpp/Robot.cpp                | 10 +--
 src/main/include/Intake.h             |  5 ++
 src/main/include/IntakeBehaviour.h    |  6 +-
 src/main/include/RobotMap.h           | 27 ++++----
 9 files changed, 176 insertions(+), 73 deletions(-)
 create mode 100644 .github/workflows/comment-command.yml

diff --git a/.gitattributes b/.gitattributes
index 416557ac..df788002 100644
--- a/.gitattributes
+++ b/.gitattributes
@@ -1,5 +1,6 @@
 * text=auto
 *.sh text eol=lf
+*.bat text eol=crlf
 *.gradle text eol=lf
 *.java text eol=lf
 *.json text eol=lf
diff --git a/.github/workflows/comment-command.yml b/.github/workflows/comment-command.yml
new file mode 100644
index 00000000..131cfd9c
--- /dev/null
+++ b/.github/workflows/comment-command.yml
@@ -0,0 +1,51 @@
+name: Comment Commands
+on:
+  issue_comment:
+    types: [ created ]
+
+jobs:
+  format:
+    if: github.event.issue.pull_request && startsWith(github.event.comment.body, '/format')
+    runs-on: ubuntu-22.04
+    steps:
+      - name: React Rocket
+        uses: actions/github-script@v7
+        with:
+          script: |
+            const {owner, repo} = context.issue
+            github.rest.reactions.createForIssueComment({
+              owner,
+              repo,
+              comment_id: context.payload.comment.id,
+              content: "rocket",
+            });
+      - uses: actions/checkout@v4
+        with:
+          fetch-depth: 0
+          token: ${{ secrets.GITHUB_TOKEN }}
+      - name: Fetch all history and metadata
+        run: |
+          git checkout -b pr
+          git branch -f master origin/master
+      - name: Checkout PR
+        run: |
+          gh pr checkout $NUMBER
+        env:
+          GITHUB_TOKEN: "${{ secrets.GITHUB_TOKEN }}"
+          NUMBER: ${{ github.event.issue.number }}
+      - name: Set up Python 3.8
+        uses: actions/setup-python@v5
+        with:
+          python-version: 3.8
+      - name: Install wpiformat
+        run: pip3 install wpiformat
+      - name: Run wpiformat
+        run: wpiformat
+      - name: Commit
+        run: |
+          # Set credentials
+          git config user.name "github-actions[bot]"
+          git config user.email "41898282+github-actions[bot]@users.noreply.github.com"
+          # Commit
+          git commit -am "Formatting fixes"
+          git push
diff --git a/README.md b/README.md
index 6b299ead..7604ddb3 100644
--- a/README.md
+++ b/README.md
@@ -13,8 +13,6 @@ Fork this repository then open up a terminal and run :
 ```bash
 git clone https://github.com/*yourusernamehere*/2024-Crescendo.git
 cd 2024-Crescendo
-chmod +x init.sh
-./init.sh
 ```
 Now look in [CONTRIBUTING.md](./CONTRIBUTING.md) before continuing!
 
@@ -24,7 +22,6 @@ Fork this repository then open up a terminal and run :
 ```powershell
 git clone https:\\github.com\*yourusernamehere*\2024-Crescendo.git
 cd 2024-Crescendo
-.\init
 ```
 Now look in [CONTRIBUTING.md](./CONTRIBUTING.md) before continuing!
 
@@ -53,11 +50,11 @@ Cleaning removes caches of your compiled code. If you do not understand an error
 Simulation
 ----------
 **Release**
-`./gradlew :nativeSimulation`
+`./gradlew :simulateNative`
 Runs a simulation of your code at highest optimisation.
 
 **Debug**
-`./gradlew :nativeSimulationDebug`
+`./gradlew :simulateNativeDebug`
 Runs a debug simulation of your code, including a variety of debugging tools similar to glass but at lower optimisation.
 
 Documentation
diff --git a/src/main/cpp/Intake.cpp b/src/main/cpp/Intake.cpp
index 75839209..f63be144 100644
--- a/src/main/cpp/Intake.cpp
+++ b/src/main/cpp/Intake.cpp
@@ -13,43 +13,51 @@ IntakeConfig Intake::GetConfig() {
 void Intake::OnUpdate(units::second_t dt) {
   switch (_state) {
     case IntakeState::kIdle: {
-      // _config.IntakeMotor.motorController->SetVoltage(0_V);
-      // if (_config.intakeSensor->Get()) {
-      //   setState(IntakeState::kHold);
-      // }
+      if (_config.intakeSensor->Get() == false) {
+        setState(IntakeState::kHold);
+      }
       _stringStateName = "Idle";
       _setVoltage = 0_V;
     } break;
+
     case IntakeState::kRaw: {
-      // _config.IntakeMotor.motorController->SetVoltage(_rawVoltage);
       _stringStateName = "Raw";
       _setVoltage = _rawVoltage;
     } break;
+
     case IntakeState::kEject: {
-      // _config.IntakeMotor.motorController->SetVoltage(-5_V);
-      // if (_config.intakeSensor->Get() == 0 && _config.magSensor->Get() == 0) {
-      //   setState(IntakeState::kIdle);
-      // }
       _stringStateName = "Eject";
-      _setVoltage = -5_V;
+      _setVoltage = 7_V;
+      if (_config.intakeSensor->Get() == true) {
+        setState(IntakeState::kIdle);
+        _ejecting = false;
+      }
     } break;
+
     case IntakeState::kHold: {
-      // _config.IntakeMotor.motorController->SetVoltage(0_V);
       _stringStateName = "Hold";
       _setVoltage = 0_V;
+      // if (_config.intakeSensor->Get() == false) {
+      //   setState(IntakeState::kHold);
+      // }
     } break;
+
     case IntakeState::kIntake: {
-      // _config.IntakeMotor.motorController->SetVoltage(5_V);
       _stringStateName = "Intake";
-      _setVoltage = 5_V;
+      _setVoltage = -7_V;
+      if (_config.intakeSensor->Get() == false) {
+        setState(IntakeState::kHold);
+        _intaking = false;
+      }
     } break;
+
     case IntakeState::kPass: {
-      // _config.IntakeMotor.motorController->SetVoltage(5_V);
-      // if (_config.shooterSensor->Get()) {
-      //   setState(IntakeState::kIdle);
-      //   _stringStateName = "Pass";
-      // }
-      _setVoltage = 5_V;
+      _stringStateName = "Pass";
+      _setVoltage = -7_V;
+      if (_config.intakeSensor->Get() == true) {
+        setState(IntakeState::kIdle);
+        _passing = false;
+      }
     } break;
     default:
       std::cout << "Error: Intake in INVALID STATE." << std::endl;
@@ -57,6 +65,8 @@ void Intake::OnUpdate(units::second_t dt) {
   }
   _table->GetEntry("State: ").SetString(_stringStateName);
   _table->GetEntry("Motor Voltage: ").SetDouble(_setVoltage.value());
+  _table->GetEntry("Intake Sensor: ").SetBoolean(_config.intakeSensor->Get());
+  // _table->GetEntry("Shooter Sensor: ").SetBoolean(_config.shooterSensor->Get());
   // _table->GetEntry("Intake Sensor: ").SetBoolean(_config.intakeSensor->Get());
   // _table->GetEntry("Shooter Sensor: ").SetBoolean(_config.shooterSensor->Get());
   // _table->GetEntry("Magazine Sensor: ").SetBoolean(_config.magSensor->Get());
@@ -72,3 +82,6 @@ void Intake::setState(IntakeState state) {
 void Intake::setRaw(units::volt_t voltage) {
   _rawVoltage = voltage;
 }
+IntakeState Intake::getState() {
+  return _state;
+}
diff --git a/src/main/cpp/IntakeBehaviour.cpp b/src/main/cpp/IntakeBehaviour.cpp
index 90867045..59558bba 100644
--- a/src/main/cpp/IntakeBehaviour.cpp
+++ b/src/main/cpp/IntakeBehaviour.cpp
@@ -12,44 +12,83 @@ IntakeManualControl::IntakeManualControl(Intake* intake, frc::XboxController& co
 }
 
 void IntakeManualControl::OnTick(units::second_t dt) {
-  if (_codriver.GetBButtonPressed()) {
-    if (_rawControl == true) {
+  if (_codriver.GetBButtonReleased()) {
+    if (_rawControl) {
       _rawControl = false;
+      _intaking = false;
+      _ejecting = false;
+      _intake->setState(IntakeState::kIdle);
     } else {
       _rawControl = true;
+      _intaking = false;
+      _ejecting = false;
+      _intake->setState(IntakeState::kRaw);
     }
   }
 
   if (_rawControl) {
-    _intake->setState(IntakeState::kRaw);
-    if (_codriver.GetLeftBumper()) {
-      _intake->setRaw(10_V);
-    } else if (_codriver.GetRightBumper()) {
-      _intake->setRaw(-10_V);
+    if (_codriver.GetRightTriggerAxis() > 0.1) {
+      _intake->setRaw(_codriver.GetRightTriggerAxis() * 10_V);
+    } else if (_codriver.GetLeftTriggerAxis() > 0.1) {
+      _intake->setRaw(_codriver.GetLeftTriggerAxis() * -10_V);
     } else {
       _intake->setRaw(0_V);
     }
+    _intake->setState(IntakeState::kRaw);
+
+  } else {
+    if (_codriver.GetRightTriggerAxis() > 0.1) {
+      if (_intaking) {
+        _intaking = false;
+        _intake->setState(IntakeState::kIdle);
+      } else {
+        _intaking = true;
+        _ejecting = false;
+      }
+    }
+
+    if (_codriver.GetLeftTriggerAxis() > 0.1) {
+      if (_ejecting) {
+        _ejecting = false;
+        _intake->setState(IntakeState::kIdle);
+      } else {
+        _ejecting = true;
+        _intaking = false;
+      }
+    }
+
+    if (_codriver.GetAButtonPressed()) {
+      if (_passing) {
+        _passing = false;
+        _intake->setState(IntakeState::kIdle);
+      } else {
+        _passing = true;
+        _intaking = false;
+      }
+    }
 
-    // _intake->setRaw(_codriver.GetLeftBumper() * 10_V);
-    // _intake->setRaw(_codriver.GetRightBumper() * -10_V);
-    std::cout << "Raw" << std::endl;
+    if (_intaking) {
+      if (_intake->getState() == IntakeState::kIdle) {
+        _intake->setState(IntakeState::kIntake);
+      }
+    }
+
+    if (_passing) {
+      if (_intake->getState() == IntakeState::kHold) {
+        _intake->setState(IntakeState::kPass);
+      }
+    }
+
+    if (_ejecting) {
+      if (_intake->getState() == IntakeState::kIdle || _intake->getState() == IntakeState::kHold) {
+        _intake->setState(IntakeState::kEject);
+      }
+    }
   }
-  // } else {
-  //   if (_codriver.GetYButtonPressed()) {
-  //     _intake->setState(IntakeState::kIntake);
-  //   }
-  //   if (_codriver.GetAButtonPressed()) {
-  //     _intake->setState(IntakeState::kPass);
-  //   }
-  // }
 }
 
-// IntakeAutoControl::IntakeAutoControl(Intake* intake) : _intake(intake) {}
+IntakeAutoControl::IntakeAutoControl(Intake* intake) : _intake(intake) {
+  Controls(intake);
+}
 
-// void IntakeAutoControl::OnTick(units::second_t dt) {
-//   if (_intake->GetConfig().intakeSensor->Get() == 1) {
-//     _intake->setState(IntakeState::kPass);
-//   } else if (_intake->GetConfig().magSensor->Get() == 0) {
-//     _intake->setState(IntakeState::kIdle);
-//   }
-// }
+void IntakeAutoControl::OnTick(units::second_t dt) {}
diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp
index 2278fa1a..473873ba 100644
--- a/src/main/cpp/Robot.cpp
+++ b/src/main/cpp/Robot.cpp
@@ -46,10 +46,6 @@ void Robot::RobotInit() {
 
   // robotmap.swerveBase.gyro->Reset();
 
-  // _swerveDrive = new wom::SwerveDrive(robotmap.swerveBase.config, frc::Pose2d());
-  // wom::BehaviourScheduler::GetInstance()->Register(_swerveDrive);
-  // _swerveDrive->SetDefaultBehaviour(
-  //     [this]() { return wom::make<wom::ManualDrivebase>(_swerveDrive, &robotmap.controllers.driver); });
   _swerveDrive = new wom::SwerveDrive(robotmap.swerveBase.config, frc::Pose2d());
   wom::BehaviourScheduler::GetInstance()->Register(_swerveDrive);
   _swerveDrive->SetDefaultBehaviour(
@@ -93,9 +89,7 @@ void Robot::RobotPeriodic() {
   shooter->OnUpdate(dt);
   sched->Tick();
 
-  robotmap.swerveTable.swerveDriveTable
-      ->GetEntry("frontLeftEncoder")
-
+  robotmap.swerveTable.swerveDriveTable->GetEntry("frontLeftEncoder")
       .SetDouble(robotmap.swerveBase.moduleConfigs[0].turnMotor.encoder->GetEncoderPosition().value());
   robotmap.swerveTable.swerveDriveTable->GetEntry("frontRightEncoder")
       .SetDouble(robotmap.swerveBase.moduleConfigs[1].turnMotor.encoder->GetEncoderPosition().value());
@@ -106,6 +100,7 @@ void Robot::RobotPeriodic() {
 
   _swerveDrive->OnUpdate(dt);
   alphaArm->OnUpdate(dt);
+  shooter->OnStart();
   intake->OnUpdate(dt);
 }
 
@@ -118,7 +113,6 @@ void Robot::AutonomousPeriodic() {}
 void Robot::TeleopInit() {
   loop.Clear();
   wom::BehaviourScheduler* sched = wom::BehaviourScheduler::GetInstance();
-  shooter->OnStart();
   sched->InterruptAll();
 
   // frontLeft->SetVoltage(4_V);
diff --git a/src/main/include/Intake.h b/src/main/include/Intake.h
index a84b41b1..8d9c80b5 100644
--- a/src/main/include/Intake.h
+++ b/src/main/include/Intake.h
@@ -28,6 +28,7 @@ class Intake : public behaviour::HasBehaviour {
 
   void setState(IntakeState state);
   void setRaw(units::volt_t voltage);
+  IntakeState getState();
   IntakeConfig GetConfig();
 
  private:
@@ -37,5 +38,9 @@ class Intake : public behaviour::HasBehaviour {
   units::volt_t _rawVoltage = 0_V;
   std::string _stringStateName = "error";
   units::volt_t _setVoltage = 0_V;
+  bool _intaking;
+  bool _ejecting;
+  bool _passing;
+
   std::shared_ptr<nt::NetworkTable> _table = nt::NetworkTableInstance::GetDefault().GetTable("Intake");
 };
diff --git a/src/main/include/IntakeBehaviour.h b/src/main/include/IntakeBehaviour.h
index c84c6d32..931273fa 100644
--- a/src/main/include/IntakeBehaviour.h
+++ b/src/main/include/IntakeBehaviour.h
@@ -20,7 +20,11 @@ class IntakeManualControl : public behaviour::Behaviour {
   frc::XboxController& _codriver;
 
   units::volt_t _rawVoltage;
-  bool _rawControl;
+  units::volt_t _setVoltage;
+  bool _rawControl = true;
+  bool _intaking = false;
+  bool _ejecting = false;
+  bool _passing = false;
 };
 
 class IntakeAutoControl : public behaviour::Behaviour {
diff --git a/src/main/include/RobotMap.h b/src/main/include/RobotMap.h
index 60c3ed19..0cadce34 100644
--- a/src/main/include/RobotMap.h
+++ b/src/main/include/RobotMap.h
@@ -27,10 +27,22 @@ struct RobotMap {
   struct Controllers {
     frc::XboxController driver = frc::XboxController(0);
     frc::XboxController codriver = frc::XboxController(1);
-    frc::XboxController testController = frc::XboxController(2);
   };
   Controllers controllers;
 
+  struct IntakeSystem {
+    rev::CANSparkMax intakeMotor{2, rev::CANSparkMax::MotorType::kBrushed};
+    // wom::CANSparkMaxEncoder intakeEncoder{&intakeMotor, 0.1_m};
+    frc::DigitalInput intakeSensor{4};
+    // frc::DigitalInput magSensor{0};
+    // frc::DigitalInput shooterSensor{0};
+
+    wom::Gearbox IntakeGearbox{&intakeMotor, nullptr, frc::DCMotor::CIM(1)};
+
+    IntakeConfig config{IntakeGearbox, &intakeSensor /*, &magSensor, &shooterSensor*/};
+  };
+  IntakeSystem intakeSystem;
+
   struct Shooter {
     rev::CANSparkMax shooterMotor{11, rev::CANSparkMax::MotorType::kBrushless};  // Port 11
     // frc::DigitalInput shooterSensor{2};
@@ -52,19 +64,6 @@ struct RobotMap {
   };
   Shooter shooterSystem;
 
-  struct IntakeSystem {
-    rev::CANSparkMax intakeMotor{2, rev::CANSparkMax::MotorType::kBrushed};
-    // wom::CANSparkMaxEncoder intakeEncoder{&intakeMotor, 0.1_m};
-    // frc::DigitalInput intakeSensor{0};
-    // frc::DigitalInput magSensor{0};
-    // frc::DigitalInput shooterSensor{0};
-
-    wom::Gearbox IntakeGearbox{&intakeMotor, nullptr, frc::DCMotor::CIM(1)};
-
-    IntakeConfig config{IntakeGearbox /*, &intakeSensor, &magSensor, &shooterSensor*/};
-  };
-  IntakeSystem intakeSystem;
-
   struct SwerveBase {
     ctre::phoenix6::hardware::CANcoder frontLeftCancoder{18, "Drivebase"};
     ctre::phoenix6::hardware::CANcoder frontRightCancoder{19, "Drivebase"};

From 2cb49082566428f88dc45ec3e6f4156456840853 Mon Sep 17 00:00:00 2001
From: SoulOfTheAssassin <oli.liu798@gmail.com>
Date: Tue, 30 Jan 2024 16:14:11 +0800
Subject: [PATCH 083/207] Fixed some issues

---
 src/main/cpp/Auto.cpp   | 459 ++++++++++++++++++----------------------
 src/main/include/Auto.h |  23 +-
 2 files changed, 221 insertions(+), 261 deletions(-)

diff --git a/src/main/cpp/Auto.cpp b/src/main/cpp/Auto.cpp
index 362a49d8..403a53e5 100644
--- a/src/main/cpp/Auto.cpp
+++ b/src/main/cpp/Auto.cpp
@@ -1,259 +1,216 @@
-// #include "Auto.h"
+#include "Auto.h"
 
-// std::shared_ptr<Behaviour> Taxi(DriveBase _driveBase, Shooter _shooter, Mag _mag, Intake _intake, Arm _arm) {
-//   return 
-//     <<make<ArmToSetPoint>(_arm, armAngle, wristAngle)
-//     <<make<AutoShoot>(_shooter, shooterVolt)
-//     <<make<ArmToSetPoint>(_arm, armAngle, wristAngle)
-//     <<make<DriveToLocation>(_driveBase, raw, distance)
-//   //Shoots starting note then moves out of starting position.
-// }
+std::shared_ptr<behaviour::Behaviour> Taxi(wom::drivetrain::SwerveDrive _driveBase, Shooter _shooter, Intake _intake, AlphaArm _alphaArm) {
+  return make<ArmToSetPoint>(_arm, armAngle)
+    <<make<AutoShoot>(_shooter, shooterVolt)
+    <<make<ArmToSetPoint>(_arm, armAngle)
+    <<make<DriveToLocation>(_driveBase, raw, distance)
+  //Shoots starting note then moves out of starting position.
+}
 
-// std::shared_ptr<Behaviour> QuadrupleClose(DriveBase _driveBase, Shooter _shooter, Mag _mag, Intake _intake, Arm _arm) {
-//   return
-//     << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
-//     << make<AutoShoot>(_shooter, shooterVolt)
-//     << make<DriveToLocation>(_driveBase, raw, distance)
-//     << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
-//     << make<AutoIntake>(_intake, intakeVolt)
-//     << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
-//     << make<AutoShoot>(_shooter, shooterVolt)
-//     << make<DriveToLocation>(_driveBase, raw, distance)
-//     << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
-//     << make<AuttoIntake>(_intake, intakeVolt)
-//     << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
-//     << make<AutoShoot>(_shooter, shooterVolt)
-//     << make<DriveToLocation>(_driveBase, raw, distance)
-//     << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
-//     << make<AutoIntake>(_intake, intakeVolt)
-//     << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
-//     << make<AutoShoo>(_shooter, shooterVolt)
+std::shared_ptr<Behaviour> QuadrupleClose(wom::drivetrain::SwerveDrive _driveBase, Shooter _shooter, Intake _intake, AlphaArm _alphaArm) {
+  return make<ArmToSetPoint>(_arm, armAngle)
+    << make<AutoShoot>(_shooter, shooterVolt)
+    << make<DriveToLocation>(_driveBase, raw, distance)
+    << make<ArmToSetPoint>(_arm, armAngle)
+    << make<AutoIntake>(_intake, intakeVolt)
+    << make<ArmToSetPoint>(_arm, armAngle)
+    << make<AutoShoot>(_shooter, shooterVolt)
+    << make<DriveToLocation>(_driveBase, raw, distance)
+    << make<ArmToSetPoint>(_arm, armAngle)
+    << make<AuttoIntake>(_intake, intakeVolt)
+    << make<ArmToSetPoint>(_arm, armAngle)
+    << make<AutoShoot>(_shooter, shooterVolt)
+    << make<DriveToLocation>(_driveBase, raw, distance)
+    << make<ArmToSetPoint>(_arm, armAngle)
+    << make<AutoIntake>(_intake, intakeVolt)
+    << make<ArmToSetPoint>(_arm, armAngle)
+    << make<AutoShoo>(_shooter, shooterVolt)
 
-//     /* 
-//       4N Close
-//     1. Shoot starting note into speaker
-//     2. Intake note from close note
-//     3. Shoot note into speaker
-//     4. Intake note from close floor note
-//     5. Shoot note into speaker
-//     6. Intake not from close floor
-//     7. Shoot note
-//     */
-// }
+    /* 
+      4N Close
+    1. Shoot starting note into speaker
+    2. Intake note from close note
+    3. Shoot note into speaker
+    4. Intake note from close floor note
+    5. Shoot note into speaker
+    6. Intake not from close floor
+    7. Shoot note
+    */
+}
 
-// std::shared_ptr<Behaviour> QuadrupleFar(DriveBase _driveBase, Shooter _shooter, Mag _mag, Intake _intake, Arm _arm) {
-//   return
-//     << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
-//     << make<AutoShoot>(_shooter, shooterVolt)
-//     << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
-//     << make<DriveToLocation>(_driveBase, raw, distance)
-//     << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
-//     << make<AutoIntake>(_intake, intakeVolt)
-//     //<< make<ArmToSetPoint>(_arm, armAngle, wristAngle)
-//     //<< make<DriveToLocation>(_driveBase, raw, distance)
-//     << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
-//     << make<AutoShoot>(_shooter, shooterVolt)
-//     << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
-//     << make<DriveToLocation>(_driveBase, raw, distance)
-//     << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
-//     << make<AutoIntake>(_intake, intakeVolt)
-//     //<< make<ArmToSetPoint>(_arm, armAngle, wristAngle)
-//     //<< make<DriveToLocation>(_driveBase, raw, distance)
-//     << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
-//     << make<AutoShoot>(_shooter, shooterVolt)
-//     << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
-//     << make<DriveToLocation>(_driveBase, raw, distance)
-//     << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
-//     << make<AutoIntake>(_intake, intakeVolt)
-//     //<< make<ArmToSetPoint>(_arm, armAngle, wristAngle)
-//     //<< make<DriveToLocation>(_driveBase, raw, distance)
-//     << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
-//     << make<AutoShoot>(_shooter, shooterVolt)
-//     // << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
-//     // << make<DriveToLocation>(_driveBase, raw, distance)   do this if possible
-//     // << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
-//     // << make<AutoIntake>(_intake, intakeVolt)
+std::shared_ptr<Behaviour> QuadrupleFar(wom::drivetrain::SwerveDrive _driveBase, Shooter _shooter, Intake _intake, AlphaArm _alphaArm) {
+  return make<ArmToSetPoint>(_arm, armAngle)
+    << make<AutoShoot>(_shooter, shooterVolt)
+    << make<ArmToSetPoint>(_arm, armAngle)
+    << make<DriveToLocation>(_driveBase, raw, distance)
+    << make<ArmToSetPoint>(_arm, armAngle)
+    << make<AutoIntake>(_intake, intakeVolt)
+    //<< make<ArmToSetPoint>(_arm, armAngle)
+    //<< make<DriveToLocation>(_driveBase, raw, distance)
+    << make<ArmToSetPoint>(_arm, armAngle)
+    << make<AutoShoot>(_shooter, shooterVolt)
+    << make<ArmToSetPoint>(_arm, armAngle)
+    << make<DriveToLocation>(_driveBase, raw, distance)
+    << make<ArmToSetPoint>(_arm, armAngle)
+    << make<AutoIntake>(_intake, intakeVolt)
+    //<< make<ArmToSetPoint>(_arm, armAngle)
+    //<< make<DriveToLocation>(_driveBase, raw, distance)
+    << make<ArmToSetPoint>(_arm, armAngle)
+    << make<AutoShoot>(_shooter, shooterVolt)
+    << make<ArmToSetPoint>(_arm, armAngle)
+    << make<DriveToLocation>(_driveBase, raw, distance)
+    << make<ArmToSetPoint>(_arm, armAngle)
+    << make<AutoIntake>(_intake, intakeVolt)
+    adasda
+    //<< make<ArmToSetPoint>(_arm, armAngle)
+    //<< make<DriveToLocation>(_driveBase, raw, distance)
+    << make<ArmToSetPoint>(_arm, armAngle)
+    << make<AutoShoot>(_shooter, shooterVolt)
+    // << make<ArmToSetPoint>(_arm, armAngle)
+    // << make<DriveToLocation>(_driveBase, raw, distance)   do this if possible
+    // << make<ArmToSetPoint>(_arm, armAngle)
+    // << make<AutoIntake>(_intake, intakeVolt)
 
-//     /*
-//       4N Far
-//     1. Shoot start note in speaker
-//     2. Drive to far note
-//     3. Intake note
-//     4. Drive back to shooting line
-//     5. Shoot note into speaker
-//     6. Drive to note
-//     7. Intake note
-//     8. Drive to shooting line
-//     9. Shoot note
-//     10. Drive to note
-//     11. Intake note
-//     12. Drive to shooting line
-//     13. Shoot note
-//     14. Drive to intake note (if possible)
-//     */
-// }
+    /*
+      4N Far
+    1. Shoot start note in speaker
+    2. Drive to far note
+    3. Intake note
+    4. Drive back to shooting line
+    5. Shoot note into speaker
+    6. Drive to note
+    7. Intake note
+    8. Drive to shooting line
+    9. Shoot note
+    10. Drive to note
+    11. Intake note
+    12. Drive to shooting line
+    13. Shoot note
+    14. Drive to intake note (if possible)
+    */
+}
 
-// std::shared_ptr<Behaviour> QuadrupleCloseDoubleFar(DriveBase _driveBase, Shooter _shooter, Mag _mag, Intake _intake, Arm _arm) {
-//   return
-//     << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
-//     << make<AutoShoot>(_shooter, shooterVolt)
-//     << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
-//     << make<DriveToLocation>(_driveBase, raw, distance)
-//     << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
-//     << make<AutoIntake>(_intake, intakeVolt)
-//     //<< make<ArmToSetPoint>(_arm, armAngle, wristAngle)
-//     //<< make<DriveToLocation>(_driveBase, raw, distance)
-//     << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
-//     << make<AutoShoot>(_shooter, shooterVolt)
-//     << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
-//     << make<DriveToLocation>(_driveBase, raw, distance)
-//     << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
-//     << make<AutoIntake>(_intake, intakeVolt)
-//     //<< make<ArmToSetPoint>(_arm, armAngle, wristAngle)
-//     //<< make<DriveToLocation>(_driveBase, raw, distance)
-//     << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
-//     << make<AutoShoot>(_shooter, shooterVolt)
-//     << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
-//     << make<DriveToLocation>(_driveBase, raw, distance)
-//     << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
-//     << make<AutoIntake>(_intake, intakeVolt)
-//     //<< make<ArmToSetPoint>(_arm, armAngle, wristAngle)
-//     //<< make<DriveToLocation>(_driveBase, raw, distance)
-//     << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
-//     << make<AutoShoot>(_shooter, shooterVolt)
-//     << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
-//     << make<DriveToLocation>(_driveBase, raw, distance)
-//     << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
-//     << make<AutoIntake>(_intake, intakeVolt)
-//     << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
-//     << make<DriveToLocation>(_driveBase, raw, distance)
-//     << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
-//     << make<AutoShoot>(_shooter, shooterVolt)
-//     << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
-//     << make<DriveToLocation>(_driveBase, raw, distance)
-//     << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
-//     << make<AutoIntake>(_intake, intakeVolt)
-//     << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
-//     << make<DriveToLocation>(_driveBase, raw, distance)
-//     << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
-//     << make<AutoShoot>(_shooter, shooterVolt)
-//     << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
-//     << make<DriveToLocation>(_driveBase, raw, distance)
-//     << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
-//     << make<AutoIntake>(_intake, intakeVolt)
-//     << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
-//     << make<AutoShoot>(_shooter, shooterVolt)
-//     /*
-//       4N Close 2N Far
-//     1. Shoot note
-//     2. Drive to close note
-//     3. Intake note
-//     4. Shoot note
-//     5. Drive to close note
-//     6. Intake note
-//     7. Shoot note
-//     8. Drive to close note
-//     9. Intake note
-//     10. Shoot note
-//     11. Drive to far note
-//     12. Intake note
-//     13. Drive to shooting line
-//     14. Shoot note
-//     15. Drive to far note
-//     16. Intake note
-//     17. Drive to shooting line
-//     18. Shoot note
-//     */
-// }
+std::shared_ptr<Behaviour> QuadrupleCloseDoubleFar(wom::drivetrain::SwerveDrive _driveBase, Shooter _shooter, Intake _intake, AlphaArm _alphaArm) {
+  return make<ArmToSetPoint>(_arm, armAngle)
+    << make<AutoShoot>(_shooter, shooterVolt)
+    << make<ArmToSetPoint>(_arm, armAngle)
+    << make<DriveToLocation>(_driveBase, raw, distance)
+    << make<ArmToSetPoint>(_arm, armAngle)
+    << make<AutoIntake>(_intake, intakeVolt)
+    //<< make<ArmToSetPoint>(_arm, armAngle)
+    //<< make<DriveToLocation>(_driveBase, raw, distance)
+    << make<ArmToSetPoint>(_arm, armAngle)
+    << make<AutoShoot>(_shooter, shooterVolt)
+    << make<ArmToSetPoint>(_arm, armAngle)
+    << make<DriveToLocation>(_driveBase, raw, distance)
+    << make<ArmToSetPoint>(_arm, armAngle)
+    << make<AutoIntake>(_intake, intakeVolt)
+    //<< make<ArmToSetPoint>(_arm, armAngle)
+    //<< make<DriveToLocation>(_driveBase, raw, distance)
+    << make<ArmToSetPoint>(_arm, armAngle)
+    << make<AutoShoot>(_shooter, shooterVolt)
+    << make<ArmToSetPoint>(_arm, armAngle)
+    << make<DriveToLocation>(_driveBase, raw, distance)
+    << make<ArmToSetPoint>(_arm, armAngle)
+    << make<AutoIntake>(_intake, intakeVolt)
+    //<< make<ArmToSetPoint>(_arm, armAngle)
+    //<< make<DriveToLocation>(_driveBase, raw, distance)
+    << make<ArmToSetPoint>(_arm, armAngle)
+    << make<AutoShoot>(_shooter, shooterVolt)
+    << make<ArmToSetPoint>(_arm, armAngle)
+    << make<DriveToLocation>(_driveBase, raw, distance)
+    << make<ArmToSetPoint>(_arm, armAngle)
+    << make<AutoIntake>(_intake, intakeVolt)
+    << make<ArmToSetPoint>(_arm, armAngle)
+    << make<DriveToLocation>(_driveBase, raw, distance)
+    << make<ArmToSetPoint>(_arm, armAngle)
+    << make<AutoShoot>(_shooter, shooterVolt)
+    << make<ArmToSetPoint>(_arm, armAngle)
+    << make<DriveToLocation>(_driveBase, raw, distance)
+    << make<ArmToSetPoint>(_arm, armAngle)
+    << make<AutoIntake>(_intake, intakeVolt)
+    << make<ArmToSetPoint>(_arm, armAngle)
+    << make<DriveToLocation>(_driveBase, raw, distance)
+    << make<ArmToSetPoint>(_arm, armAngle)
+    << make<AutoShoot>(_shooter, shooterVolt)
+    << make<ArmToSetPoint>(_arm, armAngle)
+    << make<DriveToLocation>(_driveBase, raw, distance)
+    << make<ArmToSetPoint>(_arm, armAngle)
+    << make<AutoIntake>(_intake, intakeVolt)
+    << make<ArmToSetPoint>(_arm, armAngle)
+    << make<AutoShoot>(_shooter, shooterVolt)
+    /*
+      4N Close 2N Far
+    1. Shoot note
+    2. Drive to close note
+    3. Intake note
+    4. Shoot note
+    5. Drive to close note
+    6. Intake note
+    7. Shoot note
+    8. Drive to close note
+    9. Intake note
+    10. Shoot note
+    11. Drive to far note
+    12. Intake note
+    13. Drive to shooting line
+    14. Shoot note
+    15. Drive to far note
+    16. Intake note
+    17. Drive to shooting line
+    18. Shoot note
+    */
+}
 
-// std::shared_ptr<Behaviour> QuadrupleCloseSingleFar(DriveBase _driveBase, Shooter _shooter, Mag _mag, Intake _intake, Arm _arm) {
-//   return
-//     << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
-//     << make<AutoShoot>(_shooter, shooterVolt)
-//     << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
-//     << make<DriveToLocation>(_driveBase, raw, distance)
-//     << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
-//     << make<AutoIntake>(_intake, intakeVolt)
-//     << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
-//     << make<DriveToLocation>(_driveBase, raw, distance)
-//     << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
-//     << make<AutoShoot>(_shooter, shooterVolt)
-//     << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
-//     << make<DriveToLocation>(_driveBase, raw, distance)
-//     << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
-//     << make<AutoIntake>(_intake, intakeVolt)
-//     << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
-//     << make<DriveToLocation>(_driveBase, raw, distance)
-//     << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
-//     << make<AutoShoot>(_shooter, shooterVolt)
-//     << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
-//     << make<DriveToLocation>(_driveBase, raw, distance)
-//     << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
-//     << make<AutoIntake>(_intake, intakeVolt)
-//     << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
-//     << make<DriveToLocation>(_driveBase, raw, distance)
-//     << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
-//     << make<AutoShoot>(_shooter, shooterVolt)
-//     << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+std::shared_ptr<Behaviour> QuadrupleCloseSingleFar(wom::drivetrain::SwerveDrive _driveBase, Shooter _shooter, Intake _intake, AlphaArm _alphaArm) {
+  return make<ArmToSetPoint>(_arm, armAngle)
+    << make<AutoShoot>(_shooter, shooterVolt)
+    << make<ArmToSetPoint>(_arm, armAngle)
+    << make<DriveToLocation>(_driveBase, raw, distance)
+    << make<ArmToSetPoint>(_arm, armAngle)
+    << make<AutoIntake>(_intake, intakeVolt)
+    << make<ArmToSetPoint>(_arm, armAngle)
+    << make<DriveToLocation>(_driveBase, raw, distance)
+    << make<ArmToSetPoint>(_arm, armAngle)
+    << make<AutoShoot>(_shooter, shooterVolt)
+    << make<ArmToSetPoint>(_arm, armAngle)
+    << make<DriveToLocation>(_driveBase, raw, distance)
+    << make<ArmToSetPoint>(_arm, armAngle)
+    << make<AutoIntake>(_intake, intakeVolt)
+    << make<ArmToSetPoint>(_arm, armAngle)
+    << make<DriveToLocation>(_driveBase, raw, distance)
+    << make<ArmToSetPoint>(_arm, armAngle)
+    << make<AutoShoot>(_shooter, shooterVolt)
+    << make<ArmToSetPoint>(_arm, armAngle)
+    << make<DriveToLocation>(_driveBase, raw, distance)
+    << make<ArmToSetPoint>(_arm, armAngle)
+    << make<AutoIntake>(_intake, intakeVolt)
+    << make<ArmToSetPoint>(_arm, armAngle)
+    << make<DriveToLocation>(_driveBase, raw, distance)
+    << make<ArmToSetPoint>(_arm, armAngle)
+    << make<AutoShoot>(_shooter, shooterVolt)
+    << make<ArmToSetPoint>(_arm, armAngle)
 
-//     /*
-//     4N Close 1N Far
-//     1. Shoot note
-//     2. Drive to close note
-//     3. Intake note
-//     4. Drive to speaker
-//     5. Shoot note
-//     6. Drive to close note
-//     7. Intake note
-//     8. Drive to speaker
-//     9. Shoot note
-//     10. Drive to close note
-//     11. Intake note
-//     12. Drive to speaker
-//     13. Shoot note
-//     14. Drive to far note
-//     15. Intake note
-//     15. Drive to speaker
-//     16. shoot 
-//     */
-// }
-
-// std::shared_ptr<Behaviour> TrapAuto(DriveBase _driveBase, Shooter _shooter, Mag _mag, Intake _intake, Arm _arm) {
-//   return 
-//     << make<DriveToLocation>(_driveBase, raw, distance)
-//     << make<>
-//     << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
-//     << make<AutoShoot>(_shooter, shooterVolt)
-//     << make<>
-//     << make<DriveToLocation>(_driveBase, raw, distance)
-//     << make<AutoIntake>(_intake, intakeVolt)
-//     << make<DriveToLocation>(_driveBase, raw, distance)
-    
-//     /* 
-//     TRAP AUTO
-//     1. Drive to trap
-//     2. Climb up
-//     3. Shoot note
-//     4. Climb down
-//     5. Drive to far note
-//     6. Intake note
-//     7. Drive to trap
-//     8. Climb up
-//     9. Shoot note
-//     10. Climb down
-//     11. Drive to far note
-//     12. Intake
-//     13. Drive to trap 
-//     14. Climb up
-//     15. Shoot note
-//     16. Climb down
-//     */
-// }
-
-// std::shared_ptr<Behaviour> AutoTest(DriveBase _driveBase, Shooter _shooter, Mag _mag, Intake _intake, Arm _arm) {
-//   return
-//     <<make<ArmToSetPoint>(_arm, 0, -90)
-//     <<make<DriveToLocation>(_driveBase, raw, distance)
-//     <<make<AutoShoot>(_shooter, 8_V)
-//     <<make<AutoIntake>(_intake, 8_V)
-// } // This auto is a test for auto to see if all things work, it does not build as the behaviours are not done.
\ No newline at end of file
+    /*
+    4N Close 1N Far
+    1. Shoot note
+    2. Drive to close note
+    3. Intake note
+    4. Drive to speaker
+    5. Shoot note
+    6. Drive to close note
+    7. Intake note
+    8. Drive to speaker
+    9. Shoot note
+    10. Drive to close note
+    11. Intake note
+    12. Drive to speaker
+    13. Shoot note
+    14. Drive to far note
+    15. Intake note
+    15. Drive to speaker
+    16. shoot 
+    */
+}
+   
\ No newline at end of file
diff --git a/src/main/include/Auto.h b/src/main/include/Auto.h
index 1bdafc0c..0a2b2d37 100644
--- a/src/main/include/Auto.h
+++ b/src/main/include/Auto.h
@@ -1,20 +1,23 @@
 
-// #pragma once
+#pragma once
 
-// #include "Wombat.h"
+#include "Wombat.h"
+#include "Intake.h"
+#include "Shooter.h"
+#include "AlphaArm.h"
 
-// namespace autos {
-//   std::shared_ptr<behaviour::Behaviour> AutoTest(Drivebase driveBase, Shooter shooter, Mag mag, Intake intake, Arm _arm);
+namespace autos {
+  std::shared_ptr<behaviour::Behaviour> AutoTest(wom::drivetrain::SwerveDrive Swervedrive, wom::subsystems::Shooter shooter, Intake intake, wom::subsystems::Arm _arm);
 
-//   // std::shared_ptr<behaviour::Behaviour> Taxi(Drivebase driveBase, Shooter shooter, Mag mag, Intake intake, Arm _arm);
+  // std::shared_ptr<behaviour::Behaviour> Taxi(SwerveDrive Swervedrive, Shooter shooter, Mag mag, Intake intake, Arm _arm);
 
-//   // std::shared_ptr<behaviour::Behaviour> QuadrupleClose(Drivebase driveBase, Shooter shooter, Mag mag, Intake intake, Arm _arm);
+  // std::shared_ptr<behaviour::Behaviour> QuadrupleClose(SwerveDrive Swervedrive, Shooter shooter, Mag mag, Intake intake, Arm _arm);
 
-//   // std::shared_ptr<behaviour::Behaviour> QuadrupleFar(Drivebase driveBase, Shooter shooter, Mag mag, Intake intake, Arm _arm);
+  // std::shared_ptr<behaviour::Behaviour> QuadrupleFar(SwerveDrive Swervedrive, Shooter shooter, Mag mag, Intake intake, Arm _arm);
 
-//   // std::shared_ptr<behaviour::Behaviour> QuadrupleCloseDoubleFar(Drivebase driveBase, Shooter shooter, Mag mag, Intake intake, Arm _arm);
+  // std::shared_ptr<behaviour::Behaviour> QuadrupleCloseDoubleFar(SwerveDrive Swervedrive, Shooter shooter, Mag mag, Intake intake, Arm _arm);
 
-//   // std::shared_ptr<behaviour::Behaviour> QuadrupleCloseSingleFar(Drivebase driveBase, Shooter shooter, Mag mag, Intake intake, Arm _arm);
-// }
+  // std::shared_ptr<behaviour::Behaviour> QuadrupleCloseSingleFar(SwerveDrive Swervedrive, Shooter shooter, Mag mag, Intake intake, Arm _arm);
+}
 
 

From 50dc38487aaab5a2bb0587f484525079b8401d49 Mon Sep 17 00:00:00 2001
From: Sean Chan <sean.chan3@student.education.wa.edu.au>
Date: Tue, 30 Jan 2024 16:46:24 +0800
Subject: [PATCH 084/207] added arm behaviour for auto and fixed errors

---
 src/main/cpp/AlphaArmBehaviour.cpp   |  11 +++
 src/main/cpp/Auto.cpp                | 136 +++++++++++++--------------
 src/main/cpp/ShooterBehaviour.cpp    |   1 +
 src/main/include/AlphaArmBehaviour.h |  11 +++
 src/main/include/Auto.h              |  24 ++---
 src/main/include/RobotMap.h          |   4 +-
 src/main/include/Shooter.h           |   2 +-
 7 files changed, 107 insertions(+), 82 deletions(-)

diff --git a/src/main/cpp/AlphaArmBehaviour.cpp b/src/main/cpp/AlphaArmBehaviour.cpp
index ece50ebb..29ac389b 100644
--- a/src/main/cpp/AlphaArmBehaviour.cpp
+++ b/src/main/cpp/AlphaArmBehaviour.cpp
@@ -6,6 +6,7 @@
 
 #include <frc/XboxController.h>
 
+
 AlphaArmManualControl::AlphaArmManualControl(AlphaArm* alphaArm, frc::XboxController* codriver)
     : _alphaArm(alphaArm), _codriver(codriver) {
   Controls(alphaArm);
@@ -33,3 +34,13 @@ void AlphaArmManualControl::OnTick(units::second_t dt) {
     }
   }
 }
+
+ArmToSetPoint::ArmToSetPoint(AlphaArm* alphaArm, units::degree_t armAngle, float armSpeed) : _alphaArm(alphaArm) {
+  Controls(alphaArm);
+}
+// // ArmSpeed is a float from 0-1, 1 being instantly and 0 being don't move at all.
+
+void ArmToSetPoint::OnTick(units::second_t dt) {
+  // _armCurrentDegree = _alphaArm->GetConfig().alphaArmGearbox.encoder.GetEncoderPosition();
+  // _alphaArm->GetConfig().alphaArmGearbox.encoder.SetEncoderPosition(armAngle * armSpeed);
+}
\ No newline at end of file
diff --git a/src/main/cpp/Auto.cpp b/src/main/cpp/Auto.cpp
index 362a49d8..f863afc9 100644
--- a/src/main/cpp/Auto.cpp
+++ b/src/main/cpp/Auto.cpp
@@ -1,32 +1,32 @@
-// #include "Auto.h"
+#include "Auto.h"
 
 // std::shared_ptr<Behaviour> Taxi(DriveBase _driveBase, Shooter _shooter, Mag _mag, Intake _intake, Arm _arm) {
 //   return 
-//     <<make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+//     <<make<ArmToSetPoint>(_arm, armAngle)
 //     <<make<AutoShoot>(_shooter, shooterVolt)
-//     <<make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+//     <<make<ArmToSetPoint>(_arm, armAngle)
 //     <<make<DriveToLocation>(_driveBase, raw, distance)
 //   //Shoots starting note then moves out of starting position.
 // }
 
 // std::shared_ptr<Behaviour> QuadrupleClose(DriveBase _driveBase, Shooter _shooter, Mag _mag, Intake _intake, Arm _arm) {
 //   return
-//     << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+//     << make<ArmToSetPoint>(_arm, armAngle)
 //     << make<AutoShoot>(_shooter, shooterVolt)
 //     << make<DriveToLocation>(_driveBase, raw, distance)
-//     << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+//     << make<ArmToSetPoint>(_arm, armAngle)
 //     << make<AutoIntake>(_intake, intakeVolt)
-//     << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+//     << make<ArmToSetPoint>(_arm, armAngle)
 //     << make<AutoShoot>(_shooter, shooterVolt)
 //     << make<DriveToLocation>(_driveBase, raw, distance)
-//     << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+//     << make<ArmToSetPoint>(_arm, armAngle)
 //     << make<AuttoIntake>(_intake, intakeVolt)
-//     << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+//     << make<ArmToSetPoint>(_arm, armAngle)we
 //     << make<AutoShoot>(_shooter, shooterVolt)
 //     << make<DriveToLocation>(_driveBase, raw, distance)
-//     << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+//     << make<ArmToSetPoint>(_arm, armAngle)
 //     << make<AutoIntake>(_intake, intakeVolt)
-//     << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+//     << make<ArmToSetPoint>(_arm, armAngle)
 //     << make<AutoShoo>(_shooter, shooterVolt)
 
 //     /* 
@@ -43,35 +43,35 @@
 
 // std::shared_ptr<Behaviour> QuadrupleFar(DriveBase _driveBase, Shooter _shooter, Mag _mag, Intake _intake, Arm _arm) {
 //   return
-//     << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+//     << make<ArmToSetPoint>(_arm, armAngle)
 //     << make<AutoShoot>(_shooter, shooterVolt)
-//     << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+//     << make<ArmToSetPoint>(_arm, armAngle)
 //     << make<DriveToLocation>(_driveBase, raw, distance)
-//     << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+//     << make<ArmToSetPoint>(_arm, armAngle)
 //     << make<AutoIntake>(_intake, intakeVolt)
-//     //<< make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+//     //<< make<ArmToSetPoint>(_arm, armAngle)
 //     //<< make<DriveToLocation>(_driveBase, raw, distance)
-//     << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+//     << make<ArmToSetPoint>(_arm, armAngle)
 //     << make<AutoShoot>(_shooter, shooterVolt)
-//     << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+//     << make<ArmToSetPoint>(_arm, armAngle)
 //     << make<DriveToLocation>(_driveBase, raw, distance)
-//     << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+//     << make<ArmToSetPoint>(_arm, armAngle)
 //     << make<AutoIntake>(_intake, intakeVolt)
-//     //<< make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+//     //<< make<ArmToSetPoint>(_arm, armAngle)
 //     //<< make<DriveToLocation>(_driveBase, raw, distance)
-//     << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+//     << make<ArmToSetPoint>(_arm, armAngle)
 //     << make<AutoShoot>(_shooter, shooterVolt)
-//     << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+//     << make<ArmToSetPoint>(_arm, armAngle)
 //     << make<DriveToLocation>(_driveBase, raw, distance)
-//     << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+//     << make<ArmToSetPoint>(_arm, armAngle)
 //     << make<AutoIntake>(_intake, intakeVolt)
-//     //<< make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+//     //<< make<ArmToSetPoint>(_arm, armAngle)
 //     //<< make<DriveToLocation>(_driveBase, raw, distance)
-//     << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+//     << make<ArmToSetPoint>(_arm, armAngle)
 //     << make<AutoShoot>(_shooter, shooterVolt)
-//     // << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+//     // << make<ArmToSetPoint>(_arm, armAngle)
 //     // << make<DriveToLocation>(_driveBase, raw, distance)   do this if possible
-//     // << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+//     // << make<ArmToSetPoint>(_arm, armAngle)
 //     // << make<AutoIntake>(_intake, intakeVolt)
 
 //     /*
@@ -95,53 +95,53 @@
 
 // std::shared_ptr<Behaviour> QuadrupleCloseDoubleFar(DriveBase _driveBase, Shooter _shooter, Mag _mag, Intake _intake, Arm _arm) {
 //   return
-//     << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+//     << make<ArmToSetPoint>(_arm, armAngle)
 //     << make<AutoShoot>(_shooter, shooterVolt)
-//     << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+//     << make<ArmToSetPoint>(_arm, armAngle)
 //     << make<DriveToLocation>(_driveBase, raw, distance)
-//     << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+//     << make<ArmToSetPoint>(_arm, armAngle)
 //     << make<AutoIntake>(_intake, intakeVolt)
-//     //<< make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+//     //<< make<ArmToSetPoint>(_arm, armAngle)
 //     //<< make<DriveToLocation>(_driveBase, raw, distance)
-//     << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+//     << make<ArmToSetPoint>(_arm, armAngle)
 //     << make<AutoShoot>(_shooter, shooterVolt)
-//     << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+//     << make<ArmToSetPoint>(_arm, armAngle)
 //     << make<DriveToLocation>(_driveBase, raw, distance)
-//     << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+//     << make<ArmToSetPoint>(_arm, armAngle)
 //     << make<AutoIntake>(_intake, intakeVolt)
-//     //<< make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+//     //<< make<ArmToSetPoint>(_arm, armAngle)
 //     //<< make<DriveToLocation>(_driveBase, raw, distance)
-//     << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+//     << make<ArmToSetPoint>(_arm, armAngle)
 //     << make<AutoShoot>(_shooter, shooterVolt)
-//     << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+//     << make<ArmToSetPoint>(_arm, armAngle)
 //     << make<DriveToLocation>(_driveBase, raw, distance)
-//     << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+//     << make<ArmToSetPoint>(_arm, armAngle)
 //     << make<AutoIntake>(_intake, intakeVolt)
-//     //<< make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+//     //<< make<ArmToSetPoint>(_arm, armAngle)
 //     //<< make<DriveToLocation>(_driveBase, raw, distance)
-//     << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+//     << make<ArmToSetPoint>(_arm, armAngle)
 //     << make<AutoShoot>(_shooter, shooterVolt)
-//     << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+//     << make<ArmToSetPoint>(_arm, armAngle)
 //     << make<DriveToLocation>(_driveBase, raw, distance)
-//     << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+//     << make<ArmToSetPoint>(_arm, armAngle)
 //     << make<AutoIntake>(_intake, intakeVolt)
-//     << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+//     << make<ArmToSetPoint>(_arm, armAngle)
 //     << make<DriveToLocation>(_driveBase, raw, distance)
-//     << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+//     << make<ArmToSetPoint>(_arm, armAngle)
 //     << make<AutoShoot>(_shooter, shooterVolt)
-//     << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+//     << make<ArmToSetPoint>(_arm, armAngle)
 //     << make<DriveToLocation>(_driveBase, raw, distance)
-//     << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+//     << make<ArmToSetPoint>(_arm, armAngle)
 //     << make<AutoIntake>(_intake, intakeVolt)
-//     << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+//     << make<ArmToSetPoint>(_arm, armAngle)
 //     << make<DriveToLocation>(_driveBase, raw, distance)
-//     << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+//     << make<ArmToSetPoint>(_arm, armAngle)
 //     << make<AutoShoot>(_shooter, shooterVolt)
-//     << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+//     << make<ArmToSetPoint>(_arm, armAngle)
 //     << make<DriveToLocation>(_driveBase, raw, distance)
-//     << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+//     << make<ArmToSetPoint>(_arm, armAngle)
 //     << make<AutoIntake>(_intake, intakeVolt)
-//     << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+//     << make<ArmToSetPoint>(_arm, armAngle)
 //     << make<AutoShoot>(_shooter, shooterVolt)
 //     /*
 //       4N Close 2N Far
@@ -168,33 +168,33 @@
 
 // std::shared_ptr<Behaviour> QuadrupleCloseSingleFar(DriveBase _driveBase, Shooter _shooter, Mag _mag, Intake _intake, Arm _arm) {
 //   return
-//     << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+//     << make<ArmToSetPoint>(_arm, armAngle)
 //     << make<AutoShoot>(_shooter, shooterVolt)
-//     << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+//     << make<ArmToSetPoint>(_arm, armAngle)
 //     << make<DriveToLocation>(_driveBase, raw, distance)
-//     << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+//     << make<ArmToSetPoint>(_arm, armAngle)
 //     << make<AutoIntake>(_intake, intakeVolt)
-//     << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+//     << make<ArmToSetPoint>(_arm, armAngle)
 //     << make<DriveToLocation>(_driveBase, raw, distance)
-//     << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+//     << make<ArmToSetPoint>(_arm, armAngle)
 //     << make<AutoShoot>(_shooter, shooterVolt)
-//     << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+//     << make<ArmToSetPoint>(_arm, armAngle)
 //     << make<DriveToLocation>(_driveBase, raw, distance)
-//     << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+//     << make<ArmToSetPoint>(_arm, armAngle)
 //     << make<AutoIntake>(_intake, intakeVolt)
-//     << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+//     << make<ArmToSetPoint>(_arm, armAngle)
 //     << make<DriveToLocation>(_driveBase, raw, distance)
-//     << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+//     << make<ArmToSetPoint>(_arm, armAngle)
 //     << make<AutoShoot>(_shooter, shooterVolt)
-//     << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+//     << make<ArmToSetPoint>(_arm, armAngle)
 //     << make<DriveToLocation>(_driveBase, raw, distance)
-//     << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+//     << make<ArmToSetPoint>(_arm, armAngle)
 //     << make<AutoIntake>(_intake, intakeVolt)
-//     << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+//     << make<ArmToSetPoint>(_arm, armAngle)
 //     << make<DriveToLocation>(_driveBase, raw, distance)
-//     << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+//     << make<ArmToSetPoint>(_arm, armAngle)
 //     << make<AutoShoot>(_shooter, shooterVolt)
-//     << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+//     << make<ArmToSetPoint>(_arm, armAngle)
 
 //     /*
 //     4N Close 1N Far
@@ -222,7 +222,7 @@
 //   return 
 //     << make<DriveToLocation>(_driveBase, raw, distance)
 //     << make<>
-//     << make<ArmToSetPoint>(_arm, armAngle, wristAngle)
+//     << make<ArmToSetPoint>(_arm, armAngle)
 //     << make<AutoShoot>(_shooter, shooterVolt)
 //     << make<>
 //     << make<DriveToLocation>(_driveBase, raw, distance)
@@ -250,10 +250,10 @@
 //     */
 // }
 
-// std::shared_ptr<Behaviour> AutoTest(DriveBase _driveBase, Shooter _shooter, Mag _mag, Intake _intake, Arm _arm) {
-//   return
-//     <<make<ArmToSetPoint>(_arm, 0, -90)
+std::shared_ptr<behaviour::Behaviour> autos::AutoTest(wom::drivetrain::SwerveDrive _driveBase, Shooter _shooter, Intake _intake, AlphaArm _alphaArm) {
+  return behaviour::make<ArmToSetPoint>(&_alphaArm, 0 * 1_deg, 0.2);
 //     <<make<DriveToLocation>(_driveBase, raw, distance)
 //     <<make<AutoShoot>(_shooter, 8_V)
 //     <<make<AutoIntake>(_intake, 8_V)
-// } // This auto is a test for auto to see if all things work, it does not build as the behaviours are not done.
\ No newline at end of file
+} // This auto is a test for auto to see if all things work.
+
diff --git a/src/main/cpp/ShooterBehaviour.cpp b/src/main/cpp/ShooterBehaviour.cpp
index 341eef15..4b31c06b 100644
--- a/src/main/cpp/ShooterBehaviour.cpp
+++ b/src/main/cpp/ShooterBehaviour.cpp
@@ -36,3 +36,4 @@ void ShooterManualControl::OnTick(units::second_t dt) {
     _shooter->SetState(ShooterState::kRaw);
   }
 }
+
diff --git a/src/main/include/AlphaArmBehaviour.h b/src/main/include/AlphaArmBehaviour.h
index 4c2d03fb..c48bdcae 100644
--- a/src/main/include/AlphaArmBehaviour.h
+++ b/src/main/include/AlphaArmBehaviour.h
@@ -22,3 +22,14 @@ class AlphaArmManualControl : public behaviour::Behaviour {
   // )?_codriver->GetRightY():0) * 2_V;
   bool _rawControl;
 };
+
+class ArmToSetPoint : public behaviour::Behaviour {
+ public:
+  explicit ArmToSetPoint(AlphaArm* alphaArm, units::degree_t armAngle, float armSpeed);
+  void OnTick(units::second_t dt);
+
+
+ private:
+  AlphaArm* _alphaArm;
+  // double _armCurrentDegree;
+};
diff --git a/src/main/include/Auto.h b/src/main/include/Auto.h
index 1bdafc0c..1b584812 100644
--- a/src/main/include/Auto.h
+++ b/src/main/include/Auto.h
@@ -1,20 +1,22 @@
 
-// #pragma once
+#pragma once
 
-// #include "Wombat.h"
+#include "Wombat.h"
 
-// namespace autos {
-//   std::shared_ptr<behaviour::Behaviour> AutoTest(Drivebase driveBase, Shooter shooter, Mag mag, Intake intake, Arm _arm);
+#include "AlphaArmBehaviour.h"
+#include "Intake.h"
+#include "Shooter.h"
 
-//   // std::shared_ptr<behaviour::Behaviour> Taxi(Drivebase driveBase, Shooter shooter, Mag mag, Intake intake, Arm _arm);
+namespace autos {
+  std::shared_ptr<behaviour::Behaviour> AutoTest(wom::drivetrain::SwerveDrive _swerveDrive, Shooter _shooter, Intake _intake, AlphaArm _alphaArm);
 
-//   // std::shared_ptr<behaviour::Behaviour> QuadrupleClose(Drivebase driveBase, Shooter shooter, Mag mag, Intake intake, Arm _arm);
+  // std::shared_ptr<behaviour::Behaviour> Taxi(Drivebase driveBase, Shooter shooter, Mag mag, Intake intake, Arm _arm);
 
-//   // std::shared_ptr<behaviour::Behaviour> QuadrupleFar(Drivebase driveBase, Shooter shooter, Mag mag, Intake intake, Arm _arm);
+  // std::shared_ptr<behaviour::Behaviour> QuadrupleClose(Drivebase driveBase, Shooter shooter, Mag mag, Intake intake, Arm _arm);
 
-//   // std::shared_ptr<behaviour::Behaviour> QuadrupleCloseDoubleFar(Drivebase driveBase, Shooter shooter, Mag mag, Intake intake, Arm _arm);
-
-//   // std::shared_ptr<behaviour::Behaviour> QuadrupleCloseSingleFar(Drivebase driveBase, Shooter shooter, Mag mag, Intake intake, Arm _arm);
-// }
+  // std::shared_ptr<behaviour::Behaviour> QuadrupleFar(Drivebase driveBase, Shooter shooter, Mag mag, Intake intake, Arm _arm);
 
+  // std::shared_ptr<behaviour::Behaviour> QuadrupleCloseDoubleFar(Drivebase driveBase, Shooter shooter, Mag mag, Intake intake, Arm _arm);
 
+  // std::shared_ptr<behaviour::Behaviour> QuadrupleCloseSingleFar(Drivebase driveBase, Shooter shooter, Mag mag, Intake intake, Arm _arm);
+}
\ No newline at end of file
diff --git a/src/main/include/RobotMap.h b/src/main/include/RobotMap.h
index 9818b497..83d70d37 100644
--- a/src/main/include/RobotMap.h
+++ b/src/main/include/RobotMap.h
@@ -37,8 +37,8 @@ struct RobotMap {
   struct AlphaArmSystem {
     rev::CANSparkMax alphaArmMotor{12, rev::CANSparkMax::MotorType::kBrushless};
     rev::CANSparkMax wristMotor{15, rev::CANSparkMax::MotorType::kBrushless};
-
-    wom::Gearbox alphaArmGearbox{&alphaArmMotor, nullptr, frc::DCMotor::NEO(1)};
+    wom::CANSparkMaxEncoder* alphaArmEncoder = new wom::CANSparkMaxEncoder(&alphaArmMotor, 0.1_m);
+    wom::Gearbox alphaArmGearbox{&alphaArmMotor, alphaArmEncoder, frc::DCMotor::NEO(1)};
     wom::Gearbox wristGearbox{&wristMotor, nullptr, frc::DCMotor::NEO(1)};
 
     AlphaArmConfig config{alphaArmGearbox, wristGearbox};
diff --git a/src/main/include/Shooter.h b/src/main/include/Shooter.h
index cf21c3bc..6f990649 100644
--- a/src/main/include/Shooter.h
+++ b/src/main/include/Shooter.h
@@ -15,7 +15,7 @@
 struct ShooterConfig {
   std::string path;
   wom::Gearbox ShooterGearbox;
-  // wom::PIDConfig<units::radians_per_second, units::volt> pidConfig;
+  // wom::PIDConfig<units::radians_per_second, units::volt> pidConfig;  
   // frc::DigitalInput* shooterSensor;
 };
 

From 5bca0159ab4f53cf6cf1e2feea6fc52232fa7549 Mon Sep 17 00:00:00 2001
From: SoulOfTheAssassin <oli.liu798@gmail.com>
Date: Sat, 3 Feb 2024 11:40:19 +0800
Subject: [PATCH 085/207] Continued working on Autos

---
 src/main/cpp/Auto.cpp    | 270 +++++++++++++++++++--------------------
 src/main/cpp/Shooter.cpp |   2 +-
 2 files changed, 136 insertions(+), 136 deletions(-)

diff --git a/src/main/cpp/Auto.cpp b/src/main/cpp/Auto.cpp
index 403a53e5..7517b8d3 100644
--- a/src/main/cpp/Auto.cpp
+++ b/src/main/cpp/Auto.cpp
@@ -1,33 +1,33 @@
 #include "Auto.h"
 
 std::shared_ptr<behaviour::Behaviour> Taxi(wom::drivetrain::SwerveDrive _driveBase, Shooter _shooter, Intake _intake, AlphaArm _alphaArm) {
-  return make<ArmToSetPoint>(_arm, armAngle)
-    <<make<AutoShoot>(_shooter, shooterVolt)
-    <<make<ArmToSetPoint>(_arm, armAngle)
-    <<make<DriveToLocation>(_driveBase, raw, distance)
-  //Shoots starting note then moves out of starting position.
+  return behaviour::make<ArmToSetPoint>(_arm, 60)
+    <<make<AutoShoot>(_shooter, 12);
+    <<make<ArmToSetPoint>(_arm, 0);
+    <<make<DriveToLocation>(_driveBase, 12, distance);
 }
+//Shoots starting note then moves out of starting position.
 
 std::shared_ptr<Behaviour> QuadrupleClose(wom::drivetrain::SwerveDrive _driveBase, Shooter _shooter, Intake _intake, AlphaArm _alphaArm) {
-  return make<ArmToSetPoint>(_arm, armAngle)
-    << make<AutoShoot>(_shooter, shooterVolt)
-    << make<DriveToLocation>(_driveBase, raw, distance)
-    << make<ArmToSetPoint>(_arm, armAngle)
-    << make<AutoIntake>(_intake, intakeVolt)
-    << make<ArmToSetPoint>(_arm, armAngle)
-    << make<AutoShoot>(_shooter, shooterVolt)
-    << make<DriveToLocation>(_driveBase, raw, distance)
-    << make<ArmToSetPoint>(_arm, armAngle)
-    << make<AuttoIntake>(_intake, intakeVolt)
-    << make<ArmToSetPoint>(_arm, armAngle)
-    << make<AutoShoot>(_shooter, shooterVolt)
-    << make<DriveToLocation>(_driveBase, raw, distance)
-    << make<ArmToSetPoint>(_arm, armAngle)
-    << make<AutoIntake>(_intake, intakeVolt)
-    << make<ArmToSetPoint>(_arm, armAngle)
-    << make<AutoShoo>(_shooter, shooterVolt)
+  return make<ArmToSetPoint>(_arm, 60) 
+    << make<AutoShoot>(_shooter, 12)
+    << make<DriveToLocation>(_driveBase, 12, distance, direction)
+    << make<ArmToSetPoint>(_arm, 180)
+    << make<AutoIntake>(_intake, 12)
+    << make<ArmToSetPoint>(_arm, 60)
+    << make<AutoShoot>(_shooter, 12)
+    << make<DriveToLocation>(_driveBase, 12, distance, direction)
+    << make<ArmToSetPoint>(_arm, armAngle)
+    << make<AutoIntake>(_intake, 12)
+    << make<ArmToSetPoint>(_arm, 60)
+    << make<AutoShoot>(_shooter, 12)
+    << make<DriveToLocation>(_driveBase, 12, distance, direction)
+    << make<ArmToSetPoint>(_arm, 180)
+    << make<AutoIntake>(_intake, 12)
+    << make<ArmToSetPoint>(_arm, 60)
+    << make<AutoShoo>(_shooter, 12)
 
-    /* 
+     /*
       4N Close
     1. Shoot starting note into speaker
     2. Intake note from close note
@@ -40,37 +40,37 @@ std::shared_ptr<Behaviour> QuadrupleClose(wom::drivetrain::SwerveDrive _driveBas
 }
 
 std::shared_ptr<Behaviour> QuadrupleFar(wom::drivetrain::SwerveDrive _driveBase, Shooter _shooter, Intake _intake, AlphaArm _alphaArm) {
-  return make<ArmToSetPoint>(_arm, armAngle)
-    << make<AutoShoot>(_shooter, shooterVolt)
-    << make<ArmToSetPoint>(_arm, armAngle)
-    << make<DriveToLocation>(_driveBase, raw, distance)
+  return make<ArmToSetPoint>(_arm, /12)
+    << make<AutoShoot>(_shooter, 12)
+    << make<ArmToSetPoint>(_arm, 0)
+    << make<DriveToLocation>(_driveBase, 12, distance, direction)
     << make<ArmToSetPoint>(_arm, armAngle)
-    << make<AutoIntake>(_intake, intakeVolt)
+    << make<AutoIntake>(_intake, 12)
     //<< make<ArmToSetPoint>(_arm, armAngle)
-    //<< make<DriveToLocation>(_driveBase, raw, distance)
+    //<< make<DriveToLocation>(_driveBase, 12, distance, direction)
+    << make<ArmToSetPoint>(_arm, 60)
+    << make<AutoShoot>(_shooter, 12)
     << make<ArmToSetPoint>(_arm, armAngle)
-    << make<AutoShoot>(_shooter, shooterVolt)
+    << make<DriveToLocation>(_driveBase, 12, distance, direction)
     << make<ArmToSetPoint>(_arm, armAngle)
-    << make<DriveToLocation>(_driveBase, raw, distance)
-    << make<ArmToSetPoint>(_arm, armAngle)
-    << make<AutoIntake>(_intake, intakeVolt)
+    << make<AutoIntake>(_intake, 12)
     //<< make<ArmToSetPoint>(_arm, armAngle)
-    //<< make<DriveToLocation>(_driveBase, raw, distance)
-    << make<ArmToSetPoint>(_arm, armAngle)
-    << make<AutoShoot>(_shooter, shooterVolt)
+    //<< make<DriveToLocation>(_driveBase, 12, distance, direction)
+    << make<ArmToSetPoint>(_arm, 60)
+    << make<AutoShoot>(_shooter, 12)
     << make<ArmToSetPoint>(_arm, armAngle)
-    << make<DriveToLocation>(_driveBase, raw, distance)
+    << make<DriveToLocation>(_driveBase, 12, distance, direction)
     << make<ArmToSetPoint>(_arm, armAngle)
-    << make<AutoIntake>(_intake, intakeVolt)
+    << make<AutoIntake>(_intake, 12)
     adasda
     //<< make<ArmToSetPoint>(_arm, armAngle)
-    //<< make<DriveToLocation>(_driveBase, raw, distance)
-    << make<ArmToSetPoint>(_arm, armAngle)
-    << make<AutoShoot>(_shooter, shooterVolt)
+    //<< make<DriveToLocation>(_driveBase, 12, distance, direction)
+    << make<ArmToSetPoint>(_arm, 60)
+    << make<AutoShoot>(_shooter, 12)
     // << make<ArmToSetPoint>(_arm, armAngle)
-    // << make<DriveToLocation>(_driveBase, raw, distance)   do this if possible
+    // << make<DriveToLocation>(_driveBase, 12, distance, direction)   do this if possible
     // << make<ArmToSetPoint>(_arm, armAngle)
-    // << make<AutoIntake>(_intake, intakeVolt)
+    // << make<AutoIntake>(_intake, 12)
 
     /*
       4N Far
@@ -93,124 +93,124 @@ std::shared_ptr<Behaviour> QuadrupleFar(wom::drivetrain::SwerveDrive _driveBase,
 
 std::shared_ptr<Behaviour> QuadrupleCloseDoubleFar(wom::drivetrain::SwerveDrive _driveBase, Shooter _shooter, Intake _intake, AlphaArm _alphaArm) {
   return make<ArmToSetPoint>(_arm, armAngle)
-    << make<AutoShoot>(_shooter, shooterVolt)
+    << make<AutoShoot>(_shooter, 12)
     << make<ArmToSetPoint>(_arm, armAngle)
-    << make<DriveToLocation>(_driveBase, raw, distance)
+    << make<DriveToLocation>(_driveBase, 12, distance, direction)
     << make<ArmToSetPoint>(_arm, armAngle)
-    << make<AutoIntake>(_intake, intakeVolt)
+    << make<Intake>(_intake, 12)
     //<< make<ArmToSetPoint>(_arm, armAngle)
-    //<< make<DriveToLocation>(_driveBase, raw, distance)
+    //<< make<DriveToLocation>(_driveBase, 12, distance, direction)
     << make<ArmToSetPoint>(_arm, armAngle)
-    << make<AutoShoot>(_shooter, shooterVolt)
+    << make<AutoShoot>(_shooter, 12)
     << make<ArmToSetPoint>(_arm, armAngle)
-    << make<DriveToLocation>(_driveBase, raw, distance)
+    << make<DriveToLocation>(_driveBase, 12, distance, direction)
     << make<ArmToSetPoint>(_arm, armAngle)
-    << make<AutoIntake>(_intake, intakeVolt)
+    << make<AutoIntake>(_intake, 12)
     //<< make<ArmToSetPoint>(_arm, armAngle)
-    //<< make<DriveToLocation>(_driveBase, raw, distance)
+    //<< make<DriveToLocation>(_driveBase, 12, distance, direction)
     << make<ArmToSetPoint>(_arm, armAngle)
-    << make<AutoShoot>(_shooter, shooterVolt)
+    << make<AutoShoot>(_shooter, 12)
     << make<ArmToSetPoint>(_arm, armAngle)
-    << make<DriveToLocation>(_driveBase, raw, distance)
+    << make<DriveToLocation>(_driveBase, 12, distance, direction)
     << make<ArmToSetPoint>(_arm, armAngle)
-    << make<AutoIntake>(_intake, intakeVolt)
+    << make<AutoIntake>(_intake, 12)
     //<< make<ArmToSetPoint>(_arm, armAngle)
-    //<< make<DriveToLocation>(_driveBase, raw, distance)
+    //<< make<DriveToLocation>(_driveBase, 12, distance, direction)
     << make<ArmToSetPoint>(_arm, armAngle)
-    << make<AutoShoot>(_shooter, shooterVolt)
+    << make<AutoShoot>(_shooter, 12)
     << make<ArmToSetPoint>(_arm, armAngle)
-    << make<DriveToLocation>(_driveBase, raw, distance)
+    << make<DriveToLocation>(_driveBase, 12, distance, direction)
     << make<ArmToSetPoint>(_arm, armAngle)
-    << make<AutoIntake>(_intake, intakeVolt)
+    << make<AutoIntake>(_intake, 12)
     << make<ArmToSetPoint>(_arm, armAngle)
-    << make<DriveToLocation>(_driveBase, raw, distance)
+    << make<DriveToLocation>(_driveBase, 12, distance, direction)
     << make<ArmToSetPoint>(_arm, armAngle)
-    << make<AutoShoot>(_shooter, shooterVolt)
+    << make<AutoShoot>(_shooter, 12)
     << make<ArmToSetPoint>(_arm, armAngle)
-    << make<DriveToLocation>(_driveBase, raw, distance)
+    << make<DriveToLocation>(_driveBase, 12, distance, direction)
     << make<ArmToSetPoint>(_arm, armAngle)
-    << make<AutoIntake>(_intake, intakeVolt)
+    << make<AutoIntake>(_intake, 12)
     << make<ArmToSetPoint>(_arm, armAngle)
-    << make<DriveToLocation>(_driveBase, raw, distance)
+    << make<DriveToLocation>(_driveBase, 12, distance, direction)
     << make<ArmToSetPoint>(_arm, armAngle)
-    << make<AutoShoot>(_shooter, shooterVolt)
+    << make<AutoShoot>(_shooter, 12)
     << make<ArmToSetPoint>(_arm, armAngle)
-    << make<DriveToLocation>(_driveBase, raw, distance)
+    << make<DriveToLocation>(_driveBase, 12, distance, direction)
     << make<ArmToSetPoint>(_arm, armAngle)
-    << make<AutoIntake>(_intake, intakeVolt)
+    << make<AutoIntake>(_intake, 12)
     << make<ArmToSetPoint>(_arm, armAngle)
-    << make<AutoShoot>(_shooter, shooterVolt)
-    /*
-      4N Close 2N Far
-    1. Shoot note
-    2. Drive to close note
-    3. Intake note
-    4. Shoot note
-    5. Drive to close note
-    6. Intake note
-    7. Shoot note
-    8. Drive to close note
-    9. Intake note
-    10. Shoot note
-    11. Drive to far note
-    12. Intake note
-    13. Drive to shooting line
-    14. Shoot note
-    15. Drive to far note
-    16. Intake note
-    17. Drive to shooting line
-    18. Shoot note
-    */
+    << make<AutoShoot>(_shooter, 12)
+    
+    //   4N Close 2N Far
+    // 1. Shoot note
+    // 2. Drive to close note
+    // 3. Intake note
+    // 4. Shoot note
+    // 5. Drive to close note
+    // 6. Intake note
+    // 7. Shoot note
+    // 8. Drive to close note
+    // 9. Intake note
+    // 10. Shoot note
+    // 11. Drive to far note
+    // 12. Intake note
+    // 13. Drive to shooting line
+    // 14. Shoot note
+    // 15. Drive to far note
+    // 16. Intake note
+    // 17. Drive to shooting line
+    // 18. Shoot note
+    
 }
 
 std::shared_ptr<Behaviour> QuadrupleCloseSingleFar(wom::drivetrain::SwerveDrive _driveBase, Shooter _shooter, Intake _intake, AlphaArm _alphaArm) {
-  return make<ArmToSetPoint>(_arm, armAngle)
-    << make<AutoShoot>(_shooter, shooterVolt)
-    << make<ArmToSetPoint>(_arm, armAngle)
-    << make<DriveToLocation>(_driveBase, raw, distance)
-    << make<ArmToSetPoint>(_arm, armAngle)
-    << make<AutoIntake>(_intake, intakeVolt)
-    << make<ArmToSetPoint>(_arm, armAngle)
-    << make<DriveToLocation>(_driveBase, raw, distance)
-    << make<ArmToSetPoint>(_arm, armAngle)
-    << make<AutoShoot>(_shooter, shooterVolt)
-    << make<ArmToSetPoint>(_arm, armAngle)
-    << make<DriveToLocation>(_driveBase, raw, distance)
-    << make<ArmToSetPoint>(_arm, armAngle)
-    << make<AutoIntake>(_intake, intakeVolt)
-    << make<ArmToSetPoint>(_arm, armAngle)
-    << make<DriveToLocation>(_driveBase, raw, distance)
-    << make<ArmToSetPoint>(_arm, armAngle)
-    << make<AutoShoot>(_shooter, shooterVolt)
-    << make<ArmToSetPoint>(_arm, armAngle)
-    << make<DriveToLocation>(_driveBase, raw, distance)
-    << make<ArmToSetPoint>(_arm, armAngle)
-    << make<AutoIntake>(_intake, intakeVolt)
-    << make<ArmToSetPoint>(_arm, armAngle)
-    << make<DriveToLocation>(_driveBase, raw, distance)
-    << make<ArmToSetPoint>(_arm, armAngle)
-    << make<AutoShoot>(_shooter, shooterVolt)
-    << make<ArmToSetPoint>(_arm, armAngle)
+  return make<ArmToSetPoint>(_arm, 60)
+    << make<AutoShoot>(_shooter, 12)
+    << make<ArmToSetPoint>(_arm, 0)
+    << make<DriveToLocation>(_driveBase, 12, distance, direction)
+    << make<ArmToSetPoint>(_arm, 180)
+    << make<AutoIntake>(_intake, 12)
+    << make<ArmToSetPoint>(_arm, 0)
+    << make<DriveToLocation>(_driveBase, 12, distance, direction)
+    << make<ArmToSetPoint>(_arm, 180)
+    << make<AutoShoot>(_shooter, 12)
+    << make<ArmToSetPoint>(_arm, 0)
+    << make<DriveToLocation>(_driveBase, 12, distance, direction)
+    << make<ArmToSetPoint>(_arm, 180)
+    << make<AutoIntake>(_intake, 12)
+    << make<ArmToSetPoint>(_arm, 0)
+    << make<DriveToLocation>(_driveBase, 12, distance, direction)
+    << make<ArmToSetPoint>(_arm, 180)
+    << make<AutoShoot>(_shooter, 12)
+    << make<ArmToSetPoint>(_arm, 0)
+    << make<DriveToLocation>(_driveBase, 12, distance, direction)
+    << make<ArmToSetPoint>(_arm, 180)
+    << make<AutoIntake>(_intake, 12) 
+    << make<ArmToSetPoint>(_arm, 0)
+    << make<DriveToLocation>(_driveBase, 12, distance, direction)
+    << make<ArmToSetPoint>(_arm, 180)
+    << make<AutoShoot>(_shooter, 12)
+    << make<ArmToSetPoint>(_arm, 0)
 
-    /*
-    4N Close 1N Far
-    1. Shoot note
-    2. Drive to close note
-    3. Intake note
-    4. Drive to speaker
-    5. Shoot note
-    6. Drive to close note
-    7. Intake note
-    8. Drive to speaker
-    9. Shoot note
-    10. Drive to close note
-    11. Intake note
-    12. Drive to speaker
-    13. Shoot note
-    14. Drive to far note
-    15. Intake note
-    15. Drive to speaker
-    16. shoot 
-    */
+    
+    // 4N Close 1N Far
+    // 1. Shoot note
+    // 2. Drive to close note
+    // 3. Intake note
+    // 4. Drive to speaker
+    // 5. Shoot note
+    // 6. Drive to close note
+    // 7. Intake note
+    // 8. Drive to speaker
+    // 9. Shoot note
+    // 10. Drive to close note
+    // 11. Intake note
+    // 12. Drive to speaker
+    // 13. Shoot note
+    // 14. Drive to far note
+    // 15. Intake note
+    // 15. Drive to speaker
+    // 16. shoot 
+    
 }
    
\ No newline at end of file
diff --git a/src/main/cpp/Shooter.cpp b/src/main/cpp/Shooter.cpp
index 6499e076..0f15d60d 100644
--- a/src/main/cpp/Shooter.cpp
+++ b/src/main/cpp/Shooter.cpp
@@ -6,7 +6,7 @@
 
 Shooter::Shooter(ShooterConfig config)
     : _config(config)
-// ,
+// ,  
 // _pid{frc::PIDController (1, 0, 0, 0.005_s)}
 {}  // config.path + "/pid", config.pidConfig
 void Shooter::OnUpdate(units::second_t dt) {

From c6157162b34be6e176d24b9dff3795465d4c87bb Mon Sep 17 00:00:00 2001
From: Sean Chan <sean.chan3@student.education.wa.edu.au>
Date: Sun, 4 Feb 2024 10:30:11 +0800
Subject: [PATCH 086/207] trying to merge different code

---
 src/main/cpp/Auto.cpp                |  2 +-
 src/main/cpp/IntakeBehaviour.cpp     | 20 +++++++++++---------
 src/main/include/AlphaArmBehaviour.h |  1 -
 src/main/include/IntakeBehaviour.h   |  5 ++---
 4 files changed, 14 insertions(+), 14 deletions(-)

diff --git a/src/main/cpp/Auto.cpp b/src/main/cpp/Auto.cpp
index f863afc9..97a1208a 100644
--- a/src/main/cpp/Auto.cpp
+++ b/src/main/cpp/Auto.cpp
@@ -254,6 +254,6 @@ std::shared_ptr<behaviour::Behaviour> autos::AutoTest(wom::drivetrain::SwerveDri
   return behaviour::make<ArmToSetPoint>(&_alphaArm, 0 * 1_deg, 0.2);
 //     <<make<DriveToLocation>(_driveBase, raw, distance)
 //     <<make<AutoShoot>(_shooter, 8_V)
-//     <<make<AutoIntake>(_intake, 8_V)
+  behaviour::make<AutoIntake>(&_intake, 8_V);
 } // This auto is a test for auto to see if all things work.
 
diff --git a/src/main/cpp/IntakeBehaviour.cpp b/src/main/cpp/IntakeBehaviour.cpp
index 90867045..31ea9fb5 100644
--- a/src/main/cpp/IntakeBehaviour.cpp
+++ b/src/main/cpp/IntakeBehaviour.cpp
@@ -44,12 +44,14 @@ void IntakeManualControl::OnTick(units::second_t dt) {
   // }
 }
 
-// IntakeAutoControl::IntakeAutoControl(Intake* intake) : _intake(intake) {}
-
-// void IntakeAutoControl::OnTick(units::second_t dt) {
-//   if (_intake->GetConfig().intakeSensor->Get() == 1) {
-//     _intake->setState(IntakeState::kPass);
-//   } else if (_intake->GetConfig().magSensor->Get() == 0) {
-//     _intake->setState(IntakeState::kIdle);
-//   }
-// }
+AutoIntake::AutoIntake(Intake* intake, units::volt_t intakeVolt) : _intake(intake) {
+  Controls(intake);
+}
+
+void AutoIntake::OnTick(units::second_t dt) {
+  // if (_intake->GetConfig().intakeSensor->Get() == 1) {
+  //   _intake->setState(IntakeState::kPass);
+  // } else if (_intake->GetConfig().magSensor->Get() == 0) {
+  //   _intake->setState(IntakeState::kIdle);
+  // }
+}
diff --git a/src/main/include/AlphaArmBehaviour.h b/src/main/include/AlphaArmBehaviour.h
index c48bdcae..6be1fd35 100644
--- a/src/main/include/AlphaArmBehaviour.h
+++ b/src/main/include/AlphaArmBehaviour.h
@@ -28,7 +28,6 @@ class ArmToSetPoint : public behaviour::Behaviour {
   explicit ArmToSetPoint(AlphaArm* alphaArm, units::degree_t armAngle, float armSpeed);
   void OnTick(units::second_t dt);
 
-
  private:
   AlphaArm* _alphaArm;
   // double _armCurrentDegree;
diff --git a/src/main/include/IntakeBehaviour.h b/src/main/include/IntakeBehaviour.h
index c84c6d32..77d86f2f 100644
--- a/src/main/include/IntakeBehaviour.h
+++ b/src/main/include/IntakeBehaviour.h
@@ -23,10 +23,9 @@ class IntakeManualControl : public behaviour::Behaviour {
   bool _rawControl;
 };
 
-class IntakeAutoControl : public behaviour::Behaviour {
+class AutoIntake : public behaviour::Behaviour {
  public:
-  explicit IntakeAutoControl(Intake* intake);
-
+  explicit AutoIntake(Intake* intake, units::volt_t intakeVolt);
   void OnTick(units::second_t dt) override;
 
  private:

From d4023db4c36f07de06ef56c7a9ad36fa0ea7f0d9 Mon Sep 17 00:00:00 2001
From: Sean Chan <sean.chan3@student.education.wa.edu.au>
Date: Sun, 4 Feb 2024 12:32:01 +0800
Subject: [PATCH 087/207] Finished all auto shells, have not added values

---
 src/main/cpp/AlphaArmBehaviour.cpp            |   4 +-
 src/main/cpp/Auto.cpp                         | 277 +++++++++---------
 src/main/cpp/ShooterBehaviour.cpp             |  11 +
 src/main/include/Auto.h                       |  13 +-
 src/main/include/ShooterBehaviour.h           |   9 +
 .../src/main/include/drivetrain/SwerveDrive.h |   2 +-
 6 files changed, 167 insertions(+), 149 deletions(-)

diff --git a/src/main/cpp/AlphaArmBehaviour.cpp b/src/main/cpp/AlphaArmBehaviour.cpp
index 29ac389b..13f646ba 100644
--- a/src/main/cpp/AlphaArmBehaviour.cpp
+++ b/src/main/cpp/AlphaArmBehaviour.cpp
@@ -6,9 +6,7 @@
 
 #include <frc/XboxController.h>
 
-
-AlphaArmManualControl::AlphaArmManualControl(AlphaArm* alphaArm, frc::XboxController* codriver)
-    : _alphaArm(alphaArm), _codriver(codriver) {
+AlphaArmManualControl::AlphaArmManualControl(AlphaArm* alphaArm, frc::XboxController* codriver) : _alphaArm(alphaArm), _codriver(codriver) {
   Controls(alphaArm);
 }
 
diff --git a/src/main/cpp/Auto.cpp b/src/main/cpp/Auto.cpp
index 25cc3158..2007a923 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, Shooter _shooter, Intake _intake, AlphaArm _alphaArm) {
-  return behaviour::make<ArmToSetPoint>(_arm, 60)
-    <<make<AutoShoot>(_shooter, 12);
-    <<make<ArmToSetPoint>(_arm, 0);
-    <<make<DriveToLocation>(_driveBase, 12, distance);
+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);
 }
-//Shoots starting note then moves out of starting position.
+// Shoots starting note then moves out of starting position.
 
-std::shared_ptr<Behaviour> QuadrupleClose(wom::drivetrain::SwerveDrive _driveBase, Shooter _shooter, Intake _intake, AlphaArm _alphaArm) {
-  return make<ArmToSetPoint>(_arm, 60) 
-    << make<AutoShoot>(_shooter, 12)
-    << make<DriveToLocation>(_driveBase, 12, distance, direction)
-    << make<ArmToSetPoint>(_arm, 180)
-    << make<AutoIntake>(_intake, 12)
-    << make<ArmToSetPoint>(_arm, 60)
-    << make<AutoShoot>(_shooter, 12)
-    << make<DriveToLocation>(_driveBase, 12, distance, direction)
-    << make<ArmToSetPoint>(_arm, armAngle)
-    << make<AutoIntake>(_intake, 12)
-    << make<ArmToSetPoint>(_arm, 60)
-    << make<AutoShoot>(_shooter, 12)
-    << make<DriveToLocation>(_driveBase, 12, distance, direction)
-    << make<ArmToSetPoint>(_arm, 180)
-    << make<AutoIntake>(_intake, 12)
-    << make<ArmToSetPoint>(_arm, 60)
-    << make<AutoShoot>(_shooter, 12)
+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);
 
      /*
       4N Close
@@ -39,37 +39,37 @@ std::shared_ptr<Behaviour> QuadrupleClose(wom::drivetrain::SwerveDrive _driveBas
     */
 }
 
-std::shared_ptr<Behaviour> QuadrupleFar(wom::drivetrain::SwerveDrive _driveBase, Shooter _shooter, Intake _intake, AlphaArm _alphaArm) {
-  return make<ArmToSetPoint>(_arm, /12)
-    << make<AutoShoot>(_shooter, 12)
-    << make<ArmToSetPoint>(_arm, 0)
-    << make<DriveToLocation>(_driveBase, 12, distance, direction)
-    << make<ArmToSetPoint>(_arm, armAngle)
-    << make<AutoIntake>(_intake, 12)
-    //<< make<ArmToSetPoint>(_arm, armAngle)
-    //<< make<DriveToLocation>(_driveBase, 12, distance, direction)
-    << make<ArmToSetPoint>(_arm, 60)
-    << make<AutoShoot>(_shooter, 12)
-    << make<ArmToSetPoint>(_arm, armAngle)
-    << make<DriveToLocation>(_driveBase, 12, distance, direction)
-    << make<ArmToSetPoint>(_arm, armAngle)
-    << make<AutoIntake>(_intake, 12)
-    //<< make<ArmToSetPoint>(_arm, armAngle)
-    //<< make<DriveToLocation>(_driveBase, 12, distance, direction)
-    << make<ArmToSetPoint>(_arm, 60)
-    << make<AutoShoot>(_shooter, 12)
-    << make<ArmToSetPoint>(_arm, armAngle)
-    << make<DriveToLocation>(_driveBase, 12, distance, direction)
-    << make<ArmToSetPoint>(_arm, armAngle)
-    << make<AutoIntake>(_intake, 12)
-    //<< make<ArmToSetPoint>(_arm, armAngle)
-    //<< make<DriveToLocation>(_driveBase, 12, distance, direction)
-    << make<ArmToSetPoint>(_arm, 60)
-    << make<AutoShoot>(_shooter, 12)
-    // << make<ArmToSetPoint>(_arm, armAngle)
-    // << make<DriveToLocation>(_driveBase, 12, distance, direction)   do this if possible
-    // << make<ArmToSetPoint>(_arm, armAngle)
-    // << make<AutoIntake>(_intake, 12)
+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);
 
     /*
       4N Far
@@ -90,55 +90,55 @@ std::shared_ptr<Behaviour> QuadrupleFar(wom::drivetrain::SwerveDrive _driveBase,
     */
 }
 
-std::shared_ptr<Behaviour> QuadrupleCloseDoubleFar(wom::drivetrain::SwerveDrive _driveBase, Shooter _shooter, Intake _intake, AlphaArm _alphaArm) {
-  return make<ArmToSetPoint>(_arm, armAngle)
-    << make<AutoShoot>(_shooter, 12)
-    << make<ArmToSetPoint>(_arm, armAngle)
-    << make<DriveToLocation>(_driveBase, 12, distance, direction)
-    << make<ArmToSetPoint>(_arm, armAngle)
-    << make<Intake>(_intake, 12)
-    //<< make<ArmToSetPoint>(_arm, armAngle)
-    //<< make<DriveToLocation>(_driveBase, 12, distance, direction)
-    << make<ArmToSetPoint>(_arm, armAngle)
-    << make<AutoShoot>(_shooter, 12)
-    << make<ArmToSetPoint>(_arm, armAngle)
-    << make<DriveToLocation>(_driveBase, 12, distance, direction)
-    << make<ArmToSetPoint>(_arm, armAngle)
-    << make<AutoIntake>(_intake, 12)
-    //<< make<ArmToSetPoint>(_arm, armAngle)
-    //<< make<DriveToLocation>(_driveBase, 12, distance, direction)
-    << make<ArmToSetPoint>(_arm, armAngle)
-    << make<AutoShoot>(_shooter, 12)
-    << make<ArmToSetPoint>(_arm, armAngle)
-    << make<DriveToLocation>(_driveBase, 12, distance, direction)
-    << make<ArmToSetPoint>(_arm, armAngle)
-    << make<AutoIntake>(_intake, 12)
-    //<< make<ArmToSetPoint>(_arm, armAngle)
-    //<< make<DriveToLocation>(_driveBase, 12, distance, direction)
-    << make<ArmToSetPoint>(_arm, armAngle)
-    << make<AutoShoot>(_shooter, 12)
-    << make<ArmToSetPoint>(_arm, armAngle)
-    << make<DriveToLocation>(_driveBase, 12, distance, direction)
-    << make<ArmToSetPoint>(_arm, armAngle)
-    << make<AutoIntake>(_intake, 12)
-    << make<ArmToSetPoint>(_arm, armAngle)
-    << make<DriveToLocation>(_driveBase, 12, distance, direction)
-    << make<ArmToSetPoint>(_arm, armAngle)
-    << make<AutoShoot>(_shooter, 12)
-    << make<ArmToSetPoint>(_arm, armAngle)
-    << make<DriveToLocation>(_driveBase, 12, distance, direction)
-    << make<ArmToSetPoint>(_arm, armAngle)
-    << make<AutoIntake>(_intake, 12)
-    << make<ArmToSetPoint>(_arm, armAngle)
-    << make<DriveToLocation>(_driveBase, 12, distance, direction)
-    << make<ArmToSetPoint>(_arm, armAngle)
-    << make<AutoShoot>(_shooter, 12)
-    << make<ArmToSetPoint>(_arm, armAngle)
-    << make<DriveToLocation>(_driveBase, 12, distance, direction)
-    << make<ArmToSetPoint>(_arm, armAngle)
-    << make<AutoIntake>(_intake, 12)
-    << make<ArmToSetPoint>(_arm, armAngle)
-    << make<AutoShoot>(_shooter, 12)
+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);
     
     //   4N Close 2N Far
     // 1. Shoot note
@@ -162,35 +162,35 @@ std::shared_ptr<Behaviour> QuadrupleCloseDoubleFar(wom::drivetrain::SwerveDrive
     
 }
 
-std::shared_ptr<Behaviour> QuadrupleCloseSingleFar(wom::drivetrain::SwerveDrive _driveBase, Shooter _shooter, Intake _intake, AlphaArm _alphaArm) {
-  return make<ArmToSetPoint>(_arm, 60)
-    << make<AutoShoot>(_shooter, 12)
-    << make<ArmToSetPoint>(_arm, 0)
-    << make<DriveToLocation>(_driveBase, 12, distance, direction)
-    << make<ArmToSetPoint>(_arm, 180)
-    << make<AutoIntake>(_intake, 12)
-    << make<ArmToSetPoint>(_arm, 0)
-    << make<DriveToLocation>(_driveBase, 12, distance, direction)
-    << make<ArmToSetPoint>(_arm, 180)
-    << make<AutoShoot>(_shooter, 12)
-    << make<ArmToSetPoint>(_arm, 0)
-    << make<DriveToLocation>(_driveBase, 12, distance, direction)
-    << make<ArmToSetPoint>(_arm, 180)
-    << make<AutoIntake>(_intake, 12)
-    << make<ArmToSetPoint>(_arm, 0)
-    << make<DriveToLocation>(_driveBase, 12, distance, direction)
-    << make<ArmToSetPoint>(_arm, 180)
-    << make<AutoShoot>(_shooter, 12)
-    << make<ArmToSetPoint>(_arm, 0)
-    << make<DriveToLocation>(_driveBase, 12, distance, direction)
-    << make<ArmToSetPoint>(_arm, 180)
-    << make<AutoIntake>(_intake, 12) 
-    << make<ArmToSetPoint>(_arm, 0)
-    << make<DriveToLocation>(_driveBase, 12, distance, direction)
-    << make<ArmToSetPoint>(_arm, 180)
-    << make<AutoShoot>(_shooter, 12)
-    << make<ArmToSetPoint>(_arm, 0)
-
+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);
+}
     
     // 4N Close 1N Far
     // 1. Shoot note
@@ -232,10 +232,9 @@ std::shared_ptr<Behaviour> QuadrupleCloseSingleFar(wom::drivetrain::SwerveDrive
 //     */
 // }
 
-std::shared_ptr<behaviour::Behaviour> autos::AutoTest(wom::drivetrain::SwerveDrive _driveBase, Shooter _shooter, Intake _intake, AlphaArm _alphaArm) {
+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);
-//     <<make<DriveToLocation>(_driveBase, raw, distance)
-//     <<make<AutoShoot>(_shooter, 8_V)
-  <<behaviour::make<AutoIntake>(&_intake, 8_V);
-} // This auto is a test for auto to see if all things work.
-
+    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/ShooterBehaviour.cpp b/src/main/cpp/ShooterBehaviour.cpp
index 4b31c06b..f8bb3279 100644
--- a/src/main/cpp/ShooterBehaviour.cpp
+++ b/src/main/cpp/ShooterBehaviour.cpp
@@ -37,3 +37,14 @@ void ShooterManualControl::OnTick(units::second_t dt) {
   }
 }
 
+AutoShoot::AutoShoot(Shooter* shooter, units::volt_t shooterVolt) : _shooter(shooter) {
+  Controls(shooter);
+}
+
+void AutoShoot::OnTick(units::second_t dt) {
+  // if (_intake->GetConfig().intakeSensor->Get() == 1) {
+  //   _intake->setState(IntakeState::kPass);
+  // } else if (_intake->GetConfig().magSensor->Get() == 0) {
+  //   _intake->setState(IntakeState::kIdle);
+  // }
+}
diff --git a/src/main/include/Auto.h b/src/main/include/Auto.h
index 0bddeda4..de439b35 100644
--- a/src/main/include/Auto.h
+++ b/src/main/include/Auto.h
@@ -6,16 +6,17 @@
 #include "IntakeBehaviour.h"
 #include "ShooterBehaviour.h"
 
+
 namespace autos {
-  std::shared_ptr<behaviour::Behaviour> AutoTest(wom::drivetrain::SwerveDrive _swerveDrive, 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(Drivebase driveBase, Shooter shooter, Mag mag, Intake intake, Arm _arm);
+  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(Drivebase driveBase, Shooter shooter, Mag mag, Intake intake, Arm _arm);
+  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(Drivebase driveBase, Shooter shooter, Mag mag, Intake intake, Arm _arm);
+  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(Drivebase driveBase, Shooter shooter, Mag mag, Intake intake, Arm _arm);
+  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(Drivebase driveBase, Shooter shooter, Mag mag, Intake intake, Arm _arm);
+  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/ShooterBehaviour.h b/src/main/include/ShooterBehaviour.h
index ff65b1d8..2b6bf09e 100644
--- a/src/main/include/ShooterBehaviour.h
+++ b/src/main/include/ShooterBehaviour.h
@@ -22,3 +22,12 @@ class ShooterManualControl : public behaviour::Behaviour {
 
   bool _rawControl = true;
 };
+
+class AutoShoot : public behaviour::Behaviour {
+ public:
+  explicit AutoShoot(Shooter* shooter, units::volt_t shooterVolt);
+  void OnTick(units::second_t dt) override;
+
+ private:
+  Shooter* _shooter;
+};
diff --git a/wombat/src/main/include/drivetrain/SwerveDrive.h b/wombat/src/main/include/drivetrain/SwerveDrive.h
index be4467b1..f03f7152 100644
--- a/wombat/src/main/include/drivetrain/SwerveDrive.h
+++ b/wombat/src/main/include/drivetrain/SwerveDrive.h
@@ -2,7 +2,7 @@
 // Open Source Software, you can modify it according to the terms
 // of the MIT License at the root of this project
 
-#pragma once
+#pragma once 
 
 #include <frc/controller/PIDController.h>
 #include <frc/estimator/SwerveDrivePoseEstimator.h>

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 088/207] .

---
 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;

From 73e8eb6dfcec52ad251654a2229549544311d8b4 Mon Sep 17 00:00:00 2001
From: Sean Chan <sean.chan3@student.education.wa.edu.au>
Date: Sun, 4 Feb 2024 16:16:35 +0800
Subject: [PATCH 089/207] started pid (oliver kingsley add a remote from this)

---
 src/main/cpp/Intake.cpp   | 11 ++++++++++-
 src/main/include/Intake.h |  5 +++++
 2 files changed, 15 insertions(+), 1 deletion(-)

diff --git a/src/main/cpp/Intake.cpp b/src/main/cpp/Intake.cpp
index b56d4b47..4358c7e8 100644
--- a/src/main/cpp/Intake.cpp
+++ b/src/main/cpp/Intake.cpp
@@ -4,12 +4,16 @@
 
 #include "Intake.h"
 
-Intake::Intake(IntakeConfig config) : _config(config) {}
+Intake::Intake(IntakeConfig config) : _config(config), _pid(config.path + "/pid", config.pidConfig) {}
 
 IntakeConfig Intake::GetConfig() {
   return _config;
 }
 
+void Shooter::OnStart() {
+  _pid.Reset();
+}
+
 void Intake::OnUpdate(units::second_t dt) {
   switch (_state) {
     case IntakeState::kIdle: {
@@ -67,6 +71,8 @@ void Intake::OnUpdate(units::second_t dt) {
   _table->GetEntry("State: ").SetString(_stringStateName);
   _table->GetEntry("Motor Voltage: ").SetDouble(_setVoltage.value());
   _table->GetEntry("Intake Sensor: ").SetBoolean(_config.intakeSensor->Get());
+  _table->GetEntry("Error").SetDouble(_pid.GetError().value());
+  _table->GetEntry("SetPoint").SetDouble(_pid.GetSetpoint().value());
   // _table->GetEntry("Shooter Sensor: ").SetBoolean(_config.shooterSensor->Get());
 
   std::cout << _setVoltage.value() << std::endl;
@@ -83,3 +89,6 @@ void Intake::setRaw(units::volt_t voltage) {
 IntakeState Intake::getState() {
   return _state;
 }
+void Intake::SetPidGoal(units::radians_per_second_t goal) {
+  _goal = goal;
+}
diff --git a/src/main/include/Intake.h b/src/main/include/Intake.h
index 7f945702..96cb4c91 100644
--- a/src/main/include/Intake.h
+++ b/src/main/include/Intake.h
@@ -5,6 +5,8 @@
 #pragma once
 
 #include <frc/DigitalInput.h>
+#include <frc/controller/PIDController.h>
+#include <units/angular_velocity.h>
 
 #include <memory>
 #include <string>
@@ -16,6 +18,7 @@ struct IntakeConfig {
   frc::DigitalInput* intakeSensor;
   frc::DigitalInput* magSensor;
   frc::DigitalInput* shooterSensor;
+  wom::PIDConfig<units::radians_per_second, units::volt> pidConfig;
 };
 
 enum class IntakeState { kIdle, kRaw, kHold, kEject, kIntake, kPass };
@@ -28,6 +31,7 @@ class Intake : public behaviour::HasBehaviour {
 
   void setState(IntakeState state);
   void setRaw(units::volt_t voltage);
+  void SetPidGoal(units::radians_per_second_t);
   IntakeState getState();
   IntakeConfig GetConfig();
 
@@ -41,6 +45,7 @@ class Intake : public behaviour::HasBehaviour {
   bool _intaking;
   bool _ejecting;
   bool _passing;
+  wom::PIDController<units::radians_per_second, units::volt> _pid;
   
   std::shared_ptr<nt::NetworkTable> _table = nt::NetworkTableInstance::GetDefault().GetTable("Intake");
 };

From 880a70ea3b852f34073c77ab92f3eb736338a931 Mon Sep 17 00:00:00 2001
From: Sean Chan <sean.chan3@student.education.wa.edu.au>
Date: Sun, 4 Feb 2024 16:33:13 +0800
Subject: [PATCH 090/207] added a print out for the auto selector

---
 src/main/cpp/Robot.cpp | 6 ++++--
 1 file changed, 4 insertions(+), 2 deletions(-)

diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp
index 9ea002ad..47d50b61 100644
--- a/src/main/cpp/Robot.cpp
+++ b/src/main/cpp/Robot.cpp
@@ -119,7 +119,7 @@ void Robot::AutonomousInit() {
   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") {
@@ -134,7 +134,9 @@ void Robot::AutonomousInit() {
     sched->Schedule(autos::QuadrupleCloseSingleFar(_swerveDrive, &timer, &field, shooter, intake, alphaArm));
   }
 }
-void Robot::AutonomousPeriodic() {}
+void Robot::AutonomousPeriodic() {
+  fmt::print("Auto selected: {}\n", m_autoSelected);
+}
 
 void Robot::TeleopInit() {
   loop.Clear();

From 1e6668d3ca94f90f5489d02afc79aca4484c46d3 Mon Sep 17 00:00:00 2001
From: Sean Chan <sean.chan3@student.education.wa.edu.au>
Date: Sun, 4 Feb 2024 21:50:50 +0800
Subject: [PATCH 091/207] fixed alphaarm build errors in auto

---
 src/main/include/RobotMap.h | 11 -----------
 1 file changed, 11 deletions(-)

diff --git a/src/main/include/RobotMap.h b/src/main/include/RobotMap.h
index 492f6c5e..88c936e1 100644
--- a/src/main/include/RobotMap.h
+++ b/src/main/include/RobotMap.h
@@ -181,15 +181,4 @@ struct RobotMap {
         nt::NetworkTableInstance::GetDefault().GetTable("swerve");
   };
   SwerveTable swerveTable;
-
-  struct AlphaArmSystem {
-    rev::CANSparkMax alphaArmMotor{12, rev::CANSparkMax::MotorType::kBrushless};
-    rev::CANSparkMax wristMotor{15, rev::CANSparkMax::MotorType::kBrushless};
-
-    wom::Gearbox alphaArmGearbox{&alphaArmMotor, nullptr, frc::DCMotor::NEO(1)};
-    wom::Gearbox wristGearbox{&wristMotor, nullptr, frc::DCMotor::NEO(1)};
-
-    AlphaArmConfig config{alphaArmGearbox, wristGearbox};
-  };
-  AlphaArmSystem alphaArmSystem;
 };

From 87616e9932838f842b7c9f8c2b46db1a4fa1037e Mon Sep 17 00:00:00 2001
From: Kingsley Wong <kingsley.wong@student.education.wa.edu.au>
Date: Wed, 7 Feb 2024 15:53:03 +0800
Subject: [PATCH 092/207] trying to merge recent intake

---
 src/main/cpp/IntakeBehaviour.cpp | 1 -
 1 file changed, 1 deletion(-)

diff --git a/src/main/cpp/IntakeBehaviour.cpp b/src/main/cpp/IntakeBehaviour.cpp
index a32c2791..703a3d5d 100644
--- a/src/main/cpp/IntakeBehaviour.cpp
+++ b/src/main/cpp/IntakeBehaviour.cpp
@@ -44,7 +44,6 @@ void IntakeManualControl::OnTick(units::second_t dt) {
   //   }
   // }
 
-
 // IntakeAutoControl::IntakeAutoControl(Intake* intake) : _intake(intake) {}
 
 void IntakeAutoControl::OnTick(units::second_t dt) {

From 876c05dac48e808988ee18175df92514268c7813 Mon Sep 17 00:00:00 2001
From: Sean Chan <sean.chan3@student.education.wa.edu.au>
Date: Thu, 8 Feb 2024 18:01:24 +0800
Subject: [PATCH 093/207] Fixed build errors

---
 src/main/include/Auto.h     | 2 ++
 src/main/include/Robot.h    | 1 +
 src/main/include/RobotMap.h | 4 ++--
 3 files changed, 5 insertions(+), 2 deletions(-)

diff --git a/src/main/include/Auto.h b/src/main/include/Auto.h
index ad1c4e51..f1de6245 100644
--- a/src/main/include/Auto.h
+++ b/src/main/include/Auto.h
@@ -2,6 +2,8 @@
 
 #include "Wombat.h"
 
+#include <memory>
+
 #include "AlphaArmBehaviour.h"
 #include "IntakeBehaviour.h"
 #include "ShooterBehaviour.h"
diff --git a/src/main/include/Robot.h b/src/main/include/Robot.h
index 52a7b12f..68d1eab2 100644
--- a/src/main/include/Robot.h
+++ b/src/main/include/Robot.h
@@ -17,6 +17,7 @@
 #include <networktables/NetworkTableInstance.h>
 
 #include <string>
+#include <vector>
 
 #include "AlphaArm.h"
 #include "AlphaArmBehaviour.h"
diff --git a/src/main/include/RobotMap.h b/src/main/include/RobotMap.h
index 88c936e1..8befddd6 100644
--- a/src/main/include/RobotMap.h
+++ b/src/main/include/RobotMap.h
@@ -37,8 +37,8 @@ struct RobotMap {
     rev::CANSparkMax alphaArmMotor{12, rev::CANSparkMax::MotorType::kBrushless};
     rev::CANSparkMax wristMotor{15, rev::CANSparkMax::MotorType::kBrushless};
     wom::CANSparkMaxEncoder* alphaArmEncoder = new wom::CANSparkMaxEncoder(&alphaArmMotor, 0.1_m);
-    wom::Gearbox alphaArmGearbox{&alphaArmMotor, alphaArmEncoder, frc::DCMotor::NEO(1)};
-    wom::Gearbox wristGearbox{&wristMotor, nullptr, frc::DCMotor::NEO(1)};
+    wom::Gearbox alphaArmGearbox{&alphaArmMotor, nullptr, frc::DCMotor::NEO(1)};
+    wom::Gearbox wristGearbox{&wristMotor, alphaArmEncoder, frc::DCMotor::NEO(1)};
 
     AlphaArmConfig config{alphaArmGearbox, wristGearbox};
   };

From 7854e7f7eb8cd2708bc9813fd564b95e17a6ce58 Mon Sep 17 00:00:00 2001
From: "github-actions[bot]"
 <41898282+github-actions[bot]@users.noreply.github.com>
Date: Thu, 8 Feb 2024 10:04:27 +0000
Subject: [PATCH 094/207] Formatting fixes

---
 src/main/cpp/AlphaArmBehaviour.cpp            |   5 +-
 src/main/cpp/Auto.cpp                         | 443 ++++++++++--------
 src/main/cpp/IntakeBehaviour.cpp              |   2 +-
 src/main/cpp/Robot.cpp                        |  20 +-
 src/main/include/Auto.h                       |  30 +-
 src/main/include/Robot.h                      |   7 +-
 src/main/include/RobotMap.h                   |   2 +
 src/main/include/Shooter.h                    |   2 +-
 .../src/main/include/drivetrain/SwerveDrive.h |   2 +-
 9 files changed, 280 insertions(+), 233 deletions(-)

diff --git a/src/main/cpp/AlphaArmBehaviour.cpp b/src/main/cpp/AlphaArmBehaviour.cpp
index f1c8f393..5ae1cb2e 100644
--- a/src/main/cpp/AlphaArmBehaviour.cpp
+++ b/src/main/cpp/AlphaArmBehaviour.cpp
@@ -6,7 +6,8 @@
 
 #include <frc/XboxController.h>
 
-AlphaArmManualControl::AlphaArmManualControl(AlphaArm* alphaArm, frc::XboxController* codriver) : _alphaArm(alphaArm), _codriver(codriver) {
+AlphaArmManualControl::AlphaArmManualControl(AlphaArm* alphaArm, frc::XboxController* codriver)
+    : _alphaArm(alphaArm), _codriver(codriver) {
   Controls(alphaArm);
 }
 
@@ -41,4 +42,4 @@ ArmToSetPoint::ArmToSetPoint(AlphaArm* alphaArm, units::degree_t armAngle) : _al
 void ArmToSetPoint::OnTick(units::second_t dt) {
   // _armCurrentDegree = _alphaArm->GetConfig().alphaArmGearbox.encoder.GetEncoderPosition();
   // _alphaArm->GetConfig().alphaArmGearbox.encoder.SetEncoderPosition(armAngle * armSpeed);
-}
\ No newline at end of file
+}
diff --git a/src/main/cpp/Auto.cpp b/src/main/cpp/Auto.cpp
index c0c349a4..4d22e98b 100644
--- a/src/main/cpp/Auto.cpp
+++ b/src/main/cpp/Auto.cpp
@@ -1,217 +1,255 @@
+// Copyright (c) 2023-2024 CurtinFRC
+// Open Source Software, you can modify it according to the terms
+// of the MIT License at the root of this project
+
 #include "Auto.h"
 
-std::shared_ptr<behaviour::Behaviour> autos::Taxi(wom::drivetrain::SwerveDrive* _swerveDrive, Shooter* _shooter, Intake* _intake, AlphaArm* _alphaArm) {
+std::shared_ptr<behaviour::Behaviour> autos::Taxi(wom::drivetrain::SwerveDrive* _swerveDrive,
+                                                  Shooter* _shooter, Intake* _intake, AlphaArm* _alphaArm) {
   return behaviour::make<ArmToSetPoint>(_alphaArm, 0_deg);
-    behaviour::make<AutoShoot>(_shooter); 
-    behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
-    behaviour::make<wom::drivetrain::behaviours::DrivebasePoseBehaviour>(_swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false);
+  behaviour::make<AutoShoot>(_shooter);
+  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+  behaviour::make<wom::drivetrain::behaviours::DrivebasePoseBehaviour>(
+      _swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false);
 }
 // Shoots starting note then moves out of starting position.
 
-std::shared_ptr<behaviour::Behaviour> autos::QuadrupleClose(wom::drivetrain::SwerveDrive* _swerveDrive, Shooter* _shooter, Intake* _intake, AlphaArm* _alphaArm) {
+std::shared_ptr<behaviour::Behaviour> autos::QuadrupleClose(wom::drivetrain::SwerveDrive* _swerveDrive,
+                                                            Shooter* _shooter, Intake* _intake,
+                                                            AlphaArm* _alphaArm) {
   return behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
-    behaviour::make<AutoShoot>(_shooter);
-    behaviour::make<wom::drivetrain::behaviours::DrivebasePoseBehaviour>(_swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false);
-    behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
-    behaviour::make<AutoIntake>(_intake);
-    behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
-    behaviour::make<AutoShoot>(_shooter);
-    behaviour::make<wom::drivetrain::behaviours::DrivebasePoseBehaviour>(_swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false);
-    behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
-    behaviour::make<AutoIntake>(_intake);
-    behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
-    behaviour::make<AutoShoot>(_shooter);
-    behaviour::make<wom::drivetrain::behaviours::DrivebasePoseBehaviour>(_swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false);
-    behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
-    behaviour::make<AutoIntake>(_intake);
-    behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
-    behaviour::make<AutoShoot>(_shooter);
+  behaviour::make<AutoShoot>(_shooter);
+  behaviour::make<wom::drivetrain::behaviours::DrivebasePoseBehaviour>(
+      _swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false);
+  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+  behaviour::make<AutoIntake>(_intake);
+  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+  behaviour::make<AutoShoot>(_shooter);
+  behaviour::make<wom::drivetrain::behaviours::DrivebasePoseBehaviour>(
+      _swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false);
+  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+  behaviour::make<AutoIntake>(_intake);
+  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+  behaviour::make<AutoShoot>(_shooter);
+  behaviour::make<wom::drivetrain::behaviours::DrivebasePoseBehaviour>(
+      _swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false);
+  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+  behaviour::make<AutoIntake>(_intake);
+  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+  behaviour::make<AutoShoot>(_shooter);
 
-     /*
-      4N Close
-    1. Shoot starting note into speaker
-    2. Intake note from close note
-    3. Shoot note into speaker
-    4. Intake note from close floor note
-    5. Shoot note into speaker
-    6. Intake not from close floor
-    7. Shoot note
-    */
+  /*
+   4N Close
+ 1. Shoot starting note into speaker
+ 2. Intake note from close note
+ 3. Shoot note into speaker
+ 4. Intake note from close floor note
+ 5. Shoot note into speaker
+ 6. Intake not from close floor
+ 7. Shoot note
+ */
 }
 
-std::shared_ptr<behaviour::Behaviour> autos::QuadrupleFar(wom::drivetrain::SwerveDrive* _swerveDrive, Shooter* _shooter, Intake* _intake, AlphaArm* _alphaArm) {
+std::shared_ptr<behaviour::Behaviour> autos::QuadrupleFar(wom::drivetrain::SwerveDrive* _swerveDrive,
+                                                          Shooter* _shooter, Intake* _intake,
+                                                          AlphaArm* _alphaArm) {
   return behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
-    behaviour::make<AutoShoot>(_shooter);
-    behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
-    behaviour::make<wom::drivetrain::behaviours::DrivebasePoseBehaviour>(_swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false);
-    behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
-    behaviour::make<AutoIntake>(_intake);
-    behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
-    behaviour::make<wom::drivetrain::behaviours::DrivebasePoseBehaviour>(_swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false);
-    behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
-    behaviour::make<AutoShoot>(_shooter);
-    behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
-    behaviour::make<wom::drivetrain::behaviours::DrivebasePoseBehaviour>(_swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false);
-    behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
-    behaviour::make<AutoIntake>(_intake);
-    behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
-    behaviour::make<wom::drivetrain::behaviours::DrivebasePoseBehaviour>(_swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false);
-    behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
-    behaviour::make<AutoShoot>(_shooter);
-    behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
-    behaviour::make<wom::drivetrain::behaviours::DrivebasePoseBehaviour>(_swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false);
-    behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
-    behaviour::make<AutoIntake>(_intake);
-    behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
-    behaviour::make<wom::drivetrain::behaviours::DrivebasePoseBehaviour>(_swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false);
-    behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
-    behaviour::make<AutoShoot>(_shooter);
-    behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
-    behaviour::make<wom::drivetrain::behaviours::DrivebasePoseBehaviour>(_swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false);
-    behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
-    behaviour::make<AutoIntake>(_intake);
+  behaviour::make<AutoShoot>(_shooter);
+  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+  behaviour::make<wom::drivetrain::behaviours::DrivebasePoseBehaviour>(
+      _swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false);
+  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+  behaviour::make<AutoIntake>(_intake);
+  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+  behaviour::make<wom::drivetrain::behaviours::DrivebasePoseBehaviour>(
+      _swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false);
+  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+  behaviour::make<AutoShoot>(_shooter);
+  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+  behaviour::make<wom::drivetrain::behaviours::DrivebasePoseBehaviour>(
+      _swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false);
+  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+  behaviour::make<AutoIntake>(_intake);
+  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+  behaviour::make<wom::drivetrain::behaviours::DrivebasePoseBehaviour>(
+      _swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false);
+  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+  behaviour::make<AutoShoot>(_shooter);
+  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+  behaviour::make<wom::drivetrain::behaviours::DrivebasePoseBehaviour>(
+      _swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false);
+  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+  behaviour::make<AutoIntake>(_intake);
+  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+  behaviour::make<wom::drivetrain::behaviours::DrivebasePoseBehaviour>(
+      _swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false);
+  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+  behaviour::make<AutoShoot>(_shooter);
+  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+  behaviour::make<wom::drivetrain::behaviours::DrivebasePoseBehaviour>(
+      _swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false);
+  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+  behaviour::make<AutoIntake>(_intake);
 
-    /*
-      4N Far
-    1. Shoot start note in speaker
-    2. Drive to far note
-    3. Intake note
-    4. Drive back to shooting line
-    5. Shoot note into speaker
-    6. Drive to note
-    7. Intake note
-    8. Drive to shooting line
-    9. Shoot note
-    10. Drive to note
-    11. Intake note
-    12. Drive to shooting line
-    13. Shoot note
-    14. Drive to intake note (if possible)
-    */
+  /*
+    4N Far
+  1. Shoot start note in speaker
+  2. Drive to far note
+  3. Intake note
+  4. Drive back to shooting line
+  5. Shoot note into speaker
+  6. Drive to note
+  7. Intake note
+  8. Drive to shooting line
+  9. Shoot note
+  10. Drive to note
+  11. Intake note
+  12. Drive to shooting line
+  13. Shoot note
+  14. Drive to intake note (if possible)
+  */
 }
 
-std::shared_ptr<behaviour::Behaviour> autos::QuadrupleCloseDoubleFar(wom::drivetrain::SwerveDrive* _swerveDrive, Shooter* _shooter, Intake* _intake, AlphaArm* _alphaArm) {
+std::shared_ptr<behaviour::Behaviour> autos::QuadrupleCloseDoubleFar(
+    wom::drivetrain::SwerveDrive* _swerveDrive, Shooter* _shooter, Intake* _intake, AlphaArm* _alphaArm) {
   return behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
-    behaviour::make<AutoShoot>(_shooter);
-    behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
-    behaviour::make<wom::drivetrain::behaviours::DrivebasePoseBehaviour>(_swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false);
-    behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
-    behaviour::make<AutoIntake>(_intake);
-    behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
-    behaviour::make<wom::drivetrain::behaviours::DrivebasePoseBehaviour>(_swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false);
-    behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
-    behaviour::make<AutoShoot>(_shooter);
-    behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
-    behaviour::make<wom::drivetrain::behaviours::DrivebasePoseBehaviour>(_swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false);
-    behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
-    behaviour::make<AutoIntake>(_intake);
-    behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
-    behaviour::make<wom::drivetrain::behaviours::DrivebasePoseBehaviour>(_swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false);
-    behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
-    behaviour::make<AutoShoot>(_shooter);
-    behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
-    behaviour::make<wom::drivetrain::behaviours::DrivebasePoseBehaviour>(_swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false);
-    behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
-    behaviour::make<AutoIntake>(_intake);
-    behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
-    behaviour::make<wom::drivetrain::behaviours::DrivebasePoseBehaviour>(_swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false);
-    behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
-    behaviour::make<AutoShoot>(_shooter);
-    behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
-    behaviour::make<wom::drivetrain::behaviours::DrivebasePoseBehaviour>(_swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false);
-    behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
-    behaviour::make<AutoIntake>(_intake);
-    behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
-    behaviour::make<wom::drivetrain::behaviours::DrivebasePoseBehaviour>(_swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false);
-    behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
-    behaviour::make<AutoShoot>(_shooter);
-    behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
-    behaviour::make<wom::drivetrain::behaviours::DrivebasePoseBehaviour>(_swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false);
-    behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
-    behaviour::make<AutoIntake>(_intake);
-    behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
-    behaviour::make<wom::drivetrain::behaviours::DrivebasePoseBehaviour>(_swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false);
-    behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
-    behaviour::make<AutoShoot>(_shooter);
-    behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
-    behaviour::make<wom::drivetrain::behaviours::DrivebasePoseBehaviour>(_swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false);
-    behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
-    behaviour::make<AutoIntake>(_intake);
-    behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
-    behaviour::make<AutoShoot>(_shooter);
-    
-    //   4N Close 2N Far
-    // 1. Shoot note
-    // 2. Drive to close note
-    // 3. Intake note
-    // 4. Shoot note
-    // 5. Drive to close note
-    // 6. Intake note
-    // 7. Shoot note
-    // 8. Drive to close note
-    // 9. Intake note
-    // 10. Shoot note
-    // 11. Drive to far note
-    // 12. Intake note
-    // 13. Drive to shooting line
-    // 14. Shoot note
-    // 15. Drive to far note
-    // 16. Intake note
-    // 17. Drive to shooting line
-    // 18. Shoot note
-    
+  behaviour::make<AutoShoot>(_shooter);
+  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+  behaviour::make<wom::drivetrain::behaviours::DrivebasePoseBehaviour>(
+      _swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false);
+  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+  behaviour::make<AutoIntake>(_intake);
+  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+  behaviour::make<wom::drivetrain::behaviours::DrivebasePoseBehaviour>(
+      _swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false);
+  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+  behaviour::make<AutoShoot>(_shooter);
+  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+  behaviour::make<wom::drivetrain::behaviours::DrivebasePoseBehaviour>(
+      _swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false);
+  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+  behaviour::make<AutoIntake>(_intake);
+  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+  behaviour::make<wom::drivetrain::behaviours::DrivebasePoseBehaviour>(
+      _swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false);
+  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+  behaviour::make<AutoShoot>(_shooter);
+  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+  behaviour::make<wom::drivetrain::behaviours::DrivebasePoseBehaviour>(
+      _swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false);
+  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+  behaviour::make<AutoIntake>(_intake);
+  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+  behaviour::make<wom::drivetrain::behaviours::DrivebasePoseBehaviour>(
+      _swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false);
+  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+  behaviour::make<AutoShoot>(_shooter);
+  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+  behaviour::make<wom::drivetrain::behaviours::DrivebasePoseBehaviour>(
+      _swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false);
+  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+  behaviour::make<AutoIntake>(_intake);
+  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+  behaviour::make<wom::drivetrain::behaviours::DrivebasePoseBehaviour>(
+      _swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false);
+  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+  behaviour::make<AutoShoot>(_shooter);
+  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+  behaviour::make<wom::drivetrain::behaviours::DrivebasePoseBehaviour>(
+      _swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false);
+  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+  behaviour::make<AutoIntake>(_intake);
+  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+  behaviour::make<wom::drivetrain::behaviours::DrivebasePoseBehaviour>(
+      _swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false);
+  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+  behaviour::make<AutoShoot>(_shooter);
+  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+  behaviour::make<wom::drivetrain::behaviours::DrivebasePoseBehaviour>(
+      _swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false);
+  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+  behaviour::make<AutoIntake>(_intake);
+  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+  behaviour::make<AutoShoot>(_shooter);
+
+  //   4N Close 2N Far
+  // 1. Shoot note
+  // 2. Drive to close note
+  // 3. Intake note
+  // 4. Shoot note
+  // 5. Drive to close note
+  // 6. Intake note
+  // 7. Shoot note
+  // 8. Drive to close note
+  // 9. Intake note
+  // 10. Shoot note
+  // 11. Drive to far note
+  // 12. Intake note
+  // 13. Drive to shooting line
+  // 14. Shoot note
+  // 15. Drive to far note
+  // 16. Intake note
+  // 17. Drive to shooting line
+  // 18. Shoot note
 }
 
-std::shared_ptr<behaviour::Behaviour> autos::QuadrupleCloseSingleFar(wom::drivetrain::SwerveDrive* _swerveDrive, Shooter* _shooter, Intake* _intake, AlphaArm* _alphaArm) {
+std::shared_ptr<behaviour::Behaviour> autos::QuadrupleCloseSingleFar(
+    wom::drivetrain::SwerveDrive* _swerveDrive, Shooter* _shooter, Intake* _intake, AlphaArm* _alphaArm) {
   return behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
-    behaviour::make<AutoShoot>(_shooter);
-    behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
-    behaviour::make<wom::drivetrain::behaviours::DrivebasePoseBehaviour>(_swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false);
-    behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
-    behaviour::make<AutoIntake>(_intake);
-    behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
-    behaviour::make<wom::drivetrain::behaviours::DrivebasePoseBehaviour>(_swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false);
-    behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
-    behaviour::make<AutoShoot>(_shooter);
-    behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
-    behaviour::make<wom::drivetrain::behaviours::DrivebasePoseBehaviour>(_swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false);
-    behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
-    behaviour::make<AutoIntake>(_intake);
-    behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
-    behaviour::make<wom::drivetrain::behaviours::DrivebasePoseBehaviour>(_swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false);
-    behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
-    behaviour::make<AutoShoot>(_shooter);
-    behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
-    behaviour::make<wom::drivetrain::behaviours::DrivebasePoseBehaviour>(_swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false);
-    behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
-    behaviour::make<AutoIntake>(_intake); 
-    behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
-    behaviour::make<wom::drivetrain::behaviours::DrivebasePoseBehaviour>(_swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false);
-    behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
-    behaviour::make<AutoShoot>(_shooter);
-    behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+  behaviour::make<AutoShoot>(_shooter);
+  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+  behaviour::make<wom::drivetrain::behaviours::DrivebasePoseBehaviour>(
+      _swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false);
+  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+  behaviour::make<AutoIntake>(_intake);
+  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+  behaviour::make<wom::drivetrain::behaviours::DrivebasePoseBehaviour>(
+      _swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false);
+  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+  behaviour::make<AutoShoot>(_shooter);
+  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+  behaviour::make<wom::drivetrain::behaviours::DrivebasePoseBehaviour>(
+      _swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false);
+  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+  behaviour::make<AutoIntake>(_intake);
+  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+  behaviour::make<wom::drivetrain::behaviours::DrivebasePoseBehaviour>(
+      _swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false);
+  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+  behaviour::make<AutoShoot>(_shooter);
+  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+  behaviour::make<wom::drivetrain::behaviours::DrivebasePoseBehaviour>(
+      _swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false);
+  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+  behaviour::make<AutoIntake>(_intake);
+  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+  behaviour::make<wom::drivetrain::behaviours::DrivebasePoseBehaviour>(
+      _swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false);
+  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+  behaviour::make<AutoShoot>(_shooter);
+  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
 }
-    
-    // 4N Close 1N Far
-    // 1. Shoot note
-    // 2. Drive to close note
-    // 3. Intake note
-    // 4. Drive to speaker
-    // 5. Shoot note
-    // 6. Drive to close note
-    // 7. Intake note
-    // 8. Drive to speaker
-    // 9. Shoot note
-    // 10. Drive to close note
-    // 11. Intake note
-    // 12. Drive to speaker
-    // 13. Shoot note
-    // 14. Drive to far note
-    // 15. Intake note
-    // 15. Drive to speaker
-    // 16. shoot 
-    
-//     /* 
+
+// 4N Close 1N Far
+// 1. Shoot note
+// 2. Drive to close note
+// 3. Intake note
+// 4. Drive to speaker
+// 5. Shoot note
+// 6. Drive to close note
+// 7. Intake note
+// 8. Drive to speaker
+// 9. Shoot note
+// 10. Drive to close note
+// 11. Intake note
+// 12. Drive to speaker
+// 13. Shoot note
+// 14. Drive to far note
+// 15. Intake note
+// 15. Drive to speaker
+// 16. shoot
+
+//     /*
 //     TRAP AUTO
 //     1. Drive to trap
 //     2. Climb up
@@ -225,16 +263,19 @@ std::shared_ptr<behaviour::Behaviour> autos::QuadrupleCloseSingleFar(wom::drivet
 //     10. Climb down
 //     11. Drive to far note
 //     12. Intake
-//     13. Drive to trap 
+//     13. Drive to trap
 //     14. Climb up
 //     15. Shoot note
 //     16. Climb down
 //     */
 // }
 
-std::shared_ptr<behaviour::Behaviour> autos::AutoTest(wom::drivetrain::SwerveDrive* _swerveDrive, Shooter* _shooter, Intake* _intake, AlphaArm* _alphaArm) {
+std::shared_ptr<behaviour::Behaviour> autos::AutoTest(wom::drivetrain::SwerveDrive* _swerveDrive,
+                                                      Shooter* _shooter, Intake* _intake,
+                                                      AlphaArm* _alphaArm) {
   return behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
-    behaviour::make<wom::drivetrain::behaviours::DrivebasePoseBehaviour>(_swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false);
-    behaviour::make<AutoShoot>(_shooter);
-    behaviour::make<AutoIntake>(_intake);
-} // This auto is a test for auto to see if all things work. 
\ No newline at end of file
+  behaviour::make<wom::drivetrain::behaviours::DrivebasePoseBehaviour>(
+      _swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false);
+  behaviour::make<AutoShoot>(_shooter);
+  behaviour::make<AutoIntake>(_intake);
+}  // This auto is a test for auto to see if all things work.
diff --git a/src/main/cpp/IntakeBehaviour.cpp b/src/main/cpp/IntakeBehaviour.cpp
index d3091f70..16c1aba6 100644
--- a/src/main/cpp/IntakeBehaviour.cpp
+++ b/src/main/cpp/IntakeBehaviour.cpp
@@ -97,4 +97,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 9e26bf5d..2fe78031 100644
--- a/src/main/cpp/Robot.cpp
+++ b/src/main/cpp/Robot.cpp
@@ -18,9 +18,8 @@
 #include <units/velocity.h>
 #include <units/voltage.h>
 
-#include "behaviour/HasBehaviour.h"
-
 #include "Auto.h"
+#include "behaviour/HasBehaviour.h"
 
 static units::second_t lastPeriodic;
 
@@ -33,7 +32,7 @@ void Robot::RobotInit() {
   sched = wom::BehaviourScheduler::GetInstance();
   m_chooser.SetDefaultOption("kTaxi", "kTaxi");
 
-  for (auto &option : autoOptions) {
+  for (auto& option : autoOptions) {
     m_chooser.AddOption(option, option);
   }
 
@@ -108,7 +107,6 @@ void Robot::RobotPeriodic() {
   alphaArm->OnUpdate(dt);
   shooter->OnStart();
   intake->OnUpdate(dt);
-
 }
 
 void Robot::AutonomousInit() {
@@ -116,19 +114,19 @@ void Robot::AutonomousInit() {
   sched->InterruptAll();
 
   m_autoSelected = m_chooser.GetSelected();
-  
+
   if (m_autoSelected == "Taxi") {
-    sched->Schedule(autos::Taxi(_swerveDrive, shooter, intake,  alphaArm));
+    sched->Schedule(autos::Taxi(_swerveDrive, shooter, intake, alphaArm));
   } else if (m_autoSelected == "Auto Test") {
-    sched->Schedule(autos::AutoTest(_swerveDrive, shooter, intake,  alphaArm));
+    sched->Schedule(autos::AutoTest(_swerveDrive, shooter, intake, alphaArm));
   } else if (m_autoSelected == "Quadruple Close") {
-    sched->Schedule(autos::QuadrupleClose(_swerveDrive, shooter, intake,  alphaArm));
+    sched->Schedule(autos::QuadrupleClose(_swerveDrive, shooter, intake, alphaArm));
   } else if (m_autoSelected == "Quadruple Far") {
-    sched->Schedule(autos::QuadrupleFar(_swerveDrive, shooter, intake,  alphaArm));
+    sched->Schedule(autos::QuadrupleFar(_swerveDrive, shooter, intake, alphaArm));
   } else if (m_autoSelected == "Quadruple Close Double Far") {
-    sched->Schedule(autos::QuadrupleCloseDoubleFar(_swerveDrive, shooter, intake,  alphaArm));
+    sched->Schedule(autos::QuadrupleCloseDoubleFar(_swerveDrive, shooter, intake, alphaArm));
   } else if (m_autoSelected == "Quadruple Close Single Far") {
-    sched->Schedule(autos::QuadrupleCloseSingleFar(_swerveDrive, shooter, intake,  alphaArm));
+    sched->Schedule(autos::QuadrupleCloseSingleFar(_swerveDrive, shooter, intake, alphaArm));
   }
 }
 void Robot::AutonomousPeriodic() {
diff --git a/src/main/include/Auto.h b/src/main/include/Auto.h
index f1de6245..61aecc4e 100644
--- a/src/main/include/Auto.h
+++ b/src/main/include/Auto.h
@@ -1,24 +1,34 @@
-#pragma once
+// Copyright (c) 2023-2024 CurtinFRC
+// Open Source Software, you can modify it according to the terms
+// of the MIT License at the root of this project
 
-#include "Wombat.h"
+#pragma once
 
 #include <memory>
 
 #include "AlphaArmBehaviour.h"
 #include "IntakeBehaviour.h"
 #include "ShooterBehaviour.h"
-
+#include "Wombat.h"
 
 namespace autos {
-  std::shared_ptr<behaviour::Behaviour> AutoTest(wom::drivetrain::SwerveDrive* _swerveDrive, Shooter* _shooter, Intake* _intake, AlphaArm* _alphaArm);
+std::shared_ptr<behaviour::Behaviour> AutoTest(wom::drivetrain::SwerveDrive* _swerveDrive, Shooter* _shooter,
+                                               Intake* _intake, AlphaArm* _alphaArm);
 
-  std::shared_ptr<behaviour::Behaviour> Taxi(wom::drivetrain::SwerveDrive* _swerveDrive, Shooter* _shooter, Intake* _intake, AlphaArm* _alphaArm);
+std::shared_ptr<behaviour::Behaviour> Taxi(wom::drivetrain::SwerveDrive* _swerveDrive, Shooter* _shooter,
+                                           Intake* _intake, AlphaArm* _alphaArm);
 
-  std::shared_ptr<behaviour::Behaviour> QuadrupleClose(wom::drivetrain::SwerveDrive* _swerveDrive, Shooter* _shooter, Intake* _intake, AlphaArm* _alphaArm);
+std::shared_ptr<behaviour::Behaviour> QuadrupleClose(wom::drivetrain::SwerveDrive* _swerveDrive,
+                                                     Shooter* _shooter, Intake* _intake, AlphaArm* _alphaArm);
 
-  std::shared_ptr<behaviour::Behaviour> QuadrupleFar(wom::drivetrain::SwerveDrive* _swerveDrive, Shooter* _shooter, Intake* _intake, AlphaArm* _alphaArm);
+std::shared_ptr<behaviour::Behaviour> QuadrupleFar(wom::drivetrain::SwerveDrive* _swerveDrive,
+                                                   Shooter* _shooter, Intake* _intake, AlphaArm* _alphaArm);
 
-  std::shared_ptr<behaviour::Behaviour> QuadrupleCloseDoubleFar(wom::drivetrain::SwerveDrive* _swerveDrive, Shooter* _shooter, Intake* _intake, AlphaArm* _alphaArm);
+std::shared_ptr<behaviour::Behaviour> QuadrupleCloseDoubleFar(wom::drivetrain::SwerveDrive* _swerveDrive,
+                                                              Shooter* _shooter, Intake* _intake,
+                                                              AlphaArm* _alphaArm);
 
-  std::shared_ptr<behaviour::Behaviour> QuadrupleCloseSingleFar(wom::drivetrain::SwerveDrive* _swerveDrive, Shooter* _shooter, Intake* _intake, AlphaArm* _alphaArm);
-}
\ No newline at end of file
+std::shared_ptr<behaviour::Behaviour> QuadrupleCloseSingleFar(wom::drivetrain::SwerveDrive* _swerveDrive,
+                                                              Shooter* _shooter, Intake* _intake,
+                                                              AlphaArm* _alphaArm);
+}  // namespace autos
diff --git a/src/main/include/Robot.h b/src/main/include/Robot.h
index 68d1eab2..41ee8d81 100644
--- a/src/main/include/Robot.h
+++ b/src/main/include/Robot.h
@@ -67,12 +67,7 @@ class Robot : public frc::TimedRobot {
 
   std::string defaultAuto = "kTaxi";
   std::vector<std::string> autoOptions = {
-    kTaxi,
-    kAutoTest,
-    kQuadrupleClose,
-    kQuadrupleFar,
-    kQuadrupleCloseDoubleFar,
-    kQuadrupleCloseSingleFar,
+      kTaxi, kAutoTest, kQuadrupleClose, kQuadrupleFar, kQuadrupleCloseDoubleFar, kQuadrupleCloseSingleFar,
   };
 
   // ctre::phoenix6::hardware::TalonFX *frontLeft;
diff --git a/src/main/include/RobotMap.h b/src/main/include/RobotMap.h
index 8befddd6..d5169d90 100644
--- a/src/main/include/RobotMap.h
+++ b/src/main/include/RobotMap.h
@@ -1,4 +1,6 @@
 // Copyright (c) 2023-2024 CurtinFRC
+// Open Source Software, you can modify it according to the terms
+// of the MIT License at the root of this project
 
 // Open Source Software, you can modify it according to the terms
 // of the MIT License at the root of this project
diff --git a/src/main/include/Shooter.h b/src/main/include/Shooter.h
index c3314983..6ef8eef4 100644
--- a/src/main/include/Shooter.h
+++ b/src/main/include/Shooter.h
@@ -15,7 +15,7 @@
 struct ShooterConfig {
   std::string path;
   wom::Gearbox ShooterGearbox;
-  // wom::PIDConfig<units::radians_per_second, units::volt> pidConfig;  
+  // wom::PIDConfig<units::radians_per_second, units::volt> pidConfig;
   // frc::DigitalInput* shooterSensor;
   wom::PIDConfig<units::radians_per_second, units::volt> pidConfig;
 };
diff --git a/wombat/src/main/include/drivetrain/SwerveDrive.h b/wombat/src/main/include/drivetrain/SwerveDrive.h
index f03f7152..be4467b1 100644
--- a/wombat/src/main/include/drivetrain/SwerveDrive.h
+++ b/wombat/src/main/include/drivetrain/SwerveDrive.h
@@ -2,7 +2,7 @@
 // Open Source Software, you can modify it according to the terms
 // of the MIT License at the root of this project
 
-#pragma once 
+#pragma once
 
 #include <frc/controller/PIDController.h>
 #include <frc/estimator/SwerveDrivePoseEstimator.h>

From ce91c7e20299f26eee842bf0569eacccbcccb656 Mon Sep 17 00:00:00 2001
From: Kingsley Wong <kingsley.wong@student.education.wa.edu.au>
Date: Thu, 8 Feb 2024 20:08:13 +0800
Subject: [PATCH 095/207] Fixed most things

---
 src/main/cpp/Intake.cpp          | 103 ++++++++++++++++++++++---------
 src/main/cpp/IntakeBehaviour.cpp |  30 ++++-----
 src/main/include/Intake.h        |  23 ++++---
 src/main/include/RobotMap.h      |  13 +++-
 4 files changed, 111 insertions(+), 58 deletions(-)

diff --git a/src/main/cpp/Intake.cpp b/src/main/cpp/Intake.cpp
index 70a7e5a4..ba183fd6 100644
--- a/src/main/cpp/Intake.cpp
+++ b/src/main/cpp/Intake.cpp
@@ -10,59 +10,104 @@ IntakeConfig Intake::GetConfig() {
   return _config;
 }
 
-void Shooter::OnStart() {
+void Intake::OnStart() {
   _pid.Reset();
 }
 
 void Intake::OnUpdate(units::second_t dt) {
+  //TODO you need to print the state to network tables
+
   switch (_state) {
-    case IntakeState::kIdle: {
-      if (_config.intakeSensor->Get() == false) {
-        setState(IntakeState::kHold);
+    case IntakeState::kIdle: 
+    {
+      if (_config.intakeSensor->Get() == false) { 
+        SetState(IntakeState::kHold);
       }
       _stringStateName = "Idle";
+      _pid.Reset();
       _setVoltage = 0_V;
-    } break;
+    } 
+    break;
 
-    case IntakeState::kRaw: {
+    case IntakeState::kRaw: 
+    {
       _stringStateName = "Raw";
+      _pid.Reset();
       _setVoltage = _rawVoltage;
-    } break;
+    } 
+    break;
 
-    case IntakeState::kEject: {
+    case IntakeState::kEject: 
+    {
       _stringStateName = "Eject";
       _setVoltage = 7_V;
+      _pid.Reset();
       if (_config.intakeSensor->Get() == true) {
-        setState(IntakeState::kIdle);
+        SetState(IntakeState::kIdle);
         _ejecting = false;
       }
-    } break;
+    } 
+    break;
 
-    case IntakeState::kHold: {
+    case IntakeState::kHold: 
+    {
       _stringStateName = "Hold";
       _setVoltage = 0_V;
-      // if (_config.intakeSensor->Get() == false) {
-      //   setState(IntakeState::kHold);
-      // }
-    } break;
+    } 
+    break;
 
-    case IntakeState::kIntake: {
+    case IntakeState::kIntake: 
+    {
       _stringStateName = "Intake";
-      _setVoltage = -7_V;
+      _setVoltage = -7_V; 
       if (_config.intakeSensor->Get() == false) {
-        setState(IntakeState::kHold);
+        SetState(IntakeState::kHold);
         _intaking = false;
       }
-    } break;
+    } 
+    break;
 
-    case IntakeState::kPass: {
+    case IntakeState::kPass: 
+    {
       _stringStateName = "Pass";
       _setVoltage = -7_V;
       if (_config.intakeSensor->Get() == true) {
-        setState(IntakeState::kIdle);
+        SetState(IntakeState::kIdle);
         _passing = false;
       }
-    } break;
+      _pid.SetSetpoint(_goal);
+      units::volt_t pidCalculate =
+          units::volt_t{_pid.Calculate(_config.IntakeGearbox.encoder->GetEncoderAngularVelocity(), dt, 0_V)};
+      if (_pid.IsStable()) {
+        holdVoltage = pidCalculate;
+        std::cout << "STABLE" << std::endl;
+      }
+
+      if (holdVoltage.value() == 0) {
+        _setVoltage = pidCalculate;
+      } else {
+        _setVoltage = holdVoltage;
+      }
+    } //TODO, logic is a bit ugly, make it nicer
+    break;
+
+    case IntakeState::kPID:
+    {
+    _pid.SetSetpoint(_goal);
+      units::volt_t pidCalculate =
+          units::volt_t{_pid.Calculate(_config.IntakeGearbox.encoder->GetEncoderAngularVelocity(), dt, 0_V)};
+    if (_pid.IsStable()) {
+        holdVoltage = pidCalculate;
+        std::cout << "STABLE" << std::endl;
+      }
+    if (holdVoltage.value() == 0) {
+      _setVoltage = pidCalculate;
+    } else {
+      _setVoltage = holdVoltage;
+    }
+    }
+
+
     default:
       std::cout << "Error: Intake in INVALID STATE." << std::endl;
       break;
@@ -76,21 +121,19 @@ void Intake::OnUpdate(units::second_t dt) {
   // _table->GetEntry("Intake Sensor: ").SetBoolean(_config.intakeSensor->Get());
   // _table->GetEntry("Shooter Sensor: ").SetBoolean(_config.shooterSensor->Get());
   // _table->GetEntry("Magazine Sensor: ").SetBoolean(_config.magSensor->Get());
-
-  std::cout << _setVoltage.value() << std::endl;
-
-  _config.IntakeMotor.motorController->SetVoltage(_setVoltage);
+ 
+  _config.IntakeGearbox.motorController->SetVoltage(_setVoltage);
 }
 
-void Intake::setState(IntakeState state) {
+void Intake::SetState(IntakeState state) {
   _state = state;
 }
-void Intake::setRaw(units::volt_t voltage) {
+void Intake::SetRaw(units::volt_t voltage) {
   _rawVoltage = voltage;
 }
-IntakeState Intake::getState() {
+IntakeState Intake::GetState() { //TODO Should be capital
   return _state;
 }
 void Intake::SetPidGoal(units::radians_per_second_t goal) {
   _goal = goal;
-}
+}
\ No newline at end of file
diff --git a/src/main/cpp/IntakeBehaviour.cpp b/src/main/cpp/IntakeBehaviour.cpp
index 59558bba..9b6383c6 100644
--- a/src/main/cpp/IntakeBehaviour.cpp
+++ b/src/main/cpp/IntakeBehaviour.cpp
@@ -17,30 +17,30 @@ void IntakeManualControl::OnTick(units::second_t dt) {
       _rawControl = false;
       _intaking = false;
       _ejecting = false;
-      _intake->setState(IntakeState::kIdle);
+      _intake->SetState(IntakeState::kIdle);
     } else {
       _rawControl = true;
       _intaking = false;
       _ejecting = false;
-      _intake->setState(IntakeState::kRaw);
+      _intake->SetState(IntakeState::kRaw);
     }
   }
 
   if (_rawControl) {
     if (_codriver.GetRightTriggerAxis() > 0.1) {
-      _intake->setRaw(_codriver.GetRightTriggerAxis() * 10_V);
+      _intake->SetRaw(_codriver.GetRightTriggerAxis() * 10_V);
     } else if (_codriver.GetLeftTriggerAxis() > 0.1) {
-      _intake->setRaw(_codriver.GetLeftTriggerAxis() * -10_V);
+      _intake->SetRaw(_codriver.GetLeftTriggerAxis() * -10_V);
     } else {
-      _intake->setRaw(0_V);
+      _intake->SetRaw(0_V);
     }
-    _intake->setState(IntakeState::kRaw);
+    _intake->SetState(IntakeState::kRaw);
 
   } else {
     if (_codriver.GetRightTriggerAxis() > 0.1) {
       if (_intaking) {
         _intaking = false;
-        _intake->setState(IntakeState::kIdle);
+        _intake->SetState(IntakeState::kIdle);
       } else {
         _intaking = true;
         _ejecting = false;
@@ -50,7 +50,7 @@ void IntakeManualControl::OnTick(units::second_t dt) {
     if (_codriver.GetLeftTriggerAxis() > 0.1) {
       if (_ejecting) {
         _ejecting = false;
-        _intake->setState(IntakeState::kIdle);
+        _intake->SetState(IntakeState::kIdle);
       } else {
         _ejecting = true;
         _intaking = false;
@@ -60,7 +60,7 @@ void IntakeManualControl::OnTick(units::second_t dt) {
     if (_codriver.GetAButtonPressed()) {
       if (_passing) {
         _passing = false;
-        _intake->setState(IntakeState::kIdle);
+        _intake->SetState(IntakeState::kIdle);
       } else {
         _passing = true;
         _intaking = false;
@@ -68,20 +68,20 @@ void IntakeManualControl::OnTick(units::second_t dt) {
     }
 
     if (_intaking) {
-      if (_intake->getState() == IntakeState::kIdle) {
-        _intake->setState(IntakeState::kIntake);
+      if (_intake->GetState() == IntakeState::kIdle) {
+        _intake->SetState(IntakeState::kIntake);
       }
     }
 
     if (_passing) {
-      if (_intake->getState() == IntakeState::kHold) {
-        _intake->setState(IntakeState::kPass);
+      if (_intake->GetState() == IntakeState::kHold) {
+        _intake->SetState(IntakeState::kPass);
       }
     }
 
     if (_ejecting) {
-      if (_intake->getState() == IntakeState::kIdle || _intake->getState() == IntakeState::kHold) {
-        _intake->setState(IntakeState::kEject);
+      if (_intake->GetState() == IntakeState::kIdle || _intake->GetState() == IntakeState::kHold) {
+        _intake->SetState(IntakeState::kEject);
       }
     }
   }
diff --git a/src/main/include/Intake.h b/src/main/include/Intake.h
index d2e32570..f37d19a3 100644
--- a/src/main/include/Intake.h
+++ b/src/main/include/Intake.h
@@ -14,14 +14,15 @@
 #include "Wombat.h"
 
 struct IntakeConfig {
-  wom::Gearbox IntakeMotor;
+  std::string path;
+  wom::Gearbox IntakeGearbox;
   frc::DigitalInput* intakeSensor;
-  frc::DigitalInput* magSensor;
-  frc::DigitalInput* shooterSensor;
+  // frc::DigitalInput* magSensor;
+  // frc::DigitalInput* shooterSensor;
   wom::PIDConfig<units::radians_per_second, units::volt> pidConfig;
 };
 
-enum class IntakeState { kIdle, kRaw, kHold, kEject, kIntake, kPass };
+enum class IntakeState { kIdle, kRaw, kHold, kEject, kIntake, kPass, kPID};
 
 class Intake : public behaviour::HasBehaviour {
  public:
@@ -29,10 +30,11 @@ class Intake : public behaviour::HasBehaviour {
 
   void OnUpdate(units::second_t dt);
 
-  void setState(IntakeState state);
-  void setRaw(units::volt_t voltage);
-  void SetPidGoal(units::radians_per_second_t);
-  IntakeState getState();
+  void SetState(IntakeState state);
+  void SetRaw(units::volt_t voltage);
+  void SetPidGoal(units::radians_per_second_t goal);
+  void OnStart();
+  IntakeState GetState();
   IntakeConfig GetConfig();
 
  private:
@@ -42,11 +44,12 @@ class Intake : public behaviour::HasBehaviour {
   units::volt_t _rawVoltage = 0_V;
   std::string _stringStateName = "error";
   units::volt_t _setVoltage = 0_V;
-  bool _intaking;
+  units::volt_t holdVoltage = 0_V;
+  bool _intaking; //NEEDS DEFAULTS
   bool _ejecting;
   bool _passing;
+  units::radians_per_second_t _goal;
   wom::PIDController<units::radians_per_second, units::volt> _pid;
   
-
   std::shared_ptr<nt::NetworkTable> _table = nt::NetworkTableInstance::GetDefault().GetTable("Intake");
 };
diff --git a/src/main/include/RobotMap.h b/src/main/include/RobotMap.h
index 0cadce34..35275821 100644
--- a/src/main/include/RobotMap.h
+++ b/src/main/include/RobotMap.h
@@ -32,14 +32,21 @@ struct RobotMap {
 
   struct IntakeSystem {
     rev::CANSparkMax intakeMotor{2, rev::CANSparkMax::MotorType::kBrushed};
-    // wom::CANSparkMaxEncoder intakeEncoder{&intakeMotor, 0.1_m};
+    wom::CANSparkMaxEncoder intakeEncoder{&intakeMotor, 0.1_m};
     frc::DigitalInput intakeSensor{4};
     // frc::DigitalInput magSensor{0};
     // frc::DigitalInput shooterSensor{0};
 
-    wom::Gearbox IntakeGearbox{&intakeMotor, nullptr, frc::DCMotor::CIM(1)};
+    wom::Gearbox IntakeGearbox{&intakeMotor, &intakeEncoder, frc::DCMotor::CIM(1)};
+
+    IntakeConfig config{"path", IntakeGearbox, &intakeSensor, wom::PIDConfig<units::radians_per_second, units::volts>(
+          "/intake/PID/config",
+          9_V / (180_deg / 1_s),
+          0_V / 25_deg,
+          0_V / (90_deg / 1_s / 1_s)
+        ),/*, &magSensor, &shooterSensor*/};
+
 
-    IntakeConfig config{IntakeGearbox, &intakeSensor /*, &magSensor, &shooterSensor*/};
   };
   IntakeSystem intakeSystem;
 

From a9905ba740017be4cf05424be605e1b75076cf85 Mon Sep 17 00:00:00 2001
From: Kingsley Wong <kingsley.wong@student.education.wa.edu.au>
Date: Sat, 10 Feb 2024 15:15:48 +0800
Subject: [PATCH 096/207] Fixed most things 2

---
 src/main/cpp/Intake.cpp          | 17 +++---
 src/main/cpp/IntakeBehaviour.cpp | 96 ++++++++++++++------------------
 src/main/include/Intake.h        |  8 +--
 3 files changed, 54 insertions(+), 67 deletions(-)

diff --git a/src/main/cpp/Intake.cpp b/src/main/cpp/Intake.cpp
index ba183fd6..b2194530 100644
--- a/src/main/cpp/Intake.cpp
+++ b/src/main/cpp/Intake.cpp
@@ -15,7 +15,6 @@ void Intake::OnStart() {
 }
 
 void Intake::OnUpdate(units::second_t dt) {
-  //TODO you need to print the state to network tables
 
   switch (_state) {
     case IntakeState::kIdle: 
@@ -43,8 +42,7 @@ void Intake::OnUpdate(units::second_t dt) {
       _setVoltage = 7_V;
       _pid.Reset();
       if (_config.intakeSensor->Get() == true) {
-        SetState(IntakeState::kIdle);
-        _ejecting = false;
+        SetState(IntakeState::kIdle); 
       }
     } 
     break;
@@ -52,7 +50,12 @@ void Intake::OnUpdate(units::second_t dt) {
     case IntakeState::kHold: 
     {
       _stringStateName = "Hold";
-      _setVoltage = 0_V;
+      //& add a check for if sensor is empty 
+
+      if (_config.intakeSensor->Get() == true) {
+        _setVoltage = 0_V;
+      }
+
     } 
     break;
 
@@ -62,7 +65,6 @@ void Intake::OnUpdate(units::second_t dt) {
       _setVoltage = -7_V; 
       if (_config.intakeSensor->Get() == false) {
         SetState(IntakeState::kHold);
-        _intaking = false;
       }
     } 
     break;
@@ -73,7 +75,6 @@ void Intake::OnUpdate(units::second_t dt) {
       _setVoltage = -7_V;
       if (_config.intakeSensor->Get() == true) {
         SetState(IntakeState::kIdle);
-        _passing = false;
       }
       _pid.SetSetpoint(_goal);
       units::volt_t pidCalculate =
@@ -88,7 +89,7 @@ void Intake::OnUpdate(units::second_t dt) {
       } else {
         _setVoltage = holdVoltage;
       }
-    } //TODO, logic is a bit ugly, make it nicer
+    } //&, logic is a bit ugly, make it nicer
     break;
 
     case IntakeState::kPID:
@@ -131,7 +132,7 @@ void Intake::SetState(IntakeState state) {
 void Intake::SetRaw(units::volt_t voltage) {
   _rawVoltage = voltage;
 }
-IntakeState Intake::GetState() { //TODO Should be capital
+IntakeState Intake::GetState() {
   return _state;
 }
 void Intake::SetPidGoal(units::radians_per_second_t goal) {
diff --git a/src/main/cpp/IntakeBehaviour.cpp b/src/main/cpp/IntakeBehaviour.cpp
index 9b6383c6..9e7d42cc 100644
--- a/src/main/cpp/IntakeBehaviour.cpp
+++ b/src/main/cpp/IntakeBehaviour.cpp
@@ -12,21 +12,27 @@ IntakeManualControl::IntakeManualControl(Intake* intake, frc::XboxController& co
 }
 
 void IntakeManualControl::OnTick(units::second_t dt) {
+
+  /*
+  
+  if a button is held down:
+    set state to pass
+  else if right trigger is pressed:
+    set state to intaking
+  else if left trigger is pressed:
+    set state to eject
+  else
+    set state to idle
+  */
+
+
   if (_codriver.GetBButtonReleased()) {
-    if (_rawControl) {
-      _rawControl = false;
-      _intaking = false;
-      _ejecting = false;
+    if (_intake->GetState() == IntakeState::kRaw) {
       _intake->SetState(IntakeState::kIdle);
     } else {
-      _rawControl = true;
-      _intaking = false;
-      _ejecting = false;
-      _intake->SetState(IntakeState::kRaw);
+      _intake->SetState(IntakeState::kIdle); // this is not a toggle button
     }
-  }
-
-  if (_rawControl) {
+  } else if (_rawControl) {
     if (_codriver.GetRightTriggerAxis() > 0.1) {
       _intake->SetRaw(_codriver.GetRightTriggerAxis() * 10_V);
     } else if (_codriver.GetLeftTriggerAxis() > 0.1) {
@@ -35,55 +41,35 @@ void IntakeManualControl::OnTick(units::second_t dt) {
       _intake->SetRaw(0_V);
     }
     _intake->SetState(IntakeState::kRaw);
-
+    
   } else {
     if (_codriver.GetRightTriggerAxis() > 0.1) {
-      if (_intaking) {
-        _intaking = false;
-        _intake->SetState(IntakeState::kIdle);
-      } else {
-        _intaking = true;
-        _ejecting = false;
-      }
-    }
-
-    if (_codriver.GetLeftTriggerAxis() > 0.1) {
-      if (_ejecting) {
-        _ejecting = false;
-        _intake->SetState(IntakeState::kIdle);
-      } else {
-        _ejecting = true;
-        _intaking = false;
-      }
-    }
-
-    if (_codriver.GetAButtonPressed()) {
-      if (_passing) {
-        _passing = false;
-        _intake->SetState(IntakeState::kIdle);
-      } else {
-        _passing = true;
-        _intaking = false;
-      }
-    }
-
-    if (_intaking) {
-      if (_intake->GetState() == IntakeState::kIdle) {
         _intake->SetState(IntakeState::kIntake);
-      }
-    }
+    } else if (_codriver.GetLeftTriggerAxis() > 0.1) {
+        _intake->SetState(IntakeState::kIntake);
+    } else if (_codriver.GetAButtonPressed()) {
+      _intake->SetState(IntakeState::kPass);
+    } else {
+    _intake->SetState(IntakeState::kIdle);  
+  }
 
-    if (_passing) {
-      if (_intake->GetState() == IntakeState::kHold) {
-        _intake->SetState(IntakeState::kPass);
-      }
-    }
+      // if (_intaking) {
+    //   if (_intake->GetState() == IntakeState::kIdle) {
+    //     _intake->SetState(IntakeState::kIntake);
+    //   }
+    // }
 
-    if (_ejecting) {
-      if (_intake->GetState() == IntakeState::kIdle || _intake->GetState() == IntakeState::kHold) {
-        _intake->SetState(IntakeState::kEject);
-      }
-    }
+    // if (_passing) {
+    //   if (_intake->GetState() == IntakeState::kHold) {
+    //     _intake->SetState(IntakeState::kPass);
+    //   }
+    // }
+
+    // if (_ejecting) {
+    //   if (_intake->GetState() == IntakeState::kIdle || _intake->GetState() == IntakeState::kHold) {
+    //     _intake->SetState(IntakeState::kEject);
+    //   }
+    // }
   }
 }
 
diff --git a/src/main/include/Intake.h b/src/main/include/Intake.h
index f37d19a3..d01c5ccb 100644
--- a/src/main/include/Intake.h
+++ b/src/main/include/Intake.h
@@ -45,11 +45,11 @@ class Intake : public behaviour::HasBehaviour {
   std::string _stringStateName = "error";
   units::volt_t _setVoltage = 0_V;
   units::volt_t holdVoltage = 0_V;
-  bool _intaking; //NEEDS DEFAULTS
-  bool _ejecting;
-  bool _passing;
+  // bool _intaking; //&NEEDS DEFAULTS
+  // bool _ejecting;
+  // bool _passing;
   units::radians_per_second_t _goal;
   wom::PIDController<units::radians_per_second, units::volt> _pid;
   
   std::shared_ptr<nt::NetworkTable> _table = nt::NetworkTableInstance::GetDefault().GetTable("Intake");
-};
+};
\ No newline at end of file

From 2669a4ba3febce45cc99a6d84ec8cd34376c15ae Mon Sep 17 00:00:00 2001
From: Anna Pedersen <59428185+goanna247@users.noreply.github.com>
Date: Tue, 23 Jan 2024 20:21:33 +0800
Subject: [PATCH 097/207] Swerve fix (#106)

* fixed swerve

* wrapping function
---
 .clang-format                                 |   2 +-
 .editorconfig                                 |   5 +
 .github/workflows/format.yml                  |   2 +-
 build.gradle                                  |   2 +-
 gradlew                                       |   0
 gradlew.bat                                   | 184 +++++++--------
 init.ps1                                      |   2 +
 init.sh                                       |   5 +
 src/main/cpp/Robot.cpp                        |  52 ++++-
 src/main/include/Robot.h                      |  10 +-
 src/main/include/RobotMap.h                   | 100 +++++----
 wombat/LICENSE                                |  21 ++
 wombat/README.md                              |   1 +
 wombat/settings.gradle                        |   1 +
 wombat/src/main/cpp/behaviour/Behaviour.cpp   |  23 +-
 .../main/cpp/behaviour/BehaviourScheduler.cpp |   9 +-
 .../src/main/cpp/behaviour/HasBehaviour.cpp   |   3 +-
 wombat/src/main/cpp/drivetrain/Drivetrain.cpp |  15 +-
 .../src/main/cpp/drivetrain/SwerveDrive.cpp   | 209 ++++++++++++------
 .../behaviours/SwerveBehaviours.cpp           | 169 +++++++-------
 wombat/src/main/cpp/subsystems/Arm.cpp        |  39 ++--
 wombat/src/main/cpp/subsystems/Elevator.cpp   |  34 +--
 wombat/src/main/cpp/subsystems/Shooter.cpp    |  24 +-
 wombat/src/main/cpp/utils/Encoder.cpp         |  84 ++++---
 wombat/src/main/cpp/utils/Util.cpp            |  27 ++-
 wombat/src/main/cpp/vision/Limelight.cpp      |  24 +-
 wombat/src/main/include/behaviour/Behaviour.h |  42 ++--
 .../src/main/include/behaviour/HasBehaviour.h |   3 +-
 .../src/main/include/drivetrain/SwerveDrive.h |  41 ++--
 .../drivetrain/behaviours/SwerveBehaviours.h  |  17 +-
 wombat/src/main/include/subsystems/Arm.h      |   3 +-
 wombat/src/main/include/subsystems/Shooter.h  |   3 +-
 wombat/src/main/include/utils/Encoder.h       |  38 ++--
 wombat/src/main/include/utils/PID.h           |  82 ++++---
 wombat/src/main/include/utils/Util.h          |  55 +++--
 wombat/src/main/include/vision/Limelight.h    |  22 +-
 36 files changed, 838 insertions(+), 515 deletions(-)
 create mode 100644 .editorconfig
 mode change 100755 => 100644 gradlew
 create mode 100644 init.ps1
 create mode 100644 init.sh
 create mode 100644 wombat/LICENSE
 create mode 100644 wombat/README.md
 create mode 100644 wombat/settings.gradle

diff --git a/.clang-format b/.clang-format
index a879dec4..f69bef0c 100644
--- a/.clang-format
+++ b/.clang-format
@@ -81,7 +81,7 @@ BreakBeforeTernaryOperators: true
 BreakConstructorInitializers: BeforeColon
 BreakInheritanceList: BeforeColon
 BreakStringLiterals: true
-ColumnLimit:     110
+ColumnLimit:     80
 CommentPragmas:  '^ IWYU pragma:'
 CompactNamespaces: false
 ConstructorInitializerIndentWidth: 4
diff --git a/.editorconfig b/.editorconfig
new file mode 100644
index 00000000..0020fc03
--- /dev/null
+++ b/.editorconfig
@@ -0,0 +1,5 @@
+root = true
+
+[*]
+indent_style = space
+indent_size = 2
diff --git a/.github/workflows/format.yml b/.github/workflows/format.yml
index f57ddeb4..c6fced20 100644
--- a/.github/workflows/format.yml
+++ b/.github/workflows/format.yml
@@ -21,7 +21,7 @@ jobs:
         with:
           python-version: 3.8
       - name: Install wpiformat
-        run: pip3 install wpiformat==2024.31
+        run: pip3 install wpiformat==2023.36
       - name: Run
         run: wpiformat
       - name: Check output
diff --git a/build.gradle b/build.gradle
index 84ccc0cb..91b6acb4 100644
--- a/build.gradle
+++ b/build.gradle
@@ -1,7 +1,7 @@
 plugins {
     id "cpp"
     id "google-test-test-suite"
-    id "edu.wpi.first.GradleRIO" version "2024.2.1"
+    id "edu.wpi.first.GradleRIO" version "2024.1.1"
 }
 
 // Define my targets (RoboRIO) and artifacts (deployable files)
diff --git a/gradlew b/gradlew
old mode 100755
new mode 100644
diff --git a/gradlew.bat b/gradlew.bat
index 6689b85b..93e3f59f 100644
--- a/gradlew.bat
+++ b/gradlew.bat
@@ -1,92 +1,92 @@
-@rem
-@rem Copyright 2015 the original author or authors.
-@rem
-@rem Licensed under the Apache License, Version 2.0 (the "License");
-@rem you may not use this file except in compliance with the License.
-@rem You may obtain a copy of the License at
-@rem
-@rem      https://www.apache.org/licenses/LICENSE-2.0
-@rem
-@rem Unless required by applicable law or agreed to in writing, software
-@rem distributed under the License is distributed on an "AS IS" BASIS,
-@rem WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-@rem See the License for the specific language governing permissions and
-@rem limitations under the License.
-@rem
-
-@if "%DEBUG%"=="" @echo off
-@rem ##########################################################################
-@rem
-@rem  Gradle startup script for Windows
-@rem
-@rem ##########################################################################
-
-@rem Set local scope for the variables with windows NT shell
-if "%OS%"=="Windows_NT" setlocal
-
-set DIRNAME=%~dp0
-if "%DIRNAME%"=="" set DIRNAME=.
-@rem This is normally unused
-set APP_BASE_NAME=%~n0
-set APP_HOME=%DIRNAME%
-
-@rem Resolve any "." and ".." in APP_HOME to make it shorter.
-for %%i in ("%APP_HOME%") do set APP_HOME=%%~fi
-
-@rem Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script.
-set DEFAULT_JVM_OPTS="-Xmx64m" "-Xms64m"
-
-@rem Find java.exe
-if defined JAVA_HOME goto findJavaFromJavaHome
-
-set JAVA_EXE=java.exe
-%JAVA_EXE% -version >NUL 2>&1
-if %ERRORLEVEL% equ 0 goto execute
-
-echo.
-echo ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH.
-echo.
-echo Please set the JAVA_HOME variable in your environment to match the
-echo location of your Java installation.
-
-goto fail
-
-:findJavaFromJavaHome
-set JAVA_HOME=%JAVA_HOME:"=%
-set JAVA_EXE=%JAVA_HOME%/bin/java.exe
-
-if exist "%JAVA_EXE%" goto execute
-
-echo.
-echo ERROR: JAVA_HOME is set to an invalid directory: %JAVA_HOME%
-echo.
-echo Please set the JAVA_HOME variable in your environment to match the
-echo location of your Java installation.
-
-goto fail
-
-:execute
-@rem Setup the command line
-
-set CLASSPATH=%APP_HOME%\gradle\wrapper\gradle-wrapper.jar
-
-
-@rem Execute Gradle
-"%JAVA_EXE%" %DEFAULT_JVM_OPTS% %JAVA_OPTS% %GRADLE_OPTS% "-Dorg.gradle.appname=%APP_BASE_NAME%" -classpath "%CLASSPATH%" org.gradle.wrapper.GradleWrapperMain %*
-
-:end
-@rem End local scope for the variables with windows NT shell
-if %ERRORLEVEL% equ 0 goto mainEnd
-
-:fail
-rem Set variable GRADLE_EXIT_CONSOLE if you need the _script_ return code instead of
-rem the _cmd.exe /c_ return code!
-set EXIT_CODE=%ERRORLEVEL%
-if %EXIT_CODE% equ 0 set EXIT_CODE=1
-if not ""=="%GRADLE_EXIT_CONSOLE%" exit %EXIT_CODE%
-exit /b %EXIT_CODE%
-
-:mainEnd
-if "%OS%"=="Windows_NT" endlocal
-
-:omega
+@rem
+@rem Copyright 2015 the original author or authors.
+@rem
+@rem Licensed under the Apache License, Version 2.0 (the "License");
+@rem you may not use this file except in compliance with the License.
+@rem You may obtain a copy of the License at
+@rem
+@rem      https://www.apache.org/licenses/LICENSE-2.0
+@rem
+@rem Unless required by applicable law or agreed to in writing, software
+@rem distributed under the License is distributed on an "AS IS" BASIS,
+@rem WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+@rem See the License for the specific language governing permissions and
+@rem limitations under the License.
+@rem
+
+@if "%DEBUG%"=="" @echo off
+@rem ##########################################################################
+@rem
+@rem  Gradle startup script for Windows
+@rem
+@rem ##########################################################################
+
+@rem Set local scope for the variables with windows NT shell
+if "%OS%"=="Windows_NT" setlocal
+
+set DIRNAME=%~dp0
+if "%DIRNAME%"=="" set DIRNAME=.
+@rem This is normally unused
+set APP_BASE_NAME=%~n0
+set APP_HOME=%DIRNAME%
+
+@rem Resolve any "." and ".." in APP_HOME to make it shorter.
+for %%i in ("%APP_HOME%") do set APP_HOME=%%~fi
+
+@rem Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script.
+set DEFAULT_JVM_OPTS="-Xmx64m" "-Xms64m"
+
+@rem Find java.exe
+if defined JAVA_HOME goto findJavaFromJavaHome
+
+set JAVA_EXE=java.exe
+%JAVA_EXE% -version >NUL 2>&1
+if %ERRORLEVEL% equ 0 goto execute
+
+echo.
+echo ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH.
+echo.
+echo Please set the JAVA_HOME variable in your environment to match the
+echo location of your Java installation.
+
+goto fail
+
+:findJavaFromJavaHome
+set JAVA_HOME=%JAVA_HOME:"=%
+set JAVA_EXE=%JAVA_HOME%/bin/java.exe
+
+if exist "%JAVA_EXE%" goto execute
+
+echo.
+echo ERROR: JAVA_HOME is set to an invalid directory: %JAVA_HOME%
+echo.
+echo Please set the JAVA_HOME variable in your environment to match the
+echo location of your Java installation.
+
+goto fail
+
+:execute
+@rem Setup the command line
+
+set CLASSPATH=%APP_HOME%\gradle\wrapper\gradle-wrapper.jar
+
+
+@rem Execute Gradle
+"%JAVA_EXE%" %DEFAULT_JVM_OPTS% %JAVA_OPTS% %GRADLE_OPTS% "-Dorg.gradle.appname=%APP_BASE_NAME%" -classpath "%CLASSPATH%" org.gradle.wrapper.GradleWrapperMain %*
+
+:end
+@rem End local scope for the variables with windows NT shell
+if %ERRORLEVEL% equ 0 goto mainEnd
+
+:fail
+rem Set variable GRADLE_EXIT_CONSOLE if you need the _script_ return code instead of
+rem the _cmd.exe /c_ return code!
+set EXIT_CODE=%ERRORLEVEL%
+if %EXIT_CODE% equ 0 set EXIT_CODE=1
+if not ""=="%GRADLE_EXIT_CONSOLE%" exit %EXIT_CODE%
+exit /b %EXIT_CODE%
+
+:mainEnd
+if "%OS%"=="Windows_NT" endlocal
+
+:omega
diff --git a/init.ps1 b/init.ps1
new file mode 100644
index 00000000..fa83b9a6
--- /dev/null
+++ b/init.ps1
@@ -0,0 +1,2 @@
+./gradlew installRoborioToolchain
+./gradlew build
diff --git a/init.sh b/init.sh
new file mode 100644
index 00000000..6a932ed6
--- /dev/null
+++ b/init.sh
@@ -0,0 +1,5 @@
+#!/usr/bin/sh
+sudo apt update
+chmod +x gradlew
+./gradlew installRoborioToolchain
+./gradlew build
diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp
index 80f253d5..ddccb14d 100644
--- a/src/main/cpp/Robot.cpp
+++ b/src/main/cpp/Robot.cpp
@@ -15,10 +15,12 @@
 #include <frc/kinematics/DifferentialDriveKinematics.h>
 #include <frc/controller/RamseteController.h>
 #include <frc/Timer.h>
+#include "behaviour/HasBehaviour.h"
 
 static units::second_t lastPeriodic;
 
 void Robot::RobotInit() {
+  sched = wom::BehaviourScheduler::GetInstance();
   m_chooser.SetDefaultOption("Default Auto", "Default Auto");
 
   frc::SmartDashboard::PutData("Auto Selector", &m_chooser);
@@ -36,13 +38,34 @@ void Robot::RobotInit() {
 
   robotmap.swerveBase.gyro->Reset();
 
-  _swerveDrive = new wom::SwerveDrive(robotmap.swerveBase.config, frc::Pose2d());
+  _swerveDrive =
+      new wom::SwerveDrive(robotmap.swerveBase.config, frc::Pose2d());
   wom::BehaviourScheduler::GetInstance()->Register(_swerveDrive);
-  _swerveDrive->SetDefaultBehaviour(
-      [this]() { return wom::make<wom::ManualDrivebase>(_swerveDrive, &robotmap.controllers.driver); });
+  _swerveDrive->SetDefaultBehaviour([this]() {
+    return wom::make<wom::ManualDrivebase>(_swerveDrive,
+                                           &robotmap.controllers.driver);
+  });
 
+  
   // m_driveSim = new wom::TempSimSwerveDrive(&simulation_timer, &m_field);
   // m_driveSim = wom::TempSimSwerveDrive();
+
+  robotmap.swerveBase.moduleConfigs[0].turnMotor.encoder->SetEncoderOffset(0_rad);
+  robotmap.swerveBase.moduleConfigs[1].turnMotor.encoder->SetEncoderOffset(0_rad);
+  robotmap.swerveBase.moduleConfigs[2].turnMotor.encoder->SetEncoderOffset(0_rad);
+  robotmap.swerveBase.moduleConfigs[3].turnMotor.encoder->SetEncoderOffset(0_rad);
+
+
+
+
+  // frontLeft = new ctre::phoenix6::hardware::TalonFX(7, "Drivebase");  // front left
+  // frontRight = new ctre::phoenix6::hardware::TalonFX(2, "Drivebase");   // front right
+  // backLeft = new ctre::phoenix6::hardware::TalonFX(6, "Drivebase");   // back left
+  // backRight = new ctre::phoenix6::hardware::TalonFX(4, "Drivebase");  // back right
+  // frontLeft = new ctre::phoenix6::hardware::TalonFX(9, "Drivebase");   // front left
+  // frontRight = new ctre::phoenix6::hardware::TalonFX(1, "Drivebase");   // front right
+  // backLeft = new ctre::phoenix6::hardware::TalonFX(5, "Drivebase");   // back left
+  // backRight = new ctre::phoenix6::hardware::TalonFX(3, "Drivebase");
 }
 
 void Robot::RobotPeriodic() {
@@ -50,7 +73,12 @@ void Robot::RobotPeriodic() {
   lastPeriodic = wom::now();
 
   loop.Poll();
-  wom::BehaviourScheduler::GetInstance()->Tick();
+  sched->Tick();
+
+  robotmap.swerveTable.swerveDriveTable->GetEntry("frontLeftEncoder").SetDouble(robotmap.swerveBase.moduleConfigs[0].turnMotor.encoder->GetEncoderPosition().value());
+  robotmap.swerveTable.swerveDriveTable->GetEntry("frontRightEncoder").SetDouble(robotmap.swerveBase.moduleConfigs[1].turnMotor.encoder->GetEncoderPosition().value());
+  robotmap.swerveTable.swerveDriveTable->GetEntry("backLeftEncoder").SetDouble(robotmap.swerveBase.moduleConfigs[2].turnMotor.encoder->GetEncoderPosition().value());
+  robotmap.swerveTable.swerveDriveTable->GetEntry("backRightEncoder").SetDouble(robotmap.swerveBase.moduleConfigs[3].turnMotor.encoder->GetEncoderPosition().value());
 
   _swerveDrive->OnUpdate(dt);
 }
@@ -60,15 +88,19 @@ void Robot::AutonomousInit() {
 
   loop.Clear();
   sched->InterruptAll();
-  // _swerveDrive->OnStart();
 }
 void Robot::AutonomousPeriodic() {
   // m_driveSim->OnUpdate();
 }
 
 void Robot::TeleopInit() {
-  // _swerveDrive->OnStart();
-  // sched->InterruptAll();
+  loop.Clear();
+  sched->InterruptAll();
+
+  // frontLeft->SetVoltage(4_V);
+  // frontRight->SetVoltage(4_V);
+  // backLeft->SetVoltage(4_V);
+  // backRight->SetVoltage(4_V);
 }
 void Robot::TeleopPeriodic() {}
 
@@ -76,8 +108,4 @@ void Robot::DisabledInit() {}
 void Robot::DisabledPeriodic() {}
 
 void Robot::TestInit() {}
-void Robot::TestPeriodic() {}
-
-void Robot::SimulationInit() {}
-
-void Robot::SimulationPeriodic() {}
+void Robot::TestPeriodic() {}
\ No newline at end of file
diff --git a/src/main/include/Robot.h b/src/main/include/Robot.h
index bc98df2a..9cef2cb1 100644
--- a/src/main/include/Robot.h
+++ b/src/main/include/Robot.h
@@ -31,12 +31,9 @@ class Robot : public frc::TimedRobot {
   void DisabledPeriodic() override;
   void TestInit() override;
   void TestPeriodic() override;
-  void SimulationInit() override;
-  void SimulationPeriodic() override;
-
  private:
-  behaviour::BehaviourScheduler* sched;
   RobotMap robotmap;
+  wom::BehaviourScheduler* sched;
   frc::EventLoop loop;
 
   frc::SendableChooser<std::string> m_chooser;
@@ -48,4 +45,9 @@ class Robot : public frc::TimedRobot {
   frc::SendableChooser<std::string> m_path_chooser;
 
   wom::SwerveDrive* _swerveDrive;
+
+  // ctre::phoenix6::hardware::TalonFX *frontLeft;
+  // ctre::phoenix6::hardware::TalonFX *frontRight;
+  // ctre::phoenix6::hardware::TalonFX *backLeft;
+  // ctre::phoenix6::hardware::TalonFX *backRight;
 };
diff --git a/src/main/include/RobotMap.h b/src/main/include/RobotMap.h
index d5efd27a..6e64d5e8 100644
--- a/src/main/include/RobotMap.h
+++ b/src/main/include/RobotMap.h
@@ -1,7 +1,6 @@
 // Copyright (c) 2023-2024 CurtinFRC
 // Open Source Software, you can modify it according to the terms
 // of the MIT License at the root of this project
-
 #pragma once
 
 #include <frc/Compressor.h>
@@ -28,90 +27,99 @@ struct RobotMap {
   Controllers controllers;
 
   struct SwerveBase {
-    ctre::phoenix6::hardware::CANcoder frontLeftCancoder{19};
-    ctre::phoenix6::hardware::CANcoder frontRightCancoder{17};
-    ctre::phoenix6::hardware::CANcoder backLeftCancoder{16};
-    ctre::phoenix6::hardware::CANcoder backRightCancoder{18};
+    ctre::phoenix6::hardware::CANcoder frontLeftCancoder{18, "Drivebase"};
+    ctre::phoenix6::hardware::CANcoder frontRightCancoder{19, "Drivebase"};
+    ctre::phoenix6::hardware::CANcoder backLeftCancoder{16, "Drivebase"};
+    ctre::phoenix6::hardware::CANcoder backRightCancoder{17, "Drivebase"};
 
-    ctre::phoenix6::hardware::Pigeon2* gyro = new ctre::phoenix6::hardware::Pigeon2(20, "Drivebase");
+    ctre::phoenix6::hardware::Pigeon2* gyro =
+        new ctre::phoenix6::hardware::Pigeon2(20, "Drivebase");
     wpi::array<ctre::phoenix6::hardware::TalonFX*, 4> turnMotors{
-        new ctre::phoenix6::hardware::TalonFX(7, "Drivebase"),
-        new ctre::phoenix6::hardware::TalonFX(5, "Drivebase"),
-        new ctre::phoenix6::hardware::TalonFX(1, "Drivebase"),
-        new ctre::phoenix6::hardware::TalonFX(3, "Drivebase")};
+        new ctre::phoenix6::hardware::TalonFX(7, "Drivebase"),   // front left
+        new ctre::phoenix6::hardware::TalonFX(2, "Drivebase"),   // front right
+        new ctre::phoenix6::hardware::TalonFX(6, "Drivebase"),   // back left
+        new ctre::phoenix6::hardware::TalonFX(4, "Drivebase")};  // back right
     wpi::array<ctre::phoenix6::hardware::TalonFX*, 4> driveMotors{
-        new ctre::phoenix6::hardware::TalonFX(9, "Drivebase"),
-        new ctre::phoenix6::hardware::TalonFX(6, "Drivebase"),
-        new ctre::phoenix6::hardware::TalonFX(2, "Drivebase"),
-        new ctre::phoenix6::hardware::TalonFX(4, "Drivebase")};
+        new ctre::phoenix6::hardware::TalonFX(9, "Drivebase"),   // front left
+        new ctre::phoenix6::hardware::TalonFX(1, "Drivebase"),   // front right
+        new ctre::phoenix6::hardware::TalonFX(5, "Drivebase"),   // back left
+        new ctre::phoenix6::hardware::TalonFX(3, "Drivebase")};  // back right
 
     wpi::array<wom::SwerveModuleConfig, 4> moduleConfigs{
-        wom::SwerveModuleConfig{
+        wom::SwerveModuleConfig{ //CORRECT
             // front left module
             frc::Translation2d(10.761_in, 9.455_in),
-            wom::Gearbox{driveMotors[0], new wom::TalonFXEncoder(driveMotors[0], 0.0445_m, 6.75),
-                         frc::DCMotor::Falcon500(1).WithReduction(6.75)},
-            wom::Gearbox{turnMotors[0], new wom::CanEncoder(19, 0.0445_m, 4096, 12.8),
+            wom::Gearbox{
+                driveMotors[0],
+                new wom::TalonFXEncoder(driveMotors[0], 0.0445_m, 6.75),
+                frc::DCMotor::Falcon500(1).WithReduction(6.75)},
+            wom::Gearbox{turnMotors[0],
+                         new wom::CanEncoder(18, 0.0445_m, 4096, 12.8),
                          frc::DCMotor::Falcon500(1).WithReduction(12.8)},
             &frontLeftCancoder, 4_in / 2},
-        wom::SwerveModuleConfig{
+        wom::SwerveModuleConfig{ //CORRECT
             // front right module
             frc::Translation2d(10.761_in, -9.455_in),
-            wom::Gearbox{driveMotors[1], new wom::TalonFXEncoder(driveMotors[1], 0.0445_m, 6.75),
-                         frc::DCMotor::Falcon500(1).WithReduction(6.75)},
-            wom::Gearbox{turnMotors[1], new wom::CanEncoder(17, 0.0445_m, 4096, 12.8),
+            wom::Gearbox{
+                driveMotors[1],
+                new wom::TalonFXEncoder(driveMotors[1], 0.0445_m, 6.75),
+                frc::DCMotor::Falcon500(1).WithReduction(6.75)},
+            wom::Gearbox{turnMotors[1],
+                         new wom::CanEncoder(19, 0.0445_m, 4096, 12.8),
                          frc::DCMotor::Falcon500(1).WithReduction(12.8)},
             &frontRightCancoder, 4_in / 2},
         wom::SwerveModuleConfig{
             // back left module
             frc::Translation2d(-10.761_in, 9.455_in),
-            wom::Gearbox{driveMotors[2], new wom::TalonFXEncoder(driveMotors[2], 0.0445_m, 6.75),
-                         frc::DCMotor::Falcon500(1).WithReduction(6.75)},
-            wom::Gearbox{turnMotors[2], new wom::CanEncoder(16, 0.0445_m, 4096, 12.8),
+            wom::Gearbox{
+                driveMotors[2],
+                new wom::TalonFXEncoder(driveMotors[2], 0.0445_m, 6.75),
+                frc::DCMotor::Falcon500(1).WithReduction(6.75)},
+            wom::Gearbox{turnMotors[2],
+                         new wom::CanEncoder(16, 0.0445_m, 4096, 12.8),
                          frc::DCMotor::Falcon500(1).WithReduction(12.8)},
             &backRightCancoder, 4_in / 2},
         wom::SwerveModuleConfig{
             // back right module
             frc::Translation2d(-10.761_in, -9.455_in),
-            wom::Gearbox{driveMotors[3], new wom::TalonFXEncoder(driveMotors[3], 0.0445_m, 6.75),
-                         frc::DCMotor::Falcon500(1).WithReduction(6.75)},
-            wom::Gearbox{turnMotors[3], new wom::CanEncoder(18, 0.0445_m, 4096, 12.8),
+            wom::Gearbox{
+                driveMotors[3],
+                new wom::TalonFXEncoder(driveMotors[3], 0.0445_m, 6.75),
+                frc::DCMotor::Falcon500(1).WithReduction(6.75)},
+            wom::Gearbox{turnMotors[3],
+                         new wom::CanEncoder(17, 0.0445_m, 4096, 12.8),
                          frc::DCMotor::Falcon500(1).WithReduction(12.8)},
             &backLeftCancoder, 4_in / 2},
     };
 
     // Setting the PID path and values to be used for SwerveDrive and
     // SwerveModules
-    wom::SwerveModule::angle_pid_conf_t anglePID{
-        "/drivetrain/pid/angle/config", 2_V / 360_deg, 0.0_V / (100_deg * 1_s),
-        0_V / (100_deg / 1_s),          1_deg,         0.5_deg / 2_s};
+    /*wom::SwerveModule::angle_pid_conf_t anglePID{
+        "/drivetrain/pid/angle/config", 90_V / 360_deg, 0.0_V / (100_deg * 1_s),
+        0_V / (100_deg / 1_s)};*/
     wom::SwerveModule::velocity_pid_conf_t velocityPID{
         "/drivetrain/pid/velocity/config",
         //  12_V / 4_mps // webers per metre
     };
-    wom::SwerveDriveConfig::pose_angle_conf_t poseAnglePID{
+    /*wom::SwerveDriveConfig::pose_angle_conf_t poseAnglePID{
         "/drivetrain/pid/pose/angle/config",
-        180_deg / 1_s / 45_deg,
-        wom::SwerveDriveConfig::pose_angle_conf_t::ki_t{0.1},
-        0_deg / 1_deg,
-        10_deg,
-        10_deg / 1_s};
+        0_deg / 1_s / 45_deg,
+        wom::SwerveDriveConfig::pose_angle_conf_t::ki_t{0},
+        0_deg / 1_deg};*/
     wom::SwerveDriveConfig::pose_position_conf_t posePositionPID{
         "/drivetrain/pid/pose/position/config",
-        3_mps / 1_m,
+        0_mps / 1_m,
         wom::SwerveDriveConfig::pose_position_conf_t::ki_t{0.15},
         0_m / 1_m,
-        20_cm,
-        10_cm / 1_s,
-        10_cm};
+        0_cm};
 
     // the config for the whole swerve drive
     wom::SwerveDriveConfig config{"/drivetrain",
-                                  anglePID,
+                                  //anglePID,
                                   velocityPID,
                                   moduleConfigs,  // each module
                                   gyro,
-                                  poseAnglePID,
+                                  //poseAnglePID,
                                   posePositionPID,
                                   60_kg,  // robot mass (estimate rn)
                                   {0.1, 0.1, 0.1},
@@ -129,4 +137,8 @@ struct RobotMap {
     //}
   };
   SwerveBase swerveBase;
-};
+
+  struct SwerveTable {
+    std::shared_ptr<nt::NetworkTable> swerveDriveTable = nt::NetworkTableInstance::GetDefault().GetTable("swerve");
+  }; SwerveTable swerveTable;
+};
\ No newline at end of file
diff --git a/wombat/LICENSE b/wombat/LICENSE
new file mode 100644
index 00000000..b1561bab
--- /dev/null
+++ b/wombat/LICENSE
@@ -0,0 +1,21 @@
+MIT License
+
+Copyright (c) 2023 CurtinFRC
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in all
+copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+SOFTWARE.
diff --git a/wombat/README.md b/wombat/README.md
new file mode 100644
index 00000000..0595c6cb
--- /dev/null
+++ b/wombat/README.md
@@ -0,0 +1 @@
+# Wombat
diff --git a/wombat/settings.gradle b/wombat/settings.gradle
new file mode 100644
index 00000000..8b137891
--- /dev/null
+++ b/wombat/settings.gradle
@@ -0,0 +1 @@
+
diff --git a/wombat/src/main/cpp/behaviour/Behaviour.cpp b/wombat/src/main/cpp/behaviour/Behaviour.cpp
index 7312c922..d21d9e57 100644
--- a/wombat/src/main/cpp/behaviour/Behaviour.cpp
+++ b/wombat/src/main/cpp/behaviour/Behaviour.cpp
@@ -10,7 +10,9 @@ using namespace behaviour;
 
 // Behaviour
 Behaviour::Behaviour(std::string name, units::time::second_t period)
-    : _bhvr_name(name), _bhvr_period(period), _bhvr_state(BehaviourState::INITIALISED) {}
+    : _bhvr_name(name),
+      _bhvr_period(period),
+      _bhvr_state(BehaviourState::INITIALISED) {}
 Behaviour::~Behaviour() {
   if (!IsFinished())
     Interrupt();
@@ -80,7 +82,8 @@ bool Behaviour::Tick() {
 
     if (dt > 2 * _bhvr_period) {
       std::cerr << "Behaviour missed deadline. Reduce Period. Dt=" << dt.value()
-                << " Dt(deadline)=" << (2 * _bhvr_period).value() << ". Bhvr: " << GetName() << std::endl;
+                << " Dt(deadline)=" << (2 * _bhvr_period).value()
+                << ". Bhvr: " << GetName() << std::endl;
     }
 
     if (_bhvr_timeout.value() > 0 && _bhvr_timer > _bhvr_timeout)
@@ -97,7 +100,8 @@ bool Behaviour::IsRunning() const {
 }
 
 bool Behaviour::IsFinished() const {
-  return _bhvr_state != BehaviourState::INITIALISED && _bhvr_state != BehaviourState::RUNNING;
+  return _bhvr_state != BehaviourState::INITIALISED &&
+         _bhvr_state != BehaviourState::RUNNING;
 }
 
 void Behaviour::Stop(BehaviourState new_state) {
@@ -169,7 +173,8 @@ void ConcurrentBehaviour::Add(Behaviour::ptr behaviour) {
 }
 
 std::string ConcurrentBehaviour::GetName() const {
-  std::string msg = (_reducer == ConcurrentBehaviourReducer::ALL ? "ALL { " : "RACE {");
+  std::string msg =
+      (_reducer == ConcurrentBehaviourReducer::ALL ? "ALL { " : "RACE {");
   for (auto b : _children)
     msg += b->GetName() + ", ";
   msg += "}";
@@ -185,8 +190,8 @@ void ConcurrentBehaviour::OnStart() {
         using namespace std::chrono_literals;
 
         b->Tick();
-        std::this_thread::sleep_for(
-            std::chrono::milliseconds(static_cast<int64_t>(b->GetPeriod().value() * 1000)));
+        std::this_thread::sleep_for(std::chrono::milliseconds(
+            static_cast<int64_t>(b->GetPeriod().value() * 1000)));
       }
 
       if (IsFinished() && !b->IsFinished())
@@ -268,8 +273,10 @@ void WaitFor::OnTick(units::time::second_t dt) {
 }
 
 // WaitTime
-WaitTime::WaitTime(units::time::second_t time) : WaitTime([time]() { return time; }) {}
-WaitTime::WaitTime(std::function<units::time::second_t()> time_fn) : _time_fn(time_fn) {}
+WaitTime::WaitTime(units::time::second_t time)
+    : WaitTime([time]() { return time; }) {}
+WaitTime::WaitTime(std::function<units::time::second_t()> time_fn)
+    : _time_fn(time_fn) {}
 
 void WaitTime::OnStart() {
   _time = _time_fn();
diff --git a/wombat/src/main/cpp/behaviour/BehaviourScheduler.cpp b/wombat/src/main/cpp/behaviour/BehaviourScheduler.cpp
index d34bf4aa..26863c79 100644
--- a/wombat/src/main/cpp/behaviour/BehaviourScheduler.cpp
+++ b/wombat/src/main/cpp/behaviour/BehaviourScheduler.cpp
@@ -10,7 +10,7 @@ BehaviourScheduler::BehaviourScheduler() {}
 
 BehaviourScheduler::~BehaviourScheduler() {
   for (HasBehaviour* sys : _systems) {
-    if (sys->_active_behaviour)
+    if (sys->_active_behaviour != nullptr)
       sys->_active_behaviour->Interrupt();
   }
 
@@ -52,8 +52,8 @@ void BehaviourScheduler::Schedule(Behaviour::ptr behaviour) {
         std::lock_guard<std::recursive_mutex> lk(_active_mtx);
         behaviour->Tick();
       }
-      std::this_thread::sleep_for(
-          std::chrono::milliseconds(static_cast<int64_t>(behaviour->GetPeriod().value() * 1000)));
+      std::this_thread::sleep_for(std::chrono::milliseconds(
+          static_cast<int64_t>(behaviour->GetPeriod().value() * 1000)));
     }
   });
 }
@@ -78,7 +78,8 @@ void BehaviourScheduler::Tick() {
 void BehaviourScheduler::InterruptAll() {
   std::lock_guard<std::recursive_mutex> lk(_active_mtx);
   for (HasBehaviour* sys : _systems) {
-    if (sys->_active_behaviour)
+    if (sys->_active_behaviour != nullptr) {
       sys->_active_behaviour->Interrupt();
+    }
   }
 }
diff --git a/wombat/src/main/cpp/behaviour/HasBehaviour.cpp b/wombat/src/main/cpp/behaviour/HasBehaviour.cpp
index f2f557c5..964522c6 100644
--- a/wombat/src/main/cpp/behaviour/HasBehaviour.cpp
+++ b/wombat/src/main/cpp/behaviour/HasBehaviour.cpp
@@ -8,7 +8,8 @@
 
 using namespace behaviour;
 
-void HasBehaviour::SetDefaultBehaviour(std::function<std::shared_ptr<Behaviour>(void)> fn) {
+void HasBehaviour::SetDefaultBehaviour(
+    std::function<std::shared_ptr<Behaviour>(void)> fn) {
   _default_behaviour_producer = fn;
 }
 
diff --git a/wombat/src/main/cpp/drivetrain/Drivetrain.cpp b/wombat/src/main/cpp/drivetrain/Drivetrain.cpp
index 4a5cb744..e516e7d7 100644
--- a/wombat/src/main/cpp/drivetrain/Drivetrain.cpp
+++ b/wombat/src/main/cpp/drivetrain/Drivetrain.cpp
@@ -11,7 +11,8 @@
 using namespace frc;
 using namespace units;
 
-wom::drivetrain::Drivetrain::Drivetrain(DrivetrainConfig* config, XboxController& driver)
+wom::drivetrain::Drivetrain::Drivetrain(DrivetrainConfig* config,
+                                        XboxController& driver)
     : _config(config), _driver(driver) {}
 wom::drivetrain::Drivetrain::~Drivetrain() {}
 
@@ -37,12 +38,12 @@ void wom::drivetrain::Drivetrain::OnUpdate(second_t dt) {
     case DrivetrainState::kTank: {
       double rightSpeed = wom::utils::deadzone(_driver.GetRightY());
       double leftSpeed = wom::utils::deadzone(_driver.GetLeftY());
-      _config->left1.motorController->SetVoltage(leftSpeed * maxVolts);
-      _config->left2.motorController->SetVoltage(leftSpeed * maxVolts);
-      _config->left3.motorController->SetVoltage(leftSpeed * maxVolts);
-      _config->right1.motorController->SetVoltage(rightSpeed * maxVolts);
-      _config->right2.motorController->SetVoltage(rightSpeed * maxVolts);
-      _config->right3.motorController->SetVoltage(rightSpeed * maxVolts);
+      // _config->left1.motorController->SetVoltage(leftSpeed * maxVolts);
+      // _config->left2.motorController->SetVoltage(leftSpeed * maxVolts);
+      // _config->left3.motorController->SetVoltage(leftSpeed * maxVolts);
+      // _config->right1.motorController->SetVoltage(rightSpeed * maxVolts);
+      // _config->right2.motorController->SetVoltage(rightSpeed * maxVolts);
+      // _config->right3.motorController->SetVoltage(rightSpeed * maxVolts);
       break;
     }
     case DrivetrainState::kAuto:
diff --git a/wombat/src/main/cpp/drivetrain/SwerveDrive.cpp b/wombat/src/main/cpp/drivetrain/SwerveDrive.cpp
index 7af1ff39..eb33efb1 100644
--- a/wombat/src/main/cpp/drivetrain/SwerveDrive.cpp
+++ b/wombat/src/main/cpp/drivetrain/SwerveDrive.cpp
@@ -6,6 +6,8 @@
 
 #include <networktables/NetworkTableInstance.h>
 #include <units/math.h>
+#include <units/voltage.h>
+#include <units/angle.h>
 
 #include <iostream>
 
@@ -14,24 +16,28 @@
 namespace wom {
 namespace drivetrain {
 
-void SwerveModuleConfig::WriteNT(std::shared_ptr<nt::NetworkTable> table) const {
+void SwerveModuleConfig::WriteNT(
+    std::shared_ptr<nt::NetworkTable> table) const {
   std::array<double, 2> pos{position.X().value(), position.Y().value()};
   table->GetEntry("position").SetDoubleArray(std::span(pos));
   table->GetEntry("wheelRadius").SetDouble(wheelRadius.value());
 }
 
 SwerveModule::SwerveModule(std::string path, SwerveModuleConfig config,
-                           SwerveModule::angle_pid_conf_t anglePID,
+                           //SwerveModule::angle_pid_conf_t anglePID,
                            SwerveModule::velocity_pid_conf_t velocityPID)
-    : _config(config),
-      _anglePIDController(path + "/pid/angle", anglePID),
-      _velocityPIDController(path + "/pid/velocity", velocityPID),
+    : //_anglePIDController(path + "/pid/angle", anglePID),
+      _anglePIDController{frc::PIDController(4, 0, 0, 0.005_s)},
+      _config(config),
+      _velocityPIDController(frc::PIDController(1.2, 0, 0, 0.005_s)),
       _table(nt::NetworkTableInstance::GetDefault().GetTable(path)) {
-  _anglePIDController.SetWrap(360_deg);
+  // _anglePIDController.(2 * 3.1415);
+  _anglePIDController.EnableContinuousInput(0, (2 * 3.1415));
 }
 
 void SwerveModule::OnStart() {
   // _offset = offset;
+  _config.canEncoder->SetPosition(units::turn_t{0});
   _anglePIDController.Reset();
   _velocityPIDController.Reset();
 }
@@ -40,6 +46,8 @@ void SwerveModule::OnUpdate(units::second_t dt) {
   units::volt_t driveVoltage{0};
   units::volt_t turnVoltage{0};
 
+
+
   switch (_state) {
     case SwerveModuleState::kIdle:
       driveVoltage = 0_V;
@@ -47,38 +55,60 @@ void SwerveModule::OnUpdate(units::second_t dt) {
       break;
     case SwerveModuleState::kPID: {
       auto feedforward = _config.driveMotor.motor.Voltage(
-          0_Nm,
-          units::radians_per_second_t{(_velocityPIDController.GetSetpoint() / _config.wheelRadius).value()});
-      driveVoltage = _velocityPIDController.Calculate(GetSpeed(), dt, feedforward);
-      turnVoltage = _anglePIDController.Calculate(_config.turnMotor.encoder->GetEncoderPosition(), dt);
+          0_Nm, units::radians_per_second_t{
+                    _velocityPIDController.GetSetpoint() / _config.wheelRadius.value()});
+      //std::cout << "GetSpeed()" << GetSpeed().value() << std::endl;
+      // units::radian_t input = units::math::fmod(_config.turnMotor.encoder->GetEncoderPosition(), (2_rad * 3.1415));
+      double input = -_config.turnMotor.encoder->GetEncoderPosition().value();
+      _table->GetEntry("/testing/GetEncoderPos").SetDouble(input);
+      // _velocityPIDController.SetSetpoint(3);
+      driveVoltage = units::volt_t{_velocityPIDController.Calculate(GetSpeed().value())};
+      std::cout << "Turn angle input: " << input << std::endl;
+      turnVoltage = units::volt_t{_anglePIDController.Calculate(input)};
+
+      _table->GetEntry("Input angle").SetDouble(input);
+      _table->GetEntry("Setpoint angle").SetDouble(_anglePIDController.GetSetpoint());
     } break;
+    case wom::drivetrain::SwerveModuleState::kZeroing: {
+    } break;
+    default:
+      std::cerr << "Case not handled" << std::endl;
   }
 
-  units::newton_meter_t torqueLimit = 50_kg / 4 * _config.wheelRadius * _currentAccelerationLimit;
-  units::volt_t voltageMax =
-      _config.driveMotor.motor.Voltage(torqueLimit, _config.driveMotor.encoder->GetEncoderAngularVelocity());
-  units::volt_t voltageMin =
-      _config.driveMotor.motor.Voltage(-torqueLimit, _config.driveMotor.encoder->GetEncoderAngularVelocity());
+  units::newton_meter_t torqueLimit =
+      50_kg / 4 * _config.wheelRadius * _currentAccelerationLimit;
+  // units::volt_t voltageMax = _config.driveMotor.motor.Voltage(
+  //     torqueLimit, _config.driveMotor.encoder->GetEncoderAngularVelocity());
+  // units::volt_t voltageMin = _config.driveMotor.motor.Voltage(
+  //     -torqueLimit, _config.driveMotor.encoder->GetEncoderAngularVelocity());
 
-  driveVoltage = units::math::max(units::math::min(driveVoltage, voltageMax), voltageMin);
+  // driveVoltage =
+  //     units::math::max(units::math::min(driveVoltage, voltageMax), voltageMin);
 
   // driveVoltage = units::math::min(driveVoltage, 10_V);
   turnVoltage = units::math::min(turnVoltage, 7_V);
 
-  driveVoltage = units::math::min(units::math::max(driveVoltage, -_driveModuleVoltageLimit),
-                                  _driveModuleVoltageLimit);  // was originally 10_V
-  std::cout << "drive-voltage: " << driveVoltage.value() << std::endl;
+  // driveVoltage = units::math::min(
+  //     units::math::max(driveVoltage, -_driveModuleVoltageLimit),
+  //     _driveModuleVoltageLimit);  // was originally 10_V
   units::volt_t turnVoltageMax = 7_V - (driveVoltage * (7_V / 10_V));
-  turnVoltage = units::math::min(units::math::max(turnVoltage, -turnVoltageMax), turnVoltageMax);
+  turnVoltage = units::math::min(units::math::max(turnVoltage, -turnVoltageMax),
+                                 turnVoltageMax);
   // turnVoltage = units::math::min(units::math::max(turnVoltage, -7_V), 7_V);
   // std::cout << "turn-voltage-max: " << turnVoltageMax.value() << std::endl;
 
+  _table->GetEntry("TurnVoltage").SetDouble(turnVoltage.value());
+  _table->GetEntry("TurnSetpoint").SetDouble(_anglePIDController.GetSetpoint());
+  _table->GetEntry("Error").SetDouble(_anglePIDController.GetPositionError());
+
   _config.driveMotor.motorController->SetVoltage(driveVoltage);
   _config.turnMotor.motorController->SetVoltage(turnVoltage);
 
   _table->GetEntry("speed").SetDouble(GetSpeed().value());
   _table->GetEntry("angle").SetDouble(
-      _config.turnMotor.encoder->GetEncoderPosition().convert<units::degree>().value());
+      _config.turnMotor.encoder->GetEncoderPosition()
+          .convert<units::degree>()
+          .value());
   _config.WriteNT(_table->GetSubTable("config"));
 }
 
@@ -86,11 +116,13 @@ void SwerveModule::OnUpdate(units::second_t dt) {
 //   return (_config.turnMotor.encoder->GetEncoderPosition().value());
 // }
 
-void SwerveModule::SetAccelerationLimit(units::meters_per_second_squared_t limit) {
+void SwerveModule::SetAccelerationLimit(
+    units::meters_per_second_squared_t limit) {
   _currentAccelerationLimit = limit;
 }
 
-void SwerveDrive::SetAccelerationLimit(units::meters_per_second_squared_t limit) {
+void SwerveDrive::SetAccelerationLimit(
+    units::meters_per_second_squared_t limit) {
   for (int motorNumber = 0; motorNumber < 4; motorNumber++) {
     _modules[motorNumber].SetAccelerationLimit(limit);
   }
@@ -109,46 +141,63 @@ void SwerveModule::SetZero(units::second_t dt) {
   _state = SwerveModuleState::kPID;
 }
 
-void SwerveModule::SetPID(units::radian_t angle, units::meters_per_second_t speed, units::second_t dt) {
+void SwerveModule::SetPID(units::radian_t angle,
+                          units::meters_per_second_t speed,
+                          units::second_t dt) {
   _state = SwerveModuleState::kPID;
   // @liam start added
-  double diff = std::fmod((_anglePIDController.GetSetpoint() - angle).convert<units::degree>().value(), 360);
-  if (std::abs(diff) >= 90) {
+  // std::cout << "angle Setpoint: " << _anglePIDController.GetSetpoint() << std::endl;
+  // std::cout << "angle value: " << angle.value() << std::endl;
+
+  double diff = std::fmod((_anglePIDController.GetSetpoint() - angle.value()),
+                          (2 * 3.1415));
+  std::cout << "DIFF value: " << diff << std::endl;
+  _table->GetEntry("/Differential value:").SetDouble(diff);
+  if (std::abs(diff) >= 3.1415) {
     speed *= -1;
-    angle += 180_deg;
+    angle -= 3.1415_rad;
   }
   // @liam end added
 
-  _anglePIDController.SetSetpoint(angle);
-  _velocityPIDController.SetSetpoint(speed);
+  _anglePIDController.SetSetpoint(angle.value());
+  _velocityPIDController.SetSetpoint(speed.value());
 }
 
 void SwerveModule::ModuleVectorHandler(frc::ChassisSpeeds speeds) {
   units::meters_per_second_t xVelocityComponent =
-      1_mps * (speeds.vx.value() + speeds.omega.value() * _config.position.X().value());
+      1_mps *
+      (speeds.vx.value() + speeds.omega.value() * _config.position.X().value());
   units::meters_per_second_t yVelocityComponent =
-      1_mps * (speeds.vy.value() + speeds.omega.value() * _config.position.Y().value());
+      1_mps *
+      (speeds.vy.value() + speeds.omega.value() * _config.position.Y().value());
 
   units::meters_per_second_t velocity =
-      1_mps * std::sqrt(std::pow(xVelocityComponent.value(), 2) + std::pow(yVelocityComponent.value(), 2));
-  units::degree_t angle = 1_rad * std::atan2(yVelocityComponent.value(), xVelocityComponent.value());
+      1_mps * std::sqrt(std::pow(xVelocityComponent.value(), 2) +
+                        std::pow(yVelocityComponent.value(), 2));
+  units::degree_t angle = 1_rad * std::atan2(yVelocityComponent.value(),
+                                             xVelocityComponent.value());
 
-  _anglePIDController.SetSetpoint(angle);
-  _velocityPIDController.SetSetpoint(velocity);
+  _anglePIDController.SetSetpoint(angle.value());
+  _velocityPIDController.SetSetpoint(velocity.value());
 }
 
 units::meters_per_second_t SwerveModule::GetSpeed() const {
-  return units::meters_per_second_t{_config.driveMotor.encoder->GetEncoderAngularVelocity().value() *
-                                    _config.wheelRadius.value()};
+  units::meters_per_second_t returnVal{
+    _config.driveMotor.encoder->GetVelocityValue() *
+    _config.wheelRadius.value()};
+    // std::cout << returnVal.value() << std::endl;
+    return returnVal;
 }
 
 units::meter_t SwerveModule::GetDistance() const {
-  return units::meter_t{_config.driveMotor.encoder->GetEncoderPosition().value() *
-                        _config.wheelRadius.value()};
+  return units::meter_t{
+      _config.driveMotor.encoder->GetEncoderPosition().value() *
+      _config.wheelRadius.value()};
 }
 
 frc::SwerveModulePosition SwerveModule::GetPosition() const {
-  return frc::SwerveModulePosition{GetDistance(), _config.turnMotor.encoder->GetEncoderPosition()};
+  return frc::SwerveModulePosition{
+      GetDistance(), _config.turnMotor.encoder->GetEncoderPosition()};
 }
 
 const SwerveModuleConfig& SwerveModule::GetConfig() const {
@@ -161,36 +210,40 @@ void SwerveDriveConfig::WriteNT(std::shared_ptr<nt::NetworkTable> table) {
 
 SwerveDrive::SwerveDrive(SwerveDriveConfig config, frc::Pose2d initialPose)
     : _config(config),
-      _kinematics(_config.modules[0].position, _config.modules[1].position, _config.modules[2].position,
-                  _config.modules[3].position),
+      _kinematics(_config.modules[0].position, _config.modules[1].position,
+                  _config.modules[2].position, _config.modules[3].position),
       _poseEstimator(
           _kinematics, frc::Rotation2d(0_deg),
-          wpi::array<frc::SwerveModulePosition, 4>{frc::SwerveModulePosition{0_m, frc::Rotation2d{0_deg}},
-                                                   frc::SwerveModulePosition{0_m, frc::Rotation2d{0_deg}},
-                                                   frc::SwerveModulePosition{0_m, frc::Rotation2d{0_deg}},
-                                                   frc::SwerveModulePosition{0_m, frc::Rotation2d{0_deg}}},
+          wpi::array<frc::SwerveModulePosition, 4>{
+              frc::SwerveModulePosition{0_m, frc::Rotation2d{0_deg}},
+              frc::SwerveModulePosition{0_m, frc::Rotation2d{0_deg}},
+              frc::SwerveModulePosition{0_m, frc::Rotation2d{0_deg}},
+              frc::SwerveModulePosition{0_m, frc::Rotation2d{0_deg}}},
           initialPose, _config.stateStdDevs, _config.visionMeasurementStdDevs),
-      _anglePIDController(config.path + "/pid/heading", _config.poseAnglePID),
+      _anglePIDController{frc::PIDController(1, 0, 0)},
       _xPIDController(config.path + "/pid/x", _config.posePositionPID),
       _yPIDController(config.path + "/pid/y", _config.posePositionPID),
       _table(nt::NetworkTableInstance::GetDefault().GetTable(_config.path)) {
-  _anglePIDController.SetWrap(360_deg);
+  _anglePIDController.SetTolerance(360);
 
   int i = 1;
   for (auto cfg : _config.modules) {
-    _modules.emplace_back(config.path + "/modules/" + std::to_string(i), cfg, config.anglePID,
-                          config.velocityPID);
+    _modules.emplace_back(config.path + "/modules/" + std::to_string(i), cfg,
+                          /*config.anglePID,*/ config.velocityPID);
     i++;
   }
 
   ResetPose(initialPose);
 }
 
-frc::ChassisSpeeds FieldRelativeSpeeds::ToChassisSpeeds(const units::radian_t robotHeading) {
-  return frc::ChassisSpeeds::FromFieldRelativeSpeeds(vx, vy, omega, frc::Rotation2d{robotHeading});
+frc::ChassisSpeeds FieldRelativeSpeeds::ToChassisSpeeds(
+    const units::radian_t robotHeading) {
+  return frc::ChassisSpeeds::FromFieldRelativeSpeeds(
+      vx, vy, omega, frc::Rotation2d{robotHeading});
 }
 
 void SwerveDrive::OnUpdate(units::second_t dt) {
+  //std::cout << _config.gyro->GetYaw().GetValue().value() << std::endl;
   switch (_state) {
     case SwerveDriveState::kZeroing:
       for (auto mod = _modules.begin(); mod < _modules.end(); mod++) {
@@ -205,13 +258,14 @@ void SwerveDrive::OnUpdate(units::second_t dt) {
     case SwerveDriveState::kPose: {
       _target_fr_speeds.vx = _xPIDController.Calculate(GetPose().X(), dt);
       _target_fr_speeds.vy = _yPIDController.Calculate(GetPose().Y(), dt);
-      _target_fr_speeds.omega = _anglePIDController.Calculate(GetPose().Rotation().Radians(), dt);
+      _target_fr_speeds.omega = units::radians_per_second_t{_anglePIDController.Calculate(GetPose().Rotation().Radians().value())};
     }
       [[fallthrough]];
     case SwerveDriveState::kFieldRelativeVelocity:
-      _target_speed = _target_fr_speeds.ToChassisSpeeds(GetPose().Rotation().Radians());
+      _target_speed =
+          _target_fr_speeds.ToChassisSpeeds(GetPose().Rotation().Radians());
       if (isRotateToMatchJoystick) {
-        _target_speed.omega = _anglePIDController.Calculate(GetPose().Rotation().Radians(), dt);
+        _target_speed.omega = units::radians_per_second_t{_anglePIDController.Calculate(GetPose().Rotation().Radians().value())};
       }
       // std::cout << "vx = " << _target_speed.vx.value() << " vy = " <<
       // _target_fr_speeds.vy.value() << std::endl;
@@ -219,7 +273,8 @@ void SwerveDrive::OnUpdate(units::second_t dt) {
     case SwerveDriveState::kVelocity: {
       auto target_states = _kinematics.ToSwerveModuleStates(_target_speed);
       for (size_t i = 0; i < _modules.size(); i++) {
-        _modules[i].SetPID(target_states[i].angle.Radians(), target_states[i].speed, dt);
+        _modules[i].SetPID(target_states[i].angle.Radians(),
+                           target_states[i].speed, dt);
         // std::cout << "module " << i << ": " <<
         // target_states[i].angle.Radians().value() << std::endl;
       }
@@ -242,12 +297,13 @@ void SwerveDrive::OnUpdate(units::second_t dt) {
     case SwerveDriveState::kFRVelocityRotationLock:
       _target_speed.vx = _xPIDController.Calculate(GetPose().X(), dt);
       _target_speed.vy = _yPIDController.Calculate(GetPose().Y(), dt);
-      _target_speed.omega = _anglePIDController.Calculate(GetPose().Rotation().Radians(), dt);
-      _target_speed = _requestedSpeeds.ToChassisSpeeds(GetPose().Rotation().Radians());
+      _target_speed.omega = units::radians_per_second_t{_anglePIDController.Calculate(GetPose().Rotation().Radians().value())};
+      _target_speed =
+          _requestedSpeeds.ToChassisSpeeds(GetPose().Rotation().Radians());
       auto target_states = _kinematics.ToSwerveModuleStates(_target_speed);
       for (size_t i = 0; i < _modules.size(); i++) {
-        std::cout << "Speeds :" << target_states[i].speed.value() << std::endl;
-        _modules[i].SetPID(target_states[i].angle.Radians(), target_states[i].speed, dt);
+        _modules[i].SetPID(target_states[i].angle.Radians(),
+                           target_states[i].speed, dt);
       }
       break;
   }
@@ -258,10 +314,12 @@ void SwerveDrive::OnUpdate(units::second_t dt) {
 
   _poseEstimator.Update(
       _config.gyro->GetRotation2d(),
-      wpi::array<frc::SwerveModulePosition, 4>{_modules[0].GetPosition(), _modules[1].GetPosition(),
-                                               _modules[2].GetPosition(), _modules[3].GetPosition()});
+      wpi::array<frc::SwerveModulePosition, 4>{
+          _modules[0].GetPosition(), _modules[1].GetPosition(),
+          _modules[2].GetPosition(), _modules[3].GetPosition()});
 
-  utils::WritePose2NT(_table->GetSubTable("estimatedPose"), _poseEstimator.GetEstimatedPosition());
+  utils::WritePose2NT(_table->GetSubTable("estimatedPose"),
+                      _poseEstimator.GetEstimatedPosition());
   _config.WriteNT(_table->GetSubTable("config"));
 }
 
@@ -298,16 +356,16 @@ void SwerveDrive::OnResetMode() {
   _xPIDController.Reset();
   _yPIDController.Reset();
   _anglePIDController.Reset();
-  std::cout << "reset" << std::endl;
 }
 
-void SwerveDrive::RotateMatchJoystick(units::radian_t joystickAngle, FieldRelativeSpeeds speeds) {
+void SwerveDrive::RotateMatchJoystick(units::radian_t joystickAngle,
+                                      FieldRelativeSpeeds speeds) {
   // _state = SwerveDriveState::kFRVelocityRotationLock;
   // _anglePIDController.SetSetpoint(joystickAngle);
   // _target_fr_speeds = speeds;
   _state = SwerveDriveState::kFieldRelativeVelocity;
   isRotateToMatchJoystick = true;
-  _anglePIDController.SetSetpoint(joystickAngle);
+  _anglePIDController.SetSetpoint(joystickAngle.value());
   _target_fr_speeds = speeds;
 }
 
@@ -327,14 +385,16 @@ bool SwerveDrive::GetIsFieldRelative() {
   return _isFieldRelative;
 }
 
-void SwerveDrive::SetIndividualTuning(int mod, units::radian_t angle, units::meters_per_second_t speed) {
+void SwerveDrive::SetIndividualTuning(int mod, units::radian_t angle,
+                                      units::meters_per_second_t speed) {
   _mod = mod;
   _angle = angle;
   _speed = speed;
   _state = SwerveDriveState::kIndividualTuning;
 }
 
-void SwerveDrive::SetTuning(units::radian_t angle, units::meters_per_second_t speed) {
+void SwerveDrive::SetTuning(units::radian_t angle,
+                            units::meters_per_second_t speed) {
   _angle = angle;
   _speed = speed;
   _state = SwerveDriveState::kTuning;
@@ -348,20 +408,22 @@ void SwerveDrive::SetFieldRelativeVelocity(FieldRelativeSpeeds speeds) {
 
 void SwerveDrive::SetPose(frc::Pose2d pose) {
   _state = SwerveDriveState::kPose;
-  _anglePIDController.SetSetpoint(pose.Rotation().Radians());
+  _anglePIDController.SetSetpoint(pose.Rotation().Radians().value());
   _xPIDController.SetSetpoint(pose.X());
   _yPIDController.SetSetpoint(pose.Y());
 }
 
 bool SwerveDrive::IsAtSetPose() {
-  return _anglePIDController.IsStable() && _xPIDController.IsStable() && _yPIDController.IsStable(0.05_m);
+  return /*_anglePIDController.IsStable()*/ true && _xPIDController.IsStable() &&
+         _yPIDController.IsStable(0.05_m);
 }
 
 void SwerveDrive::ResetPose(frc::Pose2d pose) {
   _poseEstimator.ResetPosition(
       _config.gyro->GetRotation2d(),
-      wpi::array<frc::SwerveModulePosition, 4>{_modules[0].GetPosition(), _modules[1].GetPosition(),
-                                               _modules[2].GetPosition(), _modules[3].GetPosition()},
+      wpi::array<frc::SwerveModulePosition, 4>{
+          _modules[0].GetPosition(), _modules[1].GetPosition(),
+          _modules[2].GetPosition(), _modules[3].GetPosition()},
       pose);
 }
 
@@ -369,7 +431,8 @@ frc::Pose2d SwerveDrive::GetPose() {
   return _poseEstimator.GetEstimatedPosition();
 }
 
-void SwerveDrive::AddVisionMeasurement(frc::Pose2d pose, units::second_t timestamp) {
+void SwerveDrive::AddVisionMeasurement(frc::Pose2d pose,
+                                       units::second_t timestamp) {
   _poseEstimator.AddVisionMeasurement(pose, timestamp);
 }
 }  // namespace drivetrain
diff --git a/wombat/src/main/cpp/drivetrain/behaviours/SwerveBehaviours.cpp b/wombat/src/main/cpp/drivetrain/behaviours/SwerveBehaviours.cpp
index 18d88f88..a82c84d8 100644
--- a/wombat/src/main/cpp/drivetrain/behaviours/SwerveBehaviours.cpp
+++ b/wombat/src/main/cpp/drivetrain/behaviours/SwerveBehaviours.cpp
@@ -29,61 +29,58 @@ ManualDrivebase::ManualDrivebase(wom::drivetrain::SwerveDrive* swerveDrivebase,
 void ManualDrivebase::OnStart() {
   _swerveDrivebase->OnStart();
   _swerveDrivebase->SetAccelerationLimit(6_mps_sq);
-  std::cout << "Manual Drivebase Start" << std::endl;
 }
 
 void ManualDrivebase::OnTick(units::second_t deltaTime) {
-  if (_driverController->GetXButtonPressed()) {
-    ResetMode();
-    if (isRotateMatch) {
-      isRotateMatch = false;
-    } else {
-      isRotateMatch = true;
-    }
-  }
-
-  if (_driverController->GetYButton()) {
-    std::cout << "RESETING POSE" << std::endl;
-    _swerveDrivebase->ResetPose(frc::Pose2d());
-  }
-
-  /*   HOLD SOLUTION   */
-  if (_driverController->GetLeftBumperPressed()) {
-    maxMovementMagnitude = lowSensitivityDriveSpeed;
-    maxRotationMagnitude = lowSensitivityRotateSpeed;
-  } else if (_driverController->GetLeftBumperReleased() && !_driverController->GetRightBumper()) {
-    maxMovementMagnitude = defaultDriveSpeed;
-    maxRotationMagnitude = defaultRotateSpeed;
-    _swerveDrivebase->SetAccelerationLimit(6_mps_sq);
-    _swerveDrivebase->SetVoltageLimit(10_V);
-  }
-  if (_driverController->GetRightBumperPressed()) {
-    maxMovementMagnitude = highSensitivityDriveSpeed;
-    maxRotationMagnitude = highSensitivityRotateSpeed;
-    _swerveDrivebase->SetAccelerationLimit(12_mps_sq);
-    _swerveDrivebase->SetVoltageLimit(14_V);
-
-  } else if (_driverController->GetRightBumperReleased() && !_driverController->GetLeftBumper()) {
-    maxMovementMagnitude = defaultDriveSpeed;
-    maxRotationMagnitude = defaultRotateSpeed;
-    _swerveDrivebase->SetAccelerationLimit(6_mps_sq);
-    _swerveDrivebase->SetVoltageLimit(10_V);
-  }
-
-  if (_driverController->GetAButtonReleased()) {
-    isZero = !isZero;
-  }
-
-  if (isZero) {
-    _swerveDrivebase->SetZeroing();
-  } else {
-    double xVelocity = wom::utils::spow2(
-        -wom::utils::deadzone(_driverController->GetLeftY(),
-                              driverDeadzone));  // GetLeftY due to x being where y should be on field
-    double yVelocity =
-        wom::utils::spow2(-wom::utils::deadzone(_driverController->GetLeftX(), driverDeadzone));
-
-    double r_x = wom::utils::spow2(-wom::utils::deadzone(_driverController->GetRightX(), turningDeadzone));
+  // if (_driverController->GetXButtonPressed()) {
+  //   ResetMode();
+  //   isRotateMatch = !isRotateMatch;
+  // }
+
+  // if (_driverController->GetYButton()) {
+  //   std::cout << "RESETING POSE" << std::endl;
+  //   _swerveDrivebase->ResetPose(frc::Pose2d());
+  // }
+
+  // if (_driverController->GetLeftBumperPressed()) {
+  //   maxMovementMagnitude = lowSensitivityDriveSpeed;
+  //   maxRotationMagnitude = lowSensitivityRotateSpeed;
+  // } else if (_driverController->GetLeftBumperReleased() &&
+  //            !_driverController->GetRightBumper()) {
+  //   maxMovementMagnitude = defaultDriveSpeed;
+  //   maxRotationMagnitude = defaultRotateSpeed;
+  //   _swerveDrivebase->SetAccelerationLimit(6_mps_sq);
+  //   _swerveDrivebase->SetVoltageLimit(10_V);
+  // }
+  // if (_driverController->GetRightBumperPressed()) {
+  //   maxMovementMagnitude = highSensitivityDriveSpeed;
+  //   maxRotationMagnitude = highSensitivityRotateSpeed;
+  //   _swerveDrivebase->SetAccelerationLimit(12_mps_sq);
+  //   _swerveDrivebase->SetVoltageLimit(14_V);
+
+  // } else if (_driverController->GetRightBumperReleased() &&
+  //            !_driverController->GetLeftBumper()) {
+  //   maxMovementMagnitude = defaultDriveSpeed;
+  //   maxRotationMagnitude = defaultRotateSpeed;
+  //   _swerveDrivebase->SetAccelerationLimit(6_mps_sq);
+  //   _swerveDrivebase->SetVoltageLimit(10_V);
+  // }
+
+  // if (_driverController->GetAButtonReleased()) {
+  //   isZero = !isZero;
+  // }
+
+  // if (isZero) {
+  //   _swerveDrivebase->SetZeroing();
+  // } else {
+    double xVelocity = wom::utils::spow2(-wom::utils::deadzone(
+        _driverController->GetLeftY(),
+        driverDeadzone));  // GetLeftY due to x being where y should be on field
+    double yVelocity = wom::utils::spow2(
+        -wom::utils::deadzone(_driverController->GetLeftX(), driverDeadzone));
+
+    double r_x = wom::utils::spow2(
+        -wom::utils::deadzone(_driverController->GetRightX(), turningDeadzone));
 
     double turnX = _driverController->GetRightX();
     double turnY = _driverController->GetRightY();
@@ -93,20 +90,26 @@ void ManualDrivebase::OnTick(units::second_t deltaTime) {
       turnY = 0;
     }
 
-    if (isRotateMatch) {
-      units::degree_t currentAngle = _swerveDrivebase->GetPose().Rotation().Degrees();
-      CalculateRequestedAngle(turnX, turnY, currentAngle);
-      _swerveDriveTable->GetEntry("RotateMatch").SetDouble(_requestedAngle.value());
-      _swerveDrivebase->RotateMatchJoystick(
-          _requestedAngle,
-          wom::drivetrain::FieldRelativeSpeeds{// also field relative
-                                               xVelocity * maxMovementMagnitude,
-                                               yVelocity * maxMovementMagnitude, r_x * maxRotationMagnitude});
-    } else {
-      _swerveDrivebase->SetFieldRelativeVelocity(wom::drivetrain::FieldRelativeSpeeds{
-          xVelocity * maxMovementMagnitude, yVelocity * maxMovementMagnitude, r_x * maxRotationMagnitude});
-    }
-  }
+    // if (isRotateMatch) {
+    //   units::degree_t currentAngle =
+    //       _swerveDrivebase->GetPose().Rotation().Degrees();
+    //   CalculateRequestedAngle(turnX, turnY, currentAngle);
+    //   _swerveDriveTable->GetEntry("RotateMatch")
+    //       .SetDouble(_requestedAngle.value());
+    //   _swerveDrivebase->RotateMatchJoystick(
+    //       _requestedAngle,
+    //       wom::drivetrain::FieldRelativeSpeeds{// also field relative
+    //                                            xVelocity * maxMovementMagnitude,
+    //                                            yVelocity * maxMovementMagnitude,
+    //                                            r_x * maxRotationMagnitude});
+    // } else {
+      _swerveDrivebase->SetFieldRelativeVelocity(
+          wom::drivetrain::FieldRelativeSpeeds{xVelocity * maxMovementMagnitude,
+                                               yVelocity * maxMovementMagnitude,
+                                               r_x * maxRotationMagnitude});
+  //   }
+  // }
+  // _swerveDrivebase->SetTuning(100_deg, 1_mps);
 }
 
 void ManualDrivebase::ResetMode() {
@@ -114,16 +117,18 @@ void ManualDrivebase::ResetMode() {
   resetMode = false;
 }
 
-void ManualDrivebase::CalculateRequestedAngle(double joystickX, double joystickY,
+void ManualDrivebase::CalculateRequestedAngle(double joystickX,
+                                              double joystickY,
                                               units::degree_t defaultAngle) {
   _requestedAngle = (1_rad * std::atan2(joystickY, -joystickX)) + 90_deg;
-  if (joystickX == 0 & joystickY == 0) {
+  if (wom::utils::deadzone(joystickX) == 0 && wom::utils::deadzone(joystickY) == 0) {
     _requestedAngle = _swerveDrivebase->GetPose().Rotation().Radians();
   }
 }
 
 // Code for x-ing the wheels on the drivebase
-XDrivebase::XDrivebase(wom::drivetrain::SwerveDrive* swerveDrivebase) : _swerveDrivebase(swerveDrivebase) {
+XDrivebase::XDrivebase(wom::drivetrain::SwerveDrive* swerveDrivebase)
+    : _swerveDrivebase(swerveDrivebase) {
   Controls(swerveDrivebase);
 }
 void XDrivebase::OnTick(units::second_t deltaTime) {
@@ -165,17 +170,20 @@ void XDrivebase::OnTick(units::second_t deltaTime) {
 //   m_timer.Start();
 // }
 
-wom::drivetrain::behaviours::TempSimSwerveDrive::TempSimSwerveDrive(frc::Timer* timer, frc::Field2d* field)
+wom::drivetrain::behaviours::TempSimSwerveDrive::TempSimSwerveDrive(
+    frc::Timer* timer, frc::Field2d* field)
     : m_timer(timer), m_field(field) {}
 
 void wom::drivetrain::behaviours::TempSimSwerveDrive::OnUpdate() {
   m_field->SetRobotPose(m_driveSim.GetPose());
 
   // get the current trajectory state
-  frc::Trajectory::State desired_state = current_trajectory.Sample(m_timer->Get());
+  frc::Trajectory::State desired_state =
+      current_trajectory.Sample(m_timer->Get());
 
   // get the current wheel speeds
-  wom::utils::WriteTrajectoryState(current_trajectory_state_table, desired_state);
+  wom::utils::WriteTrajectoryState(current_trajectory_state_table,
+                                   desired_state);
 
   // move drivebase position to the desired state
   m_driveSim.SetPose(wom::utils::TrajectoryStateToPose2d(desired_state));
@@ -193,16 +201,19 @@ frc::Pose2d wom::drivetrain::behaviours::TempSimSwerveDrive::GetPose2d() {
   return m_driveSim.GetPose();
 }
 
-void wom::drivetrain::behaviours::TempSimSwerveDrive::SetPath(std::string path) {
+void wom::drivetrain::behaviours::TempSimSwerveDrive::SetPath(
+    std::string path) {
   nt::NetworkTableInstance inst = nt::NetworkTableInstance::GetDefault();
   std::shared_ptr<nt::NetworkTable> table = inst.GetTable("FMSInfo");
 
   // create a netowrk table for the trajectory
   std::shared_ptr<nt::NetworkTable> trajectory_table =
       nt::NetworkTableInstance::GetDefault().GetTable("trajectory_path");
-  current_trajectory_table = nt::NetworkTableInstance::GetDefault().GetTable("current_trajectory");
+  current_trajectory_table =
+      nt::NetworkTableInstance::GetDefault().GetTable("current_trajectory");
   current_trajectory_state_table =
-      nt::NetworkTableInstance::GetDefault().GetTable("current_trajectory_state");
+      nt::NetworkTableInstance::GetDefault().GetTable(
+          "current_trajectory_state");
 
   current_trajectory = m_pathplanner.getTrajectory(path);
   m_driveSim.SetPose(current_trajectory.Sample(0_s).pose);
@@ -210,10 +221,12 @@ void wom::drivetrain::behaviours::TempSimSwerveDrive::SetPath(std::string path)
   m_timer->Start();
 }
 
-wom::drivetrain::behaviours::AutoSwerveDrive::AutoSwerveDrive(wom::drivetrain::SwerveDrive* swerve,
-                                                              frc::Timer* timer, frc::Field2d* field)
+wom::drivetrain::behaviours::AutoSwerveDrive::AutoSwerveDrive(
+    wom::drivetrain::SwerveDrive* swerve, frc::Timer* timer,
+    frc::Field2d* field)
     : _swerve(swerve), m_timer(timer), m_field(field) {
-  _simSwerveDrive = new wom::drivetrain::behaviours::TempSimSwerveDrive(timer, field);
+  _simSwerveDrive =
+      new wom::drivetrain::behaviours::TempSimSwerveDrive(timer, field);
 }
 
 void wom::drivetrain::behaviours::AutoSwerveDrive::OnUpdate() {
diff --git a/wombat/src/main/cpp/subsystems/Arm.cpp b/wombat/src/main/cpp/subsystems/Arm.cpp
index 942003c4..9390601a 100644
--- a/wombat/src/main/cpp/subsystems/Arm.cpp
+++ b/wombat/src/main/cpp/subsystems/Arm.cpp
@@ -10,14 +10,19 @@ using namespace frc;
 using namespace wom;
 
 // creates network table instatnce on shuffleboard
-void wom::subsystems::ArmConfig::WriteNT(std::shared_ptr<nt::NetworkTable> table) {
+void wom::subsystems::ArmConfig::WriteNT(
+    std::shared_ptr<nt::NetworkTable> table) {
   table->GetEntry("armMass").SetDouble(armMass.value());
   table->GetEntry("loadMass").SetDouble(loadMass.value());
   table->GetEntry("armLength").SetDouble(armLength.value());
-  table->GetEntry("minAngle").SetDouble(minAngle.convert<units::degree>().value());
-  table->GetEntry("maxAngle").SetDouble(maxAngle.convert<units::degree>().value());
-  table->GetEntry("initialAngle").SetDouble(initialAngle.convert<units::degree>().value());
-  table->GetEntry("angleOffset").SetDouble(initialAngle.convert<units::degree>().value());
+  table->GetEntry("minAngle")
+      .SetDouble(minAngle.convert<units::degree>().value());
+  table->GetEntry("maxAngle")
+      .SetDouble(maxAngle.convert<units::degree>().value());
+  table->GetEntry("initialAngle")
+      .SetDouble(initialAngle.convert<units::degree>().value());
+  table->GetEntry("angleOffset")
+      .SetDouble(initialAngle.convert<units::degree>().value());
 }
 
 // arm config is used
@@ -38,12 +43,14 @@ void wom::subsystems::Arm::OnUpdate(units::second_t dt) {
     case ArmState::kIdle:
       break;
     case ArmState::kVelocity: {
-      units::newton_meter_t torque = 9.81_m / 1_s / 1_s * _config.armLength *
-                                     units::math::cos(angle + _config.angleOffset) *
-                                     (0.5 * _config.armMass + _config.loadMass);
+      units::newton_meter_t torque =
+          9.81_m / 1_s / 1_s * _config.armLength *
+          units::math::cos(angle + _config.angleOffset) *
+          (0.5 * _config.armMass + _config.loadMass);
       // units::volt_t feedforward = _config.leftGearbox.motor.Voltage(torque,
       // 0_rad/1_s);
-      units::volt_t feedforward = _config.leftGearbox.motor.Voltage(torque, _velocityPID.GetSetpoint());
+      units::volt_t feedforward =
+          _config.leftGearbox.motor.Voltage(torque, _velocityPID.GetSetpoint());
       // feedforward = 3.5_V;
       // std::cout << "feedforward" << feedforward.value() << std::endl;
       voltage = _velocityPID.Calculate(GetArmVelocity(), dt, feedforward);
@@ -51,10 +58,12 @@ void wom::subsystems::Arm::OnUpdate(units::second_t dt) {
       // std::endl; voltage = 0_V;
     } break;
     case ArmState::kAngle: {
-      units::newton_meter_t torque = 9.81_m / 1_s / 1_s * _config.armLength *
-                                     units::math::cos(angle + _config.angleOffset) *
-                                     (0.5 * _config.armMass + _config.loadMass);
-      units::volt_t feedforward = _config.leftGearbox.motor.Voltage(torque, 0_rad / 1_s);
+      units::newton_meter_t torque =
+          9.81_m / 1_s / 1_s * _config.armLength *
+          units::math::cos(angle + _config.angleOffset) *
+          (0.5 * _config.armMass + _config.loadMass);
+      units::volt_t feedforward =
+          _config.leftGearbox.motor.Voltage(torque, 0_rad / 1_s);
       // std::cout << "feedforward" << feedforward.value() << std::endl;
       voltage = _pid.Calculate(angle, dt, feedforward);
     } break;
@@ -79,8 +88,8 @@ void wom::subsystems::Arm::OnUpdate(units::second_t dt) {
 
   // std::cout << "voltage: " << voltage.value() << std::endl;
 
-  _config.leftGearbox.motorController->SetVoltage(voltage);
-  _config.rightGearbox.motorController->SetVoltage(voltage);
+  // _config.leftGearbox.motorController->SetVoltage(voltage);
+  // _config.rightGearbox.motorController->SetVoltage(voltage);
 
   // creates network table instances for the angle and config of the arm
   _table->GetEntry("angle").SetDouble(angle.convert<units::degree>().value());
diff --git a/wombat/src/main/cpp/subsystems/Elevator.cpp b/wombat/src/main/cpp/subsystems/Elevator.cpp
index 6e0267e7..d6b59f8a 100644
--- a/wombat/src/main/cpp/subsystems/Elevator.cpp
+++ b/wombat/src/main/cpp/subsystems/Elevator.cpp
@@ -7,7 +7,8 @@
 #include <networktables/NetworkTableInstance.h>
 #include <units/acceleration.h>
 
-void wom::subsystems::ElevatorConfig::WriteNT(std::shared_ptr<nt::NetworkTable> table) {
+void wom::subsystems::ElevatorConfig::WriteNT(
+    std::shared_ptr<nt::NetworkTable> table) {
   table->GetEntry("radius").SetDouble(radius.value());
   table->GetEntry("mass").SetDouble(mass.value());
   table->GetEntry("maxHeight").SetDouble(maxHeight.value());
@@ -19,7 +20,8 @@ wom::subsystems::Elevator::Elevator(ElevatorConfig config)
       _pid{config.path + "/pid", config.pid},
       _velocityPID{config.path + "/velocityPID", config.velocityPID},
       _table(nt::NetworkTableInstance::GetDefault().GetTable(config.path)) {
-  _config.leftGearbox.encoder->SetEncoderPosition(_config.initialHeight / _config.radius * 1_rad);
+  _config.leftGearbox.encoder->SetEncoderPosition(_config.initialHeight /
+                                                  _config.radius * 1_rad);
 }
 
 void wom::subsystems::Elevator::OnUpdate(units::second_t dt) {
@@ -37,7 +39,8 @@ void wom::subsystems::Elevator::OnUpdate(units::second_t dt) {
     case ElevatorState::kVelocity: {
       units::volt_t feedforward = _config.rightGearbox.motor.Voltage(
           (_config.mass * 9.81_mps_sq) * _config.radius,
-          _velocityPID.GetSetpoint() / (14.0 / 60.0 * 2.0 * 3.1415 * 0.02225 * 1_m) * 1_rad);
+          _velocityPID.GetSetpoint() /
+              (14.0 / 60.0 * 2.0 * 3.1415 * 0.02225 * 1_m) * 1_rad);
       // units::volt_t feedforward = _config.rightGearbox.motor.Voltage(0_Nm,
       // _velocityPID.GetSetpoint() / (14.0/60.0 * 2.0 * 3.1415 * 0.02225 * 1_m)
       // * 1_rad);
@@ -50,8 +53,8 @@ void wom::subsystems::Elevator::OnUpdate(units::second_t dt) {
       // voltage = 0_V;
     } break;
     case ElevatorState::kPID: {
-      units::volt_t feedforward =
-          _config.rightGearbox.motor.Voltage((_config.mass * 9.81_mps_sq) * _config.radius, 0_rad_per_s);
+      units::volt_t feedforward = _config.rightGearbox.motor.Voltage(
+          (_config.mass * 9.81_mps_sq) * _config.radius, 0_rad_per_s);
       // std::cout << "feed forward" << feedforward.value() << std::endl;
       feedforward = 1.2_V;
       // voltage = _pid.Calculate(height, dt, feedforward);
@@ -84,8 +87,8 @@ void wom::subsystems::Elevator::OnUpdate(units::second_t dt) {
 
   // Set voltage to motors...
   voltage *= speedLimit;
-  _config.leftGearbox.motorController->SetVoltage(voltage);
-  _config.rightGearbox.motorController->SetVoltage(voltage);
+  // _config.leftGearbox.motorController->SetVoltage(voltage);
+  // _config.rightGearbox.motorController->SetVoltage(voltage);
 }
 
 void wom::subsystems::Elevator::SetManual(units::volt_t voltage) {
@@ -96,7 +99,8 @@ void wom::subsystems::Elevator::SetPID(units::meter_t height) {
   _pid.SetSetpoint(height);
 }
 
-void wom::subsystems::Elevator::SetVelocity(units::meters_per_second_t velocity) {
+void wom::subsystems::Elevator::SetVelocity(
+    units::meters_per_second_t velocity) {
   _velocityPID.SetSetpoint(velocity);
 }
 
@@ -128,14 +132,18 @@ units::meter_t wom::subsystems::Elevator::GetHeight() const {
   // std::cout << "elevator position"<<
   // _config.rightGearbox.encoder->GetEncoderTicks() << std::endl; return
   // _config.rightGearbox.encoder->GetEncoderDistance() * 1_m;
-  return _config.elevatorEncoder.GetPosition() * 14 / 60 * 2 * 3.1415 * 0.02225 * 1_m;
+  return _config.elevatorEncoder.GetPosition() * 14 / 60 * 2 * 3.1415 *
+         0.02225 * 1_m;
 }
 
-units::meters_per_second_t wom::subsystems::Elevator::GetElevatorVelocity() const {
-  return _config.elevatorEncoder.GetVelocity() / 60_s * 14 / 60 * 2 * 3.1415 * 0.02225 * 1_m;
+units::meters_per_second_t wom::subsystems::Elevator::GetElevatorVelocity()
+    const {
+  return _config.elevatorEncoder.GetVelocity() / 60_s * 14 / 60 * 2 * 3.1415 *
+         0.02225 * 1_m;
 }
 
 units::meters_per_second_t wom::subsystems::Elevator::MaxSpeed() const {
-  return _config.leftGearbox.motor.Speed((_config.mass * 9.81_mps_sq) * _config.radius, 12_V) / 1_rad *
-         _config.radius;
+  return _config.leftGearbox.motor.Speed(
+             (_config.mass * 9.81_mps_sq) * _config.radius, 12_V) /
+         1_rad * _config.radius;
 }
diff --git a/wombat/src/main/cpp/subsystems/Shooter.cpp b/wombat/src/main/cpp/subsystems/Shooter.cpp
index b289c179..2fede2f3 100644
--- a/wombat/src/main/cpp/subsystems/Shooter.cpp
+++ b/wombat/src/main/cpp/subsystems/Shooter.cpp
@@ -14,14 +14,16 @@ wom::subsystems::Shooter::Shooter(std::string path, ShooterParams params)
 
 void wom::subsystems::Shooter::OnUpdate(units::second_t dt) {
   units::volt_t voltage{0};
-  units::revolutions_per_minute_t currentSpeed = _params.gearbox.encoder->GetEncoderAngularVelocity();
+  units::revolutions_per_minute_t currentSpeed =
+      _params.gearbox.encoder->GetEncoderAngularVelocity();
 
   switch (_state) {
     case ShooterState::kManual:
       voltage = _setpointManual;
       break;
     case ShooterState::kPID: {
-      auto feedforward = _params.gearbox.motor.Voltage(0_Nm, _pid.GetSetpoint());
+      auto feedforward =
+          _params.gearbox.motor.Voltage(0_Nm, _pid.GetSetpoint());
       voltage = _pid.Calculate(currentSpeed, dt, feedforward);
     } break;
     case ShooterState::kIdle:
@@ -29,17 +31,20 @@ void wom::subsystems::Shooter::OnUpdate(units::second_t dt) {
       break;
   }
 
-  units::newton_meter_t max_torque_at_current_limit = _params.gearbox.motor.Torque(_params.currentLimit);
+  units::newton_meter_t max_torque_at_current_limit =
+      _params.gearbox.motor.Torque(_params.currentLimit);
   units::volt_t max_voltage_for_current_limit =
       _params.gearbox.motor.Voltage(max_torque_at_current_limit, currentSpeed);
 
-  voltage = 1_V * std::min(voltage.value(), max_voltage_for_current_limit.value());
+  voltage =
+      1_V * std::min(voltage.value(), max_voltage_for_current_limit.value());
 
-  _params.gearbox.motorController->SetVoltage(voltage);
+  // _params.gearbox.motorController->SetVoltage(voltage);
 
   _table->GetEntry("output_volts").SetDouble(voltage.value());
   _table->GetEntry("speed_rpm").SetDouble(currentSpeed.value());
-  _table->GetEntry("setpoint_rpm").SetDouble(units::revolutions_per_minute_t{_pid.GetSetpoint()}.value());
+  _table->GetEntry("setpoint_rpm")
+      .SetDouble(units::revolutions_per_minute_t{_pid.GetSetpoint()}.value());
   _table->GetEntry("stable").SetBoolean(_pid.IsStable());
 }
 
@@ -62,7 +67,8 @@ bool wom::subsystems::Shooter::IsStable() const {
 
 // Shooter Manual Set
 
-wom::subsystems::ShooterConstant::ShooterConstant(Shooter* s, units::volt_t setpoint)
+wom::subsystems::ShooterConstant::ShooterConstant(Shooter* s,
+                                                  units::volt_t setpoint)
     : _shooter(s), _setpoint(setpoint) {
   Controls(_shooter);
 }
@@ -73,7 +79,9 @@ void wom::subsystems::ShooterConstant::OnTick(units::second_t dt) {
 
 // ShooterSpinup
 
-wom::subsystems::ShooterSpinup::ShooterSpinup(Shooter* s, units::radians_per_second_t speed, bool hold)
+wom::subsystems::ShooterSpinup::ShooterSpinup(Shooter* s,
+                                              units::radians_per_second_t speed,
+                                              bool hold)
     : _shooter(s), _speed(speed), _hold(hold) {
   Controls(_shooter);
 }
diff --git a/wombat/src/main/cpp/utils/Encoder.cpp b/wombat/src/main/cpp/utils/Encoder.cpp
index 3935d82c..e5d6ab2b 100644
--- a/wombat/src/main/cpp/utils/Encoder.cpp
+++ b/wombat/src/main/cpp/utils/Encoder.cpp
@@ -6,8 +6,8 @@
 
 #include <rev/SparkRelativeEncoder.h>
 
-wom::utils::Encoder::Encoder(double encoderTicksPerRotation, int type, units::meter_t wheelRadius,
-                             double reduction)
+wom::utils::Encoder::Encoder(double encoderTicksPerRotation, int type,
+                             units::meter_t wheelRadius, double reduction)
     : _reduction(reduction),
       _encoderTicksPerRotation(encoderTicksPerRotation),
       _type(type),
@@ -43,41 +43,55 @@ void wom::utils::Encoder::SetReduction(double reduction) {
 }
 
 units::radian_t wom::utils::Encoder::GetEncoderPosition() {
-  if (_type == 0) {
-    units::turn_t n_turns{GetEncoderTicks() / GetEncoderTicksPerRotation()};
-    return n_turns;
-  } else if (_type == 2) {
-    units::degree_t pos = GetEncoderTicks() * 1_deg;
-    return pos - _offset;
-  } else {
-    units::degree_t pos = GetEncoderTicks() * 1_deg;
-    return pos - _offset;
-  }
+  //if (_type == 0) {
+  //  units::turn_t n_turns{GetEncoderTicks() / GetEncoderTicksPerRotation()};
+  //  return n_turns;
+  //} else if (_type == 2) {
+  //  units::degree_t pos = GetEncoderTicks() * 1_deg;
+  //  return pos;
+  //} else {
+  //  units::degree_t pos = GetEncoderTicks() * 1_deg;
+  //  return pos - _offset;
+  //}
+  return GetEncoderTicks() * 1_rad;
 }
 
 double wom::utils::Encoder::GetEncoderDistance() {
-  return GetEncoderTicks() * (2 * M_PI) * _wheelRadius.value();
+  return GetEncoderTicks() * (2 * 3.141592) * _wheelRadius.value();
 }
 
 units::radians_per_second_t wom::utils::Encoder::GetEncoderAngularVelocity() {
   // return GetEncoderTickVelocity() / (double)GetEncoderTicksPerRotation() * 2
   // * 3.1415926;
-  units::turns_per_second_t n_turns_per_s{GetEncoderTickVelocity() / GetEncoderTicksPerRotation()};
+  units::turns_per_second_t n_turns_per_s{GetEncoderTickVelocity() /
+                                          GetEncoderTicksPerRotation()};
   return n_turns_per_s;
 }
 
+double wom::utils::Encoder::GetVelocityValue() const {
+  // std::cout << "GET VELOCITY: " << GetVelocity() << std::endl;
+  return GetVelocity();
+  // return 0;
+}
+
 double wom::utils::DigitalEncoder::GetEncoderRawTicks() const {
   return _nativeEncoder.Get();
 }
 
+double wom::utils::DigitalEncoder::GetVelocity() const {
+  return 0;
+}
+
 double wom::utils::DigitalEncoder::GetEncoderTickVelocity() const {
   return _nativeEncoder.GetRate();
 }
 
-wom::utils::CANSparkMaxEncoder::CANSparkMaxEncoder(rev::CANSparkMax* controller, units::meter_t wheelRadius,
+wom::utils::CANSparkMaxEncoder::CANSparkMaxEncoder(rev::CANSparkMax* controller,
+                                                   units::meter_t wheelRadius,
                                                    double reduction)
     : wom::utils::Encoder(42, reduction, wheelRadius, 2),
-      _encoder(controller->GetEncoder(rev::SparkRelativeEncoder::Type::kQuadrature)) {}
+      _encoder(controller->GetEncoder(
+          rev::SparkRelativeEncoder::Type::kQuadrature)) {}
 
 double wom::utils::CANSparkMaxEncoder::GetEncoderRawTicks() const {
   return _encoder.GetPosition() * _reduction;
@@ -87,6 +101,18 @@ double wom::utils::CANSparkMaxEncoder::GetEncoderTickVelocity() const {
   return _encoder.GetVelocity() * GetEncoderTicksPerRotation() / 60;
 }
 
+double wom::utils::TalonFXEncoder::GetVelocity() const {
+  return _controller->GetVelocity().GetValue().value();
+}
+
+double wom::utils::CanEncoder::GetVelocity() const {
+  return _canEncoder->GetVelocity().GetValue().value();
+}
+
+double wom::utils::DutyCycleEncoder::GetVelocity() const {
+  return 0;
+}
+
 double wom::utils::CANSparkMaxEncoder::GetPosition() const {
   return _encoder.GetPosition();
 }
@@ -95,9 +121,11 @@ double wom::utils::CANSparkMaxEncoder::GetVelocity() const {
   return _encoder.GetVelocity();
 }
 
-wom::utils::TalonFXEncoder::TalonFXEncoder(ctre::phoenix6::hardware::TalonFX* controller,
-                                           units::meter_t wheelRadius, double reduction)
-    : utils::Encoder(2048, reduction, wheelRadius, 0), _controller(controller) {}
+wom::utils::TalonFXEncoder::TalonFXEncoder(
+    ctre::phoenix6::hardware::TalonFX* controller, units::meter_t wheelRadius,
+    double reduction)
+    : utils::Encoder(2048, reduction, wheelRadius, 0),
+      _controller(controller) {}
 
 double wom::utils::TalonFXEncoder::GetEncoderRawTicks() const {
   return _controller->GetPosition().GetValue().value();
@@ -107,9 +135,12 @@ double wom::utils::TalonFXEncoder::GetEncoderTickVelocity() const {
   return _controller->Get() * 10;
 }
 
-wom::utils::DutyCycleEncoder::DutyCycleEncoder(int channel, units::meter_t wheelRadius,
-                                               double ticksPerRotation, double reduction)
-    : wom::utils::Encoder(ticksPerRotation, reduction, wheelRadius, 0), _dutyCycleEncoder(channel) {}
+wom::utils::DutyCycleEncoder::DutyCycleEncoder(int channel,
+                                               units::meter_t wheelRadius,
+                                               double ticksPerRotation,
+                                               double reduction)
+    : wom::utils::Encoder(ticksPerRotation, reduction, wheelRadius, 0),
+      _dutyCycleEncoder(channel) {}
 
 double wom::utils::DutyCycleEncoder::GetEncoderRawTicks() const {
   return _dutyCycleEncoder.Get().value();
@@ -119,14 +150,15 @@ double wom::utils::DutyCycleEncoder::GetEncoderTickVelocity() const {
   return 0;
 }
 
-wom::utils::CanEncoder::CanEncoder(int deviceNumber, units::meter_t wheelRadius, double ticksPerRotation,
-                                   double reduction, std::string name)
-    : wom::utils::Encoder(ticksPerRotation, reduction, wheelRadius, 1) {
+wom::utils::CanEncoder::CanEncoder(int deviceNumber, units::meter_t wheelRadius,
+                                   double ticksPerRotation, double reduction,
+                                   std::string name)
+    : wom::utils::Encoder(ticksPerRotation, 2, wheelRadius, reduction) {
   _canEncoder = new ctre::phoenix6::hardware::CANcoder(deviceNumber, name);
 }
 
 double wom::utils::CanEncoder::GetEncoderRawTicks() const {
-  return _canEncoder->GetAbsolutePosition().GetValue().value();
+  return _canEncoder->GetPosition().GetValue().value() * 2 * 3.14;
 }
 
 double wom::utils::CanEncoder::GetEncoderTickVelocity() const {
diff --git a/wombat/src/main/cpp/utils/Util.cpp b/wombat/src/main/cpp/utils/Util.cpp
index dbdbb642..e529e245 100644
--- a/wombat/src/main/cpp/utils/Util.cpp
+++ b/wombat/src/main/cpp/utils/Util.cpp
@@ -13,18 +13,21 @@ units::second_t wom::utils::now() {
   return static_cast<double>(now) / 1000000 * 1_s;
 }
 
-void wom::utils::WritePose2NT(std::shared_ptr<nt::NetworkTable> table, frc::Pose2d pose) {
+void wom::utils::WritePose2NT(std::shared_ptr<nt::NetworkTable> table,
+                              frc::Pose2d pose) {
   table->GetEntry("x").SetDouble(pose.X().value());
   table->GetEntry("y").SetDouble(pose.Y().value());
   table->GetEntry("angle").SetDouble(pose.Rotation().Degrees().value());
 }
 
-void wom::utils::WritePose3NT(std::shared_ptr<nt::NetworkTable> table, frc::Pose3d pose) {
+void wom::utils::WritePose3NT(std::shared_ptr<nt::NetworkTable> table,
+                              frc::Pose3d pose) {
   table->GetEntry("x").SetDouble(pose.X().value());
   table->GetEntry("y").SetDouble(pose.Y().value());
   table->GetEntry("z").SetDouble(pose.Z().value());
 
-  table->GetEntry("angle").SetDouble(pose.Rotation().Z().convert<units::degree>().value());
+  table->GetEntry("angle").SetDouble(
+      pose.Rotation().Z().convert<units::degree>().value());
 }
 
 double wom::utils::deadzone(double val, double deadzone) {
@@ -47,24 +50,32 @@ units::volt_t wom::utils::GetVoltage(frc::MotorController* controller) {
   return controller->Get() * frc::RobotController::GetBatteryVoltage();
 }
 
-void wom::utils::WriteTrajectory(std::shared_ptr<nt::NetworkTable> table, frc::Trajectory trajectory) {
+void wom::utils::WriteTrajectory(std::shared_ptr<nt::NetworkTable> table,
+                                 frc::Trajectory trajectory) {
   table->GetEntry("length").SetDouble(trajectory.TotalTime().value());
 
   // write the trajectory to the network table
   int i = 0;
   for (auto state : trajectory.States()) {
-    table->GetSubTable(std::to_string(i))->GetEntry("x").SetDouble(state.pose.X().value());
-    table->GetSubTable(std::to_string(i))->GetEntry("y").SetDouble(state.pose.Y().value());
+    table->GetSubTable(std::to_string(i))
+        ->GetEntry("x")
+        .SetDouble(state.pose.X().value());
+    table->GetSubTable(std::to_string(i))
+        ->GetEntry("y")
+        .SetDouble(state.pose.Y().value());
     table->GetSubTable(std::to_string(i))
         ->GetEntry("angle")
         .SetDouble(state.pose.Rotation().Degrees().value());
-    table->GetSubTable(std::to_string(i))->GetEntry("time").SetDouble(state.t.value());
+    table->GetSubTable(std::to_string(i))
+        ->GetEntry("time")
+        .SetDouble(state.t.value());
 
     i++;
   }
 }
 
-void wom::utils::WriteTrajectoryState(std::shared_ptr<nt::NetworkTable> table, frc::Trajectory::State state) {
+void wom::utils::WriteTrajectoryState(std::shared_ptr<nt::NetworkTable> table,
+                                      frc::Trajectory::State state) {
   table->GetEntry("x").SetDouble(state.pose.X().value());
   table->GetEntry("y").SetDouble(state.pose.Y().value());
   table->GetEntry("angle").SetDouble(state.pose.Rotation().Degrees().value());
diff --git a/wombat/src/main/cpp/vision/Limelight.cpp b/wombat/src/main/cpp/vision/Limelight.cpp
index 906f92cb..b37148ce 100644
--- a/wombat/src/main/cpp/vision/Limelight.cpp
+++ b/wombat/src/main/cpp/vision/Limelight.cpp
@@ -8,7 +8,8 @@
 
 #include "utils/Util.h"
 
-wom::vision::Limelight::Limelight(std::string limelightName) : _limelightName(limelightName) {}
+wom::vision::Limelight::Limelight(std::string limelightName)
+    : _limelightName(limelightName) {}
 
 std::string wom::vision::Limelight::GetName() {
   return _limelightName;
@@ -23,7 +24,8 @@ std::pair<double, double> wom::vision::Limelight::GetOffset() {
   return offset;
 }
 
-std::vector<double> wom::vision::Limelight::GetAprilTagData(LimelightAprilTagData dataType) {
+std::vector<double> wom::vision::Limelight::GetAprilTagData(
+    LimelightAprilTagData dataType) {
   std::string dataName;
 
   switch (dataType) {
@@ -67,7 +69,8 @@ std::vector<double> wom::vision::Limelight::GetAprilTagData(LimelightAprilTagDat
   return table->GetNumberArray(dataName, std::vector<double>(6));
 }
 
-double wom::vision::Limelight::GetTargetingData(LimelightTargetingData dataType, double defaultValue) {
+double wom::vision::Limelight::GetTargetingData(LimelightTargetingData dataType,
+                                                double defaultValue) {
   std::string dataName;
 
   switch (dataType) {
@@ -155,8 +158,8 @@ void wom::vision::Limelight::SetCrop(std::array<double, 4> crop) {
   table->PutNumberArray("camtran", crop);
 }
 
-units::meters_per_second_t wom::vision::Limelight::GetSpeed(frc::Pose3d pose1, frc::Pose3d pose2,
-                                                            units::second_t dt) {
+units::meters_per_second_t wom::vision::Limelight::GetSpeed(
+    frc::Pose3d pose1, frc::Pose3d pose2, units::second_t dt) {
   frc::Transform3d dPose{pose1, pose2};
   frc::Translation3d dTranslation = dPose.Translation();
 
@@ -169,8 +172,9 @@ units::meters_per_second_t wom::vision::Limelight::GetSpeed(frc::Pose3d pose1, f
 
 frc::Pose3d wom::vision::Limelight::GetPose() {
   std::vector<double> pose = GetAprilTagData(LimelightAprilTagData::kBotpose);
-  return frc::Pose3d(pose[1] * 1_m, 1_m * pose[2], 1_m * pose[3],
-                     frc::Rotation3d(1_deg * (pose[4]), 1_deg * (pose[5]), 1_deg * (pose[6])));
+  return frc::Pose3d(
+      pose[1] * 1_m, 1_m * pose[2], 1_m * pose[3],
+      frc::Rotation3d(1_deg * (pose[4]), 1_deg * (pose[5]), 1_deg * (pose[6])));
 }
 
 void wom::vision::Limelight::OnStart() {
@@ -181,9 +185,11 @@ void wom::vision::Limelight::OnUpdate(units::time::second_t dt) {
   wom::utils::WritePose3NT(table, GetPose());
 }
 
-bool wom::vision::Limelight::IsAtSetPoseVision(frc::Pose3d pose, units::second_t dt) {
+bool wom::vision::Limelight::IsAtSetPoseVision(frc::Pose3d pose,
+                                               units::second_t dt) {
   frc::Pose3d actualPose = GetPose();
   frc::Pose3d relativePose = actualPose.RelativeTo(pose);
-  return (units::math::fabs(relativePose.X()) < 0.01_m && units::math::fabs(relativePose.Y()) < 0.01_m &&
+  return (units::math::fabs(relativePose.X()) < 0.01_m &&
+          units::math::fabs(relativePose.Y()) < 0.01_m &&
           GetSpeed(pose, GetPose(), dt) < 1_m / 1_s);
 }
diff --git a/wombat/src/main/include/behaviour/Behaviour.h b/wombat/src/main/include/behaviour/Behaviour.h
index 84ac8689..4f2e357e 100644
--- a/wombat/src/main/include/behaviour/Behaviour.h
+++ b/wombat/src/main/include/behaviour/Behaviour.h
@@ -22,7 +22,13 @@
 #include "HasBehaviour.h"
 
 namespace behaviour {
-enum class BehaviourState { INITIALISED, RUNNING, DONE, TIMED_OUT, INTERRUPTED };
+enum class BehaviourState {
+  INITIALISED,
+  RUNNING,
+  DONE,
+  TIMED_OUT,
+  INTERRUPTED
+};
 
 class SequentialBehaviour;
 
@@ -40,7 +46,8 @@ class Behaviour : public std::enable_shared_from_this<Behaviour> {
  public:
   using ptr = std::shared_ptr<Behaviour>;
 
-  explicit Behaviour(std::string name = "<unnamed behaviour>", units::time::second_t period = 20_ms);
+  Behaviour(std::string name = "<unnamed behaviour>",
+            units::time::second_t period = 20_ms);
   ~Behaviour();
 
   /**
@@ -186,15 +193,16 @@ class SequentialBehaviour : public Behaviour {
   std::deque<ptr> _queue;
 };
 
-inline std::shared_ptr<SequentialBehaviour> operator<<(Behaviour::ptr a, Behaviour::ptr b) {
+inline std::shared_ptr<SequentialBehaviour> operator<<(Behaviour::ptr a,
+                                                       Behaviour::ptr b) {
   auto seq = std::make_shared<SequentialBehaviour>();
   seq->Add(a);
   seq->Add(b);
   return seq;
 }
 
-inline std::shared_ptr<SequentialBehaviour> operator<<(std::shared_ptr<SequentialBehaviour> a,
-                                                       Behaviour::ptr b) {
+inline std::shared_ptr<SequentialBehaviour> operator<<(
+    std::shared_ptr<SequentialBehaviour> a, Behaviour::ptr b) {
   a->Add(b);
   return a;
 }
@@ -239,8 +247,10 @@ class ConcurrentBehaviour : public Behaviour {
  * Create a concurrent behaviour group, waiting for all behaviours
  * to finish before moving on.
  */
-inline std::shared_ptr<ConcurrentBehaviour> operator&(Behaviour::ptr a, Behaviour::ptr b) {
-  auto conc = std::make_shared<ConcurrentBehaviour>(ConcurrentBehaviourReducer::ALL);
+inline std::shared_ptr<ConcurrentBehaviour> operator&(Behaviour::ptr a,
+                                                      Behaviour::ptr b) {
+  auto conc =
+      std::make_shared<ConcurrentBehaviour>(ConcurrentBehaviourReducer::ALL);
   conc->Add(a);
   conc->Add(b);
   return conc;
@@ -251,8 +261,10 @@ inline std::shared_ptr<ConcurrentBehaviour> operator&(Behaviour::ptr a, Behaviou
  * be interrupted as soon as any members of the group are finished (the
  * behaviours are 'raced' against each other).
  */
-inline std::shared_ptr<ConcurrentBehaviour> operator|(Behaviour::ptr a, Behaviour::ptr b) {
-  auto conc = std::make_shared<ConcurrentBehaviour>(ConcurrentBehaviourReducer::ANY);
+inline std::shared_ptr<ConcurrentBehaviour> operator|(Behaviour::ptr a,
+                                                      Behaviour::ptr b) {
+  auto conc =
+      std::make_shared<ConcurrentBehaviour>(ConcurrentBehaviourReducer::ANY);
   conc->Add(a);
   conc->Add(b);
   return conc;
@@ -320,7 +332,8 @@ struct Switch : public Behaviour {
    * @param condition The function yielding true if this is the correct option
    * @param b The behaviour to call if this option is provided.
    */
-  std::shared_ptr<Switch> When(std::function<bool(T&)> condition, Behaviour::ptr b) {
+  std::shared_ptr<Switch> When(std::function<bool(T&)> condition,
+                               Behaviour::ptr b) {
     _options.push_back(std::pair(condition, b));
     Inherit(*b);
     return std::reinterpret_pointer_cast<Switch<T>>(shared_from_this());
@@ -376,7 +389,8 @@ struct Switch : public Behaviour {
 
  private:
   std::function<T()> _fn;
-  wpi::SmallVector<std::pair<std::function<bool(T&)>, Behaviour::ptr>, 4> _options;
+  wpi::SmallVector<std::pair<std::function<bool(T&)>, Behaviour::ptr>, 4>
+      _options;
   Behaviour::ptr _locked = nullptr;
 };
 
@@ -393,8 +407,10 @@ struct Decide : public Switch<std::monostate> {
    * @param condition The function yielding true if this is the correct option
    * @param b The behaviour to call if this option is provided.
    */
-  std::shared_ptr<Decide> When(std::function<bool()> condition, Behaviour::ptr b) {
-    return std::reinterpret_pointer_cast<Decide>(Switch::When([condition](auto) { return condition(); }, b));
+  std::shared_ptr<Decide> When(std::function<bool()> condition,
+                               Behaviour::ptr b) {
+    return std::reinterpret_pointer_cast<Decide>(
+        Switch::When([condition](auto) { return condition(); }, b));
   }
 };
 
diff --git a/wombat/src/main/include/behaviour/HasBehaviour.h b/wombat/src/main/include/behaviour/HasBehaviour.h
index bae5ac05..c9b35b2e 100644
--- a/wombat/src/main/include/behaviour/HasBehaviour.h
+++ b/wombat/src/main/include/behaviour/HasBehaviour.h
@@ -30,7 +30,8 @@ class HasBehaviour {
 
  protected:
   std::shared_ptr<Behaviour> _active_behaviour{nullptr};
-  std::function<std::shared_ptr<Behaviour>(void)> _default_behaviour_producer{nullptr};
+  std::function<std::shared_ptr<Behaviour>(void)> _default_behaviour_producer{
+      nullptr};
 
  private:
   friend class BehaviourScheduler;
diff --git a/wombat/src/main/include/drivetrain/SwerveDrive.h b/wombat/src/main/include/drivetrain/SwerveDrive.h
index 661a2943..b48db5a5 100644
--- a/wombat/src/main/include/drivetrain/SwerveDrive.h
+++ b/wombat/src/main/include/drivetrain/SwerveDrive.h
@@ -6,6 +6,7 @@
 
 #include <frc/estimator/SwerveDrivePoseEstimator.h>
 #include <frc/kinematics/SwerveDriveKinematics.h>
+#include <frc/controller/PIDController.h>
 #include <networktables/DoubleTopic.h>
 #include <networktables/NetworkTable.h>
 #include <networktables/NetworkTableInstance.h>
@@ -47,11 +48,12 @@ struct SwerveModuleConfig {
 
 class SwerveModule {
  public:
-  using angle_pid_conf_t = utils::PIDConfig<units::radian, units::volt>;
-  using velocity_pid_conf_t = utils::PIDConfig<units::meters_per_second, units::volt>;
+  //using angle_pid_conf_t = utils::PIDConfig<units::radian, units::volt>;
+  using velocity_pid_conf_t =
+      utils::PIDConfig<units::meters_per_second, units::volt>;
 
-  SwerveModule(std::string path, SwerveModuleConfig config, angle_pid_conf_t anglePID,
-               velocity_pid_conf_t velocityPID);
+  SwerveModule(std::string path, SwerveModuleConfig config,
+               /*angle_pid_conf_t anglePID,*/ velocity_pid_conf_t velocityPID);
   void OnUpdate(units::second_t dt);
   void OnStart();
 
@@ -65,7 +67,8 @@ class SwerveModule {
 
   void SetZero(units::second_t dt);
   void SetIdle();
-  void SetPID(units::radian_t angle, units::meters_per_second_t speed, units::second_t dt);
+  void SetPID(units::radian_t angle, units::meters_per_second_t speed,
+              units::second_t dt);
   void SetZero();
   void SetVoltageLimit(units::volt_t driveModuleVoltageLimit);
 
@@ -81,7 +84,8 @@ class SwerveModule {
 
   const SwerveModuleConfig& GetConfig() const;
 
-  utils::PIDController<units::radians, units::volt> _anglePIDController;
+  //utils::PIDController<units::radians, units::volt> _anglePIDController;
+  frc::PIDController _anglePIDController;
 
  private:
   SwerveModuleConfig _config;
@@ -91,7 +95,7 @@ class SwerveModule {
   bool _hasZeroedEncoder = false;
   bool _hasZeroed = false;
 
-  utils::PIDController<units::meters_per_second, units::volt> _velocityPIDController;
+  frc::PIDController _velocityPIDController;
 
   std::shared_ptr<nt::NetworkTable> _table;
 
@@ -102,19 +106,22 @@ class SwerveModule {
 };
 
 struct SwerveDriveConfig {
-  using pose_angle_conf_t = utils::PIDConfig<units::radian, units::radians_per_second>;
-  using pose_position_conf_t = utils::PIDConfig<units::meter, units::meters_per_second>;
-  using balance_conf_t = utils::PIDConfig<units::degree, units::meters_per_second>;
+  /*using pose_angle_conf_t =
+      utils::PIDConfig<units::radian, units::radians_per_second>;*/
+  using pose_position_conf_t =
+      utils::PIDConfig<units::meter, units::meters_per_second>;
+  using balance_conf_t =
+      utils::PIDConfig<units::degree, units::meters_per_second>;
 
   std::string path;
-  SwerveModule::angle_pid_conf_t anglePID;
+  //SwerveModule::angle_pid_conf_t anglePID;
   SwerveModule::velocity_pid_conf_t velocityPID;
 
   wpi::array<SwerveModuleConfig, 4> modules;
 
   ctre::phoenix6::hardware::Pigeon2* gyro;
 
-  pose_angle_conf_t poseAnglePID;
+  //pose_angle_conf_t poseAnglePID;
   pose_position_conf_t posePositionPID;
 
   units::kilogram_t mass;
@@ -168,7 +175,8 @@ class SwerveDrive : public behaviour::HasBehaviour {
    * @brief This function switches the state to handle the robot's rotation
    * matching that of the joystick
    */
-  void RotateMatchJoystick(units::radian_t joystickAngle, FieldRelativeSpeeds speeds);
+  void RotateMatchJoystick(units::radian_t joystickAngle,
+                           FieldRelativeSpeeds speeds);
 
   void SetIdle();
 
@@ -178,7 +186,8 @@ class SwerveDrive : public behaviour::HasBehaviour {
   void SetFieldRelativeVelocity(FieldRelativeSpeeds speeds);
   void SetPose(frc::Pose2d pose);
   bool IsAtSetPose();
-  void SetIndividualTuning(int mod, units::radian_t angle, units::meters_per_second_t speed);
+  void SetIndividualTuning(int mod, units::radian_t angle,
+                           units::meters_per_second_t speed);
   void SetTuning(units::radian_t angle, units::meters_per_second_t speed);
   void SetZero();
   void SetVoltageLimit(units::volt_t driveVoltageLimit);
@@ -213,7 +222,9 @@ class SwerveDrive : public behaviour::HasBehaviour {
   frc::SwerveDriveKinematics<4> _kinematics;
   frc::SwerveDrivePoseEstimator<4> _poseEstimator;
 
-  utils::PIDController<units::radian, units::radians_per_second> _anglePIDController;
+  /*utils::PIDController<units::radian, units::radians_per_second>
+      _anglePIDController;*/
+  frc::PIDController _anglePIDController;
   utils::PIDController<units::meter, units::meters_per_second> _xPIDController;
   utils::PIDController<units::meter, units::meters_per_second> _yPIDController;
 
diff --git a/wombat/src/main/include/drivetrain/behaviours/SwerveBehaviours.h b/wombat/src/main/include/drivetrain/behaviours/SwerveBehaviours.h
index 11400c4a..5aa1972f 100644
--- a/wombat/src/main/include/drivetrain/behaviours/SwerveBehaviours.h
+++ b/wombat/src/main/include/drivetrain/behaviours/SwerveBehaviours.h
@@ -37,14 +37,16 @@ class ManualDrivebase : public behaviour::Behaviour {
    * A pointer to the controller that the driver has been allocated (the
    * allocated memory address that stores the "driver controller" object)
    */
-  ManualDrivebase(wom::drivetrain::SwerveDrive* swerveDrivebase, frc::XboxController* driverController);
+  ManualDrivebase(wom::drivetrain::SwerveDrive* swerveDrivebase,
+                  frc::XboxController* driverController);
 
   void OnTick(units::second_t deltaTime) override;
   /**
    * @brief This function handles all of the logic behind the tangent function,
    * to be able to calculate an angle between 0 andd 360 degrees, inclusively
    */
-  void CalculateRequestedAngle(double joystickX, double joystickY, units::degree_t defaultAngle);
+  void CalculateRequestedAngle(double joystickX, double joystickY,
+                               units::degree_t defaultAngle);
   void OnStart() override;
   void ResetMode();
 
@@ -70,8 +72,8 @@ class ManualDrivebase : public behaviour::Behaviour {
   const double turningDeadzone = 0.2;
 
   // Variables for solution to Anti-tip
-  double prevJoystickX, prevJoystickY, prevPrevJoystickX, prevPrevJoystickY, usingJoystickXPos,
-      usingJoystickYPos;
+  double prevJoystickX, prevJoystickY, prevPrevJoystickX, prevPrevJoystickY,
+      usingJoystickXPos, usingJoystickYPos;
   // The speed that the joystick must travel to activate averaging over previous
   // 3 joystick positions
   const double smoothingThreshold = 1;
@@ -124,8 +126,8 @@ class GoToPose : public behaviour::Behaviour {
 
 class FollowTrajectory : public behaviour::Behaviour {
  public:
-  FollowTrajectory(wom::drivetrain::SwerveDrive* swerve, wom::utils::Pathplanner* pathplanner,
-                   std::string path);
+  FollowTrajectory(wom::drivetrain::SwerveDrive* swerve,
+                   wom::utils::Pathplanner* pathplanner, std::string path);
 
   void OnTick(units::second_t dt) override;
 
@@ -182,7 +184,8 @@ class TempSimSwerveDrive {
 
 class AutoSwerveDrive {
  public:
-  AutoSwerveDrive(wom::drivetrain::SwerveDrive* swerve, frc::Timer* timer, frc::Field2d* field);
+  AutoSwerveDrive(wom::drivetrain::SwerveDrive* swerve, frc::Timer* timer,
+                  frc::Field2d* field);
 
   void OnUpdate();
 
diff --git a/wombat/src/main/include/subsystems/Arm.h b/wombat/src/main/include/subsystems/Arm.h
index 8b91be72..155fd4cd 100644
--- a/wombat/src/main/include/subsystems/Arm.h
+++ b/wombat/src/main/include/subsystems/Arm.h
@@ -68,7 +68,8 @@ class Arm : public behaviour::HasBehaviour {
   ArmConfig _config;
   ArmState _state = ArmState::kIdle;
   wom::utils::PIDController<units::radian, units::volt> _pid;
-  wom::utils::PIDController<units::radians_per_second, units::volt> _velocityPID;
+  wom::utils::PIDController<units::radians_per_second, units::volt>
+      _velocityPID;
 
   std::shared_ptr<nt::NetworkTable> _table;
 
diff --git a/wombat/src/main/include/subsystems/Shooter.h b/wombat/src/main/include/subsystems/Shooter.h
index e5ac219d..2bbf6f5e 100644
--- a/wombat/src/main/include/subsystems/Shooter.h
+++ b/wombat/src/main/include/subsystems/Shooter.h
@@ -62,7 +62,8 @@ class ShooterConstant : public behaviour::Behaviour {
 
 class ShooterSpinup : public behaviour::Behaviour {
  public:
-  ShooterSpinup(Shooter* s, units::radians_per_second_t speed, bool hold = false);
+  ShooterSpinup(Shooter* s, units::radians_per_second_t speed,
+                bool hold = false);
 
   void OnTick(units::second_t dt) override;
 
diff --git a/wombat/src/main/include/utils/Encoder.h b/wombat/src/main/include/utils/Encoder.h
index 62ccc28f..b504e1c0 100644
--- a/wombat/src/main/include/utils/Encoder.h
+++ b/wombat/src/main/include/utils/Encoder.h
@@ -20,7 +20,8 @@ namespace wom {
 namespace utils {
 class Encoder {
  public:
-  Encoder(double encoderTicksPerRotation, int type, units::meter_t wheelRadius, double reduction = 1.0);
+  Encoder(double encoderTicksPerRotation, int type, units::meter_t wheelRadius,
+          double reduction = 1.0);
 
   virtual double GetEncoderRawTicks() const = 0;
   virtual double GetEncoderTickVelocity() const = 0;  // ticks/s
@@ -41,6 +42,9 @@ class Encoder {
   int encoderType = 0;
   double _reduction;
 
+  virtual double GetVelocity() const = 0;
+  double GetVelocityValue() const;
+
  private:
   double _encoderTicksPerRotation;
   units::radian_t _offset = 0_rad;
@@ -50,30 +54,30 @@ class Encoder {
 
 class DigitalEncoder : public Encoder {
  public:
-  DigitalEncoder(int channelA, int channelB, double ticksPerRotation, units::meter_t wheelRadius,
-                 double reduction = 1)
-      : Encoder(ticksPerRotation, reduction, wheelRadius, 0), _nativeEncoder(channelA, channelB) {}
+  DigitalEncoder(int channelA, int channelB, double ticksPerRotation,
+                 units::meter_t wheelRadius, double reduction = 1)
+      : Encoder(ticksPerRotation, reduction, wheelRadius, 0),
+        _nativeEncoder(channelA, channelB) {}
 
   double GetEncoderRawTicks() const override;
   double GetEncoderTickVelocity() const override;
 
   double GetPosition() const;
-  double GetVelocity() const;
-
+  double GetVelocity() const override;
  private:
   frc::Encoder _nativeEncoder;
 };
 
-class SimCANSparkMaxEncoder;
 class CANSparkMaxEncoder : public Encoder {
  public:
-  explicit CANSparkMaxEncoder(rev::CANSparkMax* controller, units::meter_t wheelRadius, double reduction = 1);
+  explicit CANSparkMaxEncoder(rev::CANSparkMax* controller,
+                              units::meter_t wheelRadius, double reduction = 1);
 
   double GetEncoderRawTicks() const override;
   double GetEncoderTickVelocity() const override;
 
   double GetPosition() const;
-  double GetVelocity() const;
+  double GetVelocity() const override;
 
  protected:
   rev::SparkRelativeEncoder _encoder;
@@ -82,11 +86,12 @@ class CANSparkMaxEncoder : public Encoder {
 
 class TalonFXEncoder : public Encoder {
  public:
-  TalonFXEncoder(ctre::phoenix6::hardware::TalonFX* controller, units::meter_t wheelRadius,
-                 double reduction = 1);
+  TalonFXEncoder(ctre::phoenix6::hardware::TalonFX* controller,
+                 units::meter_t wheelRadius, double reduction = 1);
 
   double GetEncoderRawTicks() const override;
   double GetEncoderTickVelocity() const override;
+  double GetVelocity() const override;
 
  private:
   ctre::phoenix6::hardware::TalonFX* _controller;
@@ -94,11 +99,12 @@ class TalonFXEncoder : public Encoder {
 
 class DutyCycleEncoder : public Encoder {
  public:
-  DutyCycleEncoder(int channel, units::meter_t wheelRadius, double ticksPerRotation = 1,
-                   double reduction = 1);
+  DutyCycleEncoder(int channel, units::meter_t wheelRadius,
+                   double ticksPerRotation = 1, double reduction = 1);
 
   double GetEncoderRawTicks() const override;
   double GetEncoderTickVelocity() const override;
+  double GetVelocity() const override;
 
  private:
   frc::DutyCycleEncoder _dutyCycleEncoder;
@@ -106,12 +112,14 @@ class DutyCycleEncoder : public Encoder {
 
 class CanEncoder : public Encoder {
  public:
-  CanEncoder(int deviceNumber, units::meter_t wheelRadius, double ticksPerRotation = 4095,
-             double reduction = 1, std::string name = "Drivebase");
+  CanEncoder(int deviceNumber, units::meter_t wheelRadius,
+             double ticksPerRotation = 4095, double reduction = 6.75,
+             std::string name = "Drivebase");
 
   double GetEncoderRawTicks() const override;
   double GetEncoderTickVelocity() const override;
   double GetAbsoluteEncoderPosition();
+  double GetVelocity() const override;
 
   const double constantValue = 0.0;
 
diff --git a/wombat/src/main/include/utils/PID.h b/wombat/src/main/include/utils/PID.h
index a0f2eaf9..2ce378ec 100644
--- a/wombat/src/main/include/utils/PID.h
+++ b/wombat/src/main/include/utils/PID.h
@@ -9,6 +9,7 @@
 #include <units/base.h>
 #include <units/time.h>
 
+#include <cmath>
 #include <memory>
 #include <optional>
 #include <string>
@@ -23,15 +24,19 @@ struct PIDConfig {
   using in_t = units::unit_t<IN>;
 
   using kp_t = units::unit_t<units::compound_unit<OUT, units::inverse<IN>>>;
-  using ki_t = units::unit_t<units::compound_unit<OUT, units::inverse<IN>, units::inverse<units::second>>>;
-  using kd_t = units::unit_t<units::compound_unit<OUT, units::inverse<IN>, units::second>>;
+  using ki_t =
+      units::unit_t<units::compound_unit<OUT, units::inverse<IN>,
+                                         units::inverse<units::second>>>;
+  using kd_t = units::unit_t<
+      units::compound_unit<OUT, units::inverse<IN>, units::second>>;
 
   using error_t = units::unit_t<IN>;
-  using deriv_t = units::unit_t<units::compound_unit<IN, units::inverse<units::second>>>;
+  using deriv_t =
+      units::unit_t<units::compound_unit<IN, units::inverse<units::second>>>;
 
-  PIDConfig(std::string path, kp_t kp = kp_t{0}, ki_t ki = ki_t{0}, kd_t kd = kd_t{0},
-            error_t stableThresh = error_t{-1}, deriv_t stableDerivThresh = deriv_t{-1},
-            in_t izone = in_t{-1})
+  PIDConfig(std::string path, kp_t kp = kp_t{0}, ki_t ki = ki_t{0},
+            kd_t kd = kd_t{0}, error_t stableThresh = error_t{-1},
+            deriv_t stableDerivThresh = deriv_t{-1}, in_t izone = in_t{-1})
       : path(path),
         kp(kp),
         ki(ki),
@@ -59,14 +64,23 @@ struct PIDConfig {
  public:
   void RegisterNT() {
     auto table = nt::NetworkTableInstance::GetDefault().GetTable(path);
-    _nt_bindings.emplace_back(std::make_shared<NTBoundUnit<typename kp_t::unit_type>>(table, "kP", kp));
-    _nt_bindings.emplace_back(std::make_shared<NTBoundUnit<typename ki_t::unit_type>>(table, "kI", ki));
-    _nt_bindings.emplace_back(std::make_shared<NTBoundUnit<typename kd_t::unit_type>>(table, "kD", kd));
     _nt_bindings.emplace_back(
-        std::make_shared<NTBoundUnit<typename error_t::unit_type>>(table, "stableThresh", stableThresh));
-    _nt_bindings.emplace_back(std::make_shared<NTBoundUnit<typename deriv_t::unit_type>>(
-        table, "stableThreshVelocity", stableDerivThresh));
-    _nt_bindings.emplace_back(std::make_shared<NTBoundUnit<IN>>(table, "izone", izone));
+        std::make_shared<NTBoundUnit<typename kp_t::unit_type>>(table, "kP",
+                                                                kp));
+    _nt_bindings.emplace_back(
+        std::make_shared<NTBoundUnit<typename ki_t::unit_type>>(table, "kI",
+                                                                ki));
+    _nt_bindings.emplace_back(
+        std::make_shared<NTBoundUnit<typename kd_t::unit_type>>(table, "kD",
+                                                                kd));
+    _nt_bindings.emplace_back(
+        std::make_shared<NTBoundUnit<typename error_t::unit_type>>(
+            table, "stableThresh", stableThresh));
+    _nt_bindings.emplace_back(
+        std::make_shared<NTBoundUnit<typename deriv_t::unit_type>>(
+            table, "stableThreshVelocity", stableDerivThresh));
+    _nt_bindings.emplace_back(
+        std::make_shared<NTBoundUnit<IN>>(table, "izone", izone));
   }
 };
 
@@ -80,15 +94,19 @@ class PIDController {
 
   config_t config;
 
-  PIDController(std::string path, config_t initialGains, in_t setpoint = in_t{0})
+  PIDController(std::string path, config_t initialGains,
+                in_t setpoint = in_t{0})
       : config(initialGains),
         _setpoint(setpoint),
-        _posFilter(frc::LinearFilter<typename config_t::error_t>::MovingAverage(20)),
-        _velFilter(frc::LinearFilter<typename config_t::deriv_t>::MovingAverage(20)),
+        _posFilter(
+            frc::LinearFilter<typename config_t::error_t>::MovingAverage(20)),
+        _velFilter(
+            frc::LinearFilter<typename config_t::deriv_t>::MovingAverage(20)),
         _table(nt::NetworkTableInstance::GetDefault().GetTable(path)) {}
 
   void SetSetpoint(in_t setpoint) {
-    if (std::abs(setpoint.value() - _setpoint.value()) > std::abs(0.1 * _setpoint.value())) {
+    if (std::abs(setpoint.value() - _setpoint.value()) >
+        std::abs(0.1 * _setpoint.value())) {
       _iterations = 0;
     }
     _setpoint = setpoint;
@@ -103,9 +121,13 @@ class PIDController {
   void Reset() { _integralSum = sum_t{0}; }
 
   out_t Calculate(in_t pv, units::second_t dt, out_t feedforward = out_t{0}) {
+    pv = units::math::fabs(pv);
     auto error = do_wrap(_setpoint - pv);
+    error = units::math::fabs(error);
     _integralSum += error * dt;
-    if (config.izone.value() > 0 && (error > config.izone || error < -config.izone))
+    _integralSum = units::math::fabs(_integralSum);
+    if (config.izone.value() > 0 &&
+        (error > config.izone || error < -config.izone))
       _integralSum = sum_t{0};
 
     typename config_t::deriv_t deriv{0};
@@ -116,16 +138,8 @@ class PIDController {
     _stablePos = _posFilter.Calculate(error);
     _stableVel = _velFilter.Calculate(deriv);
 
-    auto out = config.kp * error + config.ki * _integralSum + config.kd * deriv + feedforward;
-    // std::cout << "Out value" << out.value() << std::endl;
-
-    _table->GetEntry("pv").SetDouble(pv.value());
-    _table->GetEntry("dt").SetDouble(dt.value());
-    _table->GetEntry("setpoint").SetDouble(_setpoint.value());
-    _table->GetEntry("error").SetDouble(error.value());
-    _table->GetEntry("integralSum").SetDouble(_integralSum.value());
-    _table->GetEntry("stable").SetBoolean(IsStable());
-    _table->GetEntry("demand").SetDouble(out.value());
+    auto out = config.kp * error + config.ki * _integralSum +
+               config.kd * deriv + feedforward;
 
     _last_pv = pv;
     _last_error = error;
@@ -133,8 +147,10 @@ class PIDController {
     return out;
   }
 
-  bool IsStable(std::optional<typename config_t::error_t> stableThreshOverride = {},
-                std::optional<typename config_t::deriv_t> velocityThreshOverride = {}) const {
+  bool IsStable(
+      std::optional<typename config_t::error_t> stableThreshOverride = {},
+      std::optional<typename config_t::deriv_t> velocityThreshOverride = {})
+      const {
     auto stableThresh = config.stableThresh;
     auto stableDerivThresh = config.stableDerivThresh;
 
@@ -143,8 +159,10 @@ class PIDController {
     if (velocityThreshOverride.has_value())
       stableDerivThresh = velocityThreshOverride.value();
 
-    return _iterations > 20 && std::abs(_stablePos.value()) <= std::abs(stableThresh.value()) &&
-           (stableDerivThresh.value() < 0 || std::abs(_stableVel.value()) <= stableDerivThresh.value());
+    return _iterations > 20 &&
+           std::abs(_stablePos.value()) <= std::abs(stableThresh.value()) &&
+           (stableDerivThresh.value() < 0 ||
+            std::abs(_stableVel.value()) <= stableDerivThresh.value());
   }
 
  private:
diff --git a/wombat/src/main/include/utils/Util.h b/wombat/src/main/include/utils/Util.h
index d624e408..665c85ec 100644
--- a/wombat/src/main/include/utils/Util.h
+++ b/wombat/src/main/include/utils/Util.h
@@ -28,30 +28,39 @@ units::second_t now();
 
 class NTBound {
  public:
-  NTBound(std::shared_ptr<nt::NetworkTable> table, std::string name, const nt::Value& value,
+  NTBound(std::shared_ptr<nt::NetworkTable> table, std::string name,
+          const nt::Value& value,
           std::function<void(const nt::Value&)> onUpdateFn)
-      : _table(table), _entry(table->GetEntry(name)), _onUpdate(onUpdateFn), _name(name) {
+      : _table(table),
+        _entry(table->GetEntry(name)),
+        _onUpdate(onUpdateFn),
+        _name(name) {
     _entry.SetValue(value);
     // _listener = table->AddListener(name, , ([=](const nt::EntryNotification
     // &evt) {
     //   this->_onUpdate(evt.value);
     // }, NT_NOTIFY_UPDATE);
-    _listener =
-        table->AddListener(name, nt::EventFlags::kValueAll,
-                           ([this](nt::NetworkTable* table, std::string_view key, const nt::Event& event) {
-                             std::cout << "NT UPDATE" << std::endl;
-                             this->_onUpdate(event.GetValueEventData()->value);
-                           }));
+    _listener = table->AddListener(
+        name, nt::EventFlags::kValueAll,
+        ([this](nt::NetworkTable* table, std::string_view key,
+                const nt::Event& event) {
+          std::cout << "NT UPDATE" << std::endl;
+          this->_onUpdate(event.GetValueEventData()->value);
+        }));
   }
 
   NTBound(const NTBound& other)
-      : _table(other._table), _entry(other._entry), _onUpdate(other._onUpdate), _name(other._name) {
-    _listener =
-        _table->AddListener(_name, nt::EventFlags::kValueAll,
-                            ([this](nt::NetworkTable* table, std::string_view key, const nt::Event& event) {
-                              std::cout << "NT UPDATE" << std::endl;
-                              this->_onUpdate(event.GetValueEventData()->value);
-                            }));
+      : _table(other._table),
+        _entry(other._entry),
+        _onUpdate(other._onUpdate),
+        _name(other._name) {
+    _listener = _table->AddListener(
+        _name, nt::EventFlags::kValueAll,
+        ([this](nt::NetworkTable* table, std::string_view key,
+                const nt::Event& event) {
+          std::cout << "NT UPDATE" << std::endl;
+          this->_onUpdate(event.GetValueEventData()->value);
+        }));
   }
 
   ~NTBound() { _table->RemoveListener(_listener); }
@@ -66,7 +75,8 @@ class NTBound {
 
 class NTBoundDouble : public NTBound {
  public:
-  NTBoundDouble(std::shared_ptr<nt::NetworkTable> table, std::string name, double& val)
+  NTBoundDouble(std::shared_ptr<nt::NetworkTable> table, std::string name,
+                double& val)
       : NTBound(table, name, nt::Value::MakeDouble(val),
                 [&val](const nt::Value& v) { val = v.GetDouble(); }) {}
 };
@@ -74,15 +84,20 @@ class NTBoundDouble : public NTBound {
 template <typename T>
 class NTBoundUnit : public NTBound {
  public:
-  NTBoundUnit(std::shared_ptr<nt::NetworkTable> table, std::string name, units::unit_t<T>& val)
+  NTBoundUnit(std::shared_ptr<nt::NetworkTable> table, std::string name,
+              units::unit_t<T>& val)
       : NTBound(table, name, nt::Value::MakeDouble(val.value()),
-                [&val](const nt::Value& v) { val = units::unit_t<T>{v.GetDouble()}; }) {}
+                [&val](const nt::Value& v) {
+                  val = units::unit_t<T>{v.GetDouble()};
+                }) {}
 };
 
 void WritePose2NT(std::shared_ptr<nt::NetworkTable> table, frc::Pose2d pose);
 void WritePose3NT(std::shared_ptr<nt::NetworkTable> table, frc::Pose3d pose);
-void WriteTrajectory(std::shared_ptr<nt::NetworkTable> table, frc::Trajectory trajectory);
-void WriteTrajectoryState(std::shared_ptr<nt::NetworkTable> table, frc::Trajectory::State state);
+void WriteTrajectory(std::shared_ptr<nt::NetworkTable> table,
+                     frc::Trajectory trajectory);
+void WriteTrajectoryState(std::shared_ptr<nt::NetworkTable> table,
+                          frc::Trajectory::State state);
 
 frc::Pose2d TrajectoryStateToPose2d(frc::Trajectory::State state);
 
diff --git a/wombat/src/main/include/vision/Limelight.h b/wombat/src/main/include/vision/Limelight.h
index 5aa6d0b7..ddfb3739 100644
--- a/wombat/src/main/include/vision/Limelight.h
+++ b/wombat/src/main/include/vision/Limelight.h
@@ -26,11 +26,20 @@
 namespace wom {
 namespace vision {
 
-enum class LimelightLEDMode { kPipelineDefault = 0, kForceOff = 1, kForceBlink = 2, kForceOn = 3 };
+enum class LimelightLEDMode {
+  kPipelineDefault = 0,
+  kForceOff = 1,
+  kForceBlink = 2,
+  kForceOn = 3
+};
 
 enum class LimelightCamMode { kVisionProcessor = 0, kDriverCamera = 1 };
 
-enum class LimelightStreamMode { kStandard = 0, kPiPMain = 1, kPiPSecondary = 2 };
+enum class LimelightStreamMode {
+  kStandard = 0,
+  kPiPMain = 1,
+  kPiPSecondary = 2
+};
 
 enum class LimelightSnapshotMode { kReset = 0, kSingle = 1 };
 
@@ -82,12 +91,14 @@ class Limelight {
 
   std::string GetName();
 
-  std::shared_ptr<nt::NetworkTable> table = nt::NetworkTableInstance::GetDefault().GetTable("limelight");
+  std::shared_ptr<nt::NetworkTable> table =
+      nt::NetworkTableInstance::GetDefault().GetTable("limelight");
 
   std::pair<double, double> GetOffset();
 
   std::vector<double> GetAprilTagData(LimelightAprilTagData dataType);
-  double GetTargetingData(LimelightTargetingData dataType, double defaultValue = 0.0);
+  double GetTargetingData(LimelightTargetingData dataType,
+                          double defaultValue = 0.0);
   void SetLEDMode(LimelightLEDMode mode);
   void SetCamMode(LimelightCamMode mode);
   void SetPipeline(LimelightPipeline pipeline);
@@ -100,7 +111,8 @@ class Limelight {
 
   bool IsAtSetPoseVision(frc::Pose3d pose, units::second_t dt);
 
-  units::meters_per_second_t GetSpeed(frc::Pose3d pose1, frc::Pose3d pose2, units::second_t dt);
+  units::meters_per_second_t GetSpeed(frc::Pose3d pose1, frc::Pose3d pose2,
+                                      units::second_t dt);
 
   frc::Pose3d GetPose();
 

From d9683c73290ba263b2215e5ac39a9912f16f885e Mon Sep 17 00:00:00 2001
From: totallysomeoneyoudontknow
 <155517980+totallysomeoneyoudontknow@users.noreply.github.com>
Date: Tue, 23 Jan 2024 20:34:07 +0800
Subject: [PATCH 098/207] Shooter - Implements Networking tables (#99)

* added the state raw and changed voltage

* test

* added the state raw and changed voltage

* added pid calculate loop, fixed indenting

* added calculate function on a new line, changed the PID to not stable

* Fixed the if condition so shooterSensor is not tripped

* changed the functions so they start with a capital

* changed the value for shooterMotor

* added shuffleboard and networking tables

* added shuffleboard and networking tables

* added network tables

* fixed merge conflicts and merge errors

* fixed merge conflicts and merge errors

* fixed merge conflicts and merge errors

* fixed errors

* ran wpiformat

* fixed merge errors and conflicts

* fixed github errors

* Alpha Bot Shooter

---------

Co-authored-by: Havish12 <havishchopra10@gmail.com>
Co-authored-by: Isaac Turner <spacey_sooty@outlook.com>
---
 src/main/cpp/Robot.cpp              | 21 ++++----
 src/main/cpp/Shooter.cpp            | 79 +++++++++++++++++++++++++++++
 src/main/cpp/ShooterBehaviour.cpp   | 39 ++++++++++++++
 src/main/include/Robot.h            | 13 +++--
 src/main/include/RobotMap.h         | 21 +++++++-
 src/main/include/Shooter.h          | 43 ++++++++++++++++
 src/main/include/ShooterBehaviour.h | 24 +++++++++
 7 files changed, 222 insertions(+), 18 deletions(-)
 create mode 100644 src/main/cpp/Shooter.cpp
 create mode 100644 src/main/cpp/ShooterBehaviour.cpp
 create mode 100644 src/main/include/Shooter.h
 create mode 100644 src/main/include/ShooterBehaviour.h

diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp
index ddccb14d..0dc53d5f 100644
--- a/src/main/cpp/Robot.cpp
+++ b/src/main/cpp/Robot.cpp
@@ -20,6 +20,12 @@
 static units::second_t lastPeriodic;
 
 void Robot::RobotInit() {
+
+  shooter = new Shooter(robotmap.shooterSystem.config);
+  wom::BehaviourScheduler::GetInstance()->Register(shooter);
+  shooter->SetDefaultBehaviour(
+     [this]() {return wom::make<ShooterManualControl>(shooter, &robotmap.controllers.codriver); });
+  
   sched = wom::BehaviourScheduler::GetInstance();
   m_chooser.SetDefaultOption("Default Auto", "Default Auto");
 
@@ -73,6 +79,8 @@ void Robot::RobotPeriodic() {
   lastPeriodic = wom::now();
 
   loop.Poll();
+  wom::BehaviourScheduler::GetInstance()->Tick();
+  shooter->OnUpdate(dt);
   sched->Tick();
 
   robotmap.swerveTable.swerveDriveTable->GetEntry("frontLeftEncoder").SetDouble(robotmap.swerveBase.moduleConfigs[0].turnMotor.encoder->GetEncoderPosition().value());
@@ -84,25 +92,20 @@ void Robot::RobotPeriodic() {
 }
 
 void Robot::AutonomousInit() {
-  // m_driveSim->SetPath(m_path_chooser.GetSelected());
-
   loop.Clear();
   sched->InterruptAll();
 }
 void Robot::AutonomousPeriodic() {
-  // m_driveSim->OnUpdate();
 }
 
 void Robot::TeleopInit() {
   loop.Clear();
+  wom::BehaviourScheduler *sched = wom::BehaviourScheduler::GetInstance();
   sched->InterruptAll();
-
-  // frontLeft->SetVoltage(4_V);
-  // frontRight->SetVoltage(4_V);
-  // backLeft->SetVoltage(4_V);
-  // backRight->SetVoltage(4_V);
 }
-void Robot::TeleopPeriodic() {}
+void Robot::TeleopPeriodic() {
+  
+}
 
 void Robot::DisabledInit() {}
 void Robot::DisabledPeriodic() {}
diff --git a/src/main/cpp/Shooter.cpp b/src/main/cpp/Shooter.cpp
new file mode 100644
index 00000000..613120df
--- /dev/null
+++ b/src/main/cpp/Shooter.cpp
@@ -0,0 +1,79 @@
+// Copyright (c) 2023-2024 CurtinFRC
+// Open Source Software, you can modify it according to the terms
+// of the MIT License at the root of this project
+
+#include "Shooter.h"
+
+Shooter::Shooter(ShooterConfig config) : _config(config)
+// , 
+// _pid{frc::PIDController (1, 0, 0, 0.005_s)}
+{} //config.path + "/pid", config.pidConfig
+void Shooter::OnUpdate(units::second_t dt) {
+  // _pid.SetTolerance(0.1, 1);
+  switch (_state) {
+    case ShooterState::kIdle: {
+      std::cout << "KIdle" << std::endl;
+      _setVoltage = 0_V;
+      // if (_shooterSensor.Get()) {
+      //   _state = ShooterState::kReverse;
+      // }
+    } break;
+    case ShooterState::kSpinUp: {
+      // std::cout << "KSpinUp" << std::endl;
+      // _pid.SetSetpoint(_goal.value());
+      // units::volt_t pidCalculate =
+      //     units::volt_t{_pid.Calculate(_config.ShooterGearbox.encoder->GetEncoderAngularVelocity().value())};
+      // _setVoltage = pidCalculate;
+
+      // if (_pid.AtSetpoint()) {
+      //   SetState(ShooterState::kShooting);
+      // }
+    } break;
+    case ShooterState::kShooting: {
+      // std::cout << "KShooting" << std::endl;
+      // _pid.SetSetpoint(_goal.value());
+      // units::volt_t pidCalculate =
+      //     units::volt_t{_pid.Calculate(_config.ShooterGearbox.encoder->GetEncoderAngularVelocity().value())};
+      // _setVoltage = pidCalculate;
+
+      // if (!_pid.AtSetpoint()) {
+      //   SetState(ShooterState::kSpinUp);
+      // }
+      // if (_shooterSensor.Get()) {
+      //   SetState(ShooterState::kIdle);
+      // }
+    } break;
+
+    case ShooterState::kReverse: {
+      _setVoltage = -5_V;
+      std::cout << "KReverse" << std::endl;
+      // if (!_shooterSensor.Get()) {
+      //   SetState(ShooterState::kIdle);
+      // }
+    } break;
+    case ShooterState::kRaw: {
+      _setVoltage = _rawVoltage;
+      std::cout << "KRaw" << std::endl;
+      // if (_shooterSensor.Get()) {
+      //   SetState(ShooterState::kRaw);
+      // }
+    } break;
+    default: {
+      std::cout << "Error shooter in invalid state" << std::endl;
+    } break;
+  }
+  std::cout << "Voltage:" << _setVoltage.value() << std::endl;
+  _config.ShooterGearbox.motorController->SetVoltage(_setVoltage);
+
+}
+
+void Shooter::SetState(ShooterState state) {
+  _state = state;
+}
+void Shooter::SetRaw(units::volt_t voltage) {
+  _rawVoltage = voltage;
+  _state = ShooterState::kRaw;
+}
+void Shooter::SetPidGoal(units::radians_per_second_t goal) {
+  _goal = goal;
+}
diff --git a/src/main/cpp/ShooterBehaviour.cpp b/src/main/cpp/ShooterBehaviour.cpp
new file mode 100644
index 00000000..1a035ae0
--- /dev/null
+++ b/src/main/cpp/ShooterBehaviour.cpp
@@ -0,0 +1,39 @@
+// Copyright (c) 2023-2024 CurtinFRC
+// Open Source Software, you can modify it according to the terms
+// of the MIT License at the root of this project
+
+#include "ShooterBehaviour.h"
+
+ShooterManualControl::ShooterManualControl(Shooter* shooter, frc::XboxController* codriver)
+    : _shooter(shooter), _codriver(codriver) {
+  Controls(shooter);
+}
+
+void ShooterManualControl::OnTick(units::second_t dt) {
+  _shooter->table->GetEntry("RawControl").SetBoolean(_rawControl);
+
+
+  if (_codriver->GetAButtonReleased()) {
+    if (_rawControl) {
+      _rawControl = false;
+    } else {
+      _rawControl = true;
+    }
+  }
+
+  if (!_rawControl) {
+      if (_codriver->GetYButton()) {
+        _shooter->SetState(ShooterState::kSpinUp);
+        _shooter->SetPidGoal(10_rad_per_s);
+      }
+  } else {
+    if (_codriver->GetRightTriggerAxis() > 0.1) {
+      _shooter->SetRaw(_codriver->GetRightTriggerAxis() * 12_V);
+    } else if (_codriver->GetLeftTriggerAxis() > 0.1) {
+      _shooter->SetRaw(_codriver->GetLeftTriggerAxis() * -12_V);
+    } else {
+      _shooter->SetRaw(0_V);
+    }
+    _shooter->SetState(ShooterState::kRaw);
+  }
+}
diff --git a/src/main/include/Robot.h b/src/main/include/Robot.h
index 9cef2cb1..1806c61c 100644
--- a/src/main/include/Robot.h
+++ b/src/main/include/Robot.h
@@ -1,9 +1,7 @@
 // Copyright (c) 2023-2024 CurtinFRC
 // Open Source Software, you can modify it according to the terms
 // of the MIT License at the root of this project
-
 #pragma once
-
 #include <frc/Encoder.h>
 #include <frc/TimedRobot.h>
 #include <frc/Timer.h>
@@ -13,14 +11,16 @@
 #include <frc/smartdashboard/Field2d.h>
 #include <frc/smartdashboard/SendableChooser.h>
 #include <frc/smartdashboard/SmartDashboard.h>
-
 #include <string>
-
 #include "RobotMap.h"
+#include "Shooter.h"
+#include "ShooterBehaviour.h"
 #include "Wombat.h"
-
+#include "RobotMap.h"
 class Robot : public frc::TimedRobot {
  public:
+  void TestInit() override;
+  void TestPeriodic() override;
   void RobotInit() override;
   void RobotPeriodic() override;
   void AutonomousInit() override;
@@ -29,12 +29,11 @@ class Robot : public frc::TimedRobot {
   void TeleopPeriodic() override;
   void DisabledInit() override;
   void DisabledPeriodic() override;
-  void TestInit() override;
-  void TestPeriodic() override;
  private:
   RobotMap robotmap;
   wom::BehaviourScheduler* sched;
   frc::EventLoop loop;
+  Shooter *shooter;
 
   frc::SendableChooser<std::string> m_chooser;
 
diff --git a/src/main/include/RobotMap.h b/src/main/include/RobotMap.h
index 6e64d5e8..3ae4db10 100644
--- a/src/main/include/RobotMap.h
+++ b/src/main/include/RobotMap.h
@@ -16,16 +16,33 @@
 #include <ctre/phoenix6/Pigeon2.hpp>
 #include <ctre/phoenix6/TalonFX.hpp>
 
+#include "Shooter.h"
 #include "Wombat.h"
 
 struct RobotMap {
   struct Controllers {
     frc::XboxController driver = frc::XboxController(0);
-    frc::XboxController coDriver = frc::XboxController(1);
+    frc::XboxController codriver = frc::XboxController(1);
     frc::XboxController testController = frc::XboxController(2);
   };
   Controllers controllers;
 
+  struct Shooter {
+    rev::CANSparkMax shooterMotor{11, rev::CANSparkMax::MotorType::kBrushless};
+    // frc::DigitalInput shooterSensor{2};
+
+    // wom::VoltageController shooterMotorGroup = wom::VoltageController::Group(shooterMotor);
+    // wom::CANSparkMaxEncoder* shooterEncoder = new wom::CANSparkMaxEncoder(&shooterMotor, 0.01_m);
+    wom::Gearbox shooterGearbox{&shooterMotor, nullptr, frc::DCMotor::NEO(1)};
+
+    ShooterConfig config{
+        "shooterGearbox",
+        shooterGearbox,
+        // &shooterSensor,
+    };
+  };
+  Shooter shooterSystem;
+  
   struct SwerveBase {
     ctre::phoenix6::hardware::CANcoder frontLeftCancoder{18, "Drivebase"};
     ctre::phoenix6::hardware::CANcoder frontRightCancoder{19, "Drivebase"};
@@ -141,4 +158,4 @@ struct RobotMap {
   struct SwerveTable {
     std::shared_ptr<nt::NetworkTable> swerveDriveTable = nt::NetworkTableInstance::GetDefault().GetTable("swerve");
   }; SwerveTable swerveTable;
-};
\ No newline at end of file
+};
diff --git a/src/main/include/Shooter.h b/src/main/include/Shooter.h
new file mode 100644
index 00000000..cf21c3bc
--- /dev/null
+++ b/src/main/include/Shooter.h
@@ -0,0 +1,43 @@
+// Copyright (c) 2023-2024 CurtinFRC
+// Open Source Software, you can modify it according to the terms
+// of the MIT License at the root of this project
+
+#pragma once
+#include <frc/DigitalInput.h>
+#include <frc/controller/PIDController.h>
+#include <units/angular_velocity.h>
+
+#include <memory>
+#include <string>
+
+#include "Wombat.h"
+
+struct ShooterConfig {
+  std::string path;
+  wom::Gearbox ShooterGearbox;
+  // wom::PIDConfig<units::radians_per_second, units::volt> pidConfig;
+  // frc::DigitalInput* shooterSensor;
+};
+
+enum class ShooterState { kIdle, kShooting, kSpinUp, kReverse, kRaw };
+
+class Shooter : public behaviour::HasBehaviour {
+ public:
+  explicit Shooter(ShooterConfig config);
+
+  void OnUpdate(units::second_t dt);
+  void SetState(ShooterState state);
+  void SetRaw(units::volt_t voltage);
+  void SetPidGoal(units::radians_per_second_t);
+  std::shared_ptr<nt::NetworkTable> table = nt::NetworkTableInstance::GetDefault().GetTable("Shooter");
+  ShooterConfig GetConfig() { return _config; }
+
+ private:
+  ShooterConfig _config;
+  ShooterState _state = ShooterState::kRaw;
+  units::volt_t _rawVoltage;
+  units::radians_per_second_t _goal;
+  units::volt_t _setVoltage = 0_V;
+  // frc::PIDController _pid;
+  // frc::DigitalInput _shooterSensor{0};
+};
diff --git a/src/main/include/ShooterBehaviour.h b/src/main/include/ShooterBehaviour.h
new file mode 100644
index 00000000..ff65b1d8
--- /dev/null
+++ b/src/main/include/ShooterBehaviour.h
@@ -0,0 +1,24 @@
+// Copyright (c) 2023-2024 CurtinFRC
+// Open Source Software, you can modify it according to the terms
+// of the MIT License at the root of this project
+
+#pragma once
+
+#include <frc/XboxController.h>
+
+#include "Robot.h"
+#include "Shooter.h"
+#include "Wombat.h"
+
+class ShooterManualControl : public behaviour::Behaviour {
+ public:
+  ShooterManualControl(Shooter* shooter, frc::XboxController* codriver);
+
+  void OnTick(units::second_t dt) override;
+
+ private:
+  Shooter* _shooter;
+  frc::XboxController* _codriver;
+
+  bool _rawControl = true;
+};

From 7bc0eccf0a2d6d19f7359309fda3bae327746b25 Mon Sep 17 00:00:00 2001
From: kill-shots <155516223+kill-shots@users.noreply.github.com>
Date: Tue, 23 Jan 2024 20:47:00 +0800
Subject: [PATCH 099/207] Intake (#100)

* First version Tank Drive

* Intake draft

* Intake draft fixed errors 1

* Intake draft fixed errors 2

* Intake draft fixed errors 3

* Intake draft fixed errors 4

* Intake draft fixed errors 5

* Network files 1

* Network files 2

* Network files 3

* Almost done i think

* finished wpiformat errors

* finished wpiformat errors 2

* finished wpiformat errors 3

* finished wpiformat errors 4

* ran wpiformat

* finished wpiformat errors 5

* finished wpiformat errors 8

* finished wpiformat errors 9

* finished wpiformat errors 10

* Tested on robot

* Fixed github build errors after modifying during robot tests

* fixed more github build errors 2

* fixed more github build errors 3

* Tested on robot and works

---------

Co-authored-by: Isaac Turner <spacey_sooty@outlook.com>
---
 src/main/cpp/Intake.cpp            | 74 ++++++++++++++++++++++++++++++
 src/main/cpp/IntakeBehaviour.cpp   | 55 ++++++++++++++++++++++
 src/main/cpp/Robot.cpp             | 33 ++++++++++---
 src/main/include/Intake.h          | 41 +++++++++++++++++
 src/main/include/IntakeBehaviour.h | 34 ++++++++++++++
 src/main/include/Robot.h           |  4 ++
 src/main/include/RobotMap.h        | 19 ++++++--
 7 files changed, 250 insertions(+), 10 deletions(-)
 create mode 100644 src/main/cpp/Intake.cpp
 create mode 100644 src/main/cpp/IntakeBehaviour.cpp
 create mode 100644 src/main/include/Intake.h
 create mode 100644 src/main/include/IntakeBehaviour.h

diff --git a/src/main/cpp/Intake.cpp b/src/main/cpp/Intake.cpp
new file mode 100644
index 00000000..75839209
--- /dev/null
+++ b/src/main/cpp/Intake.cpp
@@ -0,0 +1,74 @@
+// Copyright (c) 2023-2024 CurtinFRC
+// Open Source Software, you can modify it according to the terms
+// of the MIT License at the root of this project
+
+#include "Intake.h"
+
+Intake::Intake(IntakeConfig config) : _config(config) {}
+
+IntakeConfig Intake::GetConfig() {
+  return _config;
+}
+
+void Intake::OnUpdate(units::second_t dt) {
+  switch (_state) {
+    case IntakeState::kIdle: {
+      // _config.IntakeMotor.motorController->SetVoltage(0_V);
+      // if (_config.intakeSensor->Get()) {
+      //   setState(IntakeState::kHold);
+      // }
+      _stringStateName = "Idle";
+      _setVoltage = 0_V;
+    } break;
+    case IntakeState::kRaw: {
+      // _config.IntakeMotor.motorController->SetVoltage(_rawVoltage);
+      _stringStateName = "Raw";
+      _setVoltage = _rawVoltage;
+    } break;
+    case IntakeState::kEject: {
+      // _config.IntakeMotor.motorController->SetVoltage(-5_V);
+      // if (_config.intakeSensor->Get() == 0 && _config.magSensor->Get() == 0) {
+      //   setState(IntakeState::kIdle);
+      // }
+      _stringStateName = "Eject";
+      _setVoltage = -5_V;
+    } break;
+    case IntakeState::kHold: {
+      // _config.IntakeMotor.motorController->SetVoltage(0_V);
+      _stringStateName = "Hold";
+      _setVoltage = 0_V;
+    } break;
+    case IntakeState::kIntake: {
+      // _config.IntakeMotor.motorController->SetVoltage(5_V);
+      _stringStateName = "Intake";
+      _setVoltage = 5_V;
+    } break;
+    case IntakeState::kPass: {
+      // _config.IntakeMotor.motorController->SetVoltage(5_V);
+      // if (_config.shooterSensor->Get()) {
+      //   setState(IntakeState::kIdle);
+      //   _stringStateName = "Pass";
+      // }
+      _setVoltage = 5_V;
+    } break;
+    default:
+      std::cout << "Error: Intake in INVALID STATE." << std::endl;
+      break;
+  }
+  _table->GetEntry("State: ").SetString(_stringStateName);
+  _table->GetEntry("Motor Voltage: ").SetDouble(_setVoltage.value());
+  // _table->GetEntry("Intake Sensor: ").SetBoolean(_config.intakeSensor->Get());
+  // _table->GetEntry("Shooter Sensor: ").SetBoolean(_config.shooterSensor->Get());
+  // _table->GetEntry("Magazine Sensor: ").SetBoolean(_config.magSensor->Get());
+
+  std::cout << _setVoltage.value() << std::endl;
+
+  _config.IntakeMotor.motorController->SetVoltage(_setVoltage);
+}
+
+void Intake::setState(IntakeState state) {
+  _state = state;
+}
+void Intake::setRaw(units::volt_t voltage) {
+  _rawVoltage = voltage;
+}
diff --git a/src/main/cpp/IntakeBehaviour.cpp b/src/main/cpp/IntakeBehaviour.cpp
new file mode 100644
index 00000000..90867045
--- /dev/null
+++ b/src/main/cpp/IntakeBehaviour.cpp
@@ -0,0 +1,55 @@
+// Copyright (c) 2023-2024 CurtinFRC
+// Open Source Software, you can modify it according to the terms
+// of the MIT License at the root of this project
+
+#include "IntakeBehaviour.h"
+
+#include <frc/XboxController.h>
+
+IntakeManualControl::IntakeManualControl(Intake* intake, frc::XboxController& codriver)
+    : _intake(intake), _codriver(codriver) {
+  Controls(intake);
+}
+
+void IntakeManualControl::OnTick(units::second_t dt) {
+  if (_codriver.GetBButtonPressed()) {
+    if (_rawControl == true) {
+      _rawControl = false;
+    } else {
+      _rawControl = true;
+    }
+  }
+
+  if (_rawControl) {
+    _intake->setState(IntakeState::kRaw);
+    if (_codriver.GetLeftBumper()) {
+      _intake->setRaw(10_V);
+    } else if (_codriver.GetRightBumper()) {
+      _intake->setRaw(-10_V);
+    } else {
+      _intake->setRaw(0_V);
+    }
+
+    // _intake->setRaw(_codriver.GetLeftBumper() * 10_V);
+    // _intake->setRaw(_codriver.GetRightBumper() * -10_V);
+    std::cout << "Raw" << std::endl;
+  }
+  // } else {
+  //   if (_codriver.GetYButtonPressed()) {
+  //     _intake->setState(IntakeState::kIntake);
+  //   }
+  //   if (_codriver.GetAButtonPressed()) {
+  //     _intake->setState(IntakeState::kPass);
+  //   }
+  // }
+}
+
+// IntakeAutoControl::IntakeAutoControl(Intake* intake) : _intake(intake) {}
+
+// void IntakeAutoControl::OnTick(units::second_t dt) {
+//   if (_intake->GetConfig().intakeSensor->Get() == 1) {
+//     _intake->setState(IntakeState::kPass);
+//   } else if (_intake->GetConfig().magSensor->Get() == 0) {
+//     _intake->setState(IntakeState::kIdle);
+//   }
+// }
diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp
index 0dc53d5f..9cef56a6 100644
--- a/src/main/cpp/Robot.cpp
+++ b/src/main/cpp/Robot.cpp
@@ -4,12 +4,18 @@
 
 #include "Robot.h"
 
-// include units
-#include <units/velocity.h>
+#include <frc/TimedRobot.h>
+#include <frc/Timer.h>
+#include <frc/controller/RamseteController.h>
+#include <frc/kinematics/DifferentialDriveKinematics.h>
+#include <networktables/DoubleTopic.h>
+#include <networktables/NetworkTable.h>
+#include <networktables/NetworkTableInstance.h>
 #include <units/acceleration.h>
-#include <units/length.h>
 #include <units/angle.h>
+#include <units/length.h>
 #include <units/time.h>
+#include <units/velocity.h>
 #include <units/voltage.h>
 
 #include <frc/kinematics/DifferentialDriveKinematics.h>
@@ -72,6 +78,12 @@ void Robot::RobotInit() {
   // frontRight = new ctre::phoenix6::hardware::TalonFX(1, "Drivebase");   // front right
   // backLeft = new ctre::phoenix6::hardware::TalonFX(5, "Drivebase");   // back left
   // backRight = new ctre::phoenix6::hardware::TalonFX(3, "Drivebase");
+  lastPeriodic = wom::now();
+
+  intake = new Intake(robotmap.intakeSystem.config);
+  wom::BehaviourScheduler::GetInstance()->Register(intake);
+  intake->SetDefaultBehaviour(
+      [this]() { return wom::make<IntakeManualControl>(intake, robotmap.controllers.codriver); });
 }
 
 void Robot::RobotPeriodic() {
@@ -88,7 +100,7 @@ void Robot::RobotPeriodic() {
   robotmap.swerveTable.swerveDriveTable->GetEntry("backLeftEncoder").SetDouble(robotmap.swerveBase.moduleConfigs[2].turnMotor.encoder->GetEncoderPosition().value());
   robotmap.swerveTable.swerveDriveTable->GetEntry("backRightEncoder").SetDouble(robotmap.swerveBase.moduleConfigs[3].turnMotor.encoder->GetEncoderPosition().value());
 
-  _swerveDrive->OnUpdate(dt);
+  intake->OnUpdate(dt);
 }
 
 void Robot::AutonomousInit() {
@@ -102,10 +114,17 @@ void Robot::TeleopInit() {
   loop.Clear();
   wom::BehaviourScheduler *sched = wom::BehaviourScheduler::GetInstance();
   sched->InterruptAll();
+
+  // frontLeft->SetVoltage(4_V);
+  // frontRight->SetVoltage(4_V);
+  // backLeft->SetVoltage(4_V);
+  // backRight->SetVoltage(4_V);
+  loop.Clear();
+  wom::BehaviourScheduler* scheduler = wom::BehaviourScheduler::GetInstance();
+  scheduler->InterruptAll();
 }
-void Robot::TeleopPeriodic() {
-  
-}
+
+void Robot::TeleopPeriodic() {}
 
 void Robot::DisabledInit() {}
 void Robot::DisabledPeriodic() {}
diff --git a/src/main/include/Intake.h b/src/main/include/Intake.h
new file mode 100644
index 00000000..a84b41b1
--- /dev/null
+++ b/src/main/include/Intake.h
@@ -0,0 +1,41 @@
+// Copyright (c) 2023-2024 CurtinFRC
+// Open Source Software, you can modify it according to the terms
+// of the MIT License at the root of this project
+
+#pragma once
+
+#include <frc/DigitalInput.h>
+
+#include <memory>
+#include <string>
+
+#include "Wombat.h"
+
+struct IntakeConfig {
+  wom::Gearbox IntakeMotor;
+  frc::DigitalInput* intakeSensor;
+  frc::DigitalInput* magSensor;
+  frc::DigitalInput* shooterSensor;
+};
+
+enum class IntakeState { kIdle, kRaw, kHold, kEject, kIntake, kPass };
+
+class Intake : public behaviour::HasBehaviour {
+ public:
+  explicit Intake(IntakeConfig config);
+
+  void OnUpdate(units::second_t dt);
+
+  void setState(IntakeState state);
+  void setRaw(units::volt_t voltage);
+  IntakeConfig GetConfig();
+
+ private:
+  IntakeConfig _config;
+  IntakeState _state = IntakeState::kIdle;
+
+  units::volt_t _rawVoltage = 0_V;
+  std::string _stringStateName = "error";
+  units::volt_t _setVoltage = 0_V;
+  std::shared_ptr<nt::NetworkTable> _table = nt::NetworkTableInstance::GetDefault().GetTable("Intake");
+};
diff --git a/src/main/include/IntakeBehaviour.h b/src/main/include/IntakeBehaviour.h
new file mode 100644
index 00000000..c84c6d32
--- /dev/null
+++ b/src/main/include/IntakeBehaviour.h
@@ -0,0 +1,34 @@
+// Copyright (c) 2023-2024 CurtinFRC
+// Open Source Software, you can modify it according to the terms
+// of the MIT License at the root of this project
+
+#pragma once
+
+#include <frc/XboxController.h>
+
+#include "Intake.h"
+#include "Wombat.h"
+
+class IntakeManualControl : public behaviour::Behaviour {
+ public:
+  explicit IntakeManualControl(Intake* intake, frc::XboxController& codriver);
+
+  void OnTick(units::second_t dt) override;
+
+ private:
+  Intake* _intake;
+  frc::XboxController& _codriver;
+
+  units::volt_t _rawVoltage;
+  bool _rawControl;
+};
+
+class IntakeAutoControl : public behaviour::Behaviour {
+ public:
+  explicit IntakeAutoControl(Intake* intake);
+
+  void OnTick(units::second_t dt) override;
+
+ private:
+  Intake* _intake;
+};
diff --git a/src/main/include/Robot.h b/src/main/include/Robot.h
index 1806c61c..c5a08964 100644
--- a/src/main/include/Robot.h
+++ b/src/main/include/Robot.h
@@ -12,6 +12,9 @@
 #include <frc/smartdashboard/SendableChooser.h>
 #include <frc/smartdashboard/SmartDashboard.h>
 #include <string>
+
+#include "Intake.h"
+#include "IntakeBehaviour.h"
 #include "RobotMap.h"
 #include "Shooter.h"
 #include "ShooterBehaviour.h"
@@ -35,6 +38,7 @@ class Robot : public frc::TimedRobot {
   frc::EventLoop loop;
   Shooter *shooter;
 
+  Intake* intake;
   frc::SendableChooser<std::string> m_chooser;
 
   frc::Field2d m_field;
diff --git a/src/main/include/RobotMap.h b/src/main/include/RobotMap.h
index 3ae4db10..52121f1a 100644
--- a/src/main/include/RobotMap.h
+++ b/src/main/include/RobotMap.h
@@ -16,6 +16,8 @@
 #include <ctre/phoenix6/Pigeon2.hpp>
 #include <ctre/phoenix6/TalonFX.hpp>
 
+#include "Intake.h"
+#include "IntakeBehaviour.h"
 #include "Shooter.h"
 #include "Wombat.h"
 
@@ -23,10 +25,22 @@ struct RobotMap {
   struct Controllers {
     frc::XboxController driver = frc::XboxController(0);
     frc::XboxController codriver = frc::XboxController(1);
-    frc::XboxController testController = frc::XboxController(2);
   };
   Controllers controllers;
 
+  struct IntakeSystem {
+    rev::CANSparkMax intakeMotor{2, rev::CANSparkMax::MotorType::kBrushed};
+    // wom::CANSparkMaxEncoder intakeEncoder{&intakeMotor, 0.1_m};
+    // frc::DigitalInput intakeSensor{0};
+    // frc::DigitalInput magSensor{0};
+    // frc::DigitalInput shooterSensor{0};
+
+    wom::Gearbox IntakeGearbox{&intakeMotor, nullptr, frc::DCMotor::CIM(1)};
+
+    IntakeConfig config{IntakeGearbox /*, &intakeSensor, &magSensor, &shooterSensor*/};
+  }; IntakeSystem intakeSystem;
+
+
   struct Shooter {
     rev::CANSparkMax shooterMotor{11, rev::CANSparkMax::MotorType::kBrushless};
     // frc::DigitalInput shooterSensor{2};
@@ -40,8 +54,7 @@ struct RobotMap {
         shooterGearbox,
         // &shooterSensor,
     };
-  };
-  Shooter shooterSystem;
+  }; Shooter shooterSystem;
   
   struct SwerveBase {
     ctre::phoenix6::hardware::CANcoder frontLeftCancoder{18, "Drivebase"};

From e5a9d4d2f66884f5dc9ef81f444c2ccd405fd932 Mon Sep 17 00:00:00 2001
From: JoystickMaster-race
 <97269195+JoystickMaster-race@users.noreply.github.com>
Date: Wed, 24 Jan 2024 09:45:57 +0800
Subject: [PATCH 100/207] Arm (#103)

* Alpha Arm vers 1

* Alpha Arm ver 1 w/ explicit

* Alpha arm v1 formatting

* Alpha arm w/ wrist v1

* formatted

* Alpha arm v1 with ports

* Working alpha arm no encoders

---------

Co-authored-by: Isaac Turner <spacey_sooty@outlook.com>
---
 src/main/cpp/AlphaArm.cpp            | 67 ++++++++++++++++++++++++++++
 src/main/cpp/AlphaArmBehaviour.cpp   | 35 +++++++++++++++
 src/main/cpp/Robot.cpp               | 28 ++++++++----
 src/main/include/AlphaArm.h          | 48 ++++++++++++++++++++
 src/main/include/AlphaArmBehaviour.h | 24 ++++++++++
 src/main/include/Robot.h             |  4 ++
 src/main/include/RobotMap.h          | 14 ++++++
 7 files changed, 212 insertions(+), 8 deletions(-)
 create mode 100644 src/main/cpp/AlphaArm.cpp
 create mode 100644 src/main/cpp/AlphaArmBehaviour.cpp
 create mode 100644 src/main/include/AlphaArm.h
 create mode 100644 src/main/include/AlphaArmBehaviour.h

diff --git a/src/main/cpp/AlphaArm.cpp b/src/main/cpp/AlphaArm.cpp
new file mode 100644
index 00000000..76c48dd9
--- /dev/null
+++ b/src/main/cpp/AlphaArm.cpp
@@ -0,0 +1,67 @@
+// Copyright (c) 2023-2024 CurtinFRC
+// Open Source Software, you can modify it according to the terms
+// of the MIT License at the root of this project
+
+#include "AlphaArm.h"
+
+AlphaArm::AlphaArm(AlphaArmConfig config) : _config(config) {}
+
+AlphaArmConfig AlphaArm::GetConfig(){
+    return _config;
+}
+
+void AlphaArm::OnUpdate(units::second_t dt){
+        switch(_state){
+            case AlphaArmState::kIdle:
+        
+            //_config.alphaArmGearbox.motorController->SetVoltage(0_V);
+            //_config.wristGearbox.motorController->SetVoltage(0_V);
+            _setAlphaArmVoltage = 0_V;
+            _setWristVoltage = 0_V;
+
+            break;
+            case AlphaArmState::kRaw:
+            _setAlphaArmVoltage = _rawArmVoltage;
+            _setWristVoltage = _rawWristVoltage;
+            _config.alphaArmGearbox.motorController->SetVoltage(_rawArmVoltage);
+            _config.wristGearbox.motorController->SetVoltage(_rawWristVoltage);
+            
+            break;
+            case AlphaArmState::kForwardWrist:
+            _config.wristGearbox.motorController->SetVoltage(3_V);
+            _setWristVoltage = 3_V;
+
+            case AlphaArmState::kReverseWrist:
+            _config.wristGearbox.motorController->SetVoltage(-3_V);
+            _setWristVoltage = -3_V;
+            default:
+            std::cout << "oops, wrong state" << std::endl;
+            break;
+            
+        }
+        //transmission translate
+       // _config.armGearBox.motorController->SetVoltage(setAlphaArmVoltage);
+       //_config.alphaArmGearbox.motorController->SetVoltage(setAlphaArmVoltage);
+       //_config.wristGearbox.motorController->SetVoltage(setWristVoltage);
+        _config.alphaArmGearbox.motorController->SetVoltage(_setAlphaArmVoltage);
+        _config.wristGearbox.motorController->SetVoltage(_setWristVoltage);
+        
+        //_config.wristGearbox.motorController->SetVoltage(_setVoltage);
+}
+
+void AlphaArm::SetState(AlphaArmState state) {
+  _state = state;
+}
+
+// void AlphaArm::SetRaw(units::volt_t voltage){
+//     _rawArmVoltage = voltage;
+//     _rawWristVoltage = voltage;
+// }
+
+void AlphaArm::SetArmRaw(units::volt_t voltage){
+    _rawArmVoltage = voltage;
+}
+
+void AlphaArm::setWristRaw(units::volt_t voltage){
+    _rawWristVoltage = voltage;
+}
diff --git a/src/main/cpp/AlphaArmBehaviour.cpp b/src/main/cpp/AlphaArmBehaviour.cpp
new file mode 100644
index 00000000..67f9e059
--- /dev/null
+++ b/src/main/cpp/AlphaArmBehaviour.cpp
@@ -0,0 +1,35 @@
+// Copyright (c) 2023-2024 CurtinFRC
+// Open Source Software, you can modify it according to the terms
+// of the MIT License at the root of this project
+
+#include "AlphaArmBehaviour.h"
+
+#include <frc/XboxController.h>
+
+AlphaArmManualControl::AlphaArmManualControl(AlphaArm* alphaArm, frc::XboxController* codriver)
+    : _alphaArm(alphaArm), _codriver(codriver) {
+  Controls(alphaArm);
+}
+
+void AlphaArmManualControl::OnTick(units::second_t dt) {
+  if (_codriver->GetXButtonPressed()) {
+    if (_rawControl == true) {
+      _rawControl = false;
+    } else {
+      _rawControl = true;
+    }
+  }
+
+  if (_rawControl) {
+    _alphaArm->SetState(AlphaArmState::kRaw);
+    _alphaArm->SetArmRaw(_codriver->GetRightY() * 6_V);
+    _alphaArm->setWristRaw(_codriver->GetLeftY() * -6_V);
+  } else {
+    if (_codriver->GetRightBumperPressed()){
+        _alphaArm->SetState(AlphaArmState::kForwardWrist);
+    }
+    if (_codriver->GetLeftBumperPressed()){
+        _alphaArm->SetState(AlphaArmState::kReverseWrist);
+    }
+  }
+}
diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp
index 9cef56a6..d1b997ca 100644
--- a/src/main/cpp/Robot.cpp
+++ b/src/main/cpp/Robot.cpp
@@ -35,21 +35,25 @@ void Robot::RobotInit() {
   sched = wom::BehaviourScheduler::GetInstance();
   m_chooser.SetDefaultOption("Default Auto", "Default Auto");
 
-  frc::SmartDashboard::PutData("Auto Selector", &m_chooser);
+  // frc::SmartDashboard::PutData("Auto Selector", &m_chooser);
 
-  m_path_chooser.SetDefaultOption("Path1", "paths/output/Path1.wpilib.json");
+  // m_path_chooser.SetDefaultOption("Path1", "paths/output/Path1.wpilib.json");
 
-  m_path_chooser.AddOption("Path1", "paths/output/Path1.wpilib.json");
-  m_path_chooser.AddOption("Path2", "paths/output/Path2.wpilib.json");
+  // m_path_chooser.AddOption("Path1", "paths/output/Path1.wpilib.json");
+  // m_path_chooser.AddOption("Path2", "paths/output/Path2.wpilib.json");
 
-  frc::SmartDashboard::PutData("Path Selector", &m_path_chooser);
+  // frc::SmartDashboard::PutData("Path Selector", &m_path_chooser);
 
-  frc::SmartDashboard::PutData("Field", &m_field);
+  // frc::SmartDashboard::PutData("Field", &m_field);
 
-  simulation_timer = frc::Timer();
+  // simulation_timer = frc::Timer();
 
-  robotmap.swerveBase.gyro->Reset();
+  // robotmap.swerveBase.gyro->Reset();
 
+  // _swerveDrive = new wom::SwerveDrive(robotmap.swerveBase.config, frc::Pose2d());
+  // wom::BehaviourScheduler::GetInstance()->Register(_swerveDrive);
+  // _swerveDrive->SetDefaultBehaviour(
+  //     [this]() { return wom::make<wom::ManualDrivebase>(_swerveDrive, &robotmap.controllers.driver); });
   _swerveDrive =
       new wom::SwerveDrive(robotmap.swerveBase.config, frc::Pose2d());
   wom::BehaviourScheduler::GetInstance()->Register(_swerveDrive);
@@ -62,6 +66,12 @@ void Robot::RobotInit() {
   // m_driveSim = new wom::TempSimSwerveDrive(&simulation_timer, &m_field);
   // m_driveSim = wom::TempSimSwerveDrive();
 
+  alphaArm = 
+      new AlphaArm(robotmap.alphaArmSystem.config);
+  wom::BehaviourScheduler::GetInstance()->Register(alphaArm);
+  alphaArm->SetDefaultBehaviour(
+      [this]() { return wom::make<AlphaArmManualControl>(alphaArm, &robotmap.controllers.codriver); });
+      
   robotmap.swerveBase.moduleConfigs[0].turnMotor.encoder->SetEncoderOffset(0_rad);
   robotmap.swerveBase.moduleConfigs[1].turnMotor.encoder->SetEncoderOffset(0_rad);
   robotmap.swerveBase.moduleConfigs[2].turnMotor.encoder->SetEncoderOffset(0_rad);
@@ -100,6 +110,8 @@ void Robot::RobotPeriodic() {
   robotmap.swerveTable.swerveDriveTable->GetEntry("backLeftEncoder").SetDouble(robotmap.swerveBase.moduleConfigs[2].turnMotor.encoder->GetEncoderPosition().value());
   robotmap.swerveTable.swerveDriveTable->GetEntry("backRightEncoder").SetDouble(robotmap.swerveBase.moduleConfigs[3].turnMotor.encoder->GetEncoderPosition().value());
 
+  _swerveDrive->OnUpdate(dt);
+  alphaArm->OnUpdate(dt);
   intake->OnUpdate(dt);
 }
 
diff --git a/src/main/include/AlphaArm.h b/src/main/include/AlphaArm.h
new file mode 100644
index 00000000..bc9c8d7c
--- /dev/null
+++ b/src/main/include/AlphaArm.h
@@ -0,0 +1,48 @@
+// Copyright (c) 2023-2024 CurtinFRC
+// Open Source Software, you can modify it according to the terms
+// of the MIT License at the root of this project
+
+#pragma once
+#include <frc/DigitalInput.h>
+
+#include "Wombat.h"
+
+struct AlphaArmConfig {
+    wom::Gearbox alphaArmGearbox;
+    wom::Gearbox wristGearbox;
+
+};
+
+enum class AlphaArmState {
+    kIdle,
+    kIntakeAngle,
+    kAmpAngle,
+    kSpeakerAngle,
+    kForwardWrist,
+    kReverseWrist,
+    kRaw
+};
+
+class AlphaArm : public::behaviour::HasBehaviour{
+    public:
+     explicit AlphaArm(AlphaArmConfig config);
+
+    void OnUpdate(units::second_t dt);
+    void SetArmRaw(units::volt_t voltage);
+    void setWristRaw(units::volt_t voltage);
+    void SetState(AlphaArmState state);
+    AlphaArmConfig GetConfig();
+    //void SetRaw(units::volt_t voltage);
+
+    private:
+    AlphaArmConfig _config;
+    AlphaArmState _state = AlphaArmState::kIdle;
+    units::volt_t _setAlphaArmVoltage = 0_V;
+    units::volt_t _setWristVoltage = 0_V;
+
+    units::volt_t _rawArmVoltage = 0_V;
+    units::volt_t _rawWristVoltage = 0_V;
+
+    
+   
+};
diff --git a/src/main/include/AlphaArmBehaviour.h b/src/main/include/AlphaArmBehaviour.h
new file mode 100644
index 00000000..4c2d03fb
--- /dev/null
+++ b/src/main/include/AlphaArmBehaviour.h
@@ -0,0 +1,24 @@
+// Copyright (c) 2023-2024 CurtinFRC
+// Open Source Software, you can modify it according to the terms
+// of the MIT License at the root of this project
+
+#pragma once
+
+#pragma once
+#include <frc/XboxController.h>
+
+#include "AlphaArm.h"
+#include "Wombat.h"
+
+class AlphaArmManualControl : public behaviour::Behaviour {
+ public:
+  explicit AlphaArmManualControl(AlphaArm* alphaArm, frc::XboxController* codriver);
+  void OnTick(units::second_t dt);
+
+ private:
+  AlphaArm* _alphaArm;
+  frc::XboxController* _codriver;
+  // units::volt_t _rightStick = ((_codriver->GetRightY()>0.05 || _codriver->GetRightY() < -0.05
+  // )?_codriver->GetRightY():0) * 2_V;
+  bool _rawControl;
+};
diff --git a/src/main/include/Robot.h b/src/main/include/Robot.h
index c5a08964..5fad6cde 100644
--- a/src/main/include/Robot.h
+++ b/src/main/include/Robot.h
@@ -13,6 +13,8 @@
 #include <frc/smartdashboard/SmartDashboard.h>
 #include <string>
 
+#include "AlphaArm.h"
+#include "AlphaArmBehaviour.h"
 #include "Intake.h"
 #include "IntakeBehaviour.h"
 #include "RobotMap.h"
@@ -49,6 +51,8 @@ class Robot : public frc::TimedRobot {
 
   wom::SwerveDrive* _swerveDrive;
 
+  AlphaArm *alphaArm;
+
   // ctre::phoenix6::hardware::TalonFX *frontLeft;
   // ctre::phoenix6::hardware::TalonFX *frontRight;
   // ctre::phoenix6::hardware::TalonFX *backLeft;
diff --git a/src/main/include/RobotMap.h b/src/main/include/RobotMap.h
index 52121f1a..fb6d2804 100644
--- a/src/main/include/RobotMap.h
+++ b/src/main/include/RobotMap.h
@@ -16,6 +16,8 @@
 #include <ctre/phoenix6/Pigeon2.hpp>
 #include <ctre/phoenix6/TalonFX.hpp>
 
+#include "AlphaArm.h"
+#include "AlphaArmBehaviour.h"
 #include "Intake.h"
 #include "IntakeBehaviour.h"
 #include "Shooter.h"
@@ -25,9 +27,21 @@ struct RobotMap {
   struct Controllers {
     frc::XboxController driver = frc::XboxController(0);
     frc::XboxController codriver = frc::XboxController(1);
+    frc::XboxController testController = frc::XboxController(2);
   };
   Controllers controllers;
 
+  struct AlphaArmSystem {
+    rev::CANSparkMax alphaArmMotor{12, rev::CANSparkMax::MotorType::kBrushless};
+    rev::CANSparkMax wristMotor{15, rev::CANSparkMax::MotorType::kBrushless};
+
+    wom::Gearbox alphaArmGearbox{&alphaArmMotor, nullptr, frc::DCMotor::NEO(1)};
+    wom::Gearbox wristGearbox{&wristMotor, nullptr, frc::DCMotor::NEO(1)};
+
+    AlphaArmConfig config{alphaArmGearbox, wristGearbox};
+  };
+  AlphaArmSystem alphaArmSystem;
+  
   struct IntakeSystem {
     rev::CANSparkMax intakeMotor{2, rev::CANSparkMax::MotorType::kBrushed};
     // wom::CANSparkMaxEncoder intakeEncoder{&intakeMotor, 0.1_m};

From 0029f494099e11ddcad2ab1b4c81a6244f2e4879 Mon Sep 17 00:00:00 2001
From: Isaac Turner <spacey_sooty@outlook.com>
Date: Wed, 24 Jan 2024 14:33:05 +0800
Subject: [PATCH 101/207] fix some formatting

---
 .clang-format                                 |   2 +-
 .editorconfig                                 |   5 -
 .styleguide                                   |   1 -
 build.gradle                                  |   2 +-
 init.ps1                                      |   2 -
 init.sh                                       |   5 -
 src/main/cpp/AlphaArm.cpp                     |  81 +++++-----
 src/main/cpp/AlphaArmBehaviour.cpp            |   8 +-
 src/main/cpp/Robot.cpp                        |  45 +++---
 src/main/cpp/Shooter.cpp                      |   8 +-
 src/main/cpp/ShooterBehaviour.cpp             |   9 +-
 src/main/include/AlphaArm.h                   |  60 ++++----
 src/main/include/Robot.h                      |   9 +-
 src/main/include/RobotMap.h                   |  80 +++++-----
 wombat/src/main/cpp/behaviour/Behaviour.cpp   |  23 +--
 .../main/cpp/behaviour/BehaviourScheduler.cpp |   4 +-
 .../src/main/cpp/behaviour/HasBehaviour.cpp   |   3 +-
 wombat/src/main/cpp/drivetrain/Drivetrain.cpp |   3 +-
 .../src/main/cpp/drivetrain/SwerveDrive.cpp   | 139 +++++++-----------
 .../behaviours/SwerveBehaviours.cpp           |  96 ++++++------
 wombat/src/main/cpp/subsystems/Arm.cpp        |  35 ++---
 wombat/src/main/cpp/subsystems/Elevator.cpp   |  30 ++--
 wombat/src/main/cpp/subsystems/Shooter.cpp    |  22 +--
 wombat/src/main/cpp/utils/Encoder.cpp         |  55 +++----
 wombat/src/main/cpp/utils/Util.cpp            |  27 +---
 wombat/src/main/cpp/vision/Limelight.cpp      |  24 ++-
 wombat/src/main/include/behaviour/Behaviour.h |  42 ++----
 .../src/main/include/behaviour/HasBehaviour.h |   3 +-
 .../src/main/include/drivetrain/SwerveDrive.h |  28 ++--
 .../drivetrain/behaviours/SwerveBehaviours.h  |  17 +--
 wombat/src/main/include/subsystems/Arm.h      |   3 +-
 wombat/src/main/include/subsystems/Shooter.h  |   3 +-
 wombat/src/main/include/utils/Encoder.h       |  27 ++--
 wombat/src/main/include/utils/PID.h           |  69 +++------
 wombat/src/main/include/utils/Util.h          |  55 +++----
 wombat/src/main/include/vision/Limelight.h    |  22 +--
 36 files changed, 413 insertions(+), 634 deletions(-)
 delete mode 100644 .editorconfig
 delete mode 100644 init.ps1
 delete mode 100644 init.sh

diff --git a/.clang-format b/.clang-format
index f69bef0c..a879dec4 100644
--- a/.clang-format
+++ b/.clang-format
@@ -81,7 +81,7 @@ BreakBeforeTernaryOperators: true
 BreakConstructorInitializers: BeforeColon
 BreakInheritanceList: BeforeColon
 BreakStringLiterals: true
-ColumnLimit:     80
+ColumnLimit:     110
 CommentPragmas:  '^ IWYU pragma:'
 CompactNamespaces: false
 ConstructorInitializerIndentWidth: 4
diff --git a/.editorconfig b/.editorconfig
deleted file mode 100644
index 0020fc03..00000000
--- a/.editorconfig
+++ /dev/null
@@ -1,5 +0,0 @@
-root = true
-
-[*]
-indent_style = space
-indent_size = 2
diff --git a/.styleguide b/.styleguide
index be156d3c..82efdeab 100644
--- a/.styleguide
+++ b/.styleguide
@@ -1,4 +1,3 @@
-
 cppHeaderFileInclude {
    \.h$
    \.hpp$
diff --git a/build.gradle b/build.gradle
index 91b6acb4..84ccc0cb 100644
--- a/build.gradle
+++ b/build.gradle
@@ -1,7 +1,7 @@
 plugins {
     id "cpp"
     id "google-test-test-suite"
-    id "edu.wpi.first.GradleRIO" version "2024.1.1"
+    id "edu.wpi.first.GradleRIO" version "2024.2.1"
 }
 
 // Define my targets (RoboRIO) and artifacts (deployable files)
diff --git a/init.ps1 b/init.ps1
deleted file mode 100644
index fa83b9a6..00000000
--- a/init.ps1
+++ /dev/null
@@ -1,2 +0,0 @@
-./gradlew installRoborioToolchain
-./gradlew build
diff --git a/init.sh b/init.sh
deleted file mode 100644
index 6a932ed6..00000000
--- a/init.sh
+++ /dev/null
@@ -1,5 +0,0 @@
-#!/usr/bin/sh
-sudo apt update
-chmod +x gradlew
-./gradlew installRoborioToolchain
-./gradlew build
diff --git a/src/main/cpp/AlphaArm.cpp b/src/main/cpp/AlphaArm.cpp
index 76c48dd9..848ee616 100644
--- a/src/main/cpp/AlphaArm.cpp
+++ b/src/main/cpp/AlphaArm.cpp
@@ -6,47 +6,46 @@
 
 AlphaArm::AlphaArm(AlphaArmConfig config) : _config(config) {}
 
-AlphaArmConfig AlphaArm::GetConfig(){
-    return _config;
+AlphaArmConfig AlphaArm::GetConfig() {
+  return _config;
 }
 
-void AlphaArm::OnUpdate(units::second_t dt){
-        switch(_state){
-            case AlphaArmState::kIdle:
-        
-            //_config.alphaArmGearbox.motorController->SetVoltage(0_V);
-            //_config.wristGearbox.motorController->SetVoltage(0_V);
-            _setAlphaArmVoltage = 0_V;
-            _setWristVoltage = 0_V;
+void AlphaArm::OnUpdate(units::second_t dt) {
+  switch (_state) {
+    case AlphaArmState::kIdle:
 
-            break;
-            case AlphaArmState::kRaw:
-            _setAlphaArmVoltage = _rawArmVoltage;
-            _setWristVoltage = _rawWristVoltage;
-            _config.alphaArmGearbox.motorController->SetVoltage(_rawArmVoltage);
-            _config.wristGearbox.motorController->SetVoltage(_rawWristVoltage);
-            
-            break;
-            case AlphaArmState::kForwardWrist:
-            _config.wristGearbox.motorController->SetVoltage(3_V);
-            _setWristVoltage = 3_V;
+      // _config.alphaArmGearbox.motorController->SetVoltage(0_V);
+      // _config.wristGearbox.motorController->SetVoltage(0_V);
+      _setAlphaArmVoltage = 0_V;
+      _setWristVoltage = 0_V;
 
-            case AlphaArmState::kReverseWrist:
-            _config.wristGearbox.motorController->SetVoltage(-3_V);
-            _setWristVoltage = -3_V;
-            default:
-            std::cout << "oops, wrong state" << std::endl;
-            break;
-            
-        }
-        //transmission translate
-       // _config.armGearBox.motorController->SetVoltage(setAlphaArmVoltage);
-       //_config.alphaArmGearbox.motorController->SetVoltage(setAlphaArmVoltage);
-       //_config.wristGearbox.motorController->SetVoltage(setWristVoltage);
-        _config.alphaArmGearbox.motorController->SetVoltage(_setAlphaArmVoltage);
-        _config.wristGearbox.motorController->SetVoltage(_setWristVoltage);
-        
-        //_config.wristGearbox.motorController->SetVoltage(_setVoltage);
+      break;
+    case AlphaArmState::kRaw:
+      _setAlphaArmVoltage = _rawArmVoltage;
+      _setWristVoltage = _rawWristVoltage;
+      _config.alphaArmGearbox.motorController->SetVoltage(_rawArmVoltage);
+      _config.wristGearbox.motorController->SetVoltage(_rawWristVoltage);
+
+      break;
+    case AlphaArmState::kForwardWrist:
+      _config.wristGearbox.motorController->SetVoltage(3_V);
+      _setWristVoltage = 3_V;
+
+    case AlphaArmState::kReverseWrist:
+      _config.wristGearbox.motorController->SetVoltage(-3_V);
+      _setWristVoltage = -3_V;
+    default:
+      std::cout << "oops, wrong state" << std::endl;
+      break;
+  }
+  // transmission translate
+  // _config.armGearBox.motorController->SetVoltage(setAlphaArmVoltage);
+  // _config.alphaArmGearbox.motorController->SetVoltage(setAlphaArmVoltage);
+  // _config.wristGearbox.motorController->SetVoltage(setWristVoltage);
+  _config.alphaArmGearbox.motorController->SetVoltage(_setAlphaArmVoltage);
+  _config.wristGearbox.motorController->SetVoltage(_setWristVoltage);
+
+  // _config.wristGearbox.motorController->SetVoltage(_setVoltage);
 }
 
 void AlphaArm::SetState(AlphaArmState state) {
@@ -58,10 +57,10 @@ void AlphaArm::SetState(AlphaArmState state) {
 //     _rawWristVoltage = voltage;
 // }
 
-void AlphaArm::SetArmRaw(units::volt_t voltage){
-    _rawArmVoltage = voltage;
+void AlphaArm::SetArmRaw(units::volt_t voltage) {
+  _rawArmVoltage = voltage;
 }
 
-void AlphaArm::setWristRaw(units::volt_t voltage){
-    _rawWristVoltage = voltage;
+void AlphaArm::setWristRaw(units::volt_t voltage) {
+  _rawWristVoltage = voltage;
 }
diff --git a/src/main/cpp/AlphaArmBehaviour.cpp b/src/main/cpp/AlphaArmBehaviour.cpp
index 67f9e059..ece50ebb 100644
--- a/src/main/cpp/AlphaArmBehaviour.cpp
+++ b/src/main/cpp/AlphaArmBehaviour.cpp
@@ -25,11 +25,11 @@ void AlphaArmManualControl::OnTick(units::second_t dt) {
     _alphaArm->SetArmRaw(_codriver->GetRightY() * 6_V);
     _alphaArm->setWristRaw(_codriver->GetLeftY() * -6_V);
   } else {
-    if (_codriver->GetRightBumperPressed()){
-        _alphaArm->SetState(AlphaArmState::kForwardWrist);
+    if (_codriver->GetRightBumperPressed()) {
+      _alphaArm->SetState(AlphaArmState::kForwardWrist);
     }
-    if (_codriver->GetLeftBumperPressed()){
-        _alphaArm->SetState(AlphaArmState::kReverseWrist);
+    if (_codriver->GetLeftBumperPressed()) {
+      _alphaArm->SetState(AlphaArmState::kReverseWrist);
     }
   }
 }
diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp
index d1b997ca..888245e6 100644
--- a/src/main/cpp/Robot.cpp
+++ b/src/main/cpp/Robot.cpp
@@ -18,20 +18,16 @@
 #include <units/velocity.h>
 #include <units/voltage.h>
 
-#include <frc/kinematics/DifferentialDriveKinematics.h>
-#include <frc/controller/RamseteController.h>
-#include <frc/Timer.h>
 #include "behaviour/HasBehaviour.h"
 
 static units::second_t lastPeriodic;
 
 void Robot::RobotInit() {
-
   shooter = new Shooter(robotmap.shooterSystem.config);
   wom::BehaviourScheduler::GetInstance()->Register(shooter);
   shooter->SetDefaultBehaviour(
-     [this]() {return wom::make<ShooterManualControl>(shooter, &robotmap.controllers.codriver); });
-  
+      [this]() { return wom::make<ShooterManualControl>(shooter, &robotmap.controllers.codriver); });
+
   sched = wom::BehaviourScheduler::GetInstance();
   m_chooser.SetDefaultOption("Default Auto", "Default Auto");
 
@@ -54,32 +50,24 @@ void Robot::RobotInit() {
   // wom::BehaviourScheduler::GetInstance()->Register(_swerveDrive);
   // _swerveDrive->SetDefaultBehaviour(
   //     [this]() { return wom::make<wom::ManualDrivebase>(_swerveDrive, &robotmap.controllers.driver); });
-  _swerveDrive =
-      new wom::SwerveDrive(robotmap.swerveBase.config, frc::Pose2d());
+  _swerveDrive = new wom::SwerveDrive(robotmap.swerveBase.config, frc::Pose2d());
   wom::BehaviourScheduler::GetInstance()->Register(_swerveDrive);
-  _swerveDrive->SetDefaultBehaviour([this]() {
-    return wom::make<wom::ManualDrivebase>(_swerveDrive,
-                                           &robotmap.controllers.driver);
-  });
+  _swerveDrive->SetDefaultBehaviour(
+      [this]() { return wom::make<wom::ManualDrivebase>(_swerveDrive, &robotmap.controllers.driver); });
 
-  
   // m_driveSim = new wom::TempSimSwerveDrive(&simulation_timer, &m_field);
   // m_driveSim = wom::TempSimSwerveDrive();
 
-  alphaArm = 
-      new AlphaArm(robotmap.alphaArmSystem.config);
+  alphaArm = new AlphaArm(robotmap.alphaArmSystem.config);
   wom::BehaviourScheduler::GetInstance()->Register(alphaArm);
   alphaArm->SetDefaultBehaviour(
       [this]() { return wom::make<AlphaArmManualControl>(alphaArm, &robotmap.controllers.codriver); });
-      
+
   robotmap.swerveBase.moduleConfigs[0].turnMotor.encoder->SetEncoderOffset(0_rad);
   robotmap.swerveBase.moduleConfigs[1].turnMotor.encoder->SetEncoderOffset(0_rad);
   robotmap.swerveBase.moduleConfigs[2].turnMotor.encoder->SetEncoderOffset(0_rad);
   robotmap.swerveBase.moduleConfigs[3].turnMotor.encoder->SetEncoderOffset(0_rad);
 
-
-
-
   // frontLeft = new ctre::phoenix6::hardware::TalonFX(7, "Drivebase");  // front left
   // frontRight = new ctre::phoenix6::hardware::TalonFX(2, "Drivebase");   // front right
   // backLeft = new ctre::phoenix6::hardware::TalonFX(6, "Drivebase");   // back left
@@ -105,10 +93,14 @@ void Robot::RobotPeriodic() {
   shooter->OnUpdate(dt);
   sched->Tick();
 
-  robotmap.swerveTable.swerveDriveTable->GetEntry("frontLeftEncoder").SetDouble(robotmap.swerveBase.moduleConfigs[0].turnMotor.encoder->GetEncoderPosition().value());
-  robotmap.swerveTable.swerveDriveTable->GetEntry("frontRightEncoder").SetDouble(robotmap.swerveBase.moduleConfigs[1].turnMotor.encoder->GetEncoderPosition().value());
-  robotmap.swerveTable.swerveDriveTable->GetEntry("backLeftEncoder").SetDouble(robotmap.swerveBase.moduleConfigs[2].turnMotor.encoder->GetEncoderPosition().value());
-  robotmap.swerveTable.swerveDriveTable->GetEntry("backRightEncoder").SetDouble(robotmap.swerveBase.moduleConfigs[3].turnMotor.encoder->GetEncoderPosition().value());
+  robotmap.swerveTable.swerveDriveTable->GetEntry("frontLeftEncoder")
+      .SetDouble(robotmap.swerveBase.moduleConfigs[0].turnMotor.encoder->GetEncoderPosition().value());
+  robotmap.swerveTable.swerveDriveTable->GetEntry("frontRightEncoder")
+      .SetDouble(robotmap.swerveBase.moduleConfigs[1].turnMotor.encoder->GetEncoderPosition().value());
+  robotmap.swerveTable.swerveDriveTable->GetEntry("backLeftEncoder")
+      .SetDouble(robotmap.swerveBase.moduleConfigs[2].turnMotor.encoder->GetEncoderPosition().value());
+  robotmap.swerveTable.swerveDriveTable->GetEntry("backRightEncoder")
+      .SetDouble(robotmap.swerveBase.moduleConfigs[3].turnMotor.encoder->GetEncoderPosition().value());
 
   _swerveDrive->OnUpdate(dt);
   alphaArm->OnUpdate(dt);
@@ -119,12 +111,11 @@ void Robot::AutonomousInit() {
   loop.Clear();
   sched->InterruptAll();
 }
-void Robot::AutonomousPeriodic() {
-}
+void Robot::AutonomousPeriodic() {}
 
 void Robot::TeleopInit() {
   loop.Clear();
-  wom::BehaviourScheduler *sched = wom::BehaviourScheduler::GetInstance();
+  wom::BehaviourScheduler* sched = wom::BehaviourScheduler::GetInstance();
   sched->InterruptAll();
 
   // frontLeft->SetVoltage(4_V);
@@ -142,4 +133,4 @@ void Robot::DisabledInit() {}
 void Robot::DisabledPeriodic() {}
 
 void Robot::TestInit() {}
-void Robot::TestPeriodic() {}
\ No newline at end of file
+void Robot::TestPeriodic() {}
diff --git a/src/main/cpp/Shooter.cpp b/src/main/cpp/Shooter.cpp
index 613120df..6499e076 100644
--- a/src/main/cpp/Shooter.cpp
+++ b/src/main/cpp/Shooter.cpp
@@ -4,10 +4,11 @@
 
 #include "Shooter.h"
 
-Shooter::Shooter(ShooterConfig config) : _config(config)
-// , 
+Shooter::Shooter(ShooterConfig config)
+    : _config(config)
+// ,
 // _pid{frc::PIDController (1, 0, 0, 0.005_s)}
-{} //config.path + "/pid", config.pidConfig
+{}  // config.path + "/pid", config.pidConfig
 void Shooter::OnUpdate(units::second_t dt) {
   // _pid.SetTolerance(0.1, 1);
   switch (_state) {
@@ -64,7 +65,6 @@ void Shooter::OnUpdate(units::second_t dt) {
   }
   std::cout << "Voltage:" << _setVoltage.value() << std::endl;
   _config.ShooterGearbox.motorController->SetVoltage(_setVoltage);
-
 }
 
 void Shooter::SetState(ShooterState state) {
diff --git a/src/main/cpp/ShooterBehaviour.cpp b/src/main/cpp/ShooterBehaviour.cpp
index 1a035ae0..341eef15 100644
--- a/src/main/cpp/ShooterBehaviour.cpp
+++ b/src/main/cpp/ShooterBehaviour.cpp
@@ -12,7 +12,6 @@ ShooterManualControl::ShooterManualControl(Shooter* shooter, frc::XboxController
 void ShooterManualControl::OnTick(units::second_t dt) {
   _shooter->table->GetEntry("RawControl").SetBoolean(_rawControl);
 
-
   if (_codriver->GetAButtonReleased()) {
     if (_rawControl) {
       _rawControl = false;
@@ -22,10 +21,10 @@ void ShooterManualControl::OnTick(units::second_t dt) {
   }
 
   if (!_rawControl) {
-      if (_codriver->GetYButton()) {
-        _shooter->SetState(ShooterState::kSpinUp);
-        _shooter->SetPidGoal(10_rad_per_s);
-      }
+    if (_codriver->GetYButton()) {
+      _shooter->SetState(ShooterState::kSpinUp);
+      _shooter->SetPidGoal(10_rad_per_s);
+    }
   } else {
     if (_codriver->GetRightTriggerAxis() > 0.1) {
       _shooter->SetRaw(_codriver->GetRightTriggerAxis() * 12_V);
diff --git a/src/main/include/AlphaArm.h b/src/main/include/AlphaArm.h
index bc9c8d7c..5ac2d7c0 100644
--- a/src/main/include/AlphaArm.h
+++ b/src/main/include/AlphaArm.h
@@ -8,41 +8,37 @@
 #include "Wombat.h"
 
 struct AlphaArmConfig {
-    wom::Gearbox alphaArmGearbox;
-    wom::Gearbox wristGearbox;
-
+  wom::Gearbox alphaArmGearbox;
+  wom::Gearbox wristGearbox;
 };
 
 enum class AlphaArmState {
-    kIdle,
-    kIntakeAngle,
-    kAmpAngle,
-    kSpeakerAngle,
-    kForwardWrist,
-    kReverseWrist,
-    kRaw
+  kIdle,
+  kIntakeAngle,
+  kAmpAngle,
+  kSpeakerAngle,
+  kForwardWrist,
+  kReverseWrist,
+  kRaw
 };
 
-class AlphaArm : public::behaviour::HasBehaviour{
-    public:
-     explicit AlphaArm(AlphaArmConfig config);
-
-    void OnUpdate(units::second_t dt);
-    void SetArmRaw(units::volt_t voltage);
-    void setWristRaw(units::volt_t voltage);
-    void SetState(AlphaArmState state);
-    AlphaArmConfig GetConfig();
-    //void SetRaw(units::volt_t voltage);
-
-    private:
-    AlphaArmConfig _config;
-    AlphaArmState _state = AlphaArmState::kIdle;
-    units::volt_t _setAlphaArmVoltage = 0_V;
-    units::volt_t _setWristVoltage = 0_V;
-
-    units::volt_t _rawArmVoltage = 0_V;
-    units::volt_t _rawWristVoltage = 0_V;
-
-    
-   
+class AlphaArm : public ::behaviour::HasBehaviour {
+ public:
+  explicit AlphaArm(AlphaArmConfig config);
+
+  void OnUpdate(units::second_t dt);
+  void SetArmRaw(units::volt_t voltage);
+  void setWristRaw(units::volt_t voltage);
+  void SetState(AlphaArmState state);
+  AlphaArmConfig GetConfig();
+  // void SetRaw(units::volt_t voltage);
+
+ private:
+  AlphaArmConfig _config;
+  AlphaArmState _state = AlphaArmState::kIdle;
+  units::volt_t _setAlphaArmVoltage = 0_V;
+  units::volt_t _setWristVoltage = 0_V;
+
+  units::volt_t _rawArmVoltage = 0_V;
+  units::volt_t _rawWristVoltage = 0_V;
 };
diff --git a/src/main/include/Robot.h b/src/main/include/Robot.h
index 5fad6cde..c02d71a7 100644
--- a/src/main/include/Robot.h
+++ b/src/main/include/Robot.h
@@ -1,6 +1,7 @@
 // Copyright (c) 2023-2024 CurtinFRC
 // Open Source Software, you can modify it according to the terms
 // of the MIT License at the root of this project
+
 #pragma once
 #include <frc/Encoder.h>
 #include <frc/TimedRobot.h>
@@ -11,6 +12,7 @@
 #include <frc/smartdashboard/Field2d.h>
 #include <frc/smartdashboard/SendableChooser.h>
 #include <frc/smartdashboard/SmartDashboard.h>
+
 #include <string>
 
 #include "AlphaArm.h"
@@ -21,7 +23,7 @@
 #include "Shooter.h"
 #include "ShooterBehaviour.h"
 #include "Wombat.h"
-#include "RobotMap.h"
+
 class Robot : public frc::TimedRobot {
  public:
   void TestInit() override;
@@ -34,11 +36,12 @@ class Robot : public frc::TimedRobot {
   void TeleopPeriodic() override;
   void DisabledInit() override;
   void DisabledPeriodic() override;
+
  private:
   RobotMap robotmap;
   wom::BehaviourScheduler* sched;
   frc::EventLoop loop;
-  Shooter *shooter;
+  Shooter* shooter;
 
   Intake* intake;
   frc::SendableChooser<std::string> m_chooser;
@@ -51,7 +54,7 @@ class Robot : public frc::TimedRobot {
 
   wom::SwerveDrive* _swerveDrive;
 
-  AlphaArm *alphaArm;
+  AlphaArm* alphaArm;
 
   // ctre::phoenix6::hardware::TalonFX *frontLeft;
   // ctre::phoenix6::hardware::TalonFX *frontRight;
diff --git a/src/main/include/RobotMap.h b/src/main/include/RobotMap.h
index fb6d2804..733ab1af 100644
--- a/src/main/include/RobotMap.h
+++ b/src/main/include/RobotMap.h
@@ -1,6 +1,7 @@
 // Copyright (c) 2023-2024 CurtinFRC
 // Open Source Software, you can modify it according to the terms
 // of the MIT License at the root of this project
+
 #pragma once
 
 #include <frc/Compressor.h>
@@ -10,6 +11,7 @@
 #include <units/angle.h>
 #include <units/length.h>
 
+#include <memory>
 #include <string>
 
 #include <ctre/phoenix6/CANcoder.hpp>
@@ -17,9 +19,7 @@
 #include <ctre/phoenix6/TalonFX.hpp>
 
 #include "AlphaArm.h"
-#include "AlphaArmBehaviour.h"
 #include "Intake.h"
-#include "IntakeBehaviour.h"
 #include "Shooter.h"
 #include "Wombat.h"
 
@@ -41,7 +41,7 @@ struct RobotMap {
     AlphaArmConfig config{alphaArmGearbox, wristGearbox};
   };
   AlphaArmSystem alphaArmSystem;
-  
+
   struct IntakeSystem {
     rev::CANSparkMax intakeMotor{2, rev::CANSparkMax::MotorType::kBrushed};
     // wom::CANSparkMaxEncoder intakeEncoder{&intakeMotor, 0.1_m};
@@ -52,8 +52,8 @@ struct RobotMap {
     wom::Gearbox IntakeGearbox{&intakeMotor, nullptr, frc::DCMotor::CIM(1)};
 
     IntakeConfig config{IntakeGearbox /*, &intakeSensor, &magSensor, &shooterSensor*/};
-  }; IntakeSystem intakeSystem;
-
+  };
+  IntakeSystem intakeSystem;
 
   struct Shooter {
     rev::CANSparkMax shooterMotor{11, rev::CANSparkMax::MotorType::kBrushless};
@@ -64,20 +64,19 @@ struct RobotMap {
     wom::Gearbox shooterGearbox{&shooterMotor, nullptr, frc::DCMotor::NEO(1)};
 
     ShooterConfig config{
-        "shooterGearbox",
-        shooterGearbox,
+        "shooterGearbox", shooterGearbox,
         // &shooterSensor,
     };
-  }; Shooter shooterSystem;
-  
+  };
+  Shooter shooterSystem;
+
   struct SwerveBase {
     ctre::phoenix6::hardware::CANcoder frontLeftCancoder{18, "Drivebase"};
     ctre::phoenix6::hardware::CANcoder frontRightCancoder{19, "Drivebase"};
     ctre::phoenix6::hardware::CANcoder backLeftCancoder{16, "Drivebase"};
     ctre::phoenix6::hardware::CANcoder backRightCancoder{17, "Drivebase"};
 
-    ctre::phoenix6::hardware::Pigeon2* gyro =
-        new ctre::phoenix6::hardware::Pigeon2(20, "Drivebase");
+    ctre::phoenix6::hardware::Pigeon2* gyro = new ctre::phoenix6::hardware::Pigeon2(20, "Drivebase");
     wpi::array<ctre::phoenix6::hardware::TalonFX*, 4> turnMotors{
         new ctre::phoenix6::hardware::TalonFX(7, "Drivebase"),   // front left
         new ctre::phoenix6::hardware::TalonFX(2, "Drivebase"),   // front right
@@ -90,48 +89,38 @@ struct RobotMap {
         new ctre::phoenix6::hardware::TalonFX(3, "Drivebase")};  // back right
 
     wpi::array<wom::SwerveModuleConfig, 4> moduleConfigs{
-        wom::SwerveModuleConfig{ //CORRECT
+        wom::SwerveModuleConfig{
+            // CORRECT
             // front left module
             frc::Translation2d(10.761_in, 9.455_in),
-            wom::Gearbox{
-                driveMotors[0],
-                new wom::TalonFXEncoder(driveMotors[0], 0.0445_m, 6.75),
-                frc::DCMotor::Falcon500(1).WithReduction(6.75)},
-            wom::Gearbox{turnMotors[0],
-                         new wom::CanEncoder(18, 0.0445_m, 4096, 12.8),
+            wom::Gearbox{driveMotors[0], new wom::TalonFXEncoder(driveMotors[0], 0.0445_m, 6.75),
+                         frc::DCMotor::Falcon500(1).WithReduction(6.75)},
+            wom::Gearbox{turnMotors[0], new wom::CanEncoder(18, 0.0445_m, 4096, 12.8),
                          frc::DCMotor::Falcon500(1).WithReduction(12.8)},
             &frontLeftCancoder, 4_in / 2},
-        wom::SwerveModuleConfig{ //CORRECT
+        wom::SwerveModuleConfig{
+            // CORRECT
             // front right module
             frc::Translation2d(10.761_in, -9.455_in),
-            wom::Gearbox{
-                driveMotors[1],
-                new wom::TalonFXEncoder(driveMotors[1], 0.0445_m, 6.75),
-                frc::DCMotor::Falcon500(1).WithReduction(6.75)},
-            wom::Gearbox{turnMotors[1],
-                         new wom::CanEncoder(19, 0.0445_m, 4096, 12.8),
+            wom::Gearbox{driveMotors[1], new wom::TalonFXEncoder(driveMotors[1], 0.0445_m, 6.75),
+                         frc::DCMotor::Falcon500(1).WithReduction(6.75)},
+            wom::Gearbox{turnMotors[1], new wom::CanEncoder(19, 0.0445_m, 4096, 12.8),
                          frc::DCMotor::Falcon500(1).WithReduction(12.8)},
             &frontRightCancoder, 4_in / 2},
         wom::SwerveModuleConfig{
             // back left module
             frc::Translation2d(-10.761_in, 9.455_in),
-            wom::Gearbox{
-                driveMotors[2],
-                new wom::TalonFXEncoder(driveMotors[2], 0.0445_m, 6.75),
-                frc::DCMotor::Falcon500(1).WithReduction(6.75)},
-            wom::Gearbox{turnMotors[2],
-                         new wom::CanEncoder(16, 0.0445_m, 4096, 12.8),
+            wom::Gearbox{driveMotors[2], new wom::TalonFXEncoder(driveMotors[2], 0.0445_m, 6.75),
+                         frc::DCMotor::Falcon500(1).WithReduction(6.75)},
+            wom::Gearbox{turnMotors[2], new wom::CanEncoder(16, 0.0445_m, 4096, 12.8),
                          frc::DCMotor::Falcon500(1).WithReduction(12.8)},
             &backRightCancoder, 4_in / 2},
         wom::SwerveModuleConfig{
             // back right module
             frc::Translation2d(-10.761_in, -9.455_in),
-            wom::Gearbox{
-                driveMotors[3],
-                new wom::TalonFXEncoder(driveMotors[3], 0.0445_m, 6.75),
-                frc::DCMotor::Falcon500(1).WithReduction(6.75)},
-            wom::Gearbox{turnMotors[3],
-                         new wom::CanEncoder(17, 0.0445_m, 4096, 12.8),
+            wom::Gearbox{driveMotors[3], new wom::TalonFXEncoder(driveMotors[3], 0.0445_m, 6.75),
+                         frc::DCMotor::Falcon500(1).WithReduction(6.75)},
+            wom::Gearbox{turnMotors[3], new wom::CanEncoder(17, 0.0445_m, 4096, 12.8),
                          frc::DCMotor::Falcon500(1).WithReduction(12.8)},
             &backLeftCancoder, 4_in / 2},
     };
@@ -151,19 +140,16 @@ struct RobotMap {
         wom::SwerveDriveConfig::pose_angle_conf_t::ki_t{0},
         0_deg / 1_deg};*/
     wom::SwerveDriveConfig::pose_position_conf_t posePositionPID{
-        "/drivetrain/pid/pose/position/config",
-        0_mps / 1_m,
-        wom::SwerveDriveConfig::pose_position_conf_t::ki_t{0.15},
-        0_m / 1_m,
-        0_cm};
+        "/drivetrain/pid/pose/position/config", 0_mps / 1_m,
+        wom::SwerveDriveConfig::pose_position_conf_t::ki_t{0.15}, 0_m / 1_m, 0_cm};
 
     // the config for the whole swerve drive
     wom::SwerveDriveConfig config{"/drivetrain",
-                                  //anglePID,
+                                  // anglePID,
                                   velocityPID,
                                   moduleConfigs,  // each module
                                   gyro,
-                                  //poseAnglePID,
+                                  // poseAnglePID,
                                   posePositionPID,
                                   60_kg,  // robot mass (estimate rn)
                                   {0.1, 0.1, 0.1},
@@ -183,6 +169,8 @@ struct RobotMap {
   SwerveBase swerveBase;
 
   struct SwerveTable {
-    std::shared_ptr<nt::NetworkTable> swerveDriveTable = nt::NetworkTableInstance::GetDefault().GetTable("swerve");
-  }; SwerveTable swerveTable;
+    std::shared_ptr<nt::NetworkTable> swerveDriveTable =
+        nt::NetworkTableInstance::GetDefault().GetTable("swerve");
+  };
+  SwerveTable swerveTable;
 };
diff --git a/wombat/src/main/cpp/behaviour/Behaviour.cpp b/wombat/src/main/cpp/behaviour/Behaviour.cpp
index d21d9e57..7312c922 100644
--- a/wombat/src/main/cpp/behaviour/Behaviour.cpp
+++ b/wombat/src/main/cpp/behaviour/Behaviour.cpp
@@ -10,9 +10,7 @@ using namespace behaviour;
 
 // Behaviour
 Behaviour::Behaviour(std::string name, units::time::second_t period)
-    : _bhvr_name(name),
-      _bhvr_period(period),
-      _bhvr_state(BehaviourState::INITIALISED) {}
+    : _bhvr_name(name), _bhvr_period(period), _bhvr_state(BehaviourState::INITIALISED) {}
 Behaviour::~Behaviour() {
   if (!IsFinished())
     Interrupt();
@@ -82,8 +80,7 @@ bool Behaviour::Tick() {
 
     if (dt > 2 * _bhvr_period) {
       std::cerr << "Behaviour missed deadline. Reduce Period. Dt=" << dt.value()
-                << " Dt(deadline)=" << (2 * _bhvr_period).value()
-                << ". Bhvr: " << GetName() << std::endl;
+                << " Dt(deadline)=" << (2 * _bhvr_period).value() << ". Bhvr: " << GetName() << std::endl;
     }
 
     if (_bhvr_timeout.value() > 0 && _bhvr_timer > _bhvr_timeout)
@@ -100,8 +97,7 @@ bool Behaviour::IsRunning() const {
 }
 
 bool Behaviour::IsFinished() const {
-  return _bhvr_state != BehaviourState::INITIALISED &&
-         _bhvr_state != BehaviourState::RUNNING;
+  return _bhvr_state != BehaviourState::INITIALISED && _bhvr_state != BehaviourState::RUNNING;
 }
 
 void Behaviour::Stop(BehaviourState new_state) {
@@ -173,8 +169,7 @@ void ConcurrentBehaviour::Add(Behaviour::ptr behaviour) {
 }
 
 std::string ConcurrentBehaviour::GetName() const {
-  std::string msg =
-      (_reducer == ConcurrentBehaviourReducer::ALL ? "ALL { " : "RACE {");
+  std::string msg = (_reducer == ConcurrentBehaviourReducer::ALL ? "ALL { " : "RACE {");
   for (auto b : _children)
     msg += b->GetName() + ", ";
   msg += "}";
@@ -190,8 +185,8 @@ void ConcurrentBehaviour::OnStart() {
         using namespace std::chrono_literals;
 
         b->Tick();
-        std::this_thread::sleep_for(std::chrono::milliseconds(
-            static_cast<int64_t>(b->GetPeriod().value() * 1000)));
+        std::this_thread::sleep_for(
+            std::chrono::milliseconds(static_cast<int64_t>(b->GetPeriod().value() * 1000)));
       }
 
       if (IsFinished() && !b->IsFinished())
@@ -273,10 +268,8 @@ void WaitFor::OnTick(units::time::second_t dt) {
 }
 
 // WaitTime
-WaitTime::WaitTime(units::time::second_t time)
-    : WaitTime([time]() { return time; }) {}
-WaitTime::WaitTime(std::function<units::time::second_t()> time_fn)
-    : _time_fn(time_fn) {}
+WaitTime::WaitTime(units::time::second_t time) : WaitTime([time]() { return time; }) {}
+WaitTime::WaitTime(std::function<units::time::second_t()> time_fn) : _time_fn(time_fn) {}
 
 void WaitTime::OnStart() {
   _time = _time_fn();
diff --git a/wombat/src/main/cpp/behaviour/BehaviourScheduler.cpp b/wombat/src/main/cpp/behaviour/BehaviourScheduler.cpp
index 26863c79..eb363b3e 100644
--- a/wombat/src/main/cpp/behaviour/BehaviourScheduler.cpp
+++ b/wombat/src/main/cpp/behaviour/BehaviourScheduler.cpp
@@ -52,8 +52,8 @@ void BehaviourScheduler::Schedule(Behaviour::ptr behaviour) {
         std::lock_guard<std::recursive_mutex> lk(_active_mtx);
         behaviour->Tick();
       }
-      std::this_thread::sleep_for(std::chrono::milliseconds(
-          static_cast<int64_t>(behaviour->GetPeriod().value() * 1000)));
+      std::this_thread::sleep_for(
+          std::chrono::milliseconds(static_cast<int64_t>(behaviour->GetPeriod().value() * 1000)));
     }
   });
 }
diff --git a/wombat/src/main/cpp/behaviour/HasBehaviour.cpp b/wombat/src/main/cpp/behaviour/HasBehaviour.cpp
index 964522c6..f2f557c5 100644
--- a/wombat/src/main/cpp/behaviour/HasBehaviour.cpp
+++ b/wombat/src/main/cpp/behaviour/HasBehaviour.cpp
@@ -8,8 +8,7 @@
 
 using namespace behaviour;
 
-void HasBehaviour::SetDefaultBehaviour(
-    std::function<std::shared_ptr<Behaviour>(void)> fn) {
+void HasBehaviour::SetDefaultBehaviour(std::function<std::shared_ptr<Behaviour>(void)> fn) {
   _default_behaviour_producer = fn;
 }
 
diff --git a/wombat/src/main/cpp/drivetrain/Drivetrain.cpp b/wombat/src/main/cpp/drivetrain/Drivetrain.cpp
index e516e7d7..387bbc31 100644
--- a/wombat/src/main/cpp/drivetrain/Drivetrain.cpp
+++ b/wombat/src/main/cpp/drivetrain/Drivetrain.cpp
@@ -11,8 +11,7 @@
 using namespace frc;
 using namespace units;
 
-wom::drivetrain::Drivetrain::Drivetrain(DrivetrainConfig* config,
-                                        XboxController& driver)
+wom::drivetrain::Drivetrain::Drivetrain(DrivetrainConfig* config, XboxController& driver)
     : _config(config), _driver(driver) {}
 wom::drivetrain::Drivetrain::~Drivetrain() {}
 
diff --git a/wombat/src/main/cpp/drivetrain/SwerveDrive.cpp b/wombat/src/main/cpp/drivetrain/SwerveDrive.cpp
index eb33efb1..74fc6d89 100644
--- a/wombat/src/main/cpp/drivetrain/SwerveDrive.cpp
+++ b/wombat/src/main/cpp/drivetrain/SwerveDrive.cpp
@@ -5,9 +5,9 @@
 #include "drivetrain/SwerveDrive.h"
 
 #include <networktables/NetworkTableInstance.h>
+#include <units/angle.h>
 #include <units/math.h>
 #include <units/voltage.h>
-#include <units/angle.h>
 
 #include <iostream>
 
@@ -16,17 +16,16 @@
 namespace wom {
 namespace drivetrain {
 
-void SwerveModuleConfig::WriteNT(
-    std::shared_ptr<nt::NetworkTable> table) const {
+void SwerveModuleConfig::WriteNT(std::shared_ptr<nt::NetworkTable> table) const {
   std::array<double, 2> pos{position.X().value(), position.Y().value()};
   table->GetEntry("position").SetDoubleArray(std::span(pos));
   table->GetEntry("wheelRadius").SetDouble(wheelRadius.value());
 }
 
 SwerveModule::SwerveModule(std::string path, SwerveModuleConfig config,
-                           //SwerveModule::angle_pid_conf_t anglePID,
+                           // SwerveModule::angle_pid_conf_t anglePID,
                            SwerveModule::velocity_pid_conf_t velocityPID)
-    : //_anglePIDController(path + "/pid/angle", anglePID),
+    :  // _anglePIDController(path + "/pid/angle", anglePID),
       _anglePIDController{frc::PIDController(4, 0, 0, 0.005_s)},
       _config(config),
       _velocityPIDController(frc::PIDController(1.2, 0, 0, 0.005_s)),
@@ -46,8 +45,6 @@ void SwerveModule::OnUpdate(units::second_t dt) {
   units::volt_t driveVoltage{0};
   units::volt_t turnVoltage{0};
 
-
-
   switch (_state) {
     case SwerveModuleState::kIdle:
       driveVoltage = 0_V;
@@ -55,10 +52,11 @@ void SwerveModule::OnUpdate(units::second_t dt) {
       break;
     case SwerveModuleState::kPID: {
       auto feedforward = _config.driveMotor.motor.Voltage(
-          0_Nm, units::radians_per_second_t{
-                    _velocityPIDController.GetSetpoint() / _config.wheelRadius.value()});
-      //std::cout << "GetSpeed()" << GetSpeed().value() << std::endl;
-      // units::radian_t input = units::math::fmod(_config.turnMotor.encoder->GetEncoderPosition(), (2_rad * 3.1415));
+          0_Nm,
+          units::radians_per_second_t{_velocityPIDController.GetSetpoint() / _config.wheelRadius.value()});
+      // std::cout << "GetSpeed()" << GetSpeed().value() << std::endl;
+      //  units::radian_t input = units::math::fmod(_config.turnMotor.encoder->GetEncoderPosition(), (2_rad
+      //  * 3.1415));
       double input = -_config.turnMotor.encoder->GetEncoderPosition().value();
       _table->GetEntry("/testing/GetEncoderPos").SetDouble(input);
       // _velocityPIDController.SetSetpoint(3);
@@ -75,8 +73,7 @@ void SwerveModule::OnUpdate(units::second_t dt) {
       std::cerr << "Case not handled" << std::endl;
   }
 
-  units::newton_meter_t torqueLimit =
-      50_kg / 4 * _config.wheelRadius * _currentAccelerationLimit;
+  units::newton_meter_t torqueLimit = 50_kg / 4 * _config.wheelRadius * _currentAccelerationLimit;
   // units::volt_t voltageMax = _config.driveMotor.motor.Voltage(
   //     torqueLimit, _config.driveMotor.encoder->GetEncoderAngularVelocity());
   // units::volt_t voltageMin = _config.driveMotor.motor.Voltage(
@@ -92,8 +89,7 @@ void SwerveModule::OnUpdate(units::second_t dt) {
   //     units::math::max(driveVoltage, -_driveModuleVoltageLimit),
   //     _driveModuleVoltageLimit);  // was originally 10_V
   units::volt_t turnVoltageMax = 7_V - (driveVoltage * (7_V / 10_V));
-  turnVoltage = units::math::min(units::math::max(turnVoltage, -turnVoltageMax),
-                                 turnVoltageMax);
+  turnVoltage = units::math::min(units::math::max(turnVoltage, -turnVoltageMax), turnVoltageMax);
   // turnVoltage = units::math::min(units::math::max(turnVoltage, -7_V), 7_V);
   // std::cout << "turn-voltage-max: " << turnVoltageMax.value() << std::endl;
 
@@ -106,9 +102,7 @@ void SwerveModule::OnUpdate(units::second_t dt) {
 
   _table->GetEntry("speed").SetDouble(GetSpeed().value());
   _table->GetEntry("angle").SetDouble(
-      _config.turnMotor.encoder->GetEncoderPosition()
-          .convert<units::degree>()
-          .value());
+      _config.turnMotor.encoder->GetEncoderPosition().convert<units::degree>().value());
   _config.WriteNT(_table->GetSubTable("config"));
 }
 
@@ -116,13 +110,11 @@ void SwerveModule::OnUpdate(units::second_t dt) {
 //   return (_config.turnMotor.encoder->GetEncoderPosition().value());
 // }
 
-void SwerveModule::SetAccelerationLimit(
-    units::meters_per_second_squared_t limit) {
+void SwerveModule::SetAccelerationLimit(units::meters_per_second_squared_t limit) {
   _currentAccelerationLimit = limit;
 }
 
-void SwerveDrive::SetAccelerationLimit(
-    units::meters_per_second_squared_t limit) {
+void SwerveDrive::SetAccelerationLimit(units::meters_per_second_squared_t limit) {
   for (int motorNumber = 0; motorNumber < 4; motorNumber++) {
     _modules[motorNumber].SetAccelerationLimit(limit);
   }
@@ -141,16 +133,13 @@ void SwerveModule::SetZero(units::second_t dt) {
   _state = SwerveModuleState::kPID;
 }
 
-void SwerveModule::SetPID(units::radian_t angle,
-                          units::meters_per_second_t speed,
-                          units::second_t dt) {
+void SwerveModule::SetPID(units::radian_t angle, units::meters_per_second_t speed, units::second_t dt) {
   _state = SwerveModuleState::kPID;
   // @liam start added
   // std::cout << "angle Setpoint: " << _anglePIDController.GetSetpoint() << std::endl;
   // std::cout << "angle value: " << angle.value() << std::endl;
 
-  double diff = std::fmod((_anglePIDController.GetSetpoint() - angle.value()),
-                          (2 * 3.1415));
+  double diff = std::fmod((_anglePIDController.GetSetpoint() - angle.value()), (2 * 3.1415));
   std::cout << "DIFF value: " << diff << std::endl;
   _table->GetEntry("/Differential value:").SetDouble(diff);
   if (std::abs(diff) >= 3.1415) {
@@ -165,39 +154,32 @@ void SwerveModule::SetPID(units::radian_t angle,
 
 void SwerveModule::ModuleVectorHandler(frc::ChassisSpeeds speeds) {
   units::meters_per_second_t xVelocityComponent =
-      1_mps *
-      (speeds.vx.value() + speeds.omega.value() * _config.position.X().value());
+      1_mps * (speeds.vx.value() + speeds.omega.value() * _config.position.X().value());
   units::meters_per_second_t yVelocityComponent =
-      1_mps *
-      (speeds.vy.value() + speeds.omega.value() * _config.position.Y().value());
+      1_mps * (speeds.vy.value() + speeds.omega.value() * _config.position.Y().value());
 
   units::meters_per_second_t velocity =
-      1_mps * std::sqrt(std::pow(xVelocityComponent.value(), 2) +
-                        std::pow(yVelocityComponent.value(), 2));
-  units::degree_t angle = 1_rad * std::atan2(yVelocityComponent.value(),
-                                             xVelocityComponent.value());
+      1_mps * std::sqrt(std::pow(xVelocityComponent.value(), 2) + std::pow(yVelocityComponent.value(), 2));
+  units::degree_t angle = 1_rad * std::atan2(yVelocityComponent.value(), xVelocityComponent.value());
 
   _anglePIDController.SetSetpoint(angle.value());
   _velocityPIDController.SetSetpoint(velocity.value());
 }
 
 units::meters_per_second_t SwerveModule::GetSpeed() const {
-  units::meters_per_second_t returnVal{
-    _config.driveMotor.encoder->GetVelocityValue() *
-    _config.wheelRadius.value()};
-    // std::cout << returnVal.value() << std::endl;
-    return returnVal;
+  units::meters_per_second_t returnVal{_config.driveMotor.encoder->GetVelocityValue() *
+                                       _config.wheelRadius.value()};
+  // std::cout << returnVal.value() << std::endl;
+  return returnVal;
 }
 
 units::meter_t SwerveModule::GetDistance() const {
-  return units::meter_t{
-      _config.driveMotor.encoder->GetEncoderPosition().value() *
-      _config.wheelRadius.value()};
+  return units::meter_t{_config.driveMotor.encoder->GetEncoderPosition().value() *
+                        _config.wheelRadius.value()};
 }
 
 frc::SwerveModulePosition SwerveModule::GetPosition() const {
-  return frc::SwerveModulePosition{
-      GetDistance(), _config.turnMotor.encoder->GetEncoderPosition()};
+  return frc::SwerveModulePosition{GetDistance(), _config.turnMotor.encoder->GetEncoderPosition()};
 }
 
 const SwerveModuleConfig& SwerveModule::GetConfig() const {
@@ -210,15 +192,14 @@ void SwerveDriveConfig::WriteNT(std::shared_ptr<nt::NetworkTable> table) {
 
 SwerveDrive::SwerveDrive(SwerveDriveConfig config, frc::Pose2d initialPose)
     : _config(config),
-      _kinematics(_config.modules[0].position, _config.modules[1].position,
-                  _config.modules[2].position, _config.modules[3].position),
+      _kinematics(_config.modules[0].position, _config.modules[1].position, _config.modules[2].position,
+                  _config.modules[3].position),
       _poseEstimator(
           _kinematics, frc::Rotation2d(0_deg),
-          wpi::array<frc::SwerveModulePosition, 4>{
-              frc::SwerveModulePosition{0_m, frc::Rotation2d{0_deg}},
-              frc::SwerveModulePosition{0_m, frc::Rotation2d{0_deg}},
-              frc::SwerveModulePosition{0_m, frc::Rotation2d{0_deg}},
-              frc::SwerveModulePosition{0_m, frc::Rotation2d{0_deg}}},
+          wpi::array<frc::SwerveModulePosition, 4>{frc::SwerveModulePosition{0_m, frc::Rotation2d{0_deg}},
+                                                   frc::SwerveModulePosition{0_m, frc::Rotation2d{0_deg}},
+                                                   frc::SwerveModulePosition{0_m, frc::Rotation2d{0_deg}},
+                                                   frc::SwerveModulePosition{0_m, frc::Rotation2d{0_deg}}},
           initialPose, _config.stateStdDevs, _config.visionMeasurementStdDevs),
       _anglePIDController{frc::PIDController(1, 0, 0)},
       _xPIDController(config.path + "/pid/x", _config.posePositionPID),
@@ -236,14 +217,12 @@ SwerveDrive::SwerveDrive(SwerveDriveConfig config, frc::Pose2d initialPose)
   ResetPose(initialPose);
 }
 
-frc::ChassisSpeeds FieldRelativeSpeeds::ToChassisSpeeds(
-    const units::radian_t robotHeading) {
-  return frc::ChassisSpeeds::FromFieldRelativeSpeeds(
-      vx, vy, omega, frc::Rotation2d{robotHeading});
+frc::ChassisSpeeds FieldRelativeSpeeds::ToChassisSpeeds(const units::radian_t robotHeading) {
+  return frc::ChassisSpeeds::FromFieldRelativeSpeeds(vx, vy, omega, frc::Rotation2d{robotHeading});
 }
 
 void SwerveDrive::OnUpdate(units::second_t dt) {
-  //std::cout << _config.gyro->GetYaw().GetValue().value() << std::endl;
+  // std::cout << _config.gyro->GetYaw().GetValue().value() << std::endl;
   switch (_state) {
     case SwerveDriveState::kZeroing:
       for (auto mod = _modules.begin(); mod < _modules.end(); mod++) {
@@ -258,14 +237,15 @@ void SwerveDrive::OnUpdate(units::second_t dt) {
     case SwerveDriveState::kPose: {
       _target_fr_speeds.vx = _xPIDController.Calculate(GetPose().X(), dt);
       _target_fr_speeds.vy = _yPIDController.Calculate(GetPose().Y(), dt);
-      _target_fr_speeds.omega = units::radians_per_second_t{_anglePIDController.Calculate(GetPose().Rotation().Radians().value())};
+      _target_fr_speeds.omega =
+          units::radians_per_second_t{_anglePIDController.Calculate(GetPose().Rotation().Radians().value())};
     }
       [[fallthrough]];
     case SwerveDriveState::kFieldRelativeVelocity:
-      _target_speed =
-          _target_fr_speeds.ToChassisSpeeds(GetPose().Rotation().Radians());
+      _target_speed = _target_fr_speeds.ToChassisSpeeds(GetPose().Rotation().Radians());
       if (isRotateToMatchJoystick) {
-        _target_speed.omega = units::radians_per_second_t{_anglePIDController.Calculate(GetPose().Rotation().Radians().value())};
+        _target_speed.omega = units::radians_per_second_t{
+            _anglePIDController.Calculate(GetPose().Rotation().Radians().value())};
       }
       // std::cout << "vx = " << _target_speed.vx.value() << " vy = " <<
       // _target_fr_speeds.vy.value() << std::endl;
@@ -273,8 +253,7 @@ void SwerveDrive::OnUpdate(units::second_t dt) {
     case SwerveDriveState::kVelocity: {
       auto target_states = _kinematics.ToSwerveModuleStates(_target_speed);
       for (size_t i = 0; i < _modules.size(); i++) {
-        _modules[i].SetPID(target_states[i].angle.Radians(),
-                           target_states[i].speed, dt);
+        _modules[i].SetPID(target_states[i].angle.Radians(), target_states[i].speed, dt);
         // std::cout << "module " << i << ": " <<
         // target_states[i].angle.Radians().value() << std::endl;
       }
@@ -297,13 +276,12 @@ void SwerveDrive::OnUpdate(units::second_t dt) {
     case SwerveDriveState::kFRVelocityRotationLock:
       _target_speed.vx = _xPIDController.Calculate(GetPose().X(), dt);
       _target_speed.vy = _yPIDController.Calculate(GetPose().Y(), dt);
-      _target_speed.omega = units::radians_per_second_t{_anglePIDController.Calculate(GetPose().Rotation().Radians().value())};
-      _target_speed =
-          _requestedSpeeds.ToChassisSpeeds(GetPose().Rotation().Radians());
+      _target_speed.omega =
+          units::radians_per_second_t{_anglePIDController.Calculate(GetPose().Rotation().Radians().value())};
+      _target_speed = _requestedSpeeds.ToChassisSpeeds(GetPose().Rotation().Radians());
       auto target_states = _kinematics.ToSwerveModuleStates(_target_speed);
       for (size_t i = 0; i < _modules.size(); i++) {
-        _modules[i].SetPID(target_states[i].angle.Radians(),
-                           target_states[i].speed, dt);
+        _modules[i].SetPID(target_states[i].angle.Radians(), target_states[i].speed, dt);
       }
       break;
   }
@@ -314,12 +292,10 @@ void SwerveDrive::OnUpdate(units::second_t dt) {
 
   _poseEstimator.Update(
       _config.gyro->GetRotation2d(),
-      wpi::array<frc::SwerveModulePosition, 4>{
-          _modules[0].GetPosition(), _modules[1].GetPosition(),
-          _modules[2].GetPosition(), _modules[3].GetPosition()});
+      wpi::array<frc::SwerveModulePosition, 4>{_modules[0].GetPosition(), _modules[1].GetPosition(),
+                                               _modules[2].GetPosition(), _modules[3].GetPosition()});
 
-  utils::WritePose2NT(_table->GetSubTable("estimatedPose"),
-                      _poseEstimator.GetEstimatedPosition());
+  utils::WritePose2NT(_table->GetSubTable("estimatedPose"), _poseEstimator.GetEstimatedPosition());
   _config.WriteNT(_table->GetSubTable("config"));
 }
 
@@ -358,8 +334,7 @@ void SwerveDrive::OnResetMode() {
   _anglePIDController.Reset();
 }
 
-void SwerveDrive::RotateMatchJoystick(units::radian_t joystickAngle,
-                                      FieldRelativeSpeeds speeds) {
+void SwerveDrive::RotateMatchJoystick(units::radian_t joystickAngle, FieldRelativeSpeeds speeds) {
   // _state = SwerveDriveState::kFRVelocityRotationLock;
   // _anglePIDController.SetSetpoint(joystickAngle);
   // _target_fr_speeds = speeds;
@@ -385,16 +360,14 @@ bool SwerveDrive::GetIsFieldRelative() {
   return _isFieldRelative;
 }
 
-void SwerveDrive::SetIndividualTuning(int mod, units::radian_t angle,
-                                      units::meters_per_second_t speed) {
+void SwerveDrive::SetIndividualTuning(int mod, units::radian_t angle, units::meters_per_second_t speed) {
   _mod = mod;
   _angle = angle;
   _speed = speed;
   _state = SwerveDriveState::kIndividualTuning;
 }
 
-void SwerveDrive::SetTuning(units::radian_t angle,
-                            units::meters_per_second_t speed) {
+void SwerveDrive::SetTuning(units::radian_t angle, units::meters_per_second_t speed) {
   _angle = angle;
   _speed = speed;
   _state = SwerveDriveState::kTuning;
@@ -421,9 +394,8 @@ bool SwerveDrive::IsAtSetPose() {
 void SwerveDrive::ResetPose(frc::Pose2d pose) {
   _poseEstimator.ResetPosition(
       _config.gyro->GetRotation2d(),
-      wpi::array<frc::SwerveModulePosition, 4>{
-          _modules[0].GetPosition(), _modules[1].GetPosition(),
-          _modules[2].GetPosition(), _modules[3].GetPosition()},
+      wpi::array<frc::SwerveModulePosition, 4>{_modules[0].GetPosition(), _modules[1].GetPosition(),
+                                               _modules[2].GetPosition(), _modules[3].GetPosition()},
       pose);
 }
 
@@ -431,8 +403,7 @@ frc::Pose2d SwerveDrive::GetPose() {
   return _poseEstimator.GetEstimatedPosition();
 }
 
-void SwerveDrive::AddVisionMeasurement(frc::Pose2d pose,
-                                       units::second_t timestamp) {
+void SwerveDrive::AddVisionMeasurement(frc::Pose2d pose, units::second_t timestamp) {
   _poseEstimator.AddVisionMeasurement(pose, timestamp);
 }
 }  // namespace drivetrain
diff --git a/wombat/src/main/cpp/drivetrain/behaviours/SwerveBehaviours.cpp b/wombat/src/main/cpp/drivetrain/behaviours/SwerveBehaviours.cpp
index a82c84d8..23baf7fd 100644
--- a/wombat/src/main/cpp/drivetrain/behaviours/SwerveBehaviours.cpp
+++ b/wombat/src/main/cpp/drivetrain/behaviours/SwerveBehaviours.cpp
@@ -73,40 +73,36 @@ void ManualDrivebase::OnTick(units::second_t deltaTime) {
   // if (isZero) {
   //   _swerveDrivebase->SetZeroing();
   // } else {
-    double xVelocity = wom::utils::spow2(-wom::utils::deadzone(
-        _driverController->GetLeftY(),
-        driverDeadzone));  // GetLeftY due to x being where y should be on field
-    double yVelocity = wom::utils::spow2(
-        -wom::utils::deadzone(_driverController->GetLeftX(), driverDeadzone));
-
-    double r_x = wom::utils::spow2(
-        -wom::utils::deadzone(_driverController->GetRightX(), turningDeadzone));
-
-    double turnX = _driverController->GetRightX();
-    double turnY = _driverController->GetRightY();
-    double num = std::sqrt(turnX * turnX + turnY * turnY);
-    if (num < turningDeadzone) {
-      turnX = 0;
-      turnY = 0;
-    }
-
-    // if (isRotateMatch) {
-    //   units::degree_t currentAngle =
-    //       _swerveDrivebase->GetPose().Rotation().Degrees();
-    //   CalculateRequestedAngle(turnX, turnY, currentAngle);
-    //   _swerveDriveTable->GetEntry("RotateMatch")
-    //       .SetDouble(_requestedAngle.value());
-    //   _swerveDrivebase->RotateMatchJoystick(
-    //       _requestedAngle,
-    //       wom::drivetrain::FieldRelativeSpeeds{// also field relative
-    //                                            xVelocity * maxMovementMagnitude,
-    //                                            yVelocity * maxMovementMagnitude,
-    //                                            r_x * maxRotationMagnitude});
-    // } else {
-      _swerveDrivebase->SetFieldRelativeVelocity(
-          wom::drivetrain::FieldRelativeSpeeds{xVelocity * maxMovementMagnitude,
-                                               yVelocity * maxMovementMagnitude,
-                                               r_x * maxRotationMagnitude});
+  double xVelocity = wom::utils::spow2(
+      -wom::utils::deadzone(_driverController->GetLeftY(),
+                            driverDeadzone));  // GetLeftY due to x being where y should be on field
+  double yVelocity = wom::utils::spow2(-wom::utils::deadzone(_driverController->GetLeftX(), driverDeadzone));
+
+  double r_x = wom::utils::spow2(-wom::utils::deadzone(_driverController->GetRightX(), turningDeadzone));
+
+  double turnX = _driverController->GetRightX();
+  double turnY = _driverController->GetRightY();
+  double num = std::sqrt(turnX * turnX + turnY * turnY);
+  if (num < turningDeadzone) {
+    turnX = 0;
+    turnY = 0;
+  }
+
+  // if (isRotateMatch) {
+  //   units::degree_t currentAngle =
+  //       _swerveDrivebase->GetPose().Rotation().Degrees();
+  //   CalculateRequestedAngle(turnX, turnY, currentAngle);
+  //   _swerveDriveTable->GetEntry("RotateMatch")
+  //       .SetDouble(_requestedAngle.value());
+  //   _swerveDrivebase->RotateMatchJoystick(
+  //       _requestedAngle,
+  //       wom::drivetrain::FieldRelativeSpeeds{// also field relative
+  //                                            xVelocity * maxMovementMagnitude,
+  //                                            yVelocity * maxMovementMagnitude,
+  //                                            r_x * maxRotationMagnitude});
+  // } else {
+  _swerveDrivebase->SetFieldRelativeVelocity(wom::drivetrain::FieldRelativeSpeeds{
+      xVelocity * maxMovementMagnitude, yVelocity * maxMovementMagnitude, r_x * maxRotationMagnitude});
   //   }
   // }
   // _swerveDrivebase->SetTuning(100_deg, 1_mps);
@@ -117,8 +113,7 @@ void ManualDrivebase::ResetMode() {
   resetMode = false;
 }
 
-void ManualDrivebase::CalculateRequestedAngle(double joystickX,
-                                              double joystickY,
+void ManualDrivebase::CalculateRequestedAngle(double joystickX, double joystickY,
                                               units::degree_t defaultAngle) {
   _requestedAngle = (1_rad * std::atan2(joystickY, -joystickX)) + 90_deg;
   if (wom::utils::deadzone(joystickX) == 0 && wom::utils::deadzone(joystickY) == 0) {
@@ -127,8 +122,7 @@ void ManualDrivebase::CalculateRequestedAngle(double joystickX,
 }
 
 // Code for x-ing the wheels on the drivebase
-XDrivebase::XDrivebase(wom::drivetrain::SwerveDrive* swerveDrivebase)
-    : _swerveDrivebase(swerveDrivebase) {
+XDrivebase::XDrivebase(wom::drivetrain::SwerveDrive* swerveDrivebase) : _swerveDrivebase(swerveDrivebase) {
   Controls(swerveDrivebase);
 }
 void XDrivebase::OnTick(units::second_t deltaTime) {
@@ -170,20 +164,17 @@ void XDrivebase::OnTick(units::second_t deltaTime) {
 //   m_timer.Start();
 // }
 
-wom::drivetrain::behaviours::TempSimSwerveDrive::TempSimSwerveDrive(
-    frc::Timer* timer, frc::Field2d* field)
+wom::drivetrain::behaviours::TempSimSwerveDrive::TempSimSwerveDrive(frc::Timer* timer, frc::Field2d* field)
     : m_timer(timer), m_field(field) {}
 
 void wom::drivetrain::behaviours::TempSimSwerveDrive::OnUpdate() {
   m_field->SetRobotPose(m_driveSim.GetPose());
 
   // get the current trajectory state
-  frc::Trajectory::State desired_state =
-      current_trajectory.Sample(m_timer->Get());
+  frc::Trajectory::State desired_state = current_trajectory.Sample(m_timer->Get());
 
   // get the current wheel speeds
-  wom::utils::WriteTrajectoryState(current_trajectory_state_table,
-                                   desired_state);
+  wom::utils::WriteTrajectoryState(current_trajectory_state_table, desired_state);
 
   // move drivebase position to the desired state
   m_driveSim.SetPose(wom::utils::TrajectoryStateToPose2d(desired_state));
@@ -201,19 +192,16 @@ frc::Pose2d wom::drivetrain::behaviours::TempSimSwerveDrive::GetPose2d() {
   return m_driveSim.GetPose();
 }
 
-void wom::drivetrain::behaviours::TempSimSwerveDrive::SetPath(
-    std::string path) {
+void wom::drivetrain::behaviours::TempSimSwerveDrive::SetPath(std::string path) {
   nt::NetworkTableInstance inst = nt::NetworkTableInstance::GetDefault();
   std::shared_ptr<nt::NetworkTable> table = inst.GetTable("FMSInfo");
 
   // create a netowrk table for the trajectory
   std::shared_ptr<nt::NetworkTable> trajectory_table =
       nt::NetworkTableInstance::GetDefault().GetTable("trajectory_path");
-  current_trajectory_table =
-      nt::NetworkTableInstance::GetDefault().GetTable("current_trajectory");
+  current_trajectory_table = nt::NetworkTableInstance::GetDefault().GetTable("current_trajectory");
   current_trajectory_state_table =
-      nt::NetworkTableInstance::GetDefault().GetTable(
-          "current_trajectory_state");
+      nt::NetworkTableInstance::GetDefault().GetTable("current_trajectory_state");
 
   current_trajectory = m_pathplanner.getTrajectory(path);
   m_driveSim.SetPose(current_trajectory.Sample(0_s).pose);
@@ -221,12 +209,10 @@ void wom::drivetrain::behaviours::TempSimSwerveDrive::SetPath(
   m_timer->Start();
 }
 
-wom::drivetrain::behaviours::AutoSwerveDrive::AutoSwerveDrive(
-    wom::drivetrain::SwerveDrive* swerve, frc::Timer* timer,
-    frc::Field2d* field)
+wom::drivetrain::behaviours::AutoSwerveDrive::AutoSwerveDrive(wom::drivetrain::SwerveDrive* swerve,
+                                                              frc::Timer* timer, frc::Field2d* field)
     : _swerve(swerve), m_timer(timer), m_field(field) {
-  _simSwerveDrive =
-      new wom::drivetrain::behaviours::TempSimSwerveDrive(timer, field);
+  _simSwerveDrive = new wom::drivetrain::behaviours::TempSimSwerveDrive(timer, field);
 }
 
 void wom::drivetrain::behaviours::AutoSwerveDrive::OnUpdate() {
diff --git a/wombat/src/main/cpp/subsystems/Arm.cpp b/wombat/src/main/cpp/subsystems/Arm.cpp
index 9390601a..148cec43 100644
--- a/wombat/src/main/cpp/subsystems/Arm.cpp
+++ b/wombat/src/main/cpp/subsystems/Arm.cpp
@@ -10,19 +10,14 @@ using namespace frc;
 using namespace wom;
 
 // creates network table instatnce on shuffleboard
-void wom::subsystems::ArmConfig::WriteNT(
-    std::shared_ptr<nt::NetworkTable> table) {
+void wom::subsystems::ArmConfig::WriteNT(std::shared_ptr<nt::NetworkTable> table) {
   table->GetEntry("armMass").SetDouble(armMass.value());
   table->GetEntry("loadMass").SetDouble(loadMass.value());
   table->GetEntry("armLength").SetDouble(armLength.value());
-  table->GetEntry("minAngle")
-      .SetDouble(minAngle.convert<units::degree>().value());
-  table->GetEntry("maxAngle")
-      .SetDouble(maxAngle.convert<units::degree>().value());
-  table->GetEntry("initialAngle")
-      .SetDouble(initialAngle.convert<units::degree>().value());
-  table->GetEntry("angleOffset")
-      .SetDouble(initialAngle.convert<units::degree>().value());
+  table->GetEntry("minAngle").SetDouble(minAngle.convert<units::degree>().value());
+  table->GetEntry("maxAngle").SetDouble(maxAngle.convert<units::degree>().value());
+  table->GetEntry("initialAngle").SetDouble(initialAngle.convert<units::degree>().value());
+  table->GetEntry("angleOffset").SetDouble(initialAngle.convert<units::degree>().value());
 }
 
 // arm config is used
@@ -43,14 +38,12 @@ void wom::subsystems::Arm::OnUpdate(units::second_t dt) {
     case ArmState::kIdle:
       break;
     case ArmState::kVelocity: {
-      units::newton_meter_t torque =
-          9.81_m / 1_s / 1_s * _config.armLength *
-          units::math::cos(angle + _config.angleOffset) *
-          (0.5 * _config.armMass + _config.loadMass);
+      units::newton_meter_t torque = 9.81_m / 1_s / 1_s * _config.armLength *
+                                     units::math::cos(angle + _config.angleOffset) *
+                                     (0.5 * _config.armMass + _config.loadMass);
       // units::volt_t feedforward = _config.leftGearbox.motor.Voltage(torque,
       // 0_rad/1_s);
-      units::volt_t feedforward =
-          _config.leftGearbox.motor.Voltage(torque, _velocityPID.GetSetpoint());
+      units::volt_t feedforward = _config.leftGearbox.motor.Voltage(torque, _velocityPID.GetSetpoint());
       // feedforward = 3.5_V;
       // std::cout << "feedforward" << feedforward.value() << std::endl;
       voltage = _velocityPID.Calculate(GetArmVelocity(), dt, feedforward);
@@ -58,12 +51,10 @@ void wom::subsystems::Arm::OnUpdate(units::second_t dt) {
       // std::endl; voltage = 0_V;
     } break;
     case ArmState::kAngle: {
-      units::newton_meter_t torque =
-          9.81_m / 1_s / 1_s * _config.armLength *
-          units::math::cos(angle + _config.angleOffset) *
-          (0.5 * _config.armMass + _config.loadMass);
-      units::volt_t feedforward =
-          _config.leftGearbox.motor.Voltage(torque, 0_rad / 1_s);
+      units::newton_meter_t torque = 9.81_m / 1_s / 1_s * _config.armLength *
+                                     units::math::cos(angle + _config.angleOffset) *
+                                     (0.5 * _config.armMass + _config.loadMass);
+      units::volt_t feedforward = _config.leftGearbox.motor.Voltage(torque, 0_rad / 1_s);
       // std::cout << "feedforward" << feedforward.value() << std::endl;
       voltage = _pid.Calculate(angle, dt, feedforward);
     } break;
diff --git a/wombat/src/main/cpp/subsystems/Elevator.cpp b/wombat/src/main/cpp/subsystems/Elevator.cpp
index d6b59f8a..d1cf9a8f 100644
--- a/wombat/src/main/cpp/subsystems/Elevator.cpp
+++ b/wombat/src/main/cpp/subsystems/Elevator.cpp
@@ -7,8 +7,7 @@
 #include <networktables/NetworkTableInstance.h>
 #include <units/acceleration.h>
 
-void wom::subsystems::ElevatorConfig::WriteNT(
-    std::shared_ptr<nt::NetworkTable> table) {
+void wom::subsystems::ElevatorConfig::WriteNT(std::shared_ptr<nt::NetworkTable> table) {
   table->GetEntry("radius").SetDouble(radius.value());
   table->GetEntry("mass").SetDouble(mass.value());
   table->GetEntry("maxHeight").SetDouble(maxHeight.value());
@@ -20,8 +19,7 @@ wom::subsystems::Elevator::Elevator(ElevatorConfig config)
       _pid{config.path + "/pid", config.pid},
       _velocityPID{config.path + "/velocityPID", config.velocityPID},
       _table(nt::NetworkTableInstance::GetDefault().GetTable(config.path)) {
-  _config.leftGearbox.encoder->SetEncoderPosition(_config.initialHeight /
-                                                  _config.radius * 1_rad);
+  _config.leftGearbox.encoder->SetEncoderPosition(_config.initialHeight / _config.radius * 1_rad);
 }
 
 void wom::subsystems::Elevator::OnUpdate(units::second_t dt) {
@@ -39,8 +37,7 @@ void wom::subsystems::Elevator::OnUpdate(units::second_t dt) {
     case ElevatorState::kVelocity: {
       units::volt_t feedforward = _config.rightGearbox.motor.Voltage(
           (_config.mass * 9.81_mps_sq) * _config.radius,
-          _velocityPID.GetSetpoint() /
-              (14.0 / 60.0 * 2.0 * 3.1415 * 0.02225 * 1_m) * 1_rad);
+          _velocityPID.GetSetpoint() / (14.0 / 60.0 * 2.0 * 3.1415 * 0.02225 * 1_m) * 1_rad);
       // units::volt_t feedforward = _config.rightGearbox.motor.Voltage(0_Nm,
       // _velocityPID.GetSetpoint() / (14.0/60.0 * 2.0 * 3.1415 * 0.02225 * 1_m)
       // * 1_rad);
@@ -53,8 +50,8 @@ void wom::subsystems::Elevator::OnUpdate(units::second_t dt) {
       // voltage = 0_V;
     } break;
     case ElevatorState::kPID: {
-      units::volt_t feedforward = _config.rightGearbox.motor.Voltage(
-          (_config.mass * 9.81_mps_sq) * _config.radius, 0_rad_per_s);
+      units::volt_t feedforward =
+          _config.rightGearbox.motor.Voltage((_config.mass * 9.81_mps_sq) * _config.radius, 0_rad_per_s);
       // std::cout << "feed forward" << feedforward.value() << std::endl;
       feedforward = 1.2_V;
       // voltage = _pid.Calculate(height, dt, feedforward);
@@ -99,8 +96,7 @@ void wom::subsystems::Elevator::SetPID(units::meter_t height) {
   _pid.SetSetpoint(height);
 }
 
-void wom::subsystems::Elevator::SetVelocity(
-    units::meters_per_second_t velocity) {
+void wom::subsystems::Elevator::SetVelocity(units::meters_per_second_t velocity) {
   _velocityPID.SetSetpoint(velocity);
 }
 
@@ -132,18 +128,14 @@ units::meter_t wom::subsystems::Elevator::GetHeight() const {
   // std::cout << "elevator position"<<
   // _config.rightGearbox.encoder->GetEncoderTicks() << std::endl; return
   // _config.rightGearbox.encoder->GetEncoderDistance() * 1_m;
-  return _config.elevatorEncoder.GetPosition() * 14 / 60 * 2 * 3.1415 *
-         0.02225 * 1_m;
+  return _config.elevatorEncoder.GetPosition() * 14 / 60 * 2 * 3.1415 * 0.02225 * 1_m;
 }
 
-units::meters_per_second_t wom::subsystems::Elevator::GetElevatorVelocity()
-    const {
-  return _config.elevatorEncoder.GetVelocity() / 60_s * 14 / 60 * 2 * 3.1415 *
-         0.02225 * 1_m;
+units::meters_per_second_t wom::subsystems::Elevator::GetElevatorVelocity() const {
+  return _config.elevatorEncoder.GetVelocity() / 60_s * 14 / 60 * 2 * 3.1415 * 0.02225 * 1_m;
 }
 
 units::meters_per_second_t wom::subsystems::Elevator::MaxSpeed() const {
-  return _config.leftGearbox.motor.Speed(
-             (_config.mass * 9.81_mps_sq) * _config.radius, 12_V) /
-         1_rad * _config.radius;
+  return _config.leftGearbox.motor.Speed((_config.mass * 9.81_mps_sq) * _config.radius, 12_V) / 1_rad *
+         _config.radius;
 }
diff --git a/wombat/src/main/cpp/subsystems/Shooter.cpp b/wombat/src/main/cpp/subsystems/Shooter.cpp
index 2fede2f3..c851266b 100644
--- a/wombat/src/main/cpp/subsystems/Shooter.cpp
+++ b/wombat/src/main/cpp/subsystems/Shooter.cpp
@@ -14,16 +14,14 @@ wom::subsystems::Shooter::Shooter(std::string path, ShooterParams params)
 
 void wom::subsystems::Shooter::OnUpdate(units::second_t dt) {
   units::volt_t voltage{0};
-  units::revolutions_per_minute_t currentSpeed =
-      _params.gearbox.encoder->GetEncoderAngularVelocity();
+  units::revolutions_per_minute_t currentSpeed = _params.gearbox.encoder->GetEncoderAngularVelocity();
 
   switch (_state) {
     case ShooterState::kManual:
       voltage = _setpointManual;
       break;
     case ShooterState::kPID: {
-      auto feedforward =
-          _params.gearbox.motor.Voltage(0_Nm, _pid.GetSetpoint());
+      auto feedforward = _params.gearbox.motor.Voltage(0_Nm, _pid.GetSetpoint());
       voltage = _pid.Calculate(currentSpeed, dt, feedforward);
     } break;
     case ShooterState::kIdle:
@@ -31,20 +29,17 @@ void wom::subsystems::Shooter::OnUpdate(units::second_t dt) {
       break;
   }
 
-  units::newton_meter_t max_torque_at_current_limit =
-      _params.gearbox.motor.Torque(_params.currentLimit);
+  units::newton_meter_t max_torque_at_current_limit = _params.gearbox.motor.Torque(_params.currentLimit);
   units::volt_t max_voltage_for_current_limit =
       _params.gearbox.motor.Voltage(max_torque_at_current_limit, currentSpeed);
 
-  voltage =
-      1_V * std::min(voltage.value(), max_voltage_for_current_limit.value());
+  voltage = 1_V * std::min(voltage.value(), max_voltage_for_current_limit.value());
 
   // _params.gearbox.motorController->SetVoltage(voltage);
 
   _table->GetEntry("output_volts").SetDouble(voltage.value());
   _table->GetEntry("speed_rpm").SetDouble(currentSpeed.value());
-  _table->GetEntry("setpoint_rpm")
-      .SetDouble(units::revolutions_per_minute_t{_pid.GetSetpoint()}.value());
+  _table->GetEntry("setpoint_rpm").SetDouble(units::revolutions_per_minute_t{_pid.GetSetpoint()}.value());
   _table->GetEntry("stable").SetBoolean(_pid.IsStable());
 }
 
@@ -67,8 +62,7 @@ bool wom::subsystems::Shooter::IsStable() const {
 
 // Shooter Manual Set
 
-wom::subsystems::ShooterConstant::ShooterConstant(Shooter* s,
-                                                  units::volt_t setpoint)
+wom::subsystems::ShooterConstant::ShooterConstant(Shooter* s, units::volt_t setpoint)
     : _shooter(s), _setpoint(setpoint) {
   Controls(_shooter);
 }
@@ -79,9 +73,7 @@ void wom::subsystems::ShooterConstant::OnTick(units::second_t dt) {
 
 // ShooterSpinup
 
-wom::subsystems::ShooterSpinup::ShooterSpinup(Shooter* s,
-                                              units::radians_per_second_t speed,
-                                              bool hold)
+wom::subsystems::ShooterSpinup::ShooterSpinup(Shooter* s, units::radians_per_second_t speed, bool hold)
     : _shooter(s), _speed(speed), _hold(hold) {
   Controls(_shooter);
 }
diff --git a/wombat/src/main/cpp/utils/Encoder.cpp b/wombat/src/main/cpp/utils/Encoder.cpp
index e5d6ab2b..461a8f31 100644
--- a/wombat/src/main/cpp/utils/Encoder.cpp
+++ b/wombat/src/main/cpp/utils/Encoder.cpp
@@ -6,8 +6,8 @@
 
 #include <rev/SparkRelativeEncoder.h>
 
-wom::utils::Encoder::Encoder(double encoderTicksPerRotation, int type,
-                             units::meter_t wheelRadius, double reduction)
+wom::utils::Encoder::Encoder(double encoderTicksPerRotation, int type, units::meter_t wheelRadius,
+                             double reduction)
     : _reduction(reduction),
       _encoderTicksPerRotation(encoderTicksPerRotation),
       _type(type),
@@ -43,16 +43,16 @@ void wom::utils::Encoder::SetReduction(double reduction) {
 }
 
 units::radian_t wom::utils::Encoder::GetEncoderPosition() {
-  //if (_type == 0) {
-  //  units::turn_t n_turns{GetEncoderTicks() / GetEncoderTicksPerRotation()};
-  //  return n_turns;
-  //} else if (_type == 2) {
-  //  units::degree_t pos = GetEncoderTicks() * 1_deg;
-  //  return pos;
-  //} else {
-  //  units::degree_t pos = GetEncoderTicks() * 1_deg;
-  //  return pos - _offset;
-  //}
+  // if (_type == 0) {
+  //   units::turn_t n_turns{GetEncoderTicks() / GetEncoderTicksPerRotation()};
+  //   return n_turns;
+  // } else if (_type == 2) {
+  //   units::degree_t pos = GetEncoderTicks() * 1_deg;
+  //   return pos;
+  // } else {
+  //   units::degree_t pos = GetEncoderTicks() * 1_deg;
+  //   return pos - _offset;
+  // }
   return GetEncoderTicks() * 1_rad;
 }
 
@@ -63,8 +63,7 @@ double wom::utils::Encoder::GetEncoderDistance() {
 units::radians_per_second_t wom::utils::Encoder::GetEncoderAngularVelocity() {
   // return GetEncoderTickVelocity() / (double)GetEncoderTicksPerRotation() * 2
   // * 3.1415926;
-  units::turns_per_second_t n_turns_per_s{GetEncoderTickVelocity() /
-                                          GetEncoderTicksPerRotation()};
+  units::turns_per_second_t n_turns_per_s{GetEncoderTickVelocity() / GetEncoderTicksPerRotation()};
   return n_turns_per_s;
 }
 
@@ -86,12 +85,10 @@ double wom::utils::DigitalEncoder::GetEncoderTickVelocity() const {
   return _nativeEncoder.GetRate();
 }
 
-wom::utils::CANSparkMaxEncoder::CANSparkMaxEncoder(rev::CANSparkMax* controller,
-                                                   units::meter_t wheelRadius,
+wom::utils::CANSparkMaxEncoder::CANSparkMaxEncoder(rev::CANSparkMax* controller, units::meter_t wheelRadius,
                                                    double reduction)
     : wom::utils::Encoder(42, reduction, wheelRadius, 2),
-      _encoder(controller->GetEncoder(
-          rev::SparkRelativeEncoder::Type::kQuadrature)) {}
+      _encoder(controller->GetEncoder(rev::SparkRelativeEncoder::Type::kQuadrature)) {}
 
 double wom::utils::CANSparkMaxEncoder::GetEncoderRawTicks() const {
   return _encoder.GetPosition() * _reduction;
@@ -121,11 +118,9 @@ double wom::utils::CANSparkMaxEncoder::GetVelocity() const {
   return _encoder.GetVelocity();
 }
 
-wom::utils::TalonFXEncoder::TalonFXEncoder(
-    ctre::phoenix6::hardware::TalonFX* controller, units::meter_t wheelRadius,
-    double reduction)
-    : utils::Encoder(2048, reduction, wheelRadius, 0),
-      _controller(controller) {}
+wom::utils::TalonFXEncoder::TalonFXEncoder(ctre::phoenix6::hardware::TalonFX* controller,
+                                           units::meter_t wheelRadius, double reduction)
+    : utils::Encoder(2048, reduction, wheelRadius, 0), _controller(controller) {}
 
 double wom::utils::TalonFXEncoder::GetEncoderRawTicks() const {
   return _controller->GetPosition().GetValue().value();
@@ -135,12 +130,9 @@ double wom::utils::TalonFXEncoder::GetEncoderTickVelocity() const {
   return _controller->Get() * 10;
 }
 
-wom::utils::DutyCycleEncoder::DutyCycleEncoder(int channel,
-                                               units::meter_t wheelRadius,
-                                               double ticksPerRotation,
-                                               double reduction)
-    : wom::utils::Encoder(ticksPerRotation, reduction, wheelRadius, 0),
-      _dutyCycleEncoder(channel) {}
+wom::utils::DutyCycleEncoder::DutyCycleEncoder(int channel, units::meter_t wheelRadius,
+                                               double ticksPerRotation, double reduction)
+    : wom::utils::Encoder(ticksPerRotation, reduction, wheelRadius, 0), _dutyCycleEncoder(channel) {}
 
 double wom::utils::DutyCycleEncoder::GetEncoderRawTicks() const {
   return _dutyCycleEncoder.Get().value();
@@ -150,9 +142,8 @@ double wom::utils::DutyCycleEncoder::GetEncoderTickVelocity() const {
   return 0;
 }
 
-wom::utils::CanEncoder::CanEncoder(int deviceNumber, units::meter_t wheelRadius,
-                                   double ticksPerRotation, double reduction,
-                                   std::string name)
+wom::utils::CanEncoder::CanEncoder(int deviceNumber, units::meter_t wheelRadius, double ticksPerRotation,
+                                   double reduction, std::string name)
     : wom::utils::Encoder(ticksPerRotation, 2, wheelRadius, reduction) {
   _canEncoder = new ctre::phoenix6::hardware::CANcoder(deviceNumber, name);
 }
diff --git a/wombat/src/main/cpp/utils/Util.cpp b/wombat/src/main/cpp/utils/Util.cpp
index e529e245..dbdbb642 100644
--- a/wombat/src/main/cpp/utils/Util.cpp
+++ b/wombat/src/main/cpp/utils/Util.cpp
@@ -13,21 +13,18 @@ units::second_t wom::utils::now() {
   return static_cast<double>(now) / 1000000 * 1_s;
 }
 
-void wom::utils::WritePose2NT(std::shared_ptr<nt::NetworkTable> table,
-                              frc::Pose2d pose) {
+void wom::utils::WritePose2NT(std::shared_ptr<nt::NetworkTable> table, frc::Pose2d pose) {
   table->GetEntry("x").SetDouble(pose.X().value());
   table->GetEntry("y").SetDouble(pose.Y().value());
   table->GetEntry("angle").SetDouble(pose.Rotation().Degrees().value());
 }
 
-void wom::utils::WritePose3NT(std::shared_ptr<nt::NetworkTable> table,
-                              frc::Pose3d pose) {
+void wom::utils::WritePose3NT(std::shared_ptr<nt::NetworkTable> table, frc::Pose3d pose) {
   table->GetEntry("x").SetDouble(pose.X().value());
   table->GetEntry("y").SetDouble(pose.Y().value());
   table->GetEntry("z").SetDouble(pose.Z().value());
 
-  table->GetEntry("angle").SetDouble(
-      pose.Rotation().Z().convert<units::degree>().value());
+  table->GetEntry("angle").SetDouble(pose.Rotation().Z().convert<units::degree>().value());
 }
 
 double wom::utils::deadzone(double val, double deadzone) {
@@ -50,32 +47,24 @@ units::volt_t wom::utils::GetVoltage(frc::MotorController* controller) {
   return controller->Get() * frc::RobotController::GetBatteryVoltage();
 }
 
-void wom::utils::WriteTrajectory(std::shared_ptr<nt::NetworkTable> table,
-                                 frc::Trajectory trajectory) {
+void wom::utils::WriteTrajectory(std::shared_ptr<nt::NetworkTable> table, frc::Trajectory trajectory) {
   table->GetEntry("length").SetDouble(trajectory.TotalTime().value());
 
   // write the trajectory to the network table
   int i = 0;
   for (auto state : trajectory.States()) {
-    table->GetSubTable(std::to_string(i))
-        ->GetEntry("x")
-        .SetDouble(state.pose.X().value());
-    table->GetSubTable(std::to_string(i))
-        ->GetEntry("y")
-        .SetDouble(state.pose.Y().value());
+    table->GetSubTable(std::to_string(i))->GetEntry("x").SetDouble(state.pose.X().value());
+    table->GetSubTable(std::to_string(i))->GetEntry("y").SetDouble(state.pose.Y().value());
     table->GetSubTable(std::to_string(i))
         ->GetEntry("angle")
         .SetDouble(state.pose.Rotation().Degrees().value());
-    table->GetSubTable(std::to_string(i))
-        ->GetEntry("time")
-        .SetDouble(state.t.value());
+    table->GetSubTable(std::to_string(i))->GetEntry("time").SetDouble(state.t.value());
 
     i++;
   }
 }
 
-void wom::utils::WriteTrajectoryState(std::shared_ptr<nt::NetworkTable> table,
-                                      frc::Trajectory::State state) {
+void wom::utils::WriteTrajectoryState(std::shared_ptr<nt::NetworkTable> table, frc::Trajectory::State state) {
   table->GetEntry("x").SetDouble(state.pose.X().value());
   table->GetEntry("y").SetDouble(state.pose.Y().value());
   table->GetEntry("angle").SetDouble(state.pose.Rotation().Degrees().value());
diff --git a/wombat/src/main/cpp/vision/Limelight.cpp b/wombat/src/main/cpp/vision/Limelight.cpp
index b37148ce..906f92cb 100644
--- a/wombat/src/main/cpp/vision/Limelight.cpp
+++ b/wombat/src/main/cpp/vision/Limelight.cpp
@@ -8,8 +8,7 @@
 
 #include "utils/Util.h"
 
-wom::vision::Limelight::Limelight(std::string limelightName)
-    : _limelightName(limelightName) {}
+wom::vision::Limelight::Limelight(std::string limelightName) : _limelightName(limelightName) {}
 
 std::string wom::vision::Limelight::GetName() {
   return _limelightName;
@@ -24,8 +23,7 @@ std::pair<double, double> wom::vision::Limelight::GetOffset() {
   return offset;
 }
 
-std::vector<double> wom::vision::Limelight::GetAprilTagData(
-    LimelightAprilTagData dataType) {
+std::vector<double> wom::vision::Limelight::GetAprilTagData(LimelightAprilTagData dataType) {
   std::string dataName;
 
   switch (dataType) {
@@ -69,8 +67,7 @@ std::vector<double> wom::vision::Limelight::GetAprilTagData(
   return table->GetNumberArray(dataName, std::vector<double>(6));
 }
 
-double wom::vision::Limelight::GetTargetingData(LimelightTargetingData dataType,
-                                                double defaultValue) {
+double wom::vision::Limelight::GetTargetingData(LimelightTargetingData dataType, double defaultValue) {
   std::string dataName;
 
   switch (dataType) {
@@ -158,8 +155,8 @@ void wom::vision::Limelight::SetCrop(std::array<double, 4> crop) {
   table->PutNumberArray("camtran", crop);
 }
 
-units::meters_per_second_t wom::vision::Limelight::GetSpeed(
-    frc::Pose3d pose1, frc::Pose3d pose2, units::second_t dt) {
+units::meters_per_second_t wom::vision::Limelight::GetSpeed(frc::Pose3d pose1, frc::Pose3d pose2,
+                                                            units::second_t dt) {
   frc::Transform3d dPose{pose1, pose2};
   frc::Translation3d dTranslation = dPose.Translation();
 
@@ -172,9 +169,8 @@ units::meters_per_second_t wom::vision::Limelight::GetSpeed(
 
 frc::Pose3d wom::vision::Limelight::GetPose() {
   std::vector<double> pose = GetAprilTagData(LimelightAprilTagData::kBotpose);
-  return frc::Pose3d(
-      pose[1] * 1_m, 1_m * pose[2], 1_m * pose[3],
-      frc::Rotation3d(1_deg * (pose[4]), 1_deg * (pose[5]), 1_deg * (pose[6])));
+  return frc::Pose3d(pose[1] * 1_m, 1_m * pose[2], 1_m * pose[3],
+                     frc::Rotation3d(1_deg * (pose[4]), 1_deg * (pose[5]), 1_deg * (pose[6])));
 }
 
 void wom::vision::Limelight::OnStart() {
@@ -185,11 +181,9 @@ void wom::vision::Limelight::OnUpdate(units::time::second_t dt) {
   wom::utils::WritePose3NT(table, GetPose());
 }
 
-bool wom::vision::Limelight::IsAtSetPoseVision(frc::Pose3d pose,
-                                               units::second_t dt) {
+bool wom::vision::Limelight::IsAtSetPoseVision(frc::Pose3d pose, units::second_t dt) {
   frc::Pose3d actualPose = GetPose();
   frc::Pose3d relativePose = actualPose.RelativeTo(pose);
-  return (units::math::fabs(relativePose.X()) < 0.01_m &&
-          units::math::fabs(relativePose.Y()) < 0.01_m &&
+  return (units::math::fabs(relativePose.X()) < 0.01_m && units::math::fabs(relativePose.Y()) < 0.01_m &&
           GetSpeed(pose, GetPose(), dt) < 1_m / 1_s);
 }
diff --git a/wombat/src/main/include/behaviour/Behaviour.h b/wombat/src/main/include/behaviour/Behaviour.h
index 4f2e357e..84ac8689 100644
--- a/wombat/src/main/include/behaviour/Behaviour.h
+++ b/wombat/src/main/include/behaviour/Behaviour.h
@@ -22,13 +22,7 @@
 #include "HasBehaviour.h"
 
 namespace behaviour {
-enum class BehaviourState {
-  INITIALISED,
-  RUNNING,
-  DONE,
-  TIMED_OUT,
-  INTERRUPTED
-};
+enum class BehaviourState { INITIALISED, RUNNING, DONE, TIMED_OUT, INTERRUPTED };
 
 class SequentialBehaviour;
 
@@ -46,8 +40,7 @@ class Behaviour : public std::enable_shared_from_this<Behaviour> {
  public:
   using ptr = std::shared_ptr<Behaviour>;
 
-  Behaviour(std::string name = "<unnamed behaviour>",
-            units::time::second_t period = 20_ms);
+  explicit Behaviour(std::string name = "<unnamed behaviour>", units::time::second_t period = 20_ms);
   ~Behaviour();
 
   /**
@@ -193,16 +186,15 @@ class SequentialBehaviour : public Behaviour {
   std::deque<ptr> _queue;
 };
 
-inline std::shared_ptr<SequentialBehaviour> operator<<(Behaviour::ptr a,
-                                                       Behaviour::ptr b) {
+inline std::shared_ptr<SequentialBehaviour> operator<<(Behaviour::ptr a, Behaviour::ptr b) {
   auto seq = std::make_shared<SequentialBehaviour>();
   seq->Add(a);
   seq->Add(b);
   return seq;
 }
 
-inline std::shared_ptr<SequentialBehaviour> operator<<(
-    std::shared_ptr<SequentialBehaviour> a, Behaviour::ptr b) {
+inline std::shared_ptr<SequentialBehaviour> operator<<(std::shared_ptr<SequentialBehaviour> a,
+                                                       Behaviour::ptr b) {
   a->Add(b);
   return a;
 }
@@ -247,10 +239,8 @@ class ConcurrentBehaviour : public Behaviour {
  * Create a concurrent behaviour group, waiting for all behaviours
  * to finish before moving on.
  */
-inline std::shared_ptr<ConcurrentBehaviour> operator&(Behaviour::ptr a,
-                                                      Behaviour::ptr b) {
-  auto conc =
-      std::make_shared<ConcurrentBehaviour>(ConcurrentBehaviourReducer::ALL);
+inline std::shared_ptr<ConcurrentBehaviour> operator&(Behaviour::ptr a, Behaviour::ptr b) {
+  auto conc = std::make_shared<ConcurrentBehaviour>(ConcurrentBehaviourReducer::ALL);
   conc->Add(a);
   conc->Add(b);
   return conc;
@@ -261,10 +251,8 @@ inline std::shared_ptr<ConcurrentBehaviour> operator&(Behaviour::ptr a,
  * be interrupted as soon as any members of the group are finished (the
  * behaviours are 'raced' against each other).
  */
-inline std::shared_ptr<ConcurrentBehaviour> operator|(Behaviour::ptr a,
-                                                      Behaviour::ptr b) {
-  auto conc =
-      std::make_shared<ConcurrentBehaviour>(ConcurrentBehaviourReducer::ANY);
+inline std::shared_ptr<ConcurrentBehaviour> operator|(Behaviour::ptr a, Behaviour::ptr b) {
+  auto conc = std::make_shared<ConcurrentBehaviour>(ConcurrentBehaviourReducer::ANY);
   conc->Add(a);
   conc->Add(b);
   return conc;
@@ -332,8 +320,7 @@ struct Switch : public Behaviour {
    * @param condition The function yielding true if this is the correct option
    * @param b The behaviour to call if this option is provided.
    */
-  std::shared_ptr<Switch> When(std::function<bool(T&)> condition,
-                               Behaviour::ptr b) {
+  std::shared_ptr<Switch> When(std::function<bool(T&)> condition, Behaviour::ptr b) {
     _options.push_back(std::pair(condition, b));
     Inherit(*b);
     return std::reinterpret_pointer_cast<Switch<T>>(shared_from_this());
@@ -389,8 +376,7 @@ struct Switch : public Behaviour {
 
  private:
   std::function<T()> _fn;
-  wpi::SmallVector<std::pair<std::function<bool(T&)>, Behaviour::ptr>, 4>
-      _options;
+  wpi::SmallVector<std::pair<std::function<bool(T&)>, Behaviour::ptr>, 4> _options;
   Behaviour::ptr _locked = nullptr;
 };
 
@@ -407,10 +393,8 @@ struct Decide : public Switch<std::monostate> {
    * @param condition The function yielding true if this is the correct option
    * @param b The behaviour to call if this option is provided.
    */
-  std::shared_ptr<Decide> When(std::function<bool()> condition,
-                               Behaviour::ptr b) {
-    return std::reinterpret_pointer_cast<Decide>(
-        Switch::When([condition](auto) { return condition(); }, b));
+  std::shared_ptr<Decide> When(std::function<bool()> condition, Behaviour::ptr b) {
+    return std::reinterpret_pointer_cast<Decide>(Switch::When([condition](auto) { return condition(); }, b));
   }
 };
 
diff --git a/wombat/src/main/include/behaviour/HasBehaviour.h b/wombat/src/main/include/behaviour/HasBehaviour.h
index c9b35b2e..bae5ac05 100644
--- a/wombat/src/main/include/behaviour/HasBehaviour.h
+++ b/wombat/src/main/include/behaviour/HasBehaviour.h
@@ -30,8 +30,7 @@ class HasBehaviour {
 
  protected:
   std::shared_ptr<Behaviour> _active_behaviour{nullptr};
-  std::function<std::shared_ptr<Behaviour>(void)> _default_behaviour_producer{
-      nullptr};
+  std::function<std::shared_ptr<Behaviour>(void)> _default_behaviour_producer{nullptr};
 
  private:
   friend class BehaviourScheduler;
diff --git a/wombat/src/main/include/drivetrain/SwerveDrive.h b/wombat/src/main/include/drivetrain/SwerveDrive.h
index b48db5a5..be4467b1 100644
--- a/wombat/src/main/include/drivetrain/SwerveDrive.h
+++ b/wombat/src/main/include/drivetrain/SwerveDrive.h
@@ -4,9 +4,9 @@
 
 #pragma once
 
+#include <frc/controller/PIDController.h>
 #include <frc/estimator/SwerveDrivePoseEstimator.h>
 #include <frc/kinematics/SwerveDriveKinematics.h>
-#include <frc/controller/PIDController.h>
 #include <networktables/DoubleTopic.h>
 #include <networktables/NetworkTable.h>
 #include <networktables/NetworkTableInstance.h>
@@ -48,9 +48,8 @@ struct SwerveModuleConfig {
 
 class SwerveModule {
  public:
-  //using angle_pid_conf_t = utils::PIDConfig<units::radian, units::volt>;
-  using velocity_pid_conf_t =
-      utils::PIDConfig<units::meters_per_second, units::volt>;
+  // using angle_pid_conf_t = utils::PIDConfig<units::radian, units::volt>;
+  using velocity_pid_conf_t = utils::PIDConfig<units::meters_per_second, units::volt>;
 
   SwerveModule(std::string path, SwerveModuleConfig config,
                /*angle_pid_conf_t anglePID,*/ velocity_pid_conf_t velocityPID);
@@ -67,8 +66,7 @@ class SwerveModule {
 
   void SetZero(units::second_t dt);
   void SetIdle();
-  void SetPID(units::radian_t angle, units::meters_per_second_t speed,
-              units::second_t dt);
+  void SetPID(units::radian_t angle, units::meters_per_second_t speed, units::second_t dt);
   void SetZero();
   void SetVoltageLimit(units::volt_t driveModuleVoltageLimit);
 
@@ -84,7 +82,7 @@ class SwerveModule {
 
   const SwerveModuleConfig& GetConfig() const;
 
-  //utils::PIDController<units::radians, units::volt> _anglePIDController;
+  // utils::PIDController<units::radians, units::volt> _anglePIDController;
   frc::PIDController _anglePIDController;
 
  private:
@@ -108,20 +106,18 @@ class SwerveModule {
 struct SwerveDriveConfig {
   /*using pose_angle_conf_t =
       utils::PIDConfig<units::radian, units::radians_per_second>;*/
-  using pose_position_conf_t =
-      utils::PIDConfig<units::meter, units::meters_per_second>;
-  using balance_conf_t =
-      utils::PIDConfig<units::degree, units::meters_per_second>;
+  using pose_position_conf_t = utils::PIDConfig<units::meter, units::meters_per_second>;
+  using balance_conf_t = utils::PIDConfig<units::degree, units::meters_per_second>;
 
   std::string path;
-  //SwerveModule::angle_pid_conf_t anglePID;
+  // SwerveModule::angle_pid_conf_t anglePID;
   SwerveModule::velocity_pid_conf_t velocityPID;
 
   wpi::array<SwerveModuleConfig, 4> modules;
 
   ctre::phoenix6::hardware::Pigeon2* gyro;
 
-  //pose_angle_conf_t poseAnglePID;
+  // pose_angle_conf_t poseAnglePID;
   pose_position_conf_t posePositionPID;
 
   units::kilogram_t mass;
@@ -175,8 +171,7 @@ class SwerveDrive : public behaviour::HasBehaviour {
    * @brief This function switches the state to handle the robot's rotation
    * matching that of the joystick
    */
-  void RotateMatchJoystick(units::radian_t joystickAngle,
-                           FieldRelativeSpeeds speeds);
+  void RotateMatchJoystick(units::radian_t joystickAngle, FieldRelativeSpeeds speeds);
 
   void SetIdle();
 
@@ -186,8 +181,7 @@ class SwerveDrive : public behaviour::HasBehaviour {
   void SetFieldRelativeVelocity(FieldRelativeSpeeds speeds);
   void SetPose(frc::Pose2d pose);
   bool IsAtSetPose();
-  void SetIndividualTuning(int mod, units::radian_t angle,
-                           units::meters_per_second_t speed);
+  void SetIndividualTuning(int mod, units::radian_t angle, units::meters_per_second_t speed);
   void SetTuning(units::radian_t angle, units::meters_per_second_t speed);
   void SetZero();
   void SetVoltageLimit(units::volt_t driveVoltageLimit);
diff --git a/wombat/src/main/include/drivetrain/behaviours/SwerveBehaviours.h b/wombat/src/main/include/drivetrain/behaviours/SwerveBehaviours.h
index 5aa1972f..11400c4a 100644
--- a/wombat/src/main/include/drivetrain/behaviours/SwerveBehaviours.h
+++ b/wombat/src/main/include/drivetrain/behaviours/SwerveBehaviours.h
@@ -37,16 +37,14 @@ class ManualDrivebase : public behaviour::Behaviour {
    * A pointer to the controller that the driver has been allocated (the
    * allocated memory address that stores the "driver controller" object)
    */
-  ManualDrivebase(wom::drivetrain::SwerveDrive* swerveDrivebase,
-                  frc::XboxController* driverController);
+  ManualDrivebase(wom::drivetrain::SwerveDrive* swerveDrivebase, frc::XboxController* driverController);
 
   void OnTick(units::second_t deltaTime) override;
   /**
    * @brief This function handles all of the logic behind the tangent function,
    * to be able to calculate an angle between 0 andd 360 degrees, inclusively
    */
-  void CalculateRequestedAngle(double joystickX, double joystickY,
-                               units::degree_t defaultAngle);
+  void CalculateRequestedAngle(double joystickX, double joystickY, units::degree_t defaultAngle);
   void OnStart() override;
   void ResetMode();
 
@@ -72,8 +70,8 @@ class ManualDrivebase : public behaviour::Behaviour {
   const double turningDeadzone = 0.2;
 
   // Variables for solution to Anti-tip
-  double prevJoystickX, prevJoystickY, prevPrevJoystickX, prevPrevJoystickY,
-      usingJoystickXPos, usingJoystickYPos;
+  double prevJoystickX, prevJoystickY, prevPrevJoystickX, prevPrevJoystickY, usingJoystickXPos,
+      usingJoystickYPos;
   // The speed that the joystick must travel to activate averaging over previous
   // 3 joystick positions
   const double smoothingThreshold = 1;
@@ -126,8 +124,8 @@ class GoToPose : public behaviour::Behaviour {
 
 class FollowTrajectory : public behaviour::Behaviour {
  public:
-  FollowTrajectory(wom::drivetrain::SwerveDrive* swerve,
-                   wom::utils::Pathplanner* pathplanner, std::string path);
+  FollowTrajectory(wom::drivetrain::SwerveDrive* swerve, wom::utils::Pathplanner* pathplanner,
+                   std::string path);
 
   void OnTick(units::second_t dt) override;
 
@@ -184,8 +182,7 @@ class TempSimSwerveDrive {
 
 class AutoSwerveDrive {
  public:
-  AutoSwerveDrive(wom::drivetrain::SwerveDrive* swerve, frc::Timer* timer,
-                  frc::Field2d* field);
+  AutoSwerveDrive(wom::drivetrain::SwerveDrive* swerve, frc::Timer* timer, frc::Field2d* field);
 
   void OnUpdate();
 
diff --git a/wombat/src/main/include/subsystems/Arm.h b/wombat/src/main/include/subsystems/Arm.h
index 155fd4cd..8b91be72 100644
--- a/wombat/src/main/include/subsystems/Arm.h
+++ b/wombat/src/main/include/subsystems/Arm.h
@@ -68,8 +68,7 @@ class Arm : public behaviour::HasBehaviour {
   ArmConfig _config;
   ArmState _state = ArmState::kIdle;
   wom::utils::PIDController<units::radian, units::volt> _pid;
-  wom::utils::PIDController<units::radians_per_second, units::volt>
-      _velocityPID;
+  wom::utils::PIDController<units::radians_per_second, units::volt> _velocityPID;
 
   std::shared_ptr<nt::NetworkTable> _table;
 
diff --git a/wombat/src/main/include/subsystems/Shooter.h b/wombat/src/main/include/subsystems/Shooter.h
index 2bbf6f5e..e5ac219d 100644
--- a/wombat/src/main/include/subsystems/Shooter.h
+++ b/wombat/src/main/include/subsystems/Shooter.h
@@ -62,8 +62,7 @@ class ShooterConstant : public behaviour::Behaviour {
 
 class ShooterSpinup : public behaviour::Behaviour {
  public:
-  ShooterSpinup(Shooter* s, units::radians_per_second_t speed,
-                bool hold = false);
+  ShooterSpinup(Shooter* s, units::radians_per_second_t speed, bool hold = false);
 
   void OnTick(units::second_t dt) override;
 
diff --git a/wombat/src/main/include/utils/Encoder.h b/wombat/src/main/include/utils/Encoder.h
index b504e1c0..303b2fda 100644
--- a/wombat/src/main/include/utils/Encoder.h
+++ b/wombat/src/main/include/utils/Encoder.h
@@ -20,8 +20,7 @@ namespace wom {
 namespace utils {
 class Encoder {
  public:
-  Encoder(double encoderTicksPerRotation, int type, units::meter_t wheelRadius,
-          double reduction = 1.0);
+  Encoder(double encoderTicksPerRotation, int type, units::meter_t wheelRadius, double reduction = 1.0);
 
   virtual double GetEncoderRawTicks() const = 0;
   virtual double GetEncoderTickVelocity() const = 0;  // ticks/s
@@ -54,24 +53,23 @@ class Encoder {
 
 class DigitalEncoder : public Encoder {
  public:
-  DigitalEncoder(int channelA, int channelB, double ticksPerRotation,
-                 units::meter_t wheelRadius, double reduction = 1)
-      : Encoder(ticksPerRotation, reduction, wheelRadius, 0),
-        _nativeEncoder(channelA, channelB) {}
+  DigitalEncoder(int channelA, int channelB, double ticksPerRotation, units::meter_t wheelRadius,
+                 double reduction = 1)
+      : Encoder(ticksPerRotation, reduction, wheelRadius, 0), _nativeEncoder(channelA, channelB) {}
 
   double GetEncoderRawTicks() const override;
   double GetEncoderTickVelocity() const override;
 
   double GetPosition() const;
   double GetVelocity() const override;
+
  private:
   frc::Encoder _nativeEncoder;
 };
 
 class CANSparkMaxEncoder : public Encoder {
  public:
-  explicit CANSparkMaxEncoder(rev::CANSparkMax* controller,
-                              units::meter_t wheelRadius, double reduction = 1);
+  explicit CANSparkMaxEncoder(rev::CANSparkMax* controller, units::meter_t wheelRadius, double reduction = 1);
 
   double GetEncoderRawTicks() const override;
   double GetEncoderTickVelocity() const override;
@@ -86,8 +84,8 @@ class CANSparkMaxEncoder : public Encoder {
 
 class TalonFXEncoder : public Encoder {
  public:
-  TalonFXEncoder(ctre::phoenix6::hardware::TalonFX* controller,
-                 units::meter_t wheelRadius, double reduction = 1);
+  TalonFXEncoder(ctre::phoenix6::hardware::TalonFX* controller, units::meter_t wheelRadius,
+                 double reduction = 1);
 
   double GetEncoderRawTicks() const override;
   double GetEncoderTickVelocity() const override;
@@ -99,8 +97,8 @@ class TalonFXEncoder : public Encoder {
 
 class DutyCycleEncoder : public Encoder {
  public:
-  DutyCycleEncoder(int channel, units::meter_t wheelRadius,
-                   double ticksPerRotation = 1, double reduction = 1);
+  DutyCycleEncoder(int channel, units::meter_t wheelRadius, double ticksPerRotation = 1,
+                   double reduction = 1);
 
   double GetEncoderRawTicks() const override;
   double GetEncoderTickVelocity() const override;
@@ -112,9 +110,8 @@ class DutyCycleEncoder : public Encoder {
 
 class CanEncoder : public Encoder {
  public:
-  CanEncoder(int deviceNumber, units::meter_t wheelRadius,
-             double ticksPerRotation = 4095, double reduction = 6.75,
-             std::string name = "Drivebase");
+  CanEncoder(int deviceNumber, units::meter_t wheelRadius, double ticksPerRotation = 4095,
+             double reduction = 6.75, std::string name = "Drivebase");
 
   double GetEncoderRawTicks() const override;
   double GetEncoderTickVelocity() const override;
diff --git a/wombat/src/main/include/utils/PID.h b/wombat/src/main/include/utils/PID.h
index 2ce378ec..cec94b1d 100644
--- a/wombat/src/main/include/utils/PID.h
+++ b/wombat/src/main/include/utils/PID.h
@@ -24,19 +24,15 @@ struct PIDConfig {
   using in_t = units::unit_t<IN>;
 
   using kp_t = units::unit_t<units::compound_unit<OUT, units::inverse<IN>>>;
-  using ki_t =
-      units::unit_t<units::compound_unit<OUT, units::inverse<IN>,
-                                         units::inverse<units::second>>>;
-  using kd_t = units::unit_t<
-      units::compound_unit<OUT, units::inverse<IN>, units::second>>;
+  using ki_t = units::unit_t<units::compound_unit<OUT, units::inverse<IN>, units::inverse<units::second>>>;
+  using kd_t = units::unit_t<units::compound_unit<OUT, units::inverse<IN>, units::second>>;
 
   using error_t = units::unit_t<IN>;
-  using deriv_t =
-      units::unit_t<units::compound_unit<IN, units::inverse<units::second>>>;
+  using deriv_t = units::unit_t<units::compound_unit<IN, units::inverse<units::second>>>;
 
-  PIDConfig(std::string path, kp_t kp = kp_t{0}, ki_t ki = ki_t{0},
-            kd_t kd = kd_t{0}, error_t stableThresh = error_t{-1},
-            deriv_t stableDerivThresh = deriv_t{-1}, in_t izone = in_t{-1})
+  PIDConfig(std::string path, kp_t kp = kp_t{0}, ki_t ki = ki_t{0}, kd_t kd = kd_t{0},
+            error_t stableThresh = error_t{-1}, deriv_t stableDerivThresh = deriv_t{-1},
+            in_t izone = in_t{-1})
       : path(path),
         kp(kp),
         ki(ki),
@@ -64,23 +60,14 @@ struct PIDConfig {
  public:
   void RegisterNT() {
     auto table = nt::NetworkTableInstance::GetDefault().GetTable(path);
+    _nt_bindings.emplace_back(std::make_shared<NTBoundUnit<typename kp_t::unit_type>>(table, "kP", kp));
+    _nt_bindings.emplace_back(std::make_shared<NTBoundUnit<typename ki_t::unit_type>>(table, "kI", ki));
+    _nt_bindings.emplace_back(std::make_shared<NTBoundUnit<typename kd_t::unit_type>>(table, "kD", kd));
     _nt_bindings.emplace_back(
-        std::make_shared<NTBoundUnit<typename kp_t::unit_type>>(table, "kP",
-                                                                kp));
-    _nt_bindings.emplace_back(
-        std::make_shared<NTBoundUnit<typename ki_t::unit_type>>(table, "kI",
-                                                                ki));
-    _nt_bindings.emplace_back(
-        std::make_shared<NTBoundUnit<typename kd_t::unit_type>>(table, "kD",
-                                                                kd));
-    _nt_bindings.emplace_back(
-        std::make_shared<NTBoundUnit<typename error_t::unit_type>>(
-            table, "stableThresh", stableThresh));
-    _nt_bindings.emplace_back(
-        std::make_shared<NTBoundUnit<typename deriv_t::unit_type>>(
-            table, "stableThreshVelocity", stableDerivThresh));
-    _nt_bindings.emplace_back(
-        std::make_shared<NTBoundUnit<IN>>(table, "izone", izone));
+        std::make_shared<NTBoundUnit<typename error_t::unit_type>>(table, "stableThresh", stableThresh));
+    _nt_bindings.emplace_back(std::make_shared<NTBoundUnit<typename deriv_t::unit_type>>(
+        table, "stableThreshVelocity", stableDerivThresh));
+    _nt_bindings.emplace_back(std::make_shared<NTBoundUnit<IN>>(table, "izone", izone));
   }
 };
 
@@ -94,19 +81,15 @@ class PIDController {
 
   config_t config;
 
-  PIDController(std::string path, config_t initialGains,
-                in_t setpoint = in_t{0})
+  PIDController(std::string path, config_t initialGains, in_t setpoint = in_t{0})
       : config(initialGains),
         _setpoint(setpoint),
-        _posFilter(
-            frc::LinearFilter<typename config_t::error_t>::MovingAverage(20)),
-        _velFilter(
-            frc::LinearFilter<typename config_t::deriv_t>::MovingAverage(20)),
+        _posFilter(frc::LinearFilter<typename config_t::error_t>::MovingAverage(20)),
+        _velFilter(frc::LinearFilter<typename config_t::deriv_t>::MovingAverage(20)),
         _table(nt::NetworkTableInstance::GetDefault().GetTable(path)) {}
 
   void SetSetpoint(in_t setpoint) {
-    if (std::abs(setpoint.value() - _setpoint.value()) >
-        std::abs(0.1 * _setpoint.value())) {
+    if (std::abs(setpoint.value() - _setpoint.value()) > std::abs(0.1 * _setpoint.value())) {
       _iterations = 0;
     }
     _setpoint = setpoint;
@@ -126,8 +109,7 @@ class PIDController {
     error = units::math::fabs(error);
     _integralSum += error * dt;
     _integralSum = units::math::fabs(_integralSum);
-    if (config.izone.value() > 0 &&
-        (error > config.izone || error < -config.izone))
+    if (config.izone.value() > 0 && (error > config.izone || error < -config.izone))
       _integralSum = sum_t{0};
 
     typename config_t::deriv_t deriv{0};
@@ -138,8 +120,7 @@ class PIDController {
     _stablePos = _posFilter.Calculate(error);
     _stableVel = _velFilter.Calculate(deriv);
 
-    auto out = config.kp * error + config.ki * _integralSum +
-               config.kd * deriv + feedforward;
+    auto out = config.kp * error + config.ki * _integralSum + config.kd * deriv + feedforward;
 
     _last_pv = pv;
     _last_error = error;
@@ -147,10 +128,8 @@ class PIDController {
     return out;
   }
 
-  bool IsStable(
-      std::optional<typename config_t::error_t> stableThreshOverride = {},
-      std::optional<typename config_t::deriv_t> velocityThreshOverride = {})
-      const {
+  bool IsStable(std::optional<typename config_t::error_t> stableThreshOverride = {},
+                std::optional<typename config_t::deriv_t> velocityThreshOverride = {}) const {
     auto stableThresh = config.stableThresh;
     auto stableDerivThresh = config.stableDerivThresh;
 
@@ -159,10 +138,8 @@ class PIDController {
     if (velocityThreshOverride.has_value())
       stableDerivThresh = velocityThreshOverride.value();
 
-    return _iterations > 20 &&
-           std::abs(_stablePos.value()) <= std::abs(stableThresh.value()) &&
-           (stableDerivThresh.value() < 0 ||
-            std::abs(_stableVel.value()) <= stableDerivThresh.value());
+    return _iterations > 20 && std::abs(_stablePos.value()) <= std::abs(stableThresh.value()) &&
+           (stableDerivThresh.value() < 0 || std::abs(_stableVel.value()) <= stableDerivThresh.value());
   }
 
  private:
diff --git a/wombat/src/main/include/utils/Util.h b/wombat/src/main/include/utils/Util.h
index 665c85ec..d624e408 100644
--- a/wombat/src/main/include/utils/Util.h
+++ b/wombat/src/main/include/utils/Util.h
@@ -28,39 +28,30 @@ units::second_t now();
 
 class NTBound {
  public:
-  NTBound(std::shared_ptr<nt::NetworkTable> table, std::string name,
-          const nt::Value& value,
+  NTBound(std::shared_ptr<nt::NetworkTable> table, std::string name, const nt::Value& value,
           std::function<void(const nt::Value&)> onUpdateFn)
-      : _table(table),
-        _entry(table->GetEntry(name)),
-        _onUpdate(onUpdateFn),
-        _name(name) {
+      : _table(table), _entry(table->GetEntry(name)), _onUpdate(onUpdateFn), _name(name) {
     _entry.SetValue(value);
     // _listener = table->AddListener(name, , ([=](const nt::EntryNotification
     // &evt) {
     //   this->_onUpdate(evt.value);
     // }, NT_NOTIFY_UPDATE);
-    _listener = table->AddListener(
-        name, nt::EventFlags::kValueAll,
-        ([this](nt::NetworkTable* table, std::string_view key,
-                const nt::Event& event) {
-          std::cout << "NT UPDATE" << std::endl;
-          this->_onUpdate(event.GetValueEventData()->value);
-        }));
+    _listener =
+        table->AddListener(name, nt::EventFlags::kValueAll,
+                           ([this](nt::NetworkTable* table, std::string_view key, const nt::Event& event) {
+                             std::cout << "NT UPDATE" << std::endl;
+                             this->_onUpdate(event.GetValueEventData()->value);
+                           }));
   }
 
   NTBound(const NTBound& other)
-      : _table(other._table),
-        _entry(other._entry),
-        _onUpdate(other._onUpdate),
-        _name(other._name) {
-    _listener = _table->AddListener(
-        _name, nt::EventFlags::kValueAll,
-        ([this](nt::NetworkTable* table, std::string_view key,
-                const nt::Event& event) {
-          std::cout << "NT UPDATE" << std::endl;
-          this->_onUpdate(event.GetValueEventData()->value);
-        }));
+      : _table(other._table), _entry(other._entry), _onUpdate(other._onUpdate), _name(other._name) {
+    _listener =
+        _table->AddListener(_name, nt::EventFlags::kValueAll,
+                            ([this](nt::NetworkTable* table, std::string_view key, const nt::Event& event) {
+                              std::cout << "NT UPDATE" << std::endl;
+                              this->_onUpdate(event.GetValueEventData()->value);
+                            }));
   }
 
   ~NTBound() { _table->RemoveListener(_listener); }
@@ -75,8 +66,7 @@ class NTBound {
 
 class NTBoundDouble : public NTBound {
  public:
-  NTBoundDouble(std::shared_ptr<nt::NetworkTable> table, std::string name,
-                double& val)
+  NTBoundDouble(std::shared_ptr<nt::NetworkTable> table, std::string name, double& val)
       : NTBound(table, name, nt::Value::MakeDouble(val),
                 [&val](const nt::Value& v) { val = v.GetDouble(); }) {}
 };
@@ -84,20 +74,15 @@ class NTBoundDouble : public NTBound {
 template <typename T>
 class NTBoundUnit : public NTBound {
  public:
-  NTBoundUnit(std::shared_ptr<nt::NetworkTable> table, std::string name,
-              units::unit_t<T>& val)
+  NTBoundUnit(std::shared_ptr<nt::NetworkTable> table, std::string name, units::unit_t<T>& val)
       : NTBound(table, name, nt::Value::MakeDouble(val.value()),
-                [&val](const nt::Value& v) {
-                  val = units::unit_t<T>{v.GetDouble()};
-                }) {}
+                [&val](const nt::Value& v) { val = units::unit_t<T>{v.GetDouble()}; }) {}
 };
 
 void WritePose2NT(std::shared_ptr<nt::NetworkTable> table, frc::Pose2d pose);
 void WritePose3NT(std::shared_ptr<nt::NetworkTable> table, frc::Pose3d pose);
-void WriteTrajectory(std::shared_ptr<nt::NetworkTable> table,
-                     frc::Trajectory trajectory);
-void WriteTrajectoryState(std::shared_ptr<nt::NetworkTable> table,
-                          frc::Trajectory::State state);
+void WriteTrajectory(std::shared_ptr<nt::NetworkTable> table, frc::Trajectory trajectory);
+void WriteTrajectoryState(std::shared_ptr<nt::NetworkTable> table, frc::Trajectory::State state);
 
 frc::Pose2d TrajectoryStateToPose2d(frc::Trajectory::State state);
 
diff --git a/wombat/src/main/include/vision/Limelight.h b/wombat/src/main/include/vision/Limelight.h
index ddfb3739..5aa6d0b7 100644
--- a/wombat/src/main/include/vision/Limelight.h
+++ b/wombat/src/main/include/vision/Limelight.h
@@ -26,20 +26,11 @@
 namespace wom {
 namespace vision {
 
-enum class LimelightLEDMode {
-  kPipelineDefault = 0,
-  kForceOff = 1,
-  kForceBlink = 2,
-  kForceOn = 3
-};
+enum class LimelightLEDMode { kPipelineDefault = 0, kForceOff = 1, kForceBlink = 2, kForceOn = 3 };
 
 enum class LimelightCamMode { kVisionProcessor = 0, kDriverCamera = 1 };
 
-enum class LimelightStreamMode {
-  kStandard = 0,
-  kPiPMain = 1,
-  kPiPSecondary = 2
-};
+enum class LimelightStreamMode { kStandard = 0, kPiPMain = 1, kPiPSecondary = 2 };
 
 enum class LimelightSnapshotMode { kReset = 0, kSingle = 1 };
 
@@ -91,14 +82,12 @@ class Limelight {
 
   std::string GetName();
 
-  std::shared_ptr<nt::NetworkTable> table =
-      nt::NetworkTableInstance::GetDefault().GetTable("limelight");
+  std::shared_ptr<nt::NetworkTable> table = nt::NetworkTableInstance::GetDefault().GetTable("limelight");
 
   std::pair<double, double> GetOffset();
 
   std::vector<double> GetAprilTagData(LimelightAprilTagData dataType);
-  double GetTargetingData(LimelightTargetingData dataType,
-                          double defaultValue = 0.0);
+  double GetTargetingData(LimelightTargetingData dataType, double defaultValue = 0.0);
   void SetLEDMode(LimelightLEDMode mode);
   void SetCamMode(LimelightCamMode mode);
   void SetPipeline(LimelightPipeline pipeline);
@@ -111,8 +100,7 @@ class Limelight {
 
   bool IsAtSetPoseVision(frc::Pose3d pose, units::second_t dt);
 
-  units::meters_per_second_t GetSpeed(frc::Pose3d pose1, frc::Pose3d pose2,
-                                      units::second_t dt);
+  units::meters_per_second_t GetSpeed(frc::Pose3d pose1, frc::Pose3d pose2, units::second_t dt);
 
   frc::Pose3d GetPose();
 

From 3940bd2174e907bdecd76f60c9023565fe64de77 Mon Sep 17 00:00:00 2001
From: Isaac Turner <spacey_sooty@outlook.com>
Date: Wed, 24 Jan 2024 14:45:55 +0800
Subject: [PATCH 102/207] fix more stuff

---
 .github/workflows/format.yml |  2 +-
 gradlew                      |  0
 wombat/LICENSE               | 21 ---------------------
 wombat/README.md             |  1 -
 wombat/settings.gradle       |  1 -
 5 files changed, 1 insertion(+), 24 deletions(-)
 mode change 100644 => 100755 gradlew
 delete mode 100644 wombat/LICENSE
 delete mode 100644 wombat/README.md
 delete mode 100644 wombat/settings.gradle

diff --git a/.github/workflows/format.yml b/.github/workflows/format.yml
index c6fced20..f57ddeb4 100644
--- a/.github/workflows/format.yml
+++ b/.github/workflows/format.yml
@@ -21,7 +21,7 @@ jobs:
         with:
           python-version: 3.8
       - name: Install wpiformat
-        run: pip3 install wpiformat==2023.36
+        run: pip3 install wpiformat==2024.31
       - name: Run
         run: wpiformat
       - name: Check output
diff --git a/gradlew b/gradlew
old mode 100644
new mode 100755
diff --git a/wombat/LICENSE b/wombat/LICENSE
deleted file mode 100644
index b1561bab..00000000
--- a/wombat/LICENSE
+++ /dev/null
@@ -1,21 +0,0 @@
-MIT License
-
-Copyright (c) 2023 CurtinFRC
-
-Permission is hereby granted, free of charge, to any person obtaining a copy
-of this software and associated documentation files (the "Software"), to deal
-in the Software without restriction, including without limitation the rights
-to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
-copies of the Software, and to permit persons to whom the Software is
-furnished to do so, subject to the following conditions:
-
-The above copyright notice and this permission notice shall be included in all
-copies or substantial portions of the Software.
-
-THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
-IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
-FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
-AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
-LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
-OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
-SOFTWARE.
diff --git a/wombat/README.md b/wombat/README.md
deleted file mode 100644
index 0595c6cb..00000000
--- a/wombat/README.md
+++ /dev/null
@@ -1 +0,0 @@
-# Wombat
diff --git a/wombat/settings.gradle b/wombat/settings.gradle
deleted file mode 100644
index 8b137891..00000000
--- a/wombat/settings.gradle
+++ /dev/null
@@ -1 +0,0 @@
-

From 57d11c01f01a43914567afcfcf6f4791dc7ac1bf Mon Sep 17 00:00:00 2001
From: totallysomeoneyoudontknow
 <155517980+totallysomeoneyoudontknow@users.noreply.github.com>
Date: Tue, 30 Jan 2024 14:32:03 +0800
Subject: [PATCH 103/207] Shooter pid (#117)

* added the state raw and changed voltage

* test

* added the state raw and changed voltage

* added pid calculate loop, fixed indenting

* added calculate function on a new line, changed the PID to not stable

* Fixed the if condition so shooterSensor is not tripped

* changed the functions so they start with a capital

* changed the value for shooterMotor

* added shuffleboard and networking tables

* added shuffleboard and networking tables

* added network tables

* fixed merge conflicts and merge errors

* fixed merge conflicts and merge errors

* fixed merge conflicts and merge errors

* fixed errors

* ran wpiformat

* fixed merge errors and conflicts

* fixed github errors

* Alpha Bot Shooter

* shooter with pid loop

* Fixed errors

* Fixing pid loop control

* fixing pid control

* pid loop tuning

* fixed PID control

* ran wpiformat

* ran wpiformat again;

* fixed build

* wpiformat

---------

Co-authored-by: Havish12 <havishchopra10@gmail.com>
Co-authored-by: Isaac Turner <spacey_sooty@outlook.com>
---
 src/main/cpp/Robot.cpp                        |  5 +-
 src/main/cpp/Shooter.cpp                      | 80 +++++++++++++------
 src/main/include/RobotMap.h                   | 50 +++++++-----
 src/main/include/Shooter.h                    |  8 +-
 src/main/include/ShooterBehaviour.h           |  7 +-
 .../src/main/cpp/drivetrain/SwerveDrive.cpp   |  7 +-
 wombat/src/main/cpp/utils/Encoder.cpp         |  2 +-
 7 files changed, 104 insertions(+), 55 deletions(-)

diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp
index 888245e6..2278fa1a 100644
--- a/src/main/cpp/Robot.cpp
+++ b/src/main/cpp/Robot.cpp
@@ -93,7 +93,9 @@ void Robot::RobotPeriodic() {
   shooter->OnUpdate(dt);
   sched->Tick();
 
-  robotmap.swerveTable.swerveDriveTable->GetEntry("frontLeftEncoder")
+  robotmap.swerveTable.swerveDriveTable
+      ->GetEntry("frontLeftEncoder")
+
       .SetDouble(robotmap.swerveBase.moduleConfigs[0].turnMotor.encoder->GetEncoderPosition().value());
   robotmap.swerveTable.swerveDriveTable->GetEntry("frontRightEncoder")
       .SetDouble(robotmap.swerveBase.moduleConfigs[1].turnMotor.encoder->GetEncoderPosition().value());
@@ -116,6 +118,7 @@ void Robot::AutonomousPeriodic() {}
 void Robot::TeleopInit() {
   loop.Clear();
   wom::BehaviourScheduler* sched = wom::BehaviourScheduler::GetInstance();
+  shooter->OnStart();
   sched->InterruptAll();
 
   // frontLeft->SetVoltage(4_V);
diff --git a/src/main/cpp/Shooter.cpp b/src/main/cpp/Shooter.cpp
index 6499e076..c3bebfd8 100644
--- a/src/main/cpp/Shooter.cpp
+++ b/src/main/cpp/Shooter.cpp
@@ -4,15 +4,26 @@
 
 #include "Shooter.h"
 
-Shooter::Shooter(ShooterConfig config)
-    : _config(config)
-// ,
-// _pid{frc::PIDController (1, 0, 0, 0.005_s)}
-{}  // config.path + "/pid", config.pidConfig
+Shooter::Shooter(ShooterConfig config) : _config(config), _pid(config.path + "/pid", config.pidConfig) {}
+
+void Shooter::OnStart() {
+  _pid.Reset();
+}
+
 void Shooter::OnUpdate(units::second_t dt) {
-  // _pid.SetTolerance(0.1, 1);
+  // _pid.SetTolerance(0.5, 4);
+  table->GetEntry("Error").SetDouble(_pid.GetError().value());
+  // table->GetEntry("Acceleration Error").SetDouble(_pid.GetVelocityError());
+  table->GetEntry("SetPoint").SetDouble(_pid.GetSetpoint().value());
+  // table->GetEntry("Current
+  // Pos").SetDouble(_config.ShooterGearbox.encoder->GetEncoderAngularVelocity().value());
+  // table->GetEntry("EncoderValue").SetDouble(_config.ShooterGearbox.encoder->GetVelocityValue());
+  table->GetEntry("Shooting").SetString(_statename);
   switch (_state) {
     case ShooterState::kIdle: {
+      _statename = "Idle";
+      _pid.Reset();
+      holdVoltage = 0_V;
       std::cout << "KIdle" << std::endl;
       _setVoltage = 0_V;
       // if (_shooterSensor.Get()) {
@@ -20,32 +31,48 @@ void Shooter::OnUpdate(units::second_t dt) {
       // }
     } break;
     case ShooterState::kSpinUp: {
-      // std::cout << "KSpinUp" << std::endl;
-      // _pid.SetSetpoint(_goal.value());
-      // units::volt_t pidCalculate =
-      //     units::volt_t{_pid.Calculate(_config.ShooterGearbox.encoder->GetEncoderAngularVelocity().value())};
-      // _setVoltage = pidCalculate;
+      _statename = "SpinUp";
+      std::cout << "KSpinUp" << std::endl;
+      _pid.SetSetpoint(_goal);
+      units::volt_t pidCalculate =
+          units::volt_t{_pid.Calculate(_config.ShooterGearbox.encoder->GetEncoderAngularVelocity(), dt, 0_V)};
+      std::cout << "KShooting" << std::endl;
 
-      // if (_pid.AtSetpoint()) {
-      //   SetState(ShooterState::kShooting);
-      // }
+      if (_pid.IsStable()) {
+        holdVoltage = pidCalculate;
+        std::cout << "STABLE" << std::endl;
+        _state = ShooterState::kShooting;
+      }
+
+      if (holdVoltage.value() == 0) {
+        _setVoltage = pidCalculate;
+      } else {
+        _setVoltage = holdVoltage;
+      }
     } break;
     case ShooterState::kShooting: {
-      // std::cout << "KShooting" << std::endl;
-      // _pid.SetSetpoint(_goal.value());
-      // units::volt_t pidCalculate =
-      //     units::volt_t{_pid.Calculate(_config.ShooterGearbox.encoder->GetEncoderAngularVelocity().value())};
-      // _setVoltage = pidCalculate;
+      _statename = "Shooting";
 
-      // if (!_pid.AtSetpoint()) {
-      //   SetState(ShooterState::kSpinUp);
-      // }
-      // if (_shooterSensor.Get()) {
-      //   SetState(ShooterState::kIdle);
-      // }
+      _pid.SetSetpoint(_goal);
+      units::volt_t pidCalculate =
+          units::volt_t{_pid.Calculate(_config.ShooterGearbox.encoder->GetEncoderAngularVelocity(), dt, 0_V)};
+      std::cout << "KShooting" << std::endl;
+
+      if (_pid.IsStable()) {
+        holdVoltage = pidCalculate;
+        std::cout << "STABLE" << std::endl;
+      }
+
+      if (holdVoltage.value() == 0) {
+        _setVoltage = pidCalculate;
+      } else {
+        _setVoltage = holdVoltage;
+      }
     } break;
 
     case ShooterState::kReverse: {
+      _statename = "Reverse";
+      _pid.Reset();
       _setVoltage = -5_V;
       std::cout << "KReverse" << std::endl;
       // if (!_shooterSensor.Get()) {
@@ -53,6 +80,9 @@ void Shooter::OnUpdate(units::second_t dt) {
       // }
     } break;
     case ShooterState::kRaw: {
+      _statename = "Raw";
+      holdVoltage = 0_V;
+      _pid.Reset();
       _setVoltage = _rawVoltage;
       std::cout << "KRaw" << std::endl;
       // if (_shooterSensor.Get()) {
diff --git a/src/main/include/RobotMap.h b/src/main/include/RobotMap.h
index 733ab1af..60c3ed19 100644
--- a/src/main/include/RobotMap.h
+++ b/src/main/include/RobotMap.h
@@ -31,16 +31,26 @@ struct RobotMap {
   };
   Controllers controllers;
 
-  struct AlphaArmSystem {
-    rev::CANSparkMax alphaArmMotor{12, rev::CANSparkMax::MotorType::kBrushless};
-    rev::CANSparkMax wristMotor{15, rev::CANSparkMax::MotorType::kBrushless};
+  struct Shooter {
+    rev::CANSparkMax shooterMotor{11, rev::CANSparkMax::MotorType::kBrushless};  // Port 11
+    // frc::DigitalInput shooterSensor{2};
 
-    wom::Gearbox alphaArmGearbox{&alphaArmMotor, nullptr, frc::DCMotor::NEO(1)};
-    wom::Gearbox wristGearbox{&wristMotor, nullptr, frc::DCMotor::NEO(1)};
+    // wom::VoltageController shooterMotorGroup =
+    // wom::VoltagedController::Group(shooterMotor);
+    wom::CANSparkMaxEncoder* shooterEncoder = new wom::CANSparkMaxEncoder(&shooterMotor, 0.01_m);
+    wom::Gearbox shooterGearbox{&shooterMotor, shooterEncoder, frc::DCMotor::NEO(1)};
 
-    AlphaArmConfig config{alphaArmGearbox, wristGearbox};
+    wom::utils::PIDConfig<units::radians_per_second, units::volts> pidConfigS{
+        "/armavator/arm/velocityPID/config",
+        0.1_V / (360_deg / 1_s),
+        0.03_V / 25_deg,
+        0.001_V / (90_deg / 1_s / 1_s),
+        5_rad_per_s,
+        10_rad_per_s / 1_s};
+
+    ShooterConfig config{"shooterGearbox", shooterGearbox, pidConfigS};
   };
-  AlphaArmSystem alphaArmSystem;
+  Shooter shooterSystem;
 
   struct IntakeSystem {
     rev::CANSparkMax intakeMotor{2, rev::CANSparkMax::MotorType::kBrushed};
@@ -55,21 +65,6 @@ struct RobotMap {
   };
   IntakeSystem intakeSystem;
 
-  struct Shooter {
-    rev::CANSparkMax shooterMotor{11, rev::CANSparkMax::MotorType::kBrushless};
-    // frc::DigitalInput shooterSensor{2};
-
-    // wom::VoltageController shooterMotorGroup = wom::VoltageController::Group(shooterMotor);
-    // wom::CANSparkMaxEncoder* shooterEncoder = new wom::CANSparkMaxEncoder(&shooterMotor, 0.01_m);
-    wom::Gearbox shooterGearbox{&shooterMotor, nullptr, frc::DCMotor::NEO(1)};
-
-    ShooterConfig config{
-        "shooterGearbox", shooterGearbox,
-        // &shooterSensor,
-    };
-  };
-  Shooter shooterSystem;
-
   struct SwerveBase {
     ctre::phoenix6::hardware::CANcoder frontLeftCancoder{18, "Drivebase"};
     ctre::phoenix6::hardware::CANcoder frontRightCancoder{19, "Drivebase"};
@@ -173,4 +168,15 @@ struct RobotMap {
         nt::NetworkTableInstance::GetDefault().GetTable("swerve");
   };
   SwerveTable swerveTable;
+
+  struct AlphaArmSystem {
+    rev::CANSparkMax alphaArmMotor{12, rev::CANSparkMax::MotorType::kBrushless};
+    rev::CANSparkMax wristMotor{15, rev::CANSparkMax::MotorType::kBrushless};
+
+    wom::Gearbox alphaArmGearbox{&alphaArmMotor, nullptr, frc::DCMotor::NEO(1)};
+    wom::Gearbox wristGearbox{&wristMotor, nullptr, frc::DCMotor::NEO(1)};
+
+    AlphaArmConfig config{alphaArmGearbox, wristGearbox};
+  };
+  AlphaArmSystem alphaArmSystem;
 };
diff --git a/src/main/include/Shooter.h b/src/main/include/Shooter.h
index cf21c3bc..6ef8eef4 100644
--- a/src/main/include/Shooter.h
+++ b/src/main/include/Shooter.h
@@ -17,6 +17,7 @@ struct ShooterConfig {
   wom::Gearbox ShooterGearbox;
   // wom::PIDConfig<units::radians_per_second, units::volt> pidConfig;
   // frc::DigitalInput* shooterSensor;
+  wom::PIDConfig<units::radians_per_second, units::volt> pidConfig;
 };
 
 enum class ShooterState { kIdle, kShooting, kSpinUp, kReverse, kRaw };
@@ -25,6 +26,8 @@ class Shooter : public behaviour::HasBehaviour {
  public:
   explicit Shooter(ShooterConfig config);
 
+  void OnStart();
+
   void OnUpdate(units::second_t dt);
   void SetState(ShooterState state);
   void SetRaw(units::volt_t voltage);
@@ -38,6 +41,9 @@ class Shooter : public behaviour::HasBehaviour {
   units::volt_t _rawVoltage;
   units::radians_per_second_t _goal;
   units::volt_t _setVoltage = 0_V;
-  // frc::PIDController _pid;
+  wom::PIDController<units::radians_per_second, units::volt> _pid;
   // frc::DigitalInput _shooterSensor{0};
+
+  units::volt_t holdVoltage = 0_V;
+  std::string _statename = "default";
 };
diff --git a/src/main/include/ShooterBehaviour.h b/src/main/include/ShooterBehaviour.h
index ff65b1d8..c95f78ae 100644
--- a/src/main/include/ShooterBehaviour.h
+++ b/src/main/include/ShooterBehaviour.h
@@ -6,7 +6,8 @@
 
 #include <frc/XboxController.h>
 
-#include "Robot.h"
+#include <memory>
+
 #include "Shooter.h"
 #include "Wombat.h"
 
@@ -18,7 +19,9 @@ class ShooterManualControl : public behaviour::Behaviour {
 
  private:
   Shooter* _shooter;
-  frc::XboxController* _codriver;
 
   bool _rawControl = true;
+  std::shared_ptr<nt::NetworkTable> table =
+      nt::NetworkTableInstance::GetDefault().GetTable("Shooter Behaviour");
+  frc::XboxController* _codriver;
 };
diff --git a/wombat/src/main/cpp/drivetrain/SwerveDrive.cpp b/wombat/src/main/cpp/drivetrain/SwerveDrive.cpp
index 74fc6d89..38919cb6 100644
--- a/wombat/src/main/cpp/drivetrain/SwerveDrive.cpp
+++ b/wombat/src/main/cpp/drivetrain/SwerveDrive.cpp
@@ -80,7 +80,8 @@ void SwerveModule::OnUpdate(units::second_t dt) {
   //     -torqueLimit, _config.driveMotor.encoder->GetEncoderAngularVelocity());
 
   // driveVoltage =
-  //     units::math::max(units::math::min(driveVoltage, voltageMax), voltageMin);
+  //     units::math::max(units::math::min(driveVoltage, voltageMax),
+  //     voltageMin);
 
   // driveVoltage = units::math::min(driveVoltage, 10_V);
   turnVoltage = units::math::min(turnVoltage, 7_V);
@@ -136,8 +137,8 @@ void SwerveModule::SetZero(units::second_t dt) {
 void SwerveModule::SetPID(units::radian_t angle, units::meters_per_second_t speed, units::second_t dt) {
   _state = SwerveModuleState::kPID;
   // @liam start added
-  // std::cout << "angle Setpoint: " << _anglePIDController.GetSetpoint() << std::endl;
-  // std::cout << "angle value: " << angle.value() << std::endl;
+  // std::cout << "angle Setpoint: " << _anglePIDController.GetSetpoint() <<
+  // std::endl; std::cout << "angle value: " << angle.value() << std::endl;
 
   double diff = std::fmod((_anglePIDController.GetSetpoint() - angle.value()), (2 * 3.1415));
   std::cout << "DIFF value: " << diff << std::endl;
diff --git a/wombat/src/main/cpp/utils/Encoder.cpp b/wombat/src/main/cpp/utils/Encoder.cpp
index 461a8f31..1f5a58c4 100644
--- a/wombat/src/main/cpp/utils/Encoder.cpp
+++ b/wombat/src/main/cpp/utils/Encoder.cpp
@@ -115,7 +115,7 @@ double wom::utils::CANSparkMaxEncoder::GetPosition() const {
 }
 
 double wom::utils::CANSparkMaxEncoder::GetVelocity() const {
-  return _encoder.GetVelocity();
+  return _encoder.GetVelocity() * 3.14159265 * 2 / 60;
 }
 
 wom::utils::TalonFXEncoder::TalonFXEncoder(ctre::phoenix6::hardware::TalonFX* controller,

From c30477ac7a820eb4e803b8650fcd3a66b3b66a08 Mon Sep 17 00:00:00 2001
From: prawny-boy <155516197+prawny-boy@users.noreply.github.com>
Date: Tue, 30 Jan 2024 15:12:55 +0800
Subject: [PATCH 104/207] Intake - Manual/Auto fixes (#114)

* First version Tank Drive

* Intake draft

* Intake draft fixed errors 1

* Intake draft fixed errors 2

* Intake draft fixed errors 3

* Intake draft fixed errors 4

* Intake draft fixed errors 5

* Network files 1

* Network files 2

* Network files 3

* Almost done i think

* finished wpiformat errors

* finished wpiformat errors 2

* finished wpiformat errors 3

* finished wpiformat errors 4

* ran wpiformat

* finished wpiformat errors 5

* finished wpiformat errors 8

* finished wpiformat errors 9

* finished wpiformat errors 10

* Tested on robot

* Fixed github build errors after modifying during robot tests

* fixed more github build errors 2

* fixed more github build errors 3

* Tested on robot and works

* added beam break

* [docs] Update README.md (#107)

* [docs] Update README.md

* Update README.md

* Added auto to intake

* [ci] add comment command action (#111)

* [ci] add comment command action

* fix

* Tested on robot

* fixed intake manual auto, has not been tested on robot

* fixed github build errors

* Added intake better manual and verfied on robot!

* Bump actions/setup-python from 4 to 5 (#116)

Bumps [actions/setup-python](https://github.com/actions/setup-python) from 4 to 5.
- [Release notes](https://github.com/actions/setup-python/releases)
- [Commits](https://github.com/actions/setup-python/compare/v4...v5)

---
updated-dependencies:
- dependency-name: actions/setup-python
  dependency-type: direct:production
  update-type: version-update:semver-major
...

Signed-off-by: dependabot[bot] <support@github.com>
Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com>

* Bump actions/github-script from 6 to 7 (#115)

Bumps [actions/github-script](https://github.com/actions/github-script) from 6 to 7.
- [Release notes](https://github.com/actions/github-script/releases)
- [Commits](https://github.com/actions/github-script/compare/v6...v7)

---
updated-dependencies:
- dependency-name: actions/github-script
  dependency-type: direct:production
  update-type: version-update:semver-major
...

Signed-off-by: dependabot[bot] <support@github.com>
Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com>

* Fixed readme (#118)

* ran wpiformat

* fix

* fix line endings

* builds

---------

Signed-off-by: dependabot[bot] <support@github.com>
Co-authored-by: Kingsley Wong <kingsley.wong@student.education.wa.edu.au>
Co-authored-by: Isaac Turner <spacey_sooty@outlook.com>
Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com>
Co-authored-by: Paul Hodges <111325206+Superbro525Alt@users.noreply.github.com>
---
 .gitattributes                     |  1 +
 src/main/cpp/Intake.cpp            | 51 ++++++++++-------
 src/main/cpp/IntakeBehaviour.cpp   | 91 +++++++++++++++++++++---------
 src/main/cpp/Robot.cpp             | 10 +---
 src/main/include/Intake.h          |  5 ++
 src/main/include/IntakeBehaviour.h |  6 +-
 src/main/include/RobotMap.h        | 27 +++++----
 7 files changed, 123 insertions(+), 68 deletions(-)

diff --git a/.gitattributes b/.gitattributes
index 416557ac..df788002 100644
--- a/.gitattributes
+++ b/.gitattributes
@@ -1,5 +1,6 @@
 * text=auto
 *.sh text eol=lf
+*.bat text eol=crlf
 *.gradle text eol=lf
 *.java text eol=lf
 *.json text eol=lf
diff --git a/src/main/cpp/Intake.cpp b/src/main/cpp/Intake.cpp
index 75839209..f63be144 100644
--- a/src/main/cpp/Intake.cpp
+++ b/src/main/cpp/Intake.cpp
@@ -13,43 +13,51 @@ IntakeConfig Intake::GetConfig() {
 void Intake::OnUpdate(units::second_t dt) {
   switch (_state) {
     case IntakeState::kIdle: {
-      // _config.IntakeMotor.motorController->SetVoltage(0_V);
-      // if (_config.intakeSensor->Get()) {
-      //   setState(IntakeState::kHold);
-      // }
+      if (_config.intakeSensor->Get() == false) {
+        setState(IntakeState::kHold);
+      }
       _stringStateName = "Idle";
       _setVoltage = 0_V;
     } break;
+
     case IntakeState::kRaw: {
-      // _config.IntakeMotor.motorController->SetVoltage(_rawVoltage);
       _stringStateName = "Raw";
       _setVoltage = _rawVoltage;
     } break;
+
     case IntakeState::kEject: {
-      // _config.IntakeMotor.motorController->SetVoltage(-5_V);
-      // if (_config.intakeSensor->Get() == 0 && _config.magSensor->Get() == 0) {
-      //   setState(IntakeState::kIdle);
-      // }
       _stringStateName = "Eject";
-      _setVoltage = -5_V;
+      _setVoltage = 7_V;
+      if (_config.intakeSensor->Get() == true) {
+        setState(IntakeState::kIdle);
+        _ejecting = false;
+      }
     } break;
+
     case IntakeState::kHold: {
-      // _config.IntakeMotor.motorController->SetVoltage(0_V);
       _stringStateName = "Hold";
       _setVoltage = 0_V;
+      // if (_config.intakeSensor->Get() == false) {
+      //   setState(IntakeState::kHold);
+      // }
     } break;
+
     case IntakeState::kIntake: {
-      // _config.IntakeMotor.motorController->SetVoltage(5_V);
       _stringStateName = "Intake";
-      _setVoltage = 5_V;
+      _setVoltage = -7_V;
+      if (_config.intakeSensor->Get() == false) {
+        setState(IntakeState::kHold);
+        _intaking = false;
+      }
     } break;
+
     case IntakeState::kPass: {
-      // _config.IntakeMotor.motorController->SetVoltage(5_V);
-      // if (_config.shooterSensor->Get()) {
-      //   setState(IntakeState::kIdle);
-      //   _stringStateName = "Pass";
-      // }
-      _setVoltage = 5_V;
+      _stringStateName = "Pass";
+      _setVoltage = -7_V;
+      if (_config.intakeSensor->Get() == true) {
+        setState(IntakeState::kIdle);
+        _passing = false;
+      }
     } break;
     default:
       std::cout << "Error: Intake in INVALID STATE." << std::endl;
@@ -57,6 +65,8 @@ void Intake::OnUpdate(units::second_t dt) {
   }
   _table->GetEntry("State: ").SetString(_stringStateName);
   _table->GetEntry("Motor Voltage: ").SetDouble(_setVoltage.value());
+  _table->GetEntry("Intake Sensor: ").SetBoolean(_config.intakeSensor->Get());
+  // _table->GetEntry("Shooter Sensor: ").SetBoolean(_config.shooterSensor->Get());
   // _table->GetEntry("Intake Sensor: ").SetBoolean(_config.intakeSensor->Get());
   // _table->GetEntry("Shooter Sensor: ").SetBoolean(_config.shooterSensor->Get());
   // _table->GetEntry("Magazine Sensor: ").SetBoolean(_config.magSensor->Get());
@@ -72,3 +82,6 @@ void Intake::setState(IntakeState state) {
 void Intake::setRaw(units::volt_t voltage) {
   _rawVoltage = voltage;
 }
+IntakeState Intake::getState() {
+  return _state;
+}
diff --git a/src/main/cpp/IntakeBehaviour.cpp b/src/main/cpp/IntakeBehaviour.cpp
index 90867045..59558bba 100644
--- a/src/main/cpp/IntakeBehaviour.cpp
+++ b/src/main/cpp/IntakeBehaviour.cpp
@@ -12,44 +12,83 @@ IntakeManualControl::IntakeManualControl(Intake* intake, frc::XboxController& co
 }
 
 void IntakeManualControl::OnTick(units::second_t dt) {
-  if (_codriver.GetBButtonPressed()) {
-    if (_rawControl == true) {
+  if (_codriver.GetBButtonReleased()) {
+    if (_rawControl) {
       _rawControl = false;
+      _intaking = false;
+      _ejecting = false;
+      _intake->setState(IntakeState::kIdle);
     } else {
       _rawControl = true;
+      _intaking = false;
+      _ejecting = false;
+      _intake->setState(IntakeState::kRaw);
     }
   }
 
   if (_rawControl) {
-    _intake->setState(IntakeState::kRaw);
-    if (_codriver.GetLeftBumper()) {
-      _intake->setRaw(10_V);
-    } else if (_codriver.GetRightBumper()) {
-      _intake->setRaw(-10_V);
+    if (_codriver.GetRightTriggerAxis() > 0.1) {
+      _intake->setRaw(_codriver.GetRightTriggerAxis() * 10_V);
+    } else if (_codriver.GetLeftTriggerAxis() > 0.1) {
+      _intake->setRaw(_codriver.GetLeftTriggerAxis() * -10_V);
     } else {
       _intake->setRaw(0_V);
     }
+    _intake->setState(IntakeState::kRaw);
+
+  } else {
+    if (_codriver.GetRightTriggerAxis() > 0.1) {
+      if (_intaking) {
+        _intaking = false;
+        _intake->setState(IntakeState::kIdle);
+      } else {
+        _intaking = true;
+        _ejecting = false;
+      }
+    }
+
+    if (_codriver.GetLeftTriggerAxis() > 0.1) {
+      if (_ejecting) {
+        _ejecting = false;
+        _intake->setState(IntakeState::kIdle);
+      } else {
+        _ejecting = true;
+        _intaking = false;
+      }
+    }
+
+    if (_codriver.GetAButtonPressed()) {
+      if (_passing) {
+        _passing = false;
+        _intake->setState(IntakeState::kIdle);
+      } else {
+        _passing = true;
+        _intaking = false;
+      }
+    }
 
-    // _intake->setRaw(_codriver.GetLeftBumper() * 10_V);
-    // _intake->setRaw(_codriver.GetRightBumper() * -10_V);
-    std::cout << "Raw" << std::endl;
+    if (_intaking) {
+      if (_intake->getState() == IntakeState::kIdle) {
+        _intake->setState(IntakeState::kIntake);
+      }
+    }
+
+    if (_passing) {
+      if (_intake->getState() == IntakeState::kHold) {
+        _intake->setState(IntakeState::kPass);
+      }
+    }
+
+    if (_ejecting) {
+      if (_intake->getState() == IntakeState::kIdle || _intake->getState() == IntakeState::kHold) {
+        _intake->setState(IntakeState::kEject);
+      }
+    }
   }
-  // } else {
-  //   if (_codriver.GetYButtonPressed()) {
-  //     _intake->setState(IntakeState::kIntake);
-  //   }
-  //   if (_codriver.GetAButtonPressed()) {
-  //     _intake->setState(IntakeState::kPass);
-  //   }
-  // }
 }
 
-// IntakeAutoControl::IntakeAutoControl(Intake* intake) : _intake(intake) {}
+IntakeAutoControl::IntakeAutoControl(Intake* intake) : _intake(intake) {
+  Controls(intake);
+}
 
-// void IntakeAutoControl::OnTick(units::second_t dt) {
-//   if (_intake->GetConfig().intakeSensor->Get() == 1) {
-//     _intake->setState(IntakeState::kPass);
-//   } else if (_intake->GetConfig().magSensor->Get() == 0) {
-//     _intake->setState(IntakeState::kIdle);
-//   }
-// }
+void IntakeAutoControl::OnTick(units::second_t dt) {}
diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp
index 2278fa1a..473873ba 100644
--- a/src/main/cpp/Robot.cpp
+++ b/src/main/cpp/Robot.cpp
@@ -46,10 +46,6 @@ void Robot::RobotInit() {
 
   // robotmap.swerveBase.gyro->Reset();
 
-  // _swerveDrive = new wom::SwerveDrive(robotmap.swerveBase.config, frc::Pose2d());
-  // wom::BehaviourScheduler::GetInstance()->Register(_swerveDrive);
-  // _swerveDrive->SetDefaultBehaviour(
-  //     [this]() { return wom::make<wom::ManualDrivebase>(_swerveDrive, &robotmap.controllers.driver); });
   _swerveDrive = new wom::SwerveDrive(robotmap.swerveBase.config, frc::Pose2d());
   wom::BehaviourScheduler::GetInstance()->Register(_swerveDrive);
   _swerveDrive->SetDefaultBehaviour(
@@ -93,9 +89,7 @@ void Robot::RobotPeriodic() {
   shooter->OnUpdate(dt);
   sched->Tick();
 
-  robotmap.swerveTable.swerveDriveTable
-      ->GetEntry("frontLeftEncoder")
-
+  robotmap.swerveTable.swerveDriveTable->GetEntry("frontLeftEncoder")
       .SetDouble(robotmap.swerveBase.moduleConfigs[0].turnMotor.encoder->GetEncoderPosition().value());
   robotmap.swerveTable.swerveDriveTable->GetEntry("frontRightEncoder")
       .SetDouble(robotmap.swerveBase.moduleConfigs[1].turnMotor.encoder->GetEncoderPosition().value());
@@ -106,6 +100,7 @@ void Robot::RobotPeriodic() {
 
   _swerveDrive->OnUpdate(dt);
   alphaArm->OnUpdate(dt);
+  shooter->OnStart();
   intake->OnUpdate(dt);
 }
 
@@ -118,7 +113,6 @@ void Robot::AutonomousPeriodic() {}
 void Robot::TeleopInit() {
   loop.Clear();
   wom::BehaviourScheduler* sched = wom::BehaviourScheduler::GetInstance();
-  shooter->OnStart();
   sched->InterruptAll();
 
   // frontLeft->SetVoltage(4_V);
diff --git a/src/main/include/Intake.h b/src/main/include/Intake.h
index a84b41b1..8d9c80b5 100644
--- a/src/main/include/Intake.h
+++ b/src/main/include/Intake.h
@@ -28,6 +28,7 @@ class Intake : public behaviour::HasBehaviour {
 
   void setState(IntakeState state);
   void setRaw(units::volt_t voltage);
+  IntakeState getState();
   IntakeConfig GetConfig();
 
  private:
@@ -37,5 +38,9 @@ class Intake : public behaviour::HasBehaviour {
   units::volt_t _rawVoltage = 0_V;
   std::string _stringStateName = "error";
   units::volt_t _setVoltage = 0_V;
+  bool _intaking;
+  bool _ejecting;
+  bool _passing;
+
   std::shared_ptr<nt::NetworkTable> _table = nt::NetworkTableInstance::GetDefault().GetTable("Intake");
 };
diff --git a/src/main/include/IntakeBehaviour.h b/src/main/include/IntakeBehaviour.h
index c84c6d32..931273fa 100644
--- a/src/main/include/IntakeBehaviour.h
+++ b/src/main/include/IntakeBehaviour.h
@@ -20,7 +20,11 @@ class IntakeManualControl : public behaviour::Behaviour {
   frc::XboxController& _codriver;
 
   units::volt_t _rawVoltage;
-  bool _rawControl;
+  units::volt_t _setVoltage;
+  bool _rawControl = true;
+  bool _intaking = false;
+  bool _ejecting = false;
+  bool _passing = false;
 };
 
 class IntakeAutoControl : public behaviour::Behaviour {
diff --git a/src/main/include/RobotMap.h b/src/main/include/RobotMap.h
index 60c3ed19..0cadce34 100644
--- a/src/main/include/RobotMap.h
+++ b/src/main/include/RobotMap.h
@@ -27,10 +27,22 @@ struct RobotMap {
   struct Controllers {
     frc::XboxController driver = frc::XboxController(0);
     frc::XboxController codriver = frc::XboxController(1);
-    frc::XboxController testController = frc::XboxController(2);
   };
   Controllers controllers;
 
+  struct IntakeSystem {
+    rev::CANSparkMax intakeMotor{2, rev::CANSparkMax::MotorType::kBrushed};
+    // wom::CANSparkMaxEncoder intakeEncoder{&intakeMotor, 0.1_m};
+    frc::DigitalInput intakeSensor{4};
+    // frc::DigitalInput magSensor{0};
+    // frc::DigitalInput shooterSensor{0};
+
+    wom::Gearbox IntakeGearbox{&intakeMotor, nullptr, frc::DCMotor::CIM(1)};
+
+    IntakeConfig config{IntakeGearbox, &intakeSensor /*, &magSensor, &shooterSensor*/};
+  };
+  IntakeSystem intakeSystem;
+
   struct Shooter {
     rev::CANSparkMax shooterMotor{11, rev::CANSparkMax::MotorType::kBrushless};  // Port 11
     // frc::DigitalInput shooterSensor{2};
@@ -52,19 +64,6 @@ struct RobotMap {
   };
   Shooter shooterSystem;
 
-  struct IntakeSystem {
-    rev::CANSparkMax intakeMotor{2, rev::CANSparkMax::MotorType::kBrushed};
-    // wom::CANSparkMaxEncoder intakeEncoder{&intakeMotor, 0.1_m};
-    // frc::DigitalInput intakeSensor{0};
-    // frc::DigitalInput magSensor{0};
-    // frc::DigitalInput shooterSensor{0};
-
-    wom::Gearbox IntakeGearbox{&intakeMotor, nullptr, frc::DCMotor::CIM(1)};
-
-    IntakeConfig config{IntakeGearbox /*, &intakeSensor, &magSensor, &shooterSensor*/};
-  };
-  IntakeSystem intakeSystem;
-
   struct SwerveBase {
     ctre::phoenix6::hardware::CANcoder frontLeftCancoder{18, "Drivebase"};
     ctre::phoenix6::hardware::CANcoder frontRightCancoder{19, "Drivebase"};

From c83ac05acd23de3815fb4667e6dae0ddafc55591 Mon Sep 17 00:00:00 2001
From: Paul Hodges <paul.hodges@student.education.wa.edu.au>
Date: Thu, 25 Jan 2024 12:46:21 +0800
Subject: [PATCH 105/207] [robot/vision] Started work on limelight vision

---
 src/main/cpp/Robot.cpp                   |  26 +-
 src/main/cpp/vision/Vision.cpp           | 132 ++++++++
 src/main/deploy/fmap.fmap                | 388 +++++++++++++++++++++++
 src/main/include/Robot.h                 |   8 +
 src/main/include/vision/Vision.h         |  56 ++++
 wombat/src/main/cpp/vision/Limelight.cpp |   2 +-
 6 files changed, 608 insertions(+), 4 deletions(-)
 create mode 100644 src/main/cpp/vision/Vision.cpp
 create mode 100644 src/main/deploy/fmap.fmap
 create mode 100644 src/main/include/vision/Vision.h

diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp
index 473873ba..3057ebe7 100644
--- a/src/main/cpp/Robot.cpp
+++ b/src/main/cpp/Robot.cpp
@@ -23,6 +23,7 @@
 static units::second_t lastPeriodic;
 
 void Robot::RobotInit() {
+
   shooter = new Shooter(robotmap.shooterSystem.config);
   wom::BehaviourScheduler::GetInstance()->Register(shooter);
   shooter->SetDefaultBehaviour(
@@ -31,6 +32,9 @@ void Robot::RobotInit() {
   sched = wom::BehaviourScheduler::GetInstance();
   m_chooser.SetDefaultOption("Default Auto", "Default Auto");
 
+  // m_chooser.SetDefaultOption("Default Auto", "Default Auto");
+
+
   // frc::SmartDashboard::PutData("Auto Selector", &m_chooser);
 
   // m_path_chooser.SetDefaultOption("Path1", "paths/output/Path1.wpilib.json");
@@ -46,6 +50,11 @@ void Robot::RobotInit() {
 
   // robotmap.swerveBase.gyro->Reset();
 
+  // _swerveDrive = new wom::SwerveDrive(robotmap.swerveBase.config, frc::Pose2d());
+  // wom::BehaviourScheduler::GetInstance()->Register(_swerveDrive);
+  // _swerveDrive->SetDefaultBehaviour(
+  //     [this]() { return wom::make<wom::ManualDrivebase>(_swerveDrive, &robotmap.controllers.driver); });
+
   _swerveDrive = new wom::SwerveDrive(robotmap.swerveBase.config, frc::Pose2d());
   wom::BehaviourScheduler::GetInstance()->Register(_swerveDrive);
   _swerveDrive->SetDefaultBehaviour(
@@ -78,6 +87,9 @@ void Robot::RobotInit() {
   wom::BehaviourScheduler::GetInstance()->Register(intake);
   intake->SetDefaultBehaviour(
       [this]() { return wom::make<IntakeManualControl>(intake, robotmap.controllers.codriver); });
+
+  //_vision = new Vision("limelight", FMAP("fmap.fmap"));
+
 }
 
 void Robot::RobotPeriodic() {
@@ -98,10 +110,14 @@ void Robot::RobotPeriodic() {
   robotmap.swerveTable.swerveDriveTable->GetEntry("backRightEncoder")
       .SetDouble(robotmap.swerveBase.moduleConfigs[3].turnMotor.encoder->GetEncoderPosition().value());
 
+
   _swerveDrive->OnUpdate(dt);
   alphaArm->OnUpdate(dt);
   shooter->OnStart();
   intake->OnUpdate(dt);
+
+  // _swerveDrive->OnUpdate(dt);
+
 }
 
 void Robot::AutonomousInit() {
@@ -111,6 +127,7 @@ void Robot::AutonomousInit() {
 void Robot::AutonomousPeriodic() {}
 
 void Robot::TeleopInit() {
+
   loop.Clear();
   wom::BehaviourScheduler* sched = wom::BehaviourScheduler::GetInstance();
   sched->InterruptAll();
@@ -119,9 +136,12 @@ void Robot::TeleopInit() {
   // frontRight->SetVoltage(4_V);
   // backLeft->SetVoltage(4_V);
   // backRight->SetVoltage(4_V);
-  loop.Clear();
-  wom::BehaviourScheduler* scheduler = wom::BehaviourScheduler::GetInstance();
-  scheduler->InterruptAll();
+  
+
+//  FMAP("fmap.fmap");
+  // _swerveDrive->OnStart();
+  // sched->InterruptAll();
+
 }
 
 void Robot::TeleopPeriodic() {}
diff --git a/src/main/cpp/vision/Vision.cpp b/src/main/cpp/vision/Vision.cpp
new file mode 100644
index 00000000..4d3fee35
--- /dev/null
+++ b/src/main/cpp/vision/Vision.cpp
@@ -0,0 +1,132 @@
+#include "vision/Vision.h"
+
+
+FMAP::FMAP(std::string path) : _path(path) {
+    std::cout << "Parsing FMAP" << std::endl;
+
+    deploy_dir = frc::filesystem::GetDeployDirectory();
+
+    std::ifstream file(deploy_dir + "/" + _path);
+
+    // parse the json file into a wpi::json object
+
+    wpi::json j;
+
+    file >> j;
+
+    // iterate through the json object and add each tag to the vector
+
+    for (auto& element : j["fiducials"]) {
+        AprilTag tag;
+
+        tag.id = element["id"];
+        tag.size = element["size"];
+        tag.transform[0][0] = element["transform"][0];
+        tag.transform[0][1] = element["transform"][1];
+        tag.transform[0][2] = element["transform"][2];
+        tag.transform[0][3] = element["transform"][3];
+        tag.transform[1][0] = element["transform"][4];
+        tag.transform[1][1] = element["transform"][5];
+        tag.transform[1][2] = element["transform"][6];
+        tag.transform[1][3] = element["transform"][7];
+        tag.transform[2][0] = element["transform"][8];
+        tag.transform[2][1] = element["transform"][9];
+        tag.transform[2][2] = element["transform"][10];
+        tag.transform[2][3] = element["transform"][11];
+        tag.transform[3][0] = element["transform"][12];
+        tag.transform[3][1] = element["transform"][13];
+        tag.transform[3][2] = element["transform"][14];
+        tag.transform[3][3] = element["transform"][15];
+        if (element["unique"] == 1) {
+            tag.unique = true;
+        } else {
+            tag.unique = false;
+        }
+
+        _tags.push_back(tag);
+    }
+
+    file.close();
+
+    std::cout << "Loaded " << _tags.size() << " tags" << std::endl;
+
+    for (int i = 0; i < _tags.size(); i++) {
+        std::cout << "Tag " << _tags[i].id << " is " << _tags[i].size << "m" << std::endl;
+    }
+
+    std::cout << "Loaded FMAP" << std::endl;
+};
+
+std::vector<AprilTag> FMAP::GetTags() {
+    return _tags;
+};
+
+Vision::Vision(std::string name, FMAP fmap) : _fmap(fmap) {
+    _limelight = new wom::Limelight(name);
+
+    _limelight->SetPipeline(wom::LimelightPipeline::kPipeline0);
+};
+
+frc::Pose3d Vision::GetPose() {
+    return _limelight->GetPose();
+};
+
+std::pair<units::meter_t, units::degree_t> Vision::GetDistanceToTarget(VisionTarget target) {
+    std::vector<AprilTag> tags = _fmap.GetTags();
+
+    frc::Pose3d pose = _limelight->GetPose();
+
+    for (int i = 0; i < tags.size(); i++) {
+        if (tags[i].id == (int)target) {         
+
+            frc::Pose3d aprilTagPos = frc::Pose3d(tags[i].transform[0][3] * 1_m, tags[i].transform[1][3] * 1_m, tags[i].transform[2][3] * 1_m, frc::Rotation3d(0_deg, 0_deg, 0_deg));   
+            
+            units::meter_t distance = pose.Translation().Distance(aprilTagPos.Translation());
+
+            units::degree_t angle = units::math::atan((pose.X() - units::meter_t{tags[i].transform[0][3]}) / (pose.Y() - units::meter_t{tags[i].transform[1][3]}));
+
+            return std::make_pair(distance, angle);
+        }
+    }
+
+    return std::make_pair(0_m, 0_deg);
+};
+
+std::pair<units::meter_t, units::degree_t> Vision::GetDistanceToTarget(int id) {
+    if (id < APRILTAGS_MIN || id > APRILTAGS_MAX) { return std::make_pair(0_m, 0_deg); };
+    
+    std::vector<AprilTag> tags = _fmap.GetTags();
+
+    for (int i = 0; i < tags.size(); i++) {
+        if (tags[i].id == id) {
+            // get distance to the limelight
+            frc::Pose3d currentPose = _limelight->GetPose();
+            frc::Pose3d aprilTagPos = frc::Pose3d(tags[i].transform[0][3] * 1_m, tags[i].transform[1][3] * 1_m, tags[i].transform[2][3] * 1_m, frc::Rotation3d(0_deg, 0_deg, 0_deg));
+
+            units::meter_t distance = currentPose.Translation().Distance(aprilTagPos.Translation());
+
+            units::degree_t angle = units::math::atan((currentPose.X() - units::meter_t{tags[i].transform[0][3]}) / (currentPose.Y() - units::meter_t{tags[i].transform[1][3]}));
+
+            return std::make_pair(distance, angle);
+        }
+    }
+
+    return std::make_pair(0_m, 0_deg);
+};
+
+std::vector<AprilTag> Vision::GetTags() {
+    return _fmap.GetTags();
+};
+
+frc::Pose2d Vision::AlignToTarget(VisionTarget target, wom::SwerveDrive* swerveDrive) {
+    std::pair<units::meter_t, units::degree_t> distance = GetDistanceToTarget(target);
+
+    units::meter_t x = distance.first * units::math::cos(distance.second);
+    units::meter_t y = distance.first * units::math::sin(distance.second);
+
+    frc::Pose2d pose = frc::Pose2d(x, y, distance.second);
+
+    swerveDrive->SetPose(pose);
+
+    return pose;
+};
\ No newline at end of file
diff --git a/src/main/deploy/fmap.fmap b/src/main/deploy/fmap.fmap
new file mode 100644
index 00000000..fa9e481b
--- /dev/null
+++ b/src/main/deploy/fmap.fmap
@@ -0,0 +1,388 @@
+{
+    "fiducials": [
+        {
+            "family": "apriltag3_36h11_classic",
+            "id": 1,
+            "size": 165.1,
+            "transform": [
+                -0.5,
+                -0.866025,
+                0,
+                6.808597,
+                0.866025,
+                -0.5,
+                0,
+                -3.859403,
+                0,
+                0,
+                1,
+                1.355852,
+                0,
+                0,
+                0,
+                1
+            ],
+            "unique": 1
+        },
+        {
+            "family": "apriltag3_36h11_classic",
+            "id": 2,
+            "size": 165.1,
+            "transform": [
+                -0.5,
+                -0.866025,
+                0,
+                7.914259,
+                0.866025,
+                -0.5,
+                0,
+                -3.221609,
+                0,
+                0,
+                1,
+                1.355852,
+                0,
+                0,
+                0,
+                1
+            ],
+            "unique": 1
+        },
+        {
+            "family": "apriltag3_36h11_classic",
+            "id": 3,
+            "size": 165.1,
+            "transform": [
+                -1.0,
+                0,
+                0,
+                8.308467,
+                0,
+                -1.0,
+                0,
+                0.877443,
+                0,
+                0,
+                1,
+                1.451102,
+                0,
+                0,
+                0,
+                1
+            ],
+            "unique": 1
+        },
+        {
+            "family": "apriltag3_36h11_classic",
+            "id": 4,
+            "size": 165.1,
+            "transform": [
+                -1.0,
+                0,
+                0,
+                8.308467,
+                0,
+                -1.0,
+                0,
+                1.442593,
+                0,
+                0,
+                1,
+                1.451102,
+                0,
+                0,
+                0,
+                1
+            ],
+            "unique": 1
+        },
+        {
+            "family": "apriltag3_36h11_classic",
+            "id": 5,
+            "size": 165.1,
+            "transform": [
+                0,
+                1.0,
+                0,
+                6.429883,
+                -1.0,
+                0,
+                0,
+                4.098925,
+                0,
+                0,
+                1,
+                1.355852,
+                0,
+                0,
+                0,
+                1
+            ],
+            "unique": 1
+        },
+        {
+            "family": "apriltag3_36h11_classic",
+            "id": 6,
+            "size": 165.1,
+            "transform": [
+                0,
+                1.0,
+                0,
+                -6.429375,
+                -1.0,
+                0,
+                0,
+                4.098925,
+                0,
+                0,
+                1,
+                1.355852,
+                0,
+                0,
+                0,
+                1
+            ],
+            "unique": 1
+        },
+        {
+            "family": "apriltag3_36h11_classic",
+            "id": 7,
+            "size": 165.1,
+            "transform": [
+                1.0,
+                -0.0,
+                0,
+                -8.308975,
+                0.0,
+                1.0,
+                0,
+                1.442593,
+                0,
+                0,
+                1,
+                1.451102,
+                0,
+                0,
+                0,
+                1
+            ],
+            "unique": 1
+        },
+        {
+            "family": "apriltag3_36h11_classic",
+            "id": 8,
+            "size": 165.1,
+            "transform": [
+                1.0,
+                -0.0,
+                0,
+                -8.308975,
+                0.0,
+                1.0,
+                0,
+                0.877443,
+                0,
+                0,
+                1,
+                1.451102,
+                0,
+                0,
+                0,
+                1
+            ],
+            "unique": 1
+        },
+        {
+            "family": "apriltag3_36h11_classic",
+            "id": 9,
+            "size": 165.1,
+            "transform": [
+                0.5,
+                -0.866025,
+                0,
+                -7.914767,
+                0.866025,
+                0.5,
+                0,
+                -3.221609,
+                0,
+                0,
+                1,
+                1.355852,
+                0,
+                0,
+                0,
+                1
+            ],
+            "unique": 1
+        },
+        {
+            "family": "apriltag3_36h11_classic",
+            "id": 10,
+            "size": 165.1,
+            "transform": [
+                0.5,
+                -0.866025,
+                0,
+                -6.809359,
+                0.866025,
+                0.5,
+                0,
+                -3.859403,
+                0,
+                0,
+                1,
+                1.355852,
+                0,
+                0,
+                0,
+                1
+            ],
+            "unique": 1
+        },
+        {
+            "family": "apriltag3_36h11_classic",
+            "id": 11,
+            "size": 165.1,
+            "transform": [
+                0.5,
+                0.8660254,
+                0,
+                3.633851,
+                -0.8660254,
+                0.5,
+                0,
+                -0.392049,
+                0,
+                0,
+                1,
+                1.3208,
+                0,
+                0,
+                0,
+                1
+            ],
+            "unique": 1
+        },
+        {
+            "family": "apriltag3_36h11_classic",
+            "id": 12,
+            "size": 165.1,
+            "transform": [
+                0.5,
+                -0.866025,
+                0,
+                3.633851,
+                0.866025,
+                0.5,
+                0,
+                0.393065,
+                0,
+                0,
+                1,
+                1.3208,
+                0,
+                0,
+                0,
+                1
+            ],
+            "unique": 1
+        },
+        {
+            "family": "apriltag3_36h11_classic",
+            "id": 13,
+            "size": 165.1,
+            "transform": [
+                -1.0,
+                0,
+                0,
+                2.949321,
+                0,
+                -1.0,
+                0,
+                -0.000127,
+                0,
+                0,
+                1,
+                1.3208,
+                0,
+                0,
+                0,
+                1
+            ],
+            "unique": 1
+        },
+        {
+            "family": "apriltag3_36h11_classic",
+            "id": 14,
+            "size": 165.1,
+            "transform": [
+                1.0,
+                -0.0,
+                0,
+                -2.950083,
+                0.0,
+                1.0,
+                0,
+                -0.000127,
+                0,
+                0,
+                1,
+                1.3208,
+                0,
+                0,
+                0,
+                1
+            ],
+            "unique": 1
+        },
+        {
+            "family": "apriltag3_36h11_classic",
+            "id": 15,
+            "size": 165.1,
+            "transform": [
+                -0.5,
+                -0.866025,
+                0,
+                -3.629533,
+                0.866025,
+                -0.5,
+                0,
+                0.393065,
+                0,
+                0,
+                1,
+                1.3208,
+                0,
+                0,
+                0,
+                1
+            ],
+            "unique": 1
+        },
+        {
+            "family": "apriltag3_36h11_classic",
+            "id": 16,
+            "size": 165.1,
+            "transform": [
+                -0.5,
+                0.866025,
+                0,
+                -3.629533,
+                -0.866025,
+                -0.5,
+                0,
+                -0.392049,
+                0,
+                0,
+                1,
+                1.3208,
+                0,
+                0,
+                0,
+                1
+            ],
+            "unique": 1
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/include/Robot.h b/src/main/include/Robot.h
index c02d71a7..9cfc4078 100644
--- a/src/main/include/Robot.h
+++ b/src/main/include/Robot.h
@@ -20,8 +20,12 @@
 #include "Intake.h"
 #include "IntakeBehaviour.h"
 #include "RobotMap.h"
+
 #include "Shooter.h"
 #include "ShooterBehaviour.h"
+
+#include "vision/Vision.h"
+
 #include "Wombat.h"
 
 class Robot : public frc::TimedRobot {
@@ -54,10 +58,14 @@ class Robot : public frc::TimedRobot {
 
   wom::SwerveDrive* _swerveDrive;
 
+
   AlphaArm* alphaArm;
 
   // ctre::phoenix6::hardware::TalonFX *frontLeft;
   // ctre::phoenix6::hardware::TalonFX *frontRight;
   // ctre::phoenix6::hardware::TalonFX *backLeft;
   // ctre::phoenix6::hardware::TalonFX *backRight;
+
+  Vision* _vision;
+
 };
diff --git a/src/main/include/vision/Vision.h b/src/main/include/vision/Vision.h
new file mode 100644
index 00000000..7faeb30a
--- /dev/null
+++ b/src/main/include/vision/Vision.h
@@ -0,0 +1,56 @@
+#pragma once
+
+#include <units/length.h>
+#include <string>
+#include "Wombat.h"
+#include <frc/Filesystem.h>
+#include <wpi/fs.h>
+#include <wpi/json.h>
+#include <units/math.h>
+
+#define APRILTAGS_MAX 16
+#define APRILTAGS_MIN 0
+
+enum class VisionTarget {
+    kNote = 0,
+    kAmp = 1,
+    kSource = 2,
+    kNone = 3
+};
+
+struct AprilTag {
+    int id;
+    double size;
+    std::array<std::array<double, 4>, 4> transform;
+    bool unique;
+};
+
+class FMAP {
+    public:
+        FMAP(std::string path);
+
+        std::vector<AprilTag> GetTags();
+
+    private:
+        std::vector<AprilTag> _tags;
+        std::string _path;
+        std::string deploy_dir;
+};
+
+class Vision {
+    public:
+        Vision(std::string name, FMAP fmap);
+
+        std::pair<units::meter_t, units::degree_t> GetDistanceToTarget(VisionTarget target);
+        std::pair<units::meter_t, units::degree_t> GetDistanceToTarget(int id);
+
+        frc::Pose3d GetPose();
+
+        frc::Pose2d AlignToTarget(VisionTarget target, wom::SwerveDrive* swerveDrive);
+
+        std::vector<AprilTag> GetTags();
+
+    private:
+        wom::Limelight* _limelight;    
+        FMAP _fmap;
+};
\ No newline at end of file
diff --git a/wombat/src/main/cpp/vision/Limelight.cpp b/wombat/src/main/cpp/vision/Limelight.cpp
index 906f92cb..45bdcdc6 100644
--- a/wombat/src/main/cpp/vision/Limelight.cpp
+++ b/wombat/src/main/cpp/vision/Limelight.cpp
@@ -186,4 +186,4 @@ bool wom::vision::Limelight::IsAtSetPoseVision(frc::Pose3d pose, units::second_t
   frc::Pose3d relativePose = actualPose.RelativeTo(pose);
   return (units::math::fabs(relativePose.X()) < 0.01_m && units::math::fabs(relativePose.Y()) < 0.01_m &&
           GetSpeed(pose, GetPose(), dt) < 1_m / 1_s);
-}
+}
\ No newline at end of file

From 1b6d5042a73fa674fedf3439ea10bf2518c99aee Mon Sep 17 00:00:00 2001
From: Paul Hodges <paul.hodges@student.education.wa.edu.au>
Date: Thu, 25 Jan 2024 14:50:18 +0800
Subject: [PATCH 106/207] [robot/vision] Finished vision (needs testing)

---
 src/main/cpp/vision/Vision.cpp             | 34 +++++++++++++++++
 src/main/include/vision/Vision.h           | 44 +++++++++++++++++++---
 wombat/src/main/cpp/vision/Limelight.cpp   |  4 ++
 wombat/src/main/include/vision/Limelight.h |  2 +
 4 files changed, 79 insertions(+), 5 deletions(-)

diff --git a/src/main/cpp/vision/Vision.cpp b/src/main/cpp/vision/Vision.cpp
index 4d3fee35..9a359b24 100644
--- a/src/main/cpp/vision/Vision.cpp
+++ b/src/main/cpp/vision/Vision.cpp
@@ -72,6 +72,8 @@ frc::Pose3d Vision::GetPose() {
 };
 
 std::pair<units::meter_t, units::degree_t> Vision::GetDistanceToTarget(VisionTarget target) {
+    SetMode(VisionModes::kAprilTag);
+
     std::vector<AprilTag> tags = _fmap.GetTags();
 
     frc::Pose3d pose = _limelight->GetPose();
@@ -94,6 +96,8 @@ std::pair<units::meter_t, units::degree_t> Vision::GetDistanceToTarget(VisionTar
 
 std::pair<units::meter_t, units::degree_t> Vision::GetDistanceToTarget(int id) {
     if (id < APRILTAGS_MIN || id > APRILTAGS_MAX) { return std::make_pair(0_m, 0_deg); };
+
+    SetMode(VisionModes::kAprilTag);
     
     std::vector<AprilTag> tags = _fmap.GetTags();
 
@@ -119,6 +123,8 @@ std::vector<AprilTag> Vision::GetTags() {
 };
 
 frc::Pose2d Vision::AlignToTarget(VisionTarget target, wom::SwerveDrive* swerveDrive) {
+    SetMode(VisionModes::kAprilTag);
+
     std::pair<units::meter_t, units::degree_t> distance = GetDistanceToTarget(target);
 
     units::meter_t x = distance.first * units::math::cos(distance.second);
@@ -129,4 +135,32 @@ frc::Pose2d Vision::AlignToTarget(VisionTarget target, wom::SwerveDrive* swerveD
     swerveDrive->SetPose(pose);
 
     return pose;
+};
+
+bool Vision::IsAtPose(frc::Pose3d pose, units::second_t dt) {
+    return _limelight->IsAtSetPoseVision(pose, dt);
+};
+
+void Vision::SetMode(VisionModes mode) {
+    switch (mode) {
+        case VisionModes::kAprilTag:
+        {
+            _limelight->SetPipeline(wom::LimelightPipeline::kPipeline0);
+        }
+        case VisionModes::kRing:
+        {
+            _limelight->SetPipeline(wom::LimelightPipeline::kPipeline1);
+        }
+    }
+};
+
+bool Vision::TargetIsVisible(VisionTargetObjects target) {
+    switch (target) {
+        case VisionTargetObjects::kNote:
+        {
+            SetMode(VisionModes::kRing);
+        }
+    }
+
+    return _limelight->HasTarget();
 };
\ No newline at end of file
diff --git a/src/main/include/vision/Vision.h b/src/main/include/vision/Vision.h
index 7faeb30a..9f3bebff 100644
--- a/src/main/include/vision/Vision.h
+++ b/src/main/include/vision/Vision.h
@@ -11,11 +11,38 @@
 #define APRILTAGS_MAX 16
 #define APRILTAGS_MIN 0
 
-enum class VisionTarget {
-    kNote = 0,
-    kAmp = 1,
-    kSource = 2,
-    kNone = 3
+enum class VisionTarget { 
+    /* 
+        Left is toward the blue side of the diagram here: https://firstfrc.blob.core.windows.net/frc2024/FieldAssets/2024LayoutMarkingDiagram.pdf 
+
+        The numbers are the numbers on the field diagram (and the numbers on the tags).
+    */
+    kBlueAmp = 6,
+    kBlueSpeakerCenter = 7,
+    kBlueSpeakerOffset = 8,
+    kBlueChain1 = 15,
+    kBlueChain2 = 16,
+    kBlueChain3 = 14,
+    kBlueSourceLeft = 1,
+    kBlueSourceRight = 2,
+
+    kRedAmp = 5,
+    kRedSpeakerCenter = 4,
+    kRedSpeakerOffset = 3,
+    kRedChain1 = 12,
+    kRedChain2 = 11,
+    kRedChain3 = 13,
+    kRedSourceLeft = 9,
+    kRedSourceRight = 10
+};
+
+enum class VisionModes {
+    kAprilTag = 0,
+    kRing = 1
+};
+
+enum class VisionTargetObjects {
+    kNote
 };
 
 struct AprilTag {
@@ -44,13 +71,20 @@ class Vision {
         std::pair<units::meter_t, units::degree_t> GetDistanceToTarget(VisionTarget target);
         std::pair<units::meter_t, units::degree_t> GetDistanceToTarget(int id);
 
+        void SetMode(VisionModes mode);
+
         frc::Pose3d GetPose();
 
         frc::Pose2d AlignToTarget(VisionTarget target, wom::SwerveDrive* swerveDrive);
 
         std::vector<AprilTag> GetTags();
 
+        bool IsAtPose(frc::Pose3d pose, units::second_t dt);
+
+        bool TargetIsVisible(VisionTargetObjects target);
+
     private:
         wom::Limelight* _limelight;    
         FMAP _fmap;
+        VisionModes _mode = VisionModes::kAprilTag;
 };
\ No newline at end of file
diff --git a/wombat/src/main/cpp/vision/Limelight.cpp b/wombat/src/main/cpp/vision/Limelight.cpp
index 45bdcdc6..f4ef274a 100644
--- a/wombat/src/main/cpp/vision/Limelight.cpp
+++ b/wombat/src/main/cpp/vision/Limelight.cpp
@@ -186,4 +186,8 @@ bool wom::vision::Limelight::IsAtSetPoseVision(frc::Pose3d pose, units::second_t
   frc::Pose3d relativePose = actualPose.RelativeTo(pose);
   return (units::math::fabs(relativePose.X()) < 0.01_m && units::math::fabs(relativePose.Y()) < 0.01_m &&
           GetSpeed(pose, GetPose(), dt) < 1_m / 1_s);
+}
+
+bool wom::vision::Limelight::HasTarget() {
+  return GetTargetingData(LimelightTargetingData::kTv) == 1.0;
 }
\ No newline at end of file
diff --git a/wombat/src/main/include/vision/Limelight.h b/wombat/src/main/include/vision/Limelight.h
index 5aa6d0b7..77b0c681 100644
--- a/wombat/src/main/include/vision/Limelight.h
+++ b/wombat/src/main/include/vision/Limelight.h
@@ -104,6 +104,8 @@ class Limelight {
 
   frc::Pose3d GetPose();
 
+  bool HasTarget();
+
  private:
   std::string _limelightName;
 };

From c89d969c3ab2f704696149a8bc0e70f738a190cf Mon Sep 17 00:00:00 2001
From: Paul Hodges <paul.hodges@student.education.wa.edu.au>
Date: Thu, 25 Jan 2024 15:04:56 +0800
Subject: [PATCH 107/207] [wpiformat] Run wpiformat

---
 src/main/cpp/Robot.cpp                   |   5 +
 src/main/cpp/vision/Vision.cpp           | 235 ++++++++++++-----------
 src/main/deploy/fmap.fmap                |   2 +-
 src/main/include/Robot.h                 |   2 +
 src/main/include/vision/Vision.h         | 124 ++++++------
 wombat/src/main/cpp/vision/Limelight.cpp |   2 +-
 6 files changed, 194 insertions(+), 176 deletions(-)

diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp
index 3057ebe7..348be1b8 100644
--- a/src/main/cpp/Robot.cpp
+++ b/src/main/cpp/Robot.cpp
@@ -63,6 +63,7 @@ void Robot::RobotInit() {
   // m_driveSim = new wom::TempSimSwerveDrive(&simulation_timer, &m_field);
   // m_driveSim = wom::TempSimSwerveDrive();
 
+<<<<<<< HEAD
   alphaArm = new AlphaArm(robotmap.alphaArmSystem.config);
   wom::BehaviourScheduler::GetInstance()->Register(alphaArm);
   alphaArm->SetDefaultBehaviour(
@@ -90,6 +91,8 @@ void Robot::RobotInit() {
 
   //_vision = new Vision("limelight", FMAP("fmap.fmap"));
 
+  _vision = new Vision("limelight", FMAP("fmap.fmap"));
+
 }
 
 void Robot::RobotPeriodic() {
@@ -128,6 +131,7 @@ void Robot::AutonomousPeriodic() {}
 
 void Robot::TeleopInit() {
 
+
   loop.Clear();
   wom::BehaviourScheduler* sched = wom::BehaviourScheduler::GetInstance();
   sched->InterruptAll();
@@ -139,6 +143,7 @@ void Robot::TeleopInit() {
   
 
 //  FMAP("fmap.fmap");
+
   // _swerveDrive->OnStart();
   // sched->InterruptAll();
 
diff --git a/src/main/cpp/vision/Vision.cpp b/src/main/cpp/vision/Vision.cpp
index 9a359b24..2c7285c3 100644
--- a/src/main/cpp/vision/Vision.cpp
+++ b/src/main/cpp/vision/Vision.cpp
@@ -1,166 +1,173 @@
-#include "vision/Vision.h"
+// Copyright (c) 2023-2024 CurtinFRC
+// Open Source Software, you can modify it according to the terms
+// of the MIT License at the root of this project
 
+#include "vision/Vision.h"
 
 FMAP::FMAP(std::string path) : _path(path) {
-    std::cout << "Parsing FMAP" << std::endl;
-
-    deploy_dir = frc::filesystem::GetDeployDirectory();
-
-    std::ifstream file(deploy_dir + "/" + _path);
-
-    // parse the json file into a wpi::json object
-
-    wpi::json j;
-
-    file >> j;
-
-    // iterate through the json object and add each tag to the vector
-
-    for (auto& element : j["fiducials"]) {
-        AprilTag tag;
-
-        tag.id = element["id"];
-        tag.size = element["size"];
-        tag.transform[0][0] = element["transform"][0];
-        tag.transform[0][1] = element["transform"][1];
-        tag.transform[0][2] = element["transform"][2];
-        tag.transform[0][3] = element["transform"][3];
-        tag.transform[1][0] = element["transform"][4];
-        tag.transform[1][1] = element["transform"][5];
-        tag.transform[1][2] = element["transform"][6];
-        tag.transform[1][3] = element["transform"][7];
-        tag.transform[2][0] = element["transform"][8];
-        tag.transform[2][1] = element["transform"][9];
-        tag.transform[2][2] = element["transform"][10];
-        tag.transform[2][3] = element["transform"][11];
-        tag.transform[3][0] = element["transform"][12];
-        tag.transform[3][1] = element["transform"][13];
-        tag.transform[3][2] = element["transform"][14];
-        tag.transform[3][3] = element["transform"][15];
-        if (element["unique"] == 1) {
-            tag.unique = true;
-        } else {
-            tag.unique = false;
-        }
-
-        _tags.push_back(tag);
+  std::cout << "Parsing FMAP" << std::endl;
+
+  deploy_dir = frc::filesystem::GetDeployDirectory();
+
+  std::ifstream file(deploy_dir + "/" + _path);
+
+  // parse the json file into a wpi::json object
+
+  wpi::json j;
+
+  file >> j;
+
+  // iterate through the json object and add each tag to the vector
+
+  for (auto& element : j["fiducials"]) {
+    AprilTag tag;
+
+    tag.id = element["id"];
+    tag.size = element["size"];
+    tag.transform[0][0] = element["transform"][0];
+    tag.transform[0][1] = element["transform"][1];
+    tag.transform[0][2] = element["transform"][2];
+    tag.transform[0][3] = element["transform"][3];
+    tag.transform[1][0] = element["transform"][4];
+    tag.transform[1][1] = element["transform"][5];
+    tag.transform[1][2] = element["transform"][6];
+    tag.transform[1][3] = element["transform"][7];
+    tag.transform[2][0] = element["transform"][8];
+    tag.transform[2][1] = element["transform"][9];
+    tag.transform[2][2] = element["transform"][10];
+    tag.transform[2][3] = element["transform"][11];
+    tag.transform[3][0] = element["transform"][12];
+    tag.transform[3][1] = element["transform"][13];
+    tag.transform[3][2] = element["transform"][14];
+    tag.transform[3][3] = element["transform"][15];
+    if (element["unique"] == 1) {
+      tag.unique = true;
+    } else {
+      tag.unique = false;
     }
 
-    file.close();
+    _tags.push_back(tag);
+  }
 
-    std::cout << "Loaded " << _tags.size() << " tags" << std::endl;
+  file.close();
 
-    for (int i = 0; i < _tags.size(); i++) {
-        std::cout << "Tag " << _tags[i].id << " is " << _tags[i].size << "m" << std::endl;
-    }
+  std::cout << "Loaded " << _tags.size() << " tags" << std::endl;
 
-    std::cout << "Loaded FMAP" << std::endl;
-};
+  for (int i = 0; i < _tags.size(); i++) {
+    std::cout << "Tag " << _tags[i].id << " is " << _tags[i].size << "m" << std::endl;
+  }
+
+  std::cout << "Loaded FMAP" << std::endl;
+}
 
 std::vector<AprilTag> FMAP::GetTags() {
-    return _tags;
-};
+  return _tags;
+}
 
 Vision::Vision(std::string name, FMAP fmap) : _fmap(fmap) {
-    _limelight = new wom::Limelight(name);
+  _limelight = new wom::Limelight(name);
 
-    _limelight->SetPipeline(wom::LimelightPipeline::kPipeline0);
-};
+  _limelight->SetPipeline(wom::LimelightPipeline::kPipeline0);
+}
 
 frc::Pose3d Vision::GetPose() {
-    return _limelight->GetPose();
-};
+  return _limelight->GetPose();
+}
 
 std::pair<units::meter_t, units::degree_t> Vision::GetDistanceToTarget(VisionTarget target) {
-    SetMode(VisionModes::kAprilTag);
+  SetMode(VisionModes::kAprilTag);
 
-    std::vector<AprilTag> tags = _fmap.GetTags();
+  std::vector<AprilTag> tags = _fmap.GetTags();
 
-    frc::Pose3d pose = _limelight->GetPose();
+  frc::Pose3d pose = _limelight->GetPose();
 
-    for (int i = 0; i < tags.size(); i++) {
-        if (tags[i].id == (int)target) {         
+  for (int i = 0; i < tags.size(); i++) {
+    if (tags[i].id == static_cast<int>(target)) {
+      frc::Pose3d aprilTagPos =
+          frc::Pose3d(tags[i].transform[0][3] * 1_m, tags[i].transform[1][3] * 1_m,
+                      tags[i].transform[2][3] * 1_m, frc::Rotation3d(0_deg, 0_deg, 0_deg));
 
-            frc::Pose3d aprilTagPos = frc::Pose3d(tags[i].transform[0][3] * 1_m, tags[i].transform[1][3] * 1_m, tags[i].transform[2][3] * 1_m, frc::Rotation3d(0_deg, 0_deg, 0_deg));   
-            
-            units::meter_t distance = pose.Translation().Distance(aprilTagPos.Translation());
+      units::meter_t distance = pose.Translation().Distance(aprilTagPos.Translation());
 
-            units::degree_t angle = units::math::atan((pose.X() - units::meter_t{tags[i].transform[0][3]}) / (pose.Y() - units::meter_t{tags[i].transform[1][3]}));
+      units::degree_t angle = units::math::atan((pose.X() - units::meter_t{tags[i].transform[0][3]}) /
+                                                (pose.Y() - units::meter_t{tags[i].transform[1][3]}));
 
-            return std::make_pair(distance, angle);
-        }
+      return std::make_pair(distance, angle);
     }
+  }
 
-    return std::make_pair(0_m, 0_deg);
-};
+  return std::make_pair(0_m, 0_deg);
+}
 
 std::pair<units::meter_t, units::degree_t> Vision::GetDistanceToTarget(int id) {
-    if (id < APRILTAGS_MIN || id > APRILTAGS_MAX) { return std::make_pair(0_m, 0_deg); };
+  if (id < APRILTAGS_MIN || id > APRILTAGS_MAX) {
+    return std::make_pair(0_m, 0_deg);
+  }
+
+  SetMode(VisionModes::kAprilTag);
 
-    SetMode(VisionModes::kAprilTag);
-    
-    std::vector<AprilTag> tags = _fmap.GetTags();
+  std::vector<AprilTag> tags = _fmap.GetTags();
 
-    for (int i = 0; i < tags.size(); i++) {
-        if (tags[i].id == id) {
-            // get distance to the limelight
-            frc::Pose3d currentPose = _limelight->GetPose();
-            frc::Pose3d aprilTagPos = frc::Pose3d(tags[i].transform[0][3] * 1_m, tags[i].transform[1][3] * 1_m, tags[i].transform[2][3] * 1_m, frc::Rotation3d(0_deg, 0_deg, 0_deg));
+  for (int i = 0; i < tags.size(); i++) {
+    if (tags[i].id == id) {
+      // get distance to the limelight
+      frc::Pose3d currentPose = _limelight->GetPose();
+      frc::Pose3d aprilTagPos =
+          frc::Pose3d(tags[i].transform[0][3] * 1_m, tags[i].transform[1][3] * 1_m,
+                      tags[i].transform[2][3] * 1_m, frc::Rotation3d(0_deg, 0_deg, 0_deg));
 
-            units::meter_t distance = currentPose.Translation().Distance(aprilTagPos.Translation());
+      units::meter_t distance = currentPose.Translation().Distance(aprilTagPos.Translation());
 
-            units::degree_t angle = units::math::atan((currentPose.X() - units::meter_t{tags[i].transform[0][3]}) / (currentPose.Y() - units::meter_t{tags[i].transform[1][3]}));
+      units::degree_t angle = units::math::atan((currentPose.X() - units::meter_t{tags[i].transform[0][3]}) /
+                                                (currentPose.Y() - units::meter_t{tags[i].transform[1][3]}));
 
-            return std::make_pair(distance, angle);
-        }
+      return std::make_pair(distance, angle);
     }
+  }
 
-    return std::make_pair(0_m, 0_deg);
-};
+  return std::make_pair(0_m, 0_deg);
+}
 
 std::vector<AprilTag> Vision::GetTags() {
-    return _fmap.GetTags();
-};
+  return _fmap.GetTags();
+}
 
 frc::Pose2d Vision::AlignToTarget(VisionTarget target, wom::SwerveDrive* swerveDrive) {
-    SetMode(VisionModes::kAprilTag);
+  SetMode(VisionModes::kAprilTag);
 
-    std::pair<units::meter_t, units::degree_t> distance = GetDistanceToTarget(target);
+  std::pair<units::meter_t, units::degree_t> distance = GetDistanceToTarget(target);
 
-    units::meter_t x = distance.first * units::math::cos(distance.second);
-    units::meter_t y = distance.first * units::math::sin(distance.second);
+  units::meter_t x = distance.first * units::math::cos(distance.second);
+  units::meter_t y = distance.first * units::math::sin(distance.second);
 
-    frc::Pose2d pose = frc::Pose2d(x, y, distance.second);
+  frc::Pose2d pose = frc::Pose2d(x, y, distance.second);
 
-    swerveDrive->SetPose(pose);
+  swerveDrive->SetPose(pose);
 
-    return pose;
-};
+  return pose;
+}
 
 bool Vision::IsAtPose(frc::Pose3d pose, units::second_t dt) {
-    return _limelight->IsAtSetPoseVision(pose, dt);
-};
+  return _limelight->IsAtSetPoseVision(pose, dt);
+}
 
 void Vision::SetMode(VisionModes mode) {
-    switch (mode) {
-        case VisionModes::kAprilTag:
-        {
-            _limelight->SetPipeline(wom::LimelightPipeline::kPipeline0);
-        }
-        case VisionModes::kRing:
-        {
-            _limelight->SetPipeline(wom::LimelightPipeline::kPipeline1);
-        }
+  switch (mode) {
+    case VisionModes::kAprilTag: {
+      _limelight->SetPipeline(wom::LimelightPipeline::kPipeline0);
+    }
+    case VisionModes::kRing: {
+      _limelight->SetPipeline(wom::LimelightPipeline::kPipeline1);
     }
-};
+  }
+}
 
 bool Vision::TargetIsVisible(VisionTargetObjects target) {
-    switch (target) {
-        case VisionTargetObjects::kNote:
-        {
-            SetMode(VisionModes::kRing);
-        }
+  switch (target) {
+    case VisionTargetObjects::kNote: {
+      SetMode(VisionModes::kRing);
     }
+  }
 
-    return _limelight->HasTarget();
-};
\ No newline at end of file
+  return _limelight->HasTarget();
+}
diff --git a/src/main/deploy/fmap.fmap b/src/main/deploy/fmap.fmap
index fa9e481b..9cb43e6d 100644
--- a/src/main/deploy/fmap.fmap
+++ b/src/main/deploy/fmap.fmap
@@ -385,4 +385,4 @@
             "unique": 1
         }
     ]
-}
\ No newline at end of file
+}
diff --git a/src/main/include/Robot.h b/src/main/include/Robot.h
index 9cfc4078..e6abd21e 100644
--- a/src/main/include/Robot.h
+++ b/src/main/include/Robot.h
@@ -21,6 +21,7 @@
 #include "IntakeBehaviour.h"
 #include "RobotMap.h"
 
+
 #include "Shooter.h"
 #include "ShooterBehaviour.h"
 
@@ -28,6 +29,7 @@
 
 #include "Wombat.h"
 
+
 class Robot : public frc::TimedRobot {
  public:
   void TestInit() override;
diff --git a/src/main/include/vision/Vision.h b/src/main/include/vision/Vision.h
index 9f3bebff..2271a7ed 100644
--- a/src/main/include/vision/Vision.h
+++ b/src/main/include/vision/Vision.h
@@ -1,90 +1,94 @@
+// Copyright (c) 2023-2024 CurtinFRC
+// Open Source Software, you can modify it according to the terms
+// of the MIT License at the root of this project
+
 #pragma once
 
-#include <units/length.h>
-#include <string>
-#include "Wombat.h"
 #include <frc/Filesystem.h>
+#include <units/length.h>
+#include <units/math.h>
 #include <wpi/fs.h>
 #include <wpi/json.h>
-#include <units/math.h>
+
+#include <string>
+#include <utility>
+#include <vector>
+
+#include "Wombat.h"
 
 #define APRILTAGS_MAX 16
 #define APRILTAGS_MIN 0
 
-enum class VisionTarget { 
-    /* 
-        Left is toward the blue side of the diagram here: https://firstfrc.blob.core.windows.net/frc2024/FieldAssets/2024LayoutMarkingDiagram.pdf 
-
-        The numbers are the numbers on the field diagram (and the numbers on the tags).
-    */
-    kBlueAmp = 6,
-    kBlueSpeakerCenter = 7,
-    kBlueSpeakerOffset = 8,
-    kBlueChain1 = 15,
-    kBlueChain2 = 16,
-    kBlueChain3 = 14,
-    kBlueSourceLeft = 1,
-    kBlueSourceRight = 2,
-
-    kRedAmp = 5,
-    kRedSpeakerCenter = 4,
-    kRedSpeakerOffset = 3,
-    kRedChain1 = 12,
-    kRedChain2 = 11,
-    kRedChain3 = 13,
-    kRedSourceLeft = 9,
-    kRedSourceRight = 10
+enum class VisionTarget {
+  /*
+      Left is toward the blue side of the diagram here:
+     https://firstfrc.blob.core.windows.net/frc2024/FieldAssets/2024LayoutMarkingDiagram.pdf
+
+      The numbers are the numbers on the field diagram (and the numbers on the tags).
+  */
+  kBlueAmp = 6,
+  kBlueSpeakerCenter = 7,
+  kBlueSpeakerOffset = 8,
+  kBlueChain1 = 15,
+  kBlueChain2 = 16,
+  kBlueChain3 = 14,
+  kBlueSourceLeft = 1,
+  kBlueSourceRight = 2,
+
+  kRedAmp = 5,
+  kRedSpeakerCenter = 4,
+  kRedSpeakerOffset = 3,
+  kRedChain1 = 12,
+  kRedChain2 = 11,
+  kRedChain3 = 13,
+  kRedSourceLeft = 9,
+  kRedSourceRight = 10
 };
 
-enum class VisionModes {
-    kAprilTag = 0,
-    kRing = 1
-};
+enum class VisionModes { kAprilTag = 0, kRing = 1 };
 
-enum class VisionTargetObjects {
-    kNote
-};
+enum class VisionTargetObjects { kNote };
 
 struct AprilTag {
-    int id;
-    double size;
-    std::array<std::array<double, 4>, 4> transform;
-    bool unique;
+  int id;
+  double size;
+  std::array<std::array<double, 4>, 4> transform;
+  bool unique;
 };
 
 class FMAP {
-    public:
-        FMAP(std::string path);
+ public:
+  explicit FMAP(std::string path);
 
-        std::vector<AprilTag> GetTags();
+  std::vector<AprilTag> GetTags();
 
-    private:
-        std::vector<AprilTag> _tags;
-        std::string _path;
-        std::string deploy_dir;
+ private:
+  std::vector<AprilTag> _tags;
+  std::string _path;
+  std::string deploy_dir;
 };
 
 class Vision {
-    public:
-        Vision(std::string name, FMAP fmap);
+ public:
+  Vision(std::string name, FMAP fmap);
 
-        std::pair<units::meter_t, units::degree_t> GetDistanceToTarget(VisionTarget target);
-        std::pair<units::meter_t, units::degree_t> GetDistanceToTarget(int id);
+  std::pair<units::meter_t, units::degree_t> GetDistanceToTarget(VisionTarget target);
+  std::pair<units::meter_t, units::degree_t> GetDistanceToTarget(int id);
 
-        void SetMode(VisionModes mode);
+  void SetMode(VisionModes mode);
 
-        frc::Pose3d GetPose();
+  frc::Pose3d GetPose();
 
-        frc::Pose2d AlignToTarget(VisionTarget target, wom::SwerveDrive* swerveDrive);
+  frc::Pose2d AlignToTarget(VisionTarget target, wom::SwerveDrive* swerveDrive);
 
-        std::vector<AprilTag> GetTags();
+  std::vector<AprilTag> GetTags();
 
-        bool IsAtPose(frc::Pose3d pose, units::second_t dt);
+  bool IsAtPose(frc::Pose3d pose, units::second_t dt);
 
-        bool TargetIsVisible(VisionTargetObjects target);
+  bool TargetIsVisible(VisionTargetObjects target);
 
-    private:
-        wom::Limelight* _limelight;    
-        FMAP _fmap;
-        VisionModes _mode = VisionModes::kAprilTag;
-};
\ No newline at end of file
+ private:
+  wom::Limelight* _limelight;
+  FMAP _fmap;
+  VisionModes _mode = VisionModes::kAprilTag;
+};
diff --git a/wombat/src/main/cpp/vision/Limelight.cpp b/wombat/src/main/cpp/vision/Limelight.cpp
index f4ef274a..2b37f22b 100644
--- a/wombat/src/main/cpp/vision/Limelight.cpp
+++ b/wombat/src/main/cpp/vision/Limelight.cpp
@@ -190,4 +190,4 @@ bool wom::vision::Limelight::IsAtSetPoseVision(frc::Pose3d pose, units::second_t
 
 bool wom::vision::Limelight::HasTarget() {
   return GetTargetingData(LimelightTargetingData::kTv) == 1.0;
-}
\ No newline at end of file
+}

From df6481812c73265c638b7d2760013903c69c8e40 Mon Sep 17 00:00:00 2001
From: Paul Hodges <paul.hodges@student.education.wa.edu.au>
Date: Thu, 25 Jan 2024 15:49:31 +0800
Subject: [PATCH 108/207] [robot/vision] Finish vision

---
 src/main/cpp/vision/Vision.cpp   | 7 +++++++
 src/main/include/vision/Vision.h | 2 ++
 2 files changed, 9 insertions(+)

diff --git a/src/main/cpp/vision/Vision.cpp b/src/main/cpp/vision/Vision.cpp
index 2c7285c3..f726c9b1 100644
--- a/src/main/cpp/vision/Vision.cpp
+++ b/src/main/cpp/vision/Vision.cpp
@@ -155,9 +155,11 @@ void Vision::SetMode(VisionModes mode) {
   switch (mode) {
     case VisionModes::kAprilTag: {
       _limelight->SetPipeline(wom::LimelightPipeline::kPipeline0);
+      break;
     }
     case VisionModes::kRing: {
       _limelight->SetPipeline(wom::LimelightPipeline::kPipeline1);
+      break;
     }
   }
 }
@@ -166,8 +168,13 @@ bool Vision::TargetIsVisible(VisionTargetObjects target) {
   switch (target) {
     case VisionTargetObjects::kNote: {
       SetMode(VisionModes::kRing);
+      break;
     }
   }
 
   return _limelight->HasTarget();
 }
+
+int Vision::CurrentAprilTag() {
+  return _limelight->GetAprilTagData(wom::LimelightAprilTagData::kTid)[0];
+}
diff --git a/src/main/include/vision/Vision.h b/src/main/include/vision/Vision.h
index 2271a7ed..5629cc81 100644
--- a/src/main/include/vision/Vision.h
+++ b/src/main/include/vision/Vision.h
@@ -87,6 +87,8 @@ class Vision {
 
   bool TargetIsVisible(VisionTargetObjects target);
 
+  int CurrentAprilTag();
+
  private:
   wom::Limelight* _limelight;
   FMAP _fmap;

From 0242f4a1c9432bee64ed9df451df0dc1f572b11e Mon Sep 17 00:00:00 2001
From: Paul Hodges <paul.hodges@student.education.wa.edu.au>
Date: Sun, 28 Jan 2024 09:26:05 +0800
Subject: [PATCH 109/207] Updated vision

---
 src/main/cpp/Robot.cpp                     |  1 -
 src/main/cpp/vision/Vision.cpp             |  4 +++-
 src/main/include/vision/Vision.h           |  1 -
 wombat/src/main/cpp/vision/Limelight.cpp   | 12 ++++++------
 wombat/src/main/include/vision/Limelight.h |  2 +-
 5 files changed, 10 insertions(+), 10 deletions(-)

diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp
index 348be1b8..d4498c4a 100644
--- a/src/main/cpp/Robot.cpp
+++ b/src/main/cpp/Robot.cpp
@@ -63,7 +63,6 @@ void Robot::RobotInit() {
   // m_driveSim = new wom::TempSimSwerveDrive(&simulation_timer, &m_field);
   // m_driveSim = wom::TempSimSwerveDrive();
 
-<<<<<<< HEAD
   alphaArm = new AlphaArm(robotmap.alphaArmSystem.config);
   wom::BehaviourScheduler::GetInstance()->Register(alphaArm);
   alphaArm->SetDefaultBehaviour(
diff --git a/src/main/cpp/vision/Vision.cpp b/src/main/cpp/vision/Vision.cpp
index f726c9b1..a06236fc 100644
--- a/src/main/cpp/vision/Vision.cpp
+++ b/src/main/cpp/vision/Vision.cpp
@@ -176,5 +176,7 @@ bool Vision::TargetIsVisible(VisionTargetObjects target) {
 }
 
 int Vision::CurrentAprilTag() {
-  return _limelight->GetAprilTagData(wom::LimelightAprilTagData::kTid)[0];
+  SetMode(VisionModes::kAprilTag);
+
+  return _limelight->GetTargetingData(wom::LimelightTargetingData::kTid);
 }
diff --git a/src/main/include/vision/Vision.h b/src/main/include/vision/Vision.h
index 5629cc81..8176b2a2 100644
--- a/src/main/include/vision/Vision.h
+++ b/src/main/include/vision/Vision.h
@@ -92,5 +92,4 @@ class Vision {
  private:
   wom::Limelight* _limelight;
   FMAP _fmap;
-  VisionModes _mode = VisionModes::kAprilTag;
 };
diff --git a/wombat/src/main/cpp/vision/Limelight.cpp b/wombat/src/main/cpp/vision/Limelight.cpp
index 2b37f22b..ed62337e 100644
--- a/wombat/src/main/cpp/vision/Limelight.cpp
+++ b/wombat/src/main/cpp/vision/Limelight.cpp
@@ -58,13 +58,9 @@ std::vector<double> wom::vision::Limelight::GetAprilTagData(LimelightAprilTagDat
     case LimelightAprilTagData::kCamerapose_robotspace:
       dataName = "camerapose_robotspace";
       break;
-
-    case LimelightAprilTagData::kTid:
-      dataName = "tid";
-      break;
   }
 
-  return table->GetNumberArray(dataName, std::vector<double>(6));
+  return table->GetEntry(dataName).GetDoubleArray(std::vector<double>(6));
 }
 
 double wom::vision::Limelight::GetTargetingData(LimelightTargetingData dataType, double defaultValue) {
@@ -126,9 +122,13 @@ double wom::vision::Limelight::GetTargetingData(LimelightTargetingData dataType,
     case LimelightTargetingData::kTc:
       dataName = "tc";
       break;
+
+    case LimelightTargetingData::kTid:
+      dataName = "tid";
+      break;
   }
 
-  return table->GetNumber(dataName, defaultValue);
+  return table->GetEntry(dataName).GetDouble(0);
 }
 
 void wom::vision::Limelight::SetLEDMode(LimelightLEDMode mode) {
diff --git a/wombat/src/main/include/vision/Limelight.h b/wombat/src/main/include/vision/Limelight.h
index 77b0c681..8c21efc3 100644
--- a/wombat/src/main/include/vision/Limelight.h
+++ b/wombat/src/main/include/vision/Limelight.h
@@ -62,6 +62,7 @@ enum class LimelightTargetingData {
   kJson = 11,
   kTclass = 12,
   kTc = 13,
+  kTid = 14
 };
 
 enum class LimelightAprilTagData {
@@ -73,7 +74,6 @@ enum class LimelightAprilTagData {
   kTargetpose_robotspace = 5,
   kBotpose_targetspace = 6,
   kCamerapose_robotspace = 7,
-  kTid = 8
 };
 
 class Limelight {

From 35104bf5b127cd66958d7107fcf86254b371e454 Mon Sep 17 00:00:00 2001
From: Paul Hodges <paul.hodges@student.education.wa.edu.au>
Date: Mon, 29 Jan 2024 10:25:46 +0800
Subject: [PATCH 110/207] More math stuff

---
 ctre_sim/CANCoder vers. H - 016 - 0 - ext.dat | Bin 0 -> 2048 bytes
 ctre_sim/CANCoder vers. H - 017 - 0 - ext.dat | Bin 0 -> 2048 bytes
 ctre_sim/CANCoder vers. H - 018 - 0 - ext.dat | Bin 0 -> 2048 bytes
 ctre_sim/CANCoder vers. H - 019 - 0 - ext.dat | Bin 0 -> 2048 bytes
 ctre_sim/Pigeon 2 - 020 - 0 - ext.dat         | Bin 0 -> 2048 bytes
 ctre_sim/Talon FX vers. C - 01 - 0 - ext.dat  | Bin 0 -> 2048 bytes
 ctre_sim/Talon FX vers. C - 02 - 0 - ext.dat  | Bin 0 -> 2048 bytes
 ctre_sim/Talon FX vers. C - 03 - 0 - ext.dat  | Bin 0 -> 2048 bytes
 ctre_sim/Talon FX vers. C - 04 - 0 - ext.dat  | Bin 0 -> 2048 bytes
 ctre_sim/Talon FX vers. C - 05 - 0 - ext.dat  | Bin 0 -> 2048 bytes
 ctre_sim/Talon FX vers. C - 06 - 0 - ext.dat  | Bin 0 -> 2048 bytes
 ctre_sim/Talon FX vers. C - 07 - 0 - ext.dat  | Bin 0 -> 2048 bytes
 ctre_sim/Talon FX vers. C - 09 - 0 - ext.dat  | Bin 0 -> 2048 bytes
 networktables.json                            |   1 +
 simgui-ds.json                                |  92 ++++++++
 simgui.json                                   |  23 ++
 src/main/cpp/Robot.cpp                        |  10 +
 src/main/cpp/vision/Vision.cpp                | 205 +++++++++++++-----
 src/main/include/vision/Vision.h              |  11 +-
 wombat/src/main/cpp/utils/Encoder.cpp         |   2 +-
 20 files changed, 293 insertions(+), 51 deletions(-)
 create mode 100644 ctre_sim/CANCoder vers. H - 016 - 0 - ext.dat
 create mode 100644 ctre_sim/CANCoder vers. H - 017 - 0 - ext.dat
 create mode 100644 ctre_sim/CANCoder vers. H - 018 - 0 - ext.dat
 create mode 100644 ctre_sim/CANCoder vers. H - 019 - 0 - ext.dat
 create mode 100644 ctre_sim/Pigeon 2 - 020 - 0 - ext.dat
 create mode 100644 ctre_sim/Talon FX vers. C - 01 - 0 - ext.dat
 create mode 100644 ctre_sim/Talon FX vers. C - 02 - 0 - ext.dat
 create mode 100644 ctre_sim/Talon FX vers. C - 03 - 0 - ext.dat
 create mode 100644 ctre_sim/Talon FX vers. C - 04 - 0 - ext.dat
 create mode 100644 ctre_sim/Talon FX vers. C - 05 - 0 - ext.dat
 create mode 100644 ctre_sim/Talon FX vers. C - 06 - 0 - ext.dat
 create mode 100644 ctre_sim/Talon FX vers. C - 07 - 0 - ext.dat
 create mode 100644 ctre_sim/Talon FX vers. C - 09 - 0 - ext.dat
 create mode 100644 networktables.json
 create mode 100644 simgui-ds.json
 create mode 100644 simgui.json

diff --git a/ctre_sim/CANCoder vers. H - 016 - 0 - ext.dat b/ctre_sim/CANCoder vers. H - 016 - 0 - ext.dat
new file mode 100644
index 0000000000000000000000000000000000000000..8f8bccdea82f511e5af738d8fa0b4b911c8bdd34
GIT binary patch
literal 2048
zcmeHIJ!n%=6h1GhnpB4j78R8?5i7cNY9{+Eh(*x8;!?L@21QYvq>9yw6yl&17ZsNb
zDp~|9>Jn65X{=C?DoAU#*e>cI5!6~5`n>0!^My1uTSo74@}2XY@BG}$%Q$rf^#3$)
z12cgWB0Bo(Qu{}v`idsX0|rMLRN*B}md`a%@3r<-yfJi*`yN+3wPl~hv|b$LPMOPd
z#3GM#<a7$(fXnJxGCyKX0xqb}S6$a33e53aq|TUHQa*S?n)m3p>@(3J^?%m%6H^`=
zBTWYtH>+SQtd3TxH~Qg0ANMymOvgDATPtxhPKal62Yq{w=x2KGsO9H~avb;fOWfZe
z_WP!Z1mAZ)EEsxmXUYnrUfjuBJR;?&l*!n=)x%7)JKn1O)Zlzb{K!W?wK${oU=Rn6
z;)mLTzF#8QckZq1%S<kNF+AR1weG)g<#{L{xG*4kz$mZR54g%Mn>WK(=|+mS53f%s
zJ<*l;@veJ2xF4Lkrw{G@op-)nWuLdbVDnuibRTH4t$3>QevRm57v$Z2si{}3&-adl
z{GN8HkNrSzG5TcZuxt8nncv3!WmWgno~35Sx7SJCl=gx8$Tz9P^^lKwcqf=QY$ib*
z`q^jK+c=K74c(qvTzYfIzDMi>`miP`C7DKR^G5t!Wlzk^uDGA-eYu9+KU+VpVkx$}
zBeE~-vy|?&{Uzf?yT7Q$@qS~PoQ(Ik^1R@~@BNYc9MS&Zr$zBkFC3Y_e>dOu{G#L|
z&X)Cn!FRH^oHLVB^w6<XmzJkfS2}ueAM6`TBrD%XaM+fD!`7ef{%q&L|HRqTQos04
hzr&iu-^?H&x|tTfYyA9uIQdnu(ki<3@QWY9{|lcIH_ZS5

literal 0
HcmV?d00001

diff --git a/ctre_sim/CANCoder vers. H - 017 - 0 - ext.dat b/ctre_sim/CANCoder vers. H - 017 - 0 - ext.dat
new file mode 100644
index 0000000000000000000000000000000000000000..2596b7a4155a130d6f3b83ca0a542ed4c02564b7
GIT binary patch
literal 2048
zcmeHHJ!q3*5WX*IHK`5}ELK#~L~T(QJ2ivoXF)73O>t?#A&@~)1ShTHPeclFP=bR*
zTr#L=5n6wyD3W4CC`uHhZMN7h#X&NtwKN#td+(l-q;|{bE%!a|-97jBe(xk5s?vXH
z;smCvj)!RY=hdz+hV>N{D(4Mumb|79m60a$z0tx-w-YzH?n%W{TMt`I8|0wUD}6;B
z(TL+1kzVmP;F_K+n~y+?fJ=IoQ(o60sv60)OP&eUr2g<rXxx)uGS6_2<p0r7PeA=x
z8EQJFxJ?NIUOfm(zOm1b2D!euQ98+%z&f#;c0AmZdnIT0h<XYK$E<&rD9iTT5wYhs
zivGUkZJ76+_X-AI$eFUl$QN?*7N;Z~m6J(3x3w_4*%@!sdKz%vBj<=mJ&ic2`JiA2
z_Txi!QSUF2%scyD=4CpUxfnm5UoG#y@XGa2f8gSX@Bt&et{-rf{$uV8uhR9EY#q@-
zaq0Kh<EPyZc5*$Cxo3~<{w_M-Ztz)jN72T+>d<$z#g-CPo=>g9msym1x6qKUmY??y
zL41Fg<i~u#x8xt+J?fhJ+vm4)eHrDQ-MiezIK5u-#<ULPN4yCo&WCvP!!tp@UegKe
z;Lp6c)ya1BZQQLi;_|zD_C8`B;D<IrF^M!>9h>pFN}rmZsoPKOyj<fwG_$r{$zrVc
z_+(y~XE{D#>x+a+c7A@f<Nj{a;7DjF$o;|}=h5G?&y?1Ob6S$~!s7AyhxhZHFE2|x
z?5vp&82*X7X`69;pX|FsdCpw#8Nfc+VImw^c^?^{xFGze7j|{G8XWKYYWwlA(a5>8
sV&9)WP#4E<X0=LmCocZ3@#XgceEjxVNeu^U^W#4&eU$g_`K<8&1wRKl;s5{u

literal 0
HcmV?d00001

diff --git a/ctre_sim/CANCoder vers. H - 018 - 0 - ext.dat b/ctre_sim/CANCoder vers. H - 018 - 0 - ext.dat
new file mode 100644
index 0000000000000000000000000000000000000000..d7704d30ca6103103cb4b7d2b8c0d3f59947ac95
GIT binary patch
literal 2048
zcmeHIJ!n%=6uvKMHK`6kTCAv~iQ1wrc4`LEXFx12eZ{2(hd>4e5uCIZe<D(dgAyDh
z;*vo{i_rQr1(jDC6*^c2X__szi#SLIYb^~v@44rEB}wg;(Yu`YouBXg-P}y6qn`SI
znmB<u&+;iXymYPmi)MYJrmG_w2bz@OwR&H@*hId!Qdjv->K4^KC3t4XF@x0>R#f}A
zFUMkuIEvxZ$Nm~z7tN~v2(>U66`d=*wxyJ(kK4g{CPgaohhIwKp8n!_MteE`&xU$J
z;zvSLtCND;gfQe6MVRxAe||hj^(~C4Gh_*EV!Lj~M?KkBU9(5jGkav*_~(_%lifYe
zc6TewAKRy+wC{qSG58|Zj3Gw8h*dB+!|7Bdoi=mZ2$P$c@lL6y0T+C%M?C6j#A(R~
z0XuLIA0kU~f0g2S=Rfegbmu%5?ML&g<^304yB^{XTpD3MV5FDz1Fn;Q^qrwq+JUmE
z!x?0k{$M?R*7I-=)q}I}{E6A$W$XJ*(mA_JCf*i@_=y%%iX%LqTA44m%=hkFL%v#m
z+B**7`@1<m<^#T!;KaT$Ti4&QxQpt`3Ge)Y)i%P}&73zObs#_Db*Q3zh(|v(6ZGrX
zoxl$M+{@ctWJllH-JC{Td3WF3N6Z8KkUA*Ak(#Ts9iQvu*|}ofero4sYww}r#&#h~
zklY{Od10QF<bbI!7Ac$g1;vj0yG?>)k)bg43xDXH-@MO^)Cav<Vg2m#sl`VR3SF<R
za6IfJi4Pe5se4(IapEBFyHj}1-RK>_KFDD*8e4lG37@{i{KcicJ*^ri<6lib_@B6N
tp6$-ZcwHR5nVzTA-6Z?lI+orC@X^~x$Ic9gYxCoGb|`z;@V%I4{=c6~Iurl^

literal 0
HcmV?d00001

diff --git a/ctre_sim/CANCoder vers. H - 019 - 0 - ext.dat b/ctre_sim/CANCoder vers. H - 019 - 0 - ext.dat
new file mode 100644
index 0000000000000000000000000000000000000000..9cdaf2e7a3aa8e94b850c93a25af4510ab8aff71
GIT binary patch
literal 2048
zcmeHHJ7`m36#j3LYEm5nTCAv~iL^zXIyIAC17dOMEiNrM1TrXy;G|W2M5GW0B{)dM
zC4-6<q4lvCMQ$-76fp|YHsBE3MI0o9T1$i1|NQ5CCAqa*M$dBcoyT`xf9|w8;Hm%8
z!VS!NRzRtdpO<<*Y1S92TtB06tK_A6S08O5-z#aXdLw&{`kod%v;CmKY7-mk{XCar
zu|^){u=I1h2A9RMqJKm~49<(q6<*s?%G2U@aGfd9O5zchmAr?)c%O+ruK&Z=PgLSa
zX=-&uaJvvjgJOtry@`+ahp4}WF?F0g(GBd^;{<3X`=V>^h<?iZCro@%sUrE^L+p1q
zv;MyAb%O3)3Ni*?+?p}Os28_N2Isk)s^xNKZ|h+SGdtcP{rGSxz;@)LA3x4XJt+8r
zBlsb@BJWoz-gogW?@Le4d(m;Uzee7F;nn9M@xYZ)<^x7~IX~bU{YSqUI;9=1nm(K%
z4jG6v<43)BcT+!*g~t!f{mxn6u9D5!RWbRt1f&j!Oe>D?d}w37!V<r`ufBSX{B(B+
z@&|gjKK28?<;Z0Bn63NoSlmhd6@+(w?@Bx2@r_(JEq$Oq@^z}BddSB-v=hu5)RVvu
z{=(DiUF65yI^2jK*WTPV?-Ba|KeSFtbEf9%+=`!T^oiNIru#JR%hu7ubL;zsEKPb(
znD>Q!)-r>pzht~>_7@R9-tQJkPR560G%w;|cYgCd^U@#eYMJfjrK5}Y?v%QoUEqB9
zNfRG1;<LBLO~uK5eD6--IeDdT5ci;fsYG)1`$+iMS>}Hp>h5jRIFtHf<_VKi%jr|n
r|7W`?kG`3nr_{|1$J;uVz7OD|Zy!lEKN4%KkN=tTK4tix%rpOAZ(2H3

literal 0
HcmV?d00001

diff --git a/ctre_sim/Pigeon 2 - 020 - 0 - ext.dat b/ctre_sim/Pigeon 2 - 020 - 0 - ext.dat
new file mode 100644
index 0000000000000000000000000000000000000000..d87008f103f150b8b4dd0610f2e2eaea3695614b
GIT binary patch
literal 2048
zcmeHFJ4jqX6uobapB0Os5e1d322G42h$+^}1q*EwS3)2~ip5Hlm`=A!6b-DPU=u&s
zY>|kF;HQj11Q#u0Fae|4P87sSRzXx0o^$8U!M9J-ZRx;c=G;5?oO{o_UsPw){Ysew
z8}#Dd&y=}Iv_zX%-JaAD{-BMka0>ZWa#n0h@f{VcmTjpvV`+jPW=PL9*0Da!{F!RK
z{6th^9cVlvX-Y&QgIxbV%-1+iMZ)XGF^|MikAB=Z!iA27JVIU^%pO1eSi$oUZR`2I
zuS@?#H+}FtPTsq)(`wEG$HT%0Y-GvU21se@#GiWwvUYV}!B)vMdz10()2~lh4{L8>
zTKoT|<7Yp|f+cZXA21ghn;z&^f`;dJy6{EM#D_zdo|9jE5+c+$SBO6R0pEppuKcqS
z4dm|^bAM68`|tT>HskS3(G8hCppSa)m-SGOdDtV&>rs;+4}Ra`u#fYYTV2=7jibvG
z`hSEU;K#0tLQ<)?3LoKkYj5A)NqV2~-(>9ckBi+H{#9m)U%>SS`%8DVf9SwpI;7{z
zGo2tFIG6XE-x7Vq6DGcS_Im%@WXw14Uh0wOo&5sFeelzT@LJI^t7}a}U$%uBj68Mr
z)wbY0I5uDA*S;c;y?1n6ALpv%e`z<o$lH1WsdrUJ+a{|1BK>}Nc2b_MV()t1K6dIQ
U>{nV3^Bv^M`)3=f!5z7O4a=_qIsgCw

literal 0
HcmV?d00001

diff --git a/ctre_sim/Talon FX vers. C - 01 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 01 - 0 - ext.dat
new file mode 100644
index 0000000000000000000000000000000000000000..17413c1c727940e51b58defbf5447f57f575ea0c
GIT binary patch
literal 2048
zcmeHGJ!q3*5Wb|TCe<N>Ma91uX^RR)L1-8C^N$umNGT58vRRxIM2esmv=9fyNh)13
z)FMS_;~1o-7!Yis3R=w;QW0H5!BR^DpLg%wQ@_NYK^;9v?tSj}dGFpYdj9y0P?#uc
zdYTUV^k4qr1m=C2Z{OmT)|Xthd~49nd$L3AGxWH6k^O%vv-GGs@E7@o%|~kZOc@dI
zoHqQ-;%J>3s>VMR(t99J?yAz^>dN`5SyuQEk0`igHuU#GL_SNQ+4z_yi$`3JPWh`X
zxJGin_Wx4HCvNpcacMefxXB8}8_cjn`;EUC>k)pHAvz<T_!{+dudv+7yIgR41fRJ>
z<1Ri=lox;Dg!&8XRe#+0Bq{qYHYi5D_Rwcn82#Eq?;O6Z?dVe`<L34&EMacOTZ~T~
zE{4^PeDJBq8S4iHKX42O(<OVqL^|*MQ=J#zT<5}ZGQVKmpW)!{#1C8=(0agVZ)*S~
zSsPZjYUG0#Eju2m9<_GIYVm`%(OuFHQpw$S`#XJh=9<`2+e$9qvw&k=5l1Cub)GkB
zz3h^H$M5U<1?$V+A;|A;)&7_d>Mh46T82F4-@LF@_+_o`n>{N{f``{?-?Z^Sf8=v2
zNk8P{J~AWRH_SJIAN3OtZzjZ#dvmyxdYqlQ>)s>gf%?##l-5kHwP^#6Kj{AX*ERbI
z&P#&pG3JFrz`LV5FW~fIm*dyDv+U*v2J+2|?E9+w%D&S6h%cL6qln&K(0t^HmiYh&
z;?v0a-F*=g(A5w8(mXaC<+nAN>~y>ZiyS<s{Ri(1J3O&h-%J0p^6!m|y7%fi@Ap5!
nzE3neqWRbRTG|@fb|9yik7@on5x#Rvy$kn7iixr96*sSMq9-+z

literal 0
HcmV?d00001

diff --git a/ctre_sim/Talon FX vers. C - 02 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 02 - 0 - ext.dat
new file mode 100644
index 0000000000000000000000000000000000000000..f42d0d29436a75d80f4829ac083e620dfc717714
GIT binary patch
literal 2048
zcmeHGO-NKx6h7l<z0nr4NGkeqG}QhGTiGUig`ot(NUL_U%Vj}?1eFoOv?y{DhMQTK
zhy-TKA#9|9pac!dn%y+EsE8;iOdOcbx#xWH=2e?&+1;G`oqNvrouBu-bKTb?HAJ4-
zIXYUQ|K}f0V5uVWJ^k%++Y4qb-W+oCzTc_-XY{Cif#3i1%+tg2;9unDwjHagXVwZq
zopXktS{yZ~qippjBiaY@q*pzij#uti%(KRqc&&nq=7YW;A*%4nHCjDp$>I?=Lw%KX
z3Fc@yr21b&^~8-|3{%rd!}UfOkD8-N^`@Rr^h<rEVLBr(@eTUUS~b#>e<|noh<X;f
zrd)iMC@b%|6Z)Rpr1sO6$1Sq&d{i;`4o1E>V(2wTK016$<>+%d?dJA7EMacO+pV4u
z&ey0P`KTw1)20W5ci<Q<W()TH66w6NPjp_~xz2^-WPU;3U*X{H<Q=#$sC>YXw>1Eg
ztPQJMck;#eimr}izj}LP)%aff_-@g|Da}l}{Wb0Ra7}#4oduWgTfp(2T31TacoyrF
zFSDZm@w<>-kYDzWgZ$n$)yI6mw;G#n9rn5Y#^oJSU&eS}?^~-EJi1YJQ&tD`k<Y0l
zddNpVG9&aG<xcPp{=|bD33*4~9PUmSXJ+rX?-BC=KP)GuG?TfuZpQT|JFxVsYCpkw
z`5Z0$QV(=6udvQuPv-@Jj)p#0-{Iz>n;-o6jyk^UzA`UWAMr)=BmV7q%}1VC`Okm@
z@hRj;zeBf29h}~;`}I~;4>18-e$QW;&kpHveoXbQj<mMdvG2%P#bLey<Lf%`#`T9i
mR(Mu&{E;5lf0)nHXIdQB`Al}JU06A_e0MCLnAlZvd_Ms5Gdn{7

literal 0
HcmV?d00001

diff --git a/ctre_sim/Talon FX vers. C - 03 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 03 - 0 - ext.dat
new file mode 100644
index 0000000000000000000000000000000000000000..13d7b4b41f09e42a74e24207e6ce5264952e00ce
GIT binary patch
literal 2048
zcmeHHJ!q3*5WX*IwW)#`EGqsrv9_p-g3vDNSFu>cA;rlpo1Mf#qzY=F6yl&bNu^5$
zty+Y(xCPM^BSI}yL95w9>YxtN!CFg$@$S8QYVuVb)X`h+d+xoT=braX(x;DH@j^st
z^AmKaLjUa_Dlk(KeNWC`YJJ6Bi`P<4@3D4`Kc~m#bA0}%HboE1eScA(+tgXZ=aCTs
z&k4hmmPczeP&WPvPxnBb?5fu8`pWlZi!AX+v`)bVi($U+5mnf7O~%JOSw8YoR(J1<
z>KWgs^FIgpL@iGox29u;8?9k9Y=P}MZ{+20kMJuE&?z~IuF`W}CnP)h7jy0&!Dss5
zh|A9sW#v5ArRTY|>OXCG5|{hVhZUn=%$szLF)!xj9loyPXfB<0y8Q}En$vi*@d@C3
zNaLsnpCC@#JUE;KNAO|3VDFbm_0B$1y?Argh4Vze>b^h2%g@8|feU@w4;bU^`T<w?
zZ~SKDD*c9{<B{mml<r6^zTb9hhwy=w#_ziOJMVq$msnzJ!PWbgaQJAQqmr;bFY2{l
zW<lTG`@p>F{&Mdy)OWY)eCUIIi;>Zt13vR_n%yG&GS>I)?xjY-gR6CJ(s*D#>baC;
z9_q1=Xo7viya~?HKl9*fvz%jZPInf>b5C!&_Xs`EAD)YnTFJdOuE*yJ-#hcB=6<U6
z@;O`j3m?pZUcouL8dR?z1G*f)j#$y@hq&x7w)d;vSLU_OC(`_)#WjoQ-C3<i9rOdP
z%1@%^cl#oz%Flo3=X>BlC+|8I?{K^Yiyb(l^HVnl9Uk4I@1=K9`QvxtyHozJWUa)a
pcaFP0Q$t$cvaY4Ap5yz+m-R?Wo|y2RUN|v(dnn&LyshN)`VJBeGeQ6W

literal 0
HcmV?d00001

diff --git a/ctre_sim/Talon FX vers. C - 04 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 04 - 0 - ext.dat
new file mode 100644
index 0000000000000000000000000000000000000000..d53937ea48cfd138352224a3c43b43588789a2eb
GIT binary patch
literal 2048
zcmeHIJ!q3*5WXa>Ce<N>Ma7?{mA3wXC<yJMeu`pIloThoY<9FDQVME83UN@Jq|y!<
zv`7(}I0mUH1_UisL95w9>YyT`pw!YpynFAS@a6MoP)BdM_kN$}?!L6q!6R2gVWOz%
zSvpvw|MCwvuuzlzp80yI>jif$UmtP%&i1JPDLt%S;P{`)96hKG{Y8CY<DmvVGe!hF
zXU%?Md9+3yRpXxw=^Ut&Syg&mUwL0P&k8@{O==9A5BOe)sK!!gH9n@v@{yOLleIbo
z_vqNG_+J8i;?`djx2EG}w^+k?#2jskH}!n7U-(r<>8wQJt2EA7VVTLhSa5d)pZNn*
zE<aC{mw4f*#tUmze_a2lL+)LSsEvNRL!Vt^;I)T7IQxde(Wi9U?d?~#q`4h$GCl#j
z7*;>(!6#^^4G#)&*fBgzm+bo`(tYP2>%Mq$-3#Z*{``G^X8Ug^aoD9H?FSq1b_SRv
zXT$00PJVdNvg48LS8rde(Z1Jxdz;`vD!IGver?;|UlCt&OUc!HmT>r3lcSQfKF^x9
zUuH>v$8Q68{{C`z2<rQ~6d(IRzvb9;=cvd0TNgJAzl`;Ly>q2S?6K8~n=&52M?II4
z;GrJ#$c`{?geO59{S)`EB_xiyIo(On&dl6$-y`;c{?J^M(n{{NVI7`7=z)b-4fpZy
z%j0b6FMNQ5eFbrLM0H;P$PS-y{CeBVZh!FGpAFjcP4AU?srbk*n;-da&TBpDL<=9-
zzWfwwem5_2e7gFMzch~>GGn%**YOrxWdAwEAGtZ^?CD+ly$mcX|ETk7y*z8o&B=uR
l|DGGy`co5~-OU`>m#gn1rHOi<`K8l~cgBl}$*mQ)ukYbGE=B+V

literal 0
HcmV?d00001

diff --git a/ctre_sim/Talon FX vers. C - 05 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 05 - 0 - ext.dat
new file mode 100644
index 0000000000000000000000000000000000000000..ea2dbc5f7469590835dc17ef0ab0bcaa02cd7202
GIT binary patch
literal 2048
zcmeHGOK4M35WPuSeW@<8uzv8<B+}Loh=R~=)Tby`MM!bwE<3xdAW{UiAccr1T}h>z
zENYP=v~d}vzG6VoLMv!ByO6q25m8WTY2bP1bHdB>vrw1LX70?LIp@sW_nh-buScTB
zIO=E2A>aJBf4G4KpZ0t9>y?fd7PWF~koP^+CHZIOQSG9=|5KYa4{HN|(eG_K+@NP#
zg+S+w#!t1*tdT@b^=Be72l{AMu`b4?^Ht5P*h8X8f=ikMzZ)^ex8}8~9`#1+QCBdh
z{W^ukNbeK=FCje%#V2Q>%}I@06fhCfM4RwUJRk2Rebpgzj&2gG<lb^cX(sow$L|O|
za|b85zGzI5?!9Aj@2wU8al_*@y>}@lG4S?8J~J@*QjrfF4-1|7l*{qneic(0?|8H7
z3B{$T<k1g3;W(#!aJWYt$3cBrzh7fy-^C}gFFU#H#n#dOg1A46gKtOoh|2?lhZyww
z3<wQ98=kJ7v<KIzu#QZxWP9R`@%_%R?ZgMG7Vh!;we5I+jdGc-W$t&i;K=bNmXcAN
z`DVe(FUf!WHpCair+0^;zo$d^u^-?q$0yr|T&urzaSQ3oEAH!ED=id{tQOv^>Ht6b
zZL1+Z^kW{{5$28ANpKJR<b#_@y2sqMoGl#Zr|<Chh<yMb-Zo{W(_(F0hvN^vZ{bzL
z`vmvp+G@flJ>bE<!aO@2*%t`rQzuwocdEksL%vxLd%wwh<zEUv>MNQ@{hJHYk3RCG
z2XUZ2i=N-ji<*G1y|Z7MYZEl%RJxmWQ%rWCU-$=ak8nJ>TfUdRWzp|&R{H7aTWw^+
o>rXMx?5OnL7;En&&HD@WePpSQjP%Yeomsp)T1t*@tMb0S18)N^DgXcg

literal 0
HcmV?d00001

diff --git a/ctre_sim/Talon FX vers. C - 06 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 06 - 0 - ext.dat
new file mode 100644
index 0000000000000000000000000000000000000000..a8dce6dec297d4eacf20dc676b7905f5911123fa
GIT binary patch
literal 2048
zcmeHHJ!n%=6uvKc)ucLPXi%v?ZNwB6q=Jwv>QfYhprklCnaz$0B1LEoNFff2lT^B7
z&>}@>vc|s@1A-P>L93Yzse?F(g3^`-;ypiKNS?Z=qi4DAJ3rs~?m0IJT|9Q(2^bU7
zbJiSgn*a6>C$QM0`JVkg7JqG7Yd5nz??OWSFU`}&WgGufn=_9ZBY#m}-g2Zv&l42_
zowJHxXr9?1j)v+_J8}=y(Otz7%uC-_)w5<lBCdcd>O+6eF{Wv)992Df8qG&u(VS|w
zT3C+Qe(C?srzfKPbXaP0QgOEuMuO_tA^oObO%0L0`nWkyk;poU+g<^>lQ&xCJwng?
zp=r)98B?No`MAW(8|D1G=~;~ST?q;X-#+In6Qf_hv%vV4v@>6FIiB0^Fs1Q~dsUAQ
zR|4WkJ@oi-PW$0N95{>*J*)bDjgfhmp3A)K&1Eh&kLK6T`zzeOJBkBWM}!X;?R5=+
zhSrAFwVd|D3)NUhdPuyfa3_8+aCay5gVl@od4DU3PuIwo-d5#$PZN%wa9K)Pc^0~a
zFTX6m<Bz_6?fkTN80u4T>5ut<Z#6u#YuvN?qf1*!UtW3N?OE$4Jh5K-W>g3Iqu!Pp
z>W6yVhh~KP2JKA{2Y;q`qmSaaw@qjBWAEV|evg<3_;K2zj8s~#%{}=1=^S2s+p(Ya
zd3iRQ_(>1?U|#+{yF)TBG{_`Rv%X}1jpqkH-p$~O>?{99`Xj%le&m0+B=xAHlk@<0
z=4Vl+|4t0)z2Nz_5;W0mo-@7l(md<vNx9KNEXleFlO4Pu{j;|x7|-mL_cFXH`k}+)
vsQs_B+Q<rzQ_M3rDfRCM;{#pRf1ubtk1VB<x%10smhMef`lhzmd0syNrD-y)

literal 0
HcmV?d00001

diff --git a/ctre_sim/Talon FX vers. C - 07 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 07 - 0 - ext.dat
new file mode 100644
index 0000000000000000000000000000000000000000..ec4ab94b478e8f26580c9b30b11c17fe019fdeb7
GIT binary patch
literal 2048
zcmeHGO=uHQ5PqAqc2hm%U{Ud>X{4<`APPczQCCqcg-VJiZ@Jl{7Lg*T0V%|T;z=sK
z<e){0&_-`T>J|fn7OJS#+(PO>MMOcVrGYr}X1=gle-7%=xy+k+GvD{kyj}P7;mc0M
z7+3wQIZ!kI?H_Jnp~m|?@%2L23qmbl&DecU_elPUc~Ct|_kU`0=6-eLFZ%rr2OIRv
zs1WF!)%da2nbnf0s{WKCbD)oB73;CMe7>le6*|P5Bsixz@OzFiH8Q_V^{6*mkGi}W
zsMRS5Be_TTzl8L}6`vhJn|_U36)+yvM7!`!KARY1eU(vjl5gUx<es=9Jd=0cx9<o&
z^ZO@leZiOl-}^`8-d`jB<GP1Qe(z#bV&Ls^K3ib$bvPevd`;-gr)<{l?N>3E*&T0G
zJ)yW5kv#gLCmd&$4-WT;V>qZU>Gx}l?7Q$t_C=G+UZ{@u7sUNp9DF;zM_d{aJj9^a
zXFzE9+3<ArBpp1rY;~jtCEFKkjPG>c+{%2gO8&NezxHkKFL5rlxn%o2EjV<v$x2Bn
z&eLYW%Pq-&{5HfF#OHU1p}((7_^}`0Eyt!hM?KQtwz!G)<rMezj+Iu9$6ACptvbMu
zerh$$hkne%JHotCngsX2Pu#td;CswX<<`P+Zsvyl9<dML!&6gQItgq2S{#4yLkq7O
z-Y2*(kE$7;^?(Qa3iE7tWnUohj`dr8y&Yw{Kjd5Vu=|_5SMH_oqrR+p)W10+{pe%Q
zdJqTd)9Cr#yr>EI>O1<<JWBA4)5%_|n`5?pr-VOqeayyFJLP*BS{D7T%}PHXYov`W
p^L)VK%#BO`@V(CNX3Fo)*Y}a;IyTljzjS=@)_5^7v87`7^&Py%E)xI%

literal 0
HcmV?d00001

diff --git a/ctre_sim/Talon FX vers. C - 09 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 09 - 0 - ext.dat
new file mode 100644
index 0000000000000000000000000000000000000000..949d405611073cddbd652ee812351a1e68fd2747
GIT binary patch
literal 2048
zcmeHHJ!n%=6uvJ>HK`668dT~}8Zku$Q4rEaeHO(ih$&7^W)SSCAW{T1AcZ(6PEzTT
zL8}y@%^F2h3<z3i1g&NZse^P71*I(w#Cv|e@bc6}9X-o^=kNQzd(KV57mwU@0>*^(
zoH2)5=D+>J3Cy=>zGuE)OTM+J_1ig~_jF3)ug&x3Wt;y~n>A0HBY)9f-gLM_&odPQ
zoipmc(mJzF0!`JQapWH8qq~Zw7?-}UYh=}aL_^|S(HQt$$C#G2a!mE;X|x`7MRT&%
zrmz@^eZv3ErzfiTbXaI}Lj7(9j0QEZP536?Obn5}#;7?@ndlnHTdn}z$-P?UJwngi
z!AY(!8B?Ns`KaW}>*f5s;YEV>T?vX0ygklW1_ocBQ)T~-(3vm!JkRZSK9%u|dsUCm
zuLLBHe(3T0dF8`_Jp2eg^sMRoHAd!LdMWd=H<!8CI+~vs_gB7mca(=;8xcJCpw~6P
zHMBOYt{t==Zn(}mGD8wgM>_q-{r9&MAFNS)#QR%LE#9D5W=oCxT`d?q7GfzG#i@1)
zUSUao$De$BUVPd+4E^b(@MAu}TZv5V7<H}w*urMgS5Vv!yH>l&A6qNDS=9l4^xIZL
zeCWr0Xhyhi(B1@j;Ae}s;*`g|Z8@9Ycc0wj_lS7_AE#}~N~guz*n`iX&f)p@9sBXl
z%eB>nPkO+EdHH#EhGkwLsGd8;`Ud*yJU_(oe)lfRz6$SzAN6&OqyFP1=|>-(qz8Uy
zeGYB<@5C;>7d+o~ffj~ZXH74?G}i`t(xKRNVt{p%Pj=vf@aOK1u|Kt2-plZc=*REC
w<*fa$wA#qxi^myfc3k=&4kr7%Y<z#wJC7Wdl7;4$PA@zduf!*|Hh5k?0Q>7RYXATM

literal 0
HcmV?d00001

diff --git a/networktables.json b/networktables.json
new file mode 100644
index 00000000..fe51488c
--- /dev/null
+++ b/networktables.json
@@ -0,0 +1 @@
+[]
diff --git a/simgui-ds.json b/simgui-ds.json
new file mode 100644
index 00000000..73cc713c
--- /dev/null
+++ b/simgui-ds.json
@@ -0,0 +1,92 @@
+{
+  "keyboardJoysticks": [
+    {
+      "axisConfig": [
+        {
+          "decKey": 65,
+          "incKey": 68
+        },
+        {
+          "decKey": 87,
+          "incKey": 83
+        },
+        {
+          "decKey": 69,
+          "decayRate": 0.0,
+          "incKey": 82,
+          "keyRate": 0.009999999776482582
+        }
+      ],
+      "axisCount": 3,
+      "buttonCount": 4,
+      "buttonKeys": [
+        90,
+        88,
+        67,
+        86
+      ],
+      "povConfig": [
+        {
+          "key0": 328,
+          "key135": 323,
+          "key180": 322,
+          "key225": 321,
+          "key270": 324,
+          "key315": 327,
+          "key45": 329,
+          "key90": 326
+        }
+      ],
+      "povCount": 1
+    },
+    {
+      "axisConfig": [
+        {
+          "decKey": 74,
+          "incKey": 76
+        },
+        {
+          "decKey": 73,
+          "incKey": 75
+        }
+      ],
+      "axisCount": 2,
+      "buttonCount": 4,
+      "buttonKeys": [
+        77,
+        44,
+        46,
+        47
+      ],
+      "povCount": 0
+    },
+    {
+      "axisConfig": [
+        {
+          "decKey": 263,
+          "incKey": 262
+        },
+        {
+          "decKey": 265,
+          "incKey": 264
+        }
+      ],
+      "axisCount": 2,
+      "buttonCount": 6,
+      "buttonKeys": [
+        260,
+        268,
+        266,
+        261,
+        269,
+        267
+      ],
+      "povCount": 0
+    },
+    {
+      "axisCount": 0,
+      "buttonCount": 0,
+      "povCount": 0
+    }
+  ]
+}
diff --git a/simgui.json b/simgui.json
new file mode 100644
index 00000000..3cfca74e
--- /dev/null
+++ b/simgui.json
@@ -0,0 +1,23 @@
+{
+  "NTProvider": {
+    "types": {
+      "/FMSInfo": "FMSInfo"
+    }
+  },
+  "NetworkTables": {
+    "transitory": {
+      "drivetrain": {
+        "open": true,
+        "pid": {
+          "open": true
+        }
+      }
+    }
+  },
+  "NetworkTables Info": {
+    "Clients": {
+      "open": true
+    },
+    "visible": true
+  }
+}
diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp
index d4498c4a..6015b19a 100644
--- a/src/main/cpp/Robot.cpp
+++ b/src/main/cpp/Robot.cpp
@@ -155,3 +155,13 @@ void Robot::DisabledPeriodic() {}
 
 void Robot::TestInit() {}
 void Robot::TestPeriodic() {}
+
+void Robot::SimulationInit() {
+  //_vision->GetDistanceToTarget(16);
+  for (int i = 1; i< 17; i++) {
+    _vision->AlignToTarget(i, 0_m, _swerveDrive);
+  }  
+}
+
+void Robot::SimulationPeriodic() {}
+
diff --git a/src/main/cpp/vision/Vision.cpp b/src/main/cpp/vision/Vision.cpp
index a06236fc..8cf9a46b 100644
--- a/src/main/cpp/vision/Vision.cpp
+++ b/src/main/cpp/vision/Vision.cpp
@@ -17,44 +17,44 @@ FMAP::FMAP(std::string path) : _path(path) {
 
   file >> j;
 
+  std::cout << j["fiducials"] << std::endl;
+
   // iterate through the json object and add each tag to the vector
 
-  for (auto& element : j["fiducials"]) {
-    AprilTag tag;
-
-    tag.id = element["id"];
-    tag.size = element["size"];
-    tag.transform[0][0] = element["transform"][0];
-    tag.transform[0][1] = element["transform"][1];
-    tag.transform[0][2] = element["transform"][2];
-    tag.transform[0][3] = element["transform"][3];
-    tag.transform[1][0] = element["transform"][4];
-    tag.transform[1][1] = element["transform"][5];
-    tag.transform[1][2] = element["transform"][6];
-    tag.transform[1][3] = element["transform"][7];
-    tag.transform[2][0] = element["transform"][8];
-    tag.transform[2][1] = element["transform"][9];
-    tag.transform[2][2] = element["transform"][10];
-    tag.transform[2][3] = element["transform"][11];
-    tag.transform[3][0] = element["transform"][12];
-    tag.transform[3][1] = element["transform"][13];
-    tag.transform[3][2] = element["transform"][14];
-    tag.transform[3][3] = element["transform"][15];
-    if (element["unique"] == 1) {
-      tag.unique = true;
-    } else {
-      tag.unique = false;
+  for (auto& fiducial : j["fiducials"]) {
+    std::cout << "Loading AprilTag " << fiducial["id"] << std::endl;
+      AprilTag tag;
+      tag.id = fiducial["id"];
+      tag.size = fiducial["size"];
+      if (fiducial["unique"] == 1) {
+          tag.unique = true;
+      } else {
+          tag.unique = false;
+      }
+
+      const auto& transform = fiducial["transform"];
+      for (int i = 0; i < 4; ++i) {
+          for (int j = 0; j < 4; ++j) {
+               tag.transform[i][j] = transform[i * 4 + j];
+          }
+      }
+
+      tag.yaw = units::radian_t{tag.transform[0][0]};
+      tag.pitch = units::radian_t{tag.transform[1][1]};
+      tag.roll = units::radian_t{tag.transform[2][2]};
+
+      tag.pos = frc::Pose3d(units::meter_t{tag.transform[0][3]}, units::meter_t{tag.transform[1][3]}, units::meter_t{tag.transform[2][3]}, frc::Rotation3d(0_deg, 0_deg, 0_deg));
+
+       _tags.push_back(tag);
     }
 
-    _tags.push_back(tag);
-  }
-
   file.close();
 
   std::cout << "Loaded " << _tags.size() << " tags" << std::endl;
 
   for (int i = 0; i < _tags.size(); i++) {
-    std::cout << "Tag " << _tags[i].id << " is " << _tags[i].size << "m" << std::endl;
+    std::cout << "Tag " << _tags[i].id << " is at: X: " << _tags[i].pos.X().value() << " Y: " << _tags[i].pos.Y().value()
+              << " Z: " << _tags[i].pos.Z().value() << std::endl;
   }
 
   std::cout << "Loaded FMAP" << std::endl;
@@ -79,18 +79,17 @@ std::pair<units::meter_t, units::degree_t> Vision::GetDistanceToTarget(VisionTar
 
   std::vector<AprilTag> tags = _fmap.GetTags();
 
-  frc::Pose3d pose = _limelight->GetPose();
-
   for (int i = 0; i < tags.size(); i++) {
     if (tags[i].id == static_cast<int>(target)) {
-      frc::Pose3d aprilTagPos =
-          frc::Pose3d(tags[i].transform[0][3] * 1_m, tags[i].transform[1][3] * 1_m,
-                      tags[i].transform[2][3] * 1_m, frc::Rotation3d(0_deg, 0_deg, 0_deg));
+      AprilTag tag = tags[i];
+      frc::Pose3d pose = _limelight->GetPose();
+      //frc::Pose3d pose = frc::Pose3d(0_m, 0_m, 0_m, frc::Rotation3d(0_deg, 0_deg, 0_deg));
+
+      units::meter_t distance = pose.Translation().Distance(tag.pos.Translation());
 
-      units::meter_t distance = pose.Translation().Distance(aprilTagPos.Translation());
+      units::degree_t angle = units::math::atan((pose.X() - tag.pos.X()) / (pose.Y() - tag.pos.Y()));
 
-      units::degree_t angle = units::math::atan((pose.X() - units::meter_t{tags[i].transform[0][3]}) /
-                                                (pose.Y() - units::meter_t{tags[i].transform[1][3]}));
+      std::cout << "Distance: " << distance.value() << " Angle: " << angle.value() << std::endl;
 
       return std::make_pair(distance, angle);
     }
@@ -110,16 +109,16 @@ std::pair<units::meter_t, units::degree_t> Vision::GetDistanceToTarget(int id) {
 
   for (int i = 0; i < tags.size(); i++) {
     if (tags[i].id == id) {
-      // get distance to the limelight
-      frc::Pose3d currentPose = _limelight->GetPose();
-      frc::Pose3d aprilTagPos =
-          frc::Pose3d(tags[i].transform[0][3] * 1_m, tags[i].transform[1][3] * 1_m,
-                      tags[i].transform[2][3] * 1_m, frc::Rotation3d(0_deg, 0_deg, 0_deg));
+      AprilTag tag = tags[i];
+      frc::Pose3d pose = _limelight->GetPose();
+      //frc::Pose3d pose = frc::Pose3d(0_m, 0_m, 0_m, frc::Rotation3d(0_deg, 0_deg, 0_deg));
+
+      // units::meter_t distance = pose.Translation().Distance(tag.pos.Translation());
+      units::meter_t distance = units::meter_t{std::sqrt(std::pow(tag.pos.X().value(), 2) + std::pow(tag.pos.Y().value(), 2))};
 
-      units::meter_t distance = currentPose.Translation().Distance(aprilTagPos.Translation());
+      units::degree_t angle = units::math::atan((pose.X() - tag.pos.X()) / (pose.Y() - tag.pos.Y()));
 
-      units::degree_t angle = units::math::atan((currentPose.X() - units::meter_t{tags[i].transform[0][3]}) /
-                                                (currentPose.Y() - units::meter_t{tags[i].transform[1][3]}));
+      //std::cout << "Distance: " << distance.value() << " Angle: " << angle.value() << std::endl;
 
       return std::make_pair(distance, angle);
     }
@@ -132,21 +131,129 @@ std::vector<AprilTag> Vision::GetTags() {
   return _fmap.GetTags();
 }
 
-frc::Pose2d Vision::AlignToTarget(VisionTarget target, wom::SwerveDrive* swerveDrive) {
+frc::Pose2d Vision::AlignToTarget(VisionTarget target, units::meter_t offset, wom::SwerveDrive* swerveDrive) {
   SetMode(VisionModes::kAprilTag);
 
   std::pair<units::meter_t, units::degree_t> distance = GetDistanceToTarget(target);
 
-  units::meter_t x = distance.first * units::math::cos(distance.second);
-  units::meter_t y = distance.first * units::math::sin(distance.second);
+  // Get current position from Limelight
+  frc::Pose2d current_pose = _limelight->GetPose().ToPose2d();
+  //frc::Pose2d current_pose = frc::Pose2d(0_m, 0_m, 0_deg);
+  
+  units::meter_t x_offset = distance.first * units::math::cos(distance.second);
+  units::meter_t y_offset = distance.first * units::math::sin(distance.second);
 
-  frc::Pose2d pose = frc::Pose2d(x, y, distance.second);
+  // reduce both offsets by the offset parameter (in relative amount)
+  x_offset -= offset * units::math::cos(distance.second);
+  y_offset -= offset * units::math::sin(distance.second);
 
+  frc::Pose2d pose = frc::Pose2d(y_offset, x_offset, distance.second);
+  
+
+  // Print the results
+  std::cout << "Aligning to tag " << static_cast<int>(target) << ": X: " << pose.X().value() << " Y: " << pose.Y().value() << " Angle: " << pose.Rotation().Degrees().value() << ". At offset " << offset.value() << "." << std::endl;
+
+  // Set the new pose to the swerve drive
   swerveDrive->SetPose(pose);
 
   return pose;
 }
 
+frc::Pose2d Vision::AlignToTarget(int target, units::meter_t offset, wom::SwerveDrive* swerveDrive) {
+  SetMode(VisionModes::kAprilTag);
+
+  // Get distance to target
+  std::pair<units::meter_t, units::degree_t> distance = GetDistanceToTarget(target);
+
+  // Get current position from Limelight
+  frc::Pose2d current_pose = _limelight->GetPose().ToPose2d();
+  //frc::Pose2d current_pose = frc::Pose2d(0_m, 0_m, 0_deg);
+  
+  units::meter_t x_offset = distance.first * units::math::cos(distance.second);
+  units::meter_t y_offset = distance.first * units::math::sin(distance.second);
+
+  // reduce both offsets by the offset parameter (in relative amount)
+  x_offset -= offset * units::math::cos(distance.second);
+  y_offset -= offset * units::math::sin(distance.second);
+
+  frc::Pose2d pose = frc::Pose2d(y_offset, x_offset, distance.second);
+  
+
+  // Print the results
+  std::cout << "Aligning to tag " << target << ": X: " << pose.X().value() << " Y: " << pose.Y().value() << " Angle: " << pose.Rotation().Degrees().value() << ". At offset " << offset.value() << "." << std::endl;
+
+  // Set the new pose to the swerve drive
+  //swerveDrive->SetPose(pose);
+
+  return pose;
+}
+
+frc::Pose2d Vision::AlignToTarget(VisionTarget target, frc::Translation2d offset, wom::SwerveDrive* swerveDrive) {
+  SetMode(VisionModes::kAprilTag);
+
+  std::pair<units::meter_t, units::degree_t> distance = GetDistanceToTarget(target);
+
+  // Get current position from Limelight
+  frc::Pose2d current_pose = _limelight->GetPose().ToPose2d();
+  //frc::Pose2d current_pose = frc::Pose2d(0_m, 0_m, 0_deg);
+  
+  units::meter_t x_offset = distance.first * units::math::cos(distance.second);
+  units::meter_t y_offset = distance.first * units::math::sin(distance.second);
+
+  // reduce both offsets by the offset parameter (in relative amount)
+  //x_offset -= offset * units::math::cos(distance.second);
+  //y_offset -= offset * units::math::sin(distance.second);
+
+  if (offset.X() > 0_m) {
+    x_offset -= offset.X() * units::math::cos(distance.second);
+  } else {
+    x_offset += offset.X() * units::math::cos(distance.second);
+  }
+
+  if (offset.Y() > 0_m) {
+    y_offset -= offset.Y() * units::math::sin(distance.second);
+  } else {
+    y_offset += offset.Y() * units::math::sin(distance.second);
+  }
+
+  frc::Pose2d pose = frc::Pose2d(y_offset, x_offset, distance.second);
+  
+
+  // Print the results
+  std::cout << "Aligning to tag " << static_cast<int>(target) << ": X: " << pose.X().value() << " Y: " << pose.Y().value() << " Angle: " << pose.Rotation().Degrees().value() << ". At offset: X: " << offset.X().value() << ", Y: " << offset.Y().value() << "." << std::endl;
+
+  // Set the new pose to the swerve drive
+  swerveDrive->SetPose(pose);
+
+  return pose;
+}
+
+frc::Pose2d Vision::AlignToTarget(int target, frc::Translation2d offset, wom::SwerveDrive* swerveDrive) {
+  SetMode(VisionModes::kAprilTag);
+
+  // Get distance to target
+  std::pair<units::meter_t, units::degree_t> distance = GetDistanceToTarget(target);
+
+  // Get current position from Limelight
+  frc::Pose2d current_pose = _limelight->GetPose().ToPose2d();
+  //frc::Pose2d current_pose = frc::Pose2d(0_m, 0_m, 0_deg);
+  
+  units::meter_t x_offset = distance.first * units::math::cos(distance.second);
+  units::meter_t y_offset = distance.first * units::math::sin(distance.second);
+
+  frc::Pose2d pose = frc::Pose2d(y_offset, x_offset, distance.second);
+  
+
+  // Print the results
+  std::cout << "Aligning to tag " << target << ": X: " << pose.X().value() << " Y: " << pose.Y().value() << " Angle: " << pose.Rotation().Degrees().value() << ". At offset: X: " << offset.X().value() << ", Y: " << offset.Y().value() << "." << std::endl;
+
+  // Set the new pose to the swerve drive
+  //swerveDrive->SetPose(pose);
+
+  return pose;
+}
+
+
 bool Vision::IsAtPose(frc::Pose3d pose, units::second_t dt) {
   return _limelight->IsAtSetPoseVision(pose, dt);
 }
diff --git a/src/main/include/vision/Vision.h b/src/main/include/vision/Vision.h
index 8176b2a2..4c7a224e 100644
--- a/src/main/include/vision/Vision.h
+++ b/src/main/include/vision/Vision.h
@@ -54,6 +54,11 @@ struct AprilTag {
   double size;
   std::array<std::array<double, 4>, 4> transform;
   bool unique;
+
+  frc::Pose3d pos;
+  units::radian_t yaw;
+  units::radian_t pitch;
+  units::radian_t roll;
 };
 
 class FMAP {
@@ -79,7 +84,11 @@ class Vision {
 
   frc::Pose3d GetPose();
 
-  frc::Pose2d AlignToTarget(VisionTarget target, wom::SwerveDrive* swerveDrive);
+  frc::Pose2d AlignToTarget(VisionTarget target, units::meter_t offset, wom::SwerveDrive* swerveDrive);
+  frc::Pose2d AlignToTarget(int target, units::meter_t offset, wom::SwerveDrive* swerveDrive);
+
+  frc::Pose2d AlignToTarget(VisionTarget target, frc::Translation2d offset, wom::SwerveDrive* swerveDrive);
+  frc::Pose2d AlignToTarget(int target, frc::Translation2d offset, wom::SwerveDrive* swerveDrive);
 
   std::vector<AprilTag> GetTags();
 
diff --git a/wombat/src/main/cpp/utils/Encoder.cpp b/wombat/src/main/cpp/utils/Encoder.cpp
index 1f5a58c4..bca8baa8 100644
--- a/wombat/src/main/cpp/utils/Encoder.cpp
+++ b/wombat/src/main/cpp/utils/Encoder.cpp
@@ -57,7 +57,7 @@ units::radian_t wom::utils::Encoder::GetEncoderPosition() {
 }
 
 double wom::utils::Encoder::GetEncoderDistance() {
-  return GetEncoderTicks() * (2 * 3.141592) * _wheelRadius.value();
+  return GetEncoderTicks() * (2 * 3.14159265358979323846) * _wheelRadius.value();
 }
 
 units::radians_per_second_t wom::utils::Encoder::GetEncoderAngularVelocity() {

From cbf0e37fbe6a54d496c5ae0aed1fbd869d682598 Mon Sep 17 00:00:00 2001
From: Paul Hodges <paul.hodges@student.education.wa.edu.au>
Date: Mon, 29 Jan 2024 12:32:08 +0800
Subject: [PATCH 111/207] Finish vision maths

---
 ctre_sim/CANCoder vers. H - 016 - 0 - ext.dat | Bin 2048 -> 2048 bytes
 ctre_sim/CANCoder vers. H - 017 - 0 - ext.dat | Bin 2048 -> 2048 bytes
 ctre_sim/CANCoder vers. H - 018 - 0 - ext.dat | Bin 2048 -> 2048 bytes
 ctre_sim/CANCoder vers. H - 019 - 0 - ext.dat | Bin 2048 -> 2048 bytes
 ctre_sim/Pigeon 2 - 020 - 0 - ext.dat         | Bin 2048 -> 2048 bytes
 ctre_sim/Talon FX vers. C - 01 - 0 - ext.dat  | Bin 2048 -> 2048 bytes
 ctre_sim/Talon FX vers. C - 02 - 0 - ext.dat  | Bin 2048 -> 2048 bytes
 ctre_sim/Talon FX vers. C - 03 - 0 - ext.dat  | Bin 2048 -> 2048 bytes
 ctre_sim/Talon FX vers. C - 04 - 0 - ext.dat  | Bin 2048 -> 2048 bytes
 ctre_sim/Talon FX vers. C - 05 - 0 - ext.dat  | Bin 2048 -> 2048 bytes
 ctre_sim/Talon FX vers. C - 06 - 0 - ext.dat  | Bin 2048 -> 2048 bytes
 ctre_sim/Talon FX vers. C - 07 - 0 - ext.dat  | Bin 2048 -> 2048 bytes
 ctre_sim/Talon FX vers. C - 09 - 0 - ext.dat  | Bin 2048 -> 2048 bytes
 src/main/cpp/Robot.cpp                        |  16 +-
 src/main/cpp/vision/Vision.cpp                | 175 +++++++++++-------
 15 files changed, 125 insertions(+), 66 deletions(-)

diff --git a/ctre_sim/CANCoder vers. H - 016 - 0 - ext.dat b/ctre_sim/CANCoder vers. H - 016 - 0 - ext.dat
index 8f8bccdea82f511e5af738d8fa0b4b911c8bdd34..b70c3556679497ee603521f2dda7d4398157875e 100644
GIT binary patch
delta 36
ncmZn=Xb{+tz{t6iq5t;=W(Ee1iGuE&yBK<YZ-R0bF|h#v)Rqdo

delta 36
ncmZn=Xb{+tz{t6m;qaddCI$wMiGuE&dl-)WDT8trF|h#v)s70h

diff --git a/ctre_sim/CANCoder vers. H - 017 - 0 - ext.dat b/ctre_sim/CANCoder vers. H - 017 - 0 - ext.dat
index 2596b7a4155a130d6f3b83ca0a542ed4c02564b7..be58d0c2d2c75f1ffe8d657c982b4a688f24ce71 100644
GIT binary patch
delta 30
kcmZn=Xb{+tz{s|fVe0Qq%o7E@*mf|?_`LzdTExT#0I8M=yZ`_I

delta 30
kcmZn=Xb{+tz{s|n;ryR6rip@HY<n2a{HXx37BR5_0IH4(yZ`_I

diff --git a/ctre_sim/CANCoder vers. H - 018 - 0 - ext.dat b/ctre_sim/CANCoder vers. H - 018 - 0 - ext.dat
index d7704d30ca6103103cb4b7d2b8c0d3f59947ac95..a5a5e02fd7e38bd52b5d2e78c6a069ddb1a7ff1d 100644
GIT binary patch
delta 36
ncmZn=Xb{+tz{t6iq5t;=W(Ee1iGuE&yBK<YZ-R0bF|h#v)Rqdo

delta 36
ncmZn=Xb{+tz{t6m;qaddCI$wMiGuE&dl-)WDT8trF|h#v)s70h

diff --git a/ctre_sim/CANCoder vers. H - 019 - 0 - ext.dat b/ctre_sim/CANCoder vers. H - 019 - 0 - ext.dat
index 9cdaf2e7a3aa8e94b850c93a25af4510ab8aff71..1d1df01f2e5379572fc30a4936c29417c965e6e7 100644
GIT binary patch
delta 30
kcmZn=Xb{+tz{s|fVe0Qq%o7E@*mf|?_`LzdTExT#0I8M=yZ`_I

delta 30
kcmZn=Xb{+tz{s|n;ryR6rip@HY<n2a{HXx37BR5_0IH4(yZ`_I

diff --git a/ctre_sim/Pigeon 2 - 020 - 0 - ext.dat b/ctre_sim/Pigeon 2 - 020 - 0 - ext.dat
index d87008f103f150b8b4dd0610f2e2eaea3695614b..1c15fadd76a37035c4fb5dc322174ef5e4f0ae3b 100644
GIT binary patch
delta 53
zcmZn=Xb@mI&S3d}qo6(GL<L@!6ATtGHrq~yslPWdPZacG+rcp7_XZGavjg)1769A8
B5<vg}

delta 53
zcmZn=Xb@mI%V7L}qo6(GL<L@!GYlp$HrsB7^MA^iCJK77?O{0crvk*<?7)101pwU4
B5<vg}

diff --git a/ctre_sim/Talon FX vers. C - 01 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 01 - 0 - ext.dat
index 17413c1c727940e51b58defbf5447f57f575ea0c..c286c31b23ccbab2ae10455eb784fdb6c7aba6ba 100644
GIT binary patch
delta 30
kcmZn=Xb{-Yz{qxzq4oDB=81|{Y$q5xes2J=7BR5_0Ie1Z+yDRo

delta 30
kcmZn=Xb{-Yz{qx*VgH{priqGHY-bqu{HXx37BR5_0Im)S+yDRo

diff --git a/ctre_sim/Talon FX vers. C - 02 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 02 - 0 - ext.dat
index f42d0d29436a75d80f4829ac083e620dfc717714..09761e081c6233b148c8a5cfc9a8e963345694bb 100644
GIT binary patch
delta 30
kcmZn=Xb{-Yz{qxzq4oDB=81|{Y$q5xes2J=7BR5_0Ie1Z+yDRo

delta 30
kcmZn=Xb{-Yz{qx*VgH{priqGHY-bqu{HXx37BR5_0Im)S+yDRo

diff --git a/ctre_sim/Talon FX vers. C - 03 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 03 - 0 - ext.dat
index 13d7b4b41f09e42a74e24207e6ce5264952e00ce..241982f5972195b12caa6927b24a685894f9882b 100644
GIT binary patch
delta 30
kcmZn=Xb{-Yz{qxzq4oDB=81|{Y$q5xes2J=7BR5_0Ie1Z+yDRo

delta 30
kcmZn=Xb{-Yz{qx*VgH{priqGHY-bqu{HXx37BR5_0Im)S+yDRo

diff --git a/ctre_sim/Talon FX vers. C - 04 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 04 - 0 - ext.dat
index d53937ea48cfd138352224a3c43b43588789a2eb..891fd4bd1a4cca587d981de44ca775b397e424c3 100644
GIT binary patch
delta 30
kcmZn=Xb{-Yz{qxzq4oDB=81|{Y$q5xes2J=7BR5_0Ie1Z+yDRo

delta 30
kcmZn=Xb{-Yz{qx*VgH{priqGHY-bqu{HXx37BR5_0Im)S+yDRo

diff --git a/ctre_sim/Talon FX vers. C - 05 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 05 - 0 - ext.dat
index ea2dbc5f7469590835dc17ef0ab0bcaa02cd7202..2fc8ce8768d85b3793cc5eafc3538940ccc1f481 100644
GIT binary patch
delta 30
kcmZn=Xb{-Yz{qxzq4oDB=81|{Y$q5xes2J=7BR5_0Ie1Z+yDRo

delta 30
kcmZn=Xb{-Yz{qx*VgH{priqGHY-bqu{HXx37BR5_0Im)S+yDRo

diff --git a/ctre_sim/Talon FX vers. C - 06 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 06 - 0 - ext.dat
index a8dce6dec297d4eacf20dc676b7905f5911123fa..ff19a0b9c2d1e5c18ee1df765420fa29bf621fca 100644
GIT binary patch
delta 30
kcmZn=Xb{-Yz{qxzq4oDB=81|{Y$q5xes2J=7BR5_0Ie1Z+yDRo

delta 30
kcmZn=Xb{-Yz{qx*VgH{priqGHY-bqu{HXx37BR5_0Im)S+yDRo

diff --git a/ctre_sim/Talon FX vers. C - 07 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 07 - 0 - ext.dat
index ec4ab94b478e8f26580c9b30b11c17fe019fdeb7..b89067fbb4a7264a799dd5bb65befe44e920ef64 100644
GIT binary patch
delta 30
kcmZn=Xb{-Yz{qxzq4oDB=81|{Y$q5xes2J=7BR5_0Ie1Z+yDRo

delta 30
kcmZn=Xb{-Yz{qx*VgH{priqGHY-bqu{HXx37BR5_0Im)S+yDRo

diff --git a/ctre_sim/Talon FX vers. C - 09 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 09 - 0 - ext.dat
index 949d405611073cddbd652ee812351a1e68fd2747..19f15a02966512f9c542c009f18c5281a245e51c 100644
GIT binary patch
delta 30
kcmZn=Xb{-Yz{qxzq4oDB=81|{Y$q5xes2J=7BR5_0Ie1Z+yDRo

delta 30
kcmZn=Xb{-Yz{qx*VgH{priqGHY-bqu{HXx37BR5_0Im)S+yDRo

diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp
index 6015b19a..22f12073 100644
--- a/src/main/cpp/Robot.cpp
+++ b/src/main/cpp/Robot.cpp
@@ -157,10 +157,22 @@ void Robot::TestInit() {}
 void Robot::TestPeriodic() {}
 
 void Robot::SimulationInit() {
+  std::string x = "[";
+  std::string y = "[";
   //_vision->GetDistanceToTarget(16);
   for (int i = 1; i< 17; i++) {
-    _vision->AlignToTarget(i, 0_m, _swerveDrive);
-  }  
+    for (int j = 0; j<17; j++) {
+      frc::Pose2d pose = _vision->AlignToTarget(i, units::meter_t{j * 0.1}, _swerveDrive);
+      x += std::to_string(pose.X().value()) + ",";
+      y += std::to_string(pose.Y().value()) + ",";
+    }
+  }
+
+  x += "]";
+  y += "]";
+
+  std::cout << x << std::endl;
+  std::cout << y << std::endl;
 }
 
 void Robot::SimulationPeriodic() {}
diff --git a/src/main/cpp/vision/Vision.cpp b/src/main/cpp/vision/Vision.cpp
index 8cf9a46b..3e8ef676 100644
--- a/src/main/cpp/vision/Vision.cpp
+++ b/src/main/cpp/vision/Vision.cpp
@@ -3,6 +3,7 @@
 // of the MIT License at the root of this project
 
 #include "vision/Vision.h"
+#include "units/math.h"
 
 FMAP::FMAP(std::string path) : _path(path) {
   std::cout << "Parsing FMAP" << std::endl;
@@ -82,16 +83,28 @@ std::pair<units::meter_t, units::degree_t> Vision::GetDistanceToTarget(VisionTar
   for (int i = 0; i < tags.size(); i++) {
     if (tags[i].id == static_cast<int>(target)) {
       AprilTag tag = tags[i];
-      frc::Pose3d pose = _limelight->GetPose();
-      //frc::Pose3d pose = frc::Pose3d(0_m, 0_m, 0_m, frc::Rotation3d(0_deg, 0_deg, 0_deg));
+      //frc::Pose3d pose = _limelight->GetPose();
+      frc::Pose3d pose = frc::Pose3d(0_m, 0_m, 0_m, frc::Rotation3d(0_deg, 0_deg, 0_deg));
 
-      units::meter_t distance = pose.Translation().Distance(tag.pos.Translation());
+      SetMode(VisionModes::kAprilTag);
 
-      units::degree_t angle = units::math::atan((pose.X() - tag.pos.X()) / (pose.Y() - tag.pos.Y()));
+      // Get distance to target
+      // Get current position from Limelight
+      //frc::Pose2d current_pose = _limelight->GetPose().ToPose2d();
+      frc::Pose2d current_pose = frc::Pose2d(0_m, 0_m, 0_deg);
 
-      std::cout << "Distance: " << distance.value() << " Angle: " << angle.value() << std::endl;
+      units::meter_t a = tag.pos.X() - current_pose.X();
+      units::meter_t b = tag.pos.Y() - current_pose.Y();
 
-      return std::make_pair(distance, angle);
+      units::radian_t theta = units::math::atan2(b, a);
+      
+      //std::cout << "A: " << a.value() << ", B:" << b.value() << std::endl;
+      units::meter_t r = units::meter_t{std::sqrt(std::pow(a.value(), 2) + std::pow(b.value(), 2))};
+
+      units::meter_t x = current_pose.X() + r * units::math::cos(theta);
+      units::meter_t y = current_pose.Y() + r * units::math::sin(theta);
+      
+      return std::make_pair(r, theta);
     }
   }
 
@@ -104,23 +117,34 @@ std::pair<units::meter_t, units::degree_t> Vision::GetDistanceToTarget(int id) {
   }
 
   SetMode(VisionModes::kAprilTag);
-
+      
   std::vector<AprilTag> tags = _fmap.GetTags();
 
   for (int i = 0; i < tags.size(); i++) {
     if (tags[i].id == id) {
       AprilTag tag = tags[i];
-      frc::Pose3d pose = _limelight->GetPose();
-      //frc::Pose3d pose = frc::Pose3d(0_m, 0_m, 0_m, frc::Rotation3d(0_deg, 0_deg, 0_deg));
+      //frc::Pose3d pose = _limelight->GetPose();
+      frc::Pose3d pose = frc::Pose3d(0_m, 0_m, 0_m, frc::Rotation3d(0_deg, 0_deg, 0_deg));
 
-      // units::meter_t distance = pose.Translation().Distance(tag.pos.Translation());
-      units::meter_t distance = units::meter_t{std::sqrt(std::pow(tag.pos.X().value(), 2) + std::pow(tag.pos.Y().value(), 2))};
+      SetMode(VisionModes::kAprilTag);
 
-      units::degree_t angle = units::math::atan((pose.X() - tag.pos.X()) / (pose.Y() - tag.pos.Y()));
+      // Get distance to target
+      // Get current position from Limelight
+      //frc::Pose2d current_pose = _limelight->GetPose().ToPose2d();
+      frc::Pose2d current_pose = frc::Pose2d(0_m, 0_m, 0_deg);
 
-      //std::cout << "Distance: " << distance.value() << " Angle: " << angle.value() << std::endl;
+      units::meter_t a = tag.pos.X() - current_pose.X();
+      units::meter_t b = tag.pos.Y() - current_pose.Y();
 
-      return std::make_pair(distance, angle);
+      units::radian_t theta = units::math::atan2(b, a);
+      
+      //std::cout << "A: " << a.value() << ", B:" << b.value() << std::endl;
+      units::meter_t r = units::meter_t{std::sqrt(std::pow(a.value(), 2) + std::pow(b.value(), 2))};
+
+      units::meter_t x = current_pose.X() + r * units::math::cos(theta);
+      units::meter_t y = current_pose.Y() + r * units::math::sin(theta);
+      
+      return std::make_pair(r, theta);
     }
   }
 
@@ -134,27 +158,35 @@ std::vector<AprilTag> Vision::GetTags() {
 frc::Pose2d Vision::AlignToTarget(VisionTarget target, units::meter_t offset, wom::SwerveDrive* swerveDrive) {
   SetMode(VisionModes::kAprilTag);
 
-  std::pair<units::meter_t, units::degree_t> distance = GetDistanceToTarget(target);
-
+  // Get distance to target
+  AprilTag tag = GetTags()[static_cast<int>(target) - 1];
   // Get current position from Limelight
-  frc::Pose2d current_pose = _limelight->GetPose().ToPose2d();
-  //frc::Pose2d current_pose = frc::Pose2d(0_m, 0_m, 0_deg);
+  //frc::Pose2d current_pose = _limelight->GetPose().ToPose2d();
+  frc::Pose2d current_pose = frc::Pose2d(0_m, 0_m, 0_deg);
+
+  units::meter_t a = tag.pos.X() - current_pose.X();
+  units::meter_t b = tag.pos.Y() - current_pose.Y();
+
+  units::radian_t theta = units::math::atan2(b, a);
   
-  units::meter_t x_offset = distance.first * units::math::cos(distance.second);
-  units::meter_t y_offset = distance.first * units::math::sin(distance.second);
+  //std::cout << "A: " << a.value() << ", B:" << b.value() << std::endl;
+  units::meter_t r = units::meter_t{std::sqrt(std::pow(a.value(), 2) + std::pow(b.value(), 2))};
+
+  units::meter_t x = current_pose.X() + r * units::math::cos(theta);
+  units::meter_t y = current_pose.Y() + r * units::math::sin(theta);
 
   // reduce both offsets by the offset parameter (in relative amount)
-  x_offset -= offset * units::math::cos(distance.second);
-  y_offset -= offset * units::math::sin(distance.second);
+  x += offset * units::math::cos(theta);
+  y += offset * units::math::sin(theta);
 
-  frc::Pose2d pose = frc::Pose2d(y_offset, x_offset, distance.second);
   
+  frc::Pose2d pose = frc::Pose2d(x, y, theta); 
 
   // Print the results
-  std::cout << "Aligning to tag " << static_cast<int>(target) << ": X: " << pose.X().value() << " Y: " << pose.Y().value() << " Angle: " << pose.Rotation().Degrees().value() << ". At offset " << offset.value() << "." << std::endl;
+  //std::cout << "Aligning to tag " << target << ": X: " << pose.X().value() << " Y: " << pose.Y().value() << " Angle: " << pose.Rotation().Degrees().value() << ". At offset " << offset.value() << "." << std::endl;
 
   // Set the new pose to the swerve drive
-  swerveDrive->SetPose(pose);
+  //swerveDrive->SetPose(pose);
 
   return pose;
 }
@@ -164,23 +196,31 @@ frc::Pose2d Vision::AlignToTarget(int target, units::meter_t offset, wom::Swerve
 
   // Get distance to target
   std::pair<units::meter_t, units::degree_t> distance = GetDistanceToTarget(target);
-
+  AprilTag tag = GetTags()[target - 1];
   // Get current position from Limelight
-  frc::Pose2d current_pose = _limelight->GetPose().ToPose2d();
-  //frc::Pose2d current_pose = frc::Pose2d(0_m, 0_m, 0_deg);
+  //frc::Pose2d current_pose = _limelight->GetPose().ToPose2d();
+  frc::Pose2d current_pose = frc::Pose2d(0_m, 0_m, 0_deg);
+
+  units::meter_t a = tag.pos.X() - current_pose.X();
+  units::meter_t b = tag.pos.Y() - current_pose.Y();
+
+  units::radian_t theta = units::math::atan2(b, a);
   
-  units::meter_t x_offset = distance.first * units::math::cos(distance.second);
-  units::meter_t y_offset = distance.first * units::math::sin(distance.second);
+  //std::cout << "A: " << a.value() << ", B:" << b.value() << std::endl;
+  units::meter_t r = units::meter_t{std::sqrt(std::pow(a.value(), 2) + std::pow(b.value(), 2))};
+
+  units::meter_t x = current_pose.X() + r * units::math::cos(theta);
+  units::meter_t y = current_pose.Y() + r * units::math::sin(theta);
 
   // reduce both offsets by the offset parameter (in relative amount)
-  x_offset -= offset * units::math::cos(distance.second);
-  y_offset -= offset * units::math::sin(distance.second);
+  x += offset * units::math::cos(theta);
+  y += offset * units::math::sin(theta);
 
-  frc::Pose2d pose = frc::Pose2d(y_offset, x_offset, distance.second);
   
+  frc::Pose2d pose = frc::Pose2d(x, y, theta); 
 
   // Print the results
-  std::cout << "Aligning to tag " << target << ": X: " << pose.X().value() << " Y: " << pose.Y().value() << " Angle: " << pose.Rotation().Degrees().value() << ". At offset " << offset.value() << "." << std::endl;
+  //std::cout << "Aligning to tag " << target << ": X: " << pose.X().value() << " Y: " << pose.Y().value() << " Angle: " << pose.Rotation().Degrees().value() << ". At offset " << offset.value() << "." << std::endl;
 
   // Set the new pose to the swerve drive
   //swerveDrive->SetPose(pose);
@@ -191,39 +231,34 @@ frc::Pose2d Vision::AlignToTarget(int target, units::meter_t offset, wom::Swerve
 frc::Pose2d Vision::AlignToTarget(VisionTarget target, frc::Translation2d offset, wom::SwerveDrive* swerveDrive) {
   SetMode(VisionModes::kAprilTag);
 
-  std::pair<units::meter_t, units::degree_t> distance = GetDistanceToTarget(target);
-
+  // Get distance to target
+  AprilTag tag = GetTags()[static_cast<int>(target) - 1];
   // Get current position from Limelight
-  frc::Pose2d current_pose = _limelight->GetPose().ToPose2d();
-  //frc::Pose2d current_pose = frc::Pose2d(0_m, 0_m, 0_deg);
-  
-  units::meter_t x_offset = distance.first * units::math::cos(distance.second);
-  units::meter_t y_offset = distance.first * units::math::sin(distance.second);
+  //frc::Pose2d current_pose = _limelight->GetPose().ToPose2d();
+  frc::Pose2d current_pose = frc::Pose2d(0_m, 0_m, 0_deg);
 
-  // reduce both offsets by the offset parameter (in relative amount)
-  //x_offset -= offset * units::math::cos(distance.second);
-  //y_offset -= offset * units::math::sin(distance.second);
+  units::meter_t a = tag.pos.X() - current_pose.X();
+  units::meter_t b = tag.pos.Y() - current_pose.Y();
 
-  if (offset.X() > 0_m) {
-    x_offset -= offset.X() * units::math::cos(distance.second);
-  } else {
-    x_offset += offset.X() * units::math::cos(distance.second);
-  }
+  units::radian_t theta = units::math::atan2(b, a);
+  
+  //std::cout << "A: " << a.value() << ", B:" << b.value() << std::endl;
+  units::meter_t r = units::meter_t{std::sqrt(std::pow(a.value(), 2) + std::pow(b.value(), 2))};
 
-  if (offset.Y() > 0_m) {
-    y_offset -= offset.Y() * units::math::sin(distance.second);
-  } else {
-    y_offset += offset.Y() * units::math::sin(distance.second);
-  }
+  units::meter_t x = current_pose.X() + r * units::math::cos(theta);
+  units::meter_t y = current_pose.Y() + r * units::math::sin(theta);
 
-  frc::Pose2d pose = frc::Pose2d(y_offset, x_offset, distance.second);
+  // reduce both offsets by the offset parameter (in relative amount)
+  x += offset.X();
+  y += offset.Y();
   
+  frc::Pose2d pose = frc::Pose2d(x, y, theta); 
 
   // Print the results
-  std::cout << "Aligning to tag " << static_cast<int>(target) << ": X: " << pose.X().value() << " Y: " << pose.Y().value() << " Angle: " << pose.Rotation().Degrees().value() << ". At offset: X: " << offset.X().value() << ", Y: " << offset.Y().value() << "." << std::endl;
+  //std::cout << "Aligning to tag " << target << ": X: " << pose.X().value() << " Y: " << pose.Y().value() << " Angle: " << pose.Rotation().Degrees().value() << ". At offset " << offset.value() << "." << std::endl;
 
   // Set the new pose to the swerve drive
-  swerveDrive->SetPose(pose);
+  //swerveDrive->SetPose(pose);
 
   return pose;
 }
@@ -233,19 +268,31 @@ frc::Pose2d Vision::AlignToTarget(int target, frc::Translation2d offset, wom::Sw
 
   // Get distance to target
   std::pair<units::meter_t, units::degree_t> distance = GetDistanceToTarget(target);
-
+  AprilTag tag = GetTags()[target - 1];
   // Get current position from Limelight
-  frc::Pose2d current_pose = _limelight->GetPose().ToPose2d();
-  //frc::Pose2d current_pose = frc::Pose2d(0_m, 0_m, 0_deg);
+  //frc::Pose2d current_pose = _limelight->GetPose().ToPose2d();
+  frc::Pose2d current_pose = frc::Pose2d(0_m, 0_m, 0_deg);
+
+  units::meter_t a = tag.pos.X() - current_pose.X();
+  units::meter_t b = tag.pos.Y() - current_pose.Y();
+
+  units::radian_t theta = units::math::atan2(b, a);
   
-  units::meter_t x_offset = distance.first * units::math::cos(distance.second);
-  units::meter_t y_offset = distance.first * units::math::sin(distance.second);
+  //std::cout << "A: " << a.value() << ", B:" << b.value() << std::endl;
+  units::meter_t r = units::meter_t{std::sqrt(std::pow(a.value(), 2) + std::pow(b.value(), 2))};
+
+  units::meter_t x = current_pose.X() + r * units::math::cos(theta);
+  units::meter_t y = current_pose.Y() + r * units::math::sin(theta);
+
+  // reduce both offsets by the offset parameter (in relative amount)
+  x += offset.X();
+  y += offset.Y();
 
-  frc::Pose2d pose = frc::Pose2d(y_offset, x_offset, distance.second);
   
+  frc::Pose2d pose = frc::Pose2d(x, y, theta); 
 
   // Print the results
-  std::cout << "Aligning to tag " << target << ": X: " << pose.X().value() << " Y: " << pose.Y().value() << " Angle: " << pose.Rotation().Degrees().value() << ". At offset: X: " << offset.X().value() << ", Y: " << offset.Y().value() << "." << std::endl;
+  //std::cout << "Aligning to tag " << target << ": X: " << pose.X().value() << " Y: " << pose.Y().value() << " Angle: " << pose.Rotation().Degrees().value() << ". At offset " << offset.value() << "." << std::endl;
 
   // Set the new pose to the swerve drive
   //swerveDrive->SetPose(pose);

From ae1a0fcd51e3c4b488d44f4f839a7f827fe7de73 Mon Sep 17 00:00:00 2001
From: Paul Hodges <paul.hodges@student.education.wa.edu.au>
Date: Mon, 29 Jan 2024 12:40:30 +0800
Subject: [PATCH 112/207] [wpiformat + add ctre_sim to gitignore]

---
 .gitignore                     |   4 ++
 src/main/cpp/Robot.cpp         |  15 ++--
 src/main/cpp/vision/Vision.cpp | 127 +++++++++++++++++----------------
 3 files changed, 80 insertions(+), 66 deletions(-)

diff --git a/.gitignore b/.gitignore
index 50973484..f78e01b4 100644
--- a/.gitignore
+++ b/.gitignore
@@ -1,6 +1,10 @@
 # This gitignore has been specially created by the WPILib team.
 # If you remove items from this file, intellisense might break.
 
+### Sim ###
+
+ctre_sim
+
 ### C++ ###
 # Prerequisites
 *.d
diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp
index 22f12073..f97cf520 100644
--- a/src/main/cpp/Robot.cpp
+++ b/src/main/cpp/Robot.cpp
@@ -148,7 +148,14 @@ void Robot::TeleopInit() {
 
 }
 
-void Robot::TeleopPeriodic() {}
+void Robot::TeleopPeriodic() {
+  // std::cout << "Current AprilTag: " << _vision->CurrentAprilTag() << std::endl;
+  // std::cout << "Current Target: " << _vision->TargetIsVisible(VisionTargetObjects::kNote) << std::endl;
+  std::cout << "Dist to target: " << _vision->GetDistanceToTarget(VisionTarget::kBlueAmp).first.value()
+            << std::endl;
+  std::cout << "Angle to target: " << _vision->GetDistanceToTarget(VisionTarget::kBlueAmp).second.value()
+            << std::endl;
+}
 
 void Robot::DisabledInit() {}
 void Robot::DisabledPeriodic() {}
@@ -159,9 +166,9 @@ void Robot::TestPeriodic() {}
 void Robot::SimulationInit() {
   std::string x = "[";
   std::string y = "[";
-  //_vision->GetDistanceToTarget(16);
-  for (int i = 1; i< 17; i++) {
-    for (int j = 0; j<17; j++) {
+  // _vision->GetDistanceToTarget(16);
+  for (int i = 1; i < 17; i++) {
+    for (int j = 0; j < 17; j++) {
       frc::Pose2d pose = _vision->AlignToTarget(i, units::meter_t{j * 0.1}, _swerveDrive);
       x += std::to_string(pose.X().value()) + ",";
       y += std::to_string(pose.Y().value()) + ",";
diff --git a/src/main/cpp/vision/Vision.cpp b/src/main/cpp/vision/Vision.cpp
index 3e8ef676..b79d61d3 100644
--- a/src/main/cpp/vision/Vision.cpp
+++ b/src/main/cpp/vision/Vision.cpp
@@ -3,6 +3,7 @@
 // of the MIT License at the root of this project
 
 #include "vision/Vision.h"
+
 #include "units/math.h"
 
 FMAP::FMAP(std::string path) : _path(path) {
@@ -24,38 +25,39 @@ FMAP::FMAP(std::string path) : _path(path) {
 
   for (auto& fiducial : j["fiducials"]) {
     std::cout << "Loading AprilTag " << fiducial["id"] << std::endl;
-      AprilTag tag;
-      tag.id = fiducial["id"];
-      tag.size = fiducial["size"];
-      if (fiducial["unique"] == 1) {
-          tag.unique = true;
-      } else {
-          tag.unique = false;
-      }
+    AprilTag tag;
+    tag.id = fiducial["id"];
+    tag.size = fiducial["size"];
+    if (fiducial["unique"] == 1) {
+      tag.unique = true;
+    } else {
+      tag.unique = false;
+    }
 
-      const auto& transform = fiducial["transform"];
-      for (int i = 0; i < 4; ++i) {
-          for (int j = 0; j < 4; ++j) {
-               tag.transform[i][j] = transform[i * 4 + j];
-          }
+    const auto& transform = fiducial["transform"];
+    for (int i = 0; i < 4; ++i) {
+      for (int j = 0; j < 4; ++j) {
+        tag.transform[i][j] = transform[i * 4 + j];
       }
+    }
 
-      tag.yaw = units::radian_t{tag.transform[0][0]};
-      tag.pitch = units::radian_t{tag.transform[1][1]};
-      tag.roll = units::radian_t{tag.transform[2][2]};
+    tag.yaw = units::radian_t{tag.transform[0][0]};
+    tag.pitch = units::radian_t{tag.transform[1][1]};
+    tag.roll = units::radian_t{tag.transform[2][2]};
 
-      tag.pos = frc::Pose3d(units::meter_t{tag.transform[0][3]}, units::meter_t{tag.transform[1][3]}, units::meter_t{tag.transform[2][3]}, frc::Rotation3d(0_deg, 0_deg, 0_deg));
+    tag.pos = frc::Pose3d(units::meter_t{tag.transform[0][3]}, units::meter_t{tag.transform[1][3]},
+                          units::meter_t{tag.transform[2][3]}, frc::Rotation3d(0_deg, 0_deg, 0_deg));
 
-       _tags.push_back(tag);
-    }
+    _tags.push_back(tag);
+  }
 
   file.close();
 
   std::cout << "Loaded " << _tags.size() << " tags" << std::endl;
 
   for (int i = 0; i < _tags.size(); i++) {
-    std::cout << "Tag " << _tags[i].id << " is at: X: " << _tags[i].pos.X().value() << " Y: " << _tags[i].pos.Y().value()
-              << " Z: " << _tags[i].pos.Z().value() << std::endl;
+    std::cout << "Tag " << _tags[i].id << " is at: X: " << _tags[i].pos.X().value()
+              << " Y: " << _tags[i].pos.Y().value() << " Z: " << _tags[i].pos.Z().value() << std::endl;
   }
 
   std::cout << "Loaded FMAP" << std::endl;
@@ -83,27 +85,27 @@ std::pair<units::meter_t, units::degree_t> Vision::GetDistanceToTarget(VisionTar
   for (int i = 0; i < tags.size(); i++) {
     if (tags[i].id == static_cast<int>(target)) {
       AprilTag tag = tags[i];
-      //frc::Pose3d pose = _limelight->GetPose();
+      // frc::Pose3d pose = _limelight->GetPose();
       frc::Pose3d pose = frc::Pose3d(0_m, 0_m, 0_m, frc::Rotation3d(0_deg, 0_deg, 0_deg));
 
       SetMode(VisionModes::kAprilTag);
 
       // Get distance to target
       // Get current position from Limelight
-      //frc::Pose2d current_pose = _limelight->GetPose().ToPose2d();
+      // frc::Pose2d current_pose = _limelight->GetPose().ToPose2d();
       frc::Pose2d current_pose = frc::Pose2d(0_m, 0_m, 0_deg);
 
       units::meter_t a = tag.pos.X() - current_pose.X();
       units::meter_t b = tag.pos.Y() - current_pose.Y();
 
       units::radian_t theta = units::math::atan2(b, a);
-      
-      //std::cout << "A: " << a.value() << ", B:" << b.value() << std::endl;
+
+      // std::cout << "A: " << a.value() << ", B:" << b.value() << std::endl;
       units::meter_t r = units::meter_t{std::sqrt(std::pow(a.value(), 2) + std::pow(b.value(), 2))};
 
       units::meter_t x = current_pose.X() + r * units::math::cos(theta);
       units::meter_t y = current_pose.Y() + r * units::math::sin(theta);
-      
+
       return std::make_pair(r, theta);
     }
   }
@@ -117,33 +119,33 @@ std::pair<units::meter_t, units::degree_t> Vision::GetDistanceToTarget(int id) {
   }
 
   SetMode(VisionModes::kAprilTag);
-      
+
   std::vector<AprilTag> tags = _fmap.GetTags();
 
   for (int i = 0; i < tags.size(); i++) {
     if (tags[i].id == id) {
       AprilTag tag = tags[i];
-      //frc::Pose3d pose = _limelight->GetPose();
+      // frc::Pose3d pose = _limelight->GetPose();
       frc::Pose3d pose = frc::Pose3d(0_m, 0_m, 0_m, frc::Rotation3d(0_deg, 0_deg, 0_deg));
 
       SetMode(VisionModes::kAprilTag);
 
       // Get distance to target
       // Get current position from Limelight
-      //frc::Pose2d current_pose = _limelight->GetPose().ToPose2d();
+      // frc::Pose2d current_pose = _limelight->GetPose().ToPose2d();
       frc::Pose2d current_pose = frc::Pose2d(0_m, 0_m, 0_deg);
 
       units::meter_t a = tag.pos.X() - current_pose.X();
       units::meter_t b = tag.pos.Y() - current_pose.Y();
 
       units::radian_t theta = units::math::atan2(b, a);
-      
-      //std::cout << "A: " << a.value() << ", B:" << b.value() << std::endl;
+
+      // std::cout << "A: " << a.value() << ", B:" << b.value() << std::endl;
       units::meter_t r = units::meter_t{std::sqrt(std::pow(a.value(), 2) + std::pow(b.value(), 2))};
 
       units::meter_t x = current_pose.X() + r * units::math::cos(theta);
       units::meter_t y = current_pose.Y() + r * units::math::sin(theta);
-      
+
       return std::make_pair(r, theta);
     }
   }
@@ -161,15 +163,15 @@ frc::Pose2d Vision::AlignToTarget(VisionTarget target, units::meter_t offset, wo
   // Get distance to target
   AprilTag tag = GetTags()[static_cast<int>(target) - 1];
   // Get current position from Limelight
-  //frc::Pose2d current_pose = _limelight->GetPose().ToPose2d();
+  // frc::Pose2d current_pose = _limelight->GetPose().ToPose2d();
   frc::Pose2d current_pose = frc::Pose2d(0_m, 0_m, 0_deg);
 
   units::meter_t a = tag.pos.X() - current_pose.X();
   units::meter_t b = tag.pos.Y() - current_pose.Y();
 
   units::radian_t theta = units::math::atan2(b, a);
-  
-  //std::cout << "A: " << a.value() << ", B:" << b.value() << std::endl;
+
+  // std::cout << "A: " << a.value() << ", B:" << b.value() << std::endl;
   units::meter_t r = units::meter_t{std::sqrt(std::pow(a.value(), 2) + std::pow(b.value(), 2))};
 
   units::meter_t x = current_pose.X() + r * units::math::cos(theta);
@@ -179,14 +181,14 @@ frc::Pose2d Vision::AlignToTarget(VisionTarget target, units::meter_t offset, wo
   x += offset * units::math::cos(theta);
   y += offset * units::math::sin(theta);
 
-  
-  frc::Pose2d pose = frc::Pose2d(x, y, theta); 
+  frc::Pose2d pose = frc::Pose2d(x, y, theta);
 
   // Print the results
-  //std::cout << "Aligning to tag " << target << ": X: " << pose.X().value() << " Y: " << pose.Y().value() << " Angle: " << pose.Rotation().Degrees().value() << ". At offset " << offset.value() << "." << std::endl;
+  // std::cout << "Aligning to tag " << target << ": X: " << pose.X().value() << " Y: " << pose.Y().value() <<
+  // " Angle: " << pose.Rotation().Degrees().value() << ". At offset " << offset.value() << "." << std::endl;
 
   // Set the new pose to the swerve drive
-  //swerveDrive->SetPose(pose);
+  // swerveDrive->SetPose(pose);
 
   return pose;
 }
@@ -198,15 +200,15 @@ frc::Pose2d Vision::AlignToTarget(int target, units::meter_t offset, wom::Swerve
   std::pair<units::meter_t, units::degree_t> distance = GetDistanceToTarget(target);
   AprilTag tag = GetTags()[target - 1];
   // Get current position from Limelight
-  //frc::Pose2d current_pose = _limelight->GetPose().ToPose2d();
+  // frc::Pose2d current_pose = _limelight->GetPose().ToPose2d();
   frc::Pose2d current_pose = frc::Pose2d(0_m, 0_m, 0_deg);
 
   units::meter_t a = tag.pos.X() - current_pose.X();
   units::meter_t b = tag.pos.Y() - current_pose.Y();
 
   units::radian_t theta = units::math::atan2(b, a);
-  
-  //std::cout << "A: " << a.value() << ", B:" << b.value() << std::endl;
+
+  // std::cout << "A: " << a.value() << ", B:" << b.value() << std::endl;
   units::meter_t r = units::meter_t{std::sqrt(std::pow(a.value(), 2) + std::pow(b.value(), 2))};
 
   units::meter_t x = current_pose.X() + r * units::math::cos(theta);
@@ -216,33 +218,34 @@ frc::Pose2d Vision::AlignToTarget(int target, units::meter_t offset, wom::Swerve
   x += offset * units::math::cos(theta);
   y += offset * units::math::sin(theta);
 
-  
-  frc::Pose2d pose = frc::Pose2d(x, y, theta); 
+  frc::Pose2d pose = frc::Pose2d(x, y, theta);
 
   // Print the results
-  //std::cout << "Aligning to tag " << target << ": X: " << pose.X().value() << " Y: " << pose.Y().value() << " Angle: " << pose.Rotation().Degrees().value() << ". At offset " << offset.value() << "." << std::endl;
+  // std::cout << "Aligning to tag " << target << ": X: " << pose.X().value() << " Y: " << pose.Y().value() <<
+  // " Angle: " << pose.Rotation().Degrees().value() << ". At offset " << offset.value() << "." << std::endl;
 
   // Set the new pose to the swerve drive
-  //swerveDrive->SetPose(pose);
+  // swerveDrive->SetPose(pose);
 
   return pose;
 }
 
-frc::Pose2d Vision::AlignToTarget(VisionTarget target, frc::Translation2d offset, wom::SwerveDrive* swerveDrive) {
+frc::Pose2d Vision::AlignToTarget(VisionTarget target, frc::Translation2d offset,
+                                  wom::SwerveDrive* swerveDrive) {
   SetMode(VisionModes::kAprilTag);
 
   // Get distance to target
   AprilTag tag = GetTags()[static_cast<int>(target) - 1];
   // Get current position from Limelight
-  //frc::Pose2d current_pose = _limelight->GetPose().ToPose2d();
+  // frc::Pose2d current_pose = _limelight->GetPose().ToPose2d();
   frc::Pose2d current_pose = frc::Pose2d(0_m, 0_m, 0_deg);
 
   units::meter_t a = tag.pos.X() - current_pose.X();
   units::meter_t b = tag.pos.Y() - current_pose.Y();
 
   units::radian_t theta = units::math::atan2(b, a);
-  
-  //std::cout << "A: " << a.value() << ", B:" << b.value() << std::endl;
+
+  // std::cout << "A: " << a.value() << ", B:" << b.value() << std::endl;
   units::meter_t r = units::meter_t{std::sqrt(std::pow(a.value(), 2) + std::pow(b.value(), 2))};
 
   units::meter_t x = current_pose.X() + r * units::math::cos(theta);
@@ -251,14 +254,15 @@ frc::Pose2d Vision::AlignToTarget(VisionTarget target, frc::Translation2d offset
   // reduce both offsets by the offset parameter (in relative amount)
   x += offset.X();
   y += offset.Y();
-  
-  frc::Pose2d pose = frc::Pose2d(x, y, theta); 
+
+  frc::Pose2d pose = frc::Pose2d(x, y, theta);
 
   // Print the results
-  //std::cout << "Aligning to tag " << target << ": X: " << pose.X().value() << " Y: " << pose.Y().value() << " Angle: " << pose.Rotation().Degrees().value() << ". At offset " << offset.value() << "." << std::endl;
+  // std::cout << "Aligning to tag " << target << ": X: " << pose.X().value() << " Y: " << pose.Y().value() <<
+  // " Angle: " << pose.Rotation().Degrees().value() << ". At offset " << offset.value() << "." << std::endl;
 
   // Set the new pose to the swerve drive
-  //swerveDrive->SetPose(pose);
+  // swerveDrive->SetPose(pose);
 
   return pose;
 }
@@ -270,15 +274,15 @@ frc::Pose2d Vision::AlignToTarget(int target, frc::Translation2d offset, wom::Sw
   std::pair<units::meter_t, units::degree_t> distance = GetDistanceToTarget(target);
   AprilTag tag = GetTags()[target - 1];
   // Get current position from Limelight
-  //frc::Pose2d current_pose = _limelight->GetPose().ToPose2d();
+  // frc::Pose2d current_pose = _limelight->GetPose().ToPose2d();
   frc::Pose2d current_pose = frc::Pose2d(0_m, 0_m, 0_deg);
 
   units::meter_t a = tag.pos.X() - current_pose.X();
   units::meter_t b = tag.pos.Y() - current_pose.Y();
 
   units::radian_t theta = units::math::atan2(b, a);
-  
-  //std::cout << "A: " << a.value() << ", B:" << b.value() << std::endl;
+
+  // std::cout << "A: " << a.value() << ", B:" << b.value() << std::endl;
   units::meter_t r = units::meter_t{std::sqrt(std::pow(a.value(), 2) + std::pow(b.value(), 2))};
 
   units::meter_t x = current_pose.X() + r * units::math::cos(theta);
@@ -288,19 +292,18 @@ frc::Pose2d Vision::AlignToTarget(int target, frc::Translation2d offset, wom::Sw
   x += offset.X();
   y += offset.Y();
 
-  
-  frc::Pose2d pose = frc::Pose2d(x, y, theta); 
+  frc::Pose2d pose = frc::Pose2d(x, y, theta);
 
   // Print the results
-  //std::cout << "Aligning to tag " << target << ": X: " << pose.X().value() << " Y: " << pose.Y().value() << " Angle: " << pose.Rotation().Degrees().value() << ". At offset " << offset.value() << "." << std::endl;
+  // std::cout << "Aligning to tag " << target << ": X: " << pose.X().value() << " Y: " << pose.Y().value() <<
+  // " Angle: " << pose.Rotation().Degrees().value() << ". At offset " << offset.value() << "." << std::endl;
 
   // Set the new pose to the swerve drive
-  //swerveDrive->SetPose(pose);
+  // swerveDrive->SetPose(pose);
 
   return pose;
 }
 
-
 bool Vision::IsAtPose(frc::Pose3d pose, units::second_t dt) {
   return _limelight->IsAtSetPoseVision(pose, dt);
 }

From c096260a32978b2e26aa1e3d1c7ff528173f08b8 Mon Sep 17 00:00:00 2001
From: Paul Hodges <paul.hodges@student.education.wa.edu.au>
Date: Mon, 29 Jan 2024 15:07:53 +0800
Subject: [PATCH 113/207] Finished vision (hopefully)

---
 src/main/cpp/Robot.cpp           | 27 ++++++++++++++-------------
 src/main/cpp/vision/Vision.cpp   | 31 +++++++++++++++++++++++++++++++
 src/main/include/vision/Vision.h |  4 ++++
 3 files changed, 49 insertions(+), 13 deletions(-)

diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp
index f97cf520..0130a896 100644
--- a/src/main/cpp/Robot.cpp
+++ b/src/main/cpp/Robot.cpp
@@ -164,22 +164,23 @@ void Robot::TestInit() {}
 void Robot::TestPeriodic() {}
 
 void Robot::SimulationInit() {
-  std::string x = "[";
-  std::string y = "[";
-  // _vision->GetDistanceToTarget(16);
-  for (int i = 1; i < 17; i++) {
-    for (int j = 0; j < 17; j++) {
-      frc::Pose2d pose = _vision->AlignToTarget(i, units::meter_t{j * 0.1}, _swerveDrive);
-      x += std::to_string(pose.X().value()) + ",";
-      y += std::to_string(pose.Y().value()) + ",";
+  /*  std::string x = "[";
+    std::string y = "[";
+    // _vision->GetDistanceToTarget(16);
+    for (int i = 1; i < 17; i++) {
+      for (int j = 0; j < 17; j++) {
+        frc::Pose2d pose = _vision->AlignToTarget(i, units::meter_t{j * 0.1}, _swerveDrive);
+        x += std::to_string(pose.X().value()) + ",";
+        y += std::to_string(pose.Y().value()) + ",";
+      }
     }
-  }
 
-  x += "]";
-  y += "]";
+    x += "]";
+    y += "]";
 
-  std::cout << x << std::endl;
-  std::cout << y << std::endl;
+    std::cout << x << std::endl;
+    std::cout << y << std::endl; */
+  _vision->TurnToTarget(1, _swerveDrive);
 }
 
 void Robot::SimulationPeriodic() {}
diff --git a/src/main/cpp/vision/Vision.cpp b/src/main/cpp/vision/Vision.cpp
index b79d61d3..3c834040 100644
--- a/src/main/cpp/vision/Vision.cpp
+++ b/src/main/cpp/vision/Vision.cpp
@@ -4,6 +4,9 @@
 
 #include "vision/Vision.h"
 
+#include <variant>
+
+#include "units/length.h"
 #include "units/math.h"
 
 FMAP::FMAP(std::string path) : _path(path) {
@@ -304,6 +307,34 @@ frc::Pose2d Vision::AlignToTarget(int target, frc::Translation2d offset, wom::Sw
   return pose;
 }
 
+frc::Pose2d Vision::TurnToTarget(int target, wom::SwerveDrive* swerveDrive) {
+  AprilTag tag = GetTags()[target];
+
+  units::degree_t angle = GetDistanceToTarget(target).second;
+
+  frc::Pose2d current_pose = _limelight->GetPose().ToPose2d();
+
+  frc::Pose2d pose = frc::Pose2d(current_pose.X(), current_pose.Y(), angle);
+
+  std::cout << pose.Rotation().Degrees().value() << std::endl;
+
+  swerveDrive->SetPose(pose);
+}
+
+frc::Pose2d Vision::TurnToTarget(VisionTarget target, wom::SwerveDrive* swerveDrive) {
+  AprilTag tag = GetTags()[static_cast<int>(target)];
+
+  units::degree_t angle = GetDistanceToTarget(target).second;
+
+  frc::Pose2d current_pose = _limelight->GetPose().ToPose2d();
+
+  frc::Pose2d pose = frc::Pose2d(current_pose.X(), current_pose.Y(), angle);
+
+  std::cout << pose.Rotation().Degrees().value() << std::endl;
+
+  swerveDrive->SetPose(pose);
+}
+
 bool Vision::IsAtPose(frc::Pose3d pose, units::second_t dt) {
   return _limelight->IsAtSetPoseVision(pose, dt);
 }
diff --git a/src/main/include/vision/Vision.h b/src/main/include/vision/Vision.h
index 4c7a224e..1945b1fe 100644
--- a/src/main/include/vision/Vision.h
+++ b/src/main/include/vision/Vision.h
@@ -38,6 +38,7 @@ enum class VisionTarget {
   kRedAmp = 5,
   kRedSpeakerCenter = 4,
   kRedSpeakerOffset = 3,
+
   kRedChain1 = 12,
   kRedChain2 = 11,
   kRedChain3 = 13,
@@ -90,6 +91,9 @@ class Vision {
   frc::Pose2d AlignToTarget(VisionTarget target, frc::Translation2d offset, wom::SwerveDrive* swerveDrive);
   frc::Pose2d AlignToTarget(int target, frc::Translation2d offset, wom::SwerveDrive* swerveDrive);
 
+  frc::Pose2d TurnToTarget(int target, wom::SwerveDrive* swerveDrive);
+  frc::Pose2d TurnToTarget(VisionTarget target, wom::SwerveDrive* swerveDrive);
+
   std::vector<AprilTag> GetTags();
 
   bool IsAtPose(frc::Pose3d pose, units::second_t dt);

From 2615ea8218b0432cf9417bae99a4897adb88fad6 Mon Sep 17 00:00:00 2001
From: Paul Hodges <paul.hodges@student.education.wa.edu.au>
Date: Mon, 29 Jan 2024 16:39:39 +0800
Subject: [PATCH 114/207] More vision

---
 ctre_sim/CANCoder vers. H - 016 - 0 - ext.dat | Bin 2048 -> 2048 bytes
 ctre_sim/CANCoder vers. H - 017 - 0 - ext.dat | Bin 2048 -> 2048 bytes
 ctre_sim/CANCoder vers. H - 018 - 0 - ext.dat | Bin 2048 -> 2048 bytes
 ctre_sim/CANCoder vers. H - 019 - 0 - ext.dat | Bin 2048 -> 2048 bytes
 ctre_sim/Pigeon 2 - 020 - 0 - ext.dat         | Bin 2048 -> 2048 bytes
 ctre_sim/Talon FX vers. C - 01 - 0 - ext.dat  | Bin 2048 -> 2048 bytes
 ctre_sim/Talon FX vers. C - 02 - 0 - ext.dat  | Bin 2048 -> 2048 bytes
 ctre_sim/Talon FX vers. C - 03 - 0 - ext.dat  | Bin 2048 -> 2048 bytes
 ctre_sim/Talon FX vers. C - 04 - 0 - ext.dat  | Bin 2048 -> 2048 bytes
 ctre_sim/Talon FX vers. C - 05 - 0 - ext.dat  | Bin 2048 -> 2048 bytes
 ctre_sim/Talon FX vers. C - 06 - 0 - ext.dat  | Bin 2048 -> 2048 bytes
 ctre_sim/Talon FX vers. C - 07 - 0 - ext.dat  | Bin 2048 -> 2048 bytes
 ctre_sim/Talon FX vers. C - 09 - 0 - ext.dat  | Bin 2048 -> 2048 bytes
 simgui.json                                   |   3 +++
 src/main/cpp/Robot.cpp                        |  12 ++++++++-
 src/main/cpp/vision/Vision.cpp                |  23 +++++++++++++++---
 src/main/include/vision/Vision.h              |   2 ++
 .../src/main/cpp/drivetrain/SwerveDrive.cpp   |   4 +++
 .../src/main/include/drivetrain/SwerveDrive.h |   4 ++-
 19 files changed, 42 insertions(+), 6 deletions(-)

diff --git a/ctre_sim/CANCoder vers. H - 016 - 0 - ext.dat b/ctre_sim/CANCoder vers. H - 016 - 0 - ext.dat
index b70c3556679497ee603521f2dda7d4398157875e..7af9a7c92dfa5d0375f972414c70d47775fef131 100644
GIT binary patch
delta 36
ncmZn=Xb{+tz{t6k;r7=IP6h^!iGuE&+Zb+q&4O|kF|h#v)s70h

delta 36
ncmZn=Xb{+tz{t6iq5t;=W(Ee1iGuE&yBK<YZ-R0bF|h#v)Rqdo

diff --git a/ctre_sim/CANCoder vers. H - 017 - 0 - ext.dat b/ctre_sim/CANCoder vers. H - 017 - 0 - ext.dat
index be58d0c2d2c75f1ffe8d657c982b4a688f24ce71..669137759f209f811b4743db197a98ea6b470a3a 100644
GIT binary patch
delta 30
kcmZn=Xb{+tz{s|h;o;XT&WVCvY+D$fe9Zu{7BR5_0IH4(yZ`_I

delta 30
kcmZn=Xb{+tz{s|fVe0Qq%o7E@*mf|?_`LzdTExT#0I8M=yZ`_I

diff --git a/ctre_sim/CANCoder vers. H - 018 - 0 - ext.dat b/ctre_sim/CANCoder vers. H - 018 - 0 - ext.dat
index a5a5e02fd7e38bd52b5d2e78c6a069ddb1a7ff1d..8835a41c024e6d4ed9f8c2de8ca7962412fa91d7 100644
GIT binary patch
delta 36
ncmZn=Xb{+tz{t6k;r7=IP6h^!iGuE&+Zb+q&4O|kF|h#v)s70h

delta 36
ncmZn=Xb{+tz{t6iq5t;=W(Ee1iGuE&yBK<YZ-R0bF|h#v)Rqdo

diff --git a/ctre_sim/CANCoder vers. H - 019 - 0 - ext.dat b/ctre_sim/CANCoder vers. H - 019 - 0 - ext.dat
index 1d1df01f2e5379572fc30a4936c29417c965e6e7..e7951dbf07e2c8b3c25e3036a4b58ba15d7ba128 100644
GIT binary patch
delta 30
kcmZn=Xb{+tz{s|h;o;XT&WVCvY+D$fe9Zu{7BR5_0IH4(yZ`_I

delta 30
kcmZn=Xb{+tz{s|fVe0Qq%o7E@*mf|?_`LzdTExT#0I8M=yZ`_I

diff --git a/ctre_sim/Pigeon 2 - 020 - 0 - ext.dat b/ctre_sim/Pigeon 2 - 020 - 0 - ext.dat
index 1c15fadd76a37035c4fb5dc322174ef5e4f0ae3b..941345ac17d7df96e44048b557dc28b3069039c2 100644
GIT binary patch
delta 32
mcmZn=Xb{-2fQfA@!^5vxoD&7T*tRe{`I-S@ZFXQjzybits0<AN

delta 32
mcmZn=Xb{-2fQfA<!_?oKm?sK)vF%`(@p}V^wb_CB01E)funY|V

diff --git a/ctre_sim/Talon FX vers. C - 01 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 01 - 0 - ext.dat
index c286c31b23ccbab2ae10455eb784fdb6c7aba6ba..ee4ebedc65ed22306d2234f177c2d6d5c2236c8b 100644
GIT binary patch
delta 30
kcmZn=Xb{-Yz{qx#;o{dU&WVavY)2Tbe9Zu{7BR5_0Im)S+yDRo

delta 30
kcmZn=Xb{-Yz{qxzq4oDB=81|{Y$q5xes2J=7BR5_0Ie1Z+yDRo

diff --git a/ctre_sim/Talon FX vers. C - 02 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 02 - 0 - ext.dat
index 09761e081c6233b148c8a5cfc9a8e963345694bb..948df76b81bbc157fed419f5e20da094da591c1f 100644
GIT binary patch
delta 30
kcmZn=Xb{-Yz{qx#;o{dU&WVavY)2Tbe9Zu{7BR5_0Im)S+yDRo

delta 30
kcmZn=Xb{-Yz{qxzq4oDB=81|{Y$q5xes2J=7BR5_0Ie1Z+yDRo

diff --git a/ctre_sim/Talon FX vers. C - 03 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 03 - 0 - ext.dat
index 241982f5972195b12caa6927b24a685894f9882b..7c267efd91a0f2b2aa92e9d604ec5e203624088a 100644
GIT binary patch
delta 30
kcmZn=Xb{-Yz{qx#;o{dU&WVavY)2Tbe9Zu{7BR5_0Im)S+yDRo

delta 30
kcmZn=Xb{-Yz{qxzq4oDB=81|{Y$q5xes2J=7BR5_0Ie1Z+yDRo

diff --git a/ctre_sim/Talon FX vers. C - 04 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 04 - 0 - ext.dat
index 891fd4bd1a4cca587d981de44ca775b397e424c3..ac502f4e3315670384588f08e3ae021f583877d8 100644
GIT binary patch
delta 30
kcmZn=Xb{-Yz{qx#;o{dU&WVavY)2Tbe9Zu{7BR5_0Im)S+yDRo

delta 30
kcmZn=Xb{-Yz{qxzq4oDB=81|{Y$q5xes2J=7BR5_0Ie1Z+yDRo

diff --git a/ctre_sim/Talon FX vers. C - 05 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 05 - 0 - ext.dat
index 2fc8ce8768d85b3793cc5eafc3538940ccc1f481..cb9bb70f8f10973dc4bcc2073e2b89feb8aad238 100644
GIT binary patch
delta 30
kcmZn=Xb{-Yz{qx#;o{dU&WVavY)2Tbe9Zu{7BR5_0Im)S+yDRo

delta 30
kcmZn=Xb{-Yz{qxzq4oDB=81|{Y$q5xes2J=7BR5_0Ie1Z+yDRo

diff --git a/ctre_sim/Talon FX vers. C - 06 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 06 - 0 - ext.dat
index ff19a0b9c2d1e5c18ee1df765420fa29bf621fca..113659522d70fa911d9eb144e0d2d24aa7179076 100644
GIT binary patch
delta 30
kcmZn=Xb{-Yz{qx#;o{dU&WVavY)2Tbe9Zu{7BR5_0Im)S+yDRo

delta 30
kcmZn=Xb{-Yz{qxzq4oDB=81|{Y$q5xes2J=7BR5_0Ie1Z+yDRo

diff --git a/ctre_sim/Talon FX vers. C - 07 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 07 - 0 - ext.dat
index b89067fbb4a7264a799dd5bb65befe44e920ef64..b5fbe904afc7719fd295f3b1026074af31236ff6 100644
GIT binary patch
delta 30
kcmZn=Xb{-Yz{qx#;o{dU&WVavY)2Tbe9Zu{7BR5_0Im)S+yDRo

delta 30
kcmZn=Xb{-Yz{qxzq4oDB=81|{Y$q5xes2J=7BR5_0Ie1Z+yDRo

diff --git a/ctre_sim/Talon FX vers. C - 09 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 09 - 0 - ext.dat
index 19f15a02966512f9c542c009f18c5281a245e51c..7eb8bcf1b5a63272f8196b91910c4256f82f322f 100644
GIT binary patch
delta 30
kcmZn=Xb{-Yz{qx#;o{dU&WVavY)2Tbe9Zu{7BR5_0Im)S+yDRo

delta 30
kcmZn=Xb{-Yz{qxzq4oDB=81|{Y$q5xes2J=7BR5_0Ie1Z+yDRo

diff --git a/simgui.json b/simgui.json
index 3cfca74e..17f01c64 100644
--- a/simgui.json
+++ b/simgui.json
@@ -11,6 +11,9 @@
         "pid": {
           "open": true
         }
+      },
+      "vision": {
+        "open": true
       }
     }
   },
diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp
index 0130a896..e84789d9 100644
--- a/src/main/cpp/Robot.cpp
+++ b/src/main/cpp/Robot.cpp
@@ -20,6 +20,12 @@
 
 #include "behaviour/HasBehaviour.h"
 
+#include <frc/kinematics/DifferentialDriveKinematics.h>
+#include <frc/controller/RamseteController.h>
+#include <frc/Timer.h>
+#include "networktables/NetworkTableInstance.h"
+
+
 static units::second_t lastPeriodic;
 
 void Robot::RobotInit() {
@@ -180,8 +186,12 @@ void Robot::SimulationInit() {
 
     std::cout << x << std::endl;
     std::cout << y << std::endl; */
-  _vision->TurnToTarget(1, _swerveDrive);
+  //std::cout << _vision->TurnToTarget(1, _swerveDrive).Rotation().Degrees().value() << std::endl;
+  frc::Pose2d pose = _vision->TurnToTarget(2, _swerveDrive);
+  nt::NetworkTableInstance::GetDefault().GetTable("vision")->PutNumber("rot", pose.Rotation().Degrees().value()); 
+  
 }
 
+
 void Robot::SimulationPeriodic() {}
 
diff --git a/src/main/cpp/vision/Vision.cpp b/src/main/cpp/vision/Vision.cpp
index 3c834040..66fcdeed 100644
--- a/src/main/cpp/vision/Vision.cpp
+++ b/src/main/cpp/vision/Vision.cpp
@@ -312,13 +312,16 @@ frc::Pose2d Vision::TurnToTarget(int target, wom::SwerveDrive* swerveDrive) {
 
   units::degree_t angle = GetDistanceToTarget(target).second;
 
-  frc::Pose2d current_pose = _limelight->GetPose().ToPose2d();
+  //frc::Pose2d current_pose = _limelight->GetPose().ToPose2d();
+  frc::Pose2d current_pose = frc::Pose2d();
 
   frc::Pose2d pose = frc::Pose2d(current_pose.X(), current_pose.Y(), angle);
 
-  std::cout << pose.Rotation().Degrees().value() << std::endl;
+  //std::cout << pose.Rotation().Degrees().value() << std::endl;
 
-  swerveDrive->SetPose(pose);
+  //swerveDrive->SetPose(pose);
+  
+  return pose;
 }
 
 frc::Pose2d Vision::TurnToTarget(VisionTarget target, wom::SwerveDrive* swerveDrive) {
@@ -332,7 +335,19 @@ frc::Pose2d Vision::TurnToTarget(VisionTarget target, wom::SwerveDrive* swerveDr
 
   std::cout << pose.Rotation().Degrees().value() << std::endl;
 
-  swerveDrive->SetPose(pose);
+  //swerveDrive->SetPose(pose);
+}
+
+frc::Pose2d Vision::LockOn(VisionTargetObjects object, wom::SwerveDrive* swerveDrive) {
+  SetMode(VisionModes::kRing);
+
+  if (TargetIsVisible(object)) {
+    swerveDrive->SetLocked(frc::Pose2d());
+  } else {
+    swerveDrive->SetIdle();
+  }
+
+  return frc::Pose2d();
 }
 
 bool Vision::IsAtPose(frc::Pose3d pose, units::second_t dt) {
diff --git a/src/main/include/vision/Vision.h b/src/main/include/vision/Vision.h
index 1945b1fe..0dbc7530 100644
--- a/src/main/include/vision/Vision.h
+++ b/src/main/include/vision/Vision.h
@@ -94,6 +94,8 @@ class Vision {
   frc::Pose2d TurnToTarget(int target, wom::SwerveDrive* swerveDrive);
   frc::Pose2d TurnToTarget(VisionTarget target, wom::SwerveDrive* swerveDrive);
 
+  frc::Pose2d LockOn(VisionTargetObjects target, wom::SwerveDrive* swerveDrive);
+
   std::vector<AprilTag> GetTags();
 
   bool IsAtPose(frc::Pose3d pose, units::second_t dt);
diff --git a/wombat/src/main/cpp/drivetrain/SwerveDrive.cpp b/wombat/src/main/cpp/drivetrain/SwerveDrive.cpp
index 38919cb6..16888b7e 100644
--- a/wombat/src/main/cpp/drivetrain/SwerveDrive.cpp
+++ b/wombat/src/main/cpp/drivetrain/SwerveDrive.cpp
@@ -335,6 +335,10 @@ void SwerveDrive::OnResetMode() {
   _anglePIDController.Reset();
 }
 
+void SwerveDrive::SetLocked(frc::Pose2d pose) {
+  _state = SwerveDriveState::kLockedOn;
+}
+
 void SwerveDrive::RotateMatchJoystick(units::radian_t joystickAngle, FieldRelativeSpeeds speeds) {
   // _state = SwerveDriveState::kFRVelocityRotationLock;
   // _anglePIDController.SetSetpoint(joystickAngle);
diff --git a/wombat/src/main/include/drivetrain/SwerveDrive.h b/wombat/src/main/include/drivetrain/SwerveDrive.h
index be4467b1..8eb49fa6 100644
--- a/wombat/src/main/include/drivetrain/SwerveDrive.h
+++ b/wombat/src/main/include/drivetrain/SwerveDrive.h
@@ -138,7 +138,8 @@ enum class SwerveDriveState {
   kTuning,
   kXWheels,
   kModuleTurn,
-  kFRVelocityRotationLock
+  kFRVelocityRotationLock,
+  kLockedOn
 };
 
 struct FieldRelativeSpeeds {
@@ -186,6 +187,7 @@ class SwerveDrive : public behaviour::HasBehaviour {
   void SetZero();
   void SetVoltageLimit(units::volt_t driveVoltageLimit);
   void OnResetMode();
+  void SetLocked(frc::Pose2d pose);
   // double GetModuleCANPosition(int mod);  // from liam's
 
   void SetXWheelState();

From 6f3fb2fdcf1387c1bc67cd57a1a2e38c86078e6e Mon Sep 17 00:00:00 2001
From: Paul Hodges <paul.hodges@student.education.wa.edu.au>
Date: Tue, 30 Jan 2024 10:25:48 +0800
Subject: [PATCH 115/207] Merge with swerve stuff

---
 src/main/cpp/vision/Vision.cpp | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/main/cpp/vision/Vision.cpp b/src/main/cpp/vision/Vision.cpp
index 66fcdeed..a3c4c5fb 100644
--- a/src/main/cpp/vision/Vision.cpp
+++ b/src/main/cpp/vision/Vision.cpp
@@ -106,8 +106,8 @@ std::pair<units::meter_t, units::degree_t> Vision::GetDistanceToTarget(VisionTar
       // std::cout << "A: " << a.value() << ", B:" << b.value() << std::endl;
       units::meter_t r = units::meter_t{std::sqrt(std::pow(a.value(), 2) + std::pow(b.value(), 2))};
 
-      units::meter_t x = current_pose.X() + r * units::math::cos(theta);
-      units::meter_t y = current_pose.Y() + r * units::math::sin(theta);
+      // units::meter_t x = current_pose.X() + r * units::math::cos(theta);
+      // units::meter_t y = current_pose.Y() + r * units::math::sin(theta);
 
       return std::make_pair(r, theta);
     }

From befb502e73ca308138bbf21f37b2c03353ed20e8 Mon Sep 17 00:00:00 2001
From: Paul Hodges <paul.hodges@student.education.wa.edu.au>
Date: Tue, 30 Jan 2024 12:57:21 +0800
Subject: [PATCH 116/207] More vision

---
 src/main/cpp/vision/Vision.cpp                | 64 ++++++++++++-------
 src/main/include/vision/Vision.h              |  6 +-
 .../src/main/cpp/drivetrain/SwerveDrive.cpp   | 11 +++-
 .../behaviours/SwerveBehaviours.cpp           |  8 +++
 .../src/main/include/drivetrain/SwerveDrive.h |  4 +-
 5 files changed, 65 insertions(+), 28 deletions(-)

diff --git a/src/main/cpp/vision/Vision.cpp b/src/main/cpp/vision/Vision.cpp
index a3c4c5fb..2261e06b 100644
--- a/src/main/cpp/vision/Vision.cpp
+++ b/src/main/cpp/vision/Vision.cpp
@@ -4,10 +4,12 @@
 
 #include "vision/Vision.h"
 
+#include <utility>
 #include <variant>
 
 #include "units/length.h"
 #include "units/math.h"
+#include "vision/Limelight.h"
 
 FMAP::FMAP(std::string path) : _path(path) {
   std::cout << "Parsing FMAP" << std::endl;
@@ -88,15 +90,15 @@ std::pair<units::meter_t, units::degree_t> Vision::GetDistanceToTarget(VisionTar
   for (int i = 0; i < tags.size(); i++) {
     if (tags[i].id == static_cast<int>(target)) {
       AprilTag tag = tags[i];
-      // frc::Pose3d pose = _limelight->GetPose();
-      frc::Pose3d pose = frc::Pose3d(0_m, 0_m, 0_m, frc::Rotation3d(0_deg, 0_deg, 0_deg));
+      //frc::Pose3d pose = _limelight->GetPose();
+      //frc::Pose3d pose = frc::Pose3d(0_m, 0_m, 0_m, frc::Rotation3d(0_deg, 0_deg, 0_deg));
 
       SetMode(VisionModes::kAprilTag);
 
       // Get distance to target
       // Get current position from Limelight
-      // frc::Pose2d current_pose = _limelight->GetPose().ToPose2d();
-      frc::Pose2d current_pose = frc::Pose2d(0_m, 0_m, 0_deg);
+      frc::Pose2d current_pose = _limelight->GetPose().ToPose2d();
+      //frc::Pose2d current_pose = frc::Pose2d(0_m, 0_m, 0_deg);
 
       units::meter_t a = tag.pos.X() - current_pose.X();
       units::meter_t b = tag.pos.Y() - current_pose.Y();
@@ -128,15 +130,15 @@ std::pair<units::meter_t, units::degree_t> Vision::GetDistanceToTarget(int id) {
   for (int i = 0; i < tags.size(); i++) {
     if (tags[i].id == id) {
       AprilTag tag = tags[i];
-      // frc::Pose3d pose = _limelight->GetPose();
-      frc::Pose3d pose = frc::Pose3d(0_m, 0_m, 0_m, frc::Rotation3d(0_deg, 0_deg, 0_deg));
+      //frc::Pose3d pose = _limelight->GetPose();
+      //frc::Pose3d pose = frc::Pose3d(0_m, 0_m, 0_m, frc::Rotation3d(0_deg, 0_deg, 0_deg));
 
       SetMode(VisionModes::kAprilTag);
 
       // Get distance to target
       // Get current position from Limelight
-      // frc::Pose2d current_pose = _limelight->GetPose().ToPose2d();
-      frc::Pose2d current_pose = frc::Pose2d(0_m, 0_m, 0_deg);
+      frc::Pose2d current_pose = _limelight->GetPose().ToPose2d();
+      //frc::Pose2d current_pose = frc::Pose2d(0_m, 0_m, 0_deg);
 
       units::meter_t a = tag.pos.X() - current_pose.X();
       units::meter_t b = tag.pos.Y() - current_pose.Y();
@@ -166,8 +168,8 @@ frc::Pose2d Vision::AlignToTarget(VisionTarget target, units::meter_t offset, wo
   // Get distance to target
   AprilTag tag = GetTags()[static_cast<int>(target) - 1];
   // Get current position from Limelight
-  // frc::Pose2d current_pose = _limelight->GetPose().ToPose2d();
-  frc::Pose2d current_pose = frc::Pose2d(0_m, 0_m, 0_deg);
+  frc::Pose2d current_pose = _limelight->GetPose().ToPose2d();
+  //frc::Pose2d current_pose = frc::Pose2d(0_m, 0_m, 0_deg);
 
   units::meter_t a = tag.pos.X() - current_pose.X();
   units::meter_t b = tag.pos.Y() - current_pose.Y();
@@ -203,8 +205,8 @@ frc::Pose2d Vision::AlignToTarget(int target, units::meter_t offset, wom::Swerve
   std::pair<units::meter_t, units::degree_t> distance = GetDistanceToTarget(target);
   AprilTag tag = GetTags()[target - 1];
   // Get current position from Limelight
-  // frc::Pose2d current_pose = _limelight->GetPose().ToPose2d();
-  frc::Pose2d current_pose = frc::Pose2d(0_m, 0_m, 0_deg);
+  frc::Pose2d current_pose = _limelight->GetPose().ToPose2d();
+  //frc::Pose2d current_pose = frc::Pose2d(0_m, 0_m, 0_deg);
 
   units::meter_t a = tag.pos.X() - current_pose.X();
   units::meter_t b = tag.pos.Y() - current_pose.Y();
@@ -240,8 +242,8 @@ frc::Pose2d Vision::AlignToTarget(VisionTarget target, frc::Translation2d offset
   // Get distance to target
   AprilTag tag = GetTags()[static_cast<int>(target) - 1];
   // Get current position from Limelight
-  // frc::Pose2d current_pose = _limelight->GetPose().ToPose2d();
-  frc::Pose2d current_pose = frc::Pose2d(0_m, 0_m, 0_deg);
+  frc::Pose2d current_pose = _limelight->GetPose().ToPose2d();
+  //frc::Pose2d current_pose = frc::Pose2d(0_m, 0_m, 0_deg);
 
   units::meter_t a = tag.pos.X() - current_pose.X();
   units::meter_t b = tag.pos.Y() - current_pose.Y();
@@ -277,8 +279,8 @@ frc::Pose2d Vision::AlignToTarget(int target, frc::Translation2d offset, wom::Sw
   std::pair<units::meter_t, units::degree_t> distance = GetDistanceToTarget(target);
   AprilTag tag = GetTags()[target - 1];
   // Get current position from Limelight
-  // frc::Pose2d current_pose = _limelight->GetPose().ToPose2d();
-  frc::Pose2d current_pose = frc::Pose2d(0_m, 0_m, 0_deg);
+  frc::Pose2d current_pose = _limelight->GetPose().ToPose2d();
+  //frc::Pose2d current_pose = frc::Pose2d(0_m, 0_m, 0_deg);
 
   units::meter_t a = tag.pos.X() - current_pose.X();
   units::meter_t b = tag.pos.Y() - current_pose.Y();
@@ -312,8 +314,8 @@ frc::Pose2d Vision::TurnToTarget(int target, wom::SwerveDrive* swerveDrive) {
 
   units::degree_t angle = GetDistanceToTarget(target).second;
 
-  //frc::Pose2d current_pose = _limelight->GetPose().ToPose2d();
-  frc::Pose2d current_pose = frc::Pose2d();
+  frc::Pose2d current_pose = _limelight->GetPose().ToPose2d();
+  //frc::Pose2d current_pose = frc::Pose2d();
 
   frc::Pose2d pose = frc::Pose2d(current_pose.X(), current_pose.Y(), angle);
 
@@ -338,16 +340,32 @@ frc::Pose2d Vision::TurnToTarget(VisionTarget target, wom::SwerveDrive* swerveDr
   //swerveDrive->SetPose(pose);
 }
 
-frc::Pose2d Vision::LockOn(VisionTargetObjects object, wom::SwerveDrive* swerveDrive) {
+std::pair<frc::Pose2d, units::degree_t> Vision::GetAngleToObject(VisionTargetObjects object) {
   SetMode(VisionModes::kRing);
 
   if (TargetIsVisible(object)) {
-    swerveDrive->SetLocked(frc::Pose2d());
+    frc::Pose2d pose = _limelight->GetPose().ToPose2d();
+
+    units::degree_t offset = units::degree_t{_limelight->GetOffset().first}; // degrees are how much the robot has to turn for the target to be in the center 
+   
+    frc::Pose2d new_pose = frc::Pose2d(pose.X(), pose.Y(), offset);
+
+    return std::make_pair(new_pose, offset);
   } else {
-    swerveDrive->SetIdle();
+    return std::make_pair(frc::Pose2d(), 0_deg);
   }
+}
 
-  return frc::Pose2d();
+units::degree_t Vision::LockOn(VisionTargetObjects object, wom::SwerveDrive* swerveDrive) {
+  SetMode(VisionModes::kRing);
+  
+  units::degree_t angle = -1000_deg;
+
+  if (TargetIsVisible(object)) {
+    angle = GetAngleToObject(object).second;
+  }
+
+  return angle;
 }
 
 bool Vision::IsAtPose(frc::Pose3d pose, units::second_t dt) {
@@ -355,6 +373,8 @@ bool Vision::IsAtPose(frc::Pose3d pose, units::second_t dt) {
 }
 
 void Vision::SetMode(VisionModes mode) {
+  if (static_cast<int>(mode) == _limelight->GetTargetingData(wom::LimelightTargetingData::kGetpipe)) { return; }
+
   switch (mode) {
     case VisionModes::kAprilTag: {
       _limelight->SetPipeline(wom::LimelightPipeline::kPipeline0);
diff --git a/src/main/include/vision/Vision.h b/src/main/include/vision/Vision.h
index 0dbc7530..951c27e7 100644
--- a/src/main/include/vision/Vision.h
+++ b/src/main/include/vision/Vision.h
@@ -15,6 +15,7 @@
 #include <vector>
 
 #include "Wombat.h"
+#include "units/angle.h"
 
 #define APRILTAGS_MAX 16
 #define APRILTAGS_MIN 0
@@ -93,8 +94,9 @@ class Vision {
 
   frc::Pose2d TurnToTarget(int target, wom::SwerveDrive* swerveDrive);
   frc::Pose2d TurnToTarget(VisionTarget target, wom::SwerveDrive* swerveDrive);
-
-  frc::Pose2d LockOn(VisionTargetObjects target, wom::SwerveDrive* swerveDrive);
+  
+  std::pair<frc::Pose2d, units::degree_t> GetAngleToObject(VisionTargetObjects object); 
+  units::degree_t LockOn(VisionTargetObjects target, wom::SwerveDrive* swerveDrive);
 
   std::vector<AprilTag> GetTags();
 
diff --git a/wombat/src/main/cpp/drivetrain/SwerveDrive.cpp b/wombat/src/main/cpp/drivetrain/SwerveDrive.cpp
index 16888b7e..df80ec42 100644
--- a/wombat/src/main/cpp/drivetrain/SwerveDrive.cpp
+++ b/wombat/src/main/cpp/drivetrain/SwerveDrive.cpp
@@ -11,6 +11,7 @@
 
 #include <iostream>
 
+#include "units/mass.h"
 #include "utils/Util.h"
 
 namespace wom {
@@ -227,7 +228,7 @@ void SwerveDrive::OnUpdate(units::second_t dt) {
   switch (_state) {
     case SwerveDriveState::kZeroing:
       for (auto mod = _modules.begin(); mod < _modules.end(); mod++) {
-        mod->SetZero(dt);
+      mod->SetZero(dt);
       }
       break;
     case SwerveDriveState::kIdle:
@@ -265,7 +266,7 @@ void SwerveDrive::OnUpdate(units::second_t dt) {
 
     case SwerveDriveState::kTuning:
       for (size_t i = 0; i < _modules.size(); i++) {
-        _modules[i].SetPID(_angle, _speed, dt);
+        _modules[i].SetPID(_angle, _speed, dt);      
       }
       break;
     case SwerveDriveState::kXWheels:
@@ -275,6 +276,7 @@ void SwerveDrive::OnUpdate(units::second_t dt) {
       _modules[3].SetPID(315_deg, 0_mps, dt);
       break;
     case SwerveDriveState::kFRVelocityRotationLock:
+      {
       _target_speed.vx = _xPIDController.Calculate(GetPose().X(), dt);
       _target_speed.vy = _yPIDController.Calculate(GetPose().Y(), dt);
       _target_speed.omega =
@@ -285,6 +287,8 @@ void SwerveDrive::OnUpdate(units::second_t dt) {
         _modules[i].SetPID(target_states[i].angle.Radians(), target_states[i].speed, dt);
       }
       break;
+      }
+    
   }
 
   for (auto mod = _modules.begin(); mod < _modules.end(); mod++) {
@@ -335,8 +339,9 @@ void SwerveDrive::OnResetMode() {
   _anglePIDController.Reset();
 }
 
-void SwerveDrive::SetLocked(frc::Pose2d pose) {
+void SwerveDrive::SetLocked(units::degree_t angle) {
   _state = SwerveDriveState::kLockedOn;
+  _lockedAngle = angle;
 }
 
 void SwerveDrive::RotateMatchJoystick(units::radian_t joystickAngle, FieldRelativeSpeeds speeds) {
diff --git a/wombat/src/main/cpp/drivetrain/behaviours/SwerveBehaviours.cpp b/wombat/src/main/cpp/drivetrain/behaviours/SwerveBehaviours.cpp
index 23baf7fd..e16ffaae 100644
--- a/wombat/src/main/cpp/drivetrain/behaviours/SwerveBehaviours.cpp
+++ b/wombat/src/main/cpp/drivetrain/behaviours/SwerveBehaviours.cpp
@@ -32,6 +32,7 @@ void ManualDrivebase::OnStart() {
 }
 
 void ManualDrivebase::OnTick(units::second_t deltaTime) {
+
   // if (_driverController->GetXButtonPressed()) {
   //   ResetMode();
   //   isRotateMatch = !isRotateMatch;
@@ -86,6 +87,13 @@ void ManualDrivebase::OnTick(units::second_t deltaTime) {
   if (num < turningDeadzone) {
     turnX = 0;
     turnY = 0;
+
+  if (_driverController->GetXButtonPressed()) {
+    ResetMode();
+    
+    isRotateMatch = !isRotateMatch;
+
+  }
   }
 
   // if (isRotateMatch) {
diff --git a/wombat/src/main/include/drivetrain/SwerveDrive.h b/wombat/src/main/include/drivetrain/SwerveDrive.h
index 8eb49fa6..c2d88456 100644
--- a/wombat/src/main/include/drivetrain/SwerveDrive.h
+++ b/wombat/src/main/include/drivetrain/SwerveDrive.h
@@ -187,7 +187,7 @@ class SwerveDrive : public behaviour::HasBehaviour {
   void SetZero();
   void SetVoltageLimit(units::volt_t driveVoltageLimit);
   void OnResetMode();
-  void SetLocked(frc::Pose2d pose);
+  void SetLocked(units::degree_t pose);
   // double GetModuleCANPosition(int mod);  // from liam's
 
   void SetXWheelState();
@@ -237,6 +237,8 @@ class SwerveDrive : public behaviour::HasBehaviour {
   double frontRightEncoderOffset = 167.87109375;
   double backLeftEncoderOffset = -316.669921875;
   double backRightEncoderOffset = -119.619140625;
+
+  units::degree_t _lockedAngle;
 };
 }  // namespace drivetrain
 }  // namespace wom

From 57e941a0e8c4ba2a3ca8505de5278237280c1b4f Mon Sep 17 00:00:00 2001
From: Paul Hodges <paul.hodges@student.education.wa.edu.au>
Date: Tue, 30 Jan 2024 12:57:39 +0800
Subject: [PATCH 117/207] [wpiformat]

---
 src/main/cpp/Robot.cpp                        |  8 ++--
 src/main/cpp/vision/Vision.cpp                | 42 ++++++++++---------
 src/main/include/vision/Vision.h              |  4 +-
 .../src/main/cpp/drivetrain/SwerveDrive.cpp   | 10 ++---
 .../behaviours/SwerveBehaviours.cpp           |  2 +-
 5 files changed, 34 insertions(+), 32 deletions(-)

diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp
index e84789d9..08e34416 100644
--- a/src/main/cpp/Robot.cpp
+++ b/src/main/cpp/Robot.cpp
@@ -186,12 +186,12 @@ void Robot::SimulationInit() {
 
     std::cout << x << std::endl;
     std::cout << y << std::endl; */
-  //std::cout << _vision->TurnToTarget(1, _swerveDrive).Rotation().Degrees().value() << std::endl;
+  // std::cout << _vision->TurnToTarget(1, _swerveDrive).Rotation().Degrees().value() << std::endl;
   frc::Pose2d pose = _vision->TurnToTarget(2, _swerveDrive);
-  nt::NetworkTableInstance::GetDefault().GetTable("vision")->PutNumber("rot", pose.Rotation().Degrees().value()); 
-  
+  nt::NetworkTableInstance::GetDefault().GetTable("vision")->PutNumber("rot",
+                                                                       pose.Rotation().Degrees().value());
 }
 
-
 void Robot::SimulationPeriodic() {}
 
+
diff --git a/src/main/cpp/vision/Vision.cpp b/src/main/cpp/vision/Vision.cpp
index 2261e06b..33871ec0 100644
--- a/src/main/cpp/vision/Vision.cpp
+++ b/src/main/cpp/vision/Vision.cpp
@@ -90,15 +90,15 @@ std::pair<units::meter_t, units::degree_t> Vision::GetDistanceToTarget(VisionTar
   for (int i = 0; i < tags.size(); i++) {
     if (tags[i].id == static_cast<int>(target)) {
       AprilTag tag = tags[i];
-      //frc::Pose3d pose = _limelight->GetPose();
-      //frc::Pose3d pose = frc::Pose3d(0_m, 0_m, 0_m, frc::Rotation3d(0_deg, 0_deg, 0_deg));
+      // frc::Pose3d pose = _limelight->GetPose();
+      // frc::Pose3d pose = frc::Pose3d(0_m, 0_m, 0_m, frc::Rotation3d(0_deg, 0_deg, 0_deg));
 
       SetMode(VisionModes::kAprilTag);
 
       // Get distance to target
       // Get current position from Limelight
       frc::Pose2d current_pose = _limelight->GetPose().ToPose2d();
-      //frc::Pose2d current_pose = frc::Pose2d(0_m, 0_m, 0_deg);
+      // frc::Pose2d current_pose = frc::Pose2d(0_m, 0_m, 0_deg);
 
       units::meter_t a = tag.pos.X() - current_pose.X();
       units::meter_t b = tag.pos.Y() - current_pose.Y();
@@ -130,15 +130,15 @@ std::pair<units::meter_t, units::degree_t> Vision::GetDistanceToTarget(int id) {
   for (int i = 0; i < tags.size(); i++) {
     if (tags[i].id == id) {
       AprilTag tag = tags[i];
-      //frc::Pose3d pose = _limelight->GetPose();
-      //frc::Pose3d pose = frc::Pose3d(0_m, 0_m, 0_m, frc::Rotation3d(0_deg, 0_deg, 0_deg));
+      // frc::Pose3d pose = _limelight->GetPose();
+      // frc::Pose3d pose = frc::Pose3d(0_m, 0_m, 0_m, frc::Rotation3d(0_deg, 0_deg, 0_deg));
 
       SetMode(VisionModes::kAprilTag);
 
       // Get distance to target
       // Get current position from Limelight
       frc::Pose2d current_pose = _limelight->GetPose().ToPose2d();
-      //frc::Pose2d current_pose = frc::Pose2d(0_m, 0_m, 0_deg);
+      // frc::Pose2d current_pose = frc::Pose2d(0_m, 0_m, 0_deg);
 
       units::meter_t a = tag.pos.X() - current_pose.X();
       units::meter_t b = tag.pos.Y() - current_pose.Y();
@@ -169,7 +169,7 @@ frc::Pose2d Vision::AlignToTarget(VisionTarget target, units::meter_t offset, wo
   AprilTag tag = GetTags()[static_cast<int>(target) - 1];
   // Get current position from Limelight
   frc::Pose2d current_pose = _limelight->GetPose().ToPose2d();
-  //frc::Pose2d current_pose = frc::Pose2d(0_m, 0_m, 0_deg);
+  // frc::Pose2d current_pose = frc::Pose2d(0_m, 0_m, 0_deg);
 
   units::meter_t a = tag.pos.X() - current_pose.X();
   units::meter_t b = tag.pos.Y() - current_pose.Y();
@@ -206,7 +206,7 @@ frc::Pose2d Vision::AlignToTarget(int target, units::meter_t offset, wom::Swerve
   AprilTag tag = GetTags()[target - 1];
   // Get current position from Limelight
   frc::Pose2d current_pose = _limelight->GetPose().ToPose2d();
-  //frc::Pose2d current_pose = frc::Pose2d(0_m, 0_m, 0_deg);
+  // frc::Pose2d current_pose = frc::Pose2d(0_m, 0_m, 0_deg);
 
   units::meter_t a = tag.pos.X() - current_pose.X();
   units::meter_t b = tag.pos.Y() - current_pose.Y();
@@ -243,7 +243,7 @@ frc::Pose2d Vision::AlignToTarget(VisionTarget target, frc::Translation2d offset
   AprilTag tag = GetTags()[static_cast<int>(target) - 1];
   // Get current position from Limelight
   frc::Pose2d current_pose = _limelight->GetPose().ToPose2d();
-  //frc::Pose2d current_pose = frc::Pose2d(0_m, 0_m, 0_deg);
+  // frc::Pose2d current_pose = frc::Pose2d(0_m, 0_m, 0_deg);
 
   units::meter_t a = tag.pos.X() - current_pose.X();
   units::meter_t b = tag.pos.Y() - current_pose.Y();
@@ -280,7 +280,7 @@ frc::Pose2d Vision::AlignToTarget(int target, frc::Translation2d offset, wom::Sw
   AprilTag tag = GetTags()[target - 1];
   // Get current position from Limelight
   frc::Pose2d current_pose = _limelight->GetPose().ToPose2d();
-  //frc::Pose2d current_pose = frc::Pose2d(0_m, 0_m, 0_deg);
+  // frc::Pose2d current_pose = frc::Pose2d(0_m, 0_m, 0_deg);
 
   units::meter_t a = tag.pos.X() - current_pose.X();
   units::meter_t b = tag.pos.Y() - current_pose.Y();
@@ -315,14 +315,14 @@ frc::Pose2d Vision::TurnToTarget(int target, wom::SwerveDrive* swerveDrive) {
   units::degree_t angle = GetDistanceToTarget(target).second;
 
   frc::Pose2d current_pose = _limelight->GetPose().ToPose2d();
-  //frc::Pose2d current_pose = frc::Pose2d();
+  // frc::Pose2d current_pose = frc::Pose2d();
 
   frc::Pose2d pose = frc::Pose2d(current_pose.X(), current_pose.Y(), angle);
 
-  //std::cout << pose.Rotation().Degrees().value() << std::endl;
+  // std::cout << pose.Rotation().Degrees().value() << std::endl;
+
+  // swerveDrive->SetPose(pose);
 
-  //swerveDrive->SetPose(pose);
-  
   return pose;
 }
 
@@ -337,7 +337,7 @@ frc::Pose2d Vision::TurnToTarget(VisionTarget target, wom::SwerveDrive* swerveDr
 
   std::cout << pose.Rotation().Degrees().value() << std::endl;
 
-  //swerveDrive->SetPose(pose);
+  // swerveDrive->SetPose(pose);
 }
 
 std::pair<frc::Pose2d, units::degree_t> Vision::GetAngleToObject(VisionTargetObjects object) {
@@ -346,8 +346,10 @@ std::pair<frc::Pose2d, units::degree_t> Vision::GetAngleToObject(VisionTargetObj
   if (TargetIsVisible(object)) {
     frc::Pose2d pose = _limelight->GetPose().ToPose2d();
 
-    units::degree_t offset = units::degree_t{_limelight->GetOffset().first}; // degrees are how much the robot has to turn for the target to be in the center 
-   
+    units::degree_t offset = units::degree_t{
+        _limelight->GetOffset()
+            .first};  // degrees are how much the robot has to turn for the target to be in the center
+
     frc::Pose2d new_pose = frc::Pose2d(pose.X(), pose.Y(), offset);
 
     return std::make_pair(new_pose, offset);
@@ -358,7 +360,7 @@ std::pair<frc::Pose2d, units::degree_t> Vision::GetAngleToObject(VisionTargetObj
 
 units::degree_t Vision::LockOn(VisionTargetObjects object, wom::SwerveDrive* swerveDrive) {
   SetMode(VisionModes::kRing);
-  
+
   units::degree_t angle = -1000_deg;
 
   if (TargetIsVisible(object)) {
@@ -373,7 +375,9 @@ bool Vision::IsAtPose(frc::Pose3d pose, units::second_t dt) {
 }
 
 void Vision::SetMode(VisionModes mode) {
-  if (static_cast<int>(mode) == _limelight->GetTargetingData(wom::LimelightTargetingData::kGetpipe)) { return; }
+  if (static_cast<int>(mode) == _limelight->GetTargetingData(wom::LimelightTargetingData::kGetpipe)) {
+    return;
+  }
 
   switch (mode) {
     case VisionModes::kAprilTag: {
diff --git a/src/main/include/vision/Vision.h b/src/main/include/vision/Vision.h
index 951c27e7..8bbef0a9 100644
--- a/src/main/include/vision/Vision.h
+++ b/src/main/include/vision/Vision.h
@@ -94,8 +94,8 @@ class Vision {
 
   frc::Pose2d TurnToTarget(int target, wom::SwerveDrive* swerveDrive);
   frc::Pose2d TurnToTarget(VisionTarget target, wom::SwerveDrive* swerveDrive);
-  
-  std::pair<frc::Pose2d, units::degree_t> GetAngleToObject(VisionTargetObjects object); 
+
+  std::pair<frc::Pose2d, units::degree_t> GetAngleToObject(VisionTargetObjects object);
   units::degree_t LockOn(VisionTargetObjects target, wom::SwerveDrive* swerveDrive);
 
   std::vector<AprilTag> GetTags();
diff --git a/wombat/src/main/cpp/drivetrain/SwerveDrive.cpp b/wombat/src/main/cpp/drivetrain/SwerveDrive.cpp
index df80ec42..27a1ad1f 100644
--- a/wombat/src/main/cpp/drivetrain/SwerveDrive.cpp
+++ b/wombat/src/main/cpp/drivetrain/SwerveDrive.cpp
@@ -228,7 +228,7 @@ void SwerveDrive::OnUpdate(units::second_t dt) {
   switch (_state) {
     case SwerveDriveState::kZeroing:
       for (auto mod = _modules.begin(); mod < _modules.end(); mod++) {
-      mod->SetZero(dt);
+        mod->SetZero(dt);
       }
       break;
     case SwerveDriveState::kIdle:
@@ -266,7 +266,7 @@ void SwerveDrive::OnUpdate(units::second_t dt) {
 
     case SwerveDriveState::kTuning:
       for (size_t i = 0; i < _modules.size(); i++) {
-        _modules[i].SetPID(_angle, _speed, dt);      
+        _modules[i].SetPID(_angle, _speed, dt);
       }
       break;
     case SwerveDriveState::kXWheels:
@@ -275,8 +275,7 @@ void SwerveDrive::OnUpdate(units::second_t dt) {
       _modules[2].SetPID(225_deg, 0_mps, dt);
       _modules[3].SetPID(315_deg, 0_mps, dt);
       break;
-    case SwerveDriveState::kFRVelocityRotationLock:
-      {
+    case SwerveDriveState::kFRVelocityRotationLock: {
       _target_speed.vx = _xPIDController.Calculate(GetPose().X(), dt);
       _target_speed.vy = _yPIDController.Calculate(GetPose().Y(), dt);
       _target_speed.omega =
@@ -287,8 +286,7 @@ void SwerveDrive::OnUpdate(units::second_t dt) {
         _modules[i].SetPID(target_states[i].angle.Radians(), target_states[i].speed, dt);
       }
       break;
-      }
-    
+    }
   }
 
   for (auto mod = _modules.begin(); mod < _modules.end(); mod++) {
diff --git a/wombat/src/main/cpp/drivetrain/behaviours/SwerveBehaviours.cpp b/wombat/src/main/cpp/drivetrain/behaviours/SwerveBehaviours.cpp
index e16ffaae..3ca463c1 100644
--- a/wombat/src/main/cpp/drivetrain/behaviours/SwerveBehaviours.cpp
+++ b/wombat/src/main/cpp/drivetrain/behaviours/SwerveBehaviours.cpp
@@ -90,7 +90,7 @@ void ManualDrivebase::OnTick(units::second_t deltaTime) {
 
   if (_driverController->GetXButtonPressed()) {
     ResetMode();
-    
+
     isRotateMatch = !isRotateMatch;
 
   }

From 2adee21878019296a3338618018f6e8a7f821908 Mon Sep 17 00:00:00 2001
From: Paul Hodges <paul.hodges@student.education.wa.edu.au>
Date: Tue, 30 Jan 2024 14:12:59 +0800
Subject: [PATCH 118/207] Finished turning to note

---
 src/main/cpp/vision/Vision.cpp | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/main/cpp/vision/Vision.cpp b/src/main/cpp/vision/Vision.cpp
index 33871ec0..9761613a 100644
--- a/src/main/cpp/vision/Vision.cpp
+++ b/src/main/cpp/vision/Vision.cpp
@@ -350,9 +350,9 @@ std::pair<frc::Pose2d, units::degree_t> Vision::GetAngleToObject(VisionTargetObj
         _limelight->GetOffset()
             .first};  // degrees are how much the robot has to turn for the target to be in the center
 
-    frc::Pose2d new_pose = frc::Pose2d(pose.X(), pose.Y(), offset);
+    pose.RotateBy(offset);
 
-    return std::make_pair(new_pose, offset);
+    return std::make_pair(pose, offset);
   } else {
     return std::make_pair(frc::Pose2d(), 0_deg);
   }

From 04692db129277d6928d623ba7fe9442c85918571 Mon Sep 17 00:00:00 2001
From: Paul Hodges <paul.hodges@student.education.wa.edu.au>
Date: Tue, 30 Jan 2024 14:14:03 +0800
Subject: [PATCH 119/207] [wpiformat]

---
 src/main/cpp/vision/Vision.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/cpp/vision/Vision.cpp b/src/main/cpp/vision/Vision.cpp
index 9761613a..165a4b1d 100644
--- a/src/main/cpp/vision/Vision.cpp
+++ b/src/main/cpp/vision/Vision.cpp
@@ -361,7 +361,7 @@ std::pair<frc::Pose2d, units::degree_t> Vision::GetAngleToObject(VisionTargetObj
 units::degree_t Vision::LockOn(VisionTargetObjects object, wom::SwerveDrive* swerveDrive) {
   SetMode(VisionModes::kRing);
 
-  units::degree_t angle = -1000_deg;
+  units::degree_t angle;
 
   if (TargetIsVisible(object)) {
     angle = GetAngleToObject(object).second;

From bf9b1b47aaf0a804edb669d1cd1ba2ef3706c025 Mon Sep 17 00:00:00 2001
From: Paul Hodges <paul.hodges@student.education.wa.edu.au>
Date: Tue, 30 Jan 2024 14:56:50 +0800
Subject: [PATCH 120/207] Merged with swerve

---
 src/main/cpp/Robot.cpp                        | 20 ++-----------------
 src/main/include/Robot.h                      | 11 +++-------
 .../behaviours/SwerveBehaviours.cpp           | 10 ++++------
 3 files changed, 9 insertions(+), 32 deletions(-)

diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp
index 08e34416..7ba7c016 100644
--- a/src/main/cpp/Robot.cpp
+++ b/src/main/cpp/Robot.cpp
@@ -19,17 +19,11 @@
 #include <units/voltage.h>
 
 #include "behaviour/HasBehaviour.h"
-
-#include <frc/kinematics/DifferentialDriveKinematics.h>
-#include <frc/controller/RamseteController.h>
-#include <frc/Timer.h>
 #include "networktables/NetworkTableInstance.h"
 
-
 static units::second_t lastPeriodic;
 
 void Robot::RobotInit() {
-
   shooter = new Shooter(robotmap.shooterSystem.config);
   wom::BehaviourScheduler::GetInstance()->Register(shooter);
   shooter->SetDefaultBehaviour(
@@ -40,7 +34,6 @@ void Robot::RobotInit() {
 
   // m_chooser.SetDefaultOption("Default Auto", "Default Auto");
 
-
   // frc::SmartDashboard::PutData("Auto Selector", &m_chooser);
 
   // m_path_chooser.SetDefaultOption("Path1", "paths/output/Path1.wpilib.json");
@@ -94,10 +87,9 @@ void Robot::RobotInit() {
   intake->SetDefaultBehaviour(
       [this]() { return wom::make<IntakeManualControl>(intake, robotmap.controllers.codriver); });
 
-  //_vision = new Vision("limelight", FMAP("fmap.fmap"));
+  // _vision = new Vision("limelight", FMAP("fmap.fmap"));
 
   _vision = new Vision("limelight", FMAP("fmap.fmap"));
-
 }
 
 void Robot::RobotPeriodic() {
@@ -118,14 +110,12 @@ void Robot::RobotPeriodic() {
   robotmap.swerveTable.swerveDriveTable->GetEntry("backRightEncoder")
       .SetDouble(robotmap.swerveBase.moduleConfigs[3].turnMotor.encoder->GetEncoderPosition().value());
 
-
   _swerveDrive->OnUpdate(dt);
   alphaArm->OnUpdate(dt);
   shooter->OnStart();
   intake->OnUpdate(dt);
 
   // _swerveDrive->OnUpdate(dt);
-
 }
 
 void Robot::AutonomousInit() {
@@ -135,8 +125,6 @@ void Robot::AutonomousInit() {
 void Robot::AutonomousPeriodic() {}
 
 void Robot::TeleopInit() {
-
-
   loop.Clear();
   wom::BehaviourScheduler* sched = wom::BehaviourScheduler::GetInstance();
   sched->InterruptAll();
@@ -145,13 +133,11 @@ void Robot::TeleopInit() {
   // frontRight->SetVoltage(4_V);
   // backLeft->SetVoltage(4_V);
   // backRight->SetVoltage(4_V);
-  
 
-//  FMAP("fmap.fmap");
+  //  FMAP("fmap.fmap");
 
   // _swerveDrive->OnStart();
   // sched->InterruptAll();
-
 }
 
 void Robot::TeleopPeriodic() {
@@ -193,5 +179,3 @@ void Robot::SimulationInit() {
 }
 
 void Robot::SimulationPeriodic() {}
-
-
diff --git a/src/main/include/Robot.h b/src/main/include/Robot.h
index e6abd21e..da753377 100644
--- a/src/main/include/Robot.h
+++ b/src/main/include/Robot.h
@@ -20,15 +20,10 @@
 #include "Intake.h"
 #include "IntakeBehaviour.h"
 #include "RobotMap.h"
-
-
 #include "Shooter.h"
 #include "ShooterBehaviour.h"
-
-#include "vision/Vision.h"
-
 #include "Wombat.h"
-
+#include "vision/Vision.h"
 
 class Robot : public frc::TimedRobot {
  public:
@@ -42,6 +37,8 @@ class Robot : public frc::TimedRobot {
   void TeleopPeriodic() override;
   void DisabledInit() override;
   void DisabledPeriodic() override;
+  void SimulationInit() override;
+  void SimulationPeriodic() override;
 
  private:
   RobotMap robotmap;
@@ -60,7 +57,6 @@ class Robot : public frc::TimedRobot {
 
   wom::SwerveDrive* _swerveDrive;
 
-
   AlphaArm* alphaArm;
 
   // ctre::phoenix6::hardware::TalonFX *frontLeft;
@@ -69,5 +65,4 @@ class Robot : public frc::TimedRobot {
   // ctre::phoenix6::hardware::TalonFX *backRight;
 
   Vision* _vision;
-
 };
diff --git a/wombat/src/main/cpp/drivetrain/behaviours/SwerveBehaviours.cpp b/wombat/src/main/cpp/drivetrain/behaviours/SwerveBehaviours.cpp
index 3ca463c1..1b48e5ce 100644
--- a/wombat/src/main/cpp/drivetrain/behaviours/SwerveBehaviours.cpp
+++ b/wombat/src/main/cpp/drivetrain/behaviours/SwerveBehaviours.cpp
@@ -32,7 +32,6 @@ void ManualDrivebase::OnStart() {
 }
 
 void ManualDrivebase::OnTick(units::second_t deltaTime) {
-
   // if (_driverController->GetXButtonPressed()) {
   //   ResetMode();
   //   isRotateMatch = !isRotateMatch;
@@ -88,12 +87,11 @@ void ManualDrivebase::OnTick(units::second_t deltaTime) {
     turnX = 0;
     turnY = 0;
 
-  if (_driverController->GetXButtonPressed()) {
-    ResetMode();
-
-    isRotateMatch = !isRotateMatch;
+    if (_driverController->GetXButtonPressed()) {
+      ResetMode();
 
-  }
+      isRotateMatch = !isRotateMatch;
+    }
   }
 
   // if (isRotateMatch) {

From 87c92edf2c14161ae30419b6c18d38b327fdfa7d Mon Sep 17 00:00:00 2001
From: Paul Hodges <paul.hodges@student.education.wa.edu.au>
Date: Tue, 30 Jan 2024 15:08:22 +0800
Subject: [PATCH 121/207] Did more vision stuff

---
 src/main/cpp/Robot.cpp           | 11 +++--------
 src/main/cpp/vision/Vision.cpp   |  2 +-
 src/main/include/vision/Vision.h |  2 +-
 3 files changed, 5 insertions(+), 10 deletions(-)

diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp
index 7ba7c016..9fbfaea2 100644
--- a/src/main/cpp/Robot.cpp
+++ b/src/main/cpp/Robot.cpp
@@ -138,17 +138,12 @@ void Robot::TeleopInit() {
 
   // _swerveDrive->OnStart();
   // sched->InterruptAll();
-}
 
-void Robot::TeleopPeriodic() {
-  // std::cout << "Current AprilTag: " << _vision->CurrentAprilTag() << std::endl;
-  // std::cout << "Current Target: " << _vision->TargetIsVisible(VisionTargetObjects::kNote) << std::endl;
-  std::cout << "Dist to target: " << _vision->GetDistanceToTarget(VisionTarget::kBlueAmp).first.value()
-            << std::endl;
-  std::cout << "Angle to target: " << _vision->GetDistanceToTarget(VisionTarget::kBlueAmp).second.value()
-            << std::endl;
+  _swerveDrive->SetPose(_vision->GetAngleToObject(VisionTargetObjects::kNote).first);
 }
 
+void Robot::TeleopPeriodic() {}
+
 void Robot::DisabledInit() {}
 void Robot::DisabledPeriodic() {}
 
diff --git a/src/main/cpp/vision/Vision.cpp b/src/main/cpp/vision/Vision.cpp
index 165a4b1d..ad231926 100644
--- a/src/main/cpp/vision/Vision.cpp
+++ b/src/main/cpp/vision/Vision.cpp
@@ -358,7 +358,7 @@ std::pair<frc::Pose2d, units::degree_t> Vision::GetAngleToObject(VisionTargetObj
   }
 }
 
-units::degree_t Vision::LockOn(VisionTargetObjects object, wom::SwerveDrive* swerveDrive) {
+units::degree_t Vision::LockOn(VisionTargetObjects object) {
   SetMode(VisionModes::kRing);
 
   units::degree_t angle;
diff --git a/src/main/include/vision/Vision.h b/src/main/include/vision/Vision.h
index 8bbef0a9..1cb234e1 100644
--- a/src/main/include/vision/Vision.h
+++ b/src/main/include/vision/Vision.h
@@ -96,7 +96,7 @@ class Vision {
   frc::Pose2d TurnToTarget(VisionTarget target, wom::SwerveDrive* swerveDrive);
 
   std::pair<frc::Pose2d, units::degree_t> GetAngleToObject(VisionTargetObjects object);
-  units::degree_t LockOn(VisionTargetObjects target, wom::SwerveDrive* swerveDrive);
+  units::degree_t LockOn(VisionTargetObjects target);
 
   std::vector<AprilTag> GetTags();
 

From 29614ca17baf229faf27771d522cb3caad0e35f0 Mon Sep 17 00:00:00 2001
From: Paul Hodges <paul.hodges@student.education.wa.edu.au>
Date: Mon, 12 Feb 2024 17:02:47 +0800
Subject: [PATCH 122/207] begin merge

---
 src/main/include/RobotMap.h | 8 ++++----
 1 file changed, 4 insertions(+), 4 deletions(-)

diff --git a/src/main/include/RobotMap.h b/src/main/include/RobotMap.h
index 0cadce34..f89f001f 100644
--- a/src/main/include/RobotMap.h
+++ b/src/main/include/RobotMap.h
@@ -31,7 +31,7 @@ struct RobotMap {
   Controllers controllers;
 
   struct IntakeSystem {
-    rev::CANSparkMax intakeMotor{2, rev::CANSparkMax::MotorType::kBrushed};
+    rev::CANSparkMax intakeMotor{15, rev::CANSparkMax::MotorType::kBrushed};
     // wom::CANSparkMaxEncoder intakeEncoder{&intakeMotor, 0.1_m};
     frc::DigitalInput intakeSensor{4};
     // frc::DigitalInput magSensor{0};
@@ -44,7 +44,7 @@ struct RobotMap {
   IntakeSystem intakeSystem;
 
   struct Shooter {
-    rev::CANSparkMax shooterMotor{11, rev::CANSparkMax::MotorType::kBrushless};  // Port 11
+    rev::CANSparkMax shooterMotor{1, rev::CANSparkMax::MotorType::kBrushless};  // Port 11
     // frc::DigitalInput shooterSensor{2};
 
     // wom::VoltageController shooterMotorGroup =
@@ -169,8 +169,8 @@ struct RobotMap {
   SwerveTable swerveTable;
 
   struct AlphaArmSystem {
-    rev::CANSparkMax alphaArmMotor{12, rev::CANSparkMax::MotorType::kBrushless};
-    rev::CANSparkMax wristMotor{15, rev::CANSparkMax::MotorType::kBrushless};
+    rev::CANSparkMax alphaArmMotor{6, rev::CANSparkMax::MotorType::kBrushless};
+    rev::CANSparkMax wristMotor{11, rev::CANSparkMax::MotorType::kBrushless};
 
     wom::Gearbox alphaArmGearbox{&alphaArmMotor, nullptr, frc::DCMotor::NEO(1)};
     wom::Gearbox wristGearbox{&wristMotor, nullptr, frc::DCMotor::NEO(1)};

From f161f5fd1784c03c5b63984fdf4c611aec3f116e Mon Sep 17 00:00:00 2001
From: Isaac Turner <spacey_sooty@outlook.com>
Date: Sat, 3 Feb 2024 09:33:52 +0800
Subject: [PATCH 123/207] [docs] remove unnecessary split in readme (#120)

---
 README.md | 13 -------------
 1 file changed, 13 deletions(-)

diff --git a/README.md b/README.md
index 7604ddb3..a0dabbf5 100644
--- a/README.md
+++ b/README.md
@@ -4,27 +4,14 @@ Our code for the 2024 FRC game, CRESCENDO, using GradleRIO, Wombat, and probably
 
 Setup
 ===
-
 First install WPILib and if running Windows the FRC game tools. Instructions can be found [here](https://docs.wpilib.org/en/stable/docs/zero-to-robot/step-2/index.html).
 
-Linux
----
 Fork this repository then open up a terminal and run :
 ```bash
 git clone https://github.com/*yourusernamehere*/2024-Crescendo.git
 cd 2024-Crescendo
 ```
 Now look in [CONTRIBUTING.md](./CONTRIBUTING.md) before continuing!
-
-Windows
----
-Fork this repository then open up a terminal and run :
-```powershell
-git clone https:\\github.com\*yourusernamehere*\2024-Crescendo.git
-cd 2024-Crescendo
-```
-Now look in [CONTRIBUTING.md](./CONTRIBUTING.md) before continuing!
-
 Quick Commands
 ===
 These commands can be used in a variety of combinations, feel free to experiment!

From b6a6d072dd1d5d5656cebb73367478073b705b26 Mon Sep 17 00:00:00 2001
From: Isaac Turner <spacey_sooty@outlook.com>
Date: Sat, 3 Feb 2024 09:44:10 +0800
Subject: [PATCH 124/207] [docs] make quick commands clearer (#121)

---
 README.md | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/README.md b/README.md
index a0dabbf5..95b54955 100644
--- a/README.md
+++ b/README.md
@@ -19,9 +19,9 @@ These commands can be used in a variety of combinations, feel free to experiment
 Build
 ---
 `./gradlew build`
-Build will compile and get the code ready without deploying it. It will also run all automated tests, which is great for testing your code before it ever gets run on a robot (which also means you can build whenever).
+Build will compile your code without deploying it. It will also run all automated tests, which is great for testing code before it runs on a robot.
 
-`./gradlew :Wombat:build`
+`./gradlew :wombat:build`
 Will compile and build the Wombat library. Also runs all of Wombat's inbuilt tests.
 
 Deploy

From 18d76b0992b72cba9f52f6976cc318c81ba0cb85 Mon Sep 17 00:00:00 2001
From: Isaac Turner <spacey_sooty@outlook.com>
Date: Sat, 3 Feb 2024 18:54:43 +0800
Subject: [PATCH 125/207] [build] update gradle to 8.6 (#123)

---
 build.gradle                             |  2 +-
 gradle/wrapper/gradle-wrapper.properties |  2 +-
 gradlew.bat                              | 20 ++++++++++----------
 3 files changed, 12 insertions(+), 12 deletions(-)

diff --git a/build.gradle b/build.gradle
index 84ccc0cb..728f2b3b 100644
--- a/build.gradle
+++ b/build.gradle
@@ -111,6 +111,6 @@ subprojects {
 }
 
 wrapper {
-    gradleVersion = '8.5'
+    gradleVersion = '8.6'
     distributionType = Wrapper.DistributionType.BIN
 }
diff --git a/gradle/wrapper/gradle-wrapper.properties b/gradle/wrapper/gradle-wrapper.properties
index 5e82d67b..b1d97ca4 100644
--- a/gradle/wrapper/gradle-wrapper.properties
+++ b/gradle/wrapper/gradle-wrapper.properties
@@ -1,6 +1,6 @@
 distributionBase=GRADLE_USER_HOME
 distributionPath=permwrapper/dists
-distributionUrl=https\://services.gradle.org/distributions/gradle-8.5-bin.zip
+distributionUrl=https\://services.gradle.org/distributions/gradle-8.6-bin.zip
 networkTimeout=10000
 validateDistributionUrl=true
 zipStoreBase=GRADLE_USER_HOME
diff --git a/gradlew.bat b/gradlew.bat
index 6689b85b..7101f8e4 100644
--- a/gradlew.bat
+++ b/gradlew.bat
@@ -43,11 +43,11 @@ set JAVA_EXE=java.exe
 %JAVA_EXE% -version >NUL 2>&1
 if %ERRORLEVEL% equ 0 goto execute
 
-echo.
-echo ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH.
-echo.
-echo Please set the JAVA_HOME variable in your environment to match the
-echo location of your Java installation.
+echo. 1>&2
+echo ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH. 1>&2
+echo. 1>&2
+echo Please set the JAVA_HOME variable in your environment to match the 1>&2
+echo location of your Java installation. 1>&2
 
 goto fail
 
@@ -57,11 +57,11 @@ set JAVA_EXE=%JAVA_HOME%/bin/java.exe
 
 if exist "%JAVA_EXE%" goto execute
 
-echo.
-echo ERROR: JAVA_HOME is set to an invalid directory: %JAVA_HOME%
-echo.
-echo Please set the JAVA_HOME variable in your environment to match the
-echo location of your Java installation.
+echo. 1>&2
+echo ERROR: JAVA_HOME is set to an invalid directory: %JAVA_HOME% 1>&2
+echo. 1>&2
+echo Please set the JAVA_HOME variable in your environment to match the 1>&2
+echo location of your Java installation. 1>&2
 
 goto fail
 

From 5e9957b5c9c8d7992b20c202cc4c8b437a42a6e9 Mon Sep 17 00:00:00 2001
From: Isaac Turner <spacey_sooty@outlook.com>
Date: Sun, 4 Feb 2024 16:30:53 +0800
Subject: [PATCH 126/207] [wombat/utils] fix pid handling of negative numbers
 (#122)

* merged with silly stuff

* [wombat/utils] fix pid handling of negative numbers

* Revert "merged with silly stuff"

This reverts commit d0cc0b03d45fe01173880656c33d1ae1ada74611.
---
 wombat/src/main/include/utils/PID.h | 8 ++++++++
 1 file changed, 8 insertions(+)

diff --git a/wombat/src/main/include/utils/PID.h b/wombat/src/main/include/utils/PID.h
index a0f2eaf9..7d9b9516 100644
--- a/wombat/src/main/include/utils/PID.h
+++ b/wombat/src/main/include/utils/PID.h
@@ -103,6 +103,11 @@ class PIDController {
   void Reset() { _integralSum = sum_t{0}; }
 
   out_t Calculate(in_t pv, units::second_t dt, out_t feedforward = out_t{0}) {
+    bool is_negative;
+    if (pv.value() < 0) {
+      is_negative = true;
+      pv = units::math::fabs(pv);
+    }
     auto error = do_wrap(_setpoint - pv);
     _integralSum += error * dt;
     if (config.izone.value() > 0 && (error > config.izone || error < -config.izone))
@@ -130,6 +135,9 @@ class PIDController {
     _last_pv = pv;
     _last_error = error;
     _iterations++;
+    if (is_negative) {
+      return out * -1;
+    }
     return out;
   }
 

From ad36f9173f854b6c1dccf481d8a89fbbf87de015 Mon Sep 17 00:00:00 2001
From: Isaac Turner <spacey_sooty@outlook.com>
Date: Wed, 7 Feb 2024 19:55:28 +0800
Subject: [PATCH 127/207] [wombat/drivetrain] add DrivetrainPoseBehaviour
 (#125)

Needs to be tested on a robot only being merged for auto work
---
 .../behaviours/SwerveBehaviours.cpp           | 22 +++++++++++++
 .../drivetrain/behaviours/SwerveBehaviours.h  | 33 +++++++++++++++++++
 2 files changed, 55 insertions(+)

diff --git a/wombat/src/main/cpp/drivetrain/behaviours/SwerveBehaviours.cpp b/wombat/src/main/cpp/drivetrain/behaviours/SwerveBehaviours.cpp
index 18d88f88..a8cbf3c5 100644
--- a/wombat/src/main/cpp/drivetrain/behaviours/SwerveBehaviours.cpp
+++ b/wombat/src/main/cpp/drivetrain/behaviours/SwerveBehaviours.cpp
@@ -224,3 +224,25 @@ void wom::drivetrain::behaviours::AutoSwerveDrive::OnUpdate() {
 void wom::drivetrain::behaviours::AutoSwerveDrive::SetPath(std::string path) {
   _simSwerveDrive->SetPath(path);
 }
+
+// Drivebase Pose Control behaviour
+wom::drivetrain::behaviours::DrivebasePoseBehaviour::DrivebasePoseBehaviour(SwerveDrive* swerveDrivebase,
+                                                                            frc::Pose2d pose,
+                                                                            units::volt_t voltageLimit,
+                                                                            bool hold)
+    : _swerveDrivebase(swerveDrivebase), _pose(pose), _hold(hold), _voltageLimit(voltageLimit) {
+  Controls(swerveDrivebase);
+}
+
+// used in autonomous for going to set drive poses
+void wom::drivetrain::behaviours::DrivebasePoseBehaviour::OnTick(units::second_t deltaTime) {
+  double currentAngle = _swerveDrivebase->GetPose().Rotation().Degrees().value();
+  units::degree_t adjustedAngle =
+      1_deg * (currentAngle - std::fmod(currentAngle, 360) + _pose.Rotation().Degrees().value());
+  _swerveDrivebase->SetVoltageLimit(_voltageLimit);
+  _swerveDrivebase->SetPose(frc::Pose2d{_pose.X(), _pose.Y(), adjustedAngle});
+
+  if (_swerveDrivebase->IsAtSetPose() && !_hold) {
+    SetDone();
+  }
+}
diff --git a/wombat/src/main/include/drivetrain/behaviours/SwerveBehaviours.h b/wombat/src/main/include/drivetrain/behaviours/SwerveBehaviours.h
index 11400c4a..b2ceaeba 100644
--- a/wombat/src/main/include/drivetrain/behaviours/SwerveBehaviours.h
+++ b/wombat/src/main/include/drivetrain/behaviours/SwerveBehaviours.h
@@ -199,6 +199,39 @@ class AutoSwerveDrive {
 
   std::string m_path;
 };
+
+/**
+ * @brief Behaviour Class to hangle the swerve drivebase going to and potentially maintaining the position
+ */
+class DrivebasePoseBehaviour : public behaviour::Behaviour {
+ public:
+  /**
+   * @param swerveDrivebase
+   * A pointer to the swerve drivebase
+   * @param pose
+   * A variable containing an X coordinate, a Y coordinate, and a rotation, for the drivebase to go to
+   * @param hold
+   * An optional variable (defaulting false), to say whether this position should be maintained
+   */
+  DrivebasePoseBehaviour(SwerveDrive* swerveDrivebase, frc::Pose2d pose, units::volt_t voltageLimit = 10_V,
+                         bool hold = false);
+
+  /**
+   * @brief
+   *
+   * @param deltaTime change in time since the last iteration
+   */
+  void OnTick(units::second_t deltaTime) override;
+
+ private:
+  SwerveDrive* _swerveDrivebase;
+  frc::Pose2d _pose;
+  bool _hold;
+  units::volt_t _voltageLimit;
+
+  std::shared_ptr<nt::NetworkTable> _swerveDriveTable =
+      nt::NetworkTableInstance::GetDefault().GetTable("swerve");
+};
 }  // namespace behaviours
 }  // namespace drivetrain
 }  // namespace wom

From 8df84149f3b1d956b442e570e4c3522075a802fb Mon Sep 17 00:00:00 2001
From: Anna Pedersen <59428185+goanna247@users.noreply.github.com>
Date: Sat, 10 Feb 2024 12:12:31 +0800
Subject: [PATCH 128/207] Pid testing (#132)

* added the state raw and changed voltage

* test

* added the state raw and changed voltage

* added pid calculate loop, fixed indenting

* added calculate function on a new line, changed the PID to not stable

* Fixed the if condition so shooterSensor is not tripped

* changed the functions so they start with a capital

* changed the value for shooterMotor

* added shuffleboard and networking tables

* added shuffleboard and networking tables

* added network tables

* Alpha Arm vers 1

* Alpha Arm ver 1 w/ explicit

* Alpha arm v1 formatting

* Alpha arm w/ wrist v1

* formatted

* fixed merge conflicts and merge errors

* fixed merge conflicts and merge errors

* fixed merge conflicts and merge errors

* fixed errors

* ran wpiformat

* Alpha arm v1 with ports

* fixed merge errors and conflicts

* fixed github errors

* Working alpha arm no encoders

* Alpha Bot Shooter

* Swerve fix (#106)

* fixed swerve

* wrapping function

* Shooter - Implements Networking tables (#99)

* added the state raw and changed voltage

* test

* added the state raw and changed voltage

* added pid calculate loop, fixed indenting

* added calculate function on a new line, changed the PID to not stable

* Fixed the if condition so shooterSensor is not tripped

* changed the functions so they start with a capital

* changed the value for shooterMotor

* added shuffleboard and networking tables

* added shuffleboard and networking tables

* added network tables

* fixed merge conflicts and merge errors

* fixed merge conflicts and merge errors

* fixed merge conflicts and merge errors

* fixed errors

* ran wpiformat

* fixed merge errors and conflicts

* fixed github errors

* Alpha Bot Shooter

---------

Co-authored-by: Havish12 <havishchopra10@gmail.com>
Co-authored-by: Isaac Turner <spacey_sooty@outlook.com>

* Intake (#100)

* First version Tank Drive

* Intake draft

* Intake draft fixed errors 1

* Intake draft fixed errors 2

* Intake draft fixed errors 3

* Intake draft fixed errors 4

* Intake draft fixed errors 5

* Network files 1

* Network files 2

* Network files 3

* Almost done i think

* finished wpiformat errors

* finished wpiformat errors 2

* finished wpiformat errors 3

* finished wpiformat errors 4

* ran wpiformat

* finished wpiformat errors 5

* finished wpiformat errors 8

* finished wpiformat errors 9

* finished wpiformat errors 10

* Tested on robot

* Fixed github build errors after modifying during robot tests

* fixed more github build errors 2

* fixed more github build errors 3

* Tested on robot and works

---------

Co-authored-by: Isaac Turner <spacey_sooty@outlook.com>

* Arm (#103)

* Alpha Arm vers 1

* Alpha Arm ver 1 w/ explicit

* Alpha arm v1 formatting

* Alpha arm w/ wrist v1

* formatted

* Alpha arm v1 with ports

* Working alpha arm no encoders

---------

Co-authored-by: Isaac Turner <spacey_sooty@outlook.com>

* fix some formatting

* fix more stuff

* shooter with pid loop

* Fixed errors

* ':pensive:'

* shortest path in 1 direction

* Fixing pid loop control

* fixing swerve

* robot running well, field oriented, turning pretty good needs work in 1 direction i think it's fighting a PID loop, i put acceleration and speed limit all the way up be careful, also needs LOTS more current limiting

* fixing pid control

* turning and driving fixed swerve modules being ordered wrong

* pid loop tuning

* AlphaArm PID v1 probably unit issues

* fixed PID control

* Arm WomPID

* ran wpiformat

* ran wpiformat again;

* fixed build

* wpiformat

* Shooter pid (#117)

* added the state raw and changed voltage

* test

* added the state raw and changed voltage

* added pid calculate loop, fixed indenting

* added calculate function on a new line, changed the PID to not stable

* Fixed the if condition so shooterSensor is not tripped

* changed the functions so they start with a capital

* changed the value for shooterMotor

* added shuffleboard and networking tables

* added shuffleboard and networking tables

* added network tables

* fixed merge conflicts and merge errors

* fixed merge conflicts and merge errors

* fixed merge conflicts and merge errors

* fixed errors

* ran wpiformat

* fixed merge errors and conflicts

* fixed github errors

* Alpha Bot Shooter

* shooter with pid loop

* Fixed errors

* Fixing pid loop control

* fixing pid control

* pid loop tuning

* fixed PID control

* ran wpiformat

* ran wpiformat again;

* fixed build

* wpiformat

---------

Co-authored-by: Havish12 <havishchopra10@gmail.com>
Co-authored-by: Isaac Turner <spacey_sooty@outlook.com>

* Intake - Manual/Auto fixes (#114)

* First version Tank Drive

* Intake draft

* Intake draft fixed errors 1

* Intake draft fixed errors 2

* Intake draft fixed errors 3

* Intake draft fixed errors 4

* Intake draft fixed errors 5

* Network files 1

* Network files 2

* Network files 3

* Almost done i think

* finished wpiformat errors

* finished wpiformat errors 2

* finished wpiformat errors 3

* finished wpiformat errors 4

* ran wpiformat

* finished wpiformat errors 5

* finished wpiformat errors 8

* finished wpiformat errors 9

* finished wpiformat errors 10

* Tested on robot

* Fixed github build errors after modifying during robot tests

* fixed more github build errors 2

* fixed more github build errors 3

* Tested on robot and works

* added beam break

* [docs] Update README.md (#107)

* [docs] Update README.md

* Update README.md

* Added auto to intake

* [ci] add comment command action (#111)

* [ci] add comment command action

* fix

* Tested on robot

* fixed intake manual auto, has not been tested on robot

* fixed github build errors

* Added intake better manual and verfied on robot!

* Bump actions/setup-python from 4 to 5 (#116)

Bumps [actions/setup-python](https://github.com/actions/setup-python) from 4 to 5.
- [Release notes](https://github.com/actions/setup-python/releases)
- [Commits](https://github.com/actions/setup-python/compare/v4...v5)

---
updated-dependencies:
- dependency-name: actions/setup-python
  dependency-type: direct:production
  update-type: version-update:semver-major
...

Signed-off-by: dependabot[bot] <support@github.com>
Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com>

* Bump actions/github-script from 6 to 7 (#115)

Bumps [actions/github-script](https://github.com/actions/github-script) from 6 to 7.
- [Release notes](https://github.com/actions/github-script/releases)
- [Commits](https://github.com/actions/github-script/compare/v6...v7)

---
updated-dependencies:
- dependency-name: actions/github-script
  dependency-type: direct:production
  update-type: version-update:semver-major
...

Signed-off-by: dependabot[bot] <support@github.com>
Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com>

* Fixed readme (#118)

* ran wpiformat

* fix

* fix line endings

* builds

---------

Signed-off-by: dependabot[bot] <support@github.com>
Co-authored-by: Kingsley Wong <kingsley.wong@student.education.wa.edu.au>
Co-authored-by: Isaac Turner <spacey_sooty@outlook.com>
Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com>
Co-authored-by: Paul Hodges <111325206+Superbro525Alt@users.noreply.github.com>

* arm with pid hold

* going to try frc PID loop

* Driver testing finished

* beta bot swerve working and tested, subsystems not vibing

---------

Signed-off-by: dependabot[bot] <support@github.com>
Co-authored-by: Havish12 <havishchopra10@gmail.com>
Co-authored-by: totallysomeoneyoudontknow <dev.renu@gmail.com>
Co-authored-by: JoystickMaster-race <foxlocks179@gmail.com>
Co-authored-by: Isaac Turner <spacey_sooty@outlook.com>
Co-authored-by: JoystickMaster-race <97269195+JoystickMaster-race@users.noreply.github.com>
Co-authored-by: totallysomeoneyoudontknow <155517980+totallysomeoneyoudontknow@users.noreply.github.com>
Co-authored-by: kill-shots <155516223+kill-shots@users.noreply.github.com>
Co-authored-by: prawny-boy <155516197+prawny-boy@users.noreply.github.com>
Co-authored-by: Kingsley Wong <kingsley.wong@student.education.wa.edu.au>
Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com>
Co-authored-by: Paul Hodges <111325206+Superbro525Alt@users.noreply.github.com>
---
 .gitattributes                                |   1 +
 .styleguide                                   |   1 -
 README.md                                     |   4 +
 gradlew.bat                                   | 184 ++++-----
 src/main/cpp/AlphaArm.cpp                     | 113 ++++++
 src/main/cpp/AlphaArmBehaviour.cpp            |  31 ++
 src/main/cpp/Intake.cpp                       |  81 ++++
 src/main/cpp/IntakeBehaviour.cpp              |  63 +++
 src/main/cpp/Robot.cpp                        | 119 +++++-
 src/main/cpp/Shooter.cpp                      | 121 ++++++
 src/main/cpp/ShooterBehaviour.cpp             |  49 +++
 src/main/include/AlphaArm.h                   |  78 ++++
 src/main/include/AlphaArmBehaviour.h          |  22 ++
 src/main/include/Intake.h                     |  43 +++
 src/main/include/IntakeBehaviour.h            |  34 ++
 src/main/include/Robot.h                      |  27 +-
 src/main/include/RobotMap.h                   | 195 +++++++---
 src/main/include/Shooter.h                    |  49 +++
 src/main/include/ShooterBehaviour.h           |  25 ++
 wombat/LICENSE                                |  21 +
 wombat/README.md                              |   1 +
 wombat/settings.gradle                        |   1 +
 wombat/src/main/cpp/behaviour/Behaviour.cpp   |  23 +-
 .../main/cpp/behaviour/BehaviourScheduler.cpp |   9 +-
 .../src/main/cpp/behaviour/HasBehaviour.cpp   |   3 +-
 wombat/src/main/cpp/drivetrain/Drivetrain.cpp |  18 +-
 .../src/main/cpp/drivetrain/SwerveDrive.cpp   | 362 +++++++++++++-----
 .../behaviours/SwerveBehaviours.cpp           | 165 ++++----
 wombat/src/main/cpp/subsystems/Arm.cpp        |  41 +-
 wombat/src/main/cpp/subsystems/Elevator.cpp   |  37 +-
 wombat/src/main/cpp/subsystems/Shooter.cpp    |  27 +-
 wombat/src/main/cpp/utils/Encoder.cpp         | 100 +++--
 wombat/src/main/include/Wombat.h              |   2 +-
 .../src/main/include/drivetrain/Drivetrain.h  |   3 +-
 .../src/main/include/drivetrain/SwerveDrive.h |  70 +++-
 .../drivetrain/behaviours/SwerveBehaviours.h  |  23 +-
 wombat/src/main/include/subsystems/Arm.h      |   3 +-
 wombat/src/main/include/subsystems/Elevator.h |   3 +-
 wombat/src/main/include/subsystems/Shooter.h  |   3 +-
 wombat/src/main/include/utils/Encoder.h       |  42 +-
 wombat/src/main/include/utils/PID.h           |  13 +-
 41 files changed, 1724 insertions(+), 486 deletions(-)
 create mode 100644 src/main/cpp/AlphaArm.cpp
 create mode 100644 src/main/cpp/AlphaArmBehaviour.cpp
 create mode 100644 src/main/cpp/Intake.cpp
 create mode 100644 src/main/cpp/IntakeBehaviour.cpp
 create mode 100644 src/main/cpp/Shooter.cpp
 create mode 100644 src/main/cpp/ShooterBehaviour.cpp
 create mode 100644 src/main/include/AlphaArm.h
 create mode 100644 src/main/include/AlphaArmBehaviour.h
 create mode 100644 src/main/include/Intake.h
 create mode 100644 src/main/include/IntakeBehaviour.h
 create mode 100644 src/main/include/Shooter.h
 create mode 100644 src/main/include/ShooterBehaviour.h
 create mode 100644 wombat/LICENSE
 create mode 100644 wombat/README.md
 create mode 100644 wombat/settings.gradle

diff --git a/.gitattributes b/.gitattributes
index 416557ac..df788002 100644
--- a/.gitattributes
+++ b/.gitattributes
@@ -1,5 +1,6 @@
 * text=auto
 *.sh text eol=lf
+*.bat text eol=crlf
 *.gradle text eol=lf
 *.java text eol=lf
 *.json text eol=lf
diff --git a/.styleguide b/.styleguide
index be156d3c..82efdeab 100644
--- a/.styleguide
+++ b/.styleguide
@@ -1,4 +1,3 @@
-
 cppHeaderFileInclude {
    \.h$
    \.hpp$
diff --git a/README.md b/README.md
index 95b54955..b56b0fa1 100644
--- a/README.md
+++ b/README.md
@@ -42,7 +42,11 @@ Runs a simulation of your code at highest optimisation.
 
 **Debug**
 `./gradlew :simulateNativeDebug`
+<<<<<<< HEAD
+Runs a debug simulation of your code.
+=======
 Runs a debug simulation of your code, including a variety of debugging tools similar to glass but at lower optimisation.
+>>>>>>> 0d1300dafb5ad6f45b5d78b3668778de3bced1e8
 
 Documentation
 =============
diff --git a/gradlew.bat b/gradlew.bat
index 7101f8e4..25da30db 100644
--- a/gradlew.bat
+++ b/gradlew.bat
@@ -1,92 +1,92 @@
-@rem
-@rem Copyright 2015 the original author or authors.
-@rem
-@rem Licensed under the Apache License, Version 2.0 (the "License");
-@rem you may not use this file except in compliance with the License.
-@rem You may obtain a copy of the License at
-@rem
-@rem      https://www.apache.org/licenses/LICENSE-2.0
-@rem
-@rem Unless required by applicable law or agreed to in writing, software
-@rem distributed under the License is distributed on an "AS IS" BASIS,
-@rem WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-@rem See the License for the specific language governing permissions and
-@rem limitations under the License.
-@rem
-
-@if "%DEBUG%"=="" @echo off
-@rem ##########################################################################
-@rem
-@rem  Gradle startup script for Windows
-@rem
-@rem ##########################################################################
-
-@rem Set local scope for the variables with windows NT shell
-if "%OS%"=="Windows_NT" setlocal
-
-set DIRNAME=%~dp0
-if "%DIRNAME%"=="" set DIRNAME=.
-@rem This is normally unused
-set APP_BASE_NAME=%~n0
-set APP_HOME=%DIRNAME%
-
-@rem Resolve any "." and ".." in APP_HOME to make it shorter.
-for %%i in ("%APP_HOME%") do set APP_HOME=%%~fi
-
-@rem Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script.
-set DEFAULT_JVM_OPTS="-Xmx64m" "-Xms64m"
-
-@rem Find java.exe
-if defined JAVA_HOME goto findJavaFromJavaHome
-
-set JAVA_EXE=java.exe
-%JAVA_EXE% -version >NUL 2>&1
-if %ERRORLEVEL% equ 0 goto execute
-
-echo. 1>&2
-echo ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH. 1>&2
-echo. 1>&2
-echo Please set the JAVA_HOME variable in your environment to match the 1>&2
-echo location of your Java installation. 1>&2
-
-goto fail
-
-:findJavaFromJavaHome
-set JAVA_HOME=%JAVA_HOME:"=%
-set JAVA_EXE=%JAVA_HOME%/bin/java.exe
-
-if exist "%JAVA_EXE%" goto execute
-
-echo. 1>&2
-echo ERROR: JAVA_HOME is set to an invalid directory: %JAVA_HOME% 1>&2
-echo. 1>&2
-echo Please set the JAVA_HOME variable in your environment to match the 1>&2
-echo location of your Java installation. 1>&2
-
-goto fail
-
-:execute
-@rem Setup the command line
-
-set CLASSPATH=%APP_HOME%\gradle\wrapper\gradle-wrapper.jar
-
-
-@rem Execute Gradle
-"%JAVA_EXE%" %DEFAULT_JVM_OPTS% %JAVA_OPTS% %GRADLE_OPTS% "-Dorg.gradle.appname=%APP_BASE_NAME%" -classpath "%CLASSPATH%" org.gradle.wrapper.GradleWrapperMain %*
-
-:end
-@rem End local scope for the variables with windows NT shell
-if %ERRORLEVEL% equ 0 goto mainEnd
-
-:fail
-rem Set variable GRADLE_EXIT_CONSOLE if you need the _script_ return code instead of
-rem the _cmd.exe /c_ return code!
-set EXIT_CODE=%ERRORLEVEL%
-if %EXIT_CODE% equ 0 set EXIT_CODE=1
-if not ""=="%GRADLE_EXIT_CONSOLE%" exit %EXIT_CODE%
-exit /b %EXIT_CODE%
-
-:mainEnd
-if "%OS%"=="Windows_NT" endlocal
-
-:omega
+@rem
+@rem Copyright 2015 the original author or authors.
+@rem
+@rem Licensed under the Apache License, Version 2.0 (the "License");
+@rem you may not use this file except in compliance with the License.
+@rem You may obtain a copy of the License at
+@rem
+@rem      https://www.apache.org/licenses/LICENSE-2.0
+@rem
+@rem Unless required by applicable law or agreed to in writing, software
+@rem distributed under the License is distributed on an "AS IS" BASIS,
+@rem WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+@rem See the License for the specific language governing permissions and
+@rem limitations under the License.
+@rem
+
+@if "%DEBUG%"=="" @echo off
+@rem ##########################################################################
+@rem
+@rem  Gradle startup script for Windows
+@rem
+@rem ##########################################################################
+
+@rem Set local scope for the variables with windows NT shell
+if "%OS%"=="Windows_NT" setlocal
+
+set DIRNAME=%~dp0
+if "%DIRNAME%"=="" set DIRNAME=.
+@rem This is normally unused
+set APP_BASE_NAME=%~n0
+set APP_HOME=%DIRNAME%
+
+@rem Resolve any "." and ".." in APP_HOME to make it shorter.
+for %%i in ("%APP_HOME%") do set APP_HOME=%%~fi
+
+@rem Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script.
+set DEFAULT_JVM_OPTS="-Xmx64m" "-Xms64m"
+
+@rem Find java.exe
+if defined JAVA_HOME goto findJavaFromJavaHome
+
+set JAVA_EXE=java.exe
+%JAVA_EXE% -version >NUL 2>&1
+if %ERRORLEVEL% equ 0 goto execute
+
+echo. 1>&2
+echo ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH. 1>&2
+echo. 1>&2
+echo Please set the JAVA_HOME variable in your environment to match the 1>&2
+echo location of your Java installation. 1>&2
+
+goto fail
+
+:findJavaFromJavaHome
+set JAVA_HOME=%JAVA_HOME:"=%
+set JAVA_EXE=%JAVA_HOME%/bin/java.exe
+
+if exist "%JAVA_EXE%" goto execute
+
+echo. 1>&2
+echo ERROR: JAVA_HOME is set to an invalid directory: %JAVA_HOME% 1>&2
+echo. 1>&2
+echo Please set the JAVA_HOME variable in your environment to match the 1>&2
+echo location of your Java installation. 1>&2
+
+goto fail
+
+:execute
+@rem Setup the command line
+
+set CLASSPATH=%APP_HOME%\gradle\wrapper\gradle-wrapper.jar
+
+
+@rem Execute Gradle
+"%JAVA_EXE%" %DEFAULT_JVM_OPTS% %JAVA_OPTS% %GRADLE_OPTS% "-Dorg.gradle.appname=%APP_BASE_NAME%" -classpath "%CLASSPATH%" org.gradle.wrapper.GradleWrapperMain %*
+
+:end
+@rem End local scope for the variables with windows NT shell
+if %ERRORLEVEL% equ 0 goto mainEnd
+
+:fail
+rem Set variable GRADLE_EXIT_CONSOLE if you need the _script_ return code instead of
+rem the _cmd.exe /c_ return code!
+set EXIT_CODE=%ERRORLEVEL%
+if %EXIT_CODE% equ 0 set EXIT_CODE=1
+if not ""=="%GRADLE_EXIT_CONSOLE%" exit %EXIT_CODE%
+exit /b %EXIT_CODE%
+
+:mainEnd
+if "%OS%"=="Windows_NT" endlocal
+
+:omega
diff --git a/src/main/cpp/AlphaArm.cpp b/src/main/cpp/AlphaArm.cpp
new file mode 100644
index 00000000..c97ead0f
--- /dev/null
+++ b/src/main/cpp/AlphaArm.cpp
@@ -0,0 +1,113 @@
+// Copyright (c) 2023-2024 CurtinFRC
+// Open Source Software, you can modify it according to the terms
+// of the MIT License at the root of this project
+
+#include "AlphaArm.h"
+//fiddle with these values
+AlphaArm::AlphaArm(AlphaArmConfig config) : _config(config), _pidWom(_config.path + "/pid", config.pidConfigA) /*_table(nt::NetworkTableInstance::GetDefault().GetTable(config.path)*/{}
+
+void AlphaArm::OnStart() {
+  started = false;
+  // _pidWom.Reset();
+  // _pidWom.SetSetpoint(_config.alphaArmGearbox.encoder->GetEncoderPosition());
+}
+
+void AlphaArm::OnUpdate(units::second_t dt){
+  _table->GetEntry("Error").SetDouble(_pidWom.GetError().value());
+  _table->GetEntry("Current Pos").SetDouble(_config.alphaArmGearbox.encoder->GetEncoderPosition().value());
+  _table->GetEntry("Setpoint").SetDouble(_pidWom.GetSetpoint().value());
+  _table->GetEntry("State ").SetString(_stateName);
+        switch(_state){
+          case AlphaArmState::kIdle:
+            _stateName = "Idle";
+            // _pidWom.SetSetpoint(_config.alphaArmGearbox.encoder->GetEncoderPosition());
+          
+            
+            _setAlphaArmVoltage = 0_V;
+
+          break;
+          case AlphaArmState::kRaw:
+            _stateName = "Raw";
+          
+            _setAlphaArmVoltage = _rawArmVoltage;
+
+          break;
+          case AlphaArmState::kAmpAngle:
+          {
+            _stateName = "Amp Angle";
+
+            // _pidWom.SetSetpoint(_goal);
+            // _setAlphaArmVoltage = _pidWom.Calculate(_config.alphaArmGearbox.encoder->GetEncoderPosition(), dt, 0_V);
+
+            if (started) {
+              if (_controlledRawVoltage.value() == 0) {
+                if (-_config.alphaArmGearbox.encoder->GetEncoderPosition() > (_startingPos + (3.1415_rad/2))) {
+                    // _pidWom.SetSetpoint(_encoderSetpoint);
+                    // _setAlphaArmVoltage = -_pidWom.Calculate(-_config.alphaArmGearbox.encoder->GetEncoderPosition(), dt, 0.0_V);
+                //   _table->GetEntry("Demand").SetDouble(_setAlphaArmVoltage.value());
+                // } else if (_config.alphaArmGearbox.encoder->GetEncoderPosition() < 0_rad) {
+                //   _pidWom.SetSetpoint(_encoderSetpoint);
+                //   _setAlphaArmVoltage = _pidWom.Calculate(_config.alphaArmGearbox.encoder->GetEncoderPosition(), dt, 0.0_V);
+                //   _table->GetEntry("Demand").SetDouble(_setAlphaArmVoltage.value());
+                  _setAlphaArmVoltage = 0_V;
+                } else {
+                  _pidWom.SetSetpoint(_encoderSetpoint);
+                  _setAlphaArmVoltage = -_pidWom.Calculate(-_config.alphaArmGearbox.encoder->GetEncoderPosition(), dt, 0.0_V);
+                //   _pidWom.Reset();
+                //   _encoderSetpoint = _config.alphaArmGearbox.encoder->GetEncoderPosition();
+                //   _setAlphaArmVoltage = _controlledRawVoltage;
+                }
+              } else {
+                _pidWom.Reset();
+                _encoderSetpoint = -_config.alphaArmGearbox.encoder->GetEncoderPosition();
+                _setAlphaArmVoltage = _controlledRawVoltage;
+              }
+            } else {
+              _pidWom.Reset();
+              _encoderSetpoint = -_config.alphaArmGearbox.encoder->GetEncoderPosition();
+              _setAlphaArmVoltage = _controlledRawVoltage;
+
+              if (std::abs(_controlledRawVoltage.value()) > 0) {
+                _startingPos = -_config.alphaArmGearbox.encoder->GetEncoderPosition();
+                started = true;
+              }
+            }
+            
+          }
+            break;
+          case AlphaArmState::kSpeakerAngle:
+            _stateName = "Speaker Angle";
+            //_pidWom.SetSetpoint(_goal);
+           // _setAlphaArmVoltage = _pidWom.Calculate(_config.alphaArmGearbox.encoder->GetEncoderPosition(), dt, 0_V);
+              break;
+          case AlphaArmState::kStowed:
+            _stateName = "Stowed";
+            //_pidWom.SetSetpoint(_goal);
+            //_setAlphaArmVoltage = _pidWom.Calculate(_config.alphaArmGearbox.encoder->GetEncoderPosition(), dt, 0_V);
+          break;
+          default:
+            _stateName = "Error";
+            std::cout << "oops, wrong state" << std::endl;
+          break;
+        }
+        std::cout << " ARM POSITION: " << _config.alphaArmGearbox.encoder->GetEncoderPosition().value() << std::endl;
+        //std::cout << "OUTPUT VOLTAGE: " << _setAlphaArmVoltage.value() << std::endl;
+        _config.alphaArmGearbox.motorController->SetVoltage(_setAlphaArmVoltage);
+}
+
+void AlphaArm::SetState(AlphaArmState state) {
+  _state = state;
+}
+
+void AlphaArm::SetGoal(units::radian_t goal){
+  _goal = goal;
+}
+
+void AlphaArm::SetArmRaw(units::volt_t voltage){
+  std::cout << "VOLTAGE: " << voltage.value() << std::endl;
+  _rawArmVoltage = voltage;
+}
+
+void AlphaArm::setControllerRaw(units::volt_t voltage) {
+  _controlledRawVoltage = voltage;
+}
diff --git a/src/main/cpp/AlphaArmBehaviour.cpp b/src/main/cpp/AlphaArmBehaviour.cpp
new file mode 100644
index 00000000..d3411a98
--- /dev/null
+++ b/src/main/cpp/AlphaArmBehaviour.cpp
@@ -0,0 +1,31 @@
+// Copyright (c) 2023-2024 CurtinFRC
+// Open Source Software, you can modify it according to the terms
+// of the MIT License at the root of this project
+
+#include "AlphaArmBehaviour.h"
+
+#include <frc/XboxController.h>
+
+AlphaArmManualControl::AlphaArmManualControl(AlphaArm* alphaArm, frc::XboxController* codriver)
+    : _alphaArm(alphaArm), _codriver(codriver) {
+  Controls(alphaArm);
+}
+
+void AlphaArmManualControl::OnTick(units::second_t dt) {
+
+  if (_codriver->GetStartButtonPressed()) {
+    if (_rawControl == true) {
+      _rawControl = false;
+    } else {
+      _rawControl = true;
+    }
+  }
+
+  if (_rawControl) {
+    _alphaArm->SetState(AlphaArmState::kRaw);
+    _alphaArm->SetArmRaw(_codriver->GetRightY() * 12_V);  
+  } else {
+    _alphaArm->SetState(AlphaArmState::kAmpAngle);
+    _alphaArm->setControllerRaw(wom::deadzone(_codriver->GetRightY()) * 12_V);
+  }
+}
diff --git a/src/main/cpp/Intake.cpp b/src/main/cpp/Intake.cpp
new file mode 100644
index 00000000..a96c289b
--- /dev/null
+++ b/src/main/cpp/Intake.cpp
@@ -0,0 +1,81 @@
+// Copyright (c) 2023-2024 CurtinFRC
+// Open Source Software, you can modify it according to the terms
+// of the MIT License at the root of this project
+
+#include "Intake.h"
+
+Intake::Intake(IntakeConfig config) : _config(config) {}
+
+IntakeConfig Intake::GetConfig() {
+  return _config;
+}
+
+void Intake::OnUpdate(units::second_t dt) {
+  switch (_state) {
+    case IntakeState::kIdle: {
+      if (_config.intakeSensor->Get() == false) {
+        setState(IntakeState::kHold);
+      }
+      _stringStateName = "Idle";
+      _setVoltage = 0_V;
+    } break;
+
+    case IntakeState::kRaw: {
+      _stringStateName = "Raw";
+      _setVoltage = _rawVoltage;
+    } break;
+
+    case IntakeState::kEject: {
+      _stringStateName = "Eject";
+      _setVoltage = 7_V;
+      if (_config.intakeSensor->Get() == true) {
+        setState(IntakeState::kIdle);
+      }
+    } break;
+
+    case IntakeState::kHold: {
+      _stringStateName = "Hold";
+      _setVoltage = 0_V;
+    } break;
+
+    case IntakeState::kIntake: {
+      _stringStateName = "Intake";
+      _setVoltage = -7_V;
+      if (_config.intakeSensor->Get() == false) {
+        setState(IntakeState::kHold);
+      }
+    } break;
+
+    case IntakeState::kPass: {
+      _stringStateName = "Pass";
+      _setVoltage = -7_V;
+      if (_config.intakeSensor->Get() == true) {
+        setState(IntakeState::kIdle);
+      }
+    } break;
+    default:
+      std::cout << "Error: Intake in INVALID STATE." << std::endl;
+      break;
+  }
+  _table->GetEntry("State: ").SetString(_stringStateName);
+  _table->GetEntry("Motor Voltage: ").SetDouble(_setVoltage.value());
+  _table->GetEntry("Intake Sensor: ").SetBoolean(_config.intakeSensor->Get());
+  // _table->GetEntry("Shooter Sensor: ").SetBoolean(_config.shooterSensor->Get());
+  // _table->GetEntry("Intake Sensor: ").SetBoolean(_config.intakeSensor->Get());
+  // _table->GetEntry("Shooter Sensor: ").SetBoolean(_config.shooterSensor->Get());
+  // _table->GetEntry("Magazine Sensor: ").SetBoolean(_config.magSensor->Get());
+
+  std::cout << _setVoltage.value() << std::endl;
+
+  _config.IntakeMotor.motorController->SetVoltage(_setVoltage);
+}
+
+void Intake::setState(IntakeState state) {
+  _state = state;
+}
+void Intake::setRaw(units::volt_t voltage) {
+  _rawVoltage = voltage;
+}
+IntakeState Intake::getState() {
+  return _state;
+}
\ No newline at end of file
diff --git a/src/main/cpp/IntakeBehaviour.cpp b/src/main/cpp/IntakeBehaviour.cpp
new file mode 100644
index 00000000..3192ef12
--- /dev/null
+++ b/src/main/cpp/IntakeBehaviour.cpp
@@ -0,0 +1,63 @@
+// Copyright (c) 2023-2024 CurtinFRC
+// Open Source Software, you can modify it according to the terms
+// of the MIT License at the root of this project
+
+#include "IntakeBehaviour.h"
+
+#include <frc/XboxController.h>
+
+IntakeManualControl::IntakeManualControl(Intake* intake, frc::XboxController& codriver)
+    : _intake(intake), _codriver(codriver) {
+  Controls(intake);
+}
+
+void IntakeManualControl::OnTick(units::second_t dt) {
+  if (_codriver.GetBButtonReleased()) {
+    if (_intake->getState() == IntakeState::kRaw) {
+      _intake->setState(IntakeState::kIdle);
+    } else {
+      _intake->setState(IntakeState::kRaw);
+    }
+  }
+
+  if (_intake->getState() == IntakeState::kRaw) {
+    if (_codriver.GetRightBumper()) {
+      _intake->setRaw(8_V);
+    } else if (_codriver.GetLeftBumper()) {
+      _intake->setRaw(-8_V);
+    } else {
+      _intake->setRaw(0_V);
+    }
+
+  } else {
+    if (_codriver.GetRightBumperPressed()) {
+      if (_intake->getState() == IntakeState::kIntake) {
+        _intake->setState(IntakeState::kIdle);
+      } else {
+        _intake->setState(IntakeState::kIntake);
+      }
+    }
+
+    if (_codriver.GetLeftBumper()) {
+      if (_intake->getState() == IntakeState::kEject) {
+        _intake->setState(IntakeState::kIdle);
+      } else {
+        _intake->setState(IntakeState::kEject);
+      }
+    }
+
+    if (_codriver.GetAButtonPressed()) {
+      if (_intake->getState() == IntakeState::kPass) {
+        _intake->setState(IntakeState::kIdle);
+      } else {
+        _intake->setState(IntakeState::kPass);
+      }
+    }
+  }
+}
+
+IntakeAutoControl::IntakeAutoControl(Intake* intake) : _intake(intake) {
+  Controls(intake);
+}
+
+void IntakeAutoControl::OnTick(units::second_t dt) {}
\ No newline at end of file
diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp
index 80f253d5..25e05917 100644
--- a/src/main/cpp/Robot.cpp
+++ b/src/main/cpp/Robot.cpp
@@ -4,6 +4,20 @@
 
 #include "Robot.h"
 
+#include <frc/TimedRobot.h>
+#include <frc/Timer.h>
+#include <frc/controller/RamseteController.h>
+#include <frc/kinematics/DifferentialDriveKinematics.h>
+#include <networktables/DoubleTopic.h>
+#include <networktables/NetworkTable.h>
+#include <networktables/NetworkTableInstance.h>
+#include <units/acceleration.h>
+#include <units/angle.h>
+#include <units/length.h>
+#include <units/time.h>
+#include <units/velocity.h>
+#include <units/voltage.h>
+
 // include units
 #include <units/velocity.h>
 #include <units/acceleration.h>
@@ -15,44 +29,106 @@
 #include <frc/kinematics/DifferentialDriveKinematics.h>
 #include <frc/controller/RamseteController.h>
 #include <frc/Timer.h>
+#include "behaviour/HasBehaviour.h"
 
 static units::second_t lastPeriodic;
 
 void Robot::RobotInit() {
+  sched = wom::BehaviourScheduler::GetInstance();
   m_chooser.SetDefaultOption("Default Auto", "Default Auto");
 
-  frc::SmartDashboard::PutData("Auto Selector", &m_chooser);
+  // frc::SmartDashboard::PutData("Auto Selector", &m_chooser);
+
+  // m_path_chooser.SetDefaultOption("Path1", "paths/output/Path1.wpilib.json");
 
-  m_path_chooser.SetDefaultOption("Path1", "paths/output/Path1.wpilib.json");
+  // m_path_chooser.AddOption("Path1", "paths/output/Path1.wpilib.json");
+  // m_path_chooser.AddOption("Path2", "paths/output/Path2.wpilib.json");
 
-  m_path_chooser.AddOption("Path1", "paths/output/Path1.wpilib.json");
-  m_path_chooser.AddOption("Path2", "paths/output/Path2.wpilib.json");
+  // shooter = new Shooter(robotmap.shooterSystem.config);
+  // wom::BehaviourScheduler::GetInstance()->Register(shooter);
+  // shooter->SetDefaultBehaviour(
+  //     [this]() { return wom::make<ShooterManualControl>(shooter, &robotmap.controllers.codriver); });
 
-  frc::SmartDashboard::PutData("Path Selector", &m_path_chooser);
 
-  frc::SmartDashboard::PutData("Field", &m_field);
+  // frc::SmartDashboard::PutData("Path Selector", &m_path_chooser);
+  // frc::SmartDashboard::PutData("Path Selector", &m_path_chooser);
 
-  simulation_timer = frc::Timer();
+  // frc::SmartDashboard::PutData("Field", &m_field);
 
-  robotmap.swerveBase.gyro->Reset();
+  // simulation_timer = frc::Timer();
 
-  _swerveDrive = new wom::SwerveDrive(robotmap.swerveBase.config, frc::Pose2d());
+  // robotmap.swerveBase.gyro->Reset();
+
+  _swerveDrive =
+      new wom::SwerveDrive(robotmap.swerveBase.config, frc::Pose2d());
   wom::BehaviourScheduler::GetInstance()->Register(_swerveDrive);
-  _swerveDrive->SetDefaultBehaviour(
-      [this]() { return wom::make<wom::ManualDrivebase>(_swerveDrive, &robotmap.controllers.driver); });
+  _swerveDrive->SetDefaultBehaviour([this]() {
+    return wom::make<wom::ManualDrivebase>(_swerveDrive,
+                                           &robotmap.controllers.driver);
+  });
+
+
+  // alphaArm = new AlphaArm(robotmap.alphaArmSystem.config);
+  // wom::BehaviourScheduler::GetInstance()->Register(alphaArm);
+  // alphaArm->SetDefaultBehaviour(
+  //     [this]() { return wom::make<AlphaArmManualControl>(alphaArm, &robotmap.controllers.codriver); });
+  
+  // // m_driveSim = new wom::TempSimSwerveDrive(&simulation_timer, &m_field);
+  // // m_driveSim = wom::TempSimSwerveDrive();
+
+  //   intake = new Intake(robotmap.intakeSystem.config);
+  // wom::BehaviourScheduler::GetInstance()->Register(intake);
+  // intake->SetDefaultBehaviour(
+  //     [this]() { return wom::make<IntakeManualControl>(intake, robotmap.controllers.codriver); });
+
+  robotmap.swerveBase.moduleConfigs[0].turnMotor.encoder->SetEncoderOffset(0.588_rad);
+  robotmap.swerveBase.moduleConfigs[1].turnMotor.encoder->SetEncoderOffset(2.647_rad);
+  robotmap.swerveBase.moduleConfigs[2].turnMotor.encoder->SetEncoderOffset(2.979_rad);
+  robotmap.swerveBase.moduleConfigs[3].turnMotor.encoder->SetEncoderOffset(4.388_rad);
+
+  // robotmap.swerveBase.moduleConfigs[0].turnMotor.encoder->SetEncoderOffset(0_rad);
+  // robotmap.swerveBase.moduleConfigs[1].turnMotor.encoder->SetEncoderOffset(0_rad);
+  // robotmap.swerveBase.moduleConfigs[2].turnMotor.encoder->SetEncoderOffset(0_rad);
+  // robotmap.swerveBase.moduleConfigs[3].turnMotor.encoder->SetEncoderOffset(0_rad);
+
+  // robotmap.swerveBase.moduleConfigs[0].driveMotor.motorController->SetInverted(true);
+  // robotmap.swerveBase.moduleConfigs[1].driveMotor.motorController->SetInverted(true);
+
+  // robotmap.swerveBase.moduleConfigs[0].driveMotor.motorController->SetInverted(true);
+  // robotmap.swerveBase.moduleConfigs[2].driveMotor.motorController->SetInverted(true);
+
+  //robotmap.alphaArmSystem.armEncoder->Reset();
+  
+      
+  // robotmap.swerveBase.moduleConfigs[0].turnMotor.encoder->SetEncoderOffset(0_rad);
+  // robotmap.swerveBase.moduleConfigs[1].turnMotor.encoder->SetEncoderOffset(0_rad);
+  // robotmap.swerveBase.moduleConfigs[2].turnMotor.encoder->SetEncoderOffset(0_rad);
+  // robotmap.swerveBase.moduleConfigs[3].turnMotor.encoder->SetEncoderOffset(0_rad);
+
+  lastPeriodic = wom::now();
+
 
-  // m_driveSim = new wom::TempSimSwerveDrive(&simulation_timer, &m_field);
-  // m_driveSim = wom::TempSimSwerveDrive();
 }
 
 void Robot::RobotPeriodic() {
+  //double encoderDistance = robotmap.alphaArmSystem.armEncoder.GetDistance();
   auto dt = wom::now() - lastPeriodic;
   lastPeriodic = wom::now();
 
   loop.Poll();
   wom::BehaviourScheduler::GetInstance()->Tick();
+  sched->Tick();
+
+  robotmap.swerveTable.swerveDriveTable->GetEntry("Encoder 0 offset: ").SetDouble(robotmap.swerveBase.moduleConfigs[0].turnMotor.encoder->GetEncoderPosition().value());
+  robotmap.swerveTable.swerveDriveTable->GetEntry("Encoder 1 offset: ").SetDouble(robotmap.swerveBase.moduleConfigs[1].turnMotor.encoder->GetEncoderPosition().value());
+  robotmap.swerveTable.swerveDriveTable->GetEntry("Encoder 2 offset: ").SetDouble(robotmap.swerveBase.moduleConfigs[2].turnMotor.encoder->GetEncoderPosition().value());
+  robotmap.swerveTable.swerveDriveTable->GetEntry("Encoder 3 offset: ").SetDouble(robotmap.swerveBase.moduleConfigs[3].turnMotor.encoder->GetEncoderPosition().value());
 
+  // shooter->OnUpdate(dt);
+  // intake->OnUpdate(dt);
+  // alphaArm->OnUpdate(dt);
   _swerveDrive->OnUpdate(dt);
+  
 }
 
 void Robot::AutonomousInit() {
@@ -60,15 +136,22 @@ void Robot::AutonomousInit() {
 
   loop.Clear();
   sched->InterruptAll();
-  // _swerveDrive->OnStart();
 }
 void Robot::AutonomousPeriodic() {
   // m_driveSim->OnUpdate();
 }
 
 void Robot::TeleopInit() {
-  // _swerveDrive->OnStart();
-  // sched->InterruptAll();
+  loop.Clear();
+  wom::BehaviourScheduler* sched = wom::BehaviourScheduler::GetInstance();
+  // shooter->OnStart();
+  // alphaArm->OnStart();
+  sched->InterruptAll();
+
+  // frontLeft->SetVoltage(4_V);
+  // frontRight->SetVoltage(4_V);
+  // backLeft->SetVoltage(4_V);
+  // backRight->SetVoltage(4_V);
 }
 void Robot::TeleopPeriodic() {}
 
@@ -77,7 +160,3 @@ void Robot::DisabledPeriodic() {}
 
 void Robot::TestInit() {}
 void Robot::TestPeriodic() {}
-
-void Robot::SimulationInit() {}
-
-void Robot::SimulationPeriodic() {}
diff --git a/src/main/cpp/Shooter.cpp b/src/main/cpp/Shooter.cpp
new file mode 100644
index 00000000..c6c40a02
--- /dev/null
+++ b/src/main/cpp/Shooter.cpp
@@ -0,0 +1,121 @@
+// Copyright (c) 2023-2024 CurtinFRC
+// Open Source Software, you can modify it according to the terms
+// of the MIT License at the root of this project
+#include "Shooter.h"
+
+
+Shooter::Shooter(ShooterConfig config) : _config(config), _pid(config.path + "/pid", config.pidConfig) {} 
+
+void Shooter::OnStart() {
+  _pid.Reset();
+}
+
+
+void Shooter::OnUpdate(units::second_t dt) {
+  // _pid.SetTolerance(0.5, 4);
+  table->GetEntry("Error").SetDouble(_pid.GetError().value());
+  // table->GetEntry("Acceleration Error").SetDouble(_pid.GetVelocityError());
+  table->GetEntry("SetPoint").SetDouble(_pid.GetSetpoint().value());
+  // table->GetEntry("Current Pos").SetDouble(_config.ShooterGearbox.encoder->GetEncoderAngularVelocity().value());
+  // table->GetEntry("EncoderValue").SetDouble(_config.ShooterGearbox.encoder->GetVelocityValue());
+  table->GetEntry("Shooting").SetString(_statename);
+  switch (_state) {
+    case ShooterState::kIdle: {
+      _statename = "Idle";
+      _pid.Reset();
+      holdVoltage = 0_V;
+      std::cout << "KIdle" << std::endl;
+      _setVoltage = 0_V;
+      // if (_shooterSensor.Get()) {
+      //   _state = ShooterState::kReverse;
+      // }
+      
+    } break;
+    case ShooterState::kSpinUp: {
+      _statename = "SpinUp";
+      std::cout << "KSpinUp" << std::endl;
+      _pid.SetSetpoint(_goal);
+      units::volt_t pidCalculate = units::volt_t {_pid.Calculate(_config.ShooterGearbox.encoder->GetEncoderAngularVelocity(), dt, 0_V)};
+      std::cout << "KShooting" << std::endl;
+
+      if (_pid.IsStable()) {
+        holdVoltage = pidCalculate;
+        std::cout << "STABLE" << std::endl;
+        _state = ShooterState::kShooting;
+      }
+
+      if (holdVoltage.value() == 0) {
+        _setVoltage = pidCalculate;
+      } else {
+        _setVoltage = holdVoltage;
+      }
+
+    } break;
+    case ShooterState::kShooting: {
+      _statename = "Shooting";
+      
+      _pid.SetSetpoint(_goal);
+      units::volt_t pidCalculate = units::volt_t {_pid.Calculate(_config.ShooterGearbox.encoder->GetEncoderAngularVelocity(), dt, 0_V)};
+      std::cout << "KShooting" << std::endl;
+
+      if (_pid.IsStable()) {
+        holdVoltage = pidCalculate;
+        std::cout << "STABLE" << std::endl;
+      }
+
+      if (holdVoltage.value() == 0) {
+        _setVoltage = pidCalculate;
+      } else {
+        _setVoltage = holdVoltage;
+      }
+      
+    } break;
+
+    case ShooterState::kReverse: {
+      _statename = "Reverse";
+      _pid.Reset();
+      _setVoltage = -5_V;
+      std::cout << "KReverse" << std::endl;
+      // if (!_shooterSensor.Get()) {
+      //   SetState(ShooterState::kIdle);
+      // }
+    } break;
+    case ShooterState::kRaw: {
+      _statename = "Raw";
+      holdVoltage = 0_V;
+      _pid.Reset();
+      _setVoltage = _rawVoltage;
+      std::cout << "KRaw" << std::endl;
+      // if (_shooterSensor.Get()) {
+      //   SetState(ShooterState::kRaw);
+      // }
+    } break;
+    default: {
+      std::cout << "Error shooter in invalid state" << std::endl;
+    } break;
+  }
+  // table->GetEntry("Motor OutPut").SetDouble(_setVoltage.value());
+  table->GetEntry("Encoder Output").SetDouble(_config.ShooterGearbox.encoder->GetEncoderAngularVelocity().value());
+
+
+  _config.ShooterGearbox.motorController->SetVoltage(_setVoltage);
+}
+
+//       // if (_pid.AtSetpoint()) {
+//       //   SetState(ShooterState::kShooting);
+//       // }
+//       // table->GetEntry("PID Setpoint:").SetDouble(_pid.GetSetpoint());
+//       std::cout << "KShooting" << std::endl;
+//       _pid.SetSetpoint(20);
+//       // _pid.SetSetpoint(_goal.value());
+
+void Shooter::SetState(ShooterState state) {
+  _state = state;
+}
+void Shooter::SetRaw(units::volt_t voltage) {
+  _rawVoltage = voltage;
+  _state = ShooterState::kRaw;
+}
+void Shooter::SetPidGoal(units::radians_per_second_t goal) { 
+  _goal = goal;
+}
diff --git a/src/main/cpp/ShooterBehaviour.cpp b/src/main/cpp/ShooterBehaviour.cpp
new file mode 100644
index 00000000..7b370456
--- /dev/null
+++ b/src/main/cpp/ShooterBehaviour.cpp
@@ -0,0 +1,49 @@
+// Copyright (c) 2023-2024 CurtinFRC
+// Open Source Software, you can modify it according to the terms
+// of the MIT License at the root of this project
+
+#include "ShooterBehaviour.h"
+
+ShooterManualControl::ShooterManualControl(Shooter* shooter, frc::XboxController* tester)
+    : _shooter(shooter), _codriver(tester) {
+  Controls(shooter);
+}
+
+void ShooterManualControl::OnTick(units::second_t dt) {
+  table->GetEntry("RawControl").SetBoolean(_rawControl);
+
+  if (_codriver->GetBackButtonPressed()) {
+    if (_rawControl == true) {
+      _rawControl = false;
+    } else {
+      _rawControl = true;
+    }
+  }
+
+  if (_rawControl) {
+    _shooter->SetState(ShooterState::kRaw);
+    if (_codriver->GetLeftTriggerAxis() > 0.1) {
+      _shooter->SetRaw(12_V * _codriver->GetLeftTriggerAxis());
+    } else if (_codriver->GetRightTriggerAxis() > 0.1) {
+      _shooter->SetRaw(-12_V * _codriver->GetRightTriggerAxis());
+    } else {
+      _shooter->SetRaw(0_V);
+    }
+  } else {
+    if (_codriver->GetXButton()) {
+      _shooter->SetPidGoal(150_rad_per_s);
+      _shooter->SetState(ShooterState::kSpinUp);
+    } else if (_codriver->GetYButton()) {
+      _shooter->SetPidGoal(300_rad_per_s);
+      _shooter->SetState(ShooterState::kSpinUp);
+    }  else if (_codriver->GetLeftTriggerAxis() > 0.1) {
+      _shooter->SetState(ShooterState::kRaw);
+      _shooter->SetRaw(12_V * _codriver->GetLeftTriggerAxis());
+    } else if (_codriver->GetRightTriggerAxis() > 0.1) {
+      _shooter->SetState(ShooterState::kRaw);
+      _shooter->SetRaw(-12_V * _codriver->GetRightTriggerAxis());
+    } else {
+      _shooter->SetState(ShooterState::kIdle);
+    }
+  }
+}
diff --git a/src/main/include/AlphaArm.h b/src/main/include/AlphaArm.h
new file mode 100644
index 00000000..9a5e94dc
--- /dev/null
+++ b/src/main/include/AlphaArm.h
@@ -0,0 +1,78 @@
+// Copyright (c) 2023-2024 CurtinFRC
+// Open Source Software, you can modify it according to the terms
+// of the MIT License at the root of this project
+
+#pragma once
+#include <frc/DigitalInput.h>
+#include <frc/DutyCycleEncoder.h>
+#include <frc/controller/PIDController.h>
+#include <units/angular_velocity.h>
+#include "Wombat.h"
+#include "utils/PID.h"
+
+struct AlphaArmConfig {
+    wom::Gearbox alphaArmGearbox;
+    //wom::Gearbox wristGearbox;
+    //wom::DutyCycleEncoder* armEncoder;
+    //wom::CANSparkMaxEncoder* armEncoder;
+    wom::utils::PIDConfig<units::radian, units::volt> pidConfigA;
+    //wom::utils::PIDConfig<units::radians_per_second, units::volt> velocityConfig;
+    std::string path;
+    //void WriteNT(std::shared_ptr<nt::NetworkTable> table);
+
+};
+
+enum class AlphaArmState {
+    kIdle,
+    kIntakeAngle,
+    kAmpAngle,
+    kSpeakerAngle,
+    kStowed,
+    kRaw
+    //kForwardWrist,
+    //kReverseWrist,
+};
+
+class AlphaArm : public::behaviour::HasBehaviour{
+    public:
+     explicit AlphaArm(AlphaArmConfig config);
+
+    void OnStart();
+    void OnUpdate(units::second_t dt);
+    void SetArmRaw(units::volt_t voltage);
+    void SetState(AlphaArmState state);
+    void setControllerRaw(units::volt_t);
+    //void setGoal(units::radians_per_second_t);
+    void SetGoal(units::radian_t);
+    double GetArmEncoder();
+    AlphaArmConfig GetConfig();
+    std::shared_ptr<nt::NetworkTable> table = nt::NetworkTableInstance::GetDefault().GetTable("AlphaArm");
+    //units::radians_per_second_t goal;
+    //double goal;
+    
+    //frc::DutyCycleEncoder armEncoder{12};
+    //void SetRaw(units::volt_t voltage);
+
+    private:
+    //frc::PIDController _pidFRC;
+    wom::utils::PIDController<units::radian, units::volt> _pidWom;
+   // wom::utils::PIDController<units::radians_per_second, units::volt> _velocityPID;
+
+    std::shared_ptr<nt::NetworkTable> _table = nt::NetworkTableInstance::GetDefault().GetTable("AlphaArm");
+    AlphaArmConfig _config;
+    AlphaArmState _state = AlphaArmState::kIdle;
+    units::radian_t _goal; 
+    units::volt_t _setAlphaArmVoltage = 0_V;
+    units::volt_t _setWristVoltage = 0_V;
+
+    units::volt_t _rawArmVoltage = 0_V;
+    units::volt_t _rawWristVoltage = 0_V;
+    //units::radiant_t maxAngle = 1_radian_t;
+
+    units::radian_t _encoderSetpoint = 0_rad;
+    units::volt_t _controlledRawVoltage = 0_V;
+  units::radian_t _startingPos = 0_rad;
+
+    std::string _stateName = "Default";
+    bool started = false;
+};
diff --git a/src/main/include/AlphaArmBehaviour.h b/src/main/include/AlphaArmBehaviour.h
new file mode 100644
index 00000000..71825c62
--- /dev/null
+++ b/src/main/include/AlphaArmBehaviour.h
@@ -0,0 +1,22 @@
+// Copyright (c) 2023-2024 CurtinFRC
+// Open Source Software, you can modify it according to the terms
+// of the MIT License at the root of this project
+
+#pragma once
+
+#pragma once
+#include <frc/XboxController.h>
+
+#include "AlphaArm.h"
+#include "Wombat.h"
+
+class AlphaArmManualControl : public behaviour::Behaviour {
+ public:
+  explicit AlphaArmManualControl(AlphaArm* alphaArm, frc::XboxController* codriver);
+  void OnTick(units::second_t dt);
+
+ private:
+  AlphaArm* _alphaArm;
+  frc::XboxController* _codriver;
+  bool _rawControl = false;
+};
diff --git a/src/main/include/Intake.h b/src/main/include/Intake.h
new file mode 100644
index 00000000..1aca262a
--- /dev/null
+++ b/src/main/include/Intake.h
@@ -0,0 +1,43 @@
+// Copyright (c) 2023-2024 CurtinFRC
+// Open Source Software, you can modify it according to the terms
+// of the MIT License at the root of this project
+
+#pragma once
+
+#include <frc/DigitalInput.h>
+
+#include <memory>
+#include <string>
+
+#include "Wombat.h"
+
+struct IntakeConfig {
+  wom::Gearbox IntakeMotor;
+  frc::DigitalInput* intakeSensor;
+  frc::DigitalInput* magSensor;
+  frc::DigitalInput* shooterSensor;
+};
+
+enum class IntakeState { kIdle, kRaw, kHold, kEject, kIntake, kPass };
+
+class Intake : public behaviour::HasBehaviour {
+ public:
+  explicit Intake(IntakeConfig config);
+
+  void OnUpdate(units::second_t dt);
+
+  void setState(IntakeState state);
+  void setRaw(units::volt_t voltage);
+  IntakeState getState();
+  IntakeConfig GetConfig();
+
+ private:
+  IntakeConfig _config;
+  IntakeState _state = IntakeState::kIdle;
+
+  units::volt_t _rawVoltage = 0_V;
+  std::string _stringStateName = "error";
+  units::volt_t _setVoltage = 0_V;
+
+  std::shared_ptr<nt::NetworkTable> _table = nt::NetworkTableInstance::GetDefault().GetTable("Intake");
+};
\ No newline at end of file
diff --git a/src/main/include/IntakeBehaviour.h b/src/main/include/IntakeBehaviour.h
new file mode 100644
index 00000000..5dbe8ae6
--- /dev/null
+++ b/src/main/include/IntakeBehaviour.h
@@ -0,0 +1,34 @@
+// Copyright (c) 2023-2024 CurtinFRC
+// Open Source Software, you can modify it according to the terms
+// of the MIT License at the root of this project
+
+#pragma once
+
+#include <frc/XboxController.h>
+
+#include "Intake.h"
+#include "Wombat.h"
+
+class IntakeManualControl : public behaviour::Behaviour {
+ public:
+  explicit IntakeManualControl(Intake* intake, frc::XboxController& codriver);
+
+  void OnTick(units::second_t dt) override;
+
+ private:
+  Intake* _intake;
+  frc::XboxController& _codriver;
+
+  units::volt_t _rawVoltage;
+  units::volt_t _setVoltage;
+};
+
+class IntakeAutoControl : public behaviour::Behaviour {
+ public:
+  explicit IntakeAutoControl(Intake* intake);
+
+  void OnTick(units::second_t dt) override;
+
+ private:
+  Intake* _intake;
+};
\ No newline at end of file
diff --git a/src/main/include/Robot.h b/src/main/include/Robot.h
index bc98df2a..799ae2c1 100644
--- a/src/main/include/Robot.h
+++ b/src/main/include/Robot.h
@@ -3,7 +3,6 @@
 // of the MIT License at the root of this project
 
 #pragma once
-
 #include <frc/Encoder.h>
 #include <frc/TimedRobot.h>
 #include <frc/Timer.h>
@@ -16,11 +15,20 @@
 
 #include <string>
 
+#include "AlphaArm.h"
+#include "AlphaArmBehaviour.h"
+#include "Intake.h"
+#include "IntakeBehaviour.h"
+#include "RobotMap.h"
+#include "Shooter.h"
+#include "ShooterBehaviour.h"
 #include "RobotMap.h"
 #include "Wombat.h"
 
 class Robot : public frc::TimedRobot {
  public:
+  void TestInit() override;
+  void TestPeriodic() override;
   void RobotInit() override;
   void RobotPeriodic() override;
   void AutonomousInit() override;
@@ -29,16 +37,14 @@ class Robot : public frc::TimedRobot {
   void TeleopPeriodic() override;
   void DisabledInit() override;
   void DisabledPeriodic() override;
-  void TestInit() override;
-  void TestPeriodic() override;
-  void SimulationInit() override;
-  void SimulationPeriodic() override;
 
  private:
-  behaviour::BehaviourScheduler* sched;
   RobotMap robotmap;
+  wom::BehaviourScheduler* sched;
   frc::EventLoop loop;
+  // Shooter* shooter;
 
+  // Intake* intake;
   frc::SendableChooser<std::string> m_chooser;
 
   frc::Field2d m_field;
@@ -48,4 +54,13 @@ class Robot : public frc::TimedRobot {
   frc::SendableChooser<std::string> m_path_chooser;
 
   wom::SwerveDrive* _swerveDrive;
+  // AlphaArm* alphaArm;
+
+  ctre::phoenix6::hardware::TalonFX *frontLeft;
+  // AlphaArm *alphaArm;
+
+  // ctre::phoenix6::hardware::TalonFX *frontLeft;
+  // ctre::phoenix6::hardware::TalonFX *frontRight;
+  // ctre::phoenix6::hardware::TalonFX *backLeft;
+  // ctre::phoenix6::hardware::TalonFX *backRight;
 };
diff --git a/src/main/include/RobotMap.h b/src/main/include/RobotMap.h
index d5efd27a..b58e3d02 100644
--- a/src/main/include/RobotMap.h
+++ b/src/main/include/RobotMap.h
@@ -8,8 +8,12 @@
 #include <frc/DoubleSolenoid.h>
 #include <frc/XboxController.h>
 #include <frc/system/plant/DCMotor.h>
+#include <frc/Encoder.h>
+#include <frc/DutyCycleEncoder.h>
 #include <units/angle.h>
 #include <units/length.h>
+#include "utils/PID.h"
+#include "utils/Encoder.h"
 
 #include <string>
 
@@ -17,101 +21,181 @@
 #include <ctre/phoenix6/Pigeon2.hpp>
 #include <ctre/phoenix6/TalonFX.hpp>
 
+#include "AlphaArm.h"
+#include "AlphaArmBehaviour.h"
+#include "Intake.h"
+#include "Shooter.h"
 #include "Wombat.h"
+#include "Wombat.h"
+#include "AlphaArm.h"
+#include "AlphaArmBehaviour.h"
+
+//#include "Wombat/Encoder.h"
 
 struct RobotMap {
   struct Controllers {
     frc::XboxController driver = frc::XboxController(0);
-    frc::XboxController coDriver = frc::XboxController(1);
+    frc::XboxController codriver = frc::XboxController(1);
     frc::XboxController testController = frc::XboxController(2);
   };
   Controllers controllers;
 
+//   struct AlphaArmSystem {
+//     rev::CANSparkMax alphaArmMotor{12, rev::CANSparkMax::MotorType::kBrushless};
+//     wom::CANSparkMaxEncoder* armEncoder = new wom::CANSparkMaxEncoder(&alphaArmMotor, 0.02_m);
+    
+//     wom::Gearbox alphaArmGearbox{&alphaArmMotor, armEncoder, frc::DCMotor::NEO(1)};
+    
+//     wom::utils::PIDConfig<units::radian, units::volt> pidConfigA{
+//      "/path/to/pid/in/nt/tables",
+//     15_V / 180_deg, 
+//     0_V / (1_deg * 1_s), 
+//     0_V / (1_deg / 1_s),
+//     };
+
+//     AlphaArmConfig config {
+//         alphaArmGearbox, 
+//         pidConfigA,
+//     };
+
+//   };
+//   AlphaArmSystem alphaArmSystem;
+  
+//   struct IntakeSystem {
+//     rev::CANSparkMax intakeMotor{2, rev::CANSparkMax::MotorType::kBrushed};
+//     // wom::CANSparkMaxEncoder intakeEncoder{&intakeMotor, 0.1_m};
+//     frc::DigitalInput intakeSensor{4};
+//     // frc::DigitalInput magSensor{0};
+//     // frc::DigitalInput shooterSensor{0};
+
+//     wom::Gearbox IntakeGearbox{&intakeMotor, nullptr, frc::DCMotor::CIM(1)};
+
+//     IntakeConfig config{IntakeGearbox, &intakeSensor /*, &magSensor, &shooterSensor*/};
+//   };
+//   IntakeSystem intakeSystem;
+
+//     struct Shooter {
+//     rev::CANSparkMax shooterMotor{11, rev::CANSparkMax::MotorType::kBrushless};// Port 11
+//     // frc::DigitalInput shooterSensor{2};
+
+//     // wom::VoltageController shooterMotorGroup = wom::VoltagedController::Group(shooterMotor);
+//     wom::CANSparkMaxEncoder* shooterEncoder = new wom::CANSparkMaxEncoder(&shooterMotor, 0.01_m);
+//     wom::Gearbox shooterGearbox{&shooterMotor, shooterEncoder, frc::DCMotor::NEO(1)};
+
+//     wom::utils::PIDConfig<units::radians_per_second, units::volts> pidConfigS{
+//           "/armavator/arm/velocityPID/config",
+//           0.1_V / (360_deg / 1_s),
+//           0.03_V / 25_deg,
+//           0.001_V / (90_deg / 1_s / 1_s),
+//           5_rad_per_s,
+//           10_rad_per_s / 1_s
+//     };
+
+//     ShooterConfig config{
+//         "shooterGearbox",
+//         shooterGearbox,
+//         pidConfigS
+//     };
+
+    
+
+//   };
+//   Shooter shooterSystem;
+ 
   struct SwerveBase {
-    ctre::phoenix6::hardware::CANcoder frontLeftCancoder{19};
-    ctre::phoenix6::hardware::CANcoder frontRightCancoder{17};
-    ctre::phoenix6::hardware::CANcoder backLeftCancoder{16};
-    ctre::phoenix6::hardware::CANcoder backRightCancoder{18};
+    ctre::phoenix6::hardware::CANcoder frontLeftCancoder{16, "Drivebase"};
+    ctre::phoenix6::hardware::CANcoder frontRightCancoder{18, "Drivebase"};
+    ctre::phoenix6::hardware::CANcoder backLeftCancoder{17, "Drivebase"};
+    ctre::phoenix6::hardware::CANcoder backRightCancoder{19, "Drivebase"};
 
-    ctre::phoenix6::hardware::Pigeon2* gyro = new ctre::phoenix6::hardware::Pigeon2(20, "Drivebase");
+    ctre::phoenix6::hardware::Pigeon2* gyro =
+        new ctre::phoenix6::hardware::Pigeon2(20, "Drivebase");
     wpi::array<ctre::phoenix6::hardware::TalonFX*, 4> turnMotors{
-        new ctre::phoenix6::hardware::TalonFX(7, "Drivebase"),
-        new ctre::phoenix6::hardware::TalonFX(5, "Drivebase"),
-        new ctre::phoenix6::hardware::TalonFX(1, "Drivebase"),
-        new ctre::phoenix6::hardware::TalonFX(3, "Drivebase")};
+        new ctre::phoenix6::hardware::TalonFX(6, "Drivebase"),   // front left
+        new ctre::phoenix6::hardware::TalonFX(7, "Drivebase"),   // front right
+        new ctre::phoenix6::hardware::TalonFX(4, "Drivebase"),   // back left 
+        new ctre::phoenix6::hardware::TalonFX(2, "Drivebase")};  // back right
     wpi::array<ctre::phoenix6::hardware::TalonFX*, 4> driveMotors{
-        new ctre::phoenix6::hardware::TalonFX(9, "Drivebase"),
-        new ctre::phoenix6::hardware::TalonFX(6, "Drivebase"),
-        new ctre::phoenix6::hardware::TalonFX(2, "Drivebase"),
-        new ctre::phoenix6::hardware::TalonFX(4, "Drivebase")};
+        new ctre::phoenix6::hardware::TalonFX(5, "Drivebase"),   // front left
+        new ctre::phoenix6::hardware::TalonFX(9, "Drivebase"),   // front right
+        new ctre::phoenix6::hardware::TalonFX(3, "Drivebase"),   // back left
+        new ctre::phoenix6::hardware::TalonFX(1, "Drivebase")};  // back right
 
     wpi::array<wom::SwerveModuleConfig, 4> moduleConfigs{
-        wom::SwerveModuleConfig{
+        wom::SwerveModuleConfig{ //CORRECT
             // front left module
-            frc::Translation2d(10.761_in, 9.455_in),
-            wom::Gearbox{driveMotors[0], new wom::TalonFXEncoder(driveMotors[0], 0.0445_m, 6.75),
-                         frc::DCMotor::Falcon500(1).WithReduction(6.75)},
-            wom::Gearbox{turnMotors[0], new wom::CanEncoder(19, 0.0445_m, 4096, 12.8),
+            frc::Translation2d(-10_in, 9_in),
+            wom::Gearbox{
+                driveMotors[0],
+                new wom::TalonFXEncoder(driveMotors[0], 0.0445_m, 6.75),
+                frc::DCMotor::Falcon500(1).WithReduction(6.75)},
+            wom::Gearbox{turnMotors[0],
+                         new wom::CanEncoder(16, 0.0445_m, 4096, 12.8),
                          frc::DCMotor::Falcon500(1).WithReduction(12.8)},
             &frontLeftCancoder, 4_in / 2},
-        wom::SwerveModuleConfig{
+        wom::SwerveModuleConfig{ //CORRECT
             // front right module
-            frc::Translation2d(10.761_in, -9.455_in),
-            wom::Gearbox{driveMotors[1], new wom::TalonFXEncoder(driveMotors[1], 0.0445_m, 6.75),
-                         frc::DCMotor::Falcon500(1).WithReduction(6.75)},
-            wom::Gearbox{turnMotors[1], new wom::CanEncoder(17, 0.0445_m, 4096, 12.8),
+            frc::Translation2d(10_in, 9_in),
+            wom::Gearbox{
+                driveMotors[1],
+                new wom::TalonFXEncoder(driveMotors[1], 0.0445_m, 6.75),
+                frc::DCMotor::Falcon500(1).WithReduction(6.75)},
+            wom::Gearbox{turnMotors[1],
+                         new wom::CanEncoder(18, 0.0445_m, 4096, 12.8),
                          frc::DCMotor::Falcon500(1).WithReduction(12.8)},
             &frontRightCancoder, 4_in / 2},
         wom::SwerveModuleConfig{
             // back left module
-            frc::Translation2d(-10.761_in, 9.455_in),
-            wom::Gearbox{driveMotors[2], new wom::TalonFXEncoder(driveMotors[2], 0.0445_m, 6.75),
-                         frc::DCMotor::Falcon500(1).WithReduction(6.75)},
-            wom::Gearbox{turnMotors[2], new wom::CanEncoder(16, 0.0445_m, 4096, 12.8),
+            frc::Translation2d(-10_in, 9_in),
+            wom::Gearbox{
+                driveMotors[2],
+                new wom::TalonFXEncoder(driveMotors[2], 0.0445_m, 6.75),
+                frc::DCMotor::Falcon500(1).WithReduction(6.75)},
+            wom::Gearbox{turnMotors[2],
+                         new wom::CanEncoder(17, 0.0445_m, 4096, 12.8),
                          frc::DCMotor::Falcon500(1).WithReduction(12.8)},
             &backRightCancoder, 4_in / 2},
         wom::SwerveModuleConfig{
             // back right module
-            frc::Translation2d(-10.761_in, -9.455_in),
-            wom::Gearbox{driveMotors[3], new wom::TalonFXEncoder(driveMotors[3], 0.0445_m, 6.75),
-                         frc::DCMotor::Falcon500(1).WithReduction(6.75)},
-            wom::Gearbox{turnMotors[3], new wom::CanEncoder(18, 0.0445_m, 4096, 12.8),
+            frc::Translation2d(-10_in, -9_in),
+            wom::Gearbox{
+                driveMotors[3],
+                new wom::TalonFXEncoder(driveMotors[3], 0.0445_m, 6.75),
+                frc::DCMotor::Falcon500(1).WithReduction(6.75)},
+            wom::Gearbox{turnMotors[3],
+                         new wom::CanEncoder(19, 0.0445_m, 4096, 12.8),
                          frc::DCMotor::Falcon500(1).WithReduction(12.8)},
             &backLeftCancoder, 4_in / 2},
     };
 
     // Setting the PID path and values to be used for SwerveDrive and
     // SwerveModules
-    wom::SwerveModule::angle_pid_conf_t anglePID{
-        "/drivetrain/pid/angle/config", 2_V / 360_deg, 0.0_V / (100_deg * 1_s),
-        0_V / (100_deg / 1_s),          1_deg,         0.5_deg / 2_s};
+    /*wom::SwerveModule::angle_pid_conf_t anglePID{
+        "/drivetrain/pid/angle/config", 90_V / 360_deg, 0.0_V / (100_deg * 1_s),
+        0_V / (100_deg / 1_s)};*/
     wom::SwerveModule::velocity_pid_conf_t velocityPID{
         "/drivetrain/pid/velocity/config",
         //  12_V / 4_mps // webers per metre
     };
-    wom::SwerveDriveConfig::pose_angle_conf_t poseAnglePID{
+    /*wom::SwerveDriveConfig::pose_angle_conf_t poseAnglePID{
         "/drivetrain/pid/pose/angle/config",
-        180_deg / 1_s / 45_deg,
-        wom::SwerveDriveConfig::pose_angle_conf_t::ki_t{0.1},
-        0_deg / 1_deg,
-        10_deg,
-        10_deg / 1_s};
+        0_deg / 1_s / 45_deg,
+        wom::SwerveDriveConfig::pose_angle_conf_t::ki_t{0},
+        0_deg / 1_deg};*/
     wom::SwerveDriveConfig::pose_position_conf_t posePositionPID{
         "/drivetrain/pid/pose/position/config",
-        3_mps / 1_m,
+        0_mps / 1_m,
         wom::SwerveDriveConfig::pose_position_conf_t::ki_t{0.15},
         0_m / 1_m,
-        20_cm,
-        10_cm / 1_s,
-        10_cm};
+        0_cm};
 
     // the config for the whole swerve drive
     wom::SwerveDriveConfig config{"/drivetrain",
-                                  anglePID,
+                                  //anglePID,
                                   velocityPID,
                                   moduleConfigs,  // each module
                                   gyro,
-                                  poseAnglePID,
+                                  //poseAnglePID,
                                   posePositionPID,
                                   60_kg,  // robot mass (estimate rn)
                                   {0.1, 0.1, 0.1},
@@ -129,4 +213,21 @@ struct RobotMap {
     //}
   };
   SwerveBase swerveBase;
+
+  struct SwerveTable {
+    std::shared_ptr<nt::NetworkTable> swerveDriveTable =
+        nt::NetworkTableInstance::GetDefault().GetTable("swerve");
+  };
+  SwerveTable swerveTable;
+
+  // struct AlphaArmSystem {
+  //   rev::CANSparkMax alphaArmMotor{12, rev::CANSparkMax::MotorType::kBrushless};
+  //   rev::CANSparkMax wristMotor{15, rev::CANSparkMax::MotorType::kBrushless};
+
+  //   wom::Gearbox alphaArmGearbox{&alphaArmMotor, nullptr, frc::DCMotor::NEO(1)};
+  //   wom::Gearbox wristGearbox{&wristMotor, nullptr, frc::DCMotor::NEO(1)};
+
+  //   AlphaArmConfig config{alphaArmGearbox, wristGearbox};
+  // };
+  // AlphaArmSystem alphaArmSystem;
 };
diff --git a/src/main/include/Shooter.h b/src/main/include/Shooter.h
new file mode 100644
index 00000000..965b3654
--- /dev/null
+++ b/src/main/include/Shooter.h
@@ -0,0 +1,49 @@
+// Copyright (c) 2023-2024 CurtinFRC
+// Open Source Software, you can modify it according to the terms
+// of the MIT License at the root of this project
+#pragma once
+#include <frc/DigitalInput.h>
+#include <frc/controller/PIDController.h>
+#include <units/angular_velocity.h>
+
+#include <memory>
+#include <string>
+
+#include "Wombat.h"
+
+
+struct ShooterConfig {
+  std::string path;
+  wom::Gearbox ShooterGearbox;
+  // wom::PIDConfig<units::radians_per_second, units::volt> pidConfig;
+  // frc::DigitalInput* shooterSensor;
+  wom::PIDConfig<units::radians_per_second, units::volt> pidConfig;
+};
+
+enum class ShooterState { kIdle, kShooting, kSpinUp, kReverse, kRaw };
+
+class Shooter : public behaviour::HasBehaviour {
+ public:
+  explicit Shooter(ShooterConfig config);
+
+  void OnStart();
+
+  void OnUpdate(units::second_t dt);
+  void SetState(ShooterState state);
+  void SetRaw(units::volt_t voltage);
+  void SetPidGoal(units::radians_per_second_t);
+  ShooterConfig GetConfig() { return _config; }
+
+ private:
+  ShooterConfig _config;
+  std::shared_ptr<nt::NetworkTable> table = nt::NetworkTableInstance::GetDefault().GetTable("Shooter");
+  ShooterState _state = ShooterState::kRaw;
+  units::volt_t _rawVoltage;
+  units::radians_per_second_t _goal;
+  units::volt_t _setVoltage = 0_V;
+  wom::PIDController<units::radians_per_second, units::volt> _pid;
+  // frc::DigitalInput _shooterSensor{0};
+
+  units::volt_t holdVoltage = 0_V;
+  std::string _statename = "default";
+};
diff --git a/src/main/include/ShooterBehaviour.h b/src/main/include/ShooterBehaviour.h
new file mode 100644
index 00000000..41e598df
--- /dev/null
+++ b/src/main/include/ShooterBehaviour.h
@@ -0,0 +1,25 @@
+// Copyright (c) 2023-2024 CurtinFRC
+// Open Source Software, you can modify it according to the terms
+// of the MIT License at the root of this project
+
+#pragma once
+
+#include <frc/XboxController.h>
+
+#include "Robot.h"
+#include "Shooter.h"
+#include "Wombat.h"
+
+class ShooterManualControl : public behaviour::Behaviour {
+ public:
+  ShooterManualControl(Shooter* shooter, frc::XboxController* codriver); 
+
+  void OnTick(units::second_t dt) override;
+
+ private:
+  Shooter* _shooter;
+  frc::XboxController* _codriver;
+
+  bool _rawControl = false;
+  std::shared_ptr<nt::NetworkTable> table = nt::NetworkTableInstance::GetDefault().GetTable("Shooter Behaviour");
+};
\ No newline at end of file
diff --git a/wombat/LICENSE b/wombat/LICENSE
new file mode 100644
index 00000000..b1561bab
--- /dev/null
+++ b/wombat/LICENSE
@@ -0,0 +1,21 @@
+MIT License
+
+Copyright (c) 2023 CurtinFRC
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in all
+copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+SOFTWARE.
diff --git a/wombat/README.md b/wombat/README.md
new file mode 100644
index 00000000..0595c6cb
--- /dev/null
+++ b/wombat/README.md
@@ -0,0 +1 @@
+# Wombat
diff --git a/wombat/settings.gradle b/wombat/settings.gradle
new file mode 100644
index 00000000..8b137891
--- /dev/null
+++ b/wombat/settings.gradle
@@ -0,0 +1 @@
+
diff --git a/wombat/src/main/cpp/behaviour/Behaviour.cpp b/wombat/src/main/cpp/behaviour/Behaviour.cpp
index 7312c922..d21d9e57 100644
--- a/wombat/src/main/cpp/behaviour/Behaviour.cpp
+++ b/wombat/src/main/cpp/behaviour/Behaviour.cpp
@@ -10,7 +10,9 @@ using namespace behaviour;
 
 // Behaviour
 Behaviour::Behaviour(std::string name, units::time::second_t period)
-    : _bhvr_name(name), _bhvr_period(period), _bhvr_state(BehaviourState::INITIALISED) {}
+    : _bhvr_name(name),
+      _bhvr_period(period),
+      _bhvr_state(BehaviourState::INITIALISED) {}
 Behaviour::~Behaviour() {
   if (!IsFinished())
     Interrupt();
@@ -80,7 +82,8 @@ bool Behaviour::Tick() {
 
     if (dt > 2 * _bhvr_period) {
       std::cerr << "Behaviour missed deadline. Reduce Period. Dt=" << dt.value()
-                << " Dt(deadline)=" << (2 * _bhvr_period).value() << ". Bhvr: " << GetName() << std::endl;
+                << " Dt(deadline)=" << (2 * _bhvr_period).value()
+                << ". Bhvr: " << GetName() << std::endl;
     }
 
     if (_bhvr_timeout.value() > 0 && _bhvr_timer > _bhvr_timeout)
@@ -97,7 +100,8 @@ bool Behaviour::IsRunning() const {
 }
 
 bool Behaviour::IsFinished() const {
-  return _bhvr_state != BehaviourState::INITIALISED && _bhvr_state != BehaviourState::RUNNING;
+  return _bhvr_state != BehaviourState::INITIALISED &&
+         _bhvr_state != BehaviourState::RUNNING;
 }
 
 void Behaviour::Stop(BehaviourState new_state) {
@@ -169,7 +173,8 @@ void ConcurrentBehaviour::Add(Behaviour::ptr behaviour) {
 }
 
 std::string ConcurrentBehaviour::GetName() const {
-  std::string msg = (_reducer == ConcurrentBehaviourReducer::ALL ? "ALL { " : "RACE {");
+  std::string msg =
+      (_reducer == ConcurrentBehaviourReducer::ALL ? "ALL { " : "RACE {");
   for (auto b : _children)
     msg += b->GetName() + ", ";
   msg += "}";
@@ -185,8 +190,8 @@ void ConcurrentBehaviour::OnStart() {
         using namespace std::chrono_literals;
 
         b->Tick();
-        std::this_thread::sleep_for(
-            std::chrono::milliseconds(static_cast<int64_t>(b->GetPeriod().value() * 1000)));
+        std::this_thread::sleep_for(std::chrono::milliseconds(
+            static_cast<int64_t>(b->GetPeriod().value() * 1000)));
       }
 
       if (IsFinished() && !b->IsFinished())
@@ -268,8 +273,10 @@ void WaitFor::OnTick(units::time::second_t dt) {
 }
 
 // WaitTime
-WaitTime::WaitTime(units::time::second_t time) : WaitTime([time]() { return time; }) {}
-WaitTime::WaitTime(std::function<units::time::second_t()> time_fn) : _time_fn(time_fn) {}
+WaitTime::WaitTime(units::time::second_t time)
+    : WaitTime([time]() { return time; }) {}
+WaitTime::WaitTime(std::function<units::time::second_t()> time_fn)
+    : _time_fn(time_fn) {}
 
 void WaitTime::OnStart() {
   _time = _time_fn();
diff --git a/wombat/src/main/cpp/behaviour/BehaviourScheduler.cpp b/wombat/src/main/cpp/behaviour/BehaviourScheduler.cpp
index d34bf4aa..26863c79 100644
--- a/wombat/src/main/cpp/behaviour/BehaviourScheduler.cpp
+++ b/wombat/src/main/cpp/behaviour/BehaviourScheduler.cpp
@@ -10,7 +10,7 @@ BehaviourScheduler::BehaviourScheduler() {}
 
 BehaviourScheduler::~BehaviourScheduler() {
   for (HasBehaviour* sys : _systems) {
-    if (sys->_active_behaviour)
+    if (sys->_active_behaviour != nullptr)
       sys->_active_behaviour->Interrupt();
   }
 
@@ -52,8 +52,8 @@ void BehaviourScheduler::Schedule(Behaviour::ptr behaviour) {
         std::lock_guard<std::recursive_mutex> lk(_active_mtx);
         behaviour->Tick();
       }
-      std::this_thread::sleep_for(
-          std::chrono::milliseconds(static_cast<int64_t>(behaviour->GetPeriod().value() * 1000)));
+      std::this_thread::sleep_for(std::chrono::milliseconds(
+          static_cast<int64_t>(behaviour->GetPeriod().value() * 1000)));
     }
   });
 }
@@ -78,7 +78,8 @@ void BehaviourScheduler::Tick() {
 void BehaviourScheduler::InterruptAll() {
   std::lock_guard<std::recursive_mutex> lk(_active_mtx);
   for (HasBehaviour* sys : _systems) {
-    if (sys->_active_behaviour)
+    if (sys->_active_behaviour != nullptr) {
       sys->_active_behaviour->Interrupt();
+    }
   }
 }
diff --git a/wombat/src/main/cpp/behaviour/HasBehaviour.cpp b/wombat/src/main/cpp/behaviour/HasBehaviour.cpp
index f2f557c5..964522c6 100644
--- a/wombat/src/main/cpp/behaviour/HasBehaviour.cpp
+++ b/wombat/src/main/cpp/behaviour/HasBehaviour.cpp
@@ -8,7 +8,8 @@
 
 using namespace behaviour;
 
-void HasBehaviour::SetDefaultBehaviour(std::function<std::shared_ptr<Behaviour>(void)> fn) {
+void HasBehaviour::SetDefaultBehaviour(
+    std::function<std::shared_ptr<Behaviour>(void)> fn) {
   _default_behaviour_producer = fn;
 }
 
diff --git a/wombat/src/main/cpp/drivetrain/Drivetrain.cpp b/wombat/src/main/cpp/drivetrain/Drivetrain.cpp
index 4a5cb744..f3ce364d 100644
--- a/wombat/src/main/cpp/drivetrain/Drivetrain.cpp
+++ b/wombat/src/main/cpp/drivetrain/Drivetrain.cpp
@@ -1,4 +1,4 @@
-// Copyright (c) 2023-2024 CurtinFRC
+/* // Copyright (c) 2023-2024 CurtinFRC
 // Open Source Software, you can modify it according to the terms
 // of the MIT License at the root of this project
 
@@ -11,7 +11,8 @@
 using namespace frc;
 using namespace units;
 
-wom::drivetrain::Drivetrain::Drivetrain(DrivetrainConfig* config, XboxController& driver)
+wom::drivetrain::Drivetrain::Drivetrain(DrivetrainConfig* config,
+                                        XboxController& driver)
     : _config(config), _driver(driver) {}
 wom::drivetrain::Drivetrain::~Drivetrain() {}
 
@@ -37,15 +38,16 @@ void wom::drivetrain::Drivetrain::OnUpdate(second_t dt) {
     case DrivetrainState::kTank: {
       double rightSpeed = wom::utils::deadzone(_driver.GetRightY());
       double leftSpeed = wom::utils::deadzone(_driver.GetLeftY());
-      _config->left1.motorController->SetVoltage(leftSpeed * maxVolts);
-      _config->left2.motorController->SetVoltage(leftSpeed * maxVolts);
-      _config->left3.motorController->SetVoltage(leftSpeed * maxVolts);
-      _config->right1.motorController->SetVoltage(rightSpeed * maxVolts);
-      _config->right2.motorController->SetVoltage(rightSpeed * maxVolts);
-      _config->right3.motorController->SetVoltage(rightSpeed * maxVolts);
+      // _config->left1.motorController->SetVoltage(leftSpeed * maxVolts);
+      // _config->left2.motorController->SetVoltage(leftSpeed * maxVolts);
+      // _config->left3.motorController->SetVoltage(leftSpeed * maxVolts);
+      // _config->right1.motorController->SetVoltage(rightSpeed * maxVolts);
+      // _config->right2.motorController->SetVoltage(rightSpeed * maxVolts);
+      // _config->right3.motorController->SetVoltage(rightSpeed * maxVolts);
       break;
     }
     case DrivetrainState::kAuto:
       break;
   }
 }
+ */
\ No newline at end of file
diff --git a/wombat/src/main/cpp/drivetrain/SwerveDrive.cpp b/wombat/src/main/cpp/drivetrain/SwerveDrive.cpp
index 7af1ff39..553360c9 100644
--- a/wombat/src/main/cpp/drivetrain/SwerveDrive.cpp
+++ b/wombat/src/main/cpp/drivetrain/SwerveDrive.cpp
@@ -6,33 +6,61 @@
 
 #include <networktables/NetworkTableInstance.h>
 #include <units/math.h>
+#include <units/voltage.h>
+#include <units/angle.h>
+
+#include "utils/Util.h"
 
 #include <iostream>
+#include <algorithm>
+#include <cmath>
 
-#include "utils/Util.h"
+#include <wpi/sendable/SendableBuilder.h>
+#include <wpi/sendable/SendableRegistry.h>
+
+#include "frc/MathUtil.h"
+#include "wpimath/MathShared.h"
+
+using namespace wom;
 
 namespace wom {
 namespace drivetrain {
 
-void SwerveModuleConfig::WriteNT(std::shared_ptr<nt::NetworkTable> table) const {
+
+
+void SwerveModuleConfig::WriteNT(
+    std::shared_ptr<nt::NetworkTable> table) const {
   std::array<double, 2> pos{position.X().value(), position.Y().value()};
   table->GetEntry("position").SetDoubleArray(std::span(pos));
   table->GetEntry("wheelRadius").SetDouble(wheelRadius.value());
 }
 
 SwerveModule::SwerveModule(std::string path, SwerveModuleConfig config,
-                           SwerveModule::angle_pid_conf_t anglePID,
+                           //SwerveModule::angle_pid_conf_t anglePID,
                            SwerveModule::velocity_pid_conf_t velocityPID)
-    : _config(config),
-      _anglePIDController(path + "/pid/angle", anglePID),
-      _velocityPIDController(path + "/pid/velocity", velocityPID),
+    : //_anglePIDController(path + "/pid/angle", anglePID),
+      _anglePIDController{frc::PIDController(5, 0, 0, 0.005_s)},
+      _config(config),
+      _velocityPIDController(frc::PIDController(1.2, 0, 0, 0.005_s)),
       _table(nt::NetworkTableInstance::GetDefault().GetTable(path)) {
-  _anglePIDController.SetWrap(360_deg);
+  // _anglePIDController.SetTolerance(360);
+  _anglePIDController.EnableContinuousInput(0, (2 * 3.141592));
+  // _anglePIDController.EnableContinuousInput(-3.141592, (3.141592));
+  // _anglePIDController.EnableContinuousInput(0, (3.1415 * 2));
+  // _anglePIDController.EnableContinuousInput(0, (3.1415));
+  // _anglePIDController.SetWrap(0, 3.1415);
+
 }
 
 void SwerveModule::OnStart() {
   // _offset = offset;
+  // _config.canEncoder->SetPosition(units::turn_t{0});
   _anglePIDController.Reset();
+  // _anglePIDController.EnableContinuousInput(-3.141592, 3.141592);
+  // _anglePIDController.EnableContinuousInput(-3.141592, (3.141592));
+  _anglePIDController.EnableContinuousInput(0, (2 * 3.141592));
+
+
   _velocityPIDController.Reset();
 }
 
@@ -40,6 +68,8 @@ void SwerveModule::OnUpdate(units::second_t dt) {
   units::volt_t driveVoltage{0};
   units::volt_t turnVoltage{0};
 
+
+
   switch (_state) {
     case SwerveModuleState::kIdle:
       driveVoltage = 0_V;
@@ -47,50 +77,90 @@ void SwerveModule::OnUpdate(units::second_t dt) {
       break;
     case SwerveModuleState::kPID: {
       auto feedforward = _config.driveMotor.motor.Voltage(
-          0_Nm,
-          units::radians_per_second_t{(_velocityPIDController.GetSetpoint() / _config.wheelRadius).value()});
-      driveVoltage = _velocityPIDController.Calculate(GetSpeed(), dt, feedforward);
-      turnVoltage = _anglePIDController.Calculate(_config.turnMotor.encoder->GetEncoderPosition(), dt);
+          0_Nm, units::radians_per_second_t{
+                    _velocityPIDController.GetSetpoint() / _config.wheelRadius.value()});
+      // units::radian_t input = units::math::fmod(_config.turnMotor.encoder->GetEncoderPosition(), (2_rad * 3.1415));
+      double input = _config.turnMotor.encoder->GetEncoderPosition().value();
+      _table->GetEntry("/testing/GetEncoderPos").SetDouble(input);
+      // _velocityPIDController.SetSetpoint(3);
+      
+
+      driveVoltage = units::volt_t{_velocityPIDController.Calculate(GetSpeed().value())};
+      // if (_turnOffset == TurnOffsetValues::forward) {
+        
+      // } else if (_turnOffset == TurnOffsetValues::reverse) {
+      //   input = input - (3.1415/2);
+      //   driveVoltage = -driveVoltage;
+      // }
+      double demand = _anglePIDController.Calculate(input);
+      // if ((_anglePIDController.GetSetpoint() - input) > (3.14159/2)) {
+      //   demand *= -1;
+      // }
+      turnVoltage = units::volt_t{demand};
     } break;
+    case wom::drivetrain::SwerveModuleState::kZeroing: {
+    } break;
+    default:
+      std::cerr << "Case not handled" << std::endl;
   }
 
-  units::newton_meter_t torqueLimit = 50_kg / 4 * _config.wheelRadius * _currentAccelerationLimit;
-  units::volt_t voltageMax =
-      _config.driveMotor.motor.Voltage(torqueLimit, _config.driveMotor.encoder->GetEncoderAngularVelocity());
-  units::volt_t voltageMin =
-      _config.driveMotor.motor.Voltage(-torqueLimit, _config.driveMotor.encoder->GetEncoderAngularVelocity());
+  units::newton_meter_t torqueLimit =
+      50_kg / 4 * _config.wheelRadius * _currentAccelerationLimit;
+  // units::volt_t voltageMax = _config.driveMotor.motor.Voltage(
+  //     torqueLimit, _config.driveMotor.encoder->GetEncoderAngularVelocity());
+  // units::volt_t voltageMin = _config.driveMotor.motor.Voltage(
+  //     -torqueLimit, _config.driveMotor.encoder->GetEncoderAngularVelocity());
 
-  driveVoltage = units::math::max(units::math::min(driveVoltage, voltageMax), voltageMin);
+  // driveVoltage =
+  //     units::math::max(units::math::min(driveVoltage, voltageMax), voltageMin);
 
-  // driveVoltage = units::math::min(driveVoltage, 10_V);
-  turnVoltage = units::math::min(turnVoltage, 7_V);
+  driveVoltage = units::math::min(driveVoltage, 7_V);
+  turnVoltage = units::math::min(turnVoltage, 4_V);
 
-  driveVoltage = units::math::min(units::math::max(driveVoltage, -_driveModuleVoltageLimit),
-                                  _driveModuleVoltageLimit);  // was originally 10_V
-  std::cout << "drive-voltage: " << driveVoltage.value() << std::endl;
+  // driveVoltage = units::math::min(
+  //     units::math::max(driveVoltage, -_driveModuleVoltageLimit),
+  //     _driveModuleVoltageLimit);  // was originally 10_V
   units::volt_t turnVoltageMax = 7_V - (driveVoltage * (7_V / 10_V));
-  turnVoltage = units::math::min(units::math::max(turnVoltage, -turnVoltageMax), turnVoltageMax);
+  turnVoltage = units::math::min(units::math::max(turnVoltage, -turnVoltageMax),
+                                 turnVoltageMax);
   // turnVoltage = units::math::min(units::math::max(turnVoltage, -7_V), 7_V);
-  // std::cout << "turn-voltage-max: " << turnVoltageMax.value() << std::endl;
+
+  _table->GetEntry("TurnVoltage").SetDouble(turnVoltage.value());
+  _table->GetEntry("TurnSetpoint").SetDouble(_anglePIDController.GetSetpoint());
+  _table->GetEntry("Demand").SetDouble(_config.turnMotor.encoder->GetEncoderPosition().value());
+  _table->GetEntry("Error").SetDouble(_anglePIDController.GetPositionError());
 
   _config.driveMotor.motorController->SetVoltage(driveVoltage);
   _config.turnMotor.motorController->SetVoltage(turnVoltage);
 
   _table->GetEntry("speed").SetDouble(GetSpeed().value());
-  _table->GetEntry("angle").SetDouble(
-      _config.turnMotor.encoder->GetEncoderPosition().convert<units::degree>().value());
+  _table->GetEntry("angle").SetDouble(_config.turnMotor.encoder->GetEncoderPosition().value());
   _config.WriteNT(_table->GetSubTable("config"));
 }
 
+void SwerveModule::SetTurnOffsetForward() {
+  _turnOffset = TurnOffsetValues::forward;
+}
+
+void SwerveModule::SetTurnOffsetReverse() {
+  _turnOffset = TurnOffsetValues::reverse;
+}
+
+void SwerveModule::TurnOffset() {
+  _turnOffset = TurnOffsetValues::none;
+}
+
 // double SwerveModule::GetCancoderPosition() {
 //   return (_config.turnMotor.encoder->GetEncoderPosition().value());
 // }
 
-void SwerveModule::SetAccelerationLimit(units::meters_per_second_squared_t limit) {
+void SwerveModule::SetAccelerationLimit(
+    units::meters_per_second_squared_t limit) {
   _currentAccelerationLimit = limit;
 }
 
-void SwerveDrive::SetAccelerationLimit(units::meters_per_second_squared_t limit) {
+void SwerveDrive::SetAccelerationLimit(
+    units::meters_per_second_squared_t limit) {
   for (int motorNumber = 0; motorNumber < 4; motorNumber++) {
     _modules[motorNumber].SetAccelerationLimit(limit);
   }
@@ -109,46 +179,110 @@ void SwerveModule::SetZero(units::second_t dt) {
   _state = SwerveModuleState::kPID;
 }
 
-void SwerveModule::SetPID(units::radian_t angle, units::meters_per_second_t speed, units::second_t dt) {
+void SwerveModule::SetPID(units::radian_t angle,
+                          units::meters_per_second_t speed,
+                          units::second_t dt) {
   _state = SwerveModuleState::kPID;
-  // @liam start added
-  double diff = std::fmod((_anglePIDController.GetSetpoint() - angle).convert<units::degree>().value(), 360);
-  if (std::abs(diff) >= 90) {
-    speed *= -1;
-    angle += 180_deg;
-  }
-  // @liam end added
 
-  _anglePIDController.SetSetpoint(angle);
-  _velocityPIDController.SetSetpoint(speed);
-}
+
+  // double diff = std::abs(_config.turnMotor.encoder->GetEncoderPosition().value() - angle.value());
+  // _table->GetEntry("diff").SetDouble(diff);
+  // if (std::abs(diff) > (3.14159/2)) {
+  //   speed *= -1;
+  //   angle = 3.14159_rad - units::radian_t{diff};
+  //   _anglePIDController.SetSetpoint(angle.value());
+  //   _velocityPIDController.SetSetpoint(speed.value());
+  // } else {
+  //   _anglePIDController.SetSetpoint(angle.value());
+  //   _velocityPIDController.SetSetpoint(speed.value());
+  // }
+  
+  // if (diff > (3.14159 / 2)) {
+  //   speed *= -1;
+  //   _anglePIDController.SetSetpoint((3.14159 - (angle.value() - _config.turnMotor.encoder->GetEncoderPosition().value())));
+  //   _velocityPIDController.SetSetpoint(speed.value());
+  // } else {
+    
+
+
+  // double setValue = 3.141592 - (angle.value() - _config.turnMotor.encoder->GetEncoderPosition().value());
+
+  // if (diff > (3.141592/2)) {
+  //   if (setValue < 0 ) {
+  //     _anglePIDController.SetSetpoint(0);
+  //     _velocityPIDController.SetSetpoint(-speed.value());
+  //   } else if ( setValue < (2 * 3)) {
+  //     _anglePIDController.SetSetpoint(2 * 3.141592  );
+  //     _velocityPIDController.SetSetpoint(-speed.value());
+  //   } else {
+  //     _anglePIDController.SetSetpoint(setValue);
+  //     _velocityPIDController.SetSetpoint(-speed.value());
+  //   }
+  // } else {
+    _anglePIDController.SetSetpoint(angle.value());
+    _velocityPIDController.SetSetpoint(speed.value());
+  // }
+
+  
+
+  // double currentAngle = _config.turnMotor.encoder->GetEncoderPosition().value();
+  // double setpointAngle = closestAngle(currentAngle, _anglePIDController.GetSetpoint());
+  // double setpointAngleFlipped = closestAngle(currentAngle, _anglePIDController.GetSetpoint() + 3.1415);
+  // _table->GetEntry("/Setpoint angle: ").SetDouble(setpointAngle);
+  // _table->GetEntry("/Setpoint angle flipped: ").SetDouble(setpointAngleFlipped);
+  // if (std::abs(setpointAngle) <= std::abs(setpointAngleFlipped)) {
+  //   // _anglePIDController.SetGain(1.0);
+  //   _anglePIDController.SetSetpoint(currentAngle + setpointAngle);
+  //   _velocityPIDController.SetSetpoint(speed.value());
+  // } else {
+  //   // _anglePIDController.SetGain(-1.0);
+  //   _velocityPIDController.SetSetpoint(-speed.value());
+  //   _anglePIDController.SetSetpoint(currentAngle + setpointAngleFlipped);
+  // }
+}
+
+// double SwerveModule::closestAngle(double a, double b) {
+//   double dir = std::fmod(b, (3.1415 * 2)) - std::fmod(a, (2 * 3.1415));
+//   if (std::abs(dir) > 3.1415) {
+//     dir = -(((dir < 0) ? -1 : (dir > 0)) * 3.1415) + dir;
+//   }
+//   return dir;
+// }
 
 void SwerveModule::ModuleVectorHandler(frc::ChassisSpeeds speeds) {
   units::meters_per_second_t xVelocityComponent =
-      1_mps * (speeds.vx.value() + speeds.omega.value() * _config.position.X().value());
+      1_mps *
+      (speeds.vx.value() + speeds.omega.value() * _config.position.X().value());
   units::meters_per_second_t yVelocityComponent =
-      1_mps * (speeds.vy.value() + speeds.omega.value() * _config.position.Y().value());
+      1_mps *
+      (speeds.vy.value() + speeds.omega.value() * _config.position.Y().value());
 
   units::meters_per_second_t velocity =
-      1_mps * std::sqrt(std::pow(xVelocityComponent.value(), 2) + std::pow(yVelocityComponent.value(), 2));
-  units::degree_t angle = 1_rad * std::atan2(yVelocityComponent.value(), xVelocityComponent.value());
+      1_mps * std::sqrt(std::pow(xVelocityComponent.value(), 2) +
+                        std::pow(yVelocityComponent.value(), 2));
+  units::degree_t angle = 1_rad * std::atan2(yVelocityComponent.value(),
+                                             xVelocityComponent.value());
 
-  _anglePIDController.SetSetpoint(angle);
-  _velocityPIDController.SetSetpoint(velocity);
+  _anglePIDController.SetSetpoint(angle.value());
+  _velocityPIDController.SetSetpoint(velocity.value());
 }
 
 units::meters_per_second_t SwerveModule::GetSpeed() const {
-  return units::meters_per_second_t{_config.driveMotor.encoder->GetEncoderAngularVelocity().value() *
-                                    _config.wheelRadius.value()};
+  units::meters_per_second_t returnVal{
+    _config.driveMotor.encoder->GetVelocityValue() *
+    _config.wheelRadius.value()};
+    return returnVal;
 }
 
 units::meter_t SwerveModule::GetDistance() const {
-  return units::meter_t{_config.driveMotor.encoder->GetEncoderPosition().value() *
-                        _config.wheelRadius.value()};
+  return units::meter_t{
+      _config.driveMotor.encoder->GetEncoderPosition().value() *
+      _config.wheelRadius.value()};
 }
 
 frc::SwerveModulePosition SwerveModule::GetPosition() const {
-  return frc::SwerveModulePosition{GetDistance(), _config.turnMotor.encoder->GetEncoderPosition()};
+  return frc::SwerveModulePosition{
+      GetDistance(), _config.turnMotor.encoder->GetEncoderPosition()};
 }
 
 const SwerveModuleConfig& SwerveModule::GetConfig() const {
@@ -161,40 +295,49 @@ void SwerveDriveConfig::WriteNT(std::shared_ptr<nt::NetworkTable> table) {
 
 SwerveDrive::SwerveDrive(SwerveDriveConfig config, frc::Pose2d initialPose)
     : _config(config),
-      _kinematics(_config.modules[0].position, _config.modules[1].position, _config.modules[2].position,
-                  _config.modules[3].position),
+      // _kinematics(_config.modules[1].position, _config.modules[0].position,
+      //             _config.modules[2].position, _config.modules[3].position),
+
+      _kinematics(_config.modules[3].position /*1*/, _config.modules[0].position /*0*/,
+                  _config.modules[1].position /*2*/, _config.modules[2].position /*3*/),
       _poseEstimator(
           _kinematics, frc::Rotation2d(0_deg),
-          wpi::array<frc::SwerveModulePosition, 4>{frc::SwerveModulePosition{0_m, frc::Rotation2d{0_deg}},
-                                                   frc::SwerveModulePosition{0_m, frc::Rotation2d{0_deg}},
-                                                   frc::SwerveModulePosition{0_m, frc::Rotation2d{0_deg}},
-                                                   frc::SwerveModulePosition{0_m, frc::Rotation2d{0_deg}}},
+          wpi::array<frc::SwerveModulePosition, 4>{
+              frc::SwerveModulePosition{0_m, frc::Rotation2d{0_deg}},
+              frc::SwerveModulePosition{0_m, frc::Rotation2d{0_deg}},
+              frc::SwerveModulePosition{0_m, frc::Rotation2d{0_deg}},
+              frc::SwerveModulePosition{0_m, frc::Rotation2d{0_deg}}},
           initialPose, _config.stateStdDevs, _config.visionMeasurementStdDevs),
-      _anglePIDController(config.path + "/pid/heading", _config.poseAnglePID),
+      _anglePIDController{frc::PIDController(1, 0, 0)},
       _xPIDController(config.path + "/pid/x", _config.posePositionPID),
       _yPIDController(config.path + "/pid/y", _config.posePositionPID),
       _table(nt::NetworkTableInstance::GetDefault().GetTable(_config.path)) {
-  _anglePIDController.SetWrap(360_deg);
+  _anglePIDController.SetTolerance(360);
 
   int i = 1;
   for (auto cfg : _config.modules) {
-    _modules.emplace_back(config.path + "/modules/" + std::to_string(i), cfg, config.anglePID,
-                          config.velocityPID);
+    _modules.emplace_back(config.path + "/modules/" + std::to_string(i), cfg,
+                          /*config.anglePID,*/ config.velocityPID);
     i++;
   }
 
   ResetPose(initialPose);
 }
 
-frc::ChassisSpeeds FieldRelativeSpeeds::ToChassisSpeeds(const units::radian_t robotHeading) {
-  return frc::ChassisSpeeds::FromFieldRelativeSpeeds(vx, vy, omega, frc::Rotation2d{robotHeading});
+frc::ChassisSpeeds FieldRelativeSpeeds::ToChassisSpeeds(
+    const units::radian_t robotHeading) {
+  return frc::ChassisSpeeds::FromFieldRelativeSpeeds(
+      vx, vy, omega, frc::Rotation2d{robotHeading});
 }
 
 void SwerveDrive::OnUpdate(units::second_t dt) {
+  _table->GetEntry("/gryo/z").SetDouble(_config.gyro->GetRotation3d().Z().value());
+  _table->GetEntry("/gryo/y").SetDouble(_config.gyro->GetRotation3d().Y().value());
+  _table->GetEntry("/gryo/x").SetDouble(_config.gyro->GetRotation3d().X().value());
   switch (_state) {
     case SwerveDriveState::kZeroing:
       for (auto mod = _modules.begin(); mod < _modules.end(); mod++) {
-        mod->SetZero(dt);
+        // mod->SetZero(dt);
       }
       break;
     case SwerveDriveState::kIdle:
@@ -205,23 +348,53 @@ void SwerveDrive::OnUpdate(units::second_t dt) {
     case SwerveDriveState::kPose: {
       _target_fr_speeds.vx = _xPIDController.Calculate(GetPose().X(), dt);
       _target_fr_speeds.vy = _yPIDController.Calculate(GetPose().Y(), dt);
-      _target_fr_speeds.omega = _anglePIDController.Calculate(GetPose().Rotation().Radians(), dt);
+      _target_fr_speeds.omega = units::radians_per_second_t{_anglePIDController.Calculate(GetPose().Rotation().Radians().value())};
     }
       [[fallthrough]];
     case SwerveDriveState::kFieldRelativeVelocity:
-      _target_speed = _target_fr_speeds.ToChassisSpeeds(GetPose().Rotation().Radians());
+      _target_speed =
+          _target_fr_speeds.ToChassisSpeeds(GetPose().Rotation().Radians());
       if (isRotateToMatchJoystick) {
-        _target_speed.omega = _anglePIDController.Calculate(GetPose().Rotation().Radians(), dt);
+        _target_speed.omega = units::radians_per_second_t{_anglePIDController.Calculate(GetPose().Rotation().Radians().value())};
       }
-      // std::cout << "vx = " << _target_speed.vx.value() << " vy = " <<
       // _target_fr_speeds.vy.value() << std::endl;
       [[fallthrough]];
     case SwerveDriveState::kVelocity: {
+      _table->GetEntry("Swerve module VX").SetDouble(_target_speed.vx.value());
+      _table->GetEntry("Swerve module VY").SetDouble(_target_speed.vy.value());
+      _table->GetEntry("Swerve module Omega").SetDouble(_target_speed.omega.value());
+      if (_target_speed.omega.value() > 0) {
+        // _modules[0].SetTurnOffsetForward();
+        _modules[1].SetTurnOffsetForward();
+      } else if (_target_speed.omega.value() < 0) {
+        // _modules[0].SetTurnOffsetReverse();
+        _modules[1].SetTurnOffsetReverse();
+      } else {
+        // _modules[0].TurnOffset();
+        _modules[1].TurnOffset();
+      }
+
+      if (_target_speed.vx > 0_mps || _target_speed.vy > 0_mps) {
+        _angle = _target_speed.omega * 1_s;
+      }
+
       auto target_states = _kinematics.ToSwerveModuleStates(_target_speed);
+      frc::ChassisSpeeds new_target_speed {_target_speed.vx, _target_speed.vy, -_target_speed.omega};
+      auto new_target_states = _kinematics.ToSwerveModuleStates(new_target_speed);
       for (size_t i = 0; i < _modules.size(); i++) {
-        _modules[i].SetPID(target_states[i].angle.Radians(), target_states[i].speed, dt);
-        // std::cout << "module " << i << ": " <<
-        // target_states[i].angle.Radians().value() << std::endl;
+        if ( i == 3) {
+          _modules[i].SetPID(new_target_states[i].angle.Radians(),
+                            new_target_states[i].speed, dt);
+        } else {
+          _modules[i].SetPID(target_states[i].angle.Radians(),
+                            target_states[i].speed, dt);
+          // target_states[i].angle.Radians().value() << std::endl;
+        }
+
+        // if (i == 2) {
+        //   _modules[i].SetPID(new_target_states[i].angle.Radians(),
+        //                     new_target_states[i].speed, dt);
+        // }
       }
     } break;
     case SwerveDriveState::kIndividualTuning:
@@ -242,12 +415,13 @@ void SwerveDrive::OnUpdate(units::second_t dt) {
     case SwerveDriveState::kFRVelocityRotationLock:
       _target_speed.vx = _xPIDController.Calculate(GetPose().X(), dt);
       _target_speed.vy = _yPIDController.Calculate(GetPose().Y(), dt);
-      _target_speed.omega = _anglePIDController.Calculate(GetPose().Rotation().Radians(), dt);
-      _target_speed = _requestedSpeeds.ToChassisSpeeds(GetPose().Rotation().Radians());
+      _target_speed.omega = units::radians_per_second_t{_anglePIDController.Calculate(GetPose().Rotation().Radians().value())};
+      _target_speed =
+          _requestedSpeeds.ToChassisSpeeds(GetPose().Rotation().Radians());
       auto target_states = _kinematics.ToSwerveModuleStates(_target_speed);
       for (size_t i = 0; i < _modules.size(); i++) {
-        std::cout << "Speeds :" << target_states[i].speed.value() << std::endl;
-        _modules[i].SetPID(target_states[i].angle.Radians(), target_states[i].speed, dt);
+        _modules[i].SetPID(target_states[i].angle.Radians(),
+                           target_states[i].speed, dt);
       }
       break;
   }
@@ -258,10 +432,12 @@ void SwerveDrive::OnUpdate(units::second_t dt) {
 
   _poseEstimator.Update(
       _config.gyro->GetRotation2d(),
-      wpi::array<frc::SwerveModulePosition, 4>{_modules[0].GetPosition(), _modules[1].GetPosition(),
-                                               _modules[2].GetPosition(), _modules[3].GetPosition()});
+      wpi::array<frc::SwerveModulePosition, 4>{
+          _modules[0].GetPosition(), _modules[1].GetPosition(),
+          _modules[2].GetPosition(), _modules[3].GetPosition()});
 
-  utils::WritePose2NT(_table->GetSubTable("estimatedPose"), _poseEstimator.GetEstimatedPosition());
+  utils::WritePose2NT(_table->GetSubTable("estimatedPose"),
+                      _poseEstimator.GetEstimatedPosition());
   _config.WriteNT(_table->GetSubTable("config"));
 }
 
@@ -284,9 +460,7 @@ void SwerveDrive::SetVoltageLimit(units::volt_t driveVoltageLimit) {
 // }
 
 void SwerveDrive::OnStart() {
-  _xPIDController.Reset();
-  _yPIDController.Reset();
-  _anglePIDController.Reset();
+  OnResetMode();
 
   _modules[0].OnStart();  // front left
   _modules[1].OnStart();  // front right
@@ -298,16 +472,16 @@ void SwerveDrive::OnResetMode() {
   _xPIDController.Reset();
   _yPIDController.Reset();
   _anglePIDController.Reset();
-  std::cout << "reset" << std::endl;
 }
 
-void SwerveDrive::RotateMatchJoystick(units::radian_t joystickAngle, FieldRelativeSpeeds speeds) {
+void SwerveDrive::RotateMatchJoystick(units::radian_t joystickAngle,
+                                      FieldRelativeSpeeds speeds) {
   // _state = SwerveDriveState::kFRVelocityRotationLock;
   // _anglePIDController.SetSetpoint(joystickAngle);
   // _target_fr_speeds = speeds;
   _state = SwerveDriveState::kFieldRelativeVelocity;
   isRotateToMatchJoystick = true;
-  _anglePIDController.SetSetpoint(joystickAngle);
+  _anglePIDController.SetSetpoint(joystickAngle.value());
   _target_fr_speeds = speeds;
 }
 
@@ -327,14 +501,16 @@ bool SwerveDrive::GetIsFieldRelative() {
   return _isFieldRelative;
 }
 
-void SwerveDrive::SetIndividualTuning(int mod, units::radian_t angle, units::meters_per_second_t speed) {
+void SwerveDrive::SetIndividualTuning(int mod, units::radian_t angle,
+                                      units::meters_per_second_t speed) {
   _mod = mod;
   _angle = angle;
   _speed = speed;
   _state = SwerveDriveState::kIndividualTuning;
 }
 
-void SwerveDrive::SetTuning(units::radian_t angle, units::meters_per_second_t speed) {
+void SwerveDrive::SetTuning(units::radian_t angle,
+                            units::meters_per_second_t speed) {
   _angle = angle;
   _speed = speed;
   _state = SwerveDriveState::kTuning;
@@ -348,20 +524,22 @@ void SwerveDrive::SetFieldRelativeVelocity(FieldRelativeSpeeds speeds) {
 
 void SwerveDrive::SetPose(frc::Pose2d pose) {
   _state = SwerveDriveState::kPose;
-  _anglePIDController.SetSetpoint(pose.Rotation().Radians());
+  _anglePIDController.SetSetpoint(pose.Rotation().Radians().value());
   _xPIDController.SetSetpoint(pose.X());
   _yPIDController.SetSetpoint(pose.Y());
 }
 
 bool SwerveDrive::IsAtSetPose() {
-  return _anglePIDController.IsStable() && _xPIDController.IsStable() && _yPIDController.IsStable(0.05_m);
+  return /*_anglePIDController.IsStable()*/ true && _xPIDController.IsStable() &&
+         _yPIDController.IsStable(0.05_m);
 }
 
 void SwerveDrive::ResetPose(frc::Pose2d pose) {
   _poseEstimator.ResetPosition(
       _config.gyro->GetRotation2d(),
-      wpi::array<frc::SwerveModulePosition, 4>{_modules[0].GetPosition(), _modules[1].GetPosition(),
-                                               _modules[2].GetPosition(), _modules[3].GetPosition()},
+      wpi::array<frc::SwerveModulePosition, 4>{
+          _modules[0].GetPosition(), _modules[1].GetPosition(),
+          _modules[2].GetPosition(), _modules[3].GetPosition()},
       pose);
 }
 
@@ -369,9 +547,9 @@ frc::Pose2d SwerveDrive::GetPose() {
   return _poseEstimator.GetEstimatedPosition();
 }
 
-void SwerveDrive::AddVisionMeasurement(frc::Pose2d pose, units::second_t timestamp) {
+void SwerveDrive::AddVisionMeasurement(frc::Pose2d pose,
+                                       units::second_t timestamp) {
   _poseEstimator.AddVisionMeasurement(pose, timestamp);
 }
 }  // namespace drivetrain
-
-}  // namespace wom
+}  // namespace wom
\ No newline at end of file
diff --git a/wombat/src/main/cpp/drivetrain/behaviours/SwerveBehaviours.cpp b/wombat/src/main/cpp/drivetrain/behaviours/SwerveBehaviours.cpp
index a8cbf3c5..9479dfe0 100644
--- a/wombat/src/main/cpp/drivetrain/behaviours/SwerveBehaviours.cpp
+++ b/wombat/src/main/cpp/drivetrain/behaviours/SwerveBehaviours.cpp
@@ -28,62 +28,58 @@ ManualDrivebase::ManualDrivebase(wom::drivetrain::SwerveDrive* swerveDrivebase,
 
 void ManualDrivebase::OnStart() {
   _swerveDrivebase->OnStart();
-  _swerveDrivebase->SetAccelerationLimit(6_mps_sq);
-  std::cout << "Manual Drivebase Start" << std::endl;
+  _swerveDrivebase->SetAccelerationLimit(10_mps_sq);
 }
 
 void ManualDrivebase::OnTick(units::second_t deltaTime) {
-  if (_driverController->GetXButtonPressed()) {
-    ResetMode();
-    if (isRotateMatch) {
-      isRotateMatch = false;
-    } else {
-      isRotateMatch = true;
-    }
-  }
+  // if (_driverController->GetXButtonPressed()) {
+  //   ResetMode();
+  //   isRotateMatch = !isRotateMatch;
+  // }
 
   if (_driverController->GetYButton()) {
-    std::cout << "RESETING POSE" << std::endl;
     _swerveDrivebase->ResetPose(frc::Pose2d());
   }
 
-  /*   HOLD SOLUTION   */
-  if (_driverController->GetLeftBumperPressed()) {
-    maxMovementMagnitude = lowSensitivityDriveSpeed;
-    maxRotationMagnitude = lowSensitivityRotateSpeed;
-  } else if (_driverController->GetLeftBumperReleased() && !_driverController->GetRightBumper()) {
-    maxMovementMagnitude = defaultDriveSpeed;
-    maxRotationMagnitude = defaultRotateSpeed;
-    _swerveDrivebase->SetAccelerationLimit(6_mps_sq);
-    _swerveDrivebase->SetVoltageLimit(10_V);
-  }
-  if (_driverController->GetRightBumperPressed()) {
-    maxMovementMagnitude = highSensitivityDriveSpeed;
-    maxRotationMagnitude = highSensitivityRotateSpeed;
-    _swerveDrivebase->SetAccelerationLimit(12_mps_sq);
-    _swerveDrivebase->SetVoltageLimit(14_V);
-
-  } else if (_driverController->GetRightBumperReleased() && !_driverController->GetLeftBumper()) {
-    maxMovementMagnitude = defaultDriveSpeed;
-    maxRotationMagnitude = defaultRotateSpeed;
-    _swerveDrivebase->SetAccelerationLimit(6_mps_sq);
-    _swerveDrivebase->SetVoltageLimit(10_V);
-  }
-
-  if (_driverController->GetAButtonReleased()) {
-    isZero = !isZero;
-  }
-
-  if (isZero) {
-    _swerveDrivebase->SetZeroing();
-  } else {
-    double xVelocity = wom::utils::spow2(
-        -wom::utils::deadzone(_driverController->GetLeftY(),
-                              driverDeadzone));  // GetLeftY due to x being where y should be on field
-    double yVelocity =
-        wom::utils::spow2(-wom::utils::deadzone(_driverController->GetLeftX(), driverDeadzone));
-
-    double r_x = wom::utils::spow2(-wom::utils::deadzone(_driverController->GetRightX(), turningDeadzone));
+  // if (_driverController->GetLeftBumperPressed()) {
+  //   maxMovementMagnitude = lowSensitivityDriveSpeed;
+  //   maxRotationMagnitude = lowSensitivityRotateSpeed;
+  // } else if (_driverController->GetLeftBumperReleased() &&
+  //            !_driverController->GetRightBumper()) {
+  //   maxMovementMagnitude = defaultDriveSpeed;
+  //   maxRotationMagnitude = defaultRotateSpeed;
+  //   _swerveDrivebase->SetAccelerationLimit(6_mps_sq);
+  //   _swerveDrivebase->SetVoltageLimit(10_V);
+  // }
+  // if (_driverController->GetRightBumperPressed()) {
+  //   maxMovementMagnitude = highSensitivityDriveSpeed;
+  //   maxRotationMagnitude = highSensitivityRotateSpeed;
+  //   _swerveDrivebase->SetAccelerationLimit(12_mps_sq);
+  //   _swerveDrivebase->SetVoltageLimit(14_V);
+
+  // } else if (_driverController->GetRightBumperReleased() &&
+  //            !_driverController->GetLeftBumper()) {
+  //   maxMovementMagnitude = defaultDriveSpeed;
+  //   maxRotationMagnitude = defaultRotateSpeed;
+  //   _swerveDrivebase->SetAccelerationLimit(6_mps_sq);
+  //   _swerveDrivebase->SetVoltageLimit(10_V);
+  // }
+
+  // if (_driverController->GetAButtonReleased()) {
+  //   isZero = !isZero;
+  // }
+
+  // if (isZero) {
+  //   _swerveDrivebase->SetZeroing();
+  // } else {
+    double xVelocity = wom::utils::spow2(-wom::utils::deadzone(
+        _driverController->GetLeftY(),
+        driverDeadzone));  // GetLeftY due to x being where y should be on field
+    double yVelocity = wom::utils::spow2(
+        -wom::utils::deadzone(_driverController->GetLeftX(), driverDeadzone));
+
+    double r_x = wom::utils::spow2(
+        -wom::utils::deadzone(_driverController->GetRightX(), turningDeadzone));
 
     double turnX = _driverController->GetRightX();
     double turnY = _driverController->GetRightY();
@@ -93,20 +89,31 @@ void ManualDrivebase::OnTick(units::second_t deltaTime) {
       turnY = 0;
     }
 
-    if (isRotateMatch) {
-      units::degree_t currentAngle = _swerveDrivebase->GetPose().Rotation().Degrees();
-      CalculateRequestedAngle(turnX, turnY, currentAngle);
-      _swerveDriveTable->GetEntry("RotateMatch").SetDouble(_requestedAngle.value());
-      _swerveDrivebase->RotateMatchJoystick(
-          _requestedAngle,
-          wom::drivetrain::FieldRelativeSpeeds{// also field relative
-                                               xVelocity * maxMovementMagnitude,
-                                               yVelocity * maxMovementMagnitude, r_x * maxRotationMagnitude});
-    } else {
-      _swerveDrivebase->SetFieldRelativeVelocity(wom::drivetrain::FieldRelativeSpeeds{
-          xVelocity * maxMovementMagnitude, yVelocity * maxMovementMagnitude, r_x * maxRotationMagnitude});
-    }
-  }
+    // if (isRotateMatch) {
+    //   units::degree_t currentAngle =
+    //       _swerveDrivebase->GetPose().Rotation().Degrees();
+    //   CalculateRequestedAngle(turnX, turnY, currentAngle);
+    //   _swerveDriveTable->GetEntry("RotateMatch")
+    //       .SetDouble(_requestedAngle.value());
+    //   _swerveDrivebase->RotateMatchJoystick(
+    //       _requestedAngle,
+    //       wom::drivetrain::FieldRelativeSpeeds{// also field relative
+    //                                            xVelocity * maxMovementMagnitude,
+    //                                            yVelocity * maxMovementMagnitude,
+    //                                            r_x * maxRotationMagnitude});
+    // } else {
+      _swerveDrivebase->SetFieldRelativeVelocity(
+          wom::drivetrain::FieldRelativeSpeeds{xVelocity * -maxMovementMagnitude,
+                                               yVelocity * -maxMovementMagnitude,
+                                               r_x * maxRotationMagnitude});
+
+    //  _swerveDrivebase->SetVelocity(
+    //       frc::ChassisSpeeds{xVelocity * maxMovementMagnitude,
+    //                          yVelocity * maxMovementMagnitude,
+    //                          r_x * maxRotationMagnitude});                                          
+  //   }
+  // }
+  // _swerveDrivebase->SetIndividualTuning(2, 0_deg, 0_mps);
 }
 
 void ManualDrivebase::ResetMode() {
@@ -114,16 +121,18 @@ void ManualDrivebase::ResetMode() {
   resetMode = false;
 }
 
-void ManualDrivebase::CalculateRequestedAngle(double joystickX, double joystickY,
+void ManualDrivebase::CalculateRequestedAngle(double joystickX,
+                                              double joystickY,
                                               units::degree_t defaultAngle) {
   _requestedAngle = (1_rad * std::atan2(joystickY, -joystickX)) + 90_deg;
-  if (joystickX == 0 & joystickY == 0) {
+  if (wom::utils::deadzone(joystickX) == 0 && wom::utils::deadzone(joystickY) == 0) {
     _requestedAngle = _swerveDrivebase->GetPose().Rotation().Radians();
   }
 }
 
 // Code for x-ing the wheels on the drivebase
-XDrivebase::XDrivebase(wom::drivetrain::SwerveDrive* swerveDrivebase) : _swerveDrivebase(swerveDrivebase) {
+XDrivebase::XDrivebase(wom::drivetrain::SwerveDrive* swerveDrivebase)
+    : _swerveDrivebase(swerveDrivebase) {
   Controls(swerveDrivebase);
 }
 void XDrivebase::OnTick(units::second_t deltaTime) {
@@ -165,17 +174,20 @@ void XDrivebase::OnTick(units::second_t deltaTime) {
 //   m_timer.Start();
 // }
 
-wom::drivetrain::behaviours::TempSimSwerveDrive::TempSimSwerveDrive(frc::Timer* timer, frc::Field2d* field)
+wom::drivetrain::behaviours::TempSimSwerveDrive::TempSimSwerveDrive(
+    frc::Timer* timer, frc::Field2d* field)
     : m_timer(timer), m_field(field) {}
 
 void wom::drivetrain::behaviours::TempSimSwerveDrive::OnUpdate() {
   m_field->SetRobotPose(m_driveSim.GetPose());
 
   // get the current trajectory state
-  frc::Trajectory::State desired_state = current_trajectory.Sample(m_timer->Get());
+  frc::Trajectory::State desired_state =
+      current_trajectory.Sample(m_timer->Get());
 
   // get the current wheel speeds
-  wom::utils::WriteTrajectoryState(current_trajectory_state_table, desired_state);
+  wom::utils::WriteTrajectoryState(current_trajectory_state_table,
+                                   desired_state);
 
   // move drivebase position to the desired state
   m_driveSim.SetPose(wom::utils::TrajectoryStateToPose2d(desired_state));
@@ -193,16 +205,19 @@ frc::Pose2d wom::drivetrain::behaviours::TempSimSwerveDrive::GetPose2d() {
   return m_driveSim.GetPose();
 }
 
-void wom::drivetrain::behaviours::TempSimSwerveDrive::SetPath(std::string path) {
+void wom::drivetrain::behaviours::TempSimSwerveDrive::SetPath(
+    std::string path) {
   nt::NetworkTableInstance inst = nt::NetworkTableInstance::GetDefault();
   std::shared_ptr<nt::NetworkTable> table = inst.GetTable("FMSInfo");
 
   // create a netowrk table for the trajectory
   std::shared_ptr<nt::NetworkTable> trajectory_table =
       nt::NetworkTableInstance::GetDefault().GetTable("trajectory_path");
-  current_trajectory_table = nt::NetworkTableInstance::GetDefault().GetTable("current_trajectory");
+  current_trajectory_table =
+      nt::NetworkTableInstance::GetDefault().GetTable("current_trajectory");
   current_trajectory_state_table =
-      nt::NetworkTableInstance::GetDefault().GetTable("current_trajectory_state");
+      nt::NetworkTableInstance::GetDefault().GetTable(
+          "current_trajectory_state");
 
   current_trajectory = m_pathplanner.getTrajectory(path);
   m_driveSim.SetPose(current_trajectory.Sample(0_s).pose);
@@ -210,10 +225,12 @@ void wom::drivetrain::behaviours::TempSimSwerveDrive::SetPath(std::string path)
   m_timer->Start();
 }
 
-wom::drivetrain::behaviours::AutoSwerveDrive::AutoSwerveDrive(wom::drivetrain::SwerveDrive* swerve,
-                                                              frc::Timer* timer, frc::Field2d* field)
+wom::drivetrain::behaviours::AutoSwerveDrive::AutoSwerveDrive(
+    wom::drivetrain::SwerveDrive* swerve, frc::Timer* timer,
+    frc::Field2d* field)
     : _swerve(swerve), m_timer(timer), m_field(field) {
-  _simSwerveDrive = new wom::drivetrain::behaviours::TempSimSwerveDrive(timer, field);
+  _simSwerveDrive =
+      new wom::drivetrain::behaviours::TempSimSwerveDrive(timer, field);
 }
 
 void wom::drivetrain::behaviours::AutoSwerveDrive::OnUpdate() {
diff --git a/wombat/src/main/cpp/subsystems/Arm.cpp b/wombat/src/main/cpp/subsystems/Arm.cpp
index 942003c4..3406be88 100644
--- a/wombat/src/main/cpp/subsystems/Arm.cpp
+++ b/wombat/src/main/cpp/subsystems/Arm.cpp
@@ -1,4 +1,4 @@
-// Copyright (c) 2023-2024 CurtinFRC
+/* // Copyright (c) 2023-2024 CurtinFRC
 // Open Source Software, you can modify it according to the terms
 // of the MIT License at the root of this project
 
@@ -10,14 +10,19 @@ using namespace frc;
 using namespace wom;
 
 // creates network table instatnce on shuffleboard
-void wom::subsystems::ArmConfig::WriteNT(std::shared_ptr<nt::NetworkTable> table) {
+void wom::subsystems::ArmConfig::WriteNT(
+    std::shared_ptr<nt::NetworkTable> table) {
   table->GetEntry("armMass").SetDouble(armMass.value());
   table->GetEntry("loadMass").SetDouble(loadMass.value());
   table->GetEntry("armLength").SetDouble(armLength.value());
-  table->GetEntry("minAngle").SetDouble(minAngle.convert<units::degree>().value());
-  table->GetEntry("maxAngle").SetDouble(maxAngle.convert<units::degree>().value());
-  table->GetEntry("initialAngle").SetDouble(initialAngle.convert<units::degree>().value());
-  table->GetEntry("angleOffset").SetDouble(initialAngle.convert<units::degree>().value());
+  table->GetEntry("minAngle")
+      .SetDouble(minAngle.convert<units::degree>().value());
+  table->GetEntry("maxAngle")
+      .SetDouble(maxAngle.convert<units::degree>().value());
+  table->GetEntry("initialAngle")
+      .SetDouble(initialAngle.convert<units::degree>().value());
+  table->GetEntry("angleOffset")
+      .SetDouble(initialAngle.convert<units::degree>().value());
 }
 
 // arm config is used
@@ -38,12 +43,14 @@ void wom::subsystems::Arm::OnUpdate(units::second_t dt) {
     case ArmState::kIdle:
       break;
     case ArmState::kVelocity: {
-      units::newton_meter_t torque = 9.81_m / 1_s / 1_s * _config.armLength *
-                                     units::math::cos(angle + _config.angleOffset) *
-                                     (0.5 * _config.armMass + _config.loadMass);
+      units::newton_meter_t torque =
+          9.81_m / 1_s / 1_s * _config.armLength *
+          units::math::cos(angle + _config.angleOffset) *
+          (0.5 * _config.armMass + _config.loadMass);
       // units::volt_t feedforward = _config.leftGearbox.motor.Voltage(torque,
       // 0_rad/1_s);
-      units::volt_t feedforward = _config.leftGearbox.motor.Voltage(torque, _velocityPID.GetSetpoint());
+      units::volt_t feedforward =
+          _config.leftGearbox.motor.Voltage(torque, _velocityPID.GetSetpoint());
       // feedforward = 3.5_V;
       // std::cout << "feedforward" << feedforward.value() << std::endl;
       voltage = _velocityPID.Calculate(GetArmVelocity(), dt, feedforward);
@@ -51,10 +58,12 @@ void wom::subsystems::Arm::OnUpdate(units::second_t dt) {
       // std::endl; voltage = 0_V;
     } break;
     case ArmState::kAngle: {
-      units::newton_meter_t torque = 9.81_m / 1_s / 1_s * _config.armLength *
-                                     units::math::cos(angle + _config.angleOffset) *
-                                     (0.5 * _config.armMass + _config.loadMass);
-      units::volt_t feedforward = _config.leftGearbox.motor.Voltage(torque, 0_rad / 1_s);
+      units::newton_meter_t torque =
+          9.81_m / 1_s / 1_s * _config.armLength *
+          units::math::cos(angle + _config.angleOffset) *
+          (0.5 * _config.armMass + _config.loadMass);
+      units::volt_t feedforward =
+          _config.leftGearbox.motor.Voltage(torque, 0_rad / 1_s);
       // std::cout << "feedforward" << feedforward.value() << std::endl;
       voltage = _pid.Calculate(angle, dt, feedforward);
     } break;
@@ -79,8 +88,8 @@ void wom::subsystems::Arm::OnUpdate(units::second_t dt) {
 
   // std::cout << "voltage: " << voltage.value() << std::endl;
 
-  _config.leftGearbox.motorController->SetVoltage(voltage);
-  _config.rightGearbox.motorController->SetVoltage(voltage);
+  // _config.leftGearbox.motorController->SetVoltage(voltage);
+  // _config.rightGearbox.motorController->SetVoltage(voltage);
 
   // creates network table instances for the angle and config of the arm
   _table->GetEntry("angle").SetDouble(angle.convert<units::degree>().value());
diff --git a/wombat/src/main/cpp/subsystems/Elevator.cpp b/wombat/src/main/cpp/subsystems/Elevator.cpp
index 6e0267e7..cc840302 100644
--- a/wombat/src/main/cpp/subsystems/Elevator.cpp
+++ b/wombat/src/main/cpp/subsystems/Elevator.cpp
@@ -1,4 +1,4 @@
-// Copyright (c) 2023-2024 CurtinFRC
+/* // Copyright (c) 2023-2024 CurtinFRC
 // Open Source Software, you can modify it according to the terms
 // of the MIT License at the root of this project
 
@@ -7,7 +7,8 @@
 #include <networktables/NetworkTableInstance.h>
 #include <units/acceleration.h>
 
-void wom::subsystems::ElevatorConfig::WriteNT(std::shared_ptr<nt::NetworkTable> table) {
+void wom::subsystems::ElevatorConfig::WriteNT(
+    std::shared_ptr<nt::NetworkTable> table) {
   table->GetEntry("radius").SetDouble(radius.value());
   table->GetEntry("mass").SetDouble(mass.value());
   table->GetEntry("maxHeight").SetDouble(maxHeight.value());
@@ -19,7 +20,8 @@ wom::subsystems::Elevator::Elevator(ElevatorConfig config)
       _pid{config.path + "/pid", config.pid},
       _velocityPID{config.path + "/velocityPID", config.velocityPID},
       _table(nt::NetworkTableInstance::GetDefault().GetTable(config.path)) {
-  _config.leftGearbox.encoder->SetEncoderPosition(_config.initialHeight / _config.radius * 1_rad);
+  _config.leftGearbox.encoder->SetEncoderPosition(_config.initialHeight /
+                                                  _config.radius * 1_rad);
 }
 
 void wom::subsystems::Elevator::OnUpdate(units::second_t dt) {
@@ -37,7 +39,8 @@ void wom::subsystems::Elevator::OnUpdate(units::second_t dt) {
     case ElevatorState::kVelocity: {
       units::volt_t feedforward = _config.rightGearbox.motor.Voltage(
           (_config.mass * 9.81_mps_sq) * _config.radius,
-          _velocityPID.GetSetpoint() / (14.0 / 60.0 * 2.0 * 3.1415 * 0.02225 * 1_m) * 1_rad);
+          _velocityPID.GetSetpoint() /
+              (14.0 / 60.0 * 2.0 * 3.1415 * 0.02225 * 1_m) * 1_rad);
       // units::volt_t feedforward = _config.rightGearbox.motor.Voltage(0_Nm,
       // _velocityPID.GetSetpoint() / (14.0/60.0 * 2.0 * 3.1415 * 0.02225 * 1_m)
       // * 1_rad);
@@ -50,8 +53,8 @@ void wom::subsystems::Elevator::OnUpdate(units::second_t dt) {
       // voltage = 0_V;
     } break;
     case ElevatorState::kPID: {
-      units::volt_t feedforward =
-          _config.rightGearbox.motor.Voltage((_config.mass * 9.81_mps_sq) * _config.radius, 0_rad_per_s);
+      units::volt_t feedforward = _config.rightGearbox.motor.Voltage(
+          (_config.mass * 9.81_mps_sq) * _config.radius, 0_rad_per_s);
       // std::cout << "feed forward" << feedforward.value() << std::endl;
       feedforward = 1.2_V;
       // voltage = _pid.Calculate(height, dt, feedforward);
@@ -84,8 +87,8 @@ void wom::subsystems::Elevator::OnUpdate(units::second_t dt) {
 
   // Set voltage to motors...
   voltage *= speedLimit;
-  _config.leftGearbox.motorController->SetVoltage(voltage);
-  _config.rightGearbox.motorController->SetVoltage(voltage);
+  // _config.leftGearbox.motorController->SetVoltage(voltage);
+  // _config.rightGearbox.motorController->SetVoltage(voltage);
 }
 
 void wom::subsystems::Elevator::SetManual(units::volt_t voltage) {
@@ -96,7 +99,8 @@ void wom::subsystems::Elevator::SetPID(units::meter_t height) {
   _pid.SetSetpoint(height);
 }
 
-void wom::subsystems::Elevator::SetVelocity(units::meters_per_second_t velocity) {
+void wom::subsystems::Elevator::SetVelocity(
+    units::meters_per_second_t velocity) {
   _velocityPID.SetSetpoint(velocity);
 }
 
@@ -128,14 +132,19 @@ units::meter_t wom::subsystems::Elevator::GetHeight() const {
   // std::cout << "elevator position"<<
   // _config.rightGearbox.encoder->GetEncoderTicks() << std::endl; return
   // _config.rightGearbox.encoder->GetEncoderDistance() * 1_m;
-  return _config.elevatorEncoder.GetPosition() * 14 / 60 * 2 * 3.1415 * 0.02225 * 1_m;
+  return _config.elevatorEncoder.GetPosition() * 14 / 60 * 2 * 3.1415 *
+         0.02225 * 1_m;
 }
 
-units::meters_per_second_t wom::subsystems::Elevator::GetElevatorVelocity() const {
-  return _config.elevatorEncoder.GetVelocity() / 60_s * 14 / 60 * 2 * 3.1415 * 0.02225 * 1_m;
+units::meters_per_second_t wom::subsystems::Elevator::GetElevatorVelocity()
+    const {
+  return _config.elevatorEncoder.GetVelocity() / 60_s * 14 / 60 * 2 * 3.1415 *
+         0.02225 * 1_m;
 }
 
 units::meters_per_second_t wom::subsystems::Elevator::MaxSpeed() const {
-  return _config.leftGearbox.motor.Speed((_config.mass * 9.81_mps_sq) * _config.radius, 12_V) / 1_rad *
-         _config.radius;
+  return _config.leftGearbox.motor.Speed(
+             (_config.mass * 9.81_mps_sq) * _config.radius, 12_V) /
+         1_rad * _config.radius;
 }
+ */
\ No newline at end of file
diff --git a/wombat/src/main/cpp/subsystems/Shooter.cpp b/wombat/src/main/cpp/subsystems/Shooter.cpp
index b289c179..aa2fe679 100644
--- a/wombat/src/main/cpp/subsystems/Shooter.cpp
+++ b/wombat/src/main/cpp/subsystems/Shooter.cpp
@@ -1,4 +1,4 @@
-// Copyright (c) 2023-2024 CurtinFRC
+/* // Copyright (c) 2023-2024 CurtinFRC
 // Open Source Software, you can modify it according to the terms
 // of the MIT License at the root of this project
 
@@ -14,14 +14,16 @@ wom::subsystems::Shooter::Shooter(std::string path, ShooterParams params)
 
 void wom::subsystems::Shooter::OnUpdate(units::second_t dt) {
   units::volt_t voltage{0};
-  units::revolutions_per_minute_t currentSpeed = _params.gearbox.encoder->GetEncoderAngularVelocity();
+  units::revolutions_per_minute_t currentSpeed =
+      _params.gearbox.encoder->GetEncoderAngularVelocity();
 
   switch (_state) {
     case ShooterState::kManual:
       voltage = _setpointManual;
       break;
     case ShooterState::kPID: {
-      auto feedforward = _params.gearbox.motor.Voltage(0_Nm, _pid.GetSetpoint());
+      auto feedforward =
+          _params.gearbox.motor.Voltage(0_Nm, _pid.GetSetpoint());
       voltage = _pid.Calculate(currentSpeed, dt, feedforward);
     } break;
     case ShooterState::kIdle:
@@ -29,17 +31,20 @@ void wom::subsystems::Shooter::OnUpdate(units::second_t dt) {
       break;
   }
 
-  units::newton_meter_t max_torque_at_current_limit = _params.gearbox.motor.Torque(_params.currentLimit);
+  units::newton_meter_t max_torque_at_current_limit =
+      _params.gearbox.motor.Torque(_params.currentLimit);
   units::volt_t max_voltage_for_current_limit =
       _params.gearbox.motor.Voltage(max_torque_at_current_limit, currentSpeed);
 
-  voltage = 1_V * std::min(voltage.value(), max_voltage_for_current_limit.value());
+  voltage =
+      1_V * std::min(voltage.value(), max_voltage_for_current_limit.value());
 
-  _params.gearbox.motorController->SetVoltage(voltage);
+  // _params.gearbox.motorController->SetVoltage(voltage);
 
   _table->GetEntry("output_volts").SetDouble(voltage.value());
   _table->GetEntry("speed_rpm").SetDouble(currentSpeed.value());
-  _table->GetEntry("setpoint_rpm").SetDouble(units::revolutions_per_minute_t{_pid.GetSetpoint()}.value());
+  _table->GetEntry("setpoint_rpm")
+      .SetDouble(units::revolutions_per_minute_t{_pid.GetSetpoint()}.value());
   _table->GetEntry("stable").SetBoolean(_pid.IsStable());
 }
 
@@ -62,7 +67,8 @@ bool wom::subsystems::Shooter::IsStable() const {
 
 // Shooter Manual Set
 
-wom::subsystems::ShooterConstant::ShooterConstant(Shooter* s, units::volt_t setpoint)
+wom::subsystems::ShooterConstant::ShooterConstant(Shooter* s,
+                                                  units::volt_t setpoint)
     : _shooter(s), _setpoint(setpoint) {
   Controls(_shooter);
 }
@@ -73,7 +79,9 @@ void wom::subsystems::ShooterConstant::OnTick(units::second_t dt) {
 
 // ShooterSpinup
 
-wom::subsystems::ShooterSpinup::ShooterSpinup(Shooter* s, units::radians_per_second_t speed, bool hold)
+wom::subsystems::ShooterSpinup::ShooterSpinup(Shooter* s,
+                                              units::radians_per_second_t speed,
+                                              bool hold)
     : _shooter(s), _speed(speed), _hold(hold) {
   Controls(_shooter);
 }
@@ -84,3 +92,4 @@ void wom::subsystems::ShooterSpinup::OnTick(units::second_t dt) {
   if (!_hold && _shooter->IsStable())
     SetDone();
 }
+ */
\ No newline at end of file
diff --git a/wombat/src/main/cpp/utils/Encoder.cpp b/wombat/src/main/cpp/utils/Encoder.cpp
index 3935d82c..40b37b74 100644
--- a/wombat/src/main/cpp/utils/Encoder.cpp
+++ b/wombat/src/main/cpp/utils/Encoder.cpp
@@ -6,8 +6,8 @@
 
 #include <rev/SparkRelativeEncoder.h>
 
-wom::utils::Encoder::Encoder(double encoderTicksPerRotation, int type, units::meter_t wheelRadius,
-                             double reduction)
+wom::utils::Encoder::Encoder(double encoderTicksPerRotation, int type,
+                             units::meter_t wheelRadius, double reduction)
     : _reduction(reduction),
       _encoderTicksPerRotation(encoderTicksPerRotation),
       _type(type),
@@ -22,18 +22,18 @@ double wom::utils::Encoder::GetEncoderTicksPerRotation() const {
 }
 
 void wom::utils::Encoder::ZeroEncoder() {
-  _offset = GetEncoderRawTicks() * 1_rad;
+  // _offtt = GetEncoderRawTicks() * 1_rad;
 }
 
 void wom::utils::Encoder::SetEncoderPosition(units::degree_t position) {
   // units::radian_t offset_turns = position - GetEncoderPosition();
-  units::degree_t offset = position - (GetEncoderRawTicks() * 360 * 1_deg);
-  _offset = offset;
+  // units::degree_t offset = position - (GetEncoderRawTicks() * 360 * 1_deg);
+  // _offset = offset;
   // _offset = -offset_turns;
 }
 
-void wom::utils::Encoder::SetEncoderOffset(units::radian_t offset) {
-  _offset = offset;
+void wom::utils::Encoder::SetEncoderOffset(units::radian_t offset) { //HERE!
+  _offset = offset; 
   // units::turn_t offset_turns = offset;
   // _offset = offset_turns.value() * GetEncoderTicksPerRotation();
 }
@@ -43,50 +43,76 @@ void wom::utils::Encoder::SetReduction(double reduction) {
 }
 
 units::radian_t wom::utils::Encoder::GetEncoderPosition() {
-  if (_type == 0) {
-    units::turn_t n_turns{GetEncoderTicks() / GetEncoderTicksPerRotation()};
-    return n_turns;
-  } else if (_type == 2) {
-    units::degree_t pos = GetEncoderTicks() * 1_deg;
-    return pos - _offset;
-  } else {
-    units::degree_t pos = GetEncoderTicks() * 1_deg;
-    return pos - _offset;
-  }
+  //if (_type == 0) {
+  //  units::turn_t n_turns{GetEncoderTicks() / GetEncoderTicksPerRotation()};
+  //  return n_turns;
+  //} else if (_type == 2) {
+  //  units::degree_t pos = GetEncoderTicks() * 1_deg;
+  //  return pos;
+  //} else {
+  //  units::degree_t pos = GetEncoderTicks() * 1_deg;
+  //  return pos - _offset;
+  //}
+  return GetEncoderTicks() * 1_rad;
 }
 
 double wom::utils::Encoder::GetEncoderDistance() {
-  return GetEncoderTicks() * (2 * M_PI) * _wheelRadius.value();
+  return GetEncoderTicks() * (2 * 3.1415) * _wheelRadius.value();
 }
 
 units::radians_per_second_t wom::utils::Encoder::GetEncoderAngularVelocity() {
   // return GetEncoderTickVelocity() / (double)GetEncoderTicksPerRotation() * 2
   // * 3.1415926;
-  units::turns_per_second_t n_turns_per_s{GetEncoderTickVelocity() / GetEncoderTicksPerRotation()};
+  units::turns_per_second_t n_turns_per_s{GetEncoderTickVelocity() /
+                                          GetEncoderTicksPerRotation()};
   return n_turns_per_s;
 }
 
+double wom::utils::Encoder::GetVelocityValue() const {
+  // std::cout << "GET VELOCITY: " << GetVelocity() << std::endl;
+  return GetVelocity();
+  // return 0;
+}
+
 double wom::utils::DigitalEncoder::GetEncoderRawTicks() const {
   return _nativeEncoder.Get();
 }
 
+double wom::utils::DigitalEncoder::GetVelocity() const {
+  return 0;
+}
+
 double wom::utils::DigitalEncoder::GetEncoderTickVelocity() const {
   return _nativeEncoder.GetRate();
 }
 
-wom::utils::CANSparkMaxEncoder::CANSparkMaxEncoder(rev::CANSparkMax* controller, units::meter_t wheelRadius,
+wom::utils::CANSparkMaxEncoder::CANSparkMaxEncoder(rev::CANSparkMax* controller,
+                                                   units::meter_t wheelRadius,
                                                    double reduction)
     : wom::utils::Encoder(42, reduction, wheelRadius, 2),
-      _encoder(controller->GetEncoder(rev::SparkRelativeEncoder::Type::kQuadrature)) {}
+      _encoder(controller->GetEncoder(
+          rev::SparkRelativeEncoder::Type::kHallSensor)) {}
 
 double wom::utils::CANSparkMaxEncoder::GetEncoderRawTicks() const {
-  return _encoder.GetPosition() * _reduction;
+  return ((_encoder.GetPosition() * 2 * 3.1415) / 200);
 }
 
 double wom::utils::CANSparkMaxEncoder::GetEncoderTickVelocity() const {
   return _encoder.GetVelocity() * GetEncoderTicksPerRotation() / 60;
 }
 
+double wom::utils::TalonFXEncoder::GetVelocity() const {
+  return _controller->GetVelocity().GetValue().value();
+}
+
+double wom::utils::CanEncoder::GetVelocity() const {
+  return _canEncoder->GetVelocity().GetValue().value();
+}
+
+double wom::utils::DutyCycleEncoder::GetVelocity() const {
+  return 0;
+}
+
 double wom::utils::CANSparkMaxEncoder::GetPosition() const {
   return _encoder.GetPosition();
 }
@@ -95,9 +121,11 @@ double wom::utils::CANSparkMaxEncoder::GetVelocity() const {
   return _encoder.GetVelocity();
 }
 
-wom::utils::TalonFXEncoder::TalonFXEncoder(ctre::phoenix6::hardware::TalonFX* controller,
-                                           units::meter_t wheelRadius, double reduction)
-    : utils::Encoder(2048, reduction, wheelRadius, 0), _controller(controller) {}
+wom::utils::TalonFXEncoder::TalonFXEncoder(
+    ctre::phoenix6::hardware::TalonFX* controller, units::meter_t wheelRadius,
+    double reduction)
+    : utils::Encoder(2048, reduction, wheelRadius, 0),
+      _controller(controller) {}
 
 double wom::utils::TalonFXEncoder::GetEncoderRawTicks() const {
   return _controller->GetPosition().GetValue().value();
@@ -107,9 +135,12 @@ double wom::utils::TalonFXEncoder::GetEncoderTickVelocity() const {
   return _controller->Get() * 10;
 }
 
-wom::utils::DutyCycleEncoder::DutyCycleEncoder(int channel, units::meter_t wheelRadius,
-                                               double ticksPerRotation, double reduction)
-    : wom::utils::Encoder(ticksPerRotation, reduction, wheelRadius, 0), _dutyCycleEncoder(channel) {}
+wom::utils::DutyCycleEncoder::DutyCycleEncoder(int channel,
+                                               units::meter_t wheelRadius,
+                                               double ticksPerRotation,
+                                               double reduction)
+    : wom::utils::Encoder(ticksPerRotation, reduction, wheelRadius, 0),
+      _dutyCycleEncoder(channel) {}
 
 double wom::utils::DutyCycleEncoder::GetEncoderRawTicks() const {
   return _dutyCycleEncoder.Get().value();
@@ -119,16 +150,19 @@ double wom::utils::DutyCycleEncoder::GetEncoderTickVelocity() const {
   return 0;
 }
 
-wom::utils::CanEncoder::CanEncoder(int deviceNumber, units::meter_t wheelRadius, double ticksPerRotation,
-                                   double reduction, std::string name)
-    : wom::utils::Encoder(ticksPerRotation, reduction, wheelRadius, 1) {
+wom::utils::CanEncoder::CanEncoder(int deviceNumber, units::meter_t wheelRadius,
+                                   double ticksPerRotation, double reduction,
+                                   std::string name)
+    : wom::utils::Encoder(ticksPerRotation, 2, wheelRadius, reduction) {
   _canEncoder = new ctre::phoenix6::hardware::CANcoder(deviceNumber, name);
+  // _canEncoder->ConfigAbsoluteEncoderRange(0, 1);
+
 }
 
 double wom::utils::CanEncoder::GetEncoderRawTicks() const {
-  return _canEncoder->GetAbsolutePosition().GetValue().value();
+  return (_canEncoder->GetAbsolutePosition().GetValue().value() * 2 * 3.14) - _offset.value();
 }
 
 double wom::utils::CanEncoder::GetEncoderTickVelocity() const {
   return _canEncoder->GetVelocity().GetValue().value();
-}
+}
\ No newline at end of file
diff --git a/wombat/src/main/include/Wombat.h b/wombat/src/main/include/Wombat.h
index 9d297c4f..c1b356a9 100644
--- a/wombat/src/main/include/Wombat.h
+++ b/wombat/src/main/include/Wombat.h
@@ -23,7 +23,7 @@
 namespace wom {
 using namespace wom;
 using namespace wom::utils;
-using namespace wom::subsystems;
+// using namespace wom::subsystems;
 using namespace wom::drivetrain;
 using namespace wom::drivetrain::behaviours;
 using namespace wom::vision;
diff --git a/wombat/src/main/include/drivetrain/Drivetrain.h b/wombat/src/main/include/drivetrain/Drivetrain.h
index 0b8fcc72..b3894217 100644
--- a/wombat/src/main/include/drivetrain/Drivetrain.h
+++ b/wombat/src/main/include/drivetrain/Drivetrain.h
@@ -1,4 +1,4 @@
-// Copyright (c) 2023-2024 CurtinFRC
+/* // Copyright (c) 2023-2024 CurtinFRC
 // Open Source Software, you can modify it according to the terms
 // of the MIT License at the root of this project
 
@@ -55,3 +55,4 @@ class Drivetrain : public behaviour::HasBehaviour {
 };
 }  // namespace drivetrain
 }  // namespace wom
+ */
\ No newline at end of file
diff --git a/wombat/src/main/include/drivetrain/SwerveDrive.h b/wombat/src/main/include/drivetrain/SwerveDrive.h
index 661a2943..625ef135 100644
--- a/wombat/src/main/include/drivetrain/SwerveDrive.h
+++ b/wombat/src/main/include/drivetrain/SwerveDrive.h
@@ -6,6 +6,7 @@
 
 #include <frc/estimator/SwerveDrivePoseEstimator.h>
 #include <frc/kinematics/SwerveDriveKinematics.h>
+#include <frc/controller/PIDController.h>
 #include <networktables/DoubleTopic.h>
 #include <networktables/NetworkTable.h>
 #include <networktables/NetworkTableInstance.h>
@@ -27,10 +28,21 @@
 #include "utils/Gearbox.h"
 #include "utils/PID.h"
 
+#include <functional>
+#include <limits>
+
+#include <wpi/SymbolExports.h>
+#include <wpi/sendable/Sendable.h>
+#include <wpi/sendable/SendableHelper.h>
+
+
 namespace wom {
 namespace drivetrain {
 
+
+
 enum class SwerveModuleState { kZeroing, kIdle, kPID };
+enum class TurnOffsetValues { reverse, forward, none };
 
 struct SwerveModuleConfig {
   frc::Translation2d position;
@@ -47,11 +59,12 @@ struct SwerveModuleConfig {
 
 class SwerveModule {
  public:
-  using angle_pid_conf_t = utils::PIDConfig<units::radian, units::volt>;
-  using velocity_pid_conf_t = utils::PIDConfig<units::meters_per_second, units::volt>;
+  //using angle_pid_conf_t = utils::PIDConfig<units::radian, units::volt>;
+  using velocity_pid_conf_t =
+      utils::PIDConfig<units::meters_per_second, units::volt>;
 
-  SwerveModule(std::string path, SwerveModuleConfig config, angle_pid_conf_t anglePID,
-               velocity_pid_conf_t velocityPID);
+  SwerveModule(std::string path, SwerveModuleConfig config,
+               /*angle_pid_conf_t anglePID,*/ velocity_pid_conf_t velocityPID);
   void OnUpdate(units::second_t dt);
   void OnStart();
 
@@ -65,10 +78,17 @@ class SwerveModule {
 
   void SetZero(units::second_t dt);
   void SetIdle();
-  void SetPID(units::radian_t angle, units::meters_per_second_t speed, units::second_t dt);
+  void SetPID(units::radian_t angle, units::meters_per_second_t speed,
+              units::second_t dt);
   void SetZero();
   void SetVoltageLimit(units::volt_t driveModuleVoltageLimit);
 
+  void SetTurnOffsetForward();
+  void SetTurnOffsetReverse();
+  void TurnOffset();
+
+  double closestAngle(double a, double b);
+
   // double GetCancoderPosition(); // from liam's
 
   void SetAccelerationLimit(units::meters_per_second_squared_t limit);
@@ -81,7 +101,8 @@ class SwerveModule {
 
   const SwerveModuleConfig& GetConfig() const;
 
-  utils::PIDController<units::radians, units::volt> _anglePIDController;
+  //utils::PIDController<units::radians, units::volt> _anglePIDController;
+  frc::PIDController _anglePIDController;
 
  private:
   SwerveModuleConfig _config;
@@ -91,7 +112,7 @@ class SwerveModule {
   bool _hasZeroedEncoder = false;
   bool _hasZeroed = false;
 
-  utils::PIDController<units::meters_per_second, units::volt> _velocityPIDController;
+  frc::PIDController _velocityPIDController;
 
   std::shared_ptr<nt::NetworkTable> _table;
 
@@ -99,22 +120,27 @@ class SwerveModule {
 
   double _offset;
   units::meters_per_second_squared_t _currentAccelerationLimit = 6_mps / 1_s;
+
+  TurnOffsetValues _turnOffset = TurnOffsetValues::none;
 };
 
 struct SwerveDriveConfig {
-  using pose_angle_conf_t = utils::PIDConfig<units::radian, units::radians_per_second>;
-  using pose_position_conf_t = utils::PIDConfig<units::meter, units::meters_per_second>;
-  using balance_conf_t = utils::PIDConfig<units::degree, units::meters_per_second>;
+  /*using pose_angle_conf_t =
+      utils::PIDConfig<units::radian, units::radians_per_second>;*/
+  using pose_position_conf_t =
+      utils::PIDConfig<units::meter, units::meters_per_second>;
+  using balance_conf_t =
+      utils::PIDConfig<units::degree, units::meters_per_second>;
 
   std::string path;
-  SwerveModule::angle_pid_conf_t anglePID;
+  //SwerveModule::angle_pid_conf_t anglePID;
   SwerveModule::velocity_pid_conf_t velocityPID;
 
   wpi::array<SwerveModuleConfig, 4> modules;
 
   ctre::phoenix6::hardware::Pigeon2* gyro;
 
-  pose_angle_conf_t poseAnglePID;
+  //pose_angle_conf_t poseAnglePID;
   pose_position_conf_t posePositionPID;
 
   units::kilogram_t mass;
@@ -168,7 +194,8 @@ class SwerveDrive : public behaviour::HasBehaviour {
    * @brief This function switches the state to handle the robot's rotation
    * matching that of the joystick
    */
-  void RotateMatchJoystick(units::radian_t joystickAngle, FieldRelativeSpeeds speeds);
+  void RotateMatchJoystick(units::radian_t joystickAngle,
+                           FieldRelativeSpeeds speeds);
 
   void SetIdle();
 
@@ -178,7 +205,8 @@ class SwerveDrive : public behaviour::HasBehaviour {
   void SetFieldRelativeVelocity(FieldRelativeSpeeds speeds);
   void SetPose(frc::Pose2d pose);
   bool IsAtSetPose();
-  void SetIndividualTuning(int mod, units::radian_t angle, units::meters_per_second_t speed);
+  void SetIndividualTuning(int mod, units::radian_t angle,
+                           units::meters_per_second_t speed);
   void SetTuning(units::radian_t angle, units::meters_per_second_t speed);
   void SetZero();
   void SetVoltageLimit(units::volt_t driveVoltageLimit);
@@ -213,7 +241,9 @@ class SwerveDrive : public behaviour::HasBehaviour {
   frc::SwerveDriveKinematics<4> _kinematics;
   frc::SwerveDrivePoseEstimator<4> _poseEstimator;
 
-  utils::PIDController<units::radian, units::radians_per_second> _anglePIDController;
+  /*utils::PIDController<units::radian, units::radians_per_second>
+      _anglePIDController;*/
+  frc::PIDController _anglePIDController;
   utils::PIDController<units::meter, units::meters_per_second> _xPIDController;
   utils::PIDController<units::meter, units::meters_per_second> _yPIDController;
 
@@ -226,10 +256,10 @@ class SwerveDrive : public behaviour::HasBehaviour {
   units::radian_t _angle;
   units::meters_per_second_t _speed;
 
-  double frontLeftEncoderOffset = -143.26171875;
-  double frontRightEncoderOffset = 167.87109375;
-  double backLeftEncoderOffset = -316.669921875;
-  double backRightEncoderOffset = -119.619140625;
+  // double frontLeftEncoderOffset = -143.26171875;
+  // double frontRightEncoderOffset = 167.87109375;
+  // double backLeftEncoderOffset = -316.669921875;
+  // double backRightEncoderOffset = -119.619140625;
 };
 }  // namespace drivetrain
-}  // namespace wom
+}  // namespace wom
\ No newline at end of file
diff --git a/wombat/src/main/include/drivetrain/behaviours/SwerveBehaviours.h b/wombat/src/main/include/drivetrain/behaviours/SwerveBehaviours.h
index b2ceaeba..d49d3f12 100644
--- a/wombat/src/main/include/drivetrain/behaviours/SwerveBehaviours.h
+++ b/wombat/src/main/include/drivetrain/behaviours/SwerveBehaviours.h
@@ -37,14 +37,16 @@ class ManualDrivebase : public behaviour::Behaviour {
    * A pointer to the controller that the driver has been allocated (the
    * allocated memory address that stores the "driver controller" object)
    */
-  ManualDrivebase(wom::drivetrain::SwerveDrive* swerveDrivebase, frc::XboxController* driverController);
+  ManualDrivebase(wom::drivetrain::SwerveDrive* swerveDrivebase,
+                  frc::XboxController* driverController);
 
   void OnTick(units::second_t deltaTime) override;
   /**
    * @brief This function handles all of the logic behind the tangent function,
    * to be able to calculate an angle between 0 andd 360 degrees, inclusively
    */
-  void CalculateRequestedAngle(double joystickX, double joystickY, units::degree_t defaultAngle);
+  void CalculateRequestedAngle(double joystickX, double joystickY,
+                               units::degree_t defaultAngle);
   void OnStart() override;
   void ResetMode();
 
@@ -70,8 +72,8 @@ class ManualDrivebase : public behaviour::Behaviour {
   const double turningDeadzone = 0.2;
 
   // Variables for solution to Anti-tip
-  double prevJoystickX, prevJoystickY, prevPrevJoystickX, prevPrevJoystickY, usingJoystickXPos,
-      usingJoystickYPos;
+  double prevJoystickX, prevJoystickY, prevPrevJoystickX, prevPrevJoystickY,
+      usingJoystickXPos, usingJoystickYPos;
   // The speed that the joystick must travel to activate averaging over previous
   // 3 joystick positions
   const double smoothingThreshold = 1;
@@ -82,12 +84,12 @@ class ManualDrivebase : public behaviour::Behaviour {
   // The translation speeds for when "slow speed", "normal speed", "fast speed"
   // modes are active
   const translationSpeed_ lowSensitivityDriveSpeed = 3.25_ft / 1_s;
-  const translationSpeed_ defaultDriveSpeed = 13_ft / 1_s;
+  const translationSpeed_ defaultDriveSpeed = 30_ft / 1_s;
   const translationSpeed_ highSensitivityDriveSpeed = 18_ft / 1_s;
   // The rotation speeds for when "slow speed", "normal speed", "fast speed"
   // modes are active
   const rotationSpeed_ lowSensitivityRotateSpeed = 90_deg / 1_s;
-  const rotationSpeed_ defaultRotateSpeed = 360_deg / 1_s;
+  const rotationSpeed_ defaultRotateSpeed = 500_deg / 0.7_s;
   const rotationSpeed_ highSensitivityRotateSpeed = 720_deg / 1_s;
 
   translationSpeed_ maxMovementMagnitude = defaultDriveSpeed;
@@ -124,8 +126,8 @@ class GoToPose : public behaviour::Behaviour {
 
 class FollowTrajectory : public behaviour::Behaviour {
  public:
-  FollowTrajectory(wom::drivetrain::SwerveDrive* swerve, wom::utils::Pathplanner* pathplanner,
-                   std::string path);
+  FollowTrajectory(wom::drivetrain::SwerveDrive* swerve,
+                   wom::utils::Pathplanner* pathplanner, std::string path);
 
   void OnTick(units::second_t dt) override;
 
@@ -182,7 +184,8 @@ class TempSimSwerveDrive {
 
 class AutoSwerveDrive {
  public:
-  AutoSwerveDrive(wom::drivetrain::SwerveDrive* swerve, frc::Timer* timer, frc::Field2d* field);
+  AutoSwerveDrive(wom::drivetrain::SwerveDrive* swerve, frc::Timer* timer,
+                  frc::Field2d* field);
 
   void OnUpdate();
 
@@ -234,4 +237,4 @@ class DrivebasePoseBehaviour : public behaviour::Behaviour {
 };
 }  // namespace behaviours
 }  // namespace drivetrain
-}  // namespace wom
+}  // namespace wom
\ No newline at end of file
diff --git a/wombat/src/main/include/subsystems/Arm.h b/wombat/src/main/include/subsystems/Arm.h
index 8b91be72..62c2d05b 100644
--- a/wombat/src/main/include/subsystems/Arm.h
+++ b/wombat/src/main/include/subsystems/Arm.h
@@ -1,4 +1,4 @@
-// Copyright (c) 2023-2024 CurtinFRC
+/* // Copyright (c) 2023-2024 CurtinFRC
 // Open Source Software, you can modify it according to the terms
 // of the MIT License at the root of this project
 
@@ -79,3 +79,4 @@ class Arm : public behaviour::HasBehaviour {
 };
 }  // namespace subsystems
 }  // namespace wom
+ */
\ No newline at end of file
diff --git a/wombat/src/main/include/subsystems/Elevator.h b/wombat/src/main/include/subsystems/Elevator.h
index 840470eb..ccbe2379 100644
--- a/wombat/src/main/include/subsystems/Elevator.h
+++ b/wombat/src/main/include/subsystems/Elevator.h
@@ -1,4 +1,4 @@
-// Copyright (c) 2023-2024 CurtinFRC
+/* // Copyright (c) 2023-2024 CurtinFRC
 // Open Source Software, you can modify it according to the terms
 // of the MIT License at the root of this project
 
@@ -82,3 +82,4 @@ class Elevator : public behaviour::HasBehaviour {
 };
 }  // namespace subsystems
 }  // namespace wom
+ */
\ No newline at end of file
diff --git a/wombat/src/main/include/subsystems/Shooter.h b/wombat/src/main/include/subsystems/Shooter.h
index e5ac219d..694a6d57 100644
--- a/wombat/src/main/include/subsystems/Shooter.h
+++ b/wombat/src/main/include/subsystems/Shooter.h
@@ -1,4 +1,4 @@
-// Copyright (c) 2023-2024 CurtinFRC
+/* // Copyright (c) 2023-2024 CurtinFRC
 // Open Source Software, you can modify it according to the terms
 // of the MIT License at the root of this project
 
@@ -73,3 +73,4 @@ class ShooterSpinup : public behaviour::Behaviour {
 };
 }  // namespace subsystems
 }  // namespace wom
+ */
\ No newline at end of file
diff --git a/wombat/src/main/include/utils/Encoder.h b/wombat/src/main/include/utils/Encoder.h
index 62ccc28f..073ba6cd 100644
--- a/wombat/src/main/include/utils/Encoder.h
+++ b/wombat/src/main/include/utils/Encoder.h
@@ -20,7 +20,8 @@ namespace wom {
 namespace utils {
 class Encoder {
  public:
-  Encoder(double encoderTicksPerRotation, int type, units::meter_t wheelRadius, double reduction = 1.0);
+  Encoder(double encoderTicksPerRotation, int type, units::meter_t wheelRadius,
+          double reduction = 1.0);
 
   virtual double GetEncoderRawTicks() const = 0;
   virtual double GetEncoderTickVelocity() const = 0;  // ticks/s
@@ -41,39 +42,42 @@ class Encoder {
   int encoderType = 0;
   double _reduction;
 
+  virtual double GetVelocity() const = 0;
+  double GetVelocityValue() const;
+
+  units::radian_t _offset = 0_rad; //bad
  private:
   double _encoderTicksPerRotation;
-  units::radian_t _offset = 0_rad;
   int _type = 0;
   units::meter_t _wheelRadius;
 };
 
 class DigitalEncoder : public Encoder {
  public:
-  DigitalEncoder(int channelA, int channelB, double ticksPerRotation, units::meter_t wheelRadius,
-                 double reduction = 1)
-      : Encoder(ticksPerRotation, reduction, wheelRadius, 0), _nativeEncoder(channelA, channelB) {}
+  DigitalEncoder(int channelA, int channelB, double ticksPerRotation,
+                 units::meter_t wheelRadius, double reduction = 1)
+      : Encoder(ticksPerRotation, reduction, wheelRadius, 0),
+        _nativeEncoder(channelA, channelB) {}
 
   double GetEncoderRawTicks() const override;
   double GetEncoderTickVelocity() const override;
 
   double GetPosition() const;
-  double GetVelocity() const;
-
+  double GetVelocity() const override;
  private:
   frc::Encoder _nativeEncoder;
 };
 
-class SimCANSparkMaxEncoder;
 class CANSparkMaxEncoder : public Encoder {
  public:
-  explicit CANSparkMaxEncoder(rev::CANSparkMax* controller, units::meter_t wheelRadius, double reduction = 1);
+  explicit CANSparkMaxEncoder(rev::CANSparkMax* controller,
+                              units::meter_t wheelRadius, double reduction = 1);
 
   double GetEncoderRawTicks() const override;
   double GetEncoderTickVelocity() const override;
 
   double GetPosition() const;
-  double GetVelocity() const;
+  double GetVelocity() const override;
 
  protected:
   rev::SparkRelativeEncoder _encoder;
@@ -82,11 +86,12 @@ class CANSparkMaxEncoder : public Encoder {
 
 class TalonFXEncoder : public Encoder {
  public:
-  TalonFXEncoder(ctre::phoenix6::hardware::TalonFX* controller, units::meter_t wheelRadius,
-                 double reduction = 1);
+  TalonFXEncoder(ctre::phoenix6::hardware::TalonFX* controller,
+                 units::meter_t wheelRadius, double reduction = 1);
 
   double GetEncoderRawTicks() const override;
   double GetEncoderTickVelocity() const override;
+  double GetVelocity() const override;
 
  private:
   ctre::phoenix6::hardware::TalonFX* _controller;
@@ -94,11 +99,12 @@ class TalonFXEncoder : public Encoder {
 
 class DutyCycleEncoder : public Encoder {
  public:
-  DutyCycleEncoder(int channel, units::meter_t wheelRadius, double ticksPerRotation = 1,
-                   double reduction = 1);
+  DutyCycleEncoder(int channel, units::meter_t wheelRadius,
+                   double ticksPerRotation = 1, double reduction = 1);
 
   double GetEncoderRawTicks() const override;
   double GetEncoderTickVelocity() const override;
+  double GetVelocity() const override;
 
  private:
   frc::DutyCycleEncoder _dutyCycleEncoder;
@@ -106,12 +112,14 @@ class DutyCycleEncoder : public Encoder {
 
 class CanEncoder : public Encoder {
  public:
-  CanEncoder(int deviceNumber, units::meter_t wheelRadius, double ticksPerRotation = 4095,
-             double reduction = 1, std::string name = "Drivebase");
+  CanEncoder(int deviceNumber, units::meter_t wheelRadius,
+             double ticksPerRotation = 4095, double reduction = 6.75,
+             std::string name = "Drivebase");
 
   double GetEncoderRawTicks() const override;
   double GetEncoderTickVelocity() const override;
   double GetAbsoluteEncoderPosition();
+  double GetVelocity() const override;
 
   const double constantValue = 0.0;
 
@@ -119,4 +127,4 @@ class CanEncoder : public Encoder {
   ctre::phoenix6::hardware::CANcoder* _canEncoder;
 };
 }  // namespace utils
-}  // namespace wom
+}  // namespace wom
\ No newline at end of file
diff --git a/wombat/src/main/include/utils/PID.h b/wombat/src/main/include/utils/PID.h
index 7d9b9516..5f1a3304 100644
--- a/wombat/src/main/include/utils/PID.h
+++ b/wombat/src/main/include/utils/PID.h
@@ -9,6 +9,7 @@
 #include <units/base.h>
 #include <units/time.h>
 
+#include <cmath>
 #include <memory>
 #include <optional>
 #include <string>
@@ -103,13 +104,16 @@ class PIDController {
   void Reset() { _integralSum = sum_t{0}; }
 
   out_t Calculate(in_t pv, units::second_t dt, out_t feedforward = out_t{0}) {
+    // pv = units::math::fabs(pv);
     bool is_negative;
     if (pv.value() < 0) {
       is_negative = true;
       pv = units::math::fabs(pv);
     }
     auto error = do_wrap(_setpoint - pv);
+    error = units::math::fabs(error);
     _integralSum += error * dt;
+    _integralSum = units::math::fabs(_integralSum);
     if (config.izone.value() > 0 && (error > config.izone || error < -config.izone))
       _integralSum = sum_t{0};
 
@@ -122,15 +126,6 @@ class PIDController {
     _stableVel = _velFilter.Calculate(deriv);
 
     auto out = config.kp * error + config.ki * _integralSum + config.kd * deriv + feedforward;
-    // std::cout << "Out value" << out.value() << std::endl;
-
-    _table->GetEntry("pv").SetDouble(pv.value());
-    _table->GetEntry("dt").SetDouble(dt.value());
-    _table->GetEntry("setpoint").SetDouble(_setpoint.value());
-    _table->GetEntry("error").SetDouble(error.value());
-    _table->GetEntry("integralSum").SetDouble(_integralSum.value());
-    _table->GetEntry("stable").SetBoolean(IsStable());
-    _table->GetEntry("demand").SetDouble(out.value());
 
     _last_pv = pv;
     _last_error = error;

From 4e8f5b1d21353c79bab68f66a6fc26f39c3bdeef Mon Sep 17 00:00:00 2001
From: Isaac Turner <spacey_sooty@outlook.com>
Date: Sun, 11 Feb 2024 11:25:40 +0800
Subject: [PATCH 129/207] formatted (#133)

---
 src/main/cpp/AlphaArm.cpp                     | 155 +++++++-------
 src/main/cpp/AlphaArmBehaviour.cpp            |   3 +-
 src/main/cpp/Intake.cpp                       |   2 +-
 src/main/cpp/IntakeBehaviour.cpp              |   2 +-
 src/main/cpp/Robot.cpp                        |  35 ++--
 src/main/cpp/Shooter.cpp                      |  25 ++-
 src/main/cpp/ShooterBehaviour.cpp             |   2 +-
 src/main/include/AlphaArm.h                   | 107 +++++-----
 src/main/include/Intake.h                     |   2 +-
 src/main/include/IntakeBehaviour.h            |   2 +-
 src/main/include/Robot.h                      |   3 +-
 src/main/include/RobotMap.h                   | 194 ++++++++----------
 src/main/include/Shooter.h                    |   2 +-
 src/main/include/ShooterBehaviour.h           |  10 +-
 wombat/src/main/cpp/behaviour/Behaviour.cpp   |  23 +--
 .../main/cpp/behaviour/BehaviourScheduler.cpp |   4 +-
 .../src/main/cpp/behaviour/HasBehaviour.cpp   |   3 +-
 wombat/src/main/cpp/drivetrain/Drivetrain.cpp |  53 -----
 .../src/main/cpp/drivetrain/SwerveDrive.cpp   | 162 ++++++---------
 .../behaviours/SwerveBehaviours.cpp           | 106 +++++-----
 wombat/src/main/cpp/subsystems/Arm.cpp        | 192 -----------------
 wombat/src/main/cpp/subsystems/Elevator.cpp   | 150 --------------
 wombat/src/main/cpp/subsystems/Shooter.cpp    |  95 ---------
 wombat/src/main/cpp/utils/Encoder.cpp         |  62 +++---
 wombat/src/main/include/Wombat.h              |   5 -
 .../src/main/include/drivetrain/Drivetrain.h  |  58 ------
 .../src/main/include/drivetrain/SwerveDrive.h |  45 ++--
 .../drivetrain/behaviours/SwerveBehaviours.h  |  19 +-
 wombat/src/main/include/subsystems/Arm.h      |  82 --------
 wombat/src/main/include/subsystems/Elevator.h |  85 --------
 wombat/src/main/include/subsystems/Shooter.h  |  76 -------
 wombat/src/main/include/utils/Encoder.h       |  32 ++-
 32 files changed, 449 insertions(+), 1347 deletions(-)
 delete mode 100644 wombat/src/main/cpp/drivetrain/Drivetrain.cpp
 delete mode 100644 wombat/src/main/cpp/subsystems/Arm.cpp
 delete mode 100644 wombat/src/main/cpp/subsystems/Elevator.cpp
 delete mode 100644 wombat/src/main/cpp/subsystems/Shooter.cpp
 delete mode 100644 wombat/src/main/include/drivetrain/Drivetrain.h
 delete mode 100644 wombat/src/main/include/subsystems/Arm.h
 delete mode 100644 wombat/src/main/include/subsystems/Elevator.h
 delete mode 100644 wombat/src/main/include/subsystems/Shooter.h

diff --git a/src/main/cpp/AlphaArm.cpp b/src/main/cpp/AlphaArm.cpp
index c97ead0f..93b5b5f3 100644
--- a/src/main/cpp/AlphaArm.cpp
+++ b/src/main/cpp/AlphaArm.cpp
@@ -3,8 +3,12 @@
 // of the MIT License at the root of this project
 
 #include "AlphaArm.h"
-//fiddle with these values
-AlphaArm::AlphaArm(AlphaArmConfig config) : _config(config), _pidWom(_config.path + "/pid", config.pidConfigA) /*_table(nt::NetworkTableInstance::GetDefault().GetTable(config.path)*/{}
+
+// fiddle with these values
+AlphaArm::AlphaArm(AlphaArmConfig config)
+    : _config(config),
+      _pidWom(_config.path + "/pid",
+              config.pidConfigA) /*_table(nt::NetworkTableInstance::GetDefault().GetTable(config.path)*/ {}
 
 void AlphaArm::OnStart() {
   started = false;
@@ -12,98 +16,101 @@ void AlphaArm::OnStart() {
   // _pidWom.SetSetpoint(_config.alphaArmGearbox.encoder->GetEncoderPosition());
 }
 
-void AlphaArm::OnUpdate(units::second_t dt){
+void AlphaArm::OnUpdate(units::second_t dt) {
   _table->GetEntry("Error").SetDouble(_pidWom.GetError().value());
   _table->GetEntry("Current Pos").SetDouble(_config.alphaArmGearbox.encoder->GetEncoderPosition().value());
   _table->GetEntry("Setpoint").SetDouble(_pidWom.GetSetpoint().value());
   _table->GetEntry("State ").SetString(_stateName);
-        switch(_state){
-          case AlphaArmState::kIdle:
-            _stateName = "Idle";
-            // _pidWom.SetSetpoint(_config.alphaArmGearbox.encoder->GetEncoderPosition());
-          
-            
-            _setAlphaArmVoltage = 0_V;
+  switch (_state) {
+    case AlphaArmState::kIdle:
+      _stateName = "Idle";
+      // _pidWom.SetSetpoint(_config.alphaArmGearbox.encoder->GetEncoderPosition());
+
+      _setAlphaArmVoltage = 0_V;
 
-          break;
-          case AlphaArmState::kRaw:
-            _stateName = "Raw";
-          
-            _setAlphaArmVoltage = _rawArmVoltage;
+      break;
+    case AlphaArmState::kRaw:
+      _stateName = "Raw";
 
-          break;
-          case AlphaArmState::kAmpAngle:
-          {
-            _stateName = "Amp Angle";
+      _setAlphaArmVoltage = _rawArmVoltage;
 
-            // _pidWom.SetSetpoint(_goal);
-            // _setAlphaArmVoltage = _pidWom.Calculate(_config.alphaArmGearbox.encoder->GetEncoderPosition(), dt, 0_V);
+      break;
+    case AlphaArmState::kAmpAngle: {
+      _stateName = "Amp Angle";
 
-            if (started) {
-              if (_controlledRawVoltage.value() == 0) {
-                if (-_config.alphaArmGearbox.encoder->GetEncoderPosition() > (_startingPos + (3.1415_rad/2))) {
-                    // _pidWom.SetSetpoint(_encoderSetpoint);
-                    // _setAlphaArmVoltage = -_pidWom.Calculate(-_config.alphaArmGearbox.encoder->GetEncoderPosition(), dt, 0.0_V);
-                //   _table->GetEntry("Demand").SetDouble(_setAlphaArmVoltage.value());
-                // } else if (_config.alphaArmGearbox.encoder->GetEncoderPosition() < 0_rad) {
-                //   _pidWom.SetSetpoint(_encoderSetpoint);
-                //   _setAlphaArmVoltage = _pidWom.Calculate(_config.alphaArmGearbox.encoder->GetEncoderPosition(), dt, 0.0_V);
-                //   _table->GetEntry("Demand").SetDouble(_setAlphaArmVoltage.value());
-                  _setAlphaArmVoltage = 0_V;
-                } else {
-                  _pidWom.SetSetpoint(_encoderSetpoint);
-                  _setAlphaArmVoltage = -_pidWom.Calculate(-_config.alphaArmGearbox.encoder->GetEncoderPosition(), dt, 0.0_V);
-                //   _pidWom.Reset();
-                //   _encoderSetpoint = _config.alphaArmGearbox.encoder->GetEncoderPosition();
-                //   _setAlphaArmVoltage = _controlledRawVoltage;
-                }
-              } else {
-                _pidWom.Reset();
-                _encoderSetpoint = -_config.alphaArmGearbox.encoder->GetEncoderPosition();
-                _setAlphaArmVoltage = _controlledRawVoltage;
-              }
-            } else {
-              _pidWom.Reset();
-              _encoderSetpoint = -_config.alphaArmGearbox.encoder->GetEncoderPosition();
-              _setAlphaArmVoltage = _controlledRawVoltage;
+      // _pidWom.SetSetpoint(_goal);
+      // _setAlphaArmVoltage = _pidWom.Calculate(_config.alphaArmGearbox.encoder->GetEncoderPosition(), dt,
+      // 0_V);
 
-              if (std::abs(_controlledRawVoltage.value()) > 0) {
-                _startingPos = -_config.alphaArmGearbox.encoder->GetEncoderPosition();
-                started = true;
-              }
-            }
-            
+      if (started) {
+        if (_controlledRawVoltage.value() == 0) {
+          if (-_config.alphaArmGearbox.encoder->GetEncoderPosition() > (_startingPos + (3.1415_rad / 2))) {
+            // _pidWom.SetSetpoint(_encoderSetpoint);
+            // _setAlphaArmVoltage =
+            // -_pidWom.Calculate(-_config.alphaArmGearbox.encoder->GetEncoderPosition(), dt, 0.0_V);
+            //   _table->GetEntry("Demand").SetDouble(_setAlphaArmVoltage.value());
+            // } else if (_config.alphaArmGearbox.encoder->GetEncoderPosition() < 0_rad) {
+            //   _pidWom.SetSetpoint(_encoderSetpoint);
+            //   _setAlphaArmVoltage =
+            //   _pidWom.Calculate(_config.alphaArmGearbox.encoder->GetEncoderPosition(), dt, 0.0_V);
+            //   _table->GetEntry("Demand").SetDouble(_setAlphaArmVoltage.value());
+            _setAlphaArmVoltage = 0_V;
+          } else {
+            _pidWom.SetSetpoint(_encoderSetpoint);
+            _setAlphaArmVoltage =
+                -_pidWom.Calculate(-_config.alphaArmGearbox.encoder->GetEncoderPosition(), dt, 0.0_V);
+            //   _pidWom.Reset();
+            //   _encoderSetpoint = _config.alphaArmGearbox.encoder->GetEncoderPosition();
+            //   _setAlphaArmVoltage = _controlledRawVoltage;
           }
-            break;
-          case AlphaArmState::kSpeakerAngle:
-            _stateName = "Speaker Angle";
-            //_pidWom.SetSetpoint(_goal);
-           // _setAlphaArmVoltage = _pidWom.Calculate(_config.alphaArmGearbox.encoder->GetEncoderPosition(), dt, 0_V);
-              break;
-          case AlphaArmState::kStowed:
-            _stateName = "Stowed";
-            //_pidWom.SetSetpoint(_goal);
-            //_setAlphaArmVoltage = _pidWom.Calculate(_config.alphaArmGearbox.encoder->GetEncoderPosition(), dt, 0_V);
-          break;
-          default:
-            _stateName = "Error";
-            std::cout << "oops, wrong state" << std::endl;
-          break;
+        } else {
+          _pidWom.Reset();
+          _encoderSetpoint = -_config.alphaArmGearbox.encoder->GetEncoderPosition();
+          _setAlphaArmVoltage = _controlledRawVoltage;
+        }
+      } else {
+        _pidWom.Reset();
+        _encoderSetpoint = -_config.alphaArmGearbox.encoder->GetEncoderPosition();
+        _setAlphaArmVoltage = _controlledRawVoltage;
+
+        if (std::abs(_controlledRawVoltage.value()) > 0) {
+          _startingPos = -_config.alphaArmGearbox.encoder->GetEncoderPosition();
+          started = true;
         }
-        std::cout << " ARM POSITION: " << _config.alphaArmGearbox.encoder->GetEncoderPosition().value() << std::endl;
-        //std::cout << "OUTPUT VOLTAGE: " << _setAlphaArmVoltage.value() << std::endl;
-        _config.alphaArmGearbox.motorController->SetVoltage(_setAlphaArmVoltage);
+      }
+    } break;
+    case AlphaArmState::kSpeakerAngle:
+      _stateName = "Speaker Angle";
+      // _pidWom.SetSetpoint(_goal);
+      // _setAlphaArmVoltage = _pidWom.Calculate(_config.alphaArmGearbox.encoder->GetEncoderPosition(), dt,
+      // 0_V);
+      break;
+    case AlphaArmState::kStowed:
+      _stateName = "Stowed";
+      // _pidWom.SetSetpoint(_goal);
+      // _setAlphaArmVoltage = _pidWom.Calculate(_config.alphaArmGearbox.encoder->GetEncoderPosition(), dt,
+      // 0_V);
+      break;
+    default:
+      _stateName = "Error";
+      std::cout << "oops, wrong state" << std::endl;
+      break;
+  }
+  std::cout << " ARM POSITION: " << _config.alphaArmGearbox.encoder->GetEncoderPosition().value()
+            << std::endl;
+  // std::cout << "OUTPUT VOLTAGE: " << _setAlphaArmVoltage.value() << std::endl;
+  _config.alphaArmGearbox.motorController->SetVoltage(_setAlphaArmVoltage);
 }
 
 void AlphaArm::SetState(AlphaArmState state) {
   _state = state;
 }
 
-void AlphaArm::SetGoal(units::radian_t goal){
+void AlphaArm::SetGoal(units::radian_t goal) {
   _goal = goal;
 }
 
-void AlphaArm::SetArmRaw(units::volt_t voltage){
+void AlphaArm::SetArmRaw(units::volt_t voltage) {
   std::cout << "VOLTAGE: " << voltage.value() << std::endl;
   _rawArmVoltage = voltage;
 }
diff --git a/src/main/cpp/AlphaArmBehaviour.cpp b/src/main/cpp/AlphaArmBehaviour.cpp
index d3411a98..9a28b311 100644
--- a/src/main/cpp/AlphaArmBehaviour.cpp
+++ b/src/main/cpp/AlphaArmBehaviour.cpp
@@ -12,7 +12,6 @@ AlphaArmManualControl::AlphaArmManualControl(AlphaArm* alphaArm, frc::XboxContro
 }
 
 void AlphaArmManualControl::OnTick(units::second_t dt) {
-
   if (_codriver->GetStartButtonPressed()) {
     if (_rawControl == true) {
       _rawControl = false;
@@ -23,7 +22,7 @@ void AlphaArmManualControl::OnTick(units::second_t dt) {
 
   if (_rawControl) {
     _alphaArm->SetState(AlphaArmState::kRaw);
-    _alphaArm->SetArmRaw(_codriver->GetRightY() * 12_V);  
+    _alphaArm->SetArmRaw(_codriver->GetRightY() * 12_V);
   } else {
     _alphaArm->SetState(AlphaArmState::kAmpAngle);
     _alphaArm->setControllerRaw(wom::deadzone(_codriver->GetRightY()) * 12_V);
diff --git a/src/main/cpp/Intake.cpp b/src/main/cpp/Intake.cpp
index a96c289b..6110aa7e 100644
--- a/src/main/cpp/Intake.cpp
+++ b/src/main/cpp/Intake.cpp
@@ -78,4 +78,4 @@ void Intake::setRaw(units::volt_t voltage) {
 }
 IntakeState Intake::getState() {
   return _state;
-}
\ No newline at end of file
+}
diff --git a/src/main/cpp/IntakeBehaviour.cpp b/src/main/cpp/IntakeBehaviour.cpp
index 3192ef12..3d738fb3 100644
--- a/src/main/cpp/IntakeBehaviour.cpp
+++ b/src/main/cpp/IntakeBehaviour.cpp
@@ -60,4 +60,4 @@ IntakeAutoControl::IntakeAutoControl(Intake* intake) : _intake(intake) {
   Controls(intake);
 }
 
-void IntakeAutoControl::OnTick(units::second_t dt) {}
\ No newline at end of file
+void IntakeAutoControl::OnTick(units::second_t dt) {}
diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp
index 25e05917..a1ba628b 100644
--- a/src/main/cpp/Robot.cpp
+++ b/src/main/cpp/Robot.cpp
@@ -49,7 +49,6 @@ void Robot::RobotInit() {
   // shooter->SetDefaultBehaviour(
   //     [this]() { return wom::make<ShooterManualControl>(shooter, &robotmap.controllers.codriver); });
 
-
   // frc::SmartDashboard::PutData("Path Selector", &m_path_chooser);
   // frc::SmartDashboard::PutData("Path Selector", &m_path_chooser);
 
@@ -59,20 +58,16 @@ void Robot::RobotInit() {
 
   // robotmap.swerveBase.gyro->Reset();
 
-  _swerveDrive =
-      new wom::SwerveDrive(robotmap.swerveBase.config, frc::Pose2d());
+  _swerveDrive = new wom::SwerveDrive(robotmap.swerveBase.config, frc::Pose2d());
   wom::BehaviourScheduler::GetInstance()->Register(_swerveDrive);
-  _swerveDrive->SetDefaultBehaviour([this]() {
-    return wom::make<wom::ManualDrivebase>(_swerveDrive,
-                                           &robotmap.controllers.driver);
-  });
-
+  _swerveDrive->SetDefaultBehaviour(
+      [this]() { return wom::make<wom::ManualDrivebase>(_swerveDrive, &robotmap.controllers.driver); });
 
   // alphaArm = new AlphaArm(robotmap.alphaArmSystem.config);
   // wom::BehaviourScheduler::GetInstance()->Register(alphaArm);
   // alphaArm->SetDefaultBehaviour(
   //     [this]() { return wom::make<AlphaArmManualControl>(alphaArm, &robotmap.controllers.codriver); });
-  
+
   // // m_driveSim = new wom::TempSimSwerveDrive(&simulation_timer, &m_field);
   // // m_driveSim = wom::TempSimSwerveDrive();
 
@@ -97,21 +92,18 @@ void Robot::RobotInit() {
   // robotmap.swerveBase.moduleConfigs[0].driveMotor.motorController->SetInverted(true);
   // robotmap.swerveBase.moduleConfigs[2].driveMotor.motorController->SetInverted(true);
 
-  //robotmap.alphaArmSystem.armEncoder->Reset();
-  
-      
+  // robotmap.alphaArmSystem.armEncoder->Reset();
+
   // robotmap.swerveBase.moduleConfigs[0].turnMotor.encoder->SetEncoderOffset(0_rad);
   // robotmap.swerveBase.moduleConfigs[1].turnMotor.encoder->SetEncoderOffset(0_rad);
   // robotmap.swerveBase.moduleConfigs[2].turnMotor.encoder->SetEncoderOffset(0_rad);
   // robotmap.swerveBase.moduleConfigs[3].turnMotor.encoder->SetEncoderOffset(0_rad);
 
   lastPeriodic = wom::now();
-
-
 }
 
 void Robot::RobotPeriodic() {
-  //double encoderDistance = robotmap.alphaArmSystem.armEncoder.GetDistance();
+  // double encoderDistance = robotmap.alphaArmSystem.armEncoder.GetDistance();
   auto dt = wom::now() - lastPeriodic;
   lastPeriodic = wom::now();
 
@@ -119,16 +111,19 @@ void Robot::RobotPeriodic() {
   wom::BehaviourScheduler::GetInstance()->Tick();
   sched->Tick();
 
-  robotmap.swerveTable.swerveDriveTable->GetEntry("Encoder 0 offset: ").SetDouble(robotmap.swerveBase.moduleConfigs[0].turnMotor.encoder->GetEncoderPosition().value());
-  robotmap.swerveTable.swerveDriveTable->GetEntry("Encoder 1 offset: ").SetDouble(robotmap.swerveBase.moduleConfigs[1].turnMotor.encoder->GetEncoderPosition().value());
-  robotmap.swerveTable.swerveDriveTable->GetEntry("Encoder 2 offset: ").SetDouble(robotmap.swerveBase.moduleConfigs[2].turnMotor.encoder->GetEncoderPosition().value());
-  robotmap.swerveTable.swerveDriveTable->GetEntry("Encoder 3 offset: ").SetDouble(robotmap.swerveBase.moduleConfigs[3].turnMotor.encoder->GetEncoderPosition().value());
+  robotmap.swerveTable.swerveDriveTable->GetEntry("Encoder 0 offset: ")
+      .SetDouble(robotmap.swerveBase.moduleConfigs[0].turnMotor.encoder->GetEncoderPosition().value());
+  robotmap.swerveTable.swerveDriveTable->GetEntry("Encoder 1 offset: ")
+      .SetDouble(robotmap.swerveBase.moduleConfigs[1].turnMotor.encoder->GetEncoderPosition().value());
+  robotmap.swerveTable.swerveDriveTable->GetEntry("Encoder 2 offset: ")
+      .SetDouble(robotmap.swerveBase.moduleConfigs[2].turnMotor.encoder->GetEncoderPosition().value());
+  robotmap.swerveTable.swerveDriveTable->GetEntry("Encoder 3 offset: ")
+      .SetDouble(robotmap.swerveBase.moduleConfigs[3].turnMotor.encoder->GetEncoderPosition().value());
 
   // shooter->OnUpdate(dt);
   // intake->OnUpdate(dt);
   // alphaArm->OnUpdate(dt);
   _swerveDrive->OnUpdate(dt);
-  
 }
 
 void Robot::AutonomousInit() {
diff --git a/src/main/cpp/Shooter.cpp b/src/main/cpp/Shooter.cpp
index c6c40a02..44326bda 100644
--- a/src/main/cpp/Shooter.cpp
+++ b/src/main/cpp/Shooter.cpp
@@ -1,22 +1,22 @@
 // Copyright (c) 2023-2024 CurtinFRC
 // Open Source Software, you can modify it according to the terms
 // of the MIT License at the root of this project
-#include "Shooter.h"
 
+#include "Shooter.h"
 
-Shooter::Shooter(ShooterConfig config) : _config(config), _pid(config.path + "/pid", config.pidConfig) {} 
+Shooter::Shooter(ShooterConfig config) : _config(config), _pid(config.path + "/pid", config.pidConfig) {}
 
 void Shooter::OnStart() {
   _pid.Reset();
 }
 
-
 void Shooter::OnUpdate(units::second_t dt) {
   // _pid.SetTolerance(0.5, 4);
   table->GetEntry("Error").SetDouble(_pid.GetError().value());
   // table->GetEntry("Acceleration Error").SetDouble(_pid.GetVelocityError());
   table->GetEntry("SetPoint").SetDouble(_pid.GetSetpoint().value());
-  // table->GetEntry("Current Pos").SetDouble(_config.ShooterGearbox.encoder->GetEncoderAngularVelocity().value());
+  // table->GetEntry("Current
+  // Pos").SetDouble(_config.ShooterGearbox.encoder->GetEncoderAngularVelocity().value());
   // table->GetEntry("EncoderValue").SetDouble(_config.ShooterGearbox.encoder->GetVelocityValue());
   table->GetEntry("Shooting").SetString(_statename);
   switch (_state) {
@@ -29,13 +29,13 @@ void Shooter::OnUpdate(units::second_t dt) {
       // if (_shooterSensor.Get()) {
       //   _state = ShooterState::kReverse;
       // }
-      
     } break;
     case ShooterState::kSpinUp: {
       _statename = "SpinUp";
       std::cout << "KSpinUp" << std::endl;
       _pid.SetSetpoint(_goal);
-      units::volt_t pidCalculate = units::volt_t {_pid.Calculate(_config.ShooterGearbox.encoder->GetEncoderAngularVelocity(), dt, 0_V)};
+      units::volt_t pidCalculate =
+          units::volt_t{_pid.Calculate(_config.ShooterGearbox.encoder->GetEncoderAngularVelocity(), dt, 0_V)};
       std::cout << "KShooting" << std::endl;
 
       if (_pid.IsStable()) {
@@ -49,13 +49,13 @@ void Shooter::OnUpdate(units::second_t dt) {
       } else {
         _setVoltage = holdVoltage;
       }
-
     } break;
     case ShooterState::kShooting: {
       _statename = "Shooting";
-      
+
       _pid.SetSetpoint(_goal);
-      units::volt_t pidCalculate = units::volt_t {_pid.Calculate(_config.ShooterGearbox.encoder->GetEncoderAngularVelocity(), dt, 0_V)};
+      units::volt_t pidCalculate =
+          units::volt_t{_pid.Calculate(_config.ShooterGearbox.encoder->GetEncoderAngularVelocity(), dt, 0_V)};
       std::cout << "KShooting" << std::endl;
 
       if (_pid.IsStable()) {
@@ -68,7 +68,6 @@ void Shooter::OnUpdate(units::second_t dt) {
       } else {
         _setVoltage = holdVoltage;
       }
-      
     } break;
 
     case ShooterState::kReverse: {
@@ -95,8 +94,8 @@ void Shooter::OnUpdate(units::second_t dt) {
     } break;
   }
   // table->GetEntry("Motor OutPut").SetDouble(_setVoltage.value());
-  table->GetEntry("Encoder Output").SetDouble(_config.ShooterGearbox.encoder->GetEncoderAngularVelocity().value());
-
+  table->GetEntry("Encoder Output")
+      .SetDouble(_config.ShooterGearbox.encoder->GetEncoderAngularVelocity().value());
 
   _config.ShooterGearbox.motorController->SetVoltage(_setVoltage);
 }
@@ -116,6 +115,6 @@ void Shooter::SetRaw(units::volt_t voltage) {
   _rawVoltage = voltage;
   _state = ShooterState::kRaw;
 }
-void Shooter::SetPidGoal(units::radians_per_second_t goal) { 
+void Shooter::SetPidGoal(units::radians_per_second_t goal) {
   _goal = goal;
 }
diff --git a/src/main/cpp/ShooterBehaviour.cpp b/src/main/cpp/ShooterBehaviour.cpp
index 7b370456..09a1d133 100644
--- a/src/main/cpp/ShooterBehaviour.cpp
+++ b/src/main/cpp/ShooterBehaviour.cpp
@@ -36,7 +36,7 @@ void ShooterManualControl::OnTick(units::second_t dt) {
     } else if (_codriver->GetYButton()) {
       _shooter->SetPidGoal(300_rad_per_s);
       _shooter->SetState(ShooterState::kSpinUp);
-    }  else if (_codriver->GetLeftTriggerAxis() > 0.1) {
+    } else if (_codriver->GetLeftTriggerAxis() > 0.1) {
       _shooter->SetState(ShooterState::kRaw);
       _shooter->SetRaw(12_V * _codriver->GetLeftTriggerAxis());
     } else if (_codriver->GetRightTriggerAxis() > 0.1) {
diff --git a/src/main/include/AlphaArm.h b/src/main/include/AlphaArm.h
index 9a5e94dc..db0bc3e6 100644
--- a/src/main/include/AlphaArm.h
+++ b/src/main/include/AlphaArm.h
@@ -7,72 +7,75 @@
 #include <frc/DutyCycleEncoder.h>
 #include <frc/controller/PIDController.h>
 #include <units/angular_velocity.h>
+
+#include <memory>
+#include <string>
+
 #include "Wombat.h"
 #include "utils/PID.h"
 
 struct AlphaArmConfig {
-    wom::Gearbox alphaArmGearbox;
-    //wom::Gearbox wristGearbox;
-    //wom::DutyCycleEncoder* armEncoder;
-    //wom::CANSparkMaxEncoder* armEncoder;
-    wom::utils::PIDConfig<units::radian, units::volt> pidConfigA;
-    //wom::utils::PIDConfig<units::radians_per_second, units::volt> velocityConfig;
-    std::string path;
-    //void WriteNT(std::shared_ptr<nt::NetworkTable> table);
-
+  wom::Gearbox alphaArmGearbox;
+  // wom::Gearbox wristGearbox;
+  // wom::DutyCycleEncoder* armEncoder;
+  // wom::CANSparkMaxEncoder* armEncoder;
+  wom::utils::PIDConfig<units::radian, units::volt> pidConfigA;
+  // wom::utils::PIDConfig<units::radians_per_second, units::volt> velocityConfig;
+  std::string path;
+  // void WriteNT(std::shared_ptr<nt::NetworkTable> table);
 };
 
 enum class AlphaArmState {
-    kIdle,
-    kIntakeAngle,
-    kAmpAngle,
-    kSpeakerAngle,
-    kStowed,
-    kRaw
-    //kForwardWrist,
-    //kReverseWrist,
+  kIdle,
+  kIntakeAngle,
+  kAmpAngle,
+  kSpeakerAngle,
+  kStowed,
+  kRaw
+  // kForwardWrist,
+  // kReverseWrist,
 };
 
-class AlphaArm : public::behaviour::HasBehaviour{
-    public:
-     explicit AlphaArm(AlphaArmConfig config);
+class AlphaArm : public ::behaviour::HasBehaviour {
+ public:
+  explicit AlphaArm(AlphaArmConfig config);
+
+  void OnStart();
+  void OnUpdate(units::second_t dt);
+  void SetArmRaw(units::volt_t voltage);
+  void SetState(AlphaArmState state);
+  void setControllerRaw(units::volt_t);
+  // void setGoal(units::radians_per_second_t);
+  void SetGoal(units::radian_t);
+  double GetArmEncoder();
+  AlphaArmConfig GetConfig();
+  std::shared_ptr<nt::NetworkTable> table = nt::NetworkTableInstance::GetDefault().GetTable("AlphaArm");
+  // units::radians_per_second_t goal;
+  // double goal;
 
-    void OnStart();
-    void OnUpdate(units::second_t dt);
-    void SetArmRaw(units::volt_t voltage);
-    void SetState(AlphaArmState state);
-    void setControllerRaw(units::volt_t);
-    //void setGoal(units::radians_per_second_t);
-    void SetGoal(units::radian_t);
-    double GetArmEncoder();
-    AlphaArmConfig GetConfig();
-    std::shared_ptr<nt::NetworkTable> table = nt::NetworkTableInstance::GetDefault().GetTable("AlphaArm");
-    //units::radians_per_second_t goal;
-    //double goal;
-    
-    //frc::DutyCycleEncoder armEncoder{12};
-    //void SetRaw(units::volt_t voltage);
+  // frc::DutyCycleEncoder armEncoder{12};
+  // void SetRaw(units::volt_t voltage);
 
-    private:
-    //frc::PIDController _pidFRC;
-    wom::utils::PIDController<units::radian, units::volt> _pidWom;
-   // wom::utils::PIDController<units::radians_per_second, units::volt> _velocityPID;
+ private:
+  // frc::PIDController _pidFRC;
+  wom::utils::PIDController<units::radian, units::volt> _pidWom;
+  // wom::utils::PIDController<units::radians_per_second, units::volt> _velocityPID;
 
-    std::shared_ptr<nt::NetworkTable> _table = nt::NetworkTableInstance::GetDefault().GetTable("AlphaArm");
-    AlphaArmConfig _config;
-    AlphaArmState _state = AlphaArmState::kIdle;
-    units::radian_t _goal; 
-    units::volt_t _setAlphaArmVoltage = 0_V;
-    units::volt_t _setWristVoltage = 0_V;
+  std::shared_ptr<nt::NetworkTable> _table = nt::NetworkTableInstance::GetDefault().GetTable("AlphaArm");
+  AlphaArmConfig _config;
+  AlphaArmState _state = AlphaArmState::kIdle;
+  units::radian_t _goal;
+  units::volt_t _setAlphaArmVoltage = 0_V;
+  units::volt_t _setWristVoltage = 0_V;
 
-    units::volt_t _rawArmVoltage = 0_V;
-    units::volt_t _rawWristVoltage = 0_V;
-    //units::radiant_t maxAngle = 1_radian_t;
+  units::volt_t _rawArmVoltage = 0_V;
+  units::volt_t _rawWristVoltage = 0_V;
+  // units::radiant_t maxAngle = 1_radian_t;
 
-    units::radian_t _encoderSetpoint = 0_rad;
-    units::volt_t _controlledRawVoltage = 0_V;
+  units::radian_t _encoderSetpoint = 0_rad;
+  units::volt_t _controlledRawVoltage = 0_V;
   units::radian_t _startingPos = 0_rad;
 
-    std::string _stateName = "Default";
-    bool started = false;
+  std::string _stateName = "Default";
+  bool started = false;
 };
diff --git a/src/main/include/Intake.h b/src/main/include/Intake.h
index 1aca262a..8d2a07d3 100644
--- a/src/main/include/Intake.h
+++ b/src/main/include/Intake.h
@@ -40,4 +40,4 @@ class Intake : public behaviour::HasBehaviour {
   units::volt_t _setVoltage = 0_V;
 
   std::shared_ptr<nt::NetworkTable> _table = nt::NetworkTableInstance::GetDefault().GetTable("Intake");
-};
\ No newline at end of file
+};
diff --git a/src/main/include/IntakeBehaviour.h b/src/main/include/IntakeBehaviour.h
index 5dbe8ae6..5233c3c2 100644
--- a/src/main/include/IntakeBehaviour.h
+++ b/src/main/include/IntakeBehaviour.h
@@ -31,4 +31,4 @@ class IntakeAutoControl : public behaviour::Behaviour {
 
  private:
   Intake* _intake;
-};
\ No newline at end of file
+};
diff --git a/src/main/include/Robot.h b/src/main/include/Robot.h
index 799ae2c1..a8bc9ba2 100644
--- a/src/main/include/Robot.h
+++ b/src/main/include/Robot.h
@@ -22,7 +22,6 @@
 #include "RobotMap.h"
 #include "Shooter.h"
 #include "ShooterBehaviour.h"
-#include "RobotMap.h"
 #include "Wombat.h"
 
 class Robot : public frc::TimedRobot {
@@ -56,7 +55,7 @@ class Robot : public frc::TimedRobot {
   wom::SwerveDrive* _swerveDrive;
   // AlphaArm* alphaArm;
 
-  ctre::phoenix6::hardware::TalonFX *frontLeft;
+  ctre::phoenix6::hardware::TalonFX* frontLeft;
   // AlphaArm *alphaArm;
 
   // ctre::phoenix6::hardware::TalonFX *frontLeft;
diff --git a/src/main/include/RobotMap.h b/src/main/include/RobotMap.h
index b58e3d02..2c80b462 100644
--- a/src/main/include/RobotMap.h
+++ b/src/main/include/RobotMap.h
@@ -6,15 +6,14 @@
 
 #include <frc/Compressor.h>
 #include <frc/DoubleSolenoid.h>
+#include <frc/DutyCycleEncoder.h>
+#include <frc/Encoder.h>
 #include <frc/XboxController.h>
 #include <frc/system/plant/DCMotor.h>
-#include <frc/Encoder.h>
-#include <frc/DutyCycleEncoder.h>
 #include <units/angle.h>
 #include <units/length.h>
-#include "utils/PID.h"
-#include "utils/Encoder.h"
 
+#include <memory>
 #include <string>
 
 #include <ctre/phoenix6/CANcoder.hpp>
@@ -26,11 +25,8 @@
 #include "Intake.h"
 #include "Shooter.h"
 #include "Wombat.h"
-#include "Wombat.h"
-#include "AlphaArm.h"
-#include "AlphaArmBehaviour.h"
-
-//#include "Wombat/Encoder.h"
+#include "utils/Encoder.h"
+#include "utils/PID.h"
 
 struct RobotMap {
   struct Controllers {
@@ -40,80 +36,77 @@ struct RobotMap {
   };
   Controllers controllers;
 
-//   struct AlphaArmSystem {
-//     rev::CANSparkMax alphaArmMotor{12, rev::CANSparkMax::MotorType::kBrushless};
-//     wom::CANSparkMaxEncoder* armEncoder = new wom::CANSparkMaxEncoder(&alphaArmMotor, 0.02_m);
-    
-//     wom::Gearbox alphaArmGearbox{&alphaArmMotor, armEncoder, frc::DCMotor::NEO(1)};
-    
-//     wom::utils::PIDConfig<units::radian, units::volt> pidConfigA{
-//      "/path/to/pid/in/nt/tables",
-//     15_V / 180_deg, 
-//     0_V / (1_deg * 1_s), 
-//     0_V / (1_deg / 1_s),
-//     };
-
-//     AlphaArmConfig config {
-//         alphaArmGearbox, 
-//         pidConfigA,
-//     };
-
-//   };
-//   AlphaArmSystem alphaArmSystem;
-  
-//   struct IntakeSystem {
-//     rev::CANSparkMax intakeMotor{2, rev::CANSparkMax::MotorType::kBrushed};
-//     // wom::CANSparkMaxEncoder intakeEncoder{&intakeMotor, 0.1_m};
-//     frc::DigitalInput intakeSensor{4};
-//     // frc::DigitalInput magSensor{0};
-//     // frc::DigitalInput shooterSensor{0};
-
-//     wom::Gearbox IntakeGearbox{&intakeMotor, nullptr, frc::DCMotor::CIM(1)};
-
-//     IntakeConfig config{IntakeGearbox, &intakeSensor /*, &magSensor, &shooterSensor*/};
-//   };
-//   IntakeSystem intakeSystem;
-
-//     struct Shooter {
-//     rev::CANSparkMax shooterMotor{11, rev::CANSparkMax::MotorType::kBrushless};// Port 11
-//     // frc::DigitalInput shooterSensor{2};
-
-//     // wom::VoltageController shooterMotorGroup = wom::VoltagedController::Group(shooterMotor);
-//     wom::CANSparkMaxEncoder* shooterEncoder = new wom::CANSparkMaxEncoder(&shooterMotor, 0.01_m);
-//     wom::Gearbox shooterGearbox{&shooterMotor, shooterEncoder, frc::DCMotor::NEO(1)};
-
-//     wom::utils::PIDConfig<units::radians_per_second, units::volts> pidConfigS{
-//           "/armavator/arm/velocityPID/config",
-//           0.1_V / (360_deg / 1_s),
-//           0.03_V / 25_deg,
-//           0.001_V / (90_deg / 1_s / 1_s),
-//           5_rad_per_s,
-//           10_rad_per_s / 1_s
-//     };
-
-//     ShooterConfig config{
-//         "shooterGearbox",
-//         shooterGearbox,
-//         pidConfigS
-//     };
-
-    
-
-//   };
-//   Shooter shooterSystem;
- 
+  //   struct AlphaArmSystem {
+  //     rev::CANSparkMax alphaArmMotor{12, rev::CANSparkMax::MotorType::kBrushless};
+  //     wom::CANSparkMaxEncoder* armEncoder = new wom::CANSparkMaxEncoder(&alphaArmMotor, 0.02_m);
+
+  //     wom::Gearbox alphaArmGearbox{&alphaArmMotor, armEncoder, frc::DCMotor::NEO(1)};
+
+  //     wom::utils::PIDConfig<units::radian, units::volt> pidConfigA{
+  //      "/path/to/pid/in/nt/tables",
+  //     15_V / 180_deg,
+  //     0_V / (1_deg * 1_s),
+  //     0_V / (1_deg / 1_s),
+  //     };
+
+  //     AlphaArmConfig config {
+  //         alphaArmGearbox,
+  //         pidConfigA,
+  //     };
+
+  //   };
+  //   AlphaArmSystem alphaArmSystem;
+
+  //   struct IntakeSystem {
+  //     rev::CANSparkMax intakeMotor{2, rev::CANSparkMax::MotorType::kBrushed};
+  //     // wom::CANSparkMaxEncoder intakeEncoder{&intakeMotor, 0.1_m};
+  //     frc::DigitalInput intakeSensor{4};
+  //     // frc::DigitalInput magSensor{0};
+  //     // frc::DigitalInput shooterSensor{0};
+
+  //     wom::Gearbox IntakeGearbox{&intakeMotor, nullptr, frc::DCMotor::CIM(1)};
+
+  //     IntakeConfig config{IntakeGearbox, &intakeSensor /*, &magSensor, &shooterSensor*/};
+  //   };
+  //   IntakeSystem intakeSystem;
+
+  //     struct Shooter {
+  //     rev::CANSparkMax shooterMotor{11, rev::CANSparkMax::MotorType::kBrushless};// Port 11
+  //     // frc::DigitalInput shooterSensor{2};
+
+  //     // wom::VoltageController shooterMotorGroup = wom::VoltagedController::Group(shooterMotor);
+  //     wom::CANSparkMaxEncoder* shooterEncoder = new wom::CANSparkMaxEncoder(&shooterMotor, 0.01_m);
+  //     wom::Gearbox shooterGearbox{&shooterMotor, shooterEncoder, frc::DCMotor::NEO(1)};
+
+  //     wom::utils::PIDConfig<units::radians_per_second, units::volts> pidConfigS{
+  //           "/armavator/arm/velocityPID/config",
+  //           0.1_V / (360_deg / 1_s),
+  //           0.03_V / 25_deg,
+  //           0.001_V / (90_deg / 1_s / 1_s),
+  //           5_rad_per_s,
+  //           10_rad_per_s / 1_s
+  //     };
+
+  //     ShooterConfig config{
+  //         "shooterGearbox",
+  //         shooterGearbox,
+  //         pidConfigS
+  //     };
+
+  //   };
+  //   Shooter shooterSystem;
+
   struct SwerveBase {
     ctre::phoenix6::hardware::CANcoder frontLeftCancoder{16, "Drivebase"};
     ctre::phoenix6::hardware::CANcoder frontRightCancoder{18, "Drivebase"};
     ctre::phoenix6::hardware::CANcoder backLeftCancoder{17, "Drivebase"};
     ctre::phoenix6::hardware::CANcoder backRightCancoder{19, "Drivebase"};
 
-    ctre::phoenix6::hardware::Pigeon2* gyro =
-        new ctre::phoenix6::hardware::Pigeon2(20, "Drivebase");
+    ctre::phoenix6::hardware::Pigeon2* gyro = new ctre::phoenix6::hardware::Pigeon2(20, "Drivebase");
     wpi::array<ctre::phoenix6::hardware::TalonFX*, 4> turnMotors{
         new ctre::phoenix6::hardware::TalonFX(6, "Drivebase"),   // front left
         new ctre::phoenix6::hardware::TalonFX(7, "Drivebase"),   // front right
-        new ctre::phoenix6::hardware::TalonFX(4, "Drivebase"),   // back left 
+        new ctre::phoenix6::hardware::TalonFX(4, "Drivebase"),   // back left
         new ctre::phoenix6::hardware::TalonFX(2, "Drivebase")};  // back right
     wpi::array<ctre::phoenix6::hardware::TalonFX*, 4> driveMotors{
         new ctre::phoenix6::hardware::TalonFX(5, "Drivebase"),   // front left
@@ -122,48 +115,38 @@ struct RobotMap {
         new ctre::phoenix6::hardware::TalonFX(1, "Drivebase")};  // back right
 
     wpi::array<wom::SwerveModuleConfig, 4> moduleConfigs{
-        wom::SwerveModuleConfig{ //CORRECT
+        wom::SwerveModuleConfig{
+            // CORRECT
             // front left module
             frc::Translation2d(-10_in, 9_in),
-            wom::Gearbox{
-                driveMotors[0],
-                new wom::TalonFXEncoder(driveMotors[0], 0.0445_m, 6.75),
-                frc::DCMotor::Falcon500(1).WithReduction(6.75)},
-            wom::Gearbox{turnMotors[0],
-                         new wom::CanEncoder(16, 0.0445_m, 4096, 12.8),
+            wom::Gearbox{driveMotors[0], new wom::TalonFXEncoder(driveMotors[0], 0.0445_m, 6.75),
+                         frc::DCMotor::Falcon500(1).WithReduction(6.75)},
+            wom::Gearbox{turnMotors[0], new wom::CanEncoder(16, 0.0445_m, 4096, 12.8),
                          frc::DCMotor::Falcon500(1).WithReduction(12.8)},
             &frontLeftCancoder, 4_in / 2},
-        wom::SwerveModuleConfig{ //CORRECT
+        wom::SwerveModuleConfig{
+            // CORRECT
             // front right module
             frc::Translation2d(10_in, 9_in),
-            wom::Gearbox{
-                driveMotors[1],
-                new wom::TalonFXEncoder(driveMotors[1], 0.0445_m, 6.75),
-                frc::DCMotor::Falcon500(1).WithReduction(6.75)},
-            wom::Gearbox{turnMotors[1],
-                         new wom::CanEncoder(18, 0.0445_m, 4096, 12.8),
+            wom::Gearbox{driveMotors[1], new wom::TalonFXEncoder(driveMotors[1], 0.0445_m, 6.75),
+                         frc::DCMotor::Falcon500(1).WithReduction(6.75)},
+            wom::Gearbox{turnMotors[1], new wom::CanEncoder(18, 0.0445_m, 4096, 12.8),
                          frc::DCMotor::Falcon500(1).WithReduction(12.8)},
             &frontRightCancoder, 4_in / 2},
         wom::SwerveModuleConfig{
             // back left module
             frc::Translation2d(-10_in, 9_in),
-            wom::Gearbox{
-                driveMotors[2],
-                new wom::TalonFXEncoder(driveMotors[2], 0.0445_m, 6.75),
-                frc::DCMotor::Falcon500(1).WithReduction(6.75)},
-            wom::Gearbox{turnMotors[2],
-                         new wom::CanEncoder(17, 0.0445_m, 4096, 12.8),
+            wom::Gearbox{driveMotors[2], new wom::TalonFXEncoder(driveMotors[2], 0.0445_m, 6.75),
+                         frc::DCMotor::Falcon500(1).WithReduction(6.75)},
+            wom::Gearbox{turnMotors[2], new wom::CanEncoder(17, 0.0445_m, 4096, 12.8),
                          frc::DCMotor::Falcon500(1).WithReduction(12.8)},
             &backRightCancoder, 4_in / 2},
         wom::SwerveModuleConfig{
             // back right module
             frc::Translation2d(-10_in, -9_in),
-            wom::Gearbox{
-                driveMotors[3],
-                new wom::TalonFXEncoder(driveMotors[3], 0.0445_m, 6.75),
-                frc::DCMotor::Falcon500(1).WithReduction(6.75)},
-            wom::Gearbox{turnMotors[3],
-                         new wom::CanEncoder(19, 0.0445_m, 4096, 12.8),
+            wom::Gearbox{driveMotors[3], new wom::TalonFXEncoder(driveMotors[3], 0.0445_m, 6.75),
+                         frc::DCMotor::Falcon500(1).WithReduction(6.75)},
+            wom::Gearbox{turnMotors[3], new wom::CanEncoder(19, 0.0445_m, 4096, 12.8),
                          frc::DCMotor::Falcon500(1).WithReduction(12.8)},
             &backLeftCancoder, 4_in / 2},
     };
@@ -183,19 +166,16 @@ struct RobotMap {
         wom::SwerveDriveConfig::pose_angle_conf_t::ki_t{0},
         0_deg / 1_deg};*/
     wom::SwerveDriveConfig::pose_position_conf_t posePositionPID{
-        "/drivetrain/pid/pose/position/config",
-        0_mps / 1_m,
-        wom::SwerveDriveConfig::pose_position_conf_t::ki_t{0.15},
-        0_m / 1_m,
-        0_cm};
+        "/drivetrain/pid/pose/position/config", 0_mps / 1_m,
+        wom::SwerveDriveConfig::pose_position_conf_t::ki_t{0.15}, 0_m / 1_m, 0_cm};
 
     // the config for the whole swerve drive
     wom::SwerveDriveConfig config{"/drivetrain",
-                                  //anglePID,
+                                  // anglePID,
                                   velocityPID,
                                   moduleConfigs,  // each module
                                   gyro,
-                                  //poseAnglePID,
+                                  // poseAnglePID,
                                   posePositionPID,
                                   60_kg,  // robot mass (estimate rn)
                                   {0.1, 0.1, 0.1},
diff --git a/src/main/include/Shooter.h b/src/main/include/Shooter.h
index 965b3654..566e30be 100644
--- a/src/main/include/Shooter.h
+++ b/src/main/include/Shooter.h
@@ -1,6 +1,7 @@
 // Copyright (c) 2023-2024 CurtinFRC
 // Open Source Software, you can modify it according to the terms
 // of the MIT License at the root of this project
+
 #pragma once
 #include <frc/DigitalInput.h>
 #include <frc/controller/PIDController.h>
@@ -11,7 +12,6 @@
 
 #include "Wombat.h"
 
-
 struct ShooterConfig {
   std::string path;
   wom::Gearbox ShooterGearbox;
diff --git a/src/main/include/ShooterBehaviour.h b/src/main/include/ShooterBehaviour.h
index 41e598df..12f93baa 100644
--- a/src/main/include/ShooterBehaviour.h
+++ b/src/main/include/ShooterBehaviour.h
@@ -6,13 +6,14 @@
 
 #include <frc/XboxController.h>
 
-#include "Robot.h"
+#include <memory>
+
 #include "Shooter.h"
 #include "Wombat.h"
 
 class ShooterManualControl : public behaviour::Behaviour {
  public:
-  ShooterManualControl(Shooter* shooter, frc::XboxController* codriver); 
+  ShooterManualControl(Shooter* shooter, frc::XboxController* codriver);
 
   void OnTick(units::second_t dt) override;
 
@@ -21,5 +22,6 @@ class ShooterManualControl : public behaviour::Behaviour {
   frc::XboxController* _codriver;
 
   bool _rawControl = false;
-  std::shared_ptr<nt::NetworkTable> table = nt::NetworkTableInstance::GetDefault().GetTable("Shooter Behaviour");
-};
\ No newline at end of file
+  std::shared_ptr<nt::NetworkTable> table =
+      nt::NetworkTableInstance::GetDefault().GetTable("Shooter Behaviour");
+};
diff --git a/wombat/src/main/cpp/behaviour/Behaviour.cpp b/wombat/src/main/cpp/behaviour/Behaviour.cpp
index d21d9e57..7312c922 100644
--- a/wombat/src/main/cpp/behaviour/Behaviour.cpp
+++ b/wombat/src/main/cpp/behaviour/Behaviour.cpp
@@ -10,9 +10,7 @@ using namespace behaviour;
 
 // Behaviour
 Behaviour::Behaviour(std::string name, units::time::second_t period)
-    : _bhvr_name(name),
-      _bhvr_period(period),
-      _bhvr_state(BehaviourState::INITIALISED) {}
+    : _bhvr_name(name), _bhvr_period(period), _bhvr_state(BehaviourState::INITIALISED) {}
 Behaviour::~Behaviour() {
   if (!IsFinished())
     Interrupt();
@@ -82,8 +80,7 @@ bool Behaviour::Tick() {
 
     if (dt > 2 * _bhvr_period) {
       std::cerr << "Behaviour missed deadline. Reduce Period. Dt=" << dt.value()
-                << " Dt(deadline)=" << (2 * _bhvr_period).value()
-                << ". Bhvr: " << GetName() << std::endl;
+                << " Dt(deadline)=" << (2 * _bhvr_period).value() << ". Bhvr: " << GetName() << std::endl;
     }
 
     if (_bhvr_timeout.value() > 0 && _bhvr_timer > _bhvr_timeout)
@@ -100,8 +97,7 @@ bool Behaviour::IsRunning() const {
 }
 
 bool Behaviour::IsFinished() const {
-  return _bhvr_state != BehaviourState::INITIALISED &&
-         _bhvr_state != BehaviourState::RUNNING;
+  return _bhvr_state != BehaviourState::INITIALISED && _bhvr_state != BehaviourState::RUNNING;
 }
 
 void Behaviour::Stop(BehaviourState new_state) {
@@ -173,8 +169,7 @@ void ConcurrentBehaviour::Add(Behaviour::ptr behaviour) {
 }
 
 std::string ConcurrentBehaviour::GetName() const {
-  std::string msg =
-      (_reducer == ConcurrentBehaviourReducer::ALL ? "ALL { " : "RACE {");
+  std::string msg = (_reducer == ConcurrentBehaviourReducer::ALL ? "ALL { " : "RACE {");
   for (auto b : _children)
     msg += b->GetName() + ", ";
   msg += "}";
@@ -190,8 +185,8 @@ void ConcurrentBehaviour::OnStart() {
         using namespace std::chrono_literals;
 
         b->Tick();
-        std::this_thread::sleep_for(std::chrono::milliseconds(
-            static_cast<int64_t>(b->GetPeriod().value() * 1000)));
+        std::this_thread::sleep_for(
+            std::chrono::milliseconds(static_cast<int64_t>(b->GetPeriod().value() * 1000)));
       }
 
       if (IsFinished() && !b->IsFinished())
@@ -273,10 +268,8 @@ void WaitFor::OnTick(units::time::second_t dt) {
 }
 
 // WaitTime
-WaitTime::WaitTime(units::time::second_t time)
-    : WaitTime([time]() { return time; }) {}
-WaitTime::WaitTime(std::function<units::time::second_t()> time_fn)
-    : _time_fn(time_fn) {}
+WaitTime::WaitTime(units::time::second_t time) : WaitTime([time]() { return time; }) {}
+WaitTime::WaitTime(std::function<units::time::second_t()> time_fn) : _time_fn(time_fn) {}
 
 void WaitTime::OnStart() {
   _time = _time_fn();
diff --git a/wombat/src/main/cpp/behaviour/BehaviourScheduler.cpp b/wombat/src/main/cpp/behaviour/BehaviourScheduler.cpp
index 26863c79..eb363b3e 100644
--- a/wombat/src/main/cpp/behaviour/BehaviourScheduler.cpp
+++ b/wombat/src/main/cpp/behaviour/BehaviourScheduler.cpp
@@ -52,8 +52,8 @@ void BehaviourScheduler::Schedule(Behaviour::ptr behaviour) {
         std::lock_guard<std::recursive_mutex> lk(_active_mtx);
         behaviour->Tick();
       }
-      std::this_thread::sleep_for(std::chrono::milliseconds(
-          static_cast<int64_t>(behaviour->GetPeriod().value() * 1000)));
+      std::this_thread::sleep_for(
+          std::chrono::milliseconds(static_cast<int64_t>(behaviour->GetPeriod().value() * 1000)));
     }
   });
 }
diff --git a/wombat/src/main/cpp/behaviour/HasBehaviour.cpp b/wombat/src/main/cpp/behaviour/HasBehaviour.cpp
index 964522c6..f2f557c5 100644
--- a/wombat/src/main/cpp/behaviour/HasBehaviour.cpp
+++ b/wombat/src/main/cpp/behaviour/HasBehaviour.cpp
@@ -8,8 +8,7 @@
 
 using namespace behaviour;
 
-void HasBehaviour::SetDefaultBehaviour(
-    std::function<std::shared_ptr<Behaviour>(void)> fn) {
+void HasBehaviour::SetDefaultBehaviour(std::function<std::shared_ptr<Behaviour>(void)> fn) {
   _default_behaviour_producer = fn;
 }
 
diff --git a/wombat/src/main/cpp/drivetrain/Drivetrain.cpp b/wombat/src/main/cpp/drivetrain/Drivetrain.cpp
deleted file mode 100644
index f3ce364d..00000000
--- a/wombat/src/main/cpp/drivetrain/Drivetrain.cpp
+++ /dev/null
@@ -1,53 +0,0 @@
-/* // Copyright (c) 2023-2024 CurtinFRC
-// Open Source Software, you can modify it according to the terms
-// of the MIT License at the root of this project
-
-#include "drivetrain/Drivetrain.h"
-
-#include <iostream>
-
-#include "utils/Util.h"
-
-using namespace frc;
-using namespace units;
-
-wom::drivetrain::Drivetrain::Drivetrain(DrivetrainConfig* config,
-                                        XboxController& driver)
-    : _config(config), _driver(driver) {}
-wom::drivetrain::Drivetrain::~Drivetrain() {}
-
-wom::drivetrain::DrivetrainConfig* wom::drivetrain::Drivetrain::GetConfig() {
-  return _config;
-}
-wom::drivetrain::DrivetrainState wom::drivetrain::Drivetrain::GetState() {
-  return _state;
-}
-
-void wom::drivetrain::Drivetrain::SetState(DrivetrainState state) {
-  _state = state;
-}
-
-void wom::drivetrain::Drivetrain::OnStart() {
-  std::cout << "Starting Tank" << std::endl;
-}
-
-void wom::drivetrain::Drivetrain::OnUpdate(second_t dt) {
-  switch (_state) {
-    case DrivetrainState::kIdle:
-      break;
-    case DrivetrainState::kTank: {
-      double rightSpeed = wom::utils::deadzone(_driver.GetRightY());
-      double leftSpeed = wom::utils::deadzone(_driver.GetLeftY());
-      // _config->left1.motorController->SetVoltage(leftSpeed * maxVolts);
-      // _config->left2.motorController->SetVoltage(leftSpeed * maxVolts);
-      // _config->left3.motorController->SetVoltage(leftSpeed * maxVolts);
-      // _config->right1.motorController->SetVoltage(rightSpeed * maxVolts);
-      // _config->right2.motorController->SetVoltage(rightSpeed * maxVolts);
-      // _config->right3.motorController->SetVoltage(rightSpeed * maxVolts);
-      break;
-    }
-    case DrivetrainState::kAuto:
-      break;
-  }
-}
- */
\ No newline at end of file
diff --git a/wombat/src/main/cpp/drivetrain/SwerveDrive.cpp b/wombat/src/main/cpp/drivetrain/SwerveDrive.cpp
index 553360c9..701fae07 100644
--- a/wombat/src/main/cpp/drivetrain/SwerveDrive.cpp
+++ b/wombat/src/main/cpp/drivetrain/SwerveDrive.cpp
@@ -5,20 +5,18 @@
 #include "drivetrain/SwerveDrive.h"
 
 #include <networktables/NetworkTableInstance.h>
+#include <units/angle.h>
 #include <units/math.h>
 #include <units/voltage.h>
-#include <units/angle.h>
-
-#include "utils/Util.h"
+#include <wpi/sendable/SendableBuilder.h>
+#include <wpi/sendable/SendableRegistry.h>
 
-#include <iostream>
 #include <algorithm>
 #include <cmath>
-
-#include <wpi/sendable/SendableBuilder.h>
-#include <wpi/sendable/SendableRegistry.h>
+#include <iostream>
 
 #include "frc/MathUtil.h"
+#include "utils/Util.h"
 #include "wpimath/MathShared.h"
 
 using namespace wom;
@@ -26,19 +24,16 @@ using namespace wom;
 namespace wom {
 namespace drivetrain {
 
-
-
-void SwerveModuleConfig::WriteNT(
-    std::shared_ptr<nt::NetworkTable> table) const {
+void SwerveModuleConfig::WriteNT(std::shared_ptr<nt::NetworkTable> table) const {
   std::array<double, 2> pos{position.X().value(), position.Y().value()};
   table->GetEntry("position").SetDoubleArray(std::span(pos));
   table->GetEntry("wheelRadius").SetDouble(wheelRadius.value());
 }
 
 SwerveModule::SwerveModule(std::string path, SwerveModuleConfig config,
-                           //SwerveModule::angle_pid_conf_t anglePID,
+                           // SwerveModule::angle_pid_conf_t anglePID,
                            SwerveModule::velocity_pid_conf_t velocityPID)
-    : //_anglePIDController(path + "/pid/angle", anglePID),
+    :  // _anglePIDController(path + "/pid/angle", anglePID),
       _anglePIDController{frc::PIDController(5, 0, 0, 0.005_s)},
       _config(config),
       _velocityPIDController(frc::PIDController(1.2, 0, 0, 0.005_s)),
@@ -49,7 +44,6 @@ SwerveModule::SwerveModule(std::string path, SwerveModuleConfig config,
   // _anglePIDController.EnableContinuousInput(0, (3.1415 * 2));
   // _anglePIDController.EnableContinuousInput(0, (3.1415));
   // _anglePIDController.SetWrap(0, 3.1415);
-
 }
 
 void SwerveModule::OnStart() {
@@ -60,7 +54,6 @@ void SwerveModule::OnStart() {
   // _anglePIDController.EnableContinuousInput(-3.141592, (3.141592));
   _anglePIDController.EnableContinuousInput(0, (2 * 3.141592));
 
-
   _velocityPIDController.Reset();
 }
 
@@ -68,8 +61,6 @@ void SwerveModule::OnUpdate(units::second_t dt) {
   units::volt_t driveVoltage{0};
   units::volt_t turnVoltage{0};
 
-
-
   switch (_state) {
     case SwerveModuleState::kIdle:
       driveVoltage = 0_V;
@@ -77,17 +68,17 @@ void SwerveModule::OnUpdate(units::second_t dt) {
       break;
     case SwerveModuleState::kPID: {
       auto feedforward = _config.driveMotor.motor.Voltage(
-          0_Nm, units::radians_per_second_t{
-                    _velocityPIDController.GetSetpoint() / _config.wheelRadius.value()});
-      // units::radian_t input = units::math::fmod(_config.turnMotor.encoder->GetEncoderPosition(), (2_rad * 3.1415));
+          0_Nm,
+          units::radians_per_second_t{_velocityPIDController.GetSetpoint() / _config.wheelRadius.value()});
+      // units::radian_t input = units::math::fmod(_config.turnMotor.encoder->GetEncoderPosition(), (2_rad
+      // * 3.1415));
       double input = _config.turnMotor.encoder->GetEncoderPosition().value();
       _table->GetEntry("/testing/GetEncoderPos").SetDouble(input);
       // _velocityPIDController.SetSetpoint(3);
-      
 
       driveVoltage = units::volt_t{_velocityPIDController.Calculate(GetSpeed().value())};
       // if (_turnOffset == TurnOffsetValues::forward) {
-        
+
       // } else if (_turnOffset == TurnOffsetValues::reverse) {
       //   input = input - (3.1415/2);
       //   driveVoltage = -driveVoltage;
@@ -104,8 +95,7 @@ void SwerveModule::OnUpdate(units::second_t dt) {
       std::cerr << "Case not handled" << std::endl;
   }
 
-  units::newton_meter_t torqueLimit =
-      50_kg / 4 * _config.wheelRadius * _currentAccelerationLimit;
+  units::newton_meter_t torqueLimit = 50_kg / 4 * _config.wheelRadius * _currentAccelerationLimit;
   // units::volt_t voltageMax = _config.driveMotor.motor.Voltage(
   //     torqueLimit, _config.driveMotor.encoder->GetEncoderAngularVelocity());
   // units::volt_t voltageMin = _config.driveMotor.motor.Voltage(
@@ -121,8 +111,7 @@ void SwerveModule::OnUpdate(units::second_t dt) {
   //     units::math::max(driveVoltage, -_driveModuleVoltageLimit),
   //     _driveModuleVoltageLimit);  // was originally 10_V
   units::volt_t turnVoltageMax = 7_V - (driveVoltage * (7_V / 10_V));
-  turnVoltage = units::math::min(units::math::max(turnVoltage, -turnVoltageMax),
-                                 turnVoltageMax);
+  turnVoltage = units::math::min(units::math::max(turnVoltage, -turnVoltageMax), turnVoltageMax);
   // turnVoltage = units::math::min(units::math::max(turnVoltage, -7_V), 7_V);
 
   _table->GetEntry("TurnVoltage").SetDouble(turnVoltage.value());
@@ -154,13 +143,11 @@ void SwerveModule::TurnOffset() {
 //   return (_config.turnMotor.encoder->GetEncoderPosition().value());
 // }
 
-void SwerveModule::SetAccelerationLimit(
-    units::meters_per_second_squared_t limit) {
+void SwerveModule::SetAccelerationLimit(units::meters_per_second_squared_t limit) {
   _currentAccelerationLimit = limit;
 }
 
-void SwerveDrive::SetAccelerationLimit(
-    units::meters_per_second_squared_t limit) {
+void SwerveDrive::SetAccelerationLimit(units::meters_per_second_squared_t limit) {
   for (int motorNumber = 0; motorNumber < 4; motorNumber++) {
     _modules[motorNumber].SetAccelerationLimit(limit);
   }
@@ -179,12 +166,9 @@ void SwerveModule::SetZero(units::second_t dt) {
   _state = SwerveModuleState::kPID;
 }
 
-void SwerveModule::SetPID(units::radian_t angle,
-                          units::meters_per_second_t speed,
-                          units::second_t dt) {
+void SwerveModule::SetPID(units::radian_t angle, units::meters_per_second_t speed, units::second_t dt) {
   _state = SwerveModuleState::kPID;
 
-
   // double diff = std::abs(_config.turnMotor.encoder->GetEncoderPosition().value() - angle.value());
   // _table->GetEntry("diff").SetDouble(diff);
   // if (std::abs(diff) > (3.14159/2)) {
@@ -196,14 +180,13 @@ void SwerveModule::SetPID(units::radian_t angle,
   //   _anglePIDController.SetSetpoint(angle.value());
   //   _velocityPIDController.SetSetpoint(speed.value());
   // }
-  
+
   // if (diff > (3.14159 / 2)) {
   //   speed *= -1;
-  //   _anglePIDController.SetSetpoint((3.14159 - (angle.value() - _config.turnMotor.encoder->GetEncoderPosition().value())));
+  //   _anglePIDController.SetSetpoint((3.14159 - (angle.value() -
+  //   _config.turnMotor.encoder->GetEncoderPosition().value())));
   //   _velocityPIDController.SetSetpoint(speed.value());
   // } else {
-    
-
 
   // double setValue = 3.141592 - (angle.value() - _config.turnMotor.encoder->GetEncoderPosition().value());
 
@@ -219,12 +202,10 @@ void SwerveModule::SetPID(units::radian_t angle,
   //     _velocityPIDController.SetSetpoint(-speed.value());
   //   }
   // } else {
-    _anglePIDController.SetSetpoint(angle.value());
-    _velocityPIDController.SetSetpoint(speed.value());
+  _anglePIDController.SetSetpoint(angle.value());
+  _velocityPIDController.SetSetpoint(speed.value());
   // }
 
-  
-
   // double currentAngle = _config.turnMotor.encoder->GetEncoderPosition().value();
   // double setpointAngle = closestAngle(currentAngle, _anglePIDController.GetSetpoint());
   // double setpointAngleFlipped = closestAngle(currentAngle, _anglePIDController.GetSetpoint() + 3.1415);
@@ -251,38 +232,31 @@ void SwerveModule::SetPID(units::radian_t angle,
 
 void SwerveModule::ModuleVectorHandler(frc::ChassisSpeeds speeds) {
   units::meters_per_second_t xVelocityComponent =
-      1_mps *
-      (speeds.vx.value() + speeds.omega.value() * _config.position.X().value());
+      1_mps * (speeds.vx.value() + speeds.omega.value() * _config.position.X().value());
   units::meters_per_second_t yVelocityComponent =
-      1_mps *
-      (speeds.vy.value() + speeds.omega.value() * _config.position.Y().value());
+      1_mps * (speeds.vy.value() + speeds.omega.value() * _config.position.Y().value());
 
   units::meters_per_second_t velocity =
-      1_mps * std::sqrt(std::pow(xVelocityComponent.value(), 2) +
-                        std::pow(yVelocityComponent.value(), 2));
-  units::degree_t angle = 1_rad * std::atan2(yVelocityComponent.value(),
-                                             xVelocityComponent.value());
+      1_mps * std::sqrt(std::pow(xVelocityComponent.value(), 2) + std::pow(yVelocityComponent.value(), 2));
+  units::degree_t angle = 1_rad * std::atan2(yVelocityComponent.value(), xVelocityComponent.value());
 
   _anglePIDController.SetSetpoint(angle.value());
   _velocityPIDController.SetSetpoint(velocity.value());
 }
 
 units::meters_per_second_t SwerveModule::GetSpeed() const {
-  units::meters_per_second_t returnVal{
-    _config.driveMotor.encoder->GetVelocityValue() *
-    _config.wheelRadius.value()};
-    return returnVal;
+  units::meters_per_second_t returnVal{_config.driveMotor.encoder->GetVelocityValue() *
+                                       _config.wheelRadius.value()};
+  return returnVal;
 }
 
 units::meter_t SwerveModule::GetDistance() const {
-  return units::meter_t{
-      _config.driveMotor.encoder->GetEncoderPosition().value() *
-      _config.wheelRadius.value()};
+  return units::meter_t{_config.driveMotor.encoder->GetEncoderPosition().value() *
+                        _config.wheelRadius.value()};
 }
 
 frc::SwerveModulePosition SwerveModule::GetPosition() const {
-  return frc::SwerveModulePosition{
-      GetDistance(), _config.turnMotor.encoder->GetEncoderPosition()};
+  return frc::SwerveModulePosition{GetDistance(), _config.turnMotor.encoder->GetEncoderPosition()};
 }
 
 const SwerveModuleConfig& SwerveModule::GetConfig() const {
@@ -302,11 +276,10 @@ SwerveDrive::SwerveDrive(SwerveDriveConfig config, frc::Pose2d initialPose)
                   _config.modules[1].position /*2*/, _config.modules[2].position /*3*/),
       _poseEstimator(
           _kinematics, frc::Rotation2d(0_deg),
-          wpi::array<frc::SwerveModulePosition, 4>{
-              frc::SwerveModulePosition{0_m, frc::Rotation2d{0_deg}},
-              frc::SwerveModulePosition{0_m, frc::Rotation2d{0_deg}},
-              frc::SwerveModulePosition{0_m, frc::Rotation2d{0_deg}},
-              frc::SwerveModulePosition{0_m, frc::Rotation2d{0_deg}}},
+          wpi::array<frc::SwerveModulePosition, 4>{frc::SwerveModulePosition{0_m, frc::Rotation2d{0_deg}},
+                                                   frc::SwerveModulePosition{0_m, frc::Rotation2d{0_deg}},
+                                                   frc::SwerveModulePosition{0_m, frc::Rotation2d{0_deg}},
+                                                   frc::SwerveModulePosition{0_m, frc::Rotation2d{0_deg}}},
           initialPose, _config.stateStdDevs, _config.visionMeasurementStdDevs),
       _anglePIDController{frc::PIDController(1, 0, 0)},
       _xPIDController(config.path + "/pid/x", _config.posePositionPID),
@@ -324,10 +297,8 @@ SwerveDrive::SwerveDrive(SwerveDriveConfig config, frc::Pose2d initialPose)
   ResetPose(initialPose);
 }
 
-frc::ChassisSpeeds FieldRelativeSpeeds::ToChassisSpeeds(
-    const units::radian_t robotHeading) {
-  return frc::ChassisSpeeds::FromFieldRelativeSpeeds(
-      vx, vy, omega, frc::Rotation2d{robotHeading});
+frc::ChassisSpeeds FieldRelativeSpeeds::ToChassisSpeeds(const units::radian_t robotHeading) {
+  return frc::ChassisSpeeds::FromFieldRelativeSpeeds(vx, vy, omega, frc::Rotation2d{robotHeading});
 }
 
 void SwerveDrive::OnUpdate(units::second_t dt) {
@@ -348,14 +319,15 @@ void SwerveDrive::OnUpdate(units::second_t dt) {
     case SwerveDriveState::kPose: {
       _target_fr_speeds.vx = _xPIDController.Calculate(GetPose().X(), dt);
       _target_fr_speeds.vy = _yPIDController.Calculate(GetPose().Y(), dt);
-      _target_fr_speeds.omega = units::radians_per_second_t{_anglePIDController.Calculate(GetPose().Rotation().Radians().value())};
+      _target_fr_speeds.omega =
+          units::radians_per_second_t{_anglePIDController.Calculate(GetPose().Rotation().Radians().value())};
     }
       [[fallthrough]];
     case SwerveDriveState::kFieldRelativeVelocity:
-      _target_speed =
-          _target_fr_speeds.ToChassisSpeeds(GetPose().Rotation().Radians());
+      _target_speed = _target_fr_speeds.ToChassisSpeeds(GetPose().Rotation().Radians());
       if (isRotateToMatchJoystick) {
-        _target_speed.omega = units::radians_per_second_t{_anglePIDController.Calculate(GetPose().Rotation().Radians().value())};
+        _target_speed.omega = units::radians_per_second_t{
+            _anglePIDController.Calculate(GetPose().Rotation().Radians().value())};
       }
       // _target_fr_speeds.vy.value() << std::endl;
       [[fallthrough]];
@@ -379,15 +351,13 @@ void SwerveDrive::OnUpdate(units::second_t dt) {
       }
 
       auto target_states = _kinematics.ToSwerveModuleStates(_target_speed);
-      frc::ChassisSpeeds new_target_speed {_target_speed.vx, _target_speed.vy, -_target_speed.omega};
+      frc::ChassisSpeeds new_target_speed{_target_speed.vx, _target_speed.vy, -_target_speed.omega};
       auto new_target_states = _kinematics.ToSwerveModuleStates(new_target_speed);
       for (size_t i = 0; i < _modules.size(); i++) {
-        if ( i == 3) {
-          _modules[i].SetPID(new_target_states[i].angle.Radians(),
-                            new_target_states[i].speed, dt);
+        if (i == 3) {
+          _modules[i].SetPID(new_target_states[i].angle.Radians(), new_target_states[i].speed, dt);
         } else {
-          _modules[i].SetPID(target_states[i].angle.Radians(),
-                            target_states[i].speed, dt);
+          _modules[i].SetPID(target_states[i].angle.Radians(), target_states[i].speed, dt);
           // target_states[i].angle.Radians().value() << std::endl;
         }
 
@@ -415,13 +385,12 @@ void SwerveDrive::OnUpdate(units::second_t dt) {
     case SwerveDriveState::kFRVelocityRotationLock:
       _target_speed.vx = _xPIDController.Calculate(GetPose().X(), dt);
       _target_speed.vy = _yPIDController.Calculate(GetPose().Y(), dt);
-      _target_speed.omega = units::radians_per_second_t{_anglePIDController.Calculate(GetPose().Rotation().Radians().value())};
-      _target_speed =
-          _requestedSpeeds.ToChassisSpeeds(GetPose().Rotation().Radians());
+      _target_speed.omega =
+          units::radians_per_second_t{_anglePIDController.Calculate(GetPose().Rotation().Radians().value())};
+      _target_speed = _requestedSpeeds.ToChassisSpeeds(GetPose().Rotation().Radians());
       auto target_states = _kinematics.ToSwerveModuleStates(_target_speed);
       for (size_t i = 0; i < _modules.size(); i++) {
-        _modules[i].SetPID(target_states[i].angle.Radians(),
-                           target_states[i].speed, dt);
+        _modules[i].SetPID(target_states[i].angle.Radians(), target_states[i].speed, dt);
       }
       break;
   }
@@ -432,12 +401,10 @@ void SwerveDrive::OnUpdate(units::second_t dt) {
 
   _poseEstimator.Update(
       _config.gyro->GetRotation2d(),
-      wpi::array<frc::SwerveModulePosition, 4>{
-          _modules[0].GetPosition(), _modules[1].GetPosition(),
-          _modules[2].GetPosition(), _modules[3].GetPosition()});
+      wpi::array<frc::SwerveModulePosition, 4>{_modules[0].GetPosition(), _modules[1].GetPosition(),
+                                               _modules[2].GetPosition(), _modules[3].GetPosition()});
 
-  utils::WritePose2NT(_table->GetSubTable("estimatedPose"),
-                      _poseEstimator.GetEstimatedPosition());
+  utils::WritePose2NT(_table->GetSubTable("estimatedPose"), _poseEstimator.GetEstimatedPosition());
   _config.WriteNT(_table->GetSubTable("config"));
 }
 
@@ -474,8 +441,7 @@ void SwerveDrive::OnResetMode() {
   _anglePIDController.Reset();
 }
 
-void SwerveDrive::RotateMatchJoystick(units::radian_t joystickAngle,
-                                      FieldRelativeSpeeds speeds) {
+void SwerveDrive::RotateMatchJoystick(units::radian_t joystickAngle, FieldRelativeSpeeds speeds) {
   // _state = SwerveDriveState::kFRVelocityRotationLock;
   // _anglePIDController.SetSetpoint(joystickAngle);
   // _target_fr_speeds = speeds;
@@ -501,16 +467,14 @@ bool SwerveDrive::GetIsFieldRelative() {
   return _isFieldRelative;
 }
 
-void SwerveDrive::SetIndividualTuning(int mod, units::radian_t angle,
-                                      units::meters_per_second_t speed) {
+void SwerveDrive::SetIndividualTuning(int mod, units::radian_t angle, units::meters_per_second_t speed) {
   _mod = mod;
   _angle = angle;
   _speed = speed;
   _state = SwerveDriveState::kIndividualTuning;
 }
 
-void SwerveDrive::SetTuning(units::radian_t angle,
-                            units::meters_per_second_t speed) {
+void SwerveDrive::SetTuning(units::radian_t angle, units::meters_per_second_t speed) {
   _angle = angle;
   _speed = speed;
   _state = SwerveDriveState::kTuning;
@@ -537,9 +501,8 @@ bool SwerveDrive::IsAtSetPose() {
 void SwerveDrive::ResetPose(frc::Pose2d pose) {
   _poseEstimator.ResetPosition(
       _config.gyro->GetRotation2d(),
-      wpi::array<frc::SwerveModulePosition, 4>{
-          _modules[0].GetPosition(), _modules[1].GetPosition(),
-          _modules[2].GetPosition(), _modules[3].GetPosition()},
+      wpi::array<frc::SwerveModulePosition, 4>{_modules[0].GetPosition(), _modules[1].GetPosition(),
+                                               _modules[2].GetPosition(), _modules[3].GetPosition()},
       pose);
 }
 
@@ -547,9 +510,8 @@ frc::Pose2d SwerveDrive::GetPose() {
   return _poseEstimator.GetEstimatedPosition();
 }
 
-void SwerveDrive::AddVisionMeasurement(frc::Pose2d pose,
-                                       units::second_t timestamp) {
+void SwerveDrive::AddVisionMeasurement(frc::Pose2d pose, units::second_t timestamp) {
   _poseEstimator.AddVisionMeasurement(pose, timestamp);
 }
 }  // namespace drivetrain
-}  // namespace wom
\ No newline at end of file
+}  // namespace wom
diff --git a/wombat/src/main/cpp/drivetrain/behaviours/SwerveBehaviours.cpp b/wombat/src/main/cpp/drivetrain/behaviours/SwerveBehaviours.cpp
index 9479dfe0..58782b81 100644
--- a/wombat/src/main/cpp/drivetrain/behaviours/SwerveBehaviours.cpp
+++ b/wombat/src/main/cpp/drivetrain/behaviours/SwerveBehaviours.cpp
@@ -72,45 +72,41 @@ void ManualDrivebase::OnTick(units::second_t deltaTime) {
   // if (isZero) {
   //   _swerveDrivebase->SetZeroing();
   // } else {
-    double xVelocity = wom::utils::spow2(-wom::utils::deadzone(
-        _driverController->GetLeftY(),
-        driverDeadzone));  // GetLeftY due to x being where y should be on field
-    double yVelocity = wom::utils::spow2(
-        -wom::utils::deadzone(_driverController->GetLeftX(), driverDeadzone));
-
-    double r_x = wom::utils::spow2(
-        -wom::utils::deadzone(_driverController->GetRightX(), turningDeadzone));
-
-    double turnX = _driverController->GetRightX();
-    double turnY = _driverController->GetRightY();
-    double num = std::sqrt(turnX * turnX + turnY * turnY);
-    if (num < turningDeadzone) {
-      turnX = 0;
-      turnY = 0;
-    }
-
-    // if (isRotateMatch) {
-    //   units::degree_t currentAngle =
-    //       _swerveDrivebase->GetPose().Rotation().Degrees();
-    //   CalculateRequestedAngle(turnX, turnY, currentAngle);
-    //   _swerveDriveTable->GetEntry("RotateMatch")
-    //       .SetDouble(_requestedAngle.value());
-    //   _swerveDrivebase->RotateMatchJoystick(
-    //       _requestedAngle,
-    //       wom::drivetrain::FieldRelativeSpeeds{// also field relative
-    //                                            xVelocity * maxMovementMagnitude,
-    //                                            yVelocity * maxMovementMagnitude,
-    //                                            r_x * maxRotationMagnitude});
-    // } else {
-      _swerveDrivebase->SetFieldRelativeVelocity(
-          wom::drivetrain::FieldRelativeSpeeds{xVelocity * -maxMovementMagnitude,
-                                               yVelocity * -maxMovementMagnitude,
-                                               r_x * maxRotationMagnitude});
-
-    //  _swerveDrivebase->SetVelocity(
-    //       frc::ChassisSpeeds{xVelocity * maxMovementMagnitude,
-    //                          yVelocity * maxMovementMagnitude,
-    //                          r_x * maxRotationMagnitude});                                          
+  double xVelocity = wom::utils::spow2(
+      -wom::utils::deadzone(_driverController->GetLeftY(),
+                            driverDeadzone));  // GetLeftY due to x being where y should be on field
+  double yVelocity = wom::utils::spow2(-wom::utils::deadzone(_driverController->GetLeftX(), driverDeadzone));
+
+  double r_x = wom::utils::spow2(-wom::utils::deadzone(_driverController->GetRightX(), turningDeadzone));
+
+  double turnX = _driverController->GetRightX();
+  double turnY = _driverController->GetRightY();
+  double num = std::sqrt(turnX * turnX + turnY * turnY);
+  if (num < turningDeadzone) {
+    turnX = 0;
+    turnY = 0;
+  }
+
+  // if (isRotateMatch) {
+  //   units::degree_t currentAngle =
+  //       _swerveDrivebase->GetPose().Rotation().Degrees();
+  //   CalculateRequestedAngle(turnX, turnY, currentAngle);
+  //   _swerveDriveTable->GetEntry("RotateMatch")
+  //       .SetDouble(_requestedAngle.value());
+  //   _swerveDrivebase->RotateMatchJoystick(
+  //       _requestedAngle,
+  //       wom::drivetrain::FieldRelativeSpeeds{// also field relative
+  //                                            xVelocity * maxMovementMagnitude,
+  //                                            yVelocity * maxMovementMagnitude,
+  //                                            r_x * maxRotationMagnitude});
+  // } else {
+  _swerveDrivebase->SetFieldRelativeVelocity(wom::drivetrain::FieldRelativeSpeeds{
+      xVelocity * -maxMovementMagnitude, yVelocity * -maxMovementMagnitude, r_x * maxRotationMagnitude});
+
+  //  _swerveDrivebase->SetVelocity(
+  //       frc::ChassisSpeeds{xVelocity * maxMovementMagnitude,
+  //                          yVelocity * maxMovementMagnitude,
+  //                          r_x * maxRotationMagnitude});
   //   }
   // }
   // _swerveDrivebase->SetIndividualTuning(2, 0_deg, 0_mps);
@@ -121,8 +117,7 @@ void ManualDrivebase::ResetMode() {
   resetMode = false;
 }
 
-void ManualDrivebase::CalculateRequestedAngle(double joystickX,
-                                              double joystickY,
+void ManualDrivebase::CalculateRequestedAngle(double joystickX, double joystickY,
                                               units::degree_t defaultAngle) {
   _requestedAngle = (1_rad * std::atan2(joystickY, -joystickX)) + 90_deg;
   if (wom::utils::deadzone(joystickX) == 0 && wom::utils::deadzone(joystickY) == 0) {
@@ -131,8 +126,7 @@ void ManualDrivebase::CalculateRequestedAngle(double joystickX,
 }
 
 // Code for x-ing the wheels on the drivebase
-XDrivebase::XDrivebase(wom::drivetrain::SwerveDrive* swerveDrivebase)
-    : _swerveDrivebase(swerveDrivebase) {
+XDrivebase::XDrivebase(wom::drivetrain::SwerveDrive* swerveDrivebase) : _swerveDrivebase(swerveDrivebase) {
   Controls(swerveDrivebase);
 }
 void XDrivebase::OnTick(units::second_t deltaTime) {
@@ -174,20 +168,17 @@ void XDrivebase::OnTick(units::second_t deltaTime) {
 //   m_timer.Start();
 // }
 
-wom::drivetrain::behaviours::TempSimSwerveDrive::TempSimSwerveDrive(
-    frc::Timer* timer, frc::Field2d* field)
+wom::drivetrain::behaviours::TempSimSwerveDrive::TempSimSwerveDrive(frc::Timer* timer, frc::Field2d* field)
     : m_timer(timer), m_field(field) {}
 
 void wom::drivetrain::behaviours::TempSimSwerveDrive::OnUpdate() {
   m_field->SetRobotPose(m_driveSim.GetPose());
 
   // get the current trajectory state
-  frc::Trajectory::State desired_state =
-      current_trajectory.Sample(m_timer->Get());
+  frc::Trajectory::State desired_state = current_trajectory.Sample(m_timer->Get());
 
   // get the current wheel speeds
-  wom::utils::WriteTrajectoryState(current_trajectory_state_table,
-                                   desired_state);
+  wom::utils::WriteTrajectoryState(current_trajectory_state_table, desired_state);
 
   // move drivebase position to the desired state
   m_driveSim.SetPose(wom::utils::TrajectoryStateToPose2d(desired_state));
@@ -205,19 +196,16 @@ frc::Pose2d wom::drivetrain::behaviours::TempSimSwerveDrive::GetPose2d() {
   return m_driveSim.GetPose();
 }
 
-void wom::drivetrain::behaviours::TempSimSwerveDrive::SetPath(
-    std::string path) {
+void wom::drivetrain::behaviours::TempSimSwerveDrive::SetPath(std::string path) {
   nt::NetworkTableInstance inst = nt::NetworkTableInstance::GetDefault();
   std::shared_ptr<nt::NetworkTable> table = inst.GetTable("FMSInfo");
 
   // create a netowrk table for the trajectory
   std::shared_ptr<nt::NetworkTable> trajectory_table =
       nt::NetworkTableInstance::GetDefault().GetTable("trajectory_path");
-  current_trajectory_table =
-      nt::NetworkTableInstance::GetDefault().GetTable("current_trajectory");
+  current_trajectory_table = nt::NetworkTableInstance::GetDefault().GetTable("current_trajectory");
   current_trajectory_state_table =
-      nt::NetworkTableInstance::GetDefault().GetTable(
-          "current_trajectory_state");
+      nt::NetworkTableInstance::GetDefault().GetTable("current_trajectory_state");
 
   current_trajectory = m_pathplanner.getTrajectory(path);
   m_driveSim.SetPose(current_trajectory.Sample(0_s).pose);
@@ -225,12 +213,10 @@ void wom::drivetrain::behaviours::TempSimSwerveDrive::SetPath(
   m_timer->Start();
 }
 
-wom::drivetrain::behaviours::AutoSwerveDrive::AutoSwerveDrive(
-    wom::drivetrain::SwerveDrive* swerve, frc::Timer* timer,
-    frc::Field2d* field)
+wom::drivetrain::behaviours::AutoSwerveDrive::AutoSwerveDrive(wom::drivetrain::SwerveDrive* swerve,
+                                                              frc::Timer* timer, frc::Field2d* field)
     : _swerve(swerve), m_timer(timer), m_field(field) {
-  _simSwerveDrive =
-      new wom::drivetrain::behaviours::TempSimSwerveDrive(timer, field);
+  _simSwerveDrive = new wom::drivetrain::behaviours::TempSimSwerveDrive(timer, field);
 }
 
 void wom::drivetrain::behaviours::AutoSwerveDrive::OnUpdate() {
diff --git a/wombat/src/main/cpp/subsystems/Arm.cpp b/wombat/src/main/cpp/subsystems/Arm.cpp
deleted file mode 100644
index 3406be88..00000000
--- a/wombat/src/main/cpp/subsystems/Arm.cpp
+++ /dev/null
@@ -1,192 +0,0 @@
-/* // Copyright (c) 2023-2024 CurtinFRC
-// Open Source Software, you can modify it according to the terms
-// of the MIT License at the root of this project
-
-#include "subsystems/Arm.h"
-
-#include <units/math.h>
-
-using namespace frc;
-using namespace wom;
-
-// creates network table instatnce on shuffleboard
-void wom::subsystems::ArmConfig::WriteNT(
-    std::shared_ptr<nt::NetworkTable> table) {
-  table->GetEntry("armMass").SetDouble(armMass.value());
-  table->GetEntry("loadMass").SetDouble(loadMass.value());
-  table->GetEntry("armLength").SetDouble(armLength.value());
-  table->GetEntry("minAngle")
-      .SetDouble(minAngle.convert<units::degree>().value());
-  table->GetEntry("maxAngle")
-      .SetDouble(maxAngle.convert<units::degree>().value());
-  table->GetEntry("initialAngle")
-      .SetDouble(initialAngle.convert<units::degree>().value());
-  table->GetEntry("angleOffset")
-      .SetDouble(initialAngle.convert<units::degree>().value());
-}
-
-// arm config is used
-wom::subsystems::Arm::Arm(ArmConfig config)
-    : _config(config),
-      _pid(config.path + "/pid", config.pidConfig),
-      _velocityPID(config.path + "/velocityPID", config.velocityConfig),
-      _table(nt::NetworkTableInstance::GetDefault().GetTable(config.path)) {}
-
-// the loop that allows the information to be used
-void wom::subsystems::Arm::OnUpdate(units::second_t dt) {
-  // sets the voltage and gets the current angle
-  units::volt_t voltage = 0_V;
-  auto angle = GetAngle();
-
-  // sets usable infromation for each state
-  switch (_state) {
-    case ArmState::kIdle:
-      break;
-    case ArmState::kVelocity: {
-      units::newton_meter_t torque =
-          9.81_m / 1_s / 1_s * _config.armLength *
-          units::math::cos(angle + _config.angleOffset) *
-          (0.5 * _config.armMass + _config.loadMass);
-      // units::volt_t feedforward = _config.leftGearbox.motor.Voltage(torque,
-      // 0_rad/1_s);
-      units::volt_t feedforward =
-          _config.leftGearbox.motor.Voltage(torque, _velocityPID.GetSetpoint());
-      // feedforward = 3.5_V;
-      // std::cout << "feedforward" << feedforward.value() << std::endl;
-      voltage = _velocityPID.Calculate(GetArmVelocity(), dt, feedforward);
-      // std::cout << "arm velocity voltage is: " << voltage.value() <<
-      // std::endl; voltage = 0_V;
-    } break;
-    case ArmState::kAngle: {
-      units::newton_meter_t torque =
-          9.81_m / 1_s / 1_s * _config.armLength *
-          units::math::cos(angle + _config.angleOffset) *
-          (0.5 * _config.armMass + _config.loadMass);
-      units::volt_t feedforward =
-          _config.leftGearbox.motor.Voltage(torque, 0_rad / 1_s);
-      // std::cout << "feedforward" << feedforward.value() << std::endl;
-      voltage = _pid.Calculate(angle, dt, feedforward);
-    } break;
-    case ArmState::kRaw:
-      voltage = _voltage;
-      break;
-  }
-
-  voltage *= armLimit;
-
-  // units::newton_meter_t torqueLimit = 10_kg * 1.4_m * 6_mps_sq;
-  // units::volt_t voltageMax = _config.leftGearbox.motor.Voltage(torqueLimit,
-  // _config.leftGearbox.encoder->GetEncoderAngularVelocity()); units::volt_t
-  // voltageMin = _config.leftGearbox.motor.Voltage(-torqueLimit,
-  // _config.leftGearbox.encoder->GetEncoderAngularVelocity());
-
-  // voltage = units::math::max(units::math::min(voltage, voltageMax),
-  // voltageMin);
-  units::volt_t voltageMin = -5.5_V;
-  units::volt_t voltageMax = 5.5_V;
-  voltage = units::math::max(units::math::min(voltage, voltageMax), voltageMin);
-
-  // std::cout << "voltage: " << voltage.value() << std::endl;
-
-  // _config.leftGearbox.motorController->SetVoltage(voltage);
-  // _config.rightGearbox.motorController->SetVoltage(voltage);
-
-  // creates network table instances for the angle and config of the arm
-  _table->GetEntry("angle").SetDouble(angle.convert<units::degree>().value());
-  _config.WriteNT(_table->GetSubTable("config"));
-}
-
-void wom::subsystems::Arm::SetArmSpeedLimit(double limit) {
-  armLimit = limit;
-}
-
-// defines information needed for the functions and connects the states to their
-// respective function
-
-void wom::subsystems::Arm::SetRaw(units::volt_t voltage) {
-  _voltage = voltage;
-}
-
-void wom::subsystems::Arm::SetVelocity(units::radians_per_second_t velocity) {
-  _velocityPID.SetSetpoint(velocity);
-}
-
-void wom::subsystems::Arm::SetAngle(units::radian_t angle) {
-  _pid.SetSetpoint(angle);
-}
-
-void wom::subsystems::Arm::SetState(wom::subsystems::ArmState state) {
-  _state = state;
-}
-
-wom::subsystems::ArmConfig& wom::subsystems::Arm::GetConfig() {
-  return _config;
-}
-
-units::radian_t wom::subsystems::Arm::GetAngle() const {
-  return _config.armEncoder.GetPosition() / 100 * 360 * 1_deg;
-}
-
-units::radians_per_second_t wom::subsystems::Arm::MaxSpeed() const {
-  return _config.leftGearbox.motor.Speed(0_Nm, 12_V);
-}
-
-units::radians_per_second_t wom::subsystems::Arm::GetArmVelocity() const {
-  return _config.armEncoder.GetVelocity() / 100 * 360 * 1_deg / 60_s;
-}
-
-bool wom::subsystems::Arm::IsStable() const {
-  return _pid.IsStable(5_deg);
-}
-
-/* SIMULATION */
-// #include <units/math.h>
-
-// ::wom::sim::ArmSim::ArmSim(ArmConfig config)
-//   : config(config),
-//     angle(config.initialAngle),
-//     encoder(config.gearbox.encoder->MakeSimEncoder()),
-//     lowerLimit(config.lowerLimitSwitch ? new
-//     frc::sim::DIOSim(*config.lowerLimitSwitch) : nullptr),
-//     upperLimit(config.upperLimitSwitch ? new
-//     frc::sim::DIOSim(*config.upperLimitSwitch) : nullptr),
-//     table(nt::NetworkTableInstance::GetDefault().GetTable(config.path +
-//     "/sim"))
-//   {}
-
-// units::ampere_t wom::sim::ArmSim::GetCurrent() const {
-//   return current;
-// }
-
-// void ::wom::sim::ArmSim::Update(units::second_t dt) {
-//   torque = (config.loadMass * config.armLength + config.armMass *
-//   config.armLength / 2.0) * 9.81_m / 1_s / 1_s *
-//   units::math::cos(config.angleOffset + angle) + additionalTorque; velocity =
-//   config.gearbox.motor.Speed(torque,
-//   config.gearbox.transmission->GetEstimatedRealVoltage()); angle += velocity
-//   * dt;
-
-//   if (angle <= config.minAngle) {
-//     angle = config.minAngle;
-//     velocity = 0_rad / 1_s;
-//     if (lowerLimit) lowerLimit->SetValue(true);
-//   } else {
-//     if (lowerLimit) lowerLimit->SetValue(false);
-//   }
-
-//   if (angle >= config.maxAngle) {
-//     angle = config.maxAngle;
-//     velocity = 0_rad / 1_s;
-//     if (upperLimit) upperLimit->SetValue(true);
-//   } else {
-//     if (upperLimit) upperLimit->SetValue(false);
-//   }
-
-//   current = config.gearbox.motor.Current(velocity,
-//   config.gearbox.transmission->GetEstimatedRealVoltage());
-
-//   if (encoder) encoder->SetEncoderTurns(angle - config.initialAngle);
-
-//   table->GetEntry("angle").SetDouble(angle.convert<units::degree>().value());
-//   table->GetEntry("current").SetDouble(current.value());
-// }
diff --git a/wombat/src/main/cpp/subsystems/Elevator.cpp b/wombat/src/main/cpp/subsystems/Elevator.cpp
deleted file mode 100644
index cc840302..00000000
--- a/wombat/src/main/cpp/subsystems/Elevator.cpp
+++ /dev/null
@@ -1,150 +0,0 @@
-/* // Copyright (c) 2023-2024 CurtinFRC
-// Open Source Software, you can modify it according to the terms
-// of the MIT License at the root of this project
-
-#include "subsystems/Elevator.h"
-
-#include <networktables/NetworkTableInstance.h>
-#include <units/acceleration.h>
-
-void wom::subsystems::ElevatorConfig::WriteNT(
-    std::shared_ptr<nt::NetworkTable> table) {
-  table->GetEntry("radius").SetDouble(radius.value());
-  table->GetEntry("mass").SetDouble(mass.value());
-  table->GetEntry("maxHeight").SetDouble(maxHeight.value());
-}
-
-wom::subsystems::Elevator::Elevator(ElevatorConfig config)
-    : _config(config),
-      _state(ElevatorState::kIdle),
-      _pid{config.path + "/pid", config.pid},
-      _velocityPID{config.path + "/velocityPID", config.velocityPID},
-      _table(nt::NetworkTableInstance::GetDefault().GetTable(config.path)) {
-  _config.leftGearbox.encoder->SetEncoderPosition(_config.initialHeight /
-                                                  _config.radius * 1_rad);
-}
-
-void wom::subsystems::Elevator::OnUpdate(units::second_t dt) {
-  units::volt_t voltage{0};
-
-  units::meter_t height = GetElevatorEncoderPos() * 1_m;
-
-  switch (_state) {
-    case ElevatorState::kIdle:
-      voltage = 0_V;
-      break;
-    case ElevatorState::kManual:
-      voltage = _setpointManual;
-      break;
-    case ElevatorState::kVelocity: {
-      units::volt_t feedforward = _config.rightGearbox.motor.Voltage(
-          (_config.mass * 9.81_mps_sq) * _config.radius,
-          _velocityPID.GetSetpoint() /
-              (14.0 / 60.0 * 2.0 * 3.1415 * 0.02225 * 1_m) * 1_rad);
-      // units::volt_t feedforward = _config.rightGearbox.motor.Voltage(0_Nm,
-      // _velocityPID.GetSetpoint() / (14.0/60.0 * 2.0 * 3.1415 * 0.02225 * 1_m)
-      // * 1_rad);
-      feedforward = 1.2_V;
-      voltage = _velocityPID.Calculate(GetElevatorVelocity(), dt, feedforward);
-      if (voltage > 6_V) {
-        voltage = 6_V;
-      }
-      std::cout << "elevator feedforward: " << feedforward.value() << std::endl;
-      // voltage = 0_V;
-    } break;
-    case ElevatorState::kPID: {
-      units::volt_t feedforward = _config.rightGearbox.motor.Voltage(
-          (_config.mass * 9.81_mps_sq) * _config.radius, 0_rad_per_s);
-      // std::cout << "feed forward" << feedforward.value() << std::endl;
-      feedforward = 1.2_V;
-      // voltage = _pid.Calculate(height, dt, feedforward);
-      voltage = _pid.Calculate(height, dt, feedforward);
-      if (voltage > 6_V) {
-        voltage = 6_V;
-      }
-    } break;
-  }
-
-  // Top Sensor Detector
-  // if(_config.topSensor != nullptr) {
-  //   if(_config.topSensor->Get()) {
-  //     _config.leftGearbox.encoder->SetEncoderPosition(_config.maxHeight /
-  //     _config.radius * 1_rad);
-  //     //voltage = 0_V;
-  //   }
-  // }
-
-  // //Bottom Sensor Detection
-  // if (_config.bottomSensor != nullptr) {
-  //   if (_config.bottomSensor->Get()) {
-  //     _config.leftGearbox.encoder->SetEncoderPosition(_config.minHeight /
-  //     _config.radius * 1_rad);
-  //     //voltage = 0_V;
-  //   }
-  // }
-
-  // std::cout << "elevator: " << elevator.height
-
-  // Set voltage to motors...
-  voltage *= speedLimit;
-  // _config.leftGearbox.motorController->SetVoltage(voltage);
-  // _config.rightGearbox.motorController->SetVoltage(voltage);
-}
-
-void wom::subsystems::Elevator::SetManual(units::volt_t voltage) {
-  _setpointManual = voltage;
-}
-
-void wom::subsystems::Elevator::SetPID(units::meter_t height) {
-  _pid.SetSetpoint(height);
-}
-
-void wom::subsystems::Elevator::SetVelocity(
-    units::meters_per_second_t velocity) {
-  _velocityPID.SetSetpoint(velocity);
-}
-
-void wom::subsystems::Elevator::SetState(wom::subsystems::ElevatorState state) {
-  _state = state;
-}
-
-void wom::subsystems::Elevator::SetElevatorSpeedLimit(double limit) {
-  speedLimit = limit;
-}
-
-wom::subsystems::ElevatorConfig& wom::subsystems::Elevator::GetConfig() {
-  return _config;
-}
-
-bool wom::subsystems::Elevator::IsStable() const {
-  return _pid.IsStable();
-}
-
-wom::subsystems::ElevatorState wom::subsystems::Elevator::GetState() const {
-  return _state;
-}
-
-double wom::subsystems::Elevator::GetElevatorEncoderPos() {
-  return _config.elevatorEncoder.GetPosition() * 14 / 60 * 2 * 3.1415 * 0.02225;
-}
-
-units::meter_t wom::subsystems::Elevator::GetHeight() const {
-  // std::cout << "elevator position"<<
-  // _config.rightGearbox.encoder->GetEncoderTicks() << std::endl; return
-  // _config.rightGearbox.encoder->GetEncoderDistance() * 1_m;
-  return _config.elevatorEncoder.GetPosition() * 14 / 60 * 2 * 3.1415 *
-         0.02225 * 1_m;
-}
-
-units::meters_per_second_t wom::subsystems::Elevator::GetElevatorVelocity()
-    const {
-  return _config.elevatorEncoder.GetVelocity() / 60_s * 14 / 60 * 2 * 3.1415 *
-         0.02225 * 1_m;
-}
-
-units::meters_per_second_t wom::subsystems::Elevator::MaxSpeed() const {
-  return _config.leftGearbox.motor.Speed(
-             (_config.mass * 9.81_mps_sq) * _config.radius, 12_V) /
-         1_rad * _config.radius;
-}
- */
\ No newline at end of file
diff --git a/wombat/src/main/cpp/subsystems/Shooter.cpp b/wombat/src/main/cpp/subsystems/Shooter.cpp
deleted file mode 100644
index aa2fe679..00000000
--- a/wombat/src/main/cpp/subsystems/Shooter.cpp
+++ /dev/null
@@ -1,95 +0,0 @@
-/* // Copyright (c) 2023-2024 CurtinFRC
-// Open Source Software, you can modify it according to the terms
-// of the MIT License at the root of this project
-
-#include "subsystems/Shooter.h"
-
-#include <networktables/NetworkTableInstance.h>
-
-wom::subsystems::Shooter::Shooter(std::string path, ShooterParams params)
-    : _params(params),
-      _state(ShooterState::kIdle),
-      _pid{path + "/pid", params.pid},
-      _table(nt::NetworkTableInstance::GetDefault().GetTable("shooter")) {}
-
-void wom::subsystems::Shooter::OnUpdate(units::second_t dt) {
-  units::volt_t voltage{0};
-  units::revolutions_per_minute_t currentSpeed =
-      _params.gearbox.encoder->GetEncoderAngularVelocity();
-
-  switch (_state) {
-    case ShooterState::kManual:
-      voltage = _setpointManual;
-      break;
-    case ShooterState::kPID: {
-      auto feedforward =
-          _params.gearbox.motor.Voltage(0_Nm, _pid.GetSetpoint());
-      voltage = _pid.Calculate(currentSpeed, dt, feedforward);
-    } break;
-    case ShooterState::kIdle:
-      voltage = 0_V;
-      break;
-  }
-
-  units::newton_meter_t max_torque_at_current_limit =
-      _params.gearbox.motor.Torque(_params.currentLimit);
-  units::volt_t max_voltage_for_current_limit =
-      _params.gearbox.motor.Voltage(max_torque_at_current_limit, currentSpeed);
-
-  voltage =
-      1_V * std::min(voltage.value(), max_voltage_for_current_limit.value());
-
-  // _params.gearbox.motorController->SetVoltage(voltage);
-
-  _table->GetEntry("output_volts").SetDouble(voltage.value());
-  _table->GetEntry("speed_rpm").SetDouble(currentSpeed.value());
-  _table->GetEntry("setpoint_rpm")
-      .SetDouble(units::revolutions_per_minute_t{_pid.GetSetpoint()}.value());
-  _table->GetEntry("stable").SetBoolean(_pid.IsStable());
-}
-
-void wom::subsystems::Shooter::SetManual(units::volt_t voltage) {
-  _setpointManual = voltage;
-}
-
-void wom::subsystems::Shooter::SetPID(units::radians_per_second_t goal) {
-  _state = ShooterState::kPID;
-  _pid.SetSetpoint(goal);
-}
-
-void wom::subsystems::Shooter::SetState(ShooterState state) {
-  _state = state;
-}
-
-bool wom::subsystems::Shooter::IsStable() const {
-  return _pid.IsStable();
-}
-
-// Shooter Manual Set
-
-wom::subsystems::ShooterConstant::ShooterConstant(Shooter* s,
-                                                  units::volt_t setpoint)
-    : _shooter(s), _setpoint(setpoint) {
-  Controls(_shooter);
-}
-
-void wom::subsystems::ShooterConstant::OnTick(units::second_t dt) {
-  _shooter->SetManual(_setpoint);
-}
-
-// ShooterSpinup
-
-wom::subsystems::ShooterSpinup::ShooterSpinup(Shooter* s,
-                                              units::radians_per_second_t speed,
-                                              bool hold)
-    : _shooter(s), _speed(speed), _hold(hold) {
-  Controls(_shooter);
-}
-
-void wom::subsystems::ShooterSpinup::OnTick(units::second_t dt) {
-  _shooter->SetPID(_speed);
-
-  if (!_hold && _shooter->IsStable())
-    SetDone();
-}
- */
\ No newline at end of file
diff --git a/wombat/src/main/cpp/utils/Encoder.cpp b/wombat/src/main/cpp/utils/Encoder.cpp
index 40b37b74..1120b399 100644
--- a/wombat/src/main/cpp/utils/Encoder.cpp
+++ b/wombat/src/main/cpp/utils/Encoder.cpp
@@ -6,8 +6,8 @@
 
 #include <rev/SparkRelativeEncoder.h>
 
-wom::utils::Encoder::Encoder(double encoderTicksPerRotation, int type,
-                             units::meter_t wheelRadius, double reduction)
+wom::utils::Encoder::Encoder(double encoderTicksPerRotation, int type, units::meter_t wheelRadius,
+                             double reduction)
     : _reduction(reduction),
       _encoderTicksPerRotation(encoderTicksPerRotation),
       _type(type),
@@ -32,8 +32,8 @@ void wom::utils::Encoder::SetEncoderPosition(units::degree_t position) {
   // _offset = -offset_turns;
 }
 
-void wom::utils::Encoder::SetEncoderOffset(units::radian_t offset) { //HERE!
-  _offset = offset; 
+void wom::utils::Encoder::SetEncoderOffset(units::radian_t offset) {  // HERE!
+  _offset = offset;
   // units::turn_t offset_turns = offset;
   // _offset = offset_turns.value() * GetEncoderTicksPerRotation();
 }
@@ -43,16 +43,16 @@ void wom::utils::Encoder::SetReduction(double reduction) {
 }
 
 units::radian_t wom::utils::Encoder::GetEncoderPosition() {
-  //if (_type == 0) {
-  //  units::turn_t n_turns{GetEncoderTicks() / GetEncoderTicksPerRotation()};
-  //  return n_turns;
-  //} else if (_type == 2) {
-  //  units::degree_t pos = GetEncoderTicks() * 1_deg;
-  //  return pos;
-  //} else {
-  //  units::degree_t pos = GetEncoderTicks() * 1_deg;
-  //  return pos - _offset;
-  //}
+  // if (_type == 0) {
+  //   units::turn_t n_turns{GetEncoderTicks() / GetEncoderTicksPerRotation()};
+  //   return n_turns;
+  // } else if (_type == 2) {
+  //   units::degree_t pos = GetEncoderTicks() * 1_deg;
+  //   return pos;
+  // } else {
+  //   units::degree_t pos = GetEncoderTicks() * 1_deg;
+  //   return pos - _offset;
+  // }
   return GetEncoderTicks() * 1_rad;
 }
 
@@ -63,8 +63,7 @@ double wom::utils::Encoder::GetEncoderDistance() {
 units::radians_per_second_t wom::utils::Encoder::GetEncoderAngularVelocity() {
   // return GetEncoderTickVelocity() / (double)GetEncoderTicksPerRotation() * 2
   // * 3.1415926;
-  units::turns_per_second_t n_turns_per_s{GetEncoderTickVelocity() /
-                                          GetEncoderTicksPerRotation()};
+  units::turns_per_second_t n_turns_per_s{GetEncoderTickVelocity() / GetEncoderTicksPerRotation()};
   return n_turns_per_s;
 }
 
@@ -86,12 +85,10 @@ double wom::utils::DigitalEncoder::GetEncoderTickVelocity() const {
   return _nativeEncoder.GetRate();
 }
 
-wom::utils::CANSparkMaxEncoder::CANSparkMaxEncoder(rev::CANSparkMax* controller,
-                                                   units::meter_t wheelRadius,
+wom::utils::CANSparkMaxEncoder::CANSparkMaxEncoder(rev::CANSparkMax* controller, units::meter_t wheelRadius,
                                                    double reduction)
     : wom::utils::Encoder(42, reduction, wheelRadius, 2),
-      _encoder(controller->GetEncoder(
-          rev::SparkRelativeEncoder::Type::kHallSensor)) {}
+      _encoder(controller->GetEncoder(rev::SparkRelativeEncoder::Type::kHallSensor)) {}
 
 double wom::utils::CANSparkMaxEncoder::GetEncoderRawTicks() const {
   return ((_encoder.GetPosition() * 2 * 3.1415) / 200);
@@ -121,11 +118,9 @@ double wom::utils::CANSparkMaxEncoder::GetVelocity() const {
   return _encoder.GetVelocity();
 }
 
-wom::utils::TalonFXEncoder::TalonFXEncoder(
-    ctre::phoenix6::hardware::TalonFX* controller, units::meter_t wheelRadius,
-    double reduction)
-    : utils::Encoder(2048, reduction, wheelRadius, 0),
-      _controller(controller) {}
+wom::utils::TalonFXEncoder::TalonFXEncoder(ctre::phoenix6::hardware::TalonFX* controller,
+                                           units::meter_t wheelRadius, double reduction)
+    : utils::Encoder(2048, reduction, wheelRadius, 0), _controller(controller) {}
 
 double wom::utils::TalonFXEncoder::GetEncoderRawTicks() const {
   return _controller->GetPosition().GetValue().value();
@@ -135,12 +130,9 @@ double wom::utils::TalonFXEncoder::GetEncoderTickVelocity() const {
   return _controller->Get() * 10;
 }
 
-wom::utils::DutyCycleEncoder::DutyCycleEncoder(int channel,
-                                               units::meter_t wheelRadius,
-                                               double ticksPerRotation,
-                                               double reduction)
-    : wom::utils::Encoder(ticksPerRotation, reduction, wheelRadius, 0),
-      _dutyCycleEncoder(channel) {}
+wom::utils::DutyCycleEncoder::DutyCycleEncoder(int channel, units::meter_t wheelRadius,
+                                               double ticksPerRotation, double reduction)
+    : wom::utils::Encoder(ticksPerRotation, reduction, wheelRadius, 0), _dutyCycleEncoder(channel) {}
 
 double wom::utils::DutyCycleEncoder::GetEncoderRawTicks() const {
   return _dutyCycleEncoder.Get().value();
@@ -150,13 +142,11 @@ double wom::utils::DutyCycleEncoder::GetEncoderTickVelocity() const {
   return 0;
 }
 
-wom::utils::CanEncoder::CanEncoder(int deviceNumber, units::meter_t wheelRadius,
-                                   double ticksPerRotation, double reduction,
-                                   std::string name)
+wom::utils::CanEncoder::CanEncoder(int deviceNumber, units::meter_t wheelRadius, double ticksPerRotation,
+                                   double reduction, std::string name)
     : wom::utils::Encoder(ticksPerRotation, 2, wheelRadius, reduction) {
   _canEncoder = new ctre::phoenix6::hardware::CANcoder(deviceNumber, name);
   // _canEncoder->ConfigAbsoluteEncoderRange(0, 1);
-
 }
 
 double wom::utils::CanEncoder::GetEncoderRawTicks() const {
@@ -165,4 +155,4 @@ double wom::utils::CanEncoder::GetEncoderRawTicks() const {
 
 double wom::utils::CanEncoder::GetEncoderTickVelocity() const {
   return _canEncoder->GetVelocity().GetValue().value();
-}
\ No newline at end of file
+}
diff --git a/wombat/src/main/include/Wombat.h b/wombat/src/main/include/Wombat.h
index c1b356a9..f0037344 100644
--- a/wombat/src/main/include/Wombat.h
+++ b/wombat/src/main/include/Wombat.h
@@ -7,12 +7,8 @@
 #include "behaviour/Behaviour.h"
 #include "behaviour/BehaviourScheduler.h"
 #include "behaviour/HasBehaviour.h"
-#include "drivetrain/Drivetrain.h"
 #include "drivetrain/SwerveDrive.h"
 #include "drivetrain/behaviours/SwerveBehaviours.h"
-#include "subsystems/Arm.h"
-#include "subsystems/Elevator.h"
-#include "subsystems/Shooter.h"
 #include "utils/Encoder.h"
 #include "utils/Gearbox.h"
 #include "utils/PID.h"
@@ -23,7 +19,6 @@
 namespace wom {
 using namespace wom;
 using namespace wom::utils;
-// using namespace wom::subsystems;
 using namespace wom::drivetrain;
 using namespace wom::drivetrain::behaviours;
 using namespace wom::vision;
diff --git a/wombat/src/main/include/drivetrain/Drivetrain.h b/wombat/src/main/include/drivetrain/Drivetrain.h
deleted file mode 100644
index b3894217..00000000
--- a/wombat/src/main/include/drivetrain/Drivetrain.h
+++ /dev/null
@@ -1,58 +0,0 @@
-/* // Copyright (c) 2023-2024 CurtinFRC
-// Open Source Software, you can modify it according to the terms
-// of the MIT License at the root of this project
-
-#pragma once
-
-#include <frc/XboxController.h>
-#include <units/time.h>
-#include <units/voltage.h>
-
-#include <string>
-
-#include "behaviour/HasBehaviour.h"
-#include "utils/Gearbox.h"
-
-namespace wom {
-namespace drivetrain {
-// TODO PID
-struct DrivetrainConfig {
-  std::string path;
-
-  wom::utils::Gearbox left1;
-  wom::utils::Gearbox left2;
-  wom::utils::Gearbox left3;
-  wom::utils::Gearbox right1;
-  wom::utils::Gearbox right2;
-  wom::utils::Gearbox right3;
-};
-
-enum DrivetrainState {
-  kIdle,
-  kTank,
-  kAuto,
-};
-
-class Drivetrain : public behaviour::HasBehaviour {
- public:
-  Drivetrain(DrivetrainConfig* config, frc::XboxController& driver);
-  ~Drivetrain();
-
-  DrivetrainConfig* GetConfig();
-  DrivetrainState GetState();
-
-  void SetState(DrivetrainState state);
-
-  void OnStart();
-  void OnUpdate(units::second_t dt);
-
- protected:
- private:
-  DrivetrainConfig* _config;
-  DrivetrainState _state;
-  frc::XboxController& _driver;
-  units::volt_t maxVolts = 9_V;
-};
-}  // namespace drivetrain
-}  // namespace wom
- */
\ No newline at end of file
diff --git a/wombat/src/main/include/drivetrain/SwerveDrive.h b/wombat/src/main/include/drivetrain/SwerveDrive.h
index 625ef135..d8b8e482 100644
--- a/wombat/src/main/include/drivetrain/SwerveDrive.h
+++ b/wombat/src/main/include/drivetrain/SwerveDrive.h
@@ -4,9 +4,9 @@
 
 #pragma once
 
+#include <frc/controller/PIDController.h>
 #include <frc/estimator/SwerveDrivePoseEstimator.h>
 #include <frc/kinematics/SwerveDriveKinematics.h>
-#include <frc/controller/PIDController.h>
 #include <networktables/DoubleTopic.h>
 #include <networktables/NetworkTable.h>
 #include <networktables/NetworkTableInstance.h>
@@ -16,7 +16,12 @@
 #include <units/moment_of_inertia.h>
 #include <units/time.h>
 #include <units/velocity.h>
+#include <wpi/SymbolExports.h>
+#include <wpi/sendable/Sendable.h>
+#include <wpi/sendable/SendableHelper.h>
 
+#include <functional>
+#include <limits>
 #include <memory>
 #include <string>
 #include <vector>
@@ -28,19 +33,9 @@
 #include "utils/Gearbox.h"
 #include "utils/PID.h"
 
-#include <functional>
-#include <limits>
-
-#include <wpi/SymbolExports.h>
-#include <wpi/sendable/Sendable.h>
-#include <wpi/sendable/SendableHelper.h>
-
-
 namespace wom {
 namespace drivetrain {
 
-
-
 enum class SwerveModuleState { kZeroing, kIdle, kPID };
 enum class TurnOffsetValues { reverse, forward, none };
 
@@ -59,9 +54,8 @@ struct SwerveModuleConfig {
 
 class SwerveModule {
  public:
-  //using angle_pid_conf_t = utils::PIDConfig<units::radian, units::volt>;
-  using velocity_pid_conf_t =
-      utils::PIDConfig<units::meters_per_second, units::volt>;
+  // using angle_pid_conf_t = utils::PIDConfig<units::radian, units::volt>;
+  using velocity_pid_conf_t = utils::PIDConfig<units::meters_per_second, units::volt>;
 
   SwerveModule(std::string path, SwerveModuleConfig config,
                /*angle_pid_conf_t anglePID,*/ velocity_pid_conf_t velocityPID);
@@ -78,8 +72,7 @@ class SwerveModule {
 
   void SetZero(units::second_t dt);
   void SetIdle();
-  void SetPID(units::radian_t angle, units::meters_per_second_t speed,
-              units::second_t dt);
+  void SetPID(units::radian_t angle, units::meters_per_second_t speed, units::second_t dt);
   void SetZero();
   void SetVoltageLimit(units::volt_t driveModuleVoltageLimit);
 
@@ -101,7 +94,7 @@ class SwerveModule {
 
   const SwerveModuleConfig& GetConfig() const;
 
-  //utils::PIDController<units::radians, units::volt> _anglePIDController;
+  // utils::PIDController<units::radians, units::volt> _anglePIDController;
   frc::PIDController _anglePIDController;
 
  private:
@@ -127,20 +120,18 @@ class SwerveModule {
 struct SwerveDriveConfig {
   /*using pose_angle_conf_t =
       utils::PIDConfig<units::radian, units::radians_per_second>;*/
-  using pose_position_conf_t =
-      utils::PIDConfig<units::meter, units::meters_per_second>;
-  using balance_conf_t =
-      utils::PIDConfig<units::degree, units::meters_per_second>;
+  using pose_position_conf_t = utils::PIDConfig<units::meter, units::meters_per_second>;
+  using balance_conf_t = utils::PIDConfig<units::degree, units::meters_per_second>;
 
   std::string path;
-  //SwerveModule::angle_pid_conf_t anglePID;
+  // SwerveModule::angle_pid_conf_t anglePID;
   SwerveModule::velocity_pid_conf_t velocityPID;
 
   wpi::array<SwerveModuleConfig, 4> modules;
 
   ctre::phoenix6::hardware::Pigeon2* gyro;
 
-  //pose_angle_conf_t poseAnglePID;
+  // pose_angle_conf_t poseAnglePID;
   pose_position_conf_t posePositionPID;
 
   units::kilogram_t mass;
@@ -194,8 +185,7 @@ class SwerveDrive : public behaviour::HasBehaviour {
    * @brief This function switches the state to handle the robot's rotation
    * matching that of the joystick
    */
-  void RotateMatchJoystick(units::radian_t joystickAngle,
-                           FieldRelativeSpeeds speeds);
+  void RotateMatchJoystick(units::radian_t joystickAngle, FieldRelativeSpeeds speeds);
 
   void SetIdle();
 
@@ -205,8 +195,7 @@ class SwerveDrive : public behaviour::HasBehaviour {
   void SetFieldRelativeVelocity(FieldRelativeSpeeds speeds);
   void SetPose(frc::Pose2d pose);
   bool IsAtSetPose();
-  void SetIndividualTuning(int mod, units::radian_t angle,
-                           units::meters_per_second_t speed);
+  void SetIndividualTuning(int mod, units::radian_t angle, units::meters_per_second_t speed);
   void SetTuning(units::radian_t angle, units::meters_per_second_t speed);
   void SetZero();
   void SetVoltageLimit(units::volt_t driveVoltageLimit);
@@ -262,4 +251,4 @@ class SwerveDrive : public behaviour::HasBehaviour {
   // double backRightEncoderOffset = -119.619140625;
 };
 }  // namespace drivetrain
-}  // namespace wom
\ No newline at end of file
+}  // namespace wom
diff --git a/wombat/src/main/include/drivetrain/behaviours/SwerveBehaviours.h b/wombat/src/main/include/drivetrain/behaviours/SwerveBehaviours.h
index d49d3f12..d9f844a5 100644
--- a/wombat/src/main/include/drivetrain/behaviours/SwerveBehaviours.h
+++ b/wombat/src/main/include/drivetrain/behaviours/SwerveBehaviours.h
@@ -37,16 +37,14 @@ class ManualDrivebase : public behaviour::Behaviour {
    * A pointer to the controller that the driver has been allocated (the
    * allocated memory address that stores the "driver controller" object)
    */
-  ManualDrivebase(wom::drivetrain::SwerveDrive* swerveDrivebase,
-                  frc::XboxController* driverController);
+  ManualDrivebase(wom::drivetrain::SwerveDrive* swerveDrivebase, frc::XboxController* driverController);
 
   void OnTick(units::second_t deltaTime) override;
   /**
    * @brief This function handles all of the logic behind the tangent function,
    * to be able to calculate an angle between 0 andd 360 degrees, inclusively
    */
-  void CalculateRequestedAngle(double joystickX, double joystickY,
-                               units::degree_t defaultAngle);
+  void CalculateRequestedAngle(double joystickX, double joystickY, units::degree_t defaultAngle);
   void OnStart() override;
   void ResetMode();
 
@@ -72,8 +70,8 @@ class ManualDrivebase : public behaviour::Behaviour {
   const double turningDeadzone = 0.2;
 
   // Variables for solution to Anti-tip
-  double prevJoystickX, prevJoystickY, prevPrevJoystickX, prevPrevJoystickY,
-      usingJoystickXPos, usingJoystickYPos;
+  double prevJoystickX, prevJoystickY, prevPrevJoystickX, prevPrevJoystickY, usingJoystickXPos,
+      usingJoystickYPos;
   // The speed that the joystick must travel to activate averaging over previous
   // 3 joystick positions
   const double smoothingThreshold = 1;
@@ -126,8 +124,8 @@ class GoToPose : public behaviour::Behaviour {
 
 class FollowTrajectory : public behaviour::Behaviour {
  public:
-  FollowTrajectory(wom::drivetrain::SwerveDrive* swerve,
-                   wom::utils::Pathplanner* pathplanner, std::string path);
+  FollowTrajectory(wom::drivetrain::SwerveDrive* swerve, wom::utils::Pathplanner* pathplanner,
+                   std::string path);
 
   void OnTick(units::second_t dt) override;
 
@@ -184,8 +182,7 @@ class TempSimSwerveDrive {
 
 class AutoSwerveDrive {
  public:
-  AutoSwerveDrive(wom::drivetrain::SwerveDrive* swerve, frc::Timer* timer,
-                  frc::Field2d* field);
+  AutoSwerveDrive(wom::drivetrain::SwerveDrive* swerve, frc::Timer* timer, frc::Field2d* field);
 
   void OnUpdate();
 
@@ -237,4 +234,4 @@ class DrivebasePoseBehaviour : public behaviour::Behaviour {
 };
 }  // namespace behaviours
 }  // namespace drivetrain
-}  // namespace wom
\ No newline at end of file
+}  // namespace wom
diff --git a/wombat/src/main/include/subsystems/Arm.h b/wombat/src/main/include/subsystems/Arm.h
deleted file mode 100644
index 62c2d05b..00000000
--- a/wombat/src/main/include/subsystems/Arm.h
+++ /dev/null
@@ -1,82 +0,0 @@
-/* // Copyright (c) 2023-2024 CurtinFRC
-// Open Source Software, you can modify it according to the terms
-// of the MIT License at the root of this project
-
-#pragma once
-
-#include <frc/DigitalInput.h>
-#include <frc/simulation/DIOSim.h>
-#include <units/current.h>
-#include <units/mass.h>
-#include <units/voltage.h>
-
-#include <memory>
-#include <string>
-
-#include "behaviour/HasBehaviour.h"
-#include "utils/Gearbox.h"
-#include "utils/PID.h"
-
-namespace wom {
-namespace subsystems {
-struct ArmConfig {
-  std::string path;
-
-  wom::utils::Gearbox leftGearbox;
-  wom::utils::Gearbox rightGearbox;
-  wom::utils::CANSparkMaxEncoder armEncoder;
-  wom::utils::PIDConfig<units::radian, units::volt> pidConfig;
-  wom::utils::PIDConfig<units::radians_per_second, units::volt> velocityConfig;
-
-  units::kilogram_t armMass;
-  units::kilogram_t loadMass;
-  units::meter_t armLength;
-  units::radian_t minAngle = 0_deg;
-  units::radian_t maxAngle = 180_deg;
-  units::radian_t initialAngle = 0_deg;
-  units::radian_t angleOffset = 0_deg;
-
-  void WriteNT(std::shared_ptr<nt::NetworkTable> table);
-};
-
-enum class ArmState { kIdle, kAngle, kRaw, kVelocity };
-
-class Arm : public behaviour::HasBehaviour {
- public:
-  explicit Arm(ArmConfig config);
-
-  void OnUpdate(units::second_t dt);
-
-  void SetIdle();
-  void SetAngle(units::radian_t angle);
-  void SetRaw(units::volt_t voltage);
-  void SetVelocity(units::radians_per_second_t velocity);
-
-  void SetState(wom::subsystems::ArmState state);
-
-  void SetArmSpeedLimit(double limit);  // units, what are they??
-
-  ArmConfig& GetConfig();
-
-  units::radian_t GetAngle() const;
-  units::radians_per_second_t MaxSpeed() const;
-  units::radians_per_second_t GetArmVelocity() const;
-
-  bool IsStable() const;
-
- private:
-  ArmConfig _config;
-  ArmState _state = ArmState::kIdle;
-  wom::utils::PIDController<units::radian, units::volt> _pid;
-  wom::utils::PIDController<units::radians_per_second, units::volt> _velocityPID;
-
-  std::shared_ptr<nt::NetworkTable> _table;
-
-  double armLimit = 0.4;
-  units::radians_per_second_t lastVelocity;
-
-  units::volt_t _voltage{0};
-};
-}  // namespace subsystems
-}  // namespace wom
- */
\ No newline at end of file
diff --git a/wombat/src/main/include/subsystems/Elevator.h b/wombat/src/main/include/subsystems/Elevator.h
deleted file mode 100644
index ccbe2379..00000000
--- a/wombat/src/main/include/subsystems/Elevator.h
+++ /dev/null
@@ -1,85 +0,0 @@
-/* // Copyright (c) 2023-2024 CurtinFRC
-// Open Source Software, you can modify it according to the terms
-// of the MIT License at the root of this project
-
-#pragma once
-
-#include <frc/DigitalInput.h>
-#include <frc/simulation/DIOSim.h>
-#include <frc/simulation/ElevatorSim.h>
-#include <networktables/NetworkTable.h>
-#include <units/length.h>
-#include <units/mass.h>
-
-#include <memory>
-#include <string>
-
-#include "behaviour/HasBehaviour.h"
-#include "utils/Encoder.h"
-#include "utils/Gearbox.h"
-#include "utils/PID.h"
-
-namespace wom {
-namespace subsystems {
-enum class ElevatorState { kIdle, kPID, kManual, kVelocity };
-
-struct ElevatorConfig {
-  std::string path;
-  wom::utils::Gearbox leftGearbox;
-  wom::utils::Gearbox rightGearbox;
-  wom::utils::CANSparkMaxEncoder elevatorEncoder;
-  frc::DigitalInput* topSensor;
-  frc::DigitalInput* bottomSensor;
-  units::meter_t radius;
-  units::kilogram_t mass;
-  units::meter_t maxHeight;
-  units::meter_t minHeight;
-  units::meter_t initialHeight;
-  wom::utils::PIDConfig<units::meter, units::volt> pid;
-  wom::utils::PIDConfig<units::meters_per_second, units::volt> velocityPID;
-
-  void WriteNT(std::shared_ptr<nt::NetworkTable> table);
-};
-
-class Elevator : public behaviour::HasBehaviour {
- public:
-  explicit Elevator(ElevatorConfig params);
-
-  void OnUpdate(units::second_t dt);
-
-  void SetManual(units::volt_t voltage);
-  void SetPID(units::meter_t height);
-
-  void SetState(ElevatorState state);
-
-  void SetVelocity(units::meters_per_second_t velocity);
-
-  double GetElevatorEncoderPos();
-  void SetElevatorSpeedLimit(double limit);
-
-  ElevatorConfig& GetConfig();
-
-  bool IsStable() const;
-  ElevatorState GetState() const;
-
-  units::meter_t GetHeight() const;
-  units::meters_per_second_t MaxSpeed() const;
-  units::meters_per_second_t GetElevatorVelocity() const;
-
- private:
-  units::volt_t _setpointManual{0};
-
-  ElevatorConfig _config;
-  ElevatorState _state;
-  double speedLimit = 0.5;
-
-  units::meters_per_second_t _velocity;
-
-  wom::utils::PIDController<units::meter, units::volt> _pid;
-  wom::utils::PIDController<units::meters_per_second, units::volt> _velocityPID;
-
-  std::shared_ptr<nt::NetworkTable> _table;
-};
-}  // namespace subsystems
-}  // namespace wom
- */
\ No newline at end of file
diff --git a/wombat/src/main/include/subsystems/Shooter.h b/wombat/src/main/include/subsystems/Shooter.h
deleted file mode 100644
index 694a6d57..00000000
--- a/wombat/src/main/include/subsystems/Shooter.h
+++ /dev/null
@@ -1,76 +0,0 @@
-/* // Copyright (c) 2023-2024 CurtinFRC
-// Open Source Software, you can modify it according to the terms
-// of the MIT License at the root of this project
-
-#pragma once
-
-#include <networktables/NetworkTable.h>
-#include <units/angular_velocity.h>
-#include <units/charge.h>
-
-#include <memory>
-#include <string>
-
-#include "behaviour/Behaviour.h"
-#include "behaviour/HasBehaviour.h"
-#include "utils/Gearbox.h"
-#include "utils/PID.h"
-
-namespace wom {
-namespace subsystems {
-enum class ShooterState { kPID, kManual, kIdle };
-
-struct ShooterParams {
-  wom::utils::Gearbox gearbox;
-  wom::utils::PIDConfig<units::radians_per_second, units::volt> pid;
-  units::ampere_t currentLimit;
-};
-
-class Shooter : public behaviour::HasBehaviour {
- public:
-  Shooter(std::string path, ShooterParams params);
-
-  void SetManual(units::volt_t voltage);
-  void SetPID(units::radians_per_second_t goal);
-  void SetState(ShooterState state);
-
-  void OnUpdate(units::second_t dt);
-
-  bool IsStable() const;
-
- private:
-  ShooterParams _params;
-  ShooterState _state;
-
-  units::volt_t _setpointManual{0};
-
-  wom::utils::PIDController<units::radians_per_second, units::volt> _pid;
-
-  std::shared_ptr<nt::NetworkTable> _table;
-};
-
-class ShooterConstant : public behaviour::Behaviour {
- public:
-  ShooterConstant(Shooter* s, units::volt_t setpoint);
-
-  void OnTick(units::second_t dt) override;
-
- private:
-  Shooter* _shooter;
-  units::volt_t _setpoint;
-};
-
-class ShooterSpinup : public behaviour::Behaviour {
- public:
-  ShooterSpinup(Shooter* s, units::radians_per_second_t speed, bool hold = false);
-
-  void OnTick(units::second_t dt) override;
-
- private:
-  Shooter* _shooter;
-  units::radians_per_second_t _speed;
-  bool _hold;
-};
-}  // namespace subsystems
-}  // namespace wom
- */
\ No newline at end of file
diff --git a/wombat/src/main/include/utils/Encoder.h b/wombat/src/main/include/utils/Encoder.h
index 073ba6cd..b9aae2a4 100644
--- a/wombat/src/main/include/utils/Encoder.h
+++ b/wombat/src/main/include/utils/Encoder.h
@@ -20,8 +20,7 @@ namespace wom {
 namespace utils {
 class Encoder {
  public:
-  Encoder(double encoderTicksPerRotation, int type, units::meter_t wheelRadius,
-          double reduction = 1.0);
+  Encoder(double encoderTicksPerRotation, int type, units::meter_t wheelRadius, double reduction = 1.0);
 
   virtual double GetEncoderRawTicks() const = 0;
   virtual double GetEncoderTickVelocity() const = 0;  // ticks/s
@@ -45,7 +44,8 @@ class Encoder {
   virtual double GetVelocity() const = 0;
   double GetVelocityValue() const;
 
-  units::radian_t _offset = 0_rad; //bad
+  units::radian_t _offset = 0_rad;  // bad
+
  private:
   double _encoderTicksPerRotation;
   int _type = 0;
@@ -54,24 +54,23 @@ class Encoder {
 
 class DigitalEncoder : public Encoder {
  public:
-  DigitalEncoder(int channelA, int channelB, double ticksPerRotation,
-                 units::meter_t wheelRadius, double reduction = 1)
-      : Encoder(ticksPerRotation, reduction, wheelRadius, 0),
-        _nativeEncoder(channelA, channelB) {}
+  DigitalEncoder(int channelA, int channelB, double ticksPerRotation, units::meter_t wheelRadius,
+                 double reduction = 1)
+      : Encoder(ticksPerRotation, reduction, wheelRadius, 0), _nativeEncoder(channelA, channelB) {}
 
   double GetEncoderRawTicks() const override;
   double GetEncoderTickVelocity() const override;
 
   double GetPosition() const;
   double GetVelocity() const override;
+
  private:
   frc::Encoder _nativeEncoder;
 };
 
 class CANSparkMaxEncoder : public Encoder {
  public:
-  explicit CANSparkMaxEncoder(rev::CANSparkMax* controller,
-                              units::meter_t wheelRadius, double reduction = 1);
+  explicit CANSparkMaxEncoder(rev::CANSparkMax* controller, units::meter_t wheelRadius, double reduction = 1);
 
   double GetEncoderRawTicks() const override;
   double GetEncoderTickVelocity() const override;
@@ -86,8 +85,8 @@ class CANSparkMaxEncoder : public Encoder {
 
 class TalonFXEncoder : public Encoder {
  public:
-  TalonFXEncoder(ctre::phoenix6::hardware::TalonFX* controller,
-                 units::meter_t wheelRadius, double reduction = 1);
+  TalonFXEncoder(ctre::phoenix6::hardware::TalonFX* controller, units::meter_t wheelRadius,
+                 double reduction = 1);
 
   double GetEncoderRawTicks() const override;
   double GetEncoderTickVelocity() const override;
@@ -99,8 +98,8 @@ class TalonFXEncoder : public Encoder {
 
 class DutyCycleEncoder : public Encoder {
  public:
-  DutyCycleEncoder(int channel, units::meter_t wheelRadius,
-                   double ticksPerRotation = 1, double reduction = 1);
+  DutyCycleEncoder(int channel, units::meter_t wheelRadius, double ticksPerRotation = 1,
+                   double reduction = 1);
 
   double GetEncoderRawTicks() const override;
   double GetEncoderTickVelocity() const override;
@@ -112,9 +111,8 @@ class DutyCycleEncoder : public Encoder {
 
 class CanEncoder : public Encoder {
  public:
-  CanEncoder(int deviceNumber, units::meter_t wheelRadius,
-             double ticksPerRotation = 4095, double reduction = 6.75,
-             std::string name = "Drivebase");
+  CanEncoder(int deviceNumber, units::meter_t wheelRadius, double ticksPerRotation = 4095,
+             double reduction = 6.75, std::string name = "Drivebase");
 
   double GetEncoderRawTicks() const override;
   double GetEncoderTickVelocity() const override;
@@ -127,4 +125,4 @@ class CanEncoder : public Encoder {
   ctre::phoenix6::hardware::CANcoder* _canEncoder;
 };
 }  // namespace utils
-}  // namespace wom
\ No newline at end of file
+}  // namespace wom

From a3d1491b6a78d76f057cd22711c8734e8860c858 Mon Sep 17 00:00:00 2001
From: Anna Pedersen <59428185+goanna247@users.noreply.github.com>
Date: Tue, 23 Jan 2024 20:21:33 +0800
Subject: [PATCH 130/207] Swerve fix (#106)

* fixed swerve

* wrapping function
---
 .clang-format                                 |   2 +-
 .editorconfig                                 |   5 +
 .github/workflows/format.yml                  |   2 +-
 README.md                                     |   4 -
 build.gradle                                  |   2 +-
 init.ps1                                      |   2 +
 init.sh                                       |   5 +
 src/main/cpp/Robot.cpp                        |  34 +++-
 src/main/include/Robot.h                      |   2 -
 src/main/include/RobotMap.h                   |  13 +-
 wombat/src/main/cpp/behaviour/Behaviour.cpp   |  23 ++-
 .../main/cpp/behaviour/BehaviourScheduler.cpp |   4 +-
 .../src/main/cpp/behaviour/HasBehaviour.cpp   |   3 +-
 .../src/main/cpp/drivetrain/SwerveDrive.cpp   |   2 +-
 .../behaviours/SwerveBehaviours.cpp           |   2 +-
 wombat/src/main/cpp/subsystems/Arm.cpp        | 192 ++++++++++++++++++
 wombat/src/main/cpp/utils/Encoder.cpp         |  44 ++--
 wombat/src/main/cpp/utils/Util.cpp            |  27 ++-
 wombat/src/main/cpp/vision/Limelight.cpp      |  24 ++-
 wombat/src/main/include/behaviour/Behaviour.h |  42 ++--
 .../src/main/include/behaviour/HasBehaviour.h |   3 +-
 .../src/main/include/drivetrain/SwerveDrive.h |   2 +-
 .../drivetrain/behaviours/SwerveBehaviours.h  |   2 +-
 wombat/src/main/include/subsystems/Arm.h      |  82 ++++++++
 wombat/src/main/include/subsystems/Shooter.h  |  76 +++++++
 wombat/src/main/include/utils/Encoder.h       |   2 +-
 wombat/src/main/include/utils/PID.h           |   2 +-
 wombat/src/main/include/utils/Util.h          |  55 +++--
 wombat/src/main/include/vision/Limelight.h    |  22 +-
 29 files changed, 570 insertions(+), 110 deletions(-)
 create mode 100644 .editorconfig
 create mode 100644 init.ps1
 create mode 100644 init.sh
 create mode 100644 wombat/src/main/cpp/subsystems/Arm.cpp
 create mode 100644 wombat/src/main/include/subsystems/Arm.h
 create mode 100644 wombat/src/main/include/subsystems/Shooter.h

diff --git a/.clang-format b/.clang-format
index a879dec4..f69bef0c 100644
--- a/.clang-format
+++ b/.clang-format
@@ -81,7 +81,7 @@ BreakBeforeTernaryOperators: true
 BreakConstructorInitializers: BeforeColon
 BreakInheritanceList: BeforeColon
 BreakStringLiterals: true
-ColumnLimit:     110
+ColumnLimit:     80
 CommentPragmas:  '^ IWYU pragma:'
 CompactNamespaces: false
 ConstructorInitializerIndentWidth: 4
diff --git a/.editorconfig b/.editorconfig
new file mode 100644
index 00000000..0020fc03
--- /dev/null
+++ b/.editorconfig
@@ -0,0 +1,5 @@
+root = true
+
+[*]
+indent_style = space
+indent_size = 2
diff --git a/.github/workflows/format.yml b/.github/workflows/format.yml
index f57ddeb4..c6fced20 100644
--- a/.github/workflows/format.yml
+++ b/.github/workflows/format.yml
@@ -21,7 +21,7 @@ jobs:
         with:
           python-version: 3.8
       - name: Install wpiformat
-        run: pip3 install wpiformat==2024.31
+        run: pip3 install wpiformat==2023.36
       - name: Run
         run: wpiformat
       - name: Check output
diff --git a/README.md b/README.md
index b56b0fa1..95b54955 100644
--- a/README.md
+++ b/README.md
@@ -42,11 +42,7 @@ Runs a simulation of your code at highest optimisation.
 
 **Debug**
 `./gradlew :simulateNativeDebug`
-<<<<<<< HEAD
-Runs a debug simulation of your code.
-=======
 Runs a debug simulation of your code, including a variety of debugging tools similar to glass but at lower optimisation.
->>>>>>> 0d1300dafb5ad6f45b5d78b3668778de3bced1e8
 
 Documentation
 =============
diff --git a/build.gradle b/build.gradle
index 728f2b3b..088030b4 100644
--- a/build.gradle
+++ b/build.gradle
@@ -1,7 +1,7 @@
 plugins {
     id "cpp"
     id "google-test-test-suite"
-    id "edu.wpi.first.GradleRIO" version "2024.2.1"
+    id "edu.wpi.first.GradleRIO" version "2024.1.1"
 }
 
 // Define my targets (RoboRIO) and artifacts (deployable files)
diff --git a/init.ps1 b/init.ps1
new file mode 100644
index 00000000..fa83b9a6
--- /dev/null
+++ b/init.ps1
@@ -0,0 +1,2 @@
+./gradlew installRoborioToolchain
+./gradlew build
diff --git a/init.sh b/init.sh
new file mode 100644
index 00000000..6a932ed6
--- /dev/null
+++ b/init.sh
@@ -0,0 +1,5 @@
+#!/usr/bin/sh
+sudo apt update
+chmod +x gradlew
+./gradlew installRoborioToolchain
+./gradlew build
diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp
index a1ba628b..15205504 100644
--- a/src/main/cpp/Robot.cpp
+++ b/src/main/cpp/Robot.cpp
@@ -58,10 +58,13 @@ void Robot::RobotInit() {
 
   // robotmap.swerveBase.gyro->Reset();
 
-  _swerveDrive = new wom::SwerveDrive(robotmap.swerveBase.config, frc::Pose2d());
+  _swerveDrive =
+      new wom::SwerveDrive(robotmap.swerveBase.config, frc::Pose2d());
   wom::BehaviourScheduler::GetInstance()->Register(_swerveDrive);
-  _swerveDrive->SetDefaultBehaviour(
-      [this]() { return wom::make<wom::ManualDrivebase>(_swerveDrive, &robotmap.controllers.driver); });
+  _swerveDrive->SetDefaultBehaviour([this]() {
+    return wom::make<wom::ManualDrivebase>(_swerveDrive,
+                                           &robotmap.controllers.driver);
+  });
 
   // alphaArm = new AlphaArm(robotmap.alphaArmSystem.config);
   // wom::BehaviourScheduler::GetInstance()->Register(alphaArm);
@@ -100,6 +103,26 @@ void Robot::RobotInit() {
   // robotmap.swerveBase.moduleConfigs[3].turnMotor.encoder->SetEncoderOffset(0_rad);
 
   lastPeriodic = wom::now();
+  
+  // m_driveSim = new wom::TempSimSwerveDrive(&simulation_timer, &m_field);
+  // m_driveSim = wom::TempSimSwerveDrive();
+
+  //robotmap.swerveBase.moduleConfigs[0].turnMotor.encoder->SetEncoderOffset(0_rad);
+  //robotmap.swerveBase.moduleConfigs[1].turnMotor.encoder->SetEncoderOffset(0_rad);
+  //robotmap.swerveBase.moduleConfigs[2].turnMotor.encoder->SetEncoderOffset(0_rad);
+  //robotmap.swerveBase.moduleConfigs[3].turnMotor.encoder->SetEncoderOffset(0_rad);
+
+
+
+
+  // frontLeft = new ctre::phoenix6::hardware::TalonFX(7, "Drivebase");  // front left
+  // frontRight = new ctre::phoenix6::hardware::TalonFX(2, "Drivebase");   // front right
+  // backLeft = new ctre::phoenix6::hardware::TalonFX(6, "Drivebase");   // back left
+  // backRight = new ctre::phoenix6::hardware::TalonFX(4, "Drivebase");  // back right
+  // frontLeft = new ctre::phoenix6::hardware::TalonFX(9, "Drivebase");   // front left
+  // frontRight = new ctre::phoenix6::hardware::TalonFX(1, "Drivebase");   // front right
+  // backLeft = new ctre::phoenix6::hardware::TalonFX(5, "Drivebase");   // back left
+  // backRight = new ctre::phoenix6::hardware::TalonFX(3, "Drivebase");
 }
 
 void Robot::RobotPeriodic() {
@@ -111,6 +134,11 @@ void Robot::RobotPeriodic() {
   wom::BehaviourScheduler::GetInstance()->Tick();
   sched->Tick();
 
+  robotmap.swerveTable.swerveDriveTable->GetEntry("frontLeftEncoder").SetDouble(robotmap.swerveBase.moduleConfigs[0].turnMotor.encoder->GetEncoderPosition().value());
+  robotmap.swerveTable.swerveDriveTable->GetEntry("frontRightEncoder").SetDouble(robotmap.swerveBase.moduleConfigs[1].turnMotor.encoder->GetEncoderPosition().value());
+  robotmap.swerveTable.swerveDriveTable->GetEntry("backLeftEncoder").SetDouble(robotmap.swerveBase.moduleConfigs[2].turnMotor.encoder->GetEncoderPosition().value());
+  robotmap.swerveTable.swerveDriveTable->GetEntry("backRightEncoder").SetDouble(robotmap.swerveBase.moduleConfigs[3].turnMotor.encoder->GetEncoderPosition().value());
+
   robotmap.swerveTable.swerveDriveTable->GetEntry("Encoder 0 offset: ")
       .SetDouble(robotmap.swerveBase.moduleConfigs[0].turnMotor.encoder->GetEncoderPosition().value());
   robotmap.swerveTable.swerveDriveTable->GetEntry("Encoder 1 offset: ")
diff --git a/src/main/include/Robot.h b/src/main/include/Robot.h
index a8bc9ba2..7fd54a58 100644
--- a/src/main/include/Robot.h
+++ b/src/main/include/Robot.h
@@ -36,7 +36,6 @@ class Robot : public frc::TimedRobot {
   void TeleopPeriodic() override;
   void DisabledInit() override;
   void DisabledPeriodic() override;
-
  private:
   RobotMap robotmap;
   wom::BehaviourScheduler* sched;
@@ -55,7 +54,6 @@ class Robot : public frc::TimedRobot {
   wom::SwerveDrive* _swerveDrive;
   // AlphaArm* alphaArm;
 
-  ctre::phoenix6::hardware::TalonFX* frontLeft;
   // AlphaArm *alphaArm;
 
   // ctre::phoenix6::hardware::TalonFX *frontLeft;
diff --git a/src/main/include/RobotMap.h b/src/main/include/RobotMap.h
index 2c80b462..4c59fce4 100644
--- a/src/main/include/RobotMap.h
+++ b/src/main/include/RobotMap.h
@@ -1,7 +1,6 @@
 // Copyright (c) 2023-2024 CurtinFRC
 // Open Source Software, you can modify it according to the terms
 // of the MIT License at the root of this project
-
 #pragma once
 
 #include <frc/Compressor.h>
@@ -199,15 +198,5 @@ struct RobotMap {
         nt::NetworkTableInstance::GetDefault().GetTable("swerve");
   };
   SwerveTable swerveTable;
-
-  // struct AlphaArmSystem {
-  //   rev::CANSparkMax alphaArmMotor{12, rev::CANSparkMax::MotorType::kBrushless};
-  //   rev::CANSparkMax wristMotor{15, rev::CANSparkMax::MotorType::kBrushless};
-
-  //   wom::Gearbox alphaArmGearbox{&alphaArmMotor, nullptr, frc::DCMotor::NEO(1)};
-  //   wom::Gearbox wristGearbox{&wristMotor, nullptr, frc::DCMotor::NEO(1)};
-
-  //   AlphaArmConfig config{alphaArmGearbox, wristGearbox};
-  // };
-  // AlphaArmSystem alphaArmSystem;
 };
+
diff --git a/wombat/src/main/cpp/behaviour/Behaviour.cpp b/wombat/src/main/cpp/behaviour/Behaviour.cpp
index 7312c922..d21d9e57 100644
--- a/wombat/src/main/cpp/behaviour/Behaviour.cpp
+++ b/wombat/src/main/cpp/behaviour/Behaviour.cpp
@@ -10,7 +10,9 @@ using namespace behaviour;
 
 // Behaviour
 Behaviour::Behaviour(std::string name, units::time::second_t period)
-    : _bhvr_name(name), _bhvr_period(period), _bhvr_state(BehaviourState::INITIALISED) {}
+    : _bhvr_name(name),
+      _bhvr_period(period),
+      _bhvr_state(BehaviourState::INITIALISED) {}
 Behaviour::~Behaviour() {
   if (!IsFinished())
     Interrupt();
@@ -80,7 +82,8 @@ bool Behaviour::Tick() {
 
     if (dt > 2 * _bhvr_period) {
       std::cerr << "Behaviour missed deadline. Reduce Period. Dt=" << dt.value()
-                << " Dt(deadline)=" << (2 * _bhvr_period).value() << ". Bhvr: " << GetName() << std::endl;
+                << " Dt(deadline)=" << (2 * _bhvr_period).value()
+                << ". Bhvr: " << GetName() << std::endl;
     }
 
     if (_bhvr_timeout.value() > 0 && _bhvr_timer > _bhvr_timeout)
@@ -97,7 +100,8 @@ bool Behaviour::IsRunning() const {
 }
 
 bool Behaviour::IsFinished() const {
-  return _bhvr_state != BehaviourState::INITIALISED && _bhvr_state != BehaviourState::RUNNING;
+  return _bhvr_state != BehaviourState::INITIALISED &&
+         _bhvr_state != BehaviourState::RUNNING;
 }
 
 void Behaviour::Stop(BehaviourState new_state) {
@@ -169,7 +173,8 @@ void ConcurrentBehaviour::Add(Behaviour::ptr behaviour) {
 }
 
 std::string ConcurrentBehaviour::GetName() const {
-  std::string msg = (_reducer == ConcurrentBehaviourReducer::ALL ? "ALL { " : "RACE {");
+  std::string msg =
+      (_reducer == ConcurrentBehaviourReducer::ALL ? "ALL { " : "RACE {");
   for (auto b : _children)
     msg += b->GetName() + ", ";
   msg += "}";
@@ -185,8 +190,8 @@ void ConcurrentBehaviour::OnStart() {
         using namespace std::chrono_literals;
 
         b->Tick();
-        std::this_thread::sleep_for(
-            std::chrono::milliseconds(static_cast<int64_t>(b->GetPeriod().value() * 1000)));
+        std::this_thread::sleep_for(std::chrono::milliseconds(
+            static_cast<int64_t>(b->GetPeriod().value() * 1000)));
       }
 
       if (IsFinished() && !b->IsFinished())
@@ -268,8 +273,10 @@ void WaitFor::OnTick(units::time::second_t dt) {
 }
 
 // WaitTime
-WaitTime::WaitTime(units::time::second_t time) : WaitTime([time]() { return time; }) {}
-WaitTime::WaitTime(std::function<units::time::second_t()> time_fn) : _time_fn(time_fn) {}
+WaitTime::WaitTime(units::time::second_t time)
+    : WaitTime([time]() { return time; }) {}
+WaitTime::WaitTime(std::function<units::time::second_t()> time_fn)
+    : _time_fn(time_fn) {}
 
 void WaitTime::OnStart() {
   _time = _time_fn();
diff --git a/wombat/src/main/cpp/behaviour/BehaviourScheduler.cpp b/wombat/src/main/cpp/behaviour/BehaviourScheduler.cpp
index eb363b3e..26863c79 100644
--- a/wombat/src/main/cpp/behaviour/BehaviourScheduler.cpp
+++ b/wombat/src/main/cpp/behaviour/BehaviourScheduler.cpp
@@ -52,8 +52,8 @@ void BehaviourScheduler::Schedule(Behaviour::ptr behaviour) {
         std::lock_guard<std::recursive_mutex> lk(_active_mtx);
         behaviour->Tick();
       }
-      std::this_thread::sleep_for(
-          std::chrono::milliseconds(static_cast<int64_t>(behaviour->GetPeriod().value() * 1000)));
+      std::this_thread::sleep_for(std::chrono::milliseconds(
+          static_cast<int64_t>(behaviour->GetPeriod().value() * 1000)));
     }
   });
 }
diff --git a/wombat/src/main/cpp/behaviour/HasBehaviour.cpp b/wombat/src/main/cpp/behaviour/HasBehaviour.cpp
index f2f557c5..964522c6 100644
--- a/wombat/src/main/cpp/behaviour/HasBehaviour.cpp
+++ b/wombat/src/main/cpp/behaviour/HasBehaviour.cpp
@@ -8,7 +8,8 @@
 
 using namespace behaviour;
 
-void HasBehaviour::SetDefaultBehaviour(std::function<std::shared_ptr<Behaviour>(void)> fn) {
+void HasBehaviour::SetDefaultBehaviour(
+    std::function<std::shared_ptr<Behaviour>(void)> fn) {
   _default_behaviour_producer = fn;
 }
 
diff --git a/wombat/src/main/cpp/drivetrain/SwerveDrive.cpp b/wombat/src/main/cpp/drivetrain/SwerveDrive.cpp
index 701fae07..b6772015 100644
--- a/wombat/src/main/cpp/drivetrain/SwerveDrive.cpp
+++ b/wombat/src/main/cpp/drivetrain/SwerveDrive.cpp
@@ -514,4 +514,4 @@ void SwerveDrive::AddVisionMeasurement(frc::Pose2d pose, units::second_t timesta
   _poseEstimator.AddVisionMeasurement(pose, timestamp);
 }
 }  // namespace drivetrain
-}  // namespace wom
+}  // namespace wom
\ No newline at end of file
diff --git a/wombat/src/main/cpp/drivetrain/behaviours/SwerveBehaviours.cpp b/wombat/src/main/cpp/drivetrain/behaviours/SwerveBehaviours.cpp
index 58782b81..47f25d93 100644
--- a/wombat/src/main/cpp/drivetrain/behaviours/SwerveBehaviours.cpp
+++ b/wombat/src/main/cpp/drivetrain/behaviours/SwerveBehaviours.cpp
@@ -248,4 +248,4 @@ void wom::drivetrain::behaviours::DrivebasePoseBehaviour::OnTick(units::second_t
   if (_swerveDrivebase->IsAtSetPose() && !_hold) {
     SetDone();
   }
-}
+}
\ No newline at end of file
diff --git a/wombat/src/main/cpp/subsystems/Arm.cpp b/wombat/src/main/cpp/subsystems/Arm.cpp
new file mode 100644
index 00000000..9390601a
--- /dev/null
+++ b/wombat/src/main/cpp/subsystems/Arm.cpp
@@ -0,0 +1,192 @@
+// Copyright (c) 2023-2024 CurtinFRC
+// Open Source Software, you can modify it according to the terms
+// of the MIT License at the root of this project
+
+#include "subsystems/Arm.h"
+
+#include <units/math.h>
+
+using namespace frc;
+using namespace wom;
+
+// creates network table instatnce on shuffleboard
+void wom::subsystems::ArmConfig::WriteNT(
+    std::shared_ptr<nt::NetworkTable> table) {
+  table->GetEntry("armMass").SetDouble(armMass.value());
+  table->GetEntry("loadMass").SetDouble(loadMass.value());
+  table->GetEntry("armLength").SetDouble(armLength.value());
+  table->GetEntry("minAngle")
+      .SetDouble(minAngle.convert<units::degree>().value());
+  table->GetEntry("maxAngle")
+      .SetDouble(maxAngle.convert<units::degree>().value());
+  table->GetEntry("initialAngle")
+      .SetDouble(initialAngle.convert<units::degree>().value());
+  table->GetEntry("angleOffset")
+      .SetDouble(initialAngle.convert<units::degree>().value());
+}
+
+// arm config is used
+wom::subsystems::Arm::Arm(ArmConfig config)
+    : _config(config),
+      _pid(config.path + "/pid", config.pidConfig),
+      _velocityPID(config.path + "/velocityPID", config.velocityConfig),
+      _table(nt::NetworkTableInstance::GetDefault().GetTable(config.path)) {}
+
+// the loop that allows the information to be used
+void wom::subsystems::Arm::OnUpdate(units::second_t dt) {
+  // sets the voltage and gets the current angle
+  units::volt_t voltage = 0_V;
+  auto angle = GetAngle();
+
+  // sets usable infromation for each state
+  switch (_state) {
+    case ArmState::kIdle:
+      break;
+    case ArmState::kVelocity: {
+      units::newton_meter_t torque =
+          9.81_m / 1_s / 1_s * _config.armLength *
+          units::math::cos(angle + _config.angleOffset) *
+          (0.5 * _config.armMass + _config.loadMass);
+      // units::volt_t feedforward = _config.leftGearbox.motor.Voltage(torque,
+      // 0_rad/1_s);
+      units::volt_t feedforward =
+          _config.leftGearbox.motor.Voltage(torque, _velocityPID.GetSetpoint());
+      // feedforward = 3.5_V;
+      // std::cout << "feedforward" << feedforward.value() << std::endl;
+      voltage = _velocityPID.Calculate(GetArmVelocity(), dt, feedforward);
+      // std::cout << "arm velocity voltage is: " << voltage.value() <<
+      // std::endl; voltage = 0_V;
+    } break;
+    case ArmState::kAngle: {
+      units::newton_meter_t torque =
+          9.81_m / 1_s / 1_s * _config.armLength *
+          units::math::cos(angle + _config.angleOffset) *
+          (0.5 * _config.armMass + _config.loadMass);
+      units::volt_t feedforward =
+          _config.leftGearbox.motor.Voltage(torque, 0_rad / 1_s);
+      // std::cout << "feedforward" << feedforward.value() << std::endl;
+      voltage = _pid.Calculate(angle, dt, feedforward);
+    } break;
+    case ArmState::kRaw:
+      voltage = _voltage;
+      break;
+  }
+
+  voltage *= armLimit;
+
+  // units::newton_meter_t torqueLimit = 10_kg * 1.4_m * 6_mps_sq;
+  // units::volt_t voltageMax = _config.leftGearbox.motor.Voltage(torqueLimit,
+  // _config.leftGearbox.encoder->GetEncoderAngularVelocity()); units::volt_t
+  // voltageMin = _config.leftGearbox.motor.Voltage(-torqueLimit,
+  // _config.leftGearbox.encoder->GetEncoderAngularVelocity());
+
+  // voltage = units::math::max(units::math::min(voltage, voltageMax),
+  // voltageMin);
+  units::volt_t voltageMin = -5.5_V;
+  units::volt_t voltageMax = 5.5_V;
+  voltage = units::math::max(units::math::min(voltage, voltageMax), voltageMin);
+
+  // std::cout << "voltage: " << voltage.value() << std::endl;
+
+  // _config.leftGearbox.motorController->SetVoltage(voltage);
+  // _config.rightGearbox.motorController->SetVoltage(voltage);
+
+  // creates network table instances for the angle and config of the arm
+  _table->GetEntry("angle").SetDouble(angle.convert<units::degree>().value());
+  _config.WriteNT(_table->GetSubTable("config"));
+}
+
+void wom::subsystems::Arm::SetArmSpeedLimit(double limit) {
+  armLimit = limit;
+}
+
+// defines information needed for the functions and connects the states to their
+// respective function
+
+void wom::subsystems::Arm::SetRaw(units::volt_t voltage) {
+  _voltage = voltage;
+}
+
+void wom::subsystems::Arm::SetVelocity(units::radians_per_second_t velocity) {
+  _velocityPID.SetSetpoint(velocity);
+}
+
+void wom::subsystems::Arm::SetAngle(units::radian_t angle) {
+  _pid.SetSetpoint(angle);
+}
+
+void wom::subsystems::Arm::SetState(wom::subsystems::ArmState state) {
+  _state = state;
+}
+
+wom::subsystems::ArmConfig& wom::subsystems::Arm::GetConfig() {
+  return _config;
+}
+
+units::radian_t wom::subsystems::Arm::GetAngle() const {
+  return _config.armEncoder.GetPosition() / 100 * 360 * 1_deg;
+}
+
+units::radians_per_second_t wom::subsystems::Arm::MaxSpeed() const {
+  return _config.leftGearbox.motor.Speed(0_Nm, 12_V);
+}
+
+units::radians_per_second_t wom::subsystems::Arm::GetArmVelocity() const {
+  return _config.armEncoder.GetVelocity() / 100 * 360 * 1_deg / 60_s;
+}
+
+bool wom::subsystems::Arm::IsStable() const {
+  return _pid.IsStable(5_deg);
+}
+
+/* SIMULATION */
+// #include <units/math.h>
+
+// ::wom::sim::ArmSim::ArmSim(ArmConfig config)
+//   : config(config),
+//     angle(config.initialAngle),
+//     encoder(config.gearbox.encoder->MakeSimEncoder()),
+//     lowerLimit(config.lowerLimitSwitch ? new
+//     frc::sim::DIOSim(*config.lowerLimitSwitch) : nullptr),
+//     upperLimit(config.upperLimitSwitch ? new
+//     frc::sim::DIOSim(*config.upperLimitSwitch) : nullptr),
+//     table(nt::NetworkTableInstance::GetDefault().GetTable(config.path +
+//     "/sim"))
+//   {}
+
+// units::ampere_t wom::sim::ArmSim::GetCurrent() const {
+//   return current;
+// }
+
+// void ::wom::sim::ArmSim::Update(units::second_t dt) {
+//   torque = (config.loadMass * config.armLength + config.armMass *
+//   config.armLength / 2.0) * 9.81_m / 1_s / 1_s *
+//   units::math::cos(config.angleOffset + angle) + additionalTorque; velocity =
+//   config.gearbox.motor.Speed(torque,
+//   config.gearbox.transmission->GetEstimatedRealVoltage()); angle += velocity
+//   * dt;
+
+//   if (angle <= config.minAngle) {
+//     angle = config.minAngle;
+//     velocity = 0_rad / 1_s;
+//     if (lowerLimit) lowerLimit->SetValue(true);
+//   } else {
+//     if (lowerLimit) lowerLimit->SetValue(false);
+//   }
+
+//   if (angle >= config.maxAngle) {
+//     angle = config.maxAngle;
+//     velocity = 0_rad / 1_s;
+//     if (upperLimit) upperLimit->SetValue(true);
+//   } else {
+//     if (upperLimit) upperLimit->SetValue(false);
+//   }
+
+//   current = config.gearbox.motor.Current(velocity,
+//   config.gearbox.transmission->GetEstimatedRealVoltage());
+
+//   if (encoder) encoder->SetEncoderTurns(angle - config.initialAngle);
+
+//   table->GetEntry("angle").SetDouble(angle.convert<units::degree>().value());
+//   table->GetEntry("current").SetDouble(current.value());
+// }
diff --git a/wombat/src/main/cpp/utils/Encoder.cpp b/wombat/src/main/cpp/utils/Encoder.cpp
index 1120b399..5ba47a14 100644
--- a/wombat/src/main/cpp/utils/Encoder.cpp
+++ b/wombat/src/main/cpp/utils/Encoder.cpp
@@ -6,8 +6,8 @@
 
 #include <rev/SparkRelativeEncoder.h>
 
-wom::utils::Encoder::Encoder(double encoderTicksPerRotation, int type, units::meter_t wheelRadius,
-                             double reduction)
+wom::utils::Encoder::Encoder(double encoderTicksPerRotation, int type,
+                             units::meter_t wheelRadius, double reduction)
     : _reduction(reduction),
       _encoderTicksPerRotation(encoderTicksPerRotation),
       _type(type),
@@ -53,17 +53,28 @@ units::radian_t wom::utils::Encoder::GetEncoderPosition() {
   //   units::degree_t pos = GetEncoderTicks() * 1_deg;
   //   return pos - _offset;
   // }
+  //if (_type == 0) {
+  //  units::turn_t n_turns{GetEncoderTicks() / GetEncoderTicksPerRotation()};
+  //  return n_turns;
+  //} else if (_type == 2) {
+  //  units::degree_t pos = GetEncoderTicks() * 1_deg;
+  //  return pos;
+  //} else {
+  //  units::degree_t pos = GetEncoderTicks() * 1_deg;
+  //  return pos - _offset;
+  //}
   return GetEncoderTicks() * 1_rad;
 }
 
 double wom::utils::Encoder::GetEncoderDistance() {
-  return GetEncoderTicks() * (2 * 3.1415) * _wheelRadius.value();
+  return GetEncoderTicks() * (2 * 3.141592) * _wheelRadius.value();
 }
 
 units::radians_per_second_t wom::utils::Encoder::GetEncoderAngularVelocity() {
   // return GetEncoderTickVelocity() / (double)GetEncoderTicksPerRotation() * 2
   // * 3.1415926;
-  units::turns_per_second_t n_turns_per_s{GetEncoderTickVelocity() / GetEncoderTicksPerRotation()};
+  units::turns_per_second_t n_turns_per_s{GetEncoderTickVelocity() /
+                                          GetEncoderTicksPerRotation()};
   return n_turns_per_s;
 }
 
@@ -85,7 +96,8 @@ double wom::utils::DigitalEncoder::GetEncoderTickVelocity() const {
   return _nativeEncoder.GetRate();
 }
 
-wom::utils::CANSparkMaxEncoder::CANSparkMaxEncoder(rev::CANSparkMax* controller, units::meter_t wheelRadius,
+wom::utils::CANSparkMaxEncoder::CANSparkMaxEncoder(rev::CANSparkMax* controller,
+                                                   units::meter_t wheelRadius,
                                                    double reduction)
     : wom::utils::Encoder(42, reduction, wheelRadius, 2),
       _encoder(controller->GetEncoder(rev::SparkRelativeEncoder::Type::kHallSensor)) {}
@@ -118,9 +130,11 @@ double wom::utils::CANSparkMaxEncoder::GetVelocity() const {
   return _encoder.GetVelocity();
 }
 
-wom::utils::TalonFXEncoder::TalonFXEncoder(ctre::phoenix6::hardware::TalonFX* controller,
-                                           units::meter_t wheelRadius, double reduction)
-    : utils::Encoder(2048, reduction, wheelRadius, 0), _controller(controller) {}
+wom::utils::TalonFXEncoder::TalonFXEncoder(
+    ctre::phoenix6::hardware::TalonFX* controller, units::meter_t wheelRadius,
+    double reduction)
+    : utils::Encoder(2048, reduction, wheelRadius, 0),
+      _controller(controller) {}
 
 double wom::utils::TalonFXEncoder::GetEncoderRawTicks() const {
   return _controller->GetPosition().GetValue().value();
@@ -130,9 +144,12 @@ double wom::utils::TalonFXEncoder::GetEncoderTickVelocity() const {
   return _controller->Get() * 10;
 }
 
-wom::utils::DutyCycleEncoder::DutyCycleEncoder(int channel, units::meter_t wheelRadius,
-                                               double ticksPerRotation, double reduction)
-    : wom::utils::Encoder(ticksPerRotation, reduction, wheelRadius, 0), _dutyCycleEncoder(channel) {}
+wom::utils::DutyCycleEncoder::DutyCycleEncoder(int channel,
+                                               units::meter_t wheelRadius,
+                                               double ticksPerRotation,
+                                               double reduction)
+    : wom::utils::Encoder(ticksPerRotation, reduction, wheelRadius, 0),
+      _dutyCycleEncoder(channel) {}
 
 double wom::utils::DutyCycleEncoder::GetEncoderRawTicks() const {
   return _dutyCycleEncoder.Get().value();
@@ -142,8 +159,9 @@ double wom::utils::DutyCycleEncoder::GetEncoderTickVelocity() const {
   return 0;
 }
 
-wom::utils::CanEncoder::CanEncoder(int deviceNumber, units::meter_t wheelRadius, double ticksPerRotation,
-                                   double reduction, std::string name)
+wom::utils::CanEncoder::CanEncoder(int deviceNumber, units::meter_t wheelRadius,
+                                   double ticksPerRotation, double reduction,
+                                   std::string name)
     : wom::utils::Encoder(ticksPerRotation, 2, wheelRadius, reduction) {
   _canEncoder = new ctre::phoenix6::hardware::CANcoder(deviceNumber, name);
   // _canEncoder->ConfigAbsoluteEncoderRange(0, 1);
diff --git a/wombat/src/main/cpp/utils/Util.cpp b/wombat/src/main/cpp/utils/Util.cpp
index dbdbb642..e529e245 100644
--- a/wombat/src/main/cpp/utils/Util.cpp
+++ b/wombat/src/main/cpp/utils/Util.cpp
@@ -13,18 +13,21 @@ units::second_t wom::utils::now() {
   return static_cast<double>(now) / 1000000 * 1_s;
 }
 
-void wom::utils::WritePose2NT(std::shared_ptr<nt::NetworkTable> table, frc::Pose2d pose) {
+void wom::utils::WritePose2NT(std::shared_ptr<nt::NetworkTable> table,
+                              frc::Pose2d pose) {
   table->GetEntry("x").SetDouble(pose.X().value());
   table->GetEntry("y").SetDouble(pose.Y().value());
   table->GetEntry("angle").SetDouble(pose.Rotation().Degrees().value());
 }
 
-void wom::utils::WritePose3NT(std::shared_ptr<nt::NetworkTable> table, frc::Pose3d pose) {
+void wom::utils::WritePose3NT(std::shared_ptr<nt::NetworkTable> table,
+                              frc::Pose3d pose) {
   table->GetEntry("x").SetDouble(pose.X().value());
   table->GetEntry("y").SetDouble(pose.Y().value());
   table->GetEntry("z").SetDouble(pose.Z().value());
 
-  table->GetEntry("angle").SetDouble(pose.Rotation().Z().convert<units::degree>().value());
+  table->GetEntry("angle").SetDouble(
+      pose.Rotation().Z().convert<units::degree>().value());
 }
 
 double wom::utils::deadzone(double val, double deadzone) {
@@ -47,24 +50,32 @@ units::volt_t wom::utils::GetVoltage(frc::MotorController* controller) {
   return controller->Get() * frc::RobotController::GetBatteryVoltage();
 }
 
-void wom::utils::WriteTrajectory(std::shared_ptr<nt::NetworkTable> table, frc::Trajectory trajectory) {
+void wom::utils::WriteTrajectory(std::shared_ptr<nt::NetworkTable> table,
+                                 frc::Trajectory trajectory) {
   table->GetEntry("length").SetDouble(trajectory.TotalTime().value());
 
   // write the trajectory to the network table
   int i = 0;
   for (auto state : trajectory.States()) {
-    table->GetSubTable(std::to_string(i))->GetEntry("x").SetDouble(state.pose.X().value());
-    table->GetSubTable(std::to_string(i))->GetEntry("y").SetDouble(state.pose.Y().value());
+    table->GetSubTable(std::to_string(i))
+        ->GetEntry("x")
+        .SetDouble(state.pose.X().value());
+    table->GetSubTable(std::to_string(i))
+        ->GetEntry("y")
+        .SetDouble(state.pose.Y().value());
     table->GetSubTable(std::to_string(i))
         ->GetEntry("angle")
         .SetDouble(state.pose.Rotation().Degrees().value());
-    table->GetSubTable(std::to_string(i))->GetEntry("time").SetDouble(state.t.value());
+    table->GetSubTable(std::to_string(i))
+        ->GetEntry("time")
+        .SetDouble(state.t.value());
 
     i++;
   }
 }
 
-void wom::utils::WriteTrajectoryState(std::shared_ptr<nt::NetworkTable> table, frc::Trajectory::State state) {
+void wom::utils::WriteTrajectoryState(std::shared_ptr<nt::NetworkTable> table,
+                                      frc::Trajectory::State state) {
   table->GetEntry("x").SetDouble(state.pose.X().value());
   table->GetEntry("y").SetDouble(state.pose.Y().value());
   table->GetEntry("angle").SetDouble(state.pose.Rotation().Degrees().value());
diff --git a/wombat/src/main/cpp/vision/Limelight.cpp b/wombat/src/main/cpp/vision/Limelight.cpp
index 906f92cb..b37148ce 100644
--- a/wombat/src/main/cpp/vision/Limelight.cpp
+++ b/wombat/src/main/cpp/vision/Limelight.cpp
@@ -8,7 +8,8 @@
 
 #include "utils/Util.h"
 
-wom::vision::Limelight::Limelight(std::string limelightName) : _limelightName(limelightName) {}
+wom::vision::Limelight::Limelight(std::string limelightName)
+    : _limelightName(limelightName) {}
 
 std::string wom::vision::Limelight::GetName() {
   return _limelightName;
@@ -23,7 +24,8 @@ std::pair<double, double> wom::vision::Limelight::GetOffset() {
   return offset;
 }
 
-std::vector<double> wom::vision::Limelight::GetAprilTagData(LimelightAprilTagData dataType) {
+std::vector<double> wom::vision::Limelight::GetAprilTagData(
+    LimelightAprilTagData dataType) {
   std::string dataName;
 
   switch (dataType) {
@@ -67,7 +69,8 @@ std::vector<double> wom::vision::Limelight::GetAprilTagData(LimelightAprilTagDat
   return table->GetNumberArray(dataName, std::vector<double>(6));
 }
 
-double wom::vision::Limelight::GetTargetingData(LimelightTargetingData dataType, double defaultValue) {
+double wom::vision::Limelight::GetTargetingData(LimelightTargetingData dataType,
+                                                double defaultValue) {
   std::string dataName;
 
   switch (dataType) {
@@ -155,8 +158,8 @@ void wom::vision::Limelight::SetCrop(std::array<double, 4> crop) {
   table->PutNumberArray("camtran", crop);
 }
 
-units::meters_per_second_t wom::vision::Limelight::GetSpeed(frc::Pose3d pose1, frc::Pose3d pose2,
-                                                            units::second_t dt) {
+units::meters_per_second_t wom::vision::Limelight::GetSpeed(
+    frc::Pose3d pose1, frc::Pose3d pose2, units::second_t dt) {
   frc::Transform3d dPose{pose1, pose2};
   frc::Translation3d dTranslation = dPose.Translation();
 
@@ -169,8 +172,9 @@ units::meters_per_second_t wom::vision::Limelight::GetSpeed(frc::Pose3d pose1, f
 
 frc::Pose3d wom::vision::Limelight::GetPose() {
   std::vector<double> pose = GetAprilTagData(LimelightAprilTagData::kBotpose);
-  return frc::Pose3d(pose[1] * 1_m, 1_m * pose[2], 1_m * pose[3],
-                     frc::Rotation3d(1_deg * (pose[4]), 1_deg * (pose[5]), 1_deg * (pose[6])));
+  return frc::Pose3d(
+      pose[1] * 1_m, 1_m * pose[2], 1_m * pose[3],
+      frc::Rotation3d(1_deg * (pose[4]), 1_deg * (pose[5]), 1_deg * (pose[6])));
 }
 
 void wom::vision::Limelight::OnStart() {
@@ -181,9 +185,11 @@ void wom::vision::Limelight::OnUpdate(units::time::second_t dt) {
   wom::utils::WritePose3NT(table, GetPose());
 }
 
-bool wom::vision::Limelight::IsAtSetPoseVision(frc::Pose3d pose, units::second_t dt) {
+bool wom::vision::Limelight::IsAtSetPoseVision(frc::Pose3d pose,
+                                               units::second_t dt) {
   frc::Pose3d actualPose = GetPose();
   frc::Pose3d relativePose = actualPose.RelativeTo(pose);
-  return (units::math::fabs(relativePose.X()) < 0.01_m && units::math::fabs(relativePose.Y()) < 0.01_m &&
+  return (units::math::fabs(relativePose.X()) < 0.01_m &&
+          units::math::fabs(relativePose.Y()) < 0.01_m &&
           GetSpeed(pose, GetPose(), dt) < 1_m / 1_s);
 }
diff --git a/wombat/src/main/include/behaviour/Behaviour.h b/wombat/src/main/include/behaviour/Behaviour.h
index 84ac8689..4f2e357e 100644
--- a/wombat/src/main/include/behaviour/Behaviour.h
+++ b/wombat/src/main/include/behaviour/Behaviour.h
@@ -22,7 +22,13 @@
 #include "HasBehaviour.h"
 
 namespace behaviour {
-enum class BehaviourState { INITIALISED, RUNNING, DONE, TIMED_OUT, INTERRUPTED };
+enum class BehaviourState {
+  INITIALISED,
+  RUNNING,
+  DONE,
+  TIMED_OUT,
+  INTERRUPTED
+};
 
 class SequentialBehaviour;
 
@@ -40,7 +46,8 @@ class Behaviour : public std::enable_shared_from_this<Behaviour> {
  public:
   using ptr = std::shared_ptr<Behaviour>;
 
-  explicit Behaviour(std::string name = "<unnamed behaviour>", units::time::second_t period = 20_ms);
+  Behaviour(std::string name = "<unnamed behaviour>",
+            units::time::second_t period = 20_ms);
   ~Behaviour();
 
   /**
@@ -186,15 +193,16 @@ class SequentialBehaviour : public Behaviour {
   std::deque<ptr> _queue;
 };
 
-inline std::shared_ptr<SequentialBehaviour> operator<<(Behaviour::ptr a, Behaviour::ptr b) {
+inline std::shared_ptr<SequentialBehaviour> operator<<(Behaviour::ptr a,
+                                                       Behaviour::ptr b) {
   auto seq = std::make_shared<SequentialBehaviour>();
   seq->Add(a);
   seq->Add(b);
   return seq;
 }
 
-inline std::shared_ptr<SequentialBehaviour> operator<<(std::shared_ptr<SequentialBehaviour> a,
-                                                       Behaviour::ptr b) {
+inline std::shared_ptr<SequentialBehaviour> operator<<(
+    std::shared_ptr<SequentialBehaviour> a, Behaviour::ptr b) {
   a->Add(b);
   return a;
 }
@@ -239,8 +247,10 @@ class ConcurrentBehaviour : public Behaviour {
  * Create a concurrent behaviour group, waiting for all behaviours
  * to finish before moving on.
  */
-inline std::shared_ptr<ConcurrentBehaviour> operator&(Behaviour::ptr a, Behaviour::ptr b) {
-  auto conc = std::make_shared<ConcurrentBehaviour>(ConcurrentBehaviourReducer::ALL);
+inline std::shared_ptr<ConcurrentBehaviour> operator&(Behaviour::ptr a,
+                                                      Behaviour::ptr b) {
+  auto conc =
+      std::make_shared<ConcurrentBehaviour>(ConcurrentBehaviourReducer::ALL);
   conc->Add(a);
   conc->Add(b);
   return conc;
@@ -251,8 +261,10 @@ inline std::shared_ptr<ConcurrentBehaviour> operator&(Behaviour::ptr a, Behaviou
  * be interrupted as soon as any members of the group are finished (the
  * behaviours are 'raced' against each other).
  */
-inline std::shared_ptr<ConcurrentBehaviour> operator|(Behaviour::ptr a, Behaviour::ptr b) {
-  auto conc = std::make_shared<ConcurrentBehaviour>(ConcurrentBehaviourReducer::ANY);
+inline std::shared_ptr<ConcurrentBehaviour> operator|(Behaviour::ptr a,
+                                                      Behaviour::ptr b) {
+  auto conc =
+      std::make_shared<ConcurrentBehaviour>(ConcurrentBehaviourReducer::ANY);
   conc->Add(a);
   conc->Add(b);
   return conc;
@@ -320,7 +332,8 @@ struct Switch : public Behaviour {
    * @param condition The function yielding true if this is the correct option
    * @param b The behaviour to call if this option is provided.
    */
-  std::shared_ptr<Switch> When(std::function<bool(T&)> condition, Behaviour::ptr b) {
+  std::shared_ptr<Switch> When(std::function<bool(T&)> condition,
+                               Behaviour::ptr b) {
     _options.push_back(std::pair(condition, b));
     Inherit(*b);
     return std::reinterpret_pointer_cast<Switch<T>>(shared_from_this());
@@ -376,7 +389,8 @@ struct Switch : public Behaviour {
 
  private:
   std::function<T()> _fn;
-  wpi::SmallVector<std::pair<std::function<bool(T&)>, Behaviour::ptr>, 4> _options;
+  wpi::SmallVector<std::pair<std::function<bool(T&)>, Behaviour::ptr>, 4>
+      _options;
   Behaviour::ptr _locked = nullptr;
 };
 
@@ -393,8 +407,10 @@ struct Decide : public Switch<std::monostate> {
    * @param condition The function yielding true if this is the correct option
    * @param b The behaviour to call if this option is provided.
    */
-  std::shared_ptr<Decide> When(std::function<bool()> condition, Behaviour::ptr b) {
-    return std::reinterpret_pointer_cast<Decide>(Switch::When([condition](auto) { return condition(); }, b));
+  std::shared_ptr<Decide> When(std::function<bool()> condition,
+                               Behaviour::ptr b) {
+    return std::reinterpret_pointer_cast<Decide>(
+        Switch::When([condition](auto) { return condition(); }, b));
   }
 };
 
diff --git a/wombat/src/main/include/behaviour/HasBehaviour.h b/wombat/src/main/include/behaviour/HasBehaviour.h
index bae5ac05..c9b35b2e 100644
--- a/wombat/src/main/include/behaviour/HasBehaviour.h
+++ b/wombat/src/main/include/behaviour/HasBehaviour.h
@@ -30,7 +30,8 @@ class HasBehaviour {
 
  protected:
   std::shared_ptr<Behaviour> _active_behaviour{nullptr};
-  std::function<std::shared_ptr<Behaviour>(void)> _default_behaviour_producer{nullptr};
+  std::function<std::shared_ptr<Behaviour>(void)> _default_behaviour_producer{
+      nullptr};
 
  private:
   friend class BehaviourScheduler;
diff --git a/wombat/src/main/include/drivetrain/SwerveDrive.h b/wombat/src/main/include/drivetrain/SwerveDrive.h
index d8b8e482..25d27d04 100644
--- a/wombat/src/main/include/drivetrain/SwerveDrive.h
+++ b/wombat/src/main/include/drivetrain/SwerveDrive.h
@@ -251,4 +251,4 @@ class SwerveDrive : public behaviour::HasBehaviour {
   // double backRightEncoderOffset = -119.619140625;
 };
 }  // namespace drivetrain
-}  // namespace wom
+}  // namespace wom
\ No newline at end of file
diff --git a/wombat/src/main/include/drivetrain/behaviours/SwerveBehaviours.h b/wombat/src/main/include/drivetrain/behaviours/SwerveBehaviours.h
index d9f844a5..0224c28d 100644
--- a/wombat/src/main/include/drivetrain/behaviours/SwerveBehaviours.h
+++ b/wombat/src/main/include/drivetrain/behaviours/SwerveBehaviours.h
@@ -234,4 +234,4 @@ class DrivebasePoseBehaviour : public behaviour::Behaviour {
 };
 }  // namespace behaviours
 }  // namespace drivetrain
-}  // namespace wom
+}  // namespace wom
\ No newline at end of file
diff --git a/wombat/src/main/include/subsystems/Arm.h b/wombat/src/main/include/subsystems/Arm.h
new file mode 100644
index 00000000..155fd4cd
--- /dev/null
+++ b/wombat/src/main/include/subsystems/Arm.h
@@ -0,0 +1,82 @@
+// Copyright (c) 2023-2024 CurtinFRC
+// Open Source Software, you can modify it according to the terms
+// of the MIT License at the root of this project
+
+#pragma once
+
+#include <frc/DigitalInput.h>
+#include <frc/simulation/DIOSim.h>
+#include <units/current.h>
+#include <units/mass.h>
+#include <units/voltage.h>
+
+#include <memory>
+#include <string>
+
+#include "behaviour/HasBehaviour.h"
+#include "utils/Gearbox.h"
+#include "utils/PID.h"
+
+namespace wom {
+namespace subsystems {
+struct ArmConfig {
+  std::string path;
+
+  wom::utils::Gearbox leftGearbox;
+  wom::utils::Gearbox rightGearbox;
+  wom::utils::CANSparkMaxEncoder armEncoder;
+  wom::utils::PIDConfig<units::radian, units::volt> pidConfig;
+  wom::utils::PIDConfig<units::radians_per_second, units::volt> velocityConfig;
+
+  units::kilogram_t armMass;
+  units::kilogram_t loadMass;
+  units::meter_t armLength;
+  units::radian_t minAngle = 0_deg;
+  units::radian_t maxAngle = 180_deg;
+  units::radian_t initialAngle = 0_deg;
+  units::radian_t angleOffset = 0_deg;
+
+  void WriteNT(std::shared_ptr<nt::NetworkTable> table);
+};
+
+enum class ArmState { kIdle, kAngle, kRaw, kVelocity };
+
+class Arm : public behaviour::HasBehaviour {
+ public:
+  explicit Arm(ArmConfig config);
+
+  void OnUpdate(units::second_t dt);
+
+  void SetIdle();
+  void SetAngle(units::radian_t angle);
+  void SetRaw(units::volt_t voltage);
+  void SetVelocity(units::radians_per_second_t velocity);
+
+  void SetState(wom::subsystems::ArmState state);
+
+  void SetArmSpeedLimit(double limit);  // units, what are they??
+
+  ArmConfig& GetConfig();
+
+  units::radian_t GetAngle() const;
+  units::radians_per_second_t MaxSpeed() const;
+  units::radians_per_second_t GetArmVelocity() const;
+
+  bool IsStable() const;
+
+ private:
+  ArmConfig _config;
+  ArmState _state = ArmState::kIdle;
+  wom::utils::PIDController<units::radian, units::volt> _pid;
+  wom::utils::PIDController<units::radians_per_second, units::volt>
+      _velocityPID;
+
+  std::shared_ptr<nt::NetworkTable> _table;
+
+  double armLimit = 0.4;
+  units::radians_per_second_t lastVelocity;
+
+  units::volt_t _voltage{0};
+};
+}  // namespace subsystems
+}  // namespace wom
diff --git a/wombat/src/main/include/subsystems/Shooter.h b/wombat/src/main/include/subsystems/Shooter.h
new file mode 100644
index 00000000..2bbf6f5e
--- /dev/null
+++ b/wombat/src/main/include/subsystems/Shooter.h
@@ -0,0 +1,76 @@
+// Copyright (c) 2023-2024 CurtinFRC
+// Open Source Software, you can modify it according to the terms
+// of the MIT License at the root of this project
+
+#pragma once
+
+#include <networktables/NetworkTable.h>
+#include <units/angular_velocity.h>
+#include <units/charge.h>
+
+#include <memory>
+#include <string>
+
+#include "behaviour/Behaviour.h"
+#include "behaviour/HasBehaviour.h"
+#include "utils/Gearbox.h"
+#include "utils/PID.h"
+
+namespace wom {
+namespace subsystems {
+enum class ShooterState { kPID, kManual, kIdle };
+
+struct ShooterParams {
+  wom::utils::Gearbox gearbox;
+  wom::utils::PIDConfig<units::radians_per_second, units::volt> pid;
+  units::ampere_t currentLimit;
+};
+
+class Shooter : public behaviour::HasBehaviour {
+ public:
+  Shooter(std::string path, ShooterParams params);
+
+  void SetManual(units::volt_t voltage);
+  void SetPID(units::radians_per_second_t goal);
+  void SetState(ShooterState state);
+
+  void OnUpdate(units::second_t dt);
+
+  bool IsStable() const;
+
+ private:
+  ShooterParams _params;
+  ShooterState _state;
+
+  units::volt_t _setpointManual{0};
+
+  wom::utils::PIDController<units::radians_per_second, units::volt> _pid;
+
+  std::shared_ptr<nt::NetworkTable> _table;
+};
+
+class ShooterConstant : public behaviour::Behaviour {
+ public:
+  ShooterConstant(Shooter* s, units::volt_t setpoint);
+
+  void OnTick(units::second_t dt) override;
+
+ private:
+  Shooter* _shooter;
+  units::volt_t _setpoint;
+};
+
+class ShooterSpinup : public behaviour::Behaviour {
+ public:
+  ShooterSpinup(Shooter* s, units::radians_per_second_t speed,
+                bool hold = false);
+
+  void OnTick(units::second_t dt) override;
+
+ private:
+  Shooter* _shooter;
+  units::radians_per_second_t _speed;
+  bool _hold;
+};
+}  // namespace subsystems
+}  // namespace wom
diff --git a/wombat/src/main/include/utils/Encoder.h b/wombat/src/main/include/utils/Encoder.h
index b9aae2a4..6b82882c 100644
--- a/wombat/src/main/include/utils/Encoder.h
+++ b/wombat/src/main/include/utils/Encoder.h
@@ -125,4 +125,4 @@ class CanEncoder : public Encoder {
   ctre::phoenix6::hardware::CANcoder* _canEncoder;
 };
 }  // namespace utils
-}  // namespace wom
+}  // namespace wom
\ No newline at end of file
diff --git a/wombat/src/main/include/utils/PID.h b/wombat/src/main/include/utils/PID.h
index 5f1a3304..63df3666 100644
--- a/wombat/src/main/include/utils/PID.h
+++ b/wombat/src/main/include/utils/PID.h
@@ -183,4 +183,4 @@ class PIDController {
   std::shared_ptr<nt::NetworkTable> _table;
 };
 }  // namespace utils
-}  // namespace wom
+}  // namespace wom
\ No newline at end of file
diff --git a/wombat/src/main/include/utils/Util.h b/wombat/src/main/include/utils/Util.h
index d624e408..665c85ec 100644
--- a/wombat/src/main/include/utils/Util.h
+++ b/wombat/src/main/include/utils/Util.h
@@ -28,30 +28,39 @@ units::second_t now();
 
 class NTBound {
  public:
-  NTBound(std::shared_ptr<nt::NetworkTable> table, std::string name, const nt::Value& value,
+  NTBound(std::shared_ptr<nt::NetworkTable> table, std::string name,
+          const nt::Value& value,
           std::function<void(const nt::Value&)> onUpdateFn)
-      : _table(table), _entry(table->GetEntry(name)), _onUpdate(onUpdateFn), _name(name) {
+      : _table(table),
+        _entry(table->GetEntry(name)),
+        _onUpdate(onUpdateFn),
+        _name(name) {
     _entry.SetValue(value);
     // _listener = table->AddListener(name, , ([=](const nt::EntryNotification
     // &evt) {
     //   this->_onUpdate(evt.value);
     // }, NT_NOTIFY_UPDATE);
-    _listener =
-        table->AddListener(name, nt::EventFlags::kValueAll,
-                           ([this](nt::NetworkTable* table, std::string_view key, const nt::Event& event) {
-                             std::cout << "NT UPDATE" << std::endl;
-                             this->_onUpdate(event.GetValueEventData()->value);
-                           }));
+    _listener = table->AddListener(
+        name, nt::EventFlags::kValueAll,
+        ([this](nt::NetworkTable* table, std::string_view key,
+                const nt::Event& event) {
+          std::cout << "NT UPDATE" << std::endl;
+          this->_onUpdate(event.GetValueEventData()->value);
+        }));
   }
 
   NTBound(const NTBound& other)
-      : _table(other._table), _entry(other._entry), _onUpdate(other._onUpdate), _name(other._name) {
-    _listener =
-        _table->AddListener(_name, nt::EventFlags::kValueAll,
-                            ([this](nt::NetworkTable* table, std::string_view key, const nt::Event& event) {
-                              std::cout << "NT UPDATE" << std::endl;
-                              this->_onUpdate(event.GetValueEventData()->value);
-                            }));
+      : _table(other._table),
+        _entry(other._entry),
+        _onUpdate(other._onUpdate),
+        _name(other._name) {
+    _listener = _table->AddListener(
+        _name, nt::EventFlags::kValueAll,
+        ([this](nt::NetworkTable* table, std::string_view key,
+                const nt::Event& event) {
+          std::cout << "NT UPDATE" << std::endl;
+          this->_onUpdate(event.GetValueEventData()->value);
+        }));
   }
 
   ~NTBound() { _table->RemoveListener(_listener); }
@@ -66,7 +75,8 @@ class NTBound {
 
 class NTBoundDouble : public NTBound {
  public:
-  NTBoundDouble(std::shared_ptr<nt::NetworkTable> table, std::string name, double& val)
+  NTBoundDouble(std::shared_ptr<nt::NetworkTable> table, std::string name,
+                double& val)
       : NTBound(table, name, nt::Value::MakeDouble(val),
                 [&val](const nt::Value& v) { val = v.GetDouble(); }) {}
 };
@@ -74,15 +84,20 @@ class NTBoundDouble : public NTBound {
 template <typename T>
 class NTBoundUnit : public NTBound {
  public:
-  NTBoundUnit(std::shared_ptr<nt::NetworkTable> table, std::string name, units::unit_t<T>& val)
+  NTBoundUnit(std::shared_ptr<nt::NetworkTable> table, std::string name,
+              units::unit_t<T>& val)
       : NTBound(table, name, nt::Value::MakeDouble(val.value()),
-                [&val](const nt::Value& v) { val = units::unit_t<T>{v.GetDouble()}; }) {}
+                [&val](const nt::Value& v) {
+                  val = units::unit_t<T>{v.GetDouble()};
+                }) {}
 };
 
 void WritePose2NT(std::shared_ptr<nt::NetworkTable> table, frc::Pose2d pose);
 void WritePose3NT(std::shared_ptr<nt::NetworkTable> table, frc::Pose3d pose);
-void WriteTrajectory(std::shared_ptr<nt::NetworkTable> table, frc::Trajectory trajectory);
-void WriteTrajectoryState(std::shared_ptr<nt::NetworkTable> table, frc::Trajectory::State state);
+void WriteTrajectory(std::shared_ptr<nt::NetworkTable> table,
+                     frc::Trajectory trajectory);
+void WriteTrajectoryState(std::shared_ptr<nt::NetworkTable> table,
+                          frc::Trajectory::State state);
 
 frc::Pose2d TrajectoryStateToPose2d(frc::Trajectory::State state);
 
diff --git a/wombat/src/main/include/vision/Limelight.h b/wombat/src/main/include/vision/Limelight.h
index 5aa6d0b7..ddfb3739 100644
--- a/wombat/src/main/include/vision/Limelight.h
+++ b/wombat/src/main/include/vision/Limelight.h
@@ -26,11 +26,20 @@
 namespace wom {
 namespace vision {
 
-enum class LimelightLEDMode { kPipelineDefault = 0, kForceOff = 1, kForceBlink = 2, kForceOn = 3 };
+enum class LimelightLEDMode {
+  kPipelineDefault = 0,
+  kForceOff = 1,
+  kForceBlink = 2,
+  kForceOn = 3
+};
 
 enum class LimelightCamMode { kVisionProcessor = 0, kDriverCamera = 1 };
 
-enum class LimelightStreamMode { kStandard = 0, kPiPMain = 1, kPiPSecondary = 2 };
+enum class LimelightStreamMode {
+  kStandard = 0,
+  kPiPMain = 1,
+  kPiPSecondary = 2
+};
 
 enum class LimelightSnapshotMode { kReset = 0, kSingle = 1 };
 
@@ -82,12 +91,14 @@ class Limelight {
 
   std::string GetName();
 
-  std::shared_ptr<nt::NetworkTable> table = nt::NetworkTableInstance::GetDefault().GetTable("limelight");
+  std::shared_ptr<nt::NetworkTable> table =
+      nt::NetworkTableInstance::GetDefault().GetTable("limelight");
 
   std::pair<double, double> GetOffset();
 
   std::vector<double> GetAprilTagData(LimelightAprilTagData dataType);
-  double GetTargetingData(LimelightTargetingData dataType, double defaultValue = 0.0);
+  double GetTargetingData(LimelightTargetingData dataType,
+                          double defaultValue = 0.0);
   void SetLEDMode(LimelightLEDMode mode);
   void SetCamMode(LimelightCamMode mode);
   void SetPipeline(LimelightPipeline pipeline);
@@ -100,7 +111,8 @@ class Limelight {
 
   bool IsAtSetPoseVision(frc::Pose3d pose, units::second_t dt);
 
-  units::meters_per_second_t GetSpeed(frc::Pose3d pose1, frc::Pose3d pose2, units::second_t dt);
+  units::meters_per_second_t GetSpeed(frc::Pose3d pose1, frc::Pose3d pose2,
+                                      units::second_t dt);
 
   frc::Pose3d GetPose();
 

From c48b0b5e27ae7ae4d4058004ab8f6a8624eb10e5 Mon Sep 17 00:00:00 2001
From: totallysomeoneyoudontknow
 <155517980+totallysomeoneyoudontknow@users.noreply.github.com>
Date: Tue, 23 Jan 2024 20:34:07 +0800
Subject: [PATCH 131/207] Shooter - Implements Networking tables (#99)

* added the state raw and changed voltage

* test

* added the state raw and changed voltage

* added pid calculate loop, fixed indenting

* added calculate function on a new line, changed the PID to not stable

* Fixed the if condition so shooterSensor is not tripped

* changed the functions so they start with a capital

* changed the value for shooterMotor

* added shuffleboard and networking tables

* added shuffleboard and networking tables

* added network tables

* fixed merge conflicts and merge errors

* fixed merge conflicts and merge errors

* fixed merge conflicts and merge errors

* fixed errors

* ran wpiformat

* fixed merge errors and conflicts

* fixed github errors

* Alpha Bot Shooter

---------

Co-authored-by: Havish12 <havishchopra10@gmail.com>
Co-authored-by: Isaac Turner <spacey_sooty@outlook.com>
---
 src/main/cpp/Robot.cpp              | 20 +++++++++++---------
 src/main/cpp/Shooter.cpp            |  2 +-
 src/main/cpp/ShooterBehaviour.cpp   |  2 +-
 src/main/include/Robot.h            | 11 ++---------
 src/main/include/RobotMap.h         | 13 ++++++++++++-
 src/main/include/Shooter.h          |  2 +-
 src/main/include/ShooterBehaviour.h |  2 +-
 7 files changed, 29 insertions(+), 23 deletions(-)

diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp
index 15205504..9e640dc1 100644
--- a/src/main/cpp/Robot.cpp
+++ b/src/main/cpp/Robot.cpp
@@ -34,6 +34,12 @@
 static units::second_t lastPeriodic;
 
 void Robot::RobotInit() {
+
+  shooter = new Shooter(robotmap.shooterSystem.config);
+  wom::BehaviourScheduler::GetInstance()->Register(shooter);
+  shooter->SetDefaultBehaviour(
+     [this]() {return wom::make<ShooterManualControl>(shooter, &robotmap.controllers.codriver); });
+  
   sched = wom::BehaviourScheduler::GetInstance();
   m_chooser.SetDefaultOption("Default Auto", "Default Auto");
 
@@ -132,6 +138,8 @@ void Robot::RobotPeriodic() {
 
   loop.Poll();
   wom::BehaviourScheduler::GetInstance()->Tick();
+
+  shooter->OnUpdate(dt);
   sched->Tick();
 
   robotmap.swerveTable.swerveDriveTable->GetEntry("frontLeftEncoder").SetDouble(robotmap.swerveBase.moduleConfigs[0].turnMotor.encoder->GetEncoderPosition().value());
@@ -155,13 +163,10 @@ void Robot::RobotPeriodic() {
 }
 
 void Robot::AutonomousInit() {
-  // m_driveSim->SetPath(m_path_chooser.GetSelected());
-
   loop.Clear();
   sched->InterruptAll();
 }
 void Robot::AutonomousPeriodic() {
-  // m_driveSim->OnUpdate();
 }
 
 void Robot::TeleopInit() {
@@ -170,13 +175,10 @@ void Robot::TeleopInit() {
   // shooter->OnStart();
   // alphaArm->OnStart();
   sched->InterruptAll();
-
-  // frontLeft->SetVoltage(4_V);
-  // frontRight->SetVoltage(4_V);
-  // backLeft->SetVoltage(4_V);
-  // backRight->SetVoltage(4_V);
 }
-void Robot::TeleopPeriodic() {}
+void Robot::TeleopPeriodic() {
+  
+}
 
 void Robot::DisabledInit() {}
 void Robot::DisabledPeriodic() {}
diff --git a/src/main/cpp/Shooter.cpp b/src/main/cpp/Shooter.cpp
index 44326bda..e20c059e 100644
--- a/src/main/cpp/Shooter.cpp
+++ b/src/main/cpp/Shooter.cpp
@@ -117,4 +117,4 @@ void Shooter::SetRaw(units::volt_t voltage) {
 }
 void Shooter::SetPidGoal(units::radians_per_second_t goal) {
   _goal = goal;
-}
+}
\ No newline at end of file
diff --git a/src/main/cpp/ShooterBehaviour.cpp b/src/main/cpp/ShooterBehaviour.cpp
index 09a1d133..200a8bbe 100644
--- a/src/main/cpp/ShooterBehaviour.cpp
+++ b/src/main/cpp/ShooterBehaviour.cpp
@@ -46,4 +46,4 @@ void ShooterManualControl::OnTick(units::second_t dt) {
       _shooter->SetState(ShooterState::kIdle);
     }
   }
-}
+}
\ No newline at end of file
diff --git a/src/main/include/Robot.h b/src/main/include/Robot.h
index 7fd54a58..f7227a08 100644
--- a/src/main/include/Robot.h
+++ b/src/main/include/Robot.h
@@ -1,7 +1,6 @@
 // Copyright (c) 2023-2024 CurtinFRC
 // Open Source Software, you can modify it according to the terms
 // of the MIT License at the root of this project
-
 #pragma once
 #include <frc/Encoder.h>
 #include <frc/TimedRobot.h>
@@ -12,18 +11,12 @@
 #include <frc/smartdashboard/Field2d.h>
 #include <frc/smartdashboard/SendableChooser.h>
 #include <frc/smartdashboard/SmartDashboard.h>
-
 #include <string>
-
-#include "AlphaArm.h"
-#include "AlphaArmBehaviour.h"
-#include "Intake.h"
-#include "IntakeBehaviour.h"
 #include "RobotMap.h"
 #include "Shooter.h"
 #include "ShooterBehaviour.h"
 #include "Wombat.h"
-
+#include "RobotMap.h"
 class Robot : public frc::TimedRobot {
  public:
   void TestInit() override;
@@ -40,7 +33,7 @@ class Robot : public frc::TimedRobot {
   RobotMap robotmap;
   wom::BehaviourScheduler* sched;
   frc::EventLoop loop;
-  // Shooter* shooter;
+  Shooter *shooter;
 
   // Intake* intake;
   frc::SendableChooser<std::string> m_chooser;
diff --git a/src/main/include/RobotMap.h b/src/main/include/RobotMap.h
index 4c59fce4..0ed68e8d 100644
--- a/src/main/include/RobotMap.h
+++ b/src/main/include/RobotMap.h
@@ -1,6 +1,7 @@
 // Copyright (c) 2023-2024 CurtinFRC
 // Open Source Software, you can modify it according to the terms
 // of the MIT License at the root of this project
+
 #pragma once
 
 #include <frc/Compressor.h>
@@ -198,5 +199,15 @@ struct RobotMap {
         nt::NetworkTableInstance::GetDefault().GetTable("swerve");
   };
   SwerveTable swerveTable;
-};
 
+  // struct AlphaArmSystem {
+  //   rev::CANSparkMax alphaArmMotor{12, rev::CANSparkMax::MotorType::kBrushless};
+  //   rev::CANSparkMax wristMotor{15, rev::CANSparkMax::MotorType::kBrushless};
+
+  //   wom::Gearbox alphaArmGearbox{&alphaArmMotor, nullptr, frc::DCMotor::NEO(1)};
+  //   wom::Gearbox wristGearbox{&wristMotor, nullptr, frc::DCMotor::NEO(1)};
+
+  //   AlphaArmConfig config{alphaArmGearbox, wristGearbox};
+  // };
+  // AlphaArmSystem alphaArmSystem;
+};
\ No newline at end of file
diff --git a/src/main/include/Shooter.h b/src/main/include/Shooter.h
index 566e30be..0962e80d 100644
--- a/src/main/include/Shooter.h
+++ b/src/main/include/Shooter.h
@@ -46,4 +46,4 @@ class Shooter : public behaviour::HasBehaviour {
 
   units::volt_t holdVoltage = 0_V;
   std::string _statename = "default";
-};
+};
\ No newline at end of file
diff --git a/src/main/include/ShooterBehaviour.h b/src/main/include/ShooterBehaviour.h
index 12f93baa..15b54aaf 100644
--- a/src/main/include/ShooterBehaviour.h
+++ b/src/main/include/ShooterBehaviour.h
@@ -24,4 +24,4 @@ class ShooterManualControl : public behaviour::Behaviour {
   bool _rawControl = false;
   std::shared_ptr<nt::NetworkTable> table =
       nt::NetworkTableInstance::GetDefault().GetTable("Shooter Behaviour");
-};
+};
\ No newline at end of file

From 1e38cbcd901eefca690a4d99d91e6270736ed49e Mon Sep 17 00:00:00 2001
From: kill-shots <155516223+kill-shots@users.noreply.github.com>
Date: Tue, 23 Jan 2024 20:47:00 +0800
Subject: [PATCH 132/207] Intake (#100)

* First version Tank Drive

* Intake draft

* Intake draft fixed errors 1

* Intake draft fixed errors 2

* Intake draft fixed errors 3

* Intake draft fixed errors 4

* Intake draft fixed errors 5

* Network files 1

* Network files 2

* Network files 3

* Almost done i think

* finished wpiformat errors

* finished wpiformat errors 2

* finished wpiformat errors 3

* finished wpiformat errors 4

* ran wpiformat

* finished wpiformat errors 5

* finished wpiformat errors 8

* finished wpiformat errors 9

* finished wpiformat errors 10

* Tested on robot

* Fixed github build errors after modifying during robot tests

* fixed more github build errors 2

* fixed more github build errors 3

* Tested on robot and works

---------

Co-authored-by: Isaac Turner <spacey_sooty@outlook.com>
---
 src/main/cpp/Intake.cpp            |  2 +-
 src/main/cpp/IntakeBehaviour.cpp   |  2 +-
 src/main/cpp/Robot.cpp             | 56 +++++++-----------------------
 src/main/include/Intake.h          |  2 +-
 src/main/include/IntakeBehaviour.h |  2 +-
 src/main/include/Robot.h           | 15 ++++++--
 6 files changed, 29 insertions(+), 50 deletions(-)

diff --git a/src/main/cpp/Intake.cpp b/src/main/cpp/Intake.cpp
index 6110aa7e..a96c289b 100644
--- a/src/main/cpp/Intake.cpp
+++ b/src/main/cpp/Intake.cpp
@@ -78,4 +78,4 @@ void Intake::setRaw(units::volt_t voltage) {
 }
 IntakeState Intake::getState() {
   return _state;
-}
+}
\ No newline at end of file
diff --git a/src/main/cpp/IntakeBehaviour.cpp b/src/main/cpp/IntakeBehaviour.cpp
index 3d738fb3..3192ef12 100644
--- a/src/main/cpp/IntakeBehaviour.cpp
+++ b/src/main/cpp/IntakeBehaviour.cpp
@@ -60,4 +60,4 @@ IntakeAutoControl::IntakeAutoControl(Intake* intake) : _intake(intake) {
   Controls(intake);
 }
 
-void IntakeAutoControl::OnTick(units::second_t dt) {}
+void IntakeAutoControl::OnTick(units::second_t dt) {}
\ No newline at end of file
diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp
index 9e640dc1..f09b1685 100644
--- a/src/main/cpp/Robot.cpp
+++ b/src/main/cpp/Robot.cpp
@@ -34,12 +34,6 @@
 static units::second_t lastPeriodic;
 
 void Robot::RobotInit() {
-
-  shooter = new Shooter(robotmap.shooterSystem.config);
-  wom::BehaviourScheduler::GetInstance()->Register(shooter);
-  shooter->SetDefaultBehaviour(
-     [this]() {return wom::make<ShooterManualControl>(shooter, &robotmap.controllers.codriver); });
-  
   sched = wom::BehaviourScheduler::GetInstance();
   m_chooser.SetDefaultOption("Default Auto", "Default Auto");
 
@@ -64,13 +58,10 @@ void Robot::RobotInit() {
 
   // robotmap.swerveBase.gyro->Reset();
 
-  _swerveDrive =
-      new wom::SwerveDrive(robotmap.swerveBase.config, frc::Pose2d());
+  _swerveDrive = new wom::SwerveDrive(robotmap.swerveBase.config, frc::Pose2d());
   wom::BehaviourScheduler::GetInstance()->Register(_swerveDrive);
-  _swerveDrive->SetDefaultBehaviour([this]() {
-    return wom::make<wom::ManualDrivebase>(_swerveDrive,
-                                           &robotmap.controllers.driver);
-  });
+  _swerveDrive->SetDefaultBehaviour(
+      [this]() { return wom::make<wom::ManualDrivebase>(_swerveDrive, &robotmap.controllers.driver); });
 
   // alphaArm = new AlphaArm(robotmap.alphaArmSystem.config);
   // wom::BehaviourScheduler::GetInstance()->Register(alphaArm);
@@ -109,26 +100,6 @@ void Robot::RobotInit() {
   // robotmap.swerveBase.moduleConfigs[3].turnMotor.encoder->SetEncoderOffset(0_rad);
 
   lastPeriodic = wom::now();
-  
-  // m_driveSim = new wom::TempSimSwerveDrive(&simulation_timer, &m_field);
-  // m_driveSim = wom::TempSimSwerveDrive();
-
-  //robotmap.swerveBase.moduleConfigs[0].turnMotor.encoder->SetEncoderOffset(0_rad);
-  //robotmap.swerveBase.moduleConfigs[1].turnMotor.encoder->SetEncoderOffset(0_rad);
-  //robotmap.swerveBase.moduleConfigs[2].turnMotor.encoder->SetEncoderOffset(0_rad);
-  //robotmap.swerveBase.moduleConfigs[3].turnMotor.encoder->SetEncoderOffset(0_rad);
-
-
-
-
-  // frontLeft = new ctre::phoenix6::hardware::TalonFX(7, "Drivebase");  // front left
-  // frontRight = new ctre::phoenix6::hardware::TalonFX(2, "Drivebase");   // front right
-  // backLeft = new ctre::phoenix6::hardware::TalonFX(6, "Drivebase");   // back left
-  // backRight = new ctre::phoenix6::hardware::TalonFX(4, "Drivebase");  // back right
-  // frontLeft = new ctre::phoenix6::hardware::TalonFX(9, "Drivebase");   // front left
-  // frontRight = new ctre::phoenix6::hardware::TalonFX(1, "Drivebase");   // front right
-  // backLeft = new ctre::phoenix6::hardware::TalonFX(5, "Drivebase");   // back left
-  // backRight = new ctre::phoenix6::hardware::TalonFX(3, "Drivebase");
 }
 
 void Robot::RobotPeriodic() {
@@ -138,15 +109,8 @@ void Robot::RobotPeriodic() {
 
   loop.Poll();
   wom::BehaviourScheduler::GetInstance()->Tick();
-
-  shooter->OnUpdate(dt);
   sched->Tick();
 
-  robotmap.swerveTable.swerveDriveTable->GetEntry("frontLeftEncoder").SetDouble(robotmap.swerveBase.moduleConfigs[0].turnMotor.encoder->GetEncoderPosition().value());
-  robotmap.swerveTable.swerveDriveTable->GetEntry("frontRightEncoder").SetDouble(robotmap.swerveBase.moduleConfigs[1].turnMotor.encoder->GetEncoderPosition().value());
-  robotmap.swerveTable.swerveDriveTable->GetEntry("backLeftEncoder").SetDouble(robotmap.swerveBase.moduleConfigs[2].turnMotor.encoder->GetEncoderPosition().value());
-  robotmap.swerveTable.swerveDriveTable->GetEntry("backRightEncoder").SetDouble(robotmap.swerveBase.moduleConfigs[3].turnMotor.encoder->GetEncoderPosition().value());
-
   robotmap.swerveTable.swerveDriveTable->GetEntry("Encoder 0 offset: ")
       .SetDouble(robotmap.swerveBase.moduleConfigs[0].turnMotor.encoder->GetEncoderPosition().value());
   robotmap.swerveTable.swerveDriveTable->GetEntry("Encoder 1 offset: ")
@@ -163,10 +127,13 @@ void Robot::RobotPeriodic() {
 }
 
 void Robot::AutonomousInit() {
+  // m_driveSim->SetPath(m_path_chooser.GetSelected());
+
   loop.Clear();
   sched->InterruptAll();
 }
 void Robot::AutonomousPeriodic() {
+  // m_driveSim->OnUpdate();
 }
 
 void Robot::TeleopInit() {
@@ -175,13 +142,16 @@ void Robot::TeleopInit() {
   // shooter->OnStart();
   // alphaArm->OnStart();
   sched->InterruptAll();
+
+  // frontLeft->SetVoltage(4_V);
+  // frontRight->SetVoltage(4_V);
+  // backLeft->SetVoltage(4_V);
+  // backRight->SetVoltage(4_V);
 }
-void Robot::TeleopPeriodic() {
-  
-}
+void Robot::TeleopPeriodic() {}
 
 void Robot::DisabledInit() {}
 void Robot::DisabledPeriodic() {}
 
 void Robot::TestInit() {}
-void Robot::TestPeriodic() {}
+void Robot::TestPeriodic() {}
\ No newline at end of file
diff --git a/src/main/include/Intake.h b/src/main/include/Intake.h
index 8d2a07d3..1aca262a 100644
--- a/src/main/include/Intake.h
+++ b/src/main/include/Intake.h
@@ -40,4 +40,4 @@ class Intake : public behaviour::HasBehaviour {
   units::volt_t _setVoltage = 0_V;
 
   std::shared_ptr<nt::NetworkTable> _table = nt::NetworkTableInstance::GetDefault().GetTable("Intake");
-};
+};
\ No newline at end of file
diff --git a/src/main/include/IntakeBehaviour.h b/src/main/include/IntakeBehaviour.h
index 5233c3c2..5dbe8ae6 100644
--- a/src/main/include/IntakeBehaviour.h
+++ b/src/main/include/IntakeBehaviour.h
@@ -31,4 +31,4 @@ class IntakeAutoControl : public behaviour::Behaviour {
 
  private:
   Intake* _intake;
-};
+};
\ No newline at end of file
diff --git a/src/main/include/Robot.h b/src/main/include/Robot.h
index f7227a08..40728250 100644
--- a/src/main/include/Robot.h
+++ b/src/main/include/Robot.h
@@ -1,6 +1,7 @@
 // Copyright (c) 2023-2024 CurtinFRC
 // Open Source Software, you can modify it according to the terms
 // of the MIT License at the root of this project
+
 #pragma once
 #include <frc/Encoder.h>
 #include <frc/TimedRobot.h>
@@ -11,12 +12,18 @@
 #include <frc/smartdashboard/Field2d.h>
 #include <frc/smartdashboard/SendableChooser.h>
 #include <frc/smartdashboard/SmartDashboard.h>
+
 #include <string>
+
+#include "AlphaArm.h"
+#include "AlphaArmBehaviour.h"
+#include "Intake.h"
+#include "IntakeBehaviour.h"
 #include "RobotMap.h"
 #include "Shooter.h"
 #include "ShooterBehaviour.h"
 #include "Wombat.h"
-#include "RobotMap.h"
+
 class Robot : public frc::TimedRobot {
  public:
   void TestInit() override;
@@ -29,11 +36,12 @@ class Robot : public frc::TimedRobot {
   void TeleopPeriodic() override;
   void DisabledInit() override;
   void DisabledPeriodic() override;
+
  private:
   RobotMap robotmap;
   wom::BehaviourScheduler* sched;
   frc::EventLoop loop;
-  Shooter *shooter;
+  // Shooter* shooter;
 
   // Intake* intake;
   frc::SendableChooser<std::string> m_chooser;
@@ -47,10 +55,11 @@ class Robot : public frc::TimedRobot {
   wom::SwerveDrive* _swerveDrive;
   // AlphaArm* alphaArm;
 
+  ctre::phoenix6::hardware::TalonFX* frontLeft;
   // AlphaArm *alphaArm;
 
   // ctre::phoenix6::hardware::TalonFX *frontLeft;
   // ctre::phoenix6::hardware::TalonFX *frontRight;
   // ctre::phoenix6::hardware::TalonFX *backLeft;
   // ctre::phoenix6::hardware::TalonFX *backRight;
-};
+};
\ No newline at end of file

From 8297dccb095b1d3d5f5b1088da1a12931743414d Mon Sep 17 00:00:00 2001
From: JoystickMaster-race
 <97269195+JoystickMaster-race@users.noreply.github.com>
Date: Wed, 24 Jan 2024 09:45:57 +0800
Subject: [PATCH 133/207] Arm (#103)

* Alpha Arm vers 1

* Alpha Arm ver 1 w/ explicit

* Alpha arm v1 formatting

* Alpha arm w/ wrist v1

* formatted

* Alpha arm v1 with ports

* Working alpha arm no encoders

---------

Co-authored-by: Isaac Turner <spacey_sooty@outlook.com>
---
 src/main/cpp/AlphaArm.cpp            | 2 +-
 src/main/cpp/AlphaArmBehaviour.cpp   | 2 +-
 src/main/include/AlphaArm.h          | 2 +-
 src/main/include/AlphaArmBehaviour.h | 2 +-
 src/main/include/Robot.h             | 2 ++
 5 files changed, 6 insertions(+), 4 deletions(-)

diff --git a/src/main/cpp/AlphaArm.cpp b/src/main/cpp/AlphaArm.cpp
index 93b5b5f3..f0db4381 100644
--- a/src/main/cpp/AlphaArm.cpp
+++ b/src/main/cpp/AlphaArm.cpp
@@ -117,4 +117,4 @@ void AlphaArm::SetArmRaw(units::volt_t voltage) {
 
 void AlphaArm::setControllerRaw(units::volt_t voltage) {
   _controlledRawVoltage = voltage;
-}
+}
\ No newline at end of file
diff --git a/src/main/cpp/AlphaArmBehaviour.cpp b/src/main/cpp/AlphaArmBehaviour.cpp
index 9a28b311..56e6951d 100644
--- a/src/main/cpp/AlphaArmBehaviour.cpp
+++ b/src/main/cpp/AlphaArmBehaviour.cpp
@@ -27,4 +27,4 @@ void AlphaArmManualControl::OnTick(units::second_t dt) {
     _alphaArm->SetState(AlphaArmState::kAmpAngle);
     _alphaArm->setControllerRaw(wom::deadzone(_codriver->GetRightY()) * 12_V);
   }
-}
+}
\ No newline at end of file
diff --git a/src/main/include/AlphaArm.h b/src/main/include/AlphaArm.h
index db0bc3e6..def7a5cd 100644
--- a/src/main/include/AlphaArm.h
+++ b/src/main/include/AlphaArm.h
@@ -78,4 +78,4 @@ class AlphaArm : public ::behaviour::HasBehaviour {
 
   std::string _stateName = "Default";
   bool started = false;
-};
+};
\ No newline at end of file
diff --git a/src/main/include/AlphaArmBehaviour.h b/src/main/include/AlphaArmBehaviour.h
index 71825c62..d4ce61f7 100644
--- a/src/main/include/AlphaArmBehaviour.h
+++ b/src/main/include/AlphaArmBehaviour.h
@@ -19,4 +19,4 @@ class AlphaArmManualControl : public behaviour::Behaviour {
   AlphaArm* _alphaArm;
   frc::XboxController* _codriver;
   bool _rawControl = false;
-};
+};
\ No newline at end of file
diff --git a/src/main/include/Robot.h b/src/main/include/Robot.h
index 40728250..06862255 100644
--- a/src/main/include/Robot.h
+++ b/src/main/include/Robot.h
@@ -58,6 +58,8 @@ class Robot : public frc::TimedRobot {
   ctre::phoenix6::hardware::TalonFX* frontLeft;
   // AlphaArm *alphaArm;
 
+  AlphaArm *alphaArm;
+
   // ctre::phoenix6::hardware::TalonFX *frontLeft;
   // ctre::phoenix6::hardware::TalonFX *frontRight;
   // ctre::phoenix6::hardware::TalonFX *backLeft;

From 1dc1c2422399f4c2779d936e1e3e230edd2bdbcd Mon Sep 17 00:00:00 2001
From: Isaac Turner <spacey_sooty@outlook.com>
Date: Wed, 24 Jan 2024 14:33:05 +0800
Subject: [PATCH 134/207] fix some formatting

---
 .clang-format                                 |   2 +-
 .editorconfig                                 |   5 -
 build.gradle                                  |   2 +-
 init.ps1                                      |   2 -
 init.sh                                       |   5 -
 src/main/cpp/AlphaArm.cpp                     | 104 ++++---------
 src/main/cpp/AlphaArmBehaviour.cpp            |   9 ++
 src/main/cpp/Robot.cpp                        |  61 +++++++-
 src/main/cpp/Shooter.cpp                      |  12 ++
 src/main/cpp/ShooterBehaviour.cpp             |  18 +++
 src/main/include/AlphaArm.h                   |  31 +++-
 src/main/include/Robot.h                      |   6 +-
 src/main/include/RobotMap.h                   |  64 +++++++-
 wombat/src/main/cpp/behaviour/Behaviour.cpp   |  23 +--
 .../main/cpp/behaviour/BehaviourScheduler.cpp |   4 +-
 .../src/main/cpp/behaviour/HasBehaviour.cpp   |   3 +-
 wombat/src/main/cpp/drivetrain/Drivetrain.cpp |  51 +++++++
 .../src/main/cpp/drivetrain/SwerveDrive.cpp   |  49 ++++++
 .../behaviours/SwerveBehaviours.cpp           |   4 +
 wombat/src/main/cpp/subsystems/Arm.cpp        |  35 ++---
 wombat/src/main/cpp/subsystems/Elevator.cpp   | 141 ++++++++++++++++++
 wombat/src/main/cpp/subsystems/Shooter.cpp    |  86 +++++++++++
 wombat/src/main/cpp/utils/Encoder.cpp         |  44 ++----
 wombat/src/main/cpp/utils/Util.cpp            |  27 +---
 wombat/src/main/cpp/vision/Limelight.cpp      |  24 ++-
 wombat/src/main/include/behaviour/Behaviour.h |  42 ++----
 .../src/main/include/behaviour/HasBehaviour.h |   3 +-
 wombat/src/main/include/subsystems/Arm.h      |   3 +-
 wombat/src/main/include/subsystems/Shooter.h  |   3 +-
 wombat/src/main/include/utils/Util.h          |  55 +++----
 wombat/src/main/include/vision/Limelight.h    |  22 +--
 31 files changed, 653 insertions(+), 287 deletions(-)
 delete mode 100644 .editorconfig
 delete mode 100644 init.ps1
 delete mode 100644 init.sh
 create mode 100644 wombat/src/main/cpp/drivetrain/Drivetrain.cpp
 create mode 100644 wombat/src/main/cpp/subsystems/Elevator.cpp
 create mode 100644 wombat/src/main/cpp/subsystems/Shooter.cpp

diff --git a/.clang-format b/.clang-format
index f69bef0c..a879dec4 100644
--- a/.clang-format
+++ b/.clang-format
@@ -81,7 +81,7 @@ BreakBeforeTernaryOperators: true
 BreakConstructorInitializers: BeforeColon
 BreakInheritanceList: BeforeColon
 BreakStringLiterals: true
-ColumnLimit:     80
+ColumnLimit:     110
 CommentPragmas:  '^ IWYU pragma:'
 CompactNamespaces: false
 ConstructorInitializerIndentWidth: 4
diff --git a/.editorconfig b/.editorconfig
deleted file mode 100644
index 0020fc03..00000000
--- a/.editorconfig
+++ /dev/null
@@ -1,5 +0,0 @@
-root = true
-
-[*]
-indent_style = space
-indent_size = 2
diff --git a/build.gradle b/build.gradle
index 088030b4..728f2b3b 100644
--- a/build.gradle
+++ b/build.gradle
@@ -1,7 +1,7 @@
 plugins {
     id "cpp"
     id "google-test-test-suite"
-    id "edu.wpi.first.GradleRIO" version "2024.1.1"
+    id "edu.wpi.first.GradleRIO" version "2024.2.1"
 }
 
 // Define my targets (RoboRIO) and artifacts (deployable files)
diff --git a/init.ps1 b/init.ps1
deleted file mode 100644
index fa83b9a6..00000000
--- a/init.ps1
+++ /dev/null
@@ -1,2 +0,0 @@
-./gradlew installRoborioToolchain
-./gradlew build
diff --git a/init.sh b/init.sh
deleted file mode 100644
index 6a932ed6..00000000
--- a/init.sh
+++ /dev/null
@@ -1,5 +0,0 @@
-#!/usr/bin/sh
-sudo apt update
-chmod +x gradlew
-./gradlew installRoborioToolchain
-./gradlew build
diff --git a/src/main/cpp/AlphaArm.cpp b/src/main/cpp/AlphaArm.cpp
index f0db4381..e648773d 100644
--- a/src/main/cpp/AlphaArm.cpp
+++ b/src/main/cpp/AlphaArm.cpp
@@ -10,111 +10,65 @@ AlphaArm::AlphaArm(AlphaArmConfig config)
       _pidWom(_config.path + "/pid",
               config.pidConfigA) /*_table(nt::NetworkTableInstance::GetDefault().GetTable(config.path)*/ {}
 
-void AlphaArm::OnStart() {
-  started = false;
-  // _pidWom.Reset();
-  // _pidWom.SetSetpoint(_config.alphaArmGearbox.encoder->GetEncoderPosition());
+AlphaArmConfig AlphaArm::GetConfig() {
+  return _config;
 }
 
 void AlphaArm::OnUpdate(units::second_t dt) {
-  _table->GetEntry("Error").SetDouble(_pidWom.GetError().value());
-  _table->GetEntry("Current Pos").SetDouble(_config.alphaArmGearbox.encoder->GetEncoderPosition().value());
-  _table->GetEntry("Setpoint").SetDouble(_pidWom.GetSetpoint().value());
-  _table->GetEntry("State ").SetString(_stateName);
   switch (_state) {
     case AlphaArmState::kIdle:
-      _stateName = "Idle";
-      // _pidWom.SetSetpoint(_config.alphaArmGearbox.encoder->GetEncoderPosition());
 
+      // _config.alphaArmGearbox.motorController->SetVoltage(0_V);
+      // _config.wristGearbox.motorController->SetVoltage(0_V);
       _setAlphaArmVoltage = 0_V;
+      _setWristVoltage = 0_V;
 
       break;
     case AlphaArmState::kRaw:
-      _stateName = "Raw";
-
       _setAlphaArmVoltage = _rawArmVoltage;
+      _setWristVoltage = _rawWristVoltage;
+      _config.alphaArmGearbox.motorController->SetVoltage(_rawArmVoltage);
+      _config.wristGearbox.motorController->SetVoltage(_rawWristVoltage);
 
       break;
-    case AlphaArmState::kAmpAngle: {
-      _stateName = "Amp Angle";
-
-      // _pidWom.SetSetpoint(_goal);
-      // _setAlphaArmVoltage = _pidWom.Calculate(_config.alphaArmGearbox.encoder->GetEncoderPosition(), dt,
-      // 0_V);
-
-      if (started) {
-        if (_controlledRawVoltage.value() == 0) {
-          if (-_config.alphaArmGearbox.encoder->GetEncoderPosition() > (_startingPos + (3.1415_rad / 2))) {
-            // _pidWom.SetSetpoint(_encoderSetpoint);
-            // _setAlphaArmVoltage =
-            // -_pidWom.Calculate(-_config.alphaArmGearbox.encoder->GetEncoderPosition(), dt, 0.0_V);
-            //   _table->GetEntry("Demand").SetDouble(_setAlphaArmVoltage.value());
-            // } else if (_config.alphaArmGearbox.encoder->GetEncoderPosition() < 0_rad) {
-            //   _pidWom.SetSetpoint(_encoderSetpoint);
-            //   _setAlphaArmVoltage =
-            //   _pidWom.Calculate(_config.alphaArmGearbox.encoder->GetEncoderPosition(), dt, 0.0_V);
-            //   _table->GetEntry("Demand").SetDouble(_setAlphaArmVoltage.value());
-            _setAlphaArmVoltage = 0_V;
-          } else {
-            _pidWom.SetSetpoint(_encoderSetpoint);
-            _setAlphaArmVoltage =
-                -_pidWom.Calculate(-_config.alphaArmGearbox.encoder->GetEncoderPosition(), dt, 0.0_V);
-            //   _pidWom.Reset();
-            //   _encoderSetpoint = _config.alphaArmGearbox.encoder->GetEncoderPosition();
-            //   _setAlphaArmVoltage = _controlledRawVoltage;
-          }
-        } else {
-          _pidWom.Reset();
-          _encoderSetpoint = -_config.alphaArmGearbox.encoder->GetEncoderPosition();
-          _setAlphaArmVoltage = _controlledRawVoltage;
-        }
-      } else {
-        _pidWom.Reset();
-        _encoderSetpoint = -_config.alphaArmGearbox.encoder->GetEncoderPosition();
-        _setAlphaArmVoltage = _controlledRawVoltage;
+    case AlphaArmState::kForwardWrist:
+      _config.wristGearbox.motorController->SetVoltage(3_V);
+      _setWristVoltage = 3_V;
 
-        if (std::abs(_controlledRawVoltage.value()) > 0) {
-          _startingPos = -_config.alphaArmGearbox.encoder->GetEncoderPosition();
-          started = true;
-        }
-      }
-    } break;
-    case AlphaArmState::kSpeakerAngle:
-      _stateName = "Speaker Angle";
-      // _pidWom.SetSetpoint(_goal);
-      // _setAlphaArmVoltage = _pidWom.Calculate(_config.alphaArmGearbox.encoder->GetEncoderPosition(), dt,
-      // 0_V);
-      break;
-    case AlphaArmState::kStowed:
-      _stateName = "Stowed";
-      // _pidWom.SetSetpoint(_goal);
-      // _setAlphaArmVoltage = _pidWom.Calculate(_config.alphaArmGearbox.encoder->GetEncoderPosition(), dt,
-      // 0_V);
-      break;
+    case AlphaArmState::kReverseWrist:
+      _config.wristGearbox.motorController->SetVoltage(-3_V);
+      _setWristVoltage = -3_V;
     default:
-      _stateName = "Error";
       std::cout << "oops, wrong state" << std::endl;
       break;
   }
-  std::cout << " ARM POSITION: " << _config.alphaArmGearbox.encoder->GetEncoderPosition().value()
-            << std::endl;
-  // std::cout << "OUTPUT VOLTAGE: " << _setAlphaArmVoltage.value() << std::endl;
+  // transmission translate
+  // _config.armGearBox.motorController->SetVoltage(setAlphaArmVoltage);
+  // _config.alphaArmGearbox.motorController->SetVoltage(setAlphaArmVoltage);
+  // _config.wristGearbox.motorController->SetVoltage(setWristVoltage);
   _config.alphaArmGearbox.motorController->SetVoltage(_setAlphaArmVoltage);
+  _config.wristGearbox.motorController->SetVoltage(_setWristVoltage);
+
+  // _config.wristGearbox.motorController->SetVoltage(_setVoltage);
 }
 
 void AlphaArm::SetState(AlphaArmState state) {
   _state = state;
 }
 
-void AlphaArm::SetGoal(units::radian_t goal) {
-  _goal = goal;
-}
+// void AlphaArm::SetRaw(units::volt_t voltage){
+//     _rawArmVoltage = voltage;
+//     _rawWristVoltage = voltage;
+// }
 
 void AlphaArm::SetArmRaw(units::volt_t voltage) {
-  std::cout << "VOLTAGE: " << voltage.value() << std::endl;
   _rawArmVoltage = voltage;
 }
 
+void AlphaArm::setWristRaw(units::volt_t voltage) {
+  _rawWristVoltage = voltage;
+}
+
 void AlphaArm::setControllerRaw(units::volt_t voltage) {
   _controlledRawVoltage = voltage;
 }
\ No newline at end of file
diff --git a/src/main/cpp/AlphaArmBehaviour.cpp b/src/main/cpp/AlphaArmBehaviour.cpp
index 56e6951d..0d75d6f1 100644
--- a/src/main/cpp/AlphaArmBehaviour.cpp
+++ b/src/main/cpp/AlphaArmBehaviour.cpp
@@ -24,7 +24,16 @@ void AlphaArmManualControl::OnTick(units::second_t dt) {
     _alphaArm->SetState(AlphaArmState::kRaw);
     _alphaArm->SetArmRaw(_codriver->GetRightY() * 12_V);
   } else {
+<<<<<<< HEAD
     _alphaArm->SetState(AlphaArmState::kAmpAngle);
     _alphaArm->setControllerRaw(wom::deadzone(_codriver->GetRightY()) * 12_V);
+=======
+    if (_codriver->GetRightBumperPressed()) {
+      _alphaArm->SetState(AlphaArmState::kForwardWrist);
+    }
+    if (_codriver->GetLeftBumperPressed()) {
+      _alphaArm->SetState(AlphaArmState::kReverseWrist);
+    }
+>>>>>>> 0029f49 (fix some formatting)
   }
 }
\ No newline at end of file
diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp
index f09b1685..a0d1287a 100644
--- a/src/main/cpp/Robot.cpp
+++ b/src/main/cpp/Robot.cpp
@@ -18,6 +18,7 @@
 #include <units/velocity.h>
 #include <units/voltage.h>
 
+<<<<<<< HEAD
 // include units
 #include <units/velocity.h>
 #include <units/acceleration.h>
@@ -29,11 +30,21 @@
 #include <frc/kinematics/DifferentialDriveKinematics.h>
 #include <frc/controller/RamseteController.h>
 #include <frc/Timer.h>
+=======
+>>>>>>> 0029f49 (fix some formatting)
 #include "behaviour/HasBehaviour.h"
 
 static units::second_t lastPeriodic;
 
 void Robot::RobotInit() {
+<<<<<<< HEAD
+=======
+  shooter = new Shooter(robotmap.shooterSystem.config);
+  wom::BehaviourScheduler::GetInstance()->Register(shooter);
+  shooter->SetDefaultBehaviour(
+      [this]() { return wom::make<ShooterManualControl>(shooter, &robotmap.controllers.codriver); });
+
+>>>>>>> 0029f49 (fix some formatting)
   sched = wom::BehaviourScheduler::GetInstance();
   m_chooser.SetDefaultOption("Default Auto", "Default Auto");
 
@@ -58,11 +69,19 @@ void Robot::RobotInit() {
 
   // robotmap.swerveBase.gyro->Reset();
 
+<<<<<<< HEAD
+=======
+  // _swerveDrive = new wom::SwerveDrive(robotmap.swerveBase.config, frc::Pose2d());
+  // wom::BehaviourScheduler::GetInstance()->Register(_swerveDrive);
+  // _swerveDrive->SetDefaultBehaviour(
+  //     [this]() { return wom::make<wom::ManualDrivebase>(_swerveDrive, &robotmap.controllers.driver); });
+>>>>>>> 0029f49 (fix some formatting)
   _swerveDrive = new wom::SwerveDrive(robotmap.swerveBase.config, frc::Pose2d());
   wom::BehaviourScheduler::GetInstance()->Register(_swerveDrive);
   _swerveDrive->SetDefaultBehaviour(
       [this]() { return wom::make<wom::ManualDrivebase>(_swerveDrive, &robotmap.controllers.driver); });
 
+<<<<<<< HEAD
   // alphaArm = new AlphaArm(robotmap.alphaArmSystem.config);
   // wom::BehaviourScheduler::GetInstance()->Register(alphaArm);
   // alphaArm->SetDefaultBehaviour(
@@ -99,6 +118,29 @@ void Robot::RobotInit() {
   // robotmap.swerveBase.moduleConfigs[2].turnMotor.encoder->SetEncoderOffset(0_rad);
   // robotmap.swerveBase.moduleConfigs[3].turnMotor.encoder->SetEncoderOffset(0_rad);
 
+=======
+  // m_driveSim = new wom::TempSimSwerveDrive(&simulation_timer, &m_field);
+  // m_driveSim = wom::TempSimSwerveDrive();
+
+  alphaArm = new AlphaArm(robotmap.alphaArmSystem.config);
+  wom::BehaviourScheduler::GetInstance()->Register(alphaArm);
+  alphaArm->SetDefaultBehaviour(
+      [this]() { return wom::make<AlphaArmManualControl>(alphaArm, &robotmap.controllers.codriver); });
+
+  robotmap.swerveBase.moduleConfigs[0].turnMotor.encoder->SetEncoderOffset(0_rad);
+  robotmap.swerveBase.moduleConfigs[1].turnMotor.encoder->SetEncoderOffset(0_rad);
+  robotmap.swerveBase.moduleConfigs[2].turnMotor.encoder->SetEncoderOffset(0_rad);
+  robotmap.swerveBase.moduleConfigs[3].turnMotor.encoder->SetEncoderOffset(0_rad);
+
+  // frontLeft = new ctre::phoenix6::hardware::TalonFX(7, "Drivebase");  // front left
+  // frontRight = new ctre::phoenix6::hardware::TalonFX(2, "Drivebase");   // front right
+  // backLeft = new ctre::phoenix6::hardware::TalonFX(6, "Drivebase");   // back left
+  // backRight = new ctre::phoenix6::hardware::TalonFX(4, "Drivebase");  // back right
+  // frontLeft = new ctre::phoenix6::hardware::TalonFX(9, "Drivebase");   // front left
+  // frontRight = new ctre::phoenix6::hardware::TalonFX(1, "Drivebase");   // front right
+  // backLeft = new ctre::phoenix6::hardware::TalonFX(5, "Drivebase");   // back left
+  // backRight = new ctre::phoenix6::hardware::TalonFX(3, "Drivebase");
+>>>>>>> 0029f49 (fix some formatting)
   lastPeriodic = wom::now();
 }
 
@@ -111,6 +153,7 @@ void Robot::RobotPeriodic() {
   wom::BehaviourScheduler::GetInstance()->Tick();
   sched->Tick();
 
+<<<<<<< HEAD
   robotmap.swerveTable.swerveDriveTable->GetEntry("Encoder 0 offset: ")
       .SetDouble(robotmap.swerveBase.moduleConfigs[0].turnMotor.encoder->GetEncoderPosition().value());
   robotmap.swerveTable.swerveDriveTable->GetEntry("Encoder 1 offset: ")
@@ -118,6 +161,15 @@ void Robot::RobotPeriodic() {
   robotmap.swerveTable.swerveDriveTable->GetEntry("Encoder 2 offset: ")
       .SetDouble(robotmap.swerveBase.moduleConfigs[2].turnMotor.encoder->GetEncoderPosition().value());
   robotmap.swerveTable.swerveDriveTable->GetEntry("Encoder 3 offset: ")
+=======
+  robotmap.swerveTable.swerveDriveTable->GetEntry("frontLeftEncoder")
+      .SetDouble(robotmap.swerveBase.moduleConfigs[0].turnMotor.encoder->GetEncoderPosition().value());
+  robotmap.swerveTable.swerveDriveTable->GetEntry("frontRightEncoder")
+      .SetDouble(robotmap.swerveBase.moduleConfigs[1].turnMotor.encoder->GetEncoderPosition().value());
+  robotmap.swerveTable.swerveDriveTable->GetEntry("backLeftEncoder")
+      .SetDouble(robotmap.swerveBase.moduleConfigs[2].turnMotor.encoder->GetEncoderPosition().value());
+  robotmap.swerveTable.swerveDriveTable->GetEntry("backRightEncoder")
+>>>>>>> 0029f49 (fix some formatting)
       .SetDouble(robotmap.swerveBase.moduleConfigs[3].turnMotor.encoder->GetEncoderPosition().value());
 
   // shooter->OnUpdate(dt);
@@ -132,15 +184,22 @@ void Robot::AutonomousInit() {
   loop.Clear();
   sched->InterruptAll();
 }
+<<<<<<< HEAD
 void Robot::AutonomousPeriodic() {
   // m_driveSim->OnUpdate();
 }
+=======
+void Robot::AutonomousPeriodic() {}
+>>>>>>> 0029f49 (fix some formatting)
 
 void Robot::TeleopInit() {
   loop.Clear();
   wom::BehaviourScheduler* sched = wom::BehaviourScheduler::GetInstance();
+<<<<<<< HEAD
   // shooter->OnStart();
   // alphaArm->OnStart();
+=======
+>>>>>>> 0029f49 (fix some formatting)
   sched->InterruptAll();
 
   // frontLeft->SetVoltage(4_V);
@@ -154,4 +213,4 @@ void Robot::DisabledInit() {}
 void Robot::DisabledPeriodic() {}
 
 void Robot::TestInit() {}
-void Robot::TestPeriodic() {}
\ No newline at end of file
+void Robot::TestPeriodic() {}
diff --git a/src/main/cpp/Shooter.cpp b/src/main/cpp/Shooter.cpp
index e20c059e..410da01b 100644
--- a/src/main/cpp/Shooter.cpp
+++ b/src/main/cpp/Shooter.cpp
@@ -4,12 +4,20 @@
 
 #include "Shooter.h"
 
+<<<<<<< HEAD
 Shooter::Shooter(ShooterConfig config) : _config(config), _pid(config.path + "/pid", config.pidConfig) {}
 
 void Shooter::OnStart() {
   _pid.Reset();
 }
 
+=======
+Shooter::Shooter(ShooterConfig config)
+    : _config(config)
+// ,
+// _pid{frc::PIDController (1, 0, 0, 0.005_s)}
+{}  // config.path + "/pid", config.pidConfig
+>>>>>>> 0029f49 (fix some formatting)
 void Shooter::OnUpdate(units::second_t dt) {
   // _pid.SetTolerance(0.5, 4);
   table->GetEntry("Error").SetDouble(_pid.GetError().value());
@@ -93,10 +101,14 @@ void Shooter::OnUpdate(units::second_t dt) {
       std::cout << "Error shooter in invalid state" << std::endl;
     } break;
   }
+<<<<<<< HEAD
   // table->GetEntry("Motor OutPut").SetDouble(_setVoltage.value());
   table->GetEntry("Encoder Output")
       .SetDouble(_config.ShooterGearbox.encoder->GetEncoderAngularVelocity().value());
 
+=======
+  std::cout << "Voltage:" << _setVoltage.value() << std::endl;
+>>>>>>> 0029f49 (fix some formatting)
   _config.ShooterGearbox.motorController->SetVoltage(_setVoltage);
 }
 
diff --git a/src/main/cpp/ShooterBehaviour.cpp b/src/main/cpp/ShooterBehaviour.cpp
index 200a8bbe..24aa9242 100644
--- a/src/main/cpp/ShooterBehaviour.cpp
+++ b/src/main/cpp/ShooterBehaviour.cpp
@@ -12,20 +12,38 @@ ShooterManualControl::ShooterManualControl(Shooter* shooter, frc::XboxController
 void ShooterManualControl::OnTick(units::second_t dt) {
   table->GetEntry("RawControl").SetBoolean(_rawControl);
 
+<<<<<<< HEAD
   if (_codriver->GetBackButtonPressed()) {
     if (_rawControl == true) {
+=======
+  if (_codriver->GetAButtonReleased()) {
+    if (_rawControl) {
+>>>>>>> 0029f49 (fix some formatting)
       _rawControl = false;
     } else {
       _rawControl = true;
     }
   }
 
+<<<<<<< HEAD
   if (_rawControl) {
     _shooter->SetState(ShooterState::kRaw);
     if (_codriver->GetLeftTriggerAxis() > 0.1) {
       _shooter->SetRaw(12_V * _codriver->GetLeftTriggerAxis());
     } else if (_codriver->GetRightTriggerAxis() > 0.1) {
       _shooter->SetRaw(-12_V * _codriver->GetRightTriggerAxis());
+=======
+  if (!_rawControl) {
+    if (_codriver->GetYButton()) {
+      _shooter->SetState(ShooterState::kSpinUp);
+      _shooter->SetPidGoal(10_rad_per_s);
+    }
+  } else {
+    if (_codriver->GetRightTriggerAxis() > 0.1) {
+      _shooter->SetRaw(_codriver->GetRightTriggerAxis() * 12_V);
+    } else if (_codriver->GetLeftTriggerAxis() > 0.1) {
+      _shooter->SetRaw(_codriver->GetLeftTriggerAxis() * -12_V);
+>>>>>>> 0029f49 (fix some formatting)
     } else {
       _shooter->SetRaw(0_V);
     }
diff --git a/src/main/include/AlphaArm.h b/src/main/include/AlphaArm.h
index def7a5cd..2d3bc6de 100644
--- a/src/main/include/AlphaArm.h
+++ b/src/main/include/AlphaArm.h
@@ -16,6 +16,7 @@
 
 struct AlphaArmConfig {
   wom::Gearbox alphaArmGearbox;
+<<<<<<< HEAD
   // wom::Gearbox wristGearbox;
   // wom::DutyCycleEncoder* armEncoder;
   // wom::CANSparkMaxEncoder* armEncoder;
@@ -23,6 +24,9 @@ struct AlphaArmConfig {
   // wom::utils::PIDConfig<units::radians_per_second, units::volt> velocityConfig;
   std::string path;
   // void WriteNT(std::shared_ptr<nt::NetworkTable> table);
+=======
+  wom::Gearbox wristGearbox;
+>>>>>>> 0029f49 (fix some formatting)
 };
 
 enum class AlphaArmState {
@@ -30,16 +34,23 @@ enum class AlphaArmState {
   kIntakeAngle,
   kAmpAngle,
   kSpeakerAngle,
+<<<<<<< HEAD
   kStowed,
   kRaw
   // kForwardWrist,
   // kReverseWrist,
+=======
+  kForwardWrist,
+  kReverseWrist,
+  kRaw
+>>>>>>> 0029f49 (fix some formatting)
 };
 
 class AlphaArm : public ::behaviour::HasBehaviour {
  public:
   explicit AlphaArm(AlphaArmConfig config);
 
+<<<<<<< HEAD
   void OnStart();
   void OnUpdate(units::second_t dt);
   void SetArmRaw(units::volt_t voltage);
@@ -78,4 +89,22 @@ class AlphaArm : public ::behaviour::HasBehaviour {
 
   std::string _stateName = "Default";
   bool started = false;
-};
\ No newline at end of file
+};
+=======
+  void OnUpdate(units::second_t dt);
+  void SetArmRaw(units::volt_t voltage);
+  void setWristRaw(units::volt_t voltage);
+  void SetState(AlphaArmState state);
+  AlphaArmConfig GetConfig();
+  // void SetRaw(units::volt_t voltage);
+
+ private:
+  AlphaArmConfig _config;
+  AlphaArmState _state = AlphaArmState::kIdle;
+  units::volt_t _setAlphaArmVoltage = 0_V;
+  units::volt_t _setWristVoltage = 0_V;
+
+  units::volt_t _rawArmVoltage = 0_V;
+  units::volt_t _rawWristVoltage = 0_V;
+};
+>>>>>>> 0029f49 (fix some formatting)
diff --git a/src/main/include/Robot.h b/src/main/include/Robot.h
index 06862255..ae2bf634 100644
--- a/src/main/include/Robot.h
+++ b/src/main/include/Robot.h
@@ -41,7 +41,11 @@ class Robot : public frc::TimedRobot {
   RobotMap robotmap;
   wom::BehaviourScheduler* sched;
   frc::EventLoop loop;
+<<<<<<< HEAD
   // Shooter* shooter;
+=======
+  Shooter* shooter;
+>>>>>>> 0029f49 (fix some formatting)
 
   // Intake* intake;
   frc::SendableChooser<std::string> m_chooser;
@@ -58,7 +62,7 @@ class Robot : public frc::TimedRobot {
   ctre::phoenix6::hardware::TalonFX* frontLeft;
   // AlphaArm *alphaArm;
 
-  AlphaArm *alphaArm;
+  AlphaArm* alphaArm;
 
   // ctre::phoenix6::hardware::TalonFX *frontLeft;
   // ctre::phoenix6::hardware::TalonFX *frontRight;
diff --git a/src/main/include/RobotMap.h b/src/main/include/RobotMap.h
index 0ed68e8d..b4b2a3f1 100644
--- a/src/main/include/RobotMap.h
+++ b/src/main/include/RobotMap.h
@@ -21,7 +21,6 @@
 #include <ctre/phoenix6/TalonFX.hpp>
 
 #include "AlphaArm.h"
-#include "AlphaArmBehaviour.h"
 #include "Intake.h"
 #include "Shooter.h"
 #include "Wombat.h"
@@ -42,18 +41,32 @@ struct RobotMap {
 
   //     wom::Gearbox alphaArmGearbox{&alphaArmMotor, armEncoder, frc::DCMotor::NEO(1)};
 
+<<<<<<< HEAD
   //     wom::utils::PIDConfig<units::radian, units::volt> pidConfigA{
   //      "/path/to/pid/in/nt/tables",
   //     15_V / 180_deg,
   //     0_V / (1_deg * 1_s),
   //     0_V / (1_deg / 1_s),
   //     };
+=======
+    AlphaArmConfig config{alphaArmGearbox, wristGearbox};
+  };
+  AlphaArmSystem alphaArmSystem;
+
+  struct IntakeSystem {
+    rev::CANSparkMax intakeMotor{2, rev::CANSparkMax::MotorType::kBrushed};
+    // wom::CANSparkMaxEncoder intakeEncoder{&intakeMotor, 0.1_m};
+    // frc::DigitalInput intakeSensor{0};
+    // frc::DigitalInput magSensor{0};
+    // frc::DigitalInput shooterSensor{0};
+>>>>>>> 0029f49 (fix some formatting)
 
   //     AlphaArmConfig config {
   //         alphaArmGearbox,
   //         pidConfigA,
   //     };
 
+<<<<<<< HEAD
   //   };
   //   AlphaArmSystem alphaArmSystem;
 
@@ -63,6 +76,11 @@ struct RobotMap {
   //     frc::DigitalInput intakeSensor{4};
   //     // frc::DigitalInput magSensor{0};
   //     // frc::DigitalInput shooterSensor{0};
+=======
+    IntakeConfig config{IntakeGearbox /*, &intakeSensor, &magSensor, &shooterSensor*/};
+  };
+  IntakeSystem intakeSystem;
+>>>>>>> 0029f49 (fix some formatting)
 
   //     wom::Gearbox IntakeGearbox{&intakeMotor, nullptr, frc::DCMotor::CIM(1)};
 
@@ -96,6 +114,16 @@ struct RobotMap {
   //   };
   //   Shooter shooterSystem;
 
+<<<<<<< HEAD
+=======
+    ShooterConfig config{
+        "shooterGearbox", shooterGearbox,
+        // &shooterSensor,
+    };
+  };
+  Shooter shooterSystem;
+
+>>>>>>> 0029f49 (fix some formatting)
   struct SwerveBase {
     ctre::phoenix6::hardware::CANcoder frontLeftCancoder{16, "Drivebase"};
     ctre::phoenix6::hardware::CANcoder frontRightCancoder{18, "Drivebase"};
@@ -118,35 +146,63 @@ struct RobotMap {
         wom::SwerveModuleConfig{
             // CORRECT
             // front left module
+<<<<<<< HEAD
             frc::Translation2d(-10_in, 9_in),
             wom::Gearbox{driveMotors[0], new wom::TalonFXEncoder(driveMotors[0], 0.0445_m, 6.75),
                          frc::DCMotor::Falcon500(1).WithReduction(6.75)},
             wom::Gearbox{turnMotors[0], new wom::CanEncoder(16, 0.0445_m, 4096, 12.8),
+=======
+            frc::Translation2d(10.761_in, 9.455_in),
+            wom::Gearbox{driveMotors[0], new wom::TalonFXEncoder(driveMotors[0], 0.0445_m, 6.75),
+                         frc::DCMotor::Falcon500(1).WithReduction(6.75)},
+            wom::Gearbox{turnMotors[0], new wom::CanEncoder(18, 0.0445_m, 4096, 12.8),
+>>>>>>> 0029f49 (fix some formatting)
                          frc::DCMotor::Falcon500(1).WithReduction(12.8)},
             &frontLeftCancoder, 4_in / 2},
         wom::SwerveModuleConfig{
             // CORRECT
             // front right module
+<<<<<<< HEAD
             frc::Translation2d(10_in, 9_in),
             wom::Gearbox{driveMotors[1], new wom::TalonFXEncoder(driveMotors[1], 0.0445_m, 6.75),
                          frc::DCMotor::Falcon500(1).WithReduction(6.75)},
             wom::Gearbox{turnMotors[1], new wom::CanEncoder(18, 0.0445_m, 4096, 12.8),
+=======
+            frc::Translation2d(10.761_in, -9.455_in),
+            wom::Gearbox{driveMotors[1], new wom::TalonFXEncoder(driveMotors[1], 0.0445_m, 6.75),
+                         frc::DCMotor::Falcon500(1).WithReduction(6.75)},
+            wom::Gearbox{turnMotors[1], new wom::CanEncoder(19, 0.0445_m, 4096, 12.8),
+>>>>>>> 0029f49 (fix some formatting)
                          frc::DCMotor::Falcon500(1).WithReduction(12.8)},
             &frontRightCancoder, 4_in / 2},
         wom::SwerveModuleConfig{
             // back left module
+<<<<<<< HEAD
             frc::Translation2d(-10_in, 9_in),
             wom::Gearbox{driveMotors[2], new wom::TalonFXEncoder(driveMotors[2], 0.0445_m, 6.75),
                          frc::DCMotor::Falcon500(1).WithReduction(6.75)},
             wom::Gearbox{turnMotors[2], new wom::CanEncoder(17, 0.0445_m, 4096, 12.8),
+=======
+            frc::Translation2d(-10.761_in, 9.455_in),
+            wom::Gearbox{driveMotors[2], new wom::TalonFXEncoder(driveMotors[2], 0.0445_m, 6.75),
+                         frc::DCMotor::Falcon500(1).WithReduction(6.75)},
+            wom::Gearbox{turnMotors[2], new wom::CanEncoder(16, 0.0445_m, 4096, 12.8),
+>>>>>>> 0029f49 (fix some formatting)
                          frc::DCMotor::Falcon500(1).WithReduction(12.8)},
             &backRightCancoder, 4_in / 2},
         wom::SwerveModuleConfig{
             // back right module
+<<<<<<< HEAD
             frc::Translation2d(-10_in, -9_in),
             wom::Gearbox{driveMotors[3], new wom::TalonFXEncoder(driveMotors[3], 0.0445_m, 6.75),
                          frc::DCMotor::Falcon500(1).WithReduction(6.75)},
             wom::Gearbox{turnMotors[3], new wom::CanEncoder(19, 0.0445_m, 4096, 12.8),
+=======
+            frc::Translation2d(-10.761_in, -9.455_in),
+            wom::Gearbox{driveMotors[3], new wom::TalonFXEncoder(driveMotors[3], 0.0445_m, 6.75),
+                         frc::DCMotor::Falcon500(1).WithReduction(6.75)},
+            wom::Gearbox{turnMotors[3], new wom::CanEncoder(17, 0.0445_m, 4096, 12.8),
+>>>>>>> 0029f49 (fix some formatting)
                          frc::DCMotor::Falcon500(1).WithReduction(12.8)},
             &backLeftCancoder, 4_in / 2},
     };
@@ -199,6 +255,7 @@ struct RobotMap {
         nt::NetworkTableInstance::GetDefault().GetTable("swerve");
   };
   SwerveTable swerveTable;
+<<<<<<< HEAD
 
   // struct AlphaArmSystem {
   //   rev::CANSparkMax alphaArmMotor{12, rev::CANSparkMax::MotorType::kBrushless};
@@ -210,4 +267,7 @@ struct RobotMap {
   //   AlphaArmConfig config{alphaArmGearbox, wristGearbox};
   // };
   // AlphaArmSystem alphaArmSystem;
-};
\ No newline at end of file
+};
+=======
+};
+>>>>>>> 0029f49 (fix some formatting)
diff --git a/wombat/src/main/cpp/behaviour/Behaviour.cpp b/wombat/src/main/cpp/behaviour/Behaviour.cpp
index d21d9e57..7312c922 100644
--- a/wombat/src/main/cpp/behaviour/Behaviour.cpp
+++ b/wombat/src/main/cpp/behaviour/Behaviour.cpp
@@ -10,9 +10,7 @@ using namespace behaviour;
 
 // Behaviour
 Behaviour::Behaviour(std::string name, units::time::second_t period)
-    : _bhvr_name(name),
-      _bhvr_period(period),
-      _bhvr_state(BehaviourState::INITIALISED) {}
+    : _bhvr_name(name), _bhvr_period(period), _bhvr_state(BehaviourState::INITIALISED) {}
 Behaviour::~Behaviour() {
   if (!IsFinished())
     Interrupt();
@@ -82,8 +80,7 @@ bool Behaviour::Tick() {
 
     if (dt > 2 * _bhvr_period) {
       std::cerr << "Behaviour missed deadline. Reduce Period. Dt=" << dt.value()
-                << " Dt(deadline)=" << (2 * _bhvr_period).value()
-                << ". Bhvr: " << GetName() << std::endl;
+                << " Dt(deadline)=" << (2 * _bhvr_period).value() << ". Bhvr: " << GetName() << std::endl;
     }
 
     if (_bhvr_timeout.value() > 0 && _bhvr_timer > _bhvr_timeout)
@@ -100,8 +97,7 @@ bool Behaviour::IsRunning() const {
 }
 
 bool Behaviour::IsFinished() const {
-  return _bhvr_state != BehaviourState::INITIALISED &&
-         _bhvr_state != BehaviourState::RUNNING;
+  return _bhvr_state != BehaviourState::INITIALISED && _bhvr_state != BehaviourState::RUNNING;
 }
 
 void Behaviour::Stop(BehaviourState new_state) {
@@ -173,8 +169,7 @@ void ConcurrentBehaviour::Add(Behaviour::ptr behaviour) {
 }
 
 std::string ConcurrentBehaviour::GetName() const {
-  std::string msg =
-      (_reducer == ConcurrentBehaviourReducer::ALL ? "ALL { " : "RACE {");
+  std::string msg = (_reducer == ConcurrentBehaviourReducer::ALL ? "ALL { " : "RACE {");
   for (auto b : _children)
     msg += b->GetName() + ", ";
   msg += "}";
@@ -190,8 +185,8 @@ void ConcurrentBehaviour::OnStart() {
         using namespace std::chrono_literals;
 
         b->Tick();
-        std::this_thread::sleep_for(std::chrono::milliseconds(
-            static_cast<int64_t>(b->GetPeriod().value() * 1000)));
+        std::this_thread::sleep_for(
+            std::chrono::milliseconds(static_cast<int64_t>(b->GetPeriod().value() * 1000)));
       }
 
       if (IsFinished() && !b->IsFinished())
@@ -273,10 +268,8 @@ void WaitFor::OnTick(units::time::second_t dt) {
 }
 
 // WaitTime
-WaitTime::WaitTime(units::time::second_t time)
-    : WaitTime([time]() { return time; }) {}
-WaitTime::WaitTime(std::function<units::time::second_t()> time_fn)
-    : _time_fn(time_fn) {}
+WaitTime::WaitTime(units::time::second_t time) : WaitTime([time]() { return time; }) {}
+WaitTime::WaitTime(std::function<units::time::second_t()> time_fn) : _time_fn(time_fn) {}
 
 void WaitTime::OnStart() {
   _time = _time_fn();
diff --git a/wombat/src/main/cpp/behaviour/BehaviourScheduler.cpp b/wombat/src/main/cpp/behaviour/BehaviourScheduler.cpp
index 26863c79..eb363b3e 100644
--- a/wombat/src/main/cpp/behaviour/BehaviourScheduler.cpp
+++ b/wombat/src/main/cpp/behaviour/BehaviourScheduler.cpp
@@ -52,8 +52,8 @@ void BehaviourScheduler::Schedule(Behaviour::ptr behaviour) {
         std::lock_guard<std::recursive_mutex> lk(_active_mtx);
         behaviour->Tick();
       }
-      std::this_thread::sleep_for(std::chrono::milliseconds(
-          static_cast<int64_t>(behaviour->GetPeriod().value() * 1000)));
+      std::this_thread::sleep_for(
+          std::chrono::milliseconds(static_cast<int64_t>(behaviour->GetPeriod().value() * 1000)));
     }
   });
 }
diff --git a/wombat/src/main/cpp/behaviour/HasBehaviour.cpp b/wombat/src/main/cpp/behaviour/HasBehaviour.cpp
index 964522c6..f2f557c5 100644
--- a/wombat/src/main/cpp/behaviour/HasBehaviour.cpp
+++ b/wombat/src/main/cpp/behaviour/HasBehaviour.cpp
@@ -8,8 +8,7 @@
 
 using namespace behaviour;
 
-void HasBehaviour::SetDefaultBehaviour(
-    std::function<std::shared_ptr<Behaviour>(void)> fn) {
+void HasBehaviour::SetDefaultBehaviour(std::function<std::shared_ptr<Behaviour>(void)> fn) {
   _default_behaviour_producer = fn;
 }
 
diff --git a/wombat/src/main/cpp/drivetrain/Drivetrain.cpp b/wombat/src/main/cpp/drivetrain/Drivetrain.cpp
new file mode 100644
index 00000000..387bbc31
--- /dev/null
+++ b/wombat/src/main/cpp/drivetrain/Drivetrain.cpp
@@ -0,0 +1,51 @@
+// Copyright (c) 2023-2024 CurtinFRC
+// Open Source Software, you can modify it according to the terms
+// of the MIT License at the root of this project
+
+#include "drivetrain/Drivetrain.h"
+
+#include <iostream>
+
+#include "utils/Util.h"
+
+using namespace frc;
+using namespace units;
+
+wom::drivetrain::Drivetrain::Drivetrain(DrivetrainConfig* config, XboxController& driver)
+    : _config(config), _driver(driver) {}
+wom::drivetrain::Drivetrain::~Drivetrain() {}
+
+wom::drivetrain::DrivetrainConfig* wom::drivetrain::Drivetrain::GetConfig() {
+  return _config;
+}
+wom::drivetrain::DrivetrainState wom::drivetrain::Drivetrain::GetState() {
+  return _state;
+}
+
+void wom::drivetrain::Drivetrain::SetState(DrivetrainState state) {
+  _state = state;
+}
+
+void wom::drivetrain::Drivetrain::OnStart() {
+  std::cout << "Starting Tank" << std::endl;
+}
+
+void wom::drivetrain::Drivetrain::OnUpdate(second_t dt) {
+  switch (_state) {
+    case DrivetrainState::kIdle:
+      break;
+    case DrivetrainState::kTank: {
+      double rightSpeed = wom::utils::deadzone(_driver.GetRightY());
+      double leftSpeed = wom::utils::deadzone(_driver.GetLeftY());
+      // _config->left1.motorController->SetVoltage(leftSpeed * maxVolts);
+      // _config->left2.motorController->SetVoltage(leftSpeed * maxVolts);
+      // _config->left3.motorController->SetVoltage(leftSpeed * maxVolts);
+      // _config->right1.motorController->SetVoltage(rightSpeed * maxVolts);
+      // _config->right2.motorController->SetVoltage(rightSpeed * maxVolts);
+      // _config->right3.motorController->SetVoltage(rightSpeed * maxVolts);
+      break;
+    }
+    case DrivetrainState::kAuto:
+      break;
+  }
+}
diff --git a/wombat/src/main/cpp/drivetrain/SwerveDrive.cpp b/wombat/src/main/cpp/drivetrain/SwerveDrive.cpp
index b6772015..582f04f5 100644
--- a/wombat/src/main/cpp/drivetrain/SwerveDrive.cpp
+++ b/wombat/src/main/cpp/drivetrain/SwerveDrive.cpp
@@ -8,8 +8,11 @@
 #include <units/angle.h>
 #include <units/math.h>
 #include <units/voltage.h>
+<<<<<<< HEAD
 #include <wpi/sendable/SendableBuilder.h>
 #include <wpi/sendable/SendableRegistry.h>
+=======
+>>>>>>> 0029f49 (fix some formatting)
 
 #include <algorithm>
 #include <cmath>
@@ -34,7 +37,11 @@ SwerveModule::SwerveModule(std::string path, SwerveModuleConfig config,
                            // SwerveModule::angle_pid_conf_t anglePID,
                            SwerveModule::velocity_pid_conf_t velocityPID)
     :  // _anglePIDController(path + "/pid/angle", anglePID),
+<<<<<<< HEAD
       _anglePIDController{frc::PIDController(5, 0, 0, 0.005_s)},
+=======
+      _anglePIDController{frc::PIDController(4, 0, 0, 0.005_s)},
+>>>>>>> 0029f49 (fix some formatting)
       _config(config),
       _velocityPIDController(frc::PIDController(1.2, 0, 0, 0.005_s)),
       _table(nt::NetworkTableInstance::GetDefault().GetTable(path)) {
@@ -70,9 +77,16 @@ void SwerveModule::OnUpdate(units::second_t dt) {
       auto feedforward = _config.driveMotor.motor.Voltage(
           0_Nm,
           units::radians_per_second_t{_velocityPIDController.GetSetpoint() / _config.wheelRadius.value()});
+<<<<<<< HEAD
       // units::radian_t input = units::math::fmod(_config.turnMotor.encoder->GetEncoderPosition(), (2_rad
       // * 3.1415));
       double input = _config.turnMotor.encoder->GetEncoderPosition().value();
+=======
+      // std::cout << "GetSpeed()" << GetSpeed().value() << std::endl;
+      //  units::radian_t input = units::math::fmod(_config.turnMotor.encoder->GetEncoderPosition(), (2_rad
+      //  * 3.1415));
+      double input = -_config.turnMotor.encoder->GetEncoderPosition().value();
+>>>>>>> 0029f49 (fix some formatting)
       _table->GetEntry("/testing/GetEncoderPos").SetDouble(input);
       // _velocityPIDController.SetSetpoint(3);
 
@@ -123,7 +137,12 @@ void SwerveModule::OnUpdate(units::second_t dt) {
   _config.turnMotor.motorController->SetVoltage(turnVoltage);
 
   _table->GetEntry("speed").SetDouble(GetSpeed().value());
+<<<<<<< HEAD
   _table->GetEntry("angle").SetDouble(_config.turnMotor.encoder->GetEncoderPosition().value());
+=======
+  _table->GetEntry("angle").SetDouble(
+      _config.turnMotor.encoder->GetEncoderPosition().convert<units::degree>().value());
+>>>>>>> 0029f49 (fix some formatting)
   _config.WriteNT(_table->GetSubTable("config"));
 }
 
@@ -169,6 +188,7 @@ void SwerveModule::SetZero(units::second_t dt) {
 void SwerveModule::SetPID(units::radian_t angle, units::meters_per_second_t speed, units::second_t dt) {
   _state = SwerveModuleState::kPID;
 
+<<<<<<< HEAD
   // double diff = std::abs(_config.turnMotor.encoder->GetEncoderPosition().value() - angle.value());
   // _table->GetEntry("diff").SetDouble(diff);
   // if (std::abs(diff) > (3.14159/2)) {
@@ -180,6 +200,16 @@ void SwerveModule::SetPID(units::radian_t angle, units::meters_per_second_t spee
   //   _anglePIDController.SetSetpoint(angle.value());
   //   _velocityPIDController.SetSetpoint(speed.value());
   // }
+=======
+  double diff = std::fmod((_anglePIDController.GetSetpoint() - angle.value()), (2 * 3.1415));
+  std::cout << "DIFF value: " << diff << std::endl;
+  _table->GetEntry("/Differential value:").SetDouble(diff);
+  if (std::abs(diff) >= 3.1415) {
+    speed *= -1;
+    angle -= 3.1415_rad;
+  }
+  // @liam end added
+>>>>>>> 0029f49 (fix some formatting)
 
   // if (diff > (3.14159 / 2)) {
   //   speed *= -1;
@@ -247,6 +277,10 @@ void SwerveModule::ModuleVectorHandler(frc::ChassisSpeeds speeds) {
 units::meters_per_second_t SwerveModule::GetSpeed() const {
   units::meters_per_second_t returnVal{_config.driveMotor.encoder->GetVelocityValue() *
                                        _config.wheelRadius.value()};
+<<<<<<< HEAD
+=======
+  // std::cout << returnVal.value() << std::endl;
+>>>>>>> 0029f49 (fix some formatting)
   return returnVal;
 }
 
@@ -269,11 +303,16 @@ void SwerveDriveConfig::WriteNT(std::shared_ptr<nt::NetworkTable> table) {
 
 SwerveDrive::SwerveDrive(SwerveDriveConfig config, frc::Pose2d initialPose)
     : _config(config),
+<<<<<<< HEAD
       // _kinematics(_config.modules[1].position, _config.modules[0].position,
       //             _config.modules[2].position, _config.modules[3].position),
 
       _kinematics(_config.modules[3].position /*1*/, _config.modules[0].position /*0*/,
                   _config.modules[1].position /*2*/, _config.modules[2].position /*3*/),
+=======
+      _kinematics(_config.modules[0].position, _config.modules[1].position, _config.modules[2].position,
+                  _config.modules[3].position),
+>>>>>>> 0029f49 (fix some formatting)
       _poseEstimator(
           _kinematics, frc::Rotation2d(0_deg),
           wpi::array<frc::SwerveModulePosition, 4>{frc::SwerveModulePosition{0_m, frc::Rotation2d{0_deg}},
@@ -302,9 +341,13 @@ frc::ChassisSpeeds FieldRelativeSpeeds::ToChassisSpeeds(const units::radian_t ro
 }
 
 void SwerveDrive::OnUpdate(units::second_t dt) {
+<<<<<<< HEAD
   _table->GetEntry("/gryo/z").SetDouble(_config.gyro->GetRotation3d().Z().value());
   _table->GetEntry("/gryo/y").SetDouble(_config.gyro->GetRotation3d().Y().value());
   _table->GetEntry("/gryo/x").SetDouble(_config.gyro->GetRotation3d().X().value());
+=======
+  // std::cout << _config.gyro->GetYaw().GetValue().value() << std::endl;
+>>>>>>> 0029f49 (fix some formatting)
   switch (_state) {
     case SwerveDriveState::kZeroing:
       for (auto mod = _modules.begin(); mod < _modules.end(); mod++) {
@@ -354,6 +397,7 @@ void SwerveDrive::OnUpdate(units::second_t dt) {
       frc::ChassisSpeeds new_target_speed{_target_speed.vx, _target_speed.vy, -_target_speed.omega};
       auto new_target_states = _kinematics.ToSwerveModuleStates(new_target_speed);
       for (size_t i = 0; i < _modules.size(); i++) {
+<<<<<<< HEAD
         if (i == 3) {
           _modules[i].SetPID(new_target_states[i].angle.Radians(), new_target_states[i].speed, dt);
         } else {
@@ -365,6 +409,11 @@ void SwerveDrive::OnUpdate(units::second_t dt) {
         //   _modules[i].SetPID(new_target_states[i].angle.Radians(),
         //                     new_target_states[i].speed, dt);
         // }
+=======
+        _modules[i].SetPID(target_states[i].angle.Radians(), target_states[i].speed, dt);
+        // std::cout << "module " << i << ": " <<
+        // target_states[i].angle.Radians().value() << std::endl;
+>>>>>>> 0029f49 (fix some formatting)
       }
     } break;
     case SwerveDriveState::kIndividualTuning:
diff --git a/wombat/src/main/cpp/drivetrain/behaviours/SwerveBehaviours.cpp b/wombat/src/main/cpp/drivetrain/behaviours/SwerveBehaviours.cpp
index 47f25d93..be06811e 100644
--- a/wombat/src/main/cpp/drivetrain/behaviours/SwerveBehaviours.cpp
+++ b/wombat/src/main/cpp/drivetrain/behaviours/SwerveBehaviours.cpp
@@ -101,12 +101,16 @@ void ManualDrivebase::OnTick(units::second_t deltaTime) {
   //                                            r_x * maxRotationMagnitude});
   // } else {
   _swerveDrivebase->SetFieldRelativeVelocity(wom::drivetrain::FieldRelativeSpeeds{
+<<<<<<< HEAD
       xVelocity * -maxMovementMagnitude, yVelocity * -maxMovementMagnitude, r_x * maxRotationMagnitude});
 
   //  _swerveDrivebase->SetVelocity(
   //       frc::ChassisSpeeds{xVelocity * maxMovementMagnitude,
   //                          yVelocity * maxMovementMagnitude,
   //                          r_x * maxRotationMagnitude});
+=======
+      xVelocity * maxMovementMagnitude, yVelocity * maxMovementMagnitude, r_x * maxRotationMagnitude});
+>>>>>>> 0029f49 (fix some formatting)
   //   }
   // }
   // _swerveDrivebase->SetIndividualTuning(2, 0_deg, 0_mps);
diff --git a/wombat/src/main/cpp/subsystems/Arm.cpp b/wombat/src/main/cpp/subsystems/Arm.cpp
index 9390601a..148cec43 100644
--- a/wombat/src/main/cpp/subsystems/Arm.cpp
+++ b/wombat/src/main/cpp/subsystems/Arm.cpp
@@ -10,19 +10,14 @@ using namespace frc;
 using namespace wom;
 
 // creates network table instatnce on shuffleboard
-void wom::subsystems::ArmConfig::WriteNT(
-    std::shared_ptr<nt::NetworkTable> table) {
+void wom::subsystems::ArmConfig::WriteNT(std::shared_ptr<nt::NetworkTable> table) {
   table->GetEntry("armMass").SetDouble(armMass.value());
   table->GetEntry("loadMass").SetDouble(loadMass.value());
   table->GetEntry("armLength").SetDouble(armLength.value());
-  table->GetEntry("minAngle")
-      .SetDouble(minAngle.convert<units::degree>().value());
-  table->GetEntry("maxAngle")
-      .SetDouble(maxAngle.convert<units::degree>().value());
-  table->GetEntry("initialAngle")
-      .SetDouble(initialAngle.convert<units::degree>().value());
-  table->GetEntry("angleOffset")
-      .SetDouble(initialAngle.convert<units::degree>().value());
+  table->GetEntry("minAngle").SetDouble(minAngle.convert<units::degree>().value());
+  table->GetEntry("maxAngle").SetDouble(maxAngle.convert<units::degree>().value());
+  table->GetEntry("initialAngle").SetDouble(initialAngle.convert<units::degree>().value());
+  table->GetEntry("angleOffset").SetDouble(initialAngle.convert<units::degree>().value());
 }
 
 // arm config is used
@@ -43,14 +38,12 @@ void wom::subsystems::Arm::OnUpdate(units::second_t dt) {
     case ArmState::kIdle:
       break;
     case ArmState::kVelocity: {
-      units::newton_meter_t torque =
-          9.81_m / 1_s / 1_s * _config.armLength *
-          units::math::cos(angle + _config.angleOffset) *
-          (0.5 * _config.armMass + _config.loadMass);
+      units::newton_meter_t torque = 9.81_m / 1_s / 1_s * _config.armLength *
+                                     units::math::cos(angle + _config.angleOffset) *
+                                     (0.5 * _config.armMass + _config.loadMass);
       // units::volt_t feedforward = _config.leftGearbox.motor.Voltage(torque,
       // 0_rad/1_s);
-      units::volt_t feedforward =
-          _config.leftGearbox.motor.Voltage(torque, _velocityPID.GetSetpoint());
+      units::volt_t feedforward = _config.leftGearbox.motor.Voltage(torque, _velocityPID.GetSetpoint());
       // feedforward = 3.5_V;
       // std::cout << "feedforward" << feedforward.value() << std::endl;
       voltage = _velocityPID.Calculate(GetArmVelocity(), dt, feedforward);
@@ -58,12 +51,10 @@ void wom::subsystems::Arm::OnUpdate(units::second_t dt) {
       // std::endl; voltage = 0_V;
     } break;
     case ArmState::kAngle: {
-      units::newton_meter_t torque =
-          9.81_m / 1_s / 1_s * _config.armLength *
-          units::math::cos(angle + _config.angleOffset) *
-          (0.5 * _config.armMass + _config.loadMass);
-      units::volt_t feedforward =
-          _config.leftGearbox.motor.Voltage(torque, 0_rad / 1_s);
+      units::newton_meter_t torque = 9.81_m / 1_s / 1_s * _config.armLength *
+                                     units::math::cos(angle + _config.angleOffset) *
+                                     (0.5 * _config.armMass + _config.loadMass);
+      units::volt_t feedforward = _config.leftGearbox.motor.Voltage(torque, 0_rad / 1_s);
       // std::cout << "feedforward" << feedforward.value() << std::endl;
       voltage = _pid.Calculate(angle, dt, feedforward);
     } break;
diff --git a/wombat/src/main/cpp/subsystems/Elevator.cpp b/wombat/src/main/cpp/subsystems/Elevator.cpp
new file mode 100644
index 00000000..d1cf9a8f
--- /dev/null
+++ b/wombat/src/main/cpp/subsystems/Elevator.cpp
@@ -0,0 +1,141 @@
+// Copyright (c) 2023-2024 CurtinFRC
+// Open Source Software, you can modify it according to the terms
+// of the MIT License at the root of this project
+
+#include "subsystems/Elevator.h"
+
+#include <networktables/NetworkTableInstance.h>
+#include <units/acceleration.h>
+
+void wom::subsystems::ElevatorConfig::WriteNT(std::shared_ptr<nt::NetworkTable> table) {
+  table->GetEntry("radius").SetDouble(radius.value());
+  table->GetEntry("mass").SetDouble(mass.value());
+  table->GetEntry("maxHeight").SetDouble(maxHeight.value());
+}
+
+wom::subsystems::Elevator::Elevator(ElevatorConfig config)
+    : _config(config),
+      _state(ElevatorState::kIdle),
+      _pid{config.path + "/pid", config.pid},
+      _velocityPID{config.path + "/velocityPID", config.velocityPID},
+      _table(nt::NetworkTableInstance::GetDefault().GetTable(config.path)) {
+  _config.leftGearbox.encoder->SetEncoderPosition(_config.initialHeight / _config.radius * 1_rad);
+}
+
+void wom::subsystems::Elevator::OnUpdate(units::second_t dt) {
+  units::volt_t voltage{0};
+
+  units::meter_t height = GetElevatorEncoderPos() * 1_m;
+
+  switch (_state) {
+    case ElevatorState::kIdle:
+      voltage = 0_V;
+      break;
+    case ElevatorState::kManual:
+      voltage = _setpointManual;
+      break;
+    case ElevatorState::kVelocity: {
+      units::volt_t feedforward = _config.rightGearbox.motor.Voltage(
+          (_config.mass * 9.81_mps_sq) * _config.radius,
+          _velocityPID.GetSetpoint() / (14.0 / 60.0 * 2.0 * 3.1415 * 0.02225 * 1_m) * 1_rad);
+      // units::volt_t feedforward = _config.rightGearbox.motor.Voltage(0_Nm,
+      // _velocityPID.GetSetpoint() / (14.0/60.0 * 2.0 * 3.1415 * 0.02225 * 1_m)
+      // * 1_rad);
+      feedforward = 1.2_V;
+      voltage = _velocityPID.Calculate(GetElevatorVelocity(), dt, feedforward);
+      if (voltage > 6_V) {
+        voltage = 6_V;
+      }
+      std::cout << "elevator feedforward: " << feedforward.value() << std::endl;
+      // voltage = 0_V;
+    } break;
+    case ElevatorState::kPID: {
+      units::volt_t feedforward =
+          _config.rightGearbox.motor.Voltage((_config.mass * 9.81_mps_sq) * _config.radius, 0_rad_per_s);
+      // std::cout << "feed forward" << feedforward.value() << std::endl;
+      feedforward = 1.2_V;
+      // voltage = _pid.Calculate(height, dt, feedforward);
+      voltage = _pid.Calculate(height, dt, feedforward);
+      if (voltage > 6_V) {
+        voltage = 6_V;
+      }
+    } break;
+  }
+
+  // Top Sensor Detector
+  // if(_config.topSensor != nullptr) {
+  //   if(_config.topSensor->Get()) {
+  //     _config.leftGearbox.encoder->SetEncoderPosition(_config.maxHeight /
+  //     _config.radius * 1_rad);
+  //     //voltage = 0_V;
+  //   }
+  // }
+
+  // //Bottom Sensor Detection
+  // if (_config.bottomSensor != nullptr) {
+  //   if (_config.bottomSensor->Get()) {
+  //     _config.leftGearbox.encoder->SetEncoderPosition(_config.minHeight /
+  //     _config.radius * 1_rad);
+  //     //voltage = 0_V;
+  //   }
+  // }
+
+  // std::cout << "elevator: " << elevator.height
+
+  // Set voltage to motors...
+  voltage *= speedLimit;
+  // _config.leftGearbox.motorController->SetVoltage(voltage);
+  // _config.rightGearbox.motorController->SetVoltage(voltage);
+}
+
+void wom::subsystems::Elevator::SetManual(units::volt_t voltage) {
+  _setpointManual = voltage;
+}
+
+void wom::subsystems::Elevator::SetPID(units::meter_t height) {
+  _pid.SetSetpoint(height);
+}
+
+void wom::subsystems::Elevator::SetVelocity(units::meters_per_second_t velocity) {
+  _velocityPID.SetSetpoint(velocity);
+}
+
+void wom::subsystems::Elevator::SetState(wom::subsystems::ElevatorState state) {
+  _state = state;
+}
+
+void wom::subsystems::Elevator::SetElevatorSpeedLimit(double limit) {
+  speedLimit = limit;
+}
+
+wom::subsystems::ElevatorConfig& wom::subsystems::Elevator::GetConfig() {
+  return _config;
+}
+
+bool wom::subsystems::Elevator::IsStable() const {
+  return _pid.IsStable();
+}
+
+wom::subsystems::ElevatorState wom::subsystems::Elevator::GetState() const {
+  return _state;
+}
+
+double wom::subsystems::Elevator::GetElevatorEncoderPos() {
+  return _config.elevatorEncoder.GetPosition() * 14 / 60 * 2 * 3.1415 * 0.02225;
+}
+
+units::meter_t wom::subsystems::Elevator::GetHeight() const {
+  // std::cout << "elevator position"<<
+  // _config.rightGearbox.encoder->GetEncoderTicks() << std::endl; return
+  // _config.rightGearbox.encoder->GetEncoderDistance() * 1_m;
+  return _config.elevatorEncoder.GetPosition() * 14 / 60 * 2 * 3.1415 * 0.02225 * 1_m;
+}
+
+units::meters_per_second_t wom::subsystems::Elevator::GetElevatorVelocity() const {
+  return _config.elevatorEncoder.GetVelocity() / 60_s * 14 / 60 * 2 * 3.1415 * 0.02225 * 1_m;
+}
+
+units::meters_per_second_t wom::subsystems::Elevator::MaxSpeed() const {
+  return _config.leftGearbox.motor.Speed((_config.mass * 9.81_mps_sq) * _config.radius, 12_V) / 1_rad *
+         _config.radius;
+}
diff --git a/wombat/src/main/cpp/subsystems/Shooter.cpp b/wombat/src/main/cpp/subsystems/Shooter.cpp
new file mode 100644
index 00000000..c851266b
--- /dev/null
+++ b/wombat/src/main/cpp/subsystems/Shooter.cpp
@@ -0,0 +1,86 @@
+// Copyright (c) 2023-2024 CurtinFRC
+// Open Source Software, you can modify it according to the terms
+// of the MIT License at the root of this project
+
+#include "subsystems/Shooter.h"
+
+#include <networktables/NetworkTableInstance.h>
+
+wom::subsystems::Shooter::Shooter(std::string path, ShooterParams params)
+    : _params(params),
+      _state(ShooterState::kIdle),
+      _pid{path + "/pid", params.pid},
+      _table(nt::NetworkTableInstance::GetDefault().GetTable("shooter")) {}
+
+void wom::subsystems::Shooter::OnUpdate(units::second_t dt) {
+  units::volt_t voltage{0};
+  units::revolutions_per_minute_t currentSpeed = _params.gearbox.encoder->GetEncoderAngularVelocity();
+
+  switch (_state) {
+    case ShooterState::kManual:
+      voltage = _setpointManual;
+      break;
+    case ShooterState::kPID: {
+      auto feedforward = _params.gearbox.motor.Voltage(0_Nm, _pid.GetSetpoint());
+      voltage = _pid.Calculate(currentSpeed, dt, feedforward);
+    } break;
+    case ShooterState::kIdle:
+      voltage = 0_V;
+      break;
+  }
+
+  units::newton_meter_t max_torque_at_current_limit = _params.gearbox.motor.Torque(_params.currentLimit);
+  units::volt_t max_voltage_for_current_limit =
+      _params.gearbox.motor.Voltage(max_torque_at_current_limit, currentSpeed);
+
+  voltage = 1_V * std::min(voltage.value(), max_voltage_for_current_limit.value());
+
+  // _params.gearbox.motorController->SetVoltage(voltage);
+
+  _table->GetEntry("output_volts").SetDouble(voltage.value());
+  _table->GetEntry("speed_rpm").SetDouble(currentSpeed.value());
+  _table->GetEntry("setpoint_rpm").SetDouble(units::revolutions_per_minute_t{_pid.GetSetpoint()}.value());
+  _table->GetEntry("stable").SetBoolean(_pid.IsStable());
+}
+
+void wom::subsystems::Shooter::SetManual(units::volt_t voltage) {
+  _setpointManual = voltage;
+}
+
+void wom::subsystems::Shooter::SetPID(units::radians_per_second_t goal) {
+  _state = ShooterState::kPID;
+  _pid.SetSetpoint(goal);
+}
+
+void wom::subsystems::Shooter::SetState(ShooterState state) {
+  _state = state;
+}
+
+bool wom::subsystems::Shooter::IsStable() const {
+  return _pid.IsStable();
+}
+
+// Shooter Manual Set
+
+wom::subsystems::ShooterConstant::ShooterConstant(Shooter* s, units::volt_t setpoint)
+    : _shooter(s), _setpoint(setpoint) {
+  Controls(_shooter);
+}
+
+void wom::subsystems::ShooterConstant::OnTick(units::second_t dt) {
+  _shooter->SetManual(_setpoint);
+}
+
+// ShooterSpinup
+
+wom::subsystems::ShooterSpinup::ShooterSpinup(Shooter* s, units::radians_per_second_t speed, bool hold)
+    : _shooter(s), _speed(speed), _hold(hold) {
+  Controls(_shooter);
+}
+
+void wom::subsystems::ShooterSpinup::OnTick(units::second_t dt) {
+  _shooter->SetPID(_speed);
+
+  if (!_hold && _shooter->IsStable())
+    SetDone();
+}
diff --git a/wombat/src/main/cpp/utils/Encoder.cpp b/wombat/src/main/cpp/utils/Encoder.cpp
index 5ba47a14..a4ca9ca5 100644
--- a/wombat/src/main/cpp/utils/Encoder.cpp
+++ b/wombat/src/main/cpp/utils/Encoder.cpp
@@ -6,8 +6,8 @@
 
 #include <rev/SparkRelativeEncoder.h>
 
-wom::utils::Encoder::Encoder(double encoderTicksPerRotation, int type,
-                             units::meter_t wheelRadius, double reduction)
+wom::utils::Encoder::Encoder(double encoderTicksPerRotation, int type, units::meter_t wheelRadius,
+                             double reduction)
     : _reduction(reduction),
       _encoderTicksPerRotation(encoderTicksPerRotation),
       _type(type),
@@ -53,16 +53,6 @@ units::radian_t wom::utils::Encoder::GetEncoderPosition() {
   //   units::degree_t pos = GetEncoderTicks() * 1_deg;
   //   return pos - _offset;
   // }
-  //if (_type == 0) {
-  //  units::turn_t n_turns{GetEncoderTicks() / GetEncoderTicksPerRotation()};
-  //  return n_turns;
-  //} else if (_type == 2) {
-  //  units::degree_t pos = GetEncoderTicks() * 1_deg;
-  //  return pos;
-  //} else {
-  //  units::degree_t pos = GetEncoderTicks() * 1_deg;
-  //  return pos - _offset;
-  //}
   return GetEncoderTicks() * 1_rad;
 }
 
@@ -73,8 +63,7 @@ double wom::utils::Encoder::GetEncoderDistance() {
 units::radians_per_second_t wom::utils::Encoder::GetEncoderAngularVelocity() {
   // return GetEncoderTickVelocity() / (double)GetEncoderTicksPerRotation() * 2
   // * 3.1415926;
-  units::turns_per_second_t n_turns_per_s{GetEncoderTickVelocity() /
-                                          GetEncoderTicksPerRotation()};
+  units::turns_per_second_t n_turns_per_s{GetEncoderTickVelocity() / GetEncoderTicksPerRotation()};
   return n_turns_per_s;
 }
 
@@ -96,11 +85,10 @@ double wom::utils::DigitalEncoder::GetEncoderTickVelocity() const {
   return _nativeEncoder.GetRate();
 }
 
-wom::utils::CANSparkMaxEncoder::CANSparkMaxEncoder(rev::CANSparkMax* controller,
-                                                   units::meter_t wheelRadius,
+wom::utils::CANSparkMaxEncoder::CANSparkMaxEncoder(rev::CANSparkMax* controller, units::meter_t wheelRadius,
                                                    double reduction)
     : wom::utils::Encoder(42, reduction, wheelRadius, 2),
-      _encoder(controller->GetEncoder(rev::SparkRelativeEncoder::Type::kHallSensor)) {}
+      _encoder(controller->GetEncoder(rev::SparkRelativeEncoder::Type::kQuadrature)) {}
 
 double wom::utils::CANSparkMaxEncoder::GetEncoderRawTicks() const {
   return ((_encoder.GetPosition() * 2 * 3.1415) / 200);
@@ -130,11 +118,9 @@ double wom::utils::CANSparkMaxEncoder::GetVelocity() const {
   return _encoder.GetVelocity();
 }
 
-wom::utils::TalonFXEncoder::TalonFXEncoder(
-    ctre::phoenix6::hardware::TalonFX* controller, units::meter_t wheelRadius,
-    double reduction)
-    : utils::Encoder(2048, reduction, wheelRadius, 0),
-      _controller(controller) {}
+wom::utils::TalonFXEncoder::TalonFXEncoder(ctre::phoenix6::hardware::TalonFX* controller,
+                                           units::meter_t wheelRadius, double reduction)
+    : utils::Encoder(2048, reduction, wheelRadius, 0), _controller(controller) {}
 
 double wom::utils::TalonFXEncoder::GetEncoderRawTicks() const {
   return _controller->GetPosition().GetValue().value();
@@ -144,12 +130,9 @@ double wom::utils::TalonFXEncoder::GetEncoderTickVelocity() const {
   return _controller->Get() * 10;
 }
 
-wom::utils::DutyCycleEncoder::DutyCycleEncoder(int channel,
-                                               units::meter_t wheelRadius,
-                                               double ticksPerRotation,
-                                               double reduction)
-    : wom::utils::Encoder(ticksPerRotation, reduction, wheelRadius, 0),
-      _dutyCycleEncoder(channel) {}
+wom::utils::DutyCycleEncoder::DutyCycleEncoder(int channel, units::meter_t wheelRadius,
+                                               double ticksPerRotation, double reduction)
+    : wom::utils::Encoder(ticksPerRotation, reduction, wheelRadius, 0), _dutyCycleEncoder(channel) {}
 
 double wom::utils::DutyCycleEncoder::GetEncoderRawTicks() const {
   return _dutyCycleEncoder.Get().value();
@@ -159,9 +142,8 @@ double wom::utils::DutyCycleEncoder::GetEncoderTickVelocity() const {
   return 0;
 }
 
-wom::utils::CanEncoder::CanEncoder(int deviceNumber, units::meter_t wheelRadius,
-                                   double ticksPerRotation, double reduction,
-                                   std::string name)
+wom::utils::CanEncoder::CanEncoder(int deviceNumber, units::meter_t wheelRadius, double ticksPerRotation,
+                                   double reduction, std::string name)
     : wom::utils::Encoder(ticksPerRotation, 2, wheelRadius, reduction) {
   _canEncoder = new ctre::phoenix6::hardware::CANcoder(deviceNumber, name);
   // _canEncoder->ConfigAbsoluteEncoderRange(0, 1);
diff --git a/wombat/src/main/cpp/utils/Util.cpp b/wombat/src/main/cpp/utils/Util.cpp
index e529e245..dbdbb642 100644
--- a/wombat/src/main/cpp/utils/Util.cpp
+++ b/wombat/src/main/cpp/utils/Util.cpp
@@ -13,21 +13,18 @@ units::second_t wom::utils::now() {
   return static_cast<double>(now) / 1000000 * 1_s;
 }
 
-void wom::utils::WritePose2NT(std::shared_ptr<nt::NetworkTable> table,
-                              frc::Pose2d pose) {
+void wom::utils::WritePose2NT(std::shared_ptr<nt::NetworkTable> table, frc::Pose2d pose) {
   table->GetEntry("x").SetDouble(pose.X().value());
   table->GetEntry("y").SetDouble(pose.Y().value());
   table->GetEntry("angle").SetDouble(pose.Rotation().Degrees().value());
 }
 
-void wom::utils::WritePose3NT(std::shared_ptr<nt::NetworkTable> table,
-                              frc::Pose3d pose) {
+void wom::utils::WritePose3NT(std::shared_ptr<nt::NetworkTable> table, frc::Pose3d pose) {
   table->GetEntry("x").SetDouble(pose.X().value());
   table->GetEntry("y").SetDouble(pose.Y().value());
   table->GetEntry("z").SetDouble(pose.Z().value());
 
-  table->GetEntry("angle").SetDouble(
-      pose.Rotation().Z().convert<units::degree>().value());
+  table->GetEntry("angle").SetDouble(pose.Rotation().Z().convert<units::degree>().value());
 }
 
 double wom::utils::deadzone(double val, double deadzone) {
@@ -50,32 +47,24 @@ units::volt_t wom::utils::GetVoltage(frc::MotorController* controller) {
   return controller->Get() * frc::RobotController::GetBatteryVoltage();
 }
 
-void wom::utils::WriteTrajectory(std::shared_ptr<nt::NetworkTable> table,
-                                 frc::Trajectory trajectory) {
+void wom::utils::WriteTrajectory(std::shared_ptr<nt::NetworkTable> table, frc::Trajectory trajectory) {
   table->GetEntry("length").SetDouble(trajectory.TotalTime().value());
 
   // write the trajectory to the network table
   int i = 0;
   for (auto state : trajectory.States()) {
-    table->GetSubTable(std::to_string(i))
-        ->GetEntry("x")
-        .SetDouble(state.pose.X().value());
-    table->GetSubTable(std::to_string(i))
-        ->GetEntry("y")
-        .SetDouble(state.pose.Y().value());
+    table->GetSubTable(std::to_string(i))->GetEntry("x").SetDouble(state.pose.X().value());
+    table->GetSubTable(std::to_string(i))->GetEntry("y").SetDouble(state.pose.Y().value());
     table->GetSubTable(std::to_string(i))
         ->GetEntry("angle")
         .SetDouble(state.pose.Rotation().Degrees().value());
-    table->GetSubTable(std::to_string(i))
-        ->GetEntry("time")
-        .SetDouble(state.t.value());
+    table->GetSubTable(std::to_string(i))->GetEntry("time").SetDouble(state.t.value());
 
     i++;
   }
 }
 
-void wom::utils::WriteTrajectoryState(std::shared_ptr<nt::NetworkTable> table,
-                                      frc::Trajectory::State state) {
+void wom::utils::WriteTrajectoryState(std::shared_ptr<nt::NetworkTable> table, frc::Trajectory::State state) {
   table->GetEntry("x").SetDouble(state.pose.X().value());
   table->GetEntry("y").SetDouble(state.pose.Y().value());
   table->GetEntry("angle").SetDouble(state.pose.Rotation().Degrees().value());
diff --git a/wombat/src/main/cpp/vision/Limelight.cpp b/wombat/src/main/cpp/vision/Limelight.cpp
index b37148ce..906f92cb 100644
--- a/wombat/src/main/cpp/vision/Limelight.cpp
+++ b/wombat/src/main/cpp/vision/Limelight.cpp
@@ -8,8 +8,7 @@
 
 #include "utils/Util.h"
 
-wom::vision::Limelight::Limelight(std::string limelightName)
-    : _limelightName(limelightName) {}
+wom::vision::Limelight::Limelight(std::string limelightName) : _limelightName(limelightName) {}
 
 std::string wom::vision::Limelight::GetName() {
   return _limelightName;
@@ -24,8 +23,7 @@ std::pair<double, double> wom::vision::Limelight::GetOffset() {
   return offset;
 }
 
-std::vector<double> wom::vision::Limelight::GetAprilTagData(
-    LimelightAprilTagData dataType) {
+std::vector<double> wom::vision::Limelight::GetAprilTagData(LimelightAprilTagData dataType) {
   std::string dataName;
 
   switch (dataType) {
@@ -69,8 +67,7 @@ std::vector<double> wom::vision::Limelight::GetAprilTagData(
   return table->GetNumberArray(dataName, std::vector<double>(6));
 }
 
-double wom::vision::Limelight::GetTargetingData(LimelightTargetingData dataType,
-                                                double defaultValue) {
+double wom::vision::Limelight::GetTargetingData(LimelightTargetingData dataType, double defaultValue) {
   std::string dataName;
 
   switch (dataType) {
@@ -158,8 +155,8 @@ void wom::vision::Limelight::SetCrop(std::array<double, 4> crop) {
   table->PutNumberArray("camtran", crop);
 }
 
-units::meters_per_second_t wom::vision::Limelight::GetSpeed(
-    frc::Pose3d pose1, frc::Pose3d pose2, units::second_t dt) {
+units::meters_per_second_t wom::vision::Limelight::GetSpeed(frc::Pose3d pose1, frc::Pose3d pose2,
+                                                            units::second_t dt) {
   frc::Transform3d dPose{pose1, pose2};
   frc::Translation3d dTranslation = dPose.Translation();
 
@@ -172,9 +169,8 @@ units::meters_per_second_t wom::vision::Limelight::GetSpeed(
 
 frc::Pose3d wom::vision::Limelight::GetPose() {
   std::vector<double> pose = GetAprilTagData(LimelightAprilTagData::kBotpose);
-  return frc::Pose3d(
-      pose[1] * 1_m, 1_m * pose[2], 1_m * pose[3],
-      frc::Rotation3d(1_deg * (pose[4]), 1_deg * (pose[5]), 1_deg * (pose[6])));
+  return frc::Pose3d(pose[1] * 1_m, 1_m * pose[2], 1_m * pose[3],
+                     frc::Rotation3d(1_deg * (pose[4]), 1_deg * (pose[5]), 1_deg * (pose[6])));
 }
 
 void wom::vision::Limelight::OnStart() {
@@ -185,11 +181,9 @@ void wom::vision::Limelight::OnUpdate(units::time::second_t dt) {
   wom::utils::WritePose3NT(table, GetPose());
 }
 
-bool wom::vision::Limelight::IsAtSetPoseVision(frc::Pose3d pose,
-                                               units::second_t dt) {
+bool wom::vision::Limelight::IsAtSetPoseVision(frc::Pose3d pose, units::second_t dt) {
   frc::Pose3d actualPose = GetPose();
   frc::Pose3d relativePose = actualPose.RelativeTo(pose);
-  return (units::math::fabs(relativePose.X()) < 0.01_m &&
-          units::math::fabs(relativePose.Y()) < 0.01_m &&
+  return (units::math::fabs(relativePose.X()) < 0.01_m && units::math::fabs(relativePose.Y()) < 0.01_m &&
           GetSpeed(pose, GetPose(), dt) < 1_m / 1_s);
 }
diff --git a/wombat/src/main/include/behaviour/Behaviour.h b/wombat/src/main/include/behaviour/Behaviour.h
index 4f2e357e..84ac8689 100644
--- a/wombat/src/main/include/behaviour/Behaviour.h
+++ b/wombat/src/main/include/behaviour/Behaviour.h
@@ -22,13 +22,7 @@
 #include "HasBehaviour.h"
 
 namespace behaviour {
-enum class BehaviourState {
-  INITIALISED,
-  RUNNING,
-  DONE,
-  TIMED_OUT,
-  INTERRUPTED
-};
+enum class BehaviourState { INITIALISED, RUNNING, DONE, TIMED_OUT, INTERRUPTED };
 
 class SequentialBehaviour;
 
@@ -46,8 +40,7 @@ class Behaviour : public std::enable_shared_from_this<Behaviour> {
  public:
   using ptr = std::shared_ptr<Behaviour>;
 
-  Behaviour(std::string name = "<unnamed behaviour>",
-            units::time::second_t period = 20_ms);
+  explicit Behaviour(std::string name = "<unnamed behaviour>", units::time::second_t period = 20_ms);
   ~Behaviour();
 
   /**
@@ -193,16 +186,15 @@ class SequentialBehaviour : public Behaviour {
   std::deque<ptr> _queue;
 };
 
-inline std::shared_ptr<SequentialBehaviour> operator<<(Behaviour::ptr a,
-                                                       Behaviour::ptr b) {
+inline std::shared_ptr<SequentialBehaviour> operator<<(Behaviour::ptr a, Behaviour::ptr b) {
   auto seq = std::make_shared<SequentialBehaviour>();
   seq->Add(a);
   seq->Add(b);
   return seq;
 }
 
-inline std::shared_ptr<SequentialBehaviour> operator<<(
-    std::shared_ptr<SequentialBehaviour> a, Behaviour::ptr b) {
+inline std::shared_ptr<SequentialBehaviour> operator<<(std::shared_ptr<SequentialBehaviour> a,
+                                                       Behaviour::ptr b) {
   a->Add(b);
   return a;
 }
@@ -247,10 +239,8 @@ class ConcurrentBehaviour : public Behaviour {
  * Create a concurrent behaviour group, waiting for all behaviours
  * to finish before moving on.
  */
-inline std::shared_ptr<ConcurrentBehaviour> operator&(Behaviour::ptr a,
-                                                      Behaviour::ptr b) {
-  auto conc =
-      std::make_shared<ConcurrentBehaviour>(ConcurrentBehaviourReducer::ALL);
+inline std::shared_ptr<ConcurrentBehaviour> operator&(Behaviour::ptr a, Behaviour::ptr b) {
+  auto conc = std::make_shared<ConcurrentBehaviour>(ConcurrentBehaviourReducer::ALL);
   conc->Add(a);
   conc->Add(b);
   return conc;
@@ -261,10 +251,8 @@ inline std::shared_ptr<ConcurrentBehaviour> operator&(Behaviour::ptr a,
  * be interrupted as soon as any members of the group are finished (the
  * behaviours are 'raced' against each other).
  */
-inline std::shared_ptr<ConcurrentBehaviour> operator|(Behaviour::ptr a,
-                                                      Behaviour::ptr b) {
-  auto conc =
-      std::make_shared<ConcurrentBehaviour>(ConcurrentBehaviourReducer::ANY);
+inline std::shared_ptr<ConcurrentBehaviour> operator|(Behaviour::ptr a, Behaviour::ptr b) {
+  auto conc = std::make_shared<ConcurrentBehaviour>(ConcurrentBehaviourReducer::ANY);
   conc->Add(a);
   conc->Add(b);
   return conc;
@@ -332,8 +320,7 @@ struct Switch : public Behaviour {
    * @param condition The function yielding true if this is the correct option
    * @param b The behaviour to call if this option is provided.
    */
-  std::shared_ptr<Switch> When(std::function<bool(T&)> condition,
-                               Behaviour::ptr b) {
+  std::shared_ptr<Switch> When(std::function<bool(T&)> condition, Behaviour::ptr b) {
     _options.push_back(std::pair(condition, b));
     Inherit(*b);
     return std::reinterpret_pointer_cast<Switch<T>>(shared_from_this());
@@ -389,8 +376,7 @@ struct Switch : public Behaviour {
 
  private:
   std::function<T()> _fn;
-  wpi::SmallVector<std::pair<std::function<bool(T&)>, Behaviour::ptr>, 4>
-      _options;
+  wpi::SmallVector<std::pair<std::function<bool(T&)>, Behaviour::ptr>, 4> _options;
   Behaviour::ptr _locked = nullptr;
 };
 
@@ -407,10 +393,8 @@ struct Decide : public Switch<std::monostate> {
    * @param condition The function yielding true if this is the correct option
    * @param b The behaviour to call if this option is provided.
    */
-  std::shared_ptr<Decide> When(std::function<bool()> condition,
-                               Behaviour::ptr b) {
-    return std::reinterpret_pointer_cast<Decide>(
-        Switch::When([condition](auto) { return condition(); }, b));
+  std::shared_ptr<Decide> When(std::function<bool()> condition, Behaviour::ptr b) {
+    return std::reinterpret_pointer_cast<Decide>(Switch::When([condition](auto) { return condition(); }, b));
   }
 };
 
diff --git a/wombat/src/main/include/behaviour/HasBehaviour.h b/wombat/src/main/include/behaviour/HasBehaviour.h
index c9b35b2e..bae5ac05 100644
--- a/wombat/src/main/include/behaviour/HasBehaviour.h
+++ b/wombat/src/main/include/behaviour/HasBehaviour.h
@@ -30,8 +30,7 @@ class HasBehaviour {
 
  protected:
   std::shared_ptr<Behaviour> _active_behaviour{nullptr};
-  std::function<std::shared_ptr<Behaviour>(void)> _default_behaviour_producer{
-      nullptr};
+  std::function<std::shared_ptr<Behaviour>(void)> _default_behaviour_producer{nullptr};
 
  private:
   friend class BehaviourScheduler;
diff --git a/wombat/src/main/include/subsystems/Arm.h b/wombat/src/main/include/subsystems/Arm.h
index 155fd4cd..8b91be72 100644
--- a/wombat/src/main/include/subsystems/Arm.h
+++ b/wombat/src/main/include/subsystems/Arm.h
@@ -68,8 +68,7 @@ class Arm : public behaviour::HasBehaviour {
   ArmConfig _config;
   ArmState _state = ArmState::kIdle;
   wom::utils::PIDController<units::radian, units::volt> _pid;
-  wom::utils::PIDController<units::radians_per_second, units::volt>
-      _velocityPID;
+  wom::utils::PIDController<units::radians_per_second, units::volt> _velocityPID;
 
   std::shared_ptr<nt::NetworkTable> _table;
 
diff --git a/wombat/src/main/include/subsystems/Shooter.h b/wombat/src/main/include/subsystems/Shooter.h
index 2bbf6f5e..e5ac219d 100644
--- a/wombat/src/main/include/subsystems/Shooter.h
+++ b/wombat/src/main/include/subsystems/Shooter.h
@@ -62,8 +62,7 @@ class ShooterConstant : public behaviour::Behaviour {
 
 class ShooterSpinup : public behaviour::Behaviour {
  public:
-  ShooterSpinup(Shooter* s, units::radians_per_second_t speed,
-                bool hold = false);
+  ShooterSpinup(Shooter* s, units::radians_per_second_t speed, bool hold = false);
 
   void OnTick(units::second_t dt) override;
 
diff --git a/wombat/src/main/include/utils/Util.h b/wombat/src/main/include/utils/Util.h
index 665c85ec..d624e408 100644
--- a/wombat/src/main/include/utils/Util.h
+++ b/wombat/src/main/include/utils/Util.h
@@ -28,39 +28,30 @@ units::second_t now();
 
 class NTBound {
  public:
-  NTBound(std::shared_ptr<nt::NetworkTable> table, std::string name,
-          const nt::Value& value,
+  NTBound(std::shared_ptr<nt::NetworkTable> table, std::string name, const nt::Value& value,
           std::function<void(const nt::Value&)> onUpdateFn)
-      : _table(table),
-        _entry(table->GetEntry(name)),
-        _onUpdate(onUpdateFn),
-        _name(name) {
+      : _table(table), _entry(table->GetEntry(name)), _onUpdate(onUpdateFn), _name(name) {
     _entry.SetValue(value);
     // _listener = table->AddListener(name, , ([=](const nt::EntryNotification
     // &evt) {
     //   this->_onUpdate(evt.value);
     // }, NT_NOTIFY_UPDATE);
-    _listener = table->AddListener(
-        name, nt::EventFlags::kValueAll,
-        ([this](nt::NetworkTable* table, std::string_view key,
-                const nt::Event& event) {
-          std::cout << "NT UPDATE" << std::endl;
-          this->_onUpdate(event.GetValueEventData()->value);
-        }));
+    _listener =
+        table->AddListener(name, nt::EventFlags::kValueAll,
+                           ([this](nt::NetworkTable* table, std::string_view key, const nt::Event& event) {
+                             std::cout << "NT UPDATE" << std::endl;
+                             this->_onUpdate(event.GetValueEventData()->value);
+                           }));
   }
 
   NTBound(const NTBound& other)
-      : _table(other._table),
-        _entry(other._entry),
-        _onUpdate(other._onUpdate),
-        _name(other._name) {
-    _listener = _table->AddListener(
-        _name, nt::EventFlags::kValueAll,
-        ([this](nt::NetworkTable* table, std::string_view key,
-                const nt::Event& event) {
-          std::cout << "NT UPDATE" << std::endl;
-          this->_onUpdate(event.GetValueEventData()->value);
-        }));
+      : _table(other._table), _entry(other._entry), _onUpdate(other._onUpdate), _name(other._name) {
+    _listener =
+        _table->AddListener(_name, nt::EventFlags::kValueAll,
+                            ([this](nt::NetworkTable* table, std::string_view key, const nt::Event& event) {
+                              std::cout << "NT UPDATE" << std::endl;
+                              this->_onUpdate(event.GetValueEventData()->value);
+                            }));
   }
 
   ~NTBound() { _table->RemoveListener(_listener); }
@@ -75,8 +66,7 @@ class NTBound {
 
 class NTBoundDouble : public NTBound {
  public:
-  NTBoundDouble(std::shared_ptr<nt::NetworkTable> table, std::string name,
-                double& val)
+  NTBoundDouble(std::shared_ptr<nt::NetworkTable> table, std::string name, double& val)
       : NTBound(table, name, nt::Value::MakeDouble(val),
                 [&val](const nt::Value& v) { val = v.GetDouble(); }) {}
 };
@@ -84,20 +74,15 @@ class NTBoundDouble : public NTBound {
 template <typename T>
 class NTBoundUnit : public NTBound {
  public:
-  NTBoundUnit(std::shared_ptr<nt::NetworkTable> table, std::string name,
-              units::unit_t<T>& val)
+  NTBoundUnit(std::shared_ptr<nt::NetworkTable> table, std::string name, units::unit_t<T>& val)
       : NTBound(table, name, nt::Value::MakeDouble(val.value()),
-                [&val](const nt::Value& v) {
-                  val = units::unit_t<T>{v.GetDouble()};
-                }) {}
+                [&val](const nt::Value& v) { val = units::unit_t<T>{v.GetDouble()}; }) {}
 };
 
 void WritePose2NT(std::shared_ptr<nt::NetworkTable> table, frc::Pose2d pose);
 void WritePose3NT(std::shared_ptr<nt::NetworkTable> table, frc::Pose3d pose);
-void WriteTrajectory(std::shared_ptr<nt::NetworkTable> table,
-                     frc::Trajectory trajectory);
-void WriteTrajectoryState(std::shared_ptr<nt::NetworkTable> table,
-                          frc::Trajectory::State state);
+void WriteTrajectory(std::shared_ptr<nt::NetworkTable> table, frc::Trajectory trajectory);
+void WriteTrajectoryState(std::shared_ptr<nt::NetworkTable> table, frc::Trajectory::State state);
 
 frc::Pose2d TrajectoryStateToPose2d(frc::Trajectory::State state);
 
diff --git a/wombat/src/main/include/vision/Limelight.h b/wombat/src/main/include/vision/Limelight.h
index ddfb3739..5aa6d0b7 100644
--- a/wombat/src/main/include/vision/Limelight.h
+++ b/wombat/src/main/include/vision/Limelight.h
@@ -26,20 +26,11 @@
 namespace wom {
 namespace vision {
 
-enum class LimelightLEDMode {
-  kPipelineDefault = 0,
-  kForceOff = 1,
-  kForceBlink = 2,
-  kForceOn = 3
-};
+enum class LimelightLEDMode { kPipelineDefault = 0, kForceOff = 1, kForceBlink = 2, kForceOn = 3 };
 
 enum class LimelightCamMode { kVisionProcessor = 0, kDriverCamera = 1 };
 
-enum class LimelightStreamMode {
-  kStandard = 0,
-  kPiPMain = 1,
-  kPiPSecondary = 2
-};
+enum class LimelightStreamMode { kStandard = 0, kPiPMain = 1, kPiPSecondary = 2 };
 
 enum class LimelightSnapshotMode { kReset = 0, kSingle = 1 };
 
@@ -91,14 +82,12 @@ class Limelight {
 
   std::string GetName();
 
-  std::shared_ptr<nt::NetworkTable> table =
-      nt::NetworkTableInstance::GetDefault().GetTable("limelight");
+  std::shared_ptr<nt::NetworkTable> table = nt::NetworkTableInstance::GetDefault().GetTable("limelight");
 
   std::pair<double, double> GetOffset();
 
   std::vector<double> GetAprilTagData(LimelightAprilTagData dataType);
-  double GetTargetingData(LimelightTargetingData dataType,
-                          double defaultValue = 0.0);
+  double GetTargetingData(LimelightTargetingData dataType, double defaultValue = 0.0);
   void SetLEDMode(LimelightLEDMode mode);
   void SetCamMode(LimelightCamMode mode);
   void SetPipeline(LimelightPipeline pipeline);
@@ -111,8 +100,7 @@ class Limelight {
 
   bool IsAtSetPoseVision(frc::Pose3d pose, units::second_t dt);
 
-  units::meters_per_second_t GetSpeed(frc::Pose3d pose1, frc::Pose3d pose2,
-                                      units::second_t dt);
+  units::meters_per_second_t GetSpeed(frc::Pose3d pose1, frc::Pose3d pose2, units::second_t dt);
 
   frc::Pose3d GetPose();
 

From 650da176f35d49fbd208809930934b9873f1f1c1 Mon Sep 17 00:00:00 2001
From: Isaac Turner <spacey_sooty@outlook.com>
Date: Wed, 24 Jan 2024 14:45:55 +0800
Subject: [PATCH 135/207] fix more stuff

---
 .github/workflows/format.yml |  2 +-
 wombat/LICENSE               | 21 ---------------------
 wombat/README.md             |  1 -
 wombat/settings.gradle       |  1 -
 4 files changed, 1 insertion(+), 24 deletions(-)
 delete mode 100644 wombat/LICENSE
 delete mode 100644 wombat/README.md
 delete mode 100644 wombat/settings.gradle

diff --git a/.github/workflows/format.yml b/.github/workflows/format.yml
index c6fced20..f57ddeb4 100644
--- a/.github/workflows/format.yml
+++ b/.github/workflows/format.yml
@@ -21,7 +21,7 @@ jobs:
         with:
           python-version: 3.8
       - name: Install wpiformat
-        run: pip3 install wpiformat==2023.36
+        run: pip3 install wpiformat==2024.31
       - name: Run
         run: wpiformat
       - name: Check output
diff --git a/wombat/LICENSE b/wombat/LICENSE
deleted file mode 100644
index b1561bab..00000000
--- a/wombat/LICENSE
+++ /dev/null
@@ -1,21 +0,0 @@
-MIT License
-
-Copyright (c) 2023 CurtinFRC
-
-Permission is hereby granted, free of charge, to any person obtaining a copy
-of this software and associated documentation files (the "Software"), to deal
-in the Software without restriction, including without limitation the rights
-to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
-copies of the Software, and to permit persons to whom the Software is
-furnished to do so, subject to the following conditions:
-
-The above copyright notice and this permission notice shall be included in all
-copies or substantial portions of the Software.
-
-THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
-IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
-FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
-AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
-LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
-OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
-SOFTWARE.
diff --git a/wombat/README.md b/wombat/README.md
deleted file mode 100644
index 0595c6cb..00000000
--- a/wombat/README.md
+++ /dev/null
@@ -1 +0,0 @@
-# Wombat
diff --git a/wombat/settings.gradle b/wombat/settings.gradle
deleted file mode 100644
index 8b137891..00000000
--- a/wombat/settings.gradle
+++ /dev/null
@@ -1 +0,0 @@
-

From 7a950463fa470c034b52a8bfab9d191f915333e1 Mon Sep 17 00:00:00 2001
From: totallysomeoneyoudontknow
 <155517980+totallysomeoneyoudontknow@users.noreply.github.com>
Date: Tue, 30 Jan 2024 14:32:03 +0800
Subject: [PATCH 136/207] Shooter pid (#117)

* added the state raw and changed voltage

* test

* added the state raw and changed voltage

* added pid calculate loop, fixed indenting

* added calculate function on a new line, changed the PID to not stable

* Fixed the if condition so shooterSensor is not tripped

* changed the functions so they start with a capital

* changed the value for shooterMotor

* added shuffleboard and networking tables

* added shuffleboard and networking tables

* added network tables

* fixed merge conflicts and merge errors

* fixed merge conflicts and merge errors

* fixed merge conflicts and merge errors

* fixed errors

* ran wpiformat

* fixed merge errors and conflicts

* fixed github errors

* Alpha Bot Shooter

* shooter with pid loop

* Fixed errors

* Fixing pid loop control

* fixing pid control

* pid loop tuning

* fixed PID control

* ran wpiformat

* ran wpiformat again;

* fixed build

* wpiformat

---------

Co-authored-by: Havish12 <havishchopra10@gmail.com>
Co-authored-by: Isaac Turner <spacey_sooty@outlook.com>
---
 src/main/cpp/Robot.cpp                        | 10 +++++
 src/main/cpp/Shooter.cpp                      |  6 +++
 src/main/include/RobotMap.h                   | 40 ++++++++++++++++++-
 src/main/include/Shooter.h                    |  6 ++-
 src/main/include/ShooterBehaviour.h           | 11 ++++-
 .../src/main/cpp/drivetrain/SwerveDrive.cpp   |  9 ++++-
 wombat/src/main/cpp/utils/Encoder.cpp         |  2 +-
 7 files changed, 78 insertions(+), 6 deletions(-)

diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp
index a0d1287a..72cdc28e 100644
--- a/src/main/cpp/Robot.cpp
+++ b/src/main/cpp/Robot.cpp
@@ -153,6 +153,7 @@ void Robot::RobotPeriodic() {
   wom::BehaviourScheduler::GetInstance()->Tick();
   sched->Tick();
 
+<<<<<<< HEAD
 <<<<<<< HEAD
   robotmap.swerveTable.swerveDriveTable->GetEntry("Encoder 0 offset: ")
       .SetDouble(robotmap.swerveBase.moduleConfigs[0].turnMotor.encoder->GetEncoderPosition().value());
@@ -163,6 +164,11 @@ void Robot::RobotPeriodic() {
   robotmap.swerveTable.swerveDriveTable->GetEntry("Encoder 3 offset: ")
 =======
   robotmap.swerveTable.swerveDriveTable->GetEntry("frontLeftEncoder")
+=======
+  robotmap.swerveTable.swerveDriveTable
+      ->GetEntry("frontLeftEncoder")
+
+>>>>>>> 57d11c0 (Shooter pid (#117))
       .SetDouble(robotmap.swerveBase.moduleConfigs[0].turnMotor.encoder->GetEncoderPosition().value());
   robotmap.swerveTable.swerveDriveTable->GetEntry("frontRightEncoder")
       .SetDouble(robotmap.swerveBase.moduleConfigs[1].turnMotor.encoder->GetEncoderPosition().value());
@@ -195,11 +201,15 @@ void Robot::AutonomousPeriodic() {}
 void Robot::TeleopInit() {
   loop.Clear();
   wom::BehaviourScheduler* sched = wom::BehaviourScheduler::GetInstance();
+<<<<<<< HEAD
 <<<<<<< HEAD
   // shooter->OnStart();
   // alphaArm->OnStart();
 =======
 >>>>>>> 0029f49 (fix some formatting)
+=======
+  shooter->OnStart();
+>>>>>>> 57d11c0 (Shooter pid (#117))
   sched->InterruptAll();
 
   // frontLeft->SetVoltage(4_V);
diff --git a/src/main/cpp/Shooter.cpp b/src/main/cpp/Shooter.cpp
index 410da01b..7a32a5f6 100644
--- a/src/main/cpp/Shooter.cpp
+++ b/src/main/cpp/Shooter.cpp
@@ -5,12 +5,16 @@
 #include "Shooter.h"
 
 <<<<<<< HEAD
+<<<<<<< HEAD
+=======
+>>>>>>> 57d11c0 (Shooter pid (#117))
 Shooter::Shooter(ShooterConfig config) : _config(config), _pid(config.path + "/pid", config.pidConfig) {}
 
 void Shooter::OnStart() {
   _pid.Reset();
 }
 
+<<<<<<< HEAD
 =======
 Shooter::Shooter(ShooterConfig config)
     : _config(config)
@@ -18,6 +22,8 @@ Shooter::Shooter(ShooterConfig config)
 // _pid{frc::PIDController (1, 0, 0, 0.005_s)}
 {}  // config.path + "/pid", config.pidConfig
 >>>>>>> 0029f49 (fix some formatting)
+=======
+>>>>>>> 57d11c0 (Shooter pid (#117))
 void Shooter::OnUpdate(units::second_t dt) {
   // _pid.SetTolerance(0.5, 4);
   table->GetEntry("Error").SetDouble(_pid.GetError().value());
diff --git a/src/main/include/RobotMap.h b/src/main/include/RobotMap.h
index b4b2a3f1..27d5259b 100644
--- a/src/main/include/RobotMap.h
+++ b/src/main/include/RobotMap.h
@@ -35,6 +35,7 @@ struct RobotMap {
   };
   Controllers controllers;
 
+<<<<<<< HEAD
   //   struct AlphaArmSystem {
   //     rev::CANSparkMax alphaArmMotor{12, rev::CANSparkMax::MotorType::kBrushless};
   //     wom::CANSparkMaxEncoder* armEncoder = new wom::CANSparkMaxEncoder(&alphaArmMotor, 0.02_m);
@@ -50,8 +51,28 @@ struct RobotMap {
   //     };
 =======
     AlphaArmConfig config{alphaArmGearbox, wristGearbox};
+=======
+  struct Shooter {
+    rev::CANSparkMax shooterMotor{11, rev::CANSparkMax::MotorType::kBrushless};  // Port 11
+    // frc::DigitalInput shooterSensor{2};
+
+    // wom::VoltageController shooterMotorGroup =
+    // wom::VoltagedController::Group(shooterMotor);
+    wom::CANSparkMaxEncoder* shooterEncoder = new wom::CANSparkMaxEncoder(&shooterMotor, 0.01_m);
+    wom::Gearbox shooterGearbox{&shooterMotor, shooterEncoder, frc::DCMotor::NEO(1)};
+
+    wom::utils::PIDConfig<units::radians_per_second, units::volts> pidConfigS{
+        "/armavator/arm/velocityPID/config",
+        0.1_V / (360_deg / 1_s),
+        0.03_V / 25_deg,
+        0.001_V / (90_deg / 1_s / 1_s),
+        5_rad_per_s,
+        10_rad_per_s / 1_s};
+
+    ShooterConfig config{"shooterGearbox", shooterGearbox, pidConfigS};
+>>>>>>> 57d11c0 (Shooter pid (#117))
   };
-  AlphaArmSystem alphaArmSystem;
+  Shooter shooterSystem;
 
   struct IntakeSystem {
     rev::CANSparkMax intakeMotor{2, rev::CANSparkMax::MotorType::kBrushed};
@@ -82,6 +103,7 @@ struct RobotMap {
   IntakeSystem intakeSystem;
 >>>>>>> 0029f49 (fix some formatting)
 
+<<<<<<< HEAD
   //     wom::Gearbox IntakeGearbox{&intakeMotor, nullptr, frc::DCMotor::CIM(1)};
 
   //     IntakeConfig config{IntakeGearbox, &intakeSensor /*, &magSensor, &shooterSensor*/};
@@ -124,6 +146,8 @@ struct RobotMap {
   Shooter shooterSystem;
 
 >>>>>>> 0029f49 (fix some formatting)
+=======
+>>>>>>> 57d11c0 (Shooter pid (#117))
   struct SwerveBase {
     ctre::phoenix6::hardware::CANcoder frontLeftCancoder{16, "Drivebase"};
     ctre::phoenix6::hardware::CANcoder frontRightCancoder{18, "Drivebase"};
@@ -255,6 +279,7 @@ struct RobotMap {
         nt::NetworkTableInstance::GetDefault().GetTable("swerve");
   };
   SwerveTable swerveTable;
+<<<<<<< HEAD
 <<<<<<< HEAD
 
   // struct AlphaArmSystem {
@@ -267,6 +292,19 @@ struct RobotMap {
   //   AlphaArmConfig config{alphaArmGearbox, wristGearbox};
   // };
   // AlphaArmSystem alphaArmSystem;
+=======
+
+  struct AlphaArmSystem {
+    rev::CANSparkMax alphaArmMotor{12, rev::CANSparkMax::MotorType::kBrushless};
+    rev::CANSparkMax wristMotor{15, rev::CANSparkMax::MotorType::kBrushless};
+
+    wom::Gearbox alphaArmGearbox{&alphaArmMotor, nullptr, frc::DCMotor::NEO(1)};
+    wom::Gearbox wristGearbox{&wristMotor, nullptr, frc::DCMotor::NEO(1)};
+
+    AlphaArmConfig config{alphaArmGearbox, wristGearbox};
+  };
+  AlphaArmSystem alphaArmSystem;
+>>>>>>> 57d11c0 (Shooter pid (#117))
 };
 =======
 };
diff --git a/src/main/include/Shooter.h b/src/main/include/Shooter.h
index 0962e80d..b030b75d 100644
--- a/src/main/include/Shooter.h
+++ b/src/main/include/Shooter.h
@@ -46,4 +46,8 @@ class Shooter : public behaviour::HasBehaviour {
 
   units::volt_t holdVoltage = 0_V;
   std::string _statename = "default";
-};
\ No newline at end of file
+<<<<<<< HEAD
+};
+=======
+};
+>>>>>>> 57d11c0 (Shooter pid (#117))
diff --git a/src/main/include/ShooterBehaviour.h b/src/main/include/ShooterBehaviour.h
index 15b54aaf..e39a1793 100644
--- a/src/main/include/ShooterBehaviour.h
+++ b/src/main/include/ShooterBehaviour.h
@@ -19,9 +19,16 @@ class ShooterManualControl : public behaviour::Behaviour {
 
  private:
   Shooter* _shooter;
-  frc::XboxController* _codriver;
 
+<<<<<<< HEAD
   bool _rawControl = false;
   std::shared_ptr<nt::NetworkTable> table =
       nt::NetworkTableInstance::GetDefault().GetTable("Shooter Behaviour");
-};
\ No newline at end of file
+};
+=======
+  bool _rawControl = true;
+  std::shared_ptr<nt::NetworkTable> table =
+      nt::NetworkTableInstance::GetDefault().GetTable("Shooter Behaviour");
+  frc::XboxController* _codriver;
+};
+>>>>>>> 57d11c0 (Shooter pid (#117))
diff --git a/wombat/src/main/cpp/drivetrain/SwerveDrive.cpp b/wombat/src/main/cpp/drivetrain/SwerveDrive.cpp
index 582f04f5..c98f4cc2 100644
--- a/wombat/src/main/cpp/drivetrain/SwerveDrive.cpp
+++ b/wombat/src/main/cpp/drivetrain/SwerveDrive.cpp
@@ -116,7 +116,8 @@ void SwerveModule::OnUpdate(units::second_t dt) {
   //     -torqueLimit, _config.driveMotor.encoder->GetEncoderAngularVelocity());
 
   // driveVoltage =
-  //     units::math::max(units::math::min(driveVoltage, voltageMax), voltageMin);
+  //     units::math::max(units::math::min(driveVoltage, voltageMax),
+  //     voltageMin);
 
   driveVoltage = units::math::min(driveVoltage, 7_V);
   turnVoltage = units::math::min(turnVoltage, 4_V);
@@ -187,6 +188,12 @@ void SwerveModule::SetZero(units::second_t dt) {
 
 void SwerveModule::SetPID(units::radian_t angle, units::meters_per_second_t speed, units::second_t dt) {
   _state = SwerveModuleState::kPID;
+<<<<<<< HEAD
+=======
+  // @liam start added
+  // std::cout << "angle Setpoint: " << _anglePIDController.GetSetpoint() <<
+  // std::endl; std::cout << "angle value: " << angle.value() << std::endl;
+>>>>>>> 57d11c0 (Shooter pid (#117))
 
 <<<<<<< HEAD
   // double diff = std::abs(_config.turnMotor.encoder->GetEncoderPosition().value() - angle.value());
diff --git a/wombat/src/main/cpp/utils/Encoder.cpp b/wombat/src/main/cpp/utils/Encoder.cpp
index a4ca9ca5..a40f4578 100644
--- a/wombat/src/main/cpp/utils/Encoder.cpp
+++ b/wombat/src/main/cpp/utils/Encoder.cpp
@@ -115,7 +115,7 @@ double wom::utils::CANSparkMaxEncoder::GetPosition() const {
 }
 
 double wom::utils::CANSparkMaxEncoder::GetVelocity() const {
-  return _encoder.GetVelocity();
+  return _encoder.GetVelocity() * 3.14159265 * 2 / 60;
 }
 
 wom::utils::TalonFXEncoder::TalonFXEncoder(ctre::phoenix6::hardware::TalonFX* controller,

From d5872bf924aba045304bd650ea0dea27455f5813 Mon Sep 17 00:00:00 2001
From: prawny-boy <155516197+prawny-boy@users.noreply.github.com>
Date: Tue, 30 Jan 2024 15:12:55 +0800
Subject: [PATCH 137/207] Intake - Manual/Auto fixes (#114)

* First version Tank Drive

* Intake draft

* Intake draft fixed errors 1

* Intake draft fixed errors 2

* Intake draft fixed errors 3

* Intake draft fixed errors 4

* Intake draft fixed errors 5

* Network files 1

* Network files 2

* Network files 3

* Almost done i think

* finished wpiformat errors

* finished wpiformat errors 2

* finished wpiformat errors 3

* finished wpiformat errors 4

* ran wpiformat

* finished wpiformat errors 5

* finished wpiformat errors 8

* finished wpiformat errors 9

* finished wpiformat errors 10

* Tested on robot

* Fixed github build errors after modifying during robot tests

* fixed more github build errors 2

* fixed more github build errors 3

* Tested on robot and works

* added beam break

* [docs] Update README.md (#107)

* [docs] Update README.md

* Update README.md

* Added auto to intake

* [ci] add comment command action (#111)

* [ci] add comment command action

* fix

* Tested on robot

* fixed intake manual auto, has not been tested on robot

* fixed github build errors

* Added intake better manual and verfied on robot!

* Bump actions/setup-python from 4 to 5 (#116)

Bumps [actions/setup-python](https://github.com/actions/setup-python) from 4 to 5.
- [Release notes](https://github.com/actions/setup-python/releases)
- [Commits](https://github.com/actions/setup-python/compare/v4...v5)

---
updated-dependencies:
- dependency-name: actions/setup-python
  dependency-type: direct:production
  update-type: version-update:semver-major
...

Signed-off-by: dependabot[bot] <support@github.com>
Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com>

* Bump actions/github-script from 6 to 7 (#115)

Bumps [actions/github-script](https://github.com/actions/github-script) from 6 to 7.
- [Release notes](https://github.com/actions/github-script/releases)
- [Commits](https://github.com/actions/github-script/compare/v6...v7)

---
updated-dependencies:
- dependency-name: actions/github-script
  dependency-type: direct:production
  update-type: version-update:semver-major
...

Signed-off-by: dependabot[bot] <support@github.com>
Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com>

* Fixed readme (#118)

* ran wpiformat

* fix

* fix line endings

* builds

---------

Signed-off-by: dependabot[bot] <support@github.com>
Co-authored-by: Kingsley Wong <kingsley.wong@student.education.wa.edu.au>
Co-authored-by: Isaac Turner <spacey_sooty@outlook.com>
Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com>
Co-authored-by: Paul Hodges <111325206+Superbro525Alt@users.noreply.github.com>
---
 src/main/cpp/Intake.cpp            | 24 +++++++++-
 src/main/cpp/IntakeBehaviour.cpp   | 76 +++++++++++++++++++++++++++++-
 src/main/cpp/Robot.cpp             | 16 +++++++
 src/main/include/Intake.h          |  6 +++
 src/main/include/IntakeBehaviour.h |  7 +++
 src/main/include/RobotMap.h        | 20 +++++++-
 6 files changed, 146 insertions(+), 3 deletions(-)

diff --git a/src/main/cpp/Intake.cpp b/src/main/cpp/Intake.cpp
index a96c289b..a92f14d4 100644
--- a/src/main/cpp/Intake.cpp
+++ b/src/main/cpp/Intake.cpp
@@ -30,12 +30,22 @@ void Intake::OnUpdate(units::second_t dt) {
       _setVoltage = 7_V;
       if (_config.intakeSensor->Get() == true) {
         setState(IntakeState::kIdle);
+<<<<<<< HEAD
+=======
+        _ejecting = false;
+>>>>>>> c30477a (Intake - Manual/Auto fixes (#114))
       }
     } break;
 
     case IntakeState::kHold: {
       _stringStateName = "Hold";
       _setVoltage = 0_V;
+<<<<<<< HEAD
+=======
+      // if (_config.intakeSensor->Get() == false) {
+      //   setState(IntakeState::kHold);
+      // }
+>>>>>>> c30477a (Intake - Manual/Auto fixes (#114))
     } break;
 
     case IntakeState::kIntake: {
@@ -43,6 +53,10 @@ void Intake::OnUpdate(units::second_t dt) {
       _setVoltage = -7_V;
       if (_config.intakeSensor->Get() == false) {
         setState(IntakeState::kHold);
+<<<<<<< HEAD
+=======
+        _intaking = false;
+>>>>>>> c30477a (Intake - Manual/Auto fixes (#114))
       }
     } break;
 
@@ -51,6 +65,10 @@ void Intake::OnUpdate(units::second_t dt) {
       _setVoltage = -7_V;
       if (_config.intakeSensor->Get() == true) {
         setState(IntakeState::kIdle);
+<<<<<<< HEAD
+=======
+        _passing = false;
+>>>>>>> c30477a (Intake - Manual/Auto fixes (#114))
       }
     } break;
     default:
@@ -78,4 +96,8 @@ void Intake::setRaw(units::volt_t voltage) {
 }
 IntakeState Intake::getState() {
   return _state;
-}
\ No newline at end of file
+<<<<<<< HEAD
+}
+=======
+}
+>>>>>>> c30477a (Intake - Manual/Auto fixes (#114))
diff --git a/src/main/cpp/IntakeBehaviour.cpp b/src/main/cpp/IntakeBehaviour.cpp
index 3192ef12..123e025b 100644
--- a/src/main/cpp/IntakeBehaviour.cpp
+++ b/src/main/cpp/IntakeBehaviour.cpp
@@ -13,23 +13,45 @@ IntakeManualControl::IntakeManualControl(Intake* intake, frc::XboxController& co
 
 void IntakeManualControl::OnTick(units::second_t dt) {
   if (_codriver.GetBButtonReleased()) {
+<<<<<<< HEAD
     if (_intake->getState() == IntakeState::kRaw) {
       _intake->setState(IntakeState::kIdle);
     } else {
+=======
+    if (_rawControl) {
+      _rawControl = false;
+      _intaking = false;
+      _ejecting = false;
+      _intake->setState(IntakeState::kIdle);
+    } else {
+      _rawControl = true;
+      _intaking = false;
+      _ejecting = false;
+>>>>>>> c30477a (Intake - Manual/Auto fixes (#114))
       _intake->setState(IntakeState::kRaw);
     }
   }
 
+<<<<<<< HEAD
   if (_intake->getState() == IntakeState::kRaw) {
     if (_codriver.GetRightBumper()) {
       _intake->setRaw(8_V);
     } else if (_codriver.GetLeftBumper()) {
       _intake->setRaw(-8_V);
+=======
+  if (_rawControl) {
+    if (_codriver.GetRightTriggerAxis() > 0.1) {
+      _intake->setRaw(_codriver.GetRightTriggerAxis() * 10_V);
+    } else if (_codriver.GetLeftTriggerAxis() > 0.1) {
+      _intake->setRaw(_codriver.GetLeftTriggerAxis() * -10_V);
+>>>>>>> c30477a (Intake - Manual/Auto fixes (#114))
     } else {
       _intake->setRaw(0_V);
     }
+    _intake->setState(IntakeState::kRaw);
 
   } else {
+<<<<<<< HEAD
     if (_codriver.GetRightBumperPressed()) {
       if (_intake->getState() == IntakeState::kIntake) {
         _intake->setState(IntakeState::kIdle);
@@ -43,16 +65,64 @@ void IntakeManualControl::OnTick(units::second_t dt) {
         _intake->setState(IntakeState::kIdle);
       } else {
         _intake->setState(IntakeState::kEject);
+=======
+    if (_codriver.GetRightTriggerAxis() > 0.1) {
+      if (_intaking) {
+        _intaking = false;
+        _intake->setState(IntakeState::kIdle);
+      } else {
+        _intaking = true;
+        _ejecting = false;
+      }
+    }
+
+    if (_codriver.GetLeftTriggerAxis() > 0.1) {
+      if (_ejecting) {
+        _ejecting = false;
+        _intake->setState(IntakeState::kIdle);
+      } else {
+        _ejecting = true;
+        _intaking = false;
+>>>>>>> c30477a (Intake - Manual/Auto fixes (#114))
       }
     }
 
     if (_codriver.GetAButtonPressed()) {
+<<<<<<< HEAD
       if (_intake->getState() == IntakeState::kPass) {
         _intake->setState(IntakeState::kIdle);
       } else {
         _intake->setState(IntakeState::kPass);
       }
     }
+=======
+      if (_passing) {
+        _passing = false;
+        _intake->setState(IntakeState::kIdle);
+      } else {
+        _passing = true;
+        _intaking = false;
+      }
+    }
+
+    if (_intaking) {
+      if (_intake->getState() == IntakeState::kIdle) {
+        _intake->setState(IntakeState::kIntake);
+      }
+    }
+
+    if (_passing) {
+      if (_intake->getState() == IntakeState::kHold) {
+        _intake->setState(IntakeState::kPass);
+      }
+    }
+
+    if (_ejecting) {
+      if (_intake->getState() == IntakeState::kIdle || _intake->getState() == IntakeState::kHold) {
+        _intake->setState(IntakeState::kEject);
+      }
+    }
+>>>>>>> c30477a (Intake - Manual/Auto fixes (#114))
   }
 }
 
@@ -60,4 +130,8 @@ IntakeAutoControl::IntakeAutoControl(Intake* intake) : _intake(intake) {
   Controls(intake);
 }
 
-void IntakeAutoControl::OnTick(units::second_t dt) {}
\ No newline at end of file
+<<<<<<< HEAD
+void IntakeAutoControl::OnTick(units::second_t dt) {}
+=======
+void IntakeAutoControl::OnTick(units::second_t dt) {}
+>>>>>>> c30477a (Intake - Manual/Auto fixes (#114))
diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp
index 72cdc28e..8ee61fef 100644
--- a/src/main/cpp/Robot.cpp
+++ b/src/main/cpp/Robot.cpp
@@ -69,6 +69,7 @@ void Robot::RobotInit() {
 
   // robotmap.swerveBase.gyro->Reset();
 
+<<<<<<< HEAD
 <<<<<<< HEAD
 =======
   // _swerveDrive = new wom::SwerveDrive(robotmap.swerveBase.config, frc::Pose2d());
@@ -76,6 +77,8 @@ void Robot::RobotInit() {
   // _swerveDrive->SetDefaultBehaviour(
   //     [this]() { return wom::make<wom::ManualDrivebase>(_swerveDrive, &robotmap.controllers.driver); });
 >>>>>>> 0029f49 (fix some formatting)
+=======
+>>>>>>> c30477a (Intake - Manual/Auto fixes (#114))
   _swerveDrive = new wom::SwerveDrive(robotmap.swerveBase.config, frc::Pose2d());
   wom::BehaviourScheduler::GetInstance()->Register(_swerveDrive);
   _swerveDrive->SetDefaultBehaviour(
@@ -153,6 +156,7 @@ void Robot::RobotPeriodic() {
   wom::BehaviourScheduler::GetInstance()->Tick();
   sched->Tick();
 
+<<<<<<< HEAD
 <<<<<<< HEAD
 <<<<<<< HEAD
   robotmap.swerveTable.swerveDriveTable->GetEntry("Encoder 0 offset: ")
@@ -169,6 +173,9 @@ void Robot::RobotPeriodic() {
       ->GetEntry("frontLeftEncoder")
 
 >>>>>>> 57d11c0 (Shooter pid (#117))
+=======
+  robotmap.swerveTable.swerveDriveTable->GetEntry("frontLeftEncoder")
+>>>>>>> c30477a (Intake - Manual/Auto fixes (#114))
       .SetDouble(robotmap.swerveBase.moduleConfigs[0].turnMotor.encoder->GetEncoderPosition().value());
   robotmap.swerveTable.swerveDriveTable->GetEntry("frontRightEncoder")
       .SetDouble(robotmap.swerveBase.moduleConfigs[1].turnMotor.encoder->GetEncoderPosition().value());
@@ -182,6 +189,12 @@ void Robot::RobotPeriodic() {
   // intake->OnUpdate(dt);
   // alphaArm->OnUpdate(dt);
   _swerveDrive->OnUpdate(dt);
+<<<<<<< HEAD
+=======
+  alphaArm->OnUpdate(dt);
+  shooter->OnStart();
+  intake->OnUpdate(dt);
+>>>>>>> c30477a (Intake - Manual/Auto fixes (#114))
 }
 
 void Robot::AutonomousInit() {
@@ -202,6 +215,7 @@ void Robot::TeleopInit() {
   loop.Clear();
   wom::BehaviourScheduler* sched = wom::BehaviourScheduler::GetInstance();
 <<<<<<< HEAD
+<<<<<<< HEAD
 <<<<<<< HEAD
   // shooter->OnStart();
   // alphaArm->OnStart();
@@ -210,6 +224,8 @@ void Robot::TeleopInit() {
 =======
   shooter->OnStart();
 >>>>>>> 57d11c0 (Shooter pid (#117))
+=======
+>>>>>>> c30477a (Intake - Manual/Auto fixes (#114))
   sched->InterruptAll();
 
   // frontLeft->SetVoltage(4_V);
diff --git a/src/main/include/Intake.h b/src/main/include/Intake.h
index 1aca262a..a48a4319 100644
--- a/src/main/include/Intake.h
+++ b/src/main/include/Intake.h
@@ -38,6 +38,12 @@ class Intake : public behaviour::HasBehaviour {
   units::volt_t _rawVoltage = 0_V;
   std::string _stringStateName = "error";
   units::volt_t _setVoltage = 0_V;
+<<<<<<< HEAD
+=======
+  bool _intaking;
+  bool _ejecting;
+  bool _passing;
+>>>>>>> c30477a (Intake - Manual/Auto fixes (#114))
 
   std::shared_ptr<nt::NetworkTable> _table = nt::NetworkTableInstance::GetDefault().GetTable("Intake");
 };
\ No newline at end of file
diff --git a/src/main/include/IntakeBehaviour.h b/src/main/include/IntakeBehaviour.h
index 5dbe8ae6..ff69b993 100644
--- a/src/main/include/IntakeBehaviour.h
+++ b/src/main/include/IntakeBehaviour.h
@@ -21,6 +21,13 @@ class IntakeManualControl : public behaviour::Behaviour {
 
   units::volt_t _rawVoltage;
   units::volt_t _setVoltage;
+<<<<<<< HEAD
+=======
+  bool _rawControl = true;
+  bool _intaking = false;
+  bool _ejecting = false;
+  bool _passing = false;
+>>>>>>> c30477a (Intake - Manual/Auto fixes (#114))
 };
 
 class IntakeAutoControl : public behaviour::Behaviour {
diff --git a/src/main/include/RobotMap.h b/src/main/include/RobotMap.h
index 27d5259b..e0e52350 100644
--- a/src/main/include/RobotMap.h
+++ b/src/main/include/RobotMap.h
@@ -31,10 +31,10 @@ struct RobotMap {
   struct Controllers {
     frc::XboxController driver = frc::XboxController(0);
     frc::XboxController codriver = frc::XboxController(1);
-    frc::XboxController testController = frc::XboxController(2);
   };
   Controllers controllers;
 
+<<<<<<< HEAD
 <<<<<<< HEAD
   //   struct AlphaArmSystem {
   //     rev::CANSparkMax alphaArmMotor{12, rev::CANSparkMax::MotorType::kBrushless};
@@ -52,6 +52,21 @@ struct RobotMap {
 =======
     AlphaArmConfig config{alphaArmGearbox, wristGearbox};
 =======
+=======
+  struct IntakeSystem {
+    rev::CANSparkMax intakeMotor{2, rev::CANSparkMax::MotorType::kBrushed};
+    // wom::CANSparkMaxEncoder intakeEncoder{&intakeMotor, 0.1_m};
+    frc::DigitalInput intakeSensor{4};
+    // frc::DigitalInput magSensor{0};
+    // frc::DigitalInput shooterSensor{0};
+
+    wom::Gearbox IntakeGearbox{&intakeMotor, nullptr, frc::DCMotor::CIM(1)};
+
+    IntakeConfig config{IntakeGearbox, &intakeSensor /*, &magSensor, &shooterSensor*/};
+  };
+  IntakeSystem intakeSystem;
+
+>>>>>>> c30477a (Intake - Manual/Auto fixes (#114))
   struct Shooter {
     rev::CANSparkMax shooterMotor{11, rev::CANSparkMax::MotorType::kBrushless};  // Port 11
     // frc::DigitalInput shooterSensor{2};
@@ -74,6 +89,7 @@ struct RobotMap {
   };
   Shooter shooterSystem;
 
+<<<<<<< HEAD
   struct IntakeSystem {
     rev::CANSparkMax intakeMotor{2, rev::CANSparkMax::MotorType::kBrushed};
     // wom::CANSparkMaxEncoder intakeEncoder{&intakeMotor, 0.1_m};
@@ -148,6 +164,8 @@ struct RobotMap {
 >>>>>>> 0029f49 (fix some formatting)
 =======
 >>>>>>> 57d11c0 (Shooter pid (#117))
+=======
+>>>>>>> c30477a (Intake - Manual/Auto fixes (#114))
   struct SwerveBase {
     ctre::phoenix6::hardware::CANcoder frontLeftCancoder{16, "Drivebase"};
     ctre::phoenix6::hardware::CANcoder frontRightCancoder{18, "Drivebase"};

From c87ac5f69453d76ddcaba3e1ad53fffb49e8d519 Mon Sep 17 00:00:00 2001
From: Paul Hodges <paul.hodges@student.education.wa.edu.au>
Date: Thu, 25 Jan 2024 12:46:21 +0800
Subject: [PATCH 138/207] [robot/vision] Started work on limelight vision

---
 src/main/cpp/Robot.cpp                   |  45 +++
 src/main/cpp/vision/Vision.cpp           | 132 ++++++++
 src/main/deploy/fmap.fmap                | 388 +++++++++++++++++++++++
 src/main/include/Robot.h                 |  14 +-
 src/main/include/vision/Vision.h         |  56 ++++
 wombat/src/main/cpp/vision/Limelight.cpp |   2 +-
 6 files changed, 635 insertions(+), 2 deletions(-)
 create mode 100644 src/main/cpp/vision/Vision.cpp
 create mode 100644 src/main/deploy/fmap.fmap
 create mode 100644 src/main/include/vision/Vision.h

diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp
index 8ee61fef..8ea94d4a 100644
--- a/src/main/cpp/Robot.cpp
+++ b/src/main/cpp/Robot.cpp
@@ -38,7 +38,11 @@ static units::second_t lastPeriodic;
 
 void Robot::RobotInit() {
 <<<<<<< HEAD
+<<<<<<< HEAD
+=======
 =======
+
+>>>>>>> c83ac05 ([robot/vision] Started work on limelight vision)
   shooter = new Shooter(robotmap.shooterSystem.config);
   wom::BehaviourScheduler::GetInstance()->Register(shooter);
   shooter->SetDefaultBehaviour(
@@ -48,6 +52,9 @@ void Robot::RobotInit() {
   sched = wom::BehaviourScheduler::GetInstance();
   m_chooser.SetDefaultOption("Default Auto", "Default Auto");
 
+  // m_chooser.SetDefaultOption("Default Auto", "Default Auto");
+
+
   // frc::SmartDashboard::PutData("Auto Selector", &m_chooser);
 
   // m_path_chooser.SetDefaultOption("Path1", "paths/output/Path1.wpilib.json");
@@ -71,14 +78,21 @@ void Robot::RobotInit() {
 
 <<<<<<< HEAD
 <<<<<<< HEAD
+<<<<<<< HEAD
+=======
 =======
+>>>>>>> c83ac05 ([robot/vision] Started work on limelight vision)
   // _swerveDrive = new wom::SwerveDrive(robotmap.swerveBase.config, frc::Pose2d());
   // wom::BehaviourScheduler::GetInstance()->Register(_swerveDrive);
   // _swerveDrive->SetDefaultBehaviour(
   //     [this]() { return wom::make<wom::ManualDrivebase>(_swerveDrive, &robotmap.controllers.driver); });
+<<<<<<< HEAD
 >>>>>>> 0029f49 (fix some formatting)
 =======
 >>>>>>> c30477a (Intake - Manual/Auto fixes (#114))
+=======
+
+>>>>>>> c83ac05 ([robot/vision] Started work on limelight vision)
   _swerveDrive = new wom::SwerveDrive(robotmap.swerveBase.config, frc::Pose2d());
   wom::BehaviourScheduler::GetInstance()->Register(_swerveDrive);
   _swerveDrive->SetDefaultBehaviour(
@@ -145,6 +159,17 @@ void Robot::RobotInit() {
   // backRight = new ctre::phoenix6::hardware::TalonFX(3, "Drivebase");
 >>>>>>> 0029f49 (fix some formatting)
   lastPeriodic = wom::now();
+<<<<<<< HEAD
+=======
+
+  intake = new Intake(robotmap.intakeSystem.config);
+  wom::BehaviourScheduler::GetInstance()->Register(intake);
+  intake->SetDefaultBehaviour(
+      [this]() { return wom::make<IntakeManualControl>(intake, robotmap.controllers.codriver); });
+
+  //_vision = new Vision("limelight", FMAP("fmap.fmap"));
+
+>>>>>>> c83ac05 ([robot/vision] Started work on limelight vision)
 }
 
 void Robot::RobotPeriodic() {
@@ -185,16 +210,26 @@ void Robot::RobotPeriodic() {
 >>>>>>> 0029f49 (fix some formatting)
       .SetDouble(robotmap.swerveBase.moduleConfigs[3].turnMotor.encoder->GetEncoderPosition().value());
 
+<<<<<<< HEAD
   // shooter->OnUpdate(dt);
   // intake->OnUpdate(dt);
   // alphaArm->OnUpdate(dt);
+=======
+
+>>>>>>> c83ac05 ([robot/vision] Started work on limelight vision)
   _swerveDrive->OnUpdate(dt);
 <<<<<<< HEAD
 =======
   alphaArm->OnUpdate(dt);
   shooter->OnStart();
   intake->OnUpdate(dt);
+<<<<<<< HEAD
 >>>>>>> c30477a (Intake - Manual/Auto fixes (#114))
+=======
+
+  // _swerveDrive->OnUpdate(dt);
+
+>>>>>>> c83ac05 ([robot/vision] Started work on limelight vision)
 }
 
 void Robot::AutonomousInit() {
@@ -212,6 +247,7 @@ void Robot::AutonomousPeriodic() {}
 >>>>>>> 0029f49 (fix some formatting)
 
 void Robot::TeleopInit() {
+
   loop.Clear();
   wom::BehaviourScheduler* sched = wom::BehaviourScheduler::GetInstance();
 <<<<<<< HEAD
@@ -232,6 +268,15 @@ void Robot::TeleopInit() {
   // frontRight->SetVoltage(4_V);
   // backLeft->SetVoltage(4_V);
   // backRight->SetVoltage(4_V);
+<<<<<<< HEAD
+=======
+  
+
+//  FMAP("fmap.fmap");
+  // _swerveDrive->OnStart();
+  // sched->InterruptAll();
+
+>>>>>>> c83ac05 ([robot/vision] Started work on limelight vision)
 }
 void Robot::TeleopPeriodic() {}
 
diff --git a/src/main/cpp/vision/Vision.cpp b/src/main/cpp/vision/Vision.cpp
new file mode 100644
index 00000000..4d3fee35
--- /dev/null
+++ b/src/main/cpp/vision/Vision.cpp
@@ -0,0 +1,132 @@
+#include "vision/Vision.h"
+
+
+FMAP::FMAP(std::string path) : _path(path) {
+    std::cout << "Parsing FMAP" << std::endl;
+
+    deploy_dir = frc::filesystem::GetDeployDirectory();
+
+    std::ifstream file(deploy_dir + "/" + _path);
+
+    // parse the json file into a wpi::json object
+
+    wpi::json j;
+
+    file >> j;
+
+    // iterate through the json object and add each tag to the vector
+
+    for (auto& element : j["fiducials"]) {
+        AprilTag tag;
+
+        tag.id = element["id"];
+        tag.size = element["size"];
+        tag.transform[0][0] = element["transform"][0];
+        tag.transform[0][1] = element["transform"][1];
+        tag.transform[0][2] = element["transform"][2];
+        tag.transform[0][3] = element["transform"][3];
+        tag.transform[1][0] = element["transform"][4];
+        tag.transform[1][1] = element["transform"][5];
+        tag.transform[1][2] = element["transform"][6];
+        tag.transform[1][3] = element["transform"][7];
+        tag.transform[2][0] = element["transform"][8];
+        tag.transform[2][1] = element["transform"][9];
+        tag.transform[2][2] = element["transform"][10];
+        tag.transform[2][3] = element["transform"][11];
+        tag.transform[3][0] = element["transform"][12];
+        tag.transform[3][1] = element["transform"][13];
+        tag.transform[3][2] = element["transform"][14];
+        tag.transform[3][3] = element["transform"][15];
+        if (element["unique"] == 1) {
+            tag.unique = true;
+        } else {
+            tag.unique = false;
+        }
+
+        _tags.push_back(tag);
+    }
+
+    file.close();
+
+    std::cout << "Loaded " << _tags.size() << " tags" << std::endl;
+
+    for (int i = 0; i < _tags.size(); i++) {
+        std::cout << "Tag " << _tags[i].id << " is " << _tags[i].size << "m" << std::endl;
+    }
+
+    std::cout << "Loaded FMAP" << std::endl;
+};
+
+std::vector<AprilTag> FMAP::GetTags() {
+    return _tags;
+};
+
+Vision::Vision(std::string name, FMAP fmap) : _fmap(fmap) {
+    _limelight = new wom::Limelight(name);
+
+    _limelight->SetPipeline(wom::LimelightPipeline::kPipeline0);
+};
+
+frc::Pose3d Vision::GetPose() {
+    return _limelight->GetPose();
+};
+
+std::pair<units::meter_t, units::degree_t> Vision::GetDistanceToTarget(VisionTarget target) {
+    std::vector<AprilTag> tags = _fmap.GetTags();
+
+    frc::Pose3d pose = _limelight->GetPose();
+
+    for (int i = 0; i < tags.size(); i++) {
+        if (tags[i].id == (int)target) {         
+
+            frc::Pose3d aprilTagPos = frc::Pose3d(tags[i].transform[0][3] * 1_m, tags[i].transform[1][3] * 1_m, tags[i].transform[2][3] * 1_m, frc::Rotation3d(0_deg, 0_deg, 0_deg));   
+            
+            units::meter_t distance = pose.Translation().Distance(aprilTagPos.Translation());
+
+            units::degree_t angle = units::math::atan((pose.X() - units::meter_t{tags[i].transform[0][3]}) / (pose.Y() - units::meter_t{tags[i].transform[1][3]}));
+
+            return std::make_pair(distance, angle);
+        }
+    }
+
+    return std::make_pair(0_m, 0_deg);
+};
+
+std::pair<units::meter_t, units::degree_t> Vision::GetDistanceToTarget(int id) {
+    if (id < APRILTAGS_MIN || id > APRILTAGS_MAX) { return std::make_pair(0_m, 0_deg); };
+    
+    std::vector<AprilTag> tags = _fmap.GetTags();
+
+    for (int i = 0; i < tags.size(); i++) {
+        if (tags[i].id == id) {
+            // get distance to the limelight
+            frc::Pose3d currentPose = _limelight->GetPose();
+            frc::Pose3d aprilTagPos = frc::Pose3d(tags[i].transform[0][3] * 1_m, tags[i].transform[1][3] * 1_m, tags[i].transform[2][3] * 1_m, frc::Rotation3d(0_deg, 0_deg, 0_deg));
+
+            units::meter_t distance = currentPose.Translation().Distance(aprilTagPos.Translation());
+
+            units::degree_t angle = units::math::atan((currentPose.X() - units::meter_t{tags[i].transform[0][3]}) / (currentPose.Y() - units::meter_t{tags[i].transform[1][3]}));
+
+            return std::make_pair(distance, angle);
+        }
+    }
+
+    return std::make_pair(0_m, 0_deg);
+};
+
+std::vector<AprilTag> Vision::GetTags() {
+    return _fmap.GetTags();
+};
+
+frc::Pose2d Vision::AlignToTarget(VisionTarget target, wom::SwerveDrive* swerveDrive) {
+    std::pair<units::meter_t, units::degree_t> distance = GetDistanceToTarget(target);
+
+    units::meter_t x = distance.first * units::math::cos(distance.second);
+    units::meter_t y = distance.first * units::math::sin(distance.second);
+
+    frc::Pose2d pose = frc::Pose2d(x, y, distance.second);
+
+    swerveDrive->SetPose(pose);
+
+    return pose;
+};
\ No newline at end of file
diff --git a/src/main/deploy/fmap.fmap b/src/main/deploy/fmap.fmap
new file mode 100644
index 00000000..fa9e481b
--- /dev/null
+++ b/src/main/deploy/fmap.fmap
@@ -0,0 +1,388 @@
+{
+    "fiducials": [
+        {
+            "family": "apriltag3_36h11_classic",
+            "id": 1,
+            "size": 165.1,
+            "transform": [
+                -0.5,
+                -0.866025,
+                0,
+                6.808597,
+                0.866025,
+                -0.5,
+                0,
+                -3.859403,
+                0,
+                0,
+                1,
+                1.355852,
+                0,
+                0,
+                0,
+                1
+            ],
+            "unique": 1
+        },
+        {
+            "family": "apriltag3_36h11_classic",
+            "id": 2,
+            "size": 165.1,
+            "transform": [
+                -0.5,
+                -0.866025,
+                0,
+                7.914259,
+                0.866025,
+                -0.5,
+                0,
+                -3.221609,
+                0,
+                0,
+                1,
+                1.355852,
+                0,
+                0,
+                0,
+                1
+            ],
+            "unique": 1
+        },
+        {
+            "family": "apriltag3_36h11_classic",
+            "id": 3,
+            "size": 165.1,
+            "transform": [
+                -1.0,
+                0,
+                0,
+                8.308467,
+                0,
+                -1.0,
+                0,
+                0.877443,
+                0,
+                0,
+                1,
+                1.451102,
+                0,
+                0,
+                0,
+                1
+            ],
+            "unique": 1
+        },
+        {
+            "family": "apriltag3_36h11_classic",
+            "id": 4,
+            "size": 165.1,
+            "transform": [
+                -1.0,
+                0,
+                0,
+                8.308467,
+                0,
+                -1.0,
+                0,
+                1.442593,
+                0,
+                0,
+                1,
+                1.451102,
+                0,
+                0,
+                0,
+                1
+            ],
+            "unique": 1
+        },
+        {
+            "family": "apriltag3_36h11_classic",
+            "id": 5,
+            "size": 165.1,
+            "transform": [
+                0,
+                1.0,
+                0,
+                6.429883,
+                -1.0,
+                0,
+                0,
+                4.098925,
+                0,
+                0,
+                1,
+                1.355852,
+                0,
+                0,
+                0,
+                1
+            ],
+            "unique": 1
+        },
+        {
+            "family": "apriltag3_36h11_classic",
+            "id": 6,
+            "size": 165.1,
+            "transform": [
+                0,
+                1.0,
+                0,
+                -6.429375,
+                -1.0,
+                0,
+                0,
+                4.098925,
+                0,
+                0,
+                1,
+                1.355852,
+                0,
+                0,
+                0,
+                1
+            ],
+            "unique": 1
+        },
+        {
+            "family": "apriltag3_36h11_classic",
+            "id": 7,
+            "size": 165.1,
+            "transform": [
+                1.0,
+                -0.0,
+                0,
+                -8.308975,
+                0.0,
+                1.0,
+                0,
+                1.442593,
+                0,
+                0,
+                1,
+                1.451102,
+                0,
+                0,
+                0,
+                1
+            ],
+            "unique": 1
+        },
+        {
+            "family": "apriltag3_36h11_classic",
+            "id": 8,
+            "size": 165.1,
+            "transform": [
+                1.0,
+                -0.0,
+                0,
+                -8.308975,
+                0.0,
+                1.0,
+                0,
+                0.877443,
+                0,
+                0,
+                1,
+                1.451102,
+                0,
+                0,
+                0,
+                1
+            ],
+            "unique": 1
+        },
+        {
+            "family": "apriltag3_36h11_classic",
+            "id": 9,
+            "size": 165.1,
+            "transform": [
+                0.5,
+                -0.866025,
+                0,
+                -7.914767,
+                0.866025,
+                0.5,
+                0,
+                -3.221609,
+                0,
+                0,
+                1,
+                1.355852,
+                0,
+                0,
+                0,
+                1
+            ],
+            "unique": 1
+        },
+        {
+            "family": "apriltag3_36h11_classic",
+            "id": 10,
+            "size": 165.1,
+            "transform": [
+                0.5,
+                -0.866025,
+                0,
+                -6.809359,
+                0.866025,
+                0.5,
+                0,
+                -3.859403,
+                0,
+                0,
+                1,
+                1.355852,
+                0,
+                0,
+                0,
+                1
+            ],
+            "unique": 1
+        },
+        {
+            "family": "apriltag3_36h11_classic",
+            "id": 11,
+            "size": 165.1,
+            "transform": [
+                0.5,
+                0.8660254,
+                0,
+                3.633851,
+                -0.8660254,
+                0.5,
+                0,
+                -0.392049,
+                0,
+                0,
+                1,
+                1.3208,
+                0,
+                0,
+                0,
+                1
+            ],
+            "unique": 1
+        },
+        {
+            "family": "apriltag3_36h11_classic",
+            "id": 12,
+            "size": 165.1,
+            "transform": [
+                0.5,
+                -0.866025,
+                0,
+                3.633851,
+                0.866025,
+                0.5,
+                0,
+                0.393065,
+                0,
+                0,
+                1,
+                1.3208,
+                0,
+                0,
+                0,
+                1
+            ],
+            "unique": 1
+        },
+        {
+            "family": "apriltag3_36h11_classic",
+            "id": 13,
+            "size": 165.1,
+            "transform": [
+                -1.0,
+                0,
+                0,
+                2.949321,
+                0,
+                -1.0,
+                0,
+                -0.000127,
+                0,
+                0,
+                1,
+                1.3208,
+                0,
+                0,
+                0,
+                1
+            ],
+            "unique": 1
+        },
+        {
+            "family": "apriltag3_36h11_classic",
+            "id": 14,
+            "size": 165.1,
+            "transform": [
+                1.0,
+                -0.0,
+                0,
+                -2.950083,
+                0.0,
+                1.0,
+                0,
+                -0.000127,
+                0,
+                0,
+                1,
+                1.3208,
+                0,
+                0,
+                0,
+                1
+            ],
+            "unique": 1
+        },
+        {
+            "family": "apriltag3_36h11_classic",
+            "id": 15,
+            "size": 165.1,
+            "transform": [
+                -0.5,
+                -0.866025,
+                0,
+                -3.629533,
+                0.866025,
+                -0.5,
+                0,
+                0.393065,
+                0,
+                0,
+                1,
+                1.3208,
+                0,
+                0,
+                0,
+                1
+            ],
+            "unique": 1
+        },
+        {
+            "family": "apriltag3_36h11_classic",
+            "id": 16,
+            "size": 165.1,
+            "transform": [
+                -0.5,
+                0.866025,
+                0,
+                -3.629533,
+                -0.866025,
+                -0.5,
+                0,
+                -0.392049,
+                0,
+                0,
+                1,
+                1.3208,
+                0,
+                0,
+                0,
+                1
+            ],
+            "unique": 1
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/include/Robot.h b/src/main/include/Robot.h
index ae2bf634..f077f020 100644
--- a/src/main/include/Robot.h
+++ b/src/main/include/Robot.h
@@ -20,8 +20,12 @@
 #include "Intake.h"
 #include "IntakeBehaviour.h"
 #include "RobotMap.h"
+
 #include "Shooter.h"
 #include "ShooterBehaviour.h"
+
+#include "vision/Vision.h"
+
 #include "Wombat.h"
 
 class Robot : public frc::TimedRobot {
@@ -62,10 +66,18 @@ class Robot : public frc::TimedRobot {
   ctre::phoenix6::hardware::TalonFX* frontLeft;
   // AlphaArm *alphaArm;
 
+
   AlphaArm* alphaArm;
 
   // ctre::phoenix6::hardware::TalonFX *frontLeft;
   // ctre::phoenix6::hardware::TalonFX *frontRight;
   // ctre::phoenix6::hardware::TalonFX *backLeft;
   // ctre::phoenix6::hardware::TalonFX *backRight;
-};
\ No newline at end of file
+<<<<<<< HEAD
+};
+=======
+
+  Vision* _vision;
+
+};
+>>>>>>> c83ac05 ([robot/vision] Started work on limelight vision)
diff --git a/src/main/include/vision/Vision.h b/src/main/include/vision/Vision.h
new file mode 100644
index 00000000..7faeb30a
--- /dev/null
+++ b/src/main/include/vision/Vision.h
@@ -0,0 +1,56 @@
+#pragma once
+
+#include <units/length.h>
+#include <string>
+#include "Wombat.h"
+#include <frc/Filesystem.h>
+#include <wpi/fs.h>
+#include <wpi/json.h>
+#include <units/math.h>
+
+#define APRILTAGS_MAX 16
+#define APRILTAGS_MIN 0
+
+enum class VisionTarget {
+    kNote = 0,
+    kAmp = 1,
+    kSource = 2,
+    kNone = 3
+};
+
+struct AprilTag {
+    int id;
+    double size;
+    std::array<std::array<double, 4>, 4> transform;
+    bool unique;
+};
+
+class FMAP {
+    public:
+        FMAP(std::string path);
+
+        std::vector<AprilTag> GetTags();
+
+    private:
+        std::vector<AprilTag> _tags;
+        std::string _path;
+        std::string deploy_dir;
+};
+
+class Vision {
+    public:
+        Vision(std::string name, FMAP fmap);
+
+        std::pair<units::meter_t, units::degree_t> GetDistanceToTarget(VisionTarget target);
+        std::pair<units::meter_t, units::degree_t> GetDistanceToTarget(int id);
+
+        frc::Pose3d GetPose();
+
+        frc::Pose2d AlignToTarget(VisionTarget target, wom::SwerveDrive* swerveDrive);
+
+        std::vector<AprilTag> GetTags();
+
+    private:
+        wom::Limelight* _limelight;    
+        FMAP _fmap;
+};
\ No newline at end of file
diff --git a/wombat/src/main/cpp/vision/Limelight.cpp b/wombat/src/main/cpp/vision/Limelight.cpp
index 906f92cb..45bdcdc6 100644
--- a/wombat/src/main/cpp/vision/Limelight.cpp
+++ b/wombat/src/main/cpp/vision/Limelight.cpp
@@ -186,4 +186,4 @@ bool wom::vision::Limelight::IsAtSetPoseVision(frc::Pose3d pose, units::second_t
   frc::Pose3d relativePose = actualPose.RelativeTo(pose);
   return (units::math::fabs(relativePose.X()) < 0.01_m && units::math::fabs(relativePose.Y()) < 0.01_m &&
           GetSpeed(pose, GetPose(), dt) < 1_m / 1_s);
-}
+}
\ No newline at end of file

From d1aec64a2d4c7c7cd66fce330bc56bed92f39546 Mon Sep 17 00:00:00 2001
From: Paul Hodges <paul.hodges@student.education.wa.edu.au>
Date: Thu, 25 Jan 2024 14:50:18 +0800
Subject: [PATCH 139/207] [robot/vision] Finished vision (needs testing)

---
 src/main/cpp/vision/Vision.cpp             | 34 +++++++++++++++++
 src/main/include/vision/Vision.h           | 44 +++++++++++++++++++---
 wombat/src/main/cpp/vision/Limelight.cpp   |  4 ++
 wombat/src/main/include/vision/Limelight.h |  2 +
 4 files changed, 79 insertions(+), 5 deletions(-)

diff --git a/src/main/cpp/vision/Vision.cpp b/src/main/cpp/vision/Vision.cpp
index 4d3fee35..9a359b24 100644
--- a/src/main/cpp/vision/Vision.cpp
+++ b/src/main/cpp/vision/Vision.cpp
@@ -72,6 +72,8 @@ frc::Pose3d Vision::GetPose() {
 };
 
 std::pair<units::meter_t, units::degree_t> Vision::GetDistanceToTarget(VisionTarget target) {
+    SetMode(VisionModes::kAprilTag);
+
     std::vector<AprilTag> tags = _fmap.GetTags();
 
     frc::Pose3d pose = _limelight->GetPose();
@@ -94,6 +96,8 @@ std::pair<units::meter_t, units::degree_t> Vision::GetDistanceToTarget(VisionTar
 
 std::pair<units::meter_t, units::degree_t> Vision::GetDistanceToTarget(int id) {
     if (id < APRILTAGS_MIN || id > APRILTAGS_MAX) { return std::make_pair(0_m, 0_deg); };
+
+    SetMode(VisionModes::kAprilTag);
     
     std::vector<AprilTag> tags = _fmap.GetTags();
 
@@ -119,6 +123,8 @@ std::vector<AprilTag> Vision::GetTags() {
 };
 
 frc::Pose2d Vision::AlignToTarget(VisionTarget target, wom::SwerveDrive* swerveDrive) {
+    SetMode(VisionModes::kAprilTag);
+
     std::pair<units::meter_t, units::degree_t> distance = GetDistanceToTarget(target);
 
     units::meter_t x = distance.first * units::math::cos(distance.second);
@@ -129,4 +135,32 @@ frc::Pose2d Vision::AlignToTarget(VisionTarget target, wom::SwerveDrive* swerveD
     swerveDrive->SetPose(pose);
 
     return pose;
+};
+
+bool Vision::IsAtPose(frc::Pose3d pose, units::second_t dt) {
+    return _limelight->IsAtSetPoseVision(pose, dt);
+};
+
+void Vision::SetMode(VisionModes mode) {
+    switch (mode) {
+        case VisionModes::kAprilTag:
+        {
+            _limelight->SetPipeline(wom::LimelightPipeline::kPipeline0);
+        }
+        case VisionModes::kRing:
+        {
+            _limelight->SetPipeline(wom::LimelightPipeline::kPipeline1);
+        }
+    }
+};
+
+bool Vision::TargetIsVisible(VisionTargetObjects target) {
+    switch (target) {
+        case VisionTargetObjects::kNote:
+        {
+            SetMode(VisionModes::kRing);
+        }
+    }
+
+    return _limelight->HasTarget();
 };
\ No newline at end of file
diff --git a/src/main/include/vision/Vision.h b/src/main/include/vision/Vision.h
index 7faeb30a..9f3bebff 100644
--- a/src/main/include/vision/Vision.h
+++ b/src/main/include/vision/Vision.h
@@ -11,11 +11,38 @@
 #define APRILTAGS_MAX 16
 #define APRILTAGS_MIN 0
 
-enum class VisionTarget {
-    kNote = 0,
-    kAmp = 1,
-    kSource = 2,
-    kNone = 3
+enum class VisionTarget { 
+    /* 
+        Left is toward the blue side of the diagram here: https://firstfrc.blob.core.windows.net/frc2024/FieldAssets/2024LayoutMarkingDiagram.pdf 
+
+        The numbers are the numbers on the field diagram (and the numbers on the tags).
+    */
+    kBlueAmp = 6,
+    kBlueSpeakerCenter = 7,
+    kBlueSpeakerOffset = 8,
+    kBlueChain1 = 15,
+    kBlueChain2 = 16,
+    kBlueChain3 = 14,
+    kBlueSourceLeft = 1,
+    kBlueSourceRight = 2,
+
+    kRedAmp = 5,
+    kRedSpeakerCenter = 4,
+    kRedSpeakerOffset = 3,
+    kRedChain1 = 12,
+    kRedChain2 = 11,
+    kRedChain3 = 13,
+    kRedSourceLeft = 9,
+    kRedSourceRight = 10
+};
+
+enum class VisionModes {
+    kAprilTag = 0,
+    kRing = 1
+};
+
+enum class VisionTargetObjects {
+    kNote
 };
 
 struct AprilTag {
@@ -44,13 +71,20 @@ class Vision {
         std::pair<units::meter_t, units::degree_t> GetDistanceToTarget(VisionTarget target);
         std::pair<units::meter_t, units::degree_t> GetDistanceToTarget(int id);
 
+        void SetMode(VisionModes mode);
+
         frc::Pose3d GetPose();
 
         frc::Pose2d AlignToTarget(VisionTarget target, wom::SwerveDrive* swerveDrive);
 
         std::vector<AprilTag> GetTags();
 
+        bool IsAtPose(frc::Pose3d pose, units::second_t dt);
+
+        bool TargetIsVisible(VisionTargetObjects target);
+
     private:
         wom::Limelight* _limelight;    
         FMAP _fmap;
+        VisionModes _mode = VisionModes::kAprilTag;
 };
\ No newline at end of file
diff --git a/wombat/src/main/cpp/vision/Limelight.cpp b/wombat/src/main/cpp/vision/Limelight.cpp
index 45bdcdc6..f4ef274a 100644
--- a/wombat/src/main/cpp/vision/Limelight.cpp
+++ b/wombat/src/main/cpp/vision/Limelight.cpp
@@ -186,4 +186,8 @@ bool wom::vision::Limelight::IsAtSetPoseVision(frc::Pose3d pose, units::second_t
   frc::Pose3d relativePose = actualPose.RelativeTo(pose);
   return (units::math::fabs(relativePose.X()) < 0.01_m && units::math::fabs(relativePose.Y()) < 0.01_m &&
           GetSpeed(pose, GetPose(), dt) < 1_m / 1_s);
+}
+
+bool wom::vision::Limelight::HasTarget() {
+  return GetTargetingData(LimelightTargetingData::kTv) == 1.0;
 }
\ No newline at end of file
diff --git a/wombat/src/main/include/vision/Limelight.h b/wombat/src/main/include/vision/Limelight.h
index 5aa6d0b7..77b0c681 100644
--- a/wombat/src/main/include/vision/Limelight.h
+++ b/wombat/src/main/include/vision/Limelight.h
@@ -104,6 +104,8 @@ class Limelight {
 
   frc::Pose3d GetPose();
 
+  bool HasTarget();
+
  private:
   std::string _limelightName;
 };

From a2df62ce27dcce0e5795c315158a06670069e11e Mon Sep 17 00:00:00 2001
From: Paul Hodges <paul.hodges@student.education.wa.edu.au>
Date: Thu, 25 Jan 2024 15:04:56 +0800
Subject: [PATCH 140/207] [wpiformat] Run wpiformat

---
 src/main/cpp/Robot.cpp                   |   8 +
 src/main/cpp/vision/Vision.cpp           | 235 ++++++++++++-----------
 src/main/deploy/fmap.fmap                |   2 +-
 src/main/include/Robot.h                 |   2 +
 src/main/include/vision/Vision.h         | 124 ++++++------
 wombat/src/main/cpp/vision/Limelight.cpp |   2 +-
 6 files changed, 197 insertions(+), 176 deletions(-)

diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp
index 8ea94d4a..e2946017 100644
--- a/src/main/cpp/Robot.cpp
+++ b/src/main/cpp/Robot.cpp
@@ -139,6 +139,7 @@ void Robot::RobotInit() {
   // m_driveSim = new wom::TempSimSwerveDrive(&simulation_timer, &m_field);
   // m_driveSim = wom::TempSimSwerveDrive();
 
+<<<<<<< HEAD
   alphaArm = new AlphaArm(robotmap.alphaArmSystem.config);
   wom::BehaviourScheduler::GetInstance()->Register(alphaArm);
   alphaArm->SetDefaultBehaviour(
@@ -169,7 +170,12 @@ void Robot::RobotInit() {
 
   //_vision = new Vision("limelight", FMAP("fmap.fmap"));
 
+<<<<<<< HEAD
 >>>>>>> c83ac05 ([robot/vision] Started work on limelight vision)
+=======
+  _vision = new Vision("limelight", FMAP("fmap.fmap"));
+
+>>>>>>> c89d969 ([wpiformat] Run wpiformat)
 }
 
 void Robot::RobotPeriodic() {
@@ -248,6 +254,7 @@ void Robot::AutonomousPeriodic() {}
 
 void Robot::TeleopInit() {
 
+
   loop.Clear();
   wom::BehaviourScheduler* sched = wom::BehaviourScheduler::GetInstance();
 <<<<<<< HEAD
@@ -273,6 +280,7 @@ void Robot::TeleopInit() {
   
 
 //  FMAP("fmap.fmap");
+
   // _swerveDrive->OnStart();
   // sched->InterruptAll();
 
diff --git a/src/main/cpp/vision/Vision.cpp b/src/main/cpp/vision/Vision.cpp
index 9a359b24..2c7285c3 100644
--- a/src/main/cpp/vision/Vision.cpp
+++ b/src/main/cpp/vision/Vision.cpp
@@ -1,166 +1,173 @@
-#include "vision/Vision.h"
+// Copyright (c) 2023-2024 CurtinFRC
+// Open Source Software, you can modify it according to the terms
+// of the MIT License at the root of this project
 
+#include "vision/Vision.h"
 
 FMAP::FMAP(std::string path) : _path(path) {
-    std::cout << "Parsing FMAP" << std::endl;
-
-    deploy_dir = frc::filesystem::GetDeployDirectory();
-
-    std::ifstream file(deploy_dir + "/" + _path);
-
-    // parse the json file into a wpi::json object
-
-    wpi::json j;
-
-    file >> j;
-
-    // iterate through the json object and add each tag to the vector
-
-    for (auto& element : j["fiducials"]) {
-        AprilTag tag;
-
-        tag.id = element["id"];
-        tag.size = element["size"];
-        tag.transform[0][0] = element["transform"][0];
-        tag.transform[0][1] = element["transform"][1];
-        tag.transform[0][2] = element["transform"][2];
-        tag.transform[0][3] = element["transform"][3];
-        tag.transform[1][0] = element["transform"][4];
-        tag.transform[1][1] = element["transform"][5];
-        tag.transform[1][2] = element["transform"][6];
-        tag.transform[1][3] = element["transform"][7];
-        tag.transform[2][0] = element["transform"][8];
-        tag.transform[2][1] = element["transform"][9];
-        tag.transform[2][2] = element["transform"][10];
-        tag.transform[2][3] = element["transform"][11];
-        tag.transform[3][0] = element["transform"][12];
-        tag.transform[3][1] = element["transform"][13];
-        tag.transform[3][2] = element["transform"][14];
-        tag.transform[3][3] = element["transform"][15];
-        if (element["unique"] == 1) {
-            tag.unique = true;
-        } else {
-            tag.unique = false;
-        }
-
-        _tags.push_back(tag);
+  std::cout << "Parsing FMAP" << std::endl;
+
+  deploy_dir = frc::filesystem::GetDeployDirectory();
+
+  std::ifstream file(deploy_dir + "/" + _path);
+
+  // parse the json file into a wpi::json object
+
+  wpi::json j;
+
+  file >> j;
+
+  // iterate through the json object and add each tag to the vector
+
+  for (auto& element : j["fiducials"]) {
+    AprilTag tag;
+
+    tag.id = element["id"];
+    tag.size = element["size"];
+    tag.transform[0][0] = element["transform"][0];
+    tag.transform[0][1] = element["transform"][1];
+    tag.transform[0][2] = element["transform"][2];
+    tag.transform[0][3] = element["transform"][3];
+    tag.transform[1][0] = element["transform"][4];
+    tag.transform[1][1] = element["transform"][5];
+    tag.transform[1][2] = element["transform"][6];
+    tag.transform[1][3] = element["transform"][7];
+    tag.transform[2][0] = element["transform"][8];
+    tag.transform[2][1] = element["transform"][9];
+    tag.transform[2][2] = element["transform"][10];
+    tag.transform[2][3] = element["transform"][11];
+    tag.transform[3][0] = element["transform"][12];
+    tag.transform[3][1] = element["transform"][13];
+    tag.transform[3][2] = element["transform"][14];
+    tag.transform[3][3] = element["transform"][15];
+    if (element["unique"] == 1) {
+      tag.unique = true;
+    } else {
+      tag.unique = false;
     }
 
-    file.close();
+    _tags.push_back(tag);
+  }
 
-    std::cout << "Loaded " << _tags.size() << " tags" << std::endl;
+  file.close();
 
-    for (int i = 0; i < _tags.size(); i++) {
-        std::cout << "Tag " << _tags[i].id << " is " << _tags[i].size << "m" << std::endl;
-    }
+  std::cout << "Loaded " << _tags.size() << " tags" << std::endl;
 
-    std::cout << "Loaded FMAP" << std::endl;
-};
+  for (int i = 0; i < _tags.size(); i++) {
+    std::cout << "Tag " << _tags[i].id << " is " << _tags[i].size << "m" << std::endl;
+  }
+
+  std::cout << "Loaded FMAP" << std::endl;
+}
 
 std::vector<AprilTag> FMAP::GetTags() {
-    return _tags;
-};
+  return _tags;
+}
 
 Vision::Vision(std::string name, FMAP fmap) : _fmap(fmap) {
-    _limelight = new wom::Limelight(name);
+  _limelight = new wom::Limelight(name);
 
-    _limelight->SetPipeline(wom::LimelightPipeline::kPipeline0);
-};
+  _limelight->SetPipeline(wom::LimelightPipeline::kPipeline0);
+}
 
 frc::Pose3d Vision::GetPose() {
-    return _limelight->GetPose();
-};
+  return _limelight->GetPose();
+}
 
 std::pair<units::meter_t, units::degree_t> Vision::GetDistanceToTarget(VisionTarget target) {
-    SetMode(VisionModes::kAprilTag);
+  SetMode(VisionModes::kAprilTag);
 
-    std::vector<AprilTag> tags = _fmap.GetTags();
+  std::vector<AprilTag> tags = _fmap.GetTags();
 
-    frc::Pose3d pose = _limelight->GetPose();
+  frc::Pose3d pose = _limelight->GetPose();
 
-    for (int i = 0; i < tags.size(); i++) {
-        if (tags[i].id == (int)target) {         
+  for (int i = 0; i < tags.size(); i++) {
+    if (tags[i].id == static_cast<int>(target)) {
+      frc::Pose3d aprilTagPos =
+          frc::Pose3d(tags[i].transform[0][3] * 1_m, tags[i].transform[1][3] * 1_m,
+                      tags[i].transform[2][3] * 1_m, frc::Rotation3d(0_deg, 0_deg, 0_deg));
 
-            frc::Pose3d aprilTagPos = frc::Pose3d(tags[i].transform[0][3] * 1_m, tags[i].transform[1][3] * 1_m, tags[i].transform[2][3] * 1_m, frc::Rotation3d(0_deg, 0_deg, 0_deg));   
-            
-            units::meter_t distance = pose.Translation().Distance(aprilTagPos.Translation());
+      units::meter_t distance = pose.Translation().Distance(aprilTagPos.Translation());
 
-            units::degree_t angle = units::math::atan((pose.X() - units::meter_t{tags[i].transform[0][3]}) / (pose.Y() - units::meter_t{tags[i].transform[1][3]}));
+      units::degree_t angle = units::math::atan((pose.X() - units::meter_t{tags[i].transform[0][3]}) /
+                                                (pose.Y() - units::meter_t{tags[i].transform[1][3]}));
 
-            return std::make_pair(distance, angle);
-        }
+      return std::make_pair(distance, angle);
     }
+  }
 
-    return std::make_pair(0_m, 0_deg);
-};
+  return std::make_pair(0_m, 0_deg);
+}
 
 std::pair<units::meter_t, units::degree_t> Vision::GetDistanceToTarget(int id) {
-    if (id < APRILTAGS_MIN || id > APRILTAGS_MAX) { return std::make_pair(0_m, 0_deg); };
+  if (id < APRILTAGS_MIN || id > APRILTAGS_MAX) {
+    return std::make_pair(0_m, 0_deg);
+  }
+
+  SetMode(VisionModes::kAprilTag);
 
-    SetMode(VisionModes::kAprilTag);
-    
-    std::vector<AprilTag> tags = _fmap.GetTags();
+  std::vector<AprilTag> tags = _fmap.GetTags();
 
-    for (int i = 0; i < tags.size(); i++) {
-        if (tags[i].id == id) {
-            // get distance to the limelight
-            frc::Pose3d currentPose = _limelight->GetPose();
-            frc::Pose3d aprilTagPos = frc::Pose3d(tags[i].transform[0][3] * 1_m, tags[i].transform[1][3] * 1_m, tags[i].transform[2][3] * 1_m, frc::Rotation3d(0_deg, 0_deg, 0_deg));
+  for (int i = 0; i < tags.size(); i++) {
+    if (tags[i].id == id) {
+      // get distance to the limelight
+      frc::Pose3d currentPose = _limelight->GetPose();
+      frc::Pose3d aprilTagPos =
+          frc::Pose3d(tags[i].transform[0][3] * 1_m, tags[i].transform[1][3] * 1_m,
+                      tags[i].transform[2][3] * 1_m, frc::Rotation3d(0_deg, 0_deg, 0_deg));
 
-            units::meter_t distance = currentPose.Translation().Distance(aprilTagPos.Translation());
+      units::meter_t distance = currentPose.Translation().Distance(aprilTagPos.Translation());
 
-            units::degree_t angle = units::math::atan((currentPose.X() - units::meter_t{tags[i].transform[0][3]}) / (currentPose.Y() - units::meter_t{tags[i].transform[1][3]}));
+      units::degree_t angle = units::math::atan((currentPose.X() - units::meter_t{tags[i].transform[0][3]}) /
+                                                (currentPose.Y() - units::meter_t{tags[i].transform[1][3]}));
 
-            return std::make_pair(distance, angle);
-        }
+      return std::make_pair(distance, angle);
     }
+  }
 
-    return std::make_pair(0_m, 0_deg);
-};
+  return std::make_pair(0_m, 0_deg);
+}
 
 std::vector<AprilTag> Vision::GetTags() {
-    return _fmap.GetTags();
-};
+  return _fmap.GetTags();
+}
 
 frc::Pose2d Vision::AlignToTarget(VisionTarget target, wom::SwerveDrive* swerveDrive) {
-    SetMode(VisionModes::kAprilTag);
+  SetMode(VisionModes::kAprilTag);
 
-    std::pair<units::meter_t, units::degree_t> distance = GetDistanceToTarget(target);
+  std::pair<units::meter_t, units::degree_t> distance = GetDistanceToTarget(target);
 
-    units::meter_t x = distance.first * units::math::cos(distance.second);
-    units::meter_t y = distance.first * units::math::sin(distance.second);
+  units::meter_t x = distance.first * units::math::cos(distance.second);
+  units::meter_t y = distance.first * units::math::sin(distance.second);
 
-    frc::Pose2d pose = frc::Pose2d(x, y, distance.second);
+  frc::Pose2d pose = frc::Pose2d(x, y, distance.second);
 
-    swerveDrive->SetPose(pose);
+  swerveDrive->SetPose(pose);
 
-    return pose;
-};
+  return pose;
+}
 
 bool Vision::IsAtPose(frc::Pose3d pose, units::second_t dt) {
-    return _limelight->IsAtSetPoseVision(pose, dt);
-};
+  return _limelight->IsAtSetPoseVision(pose, dt);
+}
 
 void Vision::SetMode(VisionModes mode) {
-    switch (mode) {
-        case VisionModes::kAprilTag:
-        {
-            _limelight->SetPipeline(wom::LimelightPipeline::kPipeline0);
-        }
-        case VisionModes::kRing:
-        {
-            _limelight->SetPipeline(wom::LimelightPipeline::kPipeline1);
-        }
+  switch (mode) {
+    case VisionModes::kAprilTag: {
+      _limelight->SetPipeline(wom::LimelightPipeline::kPipeline0);
+    }
+    case VisionModes::kRing: {
+      _limelight->SetPipeline(wom::LimelightPipeline::kPipeline1);
     }
-};
+  }
+}
 
 bool Vision::TargetIsVisible(VisionTargetObjects target) {
-    switch (target) {
-        case VisionTargetObjects::kNote:
-        {
-            SetMode(VisionModes::kRing);
-        }
+  switch (target) {
+    case VisionTargetObjects::kNote: {
+      SetMode(VisionModes::kRing);
     }
+  }
 
-    return _limelight->HasTarget();
-};
\ No newline at end of file
+  return _limelight->HasTarget();
+}
diff --git a/src/main/deploy/fmap.fmap b/src/main/deploy/fmap.fmap
index fa9e481b..9cb43e6d 100644
--- a/src/main/deploy/fmap.fmap
+++ b/src/main/deploy/fmap.fmap
@@ -385,4 +385,4 @@
             "unique": 1
         }
     ]
-}
\ No newline at end of file
+}
diff --git a/src/main/include/Robot.h b/src/main/include/Robot.h
index f077f020..ca1a5096 100644
--- a/src/main/include/Robot.h
+++ b/src/main/include/Robot.h
@@ -21,6 +21,7 @@
 #include "IntakeBehaviour.h"
 #include "RobotMap.h"
 
+
 #include "Shooter.h"
 #include "ShooterBehaviour.h"
 
@@ -28,6 +29,7 @@
 
 #include "Wombat.h"
 
+
 class Robot : public frc::TimedRobot {
  public:
   void TestInit() override;
diff --git a/src/main/include/vision/Vision.h b/src/main/include/vision/Vision.h
index 9f3bebff..2271a7ed 100644
--- a/src/main/include/vision/Vision.h
+++ b/src/main/include/vision/Vision.h
@@ -1,90 +1,94 @@
+// Copyright (c) 2023-2024 CurtinFRC
+// Open Source Software, you can modify it according to the terms
+// of the MIT License at the root of this project
+
 #pragma once
 
-#include <units/length.h>
-#include <string>
-#include "Wombat.h"
 #include <frc/Filesystem.h>
+#include <units/length.h>
+#include <units/math.h>
 #include <wpi/fs.h>
 #include <wpi/json.h>
-#include <units/math.h>
+
+#include <string>
+#include <utility>
+#include <vector>
+
+#include "Wombat.h"
 
 #define APRILTAGS_MAX 16
 #define APRILTAGS_MIN 0
 
-enum class VisionTarget { 
-    /* 
-        Left is toward the blue side of the diagram here: https://firstfrc.blob.core.windows.net/frc2024/FieldAssets/2024LayoutMarkingDiagram.pdf 
-
-        The numbers are the numbers on the field diagram (and the numbers on the tags).
-    */
-    kBlueAmp = 6,
-    kBlueSpeakerCenter = 7,
-    kBlueSpeakerOffset = 8,
-    kBlueChain1 = 15,
-    kBlueChain2 = 16,
-    kBlueChain3 = 14,
-    kBlueSourceLeft = 1,
-    kBlueSourceRight = 2,
-
-    kRedAmp = 5,
-    kRedSpeakerCenter = 4,
-    kRedSpeakerOffset = 3,
-    kRedChain1 = 12,
-    kRedChain2 = 11,
-    kRedChain3 = 13,
-    kRedSourceLeft = 9,
-    kRedSourceRight = 10
+enum class VisionTarget {
+  /*
+      Left is toward the blue side of the diagram here:
+     https://firstfrc.blob.core.windows.net/frc2024/FieldAssets/2024LayoutMarkingDiagram.pdf
+
+      The numbers are the numbers on the field diagram (and the numbers on the tags).
+  */
+  kBlueAmp = 6,
+  kBlueSpeakerCenter = 7,
+  kBlueSpeakerOffset = 8,
+  kBlueChain1 = 15,
+  kBlueChain2 = 16,
+  kBlueChain3 = 14,
+  kBlueSourceLeft = 1,
+  kBlueSourceRight = 2,
+
+  kRedAmp = 5,
+  kRedSpeakerCenter = 4,
+  kRedSpeakerOffset = 3,
+  kRedChain1 = 12,
+  kRedChain2 = 11,
+  kRedChain3 = 13,
+  kRedSourceLeft = 9,
+  kRedSourceRight = 10
 };
 
-enum class VisionModes {
-    kAprilTag = 0,
-    kRing = 1
-};
+enum class VisionModes { kAprilTag = 0, kRing = 1 };
 
-enum class VisionTargetObjects {
-    kNote
-};
+enum class VisionTargetObjects { kNote };
 
 struct AprilTag {
-    int id;
-    double size;
-    std::array<std::array<double, 4>, 4> transform;
-    bool unique;
+  int id;
+  double size;
+  std::array<std::array<double, 4>, 4> transform;
+  bool unique;
 };
 
 class FMAP {
-    public:
-        FMAP(std::string path);
+ public:
+  explicit FMAP(std::string path);
 
-        std::vector<AprilTag> GetTags();
+  std::vector<AprilTag> GetTags();
 
-    private:
-        std::vector<AprilTag> _tags;
-        std::string _path;
-        std::string deploy_dir;
+ private:
+  std::vector<AprilTag> _tags;
+  std::string _path;
+  std::string deploy_dir;
 };
 
 class Vision {
-    public:
-        Vision(std::string name, FMAP fmap);
+ public:
+  Vision(std::string name, FMAP fmap);
 
-        std::pair<units::meter_t, units::degree_t> GetDistanceToTarget(VisionTarget target);
-        std::pair<units::meter_t, units::degree_t> GetDistanceToTarget(int id);
+  std::pair<units::meter_t, units::degree_t> GetDistanceToTarget(VisionTarget target);
+  std::pair<units::meter_t, units::degree_t> GetDistanceToTarget(int id);
 
-        void SetMode(VisionModes mode);
+  void SetMode(VisionModes mode);
 
-        frc::Pose3d GetPose();
+  frc::Pose3d GetPose();
 
-        frc::Pose2d AlignToTarget(VisionTarget target, wom::SwerveDrive* swerveDrive);
+  frc::Pose2d AlignToTarget(VisionTarget target, wom::SwerveDrive* swerveDrive);
 
-        std::vector<AprilTag> GetTags();
+  std::vector<AprilTag> GetTags();
 
-        bool IsAtPose(frc::Pose3d pose, units::second_t dt);
+  bool IsAtPose(frc::Pose3d pose, units::second_t dt);
 
-        bool TargetIsVisible(VisionTargetObjects target);
+  bool TargetIsVisible(VisionTargetObjects target);
 
-    private:
-        wom::Limelight* _limelight;    
-        FMAP _fmap;
-        VisionModes _mode = VisionModes::kAprilTag;
-};
\ No newline at end of file
+ private:
+  wom::Limelight* _limelight;
+  FMAP _fmap;
+  VisionModes _mode = VisionModes::kAprilTag;
+};
diff --git a/wombat/src/main/cpp/vision/Limelight.cpp b/wombat/src/main/cpp/vision/Limelight.cpp
index f4ef274a..2b37f22b 100644
--- a/wombat/src/main/cpp/vision/Limelight.cpp
+++ b/wombat/src/main/cpp/vision/Limelight.cpp
@@ -190,4 +190,4 @@ bool wom::vision::Limelight::IsAtSetPoseVision(frc::Pose3d pose, units::second_t
 
 bool wom::vision::Limelight::HasTarget() {
   return GetTargetingData(LimelightTargetingData::kTv) == 1.0;
-}
\ No newline at end of file
+}

From ba3fc0ef0d1f889bdcb2a6bb1349a1d3df6f3a51 Mon Sep 17 00:00:00 2001
From: Paul Hodges <paul.hodges@student.education.wa.edu.au>
Date: Thu, 25 Jan 2024 15:49:31 +0800
Subject: [PATCH 141/207] [robot/vision] Finish vision

---
 src/main/cpp/vision/Vision.cpp   | 7 +++++++
 src/main/include/vision/Vision.h | 2 ++
 2 files changed, 9 insertions(+)

diff --git a/src/main/cpp/vision/Vision.cpp b/src/main/cpp/vision/Vision.cpp
index 2c7285c3..f726c9b1 100644
--- a/src/main/cpp/vision/Vision.cpp
+++ b/src/main/cpp/vision/Vision.cpp
@@ -155,9 +155,11 @@ void Vision::SetMode(VisionModes mode) {
   switch (mode) {
     case VisionModes::kAprilTag: {
       _limelight->SetPipeline(wom::LimelightPipeline::kPipeline0);
+      break;
     }
     case VisionModes::kRing: {
       _limelight->SetPipeline(wom::LimelightPipeline::kPipeline1);
+      break;
     }
   }
 }
@@ -166,8 +168,13 @@ bool Vision::TargetIsVisible(VisionTargetObjects target) {
   switch (target) {
     case VisionTargetObjects::kNote: {
       SetMode(VisionModes::kRing);
+      break;
     }
   }
 
   return _limelight->HasTarget();
 }
+
+int Vision::CurrentAprilTag() {
+  return _limelight->GetAprilTagData(wom::LimelightAprilTagData::kTid)[0];
+}
diff --git a/src/main/include/vision/Vision.h b/src/main/include/vision/Vision.h
index 2271a7ed..5629cc81 100644
--- a/src/main/include/vision/Vision.h
+++ b/src/main/include/vision/Vision.h
@@ -87,6 +87,8 @@ class Vision {
 
   bool TargetIsVisible(VisionTargetObjects target);
 
+  int CurrentAprilTag();
+
  private:
   wom::Limelight* _limelight;
   FMAP _fmap;

From 255c0634897265c8285085120e967a103f2bdb81 Mon Sep 17 00:00:00 2001
From: Paul Hodges <paul.hodges@student.education.wa.edu.au>
Date: Sun, 28 Jan 2024 09:26:05 +0800
Subject: [PATCH 142/207] Updated vision

---
 src/main/cpp/Robot.cpp                     |  1 -
 src/main/cpp/vision/Vision.cpp             |  4 +++-
 src/main/include/vision/Vision.h           |  1 -
 wombat/src/main/cpp/vision/Limelight.cpp   | 12 ++++++------
 wombat/src/main/include/vision/Limelight.h |  2 +-
 5 files changed, 10 insertions(+), 10 deletions(-)

diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp
index e2946017..230607f1 100644
--- a/src/main/cpp/Robot.cpp
+++ b/src/main/cpp/Robot.cpp
@@ -139,7 +139,6 @@ void Robot::RobotInit() {
   // m_driveSim = new wom::TempSimSwerveDrive(&simulation_timer, &m_field);
   // m_driveSim = wom::TempSimSwerveDrive();
 
-<<<<<<< HEAD
   alphaArm = new AlphaArm(robotmap.alphaArmSystem.config);
   wom::BehaviourScheduler::GetInstance()->Register(alphaArm);
   alphaArm->SetDefaultBehaviour(
diff --git a/src/main/cpp/vision/Vision.cpp b/src/main/cpp/vision/Vision.cpp
index f726c9b1..a06236fc 100644
--- a/src/main/cpp/vision/Vision.cpp
+++ b/src/main/cpp/vision/Vision.cpp
@@ -176,5 +176,7 @@ bool Vision::TargetIsVisible(VisionTargetObjects target) {
 }
 
 int Vision::CurrentAprilTag() {
-  return _limelight->GetAprilTagData(wom::LimelightAprilTagData::kTid)[0];
+  SetMode(VisionModes::kAprilTag);
+
+  return _limelight->GetTargetingData(wom::LimelightTargetingData::kTid);
 }
diff --git a/src/main/include/vision/Vision.h b/src/main/include/vision/Vision.h
index 5629cc81..8176b2a2 100644
--- a/src/main/include/vision/Vision.h
+++ b/src/main/include/vision/Vision.h
@@ -92,5 +92,4 @@ class Vision {
  private:
   wom::Limelight* _limelight;
   FMAP _fmap;
-  VisionModes _mode = VisionModes::kAprilTag;
 };
diff --git a/wombat/src/main/cpp/vision/Limelight.cpp b/wombat/src/main/cpp/vision/Limelight.cpp
index 2b37f22b..ed62337e 100644
--- a/wombat/src/main/cpp/vision/Limelight.cpp
+++ b/wombat/src/main/cpp/vision/Limelight.cpp
@@ -58,13 +58,9 @@ std::vector<double> wom::vision::Limelight::GetAprilTagData(LimelightAprilTagDat
     case LimelightAprilTagData::kCamerapose_robotspace:
       dataName = "camerapose_robotspace";
       break;
-
-    case LimelightAprilTagData::kTid:
-      dataName = "tid";
-      break;
   }
 
-  return table->GetNumberArray(dataName, std::vector<double>(6));
+  return table->GetEntry(dataName).GetDoubleArray(std::vector<double>(6));
 }
 
 double wom::vision::Limelight::GetTargetingData(LimelightTargetingData dataType, double defaultValue) {
@@ -126,9 +122,13 @@ double wom::vision::Limelight::GetTargetingData(LimelightTargetingData dataType,
     case LimelightTargetingData::kTc:
       dataName = "tc";
       break;
+
+    case LimelightTargetingData::kTid:
+      dataName = "tid";
+      break;
   }
 
-  return table->GetNumber(dataName, defaultValue);
+  return table->GetEntry(dataName).GetDouble(0);
 }
 
 void wom::vision::Limelight::SetLEDMode(LimelightLEDMode mode) {
diff --git a/wombat/src/main/include/vision/Limelight.h b/wombat/src/main/include/vision/Limelight.h
index 77b0c681..8c21efc3 100644
--- a/wombat/src/main/include/vision/Limelight.h
+++ b/wombat/src/main/include/vision/Limelight.h
@@ -62,6 +62,7 @@ enum class LimelightTargetingData {
   kJson = 11,
   kTclass = 12,
   kTc = 13,
+  kTid = 14
 };
 
 enum class LimelightAprilTagData {
@@ -73,7 +74,6 @@ enum class LimelightAprilTagData {
   kTargetpose_robotspace = 5,
   kBotpose_targetspace = 6,
   kCamerapose_robotspace = 7,
-  kTid = 8
 };
 
 class Limelight {

From 6250586239f8db43433fb00b48179755ec4d2a41 Mon Sep 17 00:00:00 2001
From: Paul Hodges <paul.hodges@student.education.wa.edu.au>
Date: Mon, 29 Jan 2024 10:25:46 +0800
Subject: [PATCH 143/207] More math stuff

---
 ctre_sim/CANCoder vers. H - 016 - 0 - ext.dat | Bin 0 -> 2048 bytes
 ctre_sim/CANCoder vers. H - 017 - 0 - ext.dat | Bin 0 -> 2048 bytes
 ctre_sim/CANCoder vers. H - 018 - 0 - ext.dat | Bin 0 -> 2048 bytes
 ctre_sim/CANCoder vers. H - 019 - 0 - ext.dat | Bin 0 -> 2048 bytes
 ctre_sim/Pigeon 2 - 020 - 0 - ext.dat         | Bin 0 -> 2048 bytes
 ctre_sim/Talon FX vers. C - 01 - 0 - ext.dat  | Bin 0 -> 2048 bytes
 ctre_sim/Talon FX vers. C - 02 - 0 - ext.dat  | Bin 0 -> 2048 bytes
 ctre_sim/Talon FX vers. C - 03 - 0 - ext.dat  | Bin 0 -> 2048 bytes
 ctre_sim/Talon FX vers. C - 04 - 0 - ext.dat  | Bin 0 -> 2048 bytes
 ctre_sim/Talon FX vers. C - 05 - 0 - ext.dat  | Bin 0 -> 2048 bytes
 ctre_sim/Talon FX vers. C - 06 - 0 - ext.dat  | Bin 0 -> 2048 bytes
 ctre_sim/Talon FX vers. C - 07 - 0 - ext.dat  | Bin 0 -> 2048 bytes
 ctre_sim/Talon FX vers. C - 09 - 0 - ext.dat  | Bin 0 -> 2048 bytes
 networktables.json                            |   1 +
 simgui-ds.json                                |  92 ++++++++
 simgui.json                                   |  23 ++
 src/main/cpp/Robot.cpp                        |  10 +
 src/main/cpp/vision/Vision.cpp                | 205 +++++++++++++-----
 src/main/include/vision/Vision.h              |  11 +-
 wombat/src/main/cpp/utils/Encoder.cpp         |   2 +-
 20 files changed, 293 insertions(+), 51 deletions(-)
 create mode 100644 ctre_sim/CANCoder vers. H - 016 - 0 - ext.dat
 create mode 100644 ctre_sim/CANCoder vers. H - 017 - 0 - ext.dat
 create mode 100644 ctre_sim/CANCoder vers. H - 018 - 0 - ext.dat
 create mode 100644 ctre_sim/CANCoder vers. H - 019 - 0 - ext.dat
 create mode 100644 ctre_sim/Pigeon 2 - 020 - 0 - ext.dat
 create mode 100644 ctre_sim/Talon FX vers. C - 01 - 0 - ext.dat
 create mode 100644 ctre_sim/Talon FX vers. C - 02 - 0 - ext.dat
 create mode 100644 ctre_sim/Talon FX vers. C - 03 - 0 - ext.dat
 create mode 100644 ctre_sim/Talon FX vers. C - 04 - 0 - ext.dat
 create mode 100644 ctre_sim/Talon FX vers. C - 05 - 0 - ext.dat
 create mode 100644 ctre_sim/Talon FX vers. C - 06 - 0 - ext.dat
 create mode 100644 ctre_sim/Talon FX vers. C - 07 - 0 - ext.dat
 create mode 100644 ctre_sim/Talon FX vers. C - 09 - 0 - ext.dat
 create mode 100644 networktables.json
 create mode 100644 simgui-ds.json
 create mode 100644 simgui.json

diff --git a/ctre_sim/CANCoder vers. H - 016 - 0 - ext.dat b/ctre_sim/CANCoder vers. H - 016 - 0 - ext.dat
new file mode 100644
index 0000000000000000000000000000000000000000..8f8bccdea82f511e5af738d8fa0b4b911c8bdd34
GIT binary patch
literal 2048
zcmeHIJ!n%=6h1GhnpB4j78R8?5i7cNY9{+Eh(*x8;!?L@21QYvq>9yw6yl&17ZsNb
zDp~|9>Jn65X{=C?DoAU#*e>cI5!6~5`n>0!^My1uTSo74@}2XY@BG}$%Q$rf^#3$)
z12cgWB0Bo(Qu{}v`idsX0|rMLRN*B}md`a%@3r<-yfJi*`yN+3wPl~hv|b$LPMOPd
z#3GM#<a7$(fXnJxGCyKX0xqb}S6$a33e53aq|TUHQa*S?n)m3p>@(3J^?%m%6H^`=
zBTWYtH>+SQtd3TxH~Qg0ANMymOvgDATPtxhPKal62Yq{w=x2KGsO9H~avb;fOWfZe
z_WP!Z1mAZ)EEsxmXUYnrUfjuBJR;?&l*!n=)x%7)JKn1O)Zlzb{K!W?wK${oU=Rn6
z;)mLTzF#8QckZq1%S<kNF+AR1weG)g<#{L{xG*4kz$mZR54g%Mn>WK(=|+mS53f%s
zJ<*l;@veJ2xF4Lkrw{G@op-)nWuLdbVDnuibRTH4t$3>QevRm57v$Z2si{}3&-adl
z{GN8HkNrSzG5TcZuxt8nncv3!WmWgno~35Sx7SJCl=gx8$Tz9P^^lKwcqf=QY$ib*
z`q^jK+c=K74c(qvTzYfIzDMi>`miP`C7DKR^G5t!Wlzk^uDGA-eYu9+KU+VpVkx$}
zBeE~-vy|?&{Uzf?yT7Q$@qS~PoQ(Ik^1R@~@BNYc9MS&Zr$zBkFC3Y_e>dOu{G#L|
z&X)Cn!FRH^oHLVB^w6<XmzJkfS2}ueAM6`TBrD%XaM+fD!`7ef{%q&L|HRqTQos04
hzr&iu-^?H&x|tTfYyA9uIQdnu(ki<3@QWY9{|lcIH_ZS5

literal 0
HcmV?d00001

diff --git a/ctre_sim/CANCoder vers. H - 017 - 0 - ext.dat b/ctre_sim/CANCoder vers. H - 017 - 0 - ext.dat
new file mode 100644
index 0000000000000000000000000000000000000000..2596b7a4155a130d6f3b83ca0a542ed4c02564b7
GIT binary patch
literal 2048
zcmeHHJ!q3*5WX*IHK`5}ELK#~L~T(QJ2ivoXF)73O>t?#A&@~)1ShTHPeclFP=bR*
zTr#L=5n6wyD3W4CC`uHhZMN7h#X&NtwKN#td+(l-q;|{bE%!a|-97jBe(xk5s?vXH
z;smCvj)!RY=hdz+hV>N{D(4Mumb|79m60a$z0tx-w-YzH?n%W{TMt`I8|0wUD}6;B
z(TL+1kzVmP;F_K+n~y+?fJ=IoQ(o60sv60)OP&eUr2g<rXxx)uGS6_2<p0r7PeA=x
z8EQJFxJ?NIUOfm(zOm1b2D!euQ98+%z&f#;c0AmZdnIT0h<XYK$E<&rD9iTT5wYhs
zivGUkZJ76+_X-AI$eFUl$QN?*7N;Z~m6J(3x3w_4*%@!sdKz%vBj<=mJ&ic2`JiA2
z_Txi!QSUF2%scyD=4CpUxfnm5UoG#y@XGa2f8gSX@Bt&et{-rf{$uV8uhR9EY#q@-
zaq0Kh<EPyZc5*$Cxo3~<{w_M-Ztz)jN72T+>d<$z#g-CPo=>g9msym1x6qKUmY??y
zL41Fg<i~u#x8xt+J?fhJ+vm4)eHrDQ-MiezIK5u-#<ULPN4yCo&WCvP!!tp@UegKe
z;Lp6c)ya1BZQQLi;_|zD_C8`B;D<IrF^M!>9h>pFN}rmZsoPKOyj<fwG_$r{$zrVc
z_+(y~XE{D#>x+a+c7A@f<Nj{a;7DjF$o;|}=h5G?&y?1Ob6S$~!s7AyhxhZHFE2|x
z?5vp&82*X7X`69;pX|FsdCpw#8Nfc+VImw^c^?^{xFGze7j|{G8XWKYYWwlA(a5>8
sV&9)WP#4E<X0=LmCocZ3@#XgceEjxVNeu^U^W#4&eU$g_`K<8&1wRKl;s5{u

literal 0
HcmV?d00001

diff --git a/ctre_sim/CANCoder vers. H - 018 - 0 - ext.dat b/ctre_sim/CANCoder vers. H - 018 - 0 - ext.dat
new file mode 100644
index 0000000000000000000000000000000000000000..d7704d30ca6103103cb4b7d2b8c0d3f59947ac95
GIT binary patch
literal 2048
zcmeHIJ!n%=6uvKMHK`6kTCAv~iQ1wrc4`LEXFx12eZ{2(hd>4e5uCIZe<D(dgAyDh
z;*vo{i_rQr1(jDC6*^c2X__szi#SLIYb^~v@44rEB}wg;(Yu`YouBXg-P}y6qn`SI
znmB<u&+;iXymYPmi)MYJrmG_w2bz@OwR&H@*hId!Qdjv->K4^KC3t4XF@x0>R#f}A
zFUMkuIEvxZ$Nm~z7tN~v2(>U66`d=*wxyJ(kK4g{CPgaohhIwKp8n!_MteE`&xU$J
z;zvSLtCND;gfQe6MVRxAe||hj^(~C4Gh_*EV!Lj~M?KkBU9(5jGkav*_~(_%lifYe
zc6TewAKRy+wC{qSG58|Zj3Gw8h*dB+!|7Bdoi=mZ2$P$c@lL6y0T+C%M?C6j#A(R~
z0XuLIA0kU~f0g2S=Rfegbmu%5?ML&g<^304yB^{XTpD3MV5FDz1Fn;Q^qrwq+JUmE
z!x?0k{$M?R*7I-=)q}I}{E6A$W$XJ*(mA_JCf*i@_=y%%iX%LqTA44m%=hkFL%v#m
z+B**7`@1<m<^#T!;KaT$Ti4&QxQpt`3Ge)Y)i%P}&73zObs#_Db*Q3zh(|v(6ZGrX
zoxl$M+{@ctWJllH-JC{Td3WF3N6Z8KkUA*Ak(#Ts9iQvu*|}ofero4sYww}r#&#h~
zklY{Od10QF<bbI!7Ac$g1;vj0yG?>)k)bg43xDXH-@MO^)Cav<Vg2m#sl`VR3SF<R
za6IfJi4Pe5se4(IapEBFyHj}1-RK>_KFDD*8e4lG37@{i{KcicJ*^ri<6lib_@B6N
tp6$-ZcwHR5nVzTA-6Z?lI+orC@X^~x$Ic9gYxCoGb|`z;@V%I4{=c6~Iurl^

literal 0
HcmV?d00001

diff --git a/ctre_sim/CANCoder vers. H - 019 - 0 - ext.dat b/ctre_sim/CANCoder vers. H - 019 - 0 - ext.dat
new file mode 100644
index 0000000000000000000000000000000000000000..9cdaf2e7a3aa8e94b850c93a25af4510ab8aff71
GIT binary patch
literal 2048
zcmeHHJ7`m36#j3LYEm5nTCAv~iL^zXIyIAC17dOMEiNrM1TrXy;G|W2M5GW0B{)dM
zC4-6<q4lvCMQ$-76fp|YHsBE3MI0o9T1$i1|NQ5CCAqa*M$dBcoyT`xf9|w8;Hm%8
z!VS!NRzRtdpO<<*Y1S92TtB06tK_A6S08O5-z#aXdLw&{`kod%v;CmKY7-mk{XCar
zu|^){u=I1h2A9RMqJKm~49<(q6<*s?%G2U@aGfd9O5zchmAr?)c%O+ruK&Z=PgLSa
zX=-&uaJvvjgJOtry@`+ahp4}WF?F0g(GBd^;{<3X`=V>^h<?iZCro@%sUrE^L+p1q
zv;MyAb%O3)3Ni*?+?p}Os28_N2Isk)s^xNKZ|h+SGdtcP{rGSxz;@)LA3x4XJt+8r
zBlsb@BJWoz-gogW?@Le4d(m;Uzee7F;nn9M@xYZ)<^x7~IX~bU{YSqUI;9=1nm(K%
z4jG6v<43)BcT+!*g~t!f{mxn6u9D5!RWbRt1f&j!Oe>D?d}w37!V<r`ufBSX{B(B+
z@&|gjKK28?<;Z0Bn63NoSlmhd6@+(w?@Bx2@r_(JEq$Oq@^z}BddSB-v=hu5)RVvu
z{=(DiUF65yI^2jK*WTPV?-Ba|KeSFtbEf9%+=`!T^oiNIru#JR%hu7ubL;zsEKPb(
znD>Q!)-r>pzht~>_7@R9-tQJkPR560G%w;|cYgCd^U@#eYMJfjrK5}Y?v%QoUEqB9
zNfRG1;<LBLO~uK5eD6--IeDdT5ci;fsYG)1`$+iMS>}Hp>h5jRIFtHf<_VKi%jr|n
r|7W`?kG`3nr_{|1$J;uVz7OD|Zy!lEKN4%KkN=tTK4tix%rpOAZ(2H3

literal 0
HcmV?d00001

diff --git a/ctre_sim/Pigeon 2 - 020 - 0 - ext.dat b/ctre_sim/Pigeon 2 - 020 - 0 - ext.dat
new file mode 100644
index 0000000000000000000000000000000000000000..d87008f103f150b8b4dd0610f2e2eaea3695614b
GIT binary patch
literal 2048
zcmeHFJ4jqX6uobapB0Os5e1d322G42h$+^}1q*EwS3)2~ip5Hlm`=A!6b-DPU=u&s
zY>|kF;HQj11Q#u0Fae|4P87sSRzXx0o^$8U!M9J-ZRx;c=G;5?oO{o_UsPw){Ysew
z8}#Dd&y=}Iv_zX%-JaAD{-BMka0>ZWa#n0h@f{VcmTjpvV`+jPW=PL9*0Da!{F!RK
z{6th^9cVlvX-Y&QgIxbV%-1+iMZ)XGF^|MikAB=Z!iA27JVIU^%pO1eSi$oUZR`2I
zuS@?#H+}FtPTsq)(`wEG$HT%0Y-GvU21se@#GiWwvUYV}!B)vMdz10()2~lh4{L8>
zTKoT|<7Yp|f+cZXA21ghn;z&^f`;dJy6{EM#D_zdo|9jE5+c+$SBO6R0pEppuKcqS
z4dm|^bAM68`|tT>HskS3(G8hCppSa)m-SGOdDtV&>rs;+4}Ra`u#fYYTV2=7jibvG
z`hSEU;K#0tLQ<)?3LoKkYj5A)NqV2~-(>9ckBi+H{#9m)U%>SS`%8DVf9SwpI;7{z
zGo2tFIG6XE-x7Vq6DGcS_Im%@WXw14Uh0wOo&5sFeelzT@LJI^t7}a}U$%uBj68Mr
z)wbY0I5uDA*S;c;y?1n6ALpv%e`z<o$lH1WsdrUJ+a{|1BK>}Nc2b_MV()t1K6dIQ
U>{nV3^Bv^M`)3=f!5z7O4a=_qIsgCw

literal 0
HcmV?d00001

diff --git a/ctre_sim/Talon FX vers. C - 01 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 01 - 0 - ext.dat
new file mode 100644
index 0000000000000000000000000000000000000000..17413c1c727940e51b58defbf5447f57f575ea0c
GIT binary patch
literal 2048
zcmeHGJ!q3*5Wb|TCe<N>Ma91uX^RR)L1-8C^N$umNGT58vRRxIM2esmv=9fyNh)13
z)FMS_;~1o-7!Yis3R=w;QW0H5!BR^DpLg%wQ@_NYK^;9v?tSj}dGFpYdj9y0P?#uc
zdYTUV^k4qr1m=C2Z{OmT)|Xthd~49nd$L3AGxWH6k^O%vv-GGs@E7@o%|~kZOc@dI
zoHqQ-;%J>3s>VMR(t99J?yAz^>dN`5SyuQEk0`igHuU#GL_SNQ+4z_yi$`3JPWh`X
zxJGin_Wx4HCvNpcacMefxXB8}8_cjn`;EUC>k)pHAvz<T_!{+dudv+7yIgR41fRJ>
z<1Ri=lox;Dg!&8XRe#+0Bq{qYHYi5D_Rwcn82#Eq?;O6Z?dVe`<L34&EMacOTZ~T~
zE{4^PeDJBq8S4iHKX42O(<OVqL^|*MQ=J#zT<5}ZGQVKmpW)!{#1C8=(0agVZ)*S~
zSsPZjYUG0#Eju2m9<_GIYVm`%(OuFHQpw$S`#XJh=9<`2+e$9qvw&k=5l1Cub)GkB
zz3h^H$M5U<1?$V+A;|A;)&7_d>Mh46T82F4-@LF@_+_o`n>{N{f``{?-?Z^Sf8=v2
zNk8P{J~AWRH_SJIAN3OtZzjZ#dvmyxdYqlQ>)s>gf%?##l-5kHwP^#6Kj{AX*ERbI
z&P#&pG3JFrz`LV5FW~fIm*dyDv+U*v2J+2|?E9+w%D&S6h%cL6qln&K(0t^HmiYh&
z;?v0a-F*=g(A5w8(mXaC<+nAN>~y>ZiyS<s{Ri(1J3O&h-%J0p^6!m|y7%fi@Ap5!
nzE3neqWRbRTG|@fb|9yik7@on5x#Rvy$kn7iixr96*sSMq9-+z

literal 0
HcmV?d00001

diff --git a/ctre_sim/Talon FX vers. C - 02 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 02 - 0 - ext.dat
new file mode 100644
index 0000000000000000000000000000000000000000..f42d0d29436a75d80f4829ac083e620dfc717714
GIT binary patch
literal 2048
zcmeHGO-NKx6h7l<z0nr4NGkeqG}QhGTiGUig`ot(NUL_U%Vj}?1eFoOv?y{DhMQTK
zhy-TKA#9|9pac!dn%y+EsE8;iOdOcbx#xWH=2e?&+1;G`oqNvrouBu-bKTb?HAJ4-
zIXYUQ|K}f0V5uVWJ^k%++Y4qb-W+oCzTc_-XY{Cif#3i1%+tg2;9unDwjHagXVwZq
zopXktS{yZ~qippjBiaY@q*pzij#uti%(KRqc&&nq=7YW;A*%4nHCjDp$>I?=Lw%KX
z3Fc@yr21b&^~8-|3{%rd!}UfOkD8-N^`@Rr^h<rEVLBr(@eTUUS~b#>e<|noh<X;f
zrd)iMC@b%|6Z)Rpr1sO6$1Sq&d{i;`4o1E>V(2wTK016$<>+%d?dJA7EMacO+pV4u
z&ey0P`KTw1)20W5ci<Q<W()TH66w6NPjp_~xz2^-WPU;3U*X{H<Q=#$sC>YXw>1Eg
ztPQJMck;#eimr}izj}LP)%aff_-@g|Da}l}{Wb0Ra7}#4oduWgTfp(2T31TacoyrF
zFSDZm@w<>-kYDzWgZ$n$)yI6mw;G#n9rn5Y#^oJSU&eS}?^~-EJi1YJQ&tD`k<Y0l
zddNpVG9&aG<xcPp{=|bD33*4~9PUmSXJ+rX?-BC=KP)GuG?TfuZpQT|JFxVsYCpkw
z`5Z0$QV(=6udvQuPv-@Jj)p#0-{Iz>n;-o6jyk^UzA`UWAMr)=BmV7q%}1VC`Okm@
z@hRj;zeBf29h}~;`}I~;4>18-e$QW;&kpHveoXbQj<mMdvG2%P#bLey<Lf%`#`T9i
mR(Mu&{E;5lf0)nHXIdQB`Al}JU06A_e0MCLnAlZvd_Ms5Gdn{7

literal 0
HcmV?d00001

diff --git a/ctre_sim/Talon FX vers. C - 03 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 03 - 0 - ext.dat
new file mode 100644
index 0000000000000000000000000000000000000000..13d7b4b41f09e42a74e24207e6ce5264952e00ce
GIT binary patch
literal 2048
zcmeHHJ!q3*5WX*IwW)#`EGqsrv9_p-g3vDNSFu>cA;rlpo1Mf#qzY=F6yl&bNu^5$
zty+Y(xCPM^BSI}yL95w9>YxtN!CFg$@$S8QYVuVb)X`h+d+xoT=braX(x;DH@j^st
z^AmKaLjUa_Dlk(KeNWC`YJJ6Bi`P<4@3D4`Kc~m#bA0}%HboE1eScA(+tgXZ=aCTs
z&k4hmmPczeP&WPvPxnBb?5fu8`pWlZi!AX+v`)bVi($U+5mnf7O~%JOSw8YoR(J1<
z>KWgs^FIgpL@iGox29u;8?9k9Y=P}MZ{+20kMJuE&?z~IuF`W}CnP)h7jy0&!Dss5
zh|A9sW#v5ArRTY|>OXCG5|{hVhZUn=%$szLF)!xj9loyPXfB<0y8Q}En$vi*@d@C3
zNaLsnpCC@#JUE;KNAO|3VDFbm_0B$1y?Argh4Vze>b^h2%g@8|feU@w4;bU^`T<w?
zZ~SKDD*c9{<B{mml<r6^zTb9hhwy=w#_ziOJMVq$msnzJ!PWbgaQJAQqmr;bFY2{l
zW<lTG`@p>F{&Mdy)OWY)eCUIIi;>Zt13vR_n%yG&GS>I)?xjY-gR6CJ(s*D#>baC;
z9_q1=Xo7viya~?HKl9*fvz%jZPInf>b5C!&_Xs`EAD)YnTFJdOuE*yJ-#hcB=6<U6
z@;O`j3m?pZUcouL8dR?z1G*f)j#$y@hq&x7w)d;vSLU_OC(`_)#WjoQ-C3<i9rOdP
z%1@%^cl#oz%Flo3=X>BlC+|8I?{K^Yiyb(l^HVnl9Uk4I@1=K9`QvxtyHozJWUa)a
pcaFP0Q$t$cvaY4Ap5yz+m-R?Wo|y2RUN|v(dnn&LyshN)`VJBeGeQ6W

literal 0
HcmV?d00001

diff --git a/ctre_sim/Talon FX vers. C - 04 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 04 - 0 - ext.dat
new file mode 100644
index 0000000000000000000000000000000000000000..d53937ea48cfd138352224a3c43b43588789a2eb
GIT binary patch
literal 2048
zcmeHIJ!q3*5WXa>Ce<N>Ma7?{mA3wXC<yJMeu`pIloThoY<9FDQVME83UN@Jq|y!<
zv`7(}I0mUH1_UisL95w9>YyT`pw!YpynFAS@a6MoP)BdM_kN$}?!L6q!6R2gVWOz%
zSvpvw|MCwvuuzlzp80yI>jif$UmtP%&i1JPDLt%S;P{`)96hKG{Y8CY<DmvVGe!hF
zXU%?Md9+3yRpXxw=^Ut&Syg&mUwL0P&k8@{O==9A5BOe)sK!!gH9n@v@{yOLleIbo
z_vqNG_+J8i;?`djx2EG}w^+k?#2jskH}!n7U-(r<>8wQJt2EA7VVTLhSa5d)pZNn*
zE<aC{mw4f*#tUmze_a2lL+)LSsEvNRL!Vt^;I)T7IQxde(Wi9U?d?~#q`4h$GCl#j
z7*;>(!6#^^4G#)&*fBgzm+bo`(tYP2>%Mq$-3#Z*{``G^X8Ug^aoD9H?FSq1b_SRv
zXT$00PJVdNvg48LS8rde(Z1Jxdz;`vD!IGver?;|UlCt&OUc!HmT>r3lcSQfKF^x9
zUuH>v$8Q68{{C`z2<rQ~6d(IRzvb9;=cvd0TNgJAzl`;Ly>q2S?6K8~n=&52M?II4
z;GrJ#$c`{?geO59{S)`EB_xiyIo(On&dl6$-y`;c{?J^M(n{{NVI7`7=z)b-4fpZy
z%j0b6FMNQ5eFbrLM0H;P$PS-y{CeBVZh!FGpAFjcP4AU?srbk*n;-da&TBpDL<=9-
zzWfwwem5_2e7gFMzch~>GGn%**YOrxWdAwEAGtZ^?CD+ly$mcX|ETk7y*z8o&B=uR
l|DGGy`co5~-OU`>m#gn1rHOi<`K8l~cgBl}$*mQ)ukYbGE=B+V

literal 0
HcmV?d00001

diff --git a/ctre_sim/Talon FX vers. C - 05 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 05 - 0 - ext.dat
new file mode 100644
index 0000000000000000000000000000000000000000..ea2dbc5f7469590835dc17ef0ab0bcaa02cd7202
GIT binary patch
literal 2048
zcmeHGOK4M35WPuSeW@<8uzv8<B+}Loh=R~=)Tby`MM!bwE<3xdAW{UiAccr1T}h>z
zENYP=v~d}vzG6VoLMv!ByO6q25m8WTY2bP1bHdB>vrw1LX70?LIp@sW_nh-buScTB
zIO=E2A>aJBf4G4KpZ0t9>y?fd7PWF~koP^+CHZIOQSG9=|5KYa4{HN|(eG_K+@NP#
zg+S+w#!t1*tdT@b^=Be72l{AMu`b4?^Ht5P*h8X8f=ikMzZ)^ex8}8~9`#1+QCBdh
z{W^ukNbeK=FCje%#V2Q>%}I@06fhCfM4RwUJRk2Rebpgzj&2gG<lb^cX(sow$L|O|
za|b85zGzI5?!9Aj@2wU8al_*@y>}@lG4S?8J~J@*QjrfF4-1|7l*{qneic(0?|8H7
z3B{$T<k1g3;W(#!aJWYt$3cBrzh7fy-^C}gFFU#H#n#dOg1A46gKtOoh|2?lhZyww
z3<wQ98=kJ7v<KIzu#QZxWP9R`@%_%R?ZgMG7Vh!;we5I+jdGc-W$t&i;K=bNmXcAN
z`DVe(FUf!WHpCair+0^;zo$d^u^-?q$0yr|T&urzaSQ3oEAH!ED=id{tQOv^>Ht6b
zZL1+Z^kW{{5$28ANpKJR<b#_@y2sqMoGl#Zr|<Chh<yMb-Zo{W(_(F0hvN^vZ{bzL
z`vmvp+G@flJ>bE<!aO@2*%t`rQzuwocdEksL%vxLd%wwh<zEUv>MNQ@{hJHYk3RCG
z2XUZ2i=N-ji<*G1y|Z7MYZEl%RJxmWQ%rWCU-$=ak8nJ>TfUdRWzp|&R{H7aTWw^+
o>rXMx?5OnL7;En&&HD@WePpSQjP%Yeomsp)T1t*@tMb0S18)N^DgXcg

literal 0
HcmV?d00001

diff --git a/ctre_sim/Talon FX vers. C - 06 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 06 - 0 - ext.dat
new file mode 100644
index 0000000000000000000000000000000000000000..a8dce6dec297d4eacf20dc676b7905f5911123fa
GIT binary patch
literal 2048
zcmeHHJ!n%=6uvKc)ucLPXi%v?ZNwB6q=Jwv>QfYhprklCnaz$0B1LEoNFff2lT^B7
z&>}@>vc|s@1A-P>L93Yzse?F(g3^`-;ypiKNS?Z=qi4DAJ3rs~?m0IJT|9Q(2^bU7
zbJiSgn*a6>C$QM0`JVkg7JqG7Yd5nz??OWSFU`}&WgGufn=_9ZBY#m}-g2Zv&l42_
zowJHxXr9?1j)v+_J8}=y(Otz7%uC-_)w5<lBCdcd>O+6eF{Wv)992Df8qG&u(VS|w
zT3C+Qe(C?srzfKPbXaP0QgOEuMuO_tA^oObO%0L0`nWkyk;poU+g<^>lQ&xCJwng?
zp=r)98B?No`MAW(8|D1G=~;~ST?q;X-#+In6Qf_hv%vV4v@>6FIiB0^Fs1Q~dsUAQ
zR|4WkJ@oi-PW$0N95{>*J*)bDjgfhmp3A)K&1Eh&kLK6T`zzeOJBkBWM}!X;?R5=+
zhSrAFwVd|D3)NUhdPuyfa3_8+aCay5gVl@od4DU3PuIwo-d5#$PZN%wa9K)Pc^0~a
zFTX6m<Bz_6?fkTN80u4T>5ut<Z#6u#YuvN?qf1*!UtW3N?OE$4Jh5K-W>g3Iqu!Pp
z>W6yVhh~KP2JKA{2Y;q`qmSaaw@qjBWAEV|evg<3_;K2zj8s~#%{}=1=^S2s+p(Ya
zd3iRQ_(>1?U|#+{yF)TBG{_`Rv%X}1jpqkH-p$~O>?{99`Xj%le&m0+B=xAHlk@<0
z=4Vl+|4t0)z2Nz_5;W0mo-@7l(md<vNx9KNEXleFlO4Pu{j;|x7|-mL_cFXH`k}+)
vsQs_B+Q<rzQ_M3rDfRCM;{#pRf1ubtk1VB<x%10smhMef`lhzmd0syNrD-y)

literal 0
HcmV?d00001

diff --git a/ctre_sim/Talon FX vers. C - 07 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 07 - 0 - ext.dat
new file mode 100644
index 0000000000000000000000000000000000000000..ec4ab94b478e8f26580c9b30b11c17fe019fdeb7
GIT binary patch
literal 2048
zcmeHGO=uHQ5PqAqc2hm%U{Ud>X{4<`APPczQCCqcg-VJiZ@Jl{7Lg*T0V%|T;z=sK
z<e){0&_-`T>J|fn7OJS#+(PO>MMOcVrGYr}X1=gle-7%=xy+k+GvD{kyj}P7;mc0M
z7+3wQIZ!kI?H_Jnp~m|?@%2L23qmbl&DecU_elPUc~Ct|_kU`0=6-eLFZ%rr2OIRv
zs1WF!)%da2nbnf0s{WKCbD)oB73;CMe7>le6*|P5Bsixz@OzFiH8Q_V^{6*mkGi}W
zsMRS5Be_TTzl8L}6`vhJn|_U36)+yvM7!`!KARY1eU(vjl5gUx<es=9Jd=0cx9<o&
z^ZO@leZiOl-}^`8-d`jB<GP1Qe(z#bV&Ls^K3ib$bvPevd`;-gr)<{l?N>3E*&T0G
zJ)yW5kv#gLCmd&$4-WT;V>qZU>Gx}l?7Q$t_C=G+UZ{@u7sUNp9DF;zM_d{aJj9^a
zXFzE9+3<ArBpp1rY;~jtCEFKkjPG>c+{%2gO8&NezxHkKFL5rlxn%o2EjV<v$x2Bn
z&eLYW%Pq-&{5HfF#OHU1p}((7_^}`0Eyt!hM?KQtwz!G)<rMezj+Iu9$6ACptvbMu
zerh$$hkne%JHotCngsX2Pu#td;CswX<<`P+Zsvyl9<dML!&6gQItgq2S{#4yLkq7O
z-Y2*(kE$7;^?(Qa3iE7tWnUohj`dr8y&Yw{Kjd5Vu=|_5SMH_oqrR+p)W10+{pe%Q
zdJqTd)9Cr#yr>EI>O1<<JWBA4)5%_|n`5?pr-VOqeayyFJLP*BS{D7T%}PHXYov`W
p^L)VK%#BO`@V(CNX3Fo)*Y}a;IyTljzjS=@)_5^7v87`7^&Py%E)xI%

literal 0
HcmV?d00001

diff --git a/ctre_sim/Talon FX vers. C - 09 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 09 - 0 - ext.dat
new file mode 100644
index 0000000000000000000000000000000000000000..949d405611073cddbd652ee812351a1e68fd2747
GIT binary patch
literal 2048
zcmeHHJ!n%=6uvJ>HK`668dT~}8Zku$Q4rEaeHO(ih$&7^W)SSCAW{T1AcZ(6PEzTT
zL8}y@%^F2h3<z3i1g&NZse^P71*I(w#Cv|e@bc6}9X-o^=kNQzd(KV57mwU@0>*^(
zoH2)5=D+>J3Cy=>zGuE)OTM+J_1ig~_jF3)ug&x3Wt;y~n>A0HBY)9f-gLM_&odPQ
zoipmc(mJzF0!`JQapWH8qq~Zw7?-}UYh=}aL_^|S(HQt$$C#G2a!mE;X|x`7MRT&%
zrmz@^eZv3ErzfiTbXaI}Lj7(9j0QEZP536?Obn5}#;7?@ndlnHTdn}z$-P?UJwngi
z!AY(!8B?Ns`KaW}>*f5s;YEV>T?vX0ygklW1_ocBQ)T~-(3vm!JkRZSK9%u|dsUCm
zuLLBHe(3T0dF8`_Jp2eg^sMRoHAd!LdMWd=H<!8CI+~vs_gB7mca(=;8xcJCpw~6P
zHMBOYt{t==Zn(}mGD8wgM>_q-{r9&MAFNS)#QR%LE#9D5W=oCxT`d?q7GfzG#i@1)
zUSUao$De$BUVPd+4E^b(@MAu}TZv5V7<H}w*urMgS5Vv!yH>l&A6qNDS=9l4^xIZL
zeCWr0Xhyhi(B1@j;Ae}s;*`g|Z8@9Ycc0wj_lS7_AE#}~N~guz*n`iX&f)p@9sBXl
z%eB>nPkO+EdHH#EhGkwLsGd8;`Ud*yJU_(oe)lfRz6$SzAN6&OqyFP1=|>-(qz8Uy
zeGYB<@5C;>7d+o~ffj~ZXH74?G}i`t(xKRNVt{p%Pj=vf@aOK1u|Kt2-plZc=*REC
w<*fa$wA#qxi^myfc3k=&4kr7%Y<z#wJC7Wdl7;4$PA@zduf!*|Hh5k?0Q>7RYXATM

literal 0
HcmV?d00001

diff --git a/networktables.json b/networktables.json
new file mode 100644
index 00000000..fe51488c
--- /dev/null
+++ b/networktables.json
@@ -0,0 +1 @@
+[]
diff --git a/simgui-ds.json b/simgui-ds.json
new file mode 100644
index 00000000..73cc713c
--- /dev/null
+++ b/simgui-ds.json
@@ -0,0 +1,92 @@
+{
+  "keyboardJoysticks": [
+    {
+      "axisConfig": [
+        {
+          "decKey": 65,
+          "incKey": 68
+        },
+        {
+          "decKey": 87,
+          "incKey": 83
+        },
+        {
+          "decKey": 69,
+          "decayRate": 0.0,
+          "incKey": 82,
+          "keyRate": 0.009999999776482582
+        }
+      ],
+      "axisCount": 3,
+      "buttonCount": 4,
+      "buttonKeys": [
+        90,
+        88,
+        67,
+        86
+      ],
+      "povConfig": [
+        {
+          "key0": 328,
+          "key135": 323,
+          "key180": 322,
+          "key225": 321,
+          "key270": 324,
+          "key315": 327,
+          "key45": 329,
+          "key90": 326
+        }
+      ],
+      "povCount": 1
+    },
+    {
+      "axisConfig": [
+        {
+          "decKey": 74,
+          "incKey": 76
+        },
+        {
+          "decKey": 73,
+          "incKey": 75
+        }
+      ],
+      "axisCount": 2,
+      "buttonCount": 4,
+      "buttonKeys": [
+        77,
+        44,
+        46,
+        47
+      ],
+      "povCount": 0
+    },
+    {
+      "axisConfig": [
+        {
+          "decKey": 263,
+          "incKey": 262
+        },
+        {
+          "decKey": 265,
+          "incKey": 264
+        }
+      ],
+      "axisCount": 2,
+      "buttonCount": 6,
+      "buttonKeys": [
+        260,
+        268,
+        266,
+        261,
+        269,
+        267
+      ],
+      "povCount": 0
+    },
+    {
+      "axisCount": 0,
+      "buttonCount": 0,
+      "povCount": 0
+    }
+  ]
+}
diff --git a/simgui.json b/simgui.json
new file mode 100644
index 00000000..3cfca74e
--- /dev/null
+++ b/simgui.json
@@ -0,0 +1,23 @@
+{
+  "NTProvider": {
+    "types": {
+      "/FMSInfo": "FMSInfo"
+    }
+  },
+  "NetworkTables": {
+    "transitory": {
+      "drivetrain": {
+        "open": true,
+        "pid": {
+          "open": true
+        }
+      }
+    }
+  },
+  "NetworkTables Info": {
+    "Clients": {
+      "open": true
+    },
+    "visible": true
+  }
+}
diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp
index 230607f1..9fcbca1e 100644
--- a/src/main/cpp/Robot.cpp
+++ b/src/main/cpp/Robot.cpp
@@ -292,3 +292,13 @@ void Robot::DisabledPeriodic() {}
 
 void Robot::TestInit() {}
 void Robot::TestPeriodic() {}
+
+void Robot::SimulationInit() {
+  //_vision->GetDistanceToTarget(16);
+  for (int i = 1; i< 17; i++) {
+    _vision->AlignToTarget(i, 0_m, _swerveDrive);
+  }  
+}
+
+void Robot::SimulationPeriodic() {}
+
diff --git a/src/main/cpp/vision/Vision.cpp b/src/main/cpp/vision/Vision.cpp
index a06236fc..8cf9a46b 100644
--- a/src/main/cpp/vision/Vision.cpp
+++ b/src/main/cpp/vision/Vision.cpp
@@ -17,44 +17,44 @@ FMAP::FMAP(std::string path) : _path(path) {
 
   file >> j;
 
+  std::cout << j["fiducials"] << std::endl;
+
   // iterate through the json object and add each tag to the vector
 
-  for (auto& element : j["fiducials"]) {
-    AprilTag tag;
-
-    tag.id = element["id"];
-    tag.size = element["size"];
-    tag.transform[0][0] = element["transform"][0];
-    tag.transform[0][1] = element["transform"][1];
-    tag.transform[0][2] = element["transform"][2];
-    tag.transform[0][3] = element["transform"][3];
-    tag.transform[1][0] = element["transform"][4];
-    tag.transform[1][1] = element["transform"][5];
-    tag.transform[1][2] = element["transform"][6];
-    tag.transform[1][3] = element["transform"][7];
-    tag.transform[2][0] = element["transform"][8];
-    tag.transform[2][1] = element["transform"][9];
-    tag.transform[2][2] = element["transform"][10];
-    tag.transform[2][3] = element["transform"][11];
-    tag.transform[3][0] = element["transform"][12];
-    tag.transform[3][1] = element["transform"][13];
-    tag.transform[3][2] = element["transform"][14];
-    tag.transform[3][3] = element["transform"][15];
-    if (element["unique"] == 1) {
-      tag.unique = true;
-    } else {
-      tag.unique = false;
+  for (auto& fiducial : j["fiducials"]) {
+    std::cout << "Loading AprilTag " << fiducial["id"] << std::endl;
+      AprilTag tag;
+      tag.id = fiducial["id"];
+      tag.size = fiducial["size"];
+      if (fiducial["unique"] == 1) {
+          tag.unique = true;
+      } else {
+          tag.unique = false;
+      }
+
+      const auto& transform = fiducial["transform"];
+      for (int i = 0; i < 4; ++i) {
+          for (int j = 0; j < 4; ++j) {
+               tag.transform[i][j] = transform[i * 4 + j];
+          }
+      }
+
+      tag.yaw = units::radian_t{tag.transform[0][0]};
+      tag.pitch = units::radian_t{tag.transform[1][1]};
+      tag.roll = units::radian_t{tag.transform[2][2]};
+
+      tag.pos = frc::Pose3d(units::meter_t{tag.transform[0][3]}, units::meter_t{tag.transform[1][3]}, units::meter_t{tag.transform[2][3]}, frc::Rotation3d(0_deg, 0_deg, 0_deg));
+
+       _tags.push_back(tag);
     }
 
-    _tags.push_back(tag);
-  }
-
   file.close();
 
   std::cout << "Loaded " << _tags.size() << " tags" << std::endl;
 
   for (int i = 0; i < _tags.size(); i++) {
-    std::cout << "Tag " << _tags[i].id << " is " << _tags[i].size << "m" << std::endl;
+    std::cout << "Tag " << _tags[i].id << " is at: X: " << _tags[i].pos.X().value() << " Y: " << _tags[i].pos.Y().value()
+              << " Z: " << _tags[i].pos.Z().value() << std::endl;
   }
 
   std::cout << "Loaded FMAP" << std::endl;
@@ -79,18 +79,17 @@ std::pair<units::meter_t, units::degree_t> Vision::GetDistanceToTarget(VisionTar
 
   std::vector<AprilTag> tags = _fmap.GetTags();
 
-  frc::Pose3d pose = _limelight->GetPose();
-
   for (int i = 0; i < tags.size(); i++) {
     if (tags[i].id == static_cast<int>(target)) {
-      frc::Pose3d aprilTagPos =
-          frc::Pose3d(tags[i].transform[0][3] * 1_m, tags[i].transform[1][3] * 1_m,
-                      tags[i].transform[2][3] * 1_m, frc::Rotation3d(0_deg, 0_deg, 0_deg));
+      AprilTag tag = tags[i];
+      frc::Pose3d pose = _limelight->GetPose();
+      //frc::Pose3d pose = frc::Pose3d(0_m, 0_m, 0_m, frc::Rotation3d(0_deg, 0_deg, 0_deg));
+
+      units::meter_t distance = pose.Translation().Distance(tag.pos.Translation());
 
-      units::meter_t distance = pose.Translation().Distance(aprilTagPos.Translation());
+      units::degree_t angle = units::math::atan((pose.X() - tag.pos.X()) / (pose.Y() - tag.pos.Y()));
 
-      units::degree_t angle = units::math::atan((pose.X() - units::meter_t{tags[i].transform[0][3]}) /
-                                                (pose.Y() - units::meter_t{tags[i].transform[1][3]}));
+      std::cout << "Distance: " << distance.value() << " Angle: " << angle.value() << std::endl;
 
       return std::make_pair(distance, angle);
     }
@@ -110,16 +109,16 @@ std::pair<units::meter_t, units::degree_t> Vision::GetDistanceToTarget(int id) {
 
   for (int i = 0; i < tags.size(); i++) {
     if (tags[i].id == id) {
-      // get distance to the limelight
-      frc::Pose3d currentPose = _limelight->GetPose();
-      frc::Pose3d aprilTagPos =
-          frc::Pose3d(tags[i].transform[0][3] * 1_m, tags[i].transform[1][3] * 1_m,
-                      tags[i].transform[2][3] * 1_m, frc::Rotation3d(0_deg, 0_deg, 0_deg));
+      AprilTag tag = tags[i];
+      frc::Pose3d pose = _limelight->GetPose();
+      //frc::Pose3d pose = frc::Pose3d(0_m, 0_m, 0_m, frc::Rotation3d(0_deg, 0_deg, 0_deg));
+
+      // units::meter_t distance = pose.Translation().Distance(tag.pos.Translation());
+      units::meter_t distance = units::meter_t{std::sqrt(std::pow(tag.pos.X().value(), 2) + std::pow(tag.pos.Y().value(), 2))};
 
-      units::meter_t distance = currentPose.Translation().Distance(aprilTagPos.Translation());
+      units::degree_t angle = units::math::atan((pose.X() - tag.pos.X()) / (pose.Y() - tag.pos.Y()));
 
-      units::degree_t angle = units::math::atan((currentPose.X() - units::meter_t{tags[i].transform[0][3]}) /
-                                                (currentPose.Y() - units::meter_t{tags[i].transform[1][3]}));
+      //std::cout << "Distance: " << distance.value() << " Angle: " << angle.value() << std::endl;
 
       return std::make_pair(distance, angle);
     }
@@ -132,21 +131,129 @@ std::vector<AprilTag> Vision::GetTags() {
   return _fmap.GetTags();
 }
 
-frc::Pose2d Vision::AlignToTarget(VisionTarget target, wom::SwerveDrive* swerveDrive) {
+frc::Pose2d Vision::AlignToTarget(VisionTarget target, units::meter_t offset, wom::SwerveDrive* swerveDrive) {
   SetMode(VisionModes::kAprilTag);
 
   std::pair<units::meter_t, units::degree_t> distance = GetDistanceToTarget(target);
 
-  units::meter_t x = distance.first * units::math::cos(distance.second);
-  units::meter_t y = distance.first * units::math::sin(distance.second);
+  // Get current position from Limelight
+  frc::Pose2d current_pose = _limelight->GetPose().ToPose2d();
+  //frc::Pose2d current_pose = frc::Pose2d(0_m, 0_m, 0_deg);
+  
+  units::meter_t x_offset = distance.first * units::math::cos(distance.second);
+  units::meter_t y_offset = distance.first * units::math::sin(distance.second);
 
-  frc::Pose2d pose = frc::Pose2d(x, y, distance.second);
+  // reduce both offsets by the offset parameter (in relative amount)
+  x_offset -= offset * units::math::cos(distance.second);
+  y_offset -= offset * units::math::sin(distance.second);
 
+  frc::Pose2d pose = frc::Pose2d(y_offset, x_offset, distance.second);
+  
+
+  // Print the results
+  std::cout << "Aligning to tag " << static_cast<int>(target) << ": X: " << pose.X().value() << " Y: " << pose.Y().value() << " Angle: " << pose.Rotation().Degrees().value() << ". At offset " << offset.value() << "." << std::endl;
+
+  // Set the new pose to the swerve drive
   swerveDrive->SetPose(pose);
 
   return pose;
 }
 
+frc::Pose2d Vision::AlignToTarget(int target, units::meter_t offset, wom::SwerveDrive* swerveDrive) {
+  SetMode(VisionModes::kAprilTag);
+
+  // Get distance to target
+  std::pair<units::meter_t, units::degree_t> distance = GetDistanceToTarget(target);
+
+  // Get current position from Limelight
+  frc::Pose2d current_pose = _limelight->GetPose().ToPose2d();
+  //frc::Pose2d current_pose = frc::Pose2d(0_m, 0_m, 0_deg);
+  
+  units::meter_t x_offset = distance.first * units::math::cos(distance.second);
+  units::meter_t y_offset = distance.first * units::math::sin(distance.second);
+
+  // reduce both offsets by the offset parameter (in relative amount)
+  x_offset -= offset * units::math::cos(distance.second);
+  y_offset -= offset * units::math::sin(distance.second);
+
+  frc::Pose2d pose = frc::Pose2d(y_offset, x_offset, distance.second);
+  
+
+  // Print the results
+  std::cout << "Aligning to tag " << target << ": X: " << pose.X().value() << " Y: " << pose.Y().value() << " Angle: " << pose.Rotation().Degrees().value() << ". At offset " << offset.value() << "." << std::endl;
+
+  // Set the new pose to the swerve drive
+  //swerveDrive->SetPose(pose);
+
+  return pose;
+}
+
+frc::Pose2d Vision::AlignToTarget(VisionTarget target, frc::Translation2d offset, wom::SwerveDrive* swerveDrive) {
+  SetMode(VisionModes::kAprilTag);
+
+  std::pair<units::meter_t, units::degree_t> distance = GetDistanceToTarget(target);
+
+  // Get current position from Limelight
+  frc::Pose2d current_pose = _limelight->GetPose().ToPose2d();
+  //frc::Pose2d current_pose = frc::Pose2d(0_m, 0_m, 0_deg);
+  
+  units::meter_t x_offset = distance.first * units::math::cos(distance.second);
+  units::meter_t y_offset = distance.first * units::math::sin(distance.second);
+
+  // reduce both offsets by the offset parameter (in relative amount)
+  //x_offset -= offset * units::math::cos(distance.second);
+  //y_offset -= offset * units::math::sin(distance.second);
+
+  if (offset.X() > 0_m) {
+    x_offset -= offset.X() * units::math::cos(distance.second);
+  } else {
+    x_offset += offset.X() * units::math::cos(distance.second);
+  }
+
+  if (offset.Y() > 0_m) {
+    y_offset -= offset.Y() * units::math::sin(distance.second);
+  } else {
+    y_offset += offset.Y() * units::math::sin(distance.second);
+  }
+
+  frc::Pose2d pose = frc::Pose2d(y_offset, x_offset, distance.second);
+  
+
+  // Print the results
+  std::cout << "Aligning to tag " << static_cast<int>(target) << ": X: " << pose.X().value() << " Y: " << pose.Y().value() << " Angle: " << pose.Rotation().Degrees().value() << ". At offset: X: " << offset.X().value() << ", Y: " << offset.Y().value() << "." << std::endl;
+
+  // Set the new pose to the swerve drive
+  swerveDrive->SetPose(pose);
+
+  return pose;
+}
+
+frc::Pose2d Vision::AlignToTarget(int target, frc::Translation2d offset, wom::SwerveDrive* swerveDrive) {
+  SetMode(VisionModes::kAprilTag);
+
+  // Get distance to target
+  std::pair<units::meter_t, units::degree_t> distance = GetDistanceToTarget(target);
+
+  // Get current position from Limelight
+  frc::Pose2d current_pose = _limelight->GetPose().ToPose2d();
+  //frc::Pose2d current_pose = frc::Pose2d(0_m, 0_m, 0_deg);
+  
+  units::meter_t x_offset = distance.first * units::math::cos(distance.second);
+  units::meter_t y_offset = distance.first * units::math::sin(distance.second);
+
+  frc::Pose2d pose = frc::Pose2d(y_offset, x_offset, distance.second);
+  
+
+  // Print the results
+  std::cout << "Aligning to tag " << target << ": X: " << pose.X().value() << " Y: " << pose.Y().value() << " Angle: " << pose.Rotation().Degrees().value() << ". At offset: X: " << offset.X().value() << ", Y: " << offset.Y().value() << "." << std::endl;
+
+  // Set the new pose to the swerve drive
+  //swerveDrive->SetPose(pose);
+
+  return pose;
+}
+
+
 bool Vision::IsAtPose(frc::Pose3d pose, units::second_t dt) {
   return _limelight->IsAtSetPoseVision(pose, dt);
 }
diff --git a/src/main/include/vision/Vision.h b/src/main/include/vision/Vision.h
index 8176b2a2..4c7a224e 100644
--- a/src/main/include/vision/Vision.h
+++ b/src/main/include/vision/Vision.h
@@ -54,6 +54,11 @@ struct AprilTag {
   double size;
   std::array<std::array<double, 4>, 4> transform;
   bool unique;
+
+  frc::Pose3d pos;
+  units::radian_t yaw;
+  units::radian_t pitch;
+  units::radian_t roll;
 };
 
 class FMAP {
@@ -79,7 +84,11 @@ class Vision {
 
   frc::Pose3d GetPose();
 
-  frc::Pose2d AlignToTarget(VisionTarget target, wom::SwerveDrive* swerveDrive);
+  frc::Pose2d AlignToTarget(VisionTarget target, units::meter_t offset, wom::SwerveDrive* swerveDrive);
+  frc::Pose2d AlignToTarget(int target, units::meter_t offset, wom::SwerveDrive* swerveDrive);
+
+  frc::Pose2d AlignToTarget(VisionTarget target, frc::Translation2d offset, wom::SwerveDrive* swerveDrive);
+  frc::Pose2d AlignToTarget(int target, frc::Translation2d offset, wom::SwerveDrive* swerveDrive);
 
   std::vector<AprilTag> GetTags();
 
diff --git a/wombat/src/main/cpp/utils/Encoder.cpp b/wombat/src/main/cpp/utils/Encoder.cpp
index a40f4578..98e65f7a 100644
--- a/wombat/src/main/cpp/utils/Encoder.cpp
+++ b/wombat/src/main/cpp/utils/Encoder.cpp
@@ -57,7 +57,7 @@ units::radian_t wom::utils::Encoder::GetEncoderPosition() {
 }
 
 double wom::utils::Encoder::GetEncoderDistance() {
-  return GetEncoderTicks() * (2 * 3.141592) * _wheelRadius.value();
+  return GetEncoderTicks() * (2 * 3.14159265358979323846) * _wheelRadius.value();
 }
 
 units::radians_per_second_t wom::utils::Encoder::GetEncoderAngularVelocity() {

From 33a0cbc356103b8bb4ea2b57d62ba0de17839b0f Mon Sep 17 00:00:00 2001
From: Paul Hodges <paul.hodges@student.education.wa.edu.au>
Date: Mon, 29 Jan 2024 12:32:08 +0800
Subject: [PATCH 144/207] Finish vision maths

---
 ctre_sim/CANCoder vers. H - 016 - 0 - ext.dat | Bin 2048 -> 2048 bytes
 ctre_sim/CANCoder vers. H - 017 - 0 - ext.dat | Bin 2048 -> 2048 bytes
 ctre_sim/CANCoder vers. H - 018 - 0 - ext.dat | Bin 2048 -> 2048 bytes
 ctre_sim/CANCoder vers. H - 019 - 0 - ext.dat | Bin 2048 -> 2048 bytes
 ctre_sim/Pigeon 2 - 020 - 0 - ext.dat         | Bin 2048 -> 2048 bytes
 ctre_sim/Talon FX vers. C - 01 - 0 - ext.dat  | Bin 2048 -> 2048 bytes
 ctre_sim/Talon FX vers. C - 02 - 0 - ext.dat  | Bin 2048 -> 2048 bytes
 ctre_sim/Talon FX vers. C - 03 - 0 - ext.dat  | Bin 2048 -> 2048 bytes
 ctre_sim/Talon FX vers. C - 04 - 0 - ext.dat  | Bin 2048 -> 2048 bytes
 ctre_sim/Talon FX vers. C - 05 - 0 - ext.dat  | Bin 2048 -> 2048 bytes
 ctre_sim/Talon FX vers. C - 06 - 0 - ext.dat  | Bin 2048 -> 2048 bytes
 ctre_sim/Talon FX vers. C - 07 - 0 - ext.dat  | Bin 2048 -> 2048 bytes
 ctre_sim/Talon FX vers. C - 09 - 0 - ext.dat  | Bin 2048 -> 2048 bytes
 src/main/cpp/Robot.cpp                        |  16 +-
 src/main/cpp/vision/Vision.cpp                | 175 +++++++++++-------
 15 files changed, 125 insertions(+), 66 deletions(-)

diff --git a/ctre_sim/CANCoder vers. H - 016 - 0 - ext.dat b/ctre_sim/CANCoder vers. H - 016 - 0 - ext.dat
index 8f8bccdea82f511e5af738d8fa0b4b911c8bdd34..b70c3556679497ee603521f2dda7d4398157875e 100644
GIT binary patch
delta 36
ncmZn=Xb{+tz{t6iq5t;=W(Ee1iGuE&yBK<YZ-R0bF|h#v)Rqdo

delta 36
ncmZn=Xb{+tz{t6m;qaddCI$wMiGuE&dl-)WDT8trF|h#v)s70h

diff --git a/ctre_sim/CANCoder vers. H - 017 - 0 - ext.dat b/ctre_sim/CANCoder vers. H - 017 - 0 - ext.dat
index 2596b7a4155a130d6f3b83ca0a542ed4c02564b7..be58d0c2d2c75f1ffe8d657c982b4a688f24ce71 100644
GIT binary patch
delta 30
kcmZn=Xb{+tz{s|fVe0Qq%o7E@*mf|?_`LzdTExT#0I8M=yZ`_I

delta 30
kcmZn=Xb{+tz{s|n;ryR6rip@HY<n2a{HXx37BR5_0IH4(yZ`_I

diff --git a/ctre_sim/CANCoder vers. H - 018 - 0 - ext.dat b/ctre_sim/CANCoder vers. H - 018 - 0 - ext.dat
index d7704d30ca6103103cb4b7d2b8c0d3f59947ac95..a5a5e02fd7e38bd52b5d2e78c6a069ddb1a7ff1d 100644
GIT binary patch
delta 36
ncmZn=Xb{+tz{t6iq5t;=W(Ee1iGuE&yBK<YZ-R0bF|h#v)Rqdo

delta 36
ncmZn=Xb{+tz{t6m;qaddCI$wMiGuE&dl-)WDT8trF|h#v)s70h

diff --git a/ctre_sim/CANCoder vers. H - 019 - 0 - ext.dat b/ctre_sim/CANCoder vers. H - 019 - 0 - ext.dat
index 9cdaf2e7a3aa8e94b850c93a25af4510ab8aff71..1d1df01f2e5379572fc30a4936c29417c965e6e7 100644
GIT binary patch
delta 30
kcmZn=Xb{+tz{s|fVe0Qq%o7E@*mf|?_`LzdTExT#0I8M=yZ`_I

delta 30
kcmZn=Xb{+tz{s|n;ryR6rip@HY<n2a{HXx37BR5_0IH4(yZ`_I

diff --git a/ctre_sim/Pigeon 2 - 020 - 0 - ext.dat b/ctre_sim/Pigeon 2 - 020 - 0 - ext.dat
index d87008f103f150b8b4dd0610f2e2eaea3695614b..1c15fadd76a37035c4fb5dc322174ef5e4f0ae3b 100644
GIT binary patch
delta 53
zcmZn=Xb@mI&S3d}qo6(GL<L@!6ATtGHrq~yslPWdPZacG+rcp7_XZGavjg)1769A8
B5<vg}

delta 53
zcmZn=Xb@mI%V7L}qo6(GL<L@!GYlp$HrsB7^MA^iCJK77?O{0crvk*<?7)101pwU4
B5<vg}

diff --git a/ctre_sim/Talon FX vers. C - 01 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 01 - 0 - ext.dat
index 17413c1c727940e51b58defbf5447f57f575ea0c..c286c31b23ccbab2ae10455eb784fdb6c7aba6ba 100644
GIT binary patch
delta 30
kcmZn=Xb{-Yz{qxzq4oDB=81|{Y$q5xes2J=7BR5_0Ie1Z+yDRo

delta 30
kcmZn=Xb{-Yz{qx*VgH{priqGHY-bqu{HXx37BR5_0Im)S+yDRo

diff --git a/ctre_sim/Talon FX vers. C - 02 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 02 - 0 - ext.dat
index f42d0d29436a75d80f4829ac083e620dfc717714..09761e081c6233b148c8a5cfc9a8e963345694bb 100644
GIT binary patch
delta 30
kcmZn=Xb{-Yz{qxzq4oDB=81|{Y$q5xes2J=7BR5_0Ie1Z+yDRo

delta 30
kcmZn=Xb{-Yz{qx*VgH{priqGHY-bqu{HXx37BR5_0Im)S+yDRo

diff --git a/ctre_sim/Talon FX vers. C - 03 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 03 - 0 - ext.dat
index 13d7b4b41f09e42a74e24207e6ce5264952e00ce..241982f5972195b12caa6927b24a685894f9882b 100644
GIT binary patch
delta 30
kcmZn=Xb{-Yz{qxzq4oDB=81|{Y$q5xes2J=7BR5_0Ie1Z+yDRo

delta 30
kcmZn=Xb{-Yz{qx*VgH{priqGHY-bqu{HXx37BR5_0Im)S+yDRo

diff --git a/ctre_sim/Talon FX vers. C - 04 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 04 - 0 - ext.dat
index d53937ea48cfd138352224a3c43b43588789a2eb..891fd4bd1a4cca587d981de44ca775b397e424c3 100644
GIT binary patch
delta 30
kcmZn=Xb{-Yz{qxzq4oDB=81|{Y$q5xes2J=7BR5_0Ie1Z+yDRo

delta 30
kcmZn=Xb{-Yz{qx*VgH{priqGHY-bqu{HXx37BR5_0Im)S+yDRo

diff --git a/ctre_sim/Talon FX vers. C - 05 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 05 - 0 - ext.dat
index ea2dbc5f7469590835dc17ef0ab0bcaa02cd7202..2fc8ce8768d85b3793cc5eafc3538940ccc1f481 100644
GIT binary patch
delta 30
kcmZn=Xb{-Yz{qxzq4oDB=81|{Y$q5xes2J=7BR5_0Ie1Z+yDRo

delta 30
kcmZn=Xb{-Yz{qx*VgH{priqGHY-bqu{HXx37BR5_0Im)S+yDRo

diff --git a/ctre_sim/Talon FX vers. C - 06 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 06 - 0 - ext.dat
index a8dce6dec297d4eacf20dc676b7905f5911123fa..ff19a0b9c2d1e5c18ee1df765420fa29bf621fca 100644
GIT binary patch
delta 30
kcmZn=Xb{-Yz{qxzq4oDB=81|{Y$q5xes2J=7BR5_0Ie1Z+yDRo

delta 30
kcmZn=Xb{-Yz{qx*VgH{priqGHY-bqu{HXx37BR5_0Im)S+yDRo

diff --git a/ctre_sim/Talon FX vers. C - 07 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 07 - 0 - ext.dat
index ec4ab94b478e8f26580c9b30b11c17fe019fdeb7..b89067fbb4a7264a799dd5bb65befe44e920ef64 100644
GIT binary patch
delta 30
kcmZn=Xb{-Yz{qxzq4oDB=81|{Y$q5xes2J=7BR5_0Ie1Z+yDRo

delta 30
kcmZn=Xb{-Yz{qx*VgH{priqGHY-bqu{HXx37BR5_0Im)S+yDRo

diff --git a/ctre_sim/Talon FX vers. C - 09 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 09 - 0 - ext.dat
index 949d405611073cddbd652ee812351a1e68fd2747..19f15a02966512f9c542c009f18c5281a245e51c 100644
GIT binary patch
delta 30
kcmZn=Xb{-Yz{qxzq4oDB=81|{Y$q5xes2J=7BR5_0Ie1Z+yDRo

delta 30
kcmZn=Xb{-Yz{qx*VgH{priqGHY-bqu{HXx37BR5_0Im)S+yDRo

diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp
index 9fcbca1e..d06d3d95 100644
--- a/src/main/cpp/Robot.cpp
+++ b/src/main/cpp/Robot.cpp
@@ -294,10 +294,22 @@ void Robot::TestInit() {}
 void Robot::TestPeriodic() {}
 
 void Robot::SimulationInit() {
+  std::string x = "[";
+  std::string y = "[";
   //_vision->GetDistanceToTarget(16);
   for (int i = 1; i< 17; i++) {
-    _vision->AlignToTarget(i, 0_m, _swerveDrive);
-  }  
+    for (int j = 0; j<17; j++) {
+      frc::Pose2d pose = _vision->AlignToTarget(i, units::meter_t{j * 0.1}, _swerveDrive);
+      x += std::to_string(pose.X().value()) + ",";
+      y += std::to_string(pose.Y().value()) + ",";
+    }
+  }
+
+  x += "]";
+  y += "]";
+
+  std::cout << x << std::endl;
+  std::cout << y << std::endl;
 }
 
 void Robot::SimulationPeriodic() {}
diff --git a/src/main/cpp/vision/Vision.cpp b/src/main/cpp/vision/Vision.cpp
index 8cf9a46b..3e8ef676 100644
--- a/src/main/cpp/vision/Vision.cpp
+++ b/src/main/cpp/vision/Vision.cpp
@@ -3,6 +3,7 @@
 // of the MIT License at the root of this project
 
 #include "vision/Vision.h"
+#include "units/math.h"
 
 FMAP::FMAP(std::string path) : _path(path) {
   std::cout << "Parsing FMAP" << std::endl;
@@ -82,16 +83,28 @@ std::pair<units::meter_t, units::degree_t> Vision::GetDistanceToTarget(VisionTar
   for (int i = 0; i < tags.size(); i++) {
     if (tags[i].id == static_cast<int>(target)) {
       AprilTag tag = tags[i];
-      frc::Pose3d pose = _limelight->GetPose();
-      //frc::Pose3d pose = frc::Pose3d(0_m, 0_m, 0_m, frc::Rotation3d(0_deg, 0_deg, 0_deg));
+      //frc::Pose3d pose = _limelight->GetPose();
+      frc::Pose3d pose = frc::Pose3d(0_m, 0_m, 0_m, frc::Rotation3d(0_deg, 0_deg, 0_deg));
 
-      units::meter_t distance = pose.Translation().Distance(tag.pos.Translation());
+      SetMode(VisionModes::kAprilTag);
 
-      units::degree_t angle = units::math::atan((pose.X() - tag.pos.X()) / (pose.Y() - tag.pos.Y()));
+      // Get distance to target
+      // Get current position from Limelight
+      //frc::Pose2d current_pose = _limelight->GetPose().ToPose2d();
+      frc::Pose2d current_pose = frc::Pose2d(0_m, 0_m, 0_deg);
 
-      std::cout << "Distance: " << distance.value() << " Angle: " << angle.value() << std::endl;
+      units::meter_t a = tag.pos.X() - current_pose.X();
+      units::meter_t b = tag.pos.Y() - current_pose.Y();
 
-      return std::make_pair(distance, angle);
+      units::radian_t theta = units::math::atan2(b, a);
+      
+      //std::cout << "A: " << a.value() << ", B:" << b.value() << std::endl;
+      units::meter_t r = units::meter_t{std::sqrt(std::pow(a.value(), 2) + std::pow(b.value(), 2))};
+
+      units::meter_t x = current_pose.X() + r * units::math::cos(theta);
+      units::meter_t y = current_pose.Y() + r * units::math::sin(theta);
+      
+      return std::make_pair(r, theta);
     }
   }
 
@@ -104,23 +117,34 @@ std::pair<units::meter_t, units::degree_t> Vision::GetDistanceToTarget(int id) {
   }
 
   SetMode(VisionModes::kAprilTag);
-
+      
   std::vector<AprilTag> tags = _fmap.GetTags();
 
   for (int i = 0; i < tags.size(); i++) {
     if (tags[i].id == id) {
       AprilTag tag = tags[i];
-      frc::Pose3d pose = _limelight->GetPose();
-      //frc::Pose3d pose = frc::Pose3d(0_m, 0_m, 0_m, frc::Rotation3d(0_deg, 0_deg, 0_deg));
+      //frc::Pose3d pose = _limelight->GetPose();
+      frc::Pose3d pose = frc::Pose3d(0_m, 0_m, 0_m, frc::Rotation3d(0_deg, 0_deg, 0_deg));
 
-      // units::meter_t distance = pose.Translation().Distance(tag.pos.Translation());
-      units::meter_t distance = units::meter_t{std::sqrt(std::pow(tag.pos.X().value(), 2) + std::pow(tag.pos.Y().value(), 2))};
+      SetMode(VisionModes::kAprilTag);
 
-      units::degree_t angle = units::math::atan((pose.X() - tag.pos.X()) / (pose.Y() - tag.pos.Y()));
+      // Get distance to target
+      // Get current position from Limelight
+      //frc::Pose2d current_pose = _limelight->GetPose().ToPose2d();
+      frc::Pose2d current_pose = frc::Pose2d(0_m, 0_m, 0_deg);
 
-      //std::cout << "Distance: " << distance.value() << " Angle: " << angle.value() << std::endl;
+      units::meter_t a = tag.pos.X() - current_pose.X();
+      units::meter_t b = tag.pos.Y() - current_pose.Y();
 
-      return std::make_pair(distance, angle);
+      units::radian_t theta = units::math::atan2(b, a);
+      
+      //std::cout << "A: " << a.value() << ", B:" << b.value() << std::endl;
+      units::meter_t r = units::meter_t{std::sqrt(std::pow(a.value(), 2) + std::pow(b.value(), 2))};
+
+      units::meter_t x = current_pose.X() + r * units::math::cos(theta);
+      units::meter_t y = current_pose.Y() + r * units::math::sin(theta);
+      
+      return std::make_pair(r, theta);
     }
   }
 
@@ -134,27 +158,35 @@ std::vector<AprilTag> Vision::GetTags() {
 frc::Pose2d Vision::AlignToTarget(VisionTarget target, units::meter_t offset, wom::SwerveDrive* swerveDrive) {
   SetMode(VisionModes::kAprilTag);
 
-  std::pair<units::meter_t, units::degree_t> distance = GetDistanceToTarget(target);
-
+  // Get distance to target
+  AprilTag tag = GetTags()[static_cast<int>(target) - 1];
   // Get current position from Limelight
-  frc::Pose2d current_pose = _limelight->GetPose().ToPose2d();
-  //frc::Pose2d current_pose = frc::Pose2d(0_m, 0_m, 0_deg);
+  //frc::Pose2d current_pose = _limelight->GetPose().ToPose2d();
+  frc::Pose2d current_pose = frc::Pose2d(0_m, 0_m, 0_deg);
+
+  units::meter_t a = tag.pos.X() - current_pose.X();
+  units::meter_t b = tag.pos.Y() - current_pose.Y();
+
+  units::radian_t theta = units::math::atan2(b, a);
   
-  units::meter_t x_offset = distance.first * units::math::cos(distance.second);
-  units::meter_t y_offset = distance.first * units::math::sin(distance.second);
+  //std::cout << "A: " << a.value() << ", B:" << b.value() << std::endl;
+  units::meter_t r = units::meter_t{std::sqrt(std::pow(a.value(), 2) + std::pow(b.value(), 2))};
+
+  units::meter_t x = current_pose.X() + r * units::math::cos(theta);
+  units::meter_t y = current_pose.Y() + r * units::math::sin(theta);
 
   // reduce both offsets by the offset parameter (in relative amount)
-  x_offset -= offset * units::math::cos(distance.second);
-  y_offset -= offset * units::math::sin(distance.second);
+  x += offset * units::math::cos(theta);
+  y += offset * units::math::sin(theta);
 
-  frc::Pose2d pose = frc::Pose2d(y_offset, x_offset, distance.second);
   
+  frc::Pose2d pose = frc::Pose2d(x, y, theta); 
 
   // Print the results
-  std::cout << "Aligning to tag " << static_cast<int>(target) << ": X: " << pose.X().value() << " Y: " << pose.Y().value() << " Angle: " << pose.Rotation().Degrees().value() << ". At offset " << offset.value() << "." << std::endl;
+  //std::cout << "Aligning to tag " << target << ": X: " << pose.X().value() << " Y: " << pose.Y().value() << " Angle: " << pose.Rotation().Degrees().value() << ". At offset " << offset.value() << "." << std::endl;
 
   // Set the new pose to the swerve drive
-  swerveDrive->SetPose(pose);
+  //swerveDrive->SetPose(pose);
 
   return pose;
 }
@@ -164,23 +196,31 @@ frc::Pose2d Vision::AlignToTarget(int target, units::meter_t offset, wom::Swerve
 
   // Get distance to target
   std::pair<units::meter_t, units::degree_t> distance = GetDistanceToTarget(target);
-
+  AprilTag tag = GetTags()[target - 1];
   // Get current position from Limelight
-  frc::Pose2d current_pose = _limelight->GetPose().ToPose2d();
-  //frc::Pose2d current_pose = frc::Pose2d(0_m, 0_m, 0_deg);
+  //frc::Pose2d current_pose = _limelight->GetPose().ToPose2d();
+  frc::Pose2d current_pose = frc::Pose2d(0_m, 0_m, 0_deg);
+
+  units::meter_t a = tag.pos.X() - current_pose.X();
+  units::meter_t b = tag.pos.Y() - current_pose.Y();
+
+  units::radian_t theta = units::math::atan2(b, a);
   
-  units::meter_t x_offset = distance.first * units::math::cos(distance.second);
-  units::meter_t y_offset = distance.first * units::math::sin(distance.second);
+  //std::cout << "A: " << a.value() << ", B:" << b.value() << std::endl;
+  units::meter_t r = units::meter_t{std::sqrt(std::pow(a.value(), 2) + std::pow(b.value(), 2))};
+
+  units::meter_t x = current_pose.X() + r * units::math::cos(theta);
+  units::meter_t y = current_pose.Y() + r * units::math::sin(theta);
 
   // reduce both offsets by the offset parameter (in relative amount)
-  x_offset -= offset * units::math::cos(distance.second);
-  y_offset -= offset * units::math::sin(distance.second);
+  x += offset * units::math::cos(theta);
+  y += offset * units::math::sin(theta);
 
-  frc::Pose2d pose = frc::Pose2d(y_offset, x_offset, distance.second);
   
+  frc::Pose2d pose = frc::Pose2d(x, y, theta); 
 
   // Print the results
-  std::cout << "Aligning to tag " << target << ": X: " << pose.X().value() << " Y: " << pose.Y().value() << " Angle: " << pose.Rotation().Degrees().value() << ". At offset " << offset.value() << "." << std::endl;
+  //std::cout << "Aligning to tag " << target << ": X: " << pose.X().value() << " Y: " << pose.Y().value() << " Angle: " << pose.Rotation().Degrees().value() << ". At offset " << offset.value() << "." << std::endl;
 
   // Set the new pose to the swerve drive
   //swerveDrive->SetPose(pose);
@@ -191,39 +231,34 @@ frc::Pose2d Vision::AlignToTarget(int target, units::meter_t offset, wom::Swerve
 frc::Pose2d Vision::AlignToTarget(VisionTarget target, frc::Translation2d offset, wom::SwerveDrive* swerveDrive) {
   SetMode(VisionModes::kAprilTag);
 
-  std::pair<units::meter_t, units::degree_t> distance = GetDistanceToTarget(target);
-
+  // Get distance to target
+  AprilTag tag = GetTags()[static_cast<int>(target) - 1];
   // Get current position from Limelight
-  frc::Pose2d current_pose = _limelight->GetPose().ToPose2d();
-  //frc::Pose2d current_pose = frc::Pose2d(0_m, 0_m, 0_deg);
-  
-  units::meter_t x_offset = distance.first * units::math::cos(distance.second);
-  units::meter_t y_offset = distance.first * units::math::sin(distance.second);
+  //frc::Pose2d current_pose = _limelight->GetPose().ToPose2d();
+  frc::Pose2d current_pose = frc::Pose2d(0_m, 0_m, 0_deg);
 
-  // reduce both offsets by the offset parameter (in relative amount)
-  //x_offset -= offset * units::math::cos(distance.second);
-  //y_offset -= offset * units::math::sin(distance.second);
+  units::meter_t a = tag.pos.X() - current_pose.X();
+  units::meter_t b = tag.pos.Y() - current_pose.Y();
 
-  if (offset.X() > 0_m) {
-    x_offset -= offset.X() * units::math::cos(distance.second);
-  } else {
-    x_offset += offset.X() * units::math::cos(distance.second);
-  }
+  units::radian_t theta = units::math::atan2(b, a);
+  
+  //std::cout << "A: " << a.value() << ", B:" << b.value() << std::endl;
+  units::meter_t r = units::meter_t{std::sqrt(std::pow(a.value(), 2) + std::pow(b.value(), 2))};
 
-  if (offset.Y() > 0_m) {
-    y_offset -= offset.Y() * units::math::sin(distance.second);
-  } else {
-    y_offset += offset.Y() * units::math::sin(distance.second);
-  }
+  units::meter_t x = current_pose.X() + r * units::math::cos(theta);
+  units::meter_t y = current_pose.Y() + r * units::math::sin(theta);
 
-  frc::Pose2d pose = frc::Pose2d(y_offset, x_offset, distance.second);
+  // reduce both offsets by the offset parameter (in relative amount)
+  x += offset.X();
+  y += offset.Y();
   
+  frc::Pose2d pose = frc::Pose2d(x, y, theta); 
 
   // Print the results
-  std::cout << "Aligning to tag " << static_cast<int>(target) << ": X: " << pose.X().value() << " Y: " << pose.Y().value() << " Angle: " << pose.Rotation().Degrees().value() << ". At offset: X: " << offset.X().value() << ", Y: " << offset.Y().value() << "." << std::endl;
+  //std::cout << "Aligning to tag " << target << ": X: " << pose.X().value() << " Y: " << pose.Y().value() << " Angle: " << pose.Rotation().Degrees().value() << ". At offset " << offset.value() << "." << std::endl;
 
   // Set the new pose to the swerve drive
-  swerveDrive->SetPose(pose);
+  //swerveDrive->SetPose(pose);
 
   return pose;
 }
@@ -233,19 +268,31 @@ frc::Pose2d Vision::AlignToTarget(int target, frc::Translation2d offset, wom::Sw
 
   // Get distance to target
   std::pair<units::meter_t, units::degree_t> distance = GetDistanceToTarget(target);
-
+  AprilTag tag = GetTags()[target - 1];
   // Get current position from Limelight
-  frc::Pose2d current_pose = _limelight->GetPose().ToPose2d();
-  //frc::Pose2d current_pose = frc::Pose2d(0_m, 0_m, 0_deg);
+  //frc::Pose2d current_pose = _limelight->GetPose().ToPose2d();
+  frc::Pose2d current_pose = frc::Pose2d(0_m, 0_m, 0_deg);
+
+  units::meter_t a = tag.pos.X() - current_pose.X();
+  units::meter_t b = tag.pos.Y() - current_pose.Y();
+
+  units::radian_t theta = units::math::atan2(b, a);
   
-  units::meter_t x_offset = distance.first * units::math::cos(distance.second);
-  units::meter_t y_offset = distance.first * units::math::sin(distance.second);
+  //std::cout << "A: " << a.value() << ", B:" << b.value() << std::endl;
+  units::meter_t r = units::meter_t{std::sqrt(std::pow(a.value(), 2) + std::pow(b.value(), 2))};
+
+  units::meter_t x = current_pose.X() + r * units::math::cos(theta);
+  units::meter_t y = current_pose.Y() + r * units::math::sin(theta);
+
+  // reduce both offsets by the offset parameter (in relative amount)
+  x += offset.X();
+  y += offset.Y();
 
-  frc::Pose2d pose = frc::Pose2d(y_offset, x_offset, distance.second);
   
+  frc::Pose2d pose = frc::Pose2d(x, y, theta); 
 
   // Print the results
-  std::cout << "Aligning to tag " << target << ": X: " << pose.X().value() << " Y: " << pose.Y().value() << " Angle: " << pose.Rotation().Degrees().value() << ". At offset: X: " << offset.X().value() << ", Y: " << offset.Y().value() << "." << std::endl;
+  //std::cout << "Aligning to tag " << target << ": X: " << pose.X().value() << " Y: " << pose.Y().value() << " Angle: " << pose.Rotation().Degrees().value() << ". At offset " << offset.value() << "." << std::endl;
 
   // Set the new pose to the swerve drive
   //swerveDrive->SetPose(pose);

From 9be89e73ca9b7821cb970a8cd20a2f161d2faa49 Mon Sep 17 00:00:00 2001
From: Paul Hodges <paul.hodges@student.education.wa.edu.au>
Date: Mon, 29 Jan 2024 12:40:30 +0800
Subject: [PATCH 145/207] [wpiformat + add ctre_sim to gitignore]

---
 .gitignore                     |   4 ++
 src/main/cpp/Robot.cpp         |  18 ++++-
 src/main/cpp/vision/Vision.cpp | 127 +++++++++++++++++----------------
 3 files changed, 84 insertions(+), 65 deletions(-)

diff --git a/.gitignore b/.gitignore
index 50973484..f78e01b4 100644
--- a/.gitignore
+++ b/.gitignore
@@ -1,6 +1,10 @@
 # This gitignore has been specially created by the WPILib team.
 # If you remove items from this file, intellisense might break.
 
+### Sim ###
+
+ctre_sim
+
 ### C++ ###
 # Prerequisites
 *.d
diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp
index d06d3d95..a7004b64 100644
--- a/src/main/cpp/Robot.cpp
+++ b/src/main/cpp/Robot.cpp
@@ -285,7 +285,19 @@ void Robot::TeleopInit() {
 
 >>>>>>> c83ac05 ([robot/vision] Started work on limelight vision)
 }
+<<<<<<< HEAD
 void Robot::TeleopPeriodic() {}
+=======
+
+void Robot::TeleopPeriodic() {
+  // std::cout << "Current AprilTag: " << _vision->CurrentAprilTag() << std::endl;
+  // std::cout << "Current Target: " << _vision->TargetIsVisible(VisionTargetObjects::kNote) << std::endl;
+  std::cout << "Dist to target: " << _vision->GetDistanceToTarget(VisionTarget::kBlueAmp).first.value()
+            << std::endl;
+  std::cout << "Angle to target: " << _vision->GetDistanceToTarget(VisionTarget::kBlueAmp).second.value()
+            << std::endl;
+}
+>>>>>>> ae1a0fc ([wpiformat + add ctre_sim to gitignore])
 
 void Robot::DisabledInit() {}
 void Robot::DisabledPeriodic() {}
@@ -296,9 +308,9 @@ void Robot::TestPeriodic() {}
 void Robot::SimulationInit() {
   std::string x = "[";
   std::string y = "[";
-  //_vision->GetDistanceToTarget(16);
-  for (int i = 1; i< 17; i++) {
-    for (int j = 0; j<17; j++) {
+  // _vision->GetDistanceToTarget(16);
+  for (int i = 1; i < 17; i++) {
+    for (int j = 0; j < 17; j++) {
       frc::Pose2d pose = _vision->AlignToTarget(i, units::meter_t{j * 0.1}, _swerveDrive);
       x += std::to_string(pose.X().value()) + ",";
       y += std::to_string(pose.Y().value()) + ",";
diff --git a/src/main/cpp/vision/Vision.cpp b/src/main/cpp/vision/Vision.cpp
index 3e8ef676..b79d61d3 100644
--- a/src/main/cpp/vision/Vision.cpp
+++ b/src/main/cpp/vision/Vision.cpp
@@ -3,6 +3,7 @@
 // of the MIT License at the root of this project
 
 #include "vision/Vision.h"
+
 #include "units/math.h"
 
 FMAP::FMAP(std::string path) : _path(path) {
@@ -24,38 +25,39 @@ FMAP::FMAP(std::string path) : _path(path) {
 
   for (auto& fiducial : j["fiducials"]) {
     std::cout << "Loading AprilTag " << fiducial["id"] << std::endl;
-      AprilTag tag;
-      tag.id = fiducial["id"];
-      tag.size = fiducial["size"];
-      if (fiducial["unique"] == 1) {
-          tag.unique = true;
-      } else {
-          tag.unique = false;
-      }
+    AprilTag tag;
+    tag.id = fiducial["id"];
+    tag.size = fiducial["size"];
+    if (fiducial["unique"] == 1) {
+      tag.unique = true;
+    } else {
+      tag.unique = false;
+    }
 
-      const auto& transform = fiducial["transform"];
-      for (int i = 0; i < 4; ++i) {
-          for (int j = 0; j < 4; ++j) {
-               tag.transform[i][j] = transform[i * 4 + j];
-          }
+    const auto& transform = fiducial["transform"];
+    for (int i = 0; i < 4; ++i) {
+      for (int j = 0; j < 4; ++j) {
+        tag.transform[i][j] = transform[i * 4 + j];
       }
+    }
 
-      tag.yaw = units::radian_t{tag.transform[0][0]};
-      tag.pitch = units::radian_t{tag.transform[1][1]};
-      tag.roll = units::radian_t{tag.transform[2][2]};
+    tag.yaw = units::radian_t{tag.transform[0][0]};
+    tag.pitch = units::radian_t{tag.transform[1][1]};
+    tag.roll = units::radian_t{tag.transform[2][2]};
 
-      tag.pos = frc::Pose3d(units::meter_t{tag.transform[0][3]}, units::meter_t{tag.transform[1][3]}, units::meter_t{tag.transform[2][3]}, frc::Rotation3d(0_deg, 0_deg, 0_deg));
+    tag.pos = frc::Pose3d(units::meter_t{tag.transform[0][3]}, units::meter_t{tag.transform[1][3]},
+                          units::meter_t{tag.transform[2][3]}, frc::Rotation3d(0_deg, 0_deg, 0_deg));
 
-       _tags.push_back(tag);
-    }
+    _tags.push_back(tag);
+  }
 
   file.close();
 
   std::cout << "Loaded " << _tags.size() << " tags" << std::endl;
 
   for (int i = 0; i < _tags.size(); i++) {
-    std::cout << "Tag " << _tags[i].id << " is at: X: " << _tags[i].pos.X().value() << " Y: " << _tags[i].pos.Y().value()
-              << " Z: " << _tags[i].pos.Z().value() << std::endl;
+    std::cout << "Tag " << _tags[i].id << " is at: X: " << _tags[i].pos.X().value()
+              << " Y: " << _tags[i].pos.Y().value() << " Z: " << _tags[i].pos.Z().value() << std::endl;
   }
 
   std::cout << "Loaded FMAP" << std::endl;
@@ -83,27 +85,27 @@ std::pair<units::meter_t, units::degree_t> Vision::GetDistanceToTarget(VisionTar
   for (int i = 0; i < tags.size(); i++) {
     if (tags[i].id == static_cast<int>(target)) {
       AprilTag tag = tags[i];
-      //frc::Pose3d pose = _limelight->GetPose();
+      // frc::Pose3d pose = _limelight->GetPose();
       frc::Pose3d pose = frc::Pose3d(0_m, 0_m, 0_m, frc::Rotation3d(0_deg, 0_deg, 0_deg));
 
       SetMode(VisionModes::kAprilTag);
 
       // Get distance to target
       // Get current position from Limelight
-      //frc::Pose2d current_pose = _limelight->GetPose().ToPose2d();
+      // frc::Pose2d current_pose = _limelight->GetPose().ToPose2d();
       frc::Pose2d current_pose = frc::Pose2d(0_m, 0_m, 0_deg);
 
       units::meter_t a = tag.pos.X() - current_pose.X();
       units::meter_t b = tag.pos.Y() - current_pose.Y();
 
       units::radian_t theta = units::math::atan2(b, a);
-      
-      //std::cout << "A: " << a.value() << ", B:" << b.value() << std::endl;
+
+      // std::cout << "A: " << a.value() << ", B:" << b.value() << std::endl;
       units::meter_t r = units::meter_t{std::sqrt(std::pow(a.value(), 2) + std::pow(b.value(), 2))};
 
       units::meter_t x = current_pose.X() + r * units::math::cos(theta);
       units::meter_t y = current_pose.Y() + r * units::math::sin(theta);
-      
+
       return std::make_pair(r, theta);
     }
   }
@@ -117,33 +119,33 @@ std::pair<units::meter_t, units::degree_t> Vision::GetDistanceToTarget(int id) {
   }
 
   SetMode(VisionModes::kAprilTag);
-      
+
   std::vector<AprilTag> tags = _fmap.GetTags();
 
   for (int i = 0; i < tags.size(); i++) {
     if (tags[i].id == id) {
       AprilTag tag = tags[i];
-      //frc::Pose3d pose = _limelight->GetPose();
+      // frc::Pose3d pose = _limelight->GetPose();
       frc::Pose3d pose = frc::Pose3d(0_m, 0_m, 0_m, frc::Rotation3d(0_deg, 0_deg, 0_deg));
 
       SetMode(VisionModes::kAprilTag);
 
       // Get distance to target
       // Get current position from Limelight
-      //frc::Pose2d current_pose = _limelight->GetPose().ToPose2d();
+      // frc::Pose2d current_pose = _limelight->GetPose().ToPose2d();
       frc::Pose2d current_pose = frc::Pose2d(0_m, 0_m, 0_deg);
 
       units::meter_t a = tag.pos.X() - current_pose.X();
       units::meter_t b = tag.pos.Y() - current_pose.Y();
 
       units::radian_t theta = units::math::atan2(b, a);
-      
-      //std::cout << "A: " << a.value() << ", B:" << b.value() << std::endl;
+
+      // std::cout << "A: " << a.value() << ", B:" << b.value() << std::endl;
       units::meter_t r = units::meter_t{std::sqrt(std::pow(a.value(), 2) + std::pow(b.value(), 2))};
 
       units::meter_t x = current_pose.X() + r * units::math::cos(theta);
       units::meter_t y = current_pose.Y() + r * units::math::sin(theta);
-      
+
       return std::make_pair(r, theta);
     }
   }
@@ -161,15 +163,15 @@ frc::Pose2d Vision::AlignToTarget(VisionTarget target, units::meter_t offset, wo
   // Get distance to target
   AprilTag tag = GetTags()[static_cast<int>(target) - 1];
   // Get current position from Limelight
-  //frc::Pose2d current_pose = _limelight->GetPose().ToPose2d();
+  // frc::Pose2d current_pose = _limelight->GetPose().ToPose2d();
   frc::Pose2d current_pose = frc::Pose2d(0_m, 0_m, 0_deg);
 
   units::meter_t a = tag.pos.X() - current_pose.X();
   units::meter_t b = tag.pos.Y() - current_pose.Y();
 
   units::radian_t theta = units::math::atan2(b, a);
-  
-  //std::cout << "A: " << a.value() << ", B:" << b.value() << std::endl;
+
+  // std::cout << "A: " << a.value() << ", B:" << b.value() << std::endl;
   units::meter_t r = units::meter_t{std::sqrt(std::pow(a.value(), 2) + std::pow(b.value(), 2))};
 
   units::meter_t x = current_pose.X() + r * units::math::cos(theta);
@@ -179,14 +181,14 @@ frc::Pose2d Vision::AlignToTarget(VisionTarget target, units::meter_t offset, wo
   x += offset * units::math::cos(theta);
   y += offset * units::math::sin(theta);
 
-  
-  frc::Pose2d pose = frc::Pose2d(x, y, theta); 
+  frc::Pose2d pose = frc::Pose2d(x, y, theta);
 
   // Print the results
-  //std::cout << "Aligning to tag " << target << ": X: " << pose.X().value() << " Y: " << pose.Y().value() << " Angle: " << pose.Rotation().Degrees().value() << ". At offset " << offset.value() << "." << std::endl;
+  // std::cout << "Aligning to tag " << target << ": X: " << pose.X().value() << " Y: " << pose.Y().value() <<
+  // " Angle: " << pose.Rotation().Degrees().value() << ". At offset " << offset.value() << "." << std::endl;
 
   // Set the new pose to the swerve drive
-  //swerveDrive->SetPose(pose);
+  // swerveDrive->SetPose(pose);
 
   return pose;
 }
@@ -198,15 +200,15 @@ frc::Pose2d Vision::AlignToTarget(int target, units::meter_t offset, wom::Swerve
   std::pair<units::meter_t, units::degree_t> distance = GetDistanceToTarget(target);
   AprilTag tag = GetTags()[target - 1];
   // Get current position from Limelight
-  //frc::Pose2d current_pose = _limelight->GetPose().ToPose2d();
+  // frc::Pose2d current_pose = _limelight->GetPose().ToPose2d();
   frc::Pose2d current_pose = frc::Pose2d(0_m, 0_m, 0_deg);
 
   units::meter_t a = tag.pos.X() - current_pose.X();
   units::meter_t b = tag.pos.Y() - current_pose.Y();
 
   units::radian_t theta = units::math::atan2(b, a);
-  
-  //std::cout << "A: " << a.value() << ", B:" << b.value() << std::endl;
+
+  // std::cout << "A: " << a.value() << ", B:" << b.value() << std::endl;
   units::meter_t r = units::meter_t{std::sqrt(std::pow(a.value(), 2) + std::pow(b.value(), 2))};
 
   units::meter_t x = current_pose.X() + r * units::math::cos(theta);
@@ -216,33 +218,34 @@ frc::Pose2d Vision::AlignToTarget(int target, units::meter_t offset, wom::Swerve
   x += offset * units::math::cos(theta);
   y += offset * units::math::sin(theta);
 
-  
-  frc::Pose2d pose = frc::Pose2d(x, y, theta); 
+  frc::Pose2d pose = frc::Pose2d(x, y, theta);
 
   // Print the results
-  //std::cout << "Aligning to tag " << target << ": X: " << pose.X().value() << " Y: " << pose.Y().value() << " Angle: " << pose.Rotation().Degrees().value() << ". At offset " << offset.value() << "." << std::endl;
+  // std::cout << "Aligning to tag " << target << ": X: " << pose.X().value() << " Y: " << pose.Y().value() <<
+  // " Angle: " << pose.Rotation().Degrees().value() << ". At offset " << offset.value() << "." << std::endl;
 
   // Set the new pose to the swerve drive
-  //swerveDrive->SetPose(pose);
+  // swerveDrive->SetPose(pose);
 
   return pose;
 }
 
-frc::Pose2d Vision::AlignToTarget(VisionTarget target, frc::Translation2d offset, wom::SwerveDrive* swerveDrive) {
+frc::Pose2d Vision::AlignToTarget(VisionTarget target, frc::Translation2d offset,
+                                  wom::SwerveDrive* swerveDrive) {
   SetMode(VisionModes::kAprilTag);
 
   // Get distance to target
   AprilTag tag = GetTags()[static_cast<int>(target) - 1];
   // Get current position from Limelight
-  //frc::Pose2d current_pose = _limelight->GetPose().ToPose2d();
+  // frc::Pose2d current_pose = _limelight->GetPose().ToPose2d();
   frc::Pose2d current_pose = frc::Pose2d(0_m, 0_m, 0_deg);
 
   units::meter_t a = tag.pos.X() - current_pose.X();
   units::meter_t b = tag.pos.Y() - current_pose.Y();
 
   units::radian_t theta = units::math::atan2(b, a);
-  
-  //std::cout << "A: " << a.value() << ", B:" << b.value() << std::endl;
+
+  // std::cout << "A: " << a.value() << ", B:" << b.value() << std::endl;
   units::meter_t r = units::meter_t{std::sqrt(std::pow(a.value(), 2) + std::pow(b.value(), 2))};
 
   units::meter_t x = current_pose.X() + r * units::math::cos(theta);
@@ -251,14 +254,15 @@ frc::Pose2d Vision::AlignToTarget(VisionTarget target, frc::Translation2d offset
   // reduce both offsets by the offset parameter (in relative amount)
   x += offset.X();
   y += offset.Y();
-  
-  frc::Pose2d pose = frc::Pose2d(x, y, theta); 
+
+  frc::Pose2d pose = frc::Pose2d(x, y, theta);
 
   // Print the results
-  //std::cout << "Aligning to tag " << target << ": X: " << pose.X().value() << " Y: " << pose.Y().value() << " Angle: " << pose.Rotation().Degrees().value() << ". At offset " << offset.value() << "." << std::endl;
+  // std::cout << "Aligning to tag " << target << ": X: " << pose.X().value() << " Y: " << pose.Y().value() <<
+  // " Angle: " << pose.Rotation().Degrees().value() << ". At offset " << offset.value() << "." << std::endl;
 
   // Set the new pose to the swerve drive
-  //swerveDrive->SetPose(pose);
+  // swerveDrive->SetPose(pose);
 
   return pose;
 }
@@ -270,15 +274,15 @@ frc::Pose2d Vision::AlignToTarget(int target, frc::Translation2d offset, wom::Sw
   std::pair<units::meter_t, units::degree_t> distance = GetDistanceToTarget(target);
   AprilTag tag = GetTags()[target - 1];
   // Get current position from Limelight
-  //frc::Pose2d current_pose = _limelight->GetPose().ToPose2d();
+  // frc::Pose2d current_pose = _limelight->GetPose().ToPose2d();
   frc::Pose2d current_pose = frc::Pose2d(0_m, 0_m, 0_deg);
 
   units::meter_t a = tag.pos.X() - current_pose.X();
   units::meter_t b = tag.pos.Y() - current_pose.Y();
 
   units::radian_t theta = units::math::atan2(b, a);
-  
-  //std::cout << "A: " << a.value() << ", B:" << b.value() << std::endl;
+
+  // std::cout << "A: " << a.value() << ", B:" << b.value() << std::endl;
   units::meter_t r = units::meter_t{std::sqrt(std::pow(a.value(), 2) + std::pow(b.value(), 2))};
 
   units::meter_t x = current_pose.X() + r * units::math::cos(theta);
@@ -288,19 +292,18 @@ frc::Pose2d Vision::AlignToTarget(int target, frc::Translation2d offset, wom::Sw
   x += offset.X();
   y += offset.Y();
 
-  
-  frc::Pose2d pose = frc::Pose2d(x, y, theta); 
+  frc::Pose2d pose = frc::Pose2d(x, y, theta);
 
   // Print the results
-  //std::cout << "Aligning to tag " << target << ": X: " << pose.X().value() << " Y: " << pose.Y().value() << " Angle: " << pose.Rotation().Degrees().value() << ". At offset " << offset.value() << "." << std::endl;
+  // std::cout << "Aligning to tag " << target << ": X: " << pose.X().value() << " Y: " << pose.Y().value() <<
+  // " Angle: " << pose.Rotation().Degrees().value() << ". At offset " << offset.value() << "." << std::endl;
 
   // Set the new pose to the swerve drive
-  //swerveDrive->SetPose(pose);
+  // swerveDrive->SetPose(pose);
 
   return pose;
 }
 
-
 bool Vision::IsAtPose(frc::Pose3d pose, units::second_t dt) {
   return _limelight->IsAtSetPoseVision(pose, dt);
 }

From 83b8d96599b54db535062daac33e8ac22dd7b75c Mon Sep 17 00:00:00 2001
From: Paul Hodges <paul.hodges@student.education.wa.edu.au>
Date: Mon, 29 Jan 2024 15:07:53 +0800
Subject: [PATCH 146/207] Finished vision (hopefully)

---
 src/main/cpp/Robot.cpp           | 27 ++++++++++++++-------------
 src/main/cpp/vision/Vision.cpp   | 31 +++++++++++++++++++++++++++++++
 src/main/include/vision/Vision.h |  4 ++++
 3 files changed, 49 insertions(+), 13 deletions(-)

diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp
index a7004b64..69708b80 100644
--- a/src/main/cpp/Robot.cpp
+++ b/src/main/cpp/Robot.cpp
@@ -306,22 +306,23 @@ void Robot::TestInit() {}
 void Robot::TestPeriodic() {}
 
 void Robot::SimulationInit() {
-  std::string x = "[";
-  std::string y = "[";
-  // _vision->GetDistanceToTarget(16);
-  for (int i = 1; i < 17; i++) {
-    for (int j = 0; j < 17; j++) {
-      frc::Pose2d pose = _vision->AlignToTarget(i, units::meter_t{j * 0.1}, _swerveDrive);
-      x += std::to_string(pose.X().value()) + ",";
-      y += std::to_string(pose.Y().value()) + ",";
+  /*  std::string x = "[";
+    std::string y = "[";
+    // _vision->GetDistanceToTarget(16);
+    for (int i = 1; i < 17; i++) {
+      for (int j = 0; j < 17; j++) {
+        frc::Pose2d pose = _vision->AlignToTarget(i, units::meter_t{j * 0.1}, _swerveDrive);
+        x += std::to_string(pose.X().value()) + ",";
+        y += std::to_string(pose.Y().value()) + ",";
+      }
     }
-  }
 
-  x += "]";
-  y += "]";
+    x += "]";
+    y += "]";
 
-  std::cout << x << std::endl;
-  std::cout << y << std::endl;
+    std::cout << x << std::endl;
+    std::cout << y << std::endl; */
+  _vision->TurnToTarget(1, _swerveDrive);
 }
 
 void Robot::SimulationPeriodic() {}
diff --git a/src/main/cpp/vision/Vision.cpp b/src/main/cpp/vision/Vision.cpp
index b79d61d3..3c834040 100644
--- a/src/main/cpp/vision/Vision.cpp
+++ b/src/main/cpp/vision/Vision.cpp
@@ -4,6 +4,9 @@
 
 #include "vision/Vision.h"
 
+#include <variant>
+
+#include "units/length.h"
 #include "units/math.h"
 
 FMAP::FMAP(std::string path) : _path(path) {
@@ -304,6 +307,34 @@ frc::Pose2d Vision::AlignToTarget(int target, frc::Translation2d offset, wom::Sw
   return pose;
 }
 
+frc::Pose2d Vision::TurnToTarget(int target, wom::SwerveDrive* swerveDrive) {
+  AprilTag tag = GetTags()[target];
+
+  units::degree_t angle = GetDistanceToTarget(target).second;
+
+  frc::Pose2d current_pose = _limelight->GetPose().ToPose2d();
+
+  frc::Pose2d pose = frc::Pose2d(current_pose.X(), current_pose.Y(), angle);
+
+  std::cout << pose.Rotation().Degrees().value() << std::endl;
+
+  swerveDrive->SetPose(pose);
+}
+
+frc::Pose2d Vision::TurnToTarget(VisionTarget target, wom::SwerveDrive* swerveDrive) {
+  AprilTag tag = GetTags()[static_cast<int>(target)];
+
+  units::degree_t angle = GetDistanceToTarget(target).second;
+
+  frc::Pose2d current_pose = _limelight->GetPose().ToPose2d();
+
+  frc::Pose2d pose = frc::Pose2d(current_pose.X(), current_pose.Y(), angle);
+
+  std::cout << pose.Rotation().Degrees().value() << std::endl;
+
+  swerveDrive->SetPose(pose);
+}
+
 bool Vision::IsAtPose(frc::Pose3d pose, units::second_t dt) {
   return _limelight->IsAtSetPoseVision(pose, dt);
 }
diff --git a/src/main/include/vision/Vision.h b/src/main/include/vision/Vision.h
index 4c7a224e..1945b1fe 100644
--- a/src/main/include/vision/Vision.h
+++ b/src/main/include/vision/Vision.h
@@ -38,6 +38,7 @@ enum class VisionTarget {
   kRedAmp = 5,
   kRedSpeakerCenter = 4,
   kRedSpeakerOffset = 3,
+
   kRedChain1 = 12,
   kRedChain2 = 11,
   kRedChain3 = 13,
@@ -90,6 +91,9 @@ class Vision {
   frc::Pose2d AlignToTarget(VisionTarget target, frc::Translation2d offset, wom::SwerveDrive* swerveDrive);
   frc::Pose2d AlignToTarget(int target, frc::Translation2d offset, wom::SwerveDrive* swerveDrive);
 
+  frc::Pose2d TurnToTarget(int target, wom::SwerveDrive* swerveDrive);
+  frc::Pose2d TurnToTarget(VisionTarget target, wom::SwerveDrive* swerveDrive);
+
   std::vector<AprilTag> GetTags();
 
   bool IsAtPose(frc::Pose3d pose, units::second_t dt);

From 542b4086fdb356b1355bccc1ab932f7ad17da189 Mon Sep 17 00:00:00 2001
From: Paul Hodges <paul.hodges@student.education.wa.edu.au>
Date: Mon, 29 Jan 2024 16:39:39 +0800
Subject: [PATCH 147/207] More vision

---
 ctre_sim/CANCoder vers. H - 016 - 0 - ext.dat | Bin 2048 -> 2048 bytes
 ctre_sim/CANCoder vers. H - 017 - 0 - ext.dat | Bin 2048 -> 2048 bytes
 ctre_sim/CANCoder vers. H - 018 - 0 - ext.dat | Bin 2048 -> 2048 bytes
 ctre_sim/CANCoder vers. H - 019 - 0 - ext.dat | Bin 2048 -> 2048 bytes
 ctre_sim/Pigeon 2 - 020 - 0 - ext.dat         | Bin 2048 -> 2048 bytes
 ctre_sim/Talon FX vers. C - 01 - 0 - ext.dat  | Bin 2048 -> 2048 bytes
 ctre_sim/Talon FX vers. C - 02 - 0 - ext.dat  | Bin 2048 -> 2048 bytes
 ctre_sim/Talon FX vers. C - 03 - 0 - ext.dat  | Bin 2048 -> 2048 bytes
 ctre_sim/Talon FX vers. C - 04 - 0 - ext.dat  | Bin 2048 -> 2048 bytes
 ctre_sim/Talon FX vers. C - 05 - 0 - ext.dat  | Bin 2048 -> 2048 bytes
 ctre_sim/Talon FX vers. C - 06 - 0 - ext.dat  | Bin 2048 -> 2048 bytes
 ctre_sim/Talon FX vers. C - 07 - 0 - ext.dat  | Bin 2048 -> 2048 bytes
 ctre_sim/Talon FX vers. C - 09 - 0 - ext.dat  | Bin 2048 -> 2048 bytes
 simgui.json                                   |   3 +++
 src/main/cpp/Robot.cpp                        |  12 ++++++++-
 src/main/cpp/vision/Vision.cpp                |  23 +++++++++++++++---
 src/main/include/vision/Vision.h              |   2 ++
 .../src/main/cpp/drivetrain/SwerveDrive.cpp   |   4 +++
 .../src/main/include/drivetrain/SwerveDrive.h |   4 ++-
 19 files changed, 42 insertions(+), 6 deletions(-)

diff --git a/ctre_sim/CANCoder vers. H - 016 - 0 - ext.dat b/ctre_sim/CANCoder vers. H - 016 - 0 - ext.dat
index b70c3556679497ee603521f2dda7d4398157875e..7af9a7c92dfa5d0375f972414c70d47775fef131 100644
GIT binary patch
delta 36
ncmZn=Xb{+tz{t6k;r7=IP6h^!iGuE&+Zb+q&4O|kF|h#v)s70h

delta 36
ncmZn=Xb{+tz{t6iq5t;=W(Ee1iGuE&yBK<YZ-R0bF|h#v)Rqdo

diff --git a/ctre_sim/CANCoder vers. H - 017 - 0 - ext.dat b/ctre_sim/CANCoder vers. H - 017 - 0 - ext.dat
index be58d0c2d2c75f1ffe8d657c982b4a688f24ce71..669137759f209f811b4743db197a98ea6b470a3a 100644
GIT binary patch
delta 30
kcmZn=Xb{+tz{s|h;o;XT&WVCvY+D$fe9Zu{7BR5_0IH4(yZ`_I

delta 30
kcmZn=Xb{+tz{s|fVe0Qq%o7E@*mf|?_`LzdTExT#0I8M=yZ`_I

diff --git a/ctre_sim/CANCoder vers. H - 018 - 0 - ext.dat b/ctre_sim/CANCoder vers. H - 018 - 0 - ext.dat
index a5a5e02fd7e38bd52b5d2e78c6a069ddb1a7ff1d..8835a41c024e6d4ed9f8c2de8ca7962412fa91d7 100644
GIT binary patch
delta 36
ncmZn=Xb{+tz{t6k;r7=IP6h^!iGuE&+Zb+q&4O|kF|h#v)s70h

delta 36
ncmZn=Xb{+tz{t6iq5t;=W(Ee1iGuE&yBK<YZ-R0bF|h#v)Rqdo

diff --git a/ctre_sim/CANCoder vers. H - 019 - 0 - ext.dat b/ctre_sim/CANCoder vers. H - 019 - 0 - ext.dat
index 1d1df01f2e5379572fc30a4936c29417c965e6e7..e7951dbf07e2c8b3c25e3036a4b58ba15d7ba128 100644
GIT binary patch
delta 30
kcmZn=Xb{+tz{s|h;o;XT&WVCvY+D$fe9Zu{7BR5_0IH4(yZ`_I

delta 30
kcmZn=Xb{+tz{s|fVe0Qq%o7E@*mf|?_`LzdTExT#0I8M=yZ`_I

diff --git a/ctre_sim/Pigeon 2 - 020 - 0 - ext.dat b/ctre_sim/Pigeon 2 - 020 - 0 - ext.dat
index 1c15fadd76a37035c4fb5dc322174ef5e4f0ae3b..941345ac17d7df96e44048b557dc28b3069039c2 100644
GIT binary patch
delta 32
mcmZn=Xb{-2fQfA@!^5vxoD&7T*tRe{`I-S@ZFXQjzybits0<AN

delta 32
mcmZn=Xb{-2fQfA<!_?oKm?sK)vF%`(@p}V^wb_CB01E)funY|V

diff --git a/ctre_sim/Talon FX vers. C - 01 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 01 - 0 - ext.dat
index c286c31b23ccbab2ae10455eb784fdb6c7aba6ba..ee4ebedc65ed22306d2234f177c2d6d5c2236c8b 100644
GIT binary patch
delta 30
kcmZn=Xb{-Yz{qx#;o{dU&WVavY)2Tbe9Zu{7BR5_0Im)S+yDRo

delta 30
kcmZn=Xb{-Yz{qxzq4oDB=81|{Y$q5xes2J=7BR5_0Ie1Z+yDRo

diff --git a/ctre_sim/Talon FX vers. C - 02 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 02 - 0 - ext.dat
index 09761e081c6233b148c8a5cfc9a8e963345694bb..948df76b81bbc157fed419f5e20da094da591c1f 100644
GIT binary patch
delta 30
kcmZn=Xb{-Yz{qx#;o{dU&WVavY)2Tbe9Zu{7BR5_0Im)S+yDRo

delta 30
kcmZn=Xb{-Yz{qxzq4oDB=81|{Y$q5xes2J=7BR5_0Ie1Z+yDRo

diff --git a/ctre_sim/Talon FX vers. C - 03 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 03 - 0 - ext.dat
index 241982f5972195b12caa6927b24a685894f9882b..7c267efd91a0f2b2aa92e9d604ec5e203624088a 100644
GIT binary patch
delta 30
kcmZn=Xb{-Yz{qx#;o{dU&WVavY)2Tbe9Zu{7BR5_0Im)S+yDRo

delta 30
kcmZn=Xb{-Yz{qxzq4oDB=81|{Y$q5xes2J=7BR5_0Ie1Z+yDRo

diff --git a/ctre_sim/Talon FX vers. C - 04 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 04 - 0 - ext.dat
index 891fd4bd1a4cca587d981de44ca775b397e424c3..ac502f4e3315670384588f08e3ae021f583877d8 100644
GIT binary patch
delta 30
kcmZn=Xb{-Yz{qx#;o{dU&WVavY)2Tbe9Zu{7BR5_0Im)S+yDRo

delta 30
kcmZn=Xb{-Yz{qxzq4oDB=81|{Y$q5xes2J=7BR5_0Ie1Z+yDRo

diff --git a/ctre_sim/Talon FX vers. C - 05 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 05 - 0 - ext.dat
index 2fc8ce8768d85b3793cc5eafc3538940ccc1f481..cb9bb70f8f10973dc4bcc2073e2b89feb8aad238 100644
GIT binary patch
delta 30
kcmZn=Xb{-Yz{qx#;o{dU&WVavY)2Tbe9Zu{7BR5_0Im)S+yDRo

delta 30
kcmZn=Xb{-Yz{qxzq4oDB=81|{Y$q5xes2J=7BR5_0Ie1Z+yDRo

diff --git a/ctre_sim/Talon FX vers. C - 06 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 06 - 0 - ext.dat
index ff19a0b9c2d1e5c18ee1df765420fa29bf621fca..113659522d70fa911d9eb144e0d2d24aa7179076 100644
GIT binary patch
delta 30
kcmZn=Xb{-Yz{qx#;o{dU&WVavY)2Tbe9Zu{7BR5_0Im)S+yDRo

delta 30
kcmZn=Xb{-Yz{qxzq4oDB=81|{Y$q5xes2J=7BR5_0Ie1Z+yDRo

diff --git a/ctre_sim/Talon FX vers. C - 07 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 07 - 0 - ext.dat
index b89067fbb4a7264a799dd5bb65befe44e920ef64..b5fbe904afc7719fd295f3b1026074af31236ff6 100644
GIT binary patch
delta 30
kcmZn=Xb{-Yz{qx#;o{dU&WVavY)2Tbe9Zu{7BR5_0Im)S+yDRo

delta 30
kcmZn=Xb{-Yz{qxzq4oDB=81|{Y$q5xes2J=7BR5_0Ie1Z+yDRo

diff --git a/ctre_sim/Talon FX vers. C - 09 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 09 - 0 - ext.dat
index 19f15a02966512f9c542c009f18c5281a245e51c..7eb8bcf1b5a63272f8196b91910c4256f82f322f 100644
GIT binary patch
delta 30
kcmZn=Xb{-Yz{qx#;o{dU&WVavY)2Tbe9Zu{7BR5_0Im)S+yDRo

delta 30
kcmZn=Xb{-Yz{qxzq4oDB=81|{Y$q5xes2J=7BR5_0Ie1Z+yDRo

diff --git a/simgui.json b/simgui.json
index 3cfca74e..17f01c64 100644
--- a/simgui.json
+++ b/simgui.json
@@ -11,6 +11,9 @@
         "pid": {
           "open": true
         }
+      },
+      "vision": {
+        "open": true
       }
     }
   },
diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp
index 69708b80..b4b5adc3 100644
--- a/src/main/cpp/Robot.cpp
+++ b/src/main/cpp/Robot.cpp
@@ -34,6 +34,12 @@
 >>>>>>> 0029f49 (fix some formatting)
 #include "behaviour/HasBehaviour.h"
 
+#include <frc/kinematics/DifferentialDriveKinematics.h>
+#include <frc/controller/RamseteController.h>
+#include <frc/Timer.h>
+#include "networktables/NetworkTableInstance.h"
+
+
 static units::second_t lastPeriodic;
 
 void Robot::RobotInit() {
@@ -322,8 +328,12 @@ void Robot::SimulationInit() {
 
     std::cout << x << std::endl;
     std::cout << y << std::endl; */
-  _vision->TurnToTarget(1, _swerveDrive);
+  //std::cout << _vision->TurnToTarget(1, _swerveDrive).Rotation().Degrees().value() << std::endl;
+  frc::Pose2d pose = _vision->TurnToTarget(2, _swerveDrive);
+  nt::NetworkTableInstance::GetDefault().GetTable("vision")->PutNumber("rot", pose.Rotation().Degrees().value()); 
+  
 }
 
+
 void Robot::SimulationPeriodic() {}
 
diff --git a/src/main/cpp/vision/Vision.cpp b/src/main/cpp/vision/Vision.cpp
index 3c834040..66fcdeed 100644
--- a/src/main/cpp/vision/Vision.cpp
+++ b/src/main/cpp/vision/Vision.cpp
@@ -312,13 +312,16 @@ frc::Pose2d Vision::TurnToTarget(int target, wom::SwerveDrive* swerveDrive) {
 
   units::degree_t angle = GetDistanceToTarget(target).second;
 
-  frc::Pose2d current_pose = _limelight->GetPose().ToPose2d();
+  //frc::Pose2d current_pose = _limelight->GetPose().ToPose2d();
+  frc::Pose2d current_pose = frc::Pose2d();
 
   frc::Pose2d pose = frc::Pose2d(current_pose.X(), current_pose.Y(), angle);
 
-  std::cout << pose.Rotation().Degrees().value() << std::endl;
+  //std::cout << pose.Rotation().Degrees().value() << std::endl;
 
-  swerveDrive->SetPose(pose);
+  //swerveDrive->SetPose(pose);
+  
+  return pose;
 }
 
 frc::Pose2d Vision::TurnToTarget(VisionTarget target, wom::SwerveDrive* swerveDrive) {
@@ -332,7 +335,19 @@ frc::Pose2d Vision::TurnToTarget(VisionTarget target, wom::SwerveDrive* swerveDr
 
   std::cout << pose.Rotation().Degrees().value() << std::endl;
 
-  swerveDrive->SetPose(pose);
+  //swerveDrive->SetPose(pose);
+}
+
+frc::Pose2d Vision::LockOn(VisionTargetObjects object, wom::SwerveDrive* swerveDrive) {
+  SetMode(VisionModes::kRing);
+
+  if (TargetIsVisible(object)) {
+    swerveDrive->SetLocked(frc::Pose2d());
+  } else {
+    swerveDrive->SetIdle();
+  }
+
+  return frc::Pose2d();
 }
 
 bool Vision::IsAtPose(frc::Pose3d pose, units::second_t dt) {
diff --git a/src/main/include/vision/Vision.h b/src/main/include/vision/Vision.h
index 1945b1fe..0dbc7530 100644
--- a/src/main/include/vision/Vision.h
+++ b/src/main/include/vision/Vision.h
@@ -94,6 +94,8 @@ class Vision {
   frc::Pose2d TurnToTarget(int target, wom::SwerveDrive* swerveDrive);
   frc::Pose2d TurnToTarget(VisionTarget target, wom::SwerveDrive* swerveDrive);
 
+  frc::Pose2d LockOn(VisionTargetObjects target, wom::SwerveDrive* swerveDrive);
+
   std::vector<AprilTag> GetTags();
 
   bool IsAtPose(frc::Pose3d pose, units::second_t dt);
diff --git a/wombat/src/main/cpp/drivetrain/SwerveDrive.cpp b/wombat/src/main/cpp/drivetrain/SwerveDrive.cpp
index c98f4cc2..505b1edf 100644
--- a/wombat/src/main/cpp/drivetrain/SwerveDrive.cpp
+++ b/wombat/src/main/cpp/drivetrain/SwerveDrive.cpp
@@ -497,6 +497,10 @@ void SwerveDrive::OnResetMode() {
   _anglePIDController.Reset();
 }
 
+void SwerveDrive::SetLocked(frc::Pose2d pose) {
+  _state = SwerveDriveState::kLockedOn;
+}
+
 void SwerveDrive::RotateMatchJoystick(units::radian_t joystickAngle, FieldRelativeSpeeds speeds) {
   // _state = SwerveDriveState::kFRVelocityRotationLock;
   // _anglePIDController.SetSetpoint(joystickAngle);
diff --git a/wombat/src/main/include/drivetrain/SwerveDrive.h b/wombat/src/main/include/drivetrain/SwerveDrive.h
index 25d27d04..2cb69101 100644
--- a/wombat/src/main/include/drivetrain/SwerveDrive.h
+++ b/wombat/src/main/include/drivetrain/SwerveDrive.h
@@ -152,7 +152,8 @@ enum class SwerveDriveState {
   kTuning,
   kXWheels,
   kModuleTurn,
-  kFRVelocityRotationLock
+  kFRVelocityRotationLock,
+  kLockedOn
 };
 
 struct FieldRelativeSpeeds {
@@ -200,6 +201,7 @@ class SwerveDrive : public behaviour::HasBehaviour {
   void SetZero();
   void SetVoltageLimit(units::volt_t driveVoltageLimit);
   void OnResetMode();
+  void SetLocked(frc::Pose2d pose);
   // double GetModuleCANPosition(int mod);  // from liam's
 
   void SetXWheelState();

From de137b3f7ed36ddfc78e78a2c1127fd9fd0dbe2b Mon Sep 17 00:00:00 2001
From: Paul Hodges <paul.hodges@student.education.wa.edu.au>
Date: Tue, 30 Jan 2024 10:25:48 +0800
Subject: [PATCH 148/207] Merge with swerve stuff

---
 src/main/cpp/vision/Vision.cpp | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/main/cpp/vision/Vision.cpp b/src/main/cpp/vision/Vision.cpp
index 66fcdeed..a3c4c5fb 100644
--- a/src/main/cpp/vision/Vision.cpp
+++ b/src/main/cpp/vision/Vision.cpp
@@ -106,8 +106,8 @@ std::pair<units::meter_t, units::degree_t> Vision::GetDistanceToTarget(VisionTar
       // std::cout << "A: " << a.value() << ", B:" << b.value() << std::endl;
       units::meter_t r = units::meter_t{std::sqrt(std::pow(a.value(), 2) + std::pow(b.value(), 2))};
 
-      units::meter_t x = current_pose.X() + r * units::math::cos(theta);
-      units::meter_t y = current_pose.Y() + r * units::math::sin(theta);
+      // units::meter_t x = current_pose.X() + r * units::math::cos(theta);
+      // units::meter_t y = current_pose.Y() + r * units::math::sin(theta);
 
       return std::make_pair(r, theta);
     }

From ed25becb930c194ba713aa9cd420c551fcfbcb27 Mon Sep 17 00:00:00 2001
From: Paul Hodges <paul.hodges@student.education.wa.edu.au>
Date: Tue, 30 Jan 2024 12:57:21 +0800
Subject: [PATCH 149/207] More vision

---
 src/main/cpp/vision/Vision.cpp                | 64 ++++++++++++-------
 src/main/include/vision/Vision.h              |  6 +-
 .../src/main/cpp/drivetrain/SwerveDrive.cpp   | 16 ++++-
 .../behaviours/SwerveBehaviours.cpp           |  8 +++
 .../src/main/include/drivetrain/SwerveDrive.h | 11 +++-
 5 files changed, 78 insertions(+), 27 deletions(-)

diff --git a/src/main/cpp/vision/Vision.cpp b/src/main/cpp/vision/Vision.cpp
index a3c4c5fb..2261e06b 100644
--- a/src/main/cpp/vision/Vision.cpp
+++ b/src/main/cpp/vision/Vision.cpp
@@ -4,10 +4,12 @@
 
 #include "vision/Vision.h"
 
+#include <utility>
 #include <variant>
 
 #include "units/length.h"
 #include "units/math.h"
+#include "vision/Limelight.h"
 
 FMAP::FMAP(std::string path) : _path(path) {
   std::cout << "Parsing FMAP" << std::endl;
@@ -88,15 +90,15 @@ std::pair<units::meter_t, units::degree_t> Vision::GetDistanceToTarget(VisionTar
   for (int i = 0; i < tags.size(); i++) {
     if (tags[i].id == static_cast<int>(target)) {
       AprilTag tag = tags[i];
-      // frc::Pose3d pose = _limelight->GetPose();
-      frc::Pose3d pose = frc::Pose3d(0_m, 0_m, 0_m, frc::Rotation3d(0_deg, 0_deg, 0_deg));
+      //frc::Pose3d pose = _limelight->GetPose();
+      //frc::Pose3d pose = frc::Pose3d(0_m, 0_m, 0_m, frc::Rotation3d(0_deg, 0_deg, 0_deg));
 
       SetMode(VisionModes::kAprilTag);
 
       // Get distance to target
       // Get current position from Limelight
-      // frc::Pose2d current_pose = _limelight->GetPose().ToPose2d();
-      frc::Pose2d current_pose = frc::Pose2d(0_m, 0_m, 0_deg);
+      frc::Pose2d current_pose = _limelight->GetPose().ToPose2d();
+      //frc::Pose2d current_pose = frc::Pose2d(0_m, 0_m, 0_deg);
 
       units::meter_t a = tag.pos.X() - current_pose.X();
       units::meter_t b = tag.pos.Y() - current_pose.Y();
@@ -128,15 +130,15 @@ std::pair<units::meter_t, units::degree_t> Vision::GetDistanceToTarget(int id) {
   for (int i = 0; i < tags.size(); i++) {
     if (tags[i].id == id) {
       AprilTag tag = tags[i];
-      // frc::Pose3d pose = _limelight->GetPose();
-      frc::Pose3d pose = frc::Pose3d(0_m, 0_m, 0_m, frc::Rotation3d(0_deg, 0_deg, 0_deg));
+      //frc::Pose3d pose = _limelight->GetPose();
+      //frc::Pose3d pose = frc::Pose3d(0_m, 0_m, 0_m, frc::Rotation3d(0_deg, 0_deg, 0_deg));
 
       SetMode(VisionModes::kAprilTag);
 
       // Get distance to target
       // Get current position from Limelight
-      // frc::Pose2d current_pose = _limelight->GetPose().ToPose2d();
-      frc::Pose2d current_pose = frc::Pose2d(0_m, 0_m, 0_deg);
+      frc::Pose2d current_pose = _limelight->GetPose().ToPose2d();
+      //frc::Pose2d current_pose = frc::Pose2d(0_m, 0_m, 0_deg);
 
       units::meter_t a = tag.pos.X() - current_pose.X();
       units::meter_t b = tag.pos.Y() - current_pose.Y();
@@ -166,8 +168,8 @@ frc::Pose2d Vision::AlignToTarget(VisionTarget target, units::meter_t offset, wo
   // Get distance to target
   AprilTag tag = GetTags()[static_cast<int>(target) - 1];
   // Get current position from Limelight
-  // frc::Pose2d current_pose = _limelight->GetPose().ToPose2d();
-  frc::Pose2d current_pose = frc::Pose2d(0_m, 0_m, 0_deg);
+  frc::Pose2d current_pose = _limelight->GetPose().ToPose2d();
+  //frc::Pose2d current_pose = frc::Pose2d(0_m, 0_m, 0_deg);
 
   units::meter_t a = tag.pos.X() - current_pose.X();
   units::meter_t b = tag.pos.Y() - current_pose.Y();
@@ -203,8 +205,8 @@ frc::Pose2d Vision::AlignToTarget(int target, units::meter_t offset, wom::Swerve
   std::pair<units::meter_t, units::degree_t> distance = GetDistanceToTarget(target);
   AprilTag tag = GetTags()[target - 1];
   // Get current position from Limelight
-  // frc::Pose2d current_pose = _limelight->GetPose().ToPose2d();
-  frc::Pose2d current_pose = frc::Pose2d(0_m, 0_m, 0_deg);
+  frc::Pose2d current_pose = _limelight->GetPose().ToPose2d();
+  //frc::Pose2d current_pose = frc::Pose2d(0_m, 0_m, 0_deg);
 
   units::meter_t a = tag.pos.X() - current_pose.X();
   units::meter_t b = tag.pos.Y() - current_pose.Y();
@@ -240,8 +242,8 @@ frc::Pose2d Vision::AlignToTarget(VisionTarget target, frc::Translation2d offset
   // Get distance to target
   AprilTag tag = GetTags()[static_cast<int>(target) - 1];
   // Get current position from Limelight
-  // frc::Pose2d current_pose = _limelight->GetPose().ToPose2d();
-  frc::Pose2d current_pose = frc::Pose2d(0_m, 0_m, 0_deg);
+  frc::Pose2d current_pose = _limelight->GetPose().ToPose2d();
+  //frc::Pose2d current_pose = frc::Pose2d(0_m, 0_m, 0_deg);
 
   units::meter_t a = tag.pos.X() - current_pose.X();
   units::meter_t b = tag.pos.Y() - current_pose.Y();
@@ -277,8 +279,8 @@ frc::Pose2d Vision::AlignToTarget(int target, frc::Translation2d offset, wom::Sw
   std::pair<units::meter_t, units::degree_t> distance = GetDistanceToTarget(target);
   AprilTag tag = GetTags()[target - 1];
   // Get current position from Limelight
-  // frc::Pose2d current_pose = _limelight->GetPose().ToPose2d();
-  frc::Pose2d current_pose = frc::Pose2d(0_m, 0_m, 0_deg);
+  frc::Pose2d current_pose = _limelight->GetPose().ToPose2d();
+  //frc::Pose2d current_pose = frc::Pose2d(0_m, 0_m, 0_deg);
 
   units::meter_t a = tag.pos.X() - current_pose.X();
   units::meter_t b = tag.pos.Y() - current_pose.Y();
@@ -312,8 +314,8 @@ frc::Pose2d Vision::TurnToTarget(int target, wom::SwerveDrive* swerveDrive) {
 
   units::degree_t angle = GetDistanceToTarget(target).second;
 
-  //frc::Pose2d current_pose = _limelight->GetPose().ToPose2d();
-  frc::Pose2d current_pose = frc::Pose2d();
+  frc::Pose2d current_pose = _limelight->GetPose().ToPose2d();
+  //frc::Pose2d current_pose = frc::Pose2d();
 
   frc::Pose2d pose = frc::Pose2d(current_pose.X(), current_pose.Y(), angle);
 
@@ -338,16 +340,32 @@ frc::Pose2d Vision::TurnToTarget(VisionTarget target, wom::SwerveDrive* swerveDr
   //swerveDrive->SetPose(pose);
 }
 
-frc::Pose2d Vision::LockOn(VisionTargetObjects object, wom::SwerveDrive* swerveDrive) {
+std::pair<frc::Pose2d, units::degree_t> Vision::GetAngleToObject(VisionTargetObjects object) {
   SetMode(VisionModes::kRing);
 
   if (TargetIsVisible(object)) {
-    swerveDrive->SetLocked(frc::Pose2d());
+    frc::Pose2d pose = _limelight->GetPose().ToPose2d();
+
+    units::degree_t offset = units::degree_t{_limelight->GetOffset().first}; // degrees are how much the robot has to turn for the target to be in the center 
+   
+    frc::Pose2d new_pose = frc::Pose2d(pose.X(), pose.Y(), offset);
+
+    return std::make_pair(new_pose, offset);
   } else {
-    swerveDrive->SetIdle();
+    return std::make_pair(frc::Pose2d(), 0_deg);
   }
+}
 
-  return frc::Pose2d();
+units::degree_t Vision::LockOn(VisionTargetObjects object, wom::SwerveDrive* swerveDrive) {
+  SetMode(VisionModes::kRing);
+  
+  units::degree_t angle = -1000_deg;
+
+  if (TargetIsVisible(object)) {
+    angle = GetAngleToObject(object).second;
+  }
+
+  return angle;
 }
 
 bool Vision::IsAtPose(frc::Pose3d pose, units::second_t dt) {
@@ -355,6 +373,8 @@ bool Vision::IsAtPose(frc::Pose3d pose, units::second_t dt) {
 }
 
 void Vision::SetMode(VisionModes mode) {
+  if (static_cast<int>(mode) == _limelight->GetTargetingData(wom::LimelightTargetingData::kGetpipe)) { return; }
+
   switch (mode) {
     case VisionModes::kAprilTag: {
       _limelight->SetPipeline(wom::LimelightPipeline::kPipeline0);
diff --git a/src/main/include/vision/Vision.h b/src/main/include/vision/Vision.h
index 0dbc7530..951c27e7 100644
--- a/src/main/include/vision/Vision.h
+++ b/src/main/include/vision/Vision.h
@@ -15,6 +15,7 @@
 #include <vector>
 
 #include "Wombat.h"
+#include "units/angle.h"
 
 #define APRILTAGS_MAX 16
 #define APRILTAGS_MIN 0
@@ -93,8 +94,9 @@ class Vision {
 
   frc::Pose2d TurnToTarget(int target, wom::SwerveDrive* swerveDrive);
   frc::Pose2d TurnToTarget(VisionTarget target, wom::SwerveDrive* swerveDrive);
-
-  frc::Pose2d LockOn(VisionTargetObjects target, wom::SwerveDrive* swerveDrive);
+  
+  std::pair<frc::Pose2d, units::degree_t> GetAngleToObject(VisionTargetObjects object); 
+  units::degree_t LockOn(VisionTargetObjects target, wom::SwerveDrive* swerveDrive);
 
   std::vector<AprilTag> GetTags();
 
diff --git a/wombat/src/main/cpp/drivetrain/SwerveDrive.cpp b/wombat/src/main/cpp/drivetrain/SwerveDrive.cpp
index 505b1edf..df4d704d 100644
--- a/wombat/src/main/cpp/drivetrain/SwerveDrive.cpp
+++ b/wombat/src/main/cpp/drivetrain/SwerveDrive.cpp
@@ -18,7 +18,11 @@
 #include <cmath>
 #include <iostream>
 
+<<<<<<< HEAD
 #include "frc/MathUtil.h"
+=======
+#include "units/mass.h"
+>>>>>>> befb502 (More vision)
 #include "utils/Util.h"
 #include "wpimath/MathShared.h"
 
@@ -358,7 +362,11 @@ void SwerveDrive::OnUpdate(units::second_t dt) {
   switch (_state) {
     case SwerveDriveState::kZeroing:
       for (auto mod = _modules.begin(); mod < _modules.end(); mod++) {
+<<<<<<< HEAD
         // mod->SetZero(dt);
+=======
+      mod->SetZero(dt);
+>>>>>>> befb502 (More vision)
       }
       break;
     case SwerveDriveState::kIdle:
@@ -429,7 +437,7 @@ void SwerveDrive::OnUpdate(units::second_t dt) {
 
     case SwerveDriveState::kTuning:
       for (size_t i = 0; i < _modules.size(); i++) {
-        _modules[i].SetPID(_angle, _speed, dt);
+        _modules[i].SetPID(_angle, _speed, dt);      
       }
       break;
     case SwerveDriveState::kXWheels:
@@ -439,6 +447,7 @@ void SwerveDrive::OnUpdate(units::second_t dt) {
       _modules[3].SetPID(315_deg, 0_mps, dt);
       break;
     case SwerveDriveState::kFRVelocityRotationLock:
+      {
       _target_speed.vx = _xPIDController.Calculate(GetPose().X(), dt);
       _target_speed.vy = _yPIDController.Calculate(GetPose().Y(), dt);
       _target_speed.omega =
@@ -449,6 +458,8 @@ void SwerveDrive::OnUpdate(units::second_t dt) {
         _modules[i].SetPID(target_states[i].angle.Radians(), target_states[i].speed, dt);
       }
       break;
+      }
+    
   }
 
   for (auto mod = _modules.begin(); mod < _modules.end(); mod++) {
@@ -497,8 +508,9 @@ void SwerveDrive::OnResetMode() {
   _anglePIDController.Reset();
 }
 
-void SwerveDrive::SetLocked(frc::Pose2d pose) {
+void SwerveDrive::SetLocked(units::degree_t angle) {
   _state = SwerveDriveState::kLockedOn;
+  _lockedAngle = angle;
 }
 
 void SwerveDrive::RotateMatchJoystick(units::radian_t joystickAngle, FieldRelativeSpeeds speeds) {
diff --git a/wombat/src/main/cpp/drivetrain/behaviours/SwerveBehaviours.cpp b/wombat/src/main/cpp/drivetrain/behaviours/SwerveBehaviours.cpp
index be06811e..85600a3d 100644
--- a/wombat/src/main/cpp/drivetrain/behaviours/SwerveBehaviours.cpp
+++ b/wombat/src/main/cpp/drivetrain/behaviours/SwerveBehaviours.cpp
@@ -32,6 +32,7 @@ void ManualDrivebase::OnStart() {
 }
 
 void ManualDrivebase::OnTick(units::second_t deltaTime) {
+
   // if (_driverController->GetXButtonPressed()) {
   //   ResetMode();
   //   isRotateMatch = !isRotateMatch;
@@ -85,6 +86,13 @@ void ManualDrivebase::OnTick(units::second_t deltaTime) {
   if (num < turningDeadzone) {
     turnX = 0;
     turnY = 0;
+
+  if (_driverController->GetXButtonPressed()) {
+    ResetMode();
+    
+    isRotateMatch = !isRotateMatch;
+
+  }
   }
 
   // if (isRotateMatch) {
diff --git a/wombat/src/main/include/drivetrain/SwerveDrive.h b/wombat/src/main/include/drivetrain/SwerveDrive.h
index 2cb69101..997e6641 100644
--- a/wombat/src/main/include/drivetrain/SwerveDrive.h
+++ b/wombat/src/main/include/drivetrain/SwerveDrive.h
@@ -201,7 +201,7 @@ class SwerveDrive : public behaviour::HasBehaviour {
   void SetZero();
   void SetVoltageLimit(units::volt_t driveVoltageLimit);
   void OnResetMode();
-  void SetLocked(frc::Pose2d pose);
+  void SetLocked(units::degree_t pose);
   // double GetModuleCANPosition(int mod);  // from liam's
 
   void SetXWheelState();
@@ -247,10 +247,19 @@ class SwerveDrive : public behaviour::HasBehaviour {
   units::radian_t _angle;
   units::meters_per_second_t _speed;
 
+<<<<<<< HEAD
   // double frontLeftEncoderOffset = -143.26171875;
   // double frontRightEncoderOffset = 167.87109375;
   // double backLeftEncoderOffset = -316.669921875;
   // double backRightEncoderOffset = -119.619140625;
+=======
+  double frontLeftEncoderOffset = -143.26171875;
+  double frontRightEncoderOffset = 167.87109375;
+  double backLeftEncoderOffset = -316.669921875;
+  double backRightEncoderOffset = -119.619140625;
+
+  units::degree_t _lockedAngle;
+>>>>>>> befb502 (More vision)
 };
 }  // namespace drivetrain
 }  // namespace wom
\ No newline at end of file

From 65bc79b9541c7b8770d5eb4f4e3309a861dfed4c Mon Sep 17 00:00:00 2001
From: Paul Hodges <paul.hodges@student.education.wa.edu.au>
Date: Tue, 30 Jan 2024 12:57:39 +0800
Subject: [PATCH 150/207] [wpiformat]

---
 src/main/cpp/Robot.cpp                        |  8 ++--
 src/main/cpp/vision/Vision.cpp                | 42 ++++++++++---------
 src/main/include/vision/Vision.h              |  4 +-
 .../src/main/cpp/drivetrain/SwerveDrive.cpp   | 12 +++---
 .../behaviours/SwerveBehaviours.cpp           |  2 +-
 5 files changed, 37 insertions(+), 31 deletions(-)

diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp
index b4b5adc3..b96af365 100644
--- a/src/main/cpp/Robot.cpp
+++ b/src/main/cpp/Robot.cpp
@@ -328,12 +328,12 @@ void Robot::SimulationInit() {
 
     std::cout << x << std::endl;
     std::cout << y << std::endl; */
-  //std::cout << _vision->TurnToTarget(1, _swerveDrive).Rotation().Degrees().value() << std::endl;
+  // std::cout << _vision->TurnToTarget(1, _swerveDrive).Rotation().Degrees().value() << std::endl;
   frc::Pose2d pose = _vision->TurnToTarget(2, _swerveDrive);
-  nt::NetworkTableInstance::GetDefault().GetTable("vision")->PutNumber("rot", pose.Rotation().Degrees().value()); 
-  
+  nt::NetworkTableInstance::GetDefault().GetTable("vision")->PutNumber("rot",
+                                                                       pose.Rotation().Degrees().value());
 }
 
-
 void Robot::SimulationPeriodic() {}
 
+
diff --git a/src/main/cpp/vision/Vision.cpp b/src/main/cpp/vision/Vision.cpp
index 2261e06b..33871ec0 100644
--- a/src/main/cpp/vision/Vision.cpp
+++ b/src/main/cpp/vision/Vision.cpp
@@ -90,15 +90,15 @@ std::pair<units::meter_t, units::degree_t> Vision::GetDistanceToTarget(VisionTar
   for (int i = 0; i < tags.size(); i++) {
     if (tags[i].id == static_cast<int>(target)) {
       AprilTag tag = tags[i];
-      //frc::Pose3d pose = _limelight->GetPose();
-      //frc::Pose3d pose = frc::Pose3d(0_m, 0_m, 0_m, frc::Rotation3d(0_deg, 0_deg, 0_deg));
+      // frc::Pose3d pose = _limelight->GetPose();
+      // frc::Pose3d pose = frc::Pose3d(0_m, 0_m, 0_m, frc::Rotation3d(0_deg, 0_deg, 0_deg));
 
       SetMode(VisionModes::kAprilTag);
 
       // Get distance to target
       // Get current position from Limelight
       frc::Pose2d current_pose = _limelight->GetPose().ToPose2d();
-      //frc::Pose2d current_pose = frc::Pose2d(0_m, 0_m, 0_deg);
+      // frc::Pose2d current_pose = frc::Pose2d(0_m, 0_m, 0_deg);
 
       units::meter_t a = tag.pos.X() - current_pose.X();
       units::meter_t b = tag.pos.Y() - current_pose.Y();
@@ -130,15 +130,15 @@ std::pair<units::meter_t, units::degree_t> Vision::GetDistanceToTarget(int id) {
   for (int i = 0; i < tags.size(); i++) {
     if (tags[i].id == id) {
       AprilTag tag = tags[i];
-      //frc::Pose3d pose = _limelight->GetPose();
-      //frc::Pose3d pose = frc::Pose3d(0_m, 0_m, 0_m, frc::Rotation3d(0_deg, 0_deg, 0_deg));
+      // frc::Pose3d pose = _limelight->GetPose();
+      // frc::Pose3d pose = frc::Pose3d(0_m, 0_m, 0_m, frc::Rotation3d(0_deg, 0_deg, 0_deg));
 
       SetMode(VisionModes::kAprilTag);
 
       // Get distance to target
       // Get current position from Limelight
       frc::Pose2d current_pose = _limelight->GetPose().ToPose2d();
-      //frc::Pose2d current_pose = frc::Pose2d(0_m, 0_m, 0_deg);
+      // frc::Pose2d current_pose = frc::Pose2d(0_m, 0_m, 0_deg);
 
       units::meter_t a = tag.pos.X() - current_pose.X();
       units::meter_t b = tag.pos.Y() - current_pose.Y();
@@ -169,7 +169,7 @@ frc::Pose2d Vision::AlignToTarget(VisionTarget target, units::meter_t offset, wo
   AprilTag tag = GetTags()[static_cast<int>(target) - 1];
   // Get current position from Limelight
   frc::Pose2d current_pose = _limelight->GetPose().ToPose2d();
-  //frc::Pose2d current_pose = frc::Pose2d(0_m, 0_m, 0_deg);
+  // frc::Pose2d current_pose = frc::Pose2d(0_m, 0_m, 0_deg);
 
   units::meter_t a = tag.pos.X() - current_pose.X();
   units::meter_t b = tag.pos.Y() - current_pose.Y();
@@ -206,7 +206,7 @@ frc::Pose2d Vision::AlignToTarget(int target, units::meter_t offset, wom::Swerve
   AprilTag tag = GetTags()[target - 1];
   // Get current position from Limelight
   frc::Pose2d current_pose = _limelight->GetPose().ToPose2d();
-  //frc::Pose2d current_pose = frc::Pose2d(0_m, 0_m, 0_deg);
+  // frc::Pose2d current_pose = frc::Pose2d(0_m, 0_m, 0_deg);
 
   units::meter_t a = tag.pos.X() - current_pose.X();
   units::meter_t b = tag.pos.Y() - current_pose.Y();
@@ -243,7 +243,7 @@ frc::Pose2d Vision::AlignToTarget(VisionTarget target, frc::Translation2d offset
   AprilTag tag = GetTags()[static_cast<int>(target) - 1];
   // Get current position from Limelight
   frc::Pose2d current_pose = _limelight->GetPose().ToPose2d();
-  //frc::Pose2d current_pose = frc::Pose2d(0_m, 0_m, 0_deg);
+  // frc::Pose2d current_pose = frc::Pose2d(0_m, 0_m, 0_deg);
 
   units::meter_t a = tag.pos.X() - current_pose.X();
   units::meter_t b = tag.pos.Y() - current_pose.Y();
@@ -280,7 +280,7 @@ frc::Pose2d Vision::AlignToTarget(int target, frc::Translation2d offset, wom::Sw
   AprilTag tag = GetTags()[target - 1];
   // Get current position from Limelight
   frc::Pose2d current_pose = _limelight->GetPose().ToPose2d();
-  //frc::Pose2d current_pose = frc::Pose2d(0_m, 0_m, 0_deg);
+  // frc::Pose2d current_pose = frc::Pose2d(0_m, 0_m, 0_deg);
 
   units::meter_t a = tag.pos.X() - current_pose.X();
   units::meter_t b = tag.pos.Y() - current_pose.Y();
@@ -315,14 +315,14 @@ frc::Pose2d Vision::TurnToTarget(int target, wom::SwerveDrive* swerveDrive) {
   units::degree_t angle = GetDistanceToTarget(target).second;
 
   frc::Pose2d current_pose = _limelight->GetPose().ToPose2d();
-  //frc::Pose2d current_pose = frc::Pose2d();
+  // frc::Pose2d current_pose = frc::Pose2d();
 
   frc::Pose2d pose = frc::Pose2d(current_pose.X(), current_pose.Y(), angle);
 
-  //std::cout << pose.Rotation().Degrees().value() << std::endl;
+  // std::cout << pose.Rotation().Degrees().value() << std::endl;
+
+  // swerveDrive->SetPose(pose);
 
-  //swerveDrive->SetPose(pose);
-  
   return pose;
 }
 
@@ -337,7 +337,7 @@ frc::Pose2d Vision::TurnToTarget(VisionTarget target, wom::SwerveDrive* swerveDr
 
   std::cout << pose.Rotation().Degrees().value() << std::endl;
 
-  //swerveDrive->SetPose(pose);
+  // swerveDrive->SetPose(pose);
 }
 
 std::pair<frc::Pose2d, units::degree_t> Vision::GetAngleToObject(VisionTargetObjects object) {
@@ -346,8 +346,10 @@ std::pair<frc::Pose2d, units::degree_t> Vision::GetAngleToObject(VisionTargetObj
   if (TargetIsVisible(object)) {
     frc::Pose2d pose = _limelight->GetPose().ToPose2d();
 
-    units::degree_t offset = units::degree_t{_limelight->GetOffset().first}; // degrees are how much the robot has to turn for the target to be in the center 
-   
+    units::degree_t offset = units::degree_t{
+        _limelight->GetOffset()
+            .first};  // degrees are how much the robot has to turn for the target to be in the center
+
     frc::Pose2d new_pose = frc::Pose2d(pose.X(), pose.Y(), offset);
 
     return std::make_pair(new_pose, offset);
@@ -358,7 +360,7 @@ std::pair<frc::Pose2d, units::degree_t> Vision::GetAngleToObject(VisionTargetObj
 
 units::degree_t Vision::LockOn(VisionTargetObjects object, wom::SwerveDrive* swerveDrive) {
   SetMode(VisionModes::kRing);
-  
+
   units::degree_t angle = -1000_deg;
 
   if (TargetIsVisible(object)) {
@@ -373,7 +375,9 @@ bool Vision::IsAtPose(frc::Pose3d pose, units::second_t dt) {
 }
 
 void Vision::SetMode(VisionModes mode) {
-  if (static_cast<int>(mode) == _limelight->GetTargetingData(wom::LimelightTargetingData::kGetpipe)) { return; }
+  if (static_cast<int>(mode) == _limelight->GetTargetingData(wom::LimelightTargetingData::kGetpipe)) {
+    return;
+  }
 
   switch (mode) {
     case VisionModes::kAprilTag: {
diff --git a/src/main/include/vision/Vision.h b/src/main/include/vision/Vision.h
index 951c27e7..8bbef0a9 100644
--- a/src/main/include/vision/Vision.h
+++ b/src/main/include/vision/Vision.h
@@ -94,8 +94,8 @@ class Vision {
 
   frc::Pose2d TurnToTarget(int target, wom::SwerveDrive* swerveDrive);
   frc::Pose2d TurnToTarget(VisionTarget target, wom::SwerveDrive* swerveDrive);
-  
-  std::pair<frc::Pose2d, units::degree_t> GetAngleToObject(VisionTargetObjects object); 
+
+  std::pair<frc::Pose2d, units::degree_t> GetAngleToObject(VisionTargetObjects object);
   units::degree_t LockOn(VisionTargetObjects target, wom::SwerveDrive* swerveDrive);
 
   std::vector<AprilTag> GetTags();
diff --git a/wombat/src/main/cpp/drivetrain/SwerveDrive.cpp b/wombat/src/main/cpp/drivetrain/SwerveDrive.cpp
index df4d704d..f3ac707c 100644
--- a/wombat/src/main/cpp/drivetrain/SwerveDrive.cpp
+++ b/wombat/src/main/cpp/drivetrain/SwerveDrive.cpp
@@ -362,11 +362,15 @@ void SwerveDrive::OnUpdate(units::second_t dt) {
   switch (_state) {
     case SwerveDriveState::kZeroing:
       for (auto mod = _modules.begin(); mod < _modules.end(); mod++) {
+<<<<<<< HEAD
 <<<<<<< HEAD
         // mod->SetZero(dt);
 =======
       mod->SetZero(dt);
 >>>>>>> befb502 (More vision)
+=======
+        mod->SetZero(dt);
+>>>>>>> 57e941a ([wpiformat])
       }
       break;
     case SwerveDriveState::kIdle:
@@ -437,7 +441,7 @@ void SwerveDrive::OnUpdate(units::second_t dt) {
 
     case SwerveDriveState::kTuning:
       for (size_t i = 0; i < _modules.size(); i++) {
-        _modules[i].SetPID(_angle, _speed, dt);      
+        _modules[i].SetPID(_angle, _speed, dt);
       }
       break;
     case SwerveDriveState::kXWheels:
@@ -446,8 +450,7 @@ void SwerveDrive::OnUpdate(units::second_t dt) {
       _modules[2].SetPID(225_deg, 0_mps, dt);
       _modules[3].SetPID(315_deg, 0_mps, dt);
       break;
-    case SwerveDriveState::kFRVelocityRotationLock:
-      {
+    case SwerveDriveState::kFRVelocityRotationLock: {
       _target_speed.vx = _xPIDController.Calculate(GetPose().X(), dt);
       _target_speed.vy = _yPIDController.Calculate(GetPose().Y(), dt);
       _target_speed.omega =
@@ -458,8 +461,7 @@ void SwerveDrive::OnUpdate(units::second_t dt) {
         _modules[i].SetPID(target_states[i].angle.Radians(), target_states[i].speed, dt);
       }
       break;
-      }
-    
+    }
   }
 
   for (auto mod = _modules.begin(); mod < _modules.end(); mod++) {
diff --git a/wombat/src/main/cpp/drivetrain/behaviours/SwerveBehaviours.cpp b/wombat/src/main/cpp/drivetrain/behaviours/SwerveBehaviours.cpp
index 85600a3d..46a0121d 100644
--- a/wombat/src/main/cpp/drivetrain/behaviours/SwerveBehaviours.cpp
+++ b/wombat/src/main/cpp/drivetrain/behaviours/SwerveBehaviours.cpp
@@ -89,7 +89,7 @@ void ManualDrivebase::OnTick(units::second_t deltaTime) {
 
   if (_driverController->GetXButtonPressed()) {
     ResetMode();
-    
+
     isRotateMatch = !isRotateMatch;
 
   }

From 07e7dddf59a384f1530ed77783e34b8d8359c263 Mon Sep 17 00:00:00 2001
From: Paul Hodges <paul.hodges@student.education.wa.edu.au>
Date: Tue, 30 Jan 2024 14:12:59 +0800
Subject: [PATCH 151/207] Finished turning to note

---
 src/main/cpp/vision/Vision.cpp | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/main/cpp/vision/Vision.cpp b/src/main/cpp/vision/Vision.cpp
index 33871ec0..9761613a 100644
--- a/src/main/cpp/vision/Vision.cpp
+++ b/src/main/cpp/vision/Vision.cpp
@@ -350,9 +350,9 @@ std::pair<frc::Pose2d, units::degree_t> Vision::GetAngleToObject(VisionTargetObj
         _limelight->GetOffset()
             .first};  // degrees are how much the robot has to turn for the target to be in the center
 
-    frc::Pose2d new_pose = frc::Pose2d(pose.X(), pose.Y(), offset);
+    pose.RotateBy(offset);
 
-    return std::make_pair(new_pose, offset);
+    return std::make_pair(pose, offset);
   } else {
     return std::make_pair(frc::Pose2d(), 0_deg);
   }

From 984b9c1c6cb4ed601503c4f382311111d04249c4 Mon Sep 17 00:00:00 2001
From: Paul Hodges <paul.hodges@student.education.wa.edu.au>
Date: Tue, 30 Jan 2024 14:14:03 +0800
Subject: [PATCH 152/207] [wpiformat]

---
 src/main/cpp/vision/Vision.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/cpp/vision/Vision.cpp b/src/main/cpp/vision/Vision.cpp
index 9761613a..165a4b1d 100644
--- a/src/main/cpp/vision/Vision.cpp
+++ b/src/main/cpp/vision/Vision.cpp
@@ -361,7 +361,7 @@ std::pair<frc::Pose2d, units::degree_t> Vision::GetAngleToObject(VisionTargetObj
 units::degree_t Vision::LockOn(VisionTargetObjects object, wom::SwerveDrive* swerveDrive) {
   SetMode(VisionModes::kRing);
 
-  units::degree_t angle = -1000_deg;
+  units::degree_t angle;
 
   if (TargetIsVisible(object)) {
     angle = GetAngleToObject(object).second;

From a9125e90fb918d6f1e3888af316f680a80140202 Mon Sep 17 00:00:00 2001
From: Paul Hodges <paul.hodges@student.education.wa.edu.au>
Date: Tue, 30 Jan 2024 14:56:50 +0800
Subject: [PATCH 153/207] Merged with swerve

---
 src/main/cpp/Robot.cpp                        | 32 ++++++++++++-------
 src/main/include/Robot.h                      | 11 ++-----
 .../behaviours/SwerveBehaviours.cpp           | 10 +++---
 3 files changed, 27 insertions(+), 26 deletions(-)

diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp
index b96af365..77fe89b7 100644
--- a/src/main/cpp/Robot.cpp
+++ b/src/main/cpp/Robot.cpp
@@ -33,22 +33,20 @@
 =======
 >>>>>>> 0029f49 (fix some formatting)
 #include "behaviour/HasBehaviour.h"
-
-#include <frc/kinematics/DifferentialDriveKinematics.h>
-#include <frc/controller/RamseteController.h>
-#include <frc/Timer.h>
 #include "networktables/NetworkTableInstance.h"
 
-
 static units::second_t lastPeriodic;
 
 void Robot::RobotInit() {
 <<<<<<< HEAD
 <<<<<<< HEAD
+<<<<<<< HEAD
 =======
 =======
 
 >>>>>>> c83ac05 ([robot/vision] Started work on limelight vision)
+=======
+>>>>>>> bf9b1b4 (Merged with swerve)
   shooter = new Shooter(robotmap.shooterSystem.config);
   wom::BehaviourScheduler::GetInstance()->Register(shooter);
   shooter->SetDefaultBehaviour(
@@ -60,7 +58,6 @@ void Robot::RobotInit() {
 
   // m_chooser.SetDefaultOption("Default Auto", "Default Auto");
 
-
   // frc::SmartDashboard::PutData("Auto Selector", &m_chooser);
 
   // m_path_chooser.SetDefaultOption("Path1", "paths/output/Path1.wpilib.json");
@@ -173,14 +170,17 @@ void Robot::RobotInit() {
   intake->SetDefaultBehaviour(
       [this]() { return wom::make<IntakeManualControl>(intake, robotmap.controllers.codriver); });
 
-  //_vision = new Vision("limelight", FMAP("fmap.fmap"));
+  // _vision = new Vision("limelight", FMAP("fmap.fmap"));
 
 <<<<<<< HEAD
 >>>>>>> c83ac05 ([robot/vision] Started work on limelight vision)
 =======
   _vision = new Vision("limelight", FMAP("fmap.fmap"));
+<<<<<<< HEAD
 
 >>>>>>> c89d969 ([wpiformat] Run wpiformat)
+=======
+>>>>>>> bf9b1b4 (Merged with swerve)
 }
 
 void Robot::RobotPeriodic() {
@@ -221,6 +221,7 @@ void Robot::RobotPeriodic() {
 >>>>>>> 0029f49 (fix some formatting)
       .SetDouble(robotmap.swerveBase.moduleConfigs[3].turnMotor.encoder->GetEncoderPosition().value());
 
+<<<<<<< HEAD
 <<<<<<< HEAD
   // shooter->OnUpdate(dt);
   // intake->OnUpdate(dt);
@@ -228,6 +229,8 @@ void Robot::RobotPeriodic() {
 =======
 
 >>>>>>> c83ac05 ([robot/vision] Started work on limelight vision)
+=======
+>>>>>>> bf9b1b4 (Merged with swerve)
   _swerveDrive->OnUpdate(dt);
 <<<<<<< HEAD
 =======
@@ -239,8 +242,11 @@ void Robot::RobotPeriodic() {
 =======
 
   // _swerveDrive->OnUpdate(dt);
+<<<<<<< HEAD
 
 >>>>>>> c83ac05 ([robot/vision] Started work on limelight vision)
+=======
+>>>>>>> bf9b1b4 (Merged with swerve)
 }
 
 void Robot::AutonomousInit() {
@@ -258,8 +264,6 @@ void Robot::AutonomousPeriodic() {}
 >>>>>>> 0029f49 (fix some formatting)
 
 void Robot::TeleopInit() {
-
-
   loop.Clear();
   wom::BehaviourScheduler* sched = wom::BehaviourScheduler::GetInstance();
 <<<<<<< HEAD
@@ -281,15 +285,21 @@ void Robot::TeleopInit() {
   // backLeft->SetVoltage(4_V);
   // backRight->SetVoltage(4_V);
 <<<<<<< HEAD
+<<<<<<< HEAD
 =======
   
+=======
+>>>>>>> bf9b1b4 (Merged with swerve)
 
-//  FMAP("fmap.fmap");
+  //  FMAP("fmap.fmap");
 
   // _swerveDrive->OnStart();
   // sched->InterruptAll();
+<<<<<<< HEAD
 
 >>>>>>> c83ac05 ([robot/vision] Started work on limelight vision)
+=======
+>>>>>>> bf9b1b4 (Merged with swerve)
 }
 <<<<<<< HEAD
 void Robot::TeleopPeriodic() {}
@@ -335,5 +345,3 @@ void Robot::SimulationInit() {
 }
 
 void Robot::SimulationPeriodic() {}
-
-
diff --git a/src/main/include/Robot.h b/src/main/include/Robot.h
index ca1a5096..febba223 100644
--- a/src/main/include/Robot.h
+++ b/src/main/include/Robot.h
@@ -20,15 +20,10 @@
 #include "Intake.h"
 #include "IntakeBehaviour.h"
 #include "RobotMap.h"
-
-
 #include "Shooter.h"
 #include "ShooterBehaviour.h"
-
-#include "vision/Vision.h"
-
 #include "Wombat.h"
-
+#include "vision/Vision.h"
 
 class Robot : public frc::TimedRobot {
  public:
@@ -42,6 +37,8 @@ class Robot : public frc::TimedRobot {
   void TeleopPeriodic() override;
   void DisabledInit() override;
   void DisabledPeriodic() override;
+  void SimulationInit() override;
+  void SimulationPeriodic() override;
 
  private:
   RobotMap robotmap;
@@ -68,7 +65,6 @@ class Robot : public frc::TimedRobot {
   ctre::phoenix6::hardware::TalonFX* frontLeft;
   // AlphaArm *alphaArm;
 
-
   AlphaArm* alphaArm;
 
   // ctre::phoenix6::hardware::TalonFX *frontLeft;
@@ -80,6 +76,5 @@ class Robot : public frc::TimedRobot {
 =======
 
   Vision* _vision;
-
 };
 >>>>>>> c83ac05 ([robot/vision] Started work on limelight vision)
diff --git a/wombat/src/main/cpp/drivetrain/behaviours/SwerveBehaviours.cpp b/wombat/src/main/cpp/drivetrain/behaviours/SwerveBehaviours.cpp
index 46a0121d..969fa265 100644
--- a/wombat/src/main/cpp/drivetrain/behaviours/SwerveBehaviours.cpp
+++ b/wombat/src/main/cpp/drivetrain/behaviours/SwerveBehaviours.cpp
@@ -32,7 +32,6 @@ void ManualDrivebase::OnStart() {
 }
 
 void ManualDrivebase::OnTick(units::second_t deltaTime) {
-
   // if (_driverController->GetXButtonPressed()) {
   //   ResetMode();
   //   isRotateMatch = !isRotateMatch;
@@ -87,12 +86,11 @@ void ManualDrivebase::OnTick(units::second_t deltaTime) {
     turnX = 0;
     turnY = 0;
 
-  if (_driverController->GetXButtonPressed()) {
-    ResetMode();
-
-    isRotateMatch = !isRotateMatch;
+    if (_driverController->GetXButtonPressed()) {
+      ResetMode();
 
-  }
+      isRotateMatch = !isRotateMatch;
+    }
   }
 
   // if (isRotateMatch) {

From c182ca31e16753592bd476490d49e68a59b7101b Mon Sep 17 00:00:00 2001
From: Paul Hodges <paul.hodges@student.education.wa.edu.au>
Date: Tue, 30 Jan 2024 15:08:22 +0800
Subject: [PATCH 154/207] Did more vision stuff

---
 src/main/cpp/Robot.cpp           | 9 +++++++++
 src/main/cpp/vision/Vision.cpp   | 2 +-
 src/main/include/vision/Vision.h | 2 +-
 3 files changed, 11 insertions(+), 2 deletions(-)

diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp
index 77fe89b7..68d35044 100644
--- a/src/main/cpp/Robot.cpp
+++ b/src/main/cpp/Robot.cpp
@@ -296,15 +296,21 @@ void Robot::TeleopInit() {
   // _swerveDrive->OnStart();
   // sched->InterruptAll();
 <<<<<<< HEAD
+<<<<<<< HEAD
 
 >>>>>>> c83ac05 ([robot/vision] Started work on limelight vision)
 =======
 >>>>>>> bf9b1b4 (Merged with swerve)
+=======
+
+  _swerveDrive->SetPose(_vision->GetAngleToObject(VisionTargetObjects::kNote).first);
+>>>>>>> 87c92ed (Did more vision stuff)
 }
 <<<<<<< HEAD
 void Robot::TeleopPeriodic() {}
 =======
 
+<<<<<<< HEAD
 void Robot::TeleopPeriodic() {
   // std::cout << "Current AprilTag: " << _vision->CurrentAprilTag() << std::endl;
   // std::cout << "Current Target: " << _vision->TargetIsVisible(VisionTargetObjects::kNote) << std::endl;
@@ -314,6 +320,9 @@ void Robot::TeleopPeriodic() {
             << std::endl;
 }
 >>>>>>> ae1a0fc ([wpiformat + add ctre_sim to gitignore])
+=======
+void Robot::TeleopPeriodic() {}
+>>>>>>> 87c92ed (Did more vision stuff)
 
 void Robot::DisabledInit() {}
 void Robot::DisabledPeriodic() {}
diff --git a/src/main/cpp/vision/Vision.cpp b/src/main/cpp/vision/Vision.cpp
index 165a4b1d..ad231926 100644
--- a/src/main/cpp/vision/Vision.cpp
+++ b/src/main/cpp/vision/Vision.cpp
@@ -358,7 +358,7 @@ std::pair<frc::Pose2d, units::degree_t> Vision::GetAngleToObject(VisionTargetObj
   }
 }
 
-units::degree_t Vision::LockOn(VisionTargetObjects object, wom::SwerveDrive* swerveDrive) {
+units::degree_t Vision::LockOn(VisionTargetObjects object) {
   SetMode(VisionModes::kRing);
 
   units::degree_t angle;
diff --git a/src/main/include/vision/Vision.h b/src/main/include/vision/Vision.h
index 8bbef0a9..1cb234e1 100644
--- a/src/main/include/vision/Vision.h
+++ b/src/main/include/vision/Vision.h
@@ -96,7 +96,7 @@ class Vision {
   frc::Pose2d TurnToTarget(VisionTarget target, wom::SwerveDrive* swerveDrive);
 
   std::pair<frc::Pose2d, units::degree_t> GetAngleToObject(VisionTargetObjects object);
-  units::degree_t LockOn(VisionTargetObjects target, wom::SwerveDrive* swerveDrive);
+  units::degree_t LockOn(VisionTargetObjects target);
 
   std::vector<AprilTag> GetTags();
 

From 53a32c5dc1939be666a23b047946be97d62f2fb6 Mon Sep 17 00:00:00 2001
From: Paul Hodges <paul.hodges@student.education.wa.edu.au>
Date: Mon, 12 Feb 2024 17:02:47 +0800
Subject: [PATCH 155/207] begin merge

---
 src/main/include/RobotMap.h | 8 ++++----
 1 file changed, 4 insertions(+), 4 deletions(-)

diff --git a/src/main/include/RobotMap.h b/src/main/include/RobotMap.h
index e0e52350..fef4522f 100644
--- a/src/main/include/RobotMap.h
+++ b/src/main/include/RobotMap.h
@@ -54,7 +54,7 @@ struct RobotMap {
 =======
 =======
   struct IntakeSystem {
-    rev::CANSparkMax intakeMotor{2, rev::CANSparkMax::MotorType::kBrushed};
+    rev::CANSparkMax intakeMotor{15, rev::CANSparkMax::MotorType::kBrushed};
     // wom::CANSparkMaxEncoder intakeEncoder{&intakeMotor, 0.1_m};
     frc::DigitalInput intakeSensor{4};
     // frc::DigitalInput magSensor{0};
@@ -68,7 +68,7 @@ struct RobotMap {
 
 >>>>>>> c30477a (Intake - Manual/Auto fixes (#114))
   struct Shooter {
-    rev::CANSparkMax shooterMotor{11, rev::CANSparkMax::MotorType::kBrushless};  // Port 11
+    rev::CANSparkMax shooterMotor{1, rev::CANSparkMax::MotorType::kBrushless};  // Port 11
     // frc::DigitalInput shooterSensor{2};
 
     // wom::VoltageController shooterMotorGroup =
@@ -313,8 +313,8 @@ struct RobotMap {
 =======
 
   struct AlphaArmSystem {
-    rev::CANSparkMax alphaArmMotor{12, rev::CANSparkMax::MotorType::kBrushless};
-    rev::CANSparkMax wristMotor{15, rev::CANSparkMax::MotorType::kBrushless};
+    rev::CANSparkMax alphaArmMotor{6, rev::CANSparkMax::MotorType::kBrushless};
+    rev::CANSparkMax wristMotor{11, rev::CANSparkMax::MotorType::kBrushless};
 
     wom::Gearbox alphaArmGearbox{&alphaArmMotor, nullptr, frc::DCMotor::NEO(1)};
     wom::Gearbox wristGearbox{&wristMotor, nullptr, frc::DCMotor::NEO(1)};

From 9837926959633eec0803764f9e5c18b87a295449 Mon Sep 17 00:00:00 2001
From: Paul Hodges <paul.hodges@student.education.wa.edu.au>
Date: Mon, 12 Feb 2024 18:36:56 +0800
Subject: [PATCH 156/207] Merged

---
 src/main/cpp/Robot.cpp                        | 183 +++---------------
 src/main/include/Robot.h                      |  20 +-
 src/main/include/RobotMap.h                   | 164 ++++++++--------
 wombat/src/main/cpp/drivetrain/Drivetrain.cpp |  51 -----
 .../src/main/cpp/drivetrain/SwerveDrive.cpp   | 148 +++++++-------
 wombat/src/main/cpp/subsystems/Arm.cpp        | 183 ------------------
 wombat/src/main/cpp/subsystems/Elevator.cpp   | 141 --------------
 wombat/src/main/cpp/subsystems/Shooter.cpp    |  86 --------
 wombat/src/main/cpp/utils/Encoder.cpp         |  10 +-
 .../src/main/include/drivetrain/SwerveDrive.h |  12 +-
 wombat/src/main/include/subsystems/Arm.h      |  81 --------
 wombat/src/main/include/subsystems/Shooter.h  |  75 -------
 wombat/src/main/include/utils/Encoder.h       |   2 +
 wombat/src/main/include/utils/PID.h           |   7 +-
 14 files changed, 207 insertions(+), 956 deletions(-)
 delete mode 100644 wombat/src/main/cpp/drivetrain/Drivetrain.cpp
 delete mode 100644 wombat/src/main/cpp/subsystems/Arm.cpp
 delete mode 100644 wombat/src/main/cpp/subsystems/Elevator.cpp
 delete mode 100644 wombat/src/main/cpp/subsystems/Shooter.cpp
 delete mode 100644 wombat/src/main/include/subsystems/Arm.h
 delete mode 100644 wombat/src/main/include/subsystems/Shooter.h

diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp
index a7d157af..f09b1685 100644
--- a/src/main/cpp/Robot.cpp
+++ b/src/main/cpp/Robot.cpp
@@ -18,22 +18,25 @@
 #include <units/velocity.h>
 #include <units/voltage.h>
 
+// include units
+#include <units/velocity.h>
+#include <units/acceleration.h>
+#include <units/length.h>
+#include <units/angle.h>
+#include <units/time.h>
+#include <units/voltage.h>
+
+#include <frc/kinematics/DifferentialDriveKinematics.h>
+#include <frc/controller/RamseteController.h>
+#include <frc/Timer.h>
 #include "behaviour/HasBehaviour.h"
-#include "networktables/NetworkTableInstance.h"
 
 static units::second_t lastPeriodic;
 
 void Robot::RobotInit() {
-  shooter = new Shooter(robotmap.shooterSystem.config);
-  wom::BehaviourScheduler::GetInstance()->Register(shooter);
-  shooter->SetDefaultBehaviour(
-      [this]() { return wom::make<ShooterManualControl>(shooter, &robotmap.controllers.codriver); });
-
   sched = wom::BehaviourScheduler::GetInstance();
   m_chooser.SetDefaultOption("Default Auto", "Default Auto");
 
-  // m_chooser.SetDefaultOption("Default Auto", "Default Auto");
-
   // frc::SmartDashboard::PutData("Auto Selector", &m_chooser);
 
   // m_path_chooser.SetDefaultOption("Path1", "paths/output/Path1.wpilib.json");
@@ -41,6 +44,12 @@ void Robot::RobotInit() {
   // m_path_chooser.AddOption("Path1", "paths/output/Path1.wpilib.json");
   // m_path_chooser.AddOption("Path2", "paths/output/Path2.wpilib.json");
 
+  // shooter = new Shooter(robotmap.shooterSystem.config);
+  // wom::BehaviourScheduler::GetInstance()->Register(shooter);
+  // shooter->SetDefaultBehaviour(
+  //     [this]() { return wom::make<ShooterManualControl>(shooter, &robotmap.controllers.codriver); });
+
+  // frc::SmartDashboard::PutData("Path Selector", &m_path_chooser);
   // frc::SmartDashboard::PutData("Path Selector", &m_path_chooser);
 
   // frc::SmartDashboard::PutData("Field", &m_field);
@@ -49,38 +58,11 @@ void Robot::RobotInit() {
 
   // robotmap.swerveBase.gyro->Reset();
 
-  // _swerveDrive = new wom::SwerveDrive(robotmap.swerveBase.config, frc::Pose2d());
-  // wom::BehaviourScheduler::GetInstance()->Register(_swerveDrive);
-  // _swerveDrive->SetDefaultBehaviour(
-  //     [this]() { return wom::make<wom::ManualDrivebase>(_swerveDrive, &robotmap.controllers.driver); });
-
-  // simulation_timer = frc::Timer();
-
-  // robotmap.swerveBase.gyro->Reset();
-
-<<<<<<< HEAD
-<<<<<<< HEAD
-<<<<<<< HEAD
-=======
-=======
->>>>>>> c83ac05 ([robot/vision] Started work on limelight vision)
-  // _swerveDrive = new wom::SwerveDrive(robotmap.swerveBase.config, frc::Pose2d());
-  // wom::BehaviourScheduler::GetInstance()->Register(_swerveDrive);
-  // _swerveDrive->SetDefaultBehaviour(
-  //     [this]() { return wom::make<wom::ManualDrivebase>(_swerveDrive, &robotmap.controllers.driver); });
-<<<<<<< HEAD
->>>>>>> 0029f49 (fix some formatting)
-=======
->>>>>>> c30477a (Intake - Manual/Auto fixes (#114))
-=======
-
->>>>>>> c83ac05 ([robot/vision] Started work on limelight vision)
   _swerveDrive = new wom::SwerveDrive(robotmap.swerveBase.config, frc::Pose2d());
   wom::BehaviourScheduler::GetInstance()->Register(_swerveDrive);
   _swerveDrive->SetDefaultBehaviour(
       [this]() { return wom::make<wom::ManualDrivebase>(_swerveDrive, &robotmap.controllers.driver); });
 
-<<<<<<< HEAD
   // alphaArm = new AlphaArm(robotmap.alphaArmSystem.config);
   // wom::BehaviourScheduler::GetInstance()->Register(alphaArm);
   // alphaArm->SetDefaultBehaviour(
@@ -117,38 +99,7 @@ void Robot::RobotInit() {
   // robotmap.swerveBase.moduleConfigs[2].turnMotor.encoder->SetEncoderOffset(0_rad);
   // robotmap.swerveBase.moduleConfigs[3].turnMotor.encoder->SetEncoderOffset(0_rad);
 
-=======
-  // m_driveSim = new wom::TempSimSwerveDrive(&simulation_timer, &m_field);
-  // m_driveSim = wom::TempSimSwerveDrive();
-
-  alphaArm = new AlphaArm(robotmap.alphaArmSystem.config);
-  wom::BehaviourScheduler::GetInstance()->Register(alphaArm);
-  alphaArm->SetDefaultBehaviour(
-      [this]() { return wom::make<AlphaArmManualControl>(alphaArm, &robotmap.controllers.codriver); });
-
-  robotmap.swerveBase.moduleConfigs[0].turnMotor.encoder->SetEncoderOffset(0_rad);
-  robotmap.swerveBase.moduleConfigs[1].turnMotor.encoder->SetEncoderOffset(0_rad);
-  robotmap.swerveBase.moduleConfigs[2].turnMotor.encoder->SetEncoderOffset(0_rad);
-  robotmap.swerveBase.moduleConfigs[3].turnMotor.encoder->SetEncoderOffset(0_rad);
-
-  // frontLeft = new ctre::phoenix6::hardware::TalonFX(7, "Drivebase");  // front left
-  // frontRight = new ctre::phoenix6::hardware::TalonFX(2, "Drivebase");   // front right
-  // backLeft = new ctre::phoenix6::hardware::TalonFX(6, "Drivebase");   // back left
-  // backRight = new ctre::phoenix6::hardware::TalonFX(4, "Drivebase");  // back right
-  // frontLeft = new ctre::phoenix6::hardware::TalonFX(9, "Drivebase");   // front left
-  // frontRight = new ctre::phoenix6::hardware::TalonFX(1, "Drivebase");   // front right
-  // backLeft = new ctre::phoenix6::hardware::TalonFX(5, "Drivebase");   // back left
-  // backRight = new ctre::phoenix6::hardware::TalonFX(3, "Drivebase");
   lastPeriodic = wom::now();
-
-  intake = new Intake(robotmap.intakeSystem.config);
-  wom::BehaviourScheduler::GetInstance()->Register(intake);
-  intake->SetDefaultBehaviour(
-      [this]() { return wom::make<IntakeManualControl>(intake, robotmap.controllers.codriver); });
-
-  // _vision = new Vision("limelight", FMAP("fmap.fmap"));
-
-  _vision = new Vision("limelight", FMAP("fmap.fmap"));
 }
 
 void Robot::RobotPeriodic() {
@@ -158,21 +109,8 @@ void Robot::RobotPeriodic() {
 
   loop.Poll();
   wom::BehaviourScheduler::GetInstance()->Tick();
-  shooter->OnUpdate(dt);
   sched->Tick();
 
-  robotmap.swerveTable.swerveDriveTable->GetEntry("frontLeftEncoder")
-      .SetDouble(robotmap.swerveBase.moduleConfigs[0].turnMotor.encoder->GetEncoderPosition().value());
-  robotmap.swerveTable.swerveDriveTable->GetEntry("frontRightEncoder")
-      .SetDouble(robotmap.swerveBase.moduleConfigs[1].turnMotor.encoder->GetEncoderPosition().value());
-  robotmap.swerveTable.swerveDriveTable->GetEntry("backLeftEncoder")
-      .SetDouble(robotmap.swerveBase.moduleConfigs[2].turnMotor.encoder->GetEncoderPosition().value());
-  robotmap.swerveTable.swerveDriveTable->GetEntry("backRightEncoder")
-      .SetDouble(robotmap.swerveBase.moduleConfigs[3].turnMotor.encoder->GetEncoderPosition().value());
-
-<<<<<<< HEAD
-<<<<<<< HEAD
-<<<<<<< HEAD
   robotmap.swerveTable.swerveDriveTable->GetEntry("Encoder 0 offset: ")
       .SetDouble(robotmap.swerveBase.moduleConfigs[0].turnMotor.encoder->GetEncoderPosition().value());
   robotmap.swerveTable.swerveDriveTable->GetEntry("Encoder 1 offset: ")
@@ -180,111 +118,40 @@ void Robot::RobotPeriodic() {
   robotmap.swerveTable.swerveDriveTable->GetEntry("Encoder 2 offset: ")
       .SetDouble(robotmap.swerveBase.moduleConfigs[2].turnMotor.encoder->GetEncoderPosition().value());
   robotmap.swerveTable.swerveDriveTable->GetEntry("Encoder 3 offset: ")
-=======
-  robotmap.swerveTable.swerveDriveTable->GetEntry("frontLeftEncoder")
-=======
-  robotmap.swerveTable.swerveDriveTable
-      ->GetEntry("frontLeftEncoder")
-
->>>>>>> 57d11c0 (Shooter pid (#117))
-=======
-  robotmap.swerveTable.swerveDriveTable->GetEntry("frontLeftEncoder")
->>>>>>> c30477a (Intake - Manual/Auto fixes (#114))
-      .SetDouble(robotmap.swerveBase.moduleConfigs[0].turnMotor.encoder->GetEncoderPosition().value());
-  robotmap.swerveTable.swerveDriveTable->GetEntry("frontRightEncoder")
-      .SetDouble(robotmap.swerveBase.moduleConfigs[1].turnMotor.encoder->GetEncoderPosition().value());
-  robotmap.swerveTable.swerveDriveTable->GetEntry("backLeftEncoder")
-      .SetDouble(robotmap.swerveBase.moduleConfigs[2].turnMotor.encoder->GetEncoderPosition().value());
-  robotmap.swerveTable.swerveDriveTable->GetEntry("backRightEncoder")
->>>>>>> 0029f49 (fix some formatting)
       .SetDouble(robotmap.swerveBase.moduleConfigs[3].turnMotor.encoder->GetEncoderPosition().value());
 
-<<<<<<< HEAD
-<<<<<<< HEAD
   // shooter->OnUpdate(dt);
   // intake->OnUpdate(dt);
   // alphaArm->OnUpdate(dt);
-=======
-
->>>>>>> c83ac05 ([robot/vision] Started work on limelight vision)
-=======
->>>>>>> bf9b1b4 (Merged with swerve)
   _swerveDrive->OnUpdate(dt);
-  alphaArm->OnUpdate(dt);
-  shooter->OnStart();
-  intake->OnUpdate(dt);
-
-  // _swerveDrive->OnUpdate(dt);
 }
 
 void Robot::AutonomousInit() {
+  // m_driveSim->SetPath(m_path_chooser.GetSelected());
+
   loop.Clear();
   sched->InterruptAll();
 }
-void Robot::AutonomousPeriodic() {}
+void Robot::AutonomousPeriodic() {
+  // m_driveSim->OnUpdate();
+}
 
 void Robot::TeleopInit() {
   loop.Clear();
   wom::BehaviourScheduler* sched = wom::BehaviourScheduler::GetInstance();
+  // shooter->OnStart();
+  // alphaArm->OnStart();
   sched->InterruptAll();
 
   // frontLeft->SetVoltage(4_V);
   // frontRight->SetVoltage(4_V);
   // backLeft->SetVoltage(4_V);
   // backRight->SetVoltage(4_V);
-
-  //  FMAP("fmap.fmap");
-
-  // _swerveDrive->OnStart();
-  // sched->InterruptAll();
-
-  _swerveDrive->SetPose(_vision->GetAngleToObject(VisionTargetObjects::kNote).first);
 }
-
-void Robot::TeleopPeriodic() {}
-=======
-
-<<<<<<< HEAD
-void Robot::TeleopPeriodic() {
-  // std::cout << "Current AprilTag: " << _vision->CurrentAprilTag() << std::endl;
-  // std::cout << "Current Target: " << _vision->TargetIsVisible(VisionTargetObjects::kNote) << std::endl;
-  std::cout << "Dist to target: " << _vision->GetDistanceToTarget(VisionTarget::kBlueAmp).first.value()
-            << std::endl;
-  std::cout << "Angle to target: " << _vision->GetDistanceToTarget(VisionTarget::kBlueAmp).second.value()
-            << std::endl;
-}
->>>>>>> ae1a0fc ([wpiformat + add ctre_sim to gitignore])
-=======
 void Robot::TeleopPeriodic() {}
->>>>>>> 87c92ed (Did more vision stuff)
 
 void Robot::DisabledInit() {}
 void Robot::DisabledPeriodic() {}
 
 void Robot::TestInit() {}
-void Robot::TestPeriodic() {}
-
-void Robot::SimulationInit() {
-  /*  std::string x = "[";
-    std::string y = "[";
-    // _vision->GetDistanceToTarget(16);
-    for (int i = 1; i < 17; i++) {
-      for (int j = 0; j < 17; j++) {
-        frc::Pose2d pose = _vision->AlignToTarget(i, units::meter_t{j * 0.1}, _swerveDrive);
-        x += std::to_string(pose.X().value()) + ",";
-        y += std::to_string(pose.Y().value()) + ",";
-      }
-    }
-
-    x += "]";
-    y += "]";
-
-    std::cout << x << std::endl;
-    std::cout << y << std::endl; */
-  // std::cout << _vision->TurnToTarget(1, _swerveDrive).Rotation().Degrees().value() << std::endl;
-  frc::Pose2d pose = _vision->TurnToTarget(2, _swerveDrive);
-  nt::NetworkTableInstance::GetDefault().GetTable("vision")->PutNumber("rot",
-                                                                       pose.Rotation().Degrees().value());
-}
-
-void Robot::SimulationPeriodic() {}
+void Robot::TestPeriodic() {}
\ No newline at end of file
diff --git a/src/main/include/Robot.h b/src/main/include/Robot.h
index 81a817e2..40728250 100644
--- a/src/main/include/Robot.h
+++ b/src/main/include/Robot.h
@@ -23,7 +23,6 @@
 #include "Shooter.h"
 #include "ShooterBehaviour.h"
 #include "Wombat.h"
-#include "vision/Vision.h"
 
 class Robot : public frc::TimedRobot {
  public:
@@ -37,16 +36,14 @@ class Robot : public frc::TimedRobot {
   void TeleopPeriodic() override;
   void DisabledInit() override;
   void DisabledPeriodic() override;
-  void SimulationInit() override;
-  void SimulationPeriodic() override;
 
  private:
   RobotMap robotmap;
   wom::BehaviourScheduler* sched;
   frc::EventLoop loop;
-  Shooter* shooter;
+  // Shooter* shooter;
 
-  Intake* intake;
+  // Intake* intake;
   frc::SendableChooser<std::string> m_chooser;
 
   frc::Field2d m_field;
@@ -56,18 +53,13 @@ class Robot : public frc::TimedRobot {
   frc::SendableChooser<std::string> m_path_chooser;
 
   wom::SwerveDrive* _swerveDrive;
+  // AlphaArm* alphaArm;
 
-  AlphaArm* alphaArm;
+  ctre::phoenix6::hardware::TalonFX* frontLeft;
+  // AlphaArm *alphaArm;
 
   // ctre::phoenix6::hardware::TalonFX *frontLeft;
   // ctre::phoenix6::hardware::TalonFX *frontRight;
   // ctre::phoenix6::hardware::TalonFX *backLeft;
   // ctre::phoenix6::hardware::TalonFX *backRight;
-
-  Vision* _vision;
-};
-=======
-
-  Vision* _vision;
-};
->>>>>>> c83ac05 ([robot/vision] Started work on limelight vision)
+};
\ No newline at end of file
diff --git a/src/main/include/RobotMap.h b/src/main/include/RobotMap.h
index 630b27ae..0ed68e8d 100644
--- a/src/main/include/RobotMap.h
+++ b/src/main/include/RobotMap.h
@@ -21,6 +21,7 @@
 #include <ctre/phoenix6/TalonFX.hpp>
 
 #include "AlphaArm.h"
+#include "AlphaArmBehaviour.h"
 #include "Intake.h"
 #include "Shooter.h"
 #include "Wombat.h"
@@ -31,119 +32,121 @@ struct RobotMap {
   struct Controllers {
     frc::XboxController driver = frc::XboxController(0);
     frc::XboxController codriver = frc::XboxController(1);
+    frc::XboxController testController = frc::XboxController(2);
   };
   Controllers controllers;
 
-  struct IntakeSystem {
-    rev::CANSparkMax intakeMotor{15, rev::CANSparkMax::MotorType::kBrushed};
-    // wom::CANSparkMaxEncoder intakeEncoder{&intakeMotor, 0.1_m};
-    frc::DigitalInput intakeSensor{4};
-    // frc::DigitalInput magSensor{0};
-    // frc::DigitalInput shooterSensor{0};
-
-    wom::Gearbox IntakeGearbox{&intakeMotor, nullptr, frc::DCMotor::CIM(1)};
-
-    IntakeConfig config{IntakeGearbox, &intakeSensor /*, &magSensor, &shooterSensor*/};
-  };
-  IntakeSystem intakeSystem;
-
-  struct Shooter {
-    rev::CANSparkMax shooterMotor{1, rev::CANSparkMax::MotorType::kBrushless};  // Port 11
-    // frc::DigitalInput shooterSensor{2};
-
-    // wom::VoltageController shooterMotorGroup =
-    // wom::VoltagedController::Group(shooterMotor);
-    wom::CANSparkMaxEncoder* shooterEncoder = new wom::CANSparkMaxEncoder(&shooterMotor, 0.01_m);
-    wom::Gearbox shooterGearbox{&shooterMotor, shooterEncoder, frc::DCMotor::NEO(1)};
-
-    wom::utils::PIDConfig<units::radians_per_second, units::volts> pidConfigS{
-        "/armavator/arm/velocityPID/config",
-        0.1_V / (360_deg / 1_s),
-        0.03_V / 25_deg,
-        0.001_V / (90_deg / 1_s / 1_s),
-        5_rad_per_s,
-        10_rad_per_s / 1_s};
-
-    ShooterConfig config{"shooterGearbox", shooterGearbox, pidConfigS};
-  };
-  Shooter shooterSystem;
+  //   struct AlphaArmSystem {
+  //     rev::CANSparkMax alphaArmMotor{12, rev::CANSparkMax::MotorType::kBrushless};
+  //     wom::CANSparkMaxEncoder* armEncoder = new wom::CANSparkMaxEncoder(&alphaArmMotor, 0.02_m);
+
+  //     wom::Gearbox alphaArmGearbox{&alphaArmMotor, armEncoder, frc::DCMotor::NEO(1)};
+
+  //     wom::utils::PIDConfig<units::radian, units::volt> pidConfigA{
+  //      "/path/to/pid/in/nt/tables",
+  //     15_V / 180_deg,
+  //     0_V / (1_deg * 1_s),
+  //     0_V / (1_deg / 1_s),
+  //     };
+
+  //     AlphaArmConfig config {
+  //         alphaArmGearbox,
+  //         pidConfigA,
+  //     };
+
+  //   };
+  //   AlphaArmSystem alphaArmSystem;
+
+  //   struct IntakeSystem {
+  //     rev::CANSparkMax intakeMotor{2, rev::CANSparkMax::MotorType::kBrushed};
+  //     // wom::CANSparkMaxEncoder intakeEncoder{&intakeMotor, 0.1_m};
+  //     frc::DigitalInput intakeSensor{4};
+  //     // frc::DigitalInput magSensor{0};
+  //     // frc::DigitalInput shooterSensor{0};
+
+  //     wom::Gearbox IntakeGearbox{&intakeMotor, nullptr, frc::DCMotor::CIM(1)};
+
+  //     IntakeConfig config{IntakeGearbox, &intakeSensor /*, &magSensor, &shooterSensor*/};
+  //   };
+  //   IntakeSystem intakeSystem;
+
+  //     struct Shooter {
+  //     rev::CANSparkMax shooterMotor{11, rev::CANSparkMax::MotorType::kBrushless};// Port 11
+  //     // frc::DigitalInput shooterSensor{2};
+
+  //     // wom::VoltageController shooterMotorGroup = wom::VoltagedController::Group(shooterMotor);
+  //     wom::CANSparkMaxEncoder* shooterEncoder = new wom::CANSparkMaxEncoder(&shooterMotor, 0.01_m);
+  //     wom::Gearbox shooterGearbox{&shooterMotor, shooterEncoder, frc::DCMotor::NEO(1)};
+
+  //     wom::utils::PIDConfig<units::radians_per_second, units::volts> pidConfigS{
+  //           "/armavator/arm/velocityPID/config",
+  //           0.1_V / (360_deg / 1_s),
+  //           0.03_V / 25_deg,
+  //           0.001_V / (90_deg / 1_s / 1_s),
+  //           5_rad_per_s,
+  //           10_rad_per_s / 1_s
+  //     };
+
+  //     ShooterConfig config{
+  //         "shooterGearbox",
+  //         shooterGearbox,
+  //         pidConfigS
+  //     };
+
+  //   };
+  //   Shooter shooterSystem;
 
   struct SwerveBase {
-    ctre::phoenix6::hardware::CANcoder frontLeftCancoder{18, "Drivebase"};
-    ctre::phoenix6::hardware::CANcoder frontRightCancoder{19, "Drivebase"};
-    ctre::phoenix6::hardware::CANcoder backLeftCancoder{16, "Drivebase"};
-    ctre::phoenix6::hardware::CANcoder backRightCancoder{17, "Drivebase"};
+    ctre::phoenix6::hardware::CANcoder frontLeftCancoder{16, "Drivebase"};
+    ctre::phoenix6::hardware::CANcoder frontRightCancoder{18, "Drivebase"};
+    ctre::phoenix6::hardware::CANcoder backLeftCancoder{17, "Drivebase"};
+    ctre::phoenix6::hardware::CANcoder backRightCancoder{19, "Drivebase"};
 
     ctre::phoenix6::hardware::Pigeon2* gyro = new ctre::phoenix6::hardware::Pigeon2(20, "Drivebase");
     wpi::array<ctre::phoenix6::hardware::TalonFX*, 4> turnMotors{
-        new ctre::phoenix6::hardware::TalonFX(7, "Drivebase"),   // front left
-        new ctre::phoenix6::hardware::TalonFX(2, "Drivebase"),   // front right
-        new ctre::phoenix6::hardware::TalonFX(6, "Drivebase"),   // back left
-        new ctre::phoenix6::hardware::TalonFX(4, "Drivebase")};  // back right
+        new ctre::phoenix6::hardware::TalonFX(6, "Drivebase"),   // front left
+        new ctre::phoenix6::hardware::TalonFX(7, "Drivebase"),   // front right
+        new ctre::phoenix6::hardware::TalonFX(4, "Drivebase"),   // back left
+        new ctre::phoenix6::hardware::TalonFX(2, "Drivebase")};  // back right
     wpi::array<ctre::phoenix6::hardware::TalonFX*, 4> driveMotors{
-        new ctre::phoenix6::hardware::TalonFX(9, "Drivebase"),   // front left
-        new ctre::phoenix6::hardware::TalonFX(1, "Drivebase"),   // front right
-        new ctre::phoenix6::hardware::TalonFX(5, "Drivebase"),   // back left
-        new ctre::phoenix6::hardware::TalonFX(3, "Drivebase")};  // back right
+        new ctre::phoenix6::hardware::TalonFX(5, "Drivebase"),   // front left
+        new ctre::phoenix6::hardware::TalonFX(9, "Drivebase"),   // front right
+        new ctre::phoenix6::hardware::TalonFX(3, "Drivebase"),   // back left
+        new ctre::phoenix6::hardware::TalonFX(1, "Drivebase")};  // back right
 
     wpi::array<wom::SwerveModuleConfig, 4> moduleConfigs{
         wom::SwerveModuleConfig{
             // CORRECT
             // front left module
-<<<<<<< HEAD
             frc::Translation2d(-10_in, 9_in),
             wom::Gearbox{driveMotors[0], new wom::TalonFXEncoder(driveMotors[0], 0.0445_m, 6.75),
                          frc::DCMotor::Falcon500(1).WithReduction(6.75)},
             wom::Gearbox{turnMotors[0], new wom::CanEncoder(16, 0.0445_m, 4096, 12.8),
-=======
-            frc::Translation2d(10.761_in, 9.455_in),
-            wom::Gearbox{driveMotors[0], new wom::TalonFXEncoder(driveMotors[0], 0.0445_m, 6.75),
-                         frc::DCMotor::Falcon500(1).WithReduction(6.75)},
-            wom::Gearbox{turnMotors[0], new wom::CanEncoder(18, 0.0445_m, 4096, 12.8),
                          frc::DCMotor::Falcon500(1).WithReduction(12.8)},
             &frontLeftCancoder, 4_in / 2},
         wom::SwerveModuleConfig{
             // CORRECT
             // front right module
-<<<<<<< HEAD
             frc::Translation2d(10_in, 9_in),
             wom::Gearbox{driveMotors[1], new wom::TalonFXEncoder(driveMotors[1], 0.0445_m, 6.75),
                          frc::DCMotor::Falcon500(1).WithReduction(6.75)},
             wom::Gearbox{turnMotors[1], new wom::CanEncoder(18, 0.0445_m, 4096, 12.8),
-=======
-            frc::Translation2d(10.761_in, -9.455_in),
-            wom::Gearbox{driveMotors[1], new wom::TalonFXEncoder(driveMotors[1], 0.0445_m, 6.75),
-                         frc::DCMotor::Falcon500(1).WithReduction(6.75)},
-            wom::Gearbox{turnMotors[1], new wom::CanEncoder(19, 0.0445_m, 4096, 12.8),
                          frc::DCMotor::Falcon500(1).WithReduction(12.8)},
             &frontRightCancoder, 4_in / 2},
         wom::SwerveModuleConfig{
             // back left module
-<<<<<<< HEAD
             frc::Translation2d(-10_in, 9_in),
             wom::Gearbox{driveMotors[2], new wom::TalonFXEncoder(driveMotors[2], 0.0445_m, 6.75),
                          frc::DCMotor::Falcon500(1).WithReduction(6.75)},
             wom::Gearbox{turnMotors[2], new wom::CanEncoder(17, 0.0445_m, 4096, 12.8),
-=======
-            frc::Translation2d(-10.761_in, 9.455_in),
-            wom::Gearbox{driveMotors[2], new wom::TalonFXEncoder(driveMotors[2], 0.0445_m, 6.75),
-                         frc::DCMotor::Falcon500(1).WithReduction(6.75)},
-            wom::Gearbox{turnMotors[2], new wom::CanEncoder(16, 0.0445_m, 4096, 12.8),
->>>>>>> 0029f49 (fix some formatting)
                          frc::DCMotor::Falcon500(1).WithReduction(12.8)},
             &backRightCancoder, 4_in / 2},
         wom::SwerveModuleConfig{
             // back right module
-<<<<<<< HEAD
             frc::Translation2d(-10_in, -9_in),
             wom::Gearbox{driveMotors[3], new wom::TalonFXEncoder(driveMotors[3], 0.0445_m, 6.75),
                          frc::DCMotor::Falcon500(1).WithReduction(6.75)},
             wom::Gearbox{turnMotors[3], new wom::CanEncoder(19, 0.0445_m, 4096, 12.8),
-=======
-            frc::Translation2d(-10.761_in, -9.455_in),
-            wom::Gearbox{driveMotors[3], new wom::TalonFXEncoder(driveMotors[3], 0.0445_m, 6.75),
-                         frc::DCMotor::Falcon500(1).WithReduction(6.75)},
-            wom::Gearbox{turnMotors[3], new wom::CanEncoder(17, 0.0445_m, 4096, 12.8),
                          frc::DCMotor::Falcon500(1).WithReduction(12.8)},
             &backLeftCancoder, 4_in / 2},
     };
@@ -197,17 +200,14 @@ struct RobotMap {
   };
   SwerveTable swerveTable;
 
-  struct AlphaArmSystem {
-    rev::CANSparkMax alphaArmMotor{6, rev::CANSparkMax::MotorType::kBrushless};
-    rev::CANSparkMax wristMotor{11, rev::CANSparkMax::MotorType::kBrushless};
+  // struct AlphaArmSystem {
+  //   rev::CANSparkMax alphaArmMotor{12, rev::CANSparkMax::MotorType::kBrushless};
+  //   rev::CANSparkMax wristMotor{15, rev::CANSparkMax::MotorType::kBrushless};
 
-    wom::Gearbox alphaArmGearbox{&alphaArmMotor, nullptr, frc::DCMotor::NEO(1)};
-    wom::Gearbox wristGearbox{&wristMotor, nullptr, frc::DCMotor::NEO(1)};
+  //   wom::Gearbox alphaArmGearbox{&alphaArmMotor, nullptr, frc::DCMotor::NEO(1)};
+  //   wom::Gearbox wristGearbox{&wristMotor, nullptr, frc::DCMotor::NEO(1)};
 
-    AlphaArmConfig config{alphaArmGearbox, wristGearbox};
-  };
-  AlphaArmSystem alphaArmSystem;
-};
-=======
-};
->>>>>>> 0029f49 (fix some formatting)
+  //   AlphaArmConfig config{alphaArmGearbox, wristGearbox};
+  // };
+  // AlphaArmSystem alphaArmSystem;
+};
\ No newline at end of file
diff --git a/wombat/src/main/cpp/drivetrain/Drivetrain.cpp b/wombat/src/main/cpp/drivetrain/Drivetrain.cpp
deleted file mode 100644
index 387bbc31..00000000
--- a/wombat/src/main/cpp/drivetrain/Drivetrain.cpp
+++ /dev/null
@@ -1,51 +0,0 @@
-// Copyright (c) 2023-2024 CurtinFRC
-// Open Source Software, you can modify it according to the terms
-// of the MIT License at the root of this project
-
-#include "drivetrain/Drivetrain.h"
-
-#include <iostream>
-
-#include "utils/Util.h"
-
-using namespace frc;
-using namespace units;
-
-wom::drivetrain::Drivetrain::Drivetrain(DrivetrainConfig* config, XboxController& driver)
-    : _config(config), _driver(driver) {}
-wom::drivetrain::Drivetrain::~Drivetrain() {}
-
-wom::drivetrain::DrivetrainConfig* wom::drivetrain::Drivetrain::GetConfig() {
-  return _config;
-}
-wom::drivetrain::DrivetrainState wom::drivetrain::Drivetrain::GetState() {
-  return _state;
-}
-
-void wom::drivetrain::Drivetrain::SetState(DrivetrainState state) {
-  _state = state;
-}
-
-void wom::drivetrain::Drivetrain::OnStart() {
-  std::cout << "Starting Tank" << std::endl;
-}
-
-void wom::drivetrain::Drivetrain::OnUpdate(second_t dt) {
-  switch (_state) {
-    case DrivetrainState::kIdle:
-      break;
-    case DrivetrainState::kTank: {
-      double rightSpeed = wom::utils::deadzone(_driver.GetRightY());
-      double leftSpeed = wom::utils::deadzone(_driver.GetLeftY());
-      // _config->left1.motorController->SetVoltage(leftSpeed * maxVolts);
-      // _config->left2.motorController->SetVoltage(leftSpeed * maxVolts);
-      // _config->left3.motorController->SetVoltage(leftSpeed * maxVolts);
-      // _config->right1.motorController->SetVoltage(rightSpeed * maxVolts);
-      // _config->right2.motorController->SetVoltage(rightSpeed * maxVolts);
-      // _config->right3.motorController->SetVoltage(rightSpeed * maxVolts);
-      break;
-    }
-    case DrivetrainState::kAuto:
-      break;
-  }
-}
diff --git a/wombat/src/main/cpp/drivetrain/SwerveDrive.cpp b/wombat/src/main/cpp/drivetrain/SwerveDrive.cpp
index 9a906953..b6772015 100644
--- a/wombat/src/main/cpp/drivetrain/SwerveDrive.cpp
+++ b/wombat/src/main/cpp/drivetrain/SwerveDrive.cpp
@@ -8,12 +8,14 @@
 #include <units/angle.h>
 #include <units/math.h>
 #include <units/voltage.h>
+#include <wpi/sendable/SendableBuilder.h>
+#include <wpi/sendable/SendableRegistry.h>
 
 #include <algorithm>
 #include <cmath>
 #include <iostream>
 
-#include "units/mass.h"
+#include "frc/MathUtil.h"
 #include "utils/Util.h"
 #include "wpimath/MathShared.h"
 
@@ -32,17 +34,21 @@ SwerveModule::SwerveModule(std::string path, SwerveModuleConfig config,
                            // SwerveModule::angle_pid_conf_t anglePID,
                            SwerveModule::velocity_pid_conf_t velocityPID)
     :  // _anglePIDController(path + "/pid/angle", anglePID),
-      _anglePIDController{frc::PIDController(4, 0, 0, 0.005_s)},
+      _anglePIDController{frc::PIDController(5, 0, 0, 0.005_s)},
       _config(config),
       _velocityPIDController(frc::PIDController(1.2, 0, 0, 0.005_s)),
       _table(nt::NetworkTableInstance::GetDefault().GetTable(path)) {
-  // _anglePIDController.(2 * 3.1415);
-  _anglePIDController.EnableContinuousInput(0, (2 * 3.1415));
+  // _anglePIDController.SetTolerance(360);
+  _anglePIDController.EnableContinuousInput(0, (2 * 3.141592));
+  // _anglePIDController.EnableContinuousInput(-3.141592, (3.141592));
+  // _anglePIDController.EnableContinuousInput(0, (3.1415 * 2));
+  // _anglePIDController.EnableContinuousInput(0, (3.1415));
+  // _anglePIDController.SetWrap(0, 3.1415);
 }
 
 void SwerveModule::OnStart() {
   // _offset = offset;
-  _config.canEncoder->SetPosition(units::turn_t{0});
+  // _config.canEncoder->SetPosition(units::turn_t{0});
   _anglePIDController.Reset();
   // _anglePIDController.EnableContinuousInput(-3.141592, 3.141592);
   // _anglePIDController.EnableContinuousInput(-3.141592, (3.141592));
@@ -64,18 +70,24 @@ void SwerveModule::OnUpdate(units::second_t dt) {
       auto feedforward = _config.driveMotor.motor.Voltage(
           0_Nm,
           units::radians_per_second_t{_velocityPIDController.GetSetpoint() / _config.wheelRadius.value()});
-      // std::cout << "GetSpeed()" << GetSpeed().value() << std::endl;
-      //  units::radian_t input = units::math::fmod(_config.turnMotor.encoder->GetEncoderPosition(), (2_rad
-      //  * 3.1415));
-      double input = -_config.turnMotor.encoder->GetEncoderPosition().value();
+      // units::radian_t input = units::math::fmod(_config.turnMotor.encoder->GetEncoderPosition(), (2_rad
+      // * 3.1415));
+      double input = _config.turnMotor.encoder->GetEncoderPosition().value();
       _table->GetEntry("/testing/GetEncoderPos").SetDouble(input);
       // _velocityPIDController.SetSetpoint(3);
-      driveVoltage = units::volt_t{_velocityPIDController.Calculate(GetSpeed().value())};
-      std::cout << "Turn angle input: " << input << std::endl;
-      turnVoltage = units::volt_t{_anglePIDController.Calculate(input)};
 
-      _table->GetEntry("Input angle").SetDouble(input);
-      _table->GetEntry("Setpoint angle").SetDouble(_anglePIDController.GetSetpoint());
+      driveVoltage = units::volt_t{_velocityPIDController.Calculate(GetSpeed().value())};
+      // if (_turnOffset == TurnOffsetValues::forward) {
+
+      // } else if (_turnOffset == TurnOffsetValues::reverse) {
+      //   input = input - (3.1415/2);
+      //   driveVoltage = -driveVoltage;
+      // }
+      double demand = _anglePIDController.Calculate(input);
+      // if ((_anglePIDController.GetSetpoint() - input) > (3.14159/2)) {
+      //   demand *= -1;
+      // }
+      turnVoltage = units::volt_t{demand};
     } break;
     case wom::drivetrain::SwerveModuleState::kZeroing: {
     } break;
@@ -90,8 +102,7 @@ void SwerveModule::OnUpdate(units::second_t dt) {
   //     -torqueLimit, _config.driveMotor.encoder->GetEncoderAngularVelocity());
 
   // driveVoltage =
-  //     units::math::max(units::math::min(driveVoltage, voltageMax),
-  //     voltageMin);
+  //     units::math::max(units::math::min(driveVoltage, voltageMax), voltageMin);
 
   driveVoltage = units::math::min(driveVoltage, 7_V);
   turnVoltage = units::math::min(turnVoltage, 4_V);
@@ -108,20 +119,11 @@ void SwerveModule::OnUpdate(units::second_t dt) {
   _table->GetEntry("Demand").SetDouble(_config.turnMotor.encoder->GetEncoderPosition().value());
   _table->GetEntry("Error").SetDouble(_anglePIDController.GetPositionError());
 
-  _table->GetEntry("TurnVoltage").SetDouble(turnVoltage.value());
-  _table->GetEntry("TurnSetpoint").SetDouble(_anglePIDController.GetSetpoint());
-  _table->GetEntry("Error").SetDouble(_anglePIDController.GetPositionError());
-
   _config.driveMotor.motorController->SetVoltage(driveVoltage);
   _config.turnMotor.motorController->SetVoltage(turnVoltage);
 
   _table->GetEntry("speed").SetDouble(GetSpeed().value());
-<<<<<<< HEAD
   _table->GetEntry("angle").SetDouble(_config.turnMotor.encoder->GetEncoderPosition().value());
-=======
-  _table->GetEntry("angle").SetDouble(
-      _config.turnMotor.encoder->GetEncoderPosition().convert<units::degree>().value());
->>>>>>> 0029f49 (fix some formatting)
   _config.WriteNT(_table->GetSubTable("config"));
 }
 
@@ -166,24 +168,58 @@ void SwerveModule::SetZero(units::second_t dt) {
 
 void SwerveModule::SetPID(units::radian_t angle, units::meters_per_second_t speed, units::second_t dt) {
   _state = SwerveModuleState::kPID;
-<<<<<<< HEAD
-=======
-  // @liam start added
-  // std::cout << "angle Setpoint: " << _anglePIDController.GetSetpoint() <<
-  // std::endl; std::cout << "angle value: " << angle.value() << std::endl;
-
-  double diff = std::fmod((_anglePIDController.GetSetpoint() - angle.value()), (2 * 3.1415));
-  std::cout << "DIFF value: " << diff << std::endl;
-  _table->GetEntry("/Differential value:").SetDouble(diff);
-  if (std::abs(diff) >= 3.1415) {
-    speed *= -1;
-    angle -= 3.1415_rad;
-  }
-  // @liam end added
->>>>>>> 0029f49 (fix some formatting)
 
+  // double diff = std::abs(_config.turnMotor.encoder->GetEncoderPosition().value() - angle.value());
+  // _table->GetEntry("diff").SetDouble(diff);
+  // if (std::abs(diff) > (3.14159/2)) {
+  //   speed *= -1;
+  //   angle = 3.14159_rad - units::radian_t{diff};
+  //   _anglePIDController.SetSetpoint(angle.value());
+  //   _velocityPIDController.SetSetpoint(speed.value());
+  // } else {
+  //   _anglePIDController.SetSetpoint(angle.value());
+  //   _velocityPIDController.SetSetpoint(speed.value());
+  // }
+
+  // if (diff > (3.14159 / 2)) {
+  //   speed *= -1;
+  //   _anglePIDController.SetSetpoint((3.14159 - (angle.value() -
+  //   _config.turnMotor.encoder->GetEncoderPosition().value())));
+  //   _velocityPIDController.SetSetpoint(speed.value());
+  // } else {
+
+  // double setValue = 3.141592 - (angle.value() - _config.turnMotor.encoder->GetEncoderPosition().value());
+
+  // if (diff > (3.141592/2)) {
+  //   if (setValue < 0 ) {
+  //     _anglePIDController.SetSetpoint(0);
+  //     _velocityPIDController.SetSetpoint(-speed.value());
+  //   } else if ( setValue < (2 * 3)) {
+  //     _anglePIDController.SetSetpoint(2 * 3.141592  );
+  //     _velocityPIDController.SetSetpoint(-speed.value());
+  //   } else {
+  //     _anglePIDController.SetSetpoint(setValue);
+  //     _velocityPIDController.SetSetpoint(-speed.value());
+  //   }
+  // } else {
   _anglePIDController.SetSetpoint(angle.value());
   _velocityPIDController.SetSetpoint(speed.value());
+  // }
+
+  // double currentAngle = _config.turnMotor.encoder->GetEncoderPosition().value();
+  // double setpointAngle = closestAngle(currentAngle, _anglePIDController.GetSetpoint());
+  // double setpointAngleFlipped = closestAngle(currentAngle, _anglePIDController.GetSetpoint() + 3.1415);
+  // _table->GetEntry("/Setpoint angle: ").SetDouble(setpointAngle);
+  // _table->GetEntry("/Setpoint angle flipped: ").SetDouble(setpointAngleFlipped);
+  // if (std::abs(setpointAngle) <= std::abs(setpointAngleFlipped)) {
+  //   // _anglePIDController.SetGain(1.0);
+  //   _anglePIDController.SetSetpoint(currentAngle + setpointAngle);
+  //   _velocityPIDController.SetSetpoint(speed.value());
+  // } else {
+  //   // _anglePIDController.SetGain(-1.0);
+  //   _velocityPIDController.SetSetpoint(-speed.value());
+  //   _anglePIDController.SetSetpoint(currentAngle + setpointAngleFlipped);
+  // }
 }
 
 // double SwerveModule::closestAngle(double a, double b) {
@@ -211,7 +247,6 @@ void SwerveModule::ModuleVectorHandler(frc::ChassisSpeeds speeds) {
 units::meters_per_second_t SwerveModule::GetSpeed() const {
   units::meters_per_second_t returnVal{_config.driveMotor.encoder->GetVelocityValue() *
                                        _config.wheelRadius.value()};
-  // std::cout << returnVal.value() << std::endl;
   return returnVal;
 }
 
@@ -234,16 +269,11 @@ void SwerveDriveConfig::WriteNT(std::shared_ptr<nt::NetworkTable> table) {
 
 SwerveDrive::SwerveDrive(SwerveDriveConfig config, frc::Pose2d initialPose)
     : _config(config),
-<<<<<<< HEAD
       // _kinematics(_config.modules[1].position, _config.modules[0].position,
       //             _config.modules[2].position, _config.modules[3].position),
 
       _kinematics(_config.modules[3].position /*1*/, _config.modules[0].position /*0*/,
                   _config.modules[1].position /*2*/, _config.modules[2].position /*3*/),
-=======
-      _kinematics(_config.modules[0].position, _config.modules[1].position, _config.modules[2].position,
-                  _config.modules[3].position),
->>>>>>> 0029f49 (fix some formatting)
       _poseEstimator(
           _kinematics, frc::Rotation2d(0_deg),
           wpi::array<frc::SwerveModulePosition, 4>{frc::SwerveModulePosition{0_m, frc::Rotation2d{0_deg}},
@@ -272,19 +302,13 @@ frc::ChassisSpeeds FieldRelativeSpeeds::ToChassisSpeeds(const units::radian_t ro
 }
 
 void SwerveDrive::OnUpdate(units::second_t dt) {
-  // std::cout << _config.gyro->GetYaw().GetValue().value() << std::endl;
+  _table->GetEntry("/gryo/z").SetDouble(_config.gyro->GetRotation3d().Z().value());
+  _table->GetEntry("/gryo/y").SetDouble(_config.gyro->GetRotation3d().Y().value());
+  _table->GetEntry("/gryo/x").SetDouble(_config.gyro->GetRotation3d().X().value());
   switch (_state) {
     case SwerveDriveState::kZeroing:
       for (auto mod = _modules.begin(); mod < _modules.end(); mod++) {
-<<<<<<< HEAD
-<<<<<<< HEAD
         // mod->SetZero(dt);
-=======
-      mod->SetZero(dt);
->>>>>>> befb502 (More vision)
-=======
-        mod->SetZero(dt);
->>>>>>> 57e941a ([wpiformat])
       }
       break;
     case SwerveDriveState::kIdle:
@@ -330,7 +354,6 @@ void SwerveDrive::OnUpdate(units::second_t dt) {
       frc::ChassisSpeeds new_target_speed{_target_speed.vx, _target_speed.vy, -_target_speed.omega};
       auto new_target_states = _kinematics.ToSwerveModuleStates(new_target_speed);
       for (size_t i = 0; i < _modules.size(); i++) {
-<<<<<<< HEAD
         if (i == 3) {
           _modules[i].SetPID(new_target_states[i].angle.Radians(), new_target_states[i].speed, dt);
         } else {
@@ -342,11 +365,6 @@ void SwerveDrive::OnUpdate(units::second_t dt) {
         //   _modules[i].SetPID(new_target_states[i].angle.Radians(),
         //                     new_target_states[i].speed, dt);
         // }
-=======
-        _modules[i].SetPID(target_states[i].angle.Radians(), target_states[i].speed, dt);
-        // std::cout << "module " << i << ": " <<
-        // target_states[i].angle.Radians().value() << std::endl;
->>>>>>> 0029f49 (fix some formatting)
       }
     } break;
     case SwerveDriveState::kIndividualTuning:
@@ -364,7 +382,7 @@ void SwerveDrive::OnUpdate(units::second_t dt) {
       _modules[2].SetPID(225_deg, 0_mps, dt);
       _modules[3].SetPID(315_deg, 0_mps, dt);
       break;
-    case SwerveDriveState::kFRVelocityRotationLock: {
+    case SwerveDriveState::kFRVelocityRotationLock:
       _target_speed.vx = _xPIDController.Calculate(GetPose().X(), dt);
       _target_speed.vy = _yPIDController.Calculate(GetPose().Y(), dt);
       _target_speed.omega =
@@ -375,7 +393,6 @@ void SwerveDrive::OnUpdate(units::second_t dt) {
         _modules[i].SetPID(target_states[i].angle.Radians(), target_states[i].speed, dt);
       }
       break;
-    }
   }
 
   for (auto mod = _modules.begin(); mod < _modules.end(); mod++) {
@@ -424,11 +441,6 @@ void SwerveDrive::OnResetMode() {
   _anglePIDController.Reset();
 }
 
-void SwerveDrive::SetLocked(units::degree_t angle) {
-  _state = SwerveDriveState::kLockedOn;
-  _lockedAngle = angle;
-}
-
 void SwerveDrive::RotateMatchJoystick(units::radian_t joystickAngle, FieldRelativeSpeeds speeds) {
   // _state = SwerveDriveState::kFRVelocityRotationLock;
   // _anglePIDController.SetSetpoint(joystickAngle);
diff --git a/wombat/src/main/cpp/subsystems/Arm.cpp b/wombat/src/main/cpp/subsystems/Arm.cpp
deleted file mode 100644
index 148cec43..00000000
--- a/wombat/src/main/cpp/subsystems/Arm.cpp
+++ /dev/null
@@ -1,183 +0,0 @@
-// Copyright (c) 2023-2024 CurtinFRC
-// Open Source Software, you can modify it according to the terms
-// of the MIT License at the root of this project
-
-#include "subsystems/Arm.h"
-
-#include <units/math.h>
-
-using namespace frc;
-using namespace wom;
-
-// creates network table instatnce on shuffleboard
-void wom::subsystems::ArmConfig::WriteNT(std::shared_ptr<nt::NetworkTable> table) {
-  table->GetEntry("armMass").SetDouble(armMass.value());
-  table->GetEntry("loadMass").SetDouble(loadMass.value());
-  table->GetEntry("armLength").SetDouble(armLength.value());
-  table->GetEntry("minAngle").SetDouble(minAngle.convert<units::degree>().value());
-  table->GetEntry("maxAngle").SetDouble(maxAngle.convert<units::degree>().value());
-  table->GetEntry("initialAngle").SetDouble(initialAngle.convert<units::degree>().value());
-  table->GetEntry("angleOffset").SetDouble(initialAngle.convert<units::degree>().value());
-}
-
-// arm config is used
-wom::subsystems::Arm::Arm(ArmConfig config)
-    : _config(config),
-      _pid(config.path + "/pid", config.pidConfig),
-      _velocityPID(config.path + "/velocityPID", config.velocityConfig),
-      _table(nt::NetworkTableInstance::GetDefault().GetTable(config.path)) {}
-
-// the loop that allows the information to be used
-void wom::subsystems::Arm::OnUpdate(units::second_t dt) {
-  // sets the voltage and gets the current angle
-  units::volt_t voltage = 0_V;
-  auto angle = GetAngle();
-
-  // sets usable infromation for each state
-  switch (_state) {
-    case ArmState::kIdle:
-      break;
-    case ArmState::kVelocity: {
-      units::newton_meter_t torque = 9.81_m / 1_s / 1_s * _config.armLength *
-                                     units::math::cos(angle + _config.angleOffset) *
-                                     (0.5 * _config.armMass + _config.loadMass);
-      // units::volt_t feedforward = _config.leftGearbox.motor.Voltage(torque,
-      // 0_rad/1_s);
-      units::volt_t feedforward = _config.leftGearbox.motor.Voltage(torque, _velocityPID.GetSetpoint());
-      // feedforward = 3.5_V;
-      // std::cout << "feedforward" << feedforward.value() << std::endl;
-      voltage = _velocityPID.Calculate(GetArmVelocity(), dt, feedforward);
-      // std::cout << "arm velocity voltage is: " << voltage.value() <<
-      // std::endl; voltage = 0_V;
-    } break;
-    case ArmState::kAngle: {
-      units::newton_meter_t torque = 9.81_m / 1_s / 1_s * _config.armLength *
-                                     units::math::cos(angle + _config.angleOffset) *
-                                     (0.5 * _config.armMass + _config.loadMass);
-      units::volt_t feedforward = _config.leftGearbox.motor.Voltage(torque, 0_rad / 1_s);
-      // std::cout << "feedforward" << feedforward.value() << std::endl;
-      voltage = _pid.Calculate(angle, dt, feedforward);
-    } break;
-    case ArmState::kRaw:
-      voltage = _voltage;
-      break;
-  }
-
-  voltage *= armLimit;
-
-  // units::newton_meter_t torqueLimit = 10_kg * 1.4_m * 6_mps_sq;
-  // units::volt_t voltageMax = _config.leftGearbox.motor.Voltage(torqueLimit,
-  // _config.leftGearbox.encoder->GetEncoderAngularVelocity()); units::volt_t
-  // voltageMin = _config.leftGearbox.motor.Voltage(-torqueLimit,
-  // _config.leftGearbox.encoder->GetEncoderAngularVelocity());
-
-  // voltage = units::math::max(units::math::min(voltage, voltageMax),
-  // voltageMin);
-  units::volt_t voltageMin = -5.5_V;
-  units::volt_t voltageMax = 5.5_V;
-  voltage = units::math::max(units::math::min(voltage, voltageMax), voltageMin);
-
-  // std::cout << "voltage: " << voltage.value() << std::endl;
-
-  // _config.leftGearbox.motorController->SetVoltage(voltage);
-  // _config.rightGearbox.motorController->SetVoltage(voltage);
-
-  // creates network table instances for the angle and config of the arm
-  _table->GetEntry("angle").SetDouble(angle.convert<units::degree>().value());
-  _config.WriteNT(_table->GetSubTable("config"));
-}
-
-void wom::subsystems::Arm::SetArmSpeedLimit(double limit) {
-  armLimit = limit;
-}
-
-// defines information needed for the functions and connects the states to their
-// respective function
-
-void wom::subsystems::Arm::SetRaw(units::volt_t voltage) {
-  _voltage = voltage;
-}
-
-void wom::subsystems::Arm::SetVelocity(units::radians_per_second_t velocity) {
-  _velocityPID.SetSetpoint(velocity);
-}
-
-void wom::subsystems::Arm::SetAngle(units::radian_t angle) {
-  _pid.SetSetpoint(angle);
-}
-
-void wom::subsystems::Arm::SetState(wom::subsystems::ArmState state) {
-  _state = state;
-}
-
-wom::subsystems::ArmConfig& wom::subsystems::Arm::GetConfig() {
-  return _config;
-}
-
-units::radian_t wom::subsystems::Arm::GetAngle() const {
-  return _config.armEncoder.GetPosition() / 100 * 360 * 1_deg;
-}
-
-units::radians_per_second_t wom::subsystems::Arm::MaxSpeed() const {
-  return _config.leftGearbox.motor.Speed(0_Nm, 12_V);
-}
-
-units::radians_per_second_t wom::subsystems::Arm::GetArmVelocity() const {
-  return _config.armEncoder.GetVelocity() / 100 * 360 * 1_deg / 60_s;
-}
-
-bool wom::subsystems::Arm::IsStable() const {
-  return _pid.IsStable(5_deg);
-}
-
-/* SIMULATION */
-// #include <units/math.h>
-
-// ::wom::sim::ArmSim::ArmSim(ArmConfig config)
-//   : config(config),
-//     angle(config.initialAngle),
-//     encoder(config.gearbox.encoder->MakeSimEncoder()),
-//     lowerLimit(config.lowerLimitSwitch ? new
-//     frc::sim::DIOSim(*config.lowerLimitSwitch) : nullptr),
-//     upperLimit(config.upperLimitSwitch ? new
-//     frc::sim::DIOSim(*config.upperLimitSwitch) : nullptr),
-//     table(nt::NetworkTableInstance::GetDefault().GetTable(config.path +
-//     "/sim"))
-//   {}
-
-// units::ampere_t wom::sim::ArmSim::GetCurrent() const {
-//   return current;
-// }
-
-// void ::wom::sim::ArmSim::Update(units::second_t dt) {
-//   torque = (config.loadMass * config.armLength + config.armMass *
-//   config.armLength / 2.0) * 9.81_m / 1_s / 1_s *
-//   units::math::cos(config.angleOffset + angle) + additionalTorque; velocity =
-//   config.gearbox.motor.Speed(torque,
-//   config.gearbox.transmission->GetEstimatedRealVoltage()); angle += velocity
-//   * dt;
-
-//   if (angle <= config.minAngle) {
-//     angle = config.minAngle;
-//     velocity = 0_rad / 1_s;
-//     if (lowerLimit) lowerLimit->SetValue(true);
-//   } else {
-//     if (lowerLimit) lowerLimit->SetValue(false);
-//   }
-
-//   if (angle >= config.maxAngle) {
-//     angle = config.maxAngle;
-//     velocity = 0_rad / 1_s;
-//     if (upperLimit) upperLimit->SetValue(true);
-//   } else {
-//     if (upperLimit) upperLimit->SetValue(false);
-//   }
-
-//   current = config.gearbox.motor.Current(velocity,
-//   config.gearbox.transmission->GetEstimatedRealVoltage());
-
-//   if (encoder) encoder->SetEncoderTurns(angle - config.initialAngle);
-
-//   table->GetEntry("angle").SetDouble(angle.convert<units::degree>().value());
-//   table->GetEntry("current").SetDouble(current.value());
-// }
diff --git a/wombat/src/main/cpp/subsystems/Elevator.cpp b/wombat/src/main/cpp/subsystems/Elevator.cpp
deleted file mode 100644
index d1cf9a8f..00000000
--- a/wombat/src/main/cpp/subsystems/Elevator.cpp
+++ /dev/null
@@ -1,141 +0,0 @@
-// Copyright (c) 2023-2024 CurtinFRC
-// Open Source Software, you can modify it according to the terms
-// of the MIT License at the root of this project
-
-#include "subsystems/Elevator.h"
-
-#include <networktables/NetworkTableInstance.h>
-#include <units/acceleration.h>
-
-void wom::subsystems::ElevatorConfig::WriteNT(std::shared_ptr<nt::NetworkTable> table) {
-  table->GetEntry("radius").SetDouble(radius.value());
-  table->GetEntry("mass").SetDouble(mass.value());
-  table->GetEntry("maxHeight").SetDouble(maxHeight.value());
-}
-
-wom::subsystems::Elevator::Elevator(ElevatorConfig config)
-    : _config(config),
-      _state(ElevatorState::kIdle),
-      _pid{config.path + "/pid", config.pid},
-      _velocityPID{config.path + "/velocityPID", config.velocityPID},
-      _table(nt::NetworkTableInstance::GetDefault().GetTable(config.path)) {
-  _config.leftGearbox.encoder->SetEncoderPosition(_config.initialHeight / _config.radius * 1_rad);
-}
-
-void wom::subsystems::Elevator::OnUpdate(units::second_t dt) {
-  units::volt_t voltage{0};
-
-  units::meter_t height = GetElevatorEncoderPos() * 1_m;
-
-  switch (_state) {
-    case ElevatorState::kIdle:
-      voltage = 0_V;
-      break;
-    case ElevatorState::kManual:
-      voltage = _setpointManual;
-      break;
-    case ElevatorState::kVelocity: {
-      units::volt_t feedforward = _config.rightGearbox.motor.Voltage(
-          (_config.mass * 9.81_mps_sq) * _config.radius,
-          _velocityPID.GetSetpoint() / (14.0 / 60.0 * 2.0 * 3.1415 * 0.02225 * 1_m) * 1_rad);
-      // units::volt_t feedforward = _config.rightGearbox.motor.Voltage(0_Nm,
-      // _velocityPID.GetSetpoint() / (14.0/60.0 * 2.0 * 3.1415 * 0.02225 * 1_m)
-      // * 1_rad);
-      feedforward = 1.2_V;
-      voltage = _velocityPID.Calculate(GetElevatorVelocity(), dt, feedforward);
-      if (voltage > 6_V) {
-        voltage = 6_V;
-      }
-      std::cout << "elevator feedforward: " << feedforward.value() << std::endl;
-      // voltage = 0_V;
-    } break;
-    case ElevatorState::kPID: {
-      units::volt_t feedforward =
-          _config.rightGearbox.motor.Voltage((_config.mass * 9.81_mps_sq) * _config.radius, 0_rad_per_s);
-      // std::cout << "feed forward" << feedforward.value() << std::endl;
-      feedforward = 1.2_V;
-      // voltage = _pid.Calculate(height, dt, feedforward);
-      voltage = _pid.Calculate(height, dt, feedforward);
-      if (voltage > 6_V) {
-        voltage = 6_V;
-      }
-    } break;
-  }
-
-  // Top Sensor Detector
-  // if(_config.topSensor != nullptr) {
-  //   if(_config.topSensor->Get()) {
-  //     _config.leftGearbox.encoder->SetEncoderPosition(_config.maxHeight /
-  //     _config.radius * 1_rad);
-  //     //voltage = 0_V;
-  //   }
-  // }
-
-  // //Bottom Sensor Detection
-  // if (_config.bottomSensor != nullptr) {
-  //   if (_config.bottomSensor->Get()) {
-  //     _config.leftGearbox.encoder->SetEncoderPosition(_config.minHeight /
-  //     _config.radius * 1_rad);
-  //     //voltage = 0_V;
-  //   }
-  // }
-
-  // std::cout << "elevator: " << elevator.height
-
-  // Set voltage to motors...
-  voltage *= speedLimit;
-  // _config.leftGearbox.motorController->SetVoltage(voltage);
-  // _config.rightGearbox.motorController->SetVoltage(voltage);
-}
-
-void wom::subsystems::Elevator::SetManual(units::volt_t voltage) {
-  _setpointManual = voltage;
-}
-
-void wom::subsystems::Elevator::SetPID(units::meter_t height) {
-  _pid.SetSetpoint(height);
-}
-
-void wom::subsystems::Elevator::SetVelocity(units::meters_per_second_t velocity) {
-  _velocityPID.SetSetpoint(velocity);
-}
-
-void wom::subsystems::Elevator::SetState(wom::subsystems::ElevatorState state) {
-  _state = state;
-}
-
-void wom::subsystems::Elevator::SetElevatorSpeedLimit(double limit) {
-  speedLimit = limit;
-}
-
-wom::subsystems::ElevatorConfig& wom::subsystems::Elevator::GetConfig() {
-  return _config;
-}
-
-bool wom::subsystems::Elevator::IsStable() const {
-  return _pid.IsStable();
-}
-
-wom::subsystems::ElevatorState wom::subsystems::Elevator::GetState() const {
-  return _state;
-}
-
-double wom::subsystems::Elevator::GetElevatorEncoderPos() {
-  return _config.elevatorEncoder.GetPosition() * 14 / 60 * 2 * 3.1415 * 0.02225;
-}
-
-units::meter_t wom::subsystems::Elevator::GetHeight() const {
-  // std::cout << "elevator position"<<
-  // _config.rightGearbox.encoder->GetEncoderTicks() << std::endl; return
-  // _config.rightGearbox.encoder->GetEncoderDistance() * 1_m;
-  return _config.elevatorEncoder.GetPosition() * 14 / 60 * 2 * 3.1415 * 0.02225 * 1_m;
-}
-
-units::meters_per_second_t wom::subsystems::Elevator::GetElevatorVelocity() const {
-  return _config.elevatorEncoder.GetVelocity() / 60_s * 14 / 60 * 2 * 3.1415 * 0.02225 * 1_m;
-}
-
-units::meters_per_second_t wom::subsystems::Elevator::MaxSpeed() const {
-  return _config.leftGearbox.motor.Speed((_config.mass * 9.81_mps_sq) * _config.radius, 12_V) / 1_rad *
-         _config.radius;
-}
diff --git a/wombat/src/main/cpp/subsystems/Shooter.cpp b/wombat/src/main/cpp/subsystems/Shooter.cpp
deleted file mode 100644
index c851266b..00000000
--- a/wombat/src/main/cpp/subsystems/Shooter.cpp
+++ /dev/null
@@ -1,86 +0,0 @@
-// Copyright (c) 2023-2024 CurtinFRC
-// Open Source Software, you can modify it according to the terms
-// of the MIT License at the root of this project
-
-#include "subsystems/Shooter.h"
-
-#include <networktables/NetworkTableInstance.h>
-
-wom::subsystems::Shooter::Shooter(std::string path, ShooterParams params)
-    : _params(params),
-      _state(ShooterState::kIdle),
-      _pid{path + "/pid", params.pid},
-      _table(nt::NetworkTableInstance::GetDefault().GetTable("shooter")) {}
-
-void wom::subsystems::Shooter::OnUpdate(units::second_t dt) {
-  units::volt_t voltage{0};
-  units::revolutions_per_minute_t currentSpeed = _params.gearbox.encoder->GetEncoderAngularVelocity();
-
-  switch (_state) {
-    case ShooterState::kManual:
-      voltage = _setpointManual;
-      break;
-    case ShooterState::kPID: {
-      auto feedforward = _params.gearbox.motor.Voltage(0_Nm, _pid.GetSetpoint());
-      voltage = _pid.Calculate(currentSpeed, dt, feedforward);
-    } break;
-    case ShooterState::kIdle:
-      voltage = 0_V;
-      break;
-  }
-
-  units::newton_meter_t max_torque_at_current_limit = _params.gearbox.motor.Torque(_params.currentLimit);
-  units::volt_t max_voltage_for_current_limit =
-      _params.gearbox.motor.Voltage(max_torque_at_current_limit, currentSpeed);
-
-  voltage = 1_V * std::min(voltage.value(), max_voltage_for_current_limit.value());
-
-  // _params.gearbox.motorController->SetVoltage(voltage);
-
-  _table->GetEntry("output_volts").SetDouble(voltage.value());
-  _table->GetEntry("speed_rpm").SetDouble(currentSpeed.value());
-  _table->GetEntry("setpoint_rpm").SetDouble(units::revolutions_per_minute_t{_pid.GetSetpoint()}.value());
-  _table->GetEntry("stable").SetBoolean(_pid.IsStable());
-}
-
-void wom::subsystems::Shooter::SetManual(units::volt_t voltage) {
-  _setpointManual = voltage;
-}
-
-void wom::subsystems::Shooter::SetPID(units::radians_per_second_t goal) {
-  _state = ShooterState::kPID;
-  _pid.SetSetpoint(goal);
-}
-
-void wom::subsystems::Shooter::SetState(ShooterState state) {
-  _state = state;
-}
-
-bool wom::subsystems::Shooter::IsStable() const {
-  return _pid.IsStable();
-}
-
-// Shooter Manual Set
-
-wom::subsystems::ShooterConstant::ShooterConstant(Shooter* s, units::volt_t setpoint)
-    : _shooter(s), _setpoint(setpoint) {
-  Controls(_shooter);
-}
-
-void wom::subsystems::ShooterConstant::OnTick(units::second_t dt) {
-  _shooter->SetManual(_setpoint);
-}
-
-// ShooterSpinup
-
-wom::subsystems::ShooterSpinup::ShooterSpinup(Shooter* s, units::radians_per_second_t speed, bool hold)
-    : _shooter(s), _speed(speed), _hold(hold) {
-  Controls(_shooter);
-}
-
-void wom::subsystems::ShooterSpinup::OnTick(units::second_t dt) {
-  _shooter->SetPID(_speed);
-
-  if (!_hold && _shooter->IsStable())
-    SetDone();
-}
diff --git a/wombat/src/main/cpp/utils/Encoder.cpp b/wombat/src/main/cpp/utils/Encoder.cpp
index e86b3c35..431a164f 100644
--- a/wombat/src/main/cpp/utils/Encoder.cpp
+++ b/wombat/src/main/cpp/utils/Encoder.cpp
@@ -57,7 +57,7 @@ units::radian_t wom::utils::Encoder::GetEncoderPosition() {
 }
 
 double wom::utils::Encoder::GetEncoderDistance() {
-  return GetEncoderTicks() * (2 * 3.14159265358979323846) * _wheelRadius.value();
+  return GetEncoderTicks() * (2 * 3.1415) * _wheelRadius.value();
 }
 
 units::radians_per_second_t wom::utils::Encoder::GetEncoderAngularVelocity() {
@@ -88,7 +88,7 @@ double wom::utils::DigitalEncoder::GetEncoderTickVelocity() const {
 wom::utils::CANSparkMaxEncoder::CANSparkMaxEncoder(rev::CANSparkMax* controller, units::meter_t wheelRadius,
                                                    double reduction)
     : wom::utils::Encoder(42, reduction, wheelRadius, 2),
-      _encoder(controller->GetEncoder(rev::SparkRelativeEncoder::Type::kQuadrature)) {}
+      _encoder(controller->GetEncoder(rev::SparkRelativeEncoder::Type::kHallSensor)) {}
 
 double wom::utils::CANSparkMaxEncoder::GetEncoderRawTicks() const {
   return ((_encoder.GetPosition() * 2 * 3.1415) / 200);
@@ -115,7 +115,7 @@ double wom::utils::CANSparkMaxEncoder::GetPosition() const {
 }
 
 double wom::utils::CANSparkMaxEncoder::GetVelocity() const {
-  return _encoder.GetVelocity() * 3.14159265 * 2 / 60;
+  return _encoder.GetVelocity();
 }
 
 wom::utils::TalonFXEncoder::TalonFXEncoder(ctre::phoenix6::hardware::TalonFX* controller,
@@ -150,9 +150,9 @@ wom::utils::CanEncoder::CanEncoder(int deviceNumber, units::meter_t wheelRadius,
 }
 
 double wom::utils::CanEncoder::GetEncoderRawTicks() const {
-  return _canEncoder->GetPosition().GetValue().value() * 2 * 3.14;
+  return (_canEncoder->GetAbsolutePosition().GetValue().value() * 2 * 3.14) - _offset.value();
 }
 
 double wom::utils::CanEncoder::GetEncoderTickVelocity() const {
   return _canEncoder->GetVelocity().GetValue().value();
-}
+}
\ No newline at end of file
diff --git a/wombat/src/main/include/drivetrain/SwerveDrive.h b/wombat/src/main/include/drivetrain/SwerveDrive.h
index c268cc86..25d27d04 100644
--- a/wombat/src/main/include/drivetrain/SwerveDrive.h
+++ b/wombat/src/main/include/drivetrain/SwerveDrive.h
@@ -152,8 +152,7 @@ enum class SwerveDriveState {
   kTuning,
   kXWheels,
   kModuleTurn,
-  kFRVelocityRotationLock,
-  kLockedOn
+  kFRVelocityRotationLock
 };
 
 struct FieldRelativeSpeeds {
@@ -201,7 +200,6 @@ class SwerveDrive : public behaviour::HasBehaviour {
   void SetZero();
   void SetVoltageLimit(units::volt_t driveVoltageLimit);
   void OnResetMode();
-  void SetLocked(units::degree_t pose);
   // double GetModuleCANPosition(int mod);  // from liam's
 
   void SetXWheelState();
@@ -247,18 +245,10 @@ class SwerveDrive : public behaviour::HasBehaviour {
   units::radian_t _angle;
   units::meters_per_second_t _speed;
 
-<<<<<<< HEAD
   // double frontLeftEncoderOffset = -143.26171875;
   // double frontRightEncoderOffset = 167.87109375;
   // double backLeftEncoderOffset = -316.669921875;
   // double backRightEncoderOffset = -119.619140625;
-=======
-  double frontLeftEncoderOffset = -143.26171875;
-  double frontRightEncoderOffset = 167.87109375;
-  double backLeftEncoderOffset = -316.669921875;
-  double backRightEncoderOffset = -119.619140625;
-
-  units::degree_t _lockedAngle;
 };
 }  // namespace drivetrain
 }  // namespace wom
\ No newline at end of file
diff --git a/wombat/src/main/include/subsystems/Arm.h b/wombat/src/main/include/subsystems/Arm.h
deleted file mode 100644
index 8b91be72..00000000
--- a/wombat/src/main/include/subsystems/Arm.h
+++ /dev/null
@@ -1,81 +0,0 @@
-// Copyright (c) 2023-2024 CurtinFRC
-// Open Source Software, you can modify it according to the terms
-// of the MIT License at the root of this project
-
-#pragma once
-
-#include <frc/DigitalInput.h>
-#include <frc/simulation/DIOSim.h>
-#include <units/current.h>
-#include <units/mass.h>
-#include <units/voltage.h>
-
-#include <memory>
-#include <string>
-
-#include "behaviour/HasBehaviour.h"
-#include "utils/Gearbox.h"
-#include "utils/PID.h"
-
-namespace wom {
-namespace subsystems {
-struct ArmConfig {
-  std::string path;
-
-  wom::utils::Gearbox leftGearbox;
-  wom::utils::Gearbox rightGearbox;
-  wom::utils::CANSparkMaxEncoder armEncoder;
-  wom::utils::PIDConfig<units::radian, units::volt> pidConfig;
-  wom::utils::PIDConfig<units::radians_per_second, units::volt> velocityConfig;
-
-  units::kilogram_t armMass;
-  units::kilogram_t loadMass;
-  units::meter_t armLength;
-  units::radian_t minAngle = 0_deg;
-  units::radian_t maxAngle = 180_deg;
-  units::radian_t initialAngle = 0_deg;
-  units::radian_t angleOffset = 0_deg;
-
-  void WriteNT(std::shared_ptr<nt::NetworkTable> table);
-};
-
-enum class ArmState { kIdle, kAngle, kRaw, kVelocity };
-
-class Arm : public behaviour::HasBehaviour {
- public:
-  explicit Arm(ArmConfig config);
-
-  void OnUpdate(units::second_t dt);
-
-  void SetIdle();
-  void SetAngle(units::radian_t angle);
-  void SetRaw(units::volt_t voltage);
-  void SetVelocity(units::radians_per_second_t velocity);
-
-  void SetState(wom::subsystems::ArmState state);
-
-  void SetArmSpeedLimit(double limit);  // units, what are they??
-
-  ArmConfig& GetConfig();
-
-  units::radian_t GetAngle() const;
-  units::radians_per_second_t MaxSpeed() const;
-  units::radians_per_second_t GetArmVelocity() const;
-
-  bool IsStable() const;
-
- private:
-  ArmConfig _config;
-  ArmState _state = ArmState::kIdle;
-  wom::utils::PIDController<units::radian, units::volt> _pid;
-  wom::utils::PIDController<units::radians_per_second, units::volt> _velocityPID;
-
-  std::shared_ptr<nt::NetworkTable> _table;
-
-  double armLimit = 0.4;
-  units::radians_per_second_t lastVelocity;
-
-  units::volt_t _voltage{0};
-};
-}  // namespace subsystems
-}  // namespace wom
diff --git a/wombat/src/main/include/subsystems/Shooter.h b/wombat/src/main/include/subsystems/Shooter.h
deleted file mode 100644
index e5ac219d..00000000
--- a/wombat/src/main/include/subsystems/Shooter.h
+++ /dev/null
@@ -1,75 +0,0 @@
-// Copyright (c) 2023-2024 CurtinFRC
-// Open Source Software, you can modify it according to the terms
-// of the MIT License at the root of this project
-
-#pragma once
-
-#include <networktables/NetworkTable.h>
-#include <units/angular_velocity.h>
-#include <units/charge.h>
-
-#include <memory>
-#include <string>
-
-#include "behaviour/Behaviour.h"
-#include "behaviour/HasBehaviour.h"
-#include "utils/Gearbox.h"
-#include "utils/PID.h"
-
-namespace wom {
-namespace subsystems {
-enum class ShooterState { kPID, kManual, kIdle };
-
-struct ShooterParams {
-  wom::utils::Gearbox gearbox;
-  wom::utils::PIDConfig<units::radians_per_second, units::volt> pid;
-  units::ampere_t currentLimit;
-};
-
-class Shooter : public behaviour::HasBehaviour {
- public:
-  Shooter(std::string path, ShooterParams params);
-
-  void SetManual(units::volt_t voltage);
-  void SetPID(units::radians_per_second_t goal);
-  void SetState(ShooterState state);
-
-  void OnUpdate(units::second_t dt);
-
-  bool IsStable() const;
-
- private:
-  ShooterParams _params;
-  ShooterState _state;
-
-  units::volt_t _setpointManual{0};
-
-  wom::utils::PIDController<units::radians_per_second, units::volt> _pid;
-
-  std::shared_ptr<nt::NetworkTable> _table;
-};
-
-class ShooterConstant : public behaviour::Behaviour {
- public:
-  ShooterConstant(Shooter* s, units::volt_t setpoint);
-
-  void OnTick(units::second_t dt) override;
-
- private:
-  Shooter* _shooter;
-  units::volt_t _setpoint;
-};
-
-class ShooterSpinup : public behaviour::Behaviour {
- public:
-  ShooterSpinup(Shooter* s, units::radians_per_second_t speed, bool hold = false);
-
-  void OnTick(units::second_t dt) override;
-
- private:
-  Shooter* _shooter;
-  units::radians_per_second_t _speed;
-  bool _hold;
-};
-}  // namespace subsystems
-}  // namespace wom
diff --git a/wombat/src/main/include/utils/Encoder.h b/wombat/src/main/include/utils/Encoder.h
index c6c6d85b..6b82882c 100644
--- a/wombat/src/main/include/utils/Encoder.h
+++ b/wombat/src/main/include/utils/Encoder.h
@@ -44,6 +44,8 @@ class Encoder {
   virtual double GetVelocity() const = 0;
   double GetVelocityValue() const;
 
+  units::radian_t _offset = 0_rad;  // bad
+
  private:
   double _encoderTicksPerRotation;
   int _type = 0;
diff --git a/wombat/src/main/include/utils/PID.h b/wombat/src/main/include/utils/PID.h
index 09afa04e..63df3666 100644
--- a/wombat/src/main/include/utils/PID.h
+++ b/wombat/src/main/include/utils/PID.h
@@ -104,7 +104,12 @@ class PIDController {
   void Reset() { _integralSum = sum_t{0}; }
 
   out_t Calculate(in_t pv, units::second_t dt, out_t feedforward = out_t{0}) {
-    pv = units::math::fabs(pv);
+    // pv = units::math::fabs(pv);
+    bool is_negative;
+    if (pv.value() < 0) {
+      is_negative = true;
+      pv = units::math::fabs(pv);
+    }
     auto error = do_wrap(_setpoint - pv);
     error = units::math::fabs(error);
     _integralSum += error * dt;

From 33ba8caee3e0347f1bd8c475b8eef934cab941ec Mon Sep 17 00:00:00 2001
From: = <paul.hodges@student.education.wa.edu.au>
Date: Wed, 14 Feb 2024 20:00:43 +0800
Subject: [PATCH 157/207] Photonvision

---
 src/main/cpp/ArmBehaviours.cpp                |  23 +++
 src/main/cpp/Robot.cpp                        |  35 +++-
 src/main/include/ArmBehaviours.h              |  23 +++
 src/main/include/Robot.h                      |   8 +-
 src/main/include/RobotMap.h                   |  57 +++++-
 .../src/main/cpp/drivetrain/SwerveDrive.cpp   |   2 +-
 .../behaviours/SwerveBehaviours.cpp           |   2 +-
 wombat/src/main/cpp/subsystems/Arm.cpp        | 187 ++++++++++++++++++
 wombat/src/main/cpp/subsystems/Elevator.cpp   | 141 +++++++++++++
 wombat/src/main/cpp/subsystems/Shooter.cpp    |  86 ++++++++
 wombat/src/main/cpp/utils/Encoder.cpp         |   2 +-
 wombat/src/main/cpp/vision/Camera.cpp         | 102 ++++++++++
 wombat/src/main/include/Wombat.h              |   2 +
 .../src/main/include/drivetrain/SwerveDrive.h |   2 +-
 .../drivetrain/behaviours/SwerveBehaviours.h  |   2 +-
 wombat/src/main/include/subsystems/Arm.h      |  83 ++++++++
 wombat/src/main/include/subsystems/Elevator.h |  84 ++++++++
 wombat/src/main/include/subsystems/Shooter.h  |  75 +++++++
 wombat/src/main/include/utils/Encoder.h       |   2 +-
 wombat/src/main/include/utils/PID.h           |   2 +-
 wombat/src/main/include/vision/Camera.h       |  61 ++++++
 wombat/vendordeps/WPILibNewCommands.json      |  38 ++++
 wombat/vendordeps/photonlib.json              |  42 ++++
 23 files changed, 1048 insertions(+), 13 deletions(-)
 create mode 100644 src/main/cpp/ArmBehaviours.cpp
 create mode 100644 src/main/include/ArmBehaviours.h
 create mode 100644 wombat/src/main/cpp/subsystems/Arm.cpp
 create mode 100644 wombat/src/main/cpp/subsystems/Elevator.cpp
 create mode 100644 wombat/src/main/cpp/subsystems/Shooter.cpp
 create mode 100644 wombat/src/main/cpp/vision/Camera.cpp
 create mode 100644 wombat/src/main/include/subsystems/Arm.h
 create mode 100644 wombat/src/main/include/subsystems/Elevator.h
 create mode 100644 wombat/src/main/include/subsystems/Shooter.h
 create mode 100644 wombat/src/main/include/vision/Camera.h
 create mode 100644 wombat/vendordeps/WPILibNewCommands.json
 create mode 100644 wombat/vendordeps/photonlib.json

diff --git a/src/main/cpp/ArmBehaviours.cpp b/src/main/cpp/ArmBehaviours.cpp
new file mode 100644
index 00000000..09b54d78
--- /dev/null
+++ b/src/main/cpp/ArmBehaviours.cpp
@@ -0,0 +1,23 @@
+// Copyright (c) 2023-2024 CurtinFRC
+// Open Source Software, you can modify it according to the terms
+// of the MIT License at the root of this project
+
+#include "ArmBehaviours.h"
+
+#include "Wombat.h"
+#include "subsystems/Arm.h"
+
+ArmManualControl::ArmManualControl(wom::Arm* arm, frc::XboxController* codriver)
+    : _arm(arm), _codriver(codriver) {
+  Controls(arm);
+}
+
+void ArmManualControl::OnTick(units::second_t dt) {
+  if (_codriver->GetXButtonPressed()) {
+    _arm->SetState(wom::ArmState::kRaw);
+  }
+
+  if (_arm->GetState() == wom::ArmState::kRaw) {
+    _arm->SetRaw(_codriver->GetRightY() * 6_V);
+  }
+}
diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp
index f09b1685..f55a0ea0 100644
--- a/src/main/cpp/Robot.cpp
+++ b/src/main/cpp/Robot.cpp
@@ -29,7 +29,11 @@
 #include <frc/kinematics/DifferentialDriveKinematics.h>
 #include <frc/controller/RamseteController.h>
 #include <frc/Timer.h>
+#include "ArmBehaviours.h"
 #include "behaviour/HasBehaviour.h"
+#include "drivetrain/behaviours/SwerveBehaviours.h"
+#include "frc/geometry/Pose2d.h"
+#include "vision/Vision.h"
 
 static units::second_t lastPeriodic;
 
@@ -37,6 +41,8 @@ void Robot::RobotInit() {
   sched = wom::BehaviourScheduler::GetInstance();
   m_chooser.SetDefaultOption("Default Auto", "Default Auto");
 
+  vision = new Vision("limelight", FMAP("fmap.fmap"));
+
   // frc::SmartDashboard::PutData("Auto Selector", &m_chooser);
 
   // m_path_chooser.SetDefaultOption("Path1", "paths/output/Path1.wpilib.json");
@@ -60,8 +66,14 @@ void Robot::RobotInit() {
 
   _swerveDrive = new wom::SwerveDrive(robotmap.swerveBase.config, frc::Pose2d());
   wom::BehaviourScheduler::GetInstance()->Register(_swerveDrive);
-  _swerveDrive->SetDefaultBehaviour(
-      [this]() { return wom::make<wom::ManualDrivebase>(_swerveDrive, &robotmap.controllers.driver); });
+
+  _arm = new wom::Arm(robotmap.arm.config);
+  wom::BehaviourScheduler::GetInstance()->Register(_arm);
+
+  _arm->SetDefaultBehaviour(
+      [this]() { return wom::make<ArmManualControl>(_arm, &robotmap.controllers.codriver); });
+  //_swerveDrive->SetDefaultBehaviour(
+  //    [this]() { return wom::make<wom::ManualDrivebase>(_swerveDrive, &robotmap.controllers.driver); });
 
   // alphaArm = new AlphaArm(robotmap.alphaArmSystem.config);
   // wom::BehaviourScheduler::GetInstance()->Register(alphaArm);
@@ -124,6 +136,7 @@ void Robot::RobotPeriodic() {
   // intake->OnUpdate(dt);
   // alphaArm->OnUpdate(dt);
   _swerveDrive->OnUpdate(dt);
+  _arm->OnUpdate(dt);
 }
 
 void Robot::AutonomousInit() {
@@ -148,10 +161,24 @@ void Robot::TeleopInit() {
   // backLeft->SetVoltage(4_V);
   // backRight->SetVoltage(4_V);
 }
-void Robot::TeleopPeriodic() {}
+void Robot::TeleopPeriodic() {
+  if (robotmap.controllers.driver.GetXButtonPressed() &&
+      vision->TargetIsVisible(VisionTargetObjects::kNote)) {
+    units::degree_t turn = vision->LockOn(VisionTargetObjects::kNote);
+
+    frc::Pose2d current_pose = _swerveDrive->GetPose();
+
+    std::cout << "angle: " << turn.value() << std::endl;
+    current_pose.RotateBy(turn);
+
+    wom::make<wom::DrivebasePoseBehaviour>(_swerveDrive, current_pose);
+  } else {
+    wom::make<wom::ManualDrivebase>(_swerveDrive, &robotmap.controllers.driver);
+  }
+}
 
 void Robot::DisabledInit() {}
 void Robot::DisabledPeriodic() {}
 
 void Robot::TestInit() {}
-void Robot::TestPeriodic() {}
\ No newline at end of file
+void Robot::TestPeriodic() {}
diff --git a/src/main/include/ArmBehaviours.h b/src/main/include/ArmBehaviours.h
new file mode 100644
index 00000000..2b2ab07b
--- /dev/null
+++ b/src/main/include/ArmBehaviours.h
@@ -0,0 +1,23 @@
+// Copyright (c) 2023-2024 CurtinFRC
+// Open Source Software, you can modify it according to the terms
+// of the MIT License at the root of this project
+
+#pragma once
+
+#include <frc/XboxController.h>
+
+#include "AlphaArm.h"
+#include "Wombat.h"
+
+class ArmManualControl : public behaviour::Behaviour {
+ public:
+  explicit ArmManualControl(wom::Arm* arm, frc::XboxController* codriver);
+  void OnTick(units::second_t dt);
+
+ private:
+  wom::Arm* _arm;
+  frc::XboxController* _codriver;
+  // units::volt_t _rightStick = ((_codriver->GetRightY()>0.05 || _codriver->GetRightY() < -0.05
+  // )?_codriver->GetRightY():0) * 2_V;
+  bool _rawControl;
+};
diff --git a/src/main/include/Robot.h b/src/main/include/Robot.h
index 40728250..d475b14d 100644
--- a/src/main/include/Robot.h
+++ b/src/main/include/Robot.h
@@ -3,6 +3,7 @@
 // of the MIT License at the root of this project
 
 #pragma once
+
 #include <frc/Encoder.h>
 #include <frc/TimedRobot.h>
 #include <frc/Timer.h>
@@ -23,6 +24,8 @@
 #include "Shooter.h"
 #include "ShooterBehaviour.h"
 #include "Wombat.h"
+#include "subsystems/Arm.h"
+#include "vision/Vision.h"
 
 class Robot : public frc::TimedRobot {
  public:
@@ -55,6 +58,8 @@ class Robot : public frc::TimedRobot {
   wom::SwerveDrive* _swerveDrive;
   // AlphaArm* alphaArm;
 
+  wom::Arm* _arm;
+
   ctre::phoenix6::hardware::TalonFX* frontLeft;
   // AlphaArm *alphaArm;
 
@@ -62,4 +67,5 @@ class Robot : public frc::TimedRobot {
   // ctre::phoenix6::hardware::TalonFX *frontRight;
   // ctre::phoenix6::hardware::TalonFX *backLeft;
   // ctre::phoenix6::hardware::TalonFX *backRight;
-};
\ No newline at end of file
+  Vision* vision;
+};
diff --git a/src/main/include/RobotMap.h b/src/main/include/RobotMap.h
index 0ed68e8d..ae06f78d 100644
--- a/src/main/include/RobotMap.h
+++ b/src/main/include/RobotMap.h
@@ -96,6 +96,61 @@ struct RobotMap {
   //   };
   //   Shooter shooterSystem;
 
+  struct Arm {
+    // creates the motor used for the arm as well as the port it is plugged in
+    rev::CANSparkMax leftArmMotor{21, rev::CANSparkMax::MotorType::kBrushless};   // 11
+    rev::CANSparkMax rightArmMotor{26, rev::CANSparkMax::MotorType::kBrushless};  // 12
+
+    // rev::CANSparkMax leftPretendArmMotor{28, rev::CANSparkMax::MotorType::kBrushless};
+    // rev::CANSparkMax rightPretendArmMotor{29, rev::CANSparkMax::MotorType::kBrushless};
+
+    // wom::DigitalEncoder encoder{0, 1, 2048};
+    // sets the type sof encoder that is used up
+    wom::CANSparkMaxEncoder* leftEncoder = new wom::CANSparkMaxEncoder(&leftArmMotor, 10_cm);
+    wom::CANSparkMaxEncoder* rightEncoder = new wom::CANSparkMaxEncoder(&rightArmMotor, 10_cm);
+
+    // wom::DutyCycleEncoder *encoder = new wom::DutyCycleEncoder(3, 10_cm);
+
+    // wom::CANSparkMaxEncoder leftEncoder{&leftArmMotor, 100};
+    // wom::CANSparkMaxEncoder rightEncoder{&rightArmMotor, 100};
+
+    // rev::SparkMaxAbsoluteEncoder leftOtherArmEncoder{leftArmMotor,
+    // rev::CANSparkMax::SparkMaxAbsoluteEncoder::Type::kDutyCycle}; wom::CANSparkMaxEncoder
+    // leftOtherArmEncoder = leftArmMotor.GetEncoder(); wom::CANSparkMaxEncoder rightOtherArmEncoder =
+    // rightArmMotor.GetEncoder();
+
+    // creates an instance of the arm gearbox
+    wom::Gearbox leftGearbox{&leftArmMotor, leftEncoder,
+                             // nullptr,
+                             frc::DCMotor::NEO(1).WithReduction(100)};
+
+    wom::Gearbox rightGearbox{&rightArmMotor, rightEncoder,
+                              // nullptr,
+                              frc::DCMotor::NEO(1).WithReduction(100)};
+
+    // creates arm config information
+    wom::ArmConfig config{
+        "/armavator/arm", leftGearbox, rightGearbox,
+        // nullptr,
+        leftEncoder,
+        wom::PIDConfig<units::radian, units::volts>("/armavator/arm/pid/config",
+                                                    18_V / 25_deg,        // prev 13_V/25_deg
+                                                    0_V / (1_deg * 1_s),  // 0.1_V / (1_deg * 1_s)
+                                                    0_V / (1_deg / 1_s),  // 0_V / (1_deg / 1_s)
+                                                    5_deg, 2_deg / 1_s, 10_deg),
+        wom::PIDConfig<units::radians_per_second, units::volts>("/armavator/arm/velocityPID/config",
+                                                                9_V / (180_deg / 1_s), 0_V / 25_deg,
+                                                                0_V / (90_deg / 1_s / 1_s)),
+        1_kg, 0.5_kg, 1.15_m, -90_deg, 270_deg, 0_deg};
+
+    Arm() {
+      // inverts the motor so that it goes in the right direction while using RAW controlls
+      leftArmMotor.SetInverted(true);
+      rightArmMotor.SetInverted(false);
+    }
+  };
+  Arm arm;
+
   struct SwerveBase {
     ctre::phoenix6::hardware::CANcoder frontLeftCancoder{16, "Drivebase"};
     ctre::phoenix6::hardware::CANcoder frontRightCancoder{18, "Drivebase"};
@@ -210,4 +265,4 @@ struct RobotMap {
   //   AlphaArmConfig config{alphaArmGearbox, wristGearbox};
   // };
   // AlphaArmSystem alphaArmSystem;
-};
\ No newline at end of file
+};
diff --git a/wombat/src/main/cpp/drivetrain/SwerveDrive.cpp b/wombat/src/main/cpp/drivetrain/SwerveDrive.cpp
index b6772015..701fae07 100644
--- a/wombat/src/main/cpp/drivetrain/SwerveDrive.cpp
+++ b/wombat/src/main/cpp/drivetrain/SwerveDrive.cpp
@@ -514,4 +514,4 @@ void SwerveDrive::AddVisionMeasurement(frc::Pose2d pose, units::second_t timesta
   _poseEstimator.AddVisionMeasurement(pose, timestamp);
 }
 }  // namespace drivetrain
-}  // namespace wom
\ No newline at end of file
+}  // namespace wom
diff --git a/wombat/src/main/cpp/drivetrain/behaviours/SwerveBehaviours.cpp b/wombat/src/main/cpp/drivetrain/behaviours/SwerveBehaviours.cpp
index 8a5cceeb..db6a035d 100644
--- a/wombat/src/main/cpp/drivetrain/behaviours/SwerveBehaviours.cpp
+++ b/wombat/src/main/cpp/drivetrain/behaviours/SwerveBehaviours.cpp
@@ -250,4 +250,4 @@ void wom::drivetrain::behaviours::DrivebasePoseBehaviour::OnTick(units::second_t
   if (_swerveDrivebase->IsAtSetPose() && !_hold) {
     SetDone();
   }
-}
\ No newline at end of file
+}
diff --git a/wombat/src/main/cpp/subsystems/Arm.cpp b/wombat/src/main/cpp/subsystems/Arm.cpp
new file mode 100644
index 00000000..a451d5cf
--- /dev/null
+++ b/wombat/src/main/cpp/subsystems/Arm.cpp
@@ -0,0 +1,187 @@
+// Copyright (c) 2023-2024 CurtinFRC
+// Open Source Software, you can modify it according to the terms
+// of the MIT License at the root of this project
+
+#include "subsystems/Arm.h"
+
+#include <units/math.h>
+
+using namespace frc;
+using namespace wom;
+
+// creates network table instatnce on shuffleboard
+void wom::subsystems::ArmConfig::WriteNT(std::shared_ptr<nt::NetworkTable> table) {
+  table->GetEntry("armMass").SetDouble(armMass.value());
+  table->GetEntry("loadMass").SetDouble(loadMass.value());
+  table->GetEntry("armLength").SetDouble(armLength.value());
+  table->GetEntry("minAngle").SetDouble(minAngle.convert<units::degree>().value());
+  table->GetEntry("maxAngle").SetDouble(maxAngle.convert<units::degree>().value());
+  table->GetEntry("initialAngle").SetDouble(initialAngle.convert<units::degree>().value());
+  table->GetEntry("angleOffset").SetDouble(initialAngle.convert<units::degree>().value());
+}
+
+// arm config is used
+wom::subsystems::Arm::Arm(ArmConfig config)
+    : _config(config),
+      _pid(config.path + "/pid", config.pidConfig),
+      _velocityPID(config.path + "/velocityPID", config.velocityConfig),
+      _table(nt::NetworkTableInstance::GetDefault().GetTable(config.path)) {}
+
+// the loop that allows the information to be used
+void wom::subsystems::Arm::OnUpdate(units::second_t dt) {
+  // sets the voltage and gets the current angle
+  units::volt_t voltage = 0_V;
+  auto angle = GetAngle();
+
+  // sets usable infromation for each state
+  switch (_state) {
+    case ArmState::kIdle:
+      break;
+    case ArmState::kVelocity: {
+      units::newton_meter_t torque = 9.81_m / 1_s / 1_s * _config.armLength *
+                                     units::math::cos(angle + _config.angleOffset) *
+                                     (0.5 * _config.armMass + _config.loadMass);
+      // units::volt_t feedforward = _config.leftGearbox.motor.Voltage(torque,
+      // 0_rad/1_s);
+      units::volt_t feedforward = _config.leftGearbox.motor.Voltage(torque, _velocityPID.GetSetpoint());
+      // feedforward = 3.5_V;
+      // std::cout << "feedforward" << feedforward.value() << std::endl;
+      voltage = _velocityPID.Calculate(GetArmVelocity(), dt, feedforward);
+      // std::cout << "arm velocity voltage is: " << voltage.value() <<
+      // std::endl; voltage = 0_V;
+    } break;
+    case ArmState::kAngle: {
+      units::newton_meter_t torque = 9.81_m / 1_s / 1_s * _config.armLength *
+                                     units::math::cos(angle + _config.angleOffset) *
+                                     (0.5 * _config.armMass + _config.loadMass);
+      units::volt_t feedforward = _config.leftGearbox.motor.Voltage(torque, 0_rad / 1_s);
+      // std::cout << "feedforward" << feedforward.value() << std::endl;
+      voltage = _pid.Calculate(angle, dt, feedforward);
+    } break;
+    case ArmState::kRaw:
+      voltage = _voltage;
+      break;
+  }
+
+  voltage *= armLimit;
+
+  // units::newton_meter_t torqueLimit = 10_kg * 1.4_m * 6_mps_sq;
+  // units::volt_t voltageMax = _config.leftGearbox.motor.Voltage(torqueLimit,
+  // _config.leftGearbox.encoder->GetEncoderAngularVelocity()); units::volt_t
+  // voltageMin = _config.leftGearbox.motor.Voltage(-torqueLimit,
+  // _config.leftGearbox.encoder->GetEncoderAngularVelocity());
+
+  // voltage = units::math::max(units::math::min(voltage, voltageMax),
+  // voltageMin);
+  units::volt_t voltageMin = -5.5_V;
+  units::volt_t voltageMax = 5.5_V;
+  voltage = units::math::max(units::math::min(voltage, voltageMax), voltageMin);
+
+  // std::cout << "voltage: " << voltage.value() << std::endl;
+
+  // _config.leftGearbox.motorController->SetVoltage(voltage);
+  // _config.rightGearbox.motorController->SetVoltage(voltage);
+
+  // creates network table instances for the angle and config of the arm
+  _table->GetEntry("angle").SetDouble(angle.convert<units::degree>().value());
+  _config.WriteNT(_table->GetSubTable("config"));
+}
+
+void wom::subsystems::Arm::SetArmSpeedLimit(double limit) {
+  armLimit = limit;
+}
+
+// defines information needed for the functions and connects the states to their
+// respective function
+
+void wom::subsystems::Arm::SetRaw(units::volt_t voltage) {
+  _voltage = voltage;
+}
+
+void wom::subsystems::Arm::SetVelocity(units::radians_per_second_t velocity) {
+  _velocityPID.SetSetpoint(velocity);
+}
+
+void wom::subsystems::Arm::SetAngle(units::radian_t angle) {
+  _pid.SetSetpoint(angle);
+}
+
+void wom::subsystems::Arm::SetState(wom::subsystems::ArmState state) {
+  _state = state;
+}
+
+wom::subsystems::ArmConfig& wom::subsystems::Arm::GetConfig() {
+  return _config;
+}
+
+wom::subsystems::ArmState wom::subsystems::Arm::GetState() {
+  return _state;
+}
+
+units::radian_t wom::subsystems::Arm::GetAngle() const {
+  return _config.armEncoder->GetPosition() / 100 * 360 * 1_deg;
+}
+
+units::radians_per_second_t wom::subsystems::Arm::MaxSpeed() const {
+  return _config.leftGearbox.motor.Speed(0_Nm, 12_V);
+}
+
+units::radians_per_second_t wom::subsystems::Arm::GetArmVelocity() const {
+  return _config.armEncoder->GetVelocity() / 100 * 360 * 1_deg / 60_s;
+}
+
+bool wom::subsystems::Arm::IsStable() const {
+  return _pid.IsStable(5_deg);
+}
+
+/* SIMULATION */
+// #include <units/math.h>
+
+// ::wom::sim::ArmSim::ArmSim(ArmConfig config)
+//   : config(config),
+//     angle(config.initialAngle),
+//     encoder(config.gearbox.encoder->MakeSimEncoder()),
+//     lowerLimit(config.lowerLimitSwitch ? new
+//     frc::sim::DIOSim(*config.lowerLimitSwitch) : nullptr),
+//     upperLimit(config.upperLimitSwitch ? new
+//     frc::sim::DIOSim(*config.upperLimitSwitch) : nullptr),
+//     table(nt::NetworkTableInstance::GetDefault().GetTable(config.path +
+//     "/sim"))
+//   {}
+
+// units::ampere_t wom::sim::ArmSim::GetCurrent() const {
+//   return current;
+// }
+
+// void ::wom::sim::ArmSim::Update(units::second_t dt) {
+//   torque = (config.loadMass * config.armLength + config.armMass *
+//   config.armLength / 2.0) * 9.81_m / 1_s / 1_s *
+//   units::math::cos(config.angleOffset + angle) + additionalTorque; velocity =
+//   config.gearbox.motor.Speed(torque,
+//   config.gearbox.transmission->GetEstimatedRealVoltage()); angle += velocity
+//   * dt;
+
+//   if (angle <= config.minAngle) {
+//     angle = config.minAngle;
+//     velocity = 0_rad / 1_s;
+//     if (lowerLimit) lowerLimit->SetValue(true);
+//   } else {
+//     if (lowerLimit) lowerLimit->SetValue(false);
+//   }
+
+//   if (angle >= config.maxAngle) {
+//     angle = config.maxAngle;
+//     velocity = 0_rad / 1_s;
+//     if (upperLimit) upperLimit->SetValue(true);
+//   } else {
+//     if (upperLimit) upperLimit->SetValue(false);
+//   }
+
+//   current = config.gearbox.motor.Current(velocity,
+//   config.gearbox.transmission->GetEstimatedRealVoltage());
+
+//   if (encoder) encoder->SetEncoderTurns(angle - config.initialAngle);
+
+//   table->GetEntry("angle").SetDouble(angle.convert<units::degree>().value());
+//   table->GetEntry("current").SetDouble(current.value());
+// }
diff --git a/wombat/src/main/cpp/subsystems/Elevator.cpp b/wombat/src/main/cpp/subsystems/Elevator.cpp
new file mode 100644
index 00000000..d1cf9a8f
--- /dev/null
+++ b/wombat/src/main/cpp/subsystems/Elevator.cpp
@@ -0,0 +1,141 @@
+// Copyright (c) 2023-2024 CurtinFRC
+// Open Source Software, you can modify it according to the terms
+// of the MIT License at the root of this project
+
+#include "subsystems/Elevator.h"
+
+#include <networktables/NetworkTableInstance.h>
+#include <units/acceleration.h>
+
+void wom::subsystems::ElevatorConfig::WriteNT(std::shared_ptr<nt::NetworkTable> table) {
+  table->GetEntry("radius").SetDouble(radius.value());
+  table->GetEntry("mass").SetDouble(mass.value());
+  table->GetEntry("maxHeight").SetDouble(maxHeight.value());
+}
+
+wom::subsystems::Elevator::Elevator(ElevatorConfig config)
+    : _config(config),
+      _state(ElevatorState::kIdle),
+      _pid{config.path + "/pid", config.pid},
+      _velocityPID{config.path + "/velocityPID", config.velocityPID},
+      _table(nt::NetworkTableInstance::GetDefault().GetTable(config.path)) {
+  _config.leftGearbox.encoder->SetEncoderPosition(_config.initialHeight / _config.radius * 1_rad);
+}
+
+void wom::subsystems::Elevator::OnUpdate(units::second_t dt) {
+  units::volt_t voltage{0};
+
+  units::meter_t height = GetElevatorEncoderPos() * 1_m;
+
+  switch (_state) {
+    case ElevatorState::kIdle:
+      voltage = 0_V;
+      break;
+    case ElevatorState::kManual:
+      voltage = _setpointManual;
+      break;
+    case ElevatorState::kVelocity: {
+      units::volt_t feedforward = _config.rightGearbox.motor.Voltage(
+          (_config.mass * 9.81_mps_sq) * _config.radius,
+          _velocityPID.GetSetpoint() / (14.0 / 60.0 * 2.0 * 3.1415 * 0.02225 * 1_m) * 1_rad);
+      // units::volt_t feedforward = _config.rightGearbox.motor.Voltage(0_Nm,
+      // _velocityPID.GetSetpoint() / (14.0/60.0 * 2.0 * 3.1415 * 0.02225 * 1_m)
+      // * 1_rad);
+      feedforward = 1.2_V;
+      voltage = _velocityPID.Calculate(GetElevatorVelocity(), dt, feedforward);
+      if (voltage > 6_V) {
+        voltage = 6_V;
+      }
+      std::cout << "elevator feedforward: " << feedforward.value() << std::endl;
+      // voltage = 0_V;
+    } break;
+    case ElevatorState::kPID: {
+      units::volt_t feedforward =
+          _config.rightGearbox.motor.Voltage((_config.mass * 9.81_mps_sq) * _config.radius, 0_rad_per_s);
+      // std::cout << "feed forward" << feedforward.value() << std::endl;
+      feedforward = 1.2_V;
+      // voltage = _pid.Calculate(height, dt, feedforward);
+      voltage = _pid.Calculate(height, dt, feedforward);
+      if (voltage > 6_V) {
+        voltage = 6_V;
+      }
+    } break;
+  }
+
+  // Top Sensor Detector
+  // if(_config.topSensor != nullptr) {
+  //   if(_config.topSensor->Get()) {
+  //     _config.leftGearbox.encoder->SetEncoderPosition(_config.maxHeight /
+  //     _config.radius * 1_rad);
+  //     //voltage = 0_V;
+  //   }
+  // }
+
+  // //Bottom Sensor Detection
+  // if (_config.bottomSensor != nullptr) {
+  //   if (_config.bottomSensor->Get()) {
+  //     _config.leftGearbox.encoder->SetEncoderPosition(_config.minHeight /
+  //     _config.radius * 1_rad);
+  //     //voltage = 0_V;
+  //   }
+  // }
+
+  // std::cout << "elevator: " << elevator.height
+
+  // Set voltage to motors...
+  voltage *= speedLimit;
+  // _config.leftGearbox.motorController->SetVoltage(voltage);
+  // _config.rightGearbox.motorController->SetVoltage(voltage);
+}
+
+void wom::subsystems::Elevator::SetManual(units::volt_t voltage) {
+  _setpointManual = voltage;
+}
+
+void wom::subsystems::Elevator::SetPID(units::meter_t height) {
+  _pid.SetSetpoint(height);
+}
+
+void wom::subsystems::Elevator::SetVelocity(units::meters_per_second_t velocity) {
+  _velocityPID.SetSetpoint(velocity);
+}
+
+void wom::subsystems::Elevator::SetState(wom::subsystems::ElevatorState state) {
+  _state = state;
+}
+
+void wom::subsystems::Elevator::SetElevatorSpeedLimit(double limit) {
+  speedLimit = limit;
+}
+
+wom::subsystems::ElevatorConfig& wom::subsystems::Elevator::GetConfig() {
+  return _config;
+}
+
+bool wom::subsystems::Elevator::IsStable() const {
+  return _pid.IsStable();
+}
+
+wom::subsystems::ElevatorState wom::subsystems::Elevator::GetState() const {
+  return _state;
+}
+
+double wom::subsystems::Elevator::GetElevatorEncoderPos() {
+  return _config.elevatorEncoder.GetPosition() * 14 / 60 * 2 * 3.1415 * 0.02225;
+}
+
+units::meter_t wom::subsystems::Elevator::GetHeight() const {
+  // std::cout << "elevator position"<<
+  // _config.rightGearbox.encoder->GetEncoderTicks() << std::endl; return
+  // _config.rightGearbox.encoder->GetEncoderDistance() * 1_m;
+  return _config.elevatorEncoder.GetPosition() * 14 / 60 * 2 * 3.1415 * 0.02225 * 1_m;
+}
+
+units::meters_per_second_t wom::subsystems::Elevator::GetElevatorVelocity() const {
+  return _config.elevatorEncoder.GetVelocity() / 60_s * 14 / 60 * 2 * 3.1415 * 0.02225 * 1_m;
+}
+
+units::meters_per_second_t wom::subsystems::Elevator::MaxSpeed() const {
+  return _config.leftGearbox.motor.Speed((_config.mass * 9.81_mps_sq) * _config.radius, 12_V) / 1_rad *
+         _config.radius;
+}
diff --git a/wombat/src/main/cpp/subsystems/Shooter.cpp b/wombat/src/main/cpp/subsystems/Shooter.cpp
new file mode 100644
index 00000000..c851266b
--- /dev/null
+++ b/wombat/src/main/cpp/subsystems/Shooter.cpp
@@ -0,0 +1,86 @@
+// Copyright (c) 2023-2024 CurtinFRC
+// Open Source Software, you can modify it according to the terms
+// of the MIT License at the root of this project
+
+#include "subsystems/Shooter.h"
+
+#include <networktables/NetworkTableInstance.h>
+
+wom::subsystems::Shooter::Shooter(std::string path, ShooterParams params)
+    : _params(params),
+      _state(ShooterState::kIdle),
+      _pid{path + "/pid", params.pid},
+      _table(nt::NetworkTableInstance::GetDefault().GetTable("shooter")) {}
+
+void wom::subsystems::Shooter::OnUpdate(units::second_t dt) {
+  units::volt_t voltage{0};
+  units::revolutions_per_minute_t currentSpeed = _params.gearbox.encoder->GetEncoderAngularVelocity();
+
+  switch (_state) {
+    case ShooterState::kManual:
+      voltage = _setpointManual;
+      break;
+    case ShooterState::kPID: {
+      auto feedforward = _params.gearbox.motor.Voltage(0_Nm, _pid.GetSetpoint());
+      voltage = _pid.Calculate(currentSpeed, dt, feedforward);
+    } break;
+    case ShooterState::kIdle:
+      voltage = 0_V;
+      break;
+  }
+
+  units::newton_meter_t max_torque_at_current_limit = _params.gearbox.motor.Torque(_params.currentLimit);
+  units::volt_t max_voltage_for_current_limit =
+      _params.gearbox.motor.Voltage(max_torque_at_current_limit, currentSpeed);
+
+  voltage = 1_V * std::min(voltage.value(), max_voltage_for_current_limit.value());
+
+  // _params.gearbox.motorController->SetVoltage(voltage);
+
+  _table->GetEntry("output_volts").SetDouble(voltage.value());
+  _table->GetEntry("speed_rpm").SetDouble(currentSpeed.value());
+  _table->GetEntry("setpoint_rpm").SetDouble(units::revolutions_per_minute_t{_pid.GetSetpoint()}.value());
+  _table->GetEntry("stable").SetBoolean(_pid.IsStable());
+}
+
+void wom::subsystems::Shooter::SetManual(units::volt_t voltage) {
+  _setpointManual = voltage;
+}
+
+void wom::subsystems::Shooter::SetPID(units::radians_per_second_t goal) {
+  _state = ShooterState::kPID;
+  _pid.SetSetpoint(goal);
+}
+
+void wom::subsystems::Shooter::SetState(ShooterState state) {
+  _state = state;
+}
+
+bool wom::subsystems::Shooter::IsStable() const {
+  return _pid.IsStable();
+}
+
+// Shooter Manual Set
+
+wom::subsystems::ShooterConstant::ShooterConstant(Shooter* s, units::volt_t setpoint)
+    : _shooter(s), _setpoint(setpoint) {
+  Controls(_shooter);
+}
+
+void wom::subsystems::ShooterConstant::OnTick(units::second_t dt) {
+  _shooter->SetManual(_setpoint);
+}
+
+// ShooterSpinup
+
+wom::subsystems::ShooterSpinup::ShooterSpinup(Shooter* s, units::radians_per_second_t speed, bool hold)
+    : _shooter(s), _speed(speed), _hold(hold) {
+  Controls(_shooter);
+}
+
+void wom::subsystems::ShooterSpinup::OnTick(units::second_t dt) {
+  _shooter->SetPID(_speed);
+
+  if (!_hold && _shooter->IsStable())
+    SetDone();
+}
diff --git a/wombat/src/main/cpp/utils/Encoder.cpp b/wombat/src/main/cpp/utils/Encoder.cpp
index 431a164f..1120b399 100644
--- a/wombat/src/main/cpp/utils/Encoder.cpp
+++ b/wombat/src/main/cpp/utils/Encoder.cpp
@@ -155,4 +155,4 @@ double wom::utils::CanEncoder::GetEncoderRawTicks() const {
 
 double wom::utils::CanEncoder::GetEncoderTickVelocity() const {
   return _canEncoder->GetVelocity().GetValue().value();
-}
\ No newline at end of file
+}
diff --git a/wombat/src/main/cpp/vision/Camera.cpp b/wombat/src/main/cpp/vision/Camera.cpp
new file mode 100644
index 00000000..a224a35b
--- /dev/null
+++ b/wombat/src/main/cpp/vision/Camera.cpp
@@ -0,0 +1,102 @@
+// Copyright (c) 2023-2024 CurtinFRC
+// Open Source Software, you can modify it according to the terms
+// of the MIT License at the root of this project
+
+#include "vision/Camera.h"
+
+#include <cstddef>
+#include <vector>
+
+#include "frc/geometry/Transform3d.h"
+
+PhotonVision::PhotonVision() : _name("photonvision") {
+  Initialize();
+};
+
+PhotonVision::PhotonVision(std::string name) : _name(name) {
+  Initialize();
+};
+
+void PhotonVision::Initialize() {
+  _camera = new photonlib::PhotonCamera(_name);
+
+  SetMode(PhotonVisionModes::kObject);
+}
+
+photonlib::PhotonPipelineResult PhotonVision::GetResult() {
+  return _camera->GetLatestResult();
+}
+
+void PhotonVision::SetLED(PhotonVisionLEDMode mode) {
+  _camera->SetLEDMode(mode);
+}
+
+void PhotonVision::SetPipelineIndex(int index) {
+  _camera->SetPipelineIndex(index);
+}
+
+bool PhotonVision::HasTarget() {
+  return GetResult().HasTargets();
+}
+
+std::vector<photonTarget> PhotonVision::GetTargets() {
+  const std::span<const photonTarget> res = GetResult().GetTargets();
+
+  std::vector<photonTarget> targets;
+
+  for (size_t i = 0; i < res.size(); i++) {
+    targets.push_back(res[i]);
+  };
+
+  return targets;
+}
+
+photonTarget PhotonVision::GetTarget() {
+  return GetResult().GetBestTarget();
+}
+
+double PhotonVision::GetTargetYaw(photonTarget target) {
+  return target.GetYaw();
+}
+
+double PhotonVision::GetTargetPitch(photonTarget target) {
+  return target.GetPitch();
+}
+
+double PhotonVision::GetTargetArea(photonTarget target) {
+  return target.GetArea();
+}
+
+double PhotonVision::GetTargetSkew(photonTarget target) {
+  return target.GetSkew();
+}
+
+frc::Transform3d PhotonVision::GetCameraToTarget(photonTarget target) {
+  return target.GetBestCameraToTarget();
+}
+
+std::vector<std::pair<double, double>> PhotonVision::GetTargetCorners(photonTarget target) {
+  return target.GetDetectedCorners();
+}
+
+int PhotonVision::GetTargetId(photonTarget target) {
+  return target.GetFiducialId();
+}
+
+frc::Transform3d PhotonVision::BestCameraToTarget(photonTarget target) {
+  return target.GetBestCameraToTarget();
+}
+
+frc::Transform3d PhotonVision::AlternateCameraToTarget(photonTarget target) {
+  return target.GetAlternateCameraToTarget();
+}
+
+PhotonVisionModes PhotonVision::GetMode() {
+  return _mode;
+}
+
+void PhotonVision::SetMode(PhotonVisionModes mode) {
+  _mode = mode;
+
+  SetPipelineIndex(static_cast<int>(mode));
+}
diff --git a/wombat/src/main/include/Wombat.h b/wombat/src/main/include/Wombat.h
index f0037344..89a967a7 100644
--- a/wombat/src/main/include/Wombat.h
+++ b/wombat/src/main/include/Wombat.h
@@ -9,6 +9,7 @@
 #include "behaviour/HasBehaviour.h"
 #include "drivetrain/SwerveDrive.h"
 #include "drivetrain/behaviours/SwerveBehaviours.h"
+#include "subsystems/Arm.h"
 #include "utils/Encoder.h"
 #include "utils/Gearbox.h"
 #include "utils/PID.h"
@@ -18,6 +19,7 @@
 
 namespace wom {
 using namespace wom;
+using namespace wom::subsystems;
 using namespace wom::utils;
 using namespace wom::drivetrain;
 using namespace wom::drivetrain::behaviours;
diff --git a/wombat/src/main/include/drivetrain/SwerveDrive.h b/wombat/src/main/include/drivetrain/SwerveDrive.h
index 25d27d04..d8b8e482 100644
--- a/wombat/src/main/include/drivetrain/SwerveDrive.h
+++ b/wombat/src/main/include/drivetrain/SwerveDrive.h
@@ -251,4 +251,4 @@ class SwerveDrive : public behaviour::HasBehaviour {
   // double backRightEncoderOffset = -119.619140625;
 };
 }  // namespace drivetrain
-}  // namespace wom
\ No newline at end of file
+}  // namespace wom
diff --git a/wombat/src/main/include/drivetrain/behaviours/SwerveBehaviours.h b/wombat/src/main/include/drivetrain/behaviours/SwerveBehaviours.h
index 0224c28d..d9f844a5 100644
--- a/wombat/src/main/include/drivetrain/behaviours/SwerveBehaviours.h
+++ b/wombat/src/main/include/drivetrain/behaviours/SwerveBehaviours.h
@@ -234,4 +234,4 @@ class DrivebasePoseBehaviour : public behaviour::Behaviour {
 };
 }  // namespace behaviours
 }  // namespace drivetrain
-}  // namespace wom
\ No newline at end of file
+}  // namespace wom
diff --git a/wombat/src/main/include/subsystems/Arm.h b/wombat/src/main/include/subsystems/Arm.h
new file mode 100644
index 00000000..75ec4b00
--- /dev/null
+++ b/wombat/src/main/include/subsystems/Arm.h
@@ -0,0 +1,83 @@
+// Copyright (c) 2023-2024 CurtinFRC
+// Open Source Software, you can modify it according to the terms
+// of the MIT License at the root of this project
+
+#pragma once
+
+#include <frc/DigitalInput.h>
+#include <frc/simulation/DIOSim.h>
+#include <units/current.h>
+#include <units/mass.h>
+#include <units/voltage.h>
+
+#include <memory>
+#include <string>
+
+#include "behaviour/HasBehaviour.h"
+#include "utils/Encoder.h"
+#include "utils/Gearbox.h"
+#include "utils/PID.h"
+
+namespace wom {
+namespace subsystems {
+struct ArmConfig {
+  std::string path;
+
+  wom::utils::Gearbox leftGearbox;
+  wom::utils::Gearbox rightGearbox;
+  wom::utils::CANSparkMaxEncoder* armEncoder;
+  wom::utils::PIDConfig<units::radian, units::volt> pidConfig;
+  wom::utils::PIDConfig<units::radians_per_second, units::volt> velocityConfig;
+
+  units::kilogram_t armMass;
+  units::kilogram_t loadMass;
+  units::meter_t armLength;
+  units::radian_t minAngle = 0_deg;
+  units::radian_t maxAngle = 180_deg;
+  units::radian_t initialAngle = 0_deg;
+  units::radian_t angleOffset = 0_deg;
+
+  void WriteNT(std::shared_ptr<nt::NetworkTable> table);
+};
+
+enum class ArmState { kIdle, kAngle, kRaw, kVelocity };
+
+class Arm : public behaviour::HasBehaviour {
+ public:
+  explicit Arm(ArmConfig config);
+
+  void OnUpdate(units::second_t dt);
+
+  void SetIdle();
+  void SetAngle(units::radian_t angle);
+  void SetRaw(units::volt_t voltage);
+  void SetVelocity(units::radians_per_second_t velocity);
+
+  void SetState(wom::subsystems::ArmState state);
+
+  void SetArmSpeedLimit(double limit);  // units, what are they??
+
+  ArmConfig& GetConfig();
+  ArmState GetState();
+
+  units::radian_t GetAngle() const;
+  units::radians_per_second_t MaxSpeed() const;
+  units::radians_per_second_t GetArmVelocity() const;
+
+  bool IsStable() const;
+
+ private:
+  ArmConfig _config;
+  ArmState _state = ArmState::kIdle;
+  wom::utils::PIDController<units::radian, units::volt> _pid;
+  wom::utils::PIDController<units::radians_per_second, units::volt> _velocityPID;
+
+  std::shared_ptr<nt::NetworkTable> _table;
+
+  double armLimit = 0.4;
+  units::radians_per_second_t lastVelocity;
+
+  units::volt_t _voltage{0};
+};
+}  // namespace subsystems
+}  // namespace wom
diff --git a/wombat/src/main/include/subsystems/Elevator.h b/wombat/src/main/include/subsystems/Elevator.h
new file mode 100644
index 00000000..840470eb
--- /dev/null
+++ b/wombat/src/main/include/subsystems/Elevator.h
@@ -0,0 +1,84 @@
+// Copyright (c) 2023-2024 CurtinFRC
+// Open Source Software, you can modify it according to the terms
+// of the MIT License at the root of this project
+
+#pragma once
+
+#include <frc/DigitalInput.h>
+#include <frc/simulation/DIOSim.h>
+#include <frc/simulation/ElevatorSim.h>
+#include <networktables/NetworkTable.h>
+#include <units/length.h>
+#include <units/mass.h>
+
+#include <memory>
+#include <string>
+
+#include "behaviour/HasBehaviour.h"
+#include "utils/Encoder.h"
+#include "utils/Gearbox.h"
+#include "utils/PID.h"
+
+namespace wom {
+namespace subsystems {
+enum class ElevatorState { kIdle, kPID, kManual, kVelocity };
+
+struct ElevatorConfig {
+  std::string path;
+  wom::utils::Gearbox leftGearbox;
+  wom::utils::Gearbox rightGearbox;
+  wom::utils::CANSparkMaxEncoder elevatorEncoder;
+  frc::DigitalInput* topSensor;
+  frc::DigitalInput* bottomSensor;
+  units::meter_t radius;
+  units::kilogram_t mass;
+  units::meter_t maxHeight;
+  units::meter_t minHeight;
+  units::meter_t initialHeight;
+  wom::utils::PIDConfig<units::meter, units::volt> pid;
+  wom::utils::PIDConfig<units::meters_per_second, units::volt> velocityPID;
+
+  void WriteNT(std::shared_ptr<nt::NetworkTable> table);
+};
+
+class Elevator : public behaviour::HasBehaviour {
+ public:
+  explicit Elevator(ElevatorConfig params);
+
+  void OnUpdate(units::second_t dt);
+
+  void SetManual(units::volt_t voltage);
+  void SetPID(units::meter_t height);
+
+  void SetState(ElevatorState state);
+
+  void SetVelocity(units::meters_per_second_t velocity);
+
+  double GetElevatorEncoderPos();
+  void SetElevatorSpeedLimit(double limit);
+
+  ElevatorConfig& GetConfig();
+
+  bool IsStable() const;
+  ElevatorState GetState() const;
+
+  units::meter_t GetHeight() const;
+  units::meters_per_second_t MaxSpeed() const;
+  units::meters_per_second_t GetElevatorVelocity() const;
+
+ private:
+  units::volt_t _setpointManual{0};
+
+  ElevatorConfig _config;
+  ElevatorState _state;
+  double speedLimit = 0.5;
+
+  units::meters_per_second_t _velocity;
+
+  wom::utils::PIDController<units::meter, units::volt> _pid;
+  wom::utils::PIDController<units::meters_per_second, units::volt> _velocityPID;
+
+  std::shared_ptr<nt::NetworkTable> _table;
+};
+}  // namespace subsystems
+}  // namespace wom
diff --git a/wombat/src/main/include/subsystems/Shooter.h b/wombat/src/main/include/subsystems/Shooter.h
new file mode 100644
index 00000000..e5ac219d
--- /dev/null
+++ b/wombat/src/main/include/subsystems/Shooter.h
@@ -0,0 +1,75 @@
+// Copyright (c) 2023-2024 CurtinFRC
+// Open Source Software, you can modify it according to the terms
+// of the MIT License at the root of this project
+
+#pragma once
+
+#include <networktables/NetworkTable.h>
+#include <units/angular_velocity.h>
+#include <units/charge.h>
+
+#include <memory>
+#include <string>
+
+#include "behaviour/Behaviour.h"
+#include "behaviour/HasBehaviour.h"
+#include "utils/Gearbox.h"
+#include "utils/PID.h"
+
+namespace wom {
+namespace subsystems {
+enum class ShooterState { kPID, kManual, kIdle };
+
+struct ShooterParams {
+  wom::utils::Gearbox gearbox;
+  wom::utils::PIDConfig<units::radians_per_second, units::volt> pid;
+  units::ampere_t currentLimit;
+};
+
+class Shooter : public behaviour::HasBehaviour {
+ public:
+  Shooter(std::string path, ShooterParams params);
+
+  void SetManual(units::volt_t voltage);
+  void SetPID(units::radians_per_second_t goal);
+  void SetState(ShooterState state);
+
+  void OnUpdate(units::second_t dt);
+
+  bool IsStable() const;
+
+ private:
+  ShooterParams _params;
+  ShooterState _state;
+
+  units::volt_t _setpointManual{0};
+
+  wom::utils::PIDController<units::radians_per_second, units::volt> _pid;
+
+  std::shared_ptr<nt::NetworkTable> _table;
+};
+
+class ShooterConstant : public behaviour::Behaviour {
+ public:
+  ShooterConstant(Shooter* s, units::volt_t setpoint);
+
+  void OnTick(units::second_t dt) override;
+
+ private:
+  Shooter* _shooter;
+  units::volt_t _setpoint;
+};
+
+class ShooterSpinup : public behaviour::Behaviour {
+ public:
+  ShooterSpinup(Shooter* s, units::radians_per_second_t speed, bool hold = false);
+
+  void OnTick(units::second_t dt) override;
+
+ private:
+  Shooter* _shooter;
+  units::radians_per_second_t _speed;
+  bool _hold;
+};
+}  // namespace subsystems
+}  // namespace wom
diff --git a/wombat/src/main/include/utils/Encoder.h b/wombat/src/main/include/utils/Encoder.h
index 6b82882c..b9aae2a4 100644
--- a/wombat/src/main/include/utils/Encoder.h
+++ b/wombat/src/main/include/utils/Encoder.h
@@ -125,4 +125,4 @@ class CanEncoder : public Encoder {
   ctre::phoenix6::hardware::CANcoder* _canEncoder;
 };
 }  // namespace utils
-}  // namespace wom
\ No newline at end of file
+}  // namespace wom
diff --git a/wombat/src/main/include/utils/PID.h b/wombat/src/main/include/utils/PID.h
index 63df3666..5f1a3304 100644
--- a/wombat/src/main/include/utils/PID.h
+++ b/wombat/src/main/include/utils/PID.h
@@ -183,4 +183,4 @@ class PIDController {
   std::shared_ptr<nt::NetworkTable> _table;
 };
 }  // namespace utils
-}  // namespace wom
\ No newline at end of file
+}  // namespace wom
diff --git a/wombat/src/main/include/vision/Camera.h b/wombat/src/main/include/vision/Camera.h
new file mode 100644
index 00000000..5bcf4424
--- /dev/null
+++ b/wombat/src/main/include/vision/Camera.h
@@ -0,0 +1,61 @@
+// Copyright (c) 2023-2024 CurtinFRC
+// Open Source Software, you can modify it according to the terms
+// of the MIT License at the root of this project
+
+#pragma once
+
+#include <photonlib/PhotonCamera.h>
+
+#include <string>
+#include <vector>
+
+#include "frc/geometry/Transform2d.h"
+#include "frc/geometry/Transform3d.h"
+#include "photonlib/PhotonPipelineResult.h"
+#include "photonlib/PhotonTrackedTarget.h"
+#include "wpi/SmallVector.h"
+
+enum class PhotonVisionModes { kNormal = 0, kObject = 1 };
+
+using photonTarget = photonlib::PhotonTrackedTarget;
+
+using PhotonVisionLEDMode = photonlib::LEDMode;
+
+class PhotonVision {
+ public:
+  PhotonVision();
+  explicit PhotonVision(std::string name);
+
+  photonlib::PhotonPipelineResult GetResult();
+
+  void SetLED(PhotonVisionLEDMode mode);
+  void SetPipelineIndex(int index);
+
+  bool HasTarget();
+  std::vector<photonTarget> GetTargets();
+  photonTarget GetTarget();
+
+  double GetTargetYaw(photonTarget target);
+  double GetTargetPitch(photonTarget target);
+  double GetTargetArea(photonTarget target);
+  double GetTargetSkew(photonTarget target);
+
+  frc::Transform3d GetCameraToTarget(photonTarget target);
+  std::vector<std::pair<double, double>> GetTargetCorners(photonTarget target);
+
+  int GetTargetId(photonTarget target);
+  frc::Transform3d BestCameraToTarget(photonTarget target);
+  frc::Transform3d AlternateCameraToTarget(photonTarget target);
+
+  PhotonVisionModes GetMode();
+  void SetMode(PhotonVisionModes mode);
+
+ private:
+  void Initialize();
+
+  std::string _name;
+
+  PhotonVisionModes _mode = PhotonVisionModes::kObject;
+
+  photonlib::PhotonCamera* _camera;
+};
diff --git a/wombat/vendordeps/WPILibNewCommands.json b/wombat/vendordeps/WPILibNewCommands.json
new file mode 100644
index 00000000..67bf3898
--- /dev/null
+++ b/wombat/vendordeps/WPILibNewCommands.json
@@ -0,0 +1,38 @@
+{
+  "fileName": "WPILibNewCommands.json",
+  "name": "WPILib-New-Commands",
+  "version": "1.0.0",
+  "uuid": "111e20f7-815e-48f8-9dd6-e675ce75b266",
+  "frcYear": "2024",
+  "mavenUrls": [],
+  "jsonUrl": "",
+  "javaDependencies": [
+    {
+      "groupId": "edu.wpi.first.wpilibNewCommands",
+      "artifactId": "wpilibNewCommands-java",
+      "version": "wpilib"
+    }
+  ],
+  "jniDependencies": [],
+  "cppDependencies": [
+    {
+      "groupId": "edu.wpi.first.wpilibNewCommands",
+      "artifactId": "wpilibNewCommands-cpp",
+      "version": "wpilib",
+      "libName": "wpilibNewCommands",
+      "headerClassifier": "headers",
+      "sourcesClassifier": "sources",
+      "sharedLibrary": true,
+      "skipInvalidPlatforms": true,
+      "binaryPlatforms": [
+        "linuxathena",
+        "linuxarm32",
+        "linuxarm64",
+        "windowsx86-64",
+        "windowsx86",
+        "linuxx86-64",
+        "osxuniversal"
+      ]
+    }
+  ]
+}
diff --git a/wombat/vendordeps/photonlib.json b/wombat/vendordeps/photonlib.json
new file mode 100644
index 00000000..6a4780c7
--- /dev/null
+++ b/wombat/vendordeps/photonlib.json
@@ -0,0 +1,42 @@
+{
+    "fileName": "photonlib.json",
+    "name": "photonlib",
+    "version": "v2024.1.1-beta-3.2",
+    "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004",
+    "frcYear": "2024",
+    "mavenUrls": [
+        "https://maven.photonvision.org/repository/internal",
+        "https://maven.photonvision.org/repository/snapshots"
+    ],
+    "jsonUrl": "https://maven.photonvision.org/repository/internal/org/photonvision/PhotonLib-json/1.0/PhotonLib-json-1.0.json",
+    "jniDependencies": [],
+    "cppDependencies": [
+        {
+            "groupId": "org.photonvision",
+            "artifactId": "PhotonLib-cpp",
+            "version": "v2024.1.1-beta-3.2",
+            "libName": "Photon",
+            "headerClassifier": "headers",
+            "sharedLibrary": true,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "linuxathena",
+                "linuxx86-64",
+                "osxuniversal"
+            ]
+        }
+    ],
+    "javaDependencies": [
+        {
+            "groupId": "org.photonvision",
+            "artifactId": "PhotonLib-java",
+            "version": "v2024.1.1-beta-3.2"
+        },
+        {
+            "groupId": "org.photonvision",
+            "artifactId": "PhotonTargeting-java",
+            "version": "v2024.1.1-beta-3.2"
+        }
+    ]
+}

From 31b0ae949c2fa45bd9d0f6ef151833c2ab325044 Mon Sep 17 00:00:00 2001
From: = <paul.hodges@student.education.wa.edu.au>
Date: Wed, 14 Feb 2024 20:04:52 +0800
Subject: [PATCH 158/207] wpiformat

---
 src/main/cpp/Robot.cpp                  |  2 +-
 wombat/src/main/cpp/vision/Camera.cpp   | 11 ++++++++---
 wombat/src/main/include/vision/Camera.h |  6 ++++++
 3 files changed, 15 insertions(+), 4 deletions(-)

diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp
index f55a0ea0..44ea1b2f 100644
--- a/src/main/cpp/Robot.cpp
+++ b/src/main/cpp/Robot.cpp
@@ -72,7 +72,7 @@ void Robot::RobotInit() {
 
   _arm->SetDefaultBehaviour(
       [this]() { return wom::make<ArmManualControl>(_arm, &robotmap.controllers.codriver); });
-  //_swerveDrive->SetDefaultBehaviour(
+  // _swerveDrive->SetDefaultBehaviour(
   //    [this]() { return wom::make<wom::ManualDrivebase>(_swerveDrive, &robotmap.controllers.driver); });
 
   // alphaArm = new AlphaArm(robotmap.alphaArmSystem.config);
diff --git a/wombat/src/main/cpp/vision/Camera.cpp b/wombat/src/main/cpp/vision/Camera.cpp
index a224a35b..4d6dc5c6 100644
--- a/wombat/src/main/cpp/vision/Camera.cpp
+++ b/wombat/src/main/cpp/vision/Camera.cpp
@@ -9,13 +9,15 @@
 
 #include "frc/geometry/Transform3d.h"
 
+namespace wom {
+namespace vision {
 PhotonVision::PhotonVision() : _name("photonvision") {
   Initialize();
-};
+}
 
 PhotonVision::PhotonVision(std::string name) : _name(name) {
   Initialize();
-};
+}
 
 void PhotonVision::Initialize() {
   _camera = new photonlib::PhotonCamera(_name);
@@ -46,7 +48,7 @@ std::vector<photonTarget> PhotonVision::GetTargets() {
 
   for (size_t i = 0; i < res.size(); i++) {
     targets.push_back(res[i]);
-  };
+  }
 
   return targets;
 }
@@ -100,3 +102,6 @@ void PhotonVision::SetMode(PhotonVisionModes mode) {
 
   SetPipelineIndex(static_cast<int>(mode));
 }
+
+}  // namespace vision
+}  // namespace wom
diff --git a/wombat/src/main/include/vision/Camera.h b/wombat/src/main/include/vision/Camera.h
index 5bcf4424..80348dd6 100644
--- a/wombat/src/main/include/vision/Camera.h
+++ b/wombat/src/main/include/vision/Camera.h
@@ -7,6 +7,7 @@
 #include <photonlib/PhotonCamera.h>
 
 #include <string>
+#include <utility>
 #include <vector>
 
 #include "frc/geometry/Transform2d.h"
@@ -15,6 +16,9 @@
 #include "photonlib/PhotonTrackedTarget.h"
 #include "wpi/SmallVector.h"
 
+namespace wom {
+namespace vision {
+
 enum class PhotonVisionModes { kNormal = 0, kObject = 1 };
 
 using photonTarget = photonlib::PhotonTrackedTarget;
@@ -59,3 +63,5 @@ class PhotonVision {
 
   photonlib::PhotonCamera* _camera;
 };
+}  // namespace vision
+}  // namespace wom

From 7b5e9b489c17633c5dacf3691515b65f0150ca19 Mon Sep 17 00:00:00 2001
From: Kingsley Wong <kingsley.wong@student.education.wa.edu.au>
Date: Thu, 15 Feb 2024 20:06:40 +0800
Subject: [PATCH 159/207] Fixed most things 3

---
 src/main/cpp/Intake.cpp            | 60 ++++++++++++++++-------------
 src/main/cpp/IntakeBehaviour.cpp   | 61 +++++++++++++-----------------
 src/main/include/Intake.h          |  8 ++--
 src/main/include/IntakeBehaviour.h | 12 ++++--
 4 files changed, 71 insertions(+), 70 deletions(-)

diff --git a/src/main/cpp/Intake.cpp b/src/main/cpp/Intake.cpp
index b2194530..66ae7590 100644
--- a/src/main/cpp/Intake.cpp
+++ b/src/main/cpp/Intake.cpp
@@ -50,12 +50,25 @@ void Intake::OnUpdate(units::second_t dt) {
     case IntakeState::kHold: 
     {
       _stringStateName = "Hold";
-      //& add a check for if sensor is empty 
+
+      _setVoltage = 0_V;
 
       if (_config.intakeSensor->Get() == true) {
-        _setVoltage = 0_V;
+        SetState(IntakeState::kIdle);
+      }
+      
+      _pid.SetSetpoint(_goal);
+      units::volt_t pidCalculate =
+          units::volt_t{_pid.Calculate(_config.IntakeGearbox.encoder->GetEncoderAngularVelocity(), dt, 0_V)};
+      if (_pid.IsStable()) {
+        holdVoltage = pidCalculate;
+        std::cout << "STABLE" << std::endl;
+      }
+      if (holdVoltage.value() == 0) {
+        _setVoltage = pidCalculate;
+      } else {
+        _setVoltage = holdVoltage;
       }
-
     } 
     break;
 
@@ -64,6 +77,7 @@ void Intake::OnUpdate(units::second_t dt) {
       _stringStateName = "Intake";
       _setVoltage = -7_V; 
       if (_config.intakeSensor->Get() == false) {
+
         SetState(IntakeState::kHold);
       }
     } 
@@ -80,50 +94,42 @@ void Intake::OnUpdate(units::second_t dt) {
       units::volt_t pidCalculate =
           units::volt_t{_pid.Calculate(_config.IntakeGearbox.encoder->GetEncoderAngularVelocity(), dt, 0_V)};
       if (_pid.IsStable()) {
-        holdVoltage = pidCalculate;
+        passVoltage = pidCalculate;
         std::cout << "STABLE" << std::endl;
       }
 
-      if (holdVoltage.value() == 0) {
+      if (passVoltage.value() == 0) {
         _setVoltage = pidCalculate;
       } else {
-        _setVoltage = holdVoltage;
+        _setVoltage = passVoltage;
       }
-    } //&, logic is a bit ugly, make it nicer
+    }
     break;
 
     case IntakeState::kPID:
     {
-    _pid.SetSetpoint(_goal);
-      units::volt_t pidCalculate =
-          units::volt_t{_pid.Calculate(_config.IntakeGearbox.encoder->GetEncoderAngularVelocity(), dt, 0_V)};
-    if (_pid.IsStable()) {
-        holdVoltage = pidCalculate;
-        std::cout << "STABLE" << std::endl;
+      _pid.SetSetpoint(_goal);
+        units::volt_t pidCalculate =
+            units::volt_t{_pid.Calculate(_config.IntakeGearbox.encoder->GetEncoderAngularVelocity(), dt, 0_V)};
+      if (_pid.IsStable()) {
+          holdVoltage = pidCalculate;
+          std::cout << "STABLE" << std::endl;
+        }
+      if (holdVoltage.value() == 0) {
+        _setVoltage = pidCalculate;
+      } else {
+        _setVoltage = holdVoltage;
       }
-    if (holdVoltage.value() == 0) {
-      _setVoltage = pidCalculate;
-    } else {
-      _setVoltage = holdVoltage;
     }
-    }
-
-
-    default:
-      std::cout << "Error: Intake in INVALID STATE." << std::endl;
-      break;
   }
   _table->GetEntry("State: ").SetString(_stringStateName);
   _table->GetEntry("Motor Voltage: ").SetDouble(_setVoltage.value());
   _table->GetEntry("Intake Sensor: ").SetBoolean(_config.intakeSensor->Get());
   _table->GetEntry("Error").SetDouble(_pid.GetError().value());
   _table->GetEntry("SetPoint").SetDouble(_pid.GetSetpoint().value());
-  // _table->GetEntry("Shooter Sensor: ").SetBoolean(_config.shooterSensor->Get());
-  // _table->GetEntry("Intake Sensor: ").SetBoolean(_config.intakeSensor->Get());
-  // _table->GetEntry("Shooter Sensor: ").SetBoolean(_config.shooterSensor->Get());
-  // _table->GetEntry("Magazine Sensor: ").SetBoolean(_config.magSensor->Get());
  
   _config.IntakeGearbox.motorController->SetVoltage(_setVoltage);
+
 }
 
 void Intake::SetState(IntakeState state) {
diff --git a/src/main/cpp/IntakeBehaviour.cpp b/src/main/cpp/IntakeBehaviour.cpp
index 9e7d42cc..cbe8f591 100644
--- a/src/main/cpp/IntakeBehaviour.cpp
+++ b/src/main/cpp/IntakeBehaviour.cpp
@@ -6,8 +6,7 @@
 
 #include <frc/XboxController.h>
 
-IntakeManualControl::IntakeManualControl(Intake* intake, frc::XboxController& codriver)
-    : _intake(intake), _codriver(codriver) {
+IntakeManualControl::IntakeManualControl(Intake* intake, frc::XboxController& codriver) : _intake(intake), _codriver(codriver) {
   Controls(intake);
 }
 
@@ -27,12 +26,15 @@ void IntakeManualControl::OnTick(units::second_t dt) {
 
 
   if (_codriver.GetBButtonReleased()) {
-    if (_intake->GetState() == IntakeState::kRaw) {
-      _intake->SetState(IntakeState::kIdle);
+
+    if (_rawControl) {
+      _rawControl = false;
     } else {
-      _intake->SetState(IntakeState::kIdle); // this is not a toggle button
+      _rawControl = true;
     }
-  } else if (_rawControl) {
+  } 
+  
+  if (_rawControl) {
     if (_codriver.GetRightTriggerAxis() > 0.1) {
       _intake->SetRaw(_codriver.GetRightTriggerAxis() * 10_V);
     } else if (_codriver.GetLeftTriggerAxis() > 0.1) {
@@ -41,40 +43,29 @@ void IntakeManualControl::OnTick(units::second_t dt) {
       _intake->SetRaw(0_V);
     }
     _intake->SetState(IntakeState::kRaw);
-    
+
   } else {
-    if (_codriver.GetRightTriggerAxis() > 0.1) {
-        _intake->SetState(IntakeState::kIntake);
-    } else if (_codriver.GetLeftTriggerAxis() > 0.1) {
+    
+    if (_intake->GetState() == IntakeState::kHold) {
+      if (_codriver.GetAButtonReleased()) {
+        _intake->SetState(IntakeState::kEject);
+      } else if (_codriver.GetYButtonReleased()) {
+        _intake->SetState(IntakeState::kPass);
+      } else {
+        _intake->SetState(IntakeState::kIdle);
+      }
+    } else if (_intake->GetState() == IntakeState::kIdle) {
+      if (_codriver.GetRightTriggerAxis() > 0.1) {
         _intake->SetState(IntakeState::kIntake);
-    } else if (_codriver.GetAButtonPressed()) {
-      _intake->SetState(IntakeState::kPass);
-    } else {
-    _intake->SetState(IntakeState::kIdle);  
-  }
-
-      // if (_intaking) {
-    //   if (_intake->GetState() == IntakeState::kIdle) {
-    //     _intake->SetState(IntakeState::kIntake);
-    //   }
-    // }
-
-    // if (_passing) {
-    //   if (_intake->GetState() == IntakeState::kHold) {
-    //     _intake->SetState(IntakeState::kPass);
-    //   }
-    // }
-
-    // if (_ejecting) {
-    //   if (_intake->GetState() == IntakeState::kIdle || _intake->GetState() == IntakeState::kHold) {
-    //     _intake->SetState(IntakeState::kEject);
-    //   }
-    // }
+      } else {
+        _intake->SetState(IntakeState::kIdle);
+      }
+    }
   }
 }
 
-IntakeAutoControl::IntakeAutoControl(Intake* intake) : _intake(intake) {
+AutoIntake::AutoIntake(Intake* intake) : _intake(intake) {
   Controls(intake);
 }
 
-void IntakeAutoControl::OnTick(units::second_t dt) {}
+void AutoIntake::OnTick(units::second_t dt) {}
diff --git a/src/main/include/Intake.h b/src/main/include/Intake.h
index d01c5ccb..a5d13f53 100644
--- a/src/main/include/Intake.h
+++ b/src/main/include/Intake.h
@@ -17,8 +17,7 @@ struct IntakeConfig {
   std::string path;
   wom::Gearbox IntakeGearbox;
   frc::DigitalInput* intakeSensor;
-  // frc::DigitalInput* magSensor;
-  // frc::DigitalInput* shooterSensor;
+
   wom::PIDConfig<units::radians_per_second, units::volt> pidConfig;
 };
 
@@ -45,9 +44,8 @@ class Intake : public behaviour::HasBehaviour {
   std::string _stringStateName = "error";
   units::volt_t _setVoltage = 0_V;
   units::volt_t holdVoltage = 0_V;
-  // bool _intaking; //&NEEDS DEFAULTS
-  // bool _ejecting;
-  // bool _passing;
+  units::volt_t passVoltage = 0_V;
+
   units::radians_per_second_t _goal;
   wom::PIDController<units::radians_per_second, units::volt> _pid;
   
diff --git a/src/main/include/IntakeBehaviour.h b/src/main/include/IntakeBehaviour.h
index 931273fa..bd5a8f85 100644
--- a/src/main/include/IntakeBehaviour.h
+++ b/src/main/include/IntakeBehaviour.h
@@ -9,16 +9,22 @@
 #include "Intake.h"
 #include "Wombat.h"
 
+enum class IntakeBehaviourState { kIntaking, kPassing, kEjecting, kIdleing};
+
 class IntakeManualControl : public behaviour::Behaviour {
  public:
   explicit IntakeManualControl(Intake* intake, frc::XboxController& codriver);
 
+  void SetBehaviourState(IntakeBehaviourState behaviourState);
   void OnTick(units::second_t dt) override;
+  IntakeBehaviourState GetBehaviourState();
 
  private:
   Intake* _intake;
   frc::XboxController& _codriver;
 
+  IntakeBehaviourState _behaviourState = IntakeBehaviourState::kIdleing;
+
   units::volt_t _rawVoltage;
   units::volt_t _setVoltage;
   bool _rawControl = true;
@@ -27,12 +33,12 @@ class IntakeManualControl : public behaviour::Behaviour {
   bool _passing = false;
 };
 
-class IntakeAutoControl : public behaviour::Behaviour {
+class AutoIntake : public behaviour::Behaviour {
  public:
-  explicit IntakeAutoControl(Intake* intake);
-
+  explicit AutoIntake(Intake* intake);
   void OnTick(units::second_t dt) override;
 
  private:
   Intake* _intake;
+  IntakeBehaviourState _behaviourState = IntakeBehaviourState::kIdleing;
 };

From e81a9907c865a87744afd27de61ae4b1fc4c6633 Mon Sep 17 00:00:00 2001
From: = <paul.hodges@student.education.wa.edu.au>
Date: Fri, 16 Feb 2024 09:29:12 +0800
Subject: [PATCH 160/207] did more stuff

---
 src/main/cpp/Robot.cpp                     |  54 +++++-----
 src/main/cpp/vision/Vision.cpp             |   6 +-
 src/main/cpp/vision/VisionBehaviours.cpp   |  19 ++++
 src/main/include/RobotMap.h                | 110 ++++++++++-----------
 src/main/include/vision/Vision.h           |   1 +
 src/main/include/vision/VisionBehaviours.h |  23 +++++
 wombat/src/main/cpp/vision/Camera.cpp      |  28 +++---
 wombat/src/main/cpp/vision/Limelight.cpp   |  19 ++++
 wombat/src/main/include/vision/Camera.h    |  29 +++---
 wombat/src/main/include/vision/Limelight.h |   2 +
 10 files changed, 182 insertions(+), 109 deletions(-)
 create mode 100644 src/main/cpp/vision/VisionBehaviours.cpp
 create mode 100644 src/main/include/vision/VisionBehaviours.h

diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp
index 44ea1b2f..bab64f5c 100644
--- a/src/main/cpp/Robot.cpp
+++ b/src/main/cpp/Robot.cpp
@@ -31,7 +31,6 @@
 #include <frc/Timer.h>
 #include "ArmBehaviours.h"
 #include "behaviour/HasBehaviour.h"
-#include "drivetrain/behaviours/SwerveBehaviours.h"
 #include "frc/geometry/Pose2d.h"
 #include "vision/Vision.h"
 
@@ -67,13 +66,13 @@ void Robot::RobotInit() {
   _swerveDrive = new wom::SwerveDrive(robotmap.swerveBase.config, frc::Pose2d());
   wom::BehaviourScheduler::GetInstance()->Register(_swerveDrive);
 
-  _arm = new wom::Arm(robotmap.arm.config);
-  wom::BehaviourScheduler::GetInstance()->Register(_arm);
-
-  _arm->SetDefaultBehaviour(
-      [this]() { return wom::make<ArmManualControl>(_arm, &robotmap.controllers.codriver); });
-  // _swerveDrive->SetDefaultBehaviour(
-  //    [this]() { return wom::make<wom::ManualDrivebase>(_swerveDrive, &robotmap.controllers.driver); });
+  // _arm = new wom::Arm(robotmap.arm.config);
+  // wom::BehaviourScheduler::GetInstance()->Register(_arm);
+  //
+  // _arm->SetDefaultBehaviour(
+  //     [this]() { return wom::make<ArmManualControl>(_arm, &robotmap.controllers.codriver); });
+  _swerveDrive->SetDefaultBehaviour(
+      [this]() { return wom::make<wom::ManualDrivebase>(_swerveDrive, &robotmap.controllers.driver); });
 
   // alphaArm = new AlphaArm(robotmap.alphaArmSystem.config);
   // wom::BehaviourScheduler::GetInstance()->Register(alphaArm);
@@ -131,12 +130,12 @@ void Robot::RobotPeriodic() {
       .SetDouble(robotmap.swerveBase.moduleConfigs[2].turnMotor.encoder->GetEncoderPosition().value());
   robotmap.swerveTable.swerveDriveTable->GetEntry("Encoder 3 offset: ")
       .SetDouble(robotmap.swerveBase.moduleConfigs[3].turnMotor.encoder->GetEncoderPosition().value());
-
-  // shooter->OnUpdate(dt);
-  // intake->OnUpdate(dt);
-  // alphaArm->OnUpdate(dt);
+  //
+  // // shooter->OnUpdate(dt);
+  // // intake->OnUpdate(dt);
+  // // alphaArm->OnUpdate(dt);
   _swerveDrive->OnUpdate(dt);
-  _arm->OnUpdate(dt);
+  // _arm->OnUpdate(dt);
 }
 
 void Robot::AutonomousInit() {
@@ -160,20 +159,25 @@ void Robot::TeleopInit() {
   // frontRight->SetVoltage(4_V);
   // backLeft->SetVoltage(4_V);
   // backRight->SetVoltage(4_V);
+  //
 }
 void Robot::TeleopPeriodic() {
-  if (robotmap.controllers.driver.GetXButtonPressed() &&
-      vision->TargetIsVisible(VisionTargetObjects::kNote)) {
-    units::degree_t turn = vision->LockOn(VisionTargetObjects::kNote);
-
-    frc::Pose2d current_pose = _swerveDrive->GetPose();
-
-    std::cout << "angle: " << turn.value() << std::endl;
-    current_pose.RotateBy(turn);
-
-    wom::make<wom::DrivebasePoseBehaviour>(_swerveDrive, current_pose);
-  } else {
-    wom::make<wom::ManualDrivebase>(_swerveDrive, &robotmap.controllers.driver);
+  // if (robotmap.controllers.driver.GetXButtonPressed() &&
+  //     vision->TargetIsVisible(VisionTargetObjects::kNote)) {
+  //   units::degree_t turn = vision->LockOn(VisionTargetObjects::kNote);
+  //
+  //   frc::Pose2d current_pose = _swerveDrive->GetPose();
+  //
+  //   std::cout << "angle: " << turn.value() << std::endl;
+  //   current_pose.RotateBy(turn);
+  //
+  //   wom::make<wom::DrivebasePoseBehaviour>(_swerveDrive, current_pose);
+  // } else {
+  //   wom::make<wom::ManualDrivebase>(_swerveDrive, &robotmap.controllers.driver);
+  // }
+
+  if (robotmap.controllers.driver.GetXButtonPressed()) {
+    vision->TurnToTarget(VisionTarget::kBlueSpeakerCenter, _swerveDrive);
   }
 }
 
diff --git a/src/main/cpp/vision/Vision.cpp b/src/main/cpp/vision/Vision.cpp
index ad231926..b8e041c7 100644
--- a/src/main/cpp/vision/Vision.cpp
+++ b/src/main/cpp/vision/Vision.cpp
@@ -337,7 +337,7 @@ frc::Pose2d Vision::TurnToTarget(VisionTarget target, wom::SwerveDrive* swerveDr
 
   std::cout << pose.Rotation().Degrees().value() << std::endl;
 
-  // swerveDrive->SetPose(pose);
+  swerveDrive->SetPose(pose);
 }
 
 std::pair<frc::Pose2d, units::degree_t> Vision::GetAngleToObject(VisionTargetObjects object) {
@@ -374,6 +374,10 @@ bool Vision::IsAtPose(frc::Pose3d pose, units::second_t dt) {
   return _limelight->IsAtSetPoseVision(pose, dt);
 }
 
+bool Vision::IsAtPose(frc::Pose2d pose, units::second_t dt) {
+  return _limelight->IsAtSetPoseVision(pose, dt);
+}
+
 void Vision::SetMode(VisionModes mode) {
   if (static_cast<int>(mode) == _limelight->GetTargetingData(wom::LimelightTargetingData::kGetpipe)) {
     return;
diff --git a/src/main/cpp/vision/VisionBehaviours.cpp b/src/main/cpp/vision/VisionBehaviours.cpp
new file mode 100644
index 00000000..508b0dd2
--- /dev/null
+++ b/src/main/cpp/vision/VisionBehaviours.cpp
@@ -0,0 +1,19 @@
+// Copyright (c) 2023-2024 CurtinFRC
+// Open Source Software, you can modify it according to the terms
+// of the MIT License at the root of this project
+
+#include "vision/VisionBehaviours.h"
+
+#include "frc/geometry/Pose2d.h"
+#include "units/angle.h"
+
+AlignToAprilTag::AlignToAprilTag(Vision* vision, VisionTarget target, wom::SwerveDrive* swerveDrive)
+    : _vision(vision), _target(target), _swerveDrive(swerveDrive) {}
+
+void AlignToAprilTag::OnTick(units::second_t dt) {
+  frc::Pose2d pose = _vision->TurnToTarget(_target, _swerveDrive);
+
+  if (_vision->IsAtPose(pose, dt)) {
+    SetDone();
+  }
+}
diff --git a/src/main/include/RobotMap.h b/src/main/include/RobotMap.h
index ae06f78d..f2939b3a 100644
--- a/src/main/include/RobotMap.h
+++ b/src/main/include/RobotMap.h
@@ -95,61 +95,61 @@ struct RobotMap {
 
   //   };
   //   Shooter shooterSystem;
-
-  struct Arm {
-    // creates the motor used for the arm as well as the port it is plugged in
-    rev::CANSparkMax leftArmMotor{21, rev::CANSparkMax::MotorType::kBrushless};   // 11
-    rev::CANSparkMax rightArmMotor{26, rev::CANSparkMax::MotorType::kBrushless};  // 12
-
-    // rev::CANSparkMax leftPretendArmMotor{28, rev::CANSparkMax::MotorType::kBrushless};
-    // rev::CANSparkMax rightPretendArmMotor{29, rev::CANSparkMax::MotorType::kBrushless};
-
-    // wom::DigitalEncoder encoder{0, 1, 2048};
-    // sets the type sof encoder that is used up
-    wom::CANSparkMaxEncoder* leftEncoder = new wom::CANSparkMaxEncoder(&leftArmMotor, 10_cm);
-    wom::CANSparkMaxEncoder* rightEncoder = new wom::CANSparkMaxEncoder(&rightArmMotor, 10_cm);
-
-    // wom::DutyCycleEncoder *encoder = new wom::DutyCycleEncoder(3, 10_cm);
-
-    // wom::CANSparkMaxEncoder leftEncoder{&leftArmMotor, 100};
-    // wom::CANSparkMaxEncoder rightEncoder{&rightArmMotor, 100};
-
-    // rev::SparkMaxAbsoluteEncoder leftOtherArmEncoder{leftArmMotor,
-    // rev::CANSparkMax::SparkMaxAbsoluteEncoder::Type::kDutyCycle}; wom::CANSparkMaxEncoder
-    // leftOtherArmEncoder = leftArmMotor.GetEncoder(); wom::CANSparkMaxEncoder rightOtherArmEncoder =
-    // rightArmMotor.GetEncoder();
-
-    // creates an instance of the arm gearbox
-    wom::Gearbox leftGearbox{&leftArmMotor, leftEncoder,
-                             // nullptr,
-                             frc::DCMotor::NEO(1).WithReduction(100)};
-
-    wom::Gearbox rightGearbox{&rightArmMotor, rightEncoder,
-                              // nullptr,
-                              frc::DCMotor::NEO(1).WithReduction(100)};
-
-    // creates arm config information
-    wom::ArmConfig config{
-        "/armavator/arm", leftGearbox, rightGearbox,
-        // nullptr,
-        leftEncoder,
-        wom::PIDConfig<units::radian, units::volts>("/armavator/arm/pid/config",
-                                                    18_V / 25_deg,        // prev 13_V/25_deg
-                                                    0_V / (1_deg * 1_s),  // 0.1_V / (1_deg * 1_s)
-                                                    0_V / (1_deg / 1_s),  // 0_V / (1_deg / 1_s)
-                                                    5_deg, 2_deg / 1_s, 10_deg),
-        wom::PIDConfig<units::radians_per_second, units::volts>("/armavator/arm/velocityPID/config",
-                                                                9_V / (180_deg / 1_s), 0_V / 25_deg,
-                                                                0_V / (90_deg / 1_s / 1_s)),
-        1_kg, 0.5_kg, 1.15_m, -90_deg, 270_deg, 0_deg};
-
-    Arm() {
-      // inverts the motor so that it goes in the right direction while using RAW controlls
-      leftArmMotor.SetInverted(true);
-      rightArmMotor.SetInverted(false);
-    }
-  };
-  Arm arm;
+  //
+  // struct Arm {
+  //   // creates the motor used for the arm as well as the port it is plugged in
+  //   rev::CANSparkMax leftArmMotor{21, rev::CANSparkMax::MotorType::kBrushless};   // 11
+  //   rev::CANSparkMax rightArmMotor{26, rev::CANSparkMax::MotorType::kBrushless};  // 12
+  //
+  //   // rev::CANSparkMax leftPretendArmMotor{28, rev::CANSparkMax::MotorType::kBrushless};
+  //   // rev::CANSparkMax rightPretendArmMotor{29, rev::CANSparkMax::MotorType::kBrushless};
+  //
+  //   // wom::DigitalEncoder encoder{0, 1, 2048};
+  //   // sets the type sof encoder that is used up
+  //   wom::CANSparkMaxEncoder* leftEncoder = new wom::CANSparkMaxEncoder(&leftArmMotor, 10_cm);
+  //   wom::CANSparkMaxEncoder* rightEncoder = new wom::CANSparkMaxEncoder(&rightArmMotor, 10_cm);
+  //
+  //   // wom::DutyCycleEncoder *encoder = new wom::DutyCycleEncoder(3, 10_cm);
+  //
+  //   // wom::CANSparkMaxEncoder leftEncoder{&leftArmMotor, 100};
+  //   // wom::CANSparkMaxEncoder rightEncoder{&rightArmMotor, 100};
+  //
+  //   // rev::SparkMaxAbsoluteEncoder leftOtherArmEncoder{leftArmMotor,
+  //   // rev::CANSparkMax::SparkMaxAbsoluteEncoder::Type::kDutyCycle}; wom::CANSparkMaxEncoder
+  //   // leftOtherArmEncoder = leftArmMotor.GetEncoder(); wom::CANSparkMaxEncoder rightOtherArmEncoder =
+  //   // rightArmMotor.GetEncoder();
+  //
+  //   // creates an instance of the arm gearbox
+  //   wom::Gearbox leftGearbox{&leftArmMotor, leftEncoder,
+  //                            // nullptr,
+  //                            frc::DCMotor::NEO(1).WithReduction(100)};
+  //
+  //   wom::Gearbox rightGearbox{&rightArmMotor, rightEncoder,
+  //                             // nullptr,
+  //                             frc::DCMotor::NEO(1).WithReduction(100)};
+  //
+  //   // creates arm config information
+  //   wom::ArmConfig config{
+  //       "/armavator/arm", leftGearbox, rightGearbox,
+  //       // nullptr,
+  //       leftEncoder,
+  //       wom::PIDConfig<units::radian, units::volts>("/armavator/arm/pid/config",
+  //                                                   18_V / 25_deg,        // prev 13_V/25_deg
+  //                                                   0_V / (1_deg * 1_s),  // 0.1_V / (1_deg * 1_s)
+  //                                                   0_V / (1_deg / 1_s),  // 0_V / (1_deg / 1_s)
+  //                                                   5_deg, 2_deg / 1_s, 10_deg),
+  //       wom::PIDConfig<units::radians_per_second, units::volts>("/armavator/arm/velocityPID/config",
+  //                                                               9_V / (180_deg / 1_s), 0_V / 25_deg,
+  //                                                               0_V / (90_deg / 1_s / 1_s)),
+  //       1_kg, 0.5_kg, 1.15_m, -90_deg, 270_deg, 0_deg};
+  //
+  //   Arm() {
+  //     // inverts the motor so that it goes in the right direction while using RAW controlls
+  //     leftArmMotor.SetInverted(true);
+  //     rightArmMotor.SetInverted(false);
+  //   }
+  // };
+  // Arm arm;
 
   struct SwerveBase {
     ctre::phoenix6::hardware::CANcoder frontLeftCancoder{16, "Drivebase"};
diff --git a/src/main/include/vision/Vision.h b/src/main/include/vision/Vision.h
index 1cb234e1..420dcaa4 100644
--- a/src/main/include/vision/Vision.h
+++ b/src/main/include/vision/Vision.h
@@ -101,6 +101,7 @@ class Vision {
   std::vector<AprilTag> GetTags();
 
   bool IsAtPose(frc::Pose3d pose, units::second_t dt);
+  bool IsAtPose(frc::Pose2d pose, units::second_t dt);
 
   bool TargetIsVisible(VisionTargetObjects target);
 
diff --git a/src/main/include/vision/VisionBehaviours.h b/src/main/include/vision/VisionBehaviours.h
new file mode 100644
index 00000000..eacc9a25
--- /dev/null
+++ b/src/main/include/vision/VisionBehaviours.h
@@ -0,0 +1,23 @@
+// Copyright (c) 2023-2024 CurtinFRC
+// Open Source Software, you can modify it according to the terms
+// of the MIT License at the root of this project
+
+#pragma once
+
+#include "Wombat.h"
+#include "behaviour/HasBehaviour.h"
+#include "drivetrain/SwerveDrive.h"
+#include "units/time.h"
+#include "vision/Vision.h"
+
+class AlignToAprilTag : public behaviour::Behaviour {
+ public:
+  explicit AlignToAprilTag(Vision* vision, VisionTarget target, wom::SwerveDrive* swerveDrive);
+
+  void OnTick(units::second_t dt);
+
+ private:
+  Vision* _vision;
+  wom::SwerveDrive* _swerveDrive;
+  VisionTarget _target;
+};
diff --git a/wombat/src/main/cpp/vision/Camera.cpp b/wombat/src/main/cpp/vision/Camera.cpp
index 4d6dc5c6..da257f8e 100644
--- a/wombat/src/main/cpp/vision/Camera.cpp
+++ b/wombat/src/main/cpp/vision/Camera.cpp
@@ -22,7 +22,7 @@ PhotonVision::PhotonVision(std::string name) : _name(name) {
 void PhotonVision::Initialize() {
   _camera = new photonlib::PhotonCamera(_name);
 
-  SetMode(PhotonVisionModes::kObject);
+  SetMode(_mode);
 }
 
 photonlib::PhotonPipelineResult PhotonVision::GetResult() {
@@ -41,10 +41,10 @@ bool PhotonVision::HasTarget() {
   return GetResult().HasTargets();
 }
 
-std::vector<photonTarget> PhotonVision::GetTargets() {
-  const std::span<const photonTarget> res = GetResult().GetTargets();
+std::vector<PhotonTarget> PhotonVision::GetTargets() {
+  const std::span<const PhotonTarget> res = GetResult().GetTargets();
 
-  std::vector<photonTarget> targets;
+  std::vector<PhotonTarget> targets;
 
   for (size_t i = 0; i < res.size(); i++) {
     targets.push_back(res[i]);
@@ -53,43 +53,43 @@ std::vector<photonTarget> PhotonVision::GetTargets() {
   return targets;
 }
 
-photonTarget PhotonVision::GetTarget() {
+PhotonTarget PhotonVision::GetTarget() {
   return GetResult().GetBestTarget();
 }
 
-double PhotonVision::GetTargetYaw(photonTarget target) {
+double PhotonVision::GetTargetYaw(PhotonTarget target) {
   return target.GetYaw();
 }
 
-double PhotonVision::GetTargetPitch(photonTarget target) {
+double PhotonVision::GetTargetPitch(PhotonTarget target) {
   return target.GetPitch();
 }
 
-double PhotonVision::GetTargetArea(photonTarget target) {
+double PhotonVision::GetTargetArea(PhotonTarget target) {
   return target.GetArea();
 }
 
-double PhotonVision::GetTargetSkew(photonTarget target) {
+double PhotonVision::GetTargetSkew(PhotonTarget target) {
   return target.GetSkew();
 }
 
-frc::Transform3d PhotonVision::GetCameraToTarget(photonTarget target) {
+frc::Transform3d PhotonVision::GetCameraToTarget(PhotonTarget target) {
   return target.GetBestCameraToTarget();
 }
 
-std::vector<std::pair<double, double>> PhotonVision::GetTargetCorners(photonTarget target) {
+std::vector<std::pair<double, double>> PhotonVision::GetTargetCorners(PhotonTarget target) {
   return target.GetDetectedCorners();
 }
 
-int PhotonVision::GetTargetId(photonTarget target) {
+int PhotonVision::GetTargetId(PhotonTarget target) {
   return target.GetFiducialId();
 }
 
-frc::Transform3d PhotonVision::BestCameraToTarget(photonTarget target) {
+frc::Transform3d PhotonVision::BestCameraToTarget(PhotonTarget target) {
   return target.GetBestCameraToTarget();
 }
 
-frc::Transform3d PhotonVision::AlternateCameraToTarget(photonTarget target) {
+frc::Transform3d PhotonVision::AlternateCameraToTarget(PhotonTarget target) {
   return target.GetAlternateCameraToTarget();
 }
 
diff --git a/wombat/src/main/cpp/vision/Limelight.cpp b/wombat/src/main/cpp/vision/Limelight.cpp
index ed62337e..012a5e57 100644
--- a/wombat/src/main/cpp/vision/Limelight.cpp
+++ b/wombat/src/main/cpp/vision/Limelight.cpp
@@ -167,6 +167,18 @@ units::meters_per_second_t wom::vision::Limelight::GetSpeed(frc::Pose3d pose1, f
   return units::math::fabs(dTRANSLATION / dt);
 }
 
+units::meters_per_second_t wom::vision::Limelight::GetSpeed(frc::Pose2d pose1, frc::Pose2d pose2,
+                                                            units::second_t dt) {
+  frc::Transform2d dPose{pose1, pose2};
+  frc::Translation2d dTranslation = dPose.Translation();
+
+  units::meter_t y = dTranslation.Y();
+  units::meter_t x = dTranslation.X();
+  units::radian_t theta = units::math::atan(y / x);
+  units::meter_t dTRANSLATION = x / units::math::cos(theta);
+  return units::math::fabs(dTRANSLATION / dt);
+}
+
 frc::Pose3d wom::vision::Limelight::GetPose() {
   std::vector<double> pose = GetAprilTagData(LimelightAprilTagData::kBotpose);
   return frc::Pose3d(pose[1] * 1_m, 1_m * pose[2], 1_m * pose[3],
@@ -188,6 +200,13 @@ bool wom::vision::Limelight::IsAtSetPoseVision(frc::Pose3d pose, units::second_t
           GetSpeed(pose, GetPose(), dt) < 1_m / 1_s);
 }
 
+bool wom::vision::Limelight::IsAtSetPoseVision(frc::Pose2d pose, units::second_t dt) {
+  frc::Pose2d actualPose = GetPose().ToPose2d();
+  frc::Pose2d relativePose = actualPose.RelativeTo(pose);
+  return (units::math::fabs(relativePose.X()) < 0.01_m && units::math::fabs(relativePose.Y()) < 0.01_m &&
+          GetSpeed(pose, GetPose().ToPose2d(), dt) < 1_m / 1_s);
+}
+
 bool wom::vision::Limelight::HasTarget() {
   return GetTargetingData(LimelightTargetingData::kTv) == 1.0;
 }
diff --git a/wombat/src/main/include/vision/Camera.h b/wombat/src/main/include/vision/Camera.h
index 80348dd6..83ce620d 100644
--- a/wombat/src/main/include/vision/Camera.h
+++ b/wombat/src/main/include/vision/Camera.h
@@ -14,14 +14,15 @@
 #include "frc/geometry/Transform3d.h"
 #include "photonlib/PhotonPipelineResult.h"
 #include "photonlib/PhotonTrackedTarget.h"
+#include "units/angle.h"
 #include "wpi/SmallVector.h"
 
 namespace wom {
 namespace vision {
 
-enum class PhotonVisionModes { kNormal = 0, kObject = 1 };
+enum class PhotonVisionModes { kNormal = 0, kNotes = 1 };
 
-using photonTarget = photonlib::PhotonTrackedTarget;
+using PhotonTarget = photonlib::PhotonTrackedTarget;
 
 using PhotonVisionLEDMode = photonlib::LEDMode;
 
@@ -36,20 +37,20 @@ class PhotonVision {
   void SetPipelineIndex(int index);
 
   bool HasTarget();
-  std::vector<photonTarget> GetTargets();
-  photonTarget GetTarget();
+  std::vector<PhotonTarget> GetTargets();
+  PhotonTarget GetTarget();
 
-  double GetTargetYaw(photonTarget target);
-  double GetTargetPitch(photonTarget target);
-  double GetTargetArea(photonTarget target);
-  double GetTargetSkew(photonTarget target);
+  double GetTargetYaw(PhotonTarget target);
+  double GetTargetPitch(PhotonTarget target);
+  double GetTargetArea(PhotonTarget target);
+  double GetTargetSkew(PhotonTarget target);
 
-  frc::Transform3d GetCameraToTarget(photonTarget target);
-  std::vector<std::pair<double, double>> GetTargetCorners(photonTarget target);
+  frc::Transform3d GetCameraToTarget(PhotonTarget target);
+  std::vector<std::pair<double, double>> GetTargetCorners(PhotonTarget target);
 
-  int GetTargetId(photonTarget target);
-  frc::Transform3d BestCameraToTarget(photonTarget target);
-  frc::Transform3d AlternateCameraToTarget(photonTarget target);
+  int GetTargetId(PhotonTarget target);
+  frc::Transform3d BestCameraToTarget(PhotonTarget target);
+  frc::Transform3d AlternateCameraToTarget(PhotonTarget target);
 
   PhotonVisionModes GetMode();
   void SetMode(PhotonVisionModes mode);
@@ -59,7 +60,7 @@ class PhotonVision {
 
   std::string _name;
 
-  PhotonVisionModes _mode = PhotonVisionModes::kObject;
+  PhotonVisionModes _mode = PhotonVisionModes::kNotes;
 
   photonlib::PhotonCamera* _camera;
 };
diff --git a/wombat/src/main/include/vision/Limelight.h b/wombat/src/main/include/vision/Limelight.h
index 8c21efc3..4767a44b 100644
--- a/wombat/src/main/include/vision/Limelight.h
+++ b/wombat/src/main/include/vision/Limelight.h
@@ -99,8 +99,10 @@ class Limelight {
   void OnStart();
 
   bool IsAtSetPoseVision(frc::Pose3d pose, units::second_t dt);
+  bool IsAtSetPoseVision(frc::Pose2d pose, units::second_t dt);
 
   units::meters_per_second_t GetSpeed(frc::Pose3d pose1, frc::Pose3d pose2, units::second_t dt);
+  units::meters_per_second_t GetSpeed(frc::Pose2d pose1, frc::Pose2d pose2, units::second_t dt);
 
   frc::Pose3d GetPose();
 

From bc0e5b1e0afa0750ba9ceaa1f051f00a4d3018a8 Mon Sep 17 00:00:00 2001
From: = <paul.hodges@student.education.wa.edu.au>
Date: Sat, 17 Feb 2024 15:18:57 +0800
Subject: [PATCH 161/207] Add vision behaviours

---
 src/main/cpp/Robot.cpp                     |  7 +++-
 src/main/cpp/vision/Vision.cpp             |  4 +-
 src/main/cpp/vision/VisionBehaviours.cpp   | 46 +++++++++++++++++++++-
 src/main/include/Robot.h                   |  2 +-
 src/main/include/vision/VisionBehaviours.h | 40 ++++++++++++++++++-
 vendordeps/photonlib.json                  | 43 ++++++++++++++++++++
 wombat/src/main/include/Wombat.h           |  1 +
 7 files changed, 136 insertions(+), 7 deletions(-)
 create mode 100644 vendordeps/photonlib.json

diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp
index bab64f5c..4e7a25ce 100644
--- a/src/main/cpp/Robot.cpp
+++ b/src/main/cpp/Robot.cpp
@@ -33,6 +33,7 @@
 #include "behaviour/HasBehaviour.h"
 #include "frc/geometry/Pose2d.h"
 #include "vision/Vision.h"
+#include "vision/VisionBehaviours.h"
 
 static units::second_t lastPeriodic;
 
@@ -40,7 +41,7 @@ void Robot::RobotInit() {
   sched = wom::BehaviourScheduler::GetInstance();
   m_chooser.SetDefaultOption("Default Auto", "Default Auto");
 
-  vision = new Vision("limelight", FMAP("fmap.fmap"));
+  _vision = new Vision("limelight", FMAP("fmap.fmap"));
 
   // frc::SmartDashboard::PutData("Auto Selector", &m_chooser);
 
@@ -74,6 +75,7 @@ void Robot::RobotInit() {
   _swerveDrive->SetDefaultBehaviour(
       [this]() { return wom::make<wom::ManualDrivebase>(_swerveDrive, &robotmap.controllers.driver); });
 
+
   // alphaArm = new AlphaArm(robotmap.alphaArmSystem.config);
   // wom::BehaviourScheduler::GetInstance()->Register(alphaArm);
   // alphaArm->SetDefaultBehaviour(
@@ -146,6 +148,7 @@ void Robot::AutonomousInit() {
 }
 void Robot::AutonomousPeriodic() {
   // m_driveSim->OnUpdate();
+  wom::make<AlignToAprilTag>(_vision, VisionTarget::kRedChain3, _swerveDrive);
 }
 
 void Robot::TeleopInit() {
@@ -177,7 +180,7 @@ void Robot::TeleopPeriodic() {
   // }
 
   if (robotmap.controllers.driver.GetXButtonPressed()) {
-    vision->TurnToTarget(VisionTarget::kBlueSpeakerCenter, _swerveDrive);
+    _vision->TurnToTarget(VisionTarget::kBlueSpeakerCenter, _swerveDrive);
   }
 }
 
diff --git a/src/main/cpp/vision/Vision.cpp b/src/main/cpp/vision/Vision.cpp
index b8e041c7..7e2affd6 100644
--- a/src/main/cpp/vision/Vision.cpp
+++ b/src/main/cpp/vision/Vision.cpp
@@ -331,7 +331,9 @@ frc::Pose2d Vision::TurnToTarget(VisionTarget target, wom::SwerveDrive* swerveDr
 
   units::degree_t angle = GetDistanceToTarget(target).second;
 
-  frc::Pose2d current_pose = _limelight->GetPose().ToPose2d();
+  //frc::Pose2d current_pose = _limelight->GetPose().ToPose2d();
+
+  frc::Pose2d current_pose = swerveDrive->GetPose();
 
   frc::Pose2d pose = frc::Pose2d(current_pose.X(), current_pose.Y(), angle);
 
diff --git a/src/main/cpp/vision/VisionBehaviours.cpp b/src/main/cpp/vision/VisionBehaviours.cpp
index 508b0dd2..9ffeb63e 100644
--- a/src/main/cpp/vision/VisionBehaviours.cpp
+++ b/src/main/cpp/vision/VisionBehaviours.cpp
@@ -6,14 +6,56 @@
 
 #include "frc/geometry/Pose2d.h"
 #include "units/angle.h"
+#include "vision/Vision.h"
 
-AlignToAprilTag::AlignToAprilTag(Vision* vision, VisionTarget target, wom::SwerveDrive* swerveDrive)
-    : _vision(vision), _target(target), _swerveDrive(swerveDrive) {}
+AlignToAprilTag::AlignToAprilTag(Vision* vision, VisionTarget target, wom::SwerveDrive* swerveDrive, units::meter_t offset)
+    : _vision(vision), _swerveDrive(swerveDrive), _target(target), _offset(offset) {}
 
 void AlignToAprilTag::OnTick(units::second_t dt) {
+  frc::Pose2d pose = _vision->AlignToTarget(_target, _offset, _swerveDrive);
+
+  if (_vision->IsAtPose(pose, dt)) {
+    SetDone();
+  }
+}
+
+TurnToAprilTag::TurnToAprilTag(Vision* vision, VisionTarget target, wom::SwerveDrive* swerveDrive)
+    : _vision(vision), _swerveDrive(swerveDrive), _target(target) {}
+
+void TurnToAprilTag::OnTick(units::second_t dt) {
   frc::Pose2d pose = _vision->TurnToTarget(_target, _swerveDrive);
 
   if (_vision->IsAtPose(pose, dt)) {
     SetDone();
   }
 }
+
+LockOnToTarget::LockOnToTarget(Vision* vision, VisionTargetObjects target, wom::SwerveDrive* swerveDrive)
+    : _vision(vision), _target(target), _camera(nullptr), _swerveDrive(swerveDrive), _type(VisionType::kLimelight) {}
+
+LockOnToTarget::LockOnToTarget(wom::PhotonVision* vision, Vision* limelight, wom::SwerveDrive* swerveDrive) 
+  : _camera(vision), _swerveDrive(swerveDrive), _vision(limelight), _type(VisionType::kGenericCamera) {}
+
+void LockOnToTarget::OnTick(units::second_t dt) {
+  units::degree_t angle;
+
+  switch (_type) {
+    case VisionType::kLimelight:
+      {
+        angle = _vision->LockOn(_target);
+      }
+    case VisionType::kGenericCamera:
+      {
+        angle = units::degree_t{_camera->GetTargetPitch(_camera->GetTarget())};
+      }
+  }
+
+  frc::Pose2d pose = frc::Pose2d(_swerveDrive->GetPose().X(), _swerveDrive->GetPose().Y(), angle);
+
+  _swerveDrive->SetPose(pose);
+
+  if (_vision->IsAtPose(pose, dt)) {
+    SetDone();
+  }
+}
+
diff --git a/src/main/include/Robot.h b/src/main/include/Robot.h
index d475b14d..ea659983 100644
--- a/src/main/include/Robot.h
+++ b/src/main/include/Robot.h
@@ -67,5 +67,5 @@ class Robot : public frc::TimedRobot {
   // ctre::phoenix6::hardware::TalonFX *frontRight;
   // ctre::phoenix6::hardware::TalonFX *backLeft;
   // ctre::phoenix6::hardware::TalonFX *backRight;
-  Vision* vision;
+  Vision* _vision;
 };
diff --git a/src/main/include/vision/VisionBehaviours.h b/src/main/include/vision/VisionBehaviours.h
index eacc9a25..dedc52b7 100644
--- a/src/main/include/vision/VisionBehaviours.h
+++ b/src/main/include/vision/VisionBehaviours.h
@@ -7,12 +7,20 @@
 #include "Wombat.h"
 #include "behaviour/HasBehaviour.h"
 #include "drivetrain/SwerveDrive.h"
+#include "units/length.h"
 #include "units/time.h"
 #include "vision/Vision.h"
 
+#include "Wombat.h"
+
+enum class VisionType {
+  kLimelight = 0,
+  kGenericCamera = 1
+};
+
 class AlignToAprilTag : public behaviour::Behaviour {
  public:
-  explicit AlignToAprilTag(Vision* vision, VisionTarget target, wom::SwerveDrive* swerveDrive);
+  explicit AlignToAprilTag(Vision* vision, VisionTarget target, wom::SwerveDrive* swerveDrive, units::meter_t offset = 0_m);
 
   void OnTick(units::second_t dt);
 
@@ -20,4 +28,34 @@ class AlignToAprilTag : public behaviour::Behaviour {
   Vision* _vision;
   wom::SwerveDrive* _swerveDrive;
   VisionTarget _target;
+  units::meter_t _offset;
+};
+
+class TurnToAprilTag : public behaviour::Behaviour {
+  public:
+    explicit TurnToAprilTag(Vision* vision, VisionTarget target, wom::SwerveDrive* swerveDrive);
+  
+    void OnTick(units::second_t dt);
+
+  private:
+    Vision* _vision;
+  wom::SwerveDrive* _swerveDrive;
+  VisionTarget _target;
+};
+
+class LockOnToTarget : public behaviour::Behaviour {
+  public:
+    explicit LockOnToTarget(Vision* vision, VisionTargetObjects target, wom::SwerveDrive* swerveDrive);
+    explicit LockOnToTarget(wom::PhotonVision* camera, Vision* limelight, wom::SwerveDrive* swerveDrive);
+
+    void OnTick(units::second_t dt);
+
+  private:
+    Vision* _vision;
+    wom::PhotonVision* _camera;
+
+    VisionTargetObjects _target;
+  wom::SwerveDrive* _swerveDrive;
+  VisionType _type; 
 };
+
diff --git a/vendordeps/photonlib.json b/vendordeps/photonlib.json
new file mode 100644
index 00000000..4c80f3fe
--- /dev/null
+++ b/vendordeps/photonlib.json
@@ -0,0 +1,43 @@
+{
+    "fileName": "photonlib.json",
+    "name": "photonlib",
+    "version": "v2024.1.1-beta-3.2",
+    "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004",
+    "frcYear": "2024",
+    "mavenUrls": [
+        "https://maven.photonvision.org/repository/internal",
+        "https://maven.photonvision.org/repository/snapshots"
+    ],
+    "jsonUrl": "https://maven.photonvision.org/repository/internal/org/photonvision/PhotonLib-json/1.0/PhotonLib-json-1.0.json",
+    "jniDependencies": [],
+    "cppDependencies": [
+        {
+            "groupId": "org.photonvision",
+            "artifactId": "PhotonLib-cpp",
+            "version": "v2024.1.1-beta-3.2",
+            "libName": "Photon",
+            "headerClassifier": "headers",
+            "sharedLibrary": true,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "linuxathena",
+                "linuxx86-64",
+                "osxuniversal"
+            ]
+        }
+    ],
+    "javaDependencies": [
+        {
+            "groupId": "org.photonvision",
+            "artifactId": "PhotonLib-java",
+            "version": "v2024.1.1-beta-3.2"
+        },
+        {
+            "groupId": "org.photonvision",
+            "artifactId": "PhotonTargeting-java",
+            "version": "v2024.1.1-beta-3.2"
+        }
+    ]
+}
+
diff --git a/wombat/src/main/include/Wombat.h b/wombat/src/main/include/Wombat.h
index 89a967a7..4f34e6c0 100644
--- a/wombat/src/main/include/Wombat.h
+++ b/wombat/src/main/include/Wombat.h
@@ -16,6 +16,7 @@
 #include "utils/RobotStartup.h"
 #include "utils/Util.h"
 #include "vision/Limelight.h"
+#include "vision/Camera.h"
 
 namespace wom {
 using namespace wom;

From 7a83404ea01c4990f69d9a5ddf702d26d2b75d2e Mon Sep 17 00:00:00 2001
From: = <paul.hodges@student.education.wa.edu.au>
Date: Sat, 17 Feb 2024 15:50:12 +0800
Subject: [PATCH 162/207] merge

---
 src/main/cpp/Auto.cpp   | 281 ++++++++++++++++++++++++++++++++++++++++
 src/main/include/Auto.h |  34 +++++
 2 files changed, 315 insertions(+)
 create mode 100644 src/main/cpp/Auto.cpp
 create mode 100644 src/main/include/Auto.h

diff --git a/src/main/cpp/Auto.cpp b/src/main/cpp/Auto.cpp
new file mode 100644
index 00000000..4d22e98b
--- /dev/null
+++ b/src/main/cpp/Auto.cpp
@@ -0,0 +1,281 @@
+// Copyright (c) 2023-2024 CurtinFRC
+// Open Source Software, you can modify it according to the terms
+// of the MIT License at the root of this project
+
+#include "Auto.h"
+
+std::shared_ptr<behaviour::Behaviour> autos::Taxi(wom::drivetrain::SwerveDrive* _swerveDrive,
+                                                  Shooter* _shooter, Intake* _intake, AlphaArm* _alphaArm) {
+  return behaviour::make<ArmToSetPoint>(_alphaArm, 0_deg);
+  behaviour::make<AutoShoot>(_shooter);
+  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+  behaviour::make<wom::drivetrain::behaviours::DrivebasePoseBehaviour>(
+      _swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false);
+}
+// Shoots starting note then moves out of starting position.
+
+std::shared_ptr<behaviour::Behaviour> autos::QuadrupleClose(wom::drivetrain::SwerveDrive* _swerveDrive,
+                                                            Shooter* _shooter, Intake* _intake,
+                                                            AlphaArm* _alphaArm) {
+  return behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+  behaviour::make<AutoShoot>(_shooter);
+  behaviour::make<wom::drivetrain::behaviours::DrivebasePoseBehaviour>(
+      _swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false);
+  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+  behaviour::make<AutoIntake>(_intake);
+  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+  behaviour::make<AutoShoot>(_shooter);
+  behaviour::make<wom::drivetrain::behaviours::DrivebasePoseBehaviour>(
+      _swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false);
+  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+  behaviour::make<AutoIntake>(_intake);
+  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+  behaviour::make<AutoShoot>(_shooter);
+  behaviour::make<wom::drivetrain::behaviours::DrivebasePoseBehaviour>(
+      _swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false);
+  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+  behaviour::make<AutoIntake>(_intake);
+  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+  behaviour::make<AutoShoot>(_shooter);
+
+  /*
+   4N Close
+ 1. Shoot starting note into speaker
+ 2. Intake note from close note
+ 3. Shoot note into speaker
+ 4. Intake note from close floor note
+ 5. Shoot note into speaker
+ 6. Intake not from close floor
+ 7. Shoot note
+ */
+}
+
+std::shared_ptr<behaviour::Behaviour> autos::QuadrupleFar(wom::drivetrain::SwerveDrive* _swerveDrive,
+                                                          Shooter* _shooter, Intake* _intake,
+                                                          AlphaArm* _alphaArm) {
+  return behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+  behaviour::make<AutoShoot>(_shooter);
+  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+  behaviour::make<wom::drivetrain::behaviours::DrivebasePoseBehaviour>(
+      _swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false);
+  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+  behaviour::make<AutoIntake>(_intake);
+  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+  behaviour::make<wom::drivetrain::behaviours::DrivebasePoseBehaviour>(
+      _swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false);
+  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+  behaviour::make<AutoShoot>(_shooter);
+  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+  behaviour::make<wom::drivetrain::behaviours::DrivebasePoseBehaviour>(
+      _swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false);
+  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+  behaviour::make<AutoIntake>(_intake);
+  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+  behaviour::make<wom::drivetrain::behaviours::DrivebasePoseBehaviour>(
+      _swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false);
+  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+  behaviour::make<AutoShoot>(_shooter);
+  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+  behaviour::make<wom::drivetrain::behaviours::DrivebasePoseBehaviour>(
+      _swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false);
+  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+  behaviour::make<AutoIntake>(_intake);
+  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+  behaviour::make<wom::drivetrain::behaviours::DrivebasePoseBehaviour>(
+      _swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false);
+  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+  behaviour::make<AutoShoot>(_shooter);
+  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+  behaviour::make<wom::drivetrain::behaviours::DrivebasePoseBehaviour>(
+      _swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false);
+  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+  behaviour::make<AutoIntake>(_intake);
+
+  /*
+    4N Far
+  1. Shoot start note in speaker
+  2. Drive to far note
+  3. Intake note
+  4. Drive back to shooting line
+  5. Shoot note into speaker
+  6. Drive to note
+  7. Intake note
+  8. Drive to shooting line
+  9. Shoot note
+  10. Drive to note
+  11. Intake note
+  12. Drive to shooting line
+  13. Shoot note
+  14. Drive to intake note (if possible)
+  */
+}
+
+std::shared_ptr<behaviour::Behaviour> autos::QuadrupleCloseDoubleFar(
+    wom::drivetrain::SwerveDrive* _swerveDrive, Shooter* _shooter, Intake* _intake, AlphaArm* _alphaArm) {
+  return behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+  behaviour::make<AutoShoot>(_shooter);
+  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+  behaviour::make<wom::drivetrain::behaviours::DrivebasePoseBehaviour>(
+      _swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false);
+  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+  behaviour::make<AutoIntake>(_intake);
+  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+  behaviour::make<wom::drivetrain::behaviours::DrivebasePoseBehaviour>(
+      _swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false);
+  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+  behaviour::make<AutoShoot>(_shooter);
+  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+  behaviour::make<wom::drivetrain::behaviours::DrivebasePoseBehaviour>(
+      _swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false);
+  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+  behaviour::make<AutoIntake>(_intake);
+  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+  behaviour::make<wom::drivetrain::behaviours::DrivebasePoseBehaviour>(
+      _swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false);
+  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+  behaviour::make<AutoShoot>(_shooter);
+  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+  behaviour::make<wom::drivetrain::behaviours::DrivebasePoseBehaviour>(
+      _swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false);
+  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+  behaviour::make<AutoIntake>(_intake);
+  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+  behaviour::make<wom::drivetrain::behaviours::DrivebasePoseBehaviour>(
+      _swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false);
+  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+  behaviour::make<AutoShoot>(_shooter);
+  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+  behaviour::make<wom::drivetrain::behaviours::DrivebasePoseBehaviour>(
+      _swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false);
+  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+  behaviour::make<AutoIntake>(_intake);
+  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+  behaviour::make<wom::drivetrain::behaviours::DrivebasePoseBehaviour>(
+      _swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false);
+  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+  behaviour::make<AutoShoot>(_shooter);
+  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+  behaviour::make<wom::drivetrain::behaviours::DrivebasePoseBehaviour>(
+      _swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false);
+  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+  behaviour::make<AutoIntake>(_intake);
+  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+  behaviour::make<wom::drivetrain::behaviours::DrivebasePoseBehaviour>(
+      _swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false);
+  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+  behaviour::make<AutoShoot>(_shooter);
+  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+  behaviour::make<wom::drivetrain::behaviours::DrivebasePoseBehaviour>(
+      _swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false);
+  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+  behaviour::make<AutoIntake>(_intake);
+  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+  behaviour::make<AutoShoot>(_shooter);
+
+  //   4N Close 2N Far
+  // 1. Shoot note
+  // 2. Drive to close note
+  // 3. Intake note
+  // 4. Shoot note
+  // 5. Drive to close note
+  // 6. Intake note
+  // 7. Shoot note
+  // 8. Drive to close note
+  // 9. Intake note
+  // 10. Shoot note
+  // 11. Drive to far note
+  // 12. Intake note
+  // 13. Drive to shooting line
+  // 14. Shoot note
+  // 15. Drive to far note
+  // 16. Intake note
+  // 17. Drive to shooting line
+  // 18. Shoot note
+}
+
+std::shared_ptr<behaviour::Behaviour> autos::QuadrupleCloseSingleFar(
+    wom::drivetrain::SwerveDrive* _swerveDrive, Shooter* _shooter, Intake* _intake, AlphaArm* _alphaArm) {
+  return behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+  behaviour::make<AutoShoot>(_shooter);
+  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+  behaviour::make<wom::drivetrain::behaviours::DrivebasePoseBehaviour>(
+      _swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false);
+  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+  behaviour::make<AutoIntake>(_intake);
+  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+  behaviour::make<wom::drivetrain::behaviours::DrivebasePoseBehaviour>(
+      _swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false);
+  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+  behaviour::make<AutoShoot>(_shooter);
+  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+  behaviour::make<wom::drivetrain::behaviours::DrivebasePoseBehaviour>(
+      _swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false);
+  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+  behaviour::make<AutoIntake>(_intake);
+  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+  behaviour::make<wom::drivetrain::behaviours::DrivebasePoseBehaviour>(
+      _swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false);
+  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+  behaviour::make<AutoShoot>(_shooter);
+  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+  behaviour::make<wom::drivetrain::behaviours::DrivebasePoseBehaviour>(
+      _swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false);
+  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+  behaviour::make<AutoIntake>(_intake);
+  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+  behaviour::make<wom::drivetrain::behaviours::DrivebasePoseBehaviour>(
+      _swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false);
+  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+  behaviour::make<AutoShoot>(_shooter);
+  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+}
+
+// 4N Close 1N Far
+// 1. Shoot note
+// 2. Drive to close note
+// 3. Intake note
+// 4. Drive to speaker
+// 5. Shoot note
+// 6. Drive to close note
+// 7. Intake note
+// 8. Drive to speaker
+// 9. Shoot note
+// 10. Drive to close note
+// 11. Intake note
+// 12. Drive to speaker
+// 13. Shoot note
+// 14. Drive to far note
+// 15. Intake note
+// 15. Drive to speaker
+// 16. shoot
+
+//     /*
+//     TRAP AUTO
+//     1. Drive to trap
+//     2. Climb up
+//     3. Shoot note
+//     4. Climb down
+//     5. Drive to far note
+//     6. Intake note
+//     7. Drive to trap
+//     8. Climb up
+//     9. Shoot note
+//     10. Climb down
+//     11. Drive to far note
+//     12. Intake
+//     13. Drive to trap
+//     14. Climb up
+//     15. Shoot note
+//     16. Climb down
+//     */
+// }
+
+std::shared_ptr<behaviour::Behaviour> autos::AutoTest(wom::drivetrain::SwerveDrive* _swerveDrive,
+                                                      Shooter* _shooter, Intake* _intake,
+                                                      AlphaArm* _alphaArm) {
+  return behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+  behaviour::make<wom::drivetrain::behaviours::DrivebasePoseBehaviour>(
+      _swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false);
+  behaviour::make<AutoShoot>(_shooter);
+  behaviour::make<AutoIntake>(_intake);
+}  // This auto is a test for auto to see if all things work.
diff --git a/src/main/include/Auto.h b/src/main/include/Auto.h
new file mode 100644
index 00000000..61aecc4e
--- /dev/null
+++ b/src/main/include/Auto.h
@@ -0,0 +1,34 @@
+// Copyright (c) 2023-2024 CurtinFRC
+// Open Source Software, you can modify it according to the terms
+// of the MIT License at the root of this project
+
+#pragma once
+
+#include <memory>
+
+#include "AlphaArmBehaviour.h"
+#include "IntakeBehaviour.h"
+#include "ShooterBehaviour.h"
+#include "Wombat.h"
+
+namespace autos {
+std::shared_ptr<behaviour::Behaviour> AutoTest(wom::drivetrain::SwerveDrive* _swerveDrive, Shooter* _shooter,
+                                               Intake* _intake, AlphaArm* _alphaArm);
+
+std::shared_ptr<behaviour::Behaviour> Taxi(wom::drivetrain::SwerveDrive* _swerveDrive, Shooter* _shooter,
+                                           Intake* _intake, AlphaArm* _alphaArm);
+
+std::shared_ptr<behaviour::Behaviour> QuadrupleClose(wom::drivetrain::SwerveDrive* _swerveDrive,
+                                                     Shooter* _shooter, Intake* _intake, AlphaArm* _alphaArm);
+
+std::shared_ptr<behaviour::Behaviour> QuadrupleFar(wom::drivetrain::SwerveDrive* _swerveDrive,
+                                                   Shooter* _shooter, Intake* _intake, AlphaArm* _alphaArm);
+
+std::shared_ptr<behaviour::Behaviour> QuadrupleCloseDoubleFar(wom::drivetrain::SwerveDrive* _swerveDrive,
+                                                              Shooter* _shooter, Intake* _intake,
+                                                              AlphaArm* _alphaArm);
+
+std::shared_ptr<behaviour::Behaviour> QuadrupleCloseSingleFar(wom::drivetrain::SwerveDrive* _swerveDrive,
+                                                              Shooter* _shooter, Intake* _intake,
+                                                              AlphaArm* _alphaArm);
+}  // namespace autos

From 1802e99f79d411f8254434260d93b8f34dd3ab19 Mon Sep 17 00:00:00 2001
From: = <paul.hodges@student.education.wa.edu.au>
Date: Sat, 17 Feb 2024 16:22:10 +0800
Subject: [PATCH 163/207] merge

---
 src/main/cpp/IntakeBehaviour.cpp              |  6 ---
 src/main/cpp/Robot.cpp                        |  9 ++--
 src/main/include/IntakeBehaviour.h            |  5 +-
 src/main/include/RobotMap.h                   | 22 +-------
 wombat/src/main/cpp/drivetrain/Drivetrain.cpp | 51 -------------------
 wombat/src/main/cpp/utils/Encoder.cpp         |  6 +--
 6 files changed, 11 insertions(+), 88 deletions(-)
 delete mode 100644 wombat/src/main/cpp/drivetrain/Drivetrain.cpp

diff --git a/src/main/cpp/IntakeBehaviour.cpp b/src/main/cpp/IntakeBehaviour.cpp
index 9b4dcd27..b3897b0d 100644
--- a/src/main/cpp/IntakeBehaviour.cpp
+++ b/src/main/cpp/IntakeBehaviour.cpp
@@ -86,7 +86,6 @@ void IntakeManualControl::OnTick(units::second_t dt) {
     }
   }
 }
-
 AutoIntake::AutoIntake(Intake* intake) : _intake(intake) {
   Controls(intake);
 }
@@ -98,8 +97,3 @@ void AutoIntake::OnTick(units::second_t dt) {
   //   _intake->setState(IntakeState::kIdle);
   // }
 }
-IntakeAutoControl::IntakeAutoControl(Intake* intake) : _intake(intake) {
-  Controls(intake);
-}
-
-void IntakeAutoControl::OnTick(units::second_t dt) {}
diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp
index 77677bc4..4574f139 100644
--- a/src/main/cpp/Robot.cpp
+++ b/src/main/cpp/Robot.cpp
@@ -215,10 +215,11 @@ void Robot::AutonomousInit() {
 void Robot::AutonomousPeriodic() {
   fmt::print("Auto selected: {}\n", m_autoSelected);
 }
-void Robot::AutonomousPeriodic() {
+
+// void Robot::AutonomousPeriodic() {
   // m_driveSim->OnUpdate();
-  wom::make<AlignToAprilTag>(_vision, VisionTarget::kRedChain3, _swerveDrive);
-}
+  // wom::make<AlignToAprilTag>(_vision, VisionTarget::kRedChain3, _swerveDrive);
+// }
 
 void Robot::TeleopInit() {
   loop.Clear();
@@ -233,7 +234,7 @@ void Robot::TeleopInit() {
   // backRight->SetVoltage(4_V);
 }
 
-void Robot::TeleopPeriodic() {}
+// void Robot::TeleopPeriodic() {}
 void Robot::TeleopPeriodic() {
   // if (robotmap.controllers.driver.GetXButtonPressed() &&
   //     vision->TargetIsVisible(VisionTargetObjects::kNote)) {
diff --git a/src/main/include/IntakeBehaviour.h b/src/main/include/IntakeBehaviour.h
index 931273fa..db3ff9c0 100644
--- a/src/main/include/IntakeBehaviour.h
+++ b/src/main/include/IntakeBehaviour.h
@@ -27,10 +27,9 @@ class IntakeManualControl : public behaviour::Behaviour {
   bool _passing = false;
 };
 
-class IntakeAutoControl : public behaviour::Behaviour {
+class AutoIntake : public behaviour::Behaviour {
  public:
-  explicit IntakeAutoControl(Intake* intake);
-
+  explicit AutoIntake(Intake* intake);
   void OnTick(units::second_t dt) override;
 
  private:
diff --git a/src/main/include/RobotMap.h b/src/main/include/RobotMap.h
index 623ec8d8..b14d0194 100644
--- a/src/main/include/RobotMap.h
+++ b/src/main/include/RobotMap.h
@@ -37,6 +37,7 @@ struct RobotMap {
   struct Controllers {
     frc::XboxController driver = frc::XboxController(0);
     frc::XboxController codriver = frc::XboxController(1);
+    frc::XboxController testController = frc::XboxController(2);
   };
   Controllers controllers;
 
@@ -85,27 +86,6 @@ struct RobotMap {
   };
   Shooter shooterSystem;
 
-  struct SwerveBase {
-    ctre::phoenix6::hardware::CANcoder frontLeftCancoder{18, "Drivebase"};
-    ctre::phoenix6::hardware::CANcoder frontRightCancoder{19, "Drivebase"};
-    ctre::phoenix6::hardware::CANcoder backLeftCancoder{16, "Drivebase"};
-    ctre::phoenix6::hardware::CANcoder backRightCancoder{17, "Drivebase"};
-
-    ctre::phoenix6::hardware::Pigeon2* gyro = new ctre::phoenix6::hardware::Pigeon2(20, "Drivebase");
-    wpi::array<ctre::phoenix6::hardware::TalonFX*, 4> turnMotors{
-        new ctre::phoenix6::hardware::TalonFX(7, "Drivebase"),   // front left
-        new ctre::phoenix6::hardware::TalonFX(2, "Drivebase"),   // front right
-        new ctre::phoenix6::hardware::TalonFX(6, "Drivebase"),   // back left
-        new ctre::phoenix6::hardware::TalonFX(4, "Drivebase")};  // back right
-    wpi::array<ctre::phoenix6::hardware::TalonFX*, 4> driveMotors{
-        new ctre::phoenix6::hardware::TalonFX(9, "Drivebase"),   // front left
-        new ctre::phoenix6::hardware::TalonFX(1, "Drivebase"),   // front right
-        new ctre::phoenix6::hardware::TalonFX(5, "Drivebase"),   // back left
-        new ctre::phoenix6::hardware::TalonFX(3, "Drivebase")};  // back right
-=======
-    frc::XboxController testController = frc::XboxController(2);
-  };
-  Controllers controllers;
 
   //   struct AlphaArmSystem {
   //     rev::CANSparkMax alphaArmMotor{12, rev::CANSparkMax::MotorType::kBrushless};
diff --git a/wombat/src/main/cpp/drivetrain/Drivetrain.cpp b/wombat/src/main/cpp/drivetrain/Drivetrain.cpp
deleted file mode 100644
index 387bbc31..00000000
--- a/wombat/src/main/cpp/drivetrain/Drivetrain.cpp
+++ /dev/null
@@ -1,51 +0,0 @@
-// Copyright (c) 2023-2024 CurtinFRC
-// Open Source Software, you can modify it according to the terms
-// of the MIT License at the root of this project
-
-#include "drivetrain/Drivetrain.h"
-
-#include <iostream>
-
-#include "utils/Util.h"
-
-using namespace frc;
-using namespace units;
-
-wom::drivetrain::Drivetrain::Drivetrain(DrivetrainConfig* config, XboxController& driver)
-    : _config(config), _driver(driver) {}
-wom::drivetrain::Drivetrain::~Drivetrain() {}
-
-wom::drivetrain::DrivetrainConfig* wom::drivetrain::Drivetrain::GetConfig() {
-  return _config;
-}
-wom::drivetrain::DrivetrainState wom::drivetrain::Drivetrain::GetState() {
-  return _state;
-}
-
-void wom::drivetrain::Drivetrain::SetState(DrivetrainState state) {
-  _state = state;
-}
-
-void wom::drivetrain::Drivetrain::OnStart() {
-  std::cout << "Starting Tank" << std::endl;
-}
-
-void wom::drivetrain::Drivetrain::OnUpdate(second_t dt) {
-  switch (_state) {
-    case DrivetrainState::kIdle:
-      break;
-    case DrivetrainState::kTank: {
-      double rightSpeed = wom::utils::deadzone(_driver.GetRightY());
-      double leftSpeed = wom::utils::deadzone(_driver.GetLeftY());
-      // _config->left1.motorController->SetVoltage(leftSpeed * maxVolts);
-      // _config->left2.motorController->SetVoltage(leftSpeed * maxVolts);
-      // _config->left3.motorController->SetVoltage(leftSpeed * maxVolts);
-      // _config->right1.motorController->SetVoltage(rightSpeed * maxVolts);
-      // _config->right2.motorController->SetVoltage(rightSpeed * maxVolts);
-      // _config->right3.motorController->SetVoltage(rightSpeed * maxVolts);
-      break;
-    }
-    case DrivetrainState::kAuto:
-      break;
-  }
-}
diff --git a/wombat/src/main/cpp/utils/Encoder.cpp b/wombat/src/main/cpp/utils/Encoder.cpp
index 18c1ac5c..1120b399 100644
--- a/wombat/src/main/cpp/utils/Encoder.cpp
+++ b/wombat/src/main/cpp/utils/Encoder.cpp
@@ -57,7 +57,7 @@ units::radian_t wom::utils::Encoder::GetEncoderPosition() {
 }
 
 double wom::utils::Encoder::GetEncoderDistance() {
-  return GetEncoderTicks() * (2 * 3.141592) * _wheelRadius.value();
+  return GetEncoderTicks() * (2 * 3.1415) * _wheelRadius.value();
 }
 
 units::radians_per_second_t wom::utils::Encoder::GetEncoderAngularVelocity() {
@@ -115,7 +115,7 @@ double wom::utils::CANSparkMaxEncoder::GetPosition() const {
 }
 
 double wom::utils::CANSparkMaxEncoder::GetVelocity() const {
-  return _encoder.GetVelocity() * 3.14159265 * 2 / 60;
+  return _encoder.GetVelocity();
 }
 
 wom::utils::TalonFXEncoder::TalonFXEncoder(ctre::phoenix6::hardware::TalonFX* controller,
@@ -150,7 +150,7 @@ wom::utils::CanEncoder::CanEncoder(int deviceNumber, units::meter_t wheelRadius,
 }
 
 double wom::utils::CanEncoder::GetEncoderRawTicks() const {
-  return _canEncoder->GetPosition().GetValue().value() * 2 * 3.14;
+  return (_canEncoder->GetAbsolutePosition().GetValue().value() * 2 * 3.14) - _offset.value();
 }
 
 double wom::utils::CanEncoder::GetEncoderTickVelocity() const {

From 3db3487f4e4890dd18f9e19ce9017b8925a5e389 Mon Sep 17 00:00:00 2001
From: JoystickMaster-race <foxlocks179@gmail.com>
Date: Sun, 18 Feb 2024 16:44:33 +0800
Subject: [PATCH 164/207] setpoints

---
 src/main/cpp/AlphaArm.cpp               |  99 ++++++++++
 src/main/cpp/AlphaArmBehaviour.cpp      |  74 ++++++++
 src/main/cpp/Robot.cpp                  | 172 ++++++++++++++---
 src/main/include/AlphaArm.h             |  64 +++++++
 src/main/include/AlphaArmBehaviour.h    |  28 +++
 src/main/include/Robot.h                |  35 ++--
 src/main/include/RobotMap.h             | 239 ++++++++++++++----------
 wombat/src/main/cpp/utils/Encoder.cpp   |  71 ++++---
 wombat/src/main/include/utils/Encoder.h |  20 +-
 9 files changed, 631 insertions(+), 171 deletions(-)
 create mode 100644 src/main/cpp/AlphaArm.cpp
 create mode 100644 src/main/cpp/AlphaArmBehaviour.cpp
 create mode 100644 src/main/include/AlphaArm.h
 create mode 100644 src/main/include/AlphaArmBehaviour.h

diff --git a/src/main/cpp/AlphaArm.cpp b/src/main/cpp/AlphaArm.cpp
new file mode 100644
index 00000000..2aa2fdf0
--- /dev/null
+++ b/src/main/cpp/AlphaArm.cpp
@@ -0,0 +1,99 @@
+// Copyright (c) 2023-2024 CurtinFRC
+// Open Source Software, you can modify it according to the terms
+// of the MIT License at the root of this project
+
+#include "AlphaArm.h"
+
+
+AlphaArm::AlphaArm(AlphaArmConfig *config) : _config(config), _pidArm{frc::PIDController(1.2, 0.4, 0)}, _pidArmStates{frc::PIDController(2, 0, 0)} 
+{
+  // _alphaArmPID = wom::utils::PIDController(config.alphaArmPID);
+}
+
+// AlphaArmConfig AlphaArm::GetConfig() {
+//   return _config;
+// }
+
+void AlphaArm::OnUpdate(units::second_t dt) {
+  switch (_state) {
+    case AlphaArmState::kIdle:
+    
+      _setAlphaArmVoltage = 0_V;
+      break;
+    case AlphaArmState::kRaw:
+    std::cout << "RawControl" << std::endl;
+       _setAlphaArmVoltage = _rawArmVoltage;
+      
+      break;
+
+    case AlphaArmState::kHoldAngle:
+    {
+      _pidArm.SetSetpoint(-_goal);
+       //_setAlphaArmVoltage = _pidArm.Calculate(_alphaArm->GetConfig().config->alphaArmEncoder.GetEncoderPosition());
+      _setAlphaArmVoltage = units::volt_t{_pidArmStates.Calculate(-_config->alphaArmEncoder.GetEncoderPosition().value())};
+    }
+    break;
+    case AlphaArmState::kAmpAngle:
+      std::cout << "Amp Angle" << std::endl;
+      
+
+       _pidArm.SetSetpoint(2.17);
+       _setAlphaArmVoltage = units::volt_t{_pidArmStates.Calculate(_config->alphaArmEncoder.GetEncoderPosition().value())};
+
+    break;
+
+    case AlphaArmState::kIntakeAngle:
+    std::cout << "Intake Angle" << std::endl;
+       _pidArm.SetSetpoint(0.48);
+       _setAlphaArmVoltage = units::volt_t{_pidArmStates.Calculate(_config->alphaArmEncoder.GetEncoderPosition().value())};
+    break;
+
+    case AlphaArmState::kSpeakerAngle:
+    std::cout << "Speaker Angle" << std::endl;
+       _pidArm.SetSetpoint(0.82);
+       _setAlphaArmVoltage = units::volt_t{_pidArmStates.Calculate(_config->alphaArmEncoder.GetEncoderPosition().value())};
+
+    break;
+
+    case AlphaArmState::kStowed:
+    std::cout << "IntakeAngle" << std::endl;
+       _pidArm.SetSetpoint(0.6);
+       _setAlphaArmVoltage = units::volt_t{_pidArmStates.Calculate(_config->alphaArmEncoder.GetEncoderPosition().value())};
+      default:
+      std::cout << "Error: alphaArm in INVALID STATE" << std::endl;
+      break;
+
+  }
+   _config->alphaArmGearbox.motorController->SetVoltage(_setAlphaArmVoltage);
+   _config->alphaArmGearbox2.motorController->SetVoltage(_setAlphaArmVoltage);
+    //std::cout << "Encoder Value: " << _config.alphaArmGearbox.encoder->GetEncoderPosition().value() << std::endl;
+    std::cout << "Encoder Value: " << _config->alphaArmEncoder.GetEncoderPosition().value() << std::endl;
+    _table->GetEntry("PID Error").SetDouble(_pidArm.GetPositionError());
+    _table->GetEntry("SetPoint").SetDouble(_pidArm.GetSetpoint());
+    _table->GetEntry("Input").SetDouble(_config->alphaArmEncoder.GetEncoderPosition().value());
+
+    _table->GetEntry("PID Error State").SetDouble(_pidArmStates.GetPositionError());
+    _table->GetEntry("SetPoint State").SetDouble(_pidArmStates.GetSetpoint());
+
+// //std::cout << "Encoder Position: " << armEncoder.GetAbsolutePosition() << std::endl;
+//   //std::cout << "Encoder Value: " << _config.alphaArmEncoder->GetEncoderPosition().value() << std::endl;
+     std::cout << "Voltage:" << _setAlphaArmVoltage.value() << std::endl;
+//  }
+}
+
+void AlphaArm::SetState(AlphaArmState state) {
+  _state = state;
+}
+
+void AlphaArm::SetArmRaw(units::volt_t voltage) {
+  _rawArmVoltage = voltage;
+}
+
+void AlphaArm::SetGoal(double goal){
+  _goal = goal;
+}
+
+void AlphaArm::SetControllerRaw(units::volt_t voltage){
+  _controlledRawVoltage = voltage;
+}
+
diff --git a/src/main/cpp/AlphaArmBehaviour.cpp b/src/main/cpp/AlphaArmBehaviour.cpp
new file mode 100644
index 00000000..9348a848
--- /dev/null
+++ b/src/main/cpp/AlphaArmBehaviour.cpp
@@ -0,0 +1,74 @@
+// Copyright (c) 2023-2024 CurtinFRC
+// Open Source Software, you can modify it according to the terms
+// of the MIT License at the root of this project
+
+#include "AlphaArmBehaviour.h"
+
+#include <frc/XboxController.h>
+
+AlphaArmManualControl::AlphaArmManualControl(AlphaArm* alphaArm, frc::XboxController* codriver)
+    : _alphaArm(alphaArm), _codriver(codriver) {
+  Controls(alphaArm);
+}
+
+AlphaArmConfig AlphaArm::GetConfig() {
+  return *_config;
+}
+
+void AlphaArmManualControl::OnTick(units::second_t dt) {
+  // std::cout << "HELLO" << std::endl;
+
+  _table->GetEntry("State").SetBoolean(_rawControl);
+  _table->GetEntry("Goal Value").SetBoolean(_gotValue);
+  
+
+  if (_codriver->GetStartButtonPressed()) {
+    if (_rawControl == true) {
+      _rawControl = false;
+    } else {
+      _rawControl = true;
+    }
+  }
+
+  // if (_rawControl) {
+  //   _alphaArm->SetState(AlphaArmState::kRaw);
+  //   _alphaArm->SetArmRaw(_codriver->GetRightY() * 12_V);
+  // } else {
+
+  //   if(std::abs(_codriver->GetRightY()) > 0.1)  {
+  //     _alphaArm->SetState(AlphaArmState::kRaw);
+  //     _alphaArm->SetArmRaw(_codriver->GetRightY() * 12_V);
+  //     _gotValue = false;
+  //   } else {
+  //     double input = _alphaArm->GetConfig().alphaArmEncoder.GetEncoderPosition().value();
+  //     if (!_gotValue){
+  //       _alphaArm->SetGoal(input);
+  //       _gotValue = true;
+  //     } 
+  //     _alphaArm->SetState(AlphaArmState::kHoldAngle);
+
+  //   }
+  // }
+  if(_codriver->GetYButtonPressed()){
+    _alphaArm->SetState(AlphaArmState::kIntakeAngle);
+  }
+  if(_codriver->GetBButtonPressed()){
+    _alphaArm->SetState(AlphaArmState::kAmpAngle);
+
+  }
+  if(_codriver->GetAButtonPressed()){
+    _alphaArm->SetState(AlphaArmState::kSpeakerAngle);
+  }
+  if(_codriver->GetXButtonPressed()){
+    _alphaArm->SetState(AlphaArmState::kStowed);
+  }
+}
+
+//   if (!_rawControl){
+//     if(_codriver->GetBButtonPressed()){
+//       _alphaArm->SetState(AlphaArmState::kAmpAngle);
+//       _alphaArm->SetGoal(1);
+//     }
+//  }
+
+// 
\ No newline at end of file
diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp
index 80f253d5..b1bff7a6 100644
--- a/src/main/cpp/Robot.cpp
+++ b/src/main/cpp/Robot.cpp
@@ -3,46 +3,94 @@
 // of the MIT License at the root of this project
 
 #include "Robot.h"
-
-// include units
-#include <units/velocity.h>
+#include "RobotMap.h"
+#include <frc/TimedRobot.h>
+#include <frc/Timer.h>
+#include <frc/controller/RamseteController.h>
+#include <frc/kinematics/DifferentialDriveKinematics.h>
+#include <networktables/DoubleTopic.h>
+#include <networktables/NetworkTable.h>
+#include <networktables/NetworkTableInstance.h>
 #include <units/acceleration.h>
+// #include <units/angle.h>
 #include <units/length.h>
-#include <units/angle.h>
 #include <units/time.h>
+#include <units/velocity.h>
 #include <units/voltage.h>
 
-#include <frc/kinematics/DifferentialDriveKinematics.h>
-#include <frc/controller/RamseteController.h>
-#include <frc/Timer.h>
+#include "behaviour/HasBehaviour.h"
+#include "networktables/NetworkTableInstance.h"
 
 static units::second_t lastPeriodic;
 
 void Robot::RobotInit() {
-  m_chooser.SetDefaultOption("Default Auto", "Default Auto");
+  // shooter = new Shooter(robotmap.shooterSystem.config);
+  // wom::BehaviourScheduler::GetInstance()->Register(shooter);
+  // shooter->SetDefaultBehaviour(
+  //     [this]() { return wom::make<ShooterManualControl>(shooter, &robotmap.controllers.codriver); });
+
+  // sched = wom::BehaviourScheduler::GetInstance();
+  // m_chooser.SetDefaultOption("Default Auto", "Default Auto");
+
+  // m_chooser.SetDefaultOption("Default Auto", "Default Auto");
+
+  // frc::SmartDashboard::PutData("Auto Selector", &m_chooser);
 
-  frc::SmartDashboard::PutData("Auto Selector", &m_chooser);
+  // m_path_chooser.SetDefaultOption("Path1", "paths/output/Path1.wpilib.json");
 
-  m_path_chooser.SetDefaultOption("Path1", "paths/output/Path1.wpilib.json");
+  // m_path_chooser.AddOption("Path1", "paths/output/Path1.wpilib.json");
+  // m_path_chooser.AddOption("Path2", "paths/output/Path2.wpilib.json");
 
-  m_path_chooser.AddOption("Path1", "paths/output/Path1.wpilib.json");
-  m_path_chooser.AddOption("Path2", "paths/output/Path2.wpilib.json");
+  // frc::SmartDashboard::PutData("Path Selector", &m_path_chooser);
 
-  frc::SmartDashboard::PutData("Path Selector", &m_path_chooser);
+  // frc::SmartDashboard::PutData("Field", &m_field);
 
-  frc::SmartDashboard::PutData("Field", &m_field);
+  // simulation_timer = frc::Timer();
 
-  simulation_timer = frc::Timer();
+  // robotmap.swerveBase.gyro->Reset();
+  
 
-  robotmap.swerveBase.gyro->Reset();
+  // _swerveDrive = new wom::SwerveDrive(robotmap.swerveBase.config, frc::Pose2d());
+  // wom::BehaviourScheduler::GetInstance()->Register(_swerveDrive);
+  // _swerveDrive->SetDefaultBehaviour(
+  //     [this]() { return wom::make<wom::ManualDrivebase>(_swerveDrive, &robotmap.controllers.driver); });
 
-  _swerveDrive = new wom::SwerveDrive(robotmap.swerveBase.config, frc::Pose2d());
-  wom::BehaviourScheduler::GetInstance()->Register(_swerveDrive);
-  _swerveDrive->SetDefaultBehaviour(
-      [this]() { return wom::make<wom::ManualDrivebase>(_swerveDrive, &robotmap.controllers.driver); });
+  // _swerveDrive = new wom::SwerveDrive(robotmap.swerveBase.config, frc::Pose2d());
+  // wom::BehaviourScheduler::GetInstance()->Register(_swerveDrive);
+  // _swerveDrive->SetDefaultBehaviour(
+  //     [this]() { return wom::make<wom::ManualDrivebase>(_swerveDrive, &robotmap.controllers.driver); });
 
   // m_driveSim = new wom::TempSimSwerveDrive(&simulation_timer, &m_field);
   // m_driveSim = wom::TempSimSwerveDrive();
+
+  alphaArm = new AlphaArm(&robotmap.alphaArmSystem.config);
+  wom::BehaviourScheduler::GetInstance()->Register(alphaArm);
+  alphaArm->SetDefaultBehaviour(
+      [this]() { return wom::make<AlphaArmManualControl>(alphaArm, &robotmap.controllers.codriver); });
+
+  // robotmap.swerveBase.moduleConfigs[0].turnMotor.encoder->SetEncoderOffset(0_rad);
+  // robotmap.swerveBase.moduleConfigs[1].turnMotor.encoder->SetEncoderOffset(0_rad);
+  // robotmap.swerveBase.moduleConfigs[2].turnMotor.encoder->SetEncoderOffset(0_rad);
+  // robotmap.swerveBase.moduleConfigs[3].turnMotor.encoder->SetEncoderOffset(0_rad);
+
+  // frontLeft = new ctre::phoenix6::hardware::TalonFX(7, "Drivebase");  // front left
+  // frontRight = new ctre::phoenix6::hardware::TalonFX(2, "Drivebase");   // front right
+  // backLeft = new ctre::phoenix6::hardware::TalonFX(6, "Drivebase");   // back left
+  // backRight = new ctre::phoenix6::hardware::TalonFX(4, "Drivebase");  // back right
+  // frontLeft = new ctre::phoenix6::hardware::TalonFX(9, "Drivebase");   // front left
+  // frontRight = new ctre::phoenix6::hardware::TalonFX(1, "Drivebase");   // front right
+  // backLeft = new ctre::phoenix6::hardware::TalonFX(5, "Drivebase");   // back left
+  // backRight = new ctre::phoenix6::hardware::TalonFX(3, "Drivebase");
+  lastPeriodic = wom::now();
+
+  // intake = new Intake(robotmap.intakeSystem.config);
+  // wom::BehaviourScheduler::GetInstance()->Register(intake);
+  // intake->SetDefaultBehaviour(
+  //     [this]() { return wom::make<IntakeManualControl>(intake, robotmap.controllers.codriver); });
+
+  // _vision = new Vision("limelight", FMAP("fmap.fmap"));
+
+  //robotmap->vision = new Vision("limelight", FMAP("fmap.fmap"));
 }
 
 void Robot::RobotPeriodic() {
@@ -51,26 +99,66 @@ void Robot::RobotPeriodic() {
 
   loop.Poll();
   wom::BehaviourScheduler::GetInstance()->Tick();
-
-  _swerveDrive->OnUpdate(dt);
+  //shooter->OnUpdate(dt);
+  //sched->Tick();
+
+  // robotmap.swerveTable.swerveDriveTable->GetEntry("frontLeftEncoder")
+  //     .SetDouble(robotmap.swerveBase.moduleConfigs[0].turnMotor.encoder->GetEncoderPosition().value());
+  // robotmap.swerveTable.swerveDriveTable->GetEntry("frontRightEncoder")
+  //     .SetDouble(robotmap.swerveBase.moduleConfigs[1].turnMotor.encoder->GetEncoderPosition().value());
+  // robotmap.swerveTable.swerveDriveTable->GetEntry("backLeftEncoder")
+  //     .SetDouble(robotmap.swerveBase.moduleConfigs[2].turnMotor.encoder->GetEncoderPosition().value());
+  // robotmap.swerveTable.swerveDriveTable->GetEntry("backRightEncoder")
+  //     .SetDouble(robotmap.swerveBase.moduleConfigs[3].turnMotor.encoder->GetEncoderPosition().value());
+
+  // _swerveDrive->OnUpdate(dt);
+  alphaArm->OnUpdate(dt);
+  //shooter->OnStart();
+  //intake->OnUpdate(dt);
+
+  // _swerveDrive->OnUpdate(dt);
 }
 
 void Robot::AutonomousInit() {
-  // m_driveSim->SetPath(m_path_chooser.GetSelected());
-
   loop.Clear();
   sched->InterruptAll();
-  // _swerveDrive->OnStart();
-}
-void Robot::AutonomousPeriodic() {
-  // m_driveSim->OnUpdate();
 }
+void Robot::AutonomousPeriodic() {}
 
 void Robot::TeleopInit() {
+  loop.Clear();
+  wom::BehaviourScheduler* sched = wom::BehaviourScheduler::GetInstance();
+  sched->InterruptAll();
+
+  // frontLeft->SetVoltage(4_V);
+  // frontRight->SetVoltage(4_V);
+  // backLeft->SetVoltage(4_V);
+  // backRight->SetVoltage(4_V);
+
+  //  FMAP("fmap.fmap");
+
   // _swerveDrive->OnStart();
   // sched->InterruptAll();
+
+  //reimplement when vision is reimplemented
+
+  // _swerveDrive->SetPose(_vision->GetAngleToObject(VisionTargetObjects::kNote).first);
+}
+
+void Robot::TeleopPeriodic() {
+  // if(testdriver.GetLeftY() > 0.05){
+  //   testMotorDown.Set(0.1);
+  //   testMotorUp.Set(0.1);
+  // }
+  // else if (testdriver.GetLeftY() < 0.3){
+  //   testMotorDown.Set(-0.1);
+  //   testMotorUp.Set(-0.1);
+  // }
+  // else{
+  //   testMotorDown.Set(0);
+  //   testMotorUp.Set(0);
+  // }
 }
-void Robot::TeleopPeriodic() {}
 
 void Robot::DisabledInit() {}
 void Robot::DisabledPeriodic() {}
@@ -78,6 +166,28 @@ void Robot::DisabledPeriodic() {}
 void Robot::TestInit() {}
 void Robot::TestPeriodic() {}
 
-void Robot::SimulationInit() {}
+void Robot::SimulationInit() {
+  /*  std::string x = "[";
+    std::string y = "[";
+    // _vision->GetDistanceToTarget(16);
+    for (int i = 1; i < 17; i++) {
+      for (int j = 0; j < 17; j++) {
+        frc::Pose2d pose = _vision->AlignToTarget(i, units::meter_t{j * 0.1}, _swerveDrive);
+        x += std::to_string(pose.X().value()) + ",";
+        y += std::to_string(pose.Y().value()) + ",";
+      }
+    }
+
+    x += "]";
+    y += "]";
+
+    std::cout << x << std::endl;
+    std::cout << y << std::endl; */
+  // std::cout << _vision->TurnToTarget(1, _swerveDrive).Rotation().Degrees().value() << std::endl;
+  //Reimplement when vision is reimplemented
+  // frc::Pose2d pose = _vision->TurnToTarget(2, _swerveDrive);
+  // nt::NetworkTableInstance::GetDefault().GetTable("vision")->PutNumber("rot",
+  //                                                                     pose.Rotation().Degrees().value());
+}
 
-void Robot::SimulationPeriodic() {}
+void Robot::SimulationPeriodic() {}
\ No newline at end of file
diff --git a/src/main/include/AlphaArm.h b/src/main/include/AlphaArm.h
new file mode 100644
index 00000000..b8a4f973
--- /dev/null
+++ b/src/main/include/AlphaArm.h
@@ -0,0 +1,64 @@
+// Copyright (c) 2023-2024 CurtinFRC
+// Open Source Software, you can modify it according to the terms
+// of the MIT License at the root of this project
+
+#pragma once
+#include <frc/DigitalInput.h>
+
+#include "Wombat.h"
+//#include "vision/Vision.h"
+#include "utils/PID.h"
+#include <units/angle.h>
+#include <units/voltage.h>
+#include <frc/controller/PIDController.h>
+
+struct AlphaArmConfig {
+  wom::Gearbox alphaArmGearbox; //up+down
+  wom::Gearbox alphaArmGearbox2;
+  wom::DutyCycleEncoder alphaArmEncoder;
+  //frc::PIDController pidArmConfig;
+  //wom::PIDConfig<units::degree, units::volts> alphaArmPID;
+  std::string path;
+  // Vision *vision;
+};
+
+enum class AlphaArmState {
+  kIdle,
+  kIntakeAngle,
+  kAmpAngle,
+  kSpeakerAngle,
+  kHoldAngle,
+  kStowed,
+  kRaw
+};
+
+class AlphaArm : public behaviour::HasBehaviour {
+ public:
+  AlphaArm(AlphaArmConfig *config);
+
+  void OnUpdate(units::second_t dt);
+  void SetArmRaw(units::volt_t voltage);
+  void SetState(AlphaArmState state);
+  void SetControllerRaw(units::volt_t voltage); 
+  void SetGoal(double goal);
+  AlphaArmConfig GetConfig(); //{ return _config; }
+  frc::PIDController GetPID();
+
+ private:
+  // units::radian_t CalcTargetAngle();
+
+  AlphaArmConfig *_config;
+  AlphaArmState _state = AlphaArmState::kIdle;
+  //wom::utils::PIDController<units::degree, units::volt> _alphaArmPID;
+  //frc::DutyCycleEncoder armEncoder{4};
+  frc::PIDController _pidArm;
+  frc::PIDController _pidArmStates;
+  std::shared_ptr<nt::NetworkTable> _table = nt::NetworkTableInstance::GetDefault().GetTable("AlphaArm");
+  units::volt_t _setAlphaArmVoltage = 0_V;
+
+  units::volt_t _controlledRawVoltage = 0_V;
+  units::volt_t _rawArmVoltage = 0_V;
+  units::volt_t _testRawVoltage = 3_V;
+  double _goal = 0;
+
+};
\ No newline at end of file
diff --git a/src/main/include/AlphaArmBehaviour.h b/src/main/include/AlphaArmBehaviour.h
new file mode 100644
index 00000000..fd553c09
--- /dev/null
+++ b/src/main/include/AlphaArmBehaviour.h
@@ -0,0 +1,28 @@
+// Copyright (c) 2023-2024 CurtinFRC
+// Open Source Software, you can modify it according to the terms
+// of the MIT License at the root of this project
+
+#pragma once
+
+#pragma once
+#include <frc/XboxController.h>
+
+#include "AlphaArm.h"
+#include "Wombat.h"
+
+class AlphaArmManualControl : public behaviour::Behaviour {
+ public:
+  explicit AlphaArmManualControl(AlphaArm* alphaArm, frc::XboxController* codriver);
+  void OnTick(units::second_t dt);
+
+ private:
+  AlphaArm* _alphaArm;
+  frc::XboxController* _codriver;
+  // units::volt_t _rightStick = ((_codriver->GetRightY()>0.05 || _codriver->GetRightY() < -0.05
+  // )?_codriver->GetRightY():0) * 2_V;
+  units::volt_t _setAlphaArmVoltage = 0_V;
+  bool _rawControl; //needs a default
+  bool _gotValue = false;
+  std::shared_ptr<nt::NetworkTable> _table = nt::NetworkTableInstance::GetDefault().GetTable("AlphaArmSubsystem");
+
+};
\ No newline at end of file
diff --git a/src/main/include/Robot.h b/src/main/include/Robot.h
index bc98df2a..012527ef 100644
--- a/src/main/include/Robot.h
+++ b/src/main/include/Robot.h
@@ -3,8 +3,7 @@
 // of the MIT License at the root of this project
 
 #pragma once
-
-#include <frc/Encoder.h>
+// #include <frc/Encoder.h>
 #include <frc/TimedRobot.h>
 #include <frc/Timer.h>
 #include <frc/event/EventLoop.h>
@@ -16,11 +15,15 @@
 
 #include <string>
 
+#include "AlphaArm.h"
+#include "AlphaArmBehaviour.h"
 #include "RobotMap.h"
 #include "Wombat.h"
 
 class Robot : public frc::TimedRobot {
  public:
+  void TestInit() override;
+  void TestPeriodic() override;
   void RobotInit() override;
   void RobotPeriodic() override;
   void AutonomousInit() override;
@@ -29,23 +32,33 @@ class Robot : public frc::TimedRobot {
   void TeleopPeriodic() override;
   void DisabledInit() override;
   void DisabledPeriodic() override;
-  void TestInit() override;
-  void TestPeriodic() override;
   void SimulationInit() override;
   void SimulationPeriodic() override;
 
  private:
-  behaviour::BehaviourScheduler* sched;
   RobotMap robotmap;
+  wom::BehaviourScheduler* sched;
   frc::EventLoop loop;
+  //Shooter* shooter;
+
+  // Intake* intake;
+  // frc::SendableChooser<std::string> m_chooser;
+
+  // frc::Field2d m_field;
 
-  frc::SendableChooser<std::string> m_chooser;
+  // frc::Timer simulation_timer;
 
-  frc::Field2d m_field;
+  // frc::SendableChooser<std::string> m_path_chooser;
 
-  frc::Timer simulation_timer;
+  //wom::SwerveDrive* _swerveDrive;
 
-  frc::SendableChooser<std::string> m_path_chooser;
+  // rev::CANSparkMax testMotorUp{1, rev::CANSparkMax::MotorType::kBrushless};
+  // rev::CANSparkMax testMotorDown{6, rev::CANSparkMax::MotorType::kBrushless};
+ // frc::XboxController testdriver = frc::XboxController(1);
+  AlphaArm* alphaArm;
 
-  wom::SwerveDrive* _swerveDrive;
-};
+  // ctre::phoenix6::hardware::TalonFX *frontLeft;
+  // ctre::phoenix6::hardware::TalonFX *frontRight;
+  // ctre::phoenix6::hardware::TalonFX *backLeft;
+  // ctre::phoenix6::hardware::TalonFX *backRight;
+};
\ No newline at end of file
diff --git a/src/main/include/RobotMap.h b/src/main/include/RobotMap.h
index d5efd27a..c2eba5a4 100644
--- a/src/main/include/RobotMap.h
+++ b/src/main/include/RobotMap.h
@@ -8,11 +8,13 @@
 #include <frc/DoubleSolenoid.h>
 #include <frc/XboxController.h>
 #include <frc/system/plant/DCMotor.h>
+#include <frc/controller/PIDController.h>
 #include <units/angle.h>
 #include <units/length.h>
 
 #include <string>
 
+#include "AlphaArm.h"
 #include <ctre/phoenix6/CANcoder.hpp>
 #include <ctre/phoenix6/Pigeon2.hpp>
 #include <ctre/phoenix6/TalonFX.hpp>
@@ -22,111 +24,148 @@
 struct RobotMap {
   struct Controllers {
     frc::XboxController driver = frc::XboxController(0);
-    frc::XboxController coDriver = frc::XboxController(1);
+    frc::XboxController codriver = frc::XboxController(1);
     frc::XboxController testController = frc::XboxController(2);
   };
   Controllers controllers;
 
-  struct SwerveBase {
-    ctre::phoenix6::hardware::CANcoder frontLeftCancoder{19};
-    ctre::phoenix6::hardware::CANcoder frontRightCancoder{17};
-    ctre::phoenix6::hardware::CANcoder backLeftCancoder{16};
-    ctre::phoenix6::hardware::CANcoder backRightCancoder{18};
-
-    ctre::phoenix6::hardware::Pigeon2* gyro = new ctre::phoenix6::hardware::Pigeon2(20, "Drivebase");
-    wpi::array<ctre::phoenix6::hardware::TalonFX*, 4> turnMotors{
-        new ctre::phoenix6::hardware::TalonFX(7, "Drivebase"),
-        new ctre::phoenix6::hardware::TalonFX(5, "Drivebase"),
-        new ctre::phoenix6::hardware::TalonFX(1, "Drivebase"),
-        new ctre::phoenix6::hardware::TalonFX(3, "Drivebase")};
-    wpi::array<ctre::phoenix6::hardware::TalonFX*, 4> driveMotors{
-        new ctre::phoenix6::hardware::TalonFX(9, "Drivebase"),
-        new ctre::phoenix6::hardware::TalonFX(6, "Drivebase"),
-        new ctre::phoenix6::hardware::TalonFX(2, "Drivebase"),
-        new ctre::phoenix6::hardware::TalonFX(4, "Drivebase")};
-
-    wpi::array<wom::SwerveModuleConfig, 4> moduleConfigs{
-        wom::SwerveModuleConfig{
-            // front left module
-            frc::Translation2d(10.761_in, 9.455_in),
-            wom::Gearbox{driveMotors[0], new wom::TalonFXEncoder(driveMotors[0], 0.0445_m, 6.75),
-                         frc::DCMotor::Falcon500(1).WithReduction(6.75)},
-            wom::Gearbox{turnMotors[0], new wom::CanEncoder(19, 0.0445_m, 4096, 12.8),
-                         frc::DCMotor::Falcon500(1).WithReduction(12.8)},
-            &frontLeftCancoder, 4_in / 2},
-        wom::SwerveModuleConfig{
-            // front right module
-            frc::Translation2d(10.761_in, -9.455_in),
-            wom::Gearbox{driveMotors[1], new wom::TalonFXEncoder(driveMotors[1], 0.0445_m, 6.75),
-                         frc::DCMotor::Falcon500(1).WithReduction(6.75)},
-            wom::Gearbox{turnMotors[1], new wom::CanEncoder(17, 0.0445_m, 4096, 12.8),
-                         frc::DCMotor::Falcon500(1).WithReduction(12.8)},
-            &frontRightCancoder, 4_in / 2},
-        wom::SwerveModuleConfig{
-            // back left module
-            frc::Translation2d(-10.761_in, 9.455_in),
-            wom::Gearbox{driveMotors[2], new wom::TalonFXEncoder(driveMotors[2], 0.0445_m, 6.75),
-                         frc::DCMotor::Falcon500(1).WithReduction(6.75)},
-            wom::Gearbox{turnMotors[2], new wom::CanEncoder(16, 0.0445_m, 4096, 12.8),
-                         frc::DCMotor::Falcon500(1).WithReduction(12.8)},
-            &backRightCancoder, 4_in / 2},
-        wom::SwerveModuleConfig{
-            // back right module
-            frc::Translation2d(-10.761_in, -9.455_in),
-            wom::Gearbox{driveMotors[3], new wom::TalonFXEncoder(driveMotors[3], 0.0445_m, 6.75),
-                         frc::DCMotor::Falcon500(1).WithReduction(6.75)},
-            wom::Gearbox{turnMotors[3], new wom::CanEncoder(18, 0.0445_m, 4096, 12.8),
-                         frc::DCMotor::Falcon500(1).WithReduction(12.8)},
-            &backLeftCancoder, 4_in / 2},
-    };
+//   struct SwerveBase {
+//     ctre::phoenix6::hardware::CANcoder frontLeftCancoder{19, "Drivebase"};
+//     ctre::phoenix6::hardware::CANcoder frontRightCancoder{17, "Driverbase"};
+//     ctre::phoenix6::hardware::CANcoder backLeftCancoder{16, "Drivebase"};
+//     ctre::phoenix6::hardware::CANcoder backRightCancoder{18, "Drivebase"};
+
+//     ctre::phoenix6::hardware::Pigeon2* gyro = new ctre::phoenix6::hardware::Pigeon2(20, "Drivebase");
+//     wpi::array<ctre::phoenix6::hardware::TalonFX*, 4> turnMotors{
+//         new ctre::phoenix6::hardware::TalonFX(7, "Drivebase"),
+//         new ctre::phoenix6::hardware::TalonFX(5, "Drivebase"),
+//         new ctre::phoenix6::hardware::TalonFX(1, "Drivebase"),
+//         new ctre::phoenix6::hardware::TalonFX(3, "Drivebase")};
+//     wpi::array<ctre::phoenix6::hardware::TalonFX*, 4> driveMotors{
+//         new ctre::phoenix6::hardware::TalonFX(9, "Drivebase"),
+//         new ctre::phoenix6::hardware::TalonFX(6, "Drivebase"),
+//         new ctre::phoenix6::hardware::TalonFX(2, "Drivebase"),
+//         new ctre::phoenix6::hardware::TalonFX(4, "Drivebase")};
+
+//     wpi::array<wom::SwerveModuleConfig, 4> moduleConfigs{
+//         wom::SwerveModuleConfig{
+//             // front left module
+//             frc::Translation2d(10.761_in, 9.455_in),
+//             wom::Gearbox{driveMotors[0], new wom::TalonFXEncoder(driveMotors[0], 0.0445_m, 6.75),
+//                          frc::DCMotor::Falcon500(1).WithReduction(6.75)},
+//             wom::Gearbox{turnMotors[0], new wom::CanEncoder(19, 0.0445_m, 4096, 12.8),
+//                          frc::DCMotor::Falcon500(1).WithReduction(12.8)},
+//             &frontLeftCancoder, 4_in / 2},
+//         wom::SwerveModuleConfig{
+//             // front right module
+//             frc::Translation2d(10.761_in, -9.455_in),
+//             wom::Gearbox{driveMotors[1], new wom::TalonFXEncoder(driveMotors[1], 0.0445_m, 6.75),
+//                          frc::DCMotor::Falcon500(1).WithReduction(6.75)},
+//             wom::Gearbox{turnMotors[1], new wom::CanEncoder(17, 0.0445_m, 4096, 12.8),
+//                          frc::DCMotor::Falcon500(1).WithReduction(12.8)},
+//             &frontRightCancoder, 4_in / 2},
+//         wom::SwerveModuleConfig{
+//             // back left module
+//             frc::Translation2d(-10.761_in, 9.455_in),
+//             wom::Gearbox{driveMotors[2], new wom::TalonFXEncoder(driveMotors[2], 0.0445_m, 6.75),
+//                          frc::DCMotor::Falcon500(1).WithReduction(6.75)},
+//             wom::Gearbox{turnMotors[2], new wom::CanEncoder(16, 0.0445_m, 4096, 12.8),
+//                          frc::DCMotor::Falcon500(1).WithReduction(12.8)},
+//             &backRightCancoder, 4_in / 2},
+//         wom::SwerveModuleConfig{
+//             // back right module
+//             frc::Translation2d(-10.761_in, -9.455_in),
+//             wom::Gearbox{driveMotors[3], new wom::TalonFXEncoder(driveMotors[3], 0.0445_m, 6.75),
+//                          frc::DCMotor::Falcon500(1).WithReduction(6.75)},
+//             wom::Gearbox{turnMotors[3], new wom::CanEncoder(18, 0.0445_m, 4096, 12.8),
+//                          frc::DCMotor::Falcon500(1).WithReduction(12.8)},
+//             &backLeftCancoder, 4_in / 2},
+//     };
+
+//     // Setting the PID path and values to be used for SwerveDrive and
+//     // SwerveModules
+//     wom::SwerveModule::angle_pid_conf_t anglePID{
+//         "/drivetrain/pid/angle/config", 2_V / 360_deg, 0.0_V / (100_deg * 1_s),
+//         0_V / (100_deg / 1_s),          1_deg,         0.5_deg / 2_s};
+//     wom::SwerveModule::velocity_pid_conf_t velocityPID{
+//         "/drivetrain/pid/velocity/config",
+//         //  12_V / 4_mps // webers per metre
+//     };
+//     wom::SwerveDriveConfig::pose_angle_conf_t poseAnglePID{
+//         "/drivetrain/pid/pose/angle/config",
+//         180_deg / 1_s / 45_deg,
+//         wom::SwerveDriveConfig::pose_angle_conf_t::ki_t{0.1},
+//         0_deg / 1_deg,
+//         10_deg,
+//         10_deg / 1_s};
+//     wom::SwerveDriveConfig::pose_position_conf_t posePositionPID{
+//         "/drivetrain/pid/pose/position/config",
+//         3_mps / 1_m,
+//         wom::SwerveDriveConfig::pose_position_conf_t::ki_t{0.15},
+//         0_m / 1_m,
+//         20_cm,
+//         10_cm / 1_s,
+//         10_cm};
+
+//     // the config for the whole swerve drive
+//     wom::SwerveDriveConfig config{"/drivetrain",
+//                                   anglePID,
+//                                   velocityPID,
+//                                   moduleConfigs,  // each module
+//                                   gyro,
+//                                   poseAnglePID,
+//                                   posePositionPID,
+//                                   60_kg,  // robot mass (estimate rn)
+//                                   {0.1, 0.1, 0.1},
+//                                   {0.9, 0.9, 0.9}};
+
+//     // current limiting and setting idle mode of modules to brake mode
+//     // SwerveBase() {
+//     //  for (size_t i = 0; i < 4; i++) {
+//     //    turnMotors[i]->ConfigSupplyCurrentLimit(
+//     //        SupplyCurrentLimitConfiguration(true, 15, 15, 0));
+//     //    driveMotors[i]->SetNeutralMode(NeutralMode::Brake);
+//     //    turnMotors[i]->SetNeutralMode(NeutralMode::Brake);
+//     //    driveMotors[i]->SetInverted(true);
+//     //  }
+//     //}
+//   };
+//   SwerveBase swerveBase;
 
-    // Setting the PID path and values to be used for SwerveDrive and
-    // SwerveModules
-    wom::SwerveModule::angle_pid_conf_t anglePID{
-        "/drivetrain/pid/angle/config", 2_V / 360_deg, 0.0_V / (100_deg * 1_s),
-        0_V / (100_deg / 1_s),          1_deg,         0.5_deg / 2_s};
-    wom::SwerveModule::velocity_pid_conf_t velocityPID{
-        "/drivetrain/pid/velocity/config",
-        //  12_V / 4_mps // webers per metre
+
+//     struct SwerveTable {
+//     std::shared_ptr<nt::NetworkTable> swerveDriveTable =
+//         nt::NetworkTableInstance::GetDefault().GetTable("swerve");
+//   };
+//   SwerveTable swerveTable;
+  
+   struct AlphaArmSystem {
+    rev::CANSparkMax alphaArmMotorUp{21, rev::CANSparkMax::MotorType::kBrushless};
+    rev::CANSparkMax alphaArmMotorDown{26, rev::CANSparkMax::MotorType::kBrushless};
+
+    wom::DutyCycleEncoder alphaArmEncoder{3, 0.1_m, 2048, 1};
+    wom::CANSparkMaxEncoder* alphaArmNeoEncoderUp = new wom::CANSparkMaxEncoder(&alphaArmMotorUp, 0.1_m);
+    wom::CANSparkMaxEncoder* alphaArmNeoEncoderDown = new wom::CANSparkMaxEncoder(&alphaArmMotorDown, 0.1_m);
+
+
+    wom::Gearbox alphaArmGearbox{&alphaArmMotorUp, alphaArmNeoEncoderUp, frc::DCMotor::NEO(1)};
+    wom::Gearbox alphaArmGearbox2{&alphaArmMotorDown, alphaArmNeoEncoderDown, frc::DCMotor::NEO(1)};
+
+    //frc::PIDController pidArmConfig{0.1, 0.1, 0.1};
+    
+    // wom::PIDConfig<units::degree, units::volts> alphaArmPIDConfig{
+    //    "/alphaArm/pid/config",
+    //    0.1_V / 30_deg,
+    //    0.1_V / (1_deg * 1_s),
+    //    0.1_V / (1_deg / 1_s),
+    //    5_deg, 
+    //    2_deg / 1_s,
+    //    5_deg};
+// /
+    AlphaArmConfig config{alphaArmGearbox, alphaArmGearbox2, alphaArmEncoder, "/alphaArm"
+    //, &vision
     };
-    wom::SwerveDriveConfig::pose_angle_conf_t poseAnglePID{
-        "/drivetrain/pid/pose/angle/config",
-        180_deg / 1_s / 45_deg,
-        wom::SwerveDriveConfig::pose_angle_conf_t::ki_t{0.1},
-        0_deg / 1_deg,
-        10_deg,
-        10_deg / 1_s};
-    wom::SwerveDriveConfig::pose_position_conf_t posePositionPID{
-        "/drivetrain/pid/pose/position/config",
-        3_mps / 1_m,
-        wom::SwerveDriveConfig::pose_position_conf_t::ki_t{0.15},
-        0_m / 1_m,
-        20_cm,
-        10_cm / 1_s,
-        10_cm};
-
-    // the config for the whole swerve drive
-    wom::SwerveDriveConfig config{"/drivetrain",
-                                  anglePID,
-                                  velocityPID,
-                                  moduleConfigs,  // each module
-                                  gyro,
-                                  poseAnglePID,
-                                  posePositionPID,
-                                  60_kg,  // robot mass (estimate rn)
-                                  {0.1, 0.1, 0.1},
-                                  {0.9, 0.9, 0.9}};
-
-    // current limiting and setting idle mode of modules to brake mode
-    // SwerveBase() {
-    //  for (size_t i = 0; i < 4; i++) {
-    //    turnMotors[i]->ConfigSupplyCurrentLimit(
-    //        SupplyCurrentLimitConfiguration(true, 15, 15, 0));
-    //    driveMotors[i]->SetNeutralMode(NeutralMode::Brake);
-    //    turnMotors[i]->SetNeutralMode(NeutralMode::Brake);
-    //    driveMotors[i]->SetInverted(true);
-    //  }
-    //}
   };
-  SwerveBase swerveBase;
+  AlphaArmSystem alphaArmSystem;
 };
+
diff --git a/wombat/src/main/cpp/utils/Encoder.cpp b/wombat/src/main/cpp/utils/Encoder.cpp
index 3935d82c..2a6e0d35 100644
--- a/wombat/src/main/cpp/utils/Encoder.cpp
+++ b/wombat/src/main/cpp/utils/Encoder.cpp
@@ -22,17 +22,17 @@ double wom::utils::Encoder::GetEncoderTicksPerRotation() const {
 }
 
 void wom::utils::Encoder::ZeroEncoder() {
-  _offset = GetEncoderRawTicks() * 1_rad;
+  // _offtt = GetEncoderRawTicks() * 1_rad;
 }
 
 void wom::utils::Encoder::SetEncoderPosition(units::degree_t position) {
   // units::radian_t offset_turns = position - GetEncoderPosition();
-  units::degree_t offset = position - (GetEncoderRawTicks() * 360 * 1_deg);
-  _offset = offset;
+  // units::degree_t offset = position - (GetEncoderRawTicks() * 360 * 1_deg);
+  // _offset = offset;
   // _offset = -offset_turns;
 }
 
-void wom::utils::Encoder::SetEncoderOffset(units::radian_t offset) {
+void wom::utils::Encoder::SetEncoderOffset(units::radian_t offset) {  // HERE!
   _offset = offset;
   // units::turn_t offset_turns = offset;
   // _offset = offset_turns.value() * GetEncoderTicksPerRotation();
@@ -43,20 +43,21 @@ void wom::utils::Encoder::SetReduction(double reduction) {
 }
 
 units::radian_t wom::utils::Encoder::GetEncoderPosition() {
-  if (_type == 0) {
-    units::turn_t n_turns{GetEncoderTicks() / GetEncoderTicksPerRotation()};
-    return n_turns;
-  } else if (_type == 2) {
-    units::degree_t pos = GetEncoderTicks() * 1_deg;
-    return pos - _offset;
-  } else {
-    units::degree_t pos = GetEncoderTicks() * 1_deg;
-    return pos - _offset;
-  }
+  // if (_type == 0) {
+  //   units::turn_t n_turns{GetEncoderTicks() / GetEncoderTicksPerRotation()};
+  //   return n_turns;
+  // } else if (_type == 2) {
+  //   units::degree_t pos = GetEncoderTicks() * 1_deg;
+  //   return pos;
+  // } else {
+  //   units::degree_t pos = GetEncoderTicks() * 1_deg;
+  //   return pos - _offset;
+  // }
+  return GetEncoderTicks() * 1_rad * (2 * 3.1415);
 }
 
 double wom::utils::Encoder::GetEncoderDistance() {
-  return GetEncoderTicks() * (2 * M_PI) * _wheelRadius.value();
+  return GetEncoderTicks() * (2 * 3.1415) * _wheelRadius.value();
 }
 
 units::radians_per_second_t wom::utils::Encoder::GetEncoderAngularVelocity() {
@@ -66,10 +67,20 @@ units::radians_per_second_t wom::utils::Encoder::GetEncoderAngularVelocity() {
   return n_turns_per_s;
 }
 
+double wom::utils::Encoder::GetVelocityValue() const {
+  // std::cout << "GET VELOCITY: " << GetVelocity() << std::endl;
+  return GetVelocity();
+  // return 0;
+}
+
 double wom::utils::DigitalEncoder::GetEncoderRawTicks() const {
   return _nativeEncoder.Get();
 }
 
+double wom::utils::DigitalEncoder::GetVelocity() const {
+  return 0;
+}
+
 double wom::utils::DigitalEncoder::GetEncoderTickVelocity() const {
   return _nativeEncoder.GetRate();
 }
@@ -77,16 +88,28 @@ double wom::utils::DigitalEncoder::GetEncoderTickVelocity() const {
 wom::utils::CANSparkMaxEncoder::CANSparkMaxEncoder(rev::CANSparkMax* controller, units::meter_t wheelRadius,
                                                    double reduction)
     : wom::utils::Encoder(42, reduction, wheelRadius, 2),
-      _encoder(controller->GetEncoder(rev::SparkRelativeEncoder::Type::kQuadrature)) {}
+      _encoder(controller->GetEncoder(rev::SparkRelativeEncoder::Type::kHallSensor)) {}
 
 double wom::utils::CANSparkMaxEncoder::GetEncoderRawTicks() const {
-  return _encoder.GetPosition() * _reduction;
+  return ((_encoder.GetPosition() * 2 * 3.1415) / 200);
 }
 
 double wom::utils::CANSparkMaxEncoder::GetEncoderTickVelocity() const {
   return _encoder.GetVelocity() * GetEncoderTicksPerRotation() / 60;
 }
 
+double wom::utils::TalonFXEncoder::GetVelocity() const {
+  return _controller->GetVelocity().GetValue().value();
+}
+
+double wom::utils::CanEncoder::GetVelocity() const {
+  return _canEncoder->GetVelocity().GetValue().value();
+}
+
+double wom::utils::DutyCycleEncoder::GetVelocity() const {
+  return 0;
+}
+
 double wom::utils::CANSparkMaxEncoder::GetPosition() const {
   return _encoder.GetPosition();
 }
@@ -109,10 +132,13 @@ double wom::utils::TalonFXEncoder::GetEncoderTickVelocity() const {
 
 wom::utils::DutyCycleEncoder::DutyCycleEncoder(int channel, units::meter_t wheelRadius,
                                                double ticksPerRotation, double reduction)
-    : wom::utils::Encoder(ticksPerRotation, reduction, wheelRadius, 0), _dutyCycleEncoder(channel) {}
+    : wom::utils::Encoder(ticksPerRotation, reduction, wheelRadius, 0) {
+      _dutyCycleEncoder = new frc::DutyCycleEncoder(channel);
+      
+    }
 
 double wom::utils::DutyCycleEncoder::GetEncoderRawTicks() const {
-  return _dutyCycleEncoder.Get().value();
+  return _dutyCycleEncoder->GetAbsolutePosition();
 }
 
 double wom::utils::DutyCycleEncoder::GetEncoderTickVelocity() const {
@@ -121,14 +147,15 @@ double wom::utils::DutyCycleEncoder::GetEncoderTickVelocity() const {
 
 wom::utils::CanEncoder::CanEncoder(int deviceNumber, units::meter_t wheelRadius, double ticksPerRotation,
                                    double reduction, std::string name)
-    : wom::utils::Encoder(ticksPerRotation, reduction, wheelRadius, 1) {
+    : wom::utils::Encoder(ticksPerRotation, 2, wheelRadius, reduction) {
   _canEncoder = new ctre::phoenix6::hardware::CANcoder(deviceNumber, name);
+  // _canEncoder->ConfigAbsoluteEncoderRange(0, 1);
 }
 
 double wom::utils::CanEncoder::GetEncoderRawTicks() const {
-  return _canEncoder->GetAbsolutePosition().GetValue().value();
+  return (_canEncoder->GetAbsolutePosition().GetValue().value() * 2 * 3.14) - _offset.value();
 }
 
 double wom::utils::CanEncoder::GetEncoderTickVelocity() const {
   return _canEncoder->GetVelocity().GetValue().value();
-}
+}
\ No newline at end of file
diff --git a/wombat/src/main/include/utils/Encoder.h b/wombat/src/main/include/utils/Encoder.h
index 62ccc28f..d17ba4e2 100644
--- a/wombat/src/main/include/utils/Encoder.h
+++ b/wombat/src/main/include/utils/Encoder.h
@@ -41,9 +41,13 @@ class Encoder {
   int encoderType = 0;
   double _reduction;
 
+  virtual double GetVelocity() const = 0;
+  double GetVelocityValue() const;
+
+  units::radian_t _offset = 0_rad;  // bad
+
  private:
   double _encoderTicksPerRotation;
-  units::radian_t _offset = 0_rad;
   int _type = 0;
   units::meter_t _wheelRadius;
 };
@@ -58,13 +62,12 @@ class DigitalEncoder : public Encoder {
   double GetEncoderTickVelocity() const override;
 
   double GetPosition() const;
-  double GetVelocity() const;
+  double GetVelocity() const override;
 
  private:
   frc::Encoder _nativeEncoder;
 };
 
-class SimCANSparkMaxEncoder;
 class CANSparkMaxEncoder : public Encoder {
  public:
   explicit CANSparkMaxEncoder(rev::CANSparkMax* controller, units::meter_t wheelRadius, double reduction = 1);
@@ -73,7 +76,7 @@ class CANSparkMaxEncoder : public Encoder {
   double GetEncoderTickVelocity() const override;
 
   double GetPosition() const;
-  double GetVelocity() const;
+  double GetVelocity() const override;
 
  protected:
   rev::SparkRelativeEncoder _encoder;
@@ -87,6 +90,7 @@ class TalonFXEncoder : public Encoder {
 
   double GetEncoderRawTicks() const override;
   double GetEncoderTickVelocity() const override;
+  double GetVelocity() const override;
 
  private:
   ctre::phoenix6::hardware::TalonFX* _controller;
@@ -99,19 +103,21 @@ class DutyCycleEncoder : public Encoder {
 
   double GetEncoderRawTicks() const override;
   double GetEncoderTickVelocity() const override;
+  double GetVelocity() const override;
 
  private:
-  frc::DutyCycleEncoder _dutyCycleEncoder;
+  frc::DutyCycleEncoder* _dutyCycleEncoder;
 };
 
 class CanEncoder : public Encoder {
  public:
   CanEncoder(int deviceNumber, units::meter_t wheelRadius, double ticksPerRotation = 4095,
-             double reduction = 1, std::string name = "Drivebase");
+             double reduction = 6.75, std::string name = "Drivebase");
 
   double GetEncoderRawTicks() const override;
   double GetEncoderTickVelocity() const override;
   double GetAbsoluteEncoderPosition();
+  double GetVelocity() const override;
 
   const double constantValue = 0.0;
 
@@ -119,4 +125,4 @@ class CanEncoder : public Encoder {
   ctre::phoenix6::hardware::CANcoder* _canEncoder;
 };
 }  // namespace utils
-}  // namespace wom
+}  // namespace wom
\ No newline at end of file

From e08e82c0a261bf6a63d30588ac8e90870cb0d725 Mon Sep 17 00:00:00 2001
From: = <paul.hodges@student.education.wa.edu.au>
Date: Sun, 18 Feb 2024 18:20:11 +0800
Subject: [PATCH 165/207] start on pathplanner

---
 .pathplanner/settings.json                    |  12 ++
 Autos/Taxi                                    |   1 +
 Paths/taxi.path                               |   3 +
 pathweaver.json                               |   8 +
 src/main/cpp/Auto.cpp                         |   5 +-
 src/main/cpp/Robot.cpp                        |   4 +-
 src/main/cpp/vision/Vision.cpp                |   2 +-
 src/main/cpp/vision/VisionBehaviours.cpp      |  28 +--
 .../deploy/pathplanner/autos/New Auto.auto    |  38 ++++
 src/main/deploy/pathplanner/navgrid.json      |   1 +
 .../pathplanner/paths/Example Path.path       |  68 +++++++
 src/main/include/RobotMap.h                   |   3 +-
 src/main/include/vision/VisionBehaviours.h    |  41 ++--
 vendordeps/PathplannerLib.json                |  38 ++++
 vendordeps/WPILib-New-Commands.json           |  38 ++++
 vendordeps/photonlib.json                     |   1 -
 .../src/main/cpp/drivetrain/SwerveDrive.cpp   |  10 +
 .../behaviours/SwerveBehaviours.cpp           |   2 +-
 wombat/src/main/cpp/utils/Pathplanner.cpp     | 190 ++++++++++++++++++
 wombat/src/main/include/Wombat.h              |   2 +-
 .../src/main/include/drivetrain/SwerveDrive.h |   4 +
 .../drivetrain/behaviours/SwerveBehaviours.h  |  36 ++--
 wombat/src/main/include/utils/Pathplanner.h   |  99 +++++++++
 wombat/vendordeps/PathplannerLib.json         |  38 ++++
 24 files changed, 608 insertions(+), 64 deletions(-)
 create mode 100644 .pathplanner/settings.json
 create mode 100644 Autos/Taxi
 create mode 100644 Paths/taxi.path
 create mode 100644 pathweaver.json
 create mode 100644 src/main/deploy/pathplanner/autos/New Auto.auto
 create mode 100644 src/main/deploy/pathplanner/navgrid.json
 create mode 100644 src/main/deploy/pathplanner/paths/Example Path.path
 create mode 100644 vendordeps/PathplannerLib.json
 create mode 100644 vendordeps/WPILib-New-Commands.json
 create mode 100644 wombat/vendordeps/PathplannerLib.json

diff --git a/.pathplanner/settings.json b/.pathplanner/settings.json
new file mode 100644
index 00000000..e0540537
--- /dev/null
+++ b/.pathplanner/settings.json
@@ -0,0 +1,12 @@
+{
+  "robotWidth": 0.9,
+  "robotLength": 0.9,
+  "holonomicMode": true,
+  "pathFolders": [],
+  "autoFolders": [],
+  "defaultMaxVel": 3.0,
+  "defaultMaxAccel": 3.0,
+  "defaultMaxAngVel": 540.0,
+  "defaultMaxAngAccel": 720.0,
+  "maxModuleSpeed": 4.5
+}
diff --git a/Autos/Taxi b/Autos/Taxi
new file mode 100644
index 00000000..8b137891
--- /dev/null
+++ b/Autos/Taxi
@@ -0,0 +1 @@
+
diff --git a/Paths/taxi.path b/Paths/taxi.path
new file mode 100644
index 00000000..176b2e82
--- /dev/null
+++ b/Paths/taxi.path
@@ -0,0 +1,3 @@
+X,Y,Tangent X,Tangent Y,Fixed Theta,Reversed,Name
+5.506651295445286,-7.900861866896187,0.0,0.0,true,false,
+6.5729,-13.5677075,0.0,-13.5677075,true,false,
diff --git a/pathweaver.json b/pathweaver.json
new file mode 100644
index 00000000..1c8f1fd3
--- /dev/null
+++ b/pathweaver.json
@@ -0,0 +1,8 @@
+{
+  "lengthUnit": "FOOT",
+  "exportUnit": "Always Meters",
+  "maxVelocity": 10.0,
+  "maxAcceleration": 60.0,
+  "trackWidth": 2.0,
+  "gameName": "Crescendo"
+}
diff --git a/src/main/cpp/Auto.cpp b/src/main/cpp/Auto.cpp
index 4d22e98b..a90fd17f 100644
--- a/src/main/cpp/Auto.cpp
+++ b/src/main/cpp/Auto.cpp
@@ -4,13 +4,14 @@
 
 #include "Auto.h"
 
+#include "drivetrain/behaviours/SwerveBehaviours.h"
+#include "utils/Pathplanner.h"
+
 std::shared_ptr<behaviour::Behaviour> autos::Taxi(wom::drivetrain::SwerveDrive* _swerveDrive,
                                                   Shooter* _shooter, Intake* _intake, AlphaArm* _alphaArm) {
   return behaviour::make<ArmToSetPoint>(_alphaArm, 0_deg);
   behaviour::make<AutoShoot>(_shooter);
   behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
-  behaviour::make<wom::drivetrain::behaviours::DrivebasePoseBehaviour>(
-      _swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false);
 }
 // Shoots starting note then moves out of starting position.
 
diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp
index 4574f139..6e6a4940 100644
--- a/src/main/cpp/Robot.cpp
+++ b/src/main/cpp/Robot.cpp
@@ -217,8 +217,8 @@ void Robot::AutonomousPeriodic() {
 }
 
 // void Robot::AutonomousPeriodic() {
-  // m_driveSim->OnUpdate();
-  // wom::make<AlignToAprilTag>(_vision, VisionTarget::kRedChain3, _swerveDrive);
+// m_driveSim->OnUpdate();
+// wom::make<AlignToAprilTag>(_vision, VisionTarget::kRedChain3, _swerveDrive);
 // }
 
 void Robot::TeleopInit() {
diff --git a/src/main/cpp/vision/Vision.cpp b/src/main/cpp/vision/Vision.cpp
index 7e2affd6..acd3dcbb 100644
--- a/src/main/cpp/vision/Vision.cpp
+++ b/src/main/cpp/vision/Vision.cpp
@@ -331,7 +331,7 @@ frc::Pose2d Vision::TurnToTarget(VisionTarget target, wom::SwerveDrive* swerveDr
 
   units::degree_t angle = GetDistanceToTarget(target).second;
 
-  //frc::Pose2d current_pose = _limelight->GetPose().ToPose2d();
+  // frc::Pose2d current_pose = _limelight->GetPose().ToPose2d();
 
   frc::Pose2d current_pose = swerveDrive->GetPose();
 
diff --git a/src/main/cpp/vision/VisionBehaviours.cpp b/src/main/cpp/vision/VisionBehaviours.cpp
index 9ffeb63e..03bbadf1 100644
--- a/src/main/cpp/vision/VisionBehaviours.cpp
+++ b/src/main/cpp/vision/VisionBehaviours.cpp
@@ -8,7 +8,8 @@
 #include "units/angle.h"
 #include "vision/Vision.h"
 
-AlignToAprilTag::AlignToAprilTag(Vision* vision, VisionTarget target, wom::SwerveDrive* swerveDrive, units::meter_t offset)
+AlignToAprilTag::AlignToAprilTag(Vision* vision, VisionTarget target, wom::SwerveDrive* swerveDrive,
+                                 units::meter_t offset)
     : _vision(vision), _swerveDrive(swerveDrive), _target(target), _offset(offset) {}
 
 void AlignToAprilTag::OnTick(units::second_t dt) {
@@ -31,23 +32,25 @@ void TurnToAprilTag::OnTick(units::second_t dt) {
 }
 
 LockOnToTarget::LockOnToTarget(Vision* vision, VisionTargetObjects target, wom::SwerveDrive* swerveDrive)
-    : _vision(vision), _target(target), _camera(nullptr), _swerveDrive(swerveDrive), _type(VisionType::kLimelight) {}
+    : _vision(vision),
+      _target(target),
+      _camera(nullptr),
+      _swerveDrive(swerveDrive),
+      _type(VisionType::kLimelight) {}
 
-LockOnToTarget::LockOnToTarget(wom::PhotonVision* vision, Vision* limelight, wom::SwerveDrive* swerveDrive) 
-  : _camera(vision), _swerveDrive(swerveDrive), _vision(limelight), _type(VisionType::kGenericCamera) {}
+LockOnToTarget::LockOnToTarget(wom::PhotonVision* vision, Vision* limelight, wom::SwerveDrive* swerveDrive)
+    : _camera(vision), _swerveDrive(swerveDrive), _vision(limelight), _type(VisionType::kGenericCamera) {}
 
 void LockOnToTarget::OnTick(units::second_t dt) {
   units::degree_t angle;
 
   switch (_type) {
-    case VisionType::kLimelight:
-      {
-        angle = _vision->LockOn(_target);
-      }
-    case VisionType::kGenericCamera:
-      {
-        angle = units::degree_t{_camera->GetTargetPitch(_camera->GetTarget())};
-      }
+    case VisionType::kLimelight: {
+      angle = _vision->LockOn(_target);
+    }
+    case VisionType::kGenericCamera: {
+      angle = units::degree_t{_camera->GetTargetPitch(_camera->GetTarget())};
+    }
   }
 
   frc::Pose2d pose = frc::Pose2d(_swerveDrive->GetPose().X(), _swerveDrive->GetPose().Y(), angle);
@@ -58,4 +61,3 @@ void LockOnToTarget::OnTick(units::second_t dt) {
     SetDone();
   }
 }
-
diff --git a/src/main/deploy/pathplanner/autos/New Auto.auto b/src/main/deploy/pathplanner/autos/New Auto.auto
new file mode 100644
index 00000000..f023c012
--- /dev/null
+++ b/src/main/deploy/pathplanner/autos/New Auto.auto	
@@ -0,0 +1,38 @@
+{
+  "version": 1.0,
+  "startingPose": {
+    "position": {
+      "x": 2,
+      "y": 2
+    },
+    "rotation": 0
+  },
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Example Path"
+          }
+        },
+        {
+          "type": "sequential",
+          "data": {
+            "commands": [
+              {
+                "type": "named",
+                "data": {
+                  "name": "hh"
+                }
+              }
+            ]
+          }
+        }
+      ]
+    }
+  },
+  "folder": null,
+  "choreoAuto": false
+}
diff --git a/src/main/deploy/pathplanner/navgrid.json b/src/main/deploy/pathplanner/navgrid.json
new file mode 100644
index 00000000..d49bb0f7
--- /dev/null
+++ b/src/main/deploy/pathplanner/navgrid.json
@@ -0,0 +1 @@
+{"field_size":{"x":16.54,"y":8.21},"nodeSizeMeters":0.3,"grid":[[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true],[true,true,true,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true],[true,true,true,true,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true]]}
diff --git a/src/main/deploy/pathplanner/paths/Example Path.path b/src/main/deploy/pathplanner/paths/Example Path.path
new file mode 100644
index 00000000..de5528c0
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/Example Path.path	
@@ -0,0 +1,68 @@
+{
+  "version": 1.0,
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 1.7121880748754352,
+        "y": 5.5040931765173085
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 2.7121880748754363,
+        "y": 5.5040931765173085
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 5.108330673152073,
+        "y": 7.3426945142050055
+      },
+      "prevControl": {
+        "x": 4.108330673152073,
+        "y": 7.3426945142050055
+      },
+      "nextControl": {
+        "x": 6.108330673152073,
+        "y": 7.3426945142050055
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 5.471366606071299,
+        "y": 2.8223116075333428
+      },
+      "prevControl": {
+        "x": 6.832113092186237,
+        "y": 3.284889651091713
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 0,
+    "rotateFast": false
+  },
+  "reversed": false,
+  "folder": null,
+  "previewStartingState": {
+    "rotation": 0,
+    "velocity": 0
+  },
+  "useDefaultConstraints": true
+}
diff --git a/src/main/include/RobotMap.h b/src/main/include/RobotMap.h
index b14d0194..cd526253 100644
--- a/src/main/include/RobotMap.h
+++ b/src/main/include/RobotMap.h
@@ -86,7 +86,6 @@ struct RobotMap {
   };
   Shooter shooterSystem;
 
-
   //   struct AlphaArmSystem {
   //     rev::CANSparkMax alphaArmMotor{12, rev::CANSparkMax::MotorType::kBrushless};
   //     wom::CANSparkMaxEncoder* armEncoder = new wom::CANSparkMaxEncoder(&alphaArmMotor, 0.02_m);
@@ -202,7 +201,7 @@ struct RobotMap {
   // };
   // Arm arm;
 
-struct SwerveBase {
+  struct SwerveBase {
     ctre::phoenix6::hardware::CANcoder frontLeftCancoder{16, "Drivebase"};
     ctre::phoenix6::hardware::CANcoder frontRightCancoder{18, "Drivebase"};
     ctre::phoenix6::hardware::CANcoder backLeftCancoder{17, "Drivebase"};
diff --git a/src/main/include/vision/VisionBehaviours.h b/src/main/include/vision/VisionBehaviours.h
index dedc52b7..c10a092d 100644
--- a/src/main/include/vision/VisionBehaviours.h
+++ b/src/main/include/vision/VisionBehaviours.h
@@ -11,16 +11,12 @@
 #include "units/time.h"
 #include "vision/Vision.h"
 
-#include "Wombat.h"
-
-enum class VisionType {
-  kLimelight = 0,
-  kGenericCamera = 1
-};
+enum class VisionType { kLimelight = 0, kGenericCamera = 1 };
 
 class AlignToAprilTag : public behaviour::Behaviour {
  public:
-  explicit AlignToAprilTag(Vision* vision, VisionTarget target, wom::SwerveDrive* swerveDrive, units::meter_t offset = 0_m);
+  explicit AlignToAprilTag(Vision* vision, VisionTarget target, wom::SwerveDrive* swerveDrive,
+                           units::meter_t offset = 0_m);
 
   void OnTick(units::second_t dt);
 
@@ -32,30 +28,29 @@ class AlignToAprilTag : public behaviour::Behaviour {
 };
 
 class TurnToAprilTag : public behaviour::Behaviour {
-  public:
-    explicit TurnToAprilTag(Vision* vision, VisionTarget target, wom::SwerveDrive* swerveDrive);
-  
-    void OnTick(units::second_t dt);
+ public:
+  explicit TurnToAprilTag(Vision* vision, VisionTarget target, wom::SwerveDrive* swerveDrive);
 
-  private:
-    Vision* _vision;
+  void OnTick(units::second_t dt);
+
+ private:
+  Vision* _vision;
   wom::SwerveDrive* _swerveDrive;
   VisionTarget _target;
 };
 
 class LockOnToTarget : public behaviour::Behaviour {
-  public:
-    explicit LockOnToTarget(Vision* vision, VisionTargetObjects target, wom::SwerveDrive* swerveDrive);
-    explicit LockOnToTarget(wom::PhotonVision* camera, Vision* limelight, wom::SwerveDrive* swerveDrive);
+ public:
+  explicit LockOnToTarget(Vision* vision, VisionTargetObjects target, wom::SwerveDrive* swerveDrive);
+  explicit LockOnToTarget(wom::PhotonVision* camera, Vision* limelight, wom::SwerveDrive* swerveDrive);
 
-    void OnTick(units::second_t dt);
+  void OnTick(units::second_t dt);
 
-  private:
-    Vision* _vision;
-    wom::PhotonVision* _camera;
+ private:
+  Vision* _vision;
+  wom::PhotonVision* _camera;
 
-    VisionTargetObjects _target;
+  VisionTargetObjects _target;
   wom::SwerveDrive* _swerveDrive;
-  VisionType _type; 
+  VisionType _type;
 };
-
diff --git a/vendordeps/PathplannerLib.json b/vendordeps/PathplannerLib.json
new file mode 100644
index 00000000..48b56188
--- /dev/null
+++ b/vendordeps/PathplannerLib.json
@@ -0,0 +1,38 @@
+{
+    "fileName": "PathplannerLib.json",
+    "name": "PathplannerLib",
+    "version": "2024.2.3",
+    "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786",
+    "frcYear": "2024",
+    "mavenUrls": [
+        "https://3015rangerrobotics.github.io/pathplannerlib/repo"
+    ],
+    "jsonUrl": "https://3015rangerrobotics.github.io/pathplannerlib/PathplannerLib.json",
+    "javaDependencies": [
+        {
+            "groupId": "com.pathplanner.lib",
+            "artifactId": "PathplannerLib-java",
+            "version": "2024.2.3"
+        }
+    ],
+    "jniDependencies": [],
+    "cppDependencies": [
+        {
+            "groupId": "com.pathplanner.lib",
+            "artifactId": "PathplannerLib-cpp",
+            "version": "2024.2.3",
+            "libName": "PathplannerLib",
+            "headerClassifier": "headers",
+            "sharedLibrary": false,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "osxuniversal",
+                "linuxathena",
+                "linuxarm32",
+                "linuxarm64"
+            ]
+        }
+    ]
+}
diff --git a/vendordeps/WPILib-New-Commands.json b/vendordeps/WPILib-New-Commands.json
new file mode 100644
index 00000000..67bf3898
--- /dev/null
+++ b/vendordeps/WPILib-New-Commands.json
@@ -0,0 +1,38 @@
+{
+  "fileName": "WPILibNewCommands.json",
+  "name": "WPILib-New-Commands",
+  "version": "1.0.0",
+  "uuid": "111e20f7-815e-48f8-9dd6-e675ce75b266",
+  "frcYear": "2024",
+  "mavenUrls": [],
+  "jsonUrl": "",
+  "javaDependencies": [
+    {
+      "groupId": "edu.wpi.first.wpilibNewCommands",
+      "artifactId": "wpilibNewCommands-java",
+      "version": "wpilib"
+    }
+  ],
+  "jniDependencies": [],
+  "cppDependencies": [
+    {
+      "groupId": "edu.wpi.first.wpilibNewCommands",
+      "artifactId": "wpilibNewCommands-cpp",
+      "version": "wpilib",
+      "libName": "wpilibNewCommands",
+      "headerClassifier": "headers",
+      "sourcesClassifier": "sources",
+      "sharedLibrary": true,
+      "skipInvalidPlatforms": true,
+      "binaryPlatforms": [
+        "linuxathena",
+        "linuxarm32",
+        "linuxarm64",
+        "windowsx86-64",
+        "windowsx86",
+        "linuxx86-64",
+        "osxuniversal"
+      ]
+    }
+  ]
+}
diff --git a/vendordeps/photonlib.json b/vendordeps/photonlib.json
index 4c80f3fe..6a4780c7 100644
--- a/vendordeps/photonlib.json
+++ b/vendordeps/photonlib.json
@@ -40,4 +40,3 @@
         }
     ]
 }
-
diff --git a/wombat/src/main/cpp/drivetrain/SwerveDrive.cpp b/wombat/src/main/cpp/drivetrain/SwerveDrive.cpp
index 701fae07..71ef5d69 100644
--- a/wombat/src/main/cpp/drivetrain/SwerveDrive.cpp
+++ b/wombat/src/main/cpp/drivetrain/SwerveDrive.cpp
@@ -16,6 +16,7 @@
 #include <iostream>
 
 #include "frc/MathUtil.h"
+#include "frc/kinematics/ChassisSpeeds.h"
 #include "utils/Util.h"
 #include "wpimath/MathShared.h"
 
@@ -513,5 +514,14 @@ frc::Pose2d SwerveDrive::GetPose() {
 void SwerveDrive::AddVisionMeasurement(frc::Pose2d pose, units::second_t timestamp) {
   _poseEstimator.AddVisionMeasurement(pose, timestamp);
 }
+
+FieldRelativeSpeeds SwerveDrive::GetFieldRelativeSpeeds() {
+  return _target_fr_speeds;
+}
+
+frc::ChassisSpeeds SwerveDrive::GetChassisSpeeds() {
+  return _target_speed;
+}
+
 }  // namespace drivetrain
 }  // namespace wom
diff --git a/wombat/src/main/cpp/drivetrain/behaviours/SwerveBehaviours.cpp b/wombat/src/main/cpp/drivetrain/behaviours/SwerveBehaviours.cpp
index db6a035d..6454c766 100644
--- a/wombat/src/main/cpp/drivetrain/behaviours/SwerveBehaviours.cpp
+++ b/wombat/src/main/cpp/drivetrain/behaviours/SwerveBehaviours.cpp
@@ -209,7 +209,7 @@ void wom::drivetrain::behaviours::TempSimSwerveDrive::SetPath(std::string path)
   current_trajectory_state_table =
       nt::NetworkTableInstance::GetDefault().GetTable("current_trajectory_state");
 
-  current_trajectory = m_pathplanner.getTrajectory(path);
+  // current_trajectory = m_pathplanner.getTrajectory(path);
   m_driveSim.SetPose(current_trajectory.Sample(0_s).pose);
   m_timer->Reset();
   m_timer->Start();
diff --git a/wombat/src/main/cpp/utils/Pathplanner.cpp b/wombat/src/main/cpp/utils/Pathplanner.cpp
index cc4752ae..a1170655 100644
--- a/wombat/src/main/cpp/utils/Pathplanner.cpp
+++ b/wombat/src/main/cpp/utils/Pathplanner.cpp
@@ -4,8 +4,107 @@
 
 #include "utils/Pathplanner.h"
 
+#include <frc/DriverStation.h>
+#include <pathplanner/lib/auto/AutoBuilder.h>
+#include <pathplanner/lib/util/HolonomicPathFollowerConfig.h>
+#include <pathplanner/lib/util/PIDConstants.h>
+#include <pathplanner/lib/util/ReplanningConfig.h>
+
+#include <algorithm>
+#include <array>
+#include <fstream>
+#include <string>
+#include <utility>
+#include <vector>
+
+#include "behaviour/Behaviour.h"
+#include "drivetrain/SwerveDrive.h"
+#include "drivetrain/behaviours/SwerveBehaviours.h"
+#include "frc/Filesystem.h"
+#include "frc/geometry/Pose2d.h"
+#include "frc/kinematics/ChassisSpeeds.h"
+#include "pathplanner/lib/path/PathPlannerPath.h"
+#include "units/time.h"
+
 using namespace wom;
 
+utils::Commands::Commands(std::initializer_list<std::pair<std::string, std::function<void(std::vector<std::any>)>>> init) : _commands{} {
+  for (const auto& pair : init) {
+    _commands.emplace_back(pair.first, std::vector<std::type_index>{typeid(std::vector<std::any>)}, pair.second);
+  }
+}
+
+void utils::Commands::Execute(std::string command, std::vector<std::any> args) {
+  auto it = std::find_if(_commands.begin(), _commands.end(),
+                         [command](const auto& cmd) { return cmd.name == command; });
+
+  if (it != _commands.end()) {
+    if (IsValid(command, args)) {
+      it->function(args);  
+    } else {
+      std::cerr << "Invalid arguments for command: " << command << std::endl;
+    }
+  } else {
+    std::cerr << "Unknown command: " << command << std::endl;
+  }
+}
+
+void utils::Commands::Run(std::string command) {
+  command.erase(std::remove_if(command.begin(), command.end(), ::isspace), command.end());
+
+  size_t openParenPos = command.find('(');
+  size_t closeParenPos = command.find(')');
+
+  if (openParenPos != std::string::npos && closeParenPos != std::string::npos &&
+      closeParenPos > openParenPos) {
+    std::string commandName = command.substr(0, openParenPos);
+    std::string argString = command.substr(openParenPos + 1, closeParenPos - openParenPos - 1);
+
+    std::vector<std::any> args = ParseArguments(argString);
+
+    Execute(commandName, args);
+  } else {
+    std::cerr << "Invalid command format: " << command << std::endl;
+  }
+}
+
+std::vector<std::any> utils::Commands::ParseArguments(const std::string& argString) {
+  std::vector<std::any> args;
+
+  std::istringstream iss(argString);
+  std::string token;
+
+  while (std::getline(iss, token, ',')) {
+    token.erase(std::remove_if(token.begin(), token.end(), ::isspace), token.end());
+    args.push_back(std::any(token));
+  }
+
+  return args;
+}
+
+bool utils::Commands::IsValid(std::string command, const std::vector<std::any> args) {
+  auto it = std::find_if(_commands.begin(), _commands.end(),
+                         [command](const auto& cmd) { return cmd.name == command; });
+
+  if (it != _commands.end()) {
+    const auto& expectedTypes = it->argumentTypes;
+
+    if (args.size() != expectedTypes.size()) {
+      return false;
+    }
+
+    for (size_t i = 0; i < args.size(); ++i) {
+      if (args[i].type() != expectedTypes[i]) {
+        return false;
+      }
+    }
+
+    return true;
+  } else {
+    return false;
+  }
+}
+
 utils::Pathplanner::Pathplanner() {}
 
 frc::Trajectory utils::Pathplanner::getTrajectory(std::string_view path) {
@@ -16,3 +115,94 @@ frc::Trajectory utils::Pathplanner::getTrajectory(std::string_view path) {
     return getTrajectory(path);
   }
 }
+
+utils::FollowPath::FollowPath(drivetrain::SwerveDrive* swerve, std::string path, bool flip)
+    : _swerve(swerve) {
+  _path = pathplanner::PathPlannerPath::fromPathFile(path);
+
+  if (flip) {
+    _path = _path->flipPath();
+  }
+
+  _poses = _path->getPathPoses();
+}
+
+void utils::FollowPath::OnTick(units::second_t dt) {
+  _swerve->SetPose(_poses[_currentPose]);
+
+  if (_swerve->IsAtSetPose()) {
+    if (_currentPose == static_cast<int>(_poses.size())) {
+      SetDone();
+    } else {
+      _currentPose++;
+    }
+  }
+};
+
+utils::RunCommand::RunCommand(std::string command, Commands commands) : _command(command), _commands(commands) {}
+
+void utils::RunCommand::OnTick(units::second_t dt) {
+  _commands.Run(_command);
+
+  SetDone();
+}
+
+utils::AutoBuilder::AutoBuilder(drivetrain::SwerveDrive* swerve, std::function<bool()> shouldFlipPath,
+                                std::string auto_routine, Commands commands)
+    : _swerve(swerve), _flip(shouldFlipPath()), _path(auto_routine), _commandsList(commands) {
+  SetAuto(auto_routine);
+};
+
+void utils::AutoBuilder::SetAuto(std::string_view path) {
+  fs::path deploy_directory = frc::filesystem::GetDeployDirectory();
+  fs::path location = deploy_directory / path;
+
+  std::ifstream file(location);
+
+  wpi::json j;
+
+  file >> j;
+
+  _currentPath = &j;
+  _startingPose = &j["startingPose"];
+  _commands = &j["command"]["data"]["commands"];
+}
+
+void utils::AutoBuilder::Invert() {}
+
+frc::Pose2d utils::AutoBuilder::JSONPoseToPose2d(wpi::json j) {
+  return frc::Pose2d(units::meter_t{j["position"]["x"]}, units::meter_t{j["position"]["y"]},
+                     units::degree_t{j["rotation"]});
+}
+
+std::shared_ptr<behaviour::Behaviour> utils::AutoBuilder::GetAutoPath() {
+  behaviour::Behaviour::ptr path = behaviour::make<drivetrain::behaviours::DrivebasePoseBehaviour>(
+      _swerve, JSONPoseToPose2d(*_startingPose));
+
+  for (auto& command : *_commands) {
+    if (command["type"] == "path") {
+      path << behaviour::make<FollowPath>(_swerve, command["data"]["pathName"], _flip);
+    } else if (command["type"] == "command") {
+      path << behaviour::make<RunCommand>(command["data"]["name"], _commandsList);
+    }
+  }
+
+  return path;
+}
+
+utils::SwerveAutoBuilder::SwerveAutoBuilder(wom::drivetrain::SwerveDrive* swerve, std::string name)
+    : _swerve(swerve), _name(name) {
+  _builder = new AutoBuilder(
+      swerve,
+      []() {
+        auto alliance = frc::DriverStation::GetAlliance();
+
+        if (alliance) {
+          return alliance.value() == frc::DriverStation::Alliance::kRed;
+        }
+
+        return false;
+      },
+      "taxi",
+      Commands());
+}
diff --git a/wombat/src/main/include/Wombat.h b/wombat/src/main/include/Wombat.h
index 4f34e6c0..6f7d00bb 100644
--- a/wombat/src/main/include/Wombat.h
+++ b/wombat/src/main/include/Wombat.h
@@ -15,8 +15,8 @@
 #include "utils/PID.h"
 #include "utils/RobotStartup.h"
 #include "utils/Util.h"
-#include "vision/Limelight.h"
 #include "vision/Camera.h"
+#include "vision/Limelight.h"
 
 namespace wom {
 using namespace wom;
diff --git a/wombat/src/main/include/drivetrain/SwerveDrive.h b/wombat/src/main/include/drivetrain/SwerveDrive.h
index d8b8e482..c39fe223 100644
--- a/wombat/src/main/include/drivetrain/SwerveDrive.h
+++ b/wombat/src/main/include/drivetrain/SwerveDrive.h
@@ -30,6 +30,7 @@
 #include <ctre/phoenix6/Pigeon2.hpp>
 
 #include "behaviour/HasBehaviour.h"
+#include "frc/kinematics/ChassisSpeeds.h"
 #include "utils/Gearbox.h"
 #include "utils/PID.h"
 
@@ -216,6 +217,9 @@ class SwerveDrive : public behaviour::HasBehaviour {
 
   SwerveDriveConfig& GetConfig() { return _config; }
 
+  FieldRelativeSpeeds GetFieldRelativeSpeeds();
+  frc::ChassisSpeeds GetChassisSpeeds();
+
  private:
   SwerveDriveConfig _config;
   SwerveDriveState _state = SwerveDriveState::kIdle;
diff --git a/wombat/src/main/include/drivetrain/behaviours/SwerveBehaviours.h b/wombat/src/main/include/drivetrain/behaviours/SwerveBehaviours.h
index d9f844a5..5d61d37a 100644
--- a/wombat/src/main/include/drivetrain/behaviours/SwerveBehaviours.h
+++ b/wombat/src/main/include/drivetrain/behaviours/SwerveBehaviours.h
@@ -17,7 +17,7 @@
 
 #include "behaviour/Behaviour.h"
 #include "drivetrain/SwerveDrive.h"
-#include "utils/Pathplanner.h"
+// #include "utils/Pathplanner.h"
 
 namespace wom {
 namespace drivetrain {
@@ -122,22 +122,22 @@ class GoToPose : public behaviour::Behaviour {
   frc::Pose3d _pose;
 };
 
-class FollowTrajectory : public behaviour::Behaviour {
- public:
-  FollowTrajectory(wom::drivetrain::SwerveDrive* swerve, wom::utils::Pathplanner* pathplanner,
-                   std::string path);
-
-  void OnTick(units::second_t dt) override;
-
-  void OnStart() override;
-
- private:
-  wom::utils::Pathplanner* _pathplanner;
-  std::string _path;
-  wom::drivetrain::SwerveDrive* _swerve;
-  frc::Trajectory _trajectory;
-  frc::Timer m_timer;
-};
+// class FollowTrajectory : public behaviour::Behaviour {
+//  public:
+//   FollowTrajectory(wom::drivetrain::SwerveDrive* swerve, wom::utils::Pathplanner* pathplanner,
+//                    std::string path);
+//
+//   void OnTick(units::second_t dt) override;
+//
+//   void OnStart() override;
+//
+//  private:
+//   wom::utils::Pathplanner* _pathplanner;
+//   std::string _path;
+//   wom::drivetrain::SwerveDrive* _swerve;
+//   frc::Trajectory _trajectory;
+//   frc::Timer m_timer;
+// };
 
 class TempSimSwerveDrive {
  public:
@@ -166,7 +166,7 @@ class TempSimSwerveDrive {
       // l and r position: 0.005 m
       {0.001, 0.001, 0.001, 0.1, 0.1, 0.005, 0.005}};
 
-  wom::utils::Pathplanner m_pathplanner;
+  // wom::utils::Pathplanner m_pathplanner;
 
   frc::Trajectory current_trajectory;
 
diff --git a/wombat/src/main/include/utils/Pathplanner.h b/wombat/src/main/include/utils/Pathplanner.h
index 48e760ae..440984e6 100644
--- a/wombat/src/main/include/utils/Pathplanner.h
+++ b/wombat/src/main/include/utils/Pathplanner.h
@@ -8,8 +8,46 @@
 #include <frc/trajectory/TrajectoryUtil.h>
 #include <wpi/fs.h>
 
+#include <any>
+#include <string>
+#include <typeindex>
+
+#include "behaviour/Behaviour.h"
+#include "drivetrain/SwerveDrive.h"
+#include "drivetrain/behaviours/SwerveBehaviours.h"
+#include "pathplanner/lib/path/PathPlannerPath.h"
+#include "pathplanner/lib/util/PIDConstants.h"
+#include "wpi/json_fwd.h"
+
 namespace wom {
 namespace utils {
+
+class Commands {
+ public:
+  
+  Commands(std::initializer_list<std::pair<std::string, std::function<void(std::vector<std::any>)>>> init);
+
+  Commands() = default;
+  
+  void Execute(std::string command, std::vector<std::any> args);
+
+  void Run(std::string command);
+
+  bool IsValid(std::string command, std::vector<std::any> args);
+
+ private:
+  struct CommandData {
+    std::string name;
+    std::vector<std::type_index> argumentTypes;
+    std::function<void(std::vector<std::any>)> function;
+  };
+
+  std::vector<CommandData> _commands;  // Changed name to _commands
+
+  // Helper function to parse command arguments from a string
+  std::vector<std::any> ParseArguments(const std::string& argString);
+};
+
 class Pathplanner {
  public:
   Pathplanner();
@@ -19,5 +57,66 @@ class Pathplanner {
  private:
   fs::path deploy_directory = frc::filesystem::GetDeployDirectory();
 };
+
+class FollowPath : public behaviour::Behaviour {
+ public:
+  FollowPath(drivetrain::SwerveDrive* swerve, std::string path, bool flip = false);
+
+  void OnTick(units::time::second_t dt) override;
+
+ private:
+  std::string _pathName;
+  std::shared_ptr<pathplanner::PathPlannerPath> _path;
+  std::vector<frc::Pose2d> _poses;
+
+  drivetrain::SwerveDrive* _swerve;
+
+  int _currentPose = 0;
+};
+
+class RunCommand : public behaviour::Behaviour {
+public:
+  RunCommand(std::string command, Commands commands);
+
+  void OnTick(units::time::second_t dt) override;
+
+  private:
+  std::string _command;
+  Commands _commands;
+};
+
+class AutoBuilder {
+ public:
+  AutoBuilder(drivetrain::SwerveDrive* swerve, std::function<bool()> shouldFlipPath, std::string autoRoutine, Commands commands);
+
+  void Invert();
+  void SetAuto(std::string_view path);
+
+  std::shared_ptr<behaviour::Behaviour> GetAutoPath();
+  behaviour::Behaviour GetPath(std::string path);
+
+  frc::Pose2d JSONPoseToPose2d(wpi::json j);
+
+ private:
+  std::string _path;
+  bool _flip;
+  drivetrain::SwerveDrive* _swerve;
+
+  Commands _commandsList;
+
+  wpi::json* _currentPath;
+  wpi::json* _startingPose;
+  wpi::json* _commands;
+};
+
+class SwerveAutoBuilder {
+ public:
+  SwerveAutoBuilder(wom::drivetrain::SwerveDrive* swerve, std::string name);
+
+ private:
+  AutoBuilder* _builder;
+  wom::drivetrain::SwerveDrive* _swerve;
+  std::string _name;
+};
 }  // namespace utils
 }  // namespace wom
diff --git a/wombat/vendordeps/PathplannerLib.json b/wombat/vendordeps/PathplannerLib.json
new file mode 100644
index 00000000..48b56188
--- /dev/null
+++ b/wombat/vendordeps/PathplannerLib.json
@@ -0,0 +1,38 @@
+{
+    "fileName": "PathplannerLib.json",
+    "name": "PathplannerLib",
+    "version": "2024.2.3",
+    "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786",
+    "frcYear": "2024",
+    "mavenUrls": [
+        "https://3015rangerrobotics.github.io/pathplannerlib/repo"
+    ],
+    "jsonUrl": "https://3015rangerrobotics.github.io/pathplannerlib/PathplannerLib.json",
+    "javaDependencies": [
+        {
+            "groupId": "com.pathplanner.lib",
+            "artifactId": "PathplannerLib-java",
+            "version": "2024.2.3"
+        }
+    ],
+    "jniDependencies": [],
+    "cppDependencies": [
+        {
+            "groupId": "com.pathplanner.lib",
+            "artifactId": "PathplannerLib-cpp",
+            "version": "2024.2.3",
+            "libName": "PathplannerLib",
+            "headerClassifier": "headers",
+            "sharedLibrary": false,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "osxuniversal",
+                "linuxathena",
+                "linuxarm32",
+                "linuxarm64"
+            ]
+        }
+    ]
+}

From 7b14a8950c028ff5ffa081950ab9b4f9e3333dfd Mon Sep 17 00:00:00 2001
From: = <paul.hodges@student.education.wa.edu.au>
Date: Thu, 22 Feb 2024 19:06:40 +0800
Subject: [PATCH 166/207] did pathplanner

---
 src/main/cpp/Auto.cpp                       |  35 +-
 src/main/cpp/Robot.cpp                      |   2 +-
 src/main/include/Auto.h                     |  11 +-
 wombat/src/main/cpp/utils/Pathplanner.cpp   | 345 +++++++++++---------
 wombat/src/main/include/Wombat.h            |   1 +
 wombat/src/main/include/utils/Pathplanner.h | 230 ++++++++++---
 6 files changed, 424 insertions(+), 200 deletions(-)

diff --git a/src/main/cpp/Auto.cpp b/src/main/cpp/Auto.cpp
index a90fd17f..cbf485e6 100644
--- a/src/main/cpp/Auto.cpp
+++ b/src/main/cpp/Auto.cpp
@@ -3,17 +3,40 @@
 // of the MIT License at the root of this project
 
 #include "Auto.h"
+#include <utility>
 
+#include "AlphaArmBehaviour.h"
+#include "IntakeBehaviour.h"
+#include "ShooterBehaviour.h"
+#include "behaviour/Behaviour.h"
 #include "drivetrain/behaviours/SwerveBehaviours.h"
+#include "units/angle.h"
 #include "utils/Pathplanner.h"
 
-std::shared_ptr<behaviour::Behaviour> autos::Taxi(wom::drivetrain::SwerveDrive* _swerveDrive,
-                                                  Shooter* _shooter, Intake* _intake, AlphaArm* _alphaArm) {
-  return behaviour::make<ArmToSetPoint>(_alphaArm, 0_deg);
-  behaviour::make<AutoShoot>(_shooter);
-  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
-}
 // Shoots starting note then moves out of starting position.
+wom::SwerveAutoBuilder* autos::InitCommands(wom::drivetrain::SwerveDrive* _swerveDrive, Shooter* _shooter, Intake* _intake, AlphaArm* _alphaArm) {
+    wom::Commands c = *new wom::Commands(
+    {{"ArmToSetPoint", [_alphaArm]() {
+            return wom::make<ArmToSetPoint>(_alphaArm, 20_deg);
+        }},
+                                         {"AutoShoot", [_shooter]() {
+            return wom::make<AutoShoot>(_shooter);
+        }},
+                                         {"AutoIntake", [_intake]() {
+            return wom::make<AutoIntake>(_intake);
+        }}
+    });
+
+    return new wom::utils::SwerveAutoBuilder(
+        _swerveDrive, "", c);
+}
+
+std::shared_ptr<behaviour::Behaviour> autos::Taxi(wom::SwerveAutoBuilder* builder) {
+  return builder->GetAutoRoutine("taxi");
+  // return behaviour::make<ArmToSetPoint>(_alphaArm, 0_deg);
+  // behaviour::make<AutoShoot>(_shooter);
+  // behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+}
 
 std::shared_ptr<behaviour::Behaviour> autos::QuadrupleClose(wom::drivetrain::SwerveDrive* _swerveDrive,
                                                             Shooter* _shooter, Intake* _intake,
diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp
index 6e6a4940..7f458132 100644
--- a/src/main/cpp/Robot.cpp
+++ b/src/main/cpp/Robot.cpp
@@ -199,7 +199,7 @@ void Robot::AutonomousInit() {
   m_autoSelected = m_chooser.GetSelected();
 
   if (m_autoSelected == "Taxi") {
-    sched->Schedule(autos::Taxi(_swerveDrive, shooter, intake, alphaArm));
+    sched->Schedule(autos::Taxi(autos::InitCommands(_swerveDrive, shooter, intake, alphaArm)));
   } else if (m_autoSelected == "Auto Test") {
     sched->Schedule(autos::AutoTest(_swerveDrive, shooter, intake, alphaArm));
   } else if (m_autoSelected == "Quadruple Close") {
diff --git a/src/main/include/Auto.h b/src/main/include/Auto.h
index 61aecc4e..5d37020a 100644
--- a/src/main/include/Auto.h
+++ b/src/main/include/Auto.h
@@ -10,13 +10,20 @@
 #include "IntakeBehaviour.h"
 #include "ShooterBehaviour.h"
 #include "Wombat.h"
+#include "utils/Pathplanner.h"
+
 
 namespace autos {
+
+// wom::Commands* _commands = nullptr;
+// wom::SwerveAutoBuilder* builder = nullptr;
+
+wom::utils::SwerveAutoBuilder* InitCommands(wom::drivetrain::SwerveDrive* _swerveDrive, Shooter* _shooter, Intake* _intake, AlphaArm* _alphaArm);
+
 std::shared_ptr<behaviour::Behaviour> AutoTest(wom::drivetrain::SwerveDrive* _swerveDrive, Shooter* _shooter,
                                                Intake* _intake, AlphaArm* _alphaArm);
 
-std::shared_ptr<behaviour::Behaviour> Taxi(wom::drivetrain::SwerveDrive* _swerveDrive, Shooter* _shooter,
-                                           Intake* _intake, AlphaArm* _alphaArm);
+std::shared_ptr<behaviour::Behaviour> Taxi(wom::utils::SwerveAutoBuilder* builder);
 
 std::shared_ptr<behaviour::Behaviour> QuadrupleClose(wom::drivetrain::SwerveDrive* _swerveDrive,
                                                      Shooter* _shooter, Intake* _intake, AlphaArm* _alphaArm);
diff --git a/wombat/src/main/cpp/utils/Pathplanner.cpp b/wombat/src/main/cpp/utils/Pathplanner.cpp
index a1170655..45134974 100644
--- a/wombat/src/main/cpp/utils/Pathplanner.cpp
+++ b/wombat/src/main/cpp/utils/Pathplanner.cpp
@@ -3,20 +3,18 @@
 // of the MIT License at the root of this project
 
 #include "utils/Pathplanner.h"
-
 #include <frc/DriverStation.h>
 #include <pathplanner/lib/auto/AutoBuilder.h>
 #include <pathplanner/lib/util/HolonomicPathFollowerConfig.h>
 #include <pathplanner/lib/util/PIDConstants.h>
 #include <pathplanner/lib/util/ReplanningConfig.h>
-
 #include <algorithm>
 #include <array>
 #include <fstream>
+#include <initializer_list>
 #include <string>
 #include <utility>
 #include <vector>
-
 #include "behaviour/Behaviour.h"
 #include "drivetrain/SwerveDrive.h"
 #include "drivetrain/behaviours/SwerveBehaviours.h"
@@ -25,184 +23,235 @@
 #include "frc/kinematics/ChassisSpeeds.h"
 #include "pathplanner/lib/path/PathPlannerPath.h"
 #include "units/time.h"
+#include "wpi/json_fwd.h"
 
 using namespace wom;
 
-utils::Commands::Commands(std::initializer_list<std::pair<std::string, std::function<void(std::vector<std::any>)>>> init) : _commands{} {
-  for (const auto& pair : init) {
-    _commands.emplace_back(pair.first, std::vector<std::type_index>{typeid(std::vector<std::any>)}, pair.second);
-  }
-}
-
-void utils::Commands::Execute(std::string command, std::vector<std::any> args) {
-  auto it = std::find_if(_commands.begin(), _commands.end(),
-                         [command](const auto& cmd) { return cmd.name == command; });
-
-  if (it != _commands.end()) {
-    if (IsValid(command, args)) {
-      it->function(args);  
-    } else {
-      std::cerr << "Invalid arguments for command: " << command << std::endl;
+// Command implementation
+// template <typename Ret, typename... Args>
+// utils::Command<Ret, Args...>::Command(std::string name, FunctionType func)
+    // : name_(std::move(name)), function_(std::move(func)), argumentTypes_{typeid(Args)...} {}
+
+// template <typename Ret, typename... Args>
+// Ret utils::Command<Ret, Args...>::Execute(Args... args) const {
+    // return function_(std::forward<Args>(args)...);
+// }
+
+// template <typename Ret, typename... Args>
+// std::string utils::Command<Ret, Args...>::GetName() const {
+    // return name_;
+// }
+
+// template <typename Ret, typename... Args>
+// bool utils::Command<Ret, Args...>::IsValid(std::vector<std::type_index> argTypes) const {
+    // return argumentTypes_ == argTypes;
+// }
+
+// Commands implementation
+// template <typename... CommandTypes>
+// template <typename... CommandArgs>
+// utils::Commands<CommandTypes...>::Commands(CommandArgs&&... commands)
+//     : commands_{std::make_shared<utils::ConcreteCommand<CommandTypes>>(std::forward<CommandArgs>(commands))...} {}
+//
+// template <typename... CommandTypes>
+// template <typename Ret, typename... Args>
+// Ret utils::Commands<CommandTypes...>::Execute(std::string command, Args&&... args) {
+//     auto it = std::find_if(commands_.begin(), commands_.end(), [&command](const auto& cmd) {
+//         return cmd->GetName() == command;
+//     });
+//
+//     if (it != commands_.end()) {
+//         return (*it)->template Execute<Ret>({std::forward<Args>(args)...});
+//     } else {
+//         throw std::invalid_argument("Command not found");
+//     }
+// }
+//
+// template <typename... CommandTypes>
+// bool utils::Commands<CommandTypes...>::IsValid(std::string command, std::vector<std::type_index> argTypes) const {
+//     auto it = std::find_if(commands_.begin(), commands_.end(), [&command](const auto& cmd) {
+//         return cmd->GetName() == command;
+//     });
+//
+//     if (it != commands_.end()) {
+//         return (*it)->IsValid(argTypes);
+//     } else {
+//         throw std::invalid_argument("Command not found");
+//     }
+// }
+//
+// template <typename... CommandTypes>
+// template <typename Ret, typename... Args>
+// Ret utils::Commands<CommandTypes...>::Run(std::string commandString, Args&&... args) {
+//     size_t pos = commandString.find("(");
+//     if (pos != std::string::npos) {
+//         std::string command = commandString.substr(0, pos);
+//         std::string argString = commandString.substr(pos + 1, commandString.size() - pos - 2);
+//
+//         std::vector<std::any> parsedArgs = ParseArguments(argString);
+//
+//         if (IsValid(command, GetTypeIndices<Args...>())) {
+//             return Execute<Ret>(command, std::forward<Args>(args)...);
+//         } else {
+//             throw std::invalid_argument("Invalid command or argument types");
+//         }
+//     } else {
+//         throw std::invalid_argument("Invalid command string format");
+//     }
+// }
+//
+// template <typename... CommandTypes>
+// std::vector<std::any> utils::Commands<CommandTypes...>::ParseArguments(const std::string& argString) {
+//     std::vector<std::any> args;
+//
+//     std::istringstream iss(argString);
+//     std::string arg;
+//     while (iss >> arg) {
+//         args.push_back(ParseSingleArgument(arg));
+//     }
+//
+//     return args;
+// }
+//
+// template <typename... CommandTypes>
+// std::any utils::Commands<CommandTypes...>::ParseSingleArgument(const std::string& arg) {
+//     try {
+//         return std::stoi(arg);
+//     } catch (const std::invalid_argument& e) {
+//         throw std::invalid_argument("Error parsing argument: " + arg);
+//     }
+// }
+//
+// template <typename... CommandTypes>
+// template <typename T>
+// std::type_index utils::Commands<CommandTypes...>::GetTypeIndex() {
+//     return std::type_index(typeid(T));
+// }
+
+// PathWeaver implementation
+utils::PathWeaver::PathWeaver() {}
+
+frc::Trajectory utils::PathWeaver::getTrajectory(std::string_view path) {
+    try {
+        fs::path path_location = deploy_directory / path;
+        return frc::TrajectoryUtil::FromPathweaverJson(path_location.string());
+    } catch (const std::exception& e) {
+        // Handle the exception appropriately or consider removing the recursive call
+        throw; // Rethrow the exception for now
     }
-  } else {
-    std::cerr << "Unknown command: " << command << std::endl;
-  }
-}
-
-void utils::Commands::Run(std::string command) {
-  command.erase(std::remove_if(command.begin(), command.end(), ::isspace), command.end());
-
-  size_t openParenPos = command.find('(');
-  size_t closeParenPos = command.find(')');
-
-  if (openParenPos != std::string::npos && closeParenPos != std::string::npos &&
-      closeParenPos > openParenPos) {
-    std::string commandName = command.substr(0, openParenPos);
-    std::string argString = command.substr(openParenPos + 1, closeParenPos - openParenPos - 1);
-
-    std::vector<std::any> args = ParseArguments(argString);
-
-    Execute(commandName, args);
-  } else {
-    std::cerr << "Invalid command format: " << command << std::endl;
-  }
-}
-
-std::vector<std::any> utils::Commands::ParseArguments(const std::string& argString) {
-  std::vector<std::any> args;
-
-  std::istringstream iss(argString);
-  std::string token;
-
-  while (std::getline(iss, token, ',')) {
-    token.erase(std::remove_if(token.begin(), token.end(), ::isspace), token.end());
-    args.push_back(std::any(token));
-  }
-
-  return args;
-}
-
-bool utils::Commands::IsValid(std::string command, const std::vector<std::any> args) {
-  auto it = std::find_if(_commands.begin(), _commands.end(),
-                         [command](const auto& cmd) { return cmd.name == command; });
-
-  if (it != _commands.end()) {
-    const auto& expectedTypes = it->argumentTypes;
-
-    if (args.size() != expectedTypes.size()) {
-      return false;
-    }
-
-    for (size_t i = 0; i < args.size(); ++i) {
-      if (args[i].type() != expectedTypes[i]) {
-        return false;
-      }
-    }
-
-    return true;
-  } else {
-    return false;
-  }
-}
-
-utils::Pathplanner::Pathplanner() {}
-
-frc::Trajectory utils::Pathplanner::getTrajectory(std::string_view path) {
-  try {
-    fs::path path_location = deploy_directory / path;
-    return frc::TrajectoryUtil::FromPathweaverJson(path_location.string());
-  } catch (std::exception& e) {
-    return getTrajectory(path);
-  }
 }
 
+// FollowPath implementation
 utils::FollowPath::FollowPath(drivetrain::SwerveDrive* swerve, std::string path, bool flip)
     : _swerve(swerve) {
-  _path = pathplanner::PathPlannerPath::fromPathFile(path);
+    _path = pathplanner::PathPlannerPath::fromPathFile(path);
 
-  if (flip) {
-    _path = _path->flipPath();
-  }
+    if (flip) {
+        _path = _path->flipPath();
+    }
 
-  _poses = _path->getPathPoses();
+    _poses = _path->getPathPoses();
 }
 
-void utils::FollowPath::OnTick(units::second_t dt) {
-  _swerve->SetPose(_poses[_currentPose]);
+void utils::FollowPath::OnTick(units::time::second_t dt) {
+    _swerve->SetPose(_poses[_currentPose]);
 
-  if (_swerve->IsAtSetPose()) {
-    if (_currentPose == static_cast<int>(_poses.size())) {
-      SetDone();
-    } else {
-      _currentPose++;
+    if (_swerve->IsAtSetPose()) {
+        if (_currentPose == static_cast<int>(_poses.size())) {
+            SetDone();
+        } else {
+            _currentPose++;
+        }
     }
-  }
-};
-
-utils::RunCommand::RunCommand(std::string command, Commands commands) : _command(command), _commands(commands) {}
-
-void utils::RunCommand::OnTick(units::second_t dt) {
-  _commands.Run(_command);
-
-  SetDone();
 }
 
-utils::AutoBuilder::AutoBuilder(drivetrain::SwerveDrive* swerve, std::function<bool()> shouldFlipPath,
-                                std::string auto_routine, Commands commands)
-    : _swerve(swerve), _flip(shouldFlipPath()), _path(auto_routine), _commandsList(commands) {
-  SetAuto(auto_routine);
-};
+// AutoBuilder implementation
+utils::AutoBuilder::AutoBuilder(drivetrain::SwerveDrive* swerve,
+                                              std::function<bool()> shouldFlipPath,
+                                              std::string auto_routine,
+                                              utils::Commands commands)
+    : _path(auto_routine),
+      _flip(shouldFlipPath()),
+      _swerve(swerve),
+      _commandsList(std::move(commands)) {
+    SetAuto(auto_routine);
+}
 
 void utils::AutoBuilder::SetAuto(std::string_view path) {
-  fs::path deploy_directory = frc::filesystem::GetDeployDirectory();
-  fs::path location = deploy_directory / path;
+    fs::path deploy_directory = frc::filesystem::GetDeployDirectory();
+    fs::path location = deploy_directory / path;
 
-  std::ifstream file(location);
+    std::ifstream file(location);
 
-  wpi::json j;
+    wpi::json j;
 
-  file >> j;
+    file >> j;
 
-  _currentPath = &j;
-  _startingPose = &j["startingPose"];
-  _commands = &j["command"]["data"]["commands"];
+    _currentPath = &j;
+    _startingPose = &j["startingPose"];
+    _commands = &j["command"]["data"]["commands"];
 }
 
-void utils::AutoBuilder::Invert() {}
+void utils::AutoBuilder::Invert() {
+    // Implement inversion logic if needed
+}
 
 frc::Pose2d utils::AutoBuilder::JSONPoseToPose2d(wpi::json j) {
-  return frc::Pose2d(units::meter_t{j["position"]["x"]}, units::meter_t{j["position"]["y"]},
-                     units::degree_t{j["rotation"]});
+    return frc::Pose2d(units::meter_t{j["position"]["x"]},
+                       units::meter_t{j["position"]["y"]},
+                       units::degree_t{j["rotation"]});
 }
 
 std::shared_ptr<behaviour::Behaviour> utils::AutoBuilder::GetAutoPath() {
-  behaviour::Behaviour::ptr path = behaviour::make<drivetrain::behaviours::DrivebasePoseBehaviour>(
-      _swerve, JSONPoseToPose2d(*_startingPose));
-
-  for (auto& command : *_commands) {
-    if (command["type"] == "path") {
-      path << behaviour::make<FollowPath>(_swerve, command["data"]["pathName"], _flip);
-    } else if (command["type"] == "command") {
-      path << behaviour::make<RunCommand>(command["data"]["name"], _commandsList);
+    behaviour::Behaviour::ptr path = behaviour::make<drivetrain::behaviours::DrivebasePoseBehaviour>(
+        _swerve, JSONPoseToPose2d(*_startingPose));
+
+    for (auto& command : *_commands) {
+        if (command["type"] == "path") {
+            path << behaviour::make<FollowPath>(_swerve, command["data"]["pathName"], _flip);
+        } else if (command["type"] == "command") {
+            path << _commandsList.Run(command["data"]["name"]);
+        }
     }
-  }
 
-  return path;
+    return path;
 }
 
-utils::SwerveAutoBuilder::SwerveAutoBuilder(wom::drivetrain::SwerveDrive* swerve, std::string name)
-    : _swerve(swerve), _name(name) {
-  _builder = new AutoBuilder(
-      swerve,
-      []() {
-        auto alliance = frc::DriverStation::GetAlliance();
+std::shared_ptr<behaviour::Behaviour> utils::AutoBuilder::GetAutoPath(std::string path) {
+    SetAuto(path);
+    return GetAutoPath();
+}
 
-        if (alliance) {
-          return alliance.value() == frc::DriverStation::Alliance::kRed;
-        }
+// template <typename CommandsType>
+// behaviour::Behaviour utils::AutoBuilder<CommandsType>::GetPath(std::string path) {
+    // Implement the logic to return the appropriate behaviour
+    // You might want to convert it to shared_ptr if needed
+    // return behaviour::Behaviour(); // Placeholder
+// }
+
+// SwerveAutoBuilder implementation
+utils::SwerveAutoBuilder::SwerveAutoBuilder(drivetrain::SwerveDrive* swerve,
+                                                          std::string name,
+                                                          utils::Commands commands)
+    : _builder(new AutoBuilder(
+          swerve,
+          []() {
+              auto alliance = frc::DriverStation::GetAlliance();
+              return alliance && alliance.value() == frc::DriverStation::Alliance::kRed;
+          },
+          name,
+          std::move(commands))),
+      _swerve(swerve),
+      _name(name) {}
+
+void utils::SwerveAutoBuilder::SetAuto(std::string path) {
+    _builder->SetAuto(path);
+}
+
+std::shared_ptr<behaviour::Behaviour> utils::SwerveAutoBuilder::GetAutoRoutine() {
+    return _builder->GetAutoPath();
+}
 
-        return false;
-      },
-      "taxi",
-      Commands());
+std::shared_ptr<behaviour::Behaviour> utils::SwerveAutoBuilder::GetAutoRoutine(std::string path) {
+    return _builder->GetAutoPath(path);
 }
diff --git a/wombat/src/main/include/Wombat.h b/wombat/src/main/include/Wombat.h
index 6f7d00bb..12c6ecea 100644
--- a/wombat/src/main/include/Wombat.h
+++ b/wombat/src/main/include/Wombat.h
@@ -15,6 +15,7 @@
 #include "utils/PID.h"
 #include "utils/RobotStartup.h"
 #include "utils/Util.h"
+#include "utils/Pathplanner.h"
 #include "vision/Camera.h"
 #include "vision/Limelight.h"
 
diff --git a/wombat/src/main/include/utils/Pathplanner.h b/wombat/src/main/include/utils/Pathplanner.h
index 440984e6..f7e3b255 100644
--- a/wombat/src/main/include/utils/Pathplanner.h
+++ b/wombat/src/main/include/utils/Pathplanner.h
@@ -8,49 +8,197 @@
 #include <frc/trajectory/TrajectoryUtil.h>
 #include <wpi/fs.h>
 
+#include <algorithm>
 #include <any>
+#include <functional>
+#include <memory>
 #include <string>
 #include <typeindex>
+#include <vector>
+#include <stdexcept>
 
 #include "behaviour/Behaviour.h"
 #include "drivetrain/SwerveDrive.h"
-#include "drivetrain/behaviours/SwerveBehaviours.h"
 #include "pathplanner/lib/path/PathPlannerPath.h"
-#include "pathplanner/lib/util/PIDConstants.h"
 #include "wpi/json_fwd.h"
 
 namespace wom {
 namespace utils {
 
-class Commands {
+// template <typename Ret, typename... Args>
+// class ConcreteCommand;
+
+// template <typename Ret, typename... Args>
+// class CommandBase {
+// public:
+  // virtual ~CommandBase() = default;
+    // virtual std::shared_ptr<behaviour::Behaviour> Execute(Args...) const = 0;
+    // virtual bool IsValid(std::vector<std::type_index> argTypes) const = 0;
+    // virtual std::string GetName() const = 0;
+// };
+
+// template <typename Ret, typename... Args>
+// class Command : public CommandBase<Ret, Args...>{
+// public:
+    // using FunctionType = std::function<Ret(Args...)>;
+//
+    // Command(std::string name, FunctionType func);
+
+  // std::shared_ptr<behaviour::Behaviour> Execute(Args... args) const;
+    // Ret Execute(Args... args) const;
+
+    // std::string GetName() const override;
+
+    // bool IsValid(std::vector<std::type_index> argTypes) const;
+
+// private:
+    // std::string name_;
+    // FunctionType function_;
+    // std::vector<std::type_index> argumentTypes_;
+// };
+// template <typename BehaviorType>
+//     class Command {
+//     public:
+//         using BehaviorPtr = std::shared_ptr<BehaviorType>;
+//
+//         Command(const std::string& name, std::function<BehaviorPtr()> func)
+//             : name_(name), func_(func) {}
+//
+//         BehaviorPtr Execute() const {
+//             return func_();
+//         }
+//
+//         const std::string& GetName() const {
+//             return name_;
+//         }
+//
+//         bool IsValid() const {
+//             return func_ != nullptr;
+//         }
+//
+//     private:
+//         std::string name_;
+//         std::function<BehaviorPtr()> func_;
+//     };
+//
+// class Commands {
+// public:
+//     template <typename... CommandArgs>
+//     Commands(CommandArgs&&... commands);
+//
+//     template <typename Ret, typename... Args>
+//     Ret Execute(std::string command, Args&&... args);
+//
+//     bool IsValid(std::string command, std::vector<std::type_index> argTypes) const;
+//
+//     template <typename Ret, typename... Args>
+//     Ret Run(std::string commandString, Args&&... args);
+//
+// private:
+//     std::tuple<CommandTypes...> commands_;
+//
+//     std::vector<std::any> ParseArguments(const std::string& argString);
+//     std::any ParseSingleArgument(const std::string& arg);
+//
+//     template <typename T>
+//     static std::type_index GetTypeIndex();
+//     
+//   template <typename... Args>
+//   std::vector<std::type_index> GetTypeIndices() {
+//     return {GetTypeIndex<Args>()...};
+//   }
+// };
+// template <typename BehaviorType>
+//     class Command {
+//     public:
+//         using BehaviorPtr = std::shared_ptr<behaviour::Behaviour>;
+//
+//         template <typename Function>
+//         Command(Function&& func, std::string name)
+//             : func_{std::make_shared<Function>(std::forward<Function>(func))}, name_{std::move(name)} {}
+//
+//         BehaviorPtr Execute() const  {
+//             return func_();
+//         }
+//
+//         std::string GetName() const  {
+//             return name_;
+//         }
+//
+//     private:
+//         std::function<std::shared_ptr<behaviour::Behaviour()>> func_;
+//         std::string name_;
+//     };
+
+    class Commands {
+    public:
+
+        // template <typename... CommandTypes>
+        // Commands(CommandTypes&&... commands)
+            // : commands_{std::make_shared<Command<BehaviorType, CommandTypes>>(std::forward<CommandTypes>(commands))...} {}
+
+        Commands(std::vector<std::pair<std::string, std::function<std::shared_ptr<behaviour::Behaviour>()>>> commands) : commands_(std::move(commands)) {};
+
+  std::shared_ptr<behaviour::Behaviour> Execute(std::string command) {
+    auto it = std::find_if(commands_.begin(), commands_.end(), [&command](const auto& cmd) {
+        return cmd.first == command;
+    });
+
+    if (it != commands_.end()) {
+        return (it->second)();
+    } else {
+        throw std::invalid_argument("Command not found");
+    }
+  }
+
+        // bool IsValid(std::string command) const {
+            // auto it = std::find_if(commands_.begin(), commands_.end(), [&command](const auto& cmd) {
+                // return cmd->f == command;
+            // });
+
+            // return it != commands_.end();
+        // }
+
+  std::shared_ptr<behaviour::Behaviour> Run(std::string commandString) {
+          return Execute(commandString);
+  }
+
+    private:
+        std::vector<std::pair<std::string, std::function<std::shared_ptr<behaviour::Behaviour>()>>> commands_ = {};
+    };
+
+// template <typename Ret, typename... Args>
+// class ConcreteCommand : public CommandBase<Ret, Args...> {
+// public:
+  // using CommandType = Command<Ret, Args...>;
+
+    // ConcreteCommand(std::string name, typename CommandType::FunctionType func)
+        // : command_(std::move(name), std::move(func)) {}
+//
+//     void Execute(std::vector<std::any> args) const override {
+//         // Convert args to the expected types and call Execute
+//         if (args.size() == sizeof...(Args)) {
+//             command_.Execute(std::any_cast<Args>(args[0])...);
+//         } else {
+//             throw std::invalid_argument("Incorrect number of arguments for command");
+//         }
+//     }
+//
+//     bool IsValid(std::vector<std::type_index> argTypes) const override {
+//         return command_.IsValid(argTypes);
+//     }
+//
+//     std::string GetName() const override {
+//         return command_.GetName();
+//     }
+//
+// private:
+//     Command<Ret, Args...> command_;
+// };
+
+class PathWeaver {
  public:
-  
-  Commands(std::initializer_list<std::pair<std::string, std::function<void(std::vector<std::any>)>>> init);
-
-  Commands() = default;
-  
-  void Execute(std::string command, std::vector<std::any> args);
-
-  void Run(std::string command);
-
-  bool IsValid(std::string command, std::vector<std::any> args);
-
- private:
-  struct CommandData {
-    std::string name;
-    std::vector<std::type_index> argumentTypes;
-    std::function<void(std::vector<std::any>)> function;
-  };
-
-  std::vector<CommandData> _commands;  // Changed name to _commands
-
-  // Helper function to parse command arguments from a string
-  std::vector<std::any> ParseArguments(const std::string& argString);
-};
-
-class Pathplanner {
- public:
-  Pathplanner();
+  PathWeaver();
 
   frc::Trajectory getTrajectory(std::string_view path);
 
@@ -74,17 +222,6 @@ class FollowPath : public behaviour::Behaviour {
   int _currentPose = 0;
 };
 
-class RunCommand : public behaviour::Behaviour {
-public:
-  RunCommand(std::string command, Commands commands);
-
-  void OnTick(units::time::second_t dt) override;
-
-  private:
-  std::string _command;
-  Commands _commands;
-};
-
 class AutoBuilder {
  public:
   AutoBuilder(drivetrain::SwerveDrive* swerve, std::function<bool()> shouldFlipPath, std::string autoRoutine, Commands commands);
@@ -93,6 +230,8 @@ class AutoBuilder {
   void SetAuto(std::string_view path);
 
   std::shared_ptr<behaviour::Behaviour> GetAutoPath();
+  std::shared_ptr<behaviour::Behaviour> GetAutoPath(std::string path);
+
   behaviour::Behaviour GetPath(std::string path);
 
   frc::Pose2d JSONPoseToPose2d(wpi::json j);
@@ -111,12 +250,17 @@ class AutoBuilder {
 
 class SwerveAutoBuilder {
  public:
-  SwerveAutoBuilder(wom::drivetrain::SwerveDrive* swerve, std::string name);
+  SwerveAutoBuilder(drivetrain::SwerveDrive* swerve, std::string name, Commands commands);
+  
+  void SetAuto(std::string path);
+  std::shared_ptr<behaviour::Behaviour> GetAutoRoutine();
+  std::shared_ptr<behaviour::Behaviour> GetAutoRoutine(std::string path);  
 
  private:
   AutoBuilder* _builder;
-  wom::drivetrain::SwerveDrive* _swerve;
+  drivetrain::SwerveDrive* _swerve;
   std::string _name;
 };
+
 }  // namespace utils
 }  // namespace wom

From 987548928a3ceebd745f259ad31a4cae40360daf Mon Sep 17 00:00:00 2001
From: JoystickMaster-race <foxlocks179@gmail.com>
Date: Thu, 22 Feb 2024 19:10:30 +0800
Subject: [PATCH 167/207] Arm Good PID States

---
 src/main/cpp/AlphaArm.cpp          | 26 ++++++++++--------
 src/main/cpp/AlphaArmBehaviour.cpp | 44 ++++++++++++++++++++++--------
 src/main/include/AlphaArm.h        |  2 ++
 3 files changed, 49 insertions(+), 23 deletions(-)

diff --git a/src/main/cpp/AlphaArm.cpp b/src/main/cpp/AlphaArm.cpp
index 2aa2fdf0..1e090f9e 100644
--- a/src/main/cpp/AlphaArm.cpp
+++ b/src/main/cpp/AlphaArm.cpp
@@ -5,7 +5,7 @@
 #include "AlphaArm.h"
 
 
-AlphaArm::AlphaArm(AlphaArmConfig *config) : _config(config), _pidArm{frc::PIDController(1.2, 0.4, 0)}, _pidArmStates{frc::PIDController(2, 0, 0)} 
+AlphaArm::AlphaArm(AlphaArmConfig *config) : _config(config), _pidArm{frc::PIDController(1.2, 0.4, 0)}, _pidArmStates{frc::PIDController(6.5, 0.1, 0)}, _pidIntakeState{frc::PIDController(0, 0, 0)} 
 {
   // _alphaArmPID = wom::utils::PIDController(config.alphaArmPID);
 }
@@ -14,6 +14,11 @@ AlphaArm::AlphaArm(AlphaArmConfig *config) : _config(config), _pidArm{frc::PIDCo
 //   return _config;
 // }
 
+ 
+void AlphaArm::OnStart(){
+  _pidArmStates.Reset();
+}
+
 void AlphaArm::OnUpdate(units::second_t dt) {
   switch (_state) {
     case AlphaArmState::kIdle:
@@ -36,29 +41,28 @@ void AlphaArm::OnUpdate(units::second_t dt) {
     case AlphaArmState::kAmpAngle:
       std::cout << "Amp Angle" << std::endl;
       
-
-       _pidArm.SetSetpoint(2.17);
-       _setAlphaArmVoltage = units::volt_t{_pidArmStates.Calculate(_config->alphaArmEncoder.GetEncoderPosition().value())};
+       _pidArmStates.SetSetpoint(-2.17);
+       _setAlphaArmVoltage = units::volt_t{_pidArmStates.Calculate(-_config->alphaArmEncoder.GetEncoderPosition().value())};
 
     break;
 
     case AlphaArmState::kIntakeAngle:
     std::cout << "Intake Angle" << std::endl;
-       _pidArm.SetSetpoint(0.48);
-       _setAlphaArmVoltage = units::volt_t{_pidArmStates.Calculate(_config->alphaArmEncoder.GetEncoderPosition().value())};
+       _pidArmStates.SetSetpoint(-0.48);
+       _setAlphaArmVoltage = units::volt_t{_pidArmStates.Calculate(-_config->alphaArmEncoder.GetEncoderPosition().value())};
     break;
 
     case AlphaArmState::kSpeakerAngle:
     std::cout << "Speaker Angle" << std::endl;
-       _pidArm.SetSetpoint(0.82);
-       _setAlphaArmVoltage = units::volt_t{_pidArmStates.Calculate(_config->alphaArmEncoder.GetEncoderPosition().value())};
+       _pidArmStates.SetSetpoint(-0.82);
+       _setAlphaArmVoltage = units::volt_t{_pidArmStates.Calculate(-_config->alphaArmEncoder.GetEncoderPosition().value())};
 
     break;
 
     case AlphaArmState::kStowed:
-    std::cout << "IntakeAngle" << std::endl;
-       _pidArm.SetSetpoint(0.6);
-       _setAlphaArmVoltage = units::volt_t{_pidArmStates.Calculate(_config->alphaArmEncoder.GetEncoderPosition().value())};
+    std::cout << "Stowed" << std::endl;
+       _pidArmStates.SetSetpoint(-0.52);
+       _setAlphaArmVoltage = units::volt_t{_pidArmStates.Calculate(-_config->alphaArmEncoder.GetEncoderPosition().value())};
       default:
       std::cout << "Error: alphaArm in INVALID STATE" << std::endl;
       break;
diff --git a/src/main/cpp/AlphaArmBehaviour.cpp b/src/main/cpp/AlphaArmBehaviour.cpp
index 9348a848..3363bd16 100644
--- a/src/main/cpp/AlphaArmBehaviour.cpp
+++ b/src/main/cpp/AlphaArmBehaviour.cpp
@@ -15,6 +15,7 @@ AlphaArmConfig AlphaArm::GetConfig() {
   return *_config;
 }
 
+
 void AlphaArmManualControl::OnTick(units::second_t dt) {
   // std::cout << "HELLO" << std::endl;
 
@@ -22,7 +23,7 @@ void AlphaArmManualControl::OnTick(units::second_t dt) {
   _table->GetEntry("Goal Value").SetBoolean(_gotValue);
   
 
-  if (_codriver->GetStartButtonPressed()) {
+  if (_codriver->GetRightBumperPressed()) {
     if (_rawControl == true) {
       _rawControl = false;
     } else {
@@ -46,23 +47,42 @@ void AlphaArmManualControl::OnTick(units::second_t dt) {
   //       _gotValue = true;
   //     } 
   //     _alphaArm->SetState(AlphaArmState::kHoldAngle);
-
   //   }
   // }
-  if(_codriver->GetYButtonPressed()){
-    _alphaArm->SetState(AlphaArmState::kIntakeAngle);
-  }
-  if(_codriver->GetBButtonPressed()){
-    _alphaArm->SetState(AlphaArmState::kAmpAngle);
 
-  }
-  if(_codriver->GetAButtonPressed()){
+  if(_codriver->GetLeftTriggerAxis() > 0.1){
     _alphaArm->SetState(AlphaArmState::kSpeakerAngle);
-  }
-  if(_codriver->GetXButtonPressed()){
+  } else if (_codriver->GetLeftBumper()){
+    _alphaArm->SetState(AlphaArmState::kAmpAngle);
+  } else if(_codriver->GetYButton()){
     _alphaArm->SetState(AlphaArmState::kStowed);
+  } else {
+    _alphaArm->SetState(AlphaArmState::kIntakeAngle);
   }
-}
+
+  }
+
+  // }
+  // if(_codriver->GetLeftBumper() == true){
+  //   _alphaArm->SetState(AlphaArmState::kAmpAngle);
+  // }
+  // if(_codriver->GetLeftTriggerAxis() > 0.1){
+  //   _alphaArm->SetState(AlphaArmState::kSpeakerAngle);
+  // }
+  // if(_codriver->GetYButton() == true){
+  //   _alphaArm->SetState(AlphaArmState::kStowed);
+  // }  
+
+  //  if(_codriver->GetYButton() == true){
+  //   _alphaArm->SetState(AlphaArmState::kStowed);
+  // }  
+
+
+  // else {
+  //   _alphaArm->SetState(AlphaArmState::kRaw);
+  // }
+  
+
 
 //   if (!_rawControl){
 //     if(_codriver->GetBButtonPressed()){
diff --git a/src/main/include/AlphaArm.h b/src/main/include/AlphaArm.h
index b8a4f973..8548f166 100644
--- a/src/main/include/AlphaArm.h
+++ b/src/main/include/AlphaArm.h
@@ -41,6 +41,7 @@ class AlphaArm : public behaviour::HasBehaviour {
   void SetState(AlphaArmState state);
   void SetControllerRaw(units::volt_t voltage); 
   void SetGoal(double goal);
+  void OnStart();
   AlphaArmConfig GetConfig(); //{ return _config; }
   frc::PIDController GetPID();
 
@@ -53,6 +54,7 @@ class AlphaArm : public behaviour::HasBehaviour {
   //frc::DutyCycleEncoder armEncoder{4};
   frc::PIDController _pidArm;
   frc::PIDController _pidArmStates;
+  frc::PIDController _pidIntakeState;
   std::shared_ptr<nt::NetworkTable> _table = nt::NetworkTableInstance::GetDefault().GetTable("AlphaArm");
   units::volt_t _setAlphaArmVoltage = 0_V;
 

From 60e5c302462a05bfc7fc5923899f89c0725885b5 Mon Sep 17 00:00:00 2001
From: JoystickMaster-race <foxlocks179@gmail.com>
Date: Sat, 24 Feb 2024 12:23:02 +0800
Subject: [PATCH 168/207] Cleaned up PID arm

---
 src/main/cpp/AlphaArm.cpp            | 16 +++------
 src/main/cpp/AlphaArmBehaviour.cpp   | 52 ----------------------------
 src/main/include/AlphaArm.h          |  5 ++-
 src/main/include/AlphaArmBehaviour.h |  4 +--
 4 files changed, 7 insertions(+), 70 deletions(-)

diff --git a/src/main/cpp/AlphaArm.cpp b/src/main/cpp/AlphaArm.cpp
index 1e090f9e..613e5fc7 100644
--- a/src/main/cpp/AlphaArm.cpp
+++ b/src/main/cpp/AlphaArm.cpp
@@ -5,16 +5,11 @@
 #include "AlphaArm.h"
 
 
-AlphaArm::AlphaArm(AlphaArmConfig *config) : _config(config), _pidArm{frc::PIDController(1.2, 0.4, 0)}, _pidArmStates{frc::PIDController(6.5, 0.1, 0)}, _pidIntakeState{frc::PIDController(0, 0, 0)} 
+AlphaArm::AlphaArm(AlphaArmConfig *config) : _config(config), _pidArm{frc::PIDController(1.2, 0.4, 0)}, _pidArmStates{frc::PIDController(6.5, 0.1, 0)}, _pidIntakeState{frc::PIDController(1, 0, 0)} 
 {
-  // _alphaArmPID = wom::utils::PIDController(config.alphaArmPID);
+ 
 }
 
-// AlphaArmConfig AlphaArm::GetConfig() {
-//   return _config;
-// }
-
- 
 void AlphaArm::OnStart(){
   _pidArmStates.Reset();
 }
@@ -48,7 +43,7 @@ void AlphaArm::OnUpdate(units::second_t dt) {
 
     case AlphaArmState::kIntakeAngle:
     std::cout << "Intake Angle" << std::endl;
-       _pidArmStates.SetSetpoint(-0.48);
+       _pidIntakeState.SetSetpoint(-0.48);
        _setAlphaArmVoltage = units::volt_t{_pidArmStates.Calculate(-_config->alphaArmEncoder.GetEncoderPosition().value())};
     break;
 
@@ -70,7 +65,6 @@ void AlphaArm::OnUpdate(units::second_t dt) {
   }
    _config->alphaArmGearbox.motorController->SetVoltage(_setAlphaArmVoltage);
    _config->alphaArmGearbox2.motorController->SetVoltage(_setAlphaArmVoltage);
-    //std::cout << "Encoder Value: " << _config.alphaArmGearbox.encoder->GetEncoderPosition().value() << std::endl;
     std::cout << "Encoder Value: " << _config->alphaArmEncoder.GetEncoderPosition().value() << std::endl;
     _table->GetEntry("PID Error").SetDouble(_pidArm.GetPositionError());
     _table->GetEntry("SetPoint").SetDouble(_pidArm.GetSetpoint());
@@ -79,10 +73,8 @@ void AlphaArm::OnUpdate(units::second_t dt) {
     _table->GetEntry("PID Error State").SetDouble(_pidArmStates.GetPositionError());
     _table->GetEntry("SetPoint State").SetDouble(_pidArmStates.GetSetpoint());
 
-// //std::cout << "Encoder Position: " << armEncoder.GetAbsolutePosition() << std::endl;
-//   //std::cout << "Encoder Value: " << _config.alphaArmEncoder->GetEncoderPosition().value() << std::endl;
+
      std::cout << "Voltage:" << _setAlphaArmVoltage.value() << std::endl;
-//  }
 }
 
 void AlphaArm::SetState(AlphaArmState state) {
diff --git a/src/main/cpp/AlphaArmBehaviour.cpp b/src/main/cpp/AlphaArmBehaviour.cpp
index 3363bd16..14419904 100644
--- a/src/main/cpp/AlphaArmBehaviour.cpp
+++ b/src/main/cpp/AlphaArmBehaviour.cpp
@@ -15,9 +15,7 @@ AlphaArmConfig AlphaArm::GetConfig() {
   return *_config;
 }
 
-
 void AlphaArmManualControl::OnTick(units::second_t dt) {
-  // std::cout << "HELLO" << std::endl;
 
   _table->GetEntry("State").SetBoolean(_rawControl);
   _table->GetEntry("Goal Value").SetBoolean(_gotValue);
@@ -31,25 +29,6 @@ void AlphaArmManualControl::OnTick(units::second_t dt) {
     }
   }
 
-  // if (_rawControl) {
-  //   _alphaArm->SetState(AlphaArmState::kRaw);
-  //   _alphaArm->SetArmRaw(_codriver->GetRightY() * 12_V);
-  // } else {
-
-  //   if(std::abs(_codriver->GetRightY()) > 0.1)  {
-  //     _alphaArm->SetState(AlphaArmState::kRaw);
-  //     _alphaArm->SetArmRaw(_codriver->GetRightY() * 12_V);
-  //     _gotValue = false;
-  //   } else {
-  //     double input = _alphaArm->GetConfig().alphaArmEncoder.GetEncoderPosition().value();
-  //     if (!_gotValue){
-  //       _alphaArm->SetGoal(input);
-  //       _gotValue = true;
-  //     } 
-  //     _alphaArm->SetState(AlphaArmState::kHoldAngle);
-  //   }
-  // }
-
   if(_codriver->GetLeftTriggerAxis() > 0.1){
     _alphaArm->SetState(AlphaArmState::kSpeakerAngle);
   } else if (_codriver->GetLeftBumper()){
@@ -61,34 +40,3 @@ void AlphaArmManualControl::OnTick(units::second_t dt) {
   }
 
   }
-
-  // }
-  // if(_codriver->GetLeftBumper() == true){
-  //   _alphaArm->SetState(AlphaArmState::kAmpAngle);
-  // }
-  // if(_codriver->GetLeftTriggerAxis() > 0.1){
-  //   _alphaArm->SetState(AlphaArmState::kSpeakerAngle);
-  // }
-  // if(_codriver->GetYButton() == true){
-  //   _alphaArm->SetState(AlphaArmState::kStowed);
-  // }  
-
-  //  if(_codriver->GetYButton() == true){
-  //   _alphaArm->SetState(AlphaArmState::kStowed);
-  // }  
-
-
-  // else {
-  //   _alphaArm->SetState(AlphaArmState::kRaw);
-  // }
-  
-
-
-//   if (!_rawControl){
-//     if(_codriver->GetBButtonPressed()){
-//       _alphaArm->SetState(AlphaArmState::kAmpAngle);
-//       _alphaArm->SetGoal(1);
-//     }
-//  }
-
-// 
\ No newline at end of file
diff --git a/src/main/include/AlphaArm.h b/src/main/include/AlphaArm.h
index 8548f166..c05ef0d5 100644
--- a/src/main/include/AlphaArm.h
+++ b/src/main/include/AlphaArm.h
@@ -13,11 +13,10 @@
 #include <frc/controller/PIDController.h>
 
 struct AlphaArmConfig {
-  wom::Gearbox alphaArmGearbox; //up+down
+  wom::Gearbox alphaArmGearbox;
   wom::Gearbox alphaArmGearbox2;
   wom::DutyCycleEncoder alphaArmEncoder;
-  //frc::PIDController pidArmConfig;
-  //wom::PIDConfig<units::degree, units::volts> alphaArmPID;
+
   std::string path;
   // Vision *vision;
 };
diff --git a/src/main/include/AlphaArmBehaviour.h b/src/main/include/AlphaArmBehaviour.h
index fd553c09..2714d266 100644
--- a/src/main/include/AlphaArmBehaviour.h
+++ b/src/main/include/AlphaArmBehaviour.h
@@ -18,10 +18,8 @@ class AlphaArmManualControl : public behaviour::Behaviour {
  private:
   AlphaArm* _alphaArm;
   frc::XboxController* _codriver;
-  // units::volt_t _rightStick = ((_codriver->GetRightY()>0.05 || _codriver->GetRightY() < -0.05
-  // )?_codriver->GetRightY():0) * 2_V;
   units::volt_t _setAlphaArmVoltage = 0_V;
-  bool _rawControl; //needs a default
+  bool _rawControl;
   bool _gotValue = false;
   std::shared_ptr<nt::NetworkTable> _table = nt::NetworkTableInstance::GetDefault().GetTable("AlphaArmSubsystem");
 

From 7992cc3211cf64fe58a96bc60171634224da55be Mon Sep 17 00:00:00 2001
From: Sean Chan <sean.chan3@student.education.wa.edu.au>
Date: Sat, 24 Feb 2024 13:40:59 +0800
Subject: [PATCH 169/207] Intake - Has errors, for choon to work on

---
 src/main/cpp/AlphaArm.cpp                  |  66 ----
 src/main/cpp/AlphaArmBehaviour.cpp         |  45 ---
 src/main/cpp/ArmBehaviours.cpp             |  23 --
 src/main/cpp/Auto.cpp                      | 305 ---------------
 src/main/cpp/Intake.cpp                    |  35 ++
 src/main/cpp/IntakeBehaviour.cpp           |  68 ++--
 src/main/cpp/Robot.cpp                     |   4 -
 src/main/cpp/Shooter.cpp                   | 109 ------
 src/main/cpp/ShooterBehaviour.cpp          |  50 ---
 src/main/cpp/vision/Vision.cpp             | 415 ---------------------
 src/main/cpp/vision/VisionBehaviours.cpp   |  63 ----
 src/main/include/AlphaArm.h                |  44 ---
 src/main/include/AlphaArmBehaviour.h       |  34 --
 src/main/include/ArmBehaviours.h           |  23 --
 src/main/include/Auto.h                    |  41 --
 src/main/include/Intake.h                  |   4 +
 src/main/include/Robot.h                   |   6 -
 src/main/include/RobotMap.h                |   3 -
 src/main/include/Shooter.h                 |  49 ---
 src/main/include/ShooterBehaviour.h        |  36 --
 src/main/include/vision/Vision.h           | 113 ------
 src/main/include/vision/VisionBehaviours.h |  56 ---
 22 files changed, 73 insertions(+), 1519 deletions(-)
 delete mode 100644 src/main/cpp/AlphaArm.cpp
 delete mode 100644 src/main/cpp/AlphaArmBehaviour.cpp
 delete mode 100644 src/main/cpp/ArmBehaviours.cpp
 delete mode 100644 src/main/cpp/Auto.cpp
 delete mode 100644 src/main/cpp/Shooter.cpp
 delete mode 100644 src/main/cpp/ShooterBehaviour.cpp
 delete mode 100644 src/main/cpp/vision/Vision.cpp
 delete mode 100644 src/main/cpp/vision/VisionBehaviours.cpp
 delete mode 100644 src/main/include/AlphaArm.h
 delete mode 100644 src/main/include/AlphaArmBehaviour.h
 delete mode 100644 src/main/include/ArmBehaviours.h
 delete mode 100644 src/main/include/Auto.h
 delete mode 100644 src/main/include/Shooter.h
 delete mode 100644 src/main/include/ShooterBehaviour.h
 delete mode 100644 src/main/include/vision/Vision.h
 delete mode 100644 src/main/include/vision/VisionBehaviours.h

diff --git a/src/main/cpp/AlphaArm.cpp b/src/main/cpp/AlphaArm.cpp
deleted file mode 100644
index 848ee616..00000000
--- a/src/main/cpp/AlphaArm.cpp
+++ /dev/null
@@ -1,66 +0,0 @@
-// Copyright (c) 2023-2024 CurtinFRC
-// Open Source Software, you can modify it according to the terms
-// of the MIT License at the root of this project
-
-#include "AlphaArm.h"
-
-AlphaArm::AlphaArm(AlphaArmConfig config) : _config(config) {}
-
-AlphaArmConfig AlphaArm::GetConfig() {
-  return _config;
-}
-
-void AlphaArm::OnUpdate(units::second_t dt) {
-  switch (_state) {
-    case AlphaArmState::kIdle:
-
-      // _config.alphaArmGearbox.motorController->SetVoltage(0_V);
-      // _config.wristGearbox.motorController->SetVoltage(0_V);
-      _setAlphaArmVoltage = 0_V;
-      _setWristVoltage = 0_V;
-
-      break;
-    case AlphaArmState::kRaw:
-      _setAlphaArmVoltage = _rawArmVoltage;
-      _setWristVoltage = _rawWristVoltage;
-      _config.alphaArmGearbox.motorController->SetVoltage(_rawArmVoltage);
-      _config.wristGearbox.motorController->SetVoltage(_rawWristVoltage);
-
-      break;
-    case AlphaArmState::kForwardWrist:
-      _config.wristGearbox.motorController->SetVoltage(3_V);
-      _setWristVoltage = 3_V;
-
-    case AlphaArmState::kReverseWrist:
-      _config.wristGearbox.motorController->SetVoltage(-3_V);
-      _setWristVoltage = -3_V;
-    default:
-      std::cout << "oops, wrong state" << std::endl;
-      break;
-  }
-  // transmission translate
-  // _config.armGearBox.motorController->SetVoltage(setAlphaArmVoltage);
-  // _config.alphaArmGearbox.motorController->SetVoltage(setAlphaArmVoltage);
-  // _config.wristGearbox.motorController->SetVoltage(setWristVoltage);
-  _config.alphaArmGearbox.motorController->SetVoltage(_setAlphaArmVoltage);
-  _config.wristGearbox.motorController->SetVoltage(_setWristVoltage);
-
-  // _config.wristGearbox.motorController->SetVoltage(_setVoltage);
-}
-
-void AlphaArm::SetState(AlphaArmState state) {
-  _state = state;
-}
-
-// void AlphaArm::SetRaw(units::volt_t voltage){
-//     _rawArmVoltage = voltage;
-//     _rawWristVoltage = voltage;
-// }
-
-void AlphaArm::SetArmRaw(units::volt_t voltage) {
-  _rawArmVoltage = voltage;
-}
-
-void AlphaArm::setWristRaw(units::volt_t voltage) {
-  _rawWristVoltage = voltage;
-}
diff --git a/src/main/cpp/AlphaArmBehaviour.cpp b/src/main/cpp/AlphaArmBehaviour.cpp
deleted file mode 100644
index 5ae1cb2e..00000000
--- a/src/main/cpp/AlphaArmBehaviour.cpp
+++ /dev/null
@@ -1,45 +0,0 @@
-// Copyright (c) 2023-2024 CurtinFRC
-// Open Source Software, you can modify it according to the terms
-// of the MIT License at the root of this project
-
-#include "AlphaArmBehaviour.h"
-
-#include <frc/XboxController.h>
-
-AlphaArmManualControl::AlphaArmManualControl(AlphaArm* alphaArm, frc::XboxController* codriver)
-    : _alphaArm(alphaArm), _codriver(codriver) {
-  Controls(alphaArm);
-}
-
-void AlphaArmManualControl::OnTick(units::second_t dt) {
-  if (_codriver->GetXButtonPressed()) {
-    if (_rawControl == true) {
-      _rawControl = false;
-    } else {
-      _rawControl = true;
-    }
-  }
-
-  if (_rawControl) {
-    _alphaArm->SetState(AlphaArmState::kRaw);
-    _alphaArm->SetArmRaw(_codriver->GetRightY() * 6_V);
-    _alphaArm->setWristRaw(_codriver->GetLeftY() * -6_V);
-  } else {
-    if (_codriver->GetRightBumperPressed()) {
-      _alphaArm->SetState(AlphaArmState::kForwardWrist);
-    }
-    if (_codriver->GetLeftBumperPressed()) {
-      _alphaArm->SetState(AlphaArmState::kReverseWrist);
-    }
-  }
-}
-
-ArmToSetPoint::ArmToSetPoint(AlphaArm* alphaArm, units::degree_t armAngle) : _alphaArm(alphaArm) {
-  Controls(alphaArm);
-}
-// // ArmSpeed is a float from 0-1, 1 being instantly and 0 being don't move at all.
-
-void ArmToSetPoint::OnTick(units::second_t dt) {
-  // _armCurrentDegree = _alphaArm->GetConfig().alphaArmGearbox.encoder.GetEncoderPosition();
-  // _alphaArm->GetConfig().alphaArmGearbox.encoder.SetEncoderPosition(armAngle * armSpeed);
-}
diff --git a/src/main/cpp/ArmBehaviours.cpp b/src/main/cpp/ArmBehaviours.cpp
deleted file mode 100644
index 09b54d78..00000000
--- a/src/main/cpp/ArmBehaviours.cpp
+++ /dev/null
@@ -1,23 +0,0 @@
-// Copyright (c) 2023-2024 CurtinFRC
-// Open Source Software, you can modify it according to the terms
-// of the MIT License at the root of this project
-
-#include "ArmBehaviours.h"
-
-#include "Wombat.h"
-#include "subsystems/Arm.h"
-
-ArmManualControl::ArmManualControl(wom::Arm* arm, frc::XboxController* codriver)
-    : _arm(arm), _codriver(codriver) {
-  Controls(arm);
-}
-
-void ArmManualControl::OnTick(units::second_t dt) {
-  if (_codriver->GetXButtonPressed()) {
-    _arm->SetState(wom::ArmState::kRaw);
-  }
-
-  if (_arm->GetState() == wom::ArmState::kRaw) {
-    _arm->SetRaw(_codriver->GetRightY() * 6_V);
-  }
-}
diff --git a/src/main/cpp/Auto.cpp b/src/main/cpp/Auto.cpp
deleted file mode 100644
index cbf485e6..00000000
--- a/src/main/cpp/Auto.cpp
+++ /dev/null
@@ -1,305 +0,0 @@
-// Copyright (c) 2023-2024 CurtinFRC
-// Open Source Software, you can modify it according to the terms
-// of the MIT License at the root of this project
-
-#include "Auto.h"
-#include <utility>
-
-#include "AlphaArmBehaviour.h"
-#include "IntakeBehaviour.h"
-#include "ShooterBehaviour.h"
-#include "behaviour/Behaviour.h"
-#include "drivetrain/behaviours/SwerveBehaviours.h"
-#include "units/angle.h"
-#include "utils/Pathplanner.h"
-
-// Shoots starting note then moves out of starting position.
-wom::SwerveAutoBuilder* autos::InitCommands(wom::drivetrain::SwerveDrive* _swerveDrive, Shooter* _shooter, Intake* _intake, AlphaArm* _alphaArm) {
-    wom::Commands c = *new wom::Commands(
-    {{"ArmToSetPoint", [_alphaArm]() {
-            return wom::make<ArmToSetPoint>(_alphaArm, 20_deg);
-        }},
-                                         {"AutoShoot", [_shooter]() {
-            return wom::make<AutoShoot>(_shooter);
-        }},
-                                         {"AutoIntake", [_intake]() {
-            return wom::make<AutoIntake>(_intake);
-        }}
-    });
-
-    return new wom::utils::SwerveAutoBuilder(
-        _swerveDrive, "", c);
-}
-
-std::shared_ptr<behaviour::Behaviour> autos::Taxi(wom::SwerveAutoBuilder* builder) {
-  return builder->GetAutoRoutine("taxi");
-  // return behaviour::make<ArmToSetPoint>(_alphaArm, 0_deg);
-  // behaviour::make<AutoShoot>(_shooter);
-  // behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
-}
-
-std::shared_ptr<behaviour::Behaviour> autos::QuadrupleClose(wom::drivetrain::SwerveDrive* _swerveDrive,
-                                                            Shooter* _shooter, Intake* _intake,
-                                                            AlphaArm* _alphaArm) {
-  return behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
-  behaviour::make<AutoShoot>(_shooter);
-  behaviour::make<wom::drivetrain::behaviours::DrivebasePoseBehaviour>(
-      _swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false);
-  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
-  behaviour::make<AutoIntake>(_intake);
-  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
-  behaviour::make<AutoShoot>(_shooter);
-  behaviour::make<wom::drivetrain::behaviours::DrivebasePoseBehaviour>(
-      _swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false);
-  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
-  behaviour::make<AutoIntake>(_intake);
-  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
-  behaviour::make<AutoShoot>(_shooter);
-  behaviour::make<wom::drivetrain::behaviours::DrivebasePoseBehaviour>(
-      _swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false);
-  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
-  behaviour::make<AutoIntake>(_intake);
-  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
-  behaviour::make<AutoShoot>(_shooter);
-
-  /*
-   4N Close
- 1. Shoot starting note into speaker
- 2. Intake note from close note
- 3. Shoot note into speaker
- 4. Intake note from close floor note
- 5. Shoot note into speaker
- 6. Intake not from close floor
- 7. Shoot note
- */
-}
-
-std::shared_ptr<behaviour::Behaviour> autos::QuadrupleFar(wom::drivetrain::SwerveDrive* _swerveDrive,
-                                                          Shooter* _shooter, Intake* _intake,
-                                                          AlphaArm* _alphaArm) {
-  return behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
-  behaviour::make<AutoShoot>(_shooter);
-  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
-  behaviour::make<wom::drivetrain::behaviours::DrivebasePoseBehaviour>(
-      _swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false);
-  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
-  behaviour::make<AutoIntake>(_intake);
-  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
-  behaviour::make<wom::drivetrain::behaviours::DrivebasePoseBehaviour>(
-      _swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false);
-  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
-  behaviour::make<AutoShoot>(_shooter);
-  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
-  behaviour::make<wom::drivetrain::behaviours::DrivebasePoseBehaviour>(
-      _swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false);
-  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
-  behaviour::make<AutoIntake>(_intake);
-  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
-  behaviour::make<wom::drivetrain::behaviours::DrivebasePoseBehaviour>(
-      _swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false);
-  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
-  behaviour::make<AutoShoot>(_shooter);
-  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
-  behaviour::make<wom::drivetrain::behaviours::DrivebasePoseBehaviour>(
-      _swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false);
-  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
-  behaviour::make<AutoIntake>(_intake);
-  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
-  behaviour::make<wom::drivetrain::behaviours::DrivebasePoseBehaviour>(
-      _swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false);
-  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
-  behaviour::make<AutoShoot>(_shooter);
-  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
-  behaviour::make<wom::drivetrain::behaviours::DrivebasePoseBehaviour>(
-      _swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false);
-  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
-  behaviour::make<AutoIntake>(_intake);
-
-  /*
-    4N Far
-  1. Shoot start note in speaker
-  2. Drive to far note
-  3. Intake note
-  4. Drive back to shooting line
-  5. Shoot note into speaker
-  6. Drive to note
-  7. Intake note
-  8. Drive to shooting line
-  9. Shoot note
-  10. Drive to note
-  11. Intake note
-  12. Drive to shooting line
-  13. Shoot note
-  14. Drive to intake note (if possible)
-  */
-}
-
-std::shared_ptr<behaviour::Behaviour> autos::QuadrupleCloseDoubleFar(
-    wom::drivetrain::SwerveDrive* _swerveDrive, Shooter* _shooter, Intake* _intake, AlphaArm* _alphaArm) {
-  return behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
-  behaviour::make<AutoShoot>(_shooter);
-  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
-  behaviour::make<wom::drivetrain::behaviours::DrivebasePoseBehaviour>(
-      _swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false);
-  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
-  behaviour::make<AutoIntake>(_intake);
-  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
-  behaviour::make<wom::drivetrain::behaviours::DrivebasePoseBehaviour>(
-      _swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false);
-  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
-  behaviour::make<AutoShoot>(_shooter);
-  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
-  behaviour::make<wom::drivetrain::behaviours::DrivebasePoseBehaviour>(
-      _swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false);
-  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
-  behaviour::make<AutoIntake>(_intake);
-  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
-  behaviour::make<wom::drivetrain::behaviours::DrivebasePoseBehaviour>(
-      _swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false);
-  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
-  behaviour::make<AutoShoot>(_shooter);
-  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
-  behaviour::make<wom::drivetrain::behaviours::DrivebasePoseBehaviour>(
-      _swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false);
-  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
-  behaviour::make<AutoIntake>(_intake);
-  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
-  behaviour::make<wom::drivetrain::behaviours::DrivebasePoseBehaviour>(
-      _swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false);
-  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
-  behaviour::make<AutoShoot>(_shooter);
-  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
-  behaviour::make<wom::drivetrain::behaviours::DrivebasePoseBehaviour>(
-      _swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false);
-  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
-  behaviour::make<AutoIntake>(_intake);
-  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
-  behaviour::make<wom::drivetrain::behaviours::DrivebasePoseBehaviour>(
-      _swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false);
-  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
-  behaviour::make<AutoShoot>(_shooter);
-  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
-  behaviour::make<wom::drivetrain::behaviours::DrivebasePoseBehaviour>(
-      _swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false);
-  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
-  behaviour::make<AutoIntake>(_intake);
-  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
-  behaviour::make<wom::drivetrain::behaviours::DrivebasePoseBehaviour>(
-      _swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false);
-  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
-  behaviour::make<AutoShoot>(_shooter);
-  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
-  behaviour::make<wom::drivetrain::behaviours::DrivebasePoseBehaviour>(
-      _swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false);
-  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
-  behaviour::make<AutoIntake>(_intake);
-  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
-  behaviour::make<AutoShoot>(_shooter);
-
-  //   4N Close 2N Far
-  // 1. Shoot note
-  // 2. Drive to close note
-  // 3. Intake note
-  // 4. Shoot note
-  // 5. Drive to close note
-  // 6. Intake note
-  // 7. Shoot note
-  // 8. Drive to close note
-  // 9. Intake note
-  // 10. Shoot note
-  // 11. Drive to far note
-  // 12. Intake note
-  // 13. Drive to shooting line
-  // 14. Shoot note
-  // 15. Drive to far note
-  // 16. Intake note
-  // 17. Drive to shooting line
-  // 18. Shoot note
-}
-
-std::shared_ptr<behaviour::Behaviour> autos::QuadrupleCloseSingleFar(
-    wom::drivetrain::SwerveDrive* _swerveDrive, Shooter* _shooter, Intake* _intake, AlphaArm* _alphaArm) {
-  return behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
-  behaviour::make<AutoShoot>(_shooter);
-  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
-  behaviour::make<wom::drivetrain::behaviours::DrivebasePoseBehaviour>(
-      _swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false);
-  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
-  behaviour::make<AutoIntake>(_intake);
-  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
-  behaviour::make<wom::drivetrain::behaviours::DrivebasePoseBehaviour>(
-      _swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false);
-  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
-  behaviour::make<AutoShoot>(_shooter);
-  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
-  behaviour::make<wom::drivetrain::behaviours::DrivebasePoseBehaviour>(
-      _swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false);
-  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
-  behaviour::make<AutoIntake>(_intake);
-  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
-  behaviour::make<wom::drivetrain::behaviours::DrivebasePoseBehaviour>(
-      _swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false);
-  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
-  behaviour::make<AutoShoot>(_shooter);
-  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
-  behaviour::make<wom::drivetrain::behaviours::DrivebasePoseBehaviour>(
-      _swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false);
-  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
-  behaviour::make<AutoIntake>(_intake);
-  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
-  behaviour::make<wom::drivetrain::behaviours::DrivebasePoseBehaviour>(
-      _swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false);
-  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
-  behaviour::make<AutoShoot>(_shooter);
-  behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
-}
-
-// 4N Close 1N Far
-// 1. Shoot note
-// 2. Drive to close note
-// 3. Intake note
-// 4. Drive to speaker
-// 5. Shoot note
-// 6. Drive to close note
-// 7. Intake note
-// 8. Drive to speaker
-// 9. Shoot note
-// 10. Drive to close note
-// 11. Intake note
-// 12. Drive to speaker
-// 13. Shoot note
-// 14. Drive to far note
-// 15. Intake note
-// 15. Drive to speaker
-// 16. shoot
-
-//     /*
-//     TRAP AUTO
-//     1. Drive to trap
-//     2. Climb up
-//     3. Shoot note
-//     4. Climb down
-//     5. Drive to far note
-//     6. Intake note
-//     7. Drive to trap
-//     8. Climb up
-//     9. Shoot note
-//     10. Climb down
-//     11. Drive to far note
-//     12. Intake
-//     13. Drive to trap
-//     14. Climb up
-//     15. Shoot note
-//     16. Climb down
-//     */
-// }
-
-std::shared_ptr<behaviour::Behaviour> autos::AutoTest(wom::drivetrain::SwerveDrive* _swerveDrive,
-                                                      Shooter* _shooter, Intake* _intake,
-                                                      AlphaArm* _alphaArm) {
-  return behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
-  behaviour::make<wom::drivetrain::behaviours::DrivebasePoseBehaviour>(
-      _swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false);
-  behaviour::make<AutoShoot>(_shooter);
-  behaviour::make<AutoIntake>(_intake);
-}  // This auto is a test for auto to see if all things work.
diff --git a/src/main/cpp/Intake.cpp b/src/main/cpp/Intake.cpp
index ceed7722..7b8bfa00 100644
--- a/src/main/cpp/Intake.cpp
+++ b/src/main/cpp/Intake.cpp
@@ -130,17 +130,52 @@ void Intake::OnUpdate(units::second_t dt) {
  
   _config.IntakeGearbox.motorController->SetVoltage(_setVoltage);
 
+  switch (_behaviourState) {
+    case IntakeBehaviourState::kIdleing:
+    break;
+    case IntakeBehaviourState::kIntaking:
+    {
+      if (Intake::GetState() == IntakeState::kIdle) {
+        Intake::SetState(IntakeState::kIntake);
+      }
+    }
+    break;
+    case IntakeBehaviourState::kPassing:
+    {
+      if (Intake::GetState() == IntakeState::kIdle) {
+        Intake::SetState(IntakeState::kPass);
+      }
+    }
+    break;
+    case IntakeBehaviourState::kEjecting:
+    {
+      if (Intake::GetState() == IntakeState::kIdle) {
+        Intake::SetState(IntakeState::kIntake);
+      }
+    }
+    break;
+    case IntakeBehaviourState::kRawControl:
+    break;
+  }
+
 }
 
 void Intake::SetState(IntakeState state) {
   _state = state;
 }
+void Intake::SetBehaviourState(IntakeState behaviourState) {
+  _behaviourState = behaviourState;
+}
 void Intake::SetRaw(units::volt_t voltage) {
   _rawVoltage = voltage;
 }
 IntakeState Intake::GetState() {
   return _state;
 }
+IntakeBehaviourState Intake::GetBehaviourState() {
+  return _behaviourState;
+}
 void Intake::SetPidGoal(units::radians_per_second_t goal) {
   _goal = goal;
 }
+
diff --git a/src/main/cpp/IntakeBehaviour.cpp b/src/main/cpp/IntakeBehaviour.cpp
index 0cc02b8d..64e5c7ea 100644
--- a/src/main/cpp/IntakeBehaviour.cpp
+++ b/src/main/cpp/IntakeBehaviour.cpp
@@ -12,38 +12,20 @@ IntakeManualControl::IntakeManualControl(Intake* intake, frc::XboxController& co
 
 void IntakeManualControl::OnTick(units::second_t dt) {
 
-  /*
-  
-  if a button is held down:
-    set state to pass
-  else if right trigger is pressed:
-    set state to intaking
-  else if left trigger is pressed:
-    set state to eject
-  else
-    set state to idle
-  */
-
-
   if (_codriver.GetBButtonReleased()) {
-
     if (_rawControl) {
-      _rawControl = false;
-      _intaking = false;
-      _ejecting = false;
+      _intake->SetState(IntakeBehaviourState::kIdleing);
       _intake->SetState(IntakeState::kIdle);
     } else {
-      _rawControl = true;
-      _intaking = false;
-      _ejecting = false;
+      _intake->SetState(IntakeBehaviourState::kRawControl);
       _intake->SetState(IntakeState::kRaw);
     }
   } 
   
-  if (_rawControl) {
-    if (_codriver.GetRightTriggerAxis() > 0.1) {
+  if (_intake->GetState() == IntakeBehaviourState::kRawControl) {
+    if (_codriver.GetLeftTriggerAxis() > 0.1) {
       _intake->SetRaw(_codriver.GetRightTriggerAxis() * 10_V);
-    } else if (_codriver.GetLeftTriggerAxis() > 0.1) {
+    } else if (_codriver.GetRightTriggerAxis() > 0.1) {
       _intake->SetRaw(_codriver.GetLeftTriggerAxis() * -10_V);
     } else {
       _intake->SetRaw(0_V);
@@ -51,22 +33,40 @@ void IntakeManualControl::OnTick(units::second_t dt) {
     _intake->SetState(IntakeState::kRaw);
 
   } else {
-    
-    if (_intake->GetState() == IntakeState::kHold) {
-      if (_codriver.GetAButtonReleased()) {
-        _intake->SetState(IntakeState::kEject);
-      } else if (_codriver.GetYButtonReleased()) {
-        _intake->SetState(IntakeState::kPass);
-      } else {
+    if (_codriver.GetRightTriggerAxis() > 0.1) {
+      if (_intake->GetState() == IntakeBehaviourState::kIntaking) {
+        _intake->SetBehaviourState(IntakeBehaviourState::kIdleing);
         _intake->SetState(IntakeState::kIdle);
-      }
-    } else if (_intake->GetState() == IntakeState::kIdle) {
-      if (_codriver.GetRightTriggerAxis() > 0.1) {
-        _intake->SetState(IntakeState::kIntake);
       } else {
+        _intake->SetBehaviourState(IntakeBehaviourState::kIntaking)
+      }
+    }
+
+    if (_codriver.GetLeftTriggerAxis() > 0.1) {
+      if (_intake->GetState() == IntakeBehaviourState::kEjecting) {
+        _intake->SetBehaviourState(IntakeBehaviourState::kIdleing);
         _intake->SetState(IntakeState::kIdle);
+      } else {
+        _intake->SetBehaviourState(IntakeBehaviourState::kEjecting)
       }
     }
+
+
+    // if (_intake->GetState() == IntakeState::kHold) {
+    //   if (_codriver.GetAButtonReleased()) {
+    //     _intake->SetState(IntakeState::kEject);
+    //   } else if (_codriver.GetYButtonReleased()) {
+    //     _intake->SetState(IntakeState::kPass);
+    //   } else {
+    //     _intake->SetState(IntakeState::kIdle);
+    //   }
+    // } else if (_intake->GetState() == IntakeState::kIdle) {
+    //   if (_codriver.GetRightTriggerAxis() > 0.1) {
+    //     _intake->SetState(IntakeState::kIntake);
+    //   } else {
+    //     _intake->SetState(IntakeState::kIdle);
+    //   }
+    // }
   }
 }
 
diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp
index bbbbe086..15497bc1 100644
--- a/src/main/cpp/Robot.cpp
+++ b/src/main/cpp/Robot.cpp
@@ -21,16 +21,12 @@
 #include <units/velocity.h>
 #include <units/voltage.h>
 
-#include "Auto.h"
 #include "behaviour/HasBehaviour.h"
 #include <frc/kinematics/DifferentialDriveKinematics.h>
 #include <frc/controller/RamseteController.h>
 #include <frc/Timer.h>
-#include "ArmBehaviours.h"
 #include "behaviour/HasBehaviour.h"
 #include "frc/geometry/Pose2d.h"
-#include "vision/Vision.h"
-#include "vision/VisionBehaviours.h"
 
 static units::second_t lastPeriodic;
 
diff --git a/src/main/cpp/Shooter.cpp b/src/main/cpp/Shooter.cpp
deleted file mode 100644
index c3bebfd8..00000000
--- a/src/main/cpp/Shooter.cpp
+++ /dev/null
@@ -1,109 +0,0 @@
-// Copyright (c) 2023-2024 CurtinFRC
-// Open Source Software, you can modify it according to the terms
-// of the MIT License at the root of this project
-
-#include "Shooter.h"
-
-Shooter::Shooter(ShooterConfig config) : _config(config), _pid(config.path + "/pid", config.pidConfig) {}
-
-void Shooter::OnStart() {
-  _pid.Reset();
-}
-
-void Shooter::OnUpdate(units::second_t dt) {
-  // _pid.SetTolerance(0.5, 4);
-  table->GetEntry("Error").SetDouble(_pid.GetError().value());
-  // table->GetEntry("Acceleration Error").SetDouble(_pid.GetVelocityError());
-  table->GetEntry("SetPoint").SetDouble(_pid.GetSetpoint().value());
-  // table->GetEntry("Current
-  // Pos").SetDouble(_config.ShooterGearbox.encoder->GetEncoderAngularVelocity().value());
-  // table->GetEntry("EncoderValue").SetDouble(_config.ShooterGearbox.encoder->GetVelocityValue());
-  table->GetEntry("Shooting").SetString(_statename);
-  switch (_state) {
-    case ShooterState::kIdle: {
-      _statename = "Idle";
-      _pid.Reset();
-      holdVoltage = 0_V;
-      std::cout << "KIdle" << std::endl;
-      _setVoltage = 0_V;
-      // if (_shooterSensor.Get()) {
-      //   _state = ShooterState::kReverse;
-      // }
-    } break;
-    case ShooterState::kSpinUp: {
-      _statename = "SpinUp";
-      std::cout << "KSpinUp" << std::endl;
-      _pid.SetSetpoint(_goal);
-      units::volt_t pidCalculate =
-          units::volt_t{_pid.Calculate(_config.ShooterGearbox.encoder->GetEncoderAngularVelocity(), dt, 0_V)};
-      std::cout << "KShooting" << std::endl;
-
-      if (_pid.IsStable()) {
-        holdVoltage = pidCalculate;
-        std::cout << "STABLE" << std::endl;
-        _state = ShooterState::kShooting;
-      }
-
-      if (holdVoltage.value() == 0) {
-        _setVoltage = pidCalculate;
-      } else {
-        _setVoltage = holdVoltage;
-      }
-    } break;
-    case ShooterState::kShooting: {
-      _statename = "Shooting";
-
-      _pid.SetSetpoint(_goal);
-      units::volt_t pidCalculate =
-          units::volt_t{_pid.Calculate(_config.ShooterGearbox.encoder->GetEncoderAngularVelocity(), dt, 0_V)};
-      std::cout << "KShooting" << std::endl;
-
-      if (_pid.IsStable()) {
-        holdVoltage = pidCalculate;
-        std::cout << "STABLE" << std::endl;
-      }
-
-      if (holdVoltage.value() == 0) {
-        _setVoltage = pidCalculate;
-      } else {
-        _setVoltage = holdVoltage;
-      }
-    } break;
-
-    case ShooterState::kReverse: {
-      _statename = "Reverse";
-      _pid.Reset();
-      _setVoltage = -5_V;
-      std::cout << "KReverse" << std::endl;
-      // if (!_shooterSensor.Get()) {
-      //   SetState(ShooterState::kIdle);
-      // }
-    } break;
-    case ShooterState::kRaw: {
-      _statename = "Raw";
-      holdVoltage = 0_V;
-      _pid.Reset();
-      _setVoltage = _rawVoltage;
-      std::cout << "KRaw" << std::endl;
-      // if (_shooterSensor.Get()) {
-      //   SetState(ShooterState::kRaw);
-      // }
-    } break;
-    default: {
-      std::cout << "Error shooter in invalid state" << std::endl;
-    } break;
-  }
-  std::cout << "Voltage:" << _setVoltage.value() << std::endl;
-  _config.ShooterGearbox.motorController->SetVoltage(_setVoltage);
-}
-
-void Shooter::SetState(ShooterState state) {
-  _state = state;
-}
-void Shooter::SetRaw(units::volt_t voltage) {
-  _rawVoltage = voltage;
-  _state = ShooterState::kRaw;
-}
-void Shooter::SetPidGoal(units::radians_per_second_t goal) {
-  _goal = goal;
-}
diff --git a/src/main/cpp/ShooterBehaviour.cpp b/src/main/cpp/ShooterBehaviour.cpp
deleted file mode 100644
index 98ff1b21..00000000
--- a/src/main/cpp/ShooterBehaviour.cpp
+++ /dev/null
@@ -1,50 +0,0 @@
-// Copyright (c) 2023-2024 CurtinFRC
-// Open Source Software, you can modify it according to the terms
-// of the MIT License at the root of this project
-
-#include "ShooterBehaviour.h"
-
-ShooterManualControl::ShooterManualControl(Shooter* shooter, frc::XboxController* codriver)
-    : _shooter(shooter), _codriver(codriver) {
-  Controls(shooter);
-}
-
-void ShooterManualControl::OnTick(units::second_t dt) {
-  _shooter->table->GetEntry("RawControl").SetBoolean(_rawControl);
-
-  if (_codriver->GetAButtonReleased()) {
-    if (_rawControl) {
-      _rawControl = false;
-    } else {
-      _rawControl = true;
-    }
-  }
-
-  if (!_rawControl) {
-    if (_codriver->GetYButton()) {
-      _shooter->SetState(ShooterState::kSpinUp);
-      _shooter->SetPidGoal(10_rad_per_s);
-    }
-  } else {
-    if (_codriver->GetRightTriggerAxis() > 0.1) {
-      _shooter->SetRaw(_codriver->GetRightTriggerAxis() * 12_V);
-    } else if (_codriver->GetLeftTriggerAxis() > 0.1) {
-      _shooter->SetRaw(_codriver->GetLeftTriggerAxis() * -12_V);
-    } else {
-      _shooter->SetRaw(0_V);
-    }
-    _shooter->SetState(ShooterState::kRaw);
-  }
-}
-
-AutoShoot::AutoShoot(Shooter* shooter) : _shooter(shooter) {
-  Controls(shooter);
-}
-
-void AutoShoot::OnTick(units::second_t dt) {
-  // if (_intake->GetConfig().intakeSensor->Get() == 1) {
-  //   _intake->setState(IntakeState::kPass);
-  // } else if (_intake->GetConfig().magSensor->Get() == 0) {
-  //   _intake->setState(IntakeState::kIdle);
-  // }
-}
diff --git a/src/main/cpp/vision/Vision.cpp b/src/main/cpp/vision/Vision.cpp
deleted file mode 100644
index e170a8e8..00000000
--- a/src/main/cpp/vision/Vision.cpp
+++ /dev/null
@@ -1,415 +0,0 @@
-// // Copyright (c) 2023-2024 CurtinFRC
-// // Open Source Software, you can modify it according to the terms
-// // of the MIT License at the root of this project
-
-// #include "vision/Vision.h"
-
-// #include <utility>
-// #include <variant>
-
-// #include "units/length.h"
-// #include "units/math.h"
-// #include "vision/Limelight.h"
-
-// FMAP::FMAP(std::string path) : _path(path) {
-//   std::cout << "Parsing FMAP" << std::endl;
-
-//   deploy_dir = frc::filesystem::GetDeployDirectory();
-
-//   std::ifstream file(deploy_dir + "/" + _path);
-
-//   // parse the json file into a wpi::json object
-
-//   wpi::json j;
-
-//   file >> j;
-
-//   std::cout << j["fiducials"] << std::endl;
-
-//   // iterate through the json object and add each tag to the vector
-
-//   for (auto& fiducial : j["fiducials"]) {
-//     std::cout << "Loading AprilTag " << fiducial["id"] << std::endl;
-//     AprilTag tag;
-//     tag.id = fiducial["id"];
-//     tag.size = fiducial["size"];
-//     if (fiducial["unique"] == 1) {
-//       tag.unique = true;
-//     } else {
-//       tag.unique = false;
-//     }
-
-//     const auto& transform = fiducial["transform"];
-//     for (int i = 0; i < 4; ++i) {
-//       for (int j = 0; j < 4; ++j) {
-//         tag.transform[i][j] = transform[i * 4 + j];
-//       }
-//     }
-
-//     tag.yaw = units::radian_t{tag.transform[0][0]};
-//     tag.pitch = units::radian_t{tag.transform[1][1]};
-//     tag.roll = units::radian_t{tag.transform[2][2]};
-
-//     tag.pos = frc::Pose3d(units::meter_t{tag.transform[0][3]}, units::meter_t{tag.transform[1][3]},
-//                           units::meter_t{tag.transform[2][3]}, frc::Rotation3d(0_deg, 0_deg, 0_deg));
-
-//     _tags.push_back(tag);
-//   }
-
-//   file.close();
-
-//   std::cout << "Loaded " << _tags.size() << " tags" << std::endl;
-
-//   for (int i = 0; i < _tags.size(); i++) {
-//     std::cout << "Tag " << _tags[i].id << " is at: X: " << _tags[i].pos.X().value()
-//               << " Y: " << _tags[i].pos.Y().value() << " Z: " << _tags[i].pos.Z().value() << std::endl;
-//   }
-
-//   std::cout << "Loaded FMAP" << std::endl;
-// }
-
-// std::vector<AprilTag> FMAP::GetTags() {
-//   return _tags;
-// }
-
-// Vision::Vision(std::string name, FMAP fmap) : _fmap(fmap) {
-//   _limelight = new wom::Limelight(name);
-
-//   _limelight->SetPipeline(wom::LimelightPipeline::kPipeline0);
-// }
-
-// frc::Pose3d Vision::GetPose() {
-//   return _limelight->GetPose();
-// }
-
-// std::pair<units::meter_t, units::degree_t> Vision::GetDistanceToTarget(VisionTarget target) {
-//   SetMode(VisionModes::kAprilTag);
-
-//   std::vector<AprilTag> tags = _fmap.GetTags();
-
-//   for (int i = 0; i < tags.size(); i++) {
-//     if (tags[i].id == static_cast<int>(target)) {
-//       AprilTag tag = tags[i];
-//       // frc::Pose3d pose = _limelight->GetPose();
-//       // frc::Pose3d pose = frc::Pose3d(0_m, 0_m, 0_m, frc::Rotation3d(0_deg, 0_deg, 0_deg));
-
-//       SetMode(VisionModes::kAprilTag);
-
-//       // Get distance to target
-//       // Get current position from Limelight
-//       frc::Pose2d current_pose = _limelight->GetPose().ToPose2d();
-//       // frc::Pose2d current_pose = frc::Pose2d(0_m, 0_m, 0_deg);
-
-//       units::meter_t a = tag.pos.X() - current_pose.X();
-//       units::meter_t b = tag.pos.Y() - current_pose.Y();
-
-//       units::radian_t theta = units::math::atan2(b, a);
-
-//       // std::cout << "A: " << a.value() << ", B:" << b.value() << std::endl;
-//       units::meter_t r = units::meter_t{std::sqrt(std::pow(a.value(), 2) + std::pow(b.value(), 2))};
-
-//       // units::meter_t x = current_pose.X() + r * units::math::cos(theta);
-//       // units::meter_t y = current_pose.Y() + r * units::math::sin(theta);
-
-//       return std::make_pair(r, theta);
-//     }
-//   }
-
-//   return std::make_pair(0_m, 0_deg);
-// }
-
-// std::pair<units::meter_t, units::degree_t> Vision::GetDistanceToTarget(int id) {
-//   if (id < APRILTAGS_MIN || id > APRILTAGS_MAX) {
-//     return std::make_pair(0_m, 0_deg);
-//   }
-
-//   SetMode(VisionModes::kAprilTag);
-
-//   std::vector<AprilTag> tags = _fmap.GetTags();
-
-//   for (int i = 0; i < tags.size(); i++) {
-//     if (tags[i].id == id) {
-//       AprilTag tag = tags[i];
-//       // frc::Pose3d pose = _limelight->GetPose();
-//       // frc::Pose3d pose = frc::Pose3d(0_m, 0_m, 0_m, frc::Rotation3d(0_deg, 0_deg, 0_deg));
-
-//       SetMode(VisionModes::kAprilTag);
-
-//       // Get distance to target
-//       // Get current position from Limelight
-//       frc::Pose2d current_pose = _limelight->GetPose().ToPose2d();
-//       // frc::Pose2d current_pose = frc::Pose2d(0_m, 0_m, 0_deg);
-
-//       units::meter_t a = tag.pos.X() - current_pose.X();
-//       units::meter_t b = tag.pos.Y() - current_pose.Y();
-
-//       units::radian_t theta = units::math::atan2(b, a);
-
-//       // std::cout << "A: " << a.value() << ", B:" << b.value() << std::endl;
-//       units::meter_t r = units::meter_t{std::sqrt(std::pow(a.value(), 2) + std::pow(b.value(), 2))};
-
-//       units::meter_t x = current_pose.X() + r * units::math::cos(theta);
-//       units::meter_t y = current_pose.Y() + r * units::math::sin(theta);
-
-//       return std::make_pair(r, theta);
-//     }
-//   }
-
-//   return std::make_pair(0_m, 0_deg);
-// }
-
-// std::vector<AprilTag> Vision::GetTags() {
-//   return _fmap.GetTags();
-// }
-
-// frc::Pose2d Vision::AlignToTarget(VisionTarget target, units::meter_t offset, wom::SwerveDrive* swerveDrive) {
-//   SetMode(VisionModes::kAprilTag);
-
-//   // Get distance to target
-//   AprilTag tag = GetTags()[static_cast<int>(target) - 1];
-//   // Get current position from Limelight
-//   frc::Pose2d current_pose = _limelight->GetPose().ToPose2d();
-//   // frc::Pose2d current_pose = frc::Pose2d(0_m, 0_m, 0_deg);
-
-//   units::meter_t a = tag.pos.X() - current_pose.X();
-//   units::meter_t b = tag.pos.Y() - current_pose.Y();
-
-//   units::radian_t theta = units::math::atan2(b, a);
-
-//   // std::cout << "A: " << a.value() << ", B:" << b.value() << std::endl;
-//   units::meter_t r = units::meter_t{std::sqrt(std::pow(a.value(), 2) + std::pow(b.value(), 2))};
-
-//   units::meter_t x = current_pose.X() + r * units::math::cos(theta);
-//   units::meter_t y = current_pose.Y() + r * units::math::sin(theta);
-
-//   // reduce both offsets by the offset parameter (in relative amount)
-//   x += offset * units::math::cos(theta);
-//   y += offset * units::math::sin(theta);
-
-//   frc::Pose2d pose = frc::Pose2d(x, y, theta);
-
-//   // Print the results
-//   // std::cout << "Aligning to tag " << target << ": X: " << pose.X().value() << " Y: " << pose.Y().value() <<
-//   // " Angle: " << pose.Rotation().Degrees().value() << ". At offset " << offset.value() << "." << std::endl;
-
-//   // Set the new pose to the swerve drive
-//   // swerveDrive->SetPose(pose);
-
-//   return pose;
-// }
-
-// frc::Pose2d Vision::AlignToTarget(int target, units::meter_t offset, wom::SwerveDrive* swerveDrive) {
-//   SetMode(VisionModes::kAprilTag);
-
-//   // Get distance to target
-//   std::pair<units::meter_t, units::degree_t> distance = GetDistanceToTarget(target);
-//   AprilTag tag = GetTags()[target - 1];
-//   // Get current position from Limelight
-//   frc::Pose2d current_pose = _limelight->GetPose().ToPose2d();
-//   // frc::Pose2d current_pose = frc::Pose2d(0_m, 0_m, 0_deg);
-
-//   units::meter_t a = tag.pos.X() - current_pose.X();
-//   units::meter_t b = tag.pos.Y() - current_pose.Y();
-
-//   units::radian_t theta = units::math::atan2(b, a);
-
-//   // std::cout << "A: " << a.value() << ", B:" << b.value() << std::endl;
-//   units::meter_t r = units::meter_t{std::sqrt(std::pow(a.value(), 2) + std::pow(b.value(), 2))};
-
-//   units::meter_t x = current_pose.X() + r * units::math::cos(theta);
-//   units::meter_t y = current_pose.Y() + r * units::math::sin(theta);
-
-//   // reduce both offsets by the offset parameter (in relative amount)
-//   x += offset * units::math::cos(theta);
-//   y += offset * units::math::sin(theta);
-
-//   frc::Pose2d pose = frc::Pose2d(x, y, theta);
-
-//   // Print the results
-//   // std::cout << "Aligning to tag " << target << ": X: " << pose.X().value() << " Y: " << pose.Y().value() <<
-//   // " Angle: " << pose.Rotation().Degrees().value() << ". At offset " << offset.value() << "." << std::endl;
-
-//   // Set the new pose to the swerve drive
-//   // swerveDrive->SetPose(pose);
-
-//   return pose;
-// }
-
-// frc::Pose2d Vision::AlignToTarget(VisionTarget target, frc::Translation2d offset,
-//                                   wom::SwerveDrive* swerveDrive) {
-//   SetMode(VisionModes::kAprilTag);
-
-//   // Get distance to target
-//   AprilTag tag = GetTags()[static_cast<int>(target) - 1];
-//   // Get current position from Limelight
-//   frc::Pose2d current_pose = _limelight->GetPose().ToPose2d();
-//   // frc::Pose2d current_pose = frc::Pose2d(0_m, 0_m, 0_deg);
-
-//   units::meter_t a = tag.pos.X() - current_pose.X();
-//   units::meter_t b = tag.pos.Y() - current_pose.Y();
-
-//   units::radian_t theta = units::math::atan2(b, a);
-
-//   // std::cout << "A: " << a.value() << ", B:" << b.value() << std::endl;
-//   units::meter_t r = units::meter_t{std::sqrt(std::pow(a.value(), 2) + std::pow(b.value(), 2))};
-
-//   units::meter_t x = current_pose.X() + r * units::math::cos(theta);
-//   units::meter_t y = current_pose.Y() + r * units::math::sin(theta);
-
-//   // reduce both offsets by the offset parameter (in relative amount)
-//   x += offset.X();
-//   y += offset.Y();
-
-//   frc::Pose2d pose = frc::Pose2d(x, y, theta);
-
-//   // Print the results
-//   // std::cout << "Aligning to tag " << target << ": X: " << pose.X().value() << " Y: " << pose.Y().value() <<
-//   // " Angle: " << pose.Rotation().Degrees().value() << ". At offset " << offset.value() << "." << std::endl;
-
-//   // Set the new pose to the swerve drive
-//   // swerveDrive->SetPose(pose);
-
-//   return pose;
-// }
-
-// frc::Pose2d Vision::AlignToTarget(int target, frc::Translation2d offset, wom::SwerveDrive* swerveDrive) {
-//   SetMode(VisionModes::kAprilTag);
-
-//   // Get distance to target
-//   std::pair<units::meter_t, units::degree_t> distance = GetDistanceToTarget(target);
-//   AprilTag tag = GetTags()[target - 1];
-//   // Get current position from Limelight
-//   frc::Pose2d current_pose = _limelight->GetPose().ToPose2d();
-//   // frc::Pose2d current_pose = frc::Pose2d(0_m, 0_m, 0_deg);
-
-//   units::meter_t a = tag.pos.X() - current_pose.X();
-//   units::meter_t b = tag.pos.Y() - current_pose.Y();
-
-//   units::radian_t theta = units::math::atan2(b, a);
-
-//   // std::cout << "A: " << a.value() << ", B:" << b.value() << std::endl;
-//   units::meter_t r = units::meter_t{std::sqrt(std::pow(a.value(), 2) + std::pow(b.value(), 2))};
-
-//   units::meter_t x = current_pose.X() + r * units::math::cos(theta);
-//   units::meter_t y = current_pose.Y() + r * units::math::sin(theta);
-
-//   // reduce both offsets by the offset parameter (in relative amount)
-//   x += offset.X();
-//   y += offset.Y();
-
-//   frc::Pose2d pose = frc::Pose2d(x, y, theta);
-
-//   // Print the results
-//   // std::cout << "Aligning to tag " << target << ": X: " << pose.X().value() << " Y: " << pose.Y().value() <<
-//   // " Angle: " << pose.Rotation().Degrees().value() << ". At offset " << offset.value() << "." << std::endl;
-
-//   // Set the new pose to the swerve drive
-//   // swerveDrive->SetPose(pose);
-
-//   return pose;
-// }
-
-// frc::Pose2d Vision::TurnToTarget(int target, wom::SwerveDrive* swerveDrive) {
-//   AprilTag tag = GetTags()[target];
-
-//   units::degree_t angle = GetDistanceToTarget(target).second;
-
-//   frc::Pose2d current_pose = _limelight->GetPose().ToPose2d();
-//   // frc::Pose2d current_pose = frc::Pose2d();
-
-//   frc::Pose2d pose = frc::Pose2d(current_pose.X(), current_pose.Y(), angle);
-
-//   // std::cout << pose.Rotation().Degrees().value() << std::endl;
-
-//   // swerveDrive->SetPose(pose);
-
-//   return pose;
-// }
-
-// frc::Pose2d Vision::TurnToTarget(VisionTarget target, wom::SwerveDrive* swerveDrive) {
-//   AprilTag tag = GetTags()[static_cast<int>(target)];
-
-//   units::degree_t angle = GetDistanceToTarget(target).second;
-
-//   // frc::Pose2d current_pose = _limelight->GetPose().ToPose2d();
-
-//   frc::Pose2d current_pose = swerveDrive->GetPose();
-
-//   frc::Pose2d pose = frc::Pose2d(current_pose.X(), current_pose.Y(), angle);
-
-//   std::cout << pose.Rotation().Degrees().value() << std::endl;
-
-//   swerveDrive->SetPose(pose);
-// }
-
-// std::pair<frc::Pose2d, units::degree_t> Vision::GetAngleToObject(VisionTargetObjects object) {
-//   SetMode(VisionModes::kRing);
-
-//   if (TargetIsVisible(object)) {
-//     frc::Pose2d pose = _limelight->GetPose().ToPose2d();
-
-//     units::degree_t offset = units::degree_t{
-//         _limelight->GetOffset()
-//             .first};  // degrees are how much the robot has to turn for the target to be in the center
-
-//     pose.RotateBy(offset);
-
-//     return std::make_pair(pose, offset);
-//   } else {
-//     return std::make_pair(frc::Pose2d(), 0_deg);
-//   }
-// }
-
-// units::degree_t Vision::LockOn(VisionTargetObjects object) {
-//   SetMode(VisionModes::kRing);
-
-//   units::degree_t angle;
-
-//   if (TargetIsVisible(object)) {
-//     angle = GetAngleToObject(object).second;
-//   }
-
-//   return angle;
-// }
-
-// bool Vision::IsAtPose(frc::Pose3d pose, units::second_t dt) {
-//   return _limelight->IsAtSetPoseVision(pose, dt);
-// }
-
-// bool Vision::IsAtPose(frc::Pose2d pose, units::second_t dt) {
-//   return _limelight->IsAtSetPoseVision(pose, dt);
-// }
-
-// void Vision::SetMode(VisionModes mode) {
-//   if (static_cast<int>(mode) == _limelight->GetTargetingData(wom::LimelightTargetingData::kGetpipe)) {
-//     return;
-//   }
-
-//   switch (mode) {
-//     case VisionModes::kAprilTag: {
-//       _limelight->SetPipeline(wom::LimelightPipeline::kPipeline0);
-//       break;
-//     }
-//     case VisionModes::kRing: {
-//       _limelight->SetPipeline(wom::LimelightPipeline::kPipeline1);
-//       break;
-//     }
-//   }
-// }
-
-// bool Vision::TargetIsVisible(VisionTargetObjects target) {
-//   switch (target) {
-//     case VisionTargetObjects::kNote: {
-//       SetMode(VisionModes::kRing);
-//       break;
-//     }
-//   }
-
-//   return _limelight->HasTarget();
-// }
-
-// int Vision::CurrentAprilTag() {
-//   SetMode(VisionModes::kAprilTag);
-
-//   return _limelight->GetTargetingData(wom::LimelightTargetingData::kTid);
-// }
diff --git a/src/main/cpp/vision/VisionBehaviours.cpp b/src/main/cpp/vision/VisionBehaviours.cpp
deleted file mode 100644
index 07706ad2..00000000
--- a/src/main/cpp/vision/VisionBehaviours.cpp
+++ /dev/null
@@ -1,63 +0,0 @@
-// // Copyright (c) 2023-2024 CurtinFRC
-// // Open Source Software, you can modify it according to the terms
-// // of the MIT License at the root of this project
-
-// #include "vision/VisionBehaviours.h"
-
-// #include "frc/geometry/Pose2d.h"
-// #include "units/angle.h"
-// #include "vision/Vision.h"
-
-// AlignToAprilTag::AlignToAprilTag(Vision* vision, VisionTarget target, wom::SwerveDrive* swerveDrive,
-//                                  units::meter_t offset)
-//     : _vision(vision), _swerveDrive(swerveDrive), _target(target), _offset(offset) {}
-
-// void AlignToAprilTag::OnTick(units::second_t dt) {
-//   frc::Pose2d pose = _vision->AlignToTarget(_target, _offset, _swerveDrive);
-
-//   if (_vision->IsAtPose(pose, dt)) {
-//     SetDone();
-//   }
-// }
-
-// TurnToAprilTag::TurnToAprilTag(Vision* vision, VisionTarget target, wom::SwerveDrive* swerveDrive)
-//     : _vision(vision), _swerveDrive(swerveDrive), _target(target) {}
-
-// void TurnToAprilTag::OnTick(units::second_t dt) {
-//   frc::Pose2d pose = _vision->TurnToTarget(_target, _swerveDrive);
-
-//   if (_vision->IsAtPose(pose, dt)) {
-//     SetDone();
-//   }
-// }
-
-// LockOnToTarget::LockOnToTarget(Vision* vision, VisionTargetObjects target, wom::SwerveDrive* swerveDrive)
-//     : _vision(vision),
-//       _target(target),
-//       _camera(nullptr),
-//       _swerveDrive(swerveDrive),
-//       _type(VisionType::kLimelight) {}
-
-// LockOnToTarget::LockOnToTarget(wom::PhotonVision* vision, Vision* limelight, wom::SwerveDrive* swerveDrive)
-//     : _camera(vision), _swerveDrive(swerveDrive), _vision(limelight), _type(VisionType::kGenericCamera) {}
-
-// void LockOnToTarget::OnTick(units::second_t dt) {
-//   units::degree_t angle;
-
-//   switch (_type) {
-//     case VisionType::kLimelight: {
-//       angle = _vision->LockOn(_target);
-//     }
-//     case VisionType::kGenericCamera: {
-//       angle = units::degree_t{_camera->GetTargetPitch(_camera->GetTarget())};
-//     }
-//   }
-
-//   frc::Pose2d pose = frc::Pose2d(_swerveDrive->GetPose().X(), _swerveDrive->GetPose().Y(), angle);
-
-//   _swerveDrive->SetPose(pose);
-
-//   if (_vision->IsAtPose(pose, dt)) {
-//     SetDone();
-//   }
-// }
diff --git a/src/main/include/AlphaArm.h b/src/main/include/AlphaArm.h
deleted file mode 100644
index 5ac2d7c0..00000000
--- a/src/main/include/AlphaArm.h
+++ /dev/null
@@ -1,44 +0,0 @@
-// Copyright (c) 2023-2024 CurtinFRC
-// Open Source Software, you can modify it according to the terms
-// of the MIT License at the root of this project
-
-#pragma once
-#include <frc/DigitalInput.h>
-
-#include "Wombat.h"
-
-struct AlphaArmConfig {
-  wom::Gearbox alphaArmGearbox;
-  wom::Gearbox wristGearbox;
-};
-
-enum class AlphaArmState {
-  kIdle,
-  kIntakeAngle,
-  kAmpAngle,
-  kSpeakerAngle,
-  kForwardWrist,
-  kReverseWrist,
-  kRaw
-};
-
-class AlphaArm : public ::behaviour::HasBehaviour {
- public:
-  explicit AlphaArm(AlphaArmConfig config);
-
-  void OnUpdate(units::second_t dt);
-  void SetArmRaw(units::volt_t voltage);
-  void setWristRaw(units::volt_t voltage);
-  void SetState(AlphaArmState state);
-  AlphaArmConfig GetConfig();
-  // void SetRaw(units::volt_t voltage);
-
- private:
-  AlphaArmConfig _config;
-  AlphaArmState _state = AlphaArmState::kIdle;
-  units::volt_t _setAlphaArmVoltage = 0_V;
-  units::volt_t _setWristVoltage = 0_V;
-
-  units::volt_t _rawArmVoltage = 0_V;
-  units::volt_t _rawWristVoltage = 0_V;
-};
diff --git a/src/main/include/AlphaArmBehaviour.h b/src/main/include/AlphaArmBehaviour.h
deleted file mode 100644
index 904862b7..00000000
--- a/src/main/include/AlphaArmBehaviour.h
+++ /dev/null
@@ -1,34 +0,0 @@
-// Copyright (c) 2023-2024 CurtinFRC
-// Open Source Software, you can modify it according to the terms
-// of the MIT License at the root of this project
-
-#pragma once
-
-#pragma once
-#include <frc/XboxController.h>
-
-#include "AlphaArm.h"
-#include "Wombat.h"
-
-class AlphaArmManualControl : public behaviour::Behaviour {
- public:
-  explicit AlphaArmManualControl(AlphaArm* alphaArm, frc::XboxController* codriver);
-  void OnTick(units::second_t dt);
-
- private:
-  AlphaArm* _alphaArm;
-  frc::XboxController* _codriver;
-  // units::volt_t _rightStick = ((_codriver->GetRightY()>0.05 || _codriver->GetRightY() < -0.05
-  // )?_codriver->GetRightY():0) * 2_V;
-  bool _rawControl;
-};
-
-class ArmToSetPoint : public behaviour::Behaviour {
- public:
-  explicit ArmToSetPoint(AlphaArm* alphaArm, units::degree_t armAngle);
-  void OnTick(units::second_t dt);
-
- private:
-  AlphaArm* _alphaArm;
-  // double _armCurrentDegree;
-};
diff --git a/src/main/include/ArmBehaviours.h b/src/main/include/ArmBehaviours.h
deleted file mode 100644
index 2b2ab07b..00000000
--- a/src/main/include/ArmBehaviours.h
+++ /dev/null
@@ -1,23 +0,0 @@
-// Copyright (c) 2023-2024 CurtinFRC
-// Open Source Software, you can modify it according to the terms
-// of the MIT License at the root of this project
-
-#pragma once
-
-#include <frc/XboxController.h>
-
-#include "AlphaArm.h"
-#include "Wombat.h"
-
-class ArmManualControl : public behaviour::Behaviour {
- public:
-  explicit ArmManualControl(wom::Arm* arm, frc::XboxController* codriver);
-  void OnTick(units::second_t dt);
-
- private:
-  wom::Arm* _arm;
-  frc::XboxController* _codriver;
-  // units::volt_t _rightStick = ((_codriver->GetRightY()>0.05 || _codriver->GetRightY() < -0.05
-  // )?_codriver->GetRightY():0) * 2_V;
-  bool _rawControl;
-};
diff --git a/src/main/include/Auto.h b/src/main/include/Auto.h
deleted file mode 100644
index 5d37020a..00000000
--- a/src/main/include/Auto.h
+++ /dev/null
@@ -1,41 +0,0 @@
-// Copyright (c) 2023-2024 CurtinFRC
-// Open Source Software, you can modify it according to the terms
-// of the MIT License at the root of this project
-
-#pragma once
-
-#include <memory>
-
-#include "AlphaArmBehaviour.h"
-#include "IntakeBehaviour.h"
-#include "ShooterBehaviour.h"
-#include "Wombat.h"
-#include "utils/Pathplanner.h"
-
-
-namespace autos {
-
-// wom::Commands* _commands = nullptr;
-// wom::SwerveAutoBuilder* builder = nullptr;
-
-wom::utils::SwerveAutoBuilder* InitCommands(wom::drivetrain::SwerveDrive* _swerveDrive, Shooter* _shooter, Intake* _intake, AlphaArm* _alphaArm);
-
-std::shared_ptr<behaviour::Behaviour> AutoTest(wom::drivetrain::SwerveDrive* _swerveDrive, Shooter* _shooter,
-                                               Intake* _intake, AlphaArm* _alphaArm);
-
-std::shared_ptr<behaviour::Behaviour> Taxi(wom::utils::SwerveAutoBuilder* builder);
-
-std::shared_ptr<behaviour::Behaviour> QuadrupleClose(wom::drivetrain::SwerveDrive* _swerveDrive,
-                                                     Shooter* _shooter, Intake* _intake, AlphaArm* _alphaArm);
-
-std::shared_ptr<behaviour::Behaviour> QuadrupleFar(wom::drivetrain::SwerveDrive* _swerveDrive,
-                                                   Shooter* _shooter, Intake* _intake, AlphaArm* _alphaArm);
-
-std::shared_ptr<behaviour::Behaviour> QuadrupleCloseDoubleFar(wom::drivetrain::SwerveDrive* _swerveDrive,
-                                                              Shooter* _shooter, Intake* _intake,
-                                                              AlphaArm* _alphaArm);
-
-std::shared_ptr<behaviour::Behaviour> QuadrupleCloseSingleFar(wom::drivetrain::SwerveDrive* _swerveDrive,
-                                                              Shooter* _shooter, Intake* _intake,
-                                                              AlphaArm* _alphaArm);
-}  // namespace autos
diff --git a/src/main/include/Intake.h b/src/main/include/Intake.h
index a5d13f53..f4f21afd 100644
--- a/src/main/include/Intake.h
+++ b/src/main/include/Intake.h
@@ -22,6 +22,7 @@ struct IntakeConfig {
 };
 
 enum class IntakeState { kIdle, kRaw, kHold, kEject, kIntake, kPass, kPID};
+enum class IntakeBehaviourState { kIntaking, kEjecting, kIdleing, kRawControl, kPassing};
 
 class Intake : public behaviour::HasBehaviour {
  public:
@@ -30,15 +31,18 @@ class Intake : public behaviour::HasBehaviour {
   void OnUpdate(units::second_t dt);
 
   void SetState(IntakeState state);
+  void SetBehaviourState(IntakeBehaviourState behaviourState);
   void SetRaw(units::volt_t voltage);
   void SetPidGoal(units::radians_per_second_t goal);
   void OnStart();
   IntakeState GetState();
+  IntakeBehaviourState GetBehaviourState();
   IntakeConfig GetConfig();
 
  private:
   IntakeConfig _config;
   IntakeState _state = IntakeState::kIdle;
+  IntakeBehaviourState _behaviourState = IntakeBehaviourState::kIdleing;
 
   units::volt_t _rawVoltage = 0_V;
   std::string _stringStateName = "error";
diff --git a/src/main/include/Robot.h b/src/main/include/Robot.h
index c816e812..c66ede16 100644
--- a/src/main/include/Robot.h
+++ b/src/main/include/Robot.h
@@ -19,16 +19,10 @@
 #include <string>
 #include <vector>
 
-#include "AlphaArm.h"
-#include "AlphaArmBehaviour.h"
 #include "Intake.h"
 #include "IntakeBehaviour.h"
 #include "RobotMap.h"
-#include "Shooter.h"
-#include "ShooterBehaviour.h"
 #include "Wombat.h"
-#include "subsystems/Arm.h"
-#include "vision/Vision.h"
 
 class Robot : public frc::TimedRobot {
  public:
diff --git a/src/main/include/RobotMap.h b/src/main/include/RobotMap.h
index 50492d55..db0b73ad 100644
--- a/src/main/include/RobotMap.h
+++ b/src/main/include/RobotMap.h
@@ -25,10 +25,7 @@
 #include <ctre/phoenix6/Pigeon2.hpp>
 #include <ctre/phoenix6/TalonFX.hpp>
 
-#include "AlphaArm.h"
-#include "AlphaArmBehaviour.h"
 #include "Intake.h"
-#include "Shooter.h"
 #include "Wombat.h"
 #include "utils/Encoder.h"
 #include "utils/PID.h"
diff --git a/src/main/include/Shooter.h b/src/main/include/Shooter.h
deleted file mode 100644
index 6ef8eef4..00000000
--- a/src/main/include/Shooter.h
+++ /dev/null
@@ -1,49 +0,0 @@
-// Copyright (c) 2023-2024 CurtinFRC
-// Open Source Software, you can modify it according to the terms
-// of the MIT License at the root of this project
-
-#pragma once
-#include <frc/DigitalInput.h>
-#include <frc/controller/PIDController.h>
-#include <units/angular_velocity.h>
-
-#include <memory>
-#include <string>
-
-#include "Wombat.h"
-
-struct ShooterConfig {
-  std::string path;
-  wom::Gearbox ShooterGearbox;
-  // wom::PIDConfig<units::radians_per_second, units::volt> pidConfig;
-  // frc::DigitalInput* shooterSensor;
-  wom::PIDConfig<units::radians_per_second, units::volt> pidConfig;
-};
-
-enum class ShooterState { kIdle, kShooting, kSpinUp, kReverse, kRaw };
-
-class Shooter : public behaviour::HasBehaviour {
- public:
-  explicit Shooter(ShooterConfig config);
-
-  void OnStart();
-
-  void OnUpdate(units::second_t dt);
-  void SetState(ShooterState state);
-  void SetRaw(units::volt_t voltage);
-  void SetPidGoal(units::radians_per_second_t);
-  std::shared_ptr<nt::NetworkTable> table = nt::NetworkTableInstance::GetDefault().GetTable("Shooter");
-  ShooterConfig GetConfig() { return _config; }
-
- private:
-  ShooterConfig _config;
-  ShooterState _state = ShooterState::kRaw;
-  units::volt_t _rawVoltage;
-  units::radians_per_second_t _goal;
-  units::volt_t _setVoltage = 0_V;
-  wom::PIDController<units::radians_per_second, units::volt> _pid;
-  // frc::DigitalInput _shooterSensor{0};
-
-  units::volt_t holdVoltage = 0_V;
-  std::string _statename = "default";
-};
diff --git a/src/main/include/ShooterBehaviour.h b/src/main/include/ShooterBehaviour.h
deleted file mode 100644
index ba6d778a..00000000
--- a/src/main/include/ShooterBehaviour.h
+++ /dev/null
@@ -1,36 +0,0 @@
-// Copyright (c) 2023-2024 CurtinFRC
-// Open Source Software, you can modify it according to the terms
-// of the MIT License at the root of this project
-
-#pragma once
-
-#include <frc/XboxController.h>
-
-#include <memory>
-
-#include "Shooter.h"
-#include "Wombat.h"
-
-class ShooterManualControl : public behaviour::Behaviour {
- public:
-  ShooterManualControl(Shooter* shooter, frc::XboxController* codriver);
-
-  void OnTick(units::second_t dt) override;
-
- private:
-  Shooter* _shooter;
-
-  bool _rawControl = true;
-  std::shared_ptr<nt::NetworkTable> table =
-      nt::NetworkTableInstance::GetDefault().GetTable("Shooter Behaviour");
-  frc::XboxController* _codriver;
-};
-
-class AutoShoot : public behaviour::Behaviour {
- public:
-  explicit AutoShoot(Shooter* shooter);
-  void OnTick(units::second_t dt) override;
-
- private:
-  Shooter* _shooter;
-};
diff --git a/src/main/include/vision/Vision.h b/src/main/include/vision/Vision.h
deleted file mode 100644
index 420dcaa4..00000000
--- a/src/main/include/vision/Vision.h
+++ /dev/null
@@ -1,113 +0,0 @@
-// Copyright (c) 2023-2024 CurtinFRC
-// Open Source Software, you can modify it according to the terms
-// of the MIT License at the root of this project
-
-#pragma once
-
-#include <frc/Filesystem.h>
-#include <units/length.h>
-#include <units/math.h>
-#include <wpi/fs.h>
-#include <wpi/json.h>
-
-#include <string>
-#include <utility>
-#include <vector>
-
-#include "Wombat.h"
-#include "units/angle.h"
-
-#define APRILTAGS_MAX 16
-#define APRILTAGS_MIN 0
-
-enum class VisionTarget {
-  /*
-      Left is toward the blue side of the diagram here:
-     https://firstfrc.blob.core.windows.net/frc2024/FieldAssets/2024LayoutMarkingDiagram.pdf
-
-      The numbers are the numbers on the field diagram (and the numbers on the tags).
-  */
-  kBlueAmp = 6,
-  kBlueSpeakerCenter = 7,
-  kBlueSpeakerOffset = 8,
-  kBlueChain1 = 15,
-  kBlueChain2 = 16,
-  kBlueChain3 = 14,
-  kBlueSourceLeft = 1,
-  kBlueSourceRight = 2,
-
-  kRedAmp = 5,
-  kRedSpeakerCenter = 4,
-  kRedSpeakerOffset = 3,
-
-  kRedChain1 = 12,
-  kRedChain2 = 11,
-  kRedChain3 = 13,
-  kRedSourceLeft = 9,
-  kRedSourceRight = 10
-};
-
-enum class VisionModes { kAprilTag = 0, kRing = 1 };
-
-enum class VisionTargetObjects { kNote };
-
-struct AprilTag {
-  int id;
-  double size;
-  std::array<std::array<double, 4>, 4> transform;
-  bool unique;
-
-  frc::Pose3d pos;
-  units::radian_t yaw;
-  units::radian_t pitch;
-  units::radian_t roll;
-};
-
-class FMAP {
- public:
-  explicit FMAP(std::string path);
-
-  std::vector<AprilTag> GetTags();
-
- private:
-  std::vector<AprilTag> _tags;
-  std::string _path;
-  std::string deploy_dir;
-};
-
-class Vision {
- public:
-  Vision(std::string name, FMAP fmap);
-
-  std::pair<units::meter_t, units::degree_t> GetDistanceToTarget(VisionTarget target);
-  std::pair<units::meter_t, units::degree_t> GetDistanceToTarget(int id);
-
-  void SetMode(VisionModes mode);
-
-  frc::Pose3d GetPose();
-
-  frc::Pose2d AlignToTarget(VisionTarget target, units::meter_t offset, wom::SwerveDrive* swerveDrive);
-  frc::Pose2d AlignToTarget(int target, units::meter_t offset, wom::SwerveDrive* swerveDrive);
-
-  frc::Pose2d AlignToTarget(VisionTarget target, frc::Translation2d offset, wom::SwerveDrive* swerveDrive);
-  frc::Pose2d AlignToTarget(int target, frc::Translation2d offset, wom::SwerveDrive* swerveDrive);
-
-  frc::Pose2d TurnToTarget(int target, wom::SwerveDrive* swerveDrive);
-  frc::Pose2d TurnToTarget(VisionTarget target, wom::SwerveDrive* swerveDrive);
-
-  std::pair<frc::Pose2d, units::degree_t> GetAngleToObject(VisionTargetObjects object);
-  units::degree_t LockOn(VisionTargetObjects target);
-
-  std::vector<AprilTag> GetTags();
-
-  bool IsAtPose(frc::Pose3d pose, units::second_t dt);
-  bool IsAtPose(frc::Pose2d pose, units::second_t dt);
-
-  bool TargetIsVisible(VisionTargetObjects target);
-
-  int CurrentAprilTag();
-
- private:
-  wom::Limelight* _limelight;
-  FMAP _fmap;
-};
diff --git a/src/main/include/vision/VisionBehaviours.h b/src/main/include/vision/VisionBehaviours.h
deleted file mode 100644
index c10a092d..00000000
--- a/src/main/include/vision/VisionBehaviours.h
+++ /dev/null
@@ -1,56 +0,0 @@
-// Copyright (c) 2023-2024 CurtinFRC
-// Open Source Software, you can modify it according to the terms
-// of the MIT License at the root of this project
-
-#pragma once
-
-#include "Wombat.h"
-#include "behaviour/HasBehaviour.h"
-#include "drivetrain/SwerveDrive.h"
-#include "units/length.h"
-#include "units/time.h"
-#include "vision/Vision.h"
-
-enum class VisionType { kLimelight = 0, kGenericCamera = 1 };
-
-class AlignToAprilTag : public behaviour::Behaviour {
- public:
-  explicit AlignToAprilTag(Vision* vision, VisionTarget target, wom::SwerveDrive* swerveDrive,
-                           units::meter_t offset = 0_m);
-
-  void OnTick(units::second_t dt);
-
- private:
-  Vision* _vision;
-  wom::SwerveDrive* _swerveDrive;
-  VisionTarget _target;
-  units::meter_t _offset;
-};
-
-class TurnToAprilTag : public behaviour::Behaviour {
- public:
-  explicit TurnToAprilTag(Vision* vision, VisionTarget target, wom::SwerveDrive* swerveDrive);
-
-  void OnTick(units::second_t dt);
-
- private:
-  Vision* _vision;
-  wom::SwerveDrive* _swerveDrive;
-  VisionTarget _target;
-};
-
-class LockOnToTarget : public behaviour::Behaviour {
- public:
-  explicit LockOnToTarget(Vision* vision, VisionTargetObjects target, wom::SwerveDrive* swerveDrive);
-  explicit LockOnToTarget(wom::PhotonVision* camera, Vision* limelight, wom::SwerveDrive* swerveDrive);
-
-  void OnTick(units::second_t dt);
-
- private:
-  Vision* _vision;
-  wom::PhotonVision* _camera;
-
-  VisionTargetObjects _target;
-  wom::SwerveDrive* _swerveDrive;
-  VisionType _type;
-};

From 304e70926fddffa9801ef2bbe17fd46d07c09cbe Mon Sep 17 00:00:00 2001
From: Sean Chan <sean.chan3@student.education.wa.edu.au>
Date: Sat, 24 Feb 2024 16:57:25 +0800
Subject: [PATCH 170/207] Added switch case for the variables in behaviour.cpp,
 and fixwed build errors. Has not been tested on robot.

---
 src/main/cpp/Intake.cpp            |  2 +-
 src/main/cpp/IntakeBehaviour.cpp   | 14 +++++++-------
 src/main/include/IntakeBehaviour.h |  4 ----
 3 files changed, 8 insertions(+), 12 deletions(-)

diff --git a/src/main/cpp/Intake.cpp b/src/main/cpp/Intake.cpp
index 7b8bfa00..e8f0643f 100644
--- a/src/main/cpp/Intake.cpp
+++ b/src/main/cpp/Intake.cpp
@@ -163,7 +163,7 @@ void Intake::OnUpdate(units::second_t dt) {
 void Intake::SetState(IntakeState state) {
   _state = state;
 }
-void Intake::SetBehaviourState(IntakeState behaviourState) {
+void Intake::SetBehaviourState(IntakeBehaviourState behaviourState) {
   _behaviourState = behaviourState;
 }
 void Intake::SetRaw(units::volt_t voltage) {
diff --git a/src/main/cpp/IntakeBehaviour.cpp b/src/main/cpp/IntakeBehaviour.cpp
index 64e5c7ea..c638682c 100644
--- a/src/main/cpp/IntakeBehaviour.cpp
+++ b/src/main/cpp/IntakeBehaviour.cpp
@@ -14,15 +14,15 @@ void IntakeManualControl::OnTick(units::second_t dt) {
 
   if (_codriver.GetBButtonReleased()) {
     if (_rawControl) {
-      _intake->SetState(IntakeBehaviourState::kIdleing);
+      _intake->SetBehaviourState(IntakeBehaviourState::kIdleing);
       _intake->SetState(IntakeState::kIdle);
     } else {
-      _intake->SetState(IntakeBehaviourState::kRawControl);
+      _intake->SetBehaviourState(IntakeBehaviourState::kRawControl);
       _intake->SetState(IntakeState::kRaw);
     }
   } 
   
-  if (_intake->GetState() == IntakeBehaviourState::kRawControl) {
+  if (_intake->GetBehaviourState() == IntakeBehaviourState::kRawControl) {
     if (_codriver.GetLeftTriggerAxis() > 0.1) {
       _intake->SetRaw(_codriver.GetRightTriggerAxis() * 10_V);
     } else if (_codriver.GetRightTriggerAxis() > 0.1) {
@@ -34,20 +34,20 @@ void IntakeManualControl::OnTick(units::second_t dt) {
 
   } else {
     if (_codriver.GetRightTriggerAxis() > 0.1) {
-      if (_intake->GetState() == IntakeBehaviourState::kIntaking) {
+      if (_intake->GetBehaviourState() == IntakeBehaviourState::kIntaking) {
         _intake->SetBehaviourState(IntakeBehaviourState::kIdleing);
         _intake->SetState(IntakeState::kIdle);
       } else {
-        _intake->SetBehaviourState(IntakeBehaviourState::kIntaking)
+        _intake->SetBehaviourState(IntakeBehaviourState::kIntaking);
       }
     }
 
     if (_codriver.GetLeftTriggerAxis() > 0.1) {
-      if (_intake->GetState() == IntakeBehaviourState::kEjecting) {
+      if (_intake->GetBehaviourState() == IntakeBehaviourState::kEjecting) {
         _intake->SetBehaviourState(IntakeBehaviourState::kIdleing);
         _intake->SetState(IntakeState::kIdle);
       } else {
-        _intake->SetBehaviourState(IntakeBehaviourState::kEjecting)
+        _intake->SetBehaviourState(IntakeBehaviourState::kEjecting);
       }
     }
 
diff --git a/src/main/include/IntakeBehaviour.h b/src/main/include/IntakeBehaviour.h
index bd5a8f85..e300dc52 100644
--- a/src/main/include/IntakeBehaviour.h
+++ b/src/main/include/IntakeBehaviour.h
@@ -9,7 +9,6 @@
 #include "Intake.h"
 #include "Wombat.h"
 
-enum class IntakeBehaviourState { kIntaking, kPassing, kEjecting, kIdleing};
 
 class IntakeManualControl : public behaviour::Behaviour {
  public:
@@ -17,14 +16,11 @@ class IntakeManualControl : public behaviour::Behaviour {
 
   void SetBehaviourState(IntakeBehaviourState behaviourState);
   void OnTick(units::second_t dt) override;
-  IntakeBehaviourState GetBehaviourState();
 
  private:
   Intake* _intake;
   frc::XboxController& _codriver;
 
-  IntakeBehaviourState _behaviourState = IntakeBehaviourState::kIdleing;
-
   units::volt_t _rawVoltage;
   units::volt_t _setVoltage;
   bool _rawControl = true;

From a7d931adf2255a0a266d206214741e2cfa619dac Mon Sep 17 00:00:00 2001
From: Sean Chan <sean.chan3@student.education.wa.edu.au>
Date: Sun, 25 Feb 2024 11:03:50 +0800
Subject: [PATCH 171/207] resolved comments

---
 src/main/cpp/Intake.cpp            | 89 ++----------------------------
 src/main/cpp/IntakeBehaviour.cpp   | 85 +++++++++++++++++-----------
 src/main/include/Intake.h          | 10 +---
 src/main/include/IntakeBehaviour.h |  4 ++
 4 files changed, 64 insertions(+), 124 deletions(-)

diff --git a/src/main/cpp/Intake.cpp b/src/main/cpp/Intake.cpp
index e8f0643f..58f2fdd2 100644
--- a/src/main/cpp/Intake.cpp
+++ b/src/main/cpp/Intake.cpp
@@ -51,25 +51,15 @@ void Intake::OnUpdate(units::second_t dt) {
     {
       _stringStateName = "Hold";
 
-      _setVoltage = 0_V;
-
       if (_config.intakeSensor->Get() == true) {
         SetState(IntakeState::kIdle);
       }
       
-      _pid.SetSetpoint(_goal);
+      _pid.SetSetpoint(0_rad_per_s);
       units::volt_t pidCalculate =
           units::volt_t{_pid.Calculate(_config.IntakeGearbox.encoder->GetEncoderAngularVelocity(), dt, 0_V)};
-      if (_pid.IsStable()) {
-        holdVoltage = pidCalculate;
-        std::cout << "STABLE" << std::endl;
-      }
-      if (holdVoltage.value() == 0) {
-        _setVoltage = pidCalculate;
-      } else {
-        _setVoltage = holdVoltage;
-      }
-    } 
+      _setVoltage = pidCalculate;
+    }
     break;
 
     case IntakeState::kIntake: 
@@ -90,92 +80,25 @@ void Intake::OnUpdate(units::second_t dt) {
       if (_config.intakeSensor->Get() == true) {
         SetState(IntakeState::kIdle);
       }
-      _pid.SetSetpoint(_goal);
-      units::volt_t pidCalculate =
-          units::volt_t{_pid.Calculate(_config.IntakeGearbox.encoder->GetEncoderAngularVelocity(), dt, 0_V)};
-      if (_pid.IsStable()) {
-        passVoltage = pidCalculate;
-        std::cout << "STABLE" << std::endl;
-      }
-
-      if (passVoltage.value() == 0) {
-        _setVoltage = pidCalculate;
-      } else {
-        _setVoltage = passVoltage;
-      }
     }
     break;
-
-    case IntakeState::kPID:
-    {
-      _pid.SetSetpoint(_goal);
-        units::volt_t pidCalculate =
-            units::volt_t{_pid.Calculate(_config.IntakeGearbox.encoder->GetEncoderAngularVelocity(), dt, 0_V)};
-      if (_pid.IsStable()) {
-          holdVoltage = pidCalculate;
-          std::cout << "STABLE" << std::endl;
-        }
-      if (holdVoltage.value() == 0) {
-        _setVoltage = pidCalculate;
-      } else {
-        _setVoltage = holdVoltage;
-      }
-    }
   }
   _table->GetEntry("State: ").SetString(_stringStateName);
   _table->GetEntry("Motor Voltage: ").SetDouble(_setVoltage.value());
   _table->GetEntry("Intake Sensor: ").SetBoolean(_config.intakeSensor->Get());
-  _table->GetEntry("Error").SetDouble(_pid.GetError().value());
-  _table->GetEntry("SetPoint").SetDouble(_pid.GetSetpoint().value());
+  _table->GetEntry("Error: ").SetDouble(_pid.GetError().value());
+  _table->GetEntry("SetPoint: ").SetDouble(_pid.GetSetpoint().value());
+  _table->GetEntry("Encoder: ").SetDouble(_config.IntakeGearbox.encoder->GetEncoderAngularVelocity().value());
  
   _config.IntakeGearbox.motorController->SetVoltage(_setVoltage);
-
-  switch (_behaviourState) {
-    case IntakeBehaviourState::kIdleing:
-    break;
-    case IntakeBehaviourState::kIntaking:
-    {
-      if (Intake::GetState() == IntakeState::kIdle) {
-        Intake::SetState(IntakeState::kIntake);
-      }
-    }
-    break;
-    case IntakeBehaviourState::kPassing:
-    {
-      if (Intake::GetState() == IntakeState::kIdle) {
-        Intake::SetState(IntakeState::kPass);
-      }
-    }
-    break;
-    case IntakeBehaviourState::kEjecting:
-    {
-      if (Intake::GetState() == IntakeState::kIdle) {
-        Intake::SetState(IntakeState::kIntake);
-      }
-    }
-    break;
-    case IntakeBehaviourState::kRawControl:
-    break;
-  }
-
 }
 
 void Intake::SetState(IntakeState state) {
   _state = state;
 }
-void Intake::SetBehaviourState(IntakeBehaviourState behaviourState) {
-  _behaviourState = behaviourState;
-}
 void Intake::SetRaw(units::volt_t voltage) {
   _rawVoltage = voltage;
 }
 IntakeState Intake::GetState() {
   return _state;
 }
-IntakeBehaviourState Intake::GetBehaviourState() {
-  return _behaviourState;
-}
-void Intake::SetPidGoal(units::radians_per_second_t goal) {
-  _goal = goal;
-}
-
diff --git a/src/main/cpp/IntakeBehaviour.cpp b/src/main/cpp/IntakeBehaviour.cpp
index c638682c..4a33ef95 100644
--- a/src/main/cpp/IntakeBehaviour.cpp
+++ b/src/main/cpp/IntakeBehaviour.cpp
@@ -12,17 +12,52 @@ IntakeManualControl::IntakeManualControl(Intake* intake, frc::XboxController& co
 
 void IntakeManualControl::OnTick(units::second_t dt) {
 
+  switch (_behaviourState) {
+    case IntakeBehaviourState::kIdleing:
+    {
+      if (_intake->GetState() != IntakeState::kIdle) {
+        _intake->SetState(IntakeState::kIdle);
+      }
+    }
+    break;
+    case IntakeBehaviourState::kIntaking:
+    {
+      if (_intake->GetState() == IntakeState::kIdle) {
+        _intake->SetState(IntakeState::kIntake);
+      }
+    }
+    break;
+    case IntakeBehaviourState::kPassing:
+    {
+      if (_intake->GetState() == IntakeState::kIdle) {
+        _intake->SetState(IntakeState::kPass);
+      }
+    }
+    break;
+    case IntakeBehaviourState::kEjecting:
+    {
+      if (_intake->GetState() == IntakeState::kIdle) {
+        _intake->SetState(IntakeState::kIntake);
+      }
+    }
+    break;
+    case IntakeBehaviourState::kRawControl:
+    {
+      if (_intake->GetState() != IntakeState::kRaw)
+      _intake->SetState(IntakeState::kRaw);
+    }
+    break;
+  }
+
   if (_codriver.GetBButtonReleased()) {
     if (_rawControl) {
-      _intake->SetBehaviourState(IntakeBehaviourState::kIdleing);
-      _intake->SetState(IntakeState::kIdle);
+      IntakeManualControl::SetBehaviourState(IntakeBehaviourState::kIdleing);
     } else {
-      _intake->SetBehaviourState(IntakeBehaviourState::kRawControl);
-      _intake->SetState(IntakeState::kRaw);
+      IntakeManualControl::SetBehaviourState(IntakeBehaviourState::kRawControl);
     }
   } 
   
-  if (_intake->GetBehaviourState() == IntakeBehaviourState::kRawControl) {
+  if (IntakeManualControl::GetBehaviourState() == IntakeBehaviourState::kRawControl) {
     if (_codriver.GetLeftTriggerAxis() > 0.1) {
       _intake->SetRaw(_codriver.GetRightTriggerAxis() * 10_V);
     } else if (_codriver.GetRightTriggerAxis() > 0.1) {
@@ -30,48 +65,34 @@ void IntakeManualControl::OnTick(units::second_t dt) {
     } else {
       _intake->SetRaw(0_V);
     }
-    _intake->SetState(IntakeState::kRaw);
 
   } else {
     if (_codriver.GetRightTriggerAxis() > 0.1) {
-      if (_intake->GetBehaviourState() == IntakeBehaviourState::kIntaking) {
-        _intake->SetBehaviourState(IntakeBehaviourState::kIdleing);
-        _intake->SetState(IntakeState::kIdle);
+      if (IntakeManualControl::GetBehaviourState() == IntakeBehaviourState::kIntaking) {
+        IntakeManualControl::SetBehaviourState(IntakeBehaviourState::kIdleing);
       } else {
-        _intake->SetBehaviourState(IntakeBehaviourState::kIntaking);
+        IntakeManualControl::SetBehaviourState(IntakeBehaviourState::kIntaking);
       }
     }
 
     if (_codriver.GetLeftTriggerAxis() > 0.1) {
-      if (_intake->GetBehaviourState() == IntakeBehaviourState::kEjecting) {
-        _intake->SetBehaviourState(IntakeBehaviourState::kIdleing);
-        _intake->SetState(IntakeState::kIdle);
+      if (IntakeManualControl::GetBehaviourState() == IntakeBehaviourState::kEjecting) {
+        IntakeManualControl::SetBehaviourState(IntakeBehaviourState::kIdleing);
       } else {
-        _intake->SetBehaviourState(IntakeBehaviourState::kEjecting);
+        IntakeManualControl::SetBehaviourState(IntakeBehaviourState::kEjecting);
       }
     }
-
-
-    // if (_intake->GetState() == IntakeState::kHold) {
-    //   if (_codriver.GetAButtonReleased()) {
-    //     _intake->SetState(IntakeState::kEject);
-    //   } else if (_codriver.GetYButtonReleased()) {
-    //     _intake->SetState(IntakeState::kPass);
-    //   } else {
-    //     _intake->SetState(IntakeState::kIdle);
-    //   }
-    // } else if (_intake->GetState() == IntakeState::kIdle) {
-    //   if (_codriver.GetRightTriggerAxis() > 0.1) {
-    //     _intake->SetState(IntakeState::kIntake);
-    //   } else {
-    //     _intake->SetState(IntakeState::kIdle);
-    //   }
-    // }
   }
 }
 
+IntakeBehaviourState IntakeManualControl::IntakeManualControl::GetBehaviourState() {
+  return _behaviourState;
+}
+void IntakeManualControl::IntakeManualControl::SetBehaviourState(IntakeBehaviourState behaviourState) {
+  _behaviourState = behaviourState;
+}
+
 AutoIntake::AutoIntake(Intake* intake) : _intake(intake) {
   Controls(intake);
 }
-
 void AutoIntake::OnTick(units::second_t dt) {}
diff --git a/src/main/include/Intake.h b/src/main/include/Intake.h
index f4f21afd..db60ec25 100644
--- a/src/main/include/Intake.h
+++ b/src/main/include/Intake.h
@@ -21,8 +21,7 @@ struct IntakeConfig {
   wom::PIDConfig<units::radians_per_second, units::volt> pidConfig;
 };
 
-enum class IntakeState { kIdle, kRaw, kHold, kEject, kIntake, kPass, kPID};
-enum class IntakeBehaviourState { kIntaking, kEjecting, kIdleing, kRawControl, kPassing};
+enum class IntakeState { kIdle, kRaw, kHold, kEject, kIntake, kPass};
 
 class Intake : public behaviour::HasBehaviour {
  public:
@@ -31,26 +30,19 @@ class Intake : public behaviour::HasBehaviour {
   void OnUpdate(units::second_t dt);
 
   void SetState(IntakeState state);
-  void SetBehaviourState(IntakeBehaviourState behaviourState);
   void SetRaw(units::volt_t voltage);
-  void SetPidGoal(units::radians_per_second_t goal);
   void OnStart();
   IntakeState GetState();
-  IntakeBehaviourState GetBehaviourState();
   IntakeConfig GetConfig();
 
  private:
   IntakeConfig _config;
   IntakeState _state = IntakeState::kIdle;
-  IntakeBehaviourState _behaviourState = IntakeBehaviourState::kIdleing;
 
   units::volt_t _rawVoltage = 0_V;
   std::string _stringStateName = "error";
   units::volt_t _setVoltage = 0_V;
-  units::volt_t holdVoltage = 0_V;
-  units::volt_t passVoltage = 0_V;
 
-  units::radians_per_second_t _goal;
   wom::PIDController<units::radians_per_second, units::volt> _pid;
   
   std::shared_ptr<nt::NetworkTable> _table = nt::NetworkTableInstance::GetDefault().GetTable("Intake");
diff --git a/src/main/include/IntakeBehaviour.h b/src/main/include/IntakeBehaviour.h
index e300dc52..b414d4be 100644
--- a/src/main/include/IntakeBehaviour.h
+++ b/src/main/include/IntakeBehaviour.h
@@ -9,6 +9,7 @@
 #include "Intake.h"
 #include "Wombat.h"
 
+enum class IntakeBehaviourState { kIntaking, kEjecting, kIdleing, kRawControl, kPassing};
 
 class IntakeManualControl : public behaviour::Behaviour {
  public:
@@ -17,9 +18,12 @@ class IntakeManualControl : public behaviour::Behaviour {
   void SetBehaviourState(IntakeBehaviourState behaviourState);
   void OnTick(units::second_t dt) override;
 
+  IntakeBehaviourState GetBehaviourState();
+
  private:
   Intake* _intake;
   frc::XboxController& _codriver;
+  IntakeBehaviourState _behaviourState = IntakeBehaviourState::kIdleing;
 
   units::volt_t _rawVoltage;
   units::volt_t _setVoltage;

From abec6e50b91204c876a5c18c6451150ae5def1a9 Mon Sep 17 00:00:00 2001
From: Sean Chan <sean.chan3@student.education.wa.edu.au>
Date: Sun, 25 Feb 2024 18:30:19 +0800
Subject: [PATCH 172/207] Tested on robot, intake V1 finished

---
 src/main/cpp/Intake.cpp            | 32 +++++------
 src/main/cpp/IntakeBehaviour.cpp   | 87 +++++++++---------------------
 src/main/include/Intake.h          |  3 +-
 src/main/include/IntakeBehaviour.h | 16 ++----
 src/main/include/RobotMap.h        |  2 +-
 5 files changed, 47 insertions(+), 93 deletions(-)

diff --git a/src/main/cpp/Intake.cpp b/src/main/cpp/Intake.cpp
index 58f2fdd2..128cdd2c 100644
--- a/src/main/cpp/Intake.cpp
+++ b/src/main/cpp/Intake.cpp
@@ -4,7 +4,7 @@
 
 #include "Intake.h"
 
-Intake::Intake(IntakeConfig config) : _config(config), _pid(config.path + "/pid", config.pidConfig) {}
+Intake::Intake(IntakeConfig config) : _config(config), _pid(frc::PIDController (0.0125, 0, 0, 0.05_s)), _pidPosition(frc::PIDController (0.0125, 0, 0, 0.05_s)) {}
 
 IntakeConfig Intake::GetConfig() {
   return _config;
@@ -38,57 +38,57 @@ void Intake::OnUpdate(units::second_t dt) {
 
     case IntakeState::kEject: 
     {
-      _stringStateName = "Eject";
-      _setVoltage = 7_V;
-      _pid.Reset();
       if (_config.intakeSensor->Get() == true) {
         SetState(IntakeState::kIdle); 
       }
+      _stringStateName = "Eject";
+      _setVoltage = -4_V;
+      _pid.Reset();
     } 
     break;
 
     case IntakeState::kHold: 
     {
-      _stringStateName = "Hold";
-
       if (_config.intakeSensor->Get() == true) {
         SetState(IntakeState::kIdle);
       }
-      
-      _pid.SetSetpoint(0_rad_per_s);
+      _pid.SetSetpoint(0);
       units::volt_t pidCalculate =
-          units::volt_t{_pid.Calculate(_config.IntakeGearbox.encoder->GetEncoderAngularVelocity(), dt, 0_V)};
+          units::volt_t{_pid.Calculate(_config.IntakeGearbox.encoder->GetEncoderAngularVelocity().value())};
+      // units::volt_t pidCalculate =
+      //     units::volt_t{_pidPosition.Calculate(_config.IntakeGearbox.encoder->GetEncoderPosition().value())};
       _setVoltage = pidCalculate;
+      _stringStateName = "Hold";
     }
     break;
 
     case IntakeState::kIntake: 
     {
-      _stringStateName = "Intake";
-      _setVoltage = -7_V; 
       if (_config.intakeSensor->Get() == false) {
-
         SetState(IntakeState::kHold);
       }
+      _stringStateName = "Intake";
+      _setVoltage = 4_V; 
     } 
     break;
 
     case IntakeState::kPass: 
     {
-      _stringStateName = "Pass";
-      _setVoltage = -7_V;
       if (_config.intakeSensor->Get() == true) {
         SetState(IntakeState::kIdle);
       }
+      _stringStateName = "Pass";
+      _setVoltage = 4_V;
     }
     break;
   }
   _table->GetEntry("State: ").SetString(_stringStateName);
   _table->GetEntry("Motor Voltage: ").SetDouble(_setVoltage.value());
   _table->GetEntry("Intake Sensor: ").SetBoolean(_config.intakeSensor->Get());
-  _table->GetEntry("Error: ").SetDouble(_pid.GetError().value());
-  _table->GetEntry("SetPoint: ").SetDouble(_pid.GetSetpoint().value());
+  _table->GetEntry("Error: ").SetDouble(_pid.GetPositionError());
+  _table->GetEntry("SetPoint: ").SetDouble(_pid.GetSetpoint());
   _table->GetEntry("Encoder: ").SetDouble(_config.IntakeGearbox.encoder->GetEncoderAngularVelocity().value());
+  // _table->GetEntry("Encoder: ").SetDouble(_config.IntakeGearbox.encoder->GetEncoderPosition().value());
  
   _config.IntakeGearbox.motorController->SetVoltage(_setVoltage);
 }
diff --git a/src/main/cpp/IntakeBehaviour.cpp b/src/main/cpp/IntakeBehaviour.cpp
index 4a33ef95..05b3bf5e 100644
--- a/src/main/cpp/IntakeBehaviour.cpp
+++ b/src/main/cpp/IntakeBehaviour.cpp
@@ -12,86 +12,49 @@ IntakeManualControl::IntakeManualControl(Intake* intake, frc::XboxController& co
 
 void IntakeManualControl::OnTick(units::second_t dt) {
 
-  switch (_behaviourState) {
-    case IntakeBehaviourState::kIdleing:
-    {
-      if (_intake->GetState() != IntakeState::kIdle) {
-        _intake->SetState(IntakeState::kIdle);
-      }
-    }
-    break;
-    case IntakeBehaviourState::kIntaking:
-    {
-      if (_intake->GetState() == IntakeState::kIdle) {
-        _intake->SetState(IntakeState::kIntake);
-      }
-    }
-    break;
-    case IntakeBehaviourState::kPassing:
-    {
-      if (_intake->GetState() == IntakeState::kIdle) {
-        _intake->SetState(IntakeState::kPass);
-      }
-    }
-    break;
-    case IntakeBehaviourState::kEjecting:
-    {
-      if (_intake->GetState() == IntakeState::kIdle) {
-        _intake->SetState(IntakeState::kIntake);
-      }
-    }
-    break;
-    case IntakeBehaviourState::kRawControl:
-    {
-      if (_intake->GetState() != IntakeState::kRaw)
-      _intake->SetState(IntakeState::kRaw);
-    }
-    break;
-  }
-
-  if (_codriver.GetBButtonReleased()) {
+  if (_codriver.GetBackButtonReleased()) {
     if (_rawControl) {
-      IntakeManualControl::SetBehaviourState(IntakeBehaviourState::kIdleing);
+      _rawControl = false;
+      _intake->SetState(IntakeState::kIdle);
     } else {
-      IntakeManualControl::SetBehaviourState(IntakeBehaviourState::kRawControl);
+      _rawControl = true;
+      _intake->SetState(IntakeState::kRaw);
     }
   } 
   
-  if (IntakeManualControl::GetBehaviourState() == IntakeBehaviourState::kRawControl) {
+  if (_rawControl) {
+    _intake->SetState(IntakeState::kRaw);
     if (_codriver.GetLeftTriggerAxis() > 0.1) {
-      _intake->SetRaw(_codriver.GetRightTriggerAxis() * 10_V);
-    } else if (_codriver.GetRightTriggerAxis() > 0.1) {
-      _intake->SetRaw(_codriver.GetLeftTriggerAxis() * -10_V);
+      _intake->SetRaw(_codriver.GetLeftTriggerAxis() * 10_V);
+    }
+    else if (_codriver.GetRightTriggerAxis() > 0.1) {
+      _intake->SetRaw(_codriver.GetRightTriggerAxis() * -10_V);
     } else {
       _intake->SetRaw(0_V);
     }
-
   } else {
-    if (_codriver.GetRightTriggerAxis() > 0.1) {
-      if (IntakeManualControl::GetBehaviourState() == IntakeBehaviourState::kIntaking) {
-        IntakeManualControl::SetBehaviourState(IntakeBehaviourState::kIdleing);
+    if (_codriver.GetXButtonReleased()) {
+      if (_intake->GetState() == IntakeState::kIdle) {
+        _intake->SetState(IntakeState::kIntake);
       } else {
-        IntakeManualControl::SetBehaviourState(IntakeBehaviourState::kIntaking);
+        _intake->SetState(IntakeState::kIdle);
       }
-    }
-
-    if (_codriver.GetLeftTriggerAxis() > 0.1) {
-      if (IntakeManualControl::GetBehaviourState() == IntakeBehaviourState::kEjecting) {
-        IntakeManualControl::SetBehaviourState(IntakeBehaviourState::kIdleing);
+    } else if (_codriver.GetAButtonReleased()) {
+      if (_intake->GetState() == IntakeState::kHold) {
+        _intake->SetState(IntakeState::kPass);
       } else {
-        IntakeManualControl::SetBehaviourState(IntakeBehaviourState::kEjecting);
+        _intake->SetState(IntakeState::kIdle);
+      }
+    } else if (_codriver.GetBButtonReleased()) {
+      if (_intake->GetState() == IntakeState::kHold) {
+        _intake->SetState(IntakeState::kEject);
+      } else {
+        _intake->SetState(IntakeState::kIdle);
       }
     }
   }
 }
 
-IntakeBehaviourState IntakeManualControl::IntakeManualControl::GetBehaviourState() {
-  return _behaviourState;
-}
-void IntakeManualControl::IntakeManualControl::SetBehaviourState(IntakeBehaviourState behaviourState) {
-  _behaviourState = behaviourState;
-}
-
 AutoIntake::AutoIntake(Intake* intake) : _intake(intake) {
   Controls(intake);
 }
diff --git a/src/main/include/Intake.h b/src/main/include/Intake.h
index db60ec25..00ddf45b 100644
--- a/src/main/include/Intake.h
+++ b/src/main/include/Intake.h
@@ -43,7 +43,8 @@ class Intake : public behaviour::HasBehaviour {
   std::string _stringStateName = "error";
   units::volt_t _setVoltage = 0_V;
 
-  wom::PIDController<units::radians_per_second, units::volt> _pid;
+  frc::PIDController _pid;
+  frc::PIDController _pidPosition;
   
   std::shared_ptr<nt::NetworkTable> _table = nt::NetworkTableInstance::GetDefault().GetTable("Intake");
 };
\ No newline at end of file
diff --git a/src/main/include/IntakeBehaviour.h b/src/main/include/IntakeBehaviour.h
index b414d4be..2e6e5ebb 100644
--- a/src/main/include/IntakeBehaviour.h
+++ b/src/main/include/IntakeBehaviour.h
@@ -9,28 +9,18 @@
 #include "Intake.h"
 #include "Wombat.h"
 
-enum class IntakeBehaviourState { kIntaking, kEjecting, kIdleing, kRawControl, kPassing};
+enum class IntakeBehaviourState { kIntaking, kEjecting, kPassing, kIdleing};
 
 class IntakeManualControl : public behaviour::Behaviour {
  public:
   explicit IntakeManualControl(Intake* intake, frc::XboxController& codriver);
 
-  void SetBehaviourState(IntakeBehaviourState behaviourState);
   void OnTick(units::second_t dt) override;
 
-  IntakeBehaviourState GetBehaviourState();
-
  private:
   Intake* _intake;
   frc::XboxController& _codriver;
-  IntakeBehaviourState _behaviourState = IntakeBehaviourState::kIdleing;
-
-  units::volt_t _rawVoltage;
-  units::volt_t _setVoltage;
-  bool _rawControl = true;
-  bool _intaking = false;
-  bool _ejecting = false;
-  bool _passing = false;
+  bool _rawControl = false;
 };
 
 class AutoIntake : public behaviour::Behaviour {
@@ -40,5 +30,5 @@ class AutoIntake : public behaviour::Behaviour {
 
  private:
   Intake* _intake;
-  IntakeBehaviourState _behaviourState = IntakeBehaviourState::kIdleing;
+  
 };
diff --git a/src/main/include/RobotMap.h b/src/main/include/RobotMap.h
index db0b73ad..a7b7cbcf 100644
--- a/src/main/include/RobotMap.h
+++ b/src/main/include/RobotMap.h
@@ -40,7 +40,7 @@ struct RobotMap {
   struct IntakeSystem {
     rev::CANSparkMax intakeMotor{35, rev::CANSparkMax::MotorType::kBrushless};
     wom::CANSparkMaxEncoder intakeEncoder{&intakeMotor, 0.1_m};
-    frc::DigitalInput intakeSensor{4};
+    frc::DigitalInput intakeSensor{5};
     // frc::DigitalInput magSensor{0};
     // frc::DigitalInput shooterSensor{0};
 

From fe36dd37921a884c6afc2ddfe633689c273dc142 Mon Sep 17 00:00:00 2001
From: Sean Chan <sean.chan3@student.education.wa.edu.au>
Date: Thu, 29 Feb 2024 14:09:53 +0800
Subject: [PATCH 173/207] finished intake PID

---
 src/main/cpp/Intake.cpp            |  4 +--
 src/main/cpp/IntakeBehaviour.cpp   | 41 +++++++++++++++++++++++++++++-
 src/main/include/IntakeBehaviour.h | 31 ++++++++++++++++++++--
 3 files changed, 71 insertions(+), 5 deletions(-)

diff --git a/src/main/cpp/Intake.cpp b/src/main/cpp/Intake.cpp
index 128cdd2c..401cd0f8 100644
--- a/src/main/cpp/Intake.cpp
+++ b/src/main/cpp/Intake.cpp
@@ -54,9 +54,9 @@ void Intake::OnUpdate(units::second_t dt) {
       }
       _pid.SetSetpoint(0);
       units::volt_t pidCalculate =
-          units::volt_t{_pid.Calculate(_config.IntakeGearbox.encoder->GetEncoderAngularVelocity().value())};
+          // units::volt_t{_pid.Calculate(_config.IntakeGearbox.encoder->GetEncoderAngularVelocity().value())};
       // units::volt_t pidCalculate =
-      //     units::volt_t{_pidPosition.Calculate(_config.IntakeGearbox.encoder->GetEncoderPosition().value())};
+          units::volt_t{_pidPosition.Calculate(_config.IntakeGearbox.encoder->GetEncoderPosition().value())};
       _setVoltage = pidCalculate;
       _stringStateName = "Hold";
     }
diff --git a/src/main/cpp/IntakeBehaviour.cpp b/src/main/cpp/IntakeBehaviour.cpp
index 05b3bf5e..3ba5814a 100644
--- a/src/main/cpp/IntakeBehaviour.cpp
+++ b/src/main/cpp/IntakeBehaviour.cpp
@@ -58,4 +58,43 @@ void IntakeManualControl::OnTick(units::second_t dt) {
 AutoIntake::AutoIntake(Intake* intake) : _intake(intake) {
   Controls(intake);
 }
-void AutoIntake::OnTick(units::second_t dt) {}
+
+void AutoIntake::OnTick(units::second_t dt) {
+  _intake->SetState(IntakeState::kIntake);
+}
+
+IntakeNote::IntakeNote(Intake* intake) : _intake(intake) {
+  Controls(intake);
+}
+
+void IntakeNote::OnTick(units::second_t dt) {
+  _intake->SetState(IntakeState::kIntake);
+
+  if (_intake->GetState() == IntakeState::kHold) {
+    SetDone();
+  }
+}
+
+PassNote::PassNote(Intake* intake) : _intake(intake) {
+  Controls(intake);
+}
+
+void PassNote::OnTick(units::second_t dt) {
+  _intake->SetState(IntakeState::kPass);
+
+  if (_intake->GetState() == IntakeState::kIdle) {
+    SetDone();
+  }
+}
+
+EjectNote::EjectNote(Intake* intake) : _intake(intake) {
+  Controls(intake);
+}
+
+void EjectNote::OnTick(units::second_t dt) {
+  _intake->SetState(IntakeState::kEject);
+
+  if (_intake->GetState() == IntakeState::kIdle) {
+    SetDone();
+  }
+}
\ No newline at end of file
diff --git a/src/main/include/IntakeBehaviour.h b/src/main/include/IntakeBehaviour.h
index 2e6e5ebb..642d20b2 100644
--- a/src/main/include/IntakeBehaviour.h
+++ b/src/main/include/IntakeBehaviour.h
@@ -9,8 +9,6 @@
 #include "Intake.h"
 #include "Wombat.h"
 
-enum class IntakeBehaviourState { kIntaking, kEjecting, kPassing, kIdleing};
-
 class IntakeManualControl : public behaviour::Behaviour {
  public:
   explicit IntakeManualControl(Intake* intake, frc::XboxController& codriver);
@@ -30,5 +28,34 @@ class AutoIntake : public behaviour::Behaviour {
 
  private:
   Intake* _intake;
+};
+
+class IntakeNote : public behaviour::Behaviour {
+ public:
+  explicit IntakeNote(Intake* intake);
+  void OnTick(units::second_t dt) override;
+
+ private:
+  Intake* _intake;
+  
+};
+
+class PassNote : public behaviour::Behaviour {
+ public:
+  explicit PassNote(Intake* intake);
+  void OnTick(units::second_t dt) override;
+
+ private:
+  Intake* _intake;
+  
+};
+
+class EjectNote : public behaviour::Behaviour {
+ public:
+  explicit EjectNote(Intake* intake);
+  void OnTick(units::second_t dt) override;
+
+ private:
+  Intake* _intake;
   
 };

From e3d449d365bd6c344149935c30189ce4cbd07559 Mon Sep 17 00:00:00 2001
From: JoystickMaster-race <foxlocks179@gmail.com>
Date: Thu, 29 Feb 2024 19:57:57 +0800
Subject: [PATCH 174/207] Finished Arm SetStates

---
 src/main/cpp/AlphaArm.cpp          |  16 ++++-
 src/main/cpp/AlphaArmBehaviour.cpp |   9 ++-
 src/main/cpp/Vision.cpp            |  12 ++++
 src/main/include/AlphaArm.h        |   4 +-
 src/main/include/ArmAngle.h        |  84 ++++++++++++++++++++++
 src/main/include/Vision.h          | 111 +++++++++++++++++++++++++++++
 6 files changed, 229 insertions(+), 7 deletions(-)
 create mode 100644 src/main/cpp/Vision.cpp
 create mode 100644 src/main/include/ArmAngle.h
 create mode 100644 src/main/include/Vision.h

diff --git a/src/main/cpp/AlphaArm.cpp b/src/main/cpp/AlphaArm.cpp
index 613e5fc7..9c998017 100644
--- a/src/main/cpp/AlphaArm.cpp
+++ b/src/main/cpp/AlphaArm.cpp
@@ -5,13 +5,14 @@
 #include "AlphaArm.h"
 
 
-AlphaArm::AlphaArm(AlphaArmConfig *config) : _config(config), _pidArm{frc::PIDController(1.2, 0.4, 0)}, _pidArmStates{frc::PIDController(6.5, 0.1, 0)}, _pidIntakeState{frc::PIDController(1, 0, 0)} 
+AlphaArm::AlphaArm(AlphaArmConfig *config/*, frc::Rotation2d initialAngle, wom::vision::Limelight* vision */) : _config(config), _pidArm{frc::PIDController(1.2, 0.4, 0)}, _pidArmStates{frc::PIDController(37, 0.00070, 0.15)}, _pidIntakeState{frc::PIDController(30, 0.00015, 0.005)} 
 {
  
 }
 
 void AlphaArm::OnStart(){
   _pidArmStates.Reset();
+  _pidIntakeState.Reset();
 }
 
 void AlphaArm::OnUpdate(units::second_t dt) {
@@ -43,8 +44,8 @@ void AlphaArm::OnUpdate(units::second_t dt) {
 
     case AlphaArmState::kIntakeAngle:
     std::cout << "Intake Angle" << std::endl;
-       _pidIntakeState.SetSetpoint(-0.48);
-       _setAlphaArmVoltage = units::volt_t{_pidArmStates.Calculate(-_config->alphaArmEncoder.GetEncoderPosition().value())};
+       _pidIntakeState.SetSetpoint(-0.48); //-0.48
+       _setAlphaArmVoltage = units::volt_t{_pidIntakeState.Calculate(-_config->alphaArmEncoder.GetEncoderPosition().value())};
     break;
 
     case AlphaArmState::kSpeakerAngle:
@@ -54,6 +55,11 @@ void AlphaArm::OnUpdate(units::second_t dt) {
 
     break;
 
+    // case AlphaArmState::kVisionAngle:
+    // std::cout << "Vision Angle" << std::endl;
+
+    // break;
+
     case AlphaArmState::kStowed:
     std::cout << "Stowed" << std::endl;
        _pidArmStates.SetSetpoint(-0.52);
@@ -73,6 +79,10 @@ void AlphaArm::OnUpdate(units::second_t dt) {
     _table->GetEntry("PID Error State").SetDouble(_pidArmStates.GetPositionError());
     _table->GetEntry("SetPoint State").SetDouble(_pidArmStates.GetSetpoint());
 
+    _table->GetEntry("Intake SetPoint State").SetDouble(_pidIntakeState.GetSetpoint());
+    _table->GetEntry("Intake PID Error State").SetDouble(_pidIntakeState.GetPositionError());
+
+
 
      std::cout << "Voltage:" << _setAlphaArmVoltage.value() << std::endl;
 }
diff --git a/src/main/cpp/AlphaArmBehaviour.cpp b/src/main/cpp/AlphaArmBehaviour.cpp
index 14419904..05c394d8 100644
--- a/src/main/cpp/AlphaArmBehaviour.cpp
+++ b/src/main/cpp/AlphaArmBehaviour.cpp
@@ -21,7 +21,7 @@ void AlphaArmManualControl::OnTick(units::second_t dt) {
   _table->GetEntry("Goal Value").SetBoolean(_gotValue);
   
 
-  if (_codriver->GetRightBumperPressed()) {
+  if (_codriver->GetBButton()) {
     if (_rawControl == true) {
       _rawControl = false;
     } else {
@@ -35,8 +35,11 @@ void AlphaArmManualControl::OnTick(units::second_t dt) {
     _alphaArm->SetState(AlphaArmState::kAmpAngle);
   } else if(_codriver->GetYButton()){
     _alphaArm->SetState(AlphaArmState::kStowed);
-  } else {
+  } else if(_codriver->GetRightBumper()){
     _alphaArm->SetState(AlphaArmState::kIntakeAngle);
+  } else {
+    _alphaArm->SetState(AlphaArmState::kIdle);
   }
+}
 
-  }
+  
diff --git a/src/main/cpp/Vision.cpp b/src/main/cpp/Vision.cpp
new file mode 100644
index 00000000..8684d8a4
--- /dev/null
+++ b/src/main/cpp/Vision.cpp
@@ -0,0 +1,12 @@
+// #include "Vision.h"
+
+// #include <utility>
+// #include <variant>
+
+// #include "units/length.h"
+// #include "units/math.h"
+// #include "vision/Limelight.h"
+
+// //std::pair<frc::Rotation2d, double angle> Vision::AngleToTarget(VisionTargetObjects objects){
+
+// //}
\ No newline at end of file
diff --git a/src/main/include/AlphaArm.h b/src/main/include/AlphaArm.h
index c05ef0d5..d8fca1bc 100644
--- a/src/main/include/AlphaArm.h
+++ b/src/main/include/AlphaArm.h
@@ -27,13 +27,14 @@ enum class AlphaArmState {
   kAmpAngle,
   kSpeakerAngle,
   kHoldAngle,
+  kVisionAngle,
   kStowed,
   kRaw
 };
 
 class AlphaArm : public behaviour::HasBehaviour {
  public:
-  AlphaArm(AlphaArmConfig *config);
+  AlphaArm(AlphaArmConfig *config/*, frc::Rotation2d initialAngle, wom::vision::Limelight* vision*/);
 
   void OnUpdate(units::second_t dt);
   void SetArmRaw(units::volt_t voltage);
@@ -48,6 +49,7 @@ class AlphaArm : public behaviour::HasBehaviour {
   // units::radian_t CalcTargetAngle();
 
   AlphaArmConfig *_config;
+  wom::vision::Limelight* _vision;
   AlphaArmState _state = AlphaArmState::kIdle;
   //wom::utils::PIDController<units::degree, units::volt> _alphaArmPID;
   //frc::DutyCycleEncoder armEncoder{4};
diff --git a/src/main/include/ArmAngle.h b/src/main/include/ArmAngle.h
new file mode 100644
index 00000000..051565db
--- /dev/null
+++ b/src/main/include/ArmAngle.h
@@ -0,0 +1,84 @@
+// // Copyright (c) 2023-2024 CurtinFRC
+// // Open Source Software, you can modify it according to the terms
+// // of the MIT License at the root of this project
+
+// #pragma once
+
+// #include <frc/Filesystem.h>
+// #include <units/length.h>
+// #include <units/math.h>
+// #include <wpi/fs.h>
+// #include <wpi/json.h>
+
+// #include <string>
+// #include <utility>
+// #include <vector>
+
+// #include "Wombat.h"
+// #include "units/angle.h"
+// #include "AlphaArm.h"
+
+// #define APRILTAGS_MAX 16
+// #define APRILTAGS_MIN 0
+
+// enum class VisionTarget {
+//   /*
+//       Left is toward the blue side of the diagram here:
+//      https://firstfrc.blob.core.windows.net/frc2024/FieldAssets/2024LayoutMarkingDiagram.pdf
+
+//       The numbers are the numbers on the field diagram (and the numbers on the tags).
+//   */
+//   kBlueAmp = 6,
+//   kBlueSpeakerCenter = 7,
+//   kBlueSpeakerOffset = 8,
+//   kBlueChain1 = 15,
+//   kBlueChain2 = 16,
+//   kBlueChain3 = 14,
+//   kBlueSourceLeft = 1,
+//   kBlueSourceRight = 2,
+
+//   kRedAmp = 5,
+//   kRedSpeakerCenter = 4,
+//   kRedSpeakerOffset = 3,
+
+//   kRedChain1 = 12,
+//   kRedChain2 = 11,
+//   kRedChain3 = 13,
+//   kRedSourceLeft = 9,
+//   kRedSourceRight = 10
+// };
+
+// enum class VisionModes { kAprilTag = 0, kRing = 1 };
+
+// enum class VisionTargetObjects { kNote };
+
+// struct AprilTag {
+//   int id;
+//   double size;
+//   std::array<std::array<double, 4>, 4> transform;
+//   bool unique;
+
+//   frc::Pose3d pos;
+//   units::radian_t yaw;
+//   units::radian_t pitch;
+//   units::radian_t roll;
+// };
+
+// class FMAP {
+//  public:
+//   explicit FMAP(std::string path);
+
+//   std::vector<AprilTag> GetTags();
+
+//  private:
+//   std::vector<AprilTag> _tags;
+//   std::string _path;
+//   std::string deploy_dir;
+// };
+
+// class ArmVision{
+//     public:
+//     ArmVision(std::string name, FMAP fmap);
+
+//     frc::Twist2d AngleToAprilTag(VisionTarget target, double angleOffset, );
+// };
diff --git a/src/main/include/Vision.h b/src/main/include/Vision.h
new file mode 100644
index 00000000..ea953fb9
--- /dev/null
+++ b/src/main/include/Vision.h
@@ -0,0 +1,111 @@
+// // Copyright (c) 2023-2024 CurtinFRC
+// // Open Source Software, you can modify it according to the terms
+// // of the MIT License at the root of this project
+
+// #pragma once
+
+// #include <frc/Filesystem.h>
+// #include <units/length.h>
+// #include <units/math.h>
+// #include <wpi/fs.h>
+// #include <wpi/json.h>
+
+// #include <string>
+// #include <utility>
+// #include <vector>
+
+// #include "Wombat.h"
+// #include "units/angle.h"
+// #include "AlphaArm.h"
+
+// #define APRILTAGS_MAX 16
+// #define APRILTAGS_MIN 0
+
+// enum class VisionTarget {
+//   /*
+//       Left is toward the blue side of the diagram here:
+//      https://firstfrc.blob.core.windows.net/frc2024/FieldAssets/2024LayoutMarkingDiagram.pdf
+
+//       The numbers are the numbers on the field diagram (and the numbers on the tags).
+//   */
+//   kBlueAmp = 6,
+//   kBlueSpeakerCenter = 7,
+//   kBlueSpeakerOffset = 8,
+//   kBlueChain1 = 15,
+//   kBlueChain2 = 16,
+//   kBlueChain3 = 14,
+//   kBlueSourceLeft = 1,
+//   kBlueSourceRight = 2,
+
+//   kRedAmp = 5,
+//   kRedSpeakerCenter = 4,
+//   kRedSpeakerOffset = 3,
+
+//   kRedChain1 = 12,
+//   kRedChain2 = 11,
+//   kRedChain3 = 13,
+//   kRedSourceLeft = 9,
+//   kRedSourceRight = 10
+// };
+
+// enum class VisionModes { kAprilTag = 0, kRing = 1 };
+
+// enum class VisionTargetObjects { kNote };
+
+// struct AprilTag {
+//   int id;
+//   double size;
+//   std::array<std::array<double, 4>, 4> transform;
+//   bool unique;
+
+//   frc::Pose3d pos;
+//   units::radian_t yaw;
+//   units::radian_t pitch;
+//   units::radian_t roll;
+// };
+
+// class FMAP {
+//  public:
+//   explicit FMAP(std::string path);
+
+//   std::vector<AprilTag> GetTags();
+
+//  private:
+//   std::vector<AprilTag> _tags;
+//   std::string _path;
+//   std::string deploy_dir;
+// };
+
+// class Vision {
+//  public:
+//   Vision(std::string name, FMAP fmap);
+
+//   std::pair<units::meter_t, units::degree_t> GetDistanceToTarget(VisionTarget target);
+//   std::pair<units::meter_t, units::degree_t> GetDistanceToTarget(int id);
+//   //void GetAngleToTarget(double angle);
+
+//   void SetMode(VisionModes mode);
+
+//   frc::Pose3d GetPose();
+
+//   frc::Rotation2d AngleToTarget(VisionTarget target, double angle, AlphaArm* alphaArm);
+//   std::pair<frc::Rotation2d, double> AngleToTarget(VisionTarget target);
+//   double LockOn(VisionTarget target);
+  
+//   //std::pair<frc::Rotation2d, double angle> GetAngleToTarget(Vision)
+    
+//   std::pair<frc::Pose2d, units::degree_t> GetAngleToObject(VisionTargetObjects object);
+//   units::degree_t LockOn(VisionTargetObjects target);
+
+//   std::vector<AprilTag> GetTags();
+
+//   bool IsAtPose(frc::Pose3d pose, units::second_t dt);
+
+//   bool TargetIsVisible(VisionTargetObjects target);
+
+//   int CurrentAprilTag();
+
+//  private:
+//   wom::Limelight* _limelight;
+//   FMAP _fmap;
+// };
\ No newline at end of file

From 8165613544932523af0266e096ae576bf99e7cca Mon Sep 17 00:00:00 2001
From: Kingsley Wong <kingsley.wong@student.education.wa.edu.au>
Date: Thu, 29 Feb 2024 20:04:49 +0800
Subject: [PATCH 175/207] Final code 1

---
 src/main/cpp/Intake.cpp     | 38 ++++++++++++++++++++++++++++++++-----
 src/main/include/Intake.h   |  9 +++++++--
 src/main/include/RobotMap.h |  3 ++-
 3 files changed, 42 insertions(+), 8 deletions(-)

diff --git a/src/main/cpp/Intake.cpp b/src/main/cpp/Intake.cpp
index 401cd0f8..c8450b40 100644
--- a/src/main/cpp/Intake.cpp
+++ b/src/main/cpp/Intake.cpp
@@ -17,14 +17,28 @@ void Intake::OnStart() {
 void Intake::OnUpdate(units::second_t dt) {
 
   switch (_state) {
+
+    case IntakeState::kAdjust:
+    {
+      if (_config.intakeSensor->Get() == true && _config.passSensor->Get() == true) { 
+        _setVoltage = -4_V;
+      } else if (_config.intakeSensor->Get() == true && _config.passSensor->Get() == false) { 
+        SetState(IntakeState::kHold);
+      } else if (_config.intakeSensor->Get() == false && _config.passSensor->Get() == false) {
+        SetState(IntakeState::kIdle); 
+      }
+    } 
     case IntakeState::kIdle: 
     {
-      if (_config.intakeSensor->Get() == false) { 
+      if (_config.intakeSensor->Get() == true && _config.passSensor->Get() == false) { 
         SetState(IntakeState::kHold);
+      } else if (_config.intakeSensor->Get() == true && _config.passSensor->Get() == true) { 
+        SetState(IntakeState::kAdjust);
       }
       _stringStateName = "Idle";
       _pid.Reset();
       _setVoltage = 0_V;
+      _recordNote = false;
     } 
     break;
 
@@ -38,8 +52,10 @@ void Intake::OnUpdate(units::second_t dt) {
 
     case IntakeState::kEject: 
     {
-      if (_config.intakeSensor->Get() == true) {
+      if (_config.intakeSensor->Get() == false && _config.passSensor->Get() == false) {
         SetState(IntakeState::kIdle); 
+      } else if (_config.intakeSensor->Get() == true && _config.passSensor->Get() == true) { 
+        SetState(IntakeState::kAdjust);
       }
       _stringStateName = "Eject";
       _setVoltage = -4_V;
@@ -49,8 +65,10 @@ void Intake::OnUpdate(units::second_t dt) {
 
     case IntakeState::kHold: 
     {
-      if (_config.intakeSensor->Get() == true) {
+      if (_config.intakeSensor->Get() == false && _config.passSensor->Get() == false) {
         SetState(IntakeState::kIdle);
+      } else if (_config.intakeSensor->Get() == true && _config.passSensor->Get() == true) { 
+        SetState(IntakeState::kAdjust);
       }
       _pid.SetSetpoint(0);
       units::volt_t pidCalculate =
@@ -64,8 +82,10 @@ void Intake::OnUpdate(units::second_t dt) {
 
     case IntakeState::kIntake: 
     {
-      if (_config.intakeSensor->Get() == false) {
+      if (_config.intakeSensor->Get() == true && _config.passSensor->Get() == false) {
         SetState(IntakeState::kHold);
+      } else if (_config.intakeSensor->Get() == true && _config.passSensor->Get() == true) { 
+        SetState(IntakeState::kAdjust);
       }
       _stringStateName = "Intake";
       _setVoltage = 4_V; 
@@ -74,9 +94,15 @@ void Intake::OnUpdate(units::second_t dt) {
 
     case IntakeState::kPass: 
     {
-      if (_config.intakeSensor->Get() == true) {
+      if (_config.intakeSensor->Get() == false && _config.passSensor->Get() == false) {
         SetState(IntakeState::kIdle);
+      } 
+
+      if (!_recordNote) {
+        _noteShot ++;
+        _recordNote = true;
       }
+
       _stringStateName = "Pass";
       _setVoltage = 4_V;
     }
@@ -85,9 +111,11 @@ void Intake::OnUpdate(units::second_t dt) {
   _table->GetEntry("State: ").SetString(_stringStateName);
   _table->GetEntry("Motor Voltage: ").SetDouble(_setVoltage.value());
   _table->GetEntry("Intake Sensor: ").SetBoolean(_config.intakeSensor->Get());
+  _table->GetEntry("Pass Sensor: ").SetBoolean(_config.passSensor->Get());
   _table->GetEntry("Error: ").SetDouble(_pid.GetPositionError());
   _table->GetEntry("SetPoint: ").SetDouble(_pid.GetSetpoint());
   _table->GetEntry("Encoder: ").SetDouble(_config.IntakeGearbox.encoder->GetEncoderAngularVelocity().value());
+  _table->GetEntry("Shot Count: ").SetDouble(_noteShot);
   // _table->GetEntry("Encoder: ").SetDouble(_config.IntakeGearbox.encoder->GetEncoderPosition().value());
  
   _config.IntakeGearbox.motorController->SetVoltage(_setVoltage);
diff --git a/src/main/include/Intake.h b/src/main/include/Intake.h
index 00ddf45b..0fb5548b 100644
--- a/src/main/include/Intake.h
+++ b/src/main/include/Intake.h
@@ -17,11 +17,12 @@ struct IntakeConfig {
   std::string path;
   wom::Gearbox IntakeGearbox;
   frc::DigitalInput* intakeSensor;
+  frc::DigitalInput* passSensor;
 
   wom::PIDConfig<units::radians_per_second, units::volt> pidConfig;
 };
 
-enum class IntakeState { kIdle, kRaw, kHold, kEject, kIntake, kPass};
+enum class IntakeState { kIdle, kRaw, kHold, kEject, kIntake, kPass, kAdjust};
 
 class Intake : public behaviour::HasBehaviour {
  public:
@@ -39,12 +40,16 @@ class Intake : public behaviour::HasBehaviour {
   IntakeConfig _config;
   IntakeState _state = IntakeState::kIdle;
 
+  int _noteShot = 0;
+
+  bool _recordNote = false;
+
   units::volt_t _rawVoltage = 0_V;
   std::string _stringStateName = "error";
   units::volt_t _setVoltage = 0_V;
 
   frc::PIDController _pid;
   frc::PIDController _pidPosition;
-  
+
   std::shared_ptr<nt::NetworkTable> _table = nt::NetworkTableInstance::GetDefault().GetTable("Intake");
 };
\ No newline at end of file
diff --git a/src/main/include/RobotMap.h b/src/main/include/RobotMap.h
index a7b7cbcf..81e53177 100644
--- a/src/main/include/RobotMap.h
+++ b/src/main/include/RobotMap.h
@@ -41,12 +41,13 @@ struct RobotMap {
     rev::CANSparkMax intakeMotor{35, rev::CANSparkMax::MotorType::kBrushless};
     wom::CANSparkMaxEncoder intakeEncoder{&intakeMotor, 0.1_m};
     frc::DigitalInput intakeSensor{5};
+    frc::DigitalInput passSensor{99};
     // frc::DigitalInput magSensor{0};
     // frc::DigitalInput shooterSensor{0};
 
     wom::Gearbox IntakeGearbox{&intakeMotor, &intakeEncoder, frc::DCMotor::NEO(1)};
 
-    IntakeConfig config{"path", IntakeGearbox, &intakeSensor, wom::PIDConfig<units::radians_per_second, units::volts>(
+    IntakeConfig config{"path", IntakeGearbox, &intakeSensor, &passSensor, wom::PIDConfig<units::radians_per_second, units::volts>(
           "/intake/PID/config",
           9_V / (180_deg / 1_s),
           0_V / 25_deg,

From 4a40db1df8a44d2b159b6e305bf230a715cab266 Mon Sep 17 00:00:00 2001
From: Kingsley Wong <kingsley.wong@student.education.wa.edu.au>
Date: Thu, 29 Feb 2024 21:09:56 +0800
Subject: [PATCH 176/207] merging and built successfully

---
 src/main/cpp/Robot.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp
index 2fb7283a..06e929c2 100644
--- a/src/main/cpp/Robot.cpp
+++ b/src/main/cpp/Robot.cpp
@@ -148,7 +148,7 @@ void Robot::AutonomousInit() {
 void Robot::AutonomousPeriodic() {
   // m_driveSim->OnUpdate();
 }
-void Robot::AutonomousPeriodic() {}
+
 
 void Robot::TeleopInit() {
   loop.Clear();

From fbbe38299c95e509c084d85da61269a927bc2379 Mon Sep 17 00:00:00 2001
From: Goanna247 <21496350@student.curtin.edu.au>
Date: Sat, 2 Mar 2024 10:23:50 +0800
Subject: [PATCH 177/207] Untested fixed robot

---
 src/main/cpp/AlphaArm.cpp                     |   2 +-
 src/main/cpp/AlphaArmBehaviour.cpp            |   7 +-
 src/main/cpp/Intake.cpp                       |  37 +-
 src/main/cpp/Robot.cpp                        |  84 +---
 src/main/cpp/Shooter.cpp                      |  18 +-
 src/main/include/AlphaArm.h                   |   2 +-
 src/main/include/Intake.h                     |   2 +-
 src/main/include/Robot.h                      |  37 +-
 src/main/include/RobotMap.h                   |  69 ++--
 src/main/include/Shooter.h                    |   4 +-
 wombat/src/main/cpp/behaviour/Behaviour.cpp   |  23 +-
 .../main/cpp/behaviour/BehaviourScheduler.cpp |   4 +-
 .../src/main/cpp/behaviour/HasBehaviour.cpp   |   3 +-
 .../src/main/cpp/drivetrain/SwerveDrive.cpp   |  19 +-
 .../behaviours/SwerveBehaviours.cpp           |  11 +-
 wombat/src/main/cpp/utils/Encoder.cpp         |  42 +-
 wombat/src/main/cpp/utils/Util.cpp            |  15 +-
 wombat/src/main/cpp/vision/Limelight.cpp      | 386 +++++++++---------
 wombat/src/main/include/behaviour/Behaviour.h |  36 +-
 .../src/main/include/behaviour/HasBehaviour.h |   3 +-
 .../src/main/include/drivetrain/SwerveDrive.h |  24 +-
 .../drivetrain/behaviours/SwerveBehaviours.h  |   9 +-
 wombat/src/main/include/utils/PID.h           |  82 ++--
 wombat/src/main/include/utils/Util.h          |  56 +--
 wombat/src/main/include/vision/Limelight.h    | 240 ++++++-----
 25 files changed, 523 insertions(+), 692 deletions(-)

diff --git a/src/main/cpp/AlphaArm.cpp b/src/main/cpp/AlphaArm.cpp
index 3d7f72f8..1588d010 100644
--- a/src/main/cpp/AlphaArm.cpp
+++ b/src/main/cpp/AlphaArm.cpp
@@ -39,7 +39,7 @@ void AlphaArm::OnUpdate(units::second_t dt) {
       std::cout << "Amp Angle" << std::endl;
       
        _pidArmStates.SetSetpoint(-2.17);
-       _setAlphaArmVoltage = units::volt_t{_pidArmStates.Calculate(-(_config->alphaArmEncoder.GetEncoderPosition().value() * (2 * 3.1415)) )};
+       _setAlphaArmVoltage = units::volt_t{_pidArmStates.Calculate(-(_config->alphaArmEncoder.GetEncoderPosition().value() * (2 * 3.1415)))};
 
     break;
 
diff --git a/src/main/cpp/AlphaArmBehaviour.cpp b/src/main/cpp/AlphaArmBehaviour.cpp
index cee72cb6..3e2507e5 100644
--- a/src/main/cpp/AlphaArmBehaviour.cpp
+++ b/src/main/cpp/AlphaArmBehaviour.cpp
@@ -32,12 +32,12 @@ void AlphaArmManualControl::OnTick(units::second_t dt) {
   if (_rawControl) {
     _alphaArm->SetState(AlphaArmState::kRaw);
     if (wom::deadzone(_codriver->GetRightY())) {
-      _alphaArm->SetArmRaw(_codriver->GetRightY() * 8_V);
+      _alphaArm->SetArmRaw(_codriver->GetRightY() * 7_V);
     } else {
       _alphaArm->SetArmRaw(0_V);
     }
   } else {
-     if(_codriver->GetLeftTriggerAxis() > 0.1){
+    if(_codriver->GetLeftTriggerAxis() > 0.1){
       _alphaArm->SetState(AlphaArmState::kSpeakerAngle);
     } else if (_codriver->GetLeftBumper()){
       _alphaArm->SetState(AlphaArmState::kAmpAngle);
@@ -49,9 +49,8 @@ void AlphaArmManualControl::OnTick(units::second_t dt) {
       _alphaArm->SetState(AlphaArmState::kIdle);
     }
   }
- 
+  
 }
 
-
   
 
diff --git a/src/main/cpp/Intake.cpp b/src/main/cpp/Intake.cpp
index c8450b40..6f6bdcbf 100644
--- a/src/main/cpp/Intake.cpp
+++ b/src/main/cpp/Intake.cpp
@@ -17,23 +17,10 @@ void Intake::OnStart() {
 void Intake::OnUpdate(units::second_t dt) {
 
   switch (_state) {
-
-    case IntakeState::kAdjust:
-    {
-      if (_config.intakeSensor->Get() == true && _config.passSensor->Get() == true) { 
-        _setVoltage = -4_V;
-      } else if (_config.intakeSensor->Get() == true && _config.passSensor->Get() == false) { 
-        SetState(IntakeState::kHold);
-      } else if (_config.intakeSensor->Get() == false && _config.passSensor->Get() == false) {
-        SetState(IntakeState::kIdle); 
-      }
-    } 
     case IntakeState::kIdle: 
     {
-      if (_config.intakeSensor->Get() == true && _config.passSensor->Get() == false) { 
+      if (_config.intakeSensor->Get() == false) { 
         SetState(IntakeState::kHold);
-      } else if (_config.intakeSensor->Get() == true && _config.passSensor->Get() == true) { 
-        SetState(IntakeState::kAdjust);
       }
       _stringStateName = "Idle";
       _pid.Reset();
@@ -52,10 +39,8 @@ void Intake::OnUpdate(units::second_t dt) {
 
     case IntakeState::kEject: 
     {
-      if (_config.intakeSensor->Get() == false && _config.passSensor->Get() == false) {
+      if (_config.intakeSensor->Get() == true && _config.passSensor->Get() == true) {
         SetState(IntakeState::kIdle); 
-      } else if (_config.intakeSensor->Get() == true && _config.passSensor->Get() == true) { 
-        SetState(IntakeState::kAdjust);
       }
       _stringStateName = "Eject";
       _setVoltage = -4_V;
@@ -65,27 +50,23 @@ void Intake::OnUpdate(units::second_t dt) {
 
     case IntakeState::kHold: 
     {
-      if (_config.intakeSensor->Get() == false && _config.passSensor->Get() == false) {
+      if (_config.intakeSensor->Get() == true && _config.passSensor->Get() == true) {
         SetState(IntakeState::kIdle);
-      } else if (_config.intakeSensor->Get() == true && _config.passSensor->Get() == true) { 
-        SetState(IntakeState::kAdjust);
       }
-      _pid.SetSetpoint(0);
-      units::volt_t pidCalculate =
+      // _pid.SetSetpoint(0);
+      // units::volt_t pidCalculate =
           // units::volt_t{_pid.Calculate(_config.IntakeGearbox.encoder->GetEncoderAngularVelocity().value())};
       // units::volt_t pidCalculate =
-          units::volt_t{_pidPosition.Calculate(_config.IntakeGearbox.encoder->GetEncoderPosition().value())};
-      _setVoltage = pidCalculate;
+          // units::volt_t{_pidPosition.Calculate(_config.IntakeGearbox.encoder->GetEncoderPosition().value())};
+      _setVoltage = 0_V;
       _stringStateName = "Hold";
     }
     break;
 
     case IntakeState::kIntake: 
     {
-      if (_config.intakeSensor->Get() == true && _config.passSensor->Get() == false) {
+      if (_config.intakeSensor->Get() == false) {
         SetState(IntakeState::kHold);
-      } else if (_config.intakeSensor->Get() == true && _config.passSensor->Get() == true) { 
-        SetState(IntakeState::kAdjust);
       }
       _stringStateName = "Intake";
       _setVoltage = 4_V; 
@@ -94,7 +75,7 @@ void Intake::OnUpdate(units::second_t dt) {
 
     case IntakeState::kPass: 
     {
-      if (_config.intakeSensor->Get() == false && _config.passSensor->Get() == false) {
+      if (_config.intakeSensor->Get() == true) {
         SetState(IntakeState::kIdle);
       } 
 
diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp
index 794401cc..ed43786a 100644
--- a/src/main/cpp/Robot.cpp
+++ b/src/main/cpp/Robot.cpp
@@ -37,86 +37,38 @@
 static units::second_t lastPeriodic;
 
 void Robot::RobotInit() {
-
-  // shooter = new Shooter(robotmap.shooterSystem.config);
-  // wom::BehaviourScheduler::GetInstance()->Register(shooter);
-  // shooter->SetDefaultBehaviour(
-  //     [this]() { return wom::make<ShooterManualControl>(shooter, &robotmap.controllers.codriver); });
-
-  // sched = wom::BehaviourScheduler::GetInstance();
-  // m_chooser.SetDefaultOption("Default Auto", "Default Auto");
-
-  // m_chooser.SetDefaultOption("Default Auto", "Default Auto");
-
-  frc::SmartDashboard::PutData("Auto Selector", &m_chooser);
   sched = wom::BehaviourScheduler::GetInstance();
   m_chooser.SetDefaultOption("Default Auto", "Default Auto");
 
-  sched = wom::BehaviourScheduler::GetInstance();
-  m_chooser.SetDefaultOption("kTaxi", "kTaxi");
-
-//   for (auto& option : autoOptions) {
-//     m_chooser.AddOption(option, option);
-//   }
-
-//   frc::SmartDashboard::PutData("Auto Modes", &m_chooser);
+  _led = new LED();
 
-  // m_path_chooser.SetDefaultOption("Path1", "paths/output/Path1.wpilib.json");
+  shooter = new Shooter(robotmap.shooterSystem.config);
+  wom::BehaviourScheduler::GetInstance()->Register(shooter);
+  shooter->SetDefaultBehaviour(
+      [this]() { return wom::make<ShooterManualControl>(shooter, &robotmap.controllers.codriver, _led); });
+  
 
-  // m_path_chooser.AddOption("Path1", "paths/output/Path1.wpilib.json");
-  // m_path_chooser.AddOption("Path2", "paths/output/Path2.wpilib.json");
-
-  // frc::SmartDashboard::PutData("Path Selector", &m_path_chooser);
-
-  // frc::SmartDashboard::PutData("Field", &m_field);
-
-  _swerveDrive =
-      new wom::SwerveDrive(robotmap.swerveBase.config, frc::Pose2d());
+  _swerveDrive = new wom::SwerveDrive(robotmap.swerveBase.config, frc::Pose2d());
   wom::BehaviourScheduler::GetInstance()->Register(_swerveDrive);
   _swerveDrive->SetDefaultBehaviour(
       [this]() { return wom::make<wom::ManualDrivebase>(_swerveDrive, &robotmap.controllers.driver); });
 
-  alphaArm = new AlphaArm(&robotmap.alphaArmSystem.config);
-  wom::BehaviourScheduler::GetInstance()->Register(alphaArm);
-  alphaArm->SetDefaultBehaviour(
-      [this]() { return wom::make<AlphaArmManualControl>(alphaArm, &robotmap.controllers.codriver); });
-
 
     intake = new Intake(robotmap.intakeSystem.config);
   wom::BehaviourScheduler::GetInstance()->Register(intake);
   intake->SetDefaultBehaviour(
       [this]() { return wom::make<IntakeManualControl>(intake, robotmap.controllers.codriver); });
 
-
-  // m_driveSim = new wom::TempSimSwerveDrive(&simulation_timer, &m_field);
-  // m_driveSim = wom::TempSimSwerveDrive();
-
-  // robotmap.swerveBase.moduleConfigs[0].turnMotor.encoder->SetEncoderOffset(0.5948_rad);
-  // robotmap.swerveBase.moduleConfigs[1].turnMotor.encoder->SetEncoderOffset(2.6156_rad);
-  // robotmap.swerveBase.moduleConfigs[3].turnMotor.encoder->SetEncoderOffset(2.8931_rad);
-  // robotmap.swerveBase.moduleConfigs[2].turnMotor.encoder->SetEncoderOffset(-1.7417_rad);
-
-  // robotmap.swerveBase.moduleConfigs[0].driveMotor.motorController->SetInverted(true);
-  // robotmap.swerveBase.moduleConfigs[1].driveMotor.motorController->SetInverted(true);
-
-  // robotmap.swerveBase.moduleConfigs[0].turnMotor.motorController->SetInverted(true);
-  // robotmap.swerveBase.moduleConfigs[1].turnMotor.motorController->SetInverted(true);
+  alphaArm = new AlphaArm(&robotmap.alphaArmSystem.config);
+  wom::BehaviourScheduler::GetInstance()->Register(alphaArm);
+  alphaArm->SetDefaultBehaviour(
+      [this]() { return wom::make<AlphaArmManualControl>(alphaArm, &robotmap.controllers.codriver); });
 
   robotmap.swerveBase.moduleConfigs[0].turnMotor.encoder->SetEncoderOffset(0.45229_rad);
   robotmap.swerveBase.moduleConfigs[1].turnMotor.encoder->SetEncoderOffset(2.6846_rad);
   robotmap.swerveBase.moduleConfigs[2].turnMotor.encoder->SetEncoderOffset(3.01121_rad);
   robotmap.swerveBase.moduleConfigs[3].turnMotor.encoder->SetEncoderOffset(4.4524_rad);
 
-  // frontLeft = new ctre::phoenix6::hardware::TalonFX(7, "Drivebase");  // front left
-  // frontRight = new ctre::phoenix6::hardware::TalonFX(2, "Drivebase");   // front right
-  // backLeft = new ctre::phoenix6::hardware::TalonFX(6, "Drivebase");   // back left
-  // backRight = new ctre::phoenix6::hardware::TalonFX(4, "Drivebase");  // back right
-  // frontLeft = new ctre::phoenix6::hardware::TalonFX(9, "Drivebase");   // front left
-  // frontRight = new ctre::phoenix6::hardware::TalonFX(1, "Drivebase");   // front right
-  // backLeft = new ctre::phoenix6::hardware::TalonFX(5, "Drivebase");   // back left
-  // backRight = new ctre::phoenix6::hardware::TalonFX(3, "Drivebase");
-
-
   lastPeriodic = wom::now();
 }
 
@@ -127,7 +79,6 @@ void Robot::RobotPeriodic() {
 
   loop.Poll();
   wom::BehaviourScheduler::GetInstance()->Tick();
-  //shooter->OnUpdate(dt);
   //sched->Tick();
 
   // robotmap.swerveTable.swerveDriveTable->GetEntry("frontLeftEncoder")
@@ -146,14 +97,17 @@ void Robot::RobotPeriodic() {
   // _swerveDrive->OnUpdate(dt);
 
 
-  robotmap.swerveTable.swerveDriveTable->GetEntry("Encoder 0 offset: ").SetDouble(robotmap.swerveBase.moduleConfigs[0].turnMotor.encoder->GetEncoderPosition().value());
-  robotmap.swerveTable.swerveDriveTable->GetEntry("Encoder 1 offset: ").SetDouble(robotmap.swerveBase.moduleConfigs[1].turnMotor.encoder->GetEncoderPosition().value());
-  robotmap.swerveTable.swerveDriveTable->GetEntry("Encoder 2 offset: ").SetDouble(robotmap.swerveBase.moduleConfigs[2].turnMotor.encoder->GetEncoderPosition().value());
-  robotmap.swerveTable.swerveDriveTable->GetEntry("Encoder 3 offset: ").SetDouble(robotmap.swerveBase.moduleConfigs[3].turnMotor.encoder->GetEncoderPosition().value());
+//   robotmap.swerveTable.swerveDriveTable->GetEntry("Encoder 0 offset: ").SetDouble(robotmap.swerveBase.moduleConfigs[0].turnMotor.encoder->GetEncoderPosition().value());
+//   robotmap.swerveTable.swerveDriveTable->GetEntry("Encoder 1 offset: ").SetDouble(robotmap.swerveBase.moduleConfigs[1].turnMotor.encoder->GetEncoderPosition().value());
+//   robotmap.swerveTable.swerveDriveTable->GetEntry("Encoder 2 offset: ").SetDouble(robotmap.swerveBase.moduleConfigs[2].turnMotor.encoder->GetEncoderPosition().value());
+//   robotmap.swerveTable.swerveDriveTable->GetEntry("Encoder 3 offset: ").SetDouble(robotmap.swerveBase.moduleConfigs[3].turnMotor.encoder->GetEncoderPosition().value());
 
-  intake->OnUpdate(dt);
+  _led->OnUpdate(dt);
+  shooter->OnUpdate(dt);
   alphaArm->OnUpdate(dt);
   _swerveDrive->OnUpdate(dt);
+  intake->OnUpdate(dt);
+
 }
 
 void Robot::AutonomousInit() {
diff --git a/src/main/cpp/Shooter.cpp b/src/main/cpp/Shooter.cpp
index 44326bda..2f2d424b 100644
--- a/src/main/cpp/Shooter.cpp
+++ b/src/main/cpp/Shooter.cpp
@@ -4,7 +4,7 @@
 
 #include "Shooter.h"
 
-Shooter::Shooter(ShooterConfig config) : _config(config), _pid(config.path + "/pid", config.pidConfig) {}
+Shooter::Shooter(ShooterConfig config) : _config(config), _pid(frc::PIDController(1, 0, 0, 0.05_s)) {}
 
 void Shooter::OnStart() {
   _pid.Reset();
@@ -12,9 +12,9 @@ void Shooter::OnStart() {
 
 void Shooter::OnUpdate(units::second_t dt) {
   // _pid.SetTolerance(0.5, 4);
-  table->GetEntry("Error").SetDouble(_pid.GetError().value());
+  table->GetEntry("Error").SetDouble(_pid.GetPositionError());
   // table->GetEntry("Acceleration Error").SetDouble(_pid.GetVelocityError());
-  table->GetEntry("SetPoint").SetDouble(_pid.GetSetpoint().value());
+  table->GetEntry("SetPoint").SetDouble(_pid.GetSetpoint());
   // table->GetEntry("Current
   // Pos").SetDouble(_config.ShooterGearbox.encoder->GetEncoderAngularVelocity().value());
   // table->GetEntry("EncoderValue").SetDouble(_config.ShooterGearbox.encoder->GetVelocityValue());
@@ -33,12 +33,12 @@ void Shooter::OnUpdate(units::second_t dt) {
     case ShooterState::kSpinUp: {
       _statename = "SpinUp";
       std::cout << "KSpinUp" << std::endl;
-      _pid.SetSetpoint(_goal);
+      _pid.SetSetpoint(_goal.value());
       units::volt_t pidCalculate =
-          units::volt_t{_pid.Calculate(_config.ShooterGearbox.encoder->GetEncoderAngularVelocity(), dt, 0_V)};
+          units::volt_t{_pid.Calculate(_config.ShooterGearbox.encoder->GetEncoderAngularVelocity().value())};
       std::cout << "KShooting" << std::endl;
 
-      if (_pid.IsStable()) {
+      if (_pid.GetPositionError() < 1 && _pid.GetVelocityError() < 1) {
         holdVoltage = pidCalculate;
         std::cout << "STABLE" << std::endl;
         _state = ShooterState::kShooting;
@@ -53,12 +53,12 @@ void Shooter::OnUpdate(units::second_t dt) {
     case ShooterState::kShooting: {
       _statename = "Shooting";
 
-      _pid.SetSetpoint(_goal);
+      _pid.SetSetpoint(_goal.value());
       units::volt_t pidCalculate =
-          units::volt_t{_pid.Calculate(_config.ShooterGearbox.encoder->GetEncoderAngularVelocity(), dt, 0_V)};
+          units::volt_t{_pid.Calculate(_config.ShooterGearbox.encoder->GetEncoderAngularVelocity().value())};
       std::cout << "KShooting" << std::endl;
 
-      if (_pid.IsStable()) {
+      if (_pid.GetPositionError() < 1 && _pid.GetVelocityError() < 1) {
         holdVoltage = pidCalculate;
         std::cout << "STABLE" << std::endl;
       }
diff --git a/src/main/include/AlphaArm.h b/src/main/include/AlphaArm.h
index 8429fa9d..a3c596cd 100644
--- a/src/main/include/AlphaArm.h
+++ b/src/main/include/AlphaArm.h
@@ -51,7 +51,7 @@ class AlphaArm : public behaviour::HasBehaviour {
   // units::radian_t CalcTargetAngle();
 
   AlphaArmConfig *_config;
-  wom::vision::Limelight* _vision;
+  // wom::vision::Limelight* _vision;
   AlphaArmState _state = AlphaArmState::kIdle;
   //wom::utils::PIDController<units::degree, units::volt> _alphaArmPID;
   //frc::DutyCycleEncoder armEncoder{4};
diff --git a/src/main/include/Intake.h b/src/main/include/Intake.h
index 9176fa0a..a4ecdca1 100644
--- a/src/main/include/Intake.h
+++ b/src/main/include/Intake.h
@@ -22,7 +22,7 @@ struct IntakeConfig {
   wom::PIDConfig<units::radians_per_second, units::volt> pidConfig;
 };
 
-enum class IntakeState { kIdle, kRaw, kHold, kEject, kIntake, kPass, kAdjust};
+enum class IntakeState { kIdle, kRaw, kHold, kEject, kIntake, kPass};
 
 class Intake : public behaviour::HasBehaviour {
  public:
diff --git a/src/main/include/Robot.h b/src/main/include/Robot.h
index b3b0ad2d..64ace7b4 100644
--- a/src/main/include/Robot.h
+++ b/src/main/include/Robot.h
@@ -24,8 +24,11 @@
 #include "AlphaArmBehaviour.h"
 #include "Intake.h"
 #include "IntakeBehaviour.h"
+#include "Shooter.h"
+#include "ShooterBehaviour.h"
 #include "RobotMap.h"
 #include "Wombat.h"
+#include "LED.h"
 
 class Robot : public frc::TimedRobot {
  public:
@@ -39,54 +42,32 @@ class Robot : public frc::TimedRobot {
   void TeleopPeriodic() override;
   void DisabledInit() override;
   void DisabledPeriodic() override;
-  void SimulationInit() override;
-  void SimulationPeriodic() override;
+  // void SimulationInit() override;
+  // void SimulationPeriodic() override;
 
  private:
   RobotMap robotmap;
   wom::BehaviourScheduler* sched;
   frc::EventLoop loop;
-  //Shooter* shooter;
+  Shooter* shooter;
+
+  Intake* intake;
 
-  // 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;
-
-  // Vision* _vision;
-
-  // 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,
-  // };
-  // Intake* intake;
   wom::SwerveDrive* _swerveDrive;
 
   // rev::CANSparkMax testMotorUp{1, rev::CANSparkMax::MotorType::kBrushless};
   // rev::CANSparkMax testMotorDown{6, rev::CANSparkMax::MotorType::kBrushless};
  // frc::XboxController testdriver = frc::XboxController(1);
   AlphaArm* alphaArm;
+  LED* _led;
 
 
   // ctre::phoenix6::hardware::TalonFX *frontLeft;
diff --git a/src/main/include/RobotMap.h b/src/main/include/RobotMap.h
index e6007525..2e5bb0ae 100644
--- a/src/main/include/RobotMap.h
+++ b/src/main/include/RobotMap.h
@@ -27,7 +27,7 @@
 #include <ctre/phoenix6/Pigeon2.hpp>
 #include <ctre/phoenix6/TalonFX.hpp>
 
-#include "Intake.h"
+// #include "Intake.h"
 #include "Wombat.h"
 #include "utils/Encoder.h"
 #include "utils/PID.h"
@@ -47,19 +47,30 @@ struct RobotMap {
     frc::XboxController testController = frc::XboxController(2);
   };
   Controllers controllers;
-  
-//    struct AlphaArmSystem {
-//     rev::CANSparkMax alphaArmMotorUp{21, rev::CANSparkMax::MotorType::kBrushless};
-//     rev::CANSparkMax alphaArmMotorDown{26, rev::CANSparkMax::MotorType::kBrushless};
-
-//     wom::DutyCycleEncoder alphaArmEncoder{3, 0.1_m, 2048, 1};
-//     wom::CANSparkMaxEncoder* alphaArmNeoEncoderUp = new wom::CANSparkMaxEncoder(&alphaArmMotorUp, 0.1_m);
-//     wom::CANSparkMaxEncoder* alphaArmNeoEncoderDown = new wom::CANSparkMaxEncoder(&alphaArmMotorDown, 0.1_m);
+
+    struct AlphaArmSystem {
+    rev::CANSparkMax alphaArmMotorUp{21, rev::CANSparkMax::MotorType::kBrushless};
+    rev::CANSparkMax alphaArmMotorDown{26, rev::CANSparkMax::MotorType::kBrushless};
+
+    wom::DutyCycleEncoder alphaArmEncoder{3, 0.1_m, 2048, 1};
+    wom::CANSparkMaxEncoder* alphaArmNeoEncoderUp = new wom::CANSparkMaxEncoder(&alphaArmMotorUp, 0.1_m);
+    wom::CANSparkMaxEncoder* alphaArmNeoEncoderDown = new wom::CANSparkMaxEncoder(&alphaArmMotorDown, 0.1_m);
+
+
+    wom::Gearbox alphaArmGearbox{&alphaArmMotorUp, alphaArmNeoEncoderUp, frc::DCMotor::NEO(1)};
+    wom::Gearbox alphaArmGearbox2{&alphaArmMotorDown, alphaArmNeoEncoderDown, frc::DCMotor::NEO(1)};
+
+    AlphaArmConfig config{alphaArmGearbox, alphaArmGearbox2, alphaArmEncoder, "/alphaArm"
+    //, &vision
+    };
+  };
+  AlphaArmSystem alphaArmSystem;
+
   struct IntakeSystem {
     rev::CANSparkMax intakeMotor{35, rev::CANSparkMax::MotorType::kBrushless};
     wom::CANSparkMaxEncoder intakeEncoder{&intakeMotor, 0.1_m};
     frc::DigitalInput intakeSensor{5};
-    frc::DigitalInput passSensor{99};
+    frc::DigitalInput passSensor{7};
     // frc::DigitalInput magSensor{0};
     // frc::DigitalInput shooterSensor{0};
 
@@ -74,22 +85,7 @@ struct RobotMap {
   };
   IntakeSystem intakeSystem;
 
-  struct AlphaArmSystem {
-    rev::CANSparkMax alphaArmMotor{12, rev::CANSparkMax::MotorType::kBrushless};
-    rev::CANSparkMax wristMotor{15, rev::CANSparkMax::MotorType::kBrushless};
-
-
-    wom::Gearbox alphaArmGearbox{&alphaArmMotorUp, alphaArmNeoEncoderUp, frc::DCMotor::NEO(1)};
-    wom::Gearbox alphaArmGearbox2{&alphaArmMotorDown, alphaArmNeoEncoderDown, frc::DCMotor::NEO(1)};
-
-    AlphaArmConfig config{alphaArmGearbox, alphaArmGearbox2, alphaArmEncoder, "/alphaArm"
-    //, &vision
-    };
-  };
-  AlphaArmSystem alphaArmSystem;
 
-
-  
   struct SwerveBase {
     ctre::phoenix6::hardware::CANcoder frontLeftCancoder{16, "Drivebase"};
     ctre::phoenix6::hardware::CANcoder frontRightCancoder{18, "Drivebase"};
@@ -205,9 +201,24 @@ struct RobotMap {
   struct SwerveTable {
     std::shared_ptr<nt::NetworkTable> swerveDriveTable = nt::NetworkTableInstance::GetDefault().GetTable("swerve");
   }; SwerveTable swerveTable;
-};
-<<<<<<< HEAD
 
-=======
+
+    struct Shooter {
+    rev::CANSparkMax shooterMotor{31, rev::CANSparkMax::MotorType::kBrushless};// Port 11
+    // frc::DigitalInput shooterSensor{2};
+
+    // wom::VoltageController shooterMotorGroup = wom::VoltagedController::Group(shooterMotor);
+    wom::CANSparkMaxEncoder* shooterEncoder = new wom::CANSparkMaxEncoder(&shooterMotor, 0.01_m);
+    wom::Gearbox shooterGearbox{&shooterMotor, shooterEncoder, frc::DCMotor::NEO(1)};
+
+    
+
+    ShooterConfig config{
+        "shooterGearbox",
+        shooterGearbox,
+    };
+
+  };
+  Shooter shooterSystem;
+};
   // AlphaArmSystem alphaArmSystem;
->>>>>>> 4a40db1df8a44d2b159b6e305bf230a715cab266
diff --git a/src/main/include/Shooter.h b/src/main/include/Shooter.h
index d2c22cb5..fa022180 100644
--- a/src/main/include/Shooter.h
+++ b/src/main/include/Shooter.h
@@ -17,7 +17,6 @@ struct ShooterConfig {
   wom::Gearbox ShooterGearbox;
   // wom::PIDConfig<units::radians_per_second, units::volt> pidConfig;
   // frc::DigitalInput* shooterSensor;
-  wom::PIDConfig<units::radians_per_second, units::volt> pidConfig;
 };
 
 enum class ShooterState { kIdle, kShooting, kSpinUp, kReverse, kRaw };
@@ -41,9 +40,10 @@ class Shooter : public behaviour::HasBehaviour {
   units::volt_t _rawVoltage;
   units::radians_per_second_t _goal;
   units::volt_t _setVoltage = 0_V;
-  wom::utils::PIDController<units::radians_per_second, units::volt> _pid;
   // frc::DigitalInput _shooterSensor{0};
 
+  frc::PIDController _pid;
+
   units::volt_t holdVoltage = 0_V;
   std::string _statename = "default";
 };
diff --git a/wombat/src/main/cpp/behaviour/Behaviour.cpp b/wombat/src/main/cpp/behaviour/Behaviour.cpp
index d21d9e57..7312c922 100644
--- a/wombat/src/main/cpp/behaviour/Behaviour.cpp
+++ b/wombat/src/main/cpp/behaviour/Behaviour.cpp
@@ -10,9 +10,7 @@ using namespace behaviour;
 
 // Behaviour
 Behaviour::Behaviour(std::string name, units::time::second_t period)
-    : _bhvr_name(name),
-      _bhvr_period(period),
-      _bhvr_state(BehaviourState::INITIALISED) {}
+    : _bhvr_name(name), _bhvr_period(period), _bhvr_state(BehaviourState::INITIALISED) {}
 Behaviour::~Behaviour() {
   if (!IsFinished())
     Interrupt();
@@ -82,8 +80,7 @@ bool Behaviour::Tick() {
 
     if (dt > 2 * _bhvr_period) {
       std::cerr << "Behaviour missed deadline. Reduce Period. Dt=" << dt.value()
-                << " Dt(deadline)=" << (2 * _bhvr_period).value()
-                << ". Bhvr: " << GetName() << std::endl;
+                << " Dt(deadline)=" << (2 * _bhvr_period).value() << ". Bhvr: " << GetName() << std::endl;
     }
 
     if (_bhvr_timeout.value() > 0 && _bhvr_timer > _bhvr_timeout)
@@ -100,8 +97,7 @@ bool Behaviour::IsRunning() const {
 }
 
 bool Behaviour::IsFinished() const {
-  return _bhvr_state != BehaviourState::INITIALISED &&
-         _bhvr_state != BehaviourState::RUNNING;
+  return _bhvr_state != BehaviourState::INITIALISED && _bhvr_state != BehaviourState::RUNNING;
 }
 
 void Behaviour::Stop(BehaviourState new_state) {
@@ -173,8 +169,7 @@ void ConcurrentBehaviour::Add(Behaviour::ptr behaviour) {
 }
 
 std::string ConcurrentBehaviour::GetName() const {
-  std::string msg =
-      (_reducer == ConcurrentBehaviourReducer::ALL ? "ALL { " : "RACE {");
+  std::string msg = (_reducer == ConcurrentBehaviourReducer::ALL ? "ALL { " : "RACE {");
   for (auto b : _children)
     msg += b->GetName() + ", ";
   msg += "}";
@@ -190,8 +185,8 @@ void ConcurrentBehaviour::OnStart() {
         using namespace std::chrono_literals;
 
         b->Tick();
-        std::this_thread::sleep_for(std::chrono::milliseconds(
-            static_cast<int64_t>(b->GetPeriod().value() * 1000)));
+        std::this_thread::sleep_for(
+            std::chrono::milliseconds(static_cast<int64_t>(b->GetPeriod().value() * 1000)));
       }
 
       if (IsFinished() && !b->IsFinished())
@@ -273,10 +268,8 @@ void WaitFor::OnTick(units::time::second_t dt) {
 }
 
 // WaitTime
-WaitTime::WaitTime(units::time::second_t time)
-    : WaitTime([time]() { return time; }) {}
-WaitTime::WaitTime(std::function<units::time::second_t()> time_fn)
-    : _time_fn(time_fn) {}
+WaitTime::WaitTime(units::time::second_t time) : WaitTime([time]() { return time; }) {}
+WaitTime::WaitTime(std::function<units::time::second_t()> time_fn) : _time_fn(time_fn) {}
 
 void WaitTime::OnStart() {
   _time = _time_fn();
diff --git a/wombat/src/main/cpp/behaviour/BehaviourScheduler.cpp b/wombat/src/main/cpp/behaviour/BehaviourScheduler.cpp
index 26863c79..eb363b3e 100644
--- a/wombat/src/main/cpp/behaviour/BehaviourScheduler.cpp
+++ b/wombat/src/main/cpp/behaviour/BehaviourScheduler.cpp
@@ -52,8 +52,8 @@ void BehaviourScheduler::Schedule(Behaviour::ptr behaviour) {
         std::lock_guard<std::recursive_mutex> lk(_active_mtx);
         behaviour->Tick();
       }
-      std::this_thread::sleep_for(std::chrono::milliseconds(
-          static_cast<int64_t>(behaviour->GetPeriod().value() * 1000)));
+      std::this_thread::sleep_for(
+          std::chrono::milliseconds(static_cast<int64_t>(behaviour->GetPeriod().value() * 1000)));
     }
   });
 }
diff --git a/wombat/src/main/cpp/behaviour/HasBehaviour.cpp b/wombat/src/main/cpp/behaviour/HasBehaviour.cpp
index 964522c6..f2f557c5 100644
--- a/wombat/src/main/cpp/behaviour/HasBehaviour.cpp
+++ b/wombat/src/main/cpp/behaviour/HasBehaviour.cpp
@@ -8,8 +8,7 @@
 
 using namespace behaviour;
 
-void HasBehaviour::SetDefaultBehaviour(
-    std::function<std::shared_ptr<Behaviour>(void)> fn) {
+void HasBehaviour::SetDefaultBehaviour(std::function<std::shared_ptr<Behaviour>(void)> fn) {
   _default_behaviour_producer = fn;
 }
 
diff --git a/wombat/src/main/cpp/drivetrain/SwerveDrive.cpp b/wombat/src/main/cpp/drivetrain/SwerveDrive.cpp
index c21f6972..9fbdc6a4 100644
--- a/wombat/src/main/cpp/drivetrain/SwerveDrive.cpp
+++ b/wombat/src/main/cpp/drivetrain/SwerveDrive.cpp
@@ -12,21 +12,17 @@
 #include "utils/Util.h"
 
 #include <iostream>
-#include <algorithm>
 #include <cmath>
 
+#include "frc/MathUtil.h"
 #include <wpi/sendable/SendableBuilder.h>
 #include <wpi/sendable/SendableRegistry.h>
 
-#include "frc/MathUtil.h"
-#include "wpimath/MathShared.h"
-
 using namespace wom;
 
 namespace wom {
 namespace drivetrain {
 
-
 PIDController::PIDController(double Kp, double Ki, double Kd,
                              units::second_t period)
     : m_Kp(Kp), m_Ki(Ki), m_Kd(Kd), m_period(period) {
@@ -282,8 +278,6 @@ void SwerveModule::OnUpdate(units::second_t dt) {
   units::volt_t driveVoltage{0};
   units::volt_t turnVoltage{0};
 
-
-
   switch (_state) {
     case SwerveModuleState::kIdle:
       driveVoltage = 0_V;
@@ -297,11 +291,11 @@ void SwerveModule::OnUpdate(units::second_t dt) {
       double input = _config.turnMotor.encoder->GetEncoderPosition().value();
       _table->GetEntry("/testing/GetEncoderPos").SetDouble(input);
       // _velocityPIDController.SetSetpoint(3);
-      
+
 
       driveVoltage = units::volt_t{_velocityPIDController.Calculate(GetSpeed().value())};
       // if (_turnOffset == TurnOffsetValues::forward) {
-        
+
       // } else if (_turnOffset == TurnOffsetValues::reverse) {
       //   input = input - (3.1415/2);
       //   driveVoltage = -driveVoltage;
@@ -538,9 +532,8 @@ void SwerveDrive::OnUpdate(units::second_t dt) {
         _angle = _target_speed.omega * 1_s;
       }
 
-      
       bool init = false;
-      
+
       frc::ChassisSpeeds new_target_speed {_target_speed.vx, _target_speed.vy, -_target_speed.omega};
       auto target_states = _kinematics.ToSwerveModuleStates(_target_speed);
       auto new_target_states = _kinematics.ToSwerveModuleStates(new_target_speed);
@@ -550,7 +543,6 @@ void SwerveDrive::OnUpdate(units::second_t dt) {
         } else {
           _modules[i].SetPID(target_states[i].angle.Radians(), target_states[i].speed, dt);
         }
-
       }
     } break;
     case SwerveDriveState::kIndividualTuning:
@@ -703,8 +695,7 @@ frc::Pose2d SwerveDrive::GetPose() {
   return _poseEstimator.GetEstimatedPosition();
 }
 
-void SwerveDrive::AddVisionMeasurement(frc::Pose2d pose,
-                                       units::second_t timestamp) {
+void SwerveDrive::AddVisionMeasurement(frc::Pose2d pose, units::second_t timestamp) {
   _poseEstimator.AddVisionMeasurement(pose, timestamp);
 }
 }  // namespace drivetrain
diff --git a/wombat/src/main/cpp/drivetrain/behaviours/SwerveBehaviours.cpp b/wombat/src/main/cpp/drivetrain/behaviours/SwerveBehaviours.cpp
index 32fc2b81..fd9a91be 100644
--- a/wombat/src/main/cpp/drivetrain/behaviours/SwerveBehaviours.cpp
+++ b/wombat/src/main/cpp/drivetrain/behaviours/SwerveBehaviours.cpp
@@ -110,7 +110,7 @@ void ManualDrivebase::OnTick(units::second_t deltaTime) {
     //  _swerveDrivebase->SetVelocity(
     //       frc::ChassisSpeeds{xVelocity * maxMovementMagnitude,
     //                          yVelocity * maxMovementMagnitude,
-    //                          r_x * maxRotationMagnitude});                                          
+    //                          r_x * maxRotationMagnitude});
   //   }
   // }
   // _swerveDrivebase->SetIndividualTuning(2, 0_deg, 0_mps);
@@ -121,8 +121,7 @@ void ManualDrivebase::ResetMode() {
   resetMode = false;
 }
 
-void ManualDrivebase::CalculateRequestedAngle(double joystickX,
-                                              double joystickY,
+void ManualDrivebase::CalculateRequestedAngle(double joystickX, double joystickY,
                                               units::degree_t defaultAngle) {
   _requestedAngle = (1_rad * std::atan2(joystickY, -joystickX)) + 90_deg;
   if (wom::utils::deadzone(joystickX) == 0 && wom::utils::deadzone(joystickY) == 0) {
@@ -131,8 +130,7 @@ void ManualDrivebase::CalculateRequestedAngle(double joystickX,
 }
 
 // Code for x-ing the wheels on the drivebase
-XDrivebase::XDrivebase(wom::drivetrain::SwerveDrive* swerveDrivebase)
-    : _swerveDrivebase(swerveDrivebase) {
+XDrivebase::XDrivebase(wom::drivetrain::SwerveDrive* swerveDrivebase) : _swerveDrivebase(swerveDrivebase) {
   Controls(swerveDrivebase);
 }
 void XDrivebase::OnTick(units::second_t deltaTime) {
@@ -205,8 +203,7 @@ frc::Pose2d wom::drivetrain::behaviours::TempSimSwerveDrive::GetPose2d() {
   return m_driveSim.GetPose();
 }
 
-void wom::drivetrain::behaviours::TempSimSwerveDrive::SetPath(
-    std::string path) {
+void wom::drivetrain::behaviours::TempSimSwerveDrive::SetPath(std::string path) {
   nt::NetworkTableInstance inst = nt::NetworkTableInstance::GetDefault();
   std::shared_ptr<nt::NetworkTable> table = inst.GetTable("FMSInfo");
 
diff --git a/wombat/src/main/cpp/utils/Encoder.cpp b/wombat/src/main/cpp/utils/Encoder.cpp
index baf2bb91..0c04e682 100644
--- a/wombat/src/main/cpp/utils/Encoder.cpp
+++ b/wombat/src/main/cpp/utils/Encoder.cpp
@@ -6,8 +6,8 @@
 
 #include <rev/SparkRelativeEncoder.h>
 
-wom::utils::Encoder::Encoder(double encoderTicksPerRotation, int type,
-                             units::meter_t wheelRadius, double reduction)
+wom::utils::Encoder::Encoder(double encoderTicksPerRotation, int type, units::meter_t wheelRadius,
+                             double reduction)
     : _reduction(reduction),
       _encoderTicksPerRotation(encoderTicksPerRotation),
       _type(type),
@@ -32,8 +32,8 @@ void wom::utils::Encoder::SetEncoderPosition(units::degree_t position) {
   // _offset = -offset_turns;
 }
 
-void wom::utils::Encoder::SetEncoderOffset(units::radian_t offset) { //HERE!
-  _offset = offset; 
+void wom::utils::Encoder::SetEncoderOffset(units::radian_t offset) {  // HERE!
+  _offset = offset;
   // units::turn_t offset_turns = offset;
   // _offset = offset_turns.value() * GetEncoderTicksPerRotation();
 }
@@ -43,16 +43,16 @@ void wom::utils::Encoder::SetReduction(double reduction) {
 }
 
 units::radian_t wom::utils::Encoder::GetEncoderPosition() {
-  //if (_type == 0) {
-  //  units::turn_t n_turns{GetEncoderTicks() / GetEncoderTicksPerRotation()};
-  //  return n_turns;
-  //} else if (_type == 2) {
-  //  units::degree_t pos = GetEncoderTicks() * 1_deg;
-  //  return pos;
-  //} else {
-  //  units::degree_t pos = GetEncoderTicks() * 1_deg;
-  //  return pos - _offset;
-  //}
+  // if (_type == 0) {
+  //   units::turn_t n_turns{GetEncoderTicks() / GetEncoderTicksPerRotation()};
+  //   return n_turns;
+  // } else if (_type == 2) {
+  //   units::degree_t pos = GetEncoderTicks() * 1_deg;
+  //   return pos;
+  // } else {
+  //   units::degree_t pos = GetEncoderTicks() * 1_deg;
+  //   return pos - _offset;
+  // }
   return GetEncoderTicks() * 1_rad;
 }
 
@@ -91,11 +91,6 @@ wom::utils::CANSparkMaxEncoder::CANSparkMaxEncoder(rev::CANSparkMax* controller,
                                                    double reduction)
     : wom::utils::Encoder(42, reduction, wheelRadius, 2),
       _encoder(controller->GetEncoder(rev::SparkRelativeEncoder::Type::kHallSensor)) {}
-<<<<<<< HEAD
-
-=======
->>>>>>> 4a40db1df8a44d2b159b6e305bf230a715cab266
-
 double wom::utils::CANSparkMaxEncoder::GetEncoderRawTicks() const {
   return ((_encoder.GetPosition() * 2 * 3.1415) / 200);
 }
@@ -152,11 +147,12 @@ double wom::utils::DutyCycleEncoder::GetEncoderRawTicks() const {
 double wom::utils::DutyCycleEncoder::GetEncoderTickVelocity() const {
   return 0;
 }
-wom::utils::CanEncoder::CanEncoder(int deviceNumber, units::meter_t wheelRadius,
-                                   double ticksPerRotation, double reduction,
-                                   std::string name)
+
+wom::utils::CanEncoder::CanEncoder(int deviceNumber, units::meter_t wheelRadius, double ticksPerRotation,
+                                   double reduction, std::string name)
     : wom::utils::Encoder(ticksPerRotation, 2, wheelRadius, reduction) {
   _canEncoder = new ctre::phoenix6::hardware::CANcoder(deviceNumber, name);
+  // _canEncoder->ConfigAbsoluteEncoderRange(0, 1);
 }
 
 double wom::utils::CanEncoder::GetEncoderRawTicks() const {
@@ -165,4 +161,4 @@ double wom::utils::CanEncoder::GetEncoderRawTicks() const {
 
 double wom::utils::CanEncoder::GetEncoderTickVelocity() const {
   return _canEncoder->GetVelocity().GetValue().value();
-}
+}
\ No newline at end of file
diff --git a/wombat/src/main/cpp/utils/Util.cpp b/wombat/src/main/cpp/utils/Util.cpp
index e529e245..0aa4b888 100644
--- a/wombat/src/main/cpp/utils/Util.cpp
+++ b/wombat/src/main/cpp/utils/Util.cpp
@@ -13,21 +13,18 @@ units::second_t wom::utils::now() {
   return static_cast<double>(now) / 1000000 * 1_s;
 }
 
-void wom::utils::WritePose2NT(std::shared_ptr<nt::NetworkTable> table,
-                              frc::Pose2d pose) {
+void wom::utils::WritePose2NT(std::shared_ptr<nt::NetworkTable> table, frc::Pose2d pose) {
   table->GetEntry("x").SetDouble(pose.X().value());
   table->GetEntry("y").SetDouble(pose.Y().value());
   table->GetEntry("angle").SetDouble(pose.Rotation().Degrees().value());
 }
 
-void wom::utils::WritePose3NT(std::shared_ptr<nt::NetworkTable> table,
-                              frc::Pose3d pose) {
+void wom::utils::WritePose3NT(std::shared_ptr<nt::NetworkTable> table, frc::Pose3d pose) {
   table->GetEntry("x").SetDouble(pose.X().value());
   table->GetEntry("y").SetDouble(pose.Y().value());
   table->GetEntry("z").SetDouble(pose.Z().value());
 
-  table->GetEntry("angle").SetDouble(
-      pose.Rotation().Z().convert<units::degree>().value());
+  table->GetEntry("angle").SetDouble(pose.Rotation().Z().convert<units::degree>().value());
 }
 
 double wom::utils::deadzone(double val, double deadzone) {
@@ -50,8 +47,7 @@ units::volt_t wom::utils::GetVoltage(frc::MotorController* controller) {
   return controller->Get() * frc::RobotController::GetBatteryVoltage();
 }
 
-void wom::utils::WriteTrajectory(std::shared_ptr<nt::NetworkTable> table,
-                                 frc::Trajectory trajectory) {
+void wom::utils::WriteTrajectory(std::shared_ptr<nt::NetworkTable> table, frc::Trajectory trajectory) {
   table->GetEntry("length").SetDouble(trajectory.TotalTime().value());
 
   // write the trajectory to the network table
@@ -74,8 +70,7 @@ void wom::utils::WriteTrajectory(std::shared_ptr<nt::NetworkTable> table,
   }
 }
 
-void wom::utils::WriteTrajectoryState(std::shared_ptr<nt::NetworkTable> table,
-                                      frc::Trajectory::State state) {
+void wom::utils::WriteTrajectoryState(std::shared_ptr<nt::NetworkTable> table, frc::Trajectory::State state) {
   table->GetEntry("x").SetDouble(state.pose.X().value());
   table->GetEntry("y").SetDouble(state.pose.Y().value());
   table->GetEntry("angle").SetDouble(state.pose.Rotation().Degrees().value());
diff --git a/wombat/src/main/cpp/vision/Limelight.cpp b/wombat/src/main/cpp/vision/Limelight.cpp
index e2fe4120..437d8648 100644
--- a/wombat/src/main/cpp/vision/Limelight.cpp
+++ b/wombat/src/main/cpp/vision/Limelight.cpp
@@ -1,218 +1,214 @@
-// Copyright (c) 2023-2024 CurtinFRC
-// Open Source Software, you can modify it according to the terms
-// of the MIT License at the root of this project
+// // Copyright (c) 2023-2024 CurtinFRC
+// // Open Source Software, you can modify it according to the terms
+// // of the MIT License at the root of this project
 
-#include "vision/Limelight.h"
+// #include "vision/Limelight.h"
 
-#include <iostream>
+// #include <iostream>
 
-#include "utils/Util.h"
+// #include "utils/Util.h"
 
-wom::vision::Limelight::Limelight(std::string limelightName)
-    : _limelightName(limelightName) {}
+// wom::vision::Limelight::Limelight(std::string limelightName) : _limelightName(limelightName) {
+//   table = nt::NetworkTableInstance::GetDefault().GetTable(_limelightName);
+// }
 
-std::string wom::vision::Limelight::GetName() {
-  return _limelightName;
-}
+// std::string wom::vision::Limelight::GetName() {
+//   return _limelightName;
+// }
 
-std::pair<double, double> wom::vision::Limelight::GetOffset() {
-  std::pair<double, double> offset;
+// std::pair<double, double> wom::vision::Limelight::GetOffset() {
+//   std::pair<double, double> offset;
 
-  offset.first = table->GetNumber("tx", 0.0);
-  offset.second = table->GetNumber("ty", 0.0);
+//   offset.first = table->GetNumber("tx", 0.0);
+//   offset.second = table->GetNumber("ty", 0.0);
 
-  return offset;
-}
+//   return offset;
+// }
 
-std::vector<double> wom::vision::Limelight::GetAprilTagData(
-    LimelightAprilTagData dataType) {
-  std::string dataName;
+// std::vector<double> wom::vision::Limelight::GetAprilTagData(LimelightAprilTagData dataType) {
+//   std::string dataName;
 
-  switch (dataType) {
-    case LimelightAprilTagData::kBotpose:
-      dataName = "botpose";
-      break;
+//   switch (dataType) {
+//     case LimelightAprilTagData::kBotpose:
+//       dataName = "botpose";
+//       break;
 
-    case LimelightAprilTagData::kBotpose_wpiblue:
-      dataName = "botpose_wpiblue";
-      break;
+//     case LimelightAprilTagData::kBotpose_wpiblue:
+//       dataName = "botpose_wpiblue";
+//       break;
 
-    case LimelightAprilTagData::kBotpose_wpired:
-      dataName = "botpose_wpired";
-      break;
+//     case LimelightAprilTagData::kBotpose_wpired:
+//       dataName = "botpose_wpired";
+//       break;
 
-    case LimelightAprilTagData::kCamerapose_targetspace:
-      dataName = "camerapose_targetspace";
-      break;
+//     case LimelightAprilTagData::kCamerapose_targetspace:
+//       dataName = "camerapose_targetspace";
+//       break;
 
-    case LimelightAprilTagData::kTargetpose_cameraspace:
-      dataName = "targetpose_cameraspace";
-      break;
+//     case LimelightAprilTagData::kTargetpose_cameraspace:
+//       dataName = "targetpose_cameraspace";
+//       break;
 
-    case LimelightAprilTagData::kTargetpose_robotspace:
-      dataName = "targetpose_robotspace";
-      break;
+//     case LimelightAprilTagData::kTargetpose_robotspace:
+//       dataName = "targetpose_robotspace";
+//       break;
 
-    case LimelightAprilTagData::kBotpose_targetspace:
-      dataName = "botpose_targetspace	";
-      break;
+//     case LimelightAprilTagData::kBotpose_targetspace:
+//       dataName = "botpose_targetspace	";
+//       break;
 
-    case LimelightAprilTagData::kCamerapose_robotspace:
-      dataName = "camerapose_robotspace";
-      break;
-  }
+//     case LimelightAprilTagData::kCamerapose_robotspace:
+//       dataName = "camerapose_robotspace";
+//       break;
+//   }
 
-  return table->GetEntry(dataName).GetDoubleArray(std::vector<double>(6));
-}
+//   return table->GetEntry(dataName).GetDoubleArray(std::vector<double>(6));
+// }
 
-double wom::vision::Limelight::GetTargetingData(LimelightTargetingData dataType,
-                                                double defaultValue) {
-  std::string dataName;
+// double wom::vision::Limelight::GetTargetingData(LimelightTargetingData dataType, double defaultValue) {
+//   std::string dataName;
 
-  switch (dataType) {
-    case LimelightTargetingData::kTv:
-      dataName = "tv";
-      break;
+//   switch (dataType) {
+//     case LimelightTargetingData::kTv:
+//       dataName = "tv";
+//       break;
 
-    case LimelightTargetingData::kTx:
-      dataName = "tx";
-      break;
+//     case LimelightTargetingData::kTx:
+//       dataName = "tx";
+//       break;
 
-    case LimelightTargetingData::kTy:
-      dataName = "ty";
-      break;
+//     case LimelightTargetingData::kTy:
+//       dataName = "ty";
+//       break;
 
-    case LimelightTargetingData::kTa:
-      dataName = "ta";
-      break;
-
-    case LimelightTargetingData::kTl:
-      dataName = "tl";
-      break;
-
-    case LimelightTargetingData::kCl:
-      dataName = "cl";
-      break;
-
-    case LimelightTargetingData::kTshort:
-      dataName = "tshort";
-      break;
-
-    case LimelightTargetingData::kTlong:
-      dataName = "tlong";
-      break;
-
-    case LimelightTargetingData::kThor:
-      dataName = "thor";
-      break;
-
-    case LimelightTargetingData::kTvert:
-      dataName = "tvert";
-      break;
-
-    case LimelightTargetingData::kGetpipe:
-      dataName = "getpipe";
-      break;
-
-    case LimelightTargetingData::kJson:
-      dataName = "json";
-      break;
-
-    case LimelightTargetingData::kTclass:
-      dataName = "tclass";
-      break;
-
-    case LimelightTargetingData::kTc:
-      dataName = "tc";
-      break;
-
-    case LimelightTargetingData::kTid:
-      dataName = "tid";
-      break;
-  }
-
-  return table->GetEntry(dataName).GetDouble(0);
-}
-
-void wom::vision::Limelight::SetLEDMode(LimelightLEDMode mode) {
-  table->PutNumber("ledMode", static_cast<double>(mode));
-}
-
-void wom::vision::Limelight::SetCamMode(LimelightCamMode mode) {
-  table->PutNumber("camMode", static_cast<double>(mode));
-}
-
-void wom::vision::Limelight::SetPipeline(LimelightPipeline pipeline) {
-  table->PutNumber("pipeline", static_cast<double>(pipeline));
-}
-
-void wom::vision::Limelight::SetStreamMode(LimelightStreamMode mode) {
-  table->PutNumber("stream", static_cast<double>(mode));
-}
-
-void wom::vision::Limelight::SetSnapshotMode(LimelightSnapshotMode mode) {
-  table->PutNumber("snapshot", static_cast<double>(mode));
-}
-
-void wom::vision::Limelight::SetCrop(std::array<double, 4> crop) {
-  table->PutNumberArray("camtran", crop);
-}
-
-units::meters_per_second_t wom::vision::Limelight::GetSpeed(
-    frc::Pose3d pose1, frc::Pose3d pose2, units::second_t dt) {
-  frc::Transform3d dPose{pose1, pose2};
-  frc::Translation3d dTranslation = dPose.Translation();
-
-  units::meter_t y = dTranslation.Y();
-  units::meter_t x = dTranslation.X();
-  units::radian_t theta = units::math::atan(y / x);
-  units::meter_t dTRANSLATION = x / units::math::cos(theta);
-  return units::math::fabs(dTRANSLATION / dt);
-}
-
-units::meters_per_second_t wom::vision::Limelight::GetSpeed(frc::Pose2d pose1, frc::Pose2d pose2,
-                                                            units::second_t dt) {
-  frc::Transform2d dPose{pose1, pose2};
-  frc::Translation2d dTranslation = dPose.Translation();
-
-  units::meter_t y = dTranslation.Y();
-  units::meter_t x = dTranslation.X();
-  units::radian_t theta = units::math::atan(y / x);
-  units::meter_t dTRANSLATION = x / units::math::cos(theta);
-  return units::math::fabs(dTRANSLATION / dt);
-}
-
-frc::Pose3d wom::vision::Limelight::GetPose() {
-  std::vector<double> pose = GetAprilTagData(LimelightAprilTagData::kBotpose);
-  return frc::Pose3d(
-      pose[1] * 1_m, 1_m * pose[2], 1_m * pose[3],
-      frc::Rotation3d(1_deg * (pose[4]), 1_deg * (pose[5]), 1_deg * (pose[6])));
-}
-
-void wom::vision::Limelight::OnStart() {
-  std::cout << "starting" << std::endl;
-}
-
-void wom::vision::Limelight::OnUpdate(units::time::second_t dt) {
-  wom::utils::WritePose3NT(table, GetPose());
-}
-
-bool wom::vision::Limelight::IsAtSetPoseVision(frc::Pose3d pose,
-                                               units::second_t dt) {
-  frc::Pose3d actualPose = GetPose();
-  frc::Pose3d relativePose = actualPose.RelativeTo(pose);
-  return (units::math::fabs(relativePose.X()) < 0.01_m &&
-          units::math::fabs(relativePose.Y()) < 0.01_m &&
-          GetSpeed(pose, GetPose(), dt) < 1_m / 1_s);
-}
-
-bool wom::vision::Limelight::IsAtSetPoseVision(frc::Pose2d pose, units::second_t dt) {
-  frc::Pose2d actualPose = GetPose().ToPose2d();
-  frc::Pose2d relativePose = actualPose.RelativeTo(pose);
-  return (units::math::fabs(relativePose.X()) < 0.01_m && units::math::fabs(relativePose.Y()) < 0.01_m &&
-          GetSpeed(pose, GetPose().ToPose2d(), dt) < 1_m / 1_s);
-}
-
-bool wom::vision::Limelight::HasTarget() {
-  return GetTargetingData(LimelightTargetingData::kTv) == 1.0;
-}
+//     case LimelightTargetingData::kTa:
+//       dataName = "ta";
+//       break;
+
+//     case LimelightTargetingData::kTl:
+//       dataName = "tl";
+//       break;
+
+//     case LimelightTargetingData::kCl:
+//       dataName = "cl";
+//       break;
+
+//     case LimelightTargetingData::kTshort:
+//       dataName = "tshort";
+//       break;
+
+//     case LimelightTargetingData::kTlong:
+//       dataName = "tlong";
+//       break;
+
+//     case LimelightTargetingData::kThor:
+//       dataName = "thor";
+//       break;
+
+//     case LimelightTargetingData::kTvert:
+//       dataName = "tvert";
+//       break;
+
+//     case LimelightTargetingData::kGetpipe:
+//       dataName = "getpipe";
+//       break;
+
+//     case LimelightTargetingData::kJson:
+//       dataName = "json";
+//       break;
+
+//     case LimelightTargetingData::kTclass:
+//       dataName = "tclass";
+//       break;
+
+//     case LimelightTargetingData::kTc:
+//       dataName = "tc";
+//       break;
+
+//     case LimelightTargetingData::kTid:
+//       dataName = "tid";
+//       break;
+//   }
+
+//   return table->GetEntry(dataName).GetDouble(0);
+// }
+
+// void wom::vision::Limelight::SetLEDMode(LimelightLEDMode mode) {
+//   table->PutNumber("ledMode", static_cast<double>(mode));
+// }
+
+// void wom::vision::Limelight::SetCamMode(LimelightCamMode mode) {
+//   table->PutNumber("camMode", static_cast<double>(mode));
+// }
+
+// void wom::vision::Limelight::SetPipeline(LimelightPipeline pipeline) {
+//   table->PutNumber("pipeline", static_cast<double>(pipeline));
+// }
+
+// void wom::vision::Limelight::SetStreamMode(LimelightStreamMode mode) {
+//   table->PutNumber("stream", static_cast<double>(mode));
+// }
+
+// void wom::vision::Limelight::SetSnapshotMode(LimelightSnapshotMode mode) {
+//   table->PutNumber("snapshot", static_cast<double>(mode));
+// }
+
+// void wom::vision::Limelight::SetCrop(std::array<double, 4> crop) {
+//   table->PutNumberArray("camtran", crop);
+// }
+
+// units::meters_per_second_t wom::vision::Limelight::GetSpeed(frc::Pose3d pose1, frc::Pose3d pose2,
+//                                                             units::second_t dt) {
+//   frc::Transform3d dPose{pose1, pose2};
+//   frc::Translation3d dTranslation = dPose.Translation();
+
+//   units::meter_t y = dTranslation.Y();
+//   units::meter_t x = dTranslation.X();
+//   units::radian_t theta = units::math::atan(y / x);
+//   units::meter_t dTRANSLATION = x / units::math::cos(theta);
+//   return units::math::fabs(dTRANSLATION / dt);
+// }
+
+// units::meters_per_second_t wom::vision::Limelight::GetSpeed(frc::Pose2d pose1, frc::Pose2d pose2,
+//                                                             units::second_t dt) {
+//   frc::Transform2d dPose{pose1, pose2};
+//   frc::Translation2d dTranslation = dPose.Translation();
+
+//   units::meter_t y = dTranslation.Y();
+//   units::meter_t x = dTranslation.X();
+//   units::radian_t theta = units::math::atan(y / x);
+//   units::meter_t dTRANSLATION = x / units::math::cos(theta);
+//   return units::math::fabs(dTRANSLATION / dt);
+// }
+
+// frc::Pose3d wom::vision::Limelight::GetPose() {
+//   std::vector<double> pose = GetAprilTagData(LimelightAprilTagData::kBotpose);
+//   return frc::Pose3d(pose[1] * 1_m, 1_m * pose[2], 1_m * pose[3],
+//                      frc::Rotation3d(1_deg * (pose[4]), 1_deg * (pose[5]), 1_deg * (pose[6])));
+// }
+
+// void wom::vision::Limelight::OnStart() {
+//   std::cout << "starting" << std::endl;
+// }
+
+// void wom::vision::Limelight::OnUpdate(units::time::second_t dt) {
+//   wom::utils::WritePose3NT(table, GetPose());
+// }
+
+// bool wom::vision::Limelight::IsAtSetPoseVision(frc::Pose3d pose, units::second_t dt) {
+//   frc::Pose3d actualPose = GetPose();
+//   frc::Pose3d relativePose = actualPose.RelativeTo(pose);
+//   return (units::math::fabs(relativePose.X()) < 0.01_m && units::math::fabs(relativePose.Y()) < 0.01_m &&
+//           GetSpeed(pose, GetPose(), dt) < 1_m / 1_s);
+// }
+
+// bool wom::vision::Limelight::IsAtSetPoseVision(frc::Pose2d pose, units::second_t dt) {
+//   frc::Pose2d actualPose = GetPose().ToPose2d();
+//   frc::Pose2d relativePose = actualPose.RelativeTo(pose);
+//   return (units::math::fabs(relativePose.X()) < 0.01_m && units::math::fabs(relativePose.Y()) < 0.01_m &&
+//           GetSpeed(pose, GetPose().ToPose2d(), dt) < 1_m / 1_s);
+// }
+
+// bool wom::vision::Limelight::HasTarget() {
+//   return GetTargetingData(LimelightTargetingData::kTv) == 1.0;
+// }
diff --git a/wombat/src/main/include/behaviour/Behaviour.h b/wombat/src/main/include/behaviour/Behaviour.h
index 4f2e357e..e56e6bf7 100644
--- a/wombat/src/main/include/behaviour/Behaviour.h
+++ b/wombat/src/main/include/behaviour/Behaviour.h
@@ -22,13 +22,7 @@
 #include "HasBehaviour.h"
 
 namespace behaviour {
-enum class BehaviourState {
-  INITIALISED,
-  RUNNING,
-  DONE,
-  TIMED_OUT,
-  INTERRUPTED
-};
+enum class BehaviourState { INITIALISED, RUNNING, DONE, TIMED_OUT, INTERRUPTED };
 
 class SequentialBehaviour;
 
@@ -46,8 +40,7 @@ class Behaviour : public std::enable_shared_from_this<Behaviour> {
  public:
   using ptr = std::shared_ptr<Behaviour>;
 
-  Behaviour(std::string name = "<unnamed behaviour>",
-            units::time::second_t period = 20_ms);
+  explicit Behaviour(std::string name = "<unnamed behaviour>", units::time::second_t period = 20_ms);
   ~Behaviour();
 
   /**
@@ -193,16 +186,15 @@ class SequentialBehaviour : public Behaviour {
   std::deque<ptr> _queue;
 };
 
-inline std::shared_ptr<SequentialBehaviour> operator<<(Behaviour::ptr a,
-                                                       Behaviour::ptr b) {
+inline std::shared_ptr<SequentialBehaviour> operator<<(Behaviour::ptr a, Behaviour::ptr b) {
   auto seq = std::make_shared<SequentialBehaviour>();
   seq->Add(a);
   seq->Add(b);
   return seq;
 }
 
-inline std::shared_ptr<SequentialBehaviour> operator<<(
-    std::shared_ptr<SequentialBehaviour> a, Behaviour::ptr b) {
+inline std::shared_ptr<SequentialBehaviour> operator<<(std::shared_ptr<SequentialBehaviour> a,
+                                                       Behaviour::ptr b) {
   a->Add(b);
   return a;
 }
@@ -261,10 +253,8 @@ inline std::shared_ptr<ConcurrentBehaviour> operator&(Behaviour::ptr a,
  * be interrupted as soon as any members of the group are finished (the
  * behaviours are 'raced' against each other).
  */
-inline std::shared_ptr<ConcurrentBehaviour> operator|(Behaviour::ptr a,
-                                                      Behaviour::ptr b) {
-  auto conc =
-      std::make_shared<ConcurrentBehaviour>(ConcurrentBehaviourReducer::ANY);
+inline std::shared_ptr<ConcurrentBehaviour> operator|(Behaviour::ptr a, Behaviour::ptr b) {
+  auto conc = std::make_shared<ConcurrentBehaviour>(ConcurrentBehaviourReducer::ANY);
   conc->Add(a);
   conc->Add(b);
   return conc;
@@ -332,8 +322,7 @@ struct Switch : public Behaviour {
    * @param condition The function yielding true if this is the correct option
    * @param b The behaviour to call if this option is provided.
    */
-  std::shared_ptr<Switch> When(std::function<bool(T&)> condition,
-                               Behaviour::ptr b) {
+  std::shared_ptr<Switch> When(std::function<bool(T&)> condition, Behaviour::ptr b) {
     _options.push_back(std::pair(condition, b));
     Inherit(*b);
     return std::reinterpret_pointer_cast<Switch<T>>(shared_from_this());
@@ -389,8 +378,7 @@ struct Switch : public Behaviour {
 
  private:
   std::function<T()> _fn;
-  wpi::SmallVector<std::pair<std::function<bool(T&)>, Behaviour::ptr>, 4>
-      _options;
+  wpi::SmallVector<std::pair<std::function<bool(T&)>, Behaviour::ptr>, 4> _options;
   Behaviour::ptr _locked = nullptr;
 };
 
@@ -407,10 +395,8 @@ struct Decide : public Switch<std::monostate> {
    * @param condition The function yielding true if this is the correct option
    * @param b The behaviour to call if this option is provided.
    */
-  std::shared_ptr<Decide> When(std::function<bool()> condition,
-                               Behaviour::ptr b) {
-    return std::reinterpret_pointer_cast<Decide>(
-        Switch::When([condition](auto) { return condition(); }, b));
+  std::shared_ptr<Decide> When(std::function<bool()> condition, Behaviour::ptr b) {
+    return std::reinterpret_pointer_cast<Decide>(Switch::When([condition](auto) { return condition(); }, b));
   }
 };
 
diff --git a/wombat/src/main/include/behaviour/HasBehaviour.h b/wombat/src/main/include/behaviour/HasBehaviour.h
index c9b35b2e..bae5ac05 100644
--- a/wombat/src/main/include/behaviour/HasBehaviour.h
+++ b/wombat/src/main/include/behaviour/HasBehaviour.h
@@ -30,8 +30,7 @@ class HasBehaviour {
 
  protected:
   std::shared_ptr<Behaviour> _active_behaviour{nullptr};
-  std::function<std::shared_ptr<Behaviour>(void)> _default_behaviour_producer{
-      nullptr};
+  std::function<std::shared_ptr<Behaviour>(void)> _default_behaviour_producer{nullptr};
 
  private:
   friend class BehaviourScheduler;
diff --git a/wombat/src/main/include/drivetrain/SwerveDrive.h b/wombat/src/main/include/drivetrain/SwerveDrive.h
index bec9fe49..ded5cdd2 100644
--- a/wombat/src/main/include/drivetrain/SwerveDrive.h
+++ b/wombat/src/main/include/drivetrain/SwerveDrive.h
@@ -28,14 +28,10 @@
 #include "utils/Gearbox.h"
 #include "utils/PID.h"
 
-#include <functional>
-#include <limits>
-
 #include <wpi/SymbolExports.h>
 #include <wpi/sendable/Sendable.h>
 #include <wpi/sendable/SendableHelper.h>
 
-
 namespace wom {
 namespace drivetrain {
 
@@ -323,9 +319,8 @@ struct SwerveModuleConfig {
 
 class SwerveModule {
  public:
-  //using angle_pid_conf_t = utils::PIDConfig<units::radian, units::volt>;
-  using velocity_pid_conf_t =
-      utils::PIDConfig<units::meters_per_second, units::volt>;
+  // using angle_pid_conf_t = utils::PIDConfig<units::radian, units::volt>;
+  using velocity_pid_conf_t = utils::PIDConfig<units::meters_per_second, units::volt>;
 
   SwerveModule(std::string path, SwerveModuleConfig config,
                /*angle_pid_conf_t anglePID,*/ velocity_pid_conf_t velocityPID);
@@ -342,8 +337,7 @@ class SwerveModule {
 
   void SetZero(units::second_t dt);
   void SetIdle();
-  void SetPID(units::radian_t angle, units::meters_per_second_t speed,
-              units::second_t dt);
+  void SetPID(units::radian_t angle, units::meters_per_second_t speed, units::second_t dt);
   void SetZero();
   void SetVoltageLimit(units::volt_t driveModuleVoltageLimit);
 
@@ -363,13 +357,14 @@ class SwerveModule {
 
   const SwerveModuleConfig& GetConfig() const;
 
-  //utils::PIDController<units::radians, units::volt> _anglePIDController;
+  // utils::PIDController<units::radians, units::volt> _anglePIDController;
   frc::PIDController _anglePIDController;
 
  private:
   SwerveModuleConfig _config;
   SwerveModuleState _state;
   units::volt_t _driveModuleVoltageLimit = 10_V;
+  units::volt_t _angleModuleVoltageLimit = 6_V;
 
   bool _hasZeroedEncoder = false;
   bool _hasZeroed = false;
@@ -467,8 +462,7 @@ class SwerveDrive : public behaviour::HasBehaviour {
   void SetFieldRelativeVelocity(FieldRelativeSpeeds speeds);
   void SetPose(frc::Pose2d pose);
   bool IsAtSetPose();
-  void SetIndividualTuning(int mod, units::radian_t angle,
-                           units::meters_per_second_t speed);
+  void SetIndividualTuning(int mod, units::radian_t angle, units::meters_per_second_t speed);
   void SetTuning(units::radian_t angle, units::meters_per_second_t speed);
   void SetZero();
   void SetVoltageLimit(units::volt_t driveVoltageLimit);
@@ -514,9 +508,9 @@ class SwerveDrive : public behaviour::HasBehaviour {
 
   /*utils::PIDController<units::radian, units::radians_per_second>
       _anglePIDController;*/
-  PIDController _anglePIDController;
-  utils::PIDController<units::meter, units::meters_per_second> _xPIDController;
-  utils::PIDController<units::meter, units::meters_per_second> _yPIDController;
+  wom::drivetrain::PIDController _anglePIDController;
+  wom::utils::PIDController<units::meter, units::meters_per_second> _xPIDController;
+  wom::utils::PIDController<units::meter, units::meters_per_second> _yPIDController;
 
   std::shared_ptr<nt::NetworkTable> _table;
 
diff --git a/wombat/src/main/include/drivetrain/behaviours/SwerveBehaviours.h b/wombat/src/main/include/drivetrain/behaviours/SwerveBehaviours.h
index b3894ed5..88d8ff12 100644
--- a/wombat/src/main/include/drivetrain/behaviours/SwerveBehaviours.h
+++ b/wombat/src/main/include/drivetrain/behaviours/SwerveBehaviours.h
@@ -37,16 +37,14 @@ class ManualDrivebase : public behaviour::Behaviour {
    * A pointer to the controller that the driver has been allocated (the
    * allocated memory address that stores the "driver controller" object)
    */
-  ManualDrivebase(wom::drivetrain::SwerveDrive* swerveDrivebase,
-                  frc::XboxController* driverController);
+  ManualDrivebase(wom::drivetrain::SwerveDrive* swerveDrivebase, frc::XboxController* driverController);
 
   void OnTick(units::second_t deltaTime) override;
   /**
    * @brief This function handles all of the logic behind the tangent function,
    * to be able to calculate an angle between 0 andd 360 degrees, inclusively
    */
-  void CalculateRequestedAngle(double joystickX, double joystickY,
-                               units::degree_t defaultAngle);
+  void CalculateRequestedAngle(double joystickX, double joystickY, units::degree_t defaultAngle);
   void OnStart() override;
   void ResetMode();
 
@@ -184,8 +182,7 @@ class TempSimSwerveDrive {
 
 class AutoSwerveDrive {
  public:
-  AutoSwerveDrive(wom::drivetrain::SwerveDrive* swerve, frc::Timer* timer,
-                  frc::Field2d* field);
+  AutoSwerveDrive(wom::drivetrain::SwerveDrive* swerve, frc::Timer* timer, frc::Field2d* field);
 
   void OnUpdate();
 
diff --git a/wombat/src/main/include/utils/PID.h b/wombat/src/main/include/utils/PID.h
index 2ce378ec..adf34f94 100644
--- a/wombat/src/main/include/utils/PID.h
+++ b/wombat/src/main/include/utils/PID.h
@@ -24,19 +24,15 @@ struct PIDConfig {
   using in_t = units::unit_t<IN>;
 
   using kp_t = units::unit_t<units::compound_unit<OUT, units::inverse<IN>>>;
-  using ki_t =
-      units::unit_t<units::compound_unit<OUT, units::inverse<IN>,
-                                         units::inverse<units::second>>>;
-  using kd_t = units::unit_t<
-      units::compound_unit<OUT, units::inverse<IN>, units::second>>;
+  using ki_t = units::unit_t<units::compound_unit<OUT, units::inverse<IN>, units::inverse<units::second>>>;
+  using kd_t = units::unit_t<units::compound_unit<OUT, units::inverse<IN>, units::second>>;
 
   using error_t = units::unit_t<IN>;
-  using deriv_t =
-      units::unit_t<units::compound_unit<IN, units::inverse<units::second>>>;
+  using deriv_t = units::unit_t<units::compound_unit<IN, units::inverse<units::second>>>;
 
-  PIDConfig(std::string path, kp_t kp = kp_t{0}, ki_t ki = ki_t{0},
-            kd_t kd = kd_t{0}, error_t stableThresh = error_t{-1},
-            deriv_t stableDerivThresh = deriv_t{-1}, in_t izone = in_t{-1})
+  PIDConfig(std::string path, kp_t kp = kp_t{0}, ki_t ki = ki_t{0}, kd_t kd = kd_t{0},
+            error_t stableThresh = error_t{-1}, deriv_t stableDerivThresh = deriv_t{-1},
+            in_t izone = in_t{-1})
       : path(path),
         kp(kp),
         ki(ki),
@@ -64,23 +60,14 @@ struct PIDConfig {
  public:
   void RegisterNT() {
     auto table = nt::NetworkTableInstance::GetDefault().GetTable(path);
+    _nt_bindings.emplace_back(std::make_shared<NTBoundUnit<typename kp_t::unit_type>>(table, "kP", kp));
+    _nt_bindings.emplace_back(std::make_shared<NTBoundUnit<typename ki_t::unit_type>>(table, "kI", ki));
+    _nt_bindings.emplace_back(std::make_shared<NTBoundUnit<typename kd_t::unit_type>>(table, "kD", kd));
     _nt_bindings.emplace_back(
-        std::make_shared<NTBoundUnit<typename kp_t::unit_type>>(table, "kP",
-                                                                kp));
-    _nt_bindings.emplace_back(
-        std::make_shared<NTBoundUnit<typename ki_t::unit_type>>(table, "kI",
-                                                                ki));
-    _nt_bindings.emplace_back(
-        std::make_shared<NTBoundUnit<typename kd_t::unit_type>>(table, "kD",
-                                                                kd));
-    _nt_bindings.emplace_back(
-        std::make_shared<NTBoundUnit<typename error_t::unit_type>>(
-            table, "stableThresh", stableThresh));
-    _nt_bindings.emplace_back(
-        std::make_shared<NTBoundUnit<typename deriv_t::unit_type>>(
-            table, "stableThreshVelocity", stableDerivThresh));
-    _nt_bindings.emplace_back(
-        std::make_shared<NTBoundUnit<IN>>(table, "izone", izone));
+        std::make_shared<NTBoundUnit<typename error_t::unit_type>>(table, "stableThresh", stableThresh));
+    _nt_bindings.emplace_back(std::make_shared<NTBoundUnit<typename deriv_t::unit_type>>(
+        table, "stableThreshVelocity", stableDerivThresh));
+    _nt_bindings.emplace_back(std::make_shared<NTBoundUnit<IN>>(table, "izone", izone));
   }
 };
 
@@ -94,19 +81,15 @@ class PIDController {
 
   config_t config;
 
-  PIDController(std::string path, config_t initialGains,
-                in_t setpoint = in_t{0})
+  PIDController(std::string path, config_t initialGains, in_t setpoint = in_t{0})
       : config(initialGains),
         _setpoint(setpoint),
-        _posFilter(
-            frc::LinearFilter<typename config_t::error_t>::MovingAverage(20)),
-        _velFilter(
-            frc::LinearFilter<typename config_t::deriv_t>::MovingAverage(20)),
+        _posFilter(frc::LinearFilter<typename config_t::error_t>::MovingAverage(20)),
+        _velFilter(frc::LinearFilter<typename config_t::deriv_t>::MovingAverage(20)),
         _table(nt::NetworkTableInstance::GetDefault().GetTable(path)) {}
 
   void SetSetpoint(in_t setpoint) {
-    if (std::abs(setpoint.value() - _setpoint.value()) >
-        std::abs(0.1 * _setpoint.value())) {
+    if (std::abs(setpoint.value() - _setpoint.value()) > std::abs(0.1 * _setpoint.value())) {
       _iterations = 0;
     }
     _setpoint = setpoint;
@@ -121,13 +104,17 @@ class PIDController {
   void Reset() { _integralSum = sum_t{0}; }
 
   out_t Calculate(in_t pv, units::second_t dt, out_t feedforward = out_t{0}) {
-    pv = units::math::fabs(pv);
+    // pv = units::math::fabs(pv);
+    bool is_negative;
+    if (pv.value() < 0) {
+      is_negative = true;
+      pv = units::math::fabs(pv);
+    }
     auto error = do_wrap(_setpoint - pv);
     error = units::math::fabs(error);
     _integralSum += error * dt;
     _integralSum = units::math::fabs(_integralSum);
-    if (config.izone.value() > 0 &&
-        (error > config.izone || error < -config.izone))
+    if (config.izone.value() > 0 && (error > config.izone || error < -config.izone))
       _integralSum = sum_t{0};
 
     typename config_t::deriv_t deriv{0};
@@ -138,19 +125,22 @@ class PIDController {
     _stablePos = _posFilter.Calculate(error);
     _stableVel = _velFilter.Calculate(deriv);
 
-    auto out = config.kp * error + config.ki * _integralSum +
-               config.kd * deriv + feedforward;
+    auto out = config.kp * error + config.ki * _integralSum + config.kd * deriv + feedforward;
 
     _last_pv = pv;
     _last_error = error;
     _iterations++;
+    if (is_negative) {
+      out *= -1;
+    }
+
+    _table->GetEntry("error").SetDouble(out.value());
+
     return out;
   }
 
-  bool IsStable(
-      std::optional<typename config_t::error_t> stableThreshOverride = {},
-      std::optional<typename config_t::deriv_t> velocityThreshOverride = {})
-      const {
+  bool IsStable(std::optional<typename config_t::error_t> stableThreshOverride = {},
+                std::optional<typename config_t::deriv_t> velocityThreshOverride = {}) const {
     auto stableThresh = config.stableThresh;
     auto stableDerivThresh = config.stableDerivThresh;
 
@@ -159,10 +149,8 @@ class PIDController {
     if (velocityThreshOverride.has_value())
       stableDerivThresh = velocityThreshOverride.value();
 
-    return _iterations > 20 &&
-           std::abs(_stablePos.value()) <= std::abs(stableThresh.value()) &&
-           (stableDerivThresh.value() < 0 ||
-            std::abs(_stableVel.value()) <= stableDerivThresh.value());
+    return _iterations > 20 && std::abs(_stablePos.value()) <= std::abs(stableThresh.value()) &&
+           (stableDerivThresh.value() < 0 || std::abs(_stableVel.value()) <= stableDerivThresh.value());
   }
 
  private:
diff --git a/wombat/src/main/include/utils/Util.h b/wombat/src/main/include/utils/Util.h
index 665c85ec..45e4d495 100644
--- a/wombat/src/main/include/utils/Util.h
+++ b/wombat/src/main/include/utils/Util.h
@@ -28,39 +28,30 @@ units::second_t now();
 
 class NTBound {
  public:
-  NTBound(std::shared_ptr<nt::NetworkTable> table, std::string name,
-          const nt::Value& value,
+  NTBound(std::shared_ptr<nt::NetworkTable> table, std::string name, const nt::Value& value,
           std::function<void(const nt::Value&)> onUpdateFn)
-      : _table(table),
-        _entry(table->GetEntry(name)),
-        _onUpdate(onUpdateFn),
-        _name(name) {
+      : _table(table), _entry(table->GetEntry(name)), _onUpdate(onUpdateFn), _name(name) {
     _entry.SetValue(value);
     // _listener = table->AddListener(name, , ([=](const nt::EntryNotification
     // &evt) {
     //   this->_onUpdate(evt.value);
     // }, NT_NOTIFY_UPDATE);
-    _listener = table->AddListener(
-        name, nt::EventFlags::kValueAll,
-        ([this](nt::NetworkTable* table, std::string_view key,
-                const nt::Event& event) {
-          std::cout << "NT UPDATE" << std::endl;
-          this->_onUpdate(event.GetValueEventData()->value);
-        }));
+    _listener =
+        table->AddListener(name, nt::EventFlags::kValueAll,
+                           ([this](nt::NetworkTable* table, std::string_view key, const nt::Event& event) {
+                             std::cout << "NT UPDATE" << std::endl;
+                             this->_onUpdate(event.GetValueEventData()->value);
+                           }));
   }
 
   NTBound(const NTBound& other)
-      : _table(other._table),
-        _entry(other._entry),
-        _onUpdate(other._onUpdate),
-        _name(other._name) {
-    _listener = _table->AddListener(
-        _name, nt::EventFlags::kValueAll,
-        ([this](nt::NetworkTable* table, std::string_view key,
-                const nt::Event& event) {
-          std::cout << "NT UPDATE" << std::endl;
-          this->_onUpdate(event.GetValueEventData()->value);
-        }));
+      : _table(other._table), _entry(other._entry), _onUpdate(other._onUpdate), _name(other._name) {
+    _listener =
+        _table->AddListener(_name, nt::EventFlags::kValueAll,
+                            ([this](nt::NetworkTable* table, std::string_view key, const nt::Event& event) {
+                              std::cout << "NT UPDATE" << std::endl;
+                              this->_onUpdate(event.GetValueEventData()->value);
+                            }));
   }
 
   ~NTBound() { _table->RemoveListener(_listener); }
@@ -75,29 +66,24 @@ class NTBound {
 
 class NTBoundDouble : public NTBound {
  public:
-  NTBoundDouble(std::shared_ptr<nt::NetworkTable> table, std::string name,
-                double& val)
+  NTBoundDouble(std::shared_ptr<nt::NetworkTable> table, std::string name, double& val)
       : NTBound(table, name, nt::Value::MakeDouble(val),
                 [&val](const nt::Value& v) { val = v.GetDouble(); }) {}
 };
 
+// sussy baka should be constrained to a unit type
 template <typename T>
 class NTBoundUnit : public NTBound {
  public:
-  NTBoundUnit(std::shared_ptr<nt::NetworkTable> table, std::string name,
-              units::unit_t<T>& val)
+  NTBoundUnit(std::shared_ptr<nt::NetworkTable> table, std::string name, units::unit_t<T>& val)
       : NTBound(table, name, nt::Value::MakeDouble(val.value()),
-                [&val](const nt::Value& v) {
-                  val = units::unit_t<T>{v.GetDouble()};
-                }) {}
+                [&val](const nt::Value& v) { val = units::unit_t<T>{v.GetDouble()}; }) {}
 };
 
 void WritePose2NT(std::shared_ptr<nt::NetworkTable> table, frc::Pose2d pose);
 void WritePose3NT(std::shared_ptr<nt::NetworkTable> table, frc::Pose3d pose);
-void WriteTrajectory(std::shared_ptr<nt::NetworkTable> table,
-                     frc::Trajectory trajectory);
-void WriteTrajectoryState(std::shared_ptr<nt::NetworkTable> table,
-                          frc::Trajectory::State state);
+void WriteTrajectory(std::shared_ptr<nt::NetworkTable> table, frc::Trajectory trajectory);
+void WriteTrajectoryState(std::shared_ptr<nt::NetworkTable> table, frc::Trajectory::State state);
 
 frc::Pose2d TrajectoryStateToPose2d(frc::Trajectory::State state);
 
diff --git a/wombat/src/main/include/vision/Limelight.h b/wombat/src/main/include/vision/Limelight.h
index f909c703..2f06bf00 100644
--- a/wombat/src/main/include/vision/Limelight.h
+++ b/wombat/src/main/include/vision/Limelight.h
@@ -1,126 +1,114 @@
-// Copyright (c) 2023-2024 CurtinFRC
-// Open Source Software, you can modify it according to the terms
-// of the MIT License at the root of this project
-
-#pragma once
-
-#include <frc/geometry/Pose2d.h>
-#include <frc/geometry/Pose3d.h>
-#include <frc/geometry/Rotation3d.h>
-#include <networktables/NetworkTable.h>
-#include <networktables/NetworkTableEntry.h>
-#include <networktables/NetworkTableInstance.h>
-#include <networktables/NetworkTableValue.h>
-#include <units/angle.h>
-#include <units/length.h>
-#include <units/math.h>
-#include <units/time.h>
-#include <units/velocity.h>
-#include <wpi/json.h>
-
-#include <memory>
-#include <string>
-#include <utility>
-#include <vector>
-
-namespace wom {
-namespace vision {
-
-enum class LimelightLEDMode {
-  kPipelineDefault = 0,
-  kForceOff = 1,
-  kForceBlink = 2,
-  kForceOn = 3
-};
-
-enum class LimelightCamMode { kVisionProcessor = 0, kDriverCamera = 1 };
-
-enum class LimelightStreamMode {
-  kStandard = 0,
-  kPiPMain = 1,
-  kPiPSecondary = 2
-};
-
-enum class LimelightSnapshotMode { kReset = 0, kSingle = 1 };
-
-enum class LimelightPipeline {
-  kPipeline0 = 0,
-  kPipeline1 = 1,
-  kPipeline2 = 2,
-  kPipeline3 = 3,
-  kPipeline4 = 4,
-  kPipeline5 = 5,
-  kPipeline6 = 6,
-  kPipeline7 = 7,
-  kPipeline8 = 8,
-  kPipeline9 = 9
-};
-
-enum class LimelightTargetingData {
-  kTv = 0,
-  kTx = 1,
-  kTy = 2,
-  kTa = 3,
-  kTl = 4,
-  kCl = 5,
-  kTshort = 6,
-  kTlong = 7,
-  kThor = 8,
-  kTvert = 9,
-  kGetpipe = 10,
-  kJson = 11,
-  kTclass = 12,
-  kTc = 13,
-  kTid = 14
-};
-
-enum class LimelightAprilTagData {
-  kBotpose = 0,
-  kBotpose_wpiblue = 1,
-  kBotpose_wpired = 2,
-  kCamerapose_targetspace = 3,
-  kTargetpose_cameraspace = 4,
-  kTargetpose_robotspace = 5,
-  kBotpose_targetspace = 6,
-  kCamerapose_robotspace = 7,
-};
-
-class Limelight {
- public:
-  explicit Limelight(std::string limelightName);
-
-  std::string GetName();
-
-  std::shared_ptr<nt::NetworkTable> table =
-      nt::NetworkTableInstance::GetDefault().GetTable("limelight");
-
-  std::pair<double, double> GetOffset();
-
-  std::vector<double> GetAprilTagData(LimelightAprilTagData dataType);
-  double GetTargetingData(LimelightTargetingData dataType,
-                          double defaultValue = 0.0);
-  void SetLEDMode(LimelightLEDMode mode);
-  void SetCamMode(LimelightCamMode mode);
-  void SetPipeline(LimelightPipeline pipeline);
-  void SetStreamMode(LimelightStreamMode mode);
-  void SetSnapshotMode(LimelightSnapshotMode mode);
-  void SetCrop(std::array<double, 4> crop);
-
-  void OnUpdate(units::time::second_t dt);
-  void OnStart();
-
-  bool IsAtSetPoseVision(frc::Pose3d pose, units::second_t dt);
-  bool IsAtSetPoseVision(frc::Pose2d pose, units::second_t dt);
-
-  units::meters_per_second_t GetSpeed(frc::Pose3d pose1, frc::Pose3d pose2,
-                                      units::second_t dt);
-
-  frc::Pose3d GetPose();
-
-  bool HasTarget();
-
- private:
-  std::string _limelightName;
-};
-}  // namespace vision
-}  // namespace wom
+// // Copyright (c) 2023-2024 CurtinFRC
+// // Open Source Software, you can modify it according to the terms
+// // of the MIT License at the root of this project
+
+// #pragma once
+
+// #include <frc/geometry/Pose2d.h>
+// #include <frc/geometry/Pose3d.h>
+// #include <frc/geometry/Rotation3d.h>
+// #include <networktables/NetworkTable.h>
+// #include <networktables/NetworkTableEntry.h>
+// #include <networktables/NetworkTableInstance.h>
+// #include <networktables/NetworkTableValue.h>
+// #include <units/angle.h>
+// #include <units/length.h>
+// #include <units/math.h>
+// #include <units/time.h>
+// #include <units/velocity.h>
+// #include <wpi/json.h>
+
+// #include <memory>
+// #include <string>
+// #include <utility>
+// #include <vector>
+
+// namespace wom {
+// namespace vision {
+
+// enum class LimelightLEDMode { kPipelineDefault = 0, kForceOff = 1, kForceBlink = 2, kForceOn = 3 };
+
+// enum class LimelightCamMode { kVisionProcessor = 0, kDriverCamera = 1 };
+
+// enum class LimelightStreamMode { kStandard = 0, kPiPMain = 1, kPiPSecondary = 2 };
+
+// enum class LimelightSnapshotMode { kReset = 0, kSingle = 1 };
+
+// enum class LimelightPipeline {
+//   kPipeline0 = 0,
+//   kPipeline1 = 1,
+//   kPipeline2 = 2,
+//   kPipeline3 = 3,
+//   kPipeline4 = 4,
+//   kPipeline5 = 5,
+//   kPipeline6 = 6,
+//   kPipeline7 = 7,
+//   kPipeline8 = 8,
+//   kPipeline9 = 9
+// };
+
+// enum class LimelightTargetingData {
+//   kTv = 0,
+//   kTx = 1,
+//   kTy = 2,
+//   kTa = 3,
+//   kTl = 4,
+//   kCl = 5,
+//   kTshort = 6,
+//   kTlong = 7,
+//   kThor = 8,
+//   kTvert = 9,
+//   kGetpipe = 10,
+//   kJson = 11,
+//   kTclass = 12,
+//   kTc = 13,
+//   kTid = 14
+// };
+
+// enum class LimelightAprilTagData {
+//   kBotpose = 0,
+//   kBotpose_wpiblue = 1,
+//   kBotpose_wpired = 2,
+//   kCamerapose_targetspace = 3,
+//   kTargetpose_cameraspace = 4,
+//   kTargetpose_robotspace = 5,
+//   kBotpose_targetspace = 6,
+//   kCamerapose_robotspace = 7,
+// };
+
+// class Limelight {
+//  public:
+//   explicit Limelight(std::string limelightName);
+
+//   std::string GetName();
+
+//   std::shared_ptr<nt::NetworkTable> table;
+
+//   std::pair<double, double> GetOffset();
+
+//   std::vector<double> GetAprilTagData(LimelightAprilTagData dataType);
+//   double GetTargetingData(LimelightTargetingData dataType, double defaultValue = 0.0);
+//   void SetLEDMode(LimelightLEDMode mode);
+//   void SetCamMode(LimelightCamMode mode);
+//   void SetPipeline(LimelightPipeline pipeline);
+//   void SetStreamMode(LimelightStreamMode mode);
+//   void SetSnapshotMode(LimelightSnapshotMode mode);
+//   void SetCrop(std::array<double, 4> crop);
+
+//   void OnUpdate(units::time::second_t dt);
+//   void OnStart();
+
+//   bool IsAtSetPoseVision(frc::Pose3d pose, units::second_t dt);
+//   bool IsAtSetPoseVision(frc::Pose2d pose, units::second_t dt);
+
+//   units::meters_per_second_t GetSpeed(frc::Pose3d pose1, frc::Pose3d pose2, units::second_t dt);
+
+//   frc::Pose3d GetPose();
+
+//   bool HasTarget();
+
+//  private:
+//   std::string _limelightName;
+// };
+// }  // namespace vision
+// }  // namespace wom

From b4581901e4e70e49076b3bef1060a7b9c06a2455 Mon Sep 17 00:00:00 2001
From: SoulOfTheAssassin <oli.liu798@gmail.com>
Date: Sat, 2 Mar 2024 10:43:06 +0800
Subject: [PATCH 178/207] Fixed some more errors

---
 src/main/cpp/Behaviours/ClimberBehaviour.cpp  |  12 +-
 src/main/cpp/Climber.cpp                      |   9 ++
 src/main/cpp/Mag.cpp                          | 124 +++++++++---------
 src/main/cpp/Robot.cpp                        |  51 +++----
 src/main/cpp/behaviours/MagBehaviour.cpp      | 102 +++++++-------
 .../include/Behaviours/ClimberBehaviour.h     |   2 +-
 src/main/include/Climber.h                    |   3 +-
 src/main/include/Mag.h                        |  76 +++++------
 src/main/include/Robot.h                      |   2 +-
 src/main/include/RobotMap.h                   |  23 ++--
 src/main/include/behaviours/MagBehaviour.h    |  60 ++++-----
 11 files changed, 236 insertions(+), 228 deletions(-)

diff --git a/src/main/cpp/Behaviours/ClimberBehaviour.cpp b/src/main/cpp/Behaviours/ClimberBehaviour.cpp
index 464135fa..7b9ce4c4 100644
--- a/src/main/cpp/Behaviours/ClimberBehaviour.cpp
+++ b/src/main/cpp/Behaviours/ClimberBehaviour.cpp
@@ -10,11 +10,17 @@ ClimberManualControl::ClimberManualControl(Climber* climber, frc::XboxController
 }
 
 void ClimberManualControl::OnTick(units::second_t dt) {
-  if (_codriver->GetRightStickButtonPressed()) {
+  if (_codriver->GetPOVButton() == 270) {
     if (_climber->GetState() == ClimberState::kIdle) {
-      _climber->SetState(ClimberState::kRaw);
+      _climber->SetState(ClimberState::kReady);
     } else {
      _climber->SetState(ClimberState::kIdle);
     }
+  } else if (_codriver->GetPOVButton() == 90) {
+    if (_climber->GetState() == ClimberState::kReady) {
+      _climber->SetState(ClimberState::kClimb);
+    } else {
+      _climber->SetState(ClimberState::kIdle);
+    }
   }
-}
+}
\ No newline at end of file
diff --git a/src/main/cpp/Climber.cpp b/src/main/cpp/Climber.cpp
index 5a21a771..2e2a95e6 100644
--- a/src/main/cpp/Climber.cpp
+++ b/src/main/cpp/Climber.cpp
@@ -22,6 +22,11 @@ void Climber::OnUpdate(units::second_t dt) {
       _stringStateName = "Hang";
       _setVoltage = -8_V;
     } break;
+
+    case ClimberState::kReady: {
+      _stringStateName = "Raw";
+      _setVoltage = 0_V;
+    } break;
     
     default:
       std::cout << "Error climber in invalid state" << std::endl;
@@ -40,3 +45,7 @@ void Climber::SetState(ClimberState state) {
 void Climber::SetRaw(units::volt_t voltage) {
   _rawVoltage = voltage;
 }
+
+ClimberState Climber::GetState() {
+  return _state;
+}
diff --git a/src/main/cpp/Mag.cpp b/src/main/cpp/Mag.cpp
index 1bde78f0..6f8fd915 100644
--- a/src/main/cpp/Mag.cpp
+++ b/src/main/cpp/Mag.cpp
@@ -1,72 +1,72 @@
-// Copyright (c) 2023-2024 CurtinFRC
-// Open Source Software, you can modify it according to the terms
-// of the MIT License at the root of this project
+// // Copyright (c) 2023-2024 CurtinFRC
+// // Open Source Software, you can modify it according to the terms
+// // of the MIT License at the root of this project
 
-#include "Mag.h"
+// #include "Mag.h"
 
-Mag::Mag(MagConfig config) : _config(config) {}
+// Mag::Mag(MagConfig config) : _config(config) {}
 
-void Mag::OnUpdate(units::second_t dt) {
-  switch (_state) {
-    case MagState::kIdle: {
-      if (_config.intakeSensor->Get()) {
-        SetState(MagState::kHold);
-      } else if (_config.intakeSensor->Get()) {
-        SetState(MagState::kHold);
-      }
-      _setVoltage = 0_V;
-      _stringStateName = "Idle";
-    } break;
+// void Mag::OnUpdate(units::second_t dt) {
+//   switch (_state) {
+//     case MagState::kIdle: {
+//       if (_config.intakeSensor->Get()) {
+//         SetState(MagState::kHold);
+//       } else if (_config.intakeSensor->Get()) {
+//         SetState(MagState::kHold);
+//       }
+//       _setVoltage = 0_V;
+//       _stringStateName = "Idle";
+//     } break;
 
-    case MagState::kHold: {
-      if (_config.magSensor->Get() == 0) {
-        SetState(MagState::kIdle);
-      }
-      _setVoltage = 0_V;
-      _stringStateName = "Hold";
-    } break;
+//     case MagState::kHold: {
+//       if (_config.magSensor->Get() == 0) {
+//         SetState(MagState::kIdle);
+//       }
+//       _setVoltage = 0_V;
+//       _stringStateName = "Hold";
+//     } break;
 
-    case MagState::kEject: {
-      if (_config.magSensor->Get() == 0 && _config.intakeSensor->Get() == 0) {
-        SetState(MagState::kIdle);
-      }
-      _setVoltage = -5_V;
-      _stringStateName = "Eject";
-    } break;
+//     case MagState::kEject: {
+//       if (_config.magSensor->Get() == 0 && _config.intakeSensor->Get() == 0) {
+//         SetState(MagState::kIdle);
+//       }
+//       _setVoltage = -5_V;
+//       _stringStateName = "Eject";
+//     } break;
 
-    case MagState::kRaw:
-      _setVoltage = _rawVoltage;
-      _stringStateName = "Raw";
-      break;
+//     case MagState::kRaw:
+//       _setVoltage = _rawVoltage;
+//       _stringStateName = "Raw";
+//       break;
 
-    case MagState::kPass: {
-      if (_config.shooterSensor->Get()) {
-        SetState(MagState::kIdle);
-      } else {
-        _setVoltage = 5_V;
-        _stringStateName = "Pass";
-      }
-    } break;
+//     case MagState::kPass: {
+//       if (_config.shooterSensor->Get()) {
+//         SetState(MagState::kIdle);
+//       } else {
+//         _setVoltage = 5_V;
+//         _stringStateName = "Pass";
+//       }
+//     } break;
 
-    default:
-      std::cout << "Error magazine in invalid state" << std::endl;
-      break;
-  }
-  _config.magGearbox.motorController->SetVoltage(_setVoltage);
-  _table->GetEntry("State: ").SetString(_stringStateName);
-  _table->GetEntry("Motor Voltage: ").SetDouble(_setVoltage.value());
-  _table->GetEntry("Intake Sensor: ").SetDouble(_config.intakeSensor->Get());
-  _table->GetEntry("Shooter Sensor: ").SetDouble(_config.shooterSensor->Get());
-  _table->GetEntry("Magazine Sensor: ").SetDouble(_config.magSensor->Get());
-}
+//     default:
+//       std::cout << "Error magazine in invalid state" << std::endl;
+//       break;
+//   }
+//   _config.magGearbox.motorController->SetVoltage(_setVoltage);
+//   _table->GetEntry("State: ").SetString(_stringStateName);
+//   _table->GetEntry("Motor Voltage: ").SetDouble(_setVoltage.value());
+//   _table->GetEntry("Intake Sensor: ").SetDouble(_config.intakeSensor->Get());
+//   _table->GetEntry("Shooter Sensor: ").SetDouble(_config.shooterSensor->Get());
+//   _table->GetEntry("Magazine Sensor: ").SetDouble(_config.magSensor->Get());
+// }
 
-void Mag::SetState(MagState state) {
-  _state = state;
-}
+// void Mag::SetState(MagState state) {
+//   _state = state;
+// }
 
-void Mag::SetRaw(units::volt_t voltage) {
-  _rawVoltage = voltage;
-}
-MagState Mag::GetState() {
-  return _state;
-}
+// void Mag::SetRaw(units::volt_t voltage) {
+//   _rawVoltage = voltage;
+// }
+// MagState Mag::GetState() {
+//   return _state;
+// }
diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp
index c37177fa..418a50cb 100644
--- a/src/main/cpp/Robot.cpp
+++ b/src/main/cpp/Robot.cpp
@@ -18,25 +18,27 @@
 
 static units::second_t lastPeriodic;
 
-void Robot::RobotInit() {
-  lastPeriodic = wom::now();
+// void Robot::RobotInit() {
+//   lastPeriodic = wom::now();
 
-  mag = new Mag(robotmap.MagSystem.config);
-  wom::BehaviourScheduler::GetInstance()->Register(mag);
-  mag->SetDefaultBehaviour(
-      [this]() { return wom::make<MagManualControl>(mag, &robotmap.Controllers.coDriver); });
-}
+//   mag = new Mag(robotmap.MagSystem.config);
+//   wom::BehaviourScheduler::GetInstance()->Register(mag);
+//   mag->SetDefaultBehaviour(
+//       [this]() { return wom::make<MagManualControl>(mag, &robotmap.Controllers.coDriver); });
+// }
 
-void Robot::RobotPeriodic() {
-  auto dt = wom::now() - lastPeriodic;
-  lastPeriodic = wom::now(); 
+ void Robot::RobotInit() {};
 
-  loop.Poll();
-  wom::BehaviourScheduler::GetInstance()->Tick();
+ void Robot::RobotPeriodic() {
+   auto dt = wom::now() - lastPeriodic;
+   lastPeriodic = wom::now(); 
 
-  _swerveDrive->OnUpdate(dt);
-  mag->OnUpdate(dt);
-}
+   loop.Poll();
+   wom::BehaviourScheduler::GetInstance()->Tick();
+
+   _swerveDrive->OnUpdate(dt);
+//   mag->OnUpdate(dt);
+ }
 
 void Robot::TeleopInit() {
   loop.Clear();
@@ -75,16 +77,6 @@ void Robot::TeleopInit() {
   // m_driveSim = wom::TempSimSwerveDrive();
 }
 
-void Robot::RobotPeriodic() {
-  auto dt = wom::now() - lastPeriodic;
-  lastPeriodic = wom::now();
-
-  loop.Poll();
-  wom::BehaviourScheduler::GetInstance()->Tick();
-
-  _swerveDrive->OnUpdate(dt);
-}
-
 void Robot::AutonomousInit() {
   // m_driveSim->SetPath(m_path_chooser.GetSelected());
 
@@ -92,14 +84,7 @@ void Robot::AutonomousInit() {
   sched->InterruptAll();
   // _swerveDrive->OnStart();
 }
-void Robot::AutonomousPeriodic() {
-  // m_driveSim->OnUpdate();
-}
-
-void Robot::TeleopInit() {
-  // _swerveDrive->OnStart();
-  // sched->InterruptAll();
-}
+void Robot::AutonomousPeriodic() {}
 void Robot::TeleopPeriodic() {}
 
 void Robot::DisabledInit() {}
diff --git a/src/main/cpp/behaviours/MagBehaviour.cpp b/src/main/cpp/behaviours/MagBehaviour.cpp
index f07dd1f5..07f60df6 100644
--- a/src/main/cpp/behaviours/MagBehaviour.cpp
+++ b/src/main/cpp/behaviours/MagBehaviour.cpp
@@ -1,51 +1,51 @@
-// Copyright (c) 2023-2024 CurtinFRC
-// Open Source Software, you can modify it according to the terms
-// of the MIT License at the root of this project
-
-#include "behaviours/MagBehaviour.h"
-
-MagManualControl::MagManualControl(Mag* mag, frc::XboxController* codriver) : _mag(mag), _codriver(codriver) {
-  Controls(mag);
-}
-
-void MagManualControl::OnTick(units::second_t dt) {
-  if (_codriver->GetXButtonPressed()) {
-    if (_rawControl == true) {
-      _rawControl = false;
-    } else {
-      _rawControl = true;
-    }
-  }
-
-  if (_rawControl) {
-    // Manual control, right bumper for manual override.
-    if (_codriver->GetLeftBumper()) {
-      _mag->SetRaw(5_V);
-      _mag->SetState(MagState::kRaw);
-    } else if (_codriver->GetRightBumper()) {
-      _mag->SetRaw(-5_V);
-      _mag->SetState(MagState::kRaw);
-
-    } else {
-      _mag->SetRaw(0_V);
-    }
-
-  } else {
-    _mag->SetState(MagState::kIdle);
-    if (_codriver->GetLeftBumper()) {
-      _mag->SetState(MagState::kPass);
-    }
-  }
-}
-
-MagAutoPass::MagAutoPass(Mag* mag) {}
-
-void MagAutoPass::OnTick(units::second_t dt) {
-  _mag->SetState(MagState::kPass);
-}
-
-MagAutoHold::MagAutoHold(Mag* mag) {}
-
-void MagAutoHold::OnTick(units::second_t dt) {
-  _mag->SetState(MagState::kHold);
-}
+// // Copyright (c) 2023-2024 CurtinFRC
+// // Open Source Software, you can modify it according to the terms
+// // of the MIT License at the root of this project
+
+// #include "behaviours/MagBehaviour.h"
+
+// MagManualControl::MagManualControl(Mag* mag, frc::XboxController* codriver) : _mag(mag), _codriver(codriver) {
+//   Controls(mag);
+// }
+
+// void MagManualControl::OnTick(units::second_t dt) {
+//   if (_codriver->GetXButtonPressed()) {
+//     if (_rawControl == true) {
+//       _rawControl = false;
+//     } else {
+//       _rawControl = true;
+//     }
+//   }
+
+//   if (_rawControl) {
+//     // Manual control, right bumper for manual override.
+//     if (_codriver->GetLeftBumper()) {
+//       _mag->SetRaw(5_V);
+//       _mag->SetState(MagState::kRaw);
+//     } else if (_codriver->GetRightBumper()) {
+//       _mag->SetRaw(-5_V);
+//       _mag->SetState(MagState::kRaw);
+
+//     } else {
+//       _mag->SetRaw(0_V);
+//     }
+
+//   } else {
+//     _mag->SetState(MagState::kIdle);
+//     if (_codriver->GetLeftBumper()) {
+//       _mag->SetState(MagState::kPass);
+//     }
+//   }
+// }
+
+// MagAutoPass::MagAutoPass(Mag* mag) {}
+
+// void MagAutoPass::OnTick(units::second_t dt) {
+//   _mag->SetState(MagState::kPass);
+// }
+
+// MagAutoHold::MagAutoHold(Mag* mag) {}
+
+// void MagAutoHold::OnTick(units::second_t dt) {
+//   _mag->SetState(MagState::kHold);
+// }
diff --git a/src/main/include/Behaviours/ClimberBehaviour.h b/src/main/include/Behaviours/ClimberBehaviour.h
index 41ed6c98..4555970f 100644
--- a/src/main/include/Behaviours/ClimberBehaviour.h
+++ b/src/main/include/Behaviours/ClimberBehaviour.h
@@ -12,7 +12,7 @@
 class ClimberManualControl : public behaviour::Behaviour {
  public:
   explicit ClimberManualControl(Climber* climber, frc::XboxController* codriver);
-  void OnTick(units::second_t dt) override;
+void OnTick(units::second_t dt) override;
 
  private:
   Climber* _climber;
diff --git a/src/main/include/Climber.h b/src/main/include/Climber.h
index 869f8046..b3ba1409 100644
--- a/src/main/include/Climber.h
+++ b/src/main/include/Climber.h
@@ -13,7 +13,7 @@ struct ClimberConfig {
   wom::Gearbox climberGearbox;
 };
 
-enum class ClimberState { kIdle, kRaw };
+enum class ClimberState { kIdle, kReady, kClimb, kHang };
 
 class Climber : public behaviour::HasBehaviour {
  public:
@@ -21,6 +21,7 @@ class Climber : public behaviour::HasBehaviour {
 
   void OnUpdate(units::second_t dt);
   void SetState(ClimberState state);
+  ClimberState GetState();
   void SetRaw(units::volt_t voltage);
 
  private:
diff --git a/src/main/include/Mag.h b/src/main/include/Mag.h
index 83d4ecfb..7c670ac5 100644
--- a/src/main/include/Mag.h
+++ b/src/main/include/Mag.h
@@ -1,38 +1,38 @@
-// Copyright (c) 2023-2024 CurtinFRC
-// Open Source Software, you can modify it according to the terms
-// of the MIT License at the root of this project
-
-#pragma once
-#include <frc/DigitalInput.h>
-
-#include <memory>
-#include <string>
-
-#include "Wombat.h"
-
-struct MagConfig {
-  wom::Gearbox magGearbox;
-  frc::DigitalInput* intakeSensor;
-  frc::DigitalInput* magSensor;
-  frc::DigitalInput* shooterSensor;
-};
-
-enum class MagState { kIdle, kHold, kEject, kRaw, kPass };
-
-class Mag : public behaviour::HasBehaviour {
- public:
-  explicit Mag(MagConfig config);
-
-  void OnUpdate(units::second_t dt);
-  void SetState(MagState state);
-  void SetRaw(units::volt_t voltage);
-  MagState GetState();
-
- private:
-  MagConfig _config;
-  MagState _state;
-  units::volt_t _rawVoltage = 0_V;
-  std::string _stringStateName = "No State";
-  units::volt_t _setVoltage = 0_V;
-  std::shared_ptr<nt::NetworkTable> _table = nt::NetworkTableInstance::GetDefault().GetTable("Magazine");
-};
+// // Copyright (c) 2023-2024 CurtinFRC
+// // Open Source Software, you can modify it according to the terms
+// // of the MIT License at the root of this project
+
+// #pragma once
+// #include <frc/DigitalInput.h>
+
+// #include <memory>
+// #include <string>
+
+// #include "Wombat.h"
+
+// struct MagConfig {
+//   wom::Gearbox magGearbox;
+//   frc::DigitalInput* intakeSensor;
+//   frc::DigitalInput* magSensor;
+//   frc::DigitalInput* shooterSensor;
+// };
+
+// enum class MagState { kIdle, kHold, kEject, kRaw, kPass };
+
+// class Mag : public behaviour::HasBehaviour {
+//  public:
+//   explicit Mag(MagConfig config);
+
+//   void OnUpdate(units::second_t dt);
+//   void SetState(MagState state);
+//   void SetRaw(units::volt_t voltage);
+//   MagState GetState();
+
+//  private:
+//   MagConfig _config;
+//   MagState _state;
+//   units::volt_t _rawVoltage = 0_V;
+//   std::string _stringStateName = "No State";
+//   units::volt_t _setVoltage = 0_V;
+//   std::shared_ptr<nt::NetworkTable> _table = nt::NetworkTableInstance::GetDefault().GetTable("Magazine");
+// };
diff --git a/src/main/include/Robot.h b/src/main/include/Robot.h
index b0a5f4b7..8482462b 100644
--- a/src/main/include/Robot.h
+++ b/src/main/include/Robot.h
@@ -52,5 +52,5 @@ class Robot : public frc::TimedRobot {
 
   wom::SwerveDrive* _swerveDrive;
   Climber* climber;
-  Mag* mag;
+ // Mag* mag;
 };
diff --git a/src/main/include/RobotMap.h b/src/main/include/RobotMap.h
index 5abc51b8..59672165 100644
--- a/src/main/include/RobotMap.h
+++ b/src/main/include/RobotMap.h
@@ -26,20 +26,27 @@ struct RobotMap {
     frc::XboxController driver = frc::XboxController(0);
     frc::XboxController coDriver = frc::XboxController(1);
     frc::XboxController testController = frc::XboxController(2);
+    
   };
+
+  Controllers controllers;
+
   struct ClimberSystem {
     rev::CANSparkMax climberMotor{99, rev::CANSparkMax::MotorType::kBrushed};
     wom::CANSparkMaxEncoder climberEncoder{&climberMotor, 0.1_m};
     frc::DigitalInput climberSensor{99};
+    // wom::MotorVoltageController climberMotorController = wom::MotorVoltageController::Group(climberMotor);
+    wom::Gearbox climberGearbox{&climberMotor, &climberEncoder, frc::DCMotor::NEO(99).WithReduction(1)};
+    ClimberConfig config {
+        climberGearbox
+    };
   };
-  struct MagSystem {
-    rev::CANSparkMax magMotor{99, rev::CANSparkMax::MotorType::kBrushed};
-    wom::CANSparkMaxEncoder magEncoder{&magEncoder, 0.1_m};
-    frc::DigitalInput magSensor{99};
-  }
-
-    
-  Controllers; Controllers;
+  ClimberSystem climberSystem;
+  // struct MagSystem {
+  //   rev::CANSparkMax magMotor{99, rev::CANSparkMax::MotorType::kBrushed};
+  //   wom::CANSparkMaxEncoder magEncoder{&magEncoder, 0.1_m};
+  //   frc::DigitalInput magSensor{99};
+  // };
 
   struct SwerveBase {
     ctre::phoenix6::hardware::CANcoder frontLeftCancoder{19};
diff --git a/src/main/include/behaviours/MagBehaviour.h b/src/main/include/behaviours/MagBehaviour.h
index e6cd62ab..10024e1d 100644
--- a/src/main/include/behaviours/MagBehaviour.h
+++ b/src/main/include/behaviours/MagBehaviour.h
@@ -1,42 +1,42 @@
-// Copyright (c) 2023-2024 CurtinFRC
-// Open Source Software, you can modify it according to the terms
-// of the MIT License at the root of this project
+// // Copyright (c) 2023-2024 CurtinFRC
+// // Open Source Software, you can modify it according to the terms
+// // of the MIT License at the root of this project
 
-#pragma once
+// #pragma once
 
-#include <frc/XboxController.h>
+// #include <frc/XboxController.h>
 
-#include "Mag.h"
-#include "Wombat.h"
+// #include "Mag.h"
+// #include "Wombat.h"
 
-class MagManualControl : public behaviour::Behaviour {
- public:
-  explicit MagManualControl(Mag* mag, frc::XboxController* codriver);
+// class MagManualControl : public behaviour::Behaviour {
+//  public:
+//   explicit MagManualControl(Mag* mag, frc::XboxController* codriver);
 
-  void OnTick(units::second_t dt) override;
+//   void OnTick(units::second_t dt) override;
 
- private:
-  Mag* _mag;
-  frc::XboxController* _codriver;
-  bool _rawControl;  // Default of Bool is False.
-};
+//  private:
+//   Mag* _mag;
+//   frc::XboxController* _codriver;
+//   bool _rawControl;  // Default of Bool is False.
+// };
 
-class MagAutoPass : public behaviour::Behaviour {
- public:
-  explicit MagAutoPass(Mag* mag);
+// class MagAutoPass : public behaviour::Behaviour {
+//  public:
+//   explicit MagAutoPass(Mag* mag);
 
-  void OnTick(units::second_t dt) override;
+//   void OnTick(units::second_t dt) override;
 
- private:
-  Mag* _mag;
-};
+//  private:
+//   Mag* _mag;
+// };
 
-class MagAutoHold : public behaviour::Behaviour {
- public:
-  explicit MagAutoHold(Mag* mag);
+// class MagAutoHold : public behaviour::Behaviour {
+//  public:
+//   explicit MagAutoHold(Mag* mag);
 
-  void OnTick(units::second_t dt) override;
+//   void OnTick(units::second_t dt) override;
 
- private:
-  Mag* _mag;
-};
+//  private:
+//   Mag* _mag;
+// };

From c3014be47274def81fff19a39e62461b59e582bd Mon Sep 17 00:00:00 2001
From: Goanna247 <21496350@student.curtin.edu.au>
Date: Sat, 2 Mar 2024 12:13:51 +0800
Subject: [PATCH 179/207] subsystems not climber

---
 src/main/cpp/ShooterBehaviour.cpp | 22 +++++++---------------
 1 file changed, 7 insertions(+), 15 deletions(-)

diff --git a/src/main/cpp/ShooterBehaviour.cpp b/src/main/cpp/ShooterBehaviour.cpp
index 72a4cb54..b02d64ca 100644
--- a/src/main/cpp/ShooterBehaviour.cpp
+++ b/src/main/cpp/ShooterBehaviour.cpp
@@ -12,7 +12,7 @@ ShooterManualControl::ShooterManualControl(Shooter* shooter, frc::XboxController
 void ShooterManualControl::OnTick(units::second_t dt) {
   table->GetEntry("RawControl").SetBoolean(_rawControl);
 
-  if (_codriver->GetBackButtonPressed()) {
+  if (_codriver->GetStartButtonPressed()) {
     if (_rawControl == true) {
       _rawControl = false;
     } else {
@@ -22,30 +22,22 @@ void ShooterManualControl::OnTick(units::second_t dt) {
 
   if (_rawControl) {
     _shooter->SetState(ShooterState::kRaw);
-    if (_codriver->GetLeftTriggerAxis() > 0.1) {
-      _shooter->SetRaw(12_V * _codriver->GetLeftTriggerAxis());
-    } else if (_codriver->GetRightTriggerAxis() > 0.1) {
-      _shooter->SetRaw(-12_V * _codriver->GetRightTriggerAxis());
+    if (_codriver->GetRightBumper()) {
+      _shooter->SetRaw(8_V);
+    } else if (_codriver->GetLeftBumper()) {
+      _shooter->SetRaw(-18_V);
     } else {
       _shooter->SetRaw(0_V);
     }
   } else {
-    if (_codriver->GetXButton()) {
+    if (_codriver->GetPOV() == 0) {
       _shooter->SetPidGoal(150_rad_per_s);
       _shooter->SetState(ShooterState::kSpinUp);
       _led->SetState(LEDState::kAiming);
-    } else if (_codriver->GetYButton()) {
+    } else if (_codriver->GetPOV() == 90) {
       _shooter->SetPidGoal(300_rad_per_s);
       _shooter->SetState(ShooterState::kSpinUp);
       _led->SetState(LEDState::kAiming);
-    } else if (_codriver->GetLeftTriggerAxis() > 0.1) {
-      _shooter->SetState(ShooterState::kRaw);
-      _led->SetState(LEDState::kIdle);
-      _shooter->SetRaw(12_V * _codriver->GetLeftTriggerAxis());
-    } else if (_codriver->GetRightTriggerAxis() > 0.1) {
-      _shooter->SetState(ShooterState::kRaw);
-      _shooter->SetRaw(-12_V * _codriver->GetRightTriggerAxis());
-      _led->SetState(LEDState::kIdle);
     } else {
       _shooter->SetState(ShooterState::kIdle);
       _led->SetState(LEDState::kIdle);

From d2e51bf8618660478eb69a5d501d8966a045b1cd Mon Sep 17 00:00:00 2001
From: Goanna247 <21496350@student.curtin.edu.au>
Date: Sun, 3 Mar 2024 13:31:47 +0800
Subject: [PATCH 180/207] Merge branch 'climber' of
 https://github.com/SoulOfTheAssassin/2024-Crescendo into Annas_Version

---
 src/main/cpp/Climber.cpp          | 11 ++++++++++-
 src/main/cpp/ClimberBehaviour.cpp |  2 +-
 src/main/cpp/Robot.cpp            | 18 +++++++++++++++++-
 src/main/include/Climber.h        |  3 ++-
 src/main/include/Robot.h          |  4 ++++
 src/main/include/RobotMap.h       |  3 ++-
 6 files changed, 36 insertions(+), 5 deletions(-)

diff --git a/src/main/cpp/Climber.cpp b/src/main/cpp/Climber.cpp
index f26cea09..b05de086 100644
--- a/src/main/cpp/Climber.cpp
+++ b/src/main/cpp/Climber.cpp
@@ -15,6 +15,14 @@ void Climber::OnUpdate(units::second_t dt) {
     } 
     break;
 
+    case ClimberState::kRatchet: 
+    {
+      _stringStateName = "Ratchet";
+      _setVoltage = 0_V;
+      // armServo.SetAngle(0);
+    } 
+    break;
+
     case ClimberState::kArmUp: 
     {
       _stringStateName = "Arm Up";
@@ -31,6 +39,7 @@ void Climber::OnUpdate(units::second_t dt) {
       _stringStateName = "Arm Down";
       _pid.SetSetpoint(0.07*100);
       _setVoltage = -units::volt_t{_pid.Calculate((-_config.climberGearbox.encoder->GetEncoderPosition().value()) * 100)};
+      // armServo.SetAngle(180);
       // _setVoltage = -8_V;
     } 
     break;
@@ -50,7 +59,7 @@ void Climber::OnUpdate(units::second_t dt) {
     {
       _stringStateName = "Raw";
       _setVoltage = _rawVoltage;
-    } 
+    }  
     break;
     
     default:
diff --git a/src/main/cpp/ClimberBehaviour.cpp b/src/main/cpp/ClimberBehaviour.cpp
index 5b2379f7..eaf5f006 100644
--- a/src/main/cpp/ClimberBehaviour.cpp
+++ b/src/main/cpp/ClimberBehaviour.cpp
@@ -35,7 +35,7 @@ void ClimberManualControl::OnTick(units::second_t dt) {
     else if (_codriver->GetPOV() == 180) {
       _climber->SetState(ClimberState::kArmDown);
     } else  if (_codriver->GetPOV() == 270){
-      _climber->SetState(ClimberState::kIdle);
+      _climber->SetState(ClimberState::kRatchet);
       _arm->SetState(AlphaArmState::kClimbed);
     }
   }
diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp
index 13411789..39544750 100644
--- a/src/main/cpp/Robot.cpp
+++ b/src/main/cpp/Robot.cpp
@@ -42,6 +42,8 @@ void Robot::RobotInit() {
 
   _led = new LED();
 
+  armServo = new frc::Servo(3);
+
   shooter = new Shooter(robotmap.shooterSystem.config);
   wom::BehaviourScheduler::GetInstance()->Register(shooter);
   shooter->SetDefaultBehaviour(
@@ -143,9 +145,23 @@ void Robot::TeleopInit() {
   //reimplement when vision is reimplemented
 
   // _swerveDrive->SetPose(_vision->GetAngleToObject(VisionTargetObjects::kNote).first);
+  armServo->SetAngle(130);
 }
 
-void Robot::TeleopPeriodic() {}
+void Robot::TeleopPeriodic() {
+  if (robotmap.controllers.testController.GetPOV() == 270) {
+    armServo->SetAngle(0);
+  } else if (robotmap.controllers.codriver.GetPOV() == 0) {
+    climberTimer.Start();
+    if (climberTimer.Get() > 2_s) {
+      armServo->SetAngle(130);
+      climberTimer.Stop();
+      climberTimer.Reset();
+    } 
+  }
+
+  robotmap.swerveTable.swerveDriveTable->GetEntry("SERVO POS").SetDouble(armServo->GetAngle());
+}
 
 void Robot::DisabledInit() {}
 void Robot::DisabledPeriodic() {}
diff --git a/src/main/include/Climber.h b/src/main/include/Climber.h
index 861428f2..b640fa62 100644
--- a/src/main/include/Climber.h
+++ b/src/main/include/Climber.h
@@ -8,12 +8,13 @@
 #include <string>
 
 #include "Wombat.h"
+#include <frc/Servo.h>
 
 struct ClimberConfig {
   wom::Gearbox climberGearbox;
 };
 
-enum class ClimberState { kIdle, kArmUp, kArmDown, kMatch, kRaw };
+enum class ClimberState { kIdle, kArmUp, kArmDown, kMatch, kRaw, kRatchet };
 
 class Climber : public behaviour::HasBehaviour {
  public:
diff --git a/src/main/include/Robot.h b/src/main/include/Robot.h
index 5a01c7f5..27bdffc9 100644
--- a/src/main/include/Robot.h
+++ b/src/main/include/Robot.h
@@ -31,6 +31,7 @@
 #include "LED.h"
 #include "Climber.h"
 #include "ClimberBehaviour.h"
+#include <frc/Servo.h>
 
 class Robot : public frc::TimedRobot {
  public:
@@ -56,6 +57,8 @@ class Robot : public frc::TimedRobot {
   Intake* intake;
   Climber* climber;
 
+  frc::Timer climberTimer;
+
   frc::SendableChooser<std::string> m_chooser;
 
   // frc::Field2d m_field;
@@ -80,6 +83,7 @@ class Robot : public frc::TimedRobot {
 
   //wom::SwerveDrive* _swerveDrive;
 
+  frc::Servo *armServo;
 
   //ctre::phoenix6::hardware::TalonFX *frontLeft;
   // ctre::phoenix6::hardware::TalonFX *frontRight;
diff --git a/src/main/include/RobotMap.h b/src/main/include/RobotMap.h
index f48c1ee6..7603d0c8 100644
--- a/src/main/include/RobotMap.h
+++ b/src/main/include/RobotMap.h
@@ -26,6 +26,7 @@
 #include <ctre/phoenix6/CANcoder.hpp>
 #include <ctre/phoenix6/Pigeon2.hpp>
 #include <ctre/phoenix6/TalonFX.hpp>
+// #include <frc/Servo.h>
 
 // #include "Intake.h"
 #include "Wombat.h"
@@ -233,7 +234,7 @@ struct RobotMap {
 //     // wom::MotorVoltageController climberMotorController = wom::MotorVoltageController::Group(climberMotor);
     wom::Gearbox climberGearbox{&climberMotor, climberEncoder, frc::DCMotor::NEO(1)};
     ClimberConfig config {
-        climberGearbox
+        climberGearbox,
     };
   }; ClimberSystem climberSystem;
 };

From 6a53dd6092597c615a43d4981cf4d9f7393226ff Mon Sep 17 00:00:00 2001
From: Goanna247 <21496350@student.curtin.edu.au>
Date: Mon, 4 Mar 2024 10:45:46 +0800
Subject: [PATCH 181/207] Code 3 - 3

---
 src/main/cpp/AlphaArm.cpp                     |  2 +-
 src/main/cpp/IntakeBehaviour.cpp              | 31 +++++++++----------
 src/main/cpp/Robot.cpp                        | 10 +++---
 src/main/cpp/ShooterBehaviour.cpp             |  4 +++
 .../drivetrain/behaviours/SwerveBehaviours.h  |  6 ++--
 5 files changed, 28 insertions(+), 25 deletions(-)

diff --git a/src/main/cpp/AlphaArm.cpp b/src/main/cpp/AlphaArm.cpp
index 17eae957..ac6739cd 100644
--- a/src/main/cpp/AlphaArm.cpp
+++ b/src/main/cpp/AlphaArm.cpp
@@ -45,7 +45,7 @@ void AlphaArm::OnUpdate(units::second_t dt) {
 
     case AlphaArmState::kIntakeAngle:
     std::cout << "Intake Angle" << std::endl;
-       _pidIntakeState.SetSetpoint(-0.50); //-0.48
+       _pidIntakeState.SetSetpoint(-0.52); //-0.48
        _setAlphaArmVoltage = units::volt_t{_pidIntakeState.Calculate(-(_config->alphaArmEncoder.GetEncoderPosition().value() * (2 * 3.1415)))};
     break;
     case AlphaArmState::kClimbAngle:
diff --git a/src/main/cpp/IntakeBehaviour.cpp b/src/main/cpp/IntakeBehaviour.cpp
index 6be7cb97..64de0104 100644
--- a/src/main/cpp/IntakeBehaviour.cpp
+++ b/src/main/cpp/IntakeBehaviour.cpp
@@ -26,33 +26,32 @@ void IntakeManualControl::OnTick(units::second_t dt) {
     _intake->SetState(IntakeState::kRaw);
     if (_codriver.GetLeftTriggerAxis() > 0.1) {
       _intake->SetRaw(_codriver.GetLeftTriggerAxis() * 10_V);
-    }
-    else if (_codriver.GetRightTriggerAxis() > 0.1) {
+    } else if (_codriver.GetRightTriggerAxis() > 0.1) {
       _intake->SetRaw(_codriver.GetRightTriggerAxis() * -10_V);
     } else {
       _intake->SetRaw(0_V);
     }
-    } else if (_codriver.GetXButton()) {
+  } else {
+    if (_codriver.GetXButton()) {
       if (_intake->GetState() == IntakeState::kIdle) {
         _intake->SetState(IntakeState::kIntake);
       }
     } else if (_codriver.GetRightBumper() || _codriver.GetRightTriggerAxis() > 0.1) {
-      if (_intake->GetState() == IntakeState::kHold) {
+      // if (_intake->GetState() == IntakeState::kHold) {
         _intake->SetState(IntakeState::kPass);
-        
-      } else {
-        _intake->SetState(IntakeState::kIdle);
-      }
-    } else if (_codriver.GetBButtonPressed()) {
-      if (_intake->GetState() == IntakeState::kHold) {
+      // } else {
+        // _intake->SetState(IntakeState::kIdle);
+      // }
+    } else if (_codriver.GetBButton()) {
+      // if (_intake->GetState() == IntakeState::kHold) {
         _intake->SetState(IntakeState::kEject);
-      } else {
-        _intake->SetState(IntakeState::kIdle);
-      
-    }
+      // } else {
+      //   _intake->SetState(IntakeState::kIdle);
+      // }
     } else {
-        _intake->SetState(IntakeState::kIdle);
-      }
+      _intake->SetState(IntakeState::kIdle);
+    }
+  }
 }
 
 AutoIntake::AutoIntake(Intake* intake) : _intake(intake) {
diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp
index 39544750..4212fba6 100644
--- a/src/main/cpp/Robot.cpp
+++ b/src/main/cpp/Robot.cpp
@@ -149,12 +149,12 @@ void Robot::TeleopInit() {
 }
 
 void Robot::TeleopPeriodic() {
-  if (robotmap.controllers.testController.GetPOV() == 270) {
-    armServo->SetAngle(0);
-  } else if (robotmap.controllers.codriver.GetPOV() == 0) {
+  if (robotmap.controllers.testController.GetPOV() == 0) {
+    armServo->SetAngle(130);
+  } else if (robotmap.controllers.codriver.GetPOV() == 270) {
     climberTimer.Start();
-    if (climberTimer.Get() > 2_s) {
-      armServo->SetAngle(130);
+    if (climberTimer.Get() > 1_s) {
+      armServo->SetAngle(0);
       climberTimer.Stop();
       climberTimer.Reset();
     } 
diff --git a/src/main/cpp/ShooterBehaviour.cpp b/src/main/cpp/ShooterBehaviour.cpp
index 74b7971f..9880482e 100644
--- a/src/main/cpp/ShooterBehaviour.cpp
+++ b/src/main/cpp/ShooterBehaviour.cpp
@@ -41,6 +41,10 @@ void ShooterManualControl::OnTick(units::second_t dt) {
       _shooter->SetState(ShooterState::kReverse);
       
 
+    } else if (_codriver->GetAButton()) {
+      _shooter->SetPidGoal(1500_rad_per_s);
+      _shooter->SetState(ShooterState::kSpinUp);
+      _led->SetState(LEDState::kAiming);
     } else {
       _shooter->SetState(ShooterState::kIdle);
       _led->SetState(LEDState::kIdle);
diff --git a/wombat/src/main/include/drivetrain/behaviours/SwerveBehaviours.h b/wombat/src/main/include/drivetrain/behaviours/SwerveBehaviours.h
index a466e467..0f71e01d 100644
--- a/wombat/src/main/include/drivetrain/behaviours/SwerveBehaviours.h
+++ b/wombat/src/main/include/drivetrain/behaviours/SwerveBehaviours.h
@@ -81,9 +81,9 @@ class ManualDrivebase : public behaviour::Behaviour {
 
   // The translation speeds for when "slow speed", "normal speed", "fast speed"
   // modes are active
-  const translationSpeed_ lowSensitivityDriveSpeed = 27_ft / 1_s;
-  const translationSpeed_ defaultDriveSpeed = 40_ft / 1_s;
-  const translationSpeed_ highSensitivityDriveSpeed = 27_ft / 1_s;
+  const translationSpeed_ lowSensitivityDriveSpeed = 80_ft / 1_s;
+  const translationSpeed_ defaultDriveSpeed = 80_ft / 1_s;
+  const translationSpeed_ highSensitivityDriveSpeed = 80_ft / 1_s;
   // The rotation speeds for when "slow speed", "normal speed", "fast speed"
   // modes are active
   const rotationSpeed_ lowSensitivityRotateSpeed = 720_deg / 1_s;

From 16236864cc077188791e38f9788fa970fb996d61 Mon Sep 17 00:00:00 2001
From: Goanna247 <21496350@student.curtin.edu.au>
Date: Tue, 5 Mar 2024 20:10:40 +0800
Subject: [PATCH 182/207] Intake changes

---
 src/main/cpp/AlphaArm.cpp            |  7 ++++-
 src/main/cpp/AlphaArmBehaviour.cpp   | 10 ++++--
 src/main/cpp/ClimberBehaviour.cpp    |  2 +-
 src/main/cpp/Intake.cpp              | 47 ++++++++++++++++++----------
 src/main/cpp/IntakeBehaviour.cpp     | 10 ++++--
 src/main/cpp/Robot.cpp               | 16 +++++-----
 src/main/cpp/Shooter.cpp             |  2 +-
 src/main/include/AlphaArm.h          |  3 +-
 src/main/include/AlphaArmBehaviour.h |  4 ++-
 src/main/include/Intake.h            |  1 +
 src/main/include/IntakeBehaviour.h   |  4 ++-
 src/main/include/Robot.h             |  1 +
 12 files changed, 71 insertions(+), 36 deletions(-)

diff --git a/src/main/cpp/AlphaArm.cpp b/src/main/cpp/AlphaArm.cpp
index ac6739cd..9924c29d 100644
--- a/src/main/cpp/AlphaArm.cpp
+++ b/src/main/cpp/AlphaArm.cpp
@@ -45,7 +45,12 @@ void AlphaArm::OnUpdate(units::second_t dt) {
 
     case AlphaArmState::kIntakeAngle:
     std::cout << "Intake Angle" << std::endl;
-       _pidIntakeState.SetSetpoint(-0.52); //-0.48
+       _pidIntakeState.SetSetpoint(-0.50); //-0.48
+       _setAlphaArmVoltage = units::volt_t{_pidIntakeState.Calculate(-(_config->alphaArmEncoder.GetEncoderPosition().value() * (2 * 3.1415)))};
+    break;
+    case AlphaArmState::kIntakedAngle:
+    std::cout << "Intake Angle" << std::endl;
+       _pidIntakeState.SetSetpoint(-0.55); //-0.48
        _setAlphaArmVoltage = units::volt_t{_pidIntakeState.Calculate(-(_config->alphaArmEncoder.GetEncoderPosition().value() * (2 * 3.1415)))};
     break;
     case AlphaArmState::kClimbAngle:
diff --git a/src/main/cpp/AlphaArmBehaviour.cpp b/src/main/cpp/AlphaArmBehaviour.cpp
index 93b6aeb4..47fad071 100644
--- a/src/main/cpp/AlphaArmBehaviour.cpp
+++ b/src/main/cpp/AlphaArmBehaviour.cpp
@@ -6,8 +6,8 @@
 
 #include <frc/XboxController.h>
 
-AlphaArmManualControl::AlphaArmManualControl(AlphaArm* alphaArm, frc::XboxController* codriver)
-    : _alphaArm(alphaArm), _codriver(codriver) {
+AlphaArmManualControl::AlphaArmManualControl(AlphaArm* alphaArm, Intake *intake, frc::XboxController* codriver)
+    : _alphaArm(alphaArm), _intake(intake), _codriver(codriver) {
   Controls(alphaArm);
 }
 
@@ -53,7 +53,11 @@ void AlphaArmManualControl::OnTick(units::second_t dt) {
         } else if(_codriver->GetPOV() == 90){
           _alphaArm->SetState(AlphaArmState::kClimbAngle);
         } else {
-          _alphaArm->SetState(AlphaArmState::kIntakeAngle);
+          if (_intake->GetState() == IntakeState::kHold) {
+            _alphaArm->SetState(AlphaArmState::kIntakedAngle);
+          } else {
+            _alphaArm->SetState(AlphaArmState::kIntakeAngle);
+          }
         }
       }
     }
diff --git a/src/main/cpp/ClimberBehaviour.cpp b/src/main/cpp/ClimberBehaviour.cpp
index eaf5f006..59c0a96f 100644
--- a/src/main/cpp/ClimberBehaviour.cpp
+++ b/src/main/cpp/ClimberBehaviour.cpp
@@ -11,7 +11,7 @@ ClimberManualControl::ClimberManualControl(Climber* climber, AlphaArm* arm, frc:
 
 void ClimberManualControl::OnTick(units::second_t dt) {
 
-  if (_codriver->GetAButtonPressed()) {
+  if (_codriver->GetBackButtonPressed()) {
     if (_rawControl) {
       _rawControl = false;
     } else {
diff --git a/src/main/cpp/Intake.cpp b/src/main/cpp/Intake.cpp
index 04207202..0313ea98 100644
--- a/src/main/cpp/Intake.cpp
+++ b/src/main/cpp/Intake.cpp
@@ -4,7 +4,7 @@
 
 #include "Intake.h"
 
-Intake::Intake(IntakeConfig config) : _config(config), _pid(frc::PIDController (0.0125, 0, 0, 0.05_s)), _pidPosition(frc::PIDController (0.0125, 0, 0, 0.05_s)) {}
+Intake::Intake(IntakeConfig config) : _config(config), _pid(frc::PIDController (0.0125, 0, 0, 0.05_s)), _pidPosition(frc::PIDController (1, 0, 0, 0.05_s)) {}
 
 IntakeConfig Intake::GetConfig() {
   return _config;
@@ -26,6 +26,7 @@ void Intake::OnUpdate(units::second_t dt) {
       _pid.Reset();
       _setVoltage = 0_V;
       _recordNote = false;
+      hasValue = false;
     } 
     break;
 
@@ -43,22 +44,26 @@ void Intake::OnUpdate(units::second_t dt) {
         SetState(IntakeState::kIdle); 
       }
       _stringStateName = "Eject";
-      _setVoltage = 4_V;
+      _setVoltage = 8_V;
       _pid.Reset();
     } 
     break;
 
     case IntakeState::kHold: 
     {
+      units::volt_t pidCalculate = 0_V;
       if (_config.intakeSensor->Get() == true && _config.passSensor->Get() == true) {
         SetState(IntakeState::kIdle);
       }
-      // _pid.SetSetpoint(0);
       // units::volt_t pidCalculate =
-          // units::volt_t{_pid.Calculate(_config.IntakeGearbox.encoder->GetEncoderAngularVelocity().value())};
-      // units::volt_t pidCalculate =
-          // units::volt_t{_pidPosition.Calculate(_config.IntakeGearbox.encoder->GetEncoderPosition().value())};
-      _setVoltage = 0_V;
+      //     units::volt_t{_pid.Calculate(_config.IntakeGearbox.encoder->GetEncoderAngularVelocity().value())};
+      if (_config.IntakeGearbox.encoder->GetEncoderPosition().value() < 0) {
+        pidCalculate = units::volt_t{-_pidPosition.Calculate(-_config.IntakeGearbox.encoder->GetEncoderPosition().value())};
+      } else {
+        pidCalculate = units::volt_t{_pidPosition.Calculate(_config.IntakeGearbox.encoder->GetEncoderPosition().value())};
+      }
+      
+      _setVoltage = pidCalculate;
       _stringStateName = "Hold";
     }
     break;
@@ -66,10 +71,17 @@ void Intake::OnUpdate(units::second_t dt) {
     case IntakeState::kIntake: 
     {
       if (_config.intakeSensor->Get() == false) {
+       
+        if (_config.IntakeGearbox.encoder->GetEncoderPosition().value() < 0) {
+          _pidPosition.SetSetpoint((-_config.IntakeGearbox.encoder->GetEncoderPosition().value()) - 0.5);
+        } else {
+          _pidPosition.SetSetpoint(_config.IntakeGearbox.encoder->GetEncoderPosition().value() + 0.5 );
+        }
+  
         SetState(IntakeState::kHold);
       }
       _stringStateName = "Intake";
-      _setVoltage = -4_V; 
+      _setVoltage = -10_V; 
     } 
     break;
 
@@ -85,24 +97,25 @@ void Intake::OnUpdate(units::second_t dt) {
       }
 
       _stringStateName = "Pass";
-      _setVoltage = -4_V;
+      _setVoltage = -10_V;
     }
     break;
   }
-  _table->GetEntry("State: ").SetString(_stringStateName);
-  _table->GetEntry("Motor Voltage: ").SetDouble(_setVoltage.value());
-  _table->GetEntry("Intake Sensor: ").SetBoolean(_config.intakeSensor->Get());
-  _table->GetEntry("Pass Sensor: ").SetBoolean(_config.passSensor->Get());
-  _table->GetEntry("Error: ").SetDouble(_pid.GetPositionError());
-  _table->GetEntry("SetPoint: ").SetDouble(_pid.GetSetpoint());
-  _table->GetEntry("Encoder: ").SetDouble(_config.IntakeGearbox.encoder->GetEncoderAngularVelocity().value());
-  _table->GetEntry("Shot Count: ").SetDouble(_noteShot);
+  _table->GetEntry("State").SetString(_stringStateName);
+  _table->GetEntry("Motor Voltage").SetDouble(_setVoltage.value());
+  _table->GetEntry("Intake Sensor").SetBoolean(_config.intakeSensor->Get());
+  _table->GetEntry("Pass Sensor").SetBoolean(_config.passSensor->Get());
+  _table->GetEntry("Error").SetDouble(_pidPosition.GetPositionError());
+  _table->GetEntry("SetPoint").SetDouble(_pidPosition.GetSetpoint());
+  _table->GetEntry("Encoder").SetDouble(_config.IntakeGearbox.encoder->GetEncoderPosition().value());
+  _table->GetEntry("Shot Count").SetDouble(_noteShot);
   // _table->GetEntry("Encoder: ").SetDouble(_config.IntakeGearbox.encoder->GetEncoderPosition().value());
  
   _config.IntakeGearbox.motorController->SetVoltage(_setVoltage);
 }
 
 void Intake::SetState(IntakeState state) {
+  
   _state = state;
 }
 void Intake::SetRaw(units::volt_t voltage) {
diff --git a/src/main/cpp/IntakeBehaviour.cpp b/src/main/cpp/IntakeBehaviour.cpp
index 64de0104..a88d94cb 100644
--- a/src/main/cpp/IntakeBehaviour.cpp
+++ b/src/main/cpp/IntakeBehaviour.cpp
@@ -6,7 +6,7 @@
 
 #include <frc/XboxController.h>
 
-IntakeManualControl::IntakeManualControl(Intake* intake, frc::XboxController& codriver) : _intake(intake), _codriver(codriver) {
+IntakeManualControl::IntakeManualControl(Intake* intake, AlphaArm *arm, frc::XboxController& codriver) : _intake(intake), _arm(arm), _codriver(codriver) {
   Controls(intake);
 }
 
@@ -32,6 +32,8 @@ void IntakeManualControl::OnTick(units::second_t dt) {
       _intake->SetRaw(0_V);
     }
   } else {
+    
+
     if (_codriver.GetXButton()) {
       if (_intake->GetState() == IntakeState::kIdle) {
         _intake->SetState(IntakeState::kIntake);
@@ -49,7 +51,11 @@ void IntakeManualControl::OnTick(units::second_t dt) {
       //   _intake->SetState(IntakeState::kIdle);
       // }
     } else {
-      _intake->SetState(IntakeState::kIdle);
+      if (_intake->GetState() == IntakeState::kHold) {
+        _arm->SetState(AlphaArmState::kIntakedAngle);
+      } else {
+        _intake->SetState(IntakeState::kIdle);
+      }
     }
   }
 }
diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp
index 4212fba6..4664d440 100644
--- a/src/main/cpp/Robot.cpp
+++ b/src/main/cpp/Robot.cpp
@@ -48,29 +48,29 @@ void Robot::RobotInit() {
   wom::BehaviourScheduler::GetInstance()->Register(shooter);
   shooter->SetDefaultBehaviour(
       [this]() { return wom::make<ShooterManualControl>(shooter, &robotmap.controllers.codriver, _led); });
-  
 
   _swerveDrive = new wom::SwerveDrive(robotmap.swerveBase.config, frc::Pose2d());
   wom::BehaviourScheduler::GetInstance()->Register(_swerveDrive);
   _swerveDrive->SetDefaultBehaviour(
       [this]() { return wom::make<wom::ManualDrivebase>(_swerveDrive, &robotmap.controllers.driver); });
 
-
-    intake = new Intake(robotmap.intakeSystem.config);
-  wom::BehaviourScheduler::GetInstance()->Register(intake);
-  intake->SetDefaultBehaviour(
-      [this]() { return wom::make<IntakeManualControl>(intake, robotmap.controllers.codriver); });
-
   alphaArm = new AlphaArm(&robotmap.alphaArmSystem.config);
+  intake = new Intake(robotmap.intakeSystem.config);
+  
+  
   wom::BehaviourScheduler::GetInstance()->Register(alphaArm);
   alphaArm->SetDefaultBehaviour(
-      [this]() { return wom::make<AlphaArmManualControl>(alphaArm, &robotmap.controllers.codriver); });
+      [this]() { return wom::make<AlphaArmManualControl>(alphaArm, intake, &robotmap.controllers.codriver); });
 
   climber = new Climber(robotmap.climberSystem.config);
   wom::BehaviourScheduler::GetInstance()->Register(climber);
   climber->SetDefaultBehaviour(
       [this]() { return wom::make<ClimberManualControl>(climber, alphaArm, &robotmap.controllers.codriver); });
 
+  wom::BehaviourScheduler::GetInstance()->Register(intake);
+  intake->SetDefaultBehaviour(
+      [this]() { return wom::make<IntakeManualControl>(intake, alphaArm, robotmap.controllers.codriver); });
+
   robotmap.swerveBase.moduleConfigs[0].turnMotor.encoder->SetEncoderOffset(0.45229_rad);
   robotmap.swerveBase.moduleConfigs[1].turnMotor.encoder->SetEncoderOffset(2.6846_rad);
   robotmap.swerveBase.moduleConfigs[2].turnMotor.encoder->SetEncoderOffset(3.01121_rad);
diff --git a/src/main/cpp/Shooter.cpp b/src/main/cpp/Shooter.cpp
index 05e64dae..807e90d9 100644
--- a/src/main/cpp/Shooter.cpp
+++ b/src/main/cpp/Shooter.cpp
@@ -73,7 +73,7 @@ void Shooter::OnUpdate(units::second_t dt) {
     case ShooterState::kReverse: {
       _statename = "Reverse";
       _pid.Reset();
-      _setVoltage = -5_V;
+      _setVoltage = -8_V;
       std::cout << "KReverse" << std::endl;
       // if (!_shooterSensor.Get()) {
       //   SetState(ShooterState::kIdle);
diff --git a/src/main/include/AlphaArm.h b/src/main/include/AlphaArm.h
index 1ded63d8..06fdc3a7 100644
--- a/src/main/include/AlphaArm.h
+++ b/src/main/include/AlphaArm.h
@@ -33,7 +33,8 @@ enum class AlphaArmState {
   kStowed,
   kClimbAngle,
   kClimbed,
-  kRaw
+  kRaw,
+  kIntakedAngle
 };
 
 class AlphaArm : public behaviour::HasBehaviour {
diff --git a/src/main/include/AlphaArmBehaviour.h b/src/main/include/AlphaArmBehaviour.h
index dc9b3ef0..7271d21f 100644
--- a/src/main/include/AlphaArmBehaviour.h
+++ b/src/main/include/AlphaArmBehaviour.h
@@ -8,15 +8,17 @@
 #include <frc/XboxController.h>
 
 #include "AlphaArm.h"
+#include "Intake.h"
 #include "Wombat.h"
 
 class AlphaArmManualControl : public behaviour::Behaviour {
  public:
-  explicit AlphaArmManualControl(AlphaArm* alphaArm, frc::XboxController* codriver);
+  explicit AlphaArmManualControl(AlphaArm* alphaArm, Intake* intake, frc::XboxController* codriver);
   void OnTick(units::second_t dt);
 
  private:
   AlphaArm* _alphaArm;
+  Intake *_intake;
   frc::XboxController* _codriver;
   units::volt_t _setAlphaArmVoltage = 0_V;
   bool _rawControl = false;
diff --git a/src/main/include/Intake.h b/src/main/include/Intake.h
index a4ecdca1..87169e93 100644
--- a/src/main/include/Intake.h
+++ b/src/main/include/Intake.h
@@ -43,6 +43,7 @@ class Intake : public behaviour::HasBehaviour {
   int _noteShot = 0;
 
   bool _recordNote = false;
+  bool hasValue = false;
 
   units::volt_t _rawVoltage = 0_V;
   std::string _stringStateName = "error";
diff --git a/src/main/include/IntakeBehaviour.h b/src/main/include/IntakeBehaviour.h
index 642d20b2..cdeaa687 100644
--- a/src/main/include/IntakeBehaviour.h
+++ b/src/main/include/IntakeBehaviour.h
@@ -7,16 +7,18 @@
 #include <frc/XboxController.h>
 
 #include "Intake.h"
+#include "AlphaArm.h"
 #include "Wombat.h"
 
 class IntakeManualControl : public behaviour::Behaviour {
  public:
-  explicit IntakeManualControl(Intake* intake, frc::XboxController& codriver);
+  explicit IntakeManualControl(Intake* intake, AlphaArm *arm, frc::XboxController& codriver);
 
   void OnTick(units::second_t dt) override;
 
  private:
   Intake* _intake;
+  AlphaArm *_arm;
   frc::XboxController& _codriver;
   bool _rawControl = false;
 };
diff --git a/src/main/include/Robot.h b/src/main/include/Robot.h
index 27bdffc9..8d6b68d4 100644
--- a/src/main/include/Robot.h
+++ b/src/main/include/Robot.h
@@ -61,6 +61,7 @@ class Robot : public frc::TimedRobot {
 
   frc::SendableChooser<std::string> m_chooser;
 
+
   // frc::Field2d m_field;
 
   // frc::Timer simulation_timer;

From f860a81a84540b8e5b7a501556e76040d7f79d69 Mon Sep 17 00:00:00 2001
From: = <paul.hodges@student.education.wa.edu.au>
Date: Sun, 3 Mar 2024 11:42:02 +0800
Subject: [PATCH 183/207] merge

---
 ctre_sim/CANCoder vers. H - 016 - 0 - ext.dat | Bin 2048 -> 2048 bytes
 ctre_sim/CANCoder vers. H - 017 - 0 - ext.dat | Bin 2048 -> 2048 bytes
 ctre_sim/CANCoder vers. H - 018 - 0 - ext.dat | Bin 2048 -> 2048 bytes
 ctre_sim/CANCoder vers. H - 019 - 0 - ext.dat | Bin 2048 -> 2048 bytes
 ctre_sim/Pigeon 2 - 020 - 0 - ext.dat         | Bin 2048 -> 2048 bytes
 ctre_sim/Talon FX vers. C - 01 - 0 - ext.dat  | Bin 2048 -> 2048 bytes
 ctre_sim/Talon FX vers. C - 02 - 0 - ext.dat  | Bin 2048 -> 2048 bytes
 ctre_sim/Talon FX vers. C - 03 - 0 - ext.dat  | Bin 2048 -> 2048 bytes
 ctre_sim/Talon FX vers. C - 04 - 0 - ext.dat  | Bin 2048 -> 2048 bytes
 ctre_sim/Talon FX vers. C - 05 - 0 - ext.dat  | Bin 2048 -> 2048 bytes
 ctre_sim/Talon FX vers. C - 06 - 0 - ext.dat  | Bin 2048 -> 2048 bytes
 ctre_sim/Talon FX vers. C - 07 - 0 - ext.dat  | Bin 2048 -> 2048 bytes
 ctre_sim/Talon FX vers. C - 09 - 0 - ext.dat  | Bin 2048 -> 2048 bytes
 simgui.json                                   |  22 +-
 src/main/cpp/Auto.cpp                         | 299 ++++++++++
 src/main/cpp/Robot.cpp                        |  33 +-
 src/main/cpp/vision/Vision.cpp                | 419 ++++++++++++++
 src/main/cpp/vision/VisionBehaviours.cpp      |  63 +++
 .../deploy/pathplanner/autos/New Auto.auto    |  38 --
 src/main/deploy/pathplanner/autos/Taxi.auto   |  25 +
 .../{Example Path.path => FirstNote.path}     |  36 +-
 src/main/include/Auto.h                       |  41 ++
 src/main/include/Robot.h                      |  12 +
 src/main/include/RobotMap.h                   |   3 +
 src/main/include/vision/Vision.h              | 115 ++++
 src/main/include/vision/VisionBehaviours.h    |  56 ++
 ...w-Commands.json => WPILibNewCommands.json} |   0
 .../src/main/cpp/drivetrain/SwerveDrive.cpp   |  12 +-
 .../behaviours/SwerveBehaviours.cpp           | 211 ++++----
 wombat/src/main/cpp/sim/Sim.cpp               |  21 +
 wombat/src/main/cpp/utils/Pathplanner.cpp     | 512 +++++++++++++++++-
 wombat/src/main/cpp/vision/Limelight.cpp      | 372 +++++++------
 wombat/src/main/include/Wombat.h              |   2 +
 .../src/main/include/drivetrain/SwerveDrive.h |   5 +-
 .../drivetrain/behaviours/SwerveBehaviours.h  | 183 ++++---
 wombat/src/main/include/sim/Sim.h             |  25 +
 wombat/src/main/include/utils/Pathplanner.h   | 302 ++++++++++-
 wombat/src/main/include/vision/Limelight.h    | 229 ++++----
 38 files changed, 2483 insertions(+), 553 deletions(-)
 create mode 100644 src/main/cpp/Auto.cpp
 create mode 100644 src/main/cpp/vision/Vision.cpp
 create mode 100644 src/main/cpp/vision/VisionBehaviours.cpp
 delete mode 100644 src/main/deploy/pathplanner/autos/New Auto.auto
 create mode 100644 src/main/deploy/pathplanner/autos/Taxi.auto
 rename src/main/deploy/pathplanner/paths/{Example Path.path => FirstNote.path} (54%)
 create mode 100644 src/main/include/Auto.h
 create mode 100644 src/main/include/vision/Vision.h
 create mode 100644 src/main/include/vision/VisionBehaviours.h
 rename vendordeps/{WPILib-New-Commands.json => WPILibNewCommands.json} (100%)
 create mode 100644 wombat/src/main/cpp/sim/Sim.cpp
 create mode 100644 wombat/src/main/include/sim/Sim.h

diff --git a/ctre_sim/CANCoder vers. H - 016 - 0 - ext.dat b/ctre_sim/CANCoder vers. H - 016 - 0 - ext.dat
index 7af9a7c92dfa5d0375f972414c70d47775fef131..e6ad3f526e294d9d8071d5296e3376d8ac97771c 100644
GIT binary patch
delta 36
ncmZn=Xb{+tz{t6mq510$P6h^!iGuE&dl=fjZi8|bF|h#v)N%^E

delta 36
ncmZn=Xb{+tz{t6k;r7=IP6h^!iGuE&+Zb+q&4O|kF|h#v)s70h

diff --git a/ctre_sim/CANCoder vers. H - 017 - 0 - ext.dat b/ctre_sim/CANCoder vers. H - 017 - 0 - ext.dat
index 669137759f209f811b4743db197a98ea6b470a3a..b279de60896bc5ee7ab02533a1e610e7d8c62aca 100644
GIT binary patch
delta 30
kcmZn=Xb{+tz{s|nq5tbP&WVCvY<n1bzU~0A7BR5_0I6~cyZ`_I

delta 30
kcmZn=Xb{+tz{s|h;o;XT&WVCvY+D$fe9Zu{7BR5_0IH4(yZ`_I

diff --git a/ctre_sim/CANCoder vers. H - 018 - 0 - ext.dat b/ctre_sim/CANCoder vers. H - 018 - 0 - ext.dat
index 8835a41c024e6d4ed9f8c2de8ca7962412fa91d7..a898ffa26146b8fffbbec2dd142f0788f1a6f18b 100644
GIT binary patch
delta 36
ncmZn=Xb{+tz{t6mq510$P6h^!iGuE&dl=fjZi8|bF|h#v)N%^E

delta 36
ncmZn=Xb{+tz{t6k;r7=IP6h^!iGuE&+Zb+q&4O|kF|h#v)s70h

diff --git a/ctre_sim/CANCoder vers. H - 019 - 0 - ext.dat b/ctre_sim/CANCoder vers. H - 019 - 0 - ext.dat
index e7951dbf07e2c8b3c25e3036a4b58ba15d7ba128..3d5aac8103dde875317a3f6262e68f1092140276 100644
GIT binary patch
delta 30
kcmZn=Xb{+tz{s|nq5tbP&WVCvY<n1bzU~0A7BR5_0I6~cyZ`_I

delta 30
kcmZn=Xb{+tz{s|h;o;XT&WVCvY+D$fe9Zu{7BR5_0IH4(yZ`_I

diff --git a/ctre_sim/Pigeon 2 - 020 - 0 - ext.dat b/ctre_sim/Pigeon 2 - 020 - 0 - ext.dat
index 941345ac17d7df96e44048b557dc28b3069039c2..0aaa8ded0514f2a8ab22c7ce03bfdd073478f496 100644
GIT binary patch
delta 46
zcmZn=Xb@mI$zc9}qo6(GL<Qc>0gM5RY`Yoyzi#84DCotuhoR@|4iIaz1M>kE0A|$>
A(*OVf

delta 46
zcmZn=Xb@mI&S3d}qo6(GL<Qc>0gM5RY+D%~e$C>XDCotuh2hE93=nIx1M>kE0B7wF
A(*OVf

diff --git a/ctre_sim/Talon FX vers. C - 01 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 01 - 0 - ext.dat
index ee4ebedc65ed22306d2234f177c2d6d5c2236c8b..5cebefc390a85fb32b5a9b2d0fe4f3a39a658ed1 100644
GIT binary patch
delta 30
kcmZn=Xb{-Yz{qx*q5kVO&WVavY-bp1zU~0A7BR5_0Ic!~+yDRo

delta 30
kcmZn=Xb{-Yz{qx#;o{dU&WVavY)2Tbe9Zu{7BR5_0Im)S+yDRo

diff --git a/ctre_sim/Talon FX vers. C - 02 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 02 - 0 - ext.dat
index 948df76b81bbc157fed419f5e20da094da591c1f..87f97e91fec44124de85035dd916b04cc06b7fc0 100644
GIT binary patch
delta 30
kcmZn=Xb{-Yz{qx*q5kVO&WVavY-bp1zU~0A7BR5_0Ic!~+yDRo

delta 30
kcmZn=Xb{-Yz{qx#;o{dU&WVavY)2Tbe9Zu{7BR5_0Im)S+yDRo

diff --git a/ctre_sim/Talon FX vers. C - 03 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 03 - 0 - ext.dat
index 7c267efd91a0f2b2aa92e9d604ec5e203624088a..6b6576837e9766f3d5cccd5d345df10587e49b6e 100644
GIT binary patch
delta 30
kcmZn=Xb{-Yz{qx*q5kVO&WVavY-bp1zU~0A7BR5_0Ic!~+yDRo

delta 30
kcmZn=Xb{-Yz{qx#;o{dU&WVavY)2Tbe9Zu{7BR5_0Im)S+yDRo

diff --git a/ctre_sim/Talon FX vers. C - 04 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 04 - 0 - ext.dat
index ac502f4e3315670384588f08e3ae021f583877d8..303882212add0bbe6ecad283ef6863a402872ab6 100644
GIT binary patch
delta 30
kcmZn=Xb{-Yz{qx*q5kVO&WVavY-bp1zU~0A7BR5_0Ic!~+yDRo

delta 30
kcmZn=Xb{-Yz{qx#;o{dU&WVavY)2Tbe9Zu{7BR5_0Im)S+yDRo

diff --git a/ctre_sim/Talon FX vers. C - 05 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 05 - 0 - ext.dat
index cb9bb70f8f10973dc4bcc2073e2b89feb8aad238..34b0da333dad7707db4f388ffe16afff38f7b37e 100644
GIT binary patch
delta 30
kcmZn=Xb{-Yz{qx*q5kVO&WVavY-bp1zU~0A7BR5_0Ic!~+yDRo

delta 30
kcmZn=Xb{-Yz{qx#;o{dU&WVavY)2Tbe9Zu{7BR5_0Im)S+yDRo

diff --git a/ctre_sim/Talon FX vers. C - 06 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 06 - 0 - ext.dat
index 113659522d70fa911d9eb144e0d2d24aa7179076..f49171a018a337cd3790628619d5977854ea71f1 100644
GIT binary patch
delta 30
kcmZn=Xb{-Yz{qx*q5kVO&WVavY-bp1zU~0A7BR5_0Ic!~+yDRo

delta 30
kcmZn=Xb{-Yz{qx#;o{dU&WVavY)2Tbe9Zu{7BR5_0Im)S+yDRo

diff --git a/ctre_sim/Talon FX vers. C - 07 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 07 - 0 - ext.dat
index b5fbe904afc7719fd295f3b1026074af31236ff6..8485b83c4a5c5ebca12ad925f526daeec38fb63b 100644
GIT binary patch
delta 30
kcmZn=Xb{-Yz{qx*q5kVO&WVavY-bp1zU~0A7BR5_0Ic!~+yDRo

delta 30
kcmZn=Xb{-Yz{qx#;o{dU&WVavY)2Tbe9Zu{7BR5_0Im)S+yDRo

diff --git a/ctre_sim/Talon FX vers. C - 09 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 09 - 0 - ext.dat
index 7eb8bcf1b5a63272f8196b91910c4256f82f322f..806373e9a40947a21e398b84196655a5ba6bdff2 100644
GIT binary patch
delta 30
kcmZn=Xb{-Yz{qx*q5kVO&WVavY-bp1zU~0A7BR5_0Ic!~+yDRo

delta 30
kcmZn=Xb{-Yz{qx#;o{dU&WVavY)2Tbe9Zu{7BR5_0Im)S+yDRo

diff --git a/simgui.json b/simgui.json
index 17f01c64..ff5f09a8 100644
--- a/simgui.json
+++ b/simgui.json
@@ -1,7 +1,27 @@
 {
   "NTProvider": {
     "types": {
-      "/FMSInfo": "FMSInfo"
+      "/FMSInfo": "FMSInfo",
+      "/SmartDashboard/Auto Selector": "String Chooser",
+      "/SmartDashboard/Field": "Field2d"
+    },
+    "windows": {
+      "/SmartDashboard/Auto Selector": {
+        "window": {
+          "visible": true
+        }
+      },
+      "/SmartDashboard/Field": {
+        "bottom": 1476,
+        "height": 8.210550308227539,
+        "left": 150,
+        "right": 2961,
+        "top": 79,
+        "width": 16.541748046875,
+        "window": {
+          "visible": true
+        }
+      }
     }
   },
   "NetworkTables": {
diff --git a/src/main/cpp/Auto.cpp b/src/main/cpp/Auto.cpp
new file mode 100644
index 00000000..e4571fd7
--- /dev/null
+++ b/src/main/cpp/Auto.cpp
@@ -0,0 +1,299 @@
+// Copyright (c) 2023-2024 CurtinFRC
+// Open Source Software, you can modify it according to the terms
+// of the MIT License at the root of this project
+
+#include "Auto.h"
+
+#include <utility>
+
+#include "AlphaArmBehaviour.h"
+#include "IntakeBehaviour.h"
+#include "ShooterBehaviour.h"
+#include "behaviour/Behaviour.h"
+#include "drivetrain/behaviours/SwerveBehaviours.h"
+#include "units/angle.h"
+#include "utils/Pathplanner.h"
+
+// Shoots starting note then moves out of starting position.
+wom::SwerveAutoBuilder* autos::InitCommands(wom::drivetrain::SwerveDrive* _swerveDrive, Shooter* _shooter,
+                                            Intake* _intake, AlphaArm* _alphaArm) {
+  wom::AutoCommands c = *new wom::AutoCommands(
+      {// {"ArmToSetPoint", [_alphaArm]() { return wom::make<ArmToSetPoint>(_alphaArm, 20_deg); }},
+       // {"AutoShoot", [_shooter]() { return wom::make<AutoShoot>(_shooter); }},
+       /*{"AutoIntake", [_intake]() { return wom::make<AutoIntake>(_intake); }}*/});
+
+  return new wom::utils::SwerveAutoBuilder(_swerveDrive, "Taxi", c);
+}
+
+std::shared_ptr<behaviour::Behaviour> autos::Taxi(wom::SwerveAutoBuilder* builder) {
+  return builder->GetAutoRoutine("Taxi");
+  // return behaviour::make<ArmToSetPoint>(_alphaArm, 0_deg);
+  // behaviour::make<AutoShoot>(_shooter);
+  // behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+}
+
+// std::shared_ptr<behaviour::Behaviour> autos::QuadrupleClose(wom::drivetrain::SwerveDrive* _swerveDrive,
+//                                                             Shooter* _shooter, Intake* _intake,
+//                                                             AlphaArm* _alphaArm) {
+//   return behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+//   behaviour::make<AutoShoot>(_shooter);
+//   behaviour::make<wom::drivetrain::behaviours::DrivebasePoseBehaviour>(
+//       _swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false);
+//   behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+//   behaviour::make<AutoIntake>(_intake);
+//   behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+//   behaviour::make<AutoShoot>(_shooter);
+//   behaviour::make<wom::drivetrain::behaviours::DrivebasePoseBehaviour>(
+//       _swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false);
+//   behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+//   behaviour::make<AutoIntake>(_intake);
+//   behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+//   behaviour::make<AutoShoot>(_shooter);
+//   behaviour::make<wom::drivetrain::behaviours::DrivebasePoseBehaviour>(
+//       _swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false);
+//   behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+//   behaviour::make<AutoIntake>(_intake);
+//   behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+//   behaviour::make<AutoShoot>(_shooter);
+//
+//   /*
+//    4N Close
+//  1. Shoot starting note into speaker
+//  2. Intake note from close note
+//  3. Shoot note into speaker
+//  4. Intake note from close floor note
+//  5. Shoot note into speaker
+//  6. Intake not from close floor
+//  7. Shoot note
+//  */
+// }
+//
+// std::shared_ptr<behaviour::Behaviour> autos::QuadrupleFar(wom::drivetrain::SwerveDrive* _swerveDrive,
+//                                                           Shooter* _shooter, Intake* _intake,
+//                                                           AlphaArm* _alphaArm) {
+//   return behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+//   behaviour::make<AutoShoot>(_shooter);
+//   behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+//   behaviour::make<wom::drivetrain::behaviours::DrivebasePoseBehaviour>(
+//       _swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false);
+//   behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+//   behaviour::make<AutoIntake>(_intake);
+//   behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+//   behaviour::make<wom::drivetrain::behaviours::DrivebasePoseBehaviour>(
+//       _swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false);
+//   behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+//   behaviour::make<AutoShoot>(_shooter);
+//   behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+//   behaviour::make<wom::drivetrain::behaviours::DrivebasePoseBehaviour>(
+//       _swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false);
+//   behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+//   behaviour::make<AutoIntake>(_intake);
+//   behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+//   behaviour::make<wom::drivetrain::behaviours::DrivebasePoseBehaviour>(
+//       _swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false);
+//   behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+//   behaviour::make<AutoShoot>(_shooter);
+//   behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+//   behaviour::make<wom::drivetrain::behaviours::DrivebasePoseBehaviour>(
+//       _swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false);
+//   behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+//   behaviour::make<AutoIntake>(_intake);
+//   behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+//   behaviour::make<wom::drivetrain::behaviours::DrivebasePoseBehaviour>(
+//       _swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false);
+//   behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+//   behaviour::make<AutoShoot>(_shooter);
+//   behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+//   behaviour::make<wom::drivetrain::behaviours::DrivebasePoseBehaviour>(
+//       _swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false);
+//   behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+//   behaviour::make<AutoIntake>(_intake);
+//
+//   /*
+//     4N Far
+//   1. Shoot start note in speaker
+//   2. Drive to far note
+//   3. Intake note
+//   4. Drive back to shooting line
+//   5. Shoot note into speaker
+//   6. Drive to note
+//   7. Intake note
+//   8. Drive to shooting line
+//   9. Shoot note
+//   10. Drive to note
+//   11. Intake note
+//   12. Drive to shooting line
+//   13. Shoot note
+//   14. Drive to intake note (if possible)
+//   */
+// }
+//
+// std::shared_ptr<behaviour::Behaviour> autos::QuadrupleCloseDoubleFar(
+//     wom::drivetrain::SwerveDrive* _swerveDrive, Shooter* _shooter, Intake* _intake, AlphaArm* _alphaArm) {
+//   return behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+//   behaviour::make<AutoShoot>(_shooter);
+//   behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+//   behaviour::make<wom::drivetrain::behaviours::DrivebasePoseBehaviour>(
+//       _swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false);
+//   behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+//   behaviour::make<AutoIntake>(_intake);
+//   behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+//   behaviour::make<wom::drivetrain::behaviours::DrivebasePoseBehaviour>(
+//       _swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false);
+//   behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+//   behaviour::make<AutoShoot>(_shooter);
+//   behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+//   behaviour::make<wom::drivetrain::behaviours::DrivebasePoseBehaviour>(
+//       _swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false);
+//   behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+//   behaviour::make<AutoIntake>(_intake);
+//   behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+//   behaviour::make<wom::drivetrain::behaviours::DrivebasePoseBehaviour>(
+//       _swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false);
+//   behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+//   behaviour::make<AutoShoot>(_shooter);
+//   behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+//   behaviour::make<wom::drivetrain::behaviours::DrivebasePoseBehaviour>(
+//       _swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false);
+//   behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+//   behaviour::make<AutoIntake>(_intake);
+//   behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+//   behaviour::make<wom::drivetrain::behaviours::DrivebasePoseBehaviour>(
+//       _swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false);
+//   behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+//   behaviour::make<AutoShoot>(_shooter);
+//   behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+//   behaviour::make<wom::drivetrain::behaviours::DrivebasePoseBehaviour>(
+//       _swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false);
+//   behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+//   behaviour::make<AutoIntake>(_intake);
+//   behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+//   behaviour::make<wom::drivetrain::behaviours::DrivebasePoseBehaviour>(
+//       _swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false);
+//   behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+//   behaviour::make<AutoShoot>(_shooter);
+//   behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+//   behaviour::make<wom::drivetrain::behaviours::DrivebasePoseBehaviour>(
+//       _swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false);
+//   behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+//   behaviour::make<AutoIntake>(_intake);
+//   behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+//   behaviour::make<wom::drivetrain::behaviours::DrivebasePoseBehaviour>(
+//       _swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false);
+//   behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+//   behaviour::make<AutoShoot>(_shooter);
+//   behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+//   behaviour::make<wom::drivetrain::behaviours::DrivebasePoseBehaviour>(
+//       _swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false);
+//   behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+//   behaviour::make<AutoIntake>(_intake);
+//   behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+//   behaviour::make<AutoShoot>(_shooter);
+//
+//   //   4N Close 2N Far
+//   // 1. Shoot note
+//   // 2. Drive to close note
+//   // 3. Intake note
+//   // 4. Shoot note
+//   // 5. Drive to close note
+//   // 6. Intake note
+//   // 7. Shoot note
+//   // 8. Drive to close note
+//   // 9. Intake note
+//   // 10. Shoot note
+//   // 11. Drive to far note
+//   // 12. Intake note
+//   // 13. Drive to shooting line
+//   // 14. Shoot note
+//   // 15. Drive to far note
+//   // 16. Intake note
+//   // 17. Drive to shooting line
+//   // 18. Shoot note
+// }
+//
+// std::shared_ptr<behaviour::Behaviour> autos::QuadrupleCloseSingleFar(
+//     wom::drivetrain::SwerveDrive* _swerveDrive, Shooter* _shooter, Intake* _intake, AlphaArm* _alphaArm) {
+//   return behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+//   behaviour::make<AutoShoot>(_shooter);
+//   behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+//   behaviour::make<wom::drivetrain::behaviours::DrivebasePoseBehaviour>(
+//       _swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false);
+//   behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+//   behaviour::make<AutoIntake>(_intake);
+//   behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+//   behaviour::make<wom::drivetrain::behaviours::DrivebasePoseBehaviour>(
+//       _swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false);
+//   behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+//   behaviour::make<AutoShoot>(_shooter);
+//   behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+//   behaviour::make<wom::drivetrain::behaviours::DrivebasePoseBehaviour>(
+//       _swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false);
+//   behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+//   behaviour::make<AutoIntake>(_intake);
+//   behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+//   behaviour::make<wom::drivetrain::behaviours::DrivebasePoseBehaviour>(
+//       _swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false);
+//   behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+//   behaviour::make<AutoShoot>(_shooter);
+//   behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+//   behaviour::make<wom::drivetrain::behaviours::DrivebasePoseBehaviour>(
+//       _swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false);
+//   behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+//   behaviour::make<AutoIntake>(_intake);
+//   behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+//   behaviour::make<wom::drivetrain::behaviours::DrivebasePoseBehaviour>(
+//       _swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false);
+//   behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+//   behaviour::make<AutoShoot>(_shooter);
+//   behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+// }
+//
+// // 4N Close 1N Far
+// // 1. Shoot note
+// // 2. Drive to close note
+// // 3. Intake note
+// // 4. Drive to speaker
+// // 5. Shoot note
+// // 6. Drive to close note
+// // 7. Intake note
+// // 8. Drive to speaker
+// // 9. Shoot note
+// // 10. Drive to close note
+// // 11. Intake note
+// // 12. Drive to speaker
+// // 13. Shoot note
+// // 14. Drive to far note
+// // 15. Intake note
+// // 15. Drive to speaker
+// // 16. shoot
+//
+// //     /*
+// //     TRAP AUTO
+// //     1. Drive to trap
+// //     2. Climb up
+// //     3. Shoot note
+// //     4. Climb down
+// //     5. Drive to far note
+// //     6. Intake note
+// //     7. Drive to trap
+// //     8. Climb up
+// //     9. Shoot note
+// //     10. Climb down
+// //     11. Drive to far note
+// //     12. Intake
+// //     13. Drive to trap
+// //     14. Climb up
+// //     15. Shoot note
+// //     16. Climb down
+// //     */
+// // }
+//
+// std::shared_ptr<behaviour::Behaviour> autos::AutoTest(wom::drivetrain::SwerveDrive* _swerveDrive,
+//                                                       Shooter* _shooter, Intake* _intake,
+//                                                       AlphaArm* _alphaArm) {
+//   return behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
+//   behaviour::make<wom::drivetrain::behaviours::DrivebasePoseBehaviour>(
+//       _swerveDrive, frc::Pose2d{0_m, 0_m, 0_deg}, 0_V, false);
+//   behaviour::make<AutoShoot>(_shooter);
+//   behaviour::make<AutoIntake>(_intake);
+// }  // This auto is a test for auto to see if all things work.
diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp
index 4664d440..bde3baa4 100644
--- a/src/main/cpp/Robot.cpp
+++ b/src/main/cpp/Robot.cpp
@@ -3,6 +3,7 @@
 // of the MIT License at the root of this project
 
 #include "Robot.h"
+#include "Auto.h"
 #include "RobotMap.h"
 #include <frc/TimedRobot.h>
 #include <frc/Timer.h>
@@ -12,7 +13,6 @@
 #include <networktables/NetworkTable.h>
 #include <networktables/NetworkTableInstance.h>
 #include <units/acceleration.h>
-// #include <units/angle.h>
 
 // include units
 #include <units/velocity.h>
@@ -38,7 +38,14 @@ static units::second_t lastPeriodic;
 
 void Robot::RobotInit() {
   sched = wom::BehaviourScheduler::GetInstance();
-  m_chooser.SetDefaultOption("Default Auto", "Default Auto");
+
+  frc::SmartDashboard::PutData("Auto Selector", &m_chooser);
+
+  m_chooser.SetDefaultOption(defaultAuto, defaultAuto);
+
+  for (auto& option : autoOptions) {
+    m_chooser.AddOption(option, option);
+  }
 
   _led = new LED();
 
@@ -56,8 +63,8 @@ void Robot::RobotInit() {
 
   alphaArm = new AlphaArm(&robotmap.alphaArmSystem.config);
   intake = new Intake(robotmap.intakeSystem.config);
-  
-  
+
+
   wom::BehaviourScheduler::GetInstance()->Register(alphaArm);
   alphaArm->SetDefaultBehaviour(
       [this]() { return wom::make<AlphaArmManualControl>(alphaArm, intake, &robotmap.controllers.codriver); });
@@ -76,6 +83,9 @@ void Robot::RobotInit() {
   robotmap.swerveBase.moduleConfigs[2].turnMotor.encoder->SetEncoderOffset(3.01121_rad);
   robotmap.swerveBase.moduleConfigs[3].turnMotor.encoder->SetEncoderOffset(4.4524_rad);
 
+  robotmap._builder = autos::InitCommands(_swerveDrive, shooter, intake, alphaArm);
+  robotmap._simSwerve = new wom::SimSwerve(_swerveDrive);
+
   lastPeriodic = wom::now();
 }
 
@@ -121,9 +131,20 @@ void Robot::RobotPeriodic() {
 void Robot::AutonomousInit() {
   loop.Clear();
   sched->InterruptAll();
+
+  _swerveDrive->GetConfig().gyro->Reset();
+
+  m_autoSelected = m_chooser.GetSelected();
+
+  if (m_autoSelected == "kTaxi") {
+    sched->Schedule(autos::Taxi(robotmap._builder));
+  }
 }
 
-void Robot::AutonomousPeriodic() {}
+void Robot::AutonomousPeriodic() {
+  robotmap._simSwerve->OnTick(_swerveDrive->GetSetpoint());
+  _swerveDrive->MakeAtSetPoint();
+}
 
 
 void Robot::TeleopInit() {
@@ -157,7 +178,7 @@ void Robot::TeleopPeriodic() {
       armServo->SetAngle(0);
       climberTimer.Stop();
       climberTimer.Reset();
-    } 
+    }
   }
 
   robotmap.swerveTable.swerveDriveTable->GetEntry("SERVO POS").SetDouble(armServo->GetAngle());
diff --git a/src/main/cpp/vision/Vision.cpp b/src/main/cpp/vision/Vision.cpp
new file mode 100644
index 00000000..284e16e5
--- /dev/null
+++ b/src/main/cpp/vision/Vision.cpp
@@ -0,0 +1,419 @@
+// Copyright (c) 2023-2024 CurtinFRC
+// Open Source Software, you can modify it according to the terms
+// of the MIT License at the root of this project
+
+#include "vision/Vision.h"
+
+#include <utility>
+#include <variant>
+
+#include "units/length.h"
+#include "units/math.h"
+#include "vision/Limelight.h"
+
+FMAP::FMAP(std::string path) : _path(path) {
+  std::cout << "Parsing FMAP" << std::endl;
+
+  deploy_dir = frc::filesystem::GetDeployDirectory();
+
+  std::ifstream file(deploy_dir + "/" + _path);
+
+  // parse the json file into a wpi::json object
+
+  wpi::json j;
+
+  file >> j;
+
+  std::cout << j["fiducials"] << std::endl;
+
+  // iterate through the json object and add each tag to the vector
+
+  for (auto& fiducial : j["fiducials"]) {
+    std::cout << "Loading AprilTag " << fiducial["id"] << std::endl;
+    AprilTag tag;
+    tag.id = fiducial["id"];
+    tag.size = fiducial["size"];
+    if (fiducial["unique"] == 1) {
+      tag.unique = true;
+    } else {
+      tag.unique = false;
+    }
+
+    const auto& transform = fiducial["transform"];
+    for (int i = 0; i < 4; ++i) {
+      for (int j = 0; j < 4; ++j) {
+        tag.transform[i][j] = transform[i * 4 + j];
+      }
+    }
+
+    tag.yaw = units::radian_t{tag.transform[0][0]};
+    tag.pitch = units::radian_t{tag.transform[1][1]};
+    tag.roll = units::radian_t{tag.transform[2][2]};
+
+    tag.pos = frc::Pose3d(units::meter_t{tag.transform[0][3]}, units::meter_t{tag.transform[1][3]},
+                          units::meter_t{tag.transform[2][3]}, frc::Rotation3d(0_deg, 0_deg, 0_deg));
+
+    _tags.push_back(tag);
+  }
+
+  file.close();
+
+  std::cout << "Loaded " << _tags.size() << " tags" << std::endl;
+
+  for (int i = 0; i < _tags.size(); i++) {
+    std::cout << "Tag " << _tags[i].id << " is at: X: " << _tags[i].pos.X().value()
+              << " Y: " << _tags[i].pos.Y().value() << " Z: " << _tags[i].pos.Z().value() << std::endl;
+  }
+
+  std::cout << "Loaded FMAP" << std::endl;
+}
+
+std::vector<AprilTag> FMAP::GetTags() {
+  return _tags;
+}
+
+Vision::Vision(std::string name, FMAP fmap) : _fmap(fmap) {
+  _limelight = new wom::Limelight(name);
+
+  _limelight->SetPipeline(wom::LimelightPipeline::kPipeline0);
+}
+
+frc::Pose3d Vision::GetPose() {
+  return _limelight->GetPose();
+}
+
+std::pair<units::meter_t, units::degree_t> Vision::GetDistanceToTarget(VisionTarget target) {
+  SetMode(VisionModes::kAprilTag);
+
+  std::vector<AprilTag> tags = _fmap.GetTags();
+
+  for (int i = 0; i < tags.size(); i++) {
+    if (tags[i].id == static_cast<int>(target)) {
+      AprilTag tag = tags[i];
+      // frc::Pose3d pose = _limelight->GetPose();
+      // frc::Pose3d pose = frc::Pose3d(0_m, 0_m, 0_m, frc::Rotation3d(0_deg, 0_deg, 0_deg));
+
+      SetMode(VisionModes::kAprilTag);
+
+      // Get distance to target
+      // Get current position from Limelight
+      frc::Pose2d current_pose = _limelight->GetPose().ToPose2d();
+      // frc::Pose2d current_pose = frc::Pose2d(0_m, 0_m, 0_deg);
+
+      units::meter_t a = tag.pos.X() - current_pose.X();
+      units::meter_t b = tag.pos.Y() - current_pose.Y();
+
+      units::radian_t theta = units::math::atan2(b, a);
+
+      // std::cout << "A: " << a.value() << ", B:" << b.value() << std::endl;
+      units::meter_t r = units::meter_t{std::sqrt(std::pow(a.value(), 2) + std::pow(b.value(), 2))};
+
+      // units::meter_t x = current_pose.X() + r * units::math::cos(theta);
+      // units::meter_t y = current_pose.Y() + r * units::math::sin(theta);
+
+      return std::make_pair(r, theta);
+    }
+  }
+
+  return std::make_pair(0_m, 0_deg);
+}
+
+std::pair<units::meter_t, units::degree_t> Vision::GetDistanceToTarget(int id) {
+  if (id < APRILTAGS_MIN || id > APRILTAGS_MAX) {
+    return std::make_pair(0_m, 0_deg);
+  }
+
+  SetMode(VisionModes::kAprilTag);
+
+  std::vector<AprilTag> tags = _fmap.GetTags();
+
+  for (int i = 0; i < tags.size(); i++) {
+    if (tags[i].id == id) {
+      AprilTag tag = tags[i];
+      // frc::Pose3d pose = _limelight->GetPose();
+      // frc::Pose3d pose = frc::Pose3d(0_m, 0_m, 0_m, frc::Rotation3d(0_deg, 0_deg, 0_deg));
+
+      SetMode(VisionModes::kAprilTag);
+
+      // Get distance to target
+      // Get current position from Limelight
+      frc::Pose2d current_pose = _limelight->GetPose().ToPose2d();
+      // frc::Pose2d current_pose = frc::Pose2d(0_m, 0_m, 0_deg);
+
+      units::meter_t a = tag.pos.X() - current_pose.X();
+      units::meter_t b = tag.pos.Y() - current_pose.Y();
+
+      units::radian_t theta = units::math::atan2(b, a);
+
+      // std::cout << "A: " << a.value() << ", B:" << b.value() << std::endl;
+      units::meter_t r = units::meter_t{std::sqrt(std::pow(a.value(), 2) + std::pow(b.value(), 2))};
+
+      units::meter_t x = current_pose.X() + r * units::math::cos(theta);
+      units::meter_t y = current_pose.Y() + r * units::math::sin(theta);
+
+      return std::make_pair(r, theta);
+    }
+  }
+
+  return std::make_pair(0_m, 0_deg);
+}
+
+std::vector<AprilTag> Vision::GetTags() {
+  return _fmap.GetTags();
+}
+
+frc::Pose2d Vision::AlignToTarget(VisionTarget target, units::meter_t offset, wom::SwerveDrive* swerveDrive) {
+  SetMode(VisionModes::kAprilTag);
+
+  // Get distance to target
+  AprilTag tag = GetTags()[static_cast<int>(target) - 1];
+  // Get current position from Limelight
+  frc::Pose2d current_pose = _limelight->GetPose().ToPose2d();
+  // frc::Pose2d current_pose = frc::Pose2d(0_m, 0_m, 0_deg);
+
+  units::meter_t a = tag.pos.X() - current_pose.X();
+  units::meter_t b = tag.pos.Y() - current_pose.Y();
+
+  units::radian_t theta = units::math::atan2(b, a);
+
+  // std::cout << "A: " << a.value() << ", B:" << b.value() << std::endl;
+  units::meter_t r = units::meter_t{std::sqrt(std::pow(a.value(), 2) + std::pow(b.value(), 2))};
+
+  units::meter_t x = current_pose.X() + r * units::math::cos(theta);
+  units::meter_t y = current_pose.Y() + r * units::math::sin(theta);
+
+  // reduce both offsets by the offset parameter (in relative amount)
+  x += offset * units::math::cos(theta);
+  y += offset * units::math::sin(theta);
+
+  frc::Pose2d pose = frc::Pose2d(x, y, theta);
+
+  // Print the results
+  // std::cout << "Aligning to tag " << target << ": X: " << pose.X().value() << " Y: " << pose.Y().value() <<
+  // " Angle: " << pose.Rotation().Degrees().value() << ". At offset " << offset.value() << "." << std::endl;
+
+  // Set the new pose to the swerve drive
+  // swerveDrive->SetPose(pose);
+
+  return pose;
+}
+
+frc::Pose2d Vision::AlignToTarget(int target, units::meter_t offset, wom::SwerveDrive* swerveDrive) {
+  SetMode(VisionModes::kAprilTag);
+
+  // Get distance to target
+  std::pair<units::meter_t, units::degree_t> distance = GetDistanceToTarget(target);
+  AprilTag tag = GetTags()[target - 1];
+  // Get current position from Limelight
+  frc::Pose2d current_pose = _limelight->GetPose().ToPose2d();
+  // frc::Pose2d current_pose = frc::Pose2d(0_m, 0_m, 0_deg);
+
+  units::meter_t a = tag.pos.X() - current_pose.X();
+  units::meter_t b = tag.pos.Y() - current_pose.Y();
+
+  units::radian_t theta = units::math::atan2(b, a);
+
+  // std::cout << "A: " << a.value() << ", B:" << b.value() << std::endl;
+  units::meter_t r = units::meter_t{std::sqrt(std::pow(a.value(), 2) + std::pow(b.value(), 2))};
+
+  units::meter_t x = current_pose.X() + r * units::math::cos(theta);
+  units::meter_t y = current_pose.Y() + r * units::math::sin(theta);
+
+  // reduce both offsets by the offset parameter (in relative amount)
+  x += offset * units::math::cos(theta);
+  y += offset * units::math::sin(theta);
+
+  frc::Pose2d pose = frc::Pose2d(x, y, theta);
+
+  // Print the results
+  // std::cout << "Aligning to tag " << target << ": X: " << pose.X().value() << " Y: " << pose.Y().value() <<
+  // " Angle: " << pose.Rotation().Degrees().value() << ". At offset " << offset.value() << "." << std::endl;
+
+  // Set the new pose to the swerve drive
+  // swerveDrive->SetPose(pose);
+
+  return pose;
+}
+
+frc::Pose2d Vision::AlignToTarget(VisionTarget target, frc::Translation2d offset,
+                                  wom::SwerveDrive* swerveDrive) {
+  SetMode(VisionModes::kAprilTag);
+
+  // Get distance to target
+  AprilTag tag = GetTags()[static_cast<int>(target) - 1];
+  // Get current position from Limelight
+  frc::Pose2d current_pose = _limelight->GetPose().ToPose2d();
+  // frc::Pose2d current_pose = frc::Pose2d(0_m, 0_m, 0_deg);
+
+  units::meter_t a = tag.pos.X() - current_pose.X();
+  units::meter_t b = tag.pos.Y() - current_pose.Y();
+
+  units::radian_t theta = units::math::atan2(b, a);
+
+  // std::cout << "A: " << a.value() << ", B:" << b.value() << std::endl;
+  units::meter_t r = units::meter_t{std::sqrt(std::pow(a.value(), 2) + std::pow(b.value(), 2))};
+
+  units::meter_t x = current_pose.X() + r * units::math::cos(theta);
+  units::meter_t y = current_pose.Y() + r * units::math::sin(theta);
+
+  // reduce both offsets by the offset parameter (in relative amount)
+  x += offset.X();
+  y += offset.Y();
+
+  frc::Pose2d pose = frc::Pose2d(x, y, theta);
+
+  // Print the results
+  // std::cout << "Aligning to tag " << target << ": X: " << pose.X().value() << " Y: " << pose.Y().value() <<
+  // " Angle: " << pose.Rotation().Degrees().value() << ". At offset " << offset.value() << "." << std::endl;
+
+  // Set the new pose to the swerve drive
+  // swerveDrive->SetPose(pose);
+
+  return pose;
+}
+
+frc::Pose2d Vision::AlignToTarget(int target, frc::Translation2d offset, wom::SwerveDrive* swerveDrive) {
+  SetMode(VisionModes::kAprilTag);
+
+  // Get distance to target
+  std::pair<units::meter_t, units::degree_t> distance = GetDistanceToTarget(target);
+  AprilTag tag = GetTags()[target - 1];
+  // Get current position from Limelight
+  frc::Pose2d current_pose = _limelight->GetPose().ToPose2d();
+  // frc::Pose2d current_pose = frc::Pose2d(0_m, 0_m, 0_deg);
+
+  units::meter_t a = tag.pos.X() - current_pose.X();
+  units::meter_t b = tag.pos.Y() - current_pose.Y();
+
+  units::radian_t theta = units::math::atan2(b, a);
+
+  // std::cout << "A: " << a.value() << ", B:" << b.value() << std::endl;
+  units::meter_t r = units::meter_t{std::sqrt(std::pow(a.value(), 2) + std::pow(b.value(), 2))};
+
+  units::meter_t x = current_pose.X() + r * units::math::cos(theta);
+  units::meter_t y = current_pose.Y() + r * units::math::sin(theta);
+
+  // reduce both offsets by the offset parameter (in relative amount)
+  x += offset.X();
+  y += offset.Y();
+
+  frc::Pose2d pose = frc::Pose2d(x, y, theta);
+
+  // Print the results
+  // std::cout << "Aligning to tag " << target << ": X: " << pose.X().value() << " Y: " << pose.Y().value() <<
+  // " Angle: " << pose.Rotation().Degrees().value() << ". At offset " << offset.value() << "." << std::endl;
+
+  // Set the new pose to the swerve drive
+  // swerveDrive->SetPose(pose);
+
+  return pose;
+}
+
+frc::Pose2d Vision::TurnToTarget(int target, wom::SwerveDrive* swerveDrive) {
+  AprilTag tag = GetTags()[target];
+
+  units::degree_t angle = GetDistanceToTarget(target).second;
+
+  frc::Pose2d current_pose = _limelight->GetPose().ToPose2d();
+  // frc::Pose2d current_pose = frc::Pose2d();
+
+  frc::Pose2d pose = frc::Pose2d(current_pose.X(), current_pose.Y(), angle);
+
+  // std::cout << pose.Rotation().Degrees().value() << std::endl;
+
+  // swerveDrive->SetPose(pose);
+
+  return pose;
+}
+
+frc::Pose2d Vision::TurnToTarget(VisionTarget target, wom::SwerveDrive* swerveDrive) {
+  AprilTag tag = GetTags()[static_cast<int>(target)];
+
+  units::degree_t angle = GetDistanceToTarget(target).second;
+
+  // frc::Pose2d current_pose = _limelight->GetPose().ToPose2d();
+
+  frc::Pose2d current_pose = swerveDrive->GetPose();
+
+  frc::Pose2d pose = frc::Pose2d(current_pose.X(), current_pose.Y(), angle);
+
+  std::cout << pose.Rotation().Degrees().value() << std::endl;
+
+  swerveDrive->SetPose(pose);
+}
+
+std::pair<frc::Pose2d, units::degree_t> Vision::GetAngleToObject(VisionTargetObjects object) {
+  SetMode(VisionModes::kRing);
+
+  if (TargetIsVisible(object)) {
+    frc::Pose2d pose = _limelight->GetPose().ToPose2d();
+
+    units::degree_t offset = units::degree_t{
+        _limelight->GetOffset()
+            .first};  // degrees are how much the robot has to turn for the target to be in the center
+
+    pose.RotateBy(offset);
+
+    return std::make_pair(pose, offset);
+  } else {
+    return std::make_pair(frc::Pose2d(), 0_deg);
+  }
+}
+
+units::degree_t Vision::LockOn(VisionTargetObjects object) {
+  SetMode(VisionModes::kRing);
+
+  units::degree_t angle;
+
+  if (TargetIsVisible(object)) {
+    angle = GetAngleToObject(object).second;
+  }
+
+  return angle;
+}
+
+bool Vision::IsAtPose(frc::Pose3d pose, units::second_t dt) {
+  return _limelight->IsAtSetPoseVision(pose, dt);
+}
+
+bool Vision::IsAtPose(frc::Pose2d pose, units::second_t dt) {
+  return _limelight->IsAtSetPoseVision(pose, dt);
+}
+
+void Vision::SetMode(VisionModes mode) {
+  if (static_cast<int>(mode) == _limelight->GetTargetingData(wom::LimelightTargetingData::kGetpipe)) {
+    return;
+  }
+
+  switch (mode) {
+    case VisionModes::kAprilTag: {
+      _limelight->SetPipeline(wom::LimelightPipeline::kPipeline0);
+      break;
+    }
+    case VisionModes::kRing: {
+      _limelight->SetPipeline(wom::LimelightPipeline::kPipeline1);
+      break;
+    }
+  }
+}
+
+bool Vision::TargetIsVisible(VisionTargetObjects target) {
+  switch (target) {
+    case VisionTargetObjects::kNote: {
+      SetMode(VisionModes::kRing);
+      break;
+    }
+  }
+
+  return _limelight->HasTarget();
+}
+
+int Vision::CurrentAprilTag() {
+  SetMode(VisionModes::kAprilTag);
+
+  return _limelight->GetTargetingData(wom::LimelightTargetingData::kTid);
+}
+
+wom::Limelight* Vision::GetLimelight() {
+  return _limelight;
+}
diff --git a/src/main/cpp/vision/VisionBehaviours.cpp b/src/main/cpp/vision/VisionBehaviours.cpp
new file mode 100644
index 00000000..03bbadf1
--- /dev/null
+++ b/src/main/cpp/vision/VisionBehaviours.cpp
@@ -0,0 +1,63 @@
+// Copyright (c) 2023-2024 CurtinFRC
+// Open Source Software, you can modify it according to the terms
+// of the MIT License at the root of this project
+
+#include "vision/VisionBehaviours.h"
+
+#include "frc/geometry/Pose2d.h"
+#include "units/angle.h"
+#include "vision/Vision.h"
+
+AlignToAprilTag::AlignToAprilTag(Vision* vision, VisionTarget target, wom::SwerveDrive* swerveDrive,
+                                 units::meter_t offset)
+    : _vision(vision), _swerveDrive(swerveDrive), _target(target), _offset(offset) {}
+
+void AlignToAprilTag::OnTick(units::second_t dt) {
+  frc::Pose2d pose = _vision->AlignToTarget(_target, _offset, _swerveDrive);
+
+  if (_vision->IsAtPose(pose, dt)) {
+    SetDone();
+  }
+}
+
+TurnToAprilTag::TurnToAprilTag(Vision* vision, VisionTarget target, wom::SwerveDrive* swerveDrive)
+    : _vision(vision), _swerveDrive(swerveDrive), _target(target) {}
+
+void TurnToAprilTag::OnTick(units::second_t dt) {
+  frc::Pose2d pose = _vision->TurnToTarget(_target, _swerveDrive);
+
+  if (_vision->IsAtPose(pose, dt)) {
+    SetDone();
+  }
+}
+
+LockOnToTarget::LockOnToTarget(Vision* vision, VisionTargetObjects target, wom::SwerveDrive* swerveDrive)
+    : _vision(vision),
+      _target(target),
+      _camera(nullptr),
+      _swerveDrive(swerveDrive),
+      _type(VisionType::kLimelight) {}
+
+LockOnToTarget::LockOnToTarget(wom::PhotonVision* vision, Vision* limelight, wom::SwerveDrive* swerveDrive)
+    : _camera(vision), _swerveDrive(swerveDrive), _vision(limelight), _type(VisionType::kGenericCamera) {}
+
+void LockOnToTarget::OnTick(units::second_t dt) {
+  units::degree_t angle;
+
+  switch (_type) {
+    case VisionType::kLimelight: {
+      angle = _vision->LockOn(_target);
+    }
+    case VisionType::kGenericCamera: {
+      angle = units::degree_t{_camera->GetTargetPitch(_camera->GetTarget())};
+    }
+  }
+
+  frc::Pose2d pose = frc::Pose2d(_swerveDrive->GetPose().X(), _swerveDrive->GetPose().Y(), angle);
+
+  _swerveDrive->SetPose(pose);
+
+  if (_vision->IsAtPose(pose, dt)) {
+    SetDone();
+  }
+}
diff --git a/src/main/deploy/pathplanner/autos/New Auto.auto b/src/main/deploy/pathplanner/autos/New Auto.auto
deleted file mode 100644
index f023c012..00000000
--- a/src/main/deploy/pathplanner/autos/New Auto.auto	
+++ /dev/null
@@ -1,38 +0,0 @@
-{
-  "version": 1.0,
-  "startingPose": {
-    "position": {
-      "x": 2,
-      "y": 2
-    },
-    "rotation": 0
-  },
-  "command": {
-    "type": "sequential",
-    "data": {
-      "commands": [
-        {
-          "type": "path",
-          "data": {
-            "pathName": "Example Path"
-          }
-        },
-        {
-          "type": "sequential",
-          "data": {
-            "commands": [
-              {
-                "type": "named",
-                "data": {
-                  "name": "hh"
-                }
-              }
-            ]
-          }
-        }
-      ]
-    }
-  },
-  "folder": null,
-  "choreoAuto": false
-}
diff --git a/src/main/deploy/pathplanner/autos/Taxi.auto b/src/main/deploy/pathplanner/autos/Taxi.auto
new file mode 100644
index 00000000..e0fd7b37
--- /dev/null
+++ b/src/main/deploy/pathplanner/autos/Taxi.auto
@@ -0,0 +1,25 @@
+{
+  "version": 1.0,
+  "startingPose": {
+    "position": {
+      "x": 0.0,
+      "y": 0.0
+    },
+    "rotation": 0.0
+  },
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "FirstNote"
+          }
+        }
+      ]
+    }
+  },
+  "folder": null,
+  "choreoAuto": false
+}
diff --git a/src/main/deploy/pathplanner/paths/Example Path.path b/src/main/deploy/pathplanner/paths/FirstNote.path
similarity index 54%
rename from src/main/deploy/pathplanner/paths/Example Path.path
rename to src/main/deploy/pathplanner/paths/FirstNote.path
index de5528c0..c13b43fa 100644
--- a/src/main/deploy/pathplanner/paths/Example Path.path	
+++ b/src/main/deploy/pathplanner/paths/FirstNote.path
@@ -3,41 +3,25 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 1.7121880748754352,
-        "y": 5.5040931765173085
+        "x": 0.0,
+        "y": 0.0
       },
       "prevControl": null,
       "nextControl": {
-        "x": 2.7121880748754363,
-        "y": 5.5040931765173085
+        "x": 0.9978270455827627,
+        "y": 0.04684334618312441
       },
       "isLocked": false,
       "linkedName": null
     },
     {
       "anchor": {
-        "x": 5.108330673152073,
-        "y": 7.3426945142050055
+        "x": 0.1,
+        "y": 0.0
       },
       "prevControl": {
-        "x": 4.108330673152073,
-        "y": 7.3426945142050055
-      },
-      "nextControl": {
-        "x": 6.108330673152073,
-        "y": 7.3426945142050055
-      },
-      "isLocked": false,
-      "linkedName": null
-    },
-    {
-      "anchor": {
-        "x": 5.471366606071299,
-        "y": 2.8223116075333428
-      },
-      "prevControl": {
-        "x": 6.832113092186237,
-        "y": 3.284889651091713
+        "x": -0.04910272980275429,
+        "y": -0.0322153133318448
       },
       "nextControl": null,
       "isLocked": false,
@@ -55,13 +39,13 @@
   },
   "goalEndState": {
     "velocity": 0,
-    "rotation": 0,
+    "rotation": 2.770215797200185,
     "rotateFast": false
   },
   "reversed": false,
   "folder": null,
   "previewStartingState": {
-    "rotation": 0,
+    "rotation": 1.1884043840473262,
     "velocity": 0
   },
   "useDefaultConstraints": true
diff --git a/src/main/include/Auto.h b/src/main/include/Auto.h
new file mode 100644
index 00000000..7e9f8df5
--- /dev/null
+++ b/src/main/include/Auto.h
@@ -0,0 +1,41 @@
+// Copyright (c) 2023-2024 CurtinFRC
+// Open Source Software, you can modify it according to the terms
+// of the MIT License at the root of this project
+
+#pragma once
+
+#include <memory>
+
+#include "AlphaArmBehaviour.h"
+#include "IntakeBehaviour.h"
+#include "ShooterBehaviour.h"
+#include "Wombat.h"
+#include "utils/Pathplanner.h"
+
+namespace autos {
+
+// wom::Commands* _commands = nullptr;
+// wom::SwerveAutoBuilder* builder = nullptr;
+
+wom::utils::SwerveAutoBuilder* InitCommands(wom::drivetrain::SwerveDrive* _swerveDrive, Shooter* _shooter,
+                                            Intake* _intake, AlphaArm* _alphaArm);
+
+std::shared_ptr<behaviour::Behaviour> AutoTest(wom::drivetrain::SwerveDrive* _swerveDrive, Shooter* _shooter,
+                                               Intake* _intake, AlphaArm* _alphaArm);
+
+std::shared_ptr<behaviour::Behaviour> Taxi(wom::utils::SwerveAutoBuilder* builder);
+
+std::shared_ptr<behaviour::Behaviour> QuadrupleClose(wom::drivetrain::SwerveDrive* _swerveDrive,
+                                                     Shooter* _shooter, Intake* _intake, AlphaArm* _alphaArm);
+
+std::shared_ptr<behaviour::Behaviour> QuadrupleFar(wom::drivetrain::SwerveDrive* _swerveDrive,
+                                                   Shooter* _shooter, Intake* _intake, AlphaArm* _alphaArm);
+
+std::shared_ptr<behaviour::Behaviour> QuadrupleCloseDoubleFar(wom::drivetrain::SwerveDrive* _swerveDrive,
+                                                              Shooter* _shooter, Intake* _intake,
+                                                              AlphaArm* _alphaArm);
+
+std::shared_ptr<behaviour::Behaviour> QuadrupleCloseSingleFar(wom::drivetrain::SwerveDrive* _swerveDrive,
+                                                              Shooter* _shooter, Intake* _intake,
+                                                              AlphaArm* _alphaArm);
+}  // namespace autos
diff --git a/src/main/include/Robot.h b/src/main/include/Robot.h
index 8d6b68d4..c0448245 100644
--- a/src/main/include/Robot.h
+++ b/src/main/include/Robot.h
@@ -61,6 +61,18 @@ class Robot : public frc::TimedRobot {
 
   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,
+  };
 
   // frc::Field2d m_field;
 
diff --git a/src/main/include/RobotMap.h b/src/main/include/RobotMap.h
index 7603d0c8..e03c89a7 100644
--- a/src/main/include/RobotMap.h
+++ b/src/main/include/RobotMap.h
@@ -237,5 +237,8 @@ struct RobotMap {
         climberGearbox,
     };
   }; ClimberSystem climberSystem;
+
+  wom::SwerveAutoBuilder* _builder;
+  wom::SimSwerve* _simSwerve;
 };
   // AlphaArmSystem alphaArmSystem;
diff --git a/src/main/include/vision/Vision.h b/src/main/include/vision/Vision.h
new file mode 100644
index 00000000..731bca21
--- /dev/null
+++ b/src/main/include/vision/Vision.h
@@ -0,0 +1,115 @@
+// Copyright (c) 2023-2024 CurtinFRC
+// Open Source Software, you can modify it according to the terms
+// of the MIT License at the root of this project
+
+#pragma once
+
+#include <frc/Filesystem.h>
+#include <units/length.h>
+#include <units/math.h>
+#include <wpi/fs.h>
+#include <wpi/json.h>
+
+#include <string>
+#include <utility>
+#include <vector>
+
+#include "Wombat.h"
+#include "units/angle.h"
+
+#define APRILTAGS_MAX 16
+#define APRILTAGS_MIN 0
+
+enum class VisionTarget {
+  /*
+      Left is toward the blue side of the diagram here:
+     https://firstfrc.blob.core.windows.net/frc2024/FieldAssets/2024LayoutMarkingDiagram.pdf
+
+      The numbers are the numbers on the field diagram (and the numbers on the tags).
+  */
+  kBlueAmp = 6,
+  kBlueSpeakerCenter = 7,
+  kBlueSpeakerOffset = 8,
+  kBlueChain1 = 15,
+  kBlueChain2 = 16,
+  kBlueChain3 = 14,
+  kBlueSourceLeft = 1,
+  kBlueSourceRight = 2,
+
+  kRedAmp = 5,
+  kRedSpeakerCenter = 4,
+  kRedSpeakerOffset = 3,
+
+  kRedChain1 = 12,
+  kRedChain2 = 11,
+  kRedChain3 = 13,
+  kRedSourceLeft = 9,
+  kRedSourceRight = 10
+};
+
+enum class VisionModes { kAprilTag = 0, kRing = 1 };
+
+enum class VisionTargetObjects { kNote };
+
+struct AprilTag {
+  int id;
+  double size;
+  std::array<std::array<double, 4>, 4> transform;
+  bool unique;
+
+  frc::Pose3d pos;
+  units::radian_t yaw;
+  units::radian_t pitch;
+  units::radian_t roll;
+};
+
+class FMAP {
+ public:
+  explicit FMAP(std::string path);
+
+  std::vector<AprilTag> GetTags();
+
+ private:
+  std::vector<AprilTag> _tags;
+  std::string _path;
+  std::string deploy_dir;
+};
+
+class Vision {
+ public:
+  Vision(std::string name, FMAP fmap);
+
+  std::pair<units::meter_t, units::degree_t> GetDistanceToTarget(VisionTarget target);
+  std::pair<units::meter_t, units::degree_t> GetDistanceToTarget(int id);
+
+  void SetMode(VisionModes mode);
+
+  frc::Pose3d GetPose();
+
+  frc::Pose2d AlignToTarget(VisionTarget target, units::meter_t offset, wom::SwerveDrive* swerveDrive);
+  frc::Pose2d AlignToTarget(int target, units::meter_t offset, wom::SwerveDrive* swerveDrive);
+
+  frc::Pose2d AlignToTarget(VisionTarget target, frc::Translation2d offset, wom::SwerveDrive* swerveDrive);
+  frc::Pose2d AlignToTarget(int target, frc::Translation2d offset, wom::SwerveDrive* swerveDrive);
+
+  frc::Pose2d TurnToTarget(int target, wom::SwerveDrive* swerveDrive);
+  frc::Pose2d TurnToTarget(VisionTarget target, wom::SwerveDrive* swerveDrive);
+
+  std::pair<frc::Pose2d, units::degree_t> GetAngleToObject(VisionTargetObjects object);
+  units::degree_t LockOn(VisionTargetObjects target);
+
+  std::vector<AprilTag> GetTags();
+
+  bool IsAtPose(frc::Pose3d pose, units::second_t dt);
+  bool IsAtPose(frc::Pose2d pose, units::second_t dt);
+
+  bool TargetIsVisible(VisionTargetObjects target);
+
+  int CurrentAprilTag();
+
+  wom::Limelight* GetLimelight();
+
+ private:
+  wom::Limelight* _limelight;
+  FMAP _fmap;
+};
diff --git a/src/main/include/vision/VisionBehaviours.h b/src/main/include/vision/VisionBehaviours.h
new file mode 100644
index 00000000..c10a092d
--- /dev/null
+++ b/src/main/include/vision/VisionBehaviours.h
@@ -0,0 +1,56 @@
+// Copyright (c) 2023-2024 CurtinFRC
+// Open Source Software, you can modify it according to the terms
+// of the MIT License at the root of this project
+
+#pragma once
+
+#include "Wombat.h"
+#include "behaviour/HasBehaviour.h"
+#include "drivetrain/SwerveDrive.h"
+#include "units/length.h"
+#include "units/time.h"
+#include "vision/Vision.h"
+
+enum class VisionType { kLimelight = 0, kGenericCamera = 1 };
+
+class AlignToAprilTag : public behaviour::Behaviour {
+ public:
+  explicit AlignToAprilTag(Vision* vision, VisionTarget target, wom::SwerveDrive* swerveDrive,
+                           units::meter_t offset = 0_m);
+
+  void OnTick(units::second_t dt);
+
+ private:
+  Vision* _vision;
+  wom::SwerveDrive* _swerveDrive;
+  VisionTarget _target;
+  units::meter_t _offset;
+};
+
+class TurnToAprilTag : public behaviour::Behaviour {
+ public:
+  explicit TurnToAprilTag(Vision* vision, VisionTarget target, wom::SwerveDrive* swerveDrive);
+
+  void OnTick(units::second_t dt);
+
+ private:
+  Vision* _vision;
+  wom::SwerveDrive* _swerveDrive;
+  VisionTarget _target;
+};
+
+class LockOnToTarget : public behaviour::Behaviour {
+ public:
+  explicit LockOnToTarget(Vision* vision, VisionTargetObjects target, wom::SwerveDrive* swerveDrive);
+  explicit LockOnToTarget(wom::PhotonVision* camera, Vision* limelight, wom::SwerveDrive* swerveDrive);
+
+  void OnTick(units::second_t dt);
+
+ private:
+  Vision* _vision;
+  wom::PhotonVision* _camera;
+
+  VisionTargetObjects _target;
+  wom::SwerveDrive* _swerveDrive;
+  VisionType _type;
+};
diff --git a/vendordeps/WPILib-New-Commands.json b/vendordeps/WPILibNewCommands.json
similarity index 100%
rename from vendordeps/WPILib-New-Commands.json
rename to vendordeps/WPILibNewCommands.json
diff --git a/wombat/src/main/cpp/drivetrain/SwerveDrive.cpp b/wombat/src/main/cpp/drivetrain/SwerveDrive.cpp
index 9fbdc6a4..74f7b9c9 100644
--- a/wombat/src/main/cpp/drivetrain/SwerveDrive.cpp
+++ b/wombat/src/main/cpp/drivetrain/SwerveDrive.cpp
@@ -698,5 +698,15 @@ frc::Pose2d SwerveDrive::GetPose() {
 void SwerveDrive::AddVisionMeasurement(frc::Pose2d pose, units::second_t timestamp) {
   _poseEstimator.AddVisionMeasurement(pose, timestamp);
 }
+
+frc::Pose2d SwerveDrive::GetSetpoint() {
+  return frc::Pose2d(units::meter_t{_xPIDController.GetSetpoint()},
+                     units::meter_t{_yPIDController.GetSetpoint()},
+                     units::radian_t{_anglePIDController.GetSetpoint()});
+}
+
+void SwerveDrive::MakeAtSetPoint() {
+  ResetPose(GetSetpoint());
+}
 }  // namespace drivetrain
-}  // namespace wom
\ No newline at end of file
+}  // namespace wom
diff --git a/wombat/src/main/cpp/drivetrain/behaviours/SwerveBehaviours.cpp b/wombat/src/main/cpp/drivetrain/behaviours/SwerveBehaviours.cpp
index fd9a91be..6dd29443 100644
--- a/wombat/src/main/cpp/drivetrain/behaviours/SwerveBehaviours.cpp
+++ b/wombat/src/main/cpp/drivetrain/behaviours/SwerveBehaviours.cpp
@@ -72,45 +72,41 @@ void ManualDrivebase::OnTick(units::second_t deltaTime) {
   // if (isZero) {
   //   _swerveDrivebase->SetZeroing();
   // } else {
-    double xVelocity = wom::utils::spow2(-wom::utils::deadzone(
-        _driverController->GetLeftY(),
-        driverDeadzone));  // GetLeftY due to x being where y should be on field
-    double yVelocity = wom::utils::spow2(
-        -wom::utils::deadzone(_driverController->GetLeftX(), driverDeadzone));
-
-    double r_x = wom::utils::spow2(
-        -wom::utils::deadzone(_driverController->GetRightX(), turningDeadzone));
-
-    double turnX = _driverController->GetRightX();
-    double turnY = _driverController->GetRightY();
-    double num = std::sqrt(turnX * turnX + turnY * turnY);
-    if (num < turningDeadzone) {
-      turnX = 0;
-      turnY = 0;
-    }
-
-    // if (isRotateMatch) {
-    //   units::degree_t currentAngle =
-    //       _swerveDrivebase->GetPose().Rotation().Degrees();
-    //   CalculateRequestedAngle(turnX, turnY, currentAngle);
-    //   _swerveDriveTable->GetEntry("RotateMatch")
-    //       .SetDouble(_requestedAngle.value());
-    //   _swerveDrivebase->RotateMatchJoystick(
-    //       _requestedAngle,
-    //       wom::drivetrain::FieldRelativeSpeeds{// also field relative
-    //                                            xVelocity * maxMovementMagnitude,
-    //                                            yVelocity * maxMovementMagnitude,
-    //                                            r_x * maxRotationMagnitude});
-    // } else {
-      _swerveDrivebase->SetFieldRelativeVelocity(
-          wom::drivetrain::FieldRelativeSpeeds{xVelocity * -maxMovementMagnitude,
-                                               yVelocity * -maxMovementMagnitude,
-                                               r_x * -maxRotationMagnitude});
-
-    //  _swerveDrivebase->SetVelocity(
-    //       frc::ChassisSpeeds{xVelocity * maxMovementMagnitude,
-    //                          yVelocity * maxMovementMagnitude,
-    //                          r_x * maxRotationMagnitude});
+  double xVelocity = wom::utils::spow2(
+      -wom::utils::deadzone(_driverController->GetLeftY(),
+                            driverDeadzone));  // GetLeftY due to x being where y should be on field
+  double yVelocity = wom::utils::spow2(-wom::utils::deadzone(_driverController->GetLeftX(), driverDeadzone));
+
+  double r_x = wom::utils::spow2(-wom::utils::deadzone(_driverController->GetRightX(), turningDeadzone));
+
+  double turnX = _driverController->GetRightX();
+  double turnY = _driverController->GetRightY();
+  double num = std::sqrt(turnX * turnX + turnY * turnY);
+  if (num < turningDeadzone) {
+    turnX = 0;
+    turnY = 0;
+  }
+
+  // if (isRotateMatch) {
+  //   units::degree_t currentAngle =
+  //       _swerveDrivebase->GetPose().Rotation().Degrees();
+  //   CalculateRequestedAngle(turnX, turnY, currentAngle);
+  //   _swerveDriveTable->GetEntry("RotateMatch")
+  //       .SetDouble(_requestedAngle.value());
+  //   _swerveDrivebase->RotateMatchJoystick(
+  //       _requestedAngle,
+  //       wom::drivetrain::FieldRelativeSpeeds{// also field relative
+  //                                            xVelocity * maxMovementMagnitude,
+  //                                            yVelocity * maxMovementMagnitude,
+  //                                            r_x * maxRotationMagnitude});
+  // } else {
+  _swerveDrivebase->SetFieldRelativeVelocity(wom::drivetrain::FieldRelativeSpeeds{
+      xVelocity * -maxMovementMagnitude, yVelocity * -maxMovementMagnitude, r_x * -maxRotationMagnitude});
+
+  //  _swerveDrivebase->SetVelocity(
+  //       frc::ChassisSpeeds{xVelocity * maxMovementMagnitude,
+  //                          yVelocity * maxMovementMagnitude,
+  //                          r_x * maxRotationMagnitude});
   //   }
   // }
   // _swerveDrivebase->SetIndividualTuning(2, 0_deg, 0_mps);
@@ -172,69 +168,92 @@ void XDrivebase::OnTick(units::second_t deltaTime) {
 //   m_timer.Start();
 // }
 
-wom::drivetrain::behaviours::TempSimSwerveDrive::TempSimSwerveDrive(
-    frc::Timer* timer, frc::Field2d* field)
-    : m_timer(timer), m_field(field) {}
-
-void wom::drivetrain::behaviours::TempSimSwerveDrive::OnUpdate() {
-  m_field->SetRobotPose(m_driveSim.GetPose());
-
-  // get the current trajectory state
-  frc::Trajectory::State desired_state =
-      current_trajectory.Sample(m_timer->Get());
+// wom::drivetrain::behaviours::TempSimSwerveDrive::TempSimSwerveDrive(frc::Timer* timer, frc::Field2d* field)
+//     : m_timer(timer), m_field(field) {}
+//
+// void wom::drivetrain::behaviours::TempSimSwerveDrive::OnUpdate() {
+//   m_field->SetRobotPose(m_driveSim.GetPose());
+//
+//   // get the current trajectory state
+//   frc::Trajectory::State desired_state = current_trajectory.Sample(m_timer->Get());
+//
+//   // get the current wheel speeds
+//   wom::utils::WriteTrajectoryState(current_trajectory_state_table, desired_state);
+//
+//   // move drivebase position to the desired state
+//   m_driveSim.SetPose(wom::utils::TrajectoryStateToPose2d(desired_state));
+//
+//   // update the drivebase
+//   m_driveSim.Update(20_ms);
+// }
+//
+// frc::Pose3d wom::drivetrain::behaviours::TempSimSwerveDrive::GetPose() {
+//   frc::Pose3d currentPose{m_driveSim.GetPose()};
+//   return currentPose;
+// }
+//
+// frc::Pose2d wom::drivetrain::behaviours::TempSimSwerveDrive::GetPose2d() {
+//   return m_driveSim.GetPose();
+// }
+//
+// void wom::drivetrain::behaviours::TempSimSwerveDrive::SetPath(std::string path) {
+//   nt::NetworkTableInstance inst = nt::NetworkTableInstance::GetDefault();
+//   std::shared_ptr<nt::NetworkTable> table = inst.GetTable("FMSInfo");
+//
+//   // create a netowrk table for the trajectory
+//   std::shared_ptr<nt::NetworkTable> trajectory_table =
+//       nt::NetworkTableInstance::GetDefault().GetTable("trajectory_path");
+//   current_trajectory_table = nt::NetworkTableInstance::GetDefault().GetTable("current_trajectory");
+//   current_trajectory_state_table =
+//       nt::NetworkTableInstance::GetDefault().GetTable("current_trajectory_state");
+//
+//   current_trajectory = m_pathplanner.getTrajectory(path);
+//   m_driveSim.SetPose(current_trajectory.Sample(0_s).pose);
+//   m_timer->Reset();
+//   m_timer->Start();
+// }
+//
+// wom::drivetrain::behaviours::AutoSwerveDrive::AutoSwerveDrive(wom::drivetrain::SwerveDrive* swerve,
+//                                                               frc::Timer* timer, frc::Field2d* field)
+//     : _swerve(swerve), m_timer(timer), m_field(field) {
+//   _simSwerveDrive = new wom::drivetrain::behaviours::TempSimSwerveDrive(timer, field);
+// }
+//
+// void wom::drivetrain::behaviours::AutoSwerveDrive::OnUpdate() {
+//   _simSwerveDrive->OnUpdate();
+//   _swerve->SetPose(_simSwerveDrive->GetPose2d());
+// }
+//
+// void wom::drivetrain::behaviours::AutoSwerveDrive::SetPath(std::string path) {
+//   _simSwerveDrive->SetPath(path);
+// }
 
-  // get the current wheel speeds
-  wom::utils::WriteTrajectoryState(current_trajectory_state_table,
-                                   desired_state);
+wom::drivetrain::behaviours::DrivebasePoseBehaviour::DrivebasePoseBehaviour(SwerveDrive* swerveDrivebase,
+                                                                            frc::Pose2d pose,
+                                                                            units::volt_t voltageLimit,
+                                                                            bool hold)
+    : _swerveDrivebase(swerveDrivebase), _pose(pose), _hold(hold), _voltageLimit(voltageLimit) {
+  Controls(swerveDrivebase);
+}
 
-  // move drivebase position to the desired state
-  m_driveSim.SetPose(wom::utils::TrajectoryStateToPose2d(desired_state));
+// used in autonomous for going to set drive poses
+void wom::drivetrain::behaviours::DrivebasePoseBehaviour::OnTick(units::second_t deltaTime) {
+  if (_voltageLimit >= (frc::RobotController::GetBatteryVoltage() - 0.5_V)) {
+    _voltageLimit = frc::RobotController::GetBatteryVoltage() - 1_V;
+  }
 
-  // update the drivebase
-  m_driveSim.Update(20_ms);
-}
+  double currentAngle = _swerveDrivebase->GetPose().Rotation().Degrees().value();
 
-frc::Pose3d wom::drivetrain::behaviours::TempSimSwerveDrive::GetPose() {
-  frc::Pose3d currentPose{m_driveSim.GetPose()};
-  return currentPose;
-}
+  units::degree_t adjustedAngle =
+      1_deg * (currentAngle - std::fmod(currentAngle, 360) + _pose.Rotation().Degrees().value());
 
-frc::Pose2d wom::drivetrain::behaviours::TempSimSwerveDrive::GetPose2d() {
-  return m_driveSim.GetPose();
-}
+  _swerveDrivebase->SetVoltageLimit(_voltageLimit);
 
-void wom::drivetrain::behaviours::TempSimSwerveDrive::SetPath(std::string path) {
-  nt::NetworkTableInstance inst = nt::NetworkTableInstance::GetDefault();
-  std::shared_ptr<nt::NetworkTable> table = inst.GetTable("FMSInfo");
-
-  // create a netowrk table for the trajectory
-  std::shared_ptr<nt::NetworkTable> trajectory_table =
-      nt::NetworkTableInstance::GetDefault().GetTable("trajectory_path");
-  current_trajectory_table =
-      nt::NetworkTableInstance::GetDefault().GetTable("current_trajectory");
-  current_trajectory_state_table =
-      nt::NetworkTableInstance::GetDefault().GetTable(
-          "current_trajectory_state");
-
-  current_trajectory = m_pathplanner.getTrajectory(path);
-  m_driveSim.SetPose(current_trajectory.Sample(0_s).pose);
-  m_timer->Reset();
-  m_timer->Start();
-}
+  _swerveDrivebase->SetPose(frc::Pose2d{_pose.X(), _pose.Y(), adjustedAngle});
 
-wom::drivetrain::behaviours::AutoSwerveDrive::AutoSwerveDrive(
-    wom::drivetrain::SwerveDrive* swerve, frc::Timer* timer,
-    frc::Field2d* field)
-    : _swerve(swerve), m_timer(timer), m_field(field) {
-  _simSwerveDrive =
-      new wom::drivetrain::behaviours::TempSimSwerveDrive(timer, field);
-}
+  if (_swerveDrivebase->IsAtSetPose() && !_hold) {
+    std::cout << "Exited..." << std::endl;
 
-void wom::drivetrain::behaviours::AutoSwerveDrive::OnUpdate() {
-  _simSwerveDrive->OnUpdate();
-  _swerve->SetPose(_simSwerveDrive->GetPose2d());
+    SetDone();
+  }
 }
-
-void wom::drivetrain::behaviours::AutoSwerveDrive::SetPath(std::string path) {
-  _simSwerveDrive->SetPath(path);
-}
\ No newline at end of file
diff --git a/wombat/src/main/cpp/sim/Sim.cpp b/wombat/src/main/cpp/sim/Sim.cpp
new file mode 100644
index 00000000..67848a5f
--- /dev/null
+++ b/wombat/src/main/cpp/sim/Sim.cpp
@@ -0,0 +1,21 @@
+// Copyright (c) 2023-2024 CurtinFRC
+// Open Source Software, you can modify it according to the terms
+// of the MIT License at the root of this project
+
+#include "sim/Sim.h"
+
+#include "drivetrain/SwerveDrive.h"
+#include "frc/geometry/Pose2d.h"
+#include "frc/smartdashboard/SmartDashboard.h"
+
+wom::sim::SimSwerve::SimSwerve(wom::drivetrain::SwerveDrive* _swerve) : _swerve(_swerve) {
+  frc::SmartDashboard::PutData("Field", &_field);
+}
+
+void wom::sim::SimSwerve::OnTick() {
+  _field.SetRobotPose(_swerve->GetPose());
+}
+
+void wom::sim::SimSwerve::OnTick(frc::Pose2d pose) {
+  _field.SetRobotPose(pose);
+}
diff --git a/wombat/src/main/cpp/utils/Pathplanner.cpp b/wombat/src/main/cpp/utils/Pathplanner.cpp
index 48400b6b..5838b483 100644
--- a/wombat/src/main/cpp/utils/Pathplanner.cpp
+++ b/wombat/src/main/cpp/utils/Pathplanner.cpp
@@ -4,15 +4,517 @@
 
 #include "utils/Pathplanner.h"
 
+#include <frc/DriverStation.h>
+#include <pathplanner/lib/auto/AutoBuilder.h>
+#include <pathplanner/lib/util/HolonomicPathFollowerConfig.h>
+#include <pathplanner/lib/util/PIDConstants.h>
+#include <pathplanner/lib/util/ReplanningConfig.h>
+
+#include <algorithm>
+#include <array>
+#include <cmath>
+#include <fstream>
+#include <initializer_list>
+#include <optional>
+#include <string>
+#include <string_view>
+#include <utility>
+#include <vector>
+
+#include "behaviour/Behaviour.h"
+#include "drivetrain/SwerveDrive.h"
+#include "drivetrain/behaviours/SwerveBehaviours.h"
+#include "frc/Filesystem.h"
+#include "frc/geometry/Pose2d.h"
+#include "frc/geometry/Rotation2d.h"
+#include "frc/kinematics/ChassisSpeeds.h"
+#include "frc/smartdashboard/SmartDashboard.h"
+#include "networktables/NetworkTable.h"
+#include "networktables/NetworkTableInstance.h"
+#include "pathplanner/lib/path/PathPlannerPath.h"
+#include "pathplanner/lib/path/PathPoint.h"
+#include "pathplanner/lib/path/RotationTarget.h"
+#include "units/time.h"
+#include "utils/Util.h"
+#include "wpi/detail/conversions/to_json.h"
+#include "wpi/json_fwd.h"
+
 using namespace wom;
 
-utils::Pathplanner::Pathplanner() {}
+wom::utils::BezierPoint::BezierPoint(double anchorX, double anchorY, wpi::json prevControlX,
+                                     wpi::json prevControlY, wpi::json nextControlX, wpi::json nextControlY,
+                                     bool isLocked, wpi::json linkedName)
+    : anchor({anchorX, anchorY}),
+      prevControl{prevControlX.is_null() ? NAN : prevControlX.get<double>(),
+                  prevControlY.is_null() ? NAN : prevControlY.get<double>()},
+      nextControl{nextControlX.is_null() ? NAN : nextControlX.get<double>(),
+                  nextControlY.is_null() ? NAN : nextControlY.get<double>()},
+      isLocked(isLocked),
+      linkedName(linkedName.is_null() ? "" : linkedName.get<std::string>()) {}
+
+double wom::utils::BezierPoint::calculateAngle() const {
+  return std::atan2(anchor.y - prevControl.y, anchor.x - prevControl.x);
+}
+
+double wom::utils::distance(const wom::utils::BezierPoint& p1, const wom::utils::BezierPoint& p2) {
+  return std::sqrt(std::pow(p2.anchor.x - p1.anchor.x, 2) + std::pow(p2.anchor.y - p1.anchor.y, 2));
+}
+
+std::vector<wom::utils::BezierPoint> wom::utils::interpolateAtIntervals(const wom::utils::BezierPoint& p1,
+                                                                        const wom::utils::BezierPoint& p2,
+                                                                        double interval) {
+  std::vector<wom::utils::BezierPoint> interpolatedPoints;
+
+  double totalDistance = distance(p1, p2);
+  int numPoints = static_cast<int>(std::ceil(totalDistance / interval));
+
+  for (int i = 1; i <= numPoints; ++i) {
+    double t = static_cast<double>(i) / (numPoints + 1);
+    interpolatedPoints.emplace_back(
+        p1.anchor.x + t * (p2.anchor.x - p1.anchor.x), p1.anchor.y + t * (p2.anchor.y - p1.anchor.y),
+        p1.prevControl.x + t * (p2.prevControl.x - p1.prevControl.x),
+        p1.prevControl.y + t * (p2.prevControl.y - p1.prevControl.y),
+        p1.nextControl.x + t * (p2.nextControl.x - p1.nextControl.x),
+        p1.nextControl.y + t * (p2.nextControl.y - p1.nextControl.y), p1.isLocked, p1.linkedName);
+  }
+
+  return interpolatedPoints;
+}
+
+std::vector<wom::utils::BezierPoint> wom::utils::createBezierPointsFromJson(const wpi::json& jsonData) {
+  std::vector<wom::utils::BezierPoint> bezierPoints;
+
+  for (const auto& point : jsonData["waypoints"]) {
+    auto anchorX = point["anchor"].is_null() ? NAN : point["anchor"]["x"].get<double>();
+    auto anchorY = point["anchor"].is_null() ? NAN : point["anchor"]["y"].get<double>();
+    auto prevControlX = point["prevControl"].is_null() ? NAN : point["prevControl"]["x"].get<double>();
+    auto prevControlY = point["prevControl"].is_null() ? NAN : point["prevControl"]["y"].get<double>();
+    auto nextControlX = point["nextControl"].is_null() ? NAN : point["nextControl"]["x"].get<double>();
+    auto nextControlY = point["nextControl"].is_null() ? NAN : point["nextControl"]["y"].get<double>();
+    auto isLocked = point["isLocked"].is_null() ? false : point["isLocked"].get<bool>();
+    auto linkedName = point["linkedName"].is_null() ? "" : point["linkedName"].get<std::string>();
+
+    bezierPoints.emplace_back(anchorX, anchorY, prevControlX, prevControlY, nextControlX, nextControlY,
+                              isLocked, linkedName);
+  }
+
+  return bezierPoints;
+}
+
+std::vector<wom::utils::BezierPoint> wom::utils::interpolateBezierPoints(
+    const std::vector<wom::utils::BezierPoint>& bezierPoints, double interval) {
+  std::vector<wom::utils::BezierPoint> interpolatedPoints;
+
+  for (size_t i = 0; i < bezierPoints.size() - 1; ++i) {
+    auto points = interpolateAtIntervals(bezierPoints[i], bezierPoints[i + 1], interval);
+    interpolatedPoints.insert(interpolatedPoints.end(), points.begin(), points.end());
+  }
+
+  return interpolatedPoints;
+}
+
+// Command implementation
+// template <typename Ret, typename... Args>
+// utils::Command<Ret, Args...>::Command(std::string name, FunctionType func)
+// : name_(std::move(name)), function_(std::move(func)), argumentTypes_{typeid(Args)...} {}
+
+// template <typename Ret, typename... Args>
+// Ret utils::Command<Ret, Args...>::Execute(Args... args) const {
+// return function_(std::forward<Args>(args)...);
+// }
+
+// template <typename Ret, typename... Args>
+// std::string utils::Command<Ret, Args...>::GetName() const {
+// return name_;
+// }
+
+// template <typename Ret, typename... Args>
+// bool utils::Command<Ret, Args...>::IsValid(std::vector<std::type_index> argTypes) const {
+// return argumentTypes_ == argTypes;
+// }
 
-frc::Trajectory utils::Pathplanner::getTrajectory(std::string_view path) {
+// Commands implementation
+// template <typename... CommandTypes>
+// template <typename... CommandArgs>
+// utils::Commands<CommandTypes...>::Commands(CommandArgs&&... commands)
+//     :
+//     commands_{std::make_shared<utils::ConcreteCommand<CommandTypes>>(std::forward<CommandArgs>(commands))...}
+//     {}
+//
+// template <typename... CommandTypes>
+// template <typename Ret, typename... Args>
+// Ret utils::Commands<CommandTypes...>::Execute(std::string command, Args&&... args) {
+//     auto it = std::find_if(commands_.begin(), commands_.end(), [&command](const auto& cmd) {
+//         return cmd->GetName() == command;
+//     });
+//
+//     if (it != commands_.end()) {
+//         return (*it)->template Execute<Ret>({std::forward<Args>(args)...});
+//     } else {
+//         throw std::invalid_argument("Command not found");
+//     }
+// }
+//
+// template <typename... CommandTypes>
+// bool utils::Commands<CommandTypes...>::IsValid(std::string command, std::vector<std::type_index> argTypes)
+// const {
+//     auto it = std::find_if(commands_.begin(), commands_.end(), [&command](const auto& cmd) {
+//         return cmd->GetName() == command;
+//     });
+//
+//     if (it != commands_.end()) {
+//         return (*it)->IsValid(argTypes);
+//     } else {
+//         throw std::invalid_argument("Command not found");
+//     }
+// }
+//
+// template <typename... CommandTypes>
+// template <typename Ret, typename... Args>
+// Ret utils::Commands<CommandTypes...>::Run(std::string commandString, Args&&... args) {
+//     size_t pos = commandString.find("(");
+//     if (pos != std::string::npos) {
+//         std::string command = commandString.substr(0, pos);
+//         std::string argString = commandString.substr(pos + 1, commandString.size() - pos - 2);
+//
+//         std::vector<std::any> parsedArgs = ParseArguments(argString);
+//
+//         if (IsValid(command, GetTypeIndices<Args...>())) {
+//             return Execute<Ret>(command, std::forward<Args>(args)...);
+//         } else {
+//             throw std::invalid_argument("Invalid command or argument types");
+//         }
+//     } else {
+//         throw std::invalid_argument("Invalid command string format");
+//     }
+// }
+//
+// template <typename... CommandTypes>
+// std::vector<std::any> utils::Commands<CommandTypes...>::ParseArguments(const std::string& argString) {
+//     std::vector<std::any> args;
+//
+//     std::istringstream iss(argString);
+//     std::string arg;
+//     while (iss >> arg) {
+//         args.push_back(ParseSingleArgument(arg));
+//     }
+//
+//     return args;
+// }
+//
+// template <typename... CommandTypes>
+// std::any utils::Commands<CommandTypes...>::ParseSingleArgument(const std::string& arg) {
+//     try {
+//         return std::stoi(arg);
+//     } catch (const std::invalid_argument& e) {
+//         throw std::invalid_argument("Error parsing argument: " + arg);
+//     }
+// }
+//
+// template <typename... CommandTypes>
+// template <typename T>
+// std::type_index utils::Commands<CommandTypes...>::GetTypeIndex() {
+//     return std::type_index(typeid(T));
+// }
+
+// PathWeaver implementation
+utils::PathWeaver::PathWeaver() {}
+
+frc::Trajectory utils::PathWeaver::getTrajectory(std::string_view path) {
   try {
     fs::path path_location = deploy_directory / path;
     return frc::TrajectoryUtil::FromPathweaverJson(path_location.string());
-  } catch (std::exception& e) {
-    return getTrajectory(path);
+  } catch (const std::exception& e) {
+    // Handle the exception appropriately or consider removing the recursive call
+    throw;  // Rethrow the exception for now
+  }
+}
+
+// FollowPath implementation
+utils::FollowPath::FollowPath(drivetrain::SwerveDrive* swerve, std::string path, bool flip, std::string name)
+    : _swerve(swerve), behaviour::Behaviour(name) {
+  _path = pathplanner::PathPlannerPath::fromPathFile(path);
+
+  if (flip) {
+    _path = _path->flipPath();
+  }
+
+  std::string filePath = frc::filesystem::GetDeployDirectory() + "/pathplanner/paths/" + path + ".path";
+
+  std::cout << filePath << std::endl;
+
+  std::ifstream file(filePath);
+
+  std::string cdata;
+  std::string cjson;
+
+  while (file.good()) {
+    file >> cdata;
+    cjson += cdata;
+  }
+
+  // cjson.pop_back();
+
+  std::vector<pathplanner::PathPoint> points =
+      pathplanner::PathPlannerPath::fromPathFile(path)->getAllPathPoints();
+
+  pathplanner::RotationTarget* lastRot = nullptr;
+  units::degree_t rot;
+
+  for (const pathplanner::PathPoint& point : points) {
+    if (lastRot != nullptr) {
+      rot = point.rotationTarget.value_or(*lastRot).getTarget().Degrees();
+    } else {
+      rot = point.rotationTarget->getTarget().Degrees();
+    }
+
+    pathplanner::RotationTarget t = pathplanner::RotationTarget(0, rot, false);
+
+    lastRot = &t;
+
+    _poses.emplace_back(frc::Pose2d(point.position, rot));
+  }
+  // _poses = pathplanner::PathPlannerPath::fromPathFile(path)->getPathPoses();
+  // wpi::json j = wpi::json::parse(cjson);
+  //
+  // std::vector<BezierPoint> bezierPoints = createBezierPointsFromJson(j);
+  //
+  // std::vector<BezierPoint> interpolatedPoints = interpolateBezierPoints(bezierPoints, 0.2);
+  //
+  // int i = 0;
+  // for (const BezierPoint& point : interpolatedPoints) {
+  //   frc::Rotation2d a;
+  //
+  //   if (point.calculateAngle() != NAN) {
+  //     a = frc::Rotation2d(units::degree_t{point.calculateAngle()});
+  //   } else {
+  //     a = _swerve->GetPose().Rotation();
+  //   }
+  //
+  //   frc::Pose2d p = frc::Pose2d(frc::Translation2d(units::meter_t{point.anchor.x},
+  //   units::meter_t{point.anchor.y}), a); frc::Pose2d p1 =
+  //   frc::Pose2d(frc::Translation2d(units::meter_t{point.nextControl.x},
+  //   units::meter_t{point.nextControl.y}), 0_deg); frc::Pose2d p2 =
+  //   frc::Pose2d(frc::Translation2d(units::meter_t{point.nextControl.x},
+  //   units::meter_t{point.prevControl.y}), 0_deg);
+  //
+  //   _poses.emplace_back(p);
+  //
+  //   WritePose2NT(nt::NetworkTableInstance::GetDefault().GetTable("pathplanner/" + std::to_string(i) +
+  //   "/poses/current"), p); WritePose2NT(nt::NetworkTableInstance::GetDefault().GetTable("pathplanner/" +
+  //   std::to_string(i) + "/poses/next"), p1);
+  //   WritePose2NT(nt::NetworkTableInstance::GetDefault().GetTable("pathplanner/" + std::to_string(i) +
+  //   "/poses/prev"), p2);
+  //
+  //   i++;
+  // }
+
+  int i = 0;
+  for (const frc::Pose2d pose : _poses) {
+    WritePose2NT(nt::NetworkTableInstance::GetDefault().GetTable("pathplanner/poses/" + std::to_string(i)),
+                 pose);
+    i++;
+  }
+
+  CalcTimer();
+
+  _timer.Start();
+}
+
+void utils::FollowPath::CalcTimer() {
+  frc::Pose2d current_pose = _swerve->GetPose();
+  frc::Pose2d target_pose = _poses[_currentPose];
+
+  units::meter_t deltaX = target_pose.Translation().X() - current_pose.Translation().X();
+  units::meter_t deltaY = target_pose.Translation().Y() - current_pose.Translation().Y();
+
+  units::meter_t dist = units::meter_t{std::sqrt(std::pow(deltaX.value(), 2) + std::pow(deltaY.value(), 2))};
+
+  _timer.Stop();
+  _timer.Reset();
+  _time = units::second_t{std::abs(dist.value()) * 1 /*meters per second*/};
+  _timer.Start();
+}
+
+void utils::FollowPath::OnTick(units::second_t dt) {
+  _swerve->SetPose(_poses[_currentPose]);
+
+  nt::NetworkTableInstance::GetDefault()
+      .GetTable("pathplanner")
+      ->GetEntry("atPose")
+      .SetBoolean(_swerve->IsAtSetPose());
+  nt::NetworkTableInstance::GetDefault()
+      .GetTable("pathplanner")
+      ->GetEntry("timeout")
+      .SetDouble(_time.value());
+  nt::NetworkTableInstance::GetDefault()
+      .GetTable("pathplanner")
+      ->GetEntry("currentTime")
+      .SetDouble(_timer.Get().value());
+  WritePose2NT(nt::NetworkTableInstance::GetDefault().GetTable("pathplanner/targetPose"),
+               _poses[_currentPose]);
+  WritePose2NT(nt::NetworkTableInstance::GetDefault().GetTable("pathplanner/currentPose"),
+               _swerve->GetPose());
+
+  std::cout << "Following Path" << std::endl;
+
+  if (_swerve->IsAtSetPose() || _timer.Get() > _time) {
+    if (_currentPose + 1 == static_cast<int>(_poses.size())) {
+      SetDone();
+    } else {
+      _currentPose++;
+      CalcTimer();
+    }
+  }
+}
+
+// AutoBuilder implementation
+utils::AutoBuilder::AutoBuilder(drivetrain::SwerveDrive* swerve, std::function<bool()> shouldFlipPath,
+                                std::string auto_routine, utils::AutoCommands commands)
+    : _path(auto_routine), _flip(shouldFlipPath()), _swerve(swerve), _commandsList(std::move(commands)) {
+  SetAuto(auto_routine);
+}
+
+void utils::AutoBuilder::SetAuto(std::string path) {
+  fs::path deploy_directory = frc::filesystem::GetDeployDirectory();
+
+  path = path + ".auto";
+
+  fs::path location = deploy_directory / "pathplanner" / "autos" / path;
+
+  std::cout << location << std::endl;
+
+  std::ifstream file(location);
+
+  std::string cdata;
+  std::string cjson;
+
+  while (file.good()) {
+    file >> cdata;
+    cjson += cdata;
+  }
+
+  cjson.pop_back();
+
+  wpi::json j = wpi::json::parse(cjson);
+
+  _currentPath = &j;
+  _startingPose = &j["startingPose"];
+  _commands = &j["command"]["data"]["commands"];
+
+  commands = std::vector<std::pair<std::string, std::string>>();
+
+  nt::NetworkTableInstance::GetDefault().GetTable("json")->GetEntry("data").SetString(_currentPath->dump());
+  nt::NetworkTableInstance::GetDefault().GetTable("json")->GetEntry("start").SetString(_startingPose->dump());
+  nt::NetworkTableInstance::GetDefault().GetTable("json")->GetEntry("commands").SetString(_commands->dump());
+  nt::NetworkTableInstance::GetDefault()
+      .GetTable("json")
+      ->GetEntry("commands_type")
+      .SetString(_commands->type_name());
+
+  for (auto c : *_commands) {
+    if (c["type"] == "path") {
+      commands.push_back(std::make_pair(c["type"], c["data"]["pathName"]));
+    }
+    if (c["type"] == "command") {
+      commands.push_back(std::make_pair(c["type"], c["data"]["name"]));
+    }
+  }
+
+  nt::NetworkTableInstance::GetDefault().GetTable("commands")->GetEntry("length").SetInteger(commands.size());
+
+  pathplan = behaviour::make<drivetrain::behaviours::DrivebasePoseBehaviour>(
+      _swerve, JSONPoseToPose2d(*_startingPose));
+
+  int i = 0;
+  int pathamt = 0;
+  int commandamt = 0;
+
+  for (std::pair<std::string, std::string> command : commands) {
+    nt::NetworkTableInstance::GetDefault()
+        .GetTable("commands/" + std::to_string(i))
+        ->GetEntry("type")
+        .SetString(static_cast<std::string>(command.first));
+    nt::NetworkTableInstance::GetDefault()
+        .GetTable("commands/" + std::to_string(i))
+        ->GetEntry("data")
+        .SetString(static_cast<std::string>(command.second));
+
+    if (command.first == "path") {
+      auto b = behaviour::make<FollowPath>(_swerve, command.second, _flip);
+      std::cout << b->GetName() << std::endl;
+      pathplan = pathplan << b;
+      pathamt++;
+    } else /*(command.first == "command")*/ {
+      pathplan = pathplan << _commandsList.Run(command.second);
+      commandamt++;
+    }
+
+    i++;
   }
-}
\ No newline at end of file
+
+  nt::NetworkTableInstance::GetDefault().GetTable("commands")->GetEntry("PathAmt").SetInteger(pathamt);
+  nt::NetworkTableInstance::GetDefault().GetTable("commands")->GetEntry("CommandAmt").SetInteger(commandamt);
+
+  WritePose2NT(nt::NetworkTableInstance::GetDefault().GetTable("startPose"),
+               JSONPoseToPose2d(*_startingPose));
+
+  // nt::NetworkTableInstance::GetDefault().GetTable("commands")->GetEntry("behaviours").SetStringArray(pathplan->GetName());
+}
+
+void utils::AutoBuilder::Invert() {
+  // Implement inversion logic if needed
+}
+
+frc::Pose2d utils::AutoBuilder::JSONPoseToPose2d(wpi::json j) {
+  // std::cout << j["position"].is_object() << std::endl;
+  // std::cout << j << std::endl;
+
+  return frc::Pose2d(units::meter_t{j["position"]["x"]}, units::meter_t{j["position"]["y"]},
+                     units::degree_t{j["rotation"]});
+  // return frc::Pose2d();
+}
+
+std::shared_ptr<behaviour::Behaviour> utils::AutoBuilder::GetAutoPath() {
+  std::cout << "Getting Path" << std::endl;
+
+  return pathplan;
+}
+
+std::shared_ptr<behaviour::Behaviour> utils::AutoBuilder::GetAutoPath(std::string path) {
+  SetAuto(path);
+  return GetAutoPath();
+}
+
+// template <typename CommandsType>
+// behaviour::Behaviour utils::AutoBuilder<CommandsType>::GetPath(std::string path) {
+// Implement the logic to return the appropriate behaviour
+// You might want to convert it to shared_ptr if needed
+// return behaviour::Behaviour(); // Placeholder
+// }
+
+// SwerveAutoBuilder implementation
+utils::SwerveAutoBuilder::SwerveAutoBuilder(drivetrain::SwerveDrive* swerve, std::string name,
+                                            utils::AutoCommands commands)
+    : _builder(new AutoBuilder(
+          swerve,
+          []() {
+            // auto alliance = frc::DriverStation::GetAlliance();
+            // return alliance && alliance.value() == frc::DriverStation::Alliance::kRed;
+            return false;
+          },
+          name, std::move(commands))),
+      _swerve(swerve),
+      _name(name) {}
+
+void utils::SwerveAutoBuilder::SetAuto(std::string path) {
+  _builder->SetAuto(path);
+}
+
+std::shared_ptr<behaviour::Behaviour> utils::SwerveAutoBuilder::GetAutoRoutine() {
+  return _builder->GetAutoPath();
+}
+
+std::shared_ptr<behaviour::Behaviour> utils::SwerveAutoBuilder::GetAutoRoutine(std::string path) {
+  return _builder->GetAutoPath(path);
+}
diff --git a/wombat/src/main/cpp/vision/Limelight.cpp b/wombat/src/main/cpp/vision/Limelight.cpp
index 437d8648..012a5e57 100644
--- a/wombat/src/main/cpp/vision/Limelight.cpp
+++ b/wombat/src/main/cpp/vision/Limelight.cpp
@@ -1,214 +1,212 @@
-// // Copyright (c) 2023-2024 CurtinFRC
-// // Open Source Software, you can modify it according to the terms
-// // of the MIT License at the root of this project
+// Copyright (c) 2023-2024 CurtinFRC
+// Open Source Software, you can modify it according to the terms
+// of the MIT License at the root of this project
 
-// #include "vision/Limelight.h"
+#include "vision/Limelight.h"
 
-// #include <iostream>
+#include <iostream>
 
-// #include "utils/Util.h"
+#include "utils/Util.h"
 
-// wom::vision::Limelight::Limelight(std::string limelightName) : _limelightName(limelightName) {
-//   table = nt::NetworkTableInstance::GetDefault().GetTable(_limelightName);
-// }
+wom::vision::Limelight::Limelight(std::string limelightName) : _limelightName(limelightName) {}
 
-// std::string wom::vision::Limelight::GetName() {
-//   return _limelightName;
-// }
+std::string wom::vision::Limelight::GetName() {
+  return _limelightName;
+}
 
-// std::pair<double, double> wom::vision::Limelight::GetOffset() {
-//   std::pair<double, double> offset;
+std::pair<double, double> wom::vision::Limelight::GetOffset() {
+  std::pair<double, double> offset;
 
-//   offset.first = table->GetNumber("tx", 0.0);
-//   offset.second = table->GetNumber("ty", 0.0);
+  offset.first = table->GetNumber("tx", 0.0);
+  offset.second = table->GetNumber("ty", 0.0);
 
-//   return offset;
-// }
+  return offset;
+}
 
-// std::vector<double> wom::vision::Limelight::GetAprilTagData(LimelightAprilTagData dataType) {
-//   std::string dataName;
+std::vector<double> wom::vision::Limelight::GetAprilTagData(LimelightAprilTagData dataType) {
+  std::string dataName;
 
-//   switch (dataType) {
-//     case LimelightAprilTagData::kBotpose:
-//       dataName = "botpose";
-//       break;
+  switch (dataType) {
+    case LimelightAprilTagData::kBotpose:
+      dataName = "botpose";
+      break;
 
-//     case LimelightAprilTagData::kBotpose_wpiblue:
-//       dataName = "botpose_wpiblue";
-//       break;
+    case LimelightAprilTagData::kBotpose_wpiblue:
+      dataName = "botpose_wpiblue";
+      break;
 
-//     case LimelightAprilTagData::kBotpose_wpired:
-//       dataName = "botpose_wpired";
-//       break;
+    case LimelightAprilTagData::kBotpose_wpired:
+      dataName = "botpose_wpired";
+      break;
 
-//     case LimelightAprilTagData::kCamerapose_targetspace:
-//       dataName = "camerapose_targetspace";
-//       break;
+    case LimelightAprilTagData::kCamerapose_targetspace:
+      dataName = "camerapose_targetspace";
+      break;
 
-//     case LimelightAprilTagData::kTargetpose_cameraspace:
-//       dataName = "targetpose_cameraspace";
-//       break;
+    case LimelightAprilTagData::kTargetpose_cameraspace:
+      dataName = "targetpose_cameraspace";
+      break;
 
-//     case LimelightAprilTagData::kTargetpose_robotspace:
-//       dataName = "targetpose_robotspace";
-//       break;
+    case LimelightAprilTagData::kTargetpose_robotspace:
+      dataName = "targetpose_robotspace";
+      break;
 
-//     case LimelightAprilTagData::kBotpose_targetspace:
-//       dataName = "botpose_targetspace	";
-//       break;
+    case LimelightAprilTagData::kBotpose_targetspace:
+      dataName = "botpose_targetspace	";
+      break;
 
-//     case LimelightAprilTagData::kCamerapose_robotspace:
-//       dataName = "camerapose_robotspace";
-//       break;
-//   }
+    case LimelightAprilTagData::kCamerapose_robotspace:
+      dataName = "camerapose_robotspace";
+      break;
+  }
 
-//   return table->GetEntry(dataName).GetDoubleArray(std::vector<double>(6));
-// }
+  return table->GetEntry(dataName).GetDoubleArray(std::vector<double>(6));
+}
 
-// double wom::vision::Limelight::GetTargetingData(LimelightTargetingData dataType, double defaultValue) {
-//   std::string dataName;
+double wom::vision::Limelight::GetTargetingData(LimelightTargetingData dataType, double defaultValue) {
+  std::string dataName;
 
-//   switch (dataType) {
-//     case LimelightTargetingData::kTv:
-//       dataName = "tv";
-//       break;
+  switch (dataType) {
+    case LimelightTargetingData::kTv:
+      dataName = "tv";
+      break;
 
-//     case LimelightTargetingData::kTx:
-//       dataName = "tx";
-//       break;
+    case LimelightTargetingData::kTx:
+      dataName = "tx";
+      break;
 
-//     case LimelightTargetingData::kTy:
-//       dataName = "ty";
-//       break;
+    case LimelightTargetingData::kTy:
+      dataName = "ty";
+      break;
 
-//     case LimelightTargetingData::kTa:
-//       dataName = "ta";
-//       break;
+    case LimelightTargetingData::kTa:
+      dataName = "ta";
+      break;
 
-//     case LimelightTargetingData::kTl:
-//       dataName = "tl";
-//       break;
+    case LimelightTargetingData::kTl:
+      dataName = "tl";
+      break;
 
-//     case LimelightTargetingData::kCl:
-//       dataName = "cl";
-//       break;
+    case LimelightTargetingData::kCl:
+      dataName = "cl";
+      break;
 
-//     case LimelightTargetingData::kTshort:
-//       dataName = "tshort";
-//       break;
+    case LimelightTargetingData::kTshort:
+      dataName = "tshort";
+      break;
 
-//     case LimelightTargetingData::kTlong:
-//       dataName = "tlong";
-//       break;
-
-//     case LimelightTargetingData::kThor:
-//       dataName = "thor";
-//       break;
-
-//     case LimelightTargetingData::kTvert:
-//       dataName = "tvert";
-//       break;
-
-//     case LimelightTargetingData::kGetpipe:
-//       dataName = "getpipe";
-//       break;
-
-//     case LimelightTargetingData::kJson:
-//       dataName = "json";
-//       break;
-
-//     case LimelightTargetingData::kTclass:
-//       dataName = "tclass";
-//       break;
-
-//     case LimelightTargetingData::kTc:
-//       dataName = "tc";
-//       break;
-
-//     case LimelightTargetingData::kTid:
-//       dataName = "tid";
-//       break;
-//   }
-
-//   return table->GetEntry(dataName).GetDouble(0);
-// }
-
-// void wom::vision::Limelight::SetLEDMode(LimelightLEDMode mode) {
-//   table->PutNumber("ledMode", static_cast<double>(mode));
-// }
-
-// void wom::vision::Limelight::SetCamMode(LimelightCamMode mode) {
-//   table->PutNumber("camMode", static_cast<double>(mode));
-// }
-
-// void wom::vision::Limelight::SetPipeline(LimelightPipeline pipeline) {
-//   table->PutNumber("pipeline", static_cast<double>(pipeline));
-// }
-
-// void wom::vision::Limelight::SetStreamMode(LimelightStreamMode mode) {
-//   table->PutNumber("stream", static_cast<double>(mode));
-// }
-
-// void wom::vision::Limelight::SetSnapshotMode(LimelightSnapshotMode mode) {
-//   table->PutNumber("snapshot", static_cast<double>(mode));
-// }
-
-// void wom::vision::Limelight::SetCrop(std::array<double, 4> crop) {
-//   table->PutNumberArray("camtran", crop);
-// }
-
-// units::meters_per_second_t wom::vision::Limelight::GetSpeed(frc::Pose3d pose1, frc::Pose3d pose2,
-//                                                             units::second_t dt) {
-//   frc::Transform3d dPose{pose1, pose2};
-//   frc::Translation3d dTranslation = dPose.Translation();
-
-//   units::meter_t y = dTranslation.Y();
-//   units::meter_t x = dTranslation.X();
-//   units::radian_t theta = units::math::atan(y / x);
-//   units::meter_t dTRANSLATION = x / units::math::cos(theta);
-//   return units::math::fabs(dTRANSLATION / dt);
-// }
-
-// units::meters_per_second_t wom::vision::Limelight::GetSpeed(frc::Pose2d pose1, frc::Pose2d pose2,
-//                                                             units::second_t dt) {
-//   frc::Transform2d dPose{pose1, pose2};
-//   frc::Translation2d dTranslation = dPose.Translation();
-
-//   units::meter_t y = dTranslation.Y();
-//   units::meter_t x = dTranslation.X();
-//   units::radian_t theta = units::math::atan(y / x);
-//   units::meter_t dTRANSLATION = x / units::math::cos(theta);
-//   return units::math::fabs(dTRANSLATION / dt);
-// }
-
-// frc::Pose3d wom::vision::Limelight::GetPose() {
-//   std::vector<double> pose = GetAprilTagData(LimelightAprilTagData::kBotpose);
-//   return frc::Pose3d(pose[1] * 1_m, 1_m * pose[2], 1_m * pose[3],
-//                      frc::Rotation3d(1_deg * (pose[4]), 1_deg * (pose[5]), 1_deg * (pose[6])));
-// }
-
-// void wom::vision::Limelight::OnStart() {
-//   std::cout << "starting" << std::endl;
-// }
-
-// void wom::vision::Limelight::OnUpdate(units::time::second_t dt) {
-//   wom::utils::WritePose3NT(table, GetPose());
-// }
-
-// bool wom::vision::Limelight::IsAtSetPoseVision(frc::Pose3d pose, units::second_t dt) {
-//   frc::Pose3d actualPose = GetPose();
-//   frc::Pose3d relativePose = actualPose.RelativeTo(pose);
-//   return (units::math::fabs(relativePose.X()) < 0.01_m && units::math::fabs(relativePose.Y()) < 0.01_m &&
-//           GetSpeed(pose, GetPose(), dt) < 1_m / 1_s);
-// }
-
-// bool wom::vision::Limelight::IsAtSetPoseVision(frc::Pose2d pose, units::second_t dt) {
-//   frc::Pose2d actualPose = GetPose().ToPose2d();
-//   frc::Pose2d relativePose = actualPose.RelativeTo(pose);
-//   return (units::math::fabs(relativePose.X()) < 0.01_m && units::math::fabs(relativePose.Y()) < 0.01_m &&
-//           GetSpeed(pose, GetPose().ToPose2d(), dt) < 1_m / 1_s);
-// }
-
-// bool wom::vision::Limelight::HasTarget() {
-//   return GetTargetingData(LimelightTargetingData::kTv) == 1.0;
-// }
+    case LimelightTargetingData::kTlong:
+      dataName = "tlong";
+      break;
+
+    case LimelightTargetingData::kThor:
+      dataName = "thor";
+      break;
+
+    case LimelightTargetingData::kTvert:
+      dataName = "tvert";
+      break;
+
+    case LimelightTargetingData::kGetpipe:
+      dataName = "getpipe";
+      break;
+
+    case LimelightTargetingData::kJson:
+      dataName = "json";
+      break;
+
+    case LimelightTargetingData::kTclass:
+      dataName = "tclass";
+      break;
+
+    case LimelightTargetingData::kTc:
+      dataName = "tc";
+      break;
+
+    case LimelightTargetingData::kTid:
+      dataName = "tid";
+      break;
+  }
+
+  return table->GetEntry(dataName).GetDouble(0);
+}
+
+void wom::vision::Limelight::SetLEDMode(LimelightLEDMode mode) {
+  table->PutNumber("ledMode", static_cast<double>(mode));
+}
+
+void wom::vision::Limelight::SetCamMode(LimelightCamMode mode) {
+  table->PutNumber("camMode", static_cast<double>(mode));
+}
+
+void wom::vision::Limelight::SetPipeline(LimelightPipeline pipeline) {
+  table->PutNumber("pipeline", static_cast<double>(pipeline));
+}
+
+void wom::vision::Limelight::SetStreamMode(LimelightStreamMode mode) {
+  table->PutNumber("stream", static_cast<double>(mode));
+}
+
+void wom::vision::Limelight::SetSnapshotMode(LimelightSnapshotMode mode) {
+  table->PutNumber("snapshot", static_cast<double>(mode));
+}
+
+void wom::vision::Limelight::SetCrop(std::array<double, 4> crop) {
+  table->PutNumberArray("camtran", crop);
+}
+
+units::meters_per_second_t wom::vision::Limelight::GetSpeed(frc::Pose3d pose1, frc::Pose3d pose2,
+                                                            units::second_t dt) {
+  frc::Transform3d dPose{pose1, pose2};
+  frc::Translation3d dTranslation = dPose.Translation();
+
+  units::meter_t y = dTranslation.Y();
+  units::meter_t x = dTranslation.X();
+  units::radian_t theta = units::math::atan(y / x);
+  units::meter_t dTRANSLATION = x / units::math::cos(theta);
+  return units::math::fabs(dTRANSLATION / dt);
+}
+
+units::meters_per_second_t wom::vision::Limelight::GetSpeed(frc::Pose2d pose1, frc::Pose2d pose2,
+                                                            units::second_t dt) {
+  frc::Transform2d dPose{pose1, pose2};
+  frc::Translation2d dTranslation = dPose.Translation();
+
+  units::meter_t y = dTranslation.Y();
+  units::meter_t x = dTranslation.X();
+  units::radian_t theta = units::math::atan(y / x);
+  units::meter_t dTRANSLATION = x / units::math::cos(theta);
+  return units::math::fabs(dTRANSLATION / dt);
+}
+
+frc::Pose3d wom::vision::Limelight::GetPose() {
+  std::vector<double> pose = GetAprilTagData(LimelightAprilTagData::kBotpose);
+  return frc::Pose3d(pose[1] * 1_m, 1_m * pose[2], 1_m * pose[3],
+                     frc::Rotation3d(1_deg * (pose[4]), 1_deg * (pose[5]), 1_deg * (pose[6])));
+}
+
+void wom::vision::Limelight::OnStart() {
+  std::cout << "starting" << std::endl;
+}
+
+void wom::vision::Limelight::OnUpdate(units::time::second_t dt) {
+  wom::utils::WritePose3NT(table, GetPose());
+}
+
+bool wom::vision::Limelight::IsAtSetPoseVision(frc::Pose3d pose, units::second_t dt) {
+  frc::Pose3d actualPose = GetPose();
+  frc::Pose3d relativePose = actualPose.RelativeTo(pose);
+  return (units::math::fabs(relativePose.X()) < 0.01_m && units::math::fabs(relativePose.Y()) < 0.01_m &&
+          GetSpeed(pose, GetPose(), dt) < 1_m / 1_s);
+}
+
+bool wom::vision::Limelight::IsAtSetPoseVision(frc::Pose2d pose, units::second_t dt) {
+  frc::Pose2d actualPose = GetPose().ToPose2d();
+  frc::Pose2d relativePose = actualPose.RelativeTo(pose);
+  return (units::math::fabs(relativePose.X()) < 0.01_m && units::math::fabs(relativePose.Y()) < 0.01_m &&
+          GetSpeed(pose, GetPose().ToPose2d(), dt) < 1_m / 1_s);
+}
+
+bool wom::vision::Limelight::HasTarget() {
+  return GetTargetingData(LimelightTargetingData::kTv) == 1.0;
+}
diff --git a/wombat/src/main/include/Wombat.h b/wombat/src/main/include/Wombat.h
index 12c6ecea..1e75f79e 100644
--- a/wombat/src/main/include/Wombat.h
+++ b/wombat/src/main/include/Wombat.h
@@ -18,6 +18,7 @@
 #include "utils/Pathplanner.h"
 #include "vision/Camera.h"
 #include "vision/Limelight.h"
+#include "sim/Sim.h"
 
 namespace wom {
 using namespace wom;
@@ -26,5 +27,6 @@ using namespace wom::utils;
 using namespace wom::drivetrain;
 using namespace wom::drivetrain::behaviours;
 using namespace wom::vision;
+using namespace wom::sim;
 using namespace behaviour;
 }  // namespace wom
diff --git a/wombat/src/main/include/drivetrain/SwerveDrive.h b/wombat/src/main/include/drivetrain/SwerveDrive.h
index ded5cdd2..21eea3f4 100644
--- a/wombat/src/main/include/drivetrain/SwerveDrive.h
+++ b/wombat/src/main/include/drivetrain/SwerveDrive.h
@@ -483,6 +483,9 @@ class SwerveDrive : public behaviour::HasBehaviour {
 
   SwerveDriveConfig& GetConfig() { return _config; }
 
+  frc::Pose2d GetSetpoint();
+
+  void MakeAtSetPoint();
  private:
   SwerveDriveConfig _config;
   SwerveDriveState _state = SwerveDriveState::kIdle;
@@ -527,4 +530,4 @@ class SwerveDrive : public behaviour::HasBehaviour {
   // double backRightEncoderOffset = -119.619140625;
 };
 }  // namespace drivetrain
-}  // namespace wom
\ No newline at end of file
+}  // namespace wom
diff --git a/wombat/src/main/include/drivetrain/behaviours/SwerveBehaviours.h b/wombat/src/main/include/drivetrain/behaviours/SwerveBehaviours.h
index 0f71e01d..b1b1f71f 100644
--- a/wombat/src/main/include/drivetrain/behaviours/SwerveBehaviours.h
+++ b/wombat/src/main/include/drivetrain/behaviours/SwerveBehaviours.h
@@ -70,8 +70,8 @@ class ManualDrivebase : public behaviour::Behaviour {
   const double turningDeadzone = 0.2;
 
   // Variables for solution to Anti-tip
-  double prevJoystickX, prevJoystickY, prevPrevJoystickX, prevPrevJoystickY,
-      usingJoystickXPos, usingJoystickYPos;
+  double prevJoystickX, prevJoystickY, prevPrevJoystickX, prevPrevJoystickY, usingJoystickXPos,
+      usingJoystickYPos;
   // The speed that the joystick must travel to activate averaging over previous
   // 3 joystick positions
   const double smoothingThreshold = 1;
@@ -86,8 +86,8 @@ class ManualDrivebase : public behaviour::Behaviour {
   const translationSpeed_ highSensitivityDriveSpeed = 80_ft / 1_s;
   // The rotation speeds for when "slow speed", "normal speed", "fast speed"
   // modes are active
-  const rotationSpeed_ lowSensitivityRotateSpeed = 720_deg / 1_s;
-  const rotationSpeed_ defaultRotateSpeed = 720_deg / 0.7_s;
+  const rotationSpeed_ lowSensitivityRotateSpeed = 120_deg / 1_s;
+  const rotationSpeed_ defaultRotateSpeed = 100_deg / 0.7_s;
   const rotationSpeed_ highSensitivityRotateSpeed = 720_deg / 1_s;
 
   translationSpeed_ maxMovementMagnitude = defaultDriveSpeed;
@@ -122,83 +122,116 @@ class GoToPose : public behaviour::Behaviour {
   frc::Pose3d _pose;
 };
 
-class FollowTrajectory : public behaviour::Behaviour {
- public:
-  FollowTrajectory(wom::drivetrain::SwerveDrive* swerve,
-                   wom::utils::Pathplanner* pathplanner, std::string path);
-
-  void OnTick(units::second_t dt) override;
-
-  void OnStart() override;
-
- private:
-  wom::utils::Pathplanner* _pathplanner;
-  std::string _path;
-  wom::drivetrain::SwerveDrive* _swerve;
-  frc::Trajectory _trajectory;
-  frc::Timer m_timer;
-};
-
-class TempSimSwerveDrive {
- public:
-  TempSimSwerveDrive(frc::Timer* timer, frc::Field2d* field);
-
-  void OnUpdate();
-
-  void SetPath(std::string path);
-
-  frc::Pose3d GetPose();
-  frc::Pose2d GetPose2d();
-
- private:
-  frc::sim::DifferentialDrivetrainSim m_driveSim{
-      frc::DCMotor::NEO(2),  // 2 NEO motors on each side of the drivetrain.
-      7.29,                  // 7.29:1 gearing reduction.
-      7.5_kg_sq_m,           // MOI of 7.5 kg m^2 (from CAD model).
-      60_kg,                 // The mass of the robot is 60 kg.
-      3_in,                  // The robot uses 3" radius wheels.
-      0.7112_m,              // The track width is 0.7112 meters.
-
-      // The standard deviations for measurement noise:
-      // x and y:          0.001 m
-      // heading:          0.001 rad
-      // l and r velocity: 0.1   m/s
-      // l and r position: 0.005 m
-      {0.001, 0.001, 0.001, 0.1, 0.1, 0.005, 0.005}};
-
-  wom::utils::Pathplanner m_pathplanner;
-
-  frc::Trajectory current_trajectory;
-
-  std::shared_ptr<nt::NetworkTable> current_trajectory_table;
-  std::shared_ptr<nt::NetworkTable> current_trajectory_state_table;
+// class FollowTrajectory : public behaviour::Behaviour {
+//  public:
+//   FollowTrajectory(wom::drivetrain::SwerveDrive* swerve, wom::utils::Pathplanner* pathplanner,
+//                    std::string path);
+//
+//   void OnTick(units::second_t dt) override;
+//
+//   void OnStart() override;
+//
+//  private:
+//   wom::utils::Pathplanner* _pathplanner;
+//   std::string _path;
+//   wom::drivetrain::SwerveDrive* _swerve;
+//   frc::Trajectory _trajectory;
+//   frc::Timer m_timer;
+// };
+//
+// class TempSimSwerveDrive {
+//  public:
+//   TempSimSwerveDrive(frc::Timer* timer, frc::Field2d* field);
+//
+//   void OnUpdate();
+//
+//   void SetPath(std::string path);
+//
+//   frc::Pose3d GetPose();
+//   frc::Pose2d GetPose2d();
+//
+//  private:
+//   frc::sim::DifferentialDrivetrainSim m_driveSim{
+//       frc::DCMotor::NEO(2),  // 2 NEO motors on each side of the drivetrain.
+//       7.29,                  // 7.29:1 gearing reduction.
+//       7.5_kg_sq_m,           // MOI of 7.5 kg m^2 (from CAD model).
+//       60_kg,                 // The mass of the robot is 60 kg.
+//       3_in,                  // The robot uses 3" radius wheels.
+//       0.7112_m,              // The track width is 0.7112 meters.
+//
+//       // The standard deviations for measurement noise:
+//       // x and y:          0.001 m
+//       // heading:          0.001 rad
+//       // l and r velocity: 0.1   m/s
+//       // l and r position: 0.005 m
+//       {0.001, 0.001, 0.001, 0.1, 0.1, 0.005, 0.005}};
+//
+//   wom::utils::Pathplanner m_pathplanner;
+//
+//   frc::Trajectory current_trajectory;
+//
+//   std::shared_ptr<nt::NetworkTable> current_trajectory_table;
+//   std::shared_ptr<nt::NetworkTable> current_trajectory_state_table;
+//
+//   frc::Timer* m_timer;
+//
+//   frc::Field2d* m_field;
+//
+//   std::string m_path;
+// };
+//
+// class AutoSwerveDrive {
+//  public:
+//   AutoSwerveDrive(wom::drivetrain::SwerveDrive* swerve, frc::Timer* timer, frc::Field2d* field);
+//
+//   void OnUpdate();
+//
+//   void SetPath(std::string path);
+//
+//  private:
+//   wom::drivetrain::SwerveDrive* _swerve;
+//
+//   TempSimSwerveDrive* _simSwerveDrive;
+//
+//   frc::Timer* m_timer;
+//
+//   frc::Field2d* m_field;
+//
+//   std::string m_path;
+// };
 
-  frc::Timer* m_timer;
-
-  frc::Field2d* m_field;
-
-  std::string m_path;
-};
-
-class AutoSwerveDrive {
+/**
+ * @brief Behaviour Class to hangle the swerve drivebase going to and potentially maintaining the position
+ */
+class DrivebasePoseBehaviour : public behaviour::Behaviour {
  public:
-  AutoSwerveDrive(wom::drivetrain::SwerveDrive* swerve, frc::Timer* timer, frc::Field2d* field);
-
-  void OnUpdate();
+  /**
+   * @param swerveDrivebase
+   * A pointer to the swerve drivebase
+   * @param pose
+   * A variable containing an X coordinate, a Y coordinate, and a rotation, for the drivebase to go to
+   * @param hold
+   * An optional variable (defaulting false), to say whether this position should be maintained
+   */
+  DrivebasePoseBehaviour(SwerveDrive* swerveDrivebase, frc::Pose2d pose, units::volt_t voltageLimit = 10_V,
+                         bool hold = false);
 
-  void SetPath(std::string path);
+  /**
+   * @brief
+   *
+   * @param deltaTime change in time since the last iteration
+   */
+  void OnTick(units::second_t deltaTime) override;
 
  private:
-  wom::drivetrain::SwerveDrive* _swerve;
+  SwerveDrive* _swerveDrivebase;
+  frc::Pose2d _pose;
+  bool _hold;
+  units::volt_t _voltageLimit;
 
-  TempSimSwerveDrive* _simSwerveDrive;
-
-  frc::Timer* m_timer;
-
-  frc::Field2d* m_field;
-
-  std::string m_path;
+  std::shared_ptr<nt::NetworkTable> _swerveDriveTable =
+      nt::NetworkTableInstance::GetDefault().GetTable("swerve");
 };
 }  // namespace behaviours
 }  // namespace drivetrain
-}  // namespace wom
\ No newline at end of file
+}  // namespace wom
diff --git a/wombat/src/main/include/sim/Sim.h b/wombat/src/main/include/sim/Sim.h
new file mode 100644
index 00000000..2fb92c86
--- /dev/null
+++ b/wombat/src/main/include/sim/Sim.h
@@ -0,0 +1,25 @@
+// Copyright (c) 2023-2024 CurtinFRC
+// Open Source Software, you can modify it according to the terms
+// of the MIT License at the root of this project
+
+#pragma once
+
+#include "drivetrain/SwerveDrive.h"
+#include "frc/geometry/Pose2d.h"
+#include "frc/smartdashboard/Field2d.h"
+
+namespace wom {
+namespace sim {
+class SimSwerve {
+ public:
+  explicit SimSwerve(drivetrain::SwerveDrive* _swerve);
+
+  void OnTick();
+  void OnTick(frc::Pose2d pose);
+
+ private:
+  frc::Field2d _field;
+  drivetrain::SwerveDrive* _swerve;
+};
+}  // namespace sim
+}  // namespace wom
diff --git a/wombat/src/main/include/utils/Pathplanner.h b/wombat/src/main/include/utils/Pathplanner.h
index 740d68dd..f4d54a13 100644
--- a/wombat/src/main/include/utils/Pathplanner.h
+++ b/wombat/src/main/include/utils/Pathplanner.h
@@ -8,16 +8,312 @@
 #include <frc/trajectory/TrajectoryUtil.h>
 #include <wpi/fs.h>
 
+#include <algorithm>
+#include <any>
+#include <functional>
+#include <memory>
+#include <stdexcept>
+#include <string>
+#include <typeindex>
+#include <utility>
+#include <vector>
+
+#include "behaviour/Behaviour.h"
+#include "drivetrain/SwerveDrive.h"
+#include "frc/Timer.h"
+#include "pathplanner/lib/path/PathPlannerPath.h"
+#include "units/time.h"
+#include "wpi/json_fwd.h"
+
 namespace wom {
 namespace utils {
-class Pathplanner {
+
+struct BezierPoint {
+  struct {
+    double x;
+    double y;
+  } anchor;
+
+  struct {
+    double x;
+    double y;
+  } prevControl;
+
+  struct {
+    double x;
+    double y;
+  } nextControl;
+
+  bool isLocked;
+  std::string linkedName;
+
+  BezierPoint(double anchorX, double anchorY, wpi::json prevControlX, wpi::json prevControlY,
+              wpi::json nextControlX, wpi::json nextControlY, bool isLocked, wpi::json linkedName);
+
+  // Function to calculate the angle of the point relative to the x-axis
+  double calculateAngle() const;
+};
+
+// Function to calculate the distance between two points
+double distance(const BezierPoint& p1, const BezierPoint& p2);
+
+// Function to interpolate points at regular intervals
+std::vector<BezierPoint> interpolateAtIntervals(const BezierPoint& p1, const BezierPoint& p2,
+                                                double interval);
+
+// Function to create a list of BezierPoints from JSON data
+std::vector<BezierPoint> createBezierPointsFromJson(const wpi::json& jsonData);
+
+// Function to create a list of interpolated BezierPoints
+std::vector<BezierPoint> interpolateBezierPoints(const std::vector<BezierPoint>& bezierPoints,
+                                                 double interval);
+
+// template <typename Ret, typename... Args>
+// class ConcreteCommand;
+
+// template <typename Ret, typename... Args>
+// class CommandBase {
+// public:
+// virtual ~CommandBase() = default;
+// virtual std::shared_ptr<behaviour::Behaviour> Execute(Args...) const = 0;
+// virtual bool IsValid(std::vector<std::type_index> argTypes) const = 0;
+// virtual std::string GetName() const = 0;
+// };
+
+// template <typename Ret, typename... Args>
+// class Command : public CommandBase<Ret, Args...>{
+// public:
+// using FunctionType = std::function<Ret(Args...)>;
+//
+// Command(std::string name, FunctionType func);
+
+// std::shared_ptr<behaviour::Behaviour> Execute(Args... args) const;
+// Ret Execute(Args... args) const;
+
+// std::string GetName() const override;
+
+// bool IsValid(std::vector<std::type_index> argTypes) const;
+
+// private:
+// std::string name_;
+// FunctionType function_;
+// std::vector<std::type_index> argumentTypes_;
+// };
+// template <typename BehaviorType>
+//     class Command {
+//     public:
+//         using BehaviorPtr = std::shared_ptr<BehaviorType>;
+//
+//         Command(const std::string& name, std::function<BehaviorPtr()> func)
+//             : name_(name), func_(func) {}
+//
+//         BehaviorPtr Execute() const {
+//             return func_();
+//         }
+//
+//         const std::string& GetName() const {
+//             return name_;
+//         }
+//
+//         bool IsValid() const {
+//             return func_ != nullptr;
+//         }
+//
+//     private:
+//         std::string name_;
+//         std::function<BehaviorPtr()> func_;
+//     };
+//
+// class Commands {
+// public:
+//     template <typename... CommandArgs>
+//     Commands(CommandArgs&&... commands);
+//
+//     template <typename Ret, typename... Args>
+//     Ret Execute(std::string command, Args&&... args);
+//
+//     bool IsValid(std::string command, std::vector<std::type_index> argTypes) const;
+//
+//     template <typename Ret, typename... Args>
+//     Ret Run(std::string commandString, Args&&... args);
+//
+// private:
+//     std::tuple<CommandTypes...> commands_;
+//
+//     std::vector<std::any> ParseArguments(const std::string& argString);
+//     std::any ParseSingleArgument(const std::string& arg);
+//
+//     template <typename T>
+//     static std::type_index GetTypeIndex();
+//
+//   template <typename... Args>
+//   std::vector<std::type_index> GetTypeIndices() {
+//     return {GetTypeIndex<Args>()...};
+//   }
+// };
+// template <typename BehaviorType>
+//     class Command {
+//     public:
+//         using BehaviorPtr = std::shared_ptr<behaviour::Behaviour>;
+//
+//         template <typename Function>
+//         Command(Function&& func, std::string name)
+//             : func_{std::make_shared<Function>(std::forward<Function>(func))}, name_{std::move(name)} {}
+//
+//         BehaviorPtr Execute() const  {
+//             return func_();
+//         }
+//
+//         std::string GetName() const  {
+//             return name_;
+//         }
+//
+//     private:
+//         std::function<std::shared_ptr<behaviour::Behaviour()>> func_;
+//         std::string name_;
+//     };
+
+class AutoCommands {
+ public:
+  // template <typename... CommandTypes>
+  // Commands(CommandTypes&&... commands)
+  // : commands_{std::make_shared<Command<BehaviorType,
+  // CommandTypes>>(std::forward<CommandTypes>(commands))...} {}
+
+  AutoCommands(
+      std::vector<std::pair<std::string, std::function<std::shared_ptr<behaviour::Behaviour>()>>> commands)
+      : commands_(std::move(commands)) {}
+
+  std::shared_ptr<behaviour::Behaviour> Execute(std::string command) {
+    auto it = std::find_if(commands_.begin(), commands_.end(),
+                           [&command](const auto& cmd) { return cmd.first == command; });
+
+    if (it != commands_.end()) {
+      return (it->second)();
+    } else {
+      throw std::invalid_argument("Command not found");
+    }
+  }
+
+  // bool IsValid(std::string command) const {
+  // auto it = std::find_if(commands_.begin(), commands_.end(), [&command](const auto& cmd) {
+  // return cmd->f == command;
+  // });
+
+  // return it != commands_.end();
+  // }
+
+  std::shared_ptr<behaviour::Behaviour> Run(std::string commandString) { return Execute(commandString); }
+
+ private:
+  std::vector<std::pair<std::string, std::function<std::shared_ptr<behaviour::Behaviour>()>>> commands_ = {};
+};
+
+// template <typename Ret, typename... Args>
+// class ConcreteCommand : public CommandBase<Ret, Args...> {
+// public:
+// using CommandType = Command<Ret, Args...>;
+
+// ConcreteCommand(std::string name, typename CommandType::FunctionType func)
+// : command_(std::move(name), std::move(func)) {}
+//
+//     void Execute(std::vector<std::any> args) const override {
+//         // Convert args to the expected types and call Execute
+//         if (args.size() == sizeof...(Args)) {
+//             command_.Execute(std::any_cast<Args>(args[0])...);
+//         } else {
+//             throw std::invalid_argument("Incorrect number of arguments for command");
+//         }
+//     }
+//
+//     bool IsValid(std::vector<std::type_index> argTypes) const override {
+//         return command_.IsValid(argTypes);
+//     }
+//
+//     std::string GetName() const override {
+//         return command_.GetName();
+//     }
+//
+// private:
+//     Command<Ret, Args...> command_;
+// };
+
+class PathWeaver {
  public:
-  Pathplanner();
+  PathWeaver();
 
   frc::Trajectory getTrajectory(std::string_view path);
 
  private:
   fs::path deploy_directory = frc::filesystem::GetDeployDirectory();
 };
+
+class FollowPath : public behaviour::Behaviour {
+ public:
+  FollowPath(drivetrain::SwerveDrive* swerve, std::string path, bool flip = false,
+             std::string _name = "<Follow Path>");
+
+  void OnTick(units::time::second_t dt) override;
+
+  void CalcTimer();
+
+ private:
+  units::second_t _time;
+  frc::Timer _timer;
+
+  std::string _pathName;
+  std::shared_ptr<pathplanner::PathPlannerPath> _path;
+  std::vector<frc::Pose2d> _poses;
+
+  drivetrain::SwerveDrive* _swerve;
+
+  int _currentPose = 0;
+};
+
+class AutoBuilder {
+ public:
+  AutoBuilder(drivetrain::SwerveDrive* swerve, std::function<bool()> shouldFlipPath, std::string autoRoutine,
+              AutoCommands commands);
+
+  void Invert();
+  void SetAuto(std::string path);
+
+  std::shared_ptr<behaviour::Behaviour> GetAutoPath();
+  std::shared_ptr<behaviour::Behaviour> GetAutoPath(std::string path);
+
+  behaviour::Behaviour GetPath(std::string path);
+
+  frc::Pose2d JSONPoseToPose2d(wpi::json j);
+
+ private:
+  std::string _path;
+  bool _flip;
+  drivetrain::SwerveDrive* _swerve;
+
+  AutoCommands _commandsList;
+
+  wpi::json* _currentPath;
+  wpi::json* _startingPose;
+  wpi::json* _commands;
+
+  behaviour::Behaviour::ptr pathplan;
+
+  std::vector<std::pair<std::string, std::string>> commands;
+};
+
+class SwerveAutoBuilder {
+ public:
+  SwerveAutoBuilder(drivetrain::SwerveDrive* swerve, std::string name, AutoCommands commands);
+
+  void SetAuto(std::string path);
+  std::shared_ptr<behaviour::Behaviour> GetAutoRoutine();
+  std::shared_ptr<behaviour::Behaviour> GetAutoRoutine(std::string path);
+
+ private:
+  AutoBuilder* _builder;
+  drivetrain::SwerveDrive* _swerve;
+  std::string _name;
+};
+
 }  // namespace utils
-}  // namespace wom
\ No newline at end of file
+}  // namespace wom
diff --git a/wombat/src/main/include/vision/Limelight.h b/wombat/src/main/include/vision/Limelight.h
index 2f06bf00..4767a44b 100644
--- a/wombat/src/main/include/vision/Limelight.h
+++ b/wombat/src/main/include/vision/Limelight.h
@@ -1,114 +1,115 @@
-// // Copyright (c) 2023-2024 CurtinFRC
-// // Open Source Software, you can modify it according to the terms
-// // of the MIT License at the root of this project
-
-// #pragma once
-
-// #include <frc/geometry/Pose2d.h>
-// #include <frc/geometry/Pose3d.h>
-// #include <frc/geometry/Rotation3d.h>
-// #include <networktables/NetworkTable.h>
-// #include <networktables/NetworkTableEntry.h>
-// #include <networktables/NetworkTableInstance.h>
-// #include <networktables/NetworkTableValue.h>
-// #include <units/angle.h>
-// #include <units/length.h>
-// #include <units/math.h>
-// #include <units/time.h>
-// #include <units/velocity.h>
-// #include <wpi/json.h>
-
-// #include <memory>
-// #include <string>
-// #include <utility>
-// #include <vector>
-
-// namespace wom {
-// namespace vision {
-
-// enum class LimelightLEDMode { kPipelineDefault = 0, kForceOff = 1, kForceBlink = 2, kForceOn = 3 };
-
-// enum class LimelightCamMode { kVisionProcessor = 0, kDriverCamera = 1 };
-
-// enum class LimelightStreamMode { kStandard = 0, kPiPMain = 1, kPiPSecondary = 2 };
-
-// enum class LimelightSnapshotMode { kReset = 0, kSingle = 1 };
-
-// enum class LimelightPipeline {
-//   kPipeline0 = 0,
-//   kPipeline1 = 1,
-//   kPipeline2 = 2,
-//   kPipeline3 = 3,
-//   kPipeline4 = 4,
-//   kPipeline5 = 5,
-//   kPipeline6 = 6,
-//   kPipeline7 = 7,
-//   kPipeline8 = 8,
-//   kPipeline9 = 9
-// };
-
-// enum class LimelightTargetingData {
-//   kTv = 0,
-//   kTx = 1,
-//   kTy = 2,
-//   kTa = 3,
-//   kTl = 4,
-//   kCl = 5,
-//   kTshort = 6,
-//   kTlong = 7,
-//   kThor = 8,
-//   kTvert = 9,
-//   kGetpipe = 10,
-//   kJson = 11,
-//   kTclass = 12,
-//   kTc = 13,
-//   kTid = 14
-// };
-
-// enum class LimelightAprilTagData {
-//   kBotpose = 0,
-//   kBotpose_wpiblue = 1,
-//   kBotpose_wpired = 2,
-//   kCamerapose_targetspace = 3,
-//   kTargetpose_cameraspace = 4,
-//   kTargetpose_robotspace = 5,
-//   kBotpose_targetspace = 6,
-//   kCamerapose_robotspace = 7,
-// };
-
-// class Limelight {
-//  public:
-//   explicit Limelight(std::string limelightName);
-
-//   std::string GetName();
-
-//   std::shared_ptr<nt::NetworkTable> table;
-
-//   std::pair<double, double> GetOffset();
-
-//   std::vector<double> GetAprilTagData(LimelightAprilTagData dataType);
-//   double GetTargetingData(LimelightTargetingData dataType, double defaultValue = 0.0);
-//   void SetLEDMode(LimelightLEDMode mode);
-//   void SetCamMode(LimelightCamMode mode);
-//   void SetPipeline(LimelightPipeline pipeline);
-//   void SetStreamMode(LimelightStreamMode mode);
-//   void SetSnapshotMode(LimelightSnapshotMode mode);
-//   void SetCrop(std::array<double, 4> crop);
-
-//   void OnUpdate(units::time::second_t dt);
-//   void OnStart();
-
-//   bool IsAtSetPoseVision(frc::Pose3d pose, units::second_t dt);
-//   bool IsAtSetPoseVision(frc::Pose2d pose, units::second_t dt);
-
-//   units::meters_per_second_t GetSpeed(frc::Pose3d pose1, frc::Pose3d pose2, units::second_t dt);
-
-//   frc::Pose3d GetPose();
-
-//   bool HasTarget();
-
-//  private:
-//   std::string _limelightName;
-// };
-// }  // namespace vision
-// }  // namespace wom
+// Copyright (c) 2023-2024 CurtinFRC
+// Open Source Software, you can modify it according to the terms
+// of the MIT License at the root of this project
+
+#pragma once
+
+#include <frc/geometry/Pose2d.h>
+#include <frc/geometry/Pose3d.h>
+#include <frc/geometry/Rotation3d.h>
+#include <networktables/NetworkTable.h>
+#include <networktables/NetworkTableEntry.h>
+#include <networktables/NetworkTableInstance.h>
+#include <networktables/NetworkTableValue.h>
+#include <units/angle.h>
+#include <units/length.h>
+#include <units/math.h>
+#include <units/time.h>
+#include <units/velocity.h>
+#include <wpi/json.h>
+
+#include <memory>
+#include <string>
+#include <utility>
+#include <vector>
+
+namespace wom {
+namespace vision {
+
+enum class LimelightLEDMode { kPipelineDefault = 0, kForceOff = 1, kForceBlink = 2, kForceOn = 3 };
+
+enum class LimelightCamMode { kVisionProcessor = 0, kDriverCamera = 1 };
+
+enum class LimelightStreamMode { kStandard = 0, kPiPMain = 1, kPiPSecondary = 2 };
+
+enum class LimelightSnapshotMode { kReset = 0, kSingle = 1 };
+
+enum class LimelightPipeline {
+  kPipeline0 = 0,
+  kPipeline1 = 1,
+  kPipeline2 = 2,
+  kPipeline3 = 3,
+  kPipeline4 = 4,
+  kPipeline5 = 5,
+  kPipeline6 = 6,
+  kPipeline7 = 7,
+  kPipeline8 = 8,
+  kPipeline9 = 9
+};
+
+enum class LimelightTargetingData {
+  kTv = 0,
+  kTx = 1,
+  kTy = 2,
+  kTa = 3,
+  kTl = 4,
+  kCl = 5,
+  kTshort = 6,
+  kTlong = 7,
+  kThor = 8,
+  kTvert = 9,
+  kGetpipe = 10,
+  kJson = 11,
+  kTclass = 12,
+  kTc = 13,
+  kTid = 14
+};
+
+enum class LimelightAprilTagData {
+  kBotpose = 0,
+  kBotpose_wpiblue = 1,
+  kBotpose_wpired = 2,
+  kCamerapose_targetspace = 3,
+  kTargetpose_cameraspace = 4,
+  kTargetpose_robotspace = 5,
+  kBotpose_targetspace = 6,
+  kCamerapose_robotspace = 7,
+};
+
+class Limelight {
+ public:
+  explicit Limelight(std::string limelightName);
+
+  std::string GetName();
+
+  std::shared_ptr<nt::NetworkTable> table = nt::NetworkTableInstance::GetDefault().GetTable("limelight");
+
+  std::pair<double, double> GetOffset();
+
+  std::vector<double> GetAprilTagData(LimelightAprilTagData dataType);
+  double GetTargetingData(LimelightTargetingData dataType, double defaultValue = 0.0);
+  void SetLEDMode(LimelightLEDMode mode);
+  void SetCamMode(LimelightCamMode mode);
+  void SetPipeline(LimelightPipeline pipeline);
+  void SetStreamMode(LimelightStreamMode mode);
+  void SetSnapshotMode(LimelightSnapshotMode mode);
+  void SetCrop(std::array<double, 4> crop);
+
+  void OnUpdate(units::time::second_t dt);
+  void OnStart();
+
+  bool IsAtSetPoseVision(frc::Pose3d pose, units::second_t dt);
+  bool IsAtSetPoseVision(frc::Pose2d pose, units::second_t dt);
+
+  units::meters_per_second_t GetSpeed(frc::Pose3d pose1, frc::Pose3d pose2, units::second_t dt);
+  units::meters_per_second_t GetSpeed(frc::Pose2d pose1, frc::Pose2d pose2, units::second_t dt);
+
+  frc::Pose3d GetPose();
+
+  bool HasTarget();
+
+ private:
+  std::string _limelightName;
+};
+}  // namespace vision
+}  // namespace wom

From 5a2d22bfe902c539c19744fb914b07e3e78bb7f7 Mon Sep 17 00:00:00 2001
From: = <paul.hodges@student.education.wa.edu.au>
Date: Sun, 3 Mar 2024 11:50:18 +0800
Subject: [PATCH 184/207] merge

---
 ctre_sim/CANCoder vers. H - 016 - 0 - ext.dat | Bin 2048 -> 2048 bytes
 ctre_sim/CANCoder vers. H - 017 - 0 - ext.dat | Bin 2048 -> 2048 bytes
 ctre_sim/CANCoder vers. H - 018 - 0 - ext.dat | Bin 2048 -> 2048 bytes
 ctre_sim/CANCoder vers. H - 019 - 0 - ext.dat | Bin 2048 -> 2048 bytes
 ctre_sim/Pigeon 2 - 020 - 0 - ext.dat         | Bin 2048 -> 2048 bytes
 ctre_sim/Talon FX vers. C - 01 - 0 - ext.dat  | Bin 2048 -> 2048 bytes
 ctre_sim/Talon FX vers. C - 02 - 0 - ext.dat  | Bin 2048 -> 2048 bytes
 ctre_sim/Talon FX vers. C - 03 - 0 - ext.dat  | Bin 2048 -> 2048 bytes
 ctre_sim/Talon FX vers. C - 04 - 0 - ext.dat  | Bin 2048 -> 2048 bytes
 ctre_sim/Talon FX vers. C - 05 - 0 - ext.dat  | Bin 2048 -> 2048 bytes
 ctre_sim/Talon FX vers. C - 06 - 0 - ext.dat  | Bin 2048 -> 2048 bytes
 ctre_sim/Talon FX vers. C - 07 - 0 - ext.dat  | Bin 2048 -> 2048 bytes
 ctre_sim/Talon FX vers. C - 09 - 0 - ext.dat  | Bin 2048 -> 2048 bytes
 src/main/cpp/Vision.cpp                       |  12 --
 src/main/include/Vision.h                     | 111 ------------------
 15 files changed, 123 deletions(-)
 delete mode 100644 src/main/cpp/Vision.cpp
 delete mode 100644 src/main/include/Vision.h

diff --git a/ctre_sim/CANCoder vers. H - 016 - 0 - ext.dat b/ctre_sim/CANCoder vers. H - 016 - 0 - ext.dat
index e6ad3f526e294d9d8071d5296e3376d8ac97771c..f5e6ac51f76c67836c78aa0251d29a9535ca2a93 100644
GIT binary patch
delta 36
ncmZn=Xb{+tz{t6uq4w(@P6h^!iGuE&8yLF2?t^j`F|h#v)J6)t

delta 36
ncmZn=Xb{+tz{t6mq510$P6h^!iGuE&dl=fjZi8|bF|h#v)N%^E

diff --git a/ctre_sim/CANCoder vers. H - 017 - 0 - ext.dat b/ctre_sim/CANCoder vers. H - 017 - 0 - ext.dat
index b279de60896bc5ee7ab02533a1e610e7d8c62aca..88c586c544f16cdfb4aefd04cfc666c2b8de3e00 100644
GIT binary patch
delta 30
kcmZn=Xb{+tz{s|rq511R&WVCvZ2K76zU~3B7BR5_0I6mQyZ`_I

delta 30
kcmZn=Xb{+tz{s|nq5tbP&WVCvY<n1bzU~0A7BR5_0I6~cyZ`_I

diff --git a/ctre_sim/CANCoder vers. H - 018 - 0 - ext.dat b/ctre_sim/CANCoder vers. H - 018 - 0 - ext.dat
index a898ffa26146b8fffbbec2dd142f0788f1a6f18b..7400fac3e621e111b2a134bf38ad2a977d3b2437 100644
GIT binary patch
delta 36
ncmZn=Xb{+tz{t6uq4w(@P6h^!iGuE&8yLF2?t^j`F|h#v)J6)t

delta 36
ncmZn=Xb{+tz{t6mq510$P6h^!iGuE&dl=fjZi8|bF|h#v)N%^E

diff --git a/ctre_sim/CANCoder vers. H - 019 - 0 - ext.dat b/ctre_sim/CANCoder vers. H - 019 - 0 - ext.dat
index 3d5aac8103dde875317a3f6262e68f1092140276..edb749188e02952bf9cfff4823d7faa18e973abd 100644
GIT binary patch
delta 30
kcmZn=Xb{+tz{s|rq511R&WVCvZ2K76zU~3B7BR5_0I6mQyZ`_I

delta 30
kcmZn=Xb{+tz{s|nq5tbP&WVCvY<n1bzU~0A7BR5_0I6~cyZ`_I

diff --git a/ctre_sim/Pigeon 2 - 020 - 0 - ext.dat b/ctre_sim/Pigeon 2 - 020 - 0 - ext.dat
index 0aaa8ded0514f2a8ab22c7ce03bfdd073478f496..cb3ccea8e5c05ba4d9ea1c5accaa36d17610642f 100644
GIT binary patch
delta 44
ycmZn=Xb{*Cz{qln!R-G=L3_rD3cPIl8JfTD<D4ky#kP;3?du*8YqJCM0TuvK;17QQ

delta 44
ycmZn=Xb{*Cz{qlf!Q%f$L3_rD3cPH)8T!9&<D4ky#kPl`=j#p-YqJCM0TuvK=nsDY

diff --git a/ctre_sim/Talon FX vers. C - 01 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 01 - 0 - ext.dat
index 5cebefc390a85fb32b5a9b2d0fe4f3a39a658ed1..cd721d7c2ed307e7e704e61c6683b5f9a176d4a7 100644
GIT binary patch
delta 30
kcmZn=Xb{-Yz{qx<q4?`Q&WVavZ08utzU~3B7BR5_0IcQ;+yDRo

delta 30
kcmZn=Xb{-Yz{qx*q5kVO&WVavY-bp1zU~0A7BR5_0Ic!~+yDRo

diff --git a/ctre_sim/Talon FX vers. C - 02 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 02 - 0 - ext.dat
index 87f97e91fec44124de85035dd916b04cc06b7fc0..731fe07d1662fbb2781f0e99f0e406c83b9033f2 100644
GIT binary patch
delta 30
kcmZn=Xb{-Yz{qx<q4?`Q&WVavZ08utzU~3B7BR5_0IcQ;+yDRo

delta 30
kcmZn=Xb{-Yz{qx*q5kVO&WVavY-bp1zU~0A7BR5_0Ic!~+yDRo

diff --git a/ctre_sim/Talon FX vers. C - 03 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 03 - 0 - ext.dat
index 6b6576837e9766f3d5cccd5d345df10587e49b6e..ea8365c958292093353783f831fb6cbbf0312226 100644
GIT binary patch
delta 30
kcmZn=Xb{-Yz{qx<q4?`Q&WVavZ08utzU~3B7BR5_0IcQ;+yDRo

delta 30
kcmZn=Xb{-Yz{qx*q5kVO&WVavY-bp1zU~0A7BR5_0Ic!~+yDRo

diff --git a/ctre_sim/Talon FX vers. C - 04 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 04 - 0 - ext.dat
index 303882212add0bbe6ecad283ef6863a402872ab6..93f0bbf14d90f39e1737c85678b107f14c15d5b1 100644
GIT binary patch
delta 30
kcmZn=Xb{-Yz{qx<q4?`Q&WVavZ08utzU~3B7BR5_0IcQ;+yDRo

delta 30
kcmZn=Xb{-Yz{qx*q5kVO&WVavY-bp1zU~0A7BR5_0Ic!~+yDRo

diff --git a/ctre_sim/Talon FX vers. C - 05 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 05 - 0 - ext.dat
index 34b0da333dad7707db4f388ffe16afff38f7b37e..ea162146e5326a4508fba46c684c4a86cd077b77 100644
GIT binary patch
delta 30
kcmZn=Xb{-Yz{qx<q4?`Q&WVavZ08utzU~3B7BR5_0IcQ;+yDRo

delta 30
kcmZn=Xb{-Yz{qx*q5kVO&WVavY-bp1zU~0A7BR5_0Ic!~+yDRo

diff --git a/ctre_sim/Talon FX vers. C - 06 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 06 - 0 - ext.dat
index f49171a018a337cd3790628619d5977854ea71f1..b2e050d4c307b081177f67e9bf1869ad9c36dc32 100644
GIT binary patch
delta 30
kcmZn=Xb{-Yz{qx<q4?`Q&WVavZ08utzU~3B7BR5_0IcQ;+yDRo

delta 30
kcmZn=Xb{-Yz{qx*q5kVO&WVavY-bp1zU~0A7BR5_0Ic!~+yDRo

diff --git a/ctre_sim/Talon FX vers. C - 07 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 07 - 0 - ext.dat
index 8485b83c4a5c5ebca12ad925f526daeec38fb63b..eaa0d7e7bc47f3a4c1ee95d0eb51521f8901f0e8 100644
GIT binary patch
delta 30
kcmZn=Xb{-Yz{qx<q4?`Q&WVavZ08utzU~3B7BR5_0IcQ;+yDRo

delta 30
kcmZn=Xb{-Yz{qx*q5kVO&WVavY-bp1zU~0A7BR5_0Ic!~+yDRo

diff --git a/ctre_sim/Talon FX vers. C - 09 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 09 - 0 - ext.dat
index 806373e9a40947a21e398b84196655a5ba6bdff2..887ad5be4042cec35a1bcf38ca95bee61dc87888 100644
GIT binary patch
delta 30
kcmZn=Xb{-Yz{qx<q4?`Q&WVavZ08utzU~3B7BR5_0IcQ;+yDRo

delta 30
kcmZn=Xb{-Yz{qx*q5kVO&WVavY-bp1zU~0A7BR5_0Ic!~+yDRo

diff --git a/src/main/cpp/Vision.cpp b/src/main/cpp/Vision.cpp
deleted file mode 100644
index 8684d8a4..00000000
--- a/src/main/cpp/Vision.cpp
+++ /dev/null
@@ -1,12 +0,0 @@
-// #include "Vision.h"
-
-// #include <utility>
-// #include <variant>
-
-// #include "units/length.h"
-// #include "units/math.h"
-// #include "vision/Limelight.h"
-
-// //std::pair<frc::Rotation2d, double angle> Vision::AngleToTarget(VisionTargetObjects objects){
-
-// //}
\ No newline at end of file
diff --git a/src/main/include/Vision.h b/src/main/include/Vision.h
deleted file mode 100644
index ea953fb9..00000000
--- a/src/main/include/Vision.h
+++ /dev/null
@@ -1,111 +0,0 @@
-// // Copyright (c) 2023-2024 CurtinFRC
-// // Open Source Software, you can modify it according to the terms
-// // of the MIT License at the root of this project
-
-// #pragma once
-
-// #include <frc/Filesystem.h>
-// #include <units/length.h>
-// #include <units/math.h>
-// #include <wpi/fs.h>
-// #include <wpi/json.h>
-
-// #include <string>
-// #include <utility>
-// #include <vector>
-
-// #include "Wombat.h"
-// #include "units/angle.h"
-// #include "AlphaArm.h"
-
-// #define APRILTAGS_MAX 16
-// #define APRILTAGS_MIN 0
-
-// enum class VisionTarget {
-//   /*
-//       Left is toward the blue side of the diagram here:
-//      https://firstfrc.blob.core.windows.net/frc2024/FieldAssets/2024LayoutMarkingDiagram.pdf
-
-//       The numbers are the numbers on the field diagram (and the numbers on the tags).
-//   */
-//   kBlueAmp = 6,
-//   kBlueSpeakerCenter = 7,
-//   kBlueSpeakerOffset = 8,
-//   kBlueChain1 = 15,
-//   kBlueChain2 = 16,
-//   kBlueChain3 = 14,
-//   kBlueSourceLeft = 1,
-//   kBlueSourceRight = 2,
-
-//   kRedAmp = 5,
-//   kRedSpeakerCenter = 4,
-//   kRedSpeakerOffset = 3,
-
-//   kRedChain1 = 12,
-//   kRedChain2 = 11,
-//   kRedChain3 = 13,
-//   kRedSourceLeft = 9,
-//   kRedSourceRight = 10
-// };
-
-// enum class VisionModes { kAprilTag = 0, kRing = 1 };
-
-// enum class VisionTargetObjects { kNote };
-
-// struct AprilTag {
-//   int id;
-//   double size;
-//   std::array<std::array<double, 4>, 4> transform;
-//   bool unique;
-
-//   frc::Pose3d pos;
-//   units::radian_t yaw;
-//   units::radian_t pitch;
-//   units::radian_t roll;
-// };
-
-// class FMAP {
-//  public:
-//   explicit FMAP(std::string path);
-
-//   std::vector<AprilTag> GetTags();
-
-//  private:
-//   std::vector<AprilTag> _tags;
-//   std::string _path;
-//   std::string deploy_dir;
-// };
-
-// class Vision {
-//  public:
-//   Vision(std::string name, FMAP fmap);
-
-//   std::pair<units::meter_t, units::degree_t> GetDistanceToTarget(VisionTarget target);
-//   std::pair<units::meter_t, units::degree_t> GetDistanceToTarget(int id);
-//   //void GetAngleToTarget(double angle);
-
-//   void SetMode(VisionModes mode);
-
-//   frc::Pose3d GetPose();
-
-//   frc::Rotation2d AngleToTarget(VisionTarget target, double angle, AlphaArm* alphaArm);
-//   std::pair<frc::Rotation2d, double> AngleToTarget(VisionTarget target);
-//   double LockOn(VisionTarget target);
-  
-//   //std::pair<frc::Rotation2d, double angle> GetAngleToTarget(Vision)
-    
-//   std::pair<frc::Pose2d, units::degree_t> GetAngleToObject(VisionTargetObjects object);
-//   units::degree_t LockOn(VisionTargetObjects target);
-
-//   std::vector<AprilTag> GetTags();
-
-//   bool IsAtPose(frc::Pose3d pose, units::second_t dt);
-
-//   bool TargetIsVisible(VisionTargetObjects target);
-
-//   int CurrentAprilTag();
-
-//  private:
-//   wom::Limelight* _limelight;
-//   FMAP _fmap;
-// };
\ No newline at end of file

From 9cef4d366301422cc5a8660bec4669a7a474c68c Mon Sep 17 00:00:00 2001
From: = <paul.hodges@student.education.wa.edu.au>
Date: Sun, 3 Mar 2024 11:53:37 +0800
Subject: [PATCH 185/207] format

---
 ctre_sim/CANCoder vers. H - 016 - 0 - ext.dat | Bin 2048 -> 2048 bytes
 ctre_sim/CANCoder vers. H - 017 - 0 - ext.dat | Bin 2048 -> 2048 bytes
 ctre_sim/CANCoder vers. H - 018 - 0 - ext.dat | Bin 2048 -> 2048 bytes
 ctre_sim/CANCoder vers. H - 019 - 0 - ext.dat | Bin 2048 -> 2048 bytes
 ctre_sim/Pigeon 2 - 020 - 0 - ext.dat         | Bin 2048 -> 2048 bytes
 ctre_sim/Talon FX vers. C - 01 - 0 - ext.dat  | Bin 2048 -> 2048 bytes
 ctre_sim/Talon FX vers. C - 02 - 0 - ext.dat  | Bin 2048 -> 2048 bytes
 ctre_sim/Talon FX vers. C - 03 - 0 - ext.dat  | Bin 2048 -> 2048 bytes
 ctre_sim/Talon FX vers. C - 04 - 0 - ext.dat  | Bin 2048 -> 2048 bytes
 ctre_sim/Talon FX vers. C - 05 - 0 - ext.dat  | Bin 2048 -> 2048 bytes
 ctre_sim/Talon FX vers. C - 06 - 0 - ext.dat  | Bin 2048 -> 2048 bytes
 ctre_sim/Talon FX vers. C - 07 - 0 - ext.dat  | Bin 2048 -> 2048 bytes
 ctre_sim/Talon FX vers. C - 09 - 0 - ext.dat  | Bin 2048 -> 2048 bytes
 src/main/include/ArmAngle.h                   |  84 ------------------
 src/main/include/Robot.h                      |   2 +-
 src/main/include/RobotMap.h                   |   1 -
 16 files changed, 1 insertion(+), 86 deletions(-)
 delete mode 100644 src/main/include/ArmAngle.h

diff --git a/ctre_sim/CANCoder vers. H - 016 - 0 - ext.dat b/ctre_sim/CANCoder vers. H - 016 - 0 - ext.dat
index f5e6ac51f76c67836c78aa0251d29a9535ca2a93..f85af7828428963056a0bf436613219ec49b19e6 100644
GIT binary patch
delta 36
ncmZn=Xb{+tz{t6oq4DbxP6h^!iGuE&TNvuT9)ofgF|h#v)D{Z7

delta 36
ncmZn=Xb{+tz{t6uq4w(@P6h^!iGuE&8yLF2?t^j`F|h#v)J6)t

diff --git a/ctre_sim/CANCoder vers. H - 017 - 0 - ext.dat b/ctre_sim/CANCoder vers. H - 017 - 0 - ext.dat
index 88c586c544f16cdfb4aefd04cfc666c2b8de3e00..ba168a699ba87dbfe68ecf9b84426b091b056bd2 100644
GIT binary patch
delta 30
kcmZn=Xb{+tz{s|lq5bPI&WVCvY?~Omz8(Rw7BR5_0I3!VyZ`_I

delta 30
kcmZn=Xb{+tz{s|rq511R&WVCvZ2K76zU~3B7BR5_0I6mQyZ`_I

diff --git a/ctre_sim/CANCoder vers. H - 018 - 0 - ext.dat b/ctre_sim/CANCoder vers. H - 018 - 0 - ext.dat
index 7400fac3e621e111b2a134bf38ad2a977d3b2437..42743fba40fe870155b4afe1407e60df9d0c7ca8 100644
GIT binary patch
delta 36
ncmZn=Xb{+tz{t6oq4DbxP6h^!iGuE&TNvuT9)ofgF|h#v)D{Z7

delta 36
ncmZn=Xb{+tz{t6uq4w(@P6h^!iGuE&8yLF2?t^j`F|h#v)J6)t

diff --git a/ctre_sim/CANCoder vers. H - 019 - 0 - ext.dat b/ctre_sim/CANCoder vers. H - 019 - 0 - ext.dat
index edb749188e02952bf9cfff4823d7faa18e973abd..0e7bd56446663d4d7a33717e64431af6e404aec7 100644
GIT binary patch
delta 30
kcmZn=Xb{+tz{s|lq5bPI&WVCvY?~Omz8(Rw7BR5_0I3!VyZ`_I

delta 30
kcmZn=Xb{+tz{s|rq511R&WVCvZ2K76zU~3B7BR5_0I6mQyZ`_I

diff --git a/ctre_sim/Pigeon 2 - 020 - 0 - ext.dat b/ctre_sim/Pigeon 2 - 020 - 0 - ext.dat
index cb3ccea8e5c05ba4d9ea1c5accaa36d17610642f..10deca5246fe2ba14f793023c11b424b9acecd2e 100644
GIT binary patch
delta 46
zcmZn=Xb@mI&0zX}qo6(GL<Qc>0gM5RY?~R{zaHb9DCotuiJ|N35fE#$1M>kE0A_s;
A(*OVf

delta 46
zcmZn=Xb@mI$zc9}qo6(GL<Qc>0gM5RZ2K9SzwYClDCotukD=}B9uRA@1M>kE0A|S#
A(*OVf

diff --git a/ctre_sim/Talon FX vers. C - 01 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 01 - 0 - ext.dat
index cd721d7c2ed307e7e704e61c6683b5f9a176d4a7..d142e6329edc1050d22f354f46936dd454c6ee74 100644
GIT binary patch
delta 30
kcmZn=Xb{-Yz{qx(q5SJH&WVavY=;=Cz8(Rw7BR5_0IZe@+yDRo

delta 30
kcmZn=Xb{-Yz{qx<q4?`Q&WVavZ08utzU~3B7BR5_0IcQ;+yDRo

diff --git a/ctre_sim/Talon FX vers. C - 02 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 02 - 0 - ext.dat
index 731fe07d1662fbb2781f0e99f0e406c83b9033f2..024359a7ea2f4ddbf89581b633dd4f5b93c240a3 100644
GIT binary patch
delta 30
kcmZn=Xb{-Yz{qx(q5SJH&WVavY=;=Cz8(Rw7BR5_0IZe@+yDRo

delta 30
kcmZn=Xb{-Yz{qx<q4?`Q&WVavZ08utzU~3B7BR5_0IcQ;+yDRo

diff --git a/ctre_sim/Talon FX vers. C - 03 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 03 - 0 - ext.dat
index ea8365c958292093353783f831fb6cbbf0312226..4cf50b960beecb8d9ab393405877b735ba9cc240 100644
GIT binary patch
delta 30
kcmZn=Xb{-Yz{qx(q5SJH&WVavY=;=Cz8(Rw7BR5_0IZe@+yDRo

delta 30
kcmZn=Xb{-Yz{qx<q4?`Q&WVavZ08utzU~3B7BR5_0IcQ;+yDRo

diff --git a/ctre_sim/Talon FX vers. C - 04 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 04 - 0 - ext.dat
index 93f0bbf14d90f39e1737c85678b107f14c15d5b1..6286d39ff38959e1ccce458afa63038bdb56ce52 100644
GIT binary patch
delta 30
kcmZn=Xb{-Yz{qx(q5SJH&WVavY=;=Cz8(Rw7BR5_0IZe@+yDRo

delta 30
kcmZn=Xb{-Yz{qx<q4?`Q&WVavZ08utzU~3B7BR5_0IcQ;+yDRo

diff --git a/ctre_sim/Talon FX vers. C - 05 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 05 - 0 - ext.dat
index ea162146e5326a4508fba46c684c4a86cd077b77..f00666a0da24aa3b549bec42e79ec67e8186510b 100644
GIT binary patch
delta 30
kcmZn=Xb{-Yz{qx(q5SJH&WVavY=;=Cz8(Rw7BR5_0IZe@+yDRo

delta 30
kcmZn=Xb{-Yz{qx<q4?`Q&WVavZ08utzU~3B7BR5_0IcQ;+yDRo

diff --git a/ctre_sim/Talon FX vers. C - 06 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 06 - 0 - ext.dat
index b2e050d4c307b081177f67e9bf1869ad9c36dc32..753dcc6ef6d05468a0949023a34c7a05a14f50ae 100644
GIT binary patch
delta 30
kcmZn=Xb{-Yz{qx(q5SJH&WVavY=;=Cz8(Rw7BR5_0IZe@+yDRo

delta 30
kcmZn=Xb{-Yz{qx<q4?`Q&WVavZ08utzU~3B7BR5_0IcQ;+yDRo

diff --git a/ctre_sim/Talon FX vers. C - 07 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 07 - 0 - ext.dat
index eaa0d7e7bc47f3a4c1ee95d0eb51521f8901f0e8..cfef40b12e2f46632ff37432de04e7a5d599f6ab 100644
GIT binary patch
delta 30
kcmZn=Xb{-Yz{qx(q5SJH&WVavY=;=Cz8(Rw7BR5_0IZe@+yDRo

delta 30
kcmZn=Xb{-Yz{qx<q4?`Q&WVavZ08utzU~3B7BR5_0IcQ;+yDRo

diff --git a/ctre_sim/Talon FX vers. C - 09 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 09 - 0 - ext.dat
index 887ad5be4042cec35a1bcf38ca95bee61dc87888..e02d26f6bbce8a5686e7e423f0c5a6075d13a911 100644
GIT binary patch
delta 30
kcmZn=Xb{-Yz{qx(q5SJH&WVavY=;=Cz8(Rw7BR5_0IZe@+yDRo

delta 30
kcmZn=Xb{-Yz{qx<q4?`Q&WVavZ08utzU~3B7BR5_0IcQ;+yDRo

diff --git a/src/main/include/ArmAngle.h b/src/main/include/ArmAngle.h
deleted file mode 100644
index 051565db..00000000
--- a/src/main/include/ArmAngle.h
+++ /dev/null
@@ -1,84 +0,0 @@
-// // Copyright (c) 2023-2024 CurtinFRC
-// // Open Source Software, you can modify it according to the terms
-// // of the MIT License at the root of this project
-
-// #pragma once
-
-// #include <frc/Filesystem.h>
-// #include <units/length.h>
-// #include <units/math.h>
-// #include <wpi/fs.h>
-// #include <wpi/json.h>
-
-// #include <string>
-// #include <utility>
-// #include <vector>
-
-// #include "Wombat.h"
-// #include "units/angle.h"
-// #include "AlphaArm.h"
-
-// #define APRILTAGS_MAX 16
-// #define APRILTAGS_MIN 0
-
-// enum class VisionTarget {
-//   /*
-//       Left is toward the blue side of the diagram here:
-//      https://firstfrc.blob.core.windows.net/frc2024/FieldAssets/2024LayoutMarkingDiagram.pdf
-
-//       The numbers are the numbers on the field diagram (and the numbers on the tags).
-//   */
-//   kBlueAmp = 6,
-//   kBlueSpeakerCenter = 7,
-//   kBlueSpeakerOffset = 8,
-//   kBlueChain1 = 15,
-//   kBlueChain2 = 16,
-//   kBlueChain3 = 14,
-//   kBlueSourceLeft = 1,
-//   kBlueSourceRight = 2,
-
-//   kRedAmp = 5,
-//   kRedSpeakerCenter = 4,
-//   kRedSpeakerOffset = 3,
-
-//   kRedChain1 = 12,
-//   kRedChain2 = 11,
-//   kRedChain3 = 13,
-//   kRedSourceLeft = 9,
-//   kRedSourceRight = 10
-// };
-
-// enum class VisionModes { kAprilTag = 0, kRing = 1 };
-
-// enum class VisionTargetObjects { kNote };
-
-// struct AprilTag {
-//   int id;
-//   double size;
-//   std::array<std::array<double, 4>, 4> transform;
-//   bool unique;
-
-//   frc::Pose3d pos;
-//   units::radian_t yaw;
-//   units::radian_t pitch;
-//   units::radian_t roll;
-// };
-
-// class FMAP {
-//  public:
-//   explicit FMAP(std::string path);
-
-//   std::vector<AprilTag> GetTags();
-
-//  private:
-//   std::vector<AprilTag> _tags;
-//   std::string _path;
-//   std::string deploy_dir;
-// };
-
-// class ArmVision{
-//     public:
-//     ArmVision(std::string name, FMAP fmap);
-
-//     frc::Twist2d AngleToAprilTag(VisionTarget target, double angleOffset, );
-// };
diff --git a/src/main/include/Robot.h b/src/main/include/Robot.h
index c0448245..8a64dfa4 100644
--- a/src/main/include/Robot.h
+++ b/src/main/include/Robot.h
@@ -3,7 +3,7 @@
 // of the MIT License at the root of this project
 
 #pragma once
-// #include <frc/Encoder.h>
+
 #include <frc/Encoder.h>
 #include <frc/TimedRobot.h>
 #include <frc/Timer.h>
diff --git a/src/main/include/RobotMap.h b/src/main/include/RobotMap.h
index e03c89a7..295151f0 100644
--- a/src/main/include/RobotMap.h
+++ b/src/main/include/RobotMap.h
@@ -28,7 +28,6 @@
 #include <ctre/phoenix6/TalonFX.hpp>
 // #include <frc/Servo.h>
 
-// #include "Intake.h"
 #include "Wombat.h"
 #include "utils/Encoder.h"
 #include "utils/PID.h"

From c624b26206b1e3d2f804616b279b257b22e50696 Mon Sep 17 00:00:00 2001
From: = <paul.hodges@student.education.wa.edu.au>
Date: Sun, 3 Mar 2024 11:58:11 +0800
Subject: [PATCH 186/207] format

---
 src/main/cpp/AlphaArm.cpp            | 8 ++++----
 src/main/include/AlphaArm.h          | 5 +++--
 src/main/include/AlphaArmBehaviour.h | 2 ++
 3 files changed, 9 insertions(+), 6 deletions(-)

diff --git a/src/main/cpp/AlphaArm.cpp b/src/main/cpp/AlphaArm.cpp
index 9924c29d..aa107745 100644
--- a/src/main/cpp/AlphaArm.cpp
+++ b/src/main/cpp/AlphaArm.cpp
@@ -31,7 +31,7 @@ void AlphaArm::OnUpdate(units::second_t dt) {
     case AlphaArmState::kHoldAngle:
     {
       _pidIntakeState.SetSetpoint(-_goal);
-       //_setAlphaArmVoltage = _pidArm.Calculate(_alphaArm->GetConfig().config->alphaArmEncoder.GetEncoderPosition());
+       // _setAlphaArmVoltage = _pidArm.Calculate(_alphaArm->GetConfig().config->alphaArmEncoder.GetEncoderPosition());
       _setAlphaArmVoltage = units::volt_t{_pidIntakeState.Calculate(-(_config->alphaArmEncoder.GetEncoderPosition().value() * (2 * 3.1415)))};
     }
     break;
@@ -45,7 +45,7 @@ void AlphaArm::OnUpdate(units::second_t dt) {
 
     case AlphaArmState::kIntakeAngle:
     std::cout << "Intake Angle" << std::endl;
-       _pidIntakeState.SetSetpoint(-0.50); //-0.48
+       _pidIntakeState.SetSetpoint(-0.50); // -0.48
        _setAlphaArmVoltage = units::volt_t{_pidIntakeState.Calculate(-(_config->alphaArmEncoder.GetEncoderPosition().value() * (2 * 3.1415)))};
     break;
     case AlphaArmState::kIntakedAngle:
@@ -55,13 +55,13 @@ void AlphaArm::OnUpdate(units::second_t dt) {
     break;
     case AlphaArmState::kClimbAngle:
     std::cout << "Climb Angle" << std::endl;
-       _pidArmStates.SetSetpoint(-2.00); //-0.48
+       _pidArmStates.SetSetpoint(-2.00); // -0.48
        _setAlphaArmVoltage = units::volt_t{_pidArmStates.Calculate(-(_config->alphaArmEncoder.GetEncoderPosition().value() * (2 * 3.1415)))};
     break;
 
     case AlphaArmState::kClimbed:
     std::cout << "Climb Angle" << std::endl;
-       _pidClimberStates.SetSetpoint(-0.5); //-0.48
+       _pidClimberStates.SetSetpoint(-0.5); //- 0.48
        _setAlphaArmVoltage = units::volt_t{_pidClimberStates.Calculate(-(_config->alphaArmEncoder.GetEncoderPosition().value() * (2 * 3.1415)))};
     break;
 
diff --git a/src/main/include/AlphaArm.h b/src/main/include/AlphaArm.h
index 06fdc3a7..f12e0c6d 100644
--- a/src/main/include/AlphaArm.h
+++ b/src/main/include/AlphaArm.h
@@ -7,11 +7,12 @@
 
 
 #include "Wombat.h"
-//#include "vision/Vision.h"
 #include "utils/PID.h"
 #include <units/angle.h>
 #include <units/voltage.h>
 #include <frc/controller/PIDController.h>
+#include <string>
+#include <memory>
 
 struct AlphaArmConfig {
   wom::Gearbox alphaArmGearbox;
@@ -39,7 +40,7 @@ enum class AlphaArmState {
 
 class AlphaArm : public behaviour::HasBehaviour {
  public:
-  AlphaArm(AlphaArmConfig *config/*, frc::Rotation2d initialAngle, wom::vision::Limelight* vision*/);
+  explicit AlphaArm(AlphaArmConfig *config/*, frc::Rotation2d initialAngle, wom::vision::Limelight* vision*/);
 
   void OnUpdate(units::second_t dt);
   void SetArmRaw(units::volt_t voltage);
diff --git a/src/main/include/AlphaArmBehaviour.h b/src/main/include/AlphaArmBehaviour.h
index 7271d21f..61938878 100644
--- a/src/main/include/AlphaArmBehaviour.h
+++ b/src/main/include/AlphaArmBehaviour.h
@@ -11,6 +11,8 @@
 #include "Intake.h"
 #include "Wombat.h"
 
+#include <memory>
+
 class AlphaArmManualControl : public behaviour::Behaviour {
  public:
   explicit AlphaArmManualControl(AlphaArm* alphaArm, Intake* intake, frc::XboxController* codriver);

From bfff6d908ca5c3263d80fce68942d4cfd947c39c Mon Sep 17 00:00:00 2001
From: = <paul.hodges@student.education.wa.edu.au>
Date: Sun, 3 Mar 2024 13:18:43 +0800
Subject: [PATCH 187/207] remove ctre

---
 .gitignore                                    |   1 +
 ctre_sim/CANCoder vers. H - 016 - 0 - ext.dat | Bin 2048 -> 0 bytes
 ctre_sim/CANCoder vers. H - 017 - 0 - ext.dat | Bin 2048 -> 0 bytes
 ctre_sim/CANCoder vers. H - 018 - 0 - ext.dat | Bin 2048 -> 0 bytes
 ctre_sim/CANCoder vers. H - 019 - 0 - ext.dat | Bin 2048 -> 0 bytes
 ctre_sim/Pigeon 2 - 020 - 0 - ext.dat         | Bin 2048 -> 0 bytes
 ctre_sim/Talon FX vers. C - 01 - 0 - ext.dat  | Bin 2048 -> 0 bytes
 ctre_sim/Talon FX vers. C - 02 - 0 - ext.dat  | Bin 2048 -> 0 bytes
 ctre_sim/Talon FX vers. C - 03 - 0 - ext.dat  | Bin 2048 -> 0 bytes
 ctre_sim/Talon FX vers. C - 04 - 0 - ext.dat  | Bin 2048 -> 0 bytes
 ctre_sim/Talon FX vers. C - 05 - 0 - ext.dat  | Bin 2048 -> 0 bytes
 ctre_sim/Talon FX vers. C - 06 - 0 - ext.dat  | Bin 2048 -> 0 bytes
 ctre_sim/Talon FX vers. C - 07 - 0 - ext.dat  | Bin 2048 -> 0 bytes
 ctre_sim/Talon FX vers. C - 09 - 0 - ext.dat  | Bin 2048 -> 0 bytes
 14 files changed, 1 insertion(+)
 delete mode 100644 ctre_sim/CANCoder vers. H - 016 - 0 - ext.dat
 delete mode 100644 ctre_sim/CANCoder vers. H - 017 - 0 - ext.dat
 delete mode 100644 ctre_sim/CANCoder vers. H - 018 - 0 - ext.dat
 delete mode 100644 ctre_sim/CANCoder vers. H - 019 - 0 - ext.dat
 delete mode 100644 ctre_sim/Pigeon 2 - 020 - 0 - ext.dat
 delete mode 100644 ctre_sim/Talon FX vers. C - 01 - 0 - ext.dat
 delete mode 100644 ctre_sim/Talon FX vers. C - 02 - 0 - ext.dat
 delete mode 100644 ctre_sim/Talon FX vers. C - 03 - 0 - ext.dat
 delete mode 100644 ctre_sim/Talon FX vers. C - 04 - 0 - ext.dat
 delete mode 100644 ctre_sim/Talon FX vers. C - 05 - 0 - ext.dat
 delete mode 100644 ctre_sim/Talon FX vers. C - 06 - 0 - ext.dat
 delete mode 100644 ctre_sim/Talon FX vers. C - 07 - 0 - ext.dat
 delete mode 100644 ctre_sim/Talon FX vers. C - 09 - 0 - ext.dat

diff --git a/.gitignore b/.gitignore
index 8903d9ee..bbc66bdc 100644
--- a/.gitignore
+++ b/.gitignore
@@ -4,6 +4,7 @@
 ### Sim ###
 
 ctre_sim
+./ctre_sim/
 
 ### C++ ###
 # Prerequisites
diff --git a/ctre_sim/CANCoder vers. H - 016 - 0 - ext.dat b/ctre_sim/CANCoder vers. H - 016 - 0 - ext.dat
deleted file mode 100644
index f85af7828428963056a0bf436613219ec49b19e6..0000000000000000000000000000000000000000
GIT binary patch
literal 0
HcmV?d00001

literal 2048
zcmeHIJ!n%=6h1GhnpB4j78R8?5i7cNY9{+Eh(*x8;!?L@21QXEt!QnLLL8LhqT-T4
zMT=mib;%$pJ}VTY3euV_wu?GQ1Y<1~pZDBzzL2D5%jjKBzH`3wou7MonW3J5{+}jp
zU?FfqL?_F`oga+q6pfbsC{d(I6{cypJljOQm)ckH*2oR+dq(lxjzbpHW^w%PmbpAf
zEb=%<PPgz4xTc;p^CQ+G;IjJis_QyLfjRj$sWYjTln>sB<~{i-`%H96{U3Gx#FWRz
zNYhcpttuD`tD{}&O}u|J!2K<a(P@svHcH%#6XKcNt9g5m=x6@mgyrXmavaYemUw=P
z*l*ijCHTJmuwdxLojEIvdU40McwEX+DU-2#YlN9*cf3>ksl$Fq{K!W?^*E#TU=Rm3
zPpak=^!*aazH@J6UuJUIi(&Krs&)T`E6+puz=cbq2aMu+{ebK2AM<ATD&0uY_Tdc(
zr7v2GpYD6Ghx@@<dj8nn-;ne5I{Uoc1)J|Gq4!9OZN*cacN;`6yCU!Ib6ve^eZF@b
z<o9(-ee4H%tI=nB$6V8Y+wv~%FRQvA53IE^zPm~4rnC>#N4`lVu7`Zg!#lyeVKWKh
z(9cfY?BF=&HgtRHaq0Cv`yR0m=);<%lw=yM?OXA4ojtR#SaUzs`*IC?aItY*#Zqkd
zM`T~vXDQup`%A`)c7IWg<Nd}oIT;^p=Xt@0-}@!^Ij;S|PpjgeUpcY-@V?(MeMRyS
zXUlrP;5*qn&V^YidhArHN6XW>(XM{n2m2-y$;$T;9JZz4xb>%dKiPTkKXLKA)Gxl(
h@31EEH!}!`Zl{Is8b5y@&VCWBw2E#c{NlUt{{o$wH_ZS5

diff --git a/ctre_sim/CANCoder vers. H - 017 - 0 - ext.dat b/ctre_sim/CANCoder vers. H - 017 - 0 - ext.dat
deleted file mode 100644
index ba168a699ba87dbfe68ecf9b84426b091b056bd2..0000000000000000000000000000000000000000
GIT binary patch
literal 0
HcmV?d00001

literal 2048
zcmeHHJ!n%=6h1F$HK`5}ELK#~L~T(QJ2ivovmh3irnt1=P{^Ptf}2116Om#Rl;9u{
zmkcUegx1z6gQOT03Q`4Wn=Q6WagYqgS}MkS?m1scUhS6AyPWr(pYQzLcPHscmHwXw
zPGF|$_=pB8*E_x#)(N^@c^_;X{W#sN6dK6)MoTN*Pu}6WClya^KWZ^;62<Rs=_~Gt
zL>$M+=@x$jZm4F(e1w_=Tv1(Kd0mI7YEHgY@{Fk@^@m?l<6iufc}BY={|`?+A@yTn
zsOf~_79|Y%RS}kaBcGr3bA9u}beb)p4PrO#__!zcTHfvv^-LcgvHm%t9NY89#Gc<Q
z^82>8QQmjKFBp6gXUY;IU&JX`oR)M{PNnSJ*2C;(XS_q}@!*0_^oU12UYyc=5U>M#
zuT)X*FOkeU_d(`mI+wYaK%QSM@4xWs^-zD{;w9k&Mjg6-z%}xZxih><H&C*5#QVji
zH&~Bfc0Sq7_2A6EdT#f3+4**hb@5$A8}F(^&+#T(N?dt9H49&MN$%aECtoc;?;Qv6
zy&aMt^8w#-aCGmmYwB-Z*va)}m3MCcN(<x6M#(#*bs#_DO(=0b#G@ac3HtS$PGARr
zcH&+;+tIgix5tai?;hFvh<Sh?(gY1jq~U7YiqAFj%*<@vero6C8t;MG_3cVF#ByIi
z=7o8d6MeS6Sfpg<7gRg$?-mV?MFzs$FZ`j8|B`*CwLa)+S@hFOCl{VRF0{YCD)F$h
zWIkZ{Cm&{P#?b?^?>6N*ceASx`(TH$Xl(U;WPIwP@Sk1W)7flrqUWpaC%{H?E}R$p
q!OWq$IDRv$RiXz8@pp|czYpN!x6hvRV7N9v{<G3YdH=@eg#Rx+k2&H1

diff --git a/ctre_sim/CANCoder vers. H - 018 - 0 - ext.dat b/ctre_sim/CANCoder vers. H - 018 - 0 - ext.dat
deleted file mode 100644
index 42743fba40fe870155b4afe1407e60df9d0c7ca8..0000000000000000000000000000000000000000
GIT binary patch
literal 0
HcmV?d00001

literal 2048
zcmeHIJ7|+p6h28>O{zl>ixrhLQCrl-PR$_t8xV_2|KieuLm-2K2yQ;mnj(cbD8WG@
zE*Vs`2(7kL21zk0bg&B2G+S&JagYqgS}MkS?m1scQoCjJF6aNw<2$dLn^`(hqyMLc
z6IiHuyhJ0_(VowSb(+Sig#eMiMHyyjwEC`ve6O{x^1b98u6tJT-0qVW(>783?w7t?
zk4VIEjGTV)H{h0P*33t+O~7^4<(1d*5Y^1dcS@cqm8AagOKRNnpEA!#pXC41R8LU-
zSQu(Lqqsu}gFaP+B;UlRr$b!d(m0)GOK^+WO*>xh$+?-gdqh3+$0w|Rjwr|W{3)^L
zw~PF~>rI6BUGND8U)VEeiIFetDOjAAbW};D?A$iP>}F@YN9$?A1+VB4k9wMMO7lU$
z4s5Pe!zt<gC6amP-pjm9=Q0=L&GW10{TE)p9_kNVx+;9YNUrM#+#vs$JHxAV{AF8*
zJ0vaxfkynI_wjzN2WRQ!GrPZQo^Q8V=k6=nct;&#r`v2PuJU|r7ryL@+`F$$`Re(3
z?>LAb=#l)G5BOFClLyBgQ-A03Ual{zyo-m|Iv8iRO5V8Ef&7R!p~U$RkA8S2=+|dD
zfgSwW>AT%*N8iTXo@QKm`_SG;%me(8CMYhEhO28QJ~zk<3&n>0)X&Q?-owSs?MfDB
zdB`vG!aOU9L0exmT(<KIs2%rrhXzN(!y)b${?NO>WS?oR4|-Y^{rt+=<tL8{-7_~N
z9(I<@2Mqt@gN)5Mc|`Wzr92nM`UbHNc9@Dp*WX9R=dK8Uv3j7l-QYy*i|q&h6PGWE
s-Te@2h~qc2RwKHf5P!$S^7{Zje*5@1>5))<e*DgkW{z3DSBt{`7oF2O6aWAK

diff --git a/ctre_sim/CANCoder vers. H - 019 - 0 - ext.dat b/ctre_sim/CANCoder vers. H - 019 - 0 - ext.dat
deleted file mode 100644
index 0e7bd56446663d4d7a33717e64431af6e404aec7..0000000000000000000000000000000000000000
GIT binary patch
literal 0
HcmV?d00001

literal 2048
zcmeHHJ!n%=6uvJ>HK`5(Tdb(0iL^zXIyI9%17dOMD=sZK1TrXy;N}l%BT|fl5*(!B
zl0ij_(Av6~L0<8x(7`H5+kiuC7jcjb##$=Id+s@3NnY)i(X*U<=jS_rH*ZEAuBrdh
z#0|{XtbkI()vMi~H0!jwQGFX}9{rTMRrQ+4_exqT-^t#jzGno_?mA+y+Qx?8eLR<A
zu|^){u=H`f1|wow(?6mi2A9O<3a@P`Rny|ObDc@iO5zchmAt1vd7p`1uK&Z=PgLSa
zX=-&`aH|kTgJOtrz44Ea2dTfMQFV$u(Jk!P;{<3X`-*Gsh<@e{jhpzQQbqE+N7?Ug
zXZ?NW>jd4~3o-^@+?qASs28_9gY#TYRdPAAx6LqxnH}$zetg&qupRm6$B%PT4+?%@
z|C1`o`&EkfU3|y;(v$OEbRzAqp7&pP{dq_{aOnc`0izE&Ki~%aN52_5r5!GtKAb@g
z>5nwxXFU)1Q9qES=TFT2E?VENlg-&%GWoUyq>hD5D~|AdXkos>3ctIrzIyfibax2y
z`@6Y5_5;4v$VAtut^03Z-b4Krgm>}4S}WnPtz0)PeV{(_b*iFz$j3ai6U-ablfV!D
z!u0J<@?&ltZp4o(Z|<A-h<$(`S|_DBQ*(8+;pYZ@dVZndKK1*ub@b4}=6)edlinZZ
zePN%K%z)`H884gtMZ}NyyF-$b@u3*ai+I?bUwqHJ^as0IW&7O9iRDKRyv`SwIUjz~
z#0QM{?7cBlapEA~yF+-+T<aacJt$x@kzD^i5<YpJ`Co>*dRjEjq`sJW!sOI)?yU6x
q*>1?AZ)UBg)ZGln+d7uM58$J3A4xVp9ILO7|C#eXW%#BRng1_s;yP3S

diff --git a/ctre_sim/Pigeon 2 - 020 - 0 - ext.dat b/ctre_sim/Pigeon 2 - 020 - 0 - ext.dat
deleted file mode 100644
index 10deca5246fe2ba14f793023c11b424b9acecd2e..0000000000000000000000000000000000000000
GIT binary patch
literal 0
HcmV?d00001

literal 2048
zcmeHFJ4jqX6uoc#u2_g45=cT;MNL$M6p>BkLW*n?T@k^;VzCl~rqs$0Dy|^`o2Vhp
z7AAxcSfibV2rdd*sNioqQ4lNH1Qr#Gm$`H2;M*7NwshbzbMBpc?zv~)CcU)jer3#o
z4QjmiGh=QNEz(44&6_#G1^SSRXOM5DaK*MfMCT7yD|VI4c$VOzT+!TM{n3Y?JKtNc
zhlm=iQ;nx2P02`PlI!one2w$;NO;{i?vXg^p~sCQT<BQHBjmOuks7S#d5HG)d`s)1
zpXf#h&*S914YPDC&I2a~gbx^ZjBS9FrB3|0S0MjL>k73>Cg@GaziQ?xxgXZ??;qO#
z_MXjAj)k5ibbY{Fc>N|@O9>gC?Hu8YUWgB8u6|B_@kxkKAFP)C@CSTX-dNSRY8ogz
zdCdBvhWD`cx`6R)p7aeH9q5mG*311+k9pW5%<EB;AP;`u!Y3c+F}J#|mm5cyzv%xF
zet;jlDhf-b;(Az$<9~bi{z2ON4D6+2r+-}R#_;cXzW4?F%3y!_k@gP?{N*3@d?iK+
z;(<$fzxiG1k9g9=w=UXFzI~1R#(Je5dEVJCVB7~kT?wxhow54Xr0LtPaFc0I??zs<
z;5|4tU*XrjB9DD|dR8A}b@IP-8(!pXy@1ras-tZa)qfVf@1Omx%2Bb-{*I^4eo4Do
S_rrXr#q$0IhHCOa?*9YIF9A9L

diff --git a/ctre_sim/Talon FX vers. C - 01 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 01 - 0 - ext.dat
deleted file mode 100644
index d142e6329edc1050d22f354f46936dd454c6ee74..0000000000000000000000000000000000000000
GIT binary patch
literal 0
HcmV?d00001

literal 2048
zcmeHGJ7|+p6h28)O{zl%i;AxpX^RR)L1-8C_eF~!q!fp4*(^>L1f`%Bv=9fyNh)13
z)FMS_<CvkOHYli21+8WasfaG3V5p^q-+LZkO@1GPI(i|w_dAd8eD|FDM+c5y4~2=M
zw$IREpZ>3Z*nxSU``cH&-1^e7mTwM8zo$CXK1&y>k79pb?ja3Ui+_<{*nFf;&$LN_
z&Kbi`ERNQxVa4>PLV6G6@m+Ow2rr+nm}S`=;t>UdW<z~1MC7|wXf{2zlf@%0N2mPN
z5{@IeU+aGf=!qLYZ;qNy8g4Sec!L>sXuXLS<2|geGE8UK6JMi#S1ZhS@-7u*j?goA
zXhP!iM0xfXPN=`IUiXh1XOledVuNDvwTC_nG3vF4-U+^?<>*r;BYpc7<}m5;7Sj{J
z#jx6u4?RJgv3l6x2M#`|lD%Id?RWmM_RHN|`{EM0f3>_n!?m+xKXB=y@&QAKjRBB&
zY#80D(H*>KS#+d&)Y=`Z$M@UDc5ywdO75P_@ATQHSJ{@@R+4<r0*-Y>L`uqdo;NCA
zc2U3M_knt~{5*Fo<af7fee?%>OR>q8VbAGrUf9a|vc~&n&vFyvk+oVkZ8}gN`7V{X
z9`bP??h)=Ab~k|^{D}uQ670vlUAU|u&Q9Nv_lSOgAG=FRYo_Dcv;oH-_Wt?Tb@Qq1
z7Y7G1`UN83-BIlqaJt$h`Z{-(rGHSs-#o8<Uv*yDS6UzOWwUD((cANyk38PFKfpEd
zY2^IwzKE&WS3bCx<~hSr_qL{zouZpDuY>2b{?P3a!IOLSz4R}s{@%!#yjM@W-~Sl<
nKGCyL&A-;y($?r~2XcztG3|b`Snu3o@50^DVq$!IMf&v(pszKP

diff --git a/ctre_sim/Talon FX vers. C - 02 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 02 - 0 - ext.dat
deleted file mode 100644
index 024359a7ea2f4ddbf89581b633dd4f5b93c240a3..0000000000000000000000000000000000000000
GIT binary patch
literal 0
HcmV?d00001

literal 2048
zcmeHGO-NKx6h3c8>y5UUMN-j^qoMXk*vdB9D-0zNMq0I-T`miPLP(7erbUsPFx<?-
zL?kd<&O)9!6hxpwS+kqQ78MZ%4JHb&d+s@(ym{58T6Q<*ednI@edp&s_fpqQCr0F|
zou`ul{XhS30?Pr<_k3lr^`&7g-5$2{e%K-Y1-epslK6AE$23qW{6)UM>r_=eb6N`O
zoLBrz<EUO78(M$bkv@>ey~^pZy!<_+o^|t)tPwDvKInT6QDBbWp!KLFjYr%p^#+>~
zhNJnI=zojUlT>~-hMLYQu2aHfTpf*~H~nI=kLxRs&;`CpZjpPV72}?~Yrfqh>RIfZ
zw()tQJm33g<lf&V_VbQ~X5M!pE*N}Aov)S{dQHwJi|>dWeaU9++<u2S%+7eX))T>n
znD~*8dZIY1dN8;Ljy|cPzF#7lcmAo&%XBVtF^N3CFz>H$cz1jcT)ZlLz^Fsl07$$x
ztZvh3K0LQ%>qz&Bw<l4JAGA&EXFZ(q?3CSK<AIMi*q7d0wE3O}obIl%rKFW-saE)M
ztMVVekLZQ@dG9#L?`ai%%m;jHiJ6uW&(z<rvWM%-Des#@>vfFBwu){>>wrG;O)9Y-
z^3e~^2>r%QC%6ZH>d~zf-=l96Zch~F=I+_|h<Sh?)+A*l({SzFj?bTL|MKgq{e<V`
znP}$cdZ2@OMRg9kGA{_U*Z11`jyIL;{NTqsYX2_#%DobO#Fx~M_;;5jA9-x~KLZZM
zXOP4F4&NQKaOSY=*Ig4m#Dwg|2lLWA<KP~jkBi>*(U!Ja<2!LtaFnlK`8tohwf$j_
lfteK_Z?xO?ALVo9nwBPHK2u#{7gx`%+#fHbCij&s-w*M-J3{~f

diff --git a/ctre_sim/Talon FX vers. C - 03 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 03 - 0 - ext.dat
deleted file mode 100644
index 4cf50b960beecb8d9ab393405877b735ba9cc240..0000000000000000000000000000000000000000
GIT binary patch
literal 0
HcmV?d00001

literal 2048
zcmeHHJ!q3*5WX*IwW)#`EGqsrv9_p-g3vDNSFu>cA;rlpo1Mf#Pz$YrQiy}%B$X~1
zv}zIB;+8>D8x@LB1+8Wase?L52V*T2<K27r)a0u=sH3;s_uP9w&pq#(q|Y9^;)RIP
z=BMaLh5p+=RA8<m`kpQiw7%r7#cL_2_e8tKr|ClZN#xJRJ)*vH{x9ltTRLm_JTxlc
zIc0dp@@Sm~z8e38r+c7Ic2#S4edYVIMV9#^TBo4TVwmrHL>0DNlkqW6mXExY)!qB7
zdd3gv{7(TsQOgs@t?7i}Mr#-iTVSWo8+$R@Bm7E3bVg31YxJDg3CT`=f6m<__{<(2
zbNN}Kteoe%^gOp-{l`tyak=k&STXv=ycyRR^I~4!;p;k%7Sd^_+pn;sIgPg(p8(E>
zG>&@k3F5TPgTpy+@JSWy{Sv9(*{7-(Z?3v<q3BoL_h)$Jd00Mh;iC2f1`oS_z*YVm
zzZtnozoF=OBziQZJ5r19x82$$d|;)?yYBujc^?KPme^i!^}Zz>JznRiB&^T#dhM54
z)OYtTFt575+&c{Q-K{zw`k>!ZWPJCK&-|O_w+X+D^?kE<xl!=&TAiCT9+;1ME+v_V
zdh8>bVBaurf^+oGJh<8{=h&Omodxm2<D2e1LJ#zZ=c1%ma<9!B@wv+P&b_X=pK85)
z&X)ed2XmlTaL%3v)$7NAF2}DUR&@FyF8hn^|DyMmd8PA-G{0zZ%_4exUh7c@{eY|T
zlc@RKzR0Qa<@bYp4;<*^T_@rlj<;a3L+5mU>c+6c<NNfz^e!oX{4RWV%Kw$Dm00x7
oN!RDuh}O4kXlbkG_`%5)J(7|qCVXcXPtD&R$v2PgC^@~p0}z!nLI3~&

diff --git a/ctre_sim/Talon FX vers. C - 04 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 04 - 0 - ext.dat
deleted file mode 100644
index 6286d39ff38959e1ccce458afa63038bdb56ce52..0000000000000000000000000000000000000000
GIT binary patch
literal 0
HcmV?d00001

literal 2048
zcmeHIJ!q3*5WXa>Ce<N>Ma7?{k+%MTC<yJMeu`pIloThoY<5%-lz>{0LLC$*skB1|
zEmDLgju}d7gMtWE&}z1jI;e;!7;0%DzW4KlFP}eyI{Gg6J@?+-^W5E=HhTWZ^-!27
zYIcSW*6IKGhZC5u^L)?Lu5`b2RVz1#WZt=6_0Q79+T+-t(><b0t@Ic9#f^uW^h}!+
z=$tY9)Z%E3I#x`7Dx`fNk9*bAD|Pw%s(F^(N4!Nr#(d!SLPT|UitVPyELl9_@^rG^
zpl~&kdzJrdKu_H2vvH;AxZyS{7>}5vL-{6OOboET>M)(<oA@fdcU)oa$-7*XJwngi
zfk}xk5Eb~ocvSC;Yt?>QKbz!zmm-Q$Z+Ga66b4^s=%e6UN=KhZMr3Zk!W<?u-eh_L
zxD-}D@}Vb)M~n{!_rSp?Rkrs_r1LI3(RsPfbuKQE=jX5cGwk0T-vgH~X+2=*ur&Y@
zuMMkfINgUAt%#1)fO`95&G<pj-EGW=Q_bI({dH{raE*PbEoI5~Ea3357Lk&&I?r3R
zUUpG`$L|7s{`$Ok9OU<RD?jFgdP}jXu3^vVZ(rEV`m$E{&CcaE#-pp1H*Gq=k9?O(
z%!hpR!!ts^5!VUsQ9tqUMuP9rw+ojO#M$XP@*Xh{)Q5FRY0Y%CHmt+v4|Z_=b<=+Q
z^YUCY*JnN8!MuVzJEA%-5aco^L|<QLMdk;;{n?;B-*jKuSIUq0iun=$_JZakk1gu~
z_Qj`>^Sgc#<FhO8-AnVFgL}*+`$RWmKKsup|IqDG!Be~Ry$mj?{-}60pFitPo1Ag|
m|NU%C^G}U;^|ZQ+efh>b(i*7ynOi))aBr-XnAlpCd3^`p!7fGs

diff --git a/ctre_sim/Talon FX vers. C - 05 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 05 - 0 - ext.dat
deleted file mode 100644
index f00666a0da24aa3b549bec42e79ec67e8186510b..0000000000000000000000000000000000000000
GIT binary patch
literal 0
HcmV?d00001

literal 2048
zcmeHGOK4M35WPuSeW@<8uzv8<B+}Loh=R~=)Tby`MM!bwE<3xdASea3Accr1T}h>z
zENYP=v~ih5d9^`7gjUdMb|H14BBEfZrG@96&j~Nj&q7^#H*;t1%sFRf-g7P<zZr=d
z<EWi6M||`D{KE+>_%z@1wQC(OEo$Y?AkTZMOX9QUa_vd{&*>hUfm-P=>b)&T8}v-8
z6zH5$|C#2QbrM)r{h5gLfja6{P8Z|S{kle0>?P48&Va_i??#OA?eSVwk6NSo$Sat$
zex1T%qz?%H*N~os;*+t^=Ct}P3Yds#piTHDUX1sWzUq*<Kre|k@@~1J)RTMF<2^#p
z+~En%FB(&%ckhI}d+Wu1+BBP{eV1b518-mC3j>2M75T{iu+W*$xg5{!S3afjjJK+u
zkY9>Q9QDu>_H)VygLnAhPpYinuQ4+3;!~NI?Of(!3u%5q+@JZu-O)Sz@)f~@4;{J&
zxQ5n-)zyRc;yM-9k?ECaPrT88*g3Y7_;9L)2fV+wT_0{xEVI4L^{yryJK4liGKw?b
zEO_}P`H$a)_=5PfcO2CBbO=A@1H9$<Wc!e7^|vl=BYk<reY0n!h5V7V!kbkc;77eJ
zHN=N{^g}a3zcJeh-hrQdbUR7!=-a0A3H$l!d;C3O9>9mSMOmq|SerNC`h)FTc-^p{
z;JjR$P57h-JeXIQXSXBs0>OOh6zl6wRd{}gH|u5pH`!PImGC3KqH*NEy(IOhBTIVV
z2lBJ1`CY%r3E0*5_DgeZfO?!tce8Ht=^W}8{=vH=>`(5M@1<{9^gA4tdb;{n8=c{e
nXBg-CsMOyYYwsk@2MhIiWGRmh>783Tw|IZFlpNnt<#~MvZHq1{

diff --git a/ctre_sim/Talon FX vers. C - 06 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 06 - 0 - ext.dat
deleted file mode 100644
index 753dcc6ef6d05468a0949023a34c7a05a14f50ae..0000000000000000000000000000000000000000
GIT binary patch
literal 0
HcmV?d00001

literal 2048
zcmeHHJ!q3r6n*)rCe<NBgG&8rBc^CUDhSD<eu`ocloTf?v)NIhpcJeDDa1i>l1i5h
zTBHa~)`;@e1_cpXL93Yzse?F(f}t%H;=4a5eEHNx9evCD&VBcubKbo#iCj8<BM>wu
zqWhdV(lY<oKkUF#i~4)6Ii7rNS?jm*-0xCK;`8Qm^I7!I<(`_cX7w-X-EBuZ{5(-B
z@HwaWh31(}68NtEvjMpW>gcYrQp`)=*EF(jKVo44V;Y0L6ELP_mm60<x*N?$UfG;(
zwIwV^;(+LX^ZAJ>KW&!UoKoDagt3qYc8T81tLb6#*O)LD=p?p5&aGCE?&Mr`d5-Y2
zaCnCEE5=ml+&v-Z?q=CPZ<$ZfysII>;M*Vg%EZv?4=ggiEpp~dp}>9n9i}wyaj*LE
z;c8IgsD~dvE~p+hoCEuxR88NnG1BkKbLp48x%9;rQvbZXzrx<x(K&GKitqu$hmHZz
z(AY4#cF=w}kvjXx4ofr>?ZgiU?(HT$tVa0(&u=yL={m)-J8E3-Xu`3RVYZT0p2aTV
zE3U}z_@htH%TII1LVYGF`sfe%)}pg}CLHTOzPz3M6_xkhzV&XxlN&`hr#_&MdRuBp
z5B0bY^$7P3*_+@T{JHYYK03#}Z8|SMb{^g3_lSOgAG<BeNu}l5+Jn!Z_K~Hx9rN+_
z%dy$SPkx|-e))CwMx<X5$feJ)zjS|{`-eE*&CsgMtN2Frkzdz1@;{79J?dyDKfs;&
zd6enD6GM70c)slfO$@irn_hZpjt%r=!tqif&AtiKI&@L=^LHj0&ko9a8Cetm@Zs>O
u{jap%Xq6sknCIn`)W07}4s_Z0!Lqj>c}k@fURXK1e1EFiH@&mL{rUl;@iMId

diff --git a/ctre_sim/Talon FX vers. C - 07 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 07 - 0 - ext.dat
deleted file mode 100644
index cfef40b12e2f46632ff37432de04e7a5d599f6ab..0000000000000000000000000000000000000000
GIT binary patch
literal 0
HcmV?d00001

literal 2048
zcmeHGJ!n%=6uwDX`%)b;SXBIJ8fohfh=R~A>QfX;p_1a{md%b@1f`$`q!0(iNh)13
zXpthc(Je!HwLw9IDrz-bNF7u}6b!Yr@Vw{e3op-~K^?uzx#!$-zVAEdyyu)fdNmR?
z#!)+K4*BN)`G*r&@M*rMYL~iRTGYz*EYEwoN8)qleC<*E&*>hTky`05>b*^e8}!Vm
z6zH5)|B2?AwGvoW{ppDGfja6{P7mYK{jx?@>?P48&WOgq??#OA?eW@Fk6NSo$Saru
zzfNH>Qu~DeYe-K*@yS?d)31K30w!V_XcxZ8=M#gZuR3Z@(@SEFyj!j)_2gdkc#qIC
ze{hoXi^de`-8&}l-a4_LHq51H-=&!Nz}pk~!oc9`h<s%KhR~VMxg5{!S3afjj5n*E
zkY9>Q9QDu>_H)VygLnAhPpYinuQ4+3;$xYY?Of(!3u%5q+@JZu-O)Sz@&&<z4;{J&
zxQ5n-)zyRc;yM-9ksg$2U%b)3*L{0C@!?bpcX@y9J3d^YSbA%j>s?Jaa=eM9q!s5`
zv*6{I<Uf8F;tS%_-f>Xh*CqUz5Ac@bQ=Ox()!(+bh4keW_sy=AR`SPMgg2u)z>j)c
zYKRZ@=!a&6eq**1yaPXZ|5}pX(YH<K6ZZ2nxA=R+Jb({ti!xGau{N&9^#?n&@Va3?
z!FjnhoA5~wcrdRp&rV0?1p@a(KkMu5sPOy{Z_&%%Z?doaE8$0eMdQeSdrs<6N0#)!
z59DW1^SgeL6R@lA?U&}-0QESX>Sf*J(>ZWP__H_1*q_=h-^<Xl=yy0O^>p>EHagiC
o1B~-@T<V7(bapq}`2IqD9vRA`Lwe_zPA=XVFC{0oRe4_D0lQ8v6951J

diff --git a/ctre_sim/Talon FX vers. C - 09 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 09 - 0 - ext.dat
deleted file mode 100644
index e02d26f6bbce8a5686e7e423f0c5a6075d13a911..0000000000000000000000000000000000000000
GIT binary patch
literal 0
HcmV?d00001

literal 2048
zcmeHHJ!q3r6n$TkYEm6CG^o^{G-8Shq9CM;`dJjCAf`AunL)5)1wkpO0V%{mags`x
z3|gfKZPo~q+MpmpBWN{SNFAhuC>YvOA-?-_!k157)X}%R@7#CqIp^K`lJKQtx14}6
zVcloVk(T+t{$U3eTGZcj&FjgxmbHE-$Nip3Nqo*+X}*a3x!iLz+N}IVeR<2#4nNP-
z3VhBgex-S4g9N^-|BNH|KpovxR*HG)`-Vo=>_;>tU{qt!cO7F|c9mo5M|Y$7$Sa!D
zt+s^aNE{ITZ$3X!<)_V3n^TIrl`tCAzz)%ydNVmh{u*QEBArCn$+^`E(4E|CWu7Da
z%pacO{E{&xIxioW^YTX7KW~~#(7Y=_!Qk8Hd}U(j^*L3>cSX*8$>+Imzr&QqJ?>RM
zK3oY%9QE+y$9dJmhI3&5ld9?aHAebfdMW*~H<!NHLh7HF_gC0EJ30rhT@^lH_|P!`
z8X6l$*AChbH(X~QnIVa$Bc1q3|AU>Rht()P=J~CpmTpokv#rMUt|kng2(gun@>IKo
zudpn?<4-<4FF(y43-#%w=%YX2Ta8Tb8gs4x*y2|5S5V#$d)B%MkFOWqtondH>TRhZ
zJ=Ehq)Fa$CXm5gZ@Mnv+<8+RD+jL%j>^{BE?-Bg~KXzM`l}gLCxd)#=?ZXT2JLco<
zmus_$pZq`v{qpPV4oklvP`z-5{SEZhxqpb`{qA3pc@^G?KJx1tNB+mlQja>?$q#U6
zehy{&@5GSa3!ZN~K@&r*^QM<xnrj0+=}>GYF~GhF(>gRF`nh}KjHma?dl_C8|M(ra
wd}{wItv6cnrIXC_dP3?S4JP}$Z2Vx++m9Tj(hAKlpIv-7QHf7(Z*aeU0Qz$?YXATM


From 37e1d5c2553695a611329809d2712a522e10c9f6 Mon Sep 17 00:00:00 2001
From: = <paul.hodges@student.education.wa.edu.au>
Date: Sun, 3 Mar 2024 14:09:12 +0800
Subject: [PATCH 188/207] add more behaviours

---
 src/main/cpp/AlphaArm.cpp                     |  20 +-
 src/main/cpp/AlphaArmBehaviour.cpp            |   5 +-
 src/main/cpp/Climber.cpp                      |  51 ++---
 src/main/cpp/ClimberBehaviour.cpp             |   8 +-
 src/main/cpp/Intake.cpp                       |  38 ++--
 src/main/cpp/IntakeBehaviour.cpp              |   6 +-
 src/main/cpp/Robot.cpp                        |  59 +++---
 src/main/cpp/Shooter.cpp                      |   4 +-
 src/main/cpp/ShooterBehaviour.cpp             |   3 +-
 src/main/cpp/vision/Vision.cpp                |   4 +-
 src/main/cpp/vision/VisionBehaviours.cpp      |   3 +
 src/main/include/AlphaArm.h                   |   7 +-
 src/main/include/AlphaArmBehaviour.h          |   3 -
 src/main/include/ClimberBehaviour.h           |   2 +-
 src/main/include/IntakeBehaviour.h            |   6 +-
 src/main/include/Robot.h                      |  23 +-
 src/main/include/RobotMap.h                   | 114 +++++-----
 .../src/main/cpp/drivetrain/SwerveDrive.cpp   | 196 +++++++-----------
 wombat/src/main/cpp/subsystems/Arm.cpp        |  35 ++--
 wombat/src/main/cpp/subsystems/Elevator.cpp   |  30 +--
 wombat/src/main/cpp/subsystems/Shooter.cpp    |  22 +-
 wombat/src/main/cpp/utils/Encoder.cpp         |  21 +-
 wombat/src/main/cpp/utils/Util.cpp            |  12 +-
 wombat/src/main/include/Wombat.h              |   4 +-
 wombat/src/main/include/behaviour/Behaviour.h |   6 +-
 .../src/main/include/drivetrain/SwerveDrive.h |  36 ++--
 wombat/src/main/include/subsystems/Arm.h      |   3 +-
 wombat/src/main/include/subsystems/Shooter.h  |   3 +-
 wombat/src/main/include/utils/Encoder.h       |   2 +-
 29 files changed, 298 insertions(+), 428 deletions(-)

diff --git a/src/main/cpp/AlphaArm.cpp b/src/main/cpp/AlphaArm.cpp
index aa107745..233fce50 100644
--- a/src/main/cpp/AlphaArm.cpp
+++ b/src/main/cpp/AlphaArm.cpp
@@ -5,9 +5,9 @@
 #include "AlphaArm.h"
 
 
-AlphaArm::AlphaArm(AlphaArmConfig *config/*, frc::Rotation2d initialAngle, wom::vision::Limelight* vision */) : _config(config), _pidArm{frc::PIDController(1.2, 0.4, 0)}, _pidArmStates{frc::PIDController(37, 0.00070, 0.15)}, _pidIntakeState{frc::PIDController(30, 0.00015, 0.005)}, _pidClimberStates{frc::PIDController(25, 0.00015, 0.005)} 
+AlphaArm::AlphaArm(AlphaArmConfig *config/*, frc::Rotation2d initialAngle, wom::vision::Limelight* vision */) : _config(config), _pidArm{frc::PIDController(1.2, 0.4, 0)}, _pidArmStates{frc::PIDController(37, 0.00070, 0.15)}, _pidIntakeState{frc::PIDController(30, 0.00015, 0.005)}, _pidClimberStates{frc::PIDController(25, 0.00015, 0.005)}
 {
- 
+
 }
 
 void AlphaArm::OnStart(){
@@ -19,25 +19,25 @@ void AlphaArm::OnStart(){
 void AlphaArm::OnUpdate(units::second_t dt) {
   switch (_state) {
     case AlphaArmState::kIdle:
-    
+
       _setAlphaArmVoltage = 0_V;
       break;
     case AlphaArmState::kRaw:
     std::cout << "RawControl" << std::endl;
        _setAlphaArmVoltage = _rawArmVoltage;
-      
+
       break;
 
     case AlphaArmState::kHoldAngle:
     {
       _pidIntakeState.SetSetpoint(-_goal);
-       // _setAlphaArmVoltage = _pidArm.Calculate(_alphaArm->GetConfig().config->alphaArmEncoder.GetEncoderPosition());
+       //_setAlphaArmVoltage = _pidArm.Calculate(_alphaArm->GetConfig().config->alphaArmEncoder.GetEncoderPosition());
       _setAlphaArmVoltage = units::volt_t{_pidIntakeState.Calculate(-(_config->alphaArmEncoder.GetEncoderPosition().value() * (2 * 3.1415)))};
     }
     break;
     case AlphaArmState::kAmpAngle:
       std::cout << "Amp Angle" << std::endl;
-      
+
        _pidArmStates.SetSetpoint(-2.12);
        _setAlphaArmVoltage = units::volt_t{_pidArmStates.Calculate(-(_config->alphaArmEncoder.GetEncoderPosition().value() * (2 * 3.1415)))};
 
@@ -45,7 +45,7 @@ void AlphaArm::OnUpdate(units::second_t dt) {
 
     case AlphaArmState::kIntakeAngle:
     std::cout << "Intake Angle" << std::endl;
-       _pidIntakeState.SetSetpoint(-0.50); // -0.48
+       _pidIntakeState.SetSetpoint(-0.50); //-0.48
        _setAlphaArmVoltage = units::volt_t{_pidIntakeState.Calculate(-(_config->alphaArmEncoder.GetEncoderPosition().value() * (2 * 3.1415)))};
     break;
     case AlphaArmState::kIntakedAngle:
@@ -55,13 +55,13 @@ void AlphaArm::OnUpdate(units::second_t dt) {
     break;
     case AlphaArmState::kClimbAngle:
     std::cout << "Climb Angle" << std::endl;
-       _pidArmStates.SetSetpoint(-2.00); // -0.48
+       _pidArmStates.SetSetpoint(-2.00); //-0.48
        _setAlphaArmVoltage = units::volt_t{_pidArmStates.Calculate(-(_config->alphaArmEncoder.GetEncoderPosition().value() * (2 * 3.1415)))};
     break;
 
     case AlphaArmState::kClimbed:
     std::cout << "Climb Angle" << std::endl;
-       _pidClimberStates.SetSetpoint(-0.5); //- 0.48
+       _pidClimberStates.SetSetpoint(-0.5); //-0.48
        _setAlphaArmVoltage = units::volt_t{_pidClimberStates.Calculate(-(_config->alphaArmEncoder.GetEncoderPosition().value() * (2 * 3.1415)))};
     break;
 
@@ -121,5 +121,3 @@ void AlphaArm::SetGoal(double goal){
 void AlphaArm::SetControllerRaw(units::volt_t voltage){
   _controlledRawVoltage = voltage;
 }
-
-
diff --git a/src/main/cpp/AlphaArmBehaviour.cpp b/src/main/cpp/AlphaArmBehaviour.cpp
index 47fad071..6c449c64 100644
--- a/src/main/cpp/AlphaArmBehaviour.cpp
+++ b/src/main/cpp/AlphaArmBehaviour.cpp
@@ -19,7 +19,7 @@ void AlphaArmManualControl::OnTick(units::second_t dt) {
 
   _table->GetEntry("State").SetBoolean(_rawControl);
   _table->GetEntry("Goal Value").SetBoolean(_gotValue);
-  
+
 
   if (_codriver->GetBackButton()) {
     if (_rawControl == true) {
@@ -63,8 +63,7 @@ void AlphaArmManualControl::OnTick(units::second_t dt) {
     }
 
   }
-  
+
 }
 
-  
 
diff --git a/src/main/cpp/Climber.cpp b/src/main/cpp/Climber.cpp
index b05de086..c0e263d7 100644
--- a/src/main/cpp/Climber.cpp
+++ b/src/main/cpp/Climber.cpp
@@ -8,60 +8,54 @@ Climber::Climber(ClimberConfig config) : _config(config), _pid(frc::PIDControlle
 
 void Climber::OnUpdate(units::second_t dt) {
   switch (_state) {
-    case ClimberState::kIdle: 
-    {
+    case ClimberState::kIdle: {
       _stringStateName = "Idle";
       _setVoltage = 0_V;
-    } 
-    break;
+    } break;
 
-    case ClimberState::kRatchet: 
+    case ClimberState::kRatchet:
     {
       _stringStateName = "Ratchet";
       _setVoltage = 0_V;
       // armServo.SetAngle(0);
-    } 
+    }
     break;
 
-    case ClimberState::kArmUp: 
+    case ClimberState::kArmUp:
     {
       _stringStateName = "Arm Up";
       // _setVoltage = 8_V;
 
-      _pid.SetSetpoint(0.035*100);
-      _setVoltage = -units::volt_t{_pid.Calculate((-_config.climberGearbox.encoder->GetEncoderPosition().value()) * 100)};
-      _table->GetEntry("PID OUTPUT").SetDouble(_pid.Calculate((-_config.climberGearbox.encoder->GetEncoderPosition().value()) * 100));
-    } 
-    break;
+      _pid.SetSetpoint(0.035 * 100);
+      _setVoltage = -units::volt_t{
+          _pid.Calculate((-_config.climberGearbox.encoder->GetEncoderPosition().value()) * 100)};
+      _table->GetEntry("PID OUTPUT")
+          .SetDouble(_pid.Calculate((-_config.climberGearbox.encoder->GetEncoderPosition().value()) * 100));
+    } break;
 
-    case ClimberState::kArmDown: 
-    {
+    case ClimberState::kArmDown: {
       _stringStateName = "Arm Down";
       _pid.SetSetpoint(0.07*100);
       _setVoltage = -units::volt_t{_pid.Calculate((-_config.climberGearbox.encoder->GetEncoderPosition().value()) * 100)};
       // armServo.SetAngle(180);
       // _setVoltage = -8_V;
-    } 
-    break;
+    } break;
 
-    case ClimberState::kMatch: 
-    {
+    case ClimberState::kMatch: {
       _stringStateName = "Match";
-      _pid.SetSetpoint(0.005*100);
-      _setVoltage = -units::volt_t{_pid.Calculate((-_config.climberGearbox.encoder->GetEncoderPosition().value()) * 100)};
+      _pid.SetSetpoint(0.005 * 100);
+      _setVoltage = -units::volt_t{
+          _pid.Calculate((-_config.climberGearbox.encoder->GetEncoderPosition().value()) * 100)};
 
       // _pid.SetSetpoint()
       // _setVoltage = -8_V;
-    } 
-    break;
+    } break;
 
-    case ClimberState::kRaw:
-    {
+    case ClimberState::kRaw: {
       _stringStateName = "Raw";
       _setVoltage = _rawVoltage;
-    }  
-    break;
-    
+    } break;
+
     default:
       std::cout << "Error climber in invalid state" << std::endl;
       break;
@@ -70,7 +64,8 @@ void Climber::OnUpdate(units::second_t dt) {
 
   _table->GetEntry("State: ").SetString(_stringStateName);
   _table->GetEntry("Motor Voltage: ").SetDouble(_setVoltage.value());
-  _table->GetEntry("Encoder Output: ").SetDouble(_config.climberGearbox.encoder->GetEncoderPosition().value() * 100);
+  _table->GetEntry("Encoder Output: ")
+      .SetDouble(_config.climberGearbox.encoder->GetEncoderPosition().value() * 100);
 }
 
 void Climber::SetState(ClimberState state) {
diff --git a/src/main/cpp/ClimberBehaviour.cpp b/src/main/cpp/ClimberBehaviour.cpp
index 59c0a96f..f958b2a2 100644
--- a/src/main/cpp/ClimberBehaviour.cpp
+++ b/src/main/cpp/ClimberBehaviour.cpp
@@ -10,8 +10,7 @@ ClimberManualControl::ClimberManualControl(Climber* climber, AlphaArm* arm, frc:
 }
 
 void ClimberManualControl::OnTick(units::second_t dt) {
-
-  if (_codriver->GetBackButtonPressed()) {
+  if (_codriver->GetAButtonPressed()) {
     if (_rawControl) {
       _rawControl = false;
     } else {
@@ -19,7 +18,6 @@ void ClimberManualControl::OnTick(units::second_t dt) {
     }
   }
 
-
   if (_rawControl) {
     _climber->SetState(ClimberState::kRaw);
     if (wom::deadzone(_codriver->GetLeftY())) {
@@ -31,13 +29,11 @@ void ClimberManualControl::OnTick(units::second_t dt) {
     if (_codriver->GetPOV() == 90) {
       _climber->SetState(ClimberState::kMatch);
       _arm->SetState(AlphaArmState::kClimbAngle);
-    } 
-    else if (_codriver->GetPOV() == 180) {
+    } else if (_codriver->GetPOV() == 180) {
       _climber->SetState(ClimberState::kArmDown);
     } else  if (_codriver->GetPOV() == 270){
       _climber->SetState(ClimberState::kRatchet);
       _arm->SetState(AlphaArmState::kClimbed);
     }
   }
-
 }
diff --git a/src/main/cpp/Intake.cpp b/src/main/cpp/Intake.cpp
index 0313ea98..f1848623 100644
--- a/src/main/cpp/Intake.cpp
+++ b/src/main/cpp/Intake.cpp
@@ -17,9 +17,9 @@ void Intake::OnStart() {
 void Intake::OnUpdate(units::second_t dt) {
 
   switch (_state) {
-    case IntakeState::kIdle: 
+    case IntakeState::kIdle:
     {
-      if (_config.intakeSensor->Get() == false) { 
+      if (_config.intakeSensor->Get() == false) {
         SetState(IntakeState::kHold);
       }
       _stringStateName = "Idle";
@@ -27,29 +27,29 @@ void Intake::OnUpdate(units::second_t dt) {
       _setVoltage = 0_V;
       _recordNote = false;
       hasValue = false;
-    } 
+    }
     break;
 
-    case IntakeState::kRaw: 
+    case IntakeState::kRaw:
     {
       _stringStateName = "Raw";
       _pid.Reset();
       _setVoltage = _rawVoltage;
-    } 
+    }
     break;
 
-    case IntakeState::kEject: 
+    case IntakeState::kEject:
     {
       if (_config.intakeSensor->Get() == true && _config.passSensor->Get() == true) {
-        SetState(IntakeState::kIdle); 
+        SetState(IntakeState::kIdle);
       }
       _stringStateName = "Eject";
       _setVoltage = 8_V;
       _pid.Reset();
-    } 
+    }
     break;
 
-    case IntakeState::kHold: 
+    case IntakeState::kHold:
     {
       units::volt_t pidCalculate = 0_V;
       if (_config.intakeSensor->Get() == true && _config.passSensor->Get() == true) {
@@ -62,34 +62,34 @@ void Intake::OnUpdate(units::second_t dt) {
       } else {
         pidCalculate = units::volt_t{_pidPosition.Calculate(_config.IntakeGearbox.encoder->GetEncoderPosition().value())};
       }
-      
+
       _setVoltage = pidCalculate;
       _stringStateName = "Hold";
     }
     break;
 
-    case IntakeState::kIntake: 
+    case IntakeState::kIntake:
     {
       if (_config.intakeSensor->Get() == false) {
-       
+
         if (_config.IntakeGearbox.encoder->GetEncoderPosition().value() < 0) {
           _pidPosition.SetSetpoint((-_config.IntakeGearbox.encoder->GetEncoderPosition().value()) - 0.5);
         } else {
           _pidPosition.SetSetpoint(_config.IntakeGearbox.encoder->GetEncoderPosition().value() + 0.5 );
         }
-  
+
         SetState(IntakeState::kHold);
       }
       _stringStateName = "Intake";
-      _setVoltage = -10_V; 
-    } 
+      _setVoltage = -10_V;
+    }
     break;
 
-    case IntakeState::kPass: 
+    case IntakeState::kPass:
     {
       if (_config.intakeSensor->Get() == true) {
         SetState(IntakeState::kIdle);
-      } 
+      }
 
       if (!_recordNote) {
         _noteShot ++;
@@ -110,12 +110,12 @@ void Intake::OnUpdate(units::second_t dt) {
   _table->GetEntry("Encoder").SetDouble(_config.IntakeGearbox.encoder->GetEncoderPosition().value());
   _table->GetEntry("Shot Count").SetDouble(_noteShot);
   // _table->GetEntry("Encoder: ").SetDouble(_config.IntakeGearbox.encoder->GetEncoderPosition().value());
- 
+
   _config.IntakeGearbox.motorController->SetVoltage(_setVoltage);
 }
 
 void Intake::SetState(IntakeState state) {
-  
+
   _state = state;
 }
 void Intake::SetRaw(units::volt_t voltage) {
diff --git a/src/main/cpp/IntakeBehaviour.cpp b/src/main/cpp/IntakeBehaviour.cpp
index a88d94cb..cd67d1ac 100644
--- a/src/main/cpp/IntakeBehaviour.cpp
+++ b/src/main/cpp/IntakeBehaviour.cpp
@@ -20,8 +20,8 @@ void IntakeManualControl::OnTick(units::second_t dt) {
       _rawControl = true;
       _intake->SetState(IntakeState::kRaw);
     }
-  } 
-  
+  }
+
   if (_rawControl) {
     _intake->SetState(IntakeState::kRaw);
     if (_codriver.GetLeftTriggerAxis() > 0.1) {
@@ -32,7 +32,7 @@ void IntakeManualControl::OnTick(units::second_t dt) {
       _intake->SetRaw(0_V);
     }
   } else {
-    
+
 
     if (_codriver.GetXButton()) {
       if (_intake->GetState() == IntakeState::kIdle) {
diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp
index bde3baa4..250788ea 100644
--- a/src/main/cpp/Robot.cpp
+++ b/src/main/cpp/Robot.cpp
@@ -13,6 +13,7 @@
 #include <networktables/NetworkTable.h>
 #include <networktables/NetworkTableInstance.h>
 #include <units/acceleration.h>
+// #include <units/angle.h>
 
 // include units
 #include <units/velocity.h>
@@ -23,7 +24,6 @@
 #include <units/velocity.h>
 #include <units/voltage.h>
 
-
 #include "behaviour/HasBehaviour.h"
 #include "networktables/NetworkTableInstance.h"
 
@@ -38,14 +38,7 @@ static units::second_t lastPeriodic;
 
 void Robot::RobotInit() {
   sched = wom::BehaviourScheduler::GetInstance();
-
-  frc::SmartDashboard::PutData("Auto Selector", &m_chooser);
-
-  m_chooser.SetDefaultOption(defaultAuto, defaultAuto);
-
-  for (auto& option : autoOptions) {
-    m_chooser.AddOption(option, option);
-  }
+  m_chooser.SetDefaultOption("Default Auto", "Default Auto");
 
   _led = new LED();
 
@@ -64,15 +57,16 @@ void Robot::RobotInit() {
   alphaArm = new AlphaArm(&robotmap.alphaArmSystem.config);
   intake = new Intake(robotmap.intakeSystem.config);
 
-
   wom::BehaviourScheduler::GetInstance()->Register(alphaArm);
-  alphaArm->SetDefaultBehaviour(
-      [this]() { return wom::make<AlphaArmManualControl>(alphaArm, intake, &robotmap.controllers.codriver); });
+  alphaArm->SetDefaultBehaviour([this]() {
+    return wom::make<AlphaArmManualControl>(alphaArm, intake, &robotmap.controllers.codriver);
+  });
 
   climber = new Climber(robotmap.climberSystem.config);
   wom::BehaviourScheduler::GetInstance()->Register(climber);
-  climber->SetDefaultBehaviour(
-      [this]() { return wom::make<ClimberManualControl>(climber, alphaArm, &robotmap.controllers.codriver); });
+  climber->SetDefaultBehaviour([this]() {
+    return wom::make<ClimberManualControl>(climber, alphaArm, &robotmap.controllers.codriver);
+  });
 
   wom::BehaviourScheduler::GetInstance()->Register(intake);
   intake->SetDefaultBehaviour(
@@ -83,9 +77,6 @@ void Robot::RobotInit() {
   robotmap.swerveBase.moduleConfigs[2].turnMotor.encoder->SetEncoderOffset(3.01121_rad);
   robotmap.swerveBase.moduleConfigs[3].turnMotor.encoder->SetEncoderOffset(4.4524_rad);
 
-  robotmap._builder = autos::InitCommands(_swerveDrive, shooter, intake, alphaArm);
-  robotmap._simSwerve = new wom::SimSwerve(_swerveDrive);
-
   lastPeriodic = wom::now();
 }
 
@@ -96,7 +87,7 @@ void Robot::RobotPeriodic() {
 
   loop.Poll();
   wom::BehaviourScheduler::GetInstance()->Tick();
-  //sched->Tick();
+  // sched->Tick();
 
   // robotmap.swerveTable.swerveDriveTable->GetEntry("frontLeftEncoder")
   //     .SetDouble(robotmap.swerveBase.moduleConfigs[0].turnMotor.encoder->GetEncoderPosition().value());
@@ -108,16 +99,19 @@ void Robot::RobotPeriodic() {
   //     .SetDouble(robotmap.swerveBase.moduleConfigs[3].turnMotor.encoder->GetEncoderPosition().value());
 
   // _swerveDrive->OnUpdate(dt);
-  //shooter->OnStart();
-  //intake->OnUpdate(dt);
+  // shooter->OnStart();
+  // intake->OnUpdate(dt);
 
   // _swerveDrive->OnUpdate(dt);
 
-
-//   robotmap.swerveTable.swerveDriveTable->GetEntry("Encoder 0 offset: ").SetDouble(robotmap.swerveBase.moduleConfigs[0].turnMotor.encoder->GetEncoderPosition().value());
-//   robotmap.swerveTable.swerveDriveTable->GetEntry("Encoder 1 offset: ").SetDouble(robotmap.swerveBase.moduleConfigs[1].turnMotor.encoder->GetEncoderPosition().value());
-//   robotmap.swerveTable.swerveDriveTable->GetEntry("Encoder 2 offset: ").SetDouble(robotmap.swerveBase.moduleConfigs[2].turnMotor.encoder->GetEncoderPosition().value());
-//   robotmap.swerveTable.swerveDriveTable->GetEntry("Encoder 3 offset: ").SetDouble(robotmap.swerveBase.moduleConfigs[3].turnMotor.encoder->GetEncoderPosition().value());
+  //   robotmap.swerveTable.swerveDriveTable->GetEntry("Encoder 0 offset:
+  //   ").SetDouble(robotmap.swerveBase.moduleConfigs[0].turnMotor.encoder->GetEncoderPosition().value());
+  //   robotmap.swerveTable.swerveDriveTable->GetEntry("Encoder 1 offset:
+  //   ").SetDouble(robotmap.swerveBase.moduleConfigs[1].turnMotor.encoder->GetEncoderPosition().value());
+  //   robotmap.swerveTable.swerveDriveTable->GetEntry("Encoder 2 offset:
+  //   ").SetDouble(robotmap.swerveBase.moduleConfigs[2].turnMotor.encoder->GetEncoderPosition().value());
+  //   robotmap.swerveTable.swerveDriveTable->GetEntry("Encoder 3 offset:
+  //   ").SetDouble(robotmap.swerveBase.moduleConfigs[3].turnMotor.encoder->GetEncoderPosition().value());
 
   _led->OnUpdate(dt);
   shooter->OnUpdate(dt);
@@ -125,7 +119,6 @@ void Robot::RobotPeriodic() {
   _swerveDrive->OnUpdate(dt);
   intake->OnUpdate(dt);
   climber->OnUpdate(dt);
-
 }
 
 void Robot::AutonomousInit() {
@@ -136,16 +129,16 @@ void Robot::AutonomousInit() {
 
   m_autoSelected = m_chooser.GetSelected();
 
+  robotmap._builder = autos::InitCommands(_swerveDrive, shooter, intake, alphaArm);
+
   if (m_autoSelected == "kTaxi") {
     sched->Schedule(autos::Taxi(robotmap._builder));
   }
-}
 
-void Robot::AutonomousPeriodic() {
-  robotmap._simSwerve->OnTick(_swerveDrive->GetSetpoint());
-  _swerveDrive->MakeAtSetPoint();
+  _swerveDrive->OnStart();
 }
 
+void Robot::AutonomousPeriodic() {}
 
 void Robot::TeleopInit() {
   loop.Clear();
@@ -157,13 +150,12 @@ void Robot::TeleopInit() {
   // backLeft->SetVoltage(4_V);
   // backRight->SetVoltage(4_V);
 
-
   //  FMAP("fmap.fmap");
 
   // _swerveDrive->OnStart();
   // sched->InterruptAll();
 
-  //reimplement when vision is reimplemented
+  // reimplement when vision is reimplemented
 
   // _swerveDrive->SetPose(_vision->GetAngleToObject(VisionTargetObjects::kNote).first);
   armServo->SetAngle(130);
@@ -188,5 +180,4 @@ void Robot::DisabledInit() {}
 void Robot::DisabledPeriodic() {}
 
 void Robot::TestInit() {}
-void Robot::TestPeriodic() {
-}
+void Robot::TestPeriodic() {}
diff --git a/src/main/cpp/Shooter.cpp b/src/main/cpp/Shooter.cpp
index 807e90d9..5fb03505 100644
--- a/src/main/cpp/Shooter.cpp
+++ b/src/main/cpp/Shooter.cpp
@@ -13,7 +13,8 @@ void Shooter::OnStart() {
 void Shooter::OnUpdate(units::second_t dt) {
   table->GetEntry("Error").SetDouble(_pid.GetPositionError());
   table->GetEntry("SetPoint").SetDouble(_pid.GetSetpoint());
-  table->GetEntry("Encoder Output").SetDouble(_config.ShooterGearbox.encoder->GetEncoderAngularVelocity().value());
+  table->GetEntry("Encoder Output")
+      .SetDouble(_config.ShooterGearbox.encoder->GetEncoderAngularVelocity().value());
   table->GetEntry("Shooting").SetString(_statename);
   // table->GetEntry("PID CONTROLLER").Set(_pid);
 
@@ -95,7 +96,6 @@ void Shooter::OnUpdate(units::second_t dt) {
   }
   // table->GetEntry("Motor OutPut").SetDouble(_setVoltage.value());
 
-
   _config.ShooterGearbox.motorController->SetVoltage(_setVoltage);
 }
 
diff --git a/src/main/cpp/ShooterBehaviour.cpp b/src/main/cpp/ShooterBehaviour.cpp
index 9880482e..c925c18f 100644
--- a/src/main/cpp/ShooterBehaviour.cpp
+++ b/src/main/cpp/ShooterBehaviour.cpp
@@ -37,9 +37,8 @@ void ShooterManualControl::OnTick(units::second_t dt) {
       _shooter->SetPidGoal(300_rad_per_s);
       _shooter->SetState(ShooterState::kSpinUp);
       _led->SetState(LEDState::kAiming);
-      } else if (_codriver->GetBButton()) {
+    } else if (_codriver->GetBButton()) {
       _shooter->SetState(ShooterState::kReverse);
-      
 
     } else if (_codriver->GetAButton()) {
       _shooter->SetPidGoal(1500_rad_per_s);
diff --git a/src/main/cpp/vision/Vision.cpp b/src/main/cpp/vision/Vision.cpp
index 284e16e5..3360833d 100644
--- a/src/main/cpp/vision/Vision.cpp
+++ b/src/main/cpp/vision/Vision.cpp
@@ -331,9 +331,9 @@ frc::Pose2d Vision::TurnToTarget(VisionTarget target, wom::SwerveDrive* swerveDr
 
   units::degree_t angle = GetDistanceToTarget(target).second;
 
-  // frc::Pose2d current_pose = _limelight->GetPose().ToPose2d();
+  frc::Pose2d current_pose = _limelight->GetPose().ToPose2d();
 
-  frc::Pose2d current_pose = swerveDrive->GetPose();
+  // frc::Pose2d current_pose = swerveDrive->GetPose();
 
   frc::Pose2d pose = frc::Pose2d(current_pose.X(), current_pose.Y(), angle);
 
diff --git a/src/main/cpp/vision/VisionBehaviours.cpp b/src/main/cpp/vision/VisionBehaviours.cpp
index 03bbadf1..b8bd3b40 100644
--- a/src/main/cpp/vision/VisionBehaviours.cpp
+++ b/src/main/cpp/vision/VisionBehaviours.cpp
@@ -42,6 +42,8 @@ LockOnToTarget::LockOnToTarget(wom::PhotonVision* vision, Vision* limelight, wom
     : _camera(vision), _swerveDrive(swerveDrive), _vision(limelight), _type(VisionType::kGenericCamera) {}
 
 void LockOnToTarget::OnTick(units::second_t dt) {
+  if (!_vision->TargetIsVisible(VisionTargetObjects::kNote)) { SetDone(); }
+
   units::degree_t angle;
 
   switch (_type) {
@@ -60,4 +62,5 @@ void LockOnToTarget::OnTick(units::second_t dt) {
   if (_vision->IsAtPose(pose, dt)) {
     SetDone();
   }
+  
 }
diff --git a/src/main/include/AlphaArm.h b/src/main/include/AlphaArm.h
index f12e0c6d..a3cfc953 100644
--- a/src/main/include/AlphaArm.h
+++ b/src/main/include/AlphaArm.h
@@ -7,12 +7,11 @@
 
 
 #include "Wombat.h"
+//#include "vision/Vision.h"
 #include "utils/PID.h"
 #include <units/angle.h>
 #include <units/voltage.h>
 #include <frc/controller/PIDController.h>
-#include <string>
-#include <memory>
 
 struct AlphaArmConfig {
   wom::Gearbox alphaArmGearbox;
@@ -40,12 +39,12 @@ enum class AlphaArmState {
 
 class AlphaArm : public behaviour::HasBehaviour {
  public:
-  explicit AlphaArm(AlphaArmConfig *config/*, frc::Rotation2d initialAngle, wom::vision::Limelight* vision*/);
+  AlphaArm(AlphaArmConfig *config/*, frc::Rotation2d initialAngle, wom::vision::Limelight* vision*/);
 
   void OnUpdate(units::second_t dt);
   void SetArmRaw(units::volt_t voltage);
   void SetState(AlphaArmState state);
-  void SetControllerRaw(units::volt_t voltage); 
+  void SetControllerRaw(units::volt_t voltage);
   void SetGoal(double goal);
   void OnStart();
   AlphaArmConfig GetConfig(); //{ return _config; }
diff --git a/src/main/include/AlphaArmBehaviour.h b/src/main/include/AlphaArmBehaviour.h
index 61938878..b485c9f2 100644
--- a/src/main/include/AlphaArmBehaviour.h
+++ b/src/main/include/AlphaArmBehaviour.h
@@ -11,8 +11,6 @@
 #include "Intake.h"
 #include "Wombat.h"
 
-#include <memory>
-
 class AlphaArmManualControl : public behaviour::Behaviour {
  public:
   explicit AlphaArmManualControl(AlphaArm* alphaArm, Intake* intake, frc::XboxController* codriver);
@@ -28,4 +26,3 @@ class AlphaArmManualControl : public behaviour::Behaviour {
   std::shared_ptr<nt::NetworkTable> _table = nt::NetworkTableInstance::GetDefault().GetTable("AlphaArmSubsystem");
   bool climbing = false;
 };
-
diff --git a/src/main/include/ClimberBehaviour.h b/src/main/include/ClimberBehaviour.h
index 97fc545e..2f2487f7 100644
--- a/src/main/include/ClimberBehaviour.h
+++ b/src/main/include/ClimberBehaviour.h
@@ -6,9 +6,9 @@
 
 #include <frc/XboxController.h>
 
+#include "AlphaArm.h"
 #include "Climber.h"
 #include "Wombat.h"
-#include "AlphaArm.h"
 
 class ClimberManualControl : public behaviour::Behaviour {
  public:
diff --git a/src/main/include/IntakeBehaviour.h b/src/main/include/IntakeBehaviour.h
index cdeaa687..eeb3e6e5 100644
--- a/src/main/include/IntakeBehaviour.h
+++ b/src/main/include/IntakeBehaviour.h
@@ -39,7 +39,7 @@ class IntakeNote : public behaviour::Behaviour {
 
  private:
   Intake* _intake;
-  
+
 };
 
 class PassNote : public behaviour::Behaviour {
@@ -49,7 +49,7 @@ class PassNote : public behaviour::Behaviour {
 
  private:
   Intake* _intake;
-  
+
 };
 
 class EjectNote : public behaviour::Behaviour {
@@ -59,5 +59,5 @@ class EjectNote : public behaviour::Behaviour {
 
  private:
   Intake* _intake;
-  
+
 };
diff --git a/src/main/include/Robot.h b/src/main/include/Robot.h
index 8a64dfa4..68376038 100644
--- a/src/main/include/Robot.h
+++ b/src/main/include/Robot.h
@@ -7,6 +7,7 @@
 #include <frc/Encoder.h>
 #include <frc/TimedRobot.h>
 #include <frc/Timer.h>
+#include "Auto.h"
 #include <frc/event/EventLoop.h>
 #include <frc/simulation/DifferentialDrivetrainSim.h>
 #include <frc/simulation/EncoderSim.h>
@@ -22,16 +23,17 @@
 
 #include "AlphaArm.h"
 #include "AlphaArmBehaviour.h"
+#include "Climber.h"
+#include "ClimberBehaviour.h"
+#include <frc/Servo.h>
 #include "Intake.h"
 #include "IntakeBehaviour.h"
+#include "LED.h"
+#include "RobotMap.h"
 #include "Shooter.h"
 #include "ShooterBehaviour.h"
-#include "RobotMap.h"
 #include "Wombat.h"
-#include "LED.h"
-#include "Climber.h"
-#include "ClimberBehaviour.h"
-#include <frc/Servo.h>
+#include "vision/Vision.h"
 
 class Robot : public frc::TimedRobot {
  public:
@@ -58,6 +60,7 @@ class Robot : public frc::TimedRobot {
   Climber* climber;
 
   frc::Timer climberTimer;
+  Vision* vision;
 
   frc::SendableChooser<std::string> m_chooser;
 
@@ -84,17 +87,16 @@ class Robot : public frc::TimedRobot {
 
   // rev::CANSparkMax testMotorUp{1, rev::CANSparkMax::MotorType::kBrushless};
   // rev::CANSparkMax testMotorDown{6, rev::CANSparkMax::MotorType::kBrushless};
- // frc::XboxController testdriver = frc::XboxController(1);
+  // frc::XboxController testdriver = frc::XboxController(1);
   AlphaArm* alphaArm;
   LED* _led;
 
-
   // ctre::phoenix6::hardware::TalonFX *frontLeft;
   // ctre::phoenix6::hardware::TalonFX *frontRight;
   // ctre::phoenix6::hardware::TalonFX *backLeft;
   // ctre::phoenix6::hardware::TalonFX *backRight;
 
-  //wom::SwerveDrive* _swerveDrive;
+  // wom::SwerveDrive* _swerveDrive;
 
   frc::Servo *armServo;
 
@@ -102,5 +104,8 @@ class Robot : public frc::TimedRobot {
   // ctre::phoenix6::hardware::TalonFX *frontRight;
   // ctre::phoenix6::hardware::TalonFX *backLeft;
   // ctre::phoenix6::hardware::TalonFX *backRight;
+  // ctre::phoenix6::hardware::TalonFX *frontLeft;
+  //  ctre::phoenix6::hardware::TalonFX *frontRight;
+  //  ctre::phoenix6::hardware::TalonFX *backLeft;
+  //  ctre::phoenix6::hardware::TalonFX *backRight;
 };
-
diff --git a/src/main/include/RobotMap.h b/src/main/include/RobotMap.h
index 295151f0..007b08dd 100644
--- a/src/main/include/RobotMap.h
+++ b/src/main/include/RobotMap.h
@@ -13,45 +13,39 @@
 #include <frc/DutyCycleEncoder.h>
 #include <frc/Encoder.h>
 #include <frc/XboxController.h>
+#include <frc/controller/PIDController.h>
 #include <frc/motorcontrol/MotorControllerGroup.h>
 #include <frc/system/plant/DCMotor.h>
-#include <frc/controller/PIDController.h>
 #include <units/angle.h>
 #include <units/length.h>
 
 #include <memory>
 #include <string>
 
-#include "AlphaArm.h"
 #include <ctre/phoenix6/CANcoder.hpp>
 #include <ctre/phoenix6/Pigeon2.hpp>
 #include <ctre/phoenix6/TalonFX.hpp>
 // #include <frc/Servo.h>
 
-#include "Wombat.h"
-#include "utils/Encoder.h"
-#include "utils/PID.h"
 #include "AlphaArm.h"
 #include "AlphaArmBehaviour.h"
+#include "Climber.h"
 #include "Intake.h"
 #include "Shooter.h"
 #include "Wombat.h"
-#include "Wombat.h"
-#include "AlphaArm.h"
-#include "AlphaArmBehaviour.h"
-#include "Climber.h"
+#include "utils/Encoder.h"
+#include "utils/PID.h"
 
 struct RobotMap {
   struct Controllers {
     frc::XboxController driver = frc::XboxController(0);
     frc::XboxController codriver = frc::XboxController(1);
     frc::XboxController testController = frc::XboxController(2);
-    
   };
 
   Controllers controllers;
 
-    struct AlphaArmSystem {
+  struct AlphaArmSystem {
     rev::CANSparkMax alphaArmMotorUp{21, rev::CANSparkMax::MotorType::kBrushless};
     rev::CANSparkMax alphaArmMotorDown{26, rev::CANSparkMax::MotorType::kBrushless};
 
@@ -59,12 +53,12 @@ struct RobotMap {
     wom::CANSparkMaxEncoder* alphaArmNeoEncoderUp = new wom::CANSparkMaxEncoder(&alphaArmMotorUp, 0.1_m);
     wom::CANSparkMaxEncoder* alphaArmNeoEncoderDown = new wom::CANSparkMaxEncoder(&alphaArmMotorDown, 0.1_m);
 
-
     wom::Gearbox alphaArmGearbox{&alphaArmMotorUp, alphaArmNeoEncoderUp, frc::DCMotor::NEO(1)};
     wom::Gearbox alphaArmGearbox2{&alphaArmMotorDown, alphaArmNeoEncoderDown, frc::DCMotor::NEO(1)};
 
-    AlphaArmConfig config{alphaArmGearbox, alphaArmGearbox2, alphaArmEncoder, "/alphaArm"
-    //, &vision
+    AlphaArmConfig config{
+        alphaArmGearbox, alphaArmGearbox2, alphaArmEncoder, "/alphaArm"
+        //, &vision
     };
   };
   AlphaArmSystem alphaArmSystem;
@@ -79,24 +73,24 @@ struct RobotMap {
 
     wom::Gearbox IntakeGearbox{&intakeMotor, &intakeEncoder, frc::DCMotor::NEO(1)};
 
-    IntakeConfig config{"path", IntakeGearbox, &intakeSensor, &passSensor, wom::PIDConfig<units::radians_per_second, units::volts>(
-          "/intake/PID/config",
-          9_V / (180_deg / 1_s),
-          0_V / 25_deg,
-          0_V / (90_deg / 1_s / 1_s)
-        ),/*, &magSensor, &shooterSensor*/};
+    IntakeConfig config{
+        "path",
+        IntakeGearbox,
+        &intakeSensor,
+        &passSensor,
+        wom::PIDConfig<units::radians_per_second, units::volts>("/intake/PID/config", 9_V / (180_deg / 1_s),
+                                                                0_V / 25_deg, 0_V / (90_deg / 1_s / 1_s)),
+        /*, &magSensor, &shooterSensor*/};
   };
   IntakeSystem intakeSystem;
 
-
   struct SwerveBase {
     ctre::phoenix6::hardware::CANcoder frontLeftCancoder{16, "Drivebase"};
     ctre::phoenix6::hardware::CANcoder frontRightCancoder{18, "Drivebase"};
     ctre::phoenix6::hardware::CANcoder backLeftCancoder{17, "Drivebase"};
     ctre::phoenix6::hardware::CANcoder backRightCancoder{19, "Drivebase"};
 
-    ctre::phoenix6::hardware::Pigeon2* gyro =
-        new ctre::phoenix6::hardware::Pigeon2(20, "Drivebase");
+    ctre::phoenix6::hardware::Pigeon2* gyro = new ctre::phoenix6::hardware::Pigeon2(20, "Drivebase");
     wpi::array<ctre::phoenix6::hardware::TalonFX*, 4> turnMotors{
         new ctre::phoenix6::hardware::TalonFX(6, "Drivebase"),   // front left
         new ctre::phoenix6::hardware::TalonFX(7, "Drivebase"),   // front right
@@ -109,48 +103,38 @@ struct RobotMap {
         new ctre::phoenix6::hardware::TalonFX(1, "Drivebase")};  // back right
 
     wpi::array<wom::SwerveModuleConfig, 4> moduleConfigs{
-        wom::SwerveModuleConfig{ //CORRECT
+        wom::SwerveModuleConfig{
+            // CORRECT
             // front left module
             frc::Translation2d(-10_in, 9_in),
-            wom::Gearbox{
-                driveMotors[0],
-                new wom::TalonFXEncoder(driveMotors[0], 0.0445_m, 6.75),
-                frc::DCMotor::Falcon500(1).WithReduction(6.75)},
-            wom::Gearbox{turnMotors[0],
-                         new wom::CanEncoder(16, 0.0445_m, 4096, 12.8),
+            wom::Gearbox{driveMotors[0], new wom::TalonFXEncoder(driveMotors[0], 0.0445_m, 6.75),
+                         frc::DCMotor::Falcon500(1).WithReduction(6.75)},
+            wom::Gearbox{turnMotors[0], new wom::CanEncoder(16, 0.0445_m, 4096, 12.8),
                          frc::DCMotor::Falcon500(1).WithReduction(12.8)},
             &frontLeftCancoder, 4_in / 2},
-        wom::SwerveModuleConfig{ //CORRECT
+        wom::SwerveModuleConfig{
+            // CORRECT
             // front right module
             frc::Translation2d(10_in, 9_in),
-            wom::Gearbox{
-                driveMotors[1],
-                new wom::TalonFXEncoder(driveMotors[1], 0.0445_m, 6.75),
-                frc::DCMotor::Falcon500(1).WithReduction(6.75)},
-            wom::Gearbox{turnMotors[1],
-                         new wom::CanEncoder(18, 0.0445_m, 4096, 12.8),
+            wom::Gearbox{driveMotors[1], new wom::TalonFXEncoder(driveMotors[1], 0.0445_m, 6.75),
+                         frc::DCMotor::Falcon500(1).WithReduction(6.75)},
+            wom::Gearbox{turnMotors[1], new wom::CanEncoder(18, 0.0445_m, 4096, 12.8),
                          frc::DCMotor::Falcon500(1).WithReduction(12.8)},
             &frontRightCancoder, 4_in / 2},
         wom::SwerveModuleConfig{
             // back left module
             frc::Translation2d(-10_in, 9_in),
-            wom::Gearbox{
-                driveMotors[2],
-                new wom::TalonFXEncoder(driveMotors[2], 0.0445_m, 6.75),
-                frc::DCMotor::Falcon500(1).WithReduction(6.75)},
-            wom::Gearbox{turnMotors[2],
-                         new wom::CanEncoder(17, 0.0445_m, 4096, 12.8),
+            wom::Gearbox{driveMotors[2], new wom::TalonFXEncoder(driveMotors[2], 0.0445_m, 6.75),
+                         frc::DCMotor::Falcon500(1).WithReduction(6.75)},
+            wom::Gearbox{turnMotors[2], new wom::CanEncoder(17, 0.0445_m, 4096, 12.8),
                          frc::DCMotor::Falcon500(1).WithReduction(12.8)},
             &backRightCancoder, 4_in / 2},
         wom::SwerveModuleConfig{
             // back right module
             frc::Translation2d(-10_in, -9_in),
-            wom::Gearbox{
-                driveMotors[3],
-                new wom::TalonFXEncoder(driveMotors[3], 0.0445_m, 6.75),
-                frc::DCMotor::Falcon500(1).WithReduction(6.75)},
-            wom::Gearbox{turnMotors[3],
-                         new wom::CanEncoder(19, 0.0445_m, 4096, 12.8),
+            wom::Gearbox{driveMotors[3], new wom::TalonFXEncoder(driveMotors[3], 0.0445_m, 6.75),
+                         frc::DCMotor::Falcon500(1).WithReduction(6.75)},
+            wom::Gearbox{turnMotors[3], new wom::CanEncoder(19, 0.0445_m, 4096, 12.8),
                          frc::DCMotor::Falcon500(1).WithReduction(12.8)},
             &backLeftCancoder, 4_in / 2},
     };
@@ -170,19 +154,16 @@ struct RobotMap {
         wom::SwerveDriveConfig::pose_angle_conf_t::ki_t{0},
         0_deg / 1_deg};*/
     wom::SwerveDriveConfig::pose_position_conf_t posePositionPID{
-        "/drivetrain/pid/pose/position/config",
-        0_mps / 1_m,
-        wom::SwerveDriveConfig::pose_position_conf_t::ki_t{0.15},
-        0_m / 1_m,
-        0_cm};
+        "/drivetrain/pid/pose/position/config", 0_mps / 1_m,
+        wom::SwerveDriveConfig::pose_position_conf_t::ki_t{0.15}, 0_m / 1_m, 0_cm};
 
     // the config for the whole swerve drive
     wom::SwerveDriveConfig config{"/drivetrain",
-                                  //anglePID,
+                                  // anglePID,
                                   velocityPID,
                                   moduleConfigs,  // each module
                                   gyro,
-                                  //poseAnglePID,
+                                  // poseAnglePID,
                                   posePositionPID,
                                   60_kg,  // robot mass (estimate rn)
                                   {0.1, 0.1, 0.1},
@@ -202,25 +183,23 @@ struct RobotMap {
   SwerveBase swerveBase;
 
   struct SwerveTable {
-    std::shared_ptr<nt::NetworkTable> swerveDriveTable = nt::NetworkTableInstance::GetDefault().GetTable("swerve");
-  }; SwerveTable swerveTable;
-
+    std::shared_ptr<nt::NetworkTable> swerveDriveTable =
+        nt::NetworkTableInstance::GetDefault().GetTable("swerve");
+  };
+  SwerveTable swerveTable;
 
-    struct Shooter {
-    rev::CANSparkMax shooterMotor{31, rev::CANSparkMax::MotorType::kBrushless};// Port 11
+  struct Shooter {
+    rev::CANSparkMax shooterMotor{31, rev::CANSparkMax::MotorType::kBrushless};  // Port 11
     // frc::DigitalInput shooterSensor{2};
 
     // wom::VoltageController shooterMotorGroup = wom::VoltagedController::Group(shooterMotor);
     wom::CANSparkMaxEncoder* shooterEncoder = new wom::CANSparkMaxEncoder(&shooterMotor, 0.01_m);
     wom::Gearbox shooterGearbox{&shooterMotor, shooterEncoder, frc::DCMotor::NEO(1)};
 
-    
-
     ShooterConfig config{
         "shooterGearbox",
         shooterGearbox,
     };
-
   };
   Shooter shooterSystem;
 
@@ -229,8 +208,9 @@ struct RobotMap {
     // wom::utils::CANSparkMaxEncoder climberEncoder{&climberMotor, 0.1_m};
     wom::CANSparkMaxEncoder* climberEncoder = new wom::CANSparkMaxEncoder(&climberMotor, 0.1_m);
 
-//     // frc::DigitalInput climberSensor{99};
-//     // wom::MotorVoltageController climberMotorController = wom::MotorVoltageController::Group(climberMotor);
+    //     // frc::DigitalInput climberSensor{99};
+    //     // wom::MotorVoltageController climberMotorController =
+    //     wom::MotorVoltageController::Group(climberMotor);
     wom::Gearbox climberGearbox{&climberMotor, climberEncoder, frc::DCMotor::NEO(1)};
     ClimberConfig config {
         climberGearbox,
@@ -240,4 +220,4 @@ struct RobotMap {
   wom::SwerveAutoBuilder* _builder;
   wom::SimSwerve* _simSwerve;
 };
-  // AlphaArmSystem alphaArmSystem;
+// AlphaArmSystem alphaArmSystem;
diff --git a/wombat/src/main/cpp/drivetrain/SwerveDrive.cpp b/wombat/src/main/cpp/drivetrain/SwerveDrive.cpp
index 74f7b9c9..62505d19 100644
--- a/wombat/src/main/cpp/drivetrain/SwerveDrive.cpp
+++ b/wombat/src/main/cpp/drivetrain/SwerveDrive.cpp
@@ -5,41 +5,36 @@
 #include "drivetrain/SwerveDrive.h"
 
 #include <networktables/NetworkTableInstance.h>
+#include <units/angle.h>
 #include <units/math.h>
 #include <units/voltage.h>
-#include <units/angle.h>
-
-#include "utils/Util.h"
+#include <wpi/sendable/SendableBuilder.h>
+#include <wpi/sendable/SendableRegistry.h>
 
-#include <iostream>
 #include <cmath>
+#include <iostream>
 
 #include "frc/MathUtil.h"
-#include <wpi/sendable/SendableBuilder.h>
-#include <wpi/sendable/SendableRegistry.h>
+#include "utils/Util.h"
 
 using namespace wom;
 
 namespace wom {
 namespace drivetrain {
 
-PIDController::PIDController(double Kp, double Ki, double Kd,
-                             units::second_t period)
+PIDController::PIDController(double Kp, double Ki, double Kd, units::second_t period)
     : m_Kp(Kp), m_Ki(Ki), m_Kd(Kd), m_period(period) {
   bool invalidGains = false;
   if (Kp < 0.0) {
-    wpi::math::MathSharedStore::ReportError(
-        "Kp must be a non-negative number, got {}!", Kp);
+    wpi::math::MathSharedStore::ReportError("Kp must be a non-negative number, got {}!", Kp);
     invalidGains = true;
   }
   if (Ki < 0.0) {
-    wpi::math::MathSharedStore::ReportError(
-        "Ki must be a non-negative number, got {}!", Ki);
+    wpi::math::MathSharedStore::ReportError("Ki must be a non-negative number, got {}!", Ki);
     invalidGains = true;
   }
   if (Kd < 0.0) {
-    wpi::math::MathSharedStore::ReportError(
-        "Kd must be a non-negative number, got {}!", Kd);
+    wpi::math::MathSharedStore::ReportError("Kd must be a non-negative number, got {}!", Kd);
     invalidGains = true;
   }
   if (invalidGains) {
@@ -50,17 +45,15 @@ PIDController::PIDController(double Kp, double Ki, double Kd,
   }
 
   if (period <= 0_s) {
-    wpi::math::MathSharedStore::ReportError(
-        "Controller period must be a positive number, got {}!", period.value());
+    wpi::math::MathSharedStore::ReportError("Controller period must be a positive number, got {}!",
+                                            period.value());
     m_period = 20_ms;
-    wpi::math::MathSharedStore::ReportWarning(
-        "Controller period defaulted to 20ms.");
+    wpi::math::MathSharedStore::ReportWarning("Controller period defaulted to 20ms.");
   }
   static int instances = 0;
   instances++;
 
-  wpi::math::MathSharedStore::ReportUsage(
-      wpi::math::MathUsageId::kController_PIDController2, instances);
+  wpi::math::MathSharedStore::ReportUsage(wpi::math::MathUsageId::kController_PIDController2, instances);
   wpi::SendableRegistry::Add(this, "PIDController", instances);
 }
 
@@ -84,8 +77,7 @@ void PIDController::SetD(double Kd) {
 
 void PIDController::SetIZone(double iZone) {
   if (iZone < 0) {
-    wpi::math::MathSharedStore::ReportError(
-        "IZone must be a non-negative number, got {}!", iZone);
+    wpi::math::MathSharedStore::ReportError("IZone must be a non-negative number, got {}!", iZone);
   }
   m_iZone = iZone;
 }
@@ -124,8 +116,7 @@ void PIDController::SetSetpoint(double setpoint) {
 
   if (m_continuous) {
     double errorBound = (m_maximumInput - m_minimumInput) / 2.0;
-    m_positionError =
-        frc::InputModulus(m_setpoint - m_measurement, -errorBound, errorBound);
+    m_positionError = frc::InputModulus(m_setpoint - m_measurement, -errorBound, errorBound);
   } else {
     m_positionError = m_setpoint - m_measurement;
   }
@@ -138,13 +129,11 @@ double PIDController::GetSetpoint() const {
 }
 
 bool PIDController::AtSetpoint() const {
-  return m_haveMeasurement && m_haveSetpoint &&
-         std::abs(m_positionError) < m_positionTolerance &&
+  return m_haveMeasurement && m_haveSetpoint && std::abs(m_positionError) < m_positionTolerance &&
          std::abs(m_velocityError) < m_velocityTolerance;
 }
 
-void PIDController::EnableContinuousInput(double minimumInput,
-                                          double maximumInput) {
+void PIDController::EnableContinuousInput(double minimumInput, double maximumInput) {
   m_continuous = true;
   m_minimumInput = minimumInput;
   m_maximumInput = maximumInput;
@@ -158,14 +147,12 @@ bool PIDController::IsContinuousInputEnabled() const {
   return m_continuous;
 }
 
-void PIDController::SetIntegratorRange(double minimumIntegral,
-                                       double maximumIntegral) {
+void PIDController::SetIntegratorRange(double minimumIntegral, double maximumIntegral) {
   m_minimumIntegral = minimumIntegral;
   m_maximumIntegral = maximumIntegral;
 }
 
-void PIDController::SetTolerance(double positionTolerance,
-                                 double velocityTolerance) {
+void PIDController::SetTolerance(double positionTolerance, double velocityTolerance) {
   m_positionTolerance = positionTolerance;
   m_velocityTolerance = velocityTolerance;
 }
@@ -185,8 +172,7 @@ double PIDController::Calculate(double measurement) {
 
   if (m_continuous) {
     double errorBound = (m_maximumInput - m_minimumInput) / 2.0;
-    m_positionError =
-        frc::InputModulus(m_setpoint - m_measurement, -errorBound, errorBound);
+    m_positionError = frc::InputModulus(m_setpoint - m_measurement, -errorBound, errorBound);
   } else {
     m_positionError = m_setpoint - m_measurement;
   }
@@ -198,9 +184,8 @@ double PIDController::Calculate(double measurement) {
   if (std::abs(m_positionError) > m_iZone) {
     m_totalError = 0;
   } else if (m_Ki != 0) {
-    m_totalError =
-        std::clamp(m_totalError + m_positionError * m_period.value(),
-                   m_minimumIntegral / m_Ki, m_maximumIntegral / m_Ki);
+    m_totalError = std::clamp(m_totalError + m_positionError * m_period.value(), m_minimumIntegral / m_Ki,
+                              m_maximumIntegral / m_Ki);
   }
 
   // double absError = m_setpoint - m_measurement;
@@ -236,41 +221,35 @@ void PIDController::InitSendable(wpi::SendableBuilder& builder) {
   builder.AddDoubleProperty(
       "d", [this] { return GetD(); }, [this](double value) { SetD(value); });
   builder.AddDoubleProperty(
-      "izone", [this] { return GetIZone(); },
-      [this](double value) { SetIZone(value); });
+      "izone", [this] { return GetIZone(); }, [this](double value) { SetIZone(value); });
   builder.AddDoubleProperty(
-      "setpoint", [this] { return GetSetpoint(); },
-      [this](double value) { SetSetpoint(value); });
+      "setpoint", [this] { return GetSetpoint(); }, [this](double value) { SetSetpoint(value); });
 }
 
-
-
-
-void SwerveModuleConfig::WriteNT(
-    std::shared_ptr<nt::NetworkTable> table) const {
+void SwerveModuleConfig::WriteNT(std::shared_ptr<nt::NetworkTable> table) const {
   std::array<double, 2> pos{position.X().value(), position.Y().value()};
   table->GetEntry("position").SetDoubleArray(std::span(pos));
   table->GetEntry("wheelRadius").SetDouble(wheelRadius.value());
 }
 
 SwerveModule::SwerveModule(std::string path, SwerveModuleConfig config,
-                           //SwerveModule::angle_pid_conf_t anglePID,
+                           // SwerveModule::angle_pid_conf_t anglePID,
                            SwerveModule::velocity_pid_conf_t velocityPID)
-    : //_anglePIDController(path + "/pid/angle", anglePID),
+    :  //_anglePIDController(path + "/pid/angle", anglePID),
       _anglePIDController{frc::PIDController(5, 0, 0, 0.005_s)},
       _config(config),
       _velocityPIDController(frc::PIDController(1.2, 0, 0, 0.005_s)),
       _table(nt::NetworkTableInstance::GetDefault().GetTable(path)) {
   // _anglePIDController.SetTolerance(360);
   // _anglePIDController.EnableContinuousInput(-3.1415, (2 * 3.1415));
-  _anglePIDController.EnableContinuousInput(0, (2*3.1415));
+  _anglePIDController.EnableContinuousInput(0, (2 * 3.1415));
 }
 
 void SwerveModule::OnStart() {
   // _offset = offset;
   // _config.canEncoder->SetPosition(units::turn_t{0});
   _anglePIDController.Reset();
-  _anglePIDController.EnableContinuousInput(0, 2*3.14159);
+  _anglePIDController.EnableContinuousInput(0, 2 * 3.14159);
   _velocityPIDController.Reset();
 }
 
@@ -285,14 +264,14 @@ void SwerveModule::OnUpdate(units::second_t dt) {
       break;
     case SwerveModuleState::kPID: {
       auto feedforward = _config.driveMotor.motor.Voltage(
-          0_Nm, units::radians_per_second_t{
-                    _velocityPIDController.GetSetpoint() / _config.wheelRadius.value()});
-      // units::radian_t input = units::math::fmod(_config.turnMotor.encoder->GetEncoderPosition(), (2_rad * 3.1415));
+          0_Nm,
+          units::radians_per_second_t{_velocityPIDController.GetSetpoint() / _config.wheelRadius.value()});
+      // units::radian_t input = units::math::fmod(_config.turnMotor.encoder->GetEncoderPosition(), (2_rad
+      // * 3.1415));
       double input = _config.turnMotor.encoder->GetEncoderPosition().value();
       _table->GetEntry("/testing/GetEncoderPos").SetDouble(input);
       // _velocityPIDController.SetSetpoint(3);
 
-
       driveVoltage = units::volt_t{_velocityPIDController.Calculate(GetSpeed().value())};
       // if (_turnOffset == TurnOffsetValues::forward) {
 
@@ -312,8 +291,7 @@ void SwerveModule::OnUpdate(units::second_t dt) {
       std::cerr << "Case not handled" << std::endl;
   }
 
-  units::newton_meter_t torqueLimit =
-      50_kg / 4 * _config.wheelRadius * _currentAccelerationLimit;
+  units::newton_meter_t torqueLimit = 50_kg / 4 * _config.wheelRadius * _currentAccelerationLimit;
   // units::volt_t voltageMax = _config.driveMotor.motor.Voltage(
   //     torqueLimit, _config.driveMotor.encoder->GetEncoderAngularVelocity());
   // units::volt_t voltageMin = _config.driveMotor.motor.Voltage(
@@ -329,8 +307,7 @@ void SwerveModule::OnUpdate(units::second_t dt) {
   //     units::math::max(driveVoltage, -_driveModuleVoltageLimit),
   //     _driveModuleVoltageLimit);  // was originally 10_V
   units::volt_t turnVoltageMax = 7_V - (driveVoltage * (7_V / 10_V));
-  turnVoltage = units::math::min(units::math::max(turnVoltage, -turnVoltageMax),
-                                 turnVoltageMax);
+  turnVoltage = units::math::min(units::math::max(turnVoltage, -turnVoltageMax), turnVoltageMax);
   // turnVoltage = units::math::min(units::math::max(turnVoltage, -7_V), 7_V);
 
   _table->GetEntry("TurnVoltage").SetDouble(turnVoltage.value());
@@ -362,13 +339,11 @@ void SwerveModule::TurnOffset() {
 //   return (_config.turnMotor.encoder->GetEncoderPosition().value());
 // }
 
-void SwerveModule::SetAccelerationLimit(
-    units::meters_per_second_squared_t limit) {
+void SwerveModule::SetAccelerationLimit(units::meters_per_second_squared_t limit) {
   _currentAccelerationLimit = limit;
 }
 
-void SwerveDrive::SetAccelerationLimit(
-    units::meters_per_second_squared_t limit) {
+void SwerveDrive::SetAccelerationLimit(units::meters_per_second_squared_t limit) {
   for (int motorNumber = 0; motorNumber < 4; motorNumber++) {
     _modules[motorNumber].SetAccelerationLimit(limit);
   }
@@ -387,12 +362,9 @@ void SwerveModule::SetZero(units::second_t dt) {
   _state = SwerveModuleState::kPID;
 }
 
-void SwerveModule::SetPID(units::radian_t angle,
-                          units::meters_per_second_t speed,
-                          units::second_t dt) {
+void SwerveModule::SetPID(units::radian_t angle, units::meters_per_second_t speed, units::second_t dt) {
   _state = SwerveModuleState::kPID;
 
-
   // double diff = std::abs(_config.turnMotor.encoder->GetEncoderPosition().value() - angle.value());
   // _table->GetEntry("diff").SetDouble(diff);
   // if (std::abs(diff) > (3.14159/2)) {
@@ -401,45 +373,38 @@ void SwerveModule::SetPID(units::radian_t angle,
   //   _anglePIDController.SetSetpoint(angle.value());
   //   _velocityPIDController.SetSetpoint(speed.value());
   // } else {
-    _anglePIDController.SetSetpoint(angle.value());
-    _velocityPIDController.SetSetpoint(speed.value());
+  _anglePIDController.SetSetpoint(angle.value());
+  _velocityPIDController.SetSetpoint(speed.value());
   // }
 }
 
 void SwerveModule::ModuleVectorHandler(frc::ChassisSpeeds speeds) {
   units::meters_per_second_t xVelocityComponent =
-      1_mps *
-      (speeds.vx.value() + speeds.omega.value() * _config.position.X().value());
+      1_mps * (speeds.vx.value() + speeds.omega.value() * _config.position.X().value());
   units::meters_per_second_t yVelocityComponent =
-      1_mps *
-      (speeds.vy.value() + speeds.omega.value() * _config.position.Y().value());
+      1_mps * (speeds.vy.value() + speeds.omega.value() * _config.position.Y().value());
 
   units::meters_per_second_t velocity =
-      1_mps * std::sqrt(std::pow(xVelocityComponent.value(), 2) +
-                        std::pow(yVelocityComponent.value(), 2));
-  units::degree_t angle = 1_rad * std::atan2(yVelocityComponent.value(),
-                                             xVelocityComponent.value());
+      1_mps * std::sqrt(std::pow(xVelocityComponent.value(), 2) + std::pow(yVelocityComponent.value(), 2));
+  units::degree_t angle = 1_rad * std::atan2(yVelocityComponent.value(), xVelocityComponent.value());
 
   _anglePIDController.SetSetpoint(angle.value());
   _velocityPIDController.SetSetpoint(velocity.value());
 }
 
 units::meters_per_second_t SwerveModule::GetSpeed() const {
-  units::meters_per_second_t returnVal{
-    _config.driveMotor.encoder->GetVelocityValue() *
-    _config.wheelRadius.value()};
-    return returnVal;
+  units::meters_per_second_t returnVal{_config.driveMotor.encoder->GetVelocityValue() *
+                                       _config.wheelRadius.value()};
+  return returnVal;
 }
 
 units::meter_t SwerveModule::GetDistance() const {
-  return units::meter_t{
-      _config.driveMotor.encoder->GetEncoderPosition().value() *
-      _config.wheelRadius.value()};
+  return units::meter_t{_config.driveMotor.encoder->GetEncoderPosition().value() *
+                        _config.wheelRadius.value()};
 }
 
 frc::SwerveModulePosition SwerveModule::GetPosition() const {
-  return frc::SwerveModulePosition{
-      GetDistance(), _config.turnMotor.encoder->GetEncoderPosition()};
+  return frc::SwerveModulePosition{GetDistance(), _config.turnMotor.encoder->GetEncoderPosition()};
 }
 
 const SwerveModuleConfig& SwerveModule::GetConfig() const {
@@ -452,15 +417,14 @@ void SwerveDriveConfig::WriteNT(std::shared_ptr<nt::NetworkTable> table) {
 
 SwerveDrive::SwerveDrive(SwerveDriveConfig config, frc::Pose2d initialPose)
     : _config(config),
-      _kinematics(_config.modules[3].position, _config.modules[0].position,
-                  _config.modules[1].position, _config.modules[2].position),
+      _kinematics(_config.modules[3].position, _config.modules[0].position, _config.modules[1].position,
+                  _config.modules[2].position),
       _poseEstimator(
           _kinematics, frc::Rotation2d(0_deg),
-          wpi::array<frc::SwerveModulePosition, 4>{
-              frc::SwerveModulePosition{0_m, frc::Rotation2d{0_deg}},
-              frc::SwerveModulePosition{0_m, frc::Rotation2d{0_deg}},
-              frc::SwerveModulePosition{0_m, frc::Rotation2d{0_deg}},
-              frc::SwerveModulePosition{0_m, frc::Rotation2d{0_deg}}},
+          wpi::array<frc::SwerveModulePosition, 4>{frc::SwerveModulePosition{0_m, frc::Rotation2d{0_deg}},
+                                                   frc::SwerveModulePosition{0_m, frc::Rotation2d{0_deg}},
+                                                   frc::SwerveModulePosition{0_m, frc::Rotation2d{0_deg}},
+                                                   frc::SwerveModulePosition{0_m, frc::Rotation2d{0_deg}}},
           initialPose, _config.stateStdDevs, _config.visionMeasurementStdDevs),
       _anglePIDController{PIDController(1, 0, 0)},
       _xPIDController(config.path + "/pid/x", _config.posePositionPID),
@@ -478,10 +442,8 @@ SwerveDrive::SwerveDrive(SwerveDriveConfig config, frc::Pose2d initialPose)
   ResetPose(initialPose);
 }
 
-frc::ChassisSpeeds FieldRelativeSpeeds::ToChassisSpeeds(
-    const units::radian_t robotHeading) {
-  return frc::ChassisSpeeds::FromFieldRelativeSpeeds(
-      vx, vy, omega, frc::Rotation2d{robotHeading});
+frc::ChassisSpeeds FieldRelativeSpeeds::ToChassisSpeeds(const units::radian_t robotHeading) {
+  return frc::ChassisSpeeds::FromFieldRelativeSpeeds(vx, vy, omega, frc::Rotation2d{robotHeading});
 }
 
 void SwerveDrive::OnUpdate(units::second_t dt) {
@@ -502,14 +464,15 @@ void SwerveDrive::OnUpdate(units::second_t dt) {
     case SwerveDriveState::kPose: {
       _target_fr_speeds.vx = _xPIDController.Calculate(GetPose().X(), dt);
       _target_fr_speeds.vy = _yPIDController.Calculate(GetPose().Y(), dt);
-      _target_fr_speeds.omega = units::radians_per_second_t{_anglePIDController.Calculate(GetPose().Rotation().Radians().value())};
+      _target_fr_speeds.omega =
+          units::radians_per_second_t{_anglePIDController.Calculate(GetPose().Rotation().Radians().value())};
     }
       [[fallthrough]];
     case SwerveDriveState::kFieldRelativeVelocity:
-      _target_speed =
-          _target_fr_speeds.ToChassisSpeeds(GetPose().Rotation().Radians());
+      _target_speed = _target_fr_speeds.ToChassisSpeeds(GetPose().Rotation().Radians());
       if (isRotateToMatchJoystick) {
-        _target_speed.omega = units::radians_per_second_t{_anglePIDController.Calculate(GetPose().Rotation().Radians().value())};
+        _target_speed.omega = units::radians_per_second_t{
+            _anglePIDController.Calculate(GetPose().Rotation().Radians().value())};
       }
       // _target_fr_speeds.vy.value() << std::endl;
       [[fallthrough]];
@@ -534,7 +497,7 @@ void SwerveDrive::OnUpdate(units::second_t dt) {
 
       bool init = false;
 
-      frc::ChassisSpeeds new_target_speed {_target_speed.vx, _target_speed.vy, -_target_speed.omega};
+      frc::ChassisSpeeds new_target_speed{_target_speed.vx, _target_speed.vy, -_target_speed.omega};
       auto target_states = _kinematics.ToSwerveModuleStates(_target_speed);
       auto new_target_states = _kinematics.ToSwerveModuleStates(new_target_speed);
       for (size_t i = 0; i < _modules.size(); i++) {
@@ -563,13 +526,12 @@ void SwerveDrive::OnUpdate(units::second_t dt) {
     case SwerveDriveState::kFRVelocityRotationLock:
       _target_speed.vx = _xPIDController.Calculate(GetPose().X(), dt);
       _target_speed.vy = _yPIDController.Calculate(GetPose().Y(), dt);
-      _target_speed.omega = units::radians_per_second_t{_anglePIDController.Calculate(GetPose().Rotation().Radians().value())};
-      _target_speed =
-          _requestedSpeeds.ToChassisSpeeds(GetPose().Rotation().Radians());
+      _target_speed.omega =
+          units::radians_per_second_t{_anglePIDController.Calculate(GetPose().Rotation().Radians().value())};
+      _target_speed = _requestedSpeeds.ToChassisSpeeds(GetPose().Rotation().Radians());
       auto target_states = _kinematics.ToSwerveModuleStates(_target_speed);
       for (size_t i = 0; i < _modules.size(); i++) {
-        _modules[i].SetPID(target_states[i].angle.Radians(),
-                           target_states[i].speed, dt);
+        _modules[i].SetPID(target_states[i].angle.Radians(), target_states[i].speed, dt);
       }
       break;
   }
@@ -580,12 +542,10 @@ void SwerveDrive::OnUpdate(units::second_t dt) {
 
   _poseEstimator.Update(
       _config.gyro->GetRotation2d(),
-      wpi::array<frc::SwerveModulePosition, 4>{
-          _modules[3].GetPosition(), _modules[0].GetPosition(),
-          _modules[1].GetPosition(), _modules[2].GetPosition()});
+      wpi::array<frc::SwerveModulePosition, 4>{_modules[3].GetPosition(), _modules[0].GetPosition(),
+                                               _modules[1].GetPosition(), _modules[2].GetPosition()});
 
-  utils::WritePose2NT(_table->GetSubTable("estimatedPose"),
-                      _poseEstimator.GetEstimatedPosition());
+  utils::WritePose2NT(_table->GetSubTable("estimatedPose"), _poseEstimator.GetEstimatedPosition());
   _config.WriteNT(_table->GetSubTable("config"));
 }
 
@@ -622,8 +582,7 @@ void SwerveDrive::OnResetMode() {
   _anglePIDController.Reset();
 }
 
-void SwerveDrive::RotateMatchJoystick(units::radian_t joystickAngle,
-                                      FieldRelativeSpeeds speeds) {
+void SwerveDrive::RotateMatchJoystick(units::radian_t joystickAngle, FieldRelativeSpeeds speeds) {
   // _state = SwerveDriveState::kFRVelocityRotationLock;
   // _anglePIDController.SetSetpoint(joystickAngle);
   // _target_fr_speeds = speeds;
@@ -649,16 +608,14 @@ bool SwerveDrive::GetIsFieldRelative() {
   return _isFieldRelative;
 }
 
-void SwerveDrive::SetIndividualTuning(int mod, units::radian_t angle,
-                                      units::meters_per_second_t speed) {
+void SwerveDrive::SetIndividualTuning(int mod, units::radian_t angle, units::meters_per_second_t speed) {
   _mod = mod;
   _angle = angle;
   _speed = speed;
   _state = SwerveDriveState::kIndividualTuning;
 }
 
-void SwerveDrive::SetTuning(units::radian_t angle,
-                            units::meters_per_second_t speed) {
+void SwerveDrive::SetTuning(units::radian_t angle, units::meters_per_second_t speed) {
   _angle = angle;
   _speed = speed;
   _state = SwerveDriveState::kTuning;
@@ -685,9 +642,8 @@ bool SwerveDrive::IsAtSetPose() {
 void SwerveDrive::ResetPose(frc::Pose2d pose) {
   _poseEstimator.ResetPosition(
       _config.gyro->GetRotation2d(),
-      wpi::array<frc::SwerveModulePosition, 4>{
-          _modules[0].GetPosition(), _modules[1].GetPosition(),
-          _modules[2].GetPosition(), _modules[3].GetPosition()},
+      wpi::array<frc::SwerveModulePosition, 4>{_modules[0].GetPosition(), _modules[1].GetPosition(),
+                                               _modules[2].GetPosition(), _modules[3].GetPosition()},
       pose);
 }
 
diff --git a/wombat/src/main/cpp/subsystems/Arm.cpp b/wombat/src/main/cpp/subsystems/Arm.cpp
index 152a642d..a451d5cf 100644
--- a/wombat/src/main/cpp/subsystems/Arm.cpp
+++ b/wombat/src/main/cpp/subsystems/Arm.cpp
@@ -10,19 +10,14 @@ using namespace frc;
 using namespace wom;
 
 // creates network table instatnce on shuffleboard
-void wom::subsystems::ArmConfig::WriteNT(
-    std::shared_ptr<nt::NetworkTable> table) {
+void wom::subsystems::ArmConfig::WriteNT(std::shared_ptr<nt::NetworkTable> table) {
   table->GetEntry("armMass").SetDouble(armMass.value());
   table->GetEntry("loadMass").SetDouble(loadMass.value());
   table->GetEntry("armLength").SetDouble(armLength.value());
-  table->GetEntry("minAngle")
-      .SetDouble(minAngle.convert<units::degree>().value());
-  table->GetEntry("maxAngle")
-      .SetDouble(maxAngle.convert<units::degree>().value());
-  table->GetEntry("initialAngle")
-      .SetDouble(initialAngle.convert<units::degree>().value());
-  table->GetEntry("angleOffset")
-      .SetDouble(initialAngle.convert<units::degree>().value());
+  table->GetEntry("minAngle").SetDouble(minAngle.convert<units::degree>().value());
+  table->GetEntry("maxAngle").SetDouble(maxAngle.convert<units::degree>().value());
+  table->GetEntry("initialAngle").SetDouble(initialAngle.convert<units::degree>().value());
+  table->GetEntry("angleOffset").SetDouble(initialAngle.convert<units::degree>().value());
 }
 
 // arm config is used
@@ -43,14 +38,12 @@ void wom::subsystems::Arm::OnUpdate(units::second_t dt) {
     case ArmState::kIdle:
       break;
     case ArmState::kVelocity: {
-      units::newton_meter_t torque =
-          9.81_m / 1_s / 1_s * _config.armLength *
-          units::math::cos(angle + _config.angleOffset) *
-          (0.5 * _config.armMass + _config.loadMass);
+      units::newton_meter_t torque = 9.81_m / 1_s / 1_s * _config.armLength *
+                                     units::math::cos(angle + _config.angleOffset) *
+                                     (0.5 * _config.armMass + _config.loadMass);
       // units::volt_t feedforward = _config.leftGearbox.motor.Voltage(torque,
       // 0_rad/1_s);
-      units::volt_t feedforward =
-          _config.leftGearbox.motor.Voltage(torque, _velocityPID.GetSetpoint());
+      units::volt_t feedforward = _config.leftGearbox.motor.Voltage(torque, _velocityPID.GetSetpoint());
       // feedforward = 3.5_V;
       // std::cout << "feedforward" << feedforward.value() << std::endl;
       voltage = _velocityPID.Calculate(GetArmVelocity(), dt, feedforward);
@@ -58,12 +51,10 @@ void wom::subsystems::Arm::OnUpdate(units::second_t dt) {
       // std::endl; voltage = 0_V;
     } break;
     case ArmState::kAngle: {
-      units::newton_meter_t torque =
-          9.81_m / 1_s / 1_s * _config.armLength *
-          units::math::cos(angle + _config.angleOffset) *
-          (0.5 * _config.armMass + _config.loadMass);
-      units::volt_t feedforward =
-          _config.leftGearbox.motor.Voltage(torque, 0_rad / 1_s);
+      units::newton_meter_t torque = 9.81_m / 1_s / 1_s * _config.armLength *
+                                     units::math::cos(angle + _config.angleOffset) *
+                                     (0.5 * _config.armMass + _config.loadMass);
+      units::volt_t feedforward = _config.leftGearbox.motor.Voltage(torque, 0_rad / 1_s);
       // std::cout << "feedforward" << feedforward.value() << std::endl;
       voltage = _pid.Calculate(angle, dt, feedforward);
     } break;
diff --git a/wombat/src/main/cpp/subsystems/Elevator.cpp b/wombat/src/main/cpp/subsystems/Elevator.cpp
index d6b59f8a..d1cf9a8f 100644
--- a/wombat/src/main/cpp/subsystems/Elevator.cpp
+++ b/wombat/src/main/cpp/subsystems/Elevator.cpp
@@ -7,8 +7,7 @@
 #include <networktables/NetworkTableInstance.h>
 #include <units/acceleration.h>
 
-void wom::subsystems::ElevatorConfig::WriteNT(
-    std::shared_ptr<nt::NetworkTable> table) {
+void wom::subsystems::ElevatorConfig::WriteNT(std::shared_ptr<nt::NetworkTable> table) {
   table->GetEntry("radius").SetDouble(radius.value());
   table->GetEntry("mass").SetDouble(mass.value());
   table->GetEntry("maxHeight").SetDouble(maxHeight.value());
@@ -20,8 +19,7 @@ wom::subsystems::Elevator::Elevator(ElevatorConfig config)
       _pid{config.path + "/pid", config.pid},
       _velocityPID{config.path + "/velocityPID", config.velocityPID},
       _table(nt::NetworkTableInstance::GetDefault().GetTable(config.path)) {
-  _config.leftGearbox.encoder->SetEncoderPosition(_config.initialHeight /
-                                                  _config.radius * 1_rad);
+  _config.leftGearbox.encoder->SetEncoderPosition(_config.initialHeight / _config.radius * 1_rad);
 }
 
 void wom::subsystems::Elevator::OnUpdate(units::second_t dt) {
@@ -39,8 +37,7 @@ void wom::subsystems::Elevator::OnUpdate(units::second_t dt) {
     case ElevatorState::kVelocity: {
       units::volt_t feedforward = _config.rightGearbox.motor.Voltage(
           (_config.mass * 9.81_mps_sq) * _config.radius,
-          _velocityPID.GetSetpoint() /
-              (14.0 / 60.0 * 2.0 * 3.1415 * 0.02225 * 1_m) * 1_rad);
+          _velocityPID.GetSetpoint() / (14.0 / 60.0 * 2.0 * 3.1415 * 0.02225 * 1_m) * 1_rad);
       // units::volt_t feedforward = _config.rightGearbox.motor.Voltage(0_Nm,
       // _velocityPID.GetSetpoint() / (14.0/60.0 * 2.0 * 3.1415 * 0.02225 * 1_m)
       // * 1_rad);
@@ -53,8 +50,8 @@ void wom::subsystems::Elevator::OnUpdate(units::second_t dt) {
       // voltage = 0_V;
     } break;
     case ElevatorState::kPID: {
-      units::volt_t feedforward = _config.rightGearbox.motor.Voltage(
-          (_config.mass * 9.81_mps_sq) * _config.radius, 0_rad_per_s);
+      units::volt_t feedforward =
+          _config.rightGearbox.motor.Voltage((_config.mass * 9.81_mps_sq) * _config.radius, 0_rad_per_s);
       // std::cout << "feed forward" << feedforward.value() << std::endl;
       feedforward = 1.2_V;
       // voltage = _pid.Calculate(height, dt, feedforward);
@@ -99,8 +96,7 @@ void wom::subsystems::Elevator::SetPID(units::meter_t height) {
   _pid.SetSetpoint(height);
 }
 
-void wom::subsystems::Elevator::SetVelocity(
-    units::meters_per_second_t velocity) {
+void wom::subsystems::Elevator::SetVelocity(units::meters_per_second_t velocity) {
   _velocityPID.SetSetpoint(velocity);
 }
 
@@ -132,18 +128,14 @@ units::meter_t wom::subsystems::Elevator::GetHeight() const {
   // std::cout << "elevator position"<<
   // _config.rightGearbox.encoder->GetEncoderTicks() << std::endl; return
   // _config.rightGearbox.encoder->GetEncoderDistance() * 1_m;
-  return _config.elevatorEncoder.GetPosition() * 14 / 60 * 2 * 3.1415 *
-         0.02225 * 1_m;
+  return _config.elevatorEncoder.GetPosition() * 14 / 60 * 2 * 3.1415 * 0.02225 * 1_m;
 }
 
-units::meters_per_second_t wom::subsystems::Elevator::GetElevatorVelocity()
-    const {
-  return _config.elevatorEncoder.GetVelocity() / 60_s * 14 / 60 * 2 * 3.1415 *
-         0.02225 * 1_m;
+units::meters_per_second_t wom::subsystems::Elevator::GetElevatorVelocity() const {
+  return _config.elevatorEncoder.GetVelocity() / 60_s * 14 / 60 * 2 * 3.1415 * 0.02225 * 1_m;
 }
 
 units::meters_per_second_t wom::subsystems::Elevator::MaxSpeed() const {
-  return _config.leftGearbox.motor.Speed(
-             (_config.mass * 9.81_mps_sq) * _config.radius, 12_V) /
-         1_rad * _config.radius;
+  return _config.leftGearbox.motor.Speed((_config.mass * 9.81_mps_sq) * _config.radius, 12_V) / 1_rad *
+         _config.radius;
 }
diff --git a/wombat/src/main/cpp/subsystems/Shooter.cpp b/wombat/src/main/cpp/subsystems/Shooter.cpp
index 2fede2f3..c851266b 100644
--- a/wombat/src/main/cpp/subsystems/Shooter.cpp
+++ b/wombat/src/main/cpp/subsystems/Shooter.cpp
@@ -14,16 +14,14 @@ wom::subsystems::Shooter::Shooter(std::string path, ShooterParams params)
 
 void wom::subsystems::Shooter::OnUpdate(units::second_t dt) {
   units::volt_t voltage{0};
-  units::revolutions_per_minute_t currentSpeed =
-      _params.gearbox.encoder->GetEncoderAngularVelocity();
+  units::revolutions_per_minute_t currentSpeed = _params.gearbox.encoder->GetEncoderAngularVelocity();
 
   switch (_state) {
     case ShooterState::kManual:
       voltage = _setpointManual;
       break;
     case ShooterState::kPID: {
-      auto feedforward =
-          _params.gearbox.motor.Voltage(0_Nm, _pid.GetSetpoint());
+      auto feedforward = _params.gearbox.motor.Voltage(0_Nm, _pid.GetSetpoint());
       voltage = _pid.Calculate(currentSpeed, dt, feedforward);
     } break;
     case ShooterState::kIdle:
@@ -31,20 +29,17 @@ void wom::subsystems::Shooter::OnUpdate(units::second_t dt) {
       break;
   }
 
-  units::newton_meter_t max_torque_at_current_limit =
-      _params.gearbox.motor.Torque(_params.currentLimit);
+  units::newton_meter_t max_torque_at_current_limit = _params.gearbox.motor.Torque(_params.currentLimit);
   units::volt_t max_voltage_for_current_limit =
       _params.gearbox.motor.Voltage(max_torque_at_current_limit, currentSpeed);
 
-  voltage =
-      1_V * std::min(voltage.value(), max_voltage_for_current_limit.value());
+  voltage = 1_V * std::min(voltage.value(), max_voltage_for_current_limit.value());
 
   // _params.gearbox.motorController->SetVoltage(voltage);
 
   _table->GetEntry("output_volts").SetDouble(voltage.value());
   _table->GetEntry("speed_rpm").SetDouble(currentSpeed.value());
-  _table->GetEntry("setpoint_rpm")
-      .SetDouble(units::revolutions_per_minute_t{_pid.GetSetpoint()}.value());
+  _table->GetEntry("setpoint_rpm").SetDouble(units::revolutions_per_minute_t{_pid.GetSetpoint()}.value());
   _table->GetEntry("stable").SetBoolean(_pid.IsStable());
 }
 
@@ -67,8 +62,7 @@ bool wom::subsystems::Shooter::IsStable() const {
 
 // Shooter Manual Set
 
-wom::subsystems::ShooterConstant::ShooterConstant(Shooter* s,
-                                                  units::volt_t setpoint)
+wom::subsystems::ShooterConstant::ShooterConstant(Shooter* s, units::volt_t setpoint)
     : _shooter(s), _setpoint(setpoint) {
   Controls(_shooter);
 }
@@ -79,9 +73,7 @@ void wom::subsystems::ShooterConstant::OnTick(units::second_t dt) {
 
 // ShooterSpinup
 
-wom::subsystems::ShooterSpinup::ShooterSpinup(Shooter* s,
-                                              units::radians_per_second_t speed,
-                                              bool hold)
+wom::subsystems::ShooterSpinup::ShooterSpinup(Shooter* s, units::radians_per_second_t speed, bool hold)
     : _shooter(s), _speed(speed), _hold(hold) {
   Controls(_shooter);
 }
diff --git a/wombat/src/main/cpp/utils/Encoder.cpp b/wombat/src/main/cpp/utils/Encoder.cpp
index 0c04e682..be0f576b 100644
--- a/wombat/src/main/cpp/utils/Encoder.cpp
+++ b/wombat/src/main/cpp/utils/Encoder.cpp
@@ -63,8 +63,7 @@ double wom::utils::Encoder::GetEncoderDistance() {
 units::radians_per_second_t wom::utils::Encoder::GetEncoderAngularVelocity() {
   // return GetEncoderTickVelocity() / (double)GetEncoderTicksPerRotation() * 2
   // * 3.1415926;
-  units::turns_per_second_t n_turns_per_s{GetEncoderTickVelocity() /
-                                          GetEncoderTicksPerRotation()};
+  units::turns_per_second_t n_turns_per_s{GetEncoderTickVelocity() / GetEncoderTicksPerRotation()};
   return n_turns_per_s;
 }
 
@@ -86,8 +85,7 @@ double wom::utils::DigitalEncoder::GetEncoderTickVelocity() const {
   return _nativeEncoder.GetRate();
 }
 
-wom::utils::CANSparkMaxEncoder::CANSparkMaxEncoder(rev::CANSparkMax* controller,
-                                                   units::meter_t wheelRadius,
+wom::utils::CANSparkMaxEncoder::CANSparkMaxEncoder(rev::CANSparkMax* controller, units::meter_t wheelRadius,
                                                    double reduction)
     : wom::utils::Encoder(42, reduction, wheelRadius, 2),
       _encoder(controller->GetEncoder(rev::SparkRelativeEncoder::Type::kHallSensor)) {}
@@ -119,11 +117,9 @@ double wom::utils::CANSparkMaxEncoder::GetVelocity() const {
   return _encoder.GetVelocity();
 }
 
-wom::utils::TalonFXEncoder::TalonFXEncoder(
-    ctre::phoenix6::hardware::TalonFX* controller, units::meter_t wheelRadius,
-    double reduction)
-    : utils::Encoder(2048, reduction, wheelRadius, 0),
-      _controller(controller) {}
+wom::utils::TalonFXEncoder::TalonFXEncoder(ctre::phoenix6::hardware::TalonFX* controller,
+                                           units::meter_t wheelRadius, double reduction)
+    : utils::Encoder(2048, reduction, wheelRadius, 0), _controller(controller) {}
 
 double wom::utils::TalonFXEncoder::GetEncoderRawTicks() const {
   return _controller->GetPosition().GetValue().value();
@@ -136,9 +132,8 @@ double wom::utils::TalonFXEncoder::GetEncoderTickVelocity() const {
 wom::utils::DutyCycleEncoder::DutyCycleEncoder(int channel, units::meter_t wheelRadius,
                                                double ticksPerRotation, double reduction)
     : wom::utils::Encoder(ticksPerRotation, reduction, wheelRadius, 0) {
-      _dutyCycleEncoder = new frc::DutyCycleEncoder(channel);
-      
-    }
+  _dutyCycleEncoder = new frc::DutyCycleEncoder(channel);
+}
 
 double wom::utils::DutyCycleEncoder::GetEncoderRawTicks() const {
   return _dutyCycleEncoder->GetAbsolutePosition();
@@ -161,4 +156,4 @@ double wom::utils::CanEncoder::GetEncoderRawTicks() const {
 
 double wom::utils::CanEncoder::GetEncoderTickVelocity() const {
   return _canEncoder->GetVelocity().GetValue().value();
-}
\ No newline at end of file
+}
diff --git a/wombat/src/main/cpp/utils/Util.cpp b/wombat/src/main/cpp/utils/Util.cpp
index 0aa4b888..dbdbb642 100644
--- a/wombat/src/main/cpp/utils/Util.cpp
+++ b/wombat/src/main/cpp/utils/Util.cpp
@@ -53,18 +53,12 @@ void wom::utils::WriteTrajectory(std::shared_ptr<nt::NetworkTable> table, frc::T
   // write the trajectory to the network table
   int i = 0;
   for (auto state : trajectory.States()) {
-    table->GetSubTable(std::to_string(i))
-        ->GetEntry("x")
-        .SetDouble(state.pose.X().value());
-    table->GetSubTable(std::to_string(i))
-        ->GetEntry("y")
-        .SetDouble(state.pose.Y().value());
+    table->GetSubTable(std::to_string(i))->GetEntry("x").SetDouble(state.pose.X().value());
+    table->GetSubTable(std::to_string(i))->GetEntry("y").SetDouble(state.pose.Y().value());
     table->GetSubTable(std::to_string(i))
         ->GetEntry("angle")
         .SetDouble(state.pose.Rotation().Degrees().value());
-    table->GetSubTable(std::to_string(i))
-        ->GetEntry("time")
-        .SetDouble(state.t.value());
+    table->GetSubTable(std::to_string(i))->GetEntry("time").SetDouble(state.t.value());
 
     i++;
   }
diff --git a/wombat/src/main/include/Wombat.h b/wombat/src/main/include/Wombat.h
index 1e75f79e..d48f4611 100644
--- a/wombat/src/main/include/Wombat.h
+++ b/wombat/src/main/include/Wombat.h
@@ -9,16 +9,16 @@
 #include "behaviour/HasBehaviour.h"
 #include "drivetrain/SwerveDrive.h"
 #include "drivetrain/behaviours/SwerveBehaviours.h"
+#include "sim/Sim.h"
 #include "subsystems/Arm.h"
 #include "utils/Encoder.h"
 #include "utils/Gearbox.h"
 #include "utils/PID.h"
+#include "utils/Pathplanner.h"
 #include "utils/RobotStartup.h"
 #include "utils/Util.h"
-#include "utils/Pathplanner.h"
 #include "vision/Camera.h"
 #include "vision/Limelight.h"
-#include "sim/Sim.h"
 
 namespace wom {
 using namespace wom;
diff --git a/wombat/src/main/include/behaviour/Behaviour.h b/wombat/src/main/include/behaviour/Behaviour.h
index e56e6bf7..84ac8689 100644
--- a/wombat/src/main/include/behaviour/Behaviour.h
+++ b/wombat/src/main/include/behaviour/Behaviour.h
@@ -239,10 +239,8 @@ class ConcurrentBehaviour : public Behaviour {
  * Create a concurrent behaviour group, waiting for all behaviours
  * to finish before moving on.
  */
-inline std::shared_ptr<ConcurrentBehaviour> operator&(Behaviour::ptr a,
-                                                      Behaviour::ptr b) {
-  auto conc =
-      std::make_shared<ConcurrentBehaviour>(ConcurrentBehaviourReducer::ALL);
+inline std::shared_ptr<ConcurrentBehaviour> operator&(Behaviour::ptr a, Behaviour::ptr b) {
+  auto conc = std::make_shared<ConcurrentBehaviour>(ConcurrentBehaviourReducer::ALL);
   conc->Add(a);
   conc->Add(b);
   return conc;
diff --git a/wombat/src/main/include/drivetrain/SwerveDrive.h b/wombat/src/main/include/drivetrain/SwerveDrive.h
index 21eea3f4..89fbbc7c 100644
--- a/wombat/src/main/include/drivetrain/SwerveDrive.h
+++ b/wombat/src/main/include/drivetrain/SwerveDrive.h
@@ -4,9 +4,9 @@
 
 #pragma once
 
+#include <frc/controller/PIDController.h>
 #include <frc/estimator/SwerveDrivePoseEstimator.h>
 #include <frc/kinematics/SwerveDriveKinematics.h>
-#include <frc/controller/PIDController.h>
 #include <networktables/DoubleTopic.h>
 #include <networktables/NetworkTable.h>
 #include <networktables/NetworkTableInstance.h>
@@ -16,6 +16,9 @@
 #include <units/moment_of_inertia.h>
 #include <units/time.h>
 #include <units/velocity.h>
+#include <wpi/SymbolExports.h>
+#include <wpi/sendable/Sendable.h>
+#include <wpi/sendable/SendableHelper.h>
 
 #include <memory>
 #include <string>
@@ -28,19 +31,13 @@
 #include "utils/Gearbox.h"
 #include "utils/PID.h"
 
-#include <wpi/SymbolExports.h>
-#include <wpi/sendable/Sendable.h>
-#include <wpi/sendable/SendableHelper.h>
-
 namespace wom {
 namespace drivetrain {
 
 /**
  * Implements a PID control loop.
  */
-class PIDController
-    : public wpi::Sendable,
-      public wpi::SendableHelper<PIDController> {
+class PIDController : public wpi::Sendable, public wpi::SendableHelper<PIDController> {
  public:
   /**
    * Allocates a PIDController with the given constants for Kp, Ki, and Kd.
@@ -51,8 +48,7 @@ class PIDController
    * @param period The period between controller updates in seconds. The
    *               default is 20 milliseconds. Must be positive.
    */
-  PIDController(double Kp, double Ki, double Kd,
-                units::second_t period = 20_ms);
+  PIDController(double Kp, double Ki, double Kd, units::second_t period = 20_ms);
 
   ~PIDController() override = default;
 
@@ -215,9 +211,8 @@ class PIDController
    * @param positionTolerance Position error which is tolerable.
    * @param velocityTolerance Velocity error which is tolerable.
    */
-  void SetTolerance(
-      double positionTolerance,
-      double velocityTolerance = std::numeric_limits<double>::infinity());
+  void SetTolerance(double positionTolerance,
+                    double velocityTolerance = std::numeric_limits<double>::infinity());
 
   /**
    * Returns the difference between the setpoint and the measurement.
@@ -300,7 +295,6 @@ class PIDController
   bool m_haveMeasurement = false;
 };
 
-
 enum class SwerveModuleState { kZeroing, kIdle, kPID };
 enum class TurnOffsetValues { reverse, forward, none };
 
@@ -384,20 +378,18 @@ class SwerveModule {
 struct SwerveDriveConfig {
   /*using pose_angle_conf_t =
       utils::PIDConfig<units::radian, units::radians_per_second>;*/
-  using pose_position_conf_t =
-      utils::PIDConfig<units::meter, units::meters_per_second>;
-  using balance_conf_t =
-      utils::PIDConfig<units::degree, units::meters_per_second>;
+  using pose_position_conf_t = utils::PIDConfig<units::meter, units::meters_per_second>;
+  using balance_conf_t = utils::PIDConfig<units::degree, units::meters_per_second>;
 
   std::string path;
-  //SwerveModule::angle_pid_conf_t anglePID;
+  // SwerveModule::angle_pid_conf_t anglePID;
   SwerveModule::velocity_pid_conf_t velocityPID;
 
   wpi::array<SwerveModuleConfig, 4> modules;
 
   ctre::phoenix6::hardware::Pigeon2* gyro;
 
-  //pose_angle_conf_t poseAnglePID;
+  // pose_angle_conf_t poseAnglePID;
   pose_position_conf_t posePositionPID;
 
   units::kilogram_t mass;
@@ -451,8 +443,7 @@ class SwerveDrive : public behaviour::HasBehaviour {
    * @brief This function switches the state to handle the robot's rotation
    * matching that of the joystick
    */
-  void RotateMatchJoystick(units::radian_t joystickAngle,
-                           FieldRelativeSpeeds speeds);
+  void RotateMatchJoystick(units::radian_t joystickAngle, FieldRelativeSpeeds speeds);
 
   void SetIdle();
 
@@ -486,6 +477,7 @@ class SwerveDrive : public behaviour::HasBehaviour {
   frc::Pose2d GetSetpoint();
 
   void MakeAtSetPoint();
+
  private:
   SwerveDriveConfig _config;
   SwerveDriveState _state = SwerveDriveState::kIdle;
diff --git a/wombat/src/main/include/subsystems/Arm.h b/wombat/src/main/include/subsystems/Arm.h
index 4b1f3f1e..75ec4b00 100644
--- a/wombat/src/main/include/subsystems/Arm.h
+++ b/wombat/src/main/include/subsystems/Arm.h
@@ -70,8 +70,7 @@ class Arm : public behaviour::HasBehaviour {
   ArmConfig _config;
   ArmState _state = ArmState::kIdle;
   wom::utils::PIDController<units::radian, units::volt> _pid;
-  wom::utils::PIDController<units::radians_per_second, units::volt>
-      _velocityPID;
+  wom::utils::PIDController<units::radians_per_second, units::volt> _velocityPID;
 
   std::shared_ptr<nt::NetworkTable> _table;
 
diff --git a/wombat/src/main/include/subsystems/Shooter.h b/wombat/src/main/include/subsystems/Shooter.h
index 2bbf6f5e..e5ac219d 100644
--- a/wombat/src/main/include/subsystems/Shooter.h
+++ b/wombat/src/main/include/subsystems/Shooter.h
@@ -62,8 +62,7 @@ class ShooterConstant : public behaviour::Behaviour {
 
 class ShooterSpinup : public behaviour::Behaviour {
  public:
-  ShooterSpinup(Shooter* s, units::radians_per_second_t speed,
-                bool hold = false);
+  ShooterSpinup(Shooter* s, units::radians_per_second_t speed, bool hold = false);
 
   void OnTick(units::second_t dt) override;
 
diff --git a/wombat/src/main/include/utils/Encoder.h b/wombat/src/main/include/utils/Encoder.h
index d17ba4e2..8d06d725 100644
--- a/wombat/src/main/include/utils/Encoder.h
+++ b/wombat/src/main/include/utils/Encoder.h
@@ -125,4 +125,4 @@ class CanEncoder : public Encoder {
   ctre::phoenix6::hardware::CANcoder* _canEncoder;
 };
 }  // namespace utils
-}  // namespace wom
\ No newline at end of file
+}  // namespace wom

From 1928e95e1c4273c2202724534681f49b1b91459b Mon Sep 17 00:00:00 2001
From: = <paul.hodges@student.education.wa.edu.au>
Date: Sun, 3 Mar 2024 16:18:38 +0800
Subject: [PATCH 189/207] pathplanner

---
 src/main/cpp/AlphaArmBehaviour.cpp            |  2 +
 src/main/cpp/Robot.cpp                        |  5 +-
 src/main/cpp/vision/VisionBehaviours.cpp      |  5 +-
 .../deploy/pathplanner/paths/FirstNote.path   | 10 ++--
 src/main/include/AlphaArmBehaviour.h          |  3 +-
 .../src/main/cpp/drivetrain/SwerveDrive.cpp   | 49 +++++++++++++------
 wombat/src/main/cpp/sim/Sim.cpp               |  3 ++
 wombat/src/main/cpp/utils/Pathplanner.cpp     |  6 ++-
 .../src/main/include/drivetrain/SwerveDrive.h |  6 ++-
 9 files changed, 61 insertions(+), 28 deletions(-)

diff --git a/src/main/cpp/AlphaArmBehaviour.cpp b/src/main/cpp/AlphaArmBehaviour.cpp
index 6c449c64..2553e9b4 100644
--- a/src/main/cpp/AlphaArmBehaviour.cpp
+++ b/src/main/cpp/AlphaArmBehaviour.cpp
@@ -6,6 +6,8 @@
 
 #include <frc/XboxController.h>
 
+#include "vision/Vision.h"
+
 AlphaArmManualControl::AlphaArmManualControl(AlphaArm* alphaArm, Intake *intake, frc::XboxController* codriver)
     : _alphaArm(alphaArm), _intake(intake), _codriver(codriver) {
   Controls(alphaArm);
diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp
index 250788ea..1f7bbce0 100644
--- a/src/main/cpp/Robot.cpp
+++ b/src/main/cpp/Robot.cpp
@@ -138,7 +138,10 @@ void Robot::AutonomousInit() {
   _swerveDrive->OnStart();
 }
 
-void Robot::AutonomousPeriodic() {}
+void Robot::AutonomousPeriodic() {
+  // robotmap._simSwerve->OnTick(_swerveDrive->GetSetpoint());
+  // _swerveDrive->MakeAtSetPoint();
+}
 
 void Robot::TeleopInit() {
   loop.Clear();
diff --git a/src/main/cpp/vision/VisionBehaviours.cpp b/src/main/cpp/vision/VisionBehaviours.cpp
index b8bd3b40..ab2d8724 100644
--- a/src/main/cpp/vision/VisionBehaviours.cpp
+++ b/src/main/cpp/vision/VisionBehaviours.cpp
@@ -42,7 +42,9 @@ LockOnToTarget::LockOnToTarget(wom::PhotonVision* vision, Vision* limelight, wom
     : _camera(vision), _swerveDrive(swerveDrive), _vision(limelight), _type(VisionType::kGenericCamera) {}
 
 void LockOnToTarget::OnTick(units::second_t dt) {
-  if (!_vision->TargetIsVisible(VisionTargetObjects::kNote)) { SetDone(); }
+  if (!_vision->TargetIsVisible(VisionTargetObjects::kNote)) {
+    SetDone();
+  }
 
   units::degree_t angle;
 
@@ -62,5 +64,4 @@ void LockOnToTarget::OnTick(units::second_t dt) {
   if (_vision->IsAtPose(pose, dt)) {
     SetDone();
   }
-  
 }
diff --git a/src/main/deploy/pathplanner/paths/FirstNote.path b/src/main/deploy/pathplanner/paths/FirstNote.path
index c13b43fa..ecb9bbc0 100644
--- a/src/main/deploy/pathplanner/paths/FirstNote.path
+++ b/src/main/deploy/pathplanner/paths/FirstNote.path
@@ -16,12 +16,12 @@
     },
     {
       "anchor": {
-        "x": 0.1,
-        "y": 0.0
+        "x": 1.9932481519741916,
+        "y": -0.5855418272890752
       },
       "prevControl": {
-        "x": -0.04910272980275429,
-        "y": -0.0322153133318448
+        "x": 1.8441454221714373,
+        "y": -0.61775714062092
       },
       "nextControl": null,
       "isLocked": false,
@@ -49,4 +49,4 @@
     "velocity": 0
   },
   "useDefaultConstraints": true
-}
+}
\ No newline at end of file
diff --git a/src/main/include/AlphaArmBehaviour.h b/src/main/include/AlphaArmBehaviour.h
index b485c9f2..b72020ff 100644
--- a/src/main/include/AlphaArmBehaviour.h
+++ b/src/main/include/AlphaArmBehaviour.h
@@ -3,8 +3,7 @@
 // of the MIT License at the root of this project
 
 #pragma once
-
-#pragma once
+#include "vision/Vision.h"
 #include <frc/XboxController.h>
 
 #include "AlphaArm.h"
diff --git a/wombat/src/main/cpp/drivetrain/SwerveDrive.cpp b/wombat/src/main/cpp/drivetrain/SwerveDrive.cpp
index 62505d19..1538b43d 100644
--- a/wombat/src/main/cpp/drivetrain/SwerveDrive.cpp
+++ b/wombat/src/main/cpp/drivetrain/SwerveDrive.cpp
@@ -12,9 +12,11 @@
 #include <wpi/sendable/SendableRegistry.h>
 
 #include <cmath>
+#include <cstdlib>
 #include <iostream>
 
 #include "frc/MathUtil.h"
+#include "units/velocity.h"
 #include "utils/Util.h"
 
 using namespace wom;
@@ -426,9 +428,12 @@ SwerveDrive::SwerveDrive(SwerveDriveConfig config, frc::Pose2d initialPose)
                                                    frc::SwerveModulePosition{0_m, frc::Rotation2d{0_deg}},
                                                    frc::SwerveModulePosition{0_m, frc::Rotation2d{0_deg}}},
           initialPose, _config.stateStdDevs, _config.visionMeasurementStdDevs),
-      _anglePIDController{PIDController(1, 0, 0)},
-      _xPIDController(config.path + "/pid/x", _config.posePositionPID),
-      _yPIDController(config.path + "/pid/y", _config.posePositionPID),
+      _anglePIDController{PIDController(1.5, 0, 0)},
+      _xPIDController(PIDController(1, 0, 0)),
+      _yPIDController(PIDController(1, 0, 0)),
+      // _xPIDController(std::string path, config_t initialGains)
+      // _xPIDController(config.path + "/pid/x", _config.posePositionPID),
+      // _yPIDController(config.path + "/pid/y", _config.posePositionPID),
       _table(nt::NetworkTableInstance::GetDefault().GetTable(_config.path)) {
   _anglePIDController.SetTolerance(360);
 
@@ -462,10 +467,17 @@ void SwerveDrive::OnUpdate(units::second_t dt) {
       }
       break;
     case SwerveDriveState::kPose: {
-      _target_fr_speeds.vx = _xPIDController.Calculate(GetPose().X(), dt);
-      _target_fr_speeds.vy = _yPIDController.Calculate(GetPose().Y(), dt);
-      _target_fr_speeds.omega =
-          units::radians_per_second_t{_anglePIDController.Calculate(GetPose().Rotation().Radians().value())};
+      // _target_fr_speeds.vx = _xPIDController.Calculate(GetPose().X(), dt);
+      // _target_fr_speeds.vy = _yPIDController.Calculate(GetPose().Y(), dt);
+      _target_fr_speeds.vx = units::meters_per_second_t{_xPIDController.Calculate(GetPose().X().value())};
+      _target_fr_speeds.vx = units::meters_per_second_t{_xPIDController.Calculate(GetPose().Y().value())};
+      _target_fr_speeds.omega = 0_rad_per_s;
+          // -units::radians_per_second_t{_anglePIDController.Calculate(GetPose().Rotation().Radians().value())};
+
+      _table->GetEntry("Swerve vx").SetDouble(_target_fr_speeds.vx.value());
+      _table->GetEntry("Swerve vy").SetDouble(_target_fr_speeds.vy.value());
+      _table->GetEntry("Swerve omega").SetDouble(_target_fr_speeds.omega.value());
+
     }
       [[fallthrough]];
     case SwerveDriveState::kFieldRelativeVelocity:
@@ -524,8 +536,11 @@ void SwerveDrive::OnUpdate(units::second_t dt) {
       _modules[3].SetPID(315_deg, 0_mps, dt);
       break;
     case SwerveDriveState::kFRVelocityRotationLock:
-      _target_speed.vx = _xPIDController.Calculate(GetPose().X(), dt);
-      _target_speed.vy = _yPIDController.Calculate(GetPose().Y(), dt);
+      // _target_speed.vx = _xPIDController.Calculate(GetPose().X(), dt);
+      // _target_speed.vy = _yPIDController.Calculate(GetPose().Y(), dt);
+      _target_fr_speeds.vx = units::meters_per_second_t{_xPIDController.Calculate(GetPose().X().value())};
+      _target_fr_speeds.vx = units::meters_per_second_t{_xPIDController.Calculate(GetPose().Y().value())};
+
       _target_speed.omega =
           units::radians_per_second_t{_anglePIDController.Calculate(GetPose().Rotation().Radians().value())};
       _target_speed = _requestedSpeeds.ToChassisSpeeds(GetPose().Rotation().Radians());
@@ -630,13 +645,19 @@ void SwerveDrive::SetFieldRelativeVelocity(FieldRelativeSpeeds speeds) {
 void SwerveDrive::SetPose(frc::Pose2d pose) {
   _state = SwerveDriveState::kPose;
   _anglePIDController.SetSetpoint(pose.Rotation().Radians().value());
-  _xPIDController.SetSetpoint(pose.X());
-  _yPIDController.SetSetpoint(pose.Y());
+  _xPIDController.SetSetpoint(pose.X().value());
+  _yPIDController.SetSetpoint(pose.Y().value());
+
+  utils::WritePose2NT(nt::NetworkTableInstance::GetDefault().GetTable("swerveSetpoint"), GetSetpoint());
 }
 
 bool SwerveDrive::IsAtSetPose() {
-  return /*_anglePIDController.IsStable()*/ true && _xPIDController.IsStable() &&
-         _yPIDController.IsStable(0.05_m);
+  if (std::abs(_xPIDController.GetPositionError()) < 0.1 && std::abs(_yPIDController.GetPositionError()) < 0.1 && std::abs(_anglePIDController.GetPositionError()) < 0.1) {
+    if (std::abs(_xPIDController.GetVelocityError()) < 1 && std::abs(_yPIDController.GetVelocityError()) < 1 && std::abs(_anglePIDController.GetVelocityError()) < 1) {
+      return true;
+    }
+  }
+  return false;
 }
 
 void SwerveDrive::ResetPose(frc::Pose2d pose) {
@@ -652,7 +673,7 @@ frc::Pose2d SwerveDrive::GetPose() {
 }
 
 void SwerveDrive::AddVisionMeasurement(frc::Pose2d pose, units::second_t timestamp) {
-  _poseEstimator.AddVisionMeasurement(pose, timestamp);
+  // _poseEstimator.AddVisionMeasurement(pose, timestamp);
 }
 
 frc::Pose2d SwerveDrive::GetSetpoint() {
diff --git a/wombat/src/main/cpp/sim/Sim.cpp b/wombat/src/main/cpp/sim/Sim.cpp
index 67848a5f..0234eaa6 100644
--- a/wombat/src/main/cpp/sim/Sim.cpp
+++ b/wombat/src/main/cpp/sim/Sim.cpp
@@ -7,6 +7,8 @@
 #include "drivetrain/SwerveDrive.h"
 #include "frc/geometry/Pose2d.h"
 #include "frc/smartdashboard/SmartDashboard.h"
+#include "networktables/NetworkTableInstance.h"
+#include "utils/Util.h"
 
 wom::sim::SimSwerve::SimSwerve(wom::drivetrain::SwerveDrive* _swerve) : _swerve(_swerve) {
   frc::SmartDashboard::PutData("Field", &_field);
@@ -14,6 +16,7 @@ wom::sim::SimSwerve::SimSwerve(wom::drivetrain::SwerveDrive* _swerve) : _swerve(
 
 void wom::sim::SimSwerve::OnTick() {
   _field.SetRobotPose(_swerve->GetPose());
+  utils::WritePose2NT(nt::NetworkTableInstance::GetDefault().GetTable("swerveSetpoint"), _swerve->GetSetpoint());
 }
 
 void wom::sim::SimSwerve::OnTick(frc::Pose2d pose) {
diff --git a/wombat/src/main/cpp/utils/Pathplanner.cpp b/wombat/src/main/cpp/utils/Pathplanner.cpp
index 5838b483..c217ac5e 100644
--- a/wombat/src/main/cpp/utils/Pathplanner.cpp
+++ b/wombat/src/main/cpp/utils/Pathplanner.cpp
@@ -332,13 +332,14 @@ void utils::FollowPath::CalcTimer() {
 
   _timer.Stop();
   _timer.Reset();
-  _time = units::second_t{std::abs(dist.value()) * 1 /*meters per second*/};
+  // _time = units::second_t{std::abs(dist.value()) * 1 /*meters per second*/};
+  _time = 20_s;
   _timer.Start();
 }
 
 void utils::FollowPath::OnTick(units::second_t dt) {
   _swerve->SetPose(_poses[_currentPose]);
-
+  
   nt::NetworkTableInstance::GetDefault()
       .GetTable("pathplanner")
       ->GetEntry("atPose")
@@ -360,6 +361,7 @@ void utils::FollowPath::OnTick(units::second_t dt) {
 
   if (_swerve->IsAtSetPose() || _timer.Get() > _time) {
     if (_currentPose + 1 == static_cast<int>(_poses.size())) {
+      _swerve->MakeAtSetPoint();
       SetDone();
     } else {
       _currentPose++;
diff --git a/wombat/src/main/include/drivetrain/SwerveDrive.h b/wombat/src/main/include/drivetrain/SwerveDrive.h
index 89fbbc7c..852aaf5b 100644
--- a/wombat/src/main/include/drivetrain/SwerveDrive.h
+++ b/wombat/src/main/include/drivetrain/SwerveDrive.h
@@ -504,8 +504,10 @@ class SwerveDrive : public behaviour::HasBehaviour {
   /*utils::PIDController<units::radian, units::radians_per_second>
       _anglePIDController;*/
   wom::drivetrain::PIDController _anglePIDController;
-  wom::utils::PIDController<units::meter, units::meters_per_second> _xPIDController;
-  wom::utils::PIDController<units::meter, units::meters_per_second> _yPIDController;
+  wom::drivetrain::PIDController _xPIDController;
+  wom::drivetrain::PIDController _yPIDController;
+  // wom::utils::PIDController<units::meter, units::meters_per_second> _xPIDController;
+  // wom::utils::PIDController<units::meter, units::meters_per_second> _yPIDController;
 
   std::shared_ptr<nt::NetworkTable> _table;
 

From 2c89aa3883ff4a9e8ddf0e6ebfb1c89f35415eba Mon Sep 17 00:00:00 2001
From: = <paul.hodges@student.education.wa.edu.au>
Date: Mon, 4 Mar 2024 12:22:58 +0800
Subject: [PATCH 190/207] auto

---
 simgui.json                                   |  3 ---
 src/main/cpp/Auto.cpp                         |  9 ++++++---
 src/main/cpp/Robot.cpp                        |  2 +-
 src/main/deploy/pathplanner/autos/Taxi.auto   | 12 ++++++++---
 .../deploy/pathplanner/paths/FirstNote.path   | 20 +++++++++----------
 .../src/main/cpp/drivetrain/SwerveDrive.cpp   | 20 ++++++++++++-------
 wombat/src/main/cpp/utils/Pathplanner.cpp     | 19 ++++++++++++------
 wombat/src/main/include/utils/Pathplanner.h   |  2 ++
 8 files changed, 54 insertions(+), 33 deletions(-)

diff --git a/simgui.json b/simgui.json
index ff5f09a8..d2bce9a8 100644
--- a/simgui.json
+++ b/simgui.json
@@ -38,9 +38,6 @@
     }
   },
   "NetworkTables Info": {
-    "Clients": {
-      "open": true
-    },
     "visible": true
   }
 }
diff --git a/src/main/cpp/Auto.cpp b/src/main/cpp/Auto.cpp
index e4571fd7..8e4bec0d 100644
--- a/src/main/cpp/Auto.cpp
+++ b/src/main/cpp/Auto.cpp
@@ -19,14 +19,17 @@ wom::SwerveAutoBuilder* autos::InitCommands(wom::drivetrain::SwerveDrive* _swerv
                                             Intake* _intake, AlphaArm* _alphaArm) {
   wom::AutoCommands c = *new wom::AutoCommands(
       {// {"ArmToSetPoint", [_alphaArm]() { return wom::make<ArmToSetPoint>(_alphaArm, 20_deg); }},
-       // {"AutoShoot", [_shooter]() { return wom::make<AutoShoot>(_shooter); }},
-       /*{"AutoIntake", [_intake]() { return wom::make<AutoIntake>(_intake); }}*/});
+       // {"Shoot", [_shooter]() { return wom::make<AutoShoot>(_shooter); }},
+       {"IntakeNote", [_intake]() { return wom::make<IntakeNote>(_intake)->WithTimeout(1_s); }},
+       {"PassNote", [_intake]() { return wom::make<PassNote>(_intake)->WithTimeout(1_s); }},
+       {"EjectNote", [_intake]() { return wom::make<EjectNote>(_intake)->WithTimeout(1_s); } }});
 
   return new wom::utils::SwerveAutoBuilder(_swerveDrive, "Taxi", c);
 }
 
 std::shared_ptr<behaviour::Behaviour> autos::Taxi(wom::SwerveAutoBuilder* builder) {
-  return builder->GetAutoRoutine("Taxi");
+  return wom::make<wom::DrivebasePoseBehaviour>(builder->GetSwerve(), frc::Pose2d(0_m, 0_m, -120_deg));
+  // return builder->GetAutoRoutine("Taxi");
   // return behaviour::make<ArmToSetPoint>(_alphaArm, 0_deg);
   // behaviour::make<AutoShoot>(_shooter);
   // behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp
index 1f7bbce0..13fb598f 100644
--- a/src/main/cpp/Robot.cpp
+++ b/src/main/cpp/Robot.cpp
@@ -155,7 +155,7 @@ void Robot::TeleopInit() {
 
   //  FMAP("fmap.fmap");
 
-  // _swerveDrive->OnStart();
+  _swerveDrive->OnStart();
   // sched->InterruptAll();
 
   // reimplement when vision is reimplemented
diff --git a/src/main/deploy/pathplanner/autos/Taxi.auto b/src/main/deploy/pathplanner/autos/Taxi.auto
index e0fd7b37..b39612f0 100644
--- a/src/main/deploy/pathplanner/autos/Taxi.auto
+++ b/src/main/deploy/pathplanner/autos/Taxi.auto
@@ -2,8 +2,8 @@
   "version": 1.0,
   "startingPose": {
     "position": {
-      "x": 0.0,
-      "y": 0.0
+      "x": 1.3732635157578024,
+      "y": 5.564955745296107
     },
     "rotation": 0.0
   },
@@ -16,10 +16,16 @@
           "data": {
             "pathName": "FirstNote"
           }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "IntakeNote"
+          }
         }
       ]
     }
   },
   "folder": null,
   "choreoAuto": false
-}
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/FirstNote.path b/src/main/deploy/pathplanner/paths/FirstNote.path
index ecb9bbc0..40a2e58a 100644
--- a/src/main/deploy/pathplanner/paths/FirstNote.path
+++ b/src/main/deploy/pathplanner/paths/FirstNote.path
@@ -3,25 +3,25 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 0.0,
-        "y": 0.0
+        "x": 1.37,
+        "y": 5.56
       },
       "prevControl": null,
       "nextControl": {
-        "x": 0.9978270455827627,
-        "y": 0.04684334618312441
+        "x": 2.37,
+        "y": 5.56
       },
       "isLocked": false,
       "linkedName": null
     },
     {
       "anchor": {
-        "x": 1.9932481519741916,
-        "y": -0.5855418272890752
+        "x": 2.918404239090931,
+        "y": 5.56
       },
       "prevControl": {
-        "x": 1.8441454221714373,
-        "y": -0.61775714062092
+        "x": 1.9184042390909308,
+        "y": 5.56
       },
       "nextControl": null,
       "isLocked": false,
@@ -39,13 +39,13 @@
   },
   "goalEndState": {
     "velocity": 0,
-    "rotation": 2.770215797200185,
+    "rotation": 40.88347073551137,
     "rotateFast": false
   },
   "reversed": false,
   "folder": null,
   "previewStartingState": {
-    "rotation": 1.1884043840473262,
+    "rotation": 0,
     "velocity": 0
   },
   "useDefaultConstraints": true
diff --git a/wombat/src/main/cpp/drivetrain/SwerveDrive.cpp b/wombat/src/main/cpp/drivetrain/SwerveDrive.cpp
index 1538b43d..e2622bde 100644
--- a/wombat/src/main/cpp/drivetrain/SwerveDrive.cpp
+++ b/wombat/src/main/cpp/drivetrain/SwerveDrive.cpp
@@ -428,15 +428,17 @@ SwerveDrive::SwerveDrive(SwerveDriveConfig config, frc::Pose2d initialPose)
                                                    frc::SwerveModulePosition{0_m, frc::Rotation2d{0_deg}},
                                                    frc::SwerveModulePosition{0_m, frc::Rotation2d{0_deg}}},
           initialPose, _config.stateStdDevs, _config.visionMeasurementStdDevs),
-      _anglePIDController{PIDController(1.5, 0, 0)},
-      _xPIDController(PIDController(1, 0, 0)),
-      _yPIDController(PIDController(1, 0, 0)),
+      _anglePIDController{PIDController(0.7, 0, 0)},
+      _xPIDController(PIDController(0, 0, 0)),
+      _yPIDController(PIDController(0, 0, 0)),
       // _xPIDController(std::string path, config_t initialGains)
       // _xPIDController(config.path + "/pid/x", _config.posePositionPID),
       // _yPIDController(config.path + "/pid/y", _config.posePositionPID),
       _table(nt::NetworkTableInstance::GetDefault().GetTable(_config.path)) {
   _anglePIDController.SetTolerance(360);
-
+  _anglePIDController.EnableContinuousInput(0, 2 * 3.14159);
+  // _anglePIDController.EnableContinuousInput(-3.14159, 3.14159);
+    
   int i = 1;
   for (auto cfg : _config.modules) {
     _modules.emplace_back(config.path + "/modules/" + std::to_string(i), cfg,
@@ -470,9 +472,9 @@ void SwerveDrive::OnUpdate(units::second_t dt) {
       // _target_fr_speeds.vx = _xPIDController.Calculate(GetPose().X(), dt);
       // _target_fr_speeds.vy = _yPIDController.Calculate(GetPose().Y(), dt);
       _target_fr_speeds.vx = units::meters_per_second_t{_xPIDController.Calculate(GetPose().X().value())};
-      _target_fr_speeds.vx = units::meters_per_second_t{_xPIDController.Calculate(GetPose().Y().value())};
-      _target_fr_speeds.omega = 0_rad_per_s;
-          // -units::radians_per_second_t{_anglePIDController.Calculate(GetPose().Rotation().Radians().value())};
+      _target_fr_speeds.vy = units::meters_per_second_t{_yPIDController.Calculate(GetPose().Y().value())};
+      _target_fr_speeds.omega = /*0_rad_per_s;*/
+          units::radians_per_second_t{_anglePIDController.Calculate(GetPose().Rotation().Radians().value())};
 
       _table->GetEntry("Swerve vx").SetDouble(_target_fr_speeds.vx.value());
       _table->GetEntry("Swerve vy").SetDouble(_target_fr_speeds.vy.value());
@@ -585,6 +587,9 @@ void SwerveDrive::SetVoltageLimit(units::volt_t driveVoltageLimit) {
 void SwerveDrive::OnStart() {
   OnResetMode();
 
+  _anglePIDController.EnableContinuousInput(0, 2 * 3.14159);
+  // _anglePIDController.EnableContinuousInput(-3.14159, 3.14159);
+
   _modules[0].OnStart();  // front left
   _modules[1].OnStart();  // front right
   _modules[2].OnStart();  // back right
@@ -670,6 +675,7 @@ void SwerveDrive::ResetPose(frc::Pose2d pose) {
 
 frc::Pose2d SwerveDrive::GetPose() {
   return _poseEstimator.GetEstimatedPosition();
+  // return frc::Pose2d(1_m, 1_m, 0_deg);
 }
 
 void SwerveDrive::AddVisionMeasurement(frc::Pose2d pose, units::second_t timestamp) {
diff --git a/wombat/src/main/cpp/utils/Pathplanner.cpp b/wombat/src/main/cpp/utils/Pathplanner.cpp
index c217ac5e..4fcd6907 100644
--- a/wombat/src/main/cpp/utils/Pathplanner.cpp
+++ b/wombat/src/main/cpp/utils/Pathplanner.cpp
@@ -27,6 +27,7 @@
 #include "frc/Filesystem.h"
 #include "frc/geometry/Pose2d.h"
 #include "frc/geometry/Rotation2d.h"
+#include "frc/geometry/Transform2d.h"
 #include "frc/kinematics/ChassisSpeeds.h"
 #include "frc/smartdashboard/SmartDashboard.h"
 #include "networktables/NetworkTable.h"
@@ -272,7 +273,7 @@ utils::FollowPath::FollowPath(drivetrain::SwerveDrive* swerve, std::string path,
 
     lastRot = &t;
 
-    _poses.emplace_back(frc::Pose2d(point.position, rot));
+    _poses.emplace_back(frc::Pose2d(point.position, rot).TransformBy(frc::Transform2d(-1.37_m, -5.56_m, 0_deg)));
   }
   // _poses = pathplanner::PathPlannerPath::fromPathFile(path)->getPathPoses();
   // wpi::json j = wpi::json::parse(cjson);
@@ -332,8 +333,8 @@ void utils::FollowPath::CalcTimer() {
 
   _timer.Stop();
   _timer.Reset();
-  // _time = units::second_t{std::abs(dist.value()) * 1 /*meters per second*/};
-  _time = 20_s;
+  _time = units::second_t{std::abs(dist.value()) * 2 /*meters per second*/};
+  // _time = 20_s;
   _timer.Start();
 }
 
@@ -361,6 +362,8 @@ void utils::FollowPath::OnTick(units::second_t dt) {
 
   if (_swerve->IsAtSetPose() || _timer.Get() > _time) {
     if (_currentPose + 1 == static_cast<int>(_poses.size())) {
+      // _swerve->MakeAtSetPoint();
+      _swerve->SetVelocity(frc::ChassisSpeeds());
       _swerve->MakeAtSetPoint();
       SetDone();
     } else {
@@ -396,7 +399,7 @@ void utils::AutoBuilder::SetAuto(std::string path) {
     cjson += cdata;
   }
 
-  cjson.pop_back();
+  // cjson.pop_back();
 
   wpi::json j = wpi::json::parse(cjson);
 
@@ -418,7 +421,7 @@ void utils::AutoBuilder::SetAuto(std::string path) {
     if (c["type"] == "path") {
       commands.push_back(std::make_pair(c["type"], c["data"]["pathName"]));
     }
-    if (c["type"] == "command") {
+    if (c["type"] == "named") {
       commands.push_back(std::make_pair(c["type"], c["data"]["name"]));
     }
   }
@@ -426,7 +429,7 @@ void utils::AutoBuilder::SetAuto(std::string path) {
   nt::NetworkTableInstance::GetDefault().GetTable("commands")->GetEntry("length").SetInteger(commands.size());
 
   pathplan = behaviour::make<drivetrain::behaviours::DrivebasePoseBehaviour>(
-      _swerve, JSONPoseToPose2d(*_startingPose));
+      _swerve, JSONPoseToPose2d(*_startingPose).TransformBy(frc::Transform2d(-1.37_m, -5.56_m, 0_deg)));
 
   int i = 0;
   int pathamt = 0;
@@ -520,3 +523,7 @@ std::shared_ptr<behaviour::Behaviour> utils::SwerveAutoBuilder::GetAutoRoutine()
 std::shared_ptr<behaviour::Behaviour> utils::SwerveAutoBuilder::GetAutoRoutine(std::string path) {
   return _builder->GetAutoPath(path);
 }
+
+drivetrain::SwerveDrive* utils::SwerveAutoBuilder::GetSwerve() {
+  return _swerve;
+}
diff --git a/wombat/src/main/include/utils/Pathplanner.h b/wombat/src/main/include/utils/Pathplanner.h
index f4d54a13..f9980349 100644
--- a/wombat/src/main/include/utils/Pathplanner.h
+++ b/wombat/src/main/include/utils/Pathplanner.h
@@ -309,6 +309,8 @@ class SwerveAutoBuilder {
   std::shared_ptr<behaviour::Behaviour> GetAutoRoutine();
   std::shared_ptr<behaviour::Behaviour> GetAutoRoutine(std::string path);
 
+  drivetrain::SwerveDrive* GetSwerve();
+
  private:
   AutoBuilder* _builder;
   drivetrain::SwerveDrive* _swerve;

From bd517e6ab31cd3fd7e5169f05440bc7624fec497 Mon Sep 17 00:00:00 2001
From: = <paul.hodges@student.education.wa.edu.au>
Date: Mon, 4 Mar 2024 19:59:11 +0800
Subject: [PATCH 191/207] auto

---
 src/main/cpp/Auto.cpp                         |  7 +-
 src/main/cpp/Robot.cpp                        |  4 +-
 src/main/cpp/ShooterBehaviour.cpp             | 27 ++++++
 src/main/deploy/pathplanner/autos/Taxi.auto   | 22 ++++-
 .../deploy/pathplanner/paths/BackToStart.path | 52 +++++++++++
 .../deploy/pathplanner/paths/FirstNote.path   |  6 +-
 src/main/include/ShooterBehaviour.h           | 17 ++++
 .../src/main/cpp/drivetrain/SwerveDrive.cpp   | 87 +++++++++++++++----
 .../behaviours/SwerveBehaviours.cpp           | 36 ++++++--
 wombat/src/main/cpp/utils/Encoder.cpp         |  1 +
 wombat/src/main/cpp/utils/Pathplanner.cpp     | 76 ++++++++++++----
 .../src/main/include/drivetrain/SwerveDrive.h | 12 +++
 .../drivetrain/behaviours/SwerveBehaviours.h  |  3 +
 13 files changed, 300 insertions(+), 50 deletions(-)
 create mode 100644 src/main/deploy/pathplanner/paths/BackToStart.path

diff --git a/src/main/cpp/Auto.cpp b/src/main/cpp/Auto.cpp
index 8e4bec0d..8c37a0e5 100644
--- a/src/main/cpp/Auto.cpp
+++ b/src/main/cpp/Auto.cpp
@@ -22,14 +22,15 @@ wom::SwerveAutoBuilder* autos::InitCommands(wom::drivetrain::SwerveDrive* _swerv
        // {"Shoot", [_shooter]() { return wom::make<AutoShoot>(_shooter); }},
        {"IntakeNote", [_intake]() { return wom::make<IntakeNote>(_intake)->WithTimeout(1_s); }},
        {"PassNote", [_intake]() { return wom::make<PassNote>(_intake)->WithTimeout(1_s); }},
-       {"EjectNote", [_intake]() { return wom::make<EjectNote>(_intake)->WithTimeout(1_s); } }});
+       {"EjectNote", [_intake]() { return wom::make<EjectNote>(_intake)->WithTimeout(1_s); }},
+       {"Shoot", [_shooter, _intake]() { return wom::make<AutoShooter>(_shooter, _intake, 1500_rad_per_s); }} 
+      });
 
   return new wom::utils::SwerveAutoBuilder(_swerveDrive, "Taxi", c);
 }
 
 std::shared_ptr<behaviour::Behaviour> autos::Taxi(wom::SwerveAutoBuilder* builder) {
-  return wom::make<wom::DrivebasePoseBehaviour>(builder->GetSwerve(), frc::Pose2d(0_m, 0_m, -120_deg));
-  // return builder->GetAutoRoutine("Taxi");
+  return builder->GetAutoRoutine("Taxi");
   // return behaviour::make<ArmToSetPoint>(_alphaArm, 0_deg);
   // behaviour::make<AutoShoot>(_shooter);
   // behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp
index 13fb598f..8b4962bd 100644
--- a/src/main/cpp/Robot.cpp
+++ b/src/main/cpp/Robot.cpp
@@ -49,7 +49,7 @@ void Robot::RobotInit() {
   shooter->SetDefaultBehaviour(
       [this]() { return wom::make<ShooterManualControl>(shooter, &robotmap.controllers.codriver, _led); });
 
-  _swerveDrive = new wom::SwerveDrive(robotmap.swerveBase.config, frc::Pose2d());
+  _swerveDrive = new wom::SwerveDrive(robotmap.swerveBase.config, frc::Pose2d(0_m, 0_m, 0_deg));
   wom::BehaviourScheduler::GetInstance()->Register(_swerveDrive);
   _swerveDrive->SetDefaultBehaviour(
       [this]() { return wom::make<wom::ManualDrivebase>(_swerveDrive, &robotmap.controllers.driver); });
@@ -87,6 +87,8 @@ void Robot::RobotPeriodic() {
 
   loop.Poll();
   wom::BehaviourScheduler::GetInstance()->Tick();
+
+        nt::NetworkTableInstance::GetDefault().GetTable("drivetrainpose")->GetEntry("state").SetInteger(static_cast<int>(_swerveDrive->GetState()));
   // sched->Tick();
 
   // robotmap.swerveTable.swerveDriveTable->GetEntry("frontLeftEncoder")
diff --git a/src/main/cpp/ShooterBehaviour.cpp b/src/main/cpp/ShooterBehaviour.cpp
index c925c18f..b2daeab5 100644
--- a/src/main/cpp/ShooterBehaviour.cpp
+++ b/src/main/cpp/ShooterBehaviour.cpp
@@ -3,6 +3,11 @@
 // of the MIT License at the root of this project
 
 #include "ShooterBehaviour.h"
+#include "Intake.h"
+#include "Shooter.h"
+#include "units/angle.h"
+#include "units/angular_velocity.h"
+#include "units/time.h"
 
 ShooterManualControl::ShooterManualControl(Shooter* shooter, frc::XboxController* tester, LED* led)
     : _shooter(shooter), _codriver(tester), _led(led) {
@@ -50,3 +55,25 @@ void ShooterManualControl::OnTick(units::second_t dt) {
     }
   }
 }
+
+AutoShooter::AutoShooter(Shooter* shooter, Intake* intake, units::radians_per_second_t goal) : _shooter(shooter), _intake(intake), _goal(goal) {
+  Controls(shooter);
+
+  _timer.Start();
+}
+
+void AutoShooter::OnTick(units::second_t dt) {
+  _shooter->SetState(ShooterState::kSpinUp);
+  _shooter->SetPidGoal(_goal);
+
+  if (_timer.Get() > 3_s) {
+    _intake->SetState(IntakeState::kPass);
+
+    if (_timer.Get() > 5_s) {
+      _intake->SetState(IntakeState::kIdle);
+      _shooter->SetState(ShooterState::kIdle);
+
+      SetDone();
+    }
+  }
+}
diff --git a/src/main/deploy/pathplanner/autos/Taxi.auto b/src/main/deploy/pathplanner/autos/Taxi.auto
index b39612f0..faf982ba 100644
--- a/src/main/deploy/pathplanner/autos/Taxi.auto
+++ b/src/main/deploy/pathplanner/autos/Taxi.auto
@@ -2,8 +2,8 @@
   "version": 1.0,
   "startingPose": {
     "position": {
-      "x": 1.3732635157578024,
-      "y": 5.564955745296107
+      "x": 1.37,
+      "y": 5.56
     },
     "rotation": 0.0
   },
@@ -11,6 +11,12 @@
     "type": "sequential",
     "data": {
       "commands": [
+        {
+          "type": "named",
+          "data": {
+            "name": "Shoot"
+          }
+        },
         {
           "type": "path",
           "data": {
@@ -22,6 +28,18 @@
           "data": {
             "name": "IntakeNote"
           }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "BackToStart"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Shoot"
+          }
         }
       ]
     }
diff --git a/src/main/deploy/pathplanner/paths/BackToStart.path b/src/main/deploy/pathplanner/paths/BackToStart.path
new file mode 100644
index 00000000..fe5096a5
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/BackToStart.path
@@ -0,0 +1,52 @@
+{
+  "version": 1.0,
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 2.9066934025451485,
+        "y": 5.574358195791997
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.2727390112774013,
+        "y": 5.574358195791997
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 1.36086297850199,
+        "y": 5.574358195791997
+      },
+      "prevControl": {
+        "x": 0.622987338824146,
+        "y": 5.574358195791997
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 3.094058058917208,
+    "rotateFast": false
+  },
+  "reversed": false,
+  "folder": null,
+  "previewStartingState": {
+    "rotation": 0.0,
+    "velocity": 0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/FirstNote.path b/src/main/deploy/pathplanner/paths/FirstNote.path
index 40a2e58a..226d93ee 100644
--- a/src/main/deploy/pathplanner/paths/FirstNote.path
+++ b/src/main/deploy/pathplanner/paths/FirstNote.path
@@ -16,11 +16,11 @@
     },
     {
       "anchor": {
-        "x": 2.918404239090931,
+        "x": 2.91,
         "y": 5.56
       },
       "prevControl": {
-        "x": 1.9184042390909308,
+        "x": 1.9100000000000001,
         "y": 5.56
       },
       "nextControl": null,
@@ -39,7 +39,7 @@
   },
   "goalEndState": {
     "velocity": 0,
-    "rotation": 40.88347073551137,
+    "rotation": 0.31590706194753587,
     "rotateFast": false
   },
   "reversed": false,
diff --git a/src/main/include/ShooterBehaviour.h b/src/main/include/ShooterBehaviour.h
index d5ace50c..6aa71f87 100644
--- a/src/main/include/ShooterBehaviour.h
+++ b/src/main/include/ShooterBehaviour.h
@@ -8,9 +8,12 @@
 
 #include <memory>
 
+#include "Intake.h"
 #include "LED.h"
 #include "Shooter.h"
 #include "Wombat.h"
+#include "frc/Timer.h"
+#include <units/angular_velocity.h>
 
 class ShooterManualControl : public behaviour::Behaviour {
  public:
@@ -28,3 +31,17 @@ class ShooterManualControl : public behaviour::Behaviour {
   std::shared_ptr<nt::NetworkTable> table =
       nt::NetworkTableInstance::GetDefault().GetTable("Shooter Behaviour");
 };
+
+class AutoShooter : public behaviour::Behaviour {
+ public:
+  AutoShooter(Shooter* shooter, Intake* intake, units::radians_per_second_t goal);
+
+  void OnTick(units::second_t dt) override;
+
+ private:
+  Shooter* _shooter;
+  Intake* _intake;
+  units::radians_per_second_t _goal;
+
+  frc::Timer _timer;
+};
diff --git a/wombat/src/main/cpp/drivetrain/SwerveDrive.cpp b/wombat/src/main/cpp/drivetrain/SwerveDrive.cpp
index e2622bde..11961907 100644
--- a/wombat/src/main/cpp/drivetrain/SwerveDrive.cpp
+++ b/wombat/src/main/cpp/drivetrain/SwerveDrive.cpp
@@ -388,7 +388,7 @@ void SwerveModule::ModuleVectorHandler(frc::ChassisSpeeds speeds) {
 
   units::meters_per_second_t velocity =
       1_mps * std::sqrt(std::pow(xVelocityComponent.value(), 2) + std::pow(yVelocityComponent.value(), 2));
-  units::degree_t angle = 1_rad * std::atan2(yVelocityComponent.value(), xVelocityComponent.value());
+  units::radian_t angle = 1_rad * std::atan2(yVelocityComponent.value(), xVelocityComponent.value());
 
   _anglePIDController.SetSetpoint(angle.value());
   _velocityPIDController.SetSetpoint(velocity.value());
@@ -428,9 +428,10 @@ SwerveDrive::SwerveDrive(SwerveDriveConfig config, frc::Pose2d initialPose)
                                                    frc::SwerveModulePosition{0_m, frc::Rotation2d{0_deg}},
                                                    frc::SwerveModulePosition{0_m, frc::Rotation2d{0_deg}}},
           initialPose, _config.stateStdDevs, _config.visionMeasurementStdDevs),
-      _anglePIDController{PIDController(0.7, 0, 0)},
-      _xPIDController(PIDController(0, 0, 0)),
-      _yPIDController(PIDController(0, 0, 0)),
+      _anglePIDController{PIDController(0, 0, 0)},
+      _xPIDController(PIDController(4, 0.1, 0)),
+      _yPIDController(PIDController(4, 0.1, 0)),
+      _turnPIDController(PIDController(1, 0, 0)),
       // _xPIDController(std::string path, config_t initialGains)
       // _xPIDController(config.path + "/pid/x", _config.posePositionPID),
       // _yPIDController(config.path + "/pid/y", _config.posePositionPID),
@@ -468,13 +469,52 @@ void SwerveDrive::OnUpdate(units::second_t dt) {
         mod->SetIdle();
       }
       break;
+
+    case SwerveDriveState::kAngle: {
+      _turnPIDController.SetSetpoint(_reqAngle.value());
+
+      double direction = _turnPIDController.Calculate(_config.gyro->GetRotation2d().Radians().value()); 
+
+      for (size_t i = 0; i < _modules.size(); i++) {
+        switch (i) {
+          case 0:
+          {
+            _modules[i].SetPID(135_deg, units::meters_per_second_t{-direction}, dt);
+          }
+          break;
+
+          case 1:
+          {
+            _modules[i].SetPID(45_deg, units::meters_per_second_t{-direction}, dt);
+          }
+          break;
+
+          case 2:
+          {
+            _modules[i].SetPID(135_deg, units::meters_per_second_t{direction}, dt);
+          }
+          break;
+
+          case 3:
+          {
+            _modules[i].SetPID(45_deg, units::meters_per_second_t{direction}, dt);
+          }
+          break;
+
+        }
+      }
+
+      _table->GetEntry("direction").SetDouble(direction);
+      _table->GetEntry("_reqAngle").SetDouble(_reqAngle.value());
+    }
+    break;
     case SwerveDriveState::kPose: {
       // _target_fr_speeds.vx = _xPIDController.Calculate(GetPose().X(), dt);
       // _target_fr_speeds.vy = _yPIDController.Calculate(GetPose().Y(), dt);
       _target_fr_speeds.vx = units::meters_per_second_t{_xPIDController.Calculate(GetPose().X().value())};
       _target_fr_speeds.vy = units::meters_per_second_t{_yPIDController.Calculate(GetPose().Y().value())};
-      _target_fr_speeds.omega = /*0_rad_per_s;*/
-          units::radians_per_second_t{_anglePIDController.Calculate(GetPose().Rotation().Radians().value())};
+      _target_fr_speeds.omega = 0_rad_per_s;
+          // units::radians_per_second_t{_anglePIDController.Calculate(GetPose().Rotation().Radians().value() - (3.14159))};
 
       _table->GetEntry("Swerve vx").SetDouble(_target_fr_speeds.vx.value());
       _table->GetEntry("Swerve vy").SetDouble(_target_fr_speeds.vy.value());
@@ -509,17 +549,15 @@ void SwerveDrive::OnUpdate(units::second_t dt) {
         _angle = _target_speed.omega * 1_s;
       }
 
-      bool init = false;
-
       frc::ChassisSpeeds new_target_speed{_target_speed.vx, _target_speed.vy, -_target_speed.omega};
       auto target_states = _kinematics.ToSwerveModuleStates(_target_speed);
       auto new_target_states = _kinematics.ToSwerveModuleStates(new_target_speed);
       for (size_t i = 0; i < _modules.size(); i++) {
-        if (i == 0 || i == 2 || i == 1) {
+         if (i == 0 || i == 2 || i == 1) {
           _modules[i].SetPID(new_target_states[i].angle.Radians(), new_target_states[i].speed, dt);
         } else {
           _modules[i].SetPID(target_states[i].angle.Radians(), target_states[i].speed, dt);
-        }
+        }         
       }
     } break;
     case SwerveDriveState::kIndividualTuning:
@@ -558,7 +596,7 @@ void SwerveDrive::OnUpdate(units::second_t dt) {
   }
 
   _poseEstimator.Update(
-      _config.gyro->GetRotation2d(),
+      _config.gyro->GetRotation2d() - 1_rad,
       wpi::array<frc::SwerveModulePosition, 4>{_modules[3].GetPosition(), _modules[0].GetPosition(),
                                                _modules[1].GetPosition(), _modules[2].GetPosition()});
 
@@ -649,7 +687,7 @@ void SwerveDrive::SetFieldRelativeVelocity(FieldRelativeSpeeds speeds) {
 
 void SwerveDrive::SetPose(frc::Pose2d pose) {
   _state = SwerveDriveState::kPose;
-  _anglePIDController.SetSetpoint(pose.Rotation().Radians().value());
+  _anglePIDController.SetSetpoint(pose.Rotation().Radians().value() - 3.14159);
   _xPIDController.SetSetpoint(pose.X().value());
   _yPIDController.SetSetpoint(pose.Y().value());
 
@@ -657,17 +695,28 @@ void SwerveDrive::SetPose(frc::Pose2d pose) {
 }
 
 bool SwerveDrive::IsAtSetPose() {
-  if (std::abs(_xPIDController.GetPositionError()) < 0.1 && std::abs(_yPIDController.GetPositionError()) < 0.1 && std::abs(_anglePIDController.GetPositionError()) < 0.1) {
-    if (std::abs(_xPIDController.GetVelocityError()) < 1 && std::abs(_yPIDController.GetVelocityError()) < 1 && std::abs(_anglePIDController.GetVelocityError()) < 1) {
-      return true;
+  if (std::abs(_xPIDController.GetPositionError()) < 0.1 && std::abs(_yPIDController.GetPositionError()) < 0.1 && std::abs(_turnPIDController.GetPositionError()) < 0.1) {
+    if (std::abs(_xPIDController.GetVelocityError()) < 1 && std::abs(_yPIDController.GetVelocityError()) < 1 && std::abs(_turnPIDController.GetVelocityError()) < 1) {
+      return false;
     }
   }
   return false;
 }
 
+bool SwerveDrive::IsAtSetAngle() {
+  if (std::abs(_turnPIDController.GetPositionError()) < 0.1 && std::abs(_turnPIDController.GetVelocityError()) < 0.1) {
+    return true;
+  }
+  return false;
+}
+
+SwerveDriveState SwerveDrive::GetState() {
+  return _state;
+}
+
 void SwerveDrive::ResetPose(frc::Pose2d pose) {
   _poseEstimator.ResetPosition(
-      _config.gyro->GetRotation2d(),
+      _config.gyro->GetRotation2d() - 1_rad,
       wpi::array<frc::SwerveModulePosition, 4>{_modules[0].GetPosition(), _modules[1].GetPosition(),
                                                _modules[2].GetPosition(), _modules[3].GetPosition()},
       pose);
@@ -691,5 +740,11 @@ frc::Pose2d SwerveDrive::GetSetpoint() {
 void SwerveDrive::MakeAtSetPoint() {
   ResetPose(GetSetpoint());
 }
+
+void SwerveDrive::TurnToAngle(units::radian_t angle) {
+  _reqAngle = angle;
+  _state = SwerveDriveState::kAngle;
+  std::cout << "awiodjauwohdjiowajdoiwjoaidjoiwJ" << std::endl;
+}
 }  // namespace drivetrain
 }  // namespace wom
diff --git a/wombat/src/main/cpp/drivetrain/behaviours/SwerveBehaviours.cpp b/wombat/src/main/cpp/drivetrain/behaviours/SwerveBehaviours.cpp
index 6dd29443..42e058d3 100644
--- a/wombat/src/main/cpp/drivetrain/behaviours/SwerveBehaviours.cpp
+++ b/wombat/src/main/cpp/drivetrain/behaviours/SwerveBehaviours.cpp
@@ -12,6 +12,9 @@
 #include <units/charge.h>
 #include <units/moment_of_inertia.h>
 
+#include "drivetrain/SwerveDrive.h"
+#include "networktables/NetworkTableInstance.h"
+#include "units/angle.h"
 #include "utils/Pathplanner.h"
 #include "utils/Util.h"
 
@@ -236,24 +239,39 @@ wom::drivetrain::behaviours::DrivebasePoseBehaviour::DrivebasePoseBehaviour(Swer
   Controls(swerveDrivebase);
 }
 
-// used in autonomous for going to set drive poses
-void wom::drivetrain::behaviours::DrivebasePoseBehaviour::OnTick(units::second_t deltaTime) {
-  if (_voltageLimit >= (frc::RobotController::GetBatteryVoltage() - 0.5_V)) {
+void wom::drivetrain::behaviours::DrivebasePoseBehaviour::OnStart() {
+    if (_voltageLimit >= (frc::RobotController::GetBatteryVoltage() - 0.5_V)) {
     _voltageLimit = frc::RobotController::GetBatteryVoltage() - 1_V;
   }
 
-  double currentAngle = _swerveDrivebase->GetPose().Rotation().Degrees().value();
+  double currentAngle = _swerveDrivebase->GetPose().Rotation().Radians().value();
 
-  units::degree_t adjustedAngle =
-      1_deg * (currentAngle - std::fmod(currentAngle, 360) + _pose.Rotation().Degrees().value());
+  units::radian_t adjustedAngle =
+      1_rad * (currentAngle - std::fmod(currentAngle, 360) + _pose.Rotation().Radians().value() - 3.14159);
 
   _swerveDrivebase->SetVoltageLimit(_voltageLimit);
 
-  _swerveDrivebase->SetPose(frc::Pose2d{_pose.X(), _pose.Y(), adjustedAngle});
+  // _swerveDrivebase->SetPose(frc::Pose2d{_pose.X(), _pose.Y(), adjustedAngle});
+  _swerveDrivebase->TurnToAngle(adjustedAngle);
+
+  _timer.Start();
+}
 
-  if (_swerveDrivebase->IsAtSetPose() && !_hold) {
-    std::cout << "Exited..." << std::endl;
 
+// used in autonomous for going to set drive poses
+void wom::drivetrain::behaviours::DrivebasePoseBehaviour::OnTick(units::second_t deltaTime) {
+        nt::NetworkTableInstance::GetDefault().GetTable("drivetrainpose")->GetEntry("going").SetBoolean(true);
+  if (_timer.Get() > 1_s) {
+            nt::NetworkTableInstance::GetDefault().GetTable("drivetrainpose")->GetEntry("going").SetBoolean(false);
     SetDone();
   }
+  if (_swerveDrivebase->IsAtSetAngle() && _swerveDrivebase->GetState() == SwerveDriveState::kAngle) {
+    _swerveDrivebase->SetPose(frc::Pose2d(_pose.X(), _pose.Y(), 0_deg));
+  } else {
+    if (_swerveDrivebase->IsAtSetPose() && !_hold) {
+      std::cout << "Exited..." << std::endl;
+        nt::NetworkTableInstance::GetDefault().GetTable("drivetrainpose")->GetEntry("going").SetBoolean(false);
+      SetDone();
+    }
+  }
 }
diff --git a/wombat/src/main/cpp/utils/Encoder.cpp b/wombat/src/main/cpp/utils/Encoder.cpp
index be0f576b..061ed30a 100644
--- a/wombat/src/main/cpp/utils/Encoder.cpp
+++ b/wombat/src/main/cpp/utils/Encoder.cpp
@@ -5,6 +5,7 @@
 #include "utils/Encoder.h"
 
 #include <rev/SparkRelativeEncoder.h>
+#include <cmath>
 
 wom::utils::Encoder::Encoder(double encoderTicksPerRotation, int type, units::meter_t wheelRadius,
                              double reduction)
diff --git a/wombat/src/main/cpp/utils/Pathplanner.cpp b/wombat/src/main/cpp/utils/Pathplanner.cpp
index 4fcd6907..835fcb8e 100644
--- a/wombat/src/main/cpp/utils/Pathplanner.cpp
+++ b/wombat/src/main/cpp/utils/Pathplanner.cpp
@@ -262,6 +262,16 @@ utils::FollowPath::FollowPath(drivetrain::SwerveDrive* swerve, std::string path,
   pathplanner::RotationTarget* lastRot = nullptr;
   units::degree_t rot;
 
+  int amt = 2;
+  size_t tot = points.size();
+  int index = std::round((static_cast<double>(amt) / static_cast<int>(tot)) * 100);
+  // double index = (amt / tot) * 100;
+  int i = 0;
+  bool f = true;
+
+  nt::NetworkTableInstance::GetDefault().GetTable("pathplanner")->GetEntry("index").SetDouble(index);
+  nt::NetworkTableInstance::GetDefault().GetTable("pathplanner")->GetEntry("total").SetInteger(tot);
+
   for (const pathplanner::PathPoint& point : points) {
     if (lastRot != nullptr) {
       rot = point.rotationTarget.value_or(*lastRot).getTarget().Degrees();
@@ -273,7 +283,16 @@ utils::FollowPath::FollowPath(drivetrain::SwerveDrive* swerve, std::string path,
 
     lastRot = &t;
 
-    _poses.emplace_back(frc::Pose2d(point.position, rot).TransformBy(frc::Transform2d(-1.37_m, -5.56_m, 0_deg)));
+    frc::Translation2d tr = frc::Translation2d(point.position.X() * -1, point.position.Y() * -1);
+    frc::Pose2d pose2 = frc::Pose2d(tr, rot);//.TransformBy(frc::Transform2d(-1.37_m, -5.56_m, 0_deg));
+
+    if (i == index || i == tot || f) {
+      _poses.emplace_back(frc::Pose2d(pose2.Y() - 5.56_m, pose2.X() - 2.91_m, pose2.Rotation()));
+      i = 0;
+      f = false;
+    }
+
+    i++;
   }
   // _poses = pathplanner::PathPlannerPath::fromPathFile(path)->getPathPoses();
   // wpi::json j = wpi::json::parse(cjson);
@@ -310,7 +329,7 @@ utils::FollowPath::FollowPath(drivetrain::SwerveDrive* swerve, std::string path,
   //   i++;
   // }
 
-  int i = 0;
+  i = 0;
   for (const frc::Pose2d pose : _poses) {
     WritePose2NT(nt::NetworkTableInstance::GetDefault().GetTable("pathplanner/poses/" + std::to_string(i)),
                  pose);
@@ -320,6 +339,8 @@ utils::FollowPath::FollowPath(drivetrain::SwerveDrive* swerve, std::string path,
   CalcTimer();
 
   _timer.Start();
+
+  // _swerve->TurnToAngle(_poses[_currentPose].Rotation().Radians());
 }
 
 void utils::FollowPath::CalcTimer() {
@@ -333,13 +354,12 @@ void utils::FollowPath::CalcTimer() {
 
   _timer.Stop();
   _timer.Reset();
-  _time = units::second_t{std::abs(dist.value()) * 2 /*meters per second*/};
+  _time = units::second_t{std::abs(dist.value()) * 1.2 /*meters per second*/};
   // _time = 20_s;
   _timer.Start();
 }
 
 void utils::FollowPath::OnTick(units::second_t dt) {
-  _swerve->SetPose(_poses[_currentPose]);
   
   nt::NetworkTableInstance::GetDefault()
       .GetTable("pathplanner")
@@ -359,8 +379,11 @@ void utils::FollowPath::OnTick(units::second_t dt) {
                _swerve->GetPose());
 
   std::cout << "Following Path" << std::endl;
-
-  if (_swerve->IsAtSetPose() || _timer.Get() > _time) {
+  
+  // if (_swerve->IsAtSetAngle() && _swerve->GetState() == drivetrain::SwerveDriveState::kAngle) {
+    _swerve->SetPose(_poses[_currentPose]);
+  // }
+  /*else*/ if (_swerve->IsAtSetPose() || _timer.Get() > _time) {
     if (_currentPose + 1 == static_cast<int>(_poses.size())) {
       // _swerve->MakeAtSetPoint();
       _swerve->SetVelocity(frc::ChassisSpeeds());
@@ -368,6 +391,7 @@ void utils::FollowPath::OnTick(units::second_t dt) {
       SetDone();
     } else {
       _currentPose++;
+      // _swerve->TurnToAngle(_poses[_currentPose].Rotation().Radians());
       CalcTimer();
     }
   }
@@ -424,37 +448,57 @@ void utils::AutoBuilder::SetAuto(std::string path) {
     if (c["type"] == "named") {
       commands.push_back(std::make_pair(c["type"], c["data"]["name"]));
     }
+    // if (c["type"] == "parallel") {
+      // commands.push_back(std::make_pair(c["type"], ""));
+    // }
   }
 
   nt::NetworkTableInstance::GetDefault().GetTable("commands")->GetEntry("length").SetInteger(commands.size());
 
-  pathplan = behaviour::make<drivetrain::behaviours::DrivebasePoseBehaviour>(
-      _swerve, JSONPoseToPose2d(*_startingPose).TransformBy(frc::Transform2d(-1.37_m, -5.56_m, 0_deg)));
+  pathplan = behaviour::make<behaviour::SequentialBehaviour>();
 
   int i = 0;
   int pathamt = 0;
   int commandamt = 0;
 
-  for (std::pair<std::string, std::string> command : commands) {
+  for (auto command : *_commands) {
     nt::NetworkTableInstance::GetDefault()
         .GetTable("commands/" + std::to_string(i))
         ->GetEntry("type")
-        .SetString(static_cast<std::string>(command.first));
+        .SetString(static_cast<std::string>(command["type"]));
     nt::NetworkTableInstance::GetDefault()
         .GetTable("commands/" + std::to_string(i))
         ->GetEntry("data")
-        .SetString(static_cast<std::string>(command.second));
+        .SetString(static_cast<std::string>(command["data"].dump()));
 
-    if (command.first == "path") {
-      auto b = behaviour::make<FollowPath>(_swerve, command.second, _flip);
+    if (command["type"] == "path") {
+      auto b = behaviour::make<FollowPath>(_swerve, command["data"]["pathName"], _flip);
       std::cout << b->GetName() << std::endl;
       pathplan = pathplan << b;
       pathamt++;
-    } else /*(command.first == "command")*/ {
-      pathplan = pathplan << _commandsList.Run(command.second);
+    } else if (command["type"] == "named") {
+      pathplan = pathplan << _commandsList.Run(command["data"]["name"]);
       commandamt++;
+    } else if (command["type"] == "parallel") {
+      
+      std::shared_ptr<behaviour::Behaviour> nb = behaviour::make<behaviour::ConcurrentBehaviour>(behaviour::ConcurrentBehaviourReducer::ALL);
+      int j = 0;
+      for (auto c : command["data"]["commands"]) { 
+        nt::NetworkTableInstance::GetDefault().GetTable("commands/parallel/" + std::to_string(j))->GetEntry("type").SetString(static_cast<std::string>(c["type"]));
+
+        nt::NetworkTableInstance::GetDefault().GetTable("commands/parallel/" + std::to_string(j))->GetEntry("typeisstring").SetBoolean(c["type"].is_string());
+        if (static_cast<std::string>(c["type"]) == "path") {
+          nb = nb &  behaviour::make<FollowPath>(_swerve, c["data"]["pathName"], _flip);
+          pathamt++;
+        } else if (static_cast<std::string>(c["type"]) == "named") {
+          nb = nb & _commandsList.Run(c["data"]["name"]);
+          commandamt++;
+        }
+        j++;
+      }
+      nt::NetworkTableInstance::GetDefault().GetTable("commands")->GetEntry("parallelcommandsamt").SetInteger(j);
+      pathplan = pathplan << nb;
     }
-
     i++;
   }
 
diff --git a/wombat/src/main/include/drivetrain/SwerveDrive.h b/wombat/src/main/include/drivetrain/SwerveDrive.h
index 852aaf5b..6167fe58 100644
--- a/wombat/src/main/include/drivetrain/SwerveDrive.h
+++ b/wombat/src/main/include/drivetrain/SwerveDrive.h
@@ -28,6 +28,7 @@
 #include <ctre/phoenix6/Pigeon2.hpp>
 
 #include "behaviour/HasBehaviour.h"
+#include "units/angle.h"
 #include "utils/Gearbox.h"
 #include "utils/PID.h"
 
@@ -403,6 +404,7 @@ struct SwerveDriveConfig {
 enum class SwerveDriveState {
   kZeroing,
   kIdle,
+  kAngle,
   kVelocity,
   kFieldRelativeVelocity,
   kPose,
@@ -458,6 +460,10 @@ class SwerveDrive : public behaviour::HasBehaviour {
   void SetZero();
   void SetVoltageLimit(units::volt_t driveVoltageLimit);
   void OnResetMode();
+
+  SwerveDriveState GetState();
+  bool IsAtSetAngle();
+
   // double GetModuleCANPosition(int mod);  // from liam's
 
   void SetXWheelState();
@@ -472,6 +478,8 @@ class SwerveDrive : public behaviour::HasBehaviour {
   frc::Pose2d GetPose();
   void AddVisionMeasurement(frc::Pose2d pose, units::second_t timestamp);
 
+  void TurnToAngle(units::radian_t angle);
+
   SwerveDriveConfig& GetConfig() { return _config; }
 
   frc::Pose2d GetSetpoint();
@@ -506,6 +514,7 @@ class SwerveDrive : public behaviour::HasBehaviour {
   wom::drivetrain::PIDController _anglePIDController;
   wom::drivetrain::PIDController _xPIDController;
   wom::drivetrain::PIDController _yPIDController;
+  wom::drivetrain::PIDController _turnPIDController;
   // wom::utils::PIDController<units::meter, units::meters_per_second> _xPIDController;
   // wom::utils::PIDController<units::meter, units::meters_per_second> _yPIDController;
 
@@ -517,7 +526,10 @@ class SwerveDrive : public behaviour::HasBehaviour {
   int _mod;
   units::radian_t _angle;
   units::meters_per_second_t _speed;
+  frc::SwerveModuleState laststates[4];
+  frc::SwerveModuleState lastnewstates[4];
 
+  units::radian_t _reqAngle;
   // double frontLeftEncoderOffset = -143.26171875;
   // double frontRightEncoderOffset = 167.87109375;
   // double backLeftEncoderOffset = -316.669921875;
diff --git a/wombat/src/main/include/drivetrain/behaviours/SwerveBehaviours.h b/wombat/src/main/include/drivetrain/behaviours/SwerveBehaviours.h
index b1b1f71f..7db5560c 100644
--- a/wombat/src/main/include/drivetrain/behaviours/SwerveBehaviours.h
+++ b/wombat/src/main/include/drivetrain/behaviours/SwerveBehaviours.h
@@ -222,6 +222,7 @@ class DrivebasePoseBehaviour : public behaviour::Behaviour {
    * @param deltaTime change in time since the last iteration
    */
   void OnTick(units::second_t deltaTime) override;
+  void OnStart() override;
 
  private:
   SwerveDrive* _swerveDrivebase;
@@ -229,6 +230,8 @@ class DrivebasePoseBehaviour : public behaviour::Behaviour {
   bool _hold;
   units::volt_t _voltageLimit;
 
+  frc::Timer _timer;
+
   std::shared_ptr<nt::NetworkTable> _swerveDriveTable =
       nt::NetworkTableInstance::GetDefault().GetTable("swerve");
 };

From 7f0027b15053ccdff787e90f256549acf313bc64 Mon Sep 17 00:00:00 2001
From: Isaac Turner <spacey_sooty@outlook.com>
Date: Tue, 5 Mar 2024 20:20:13 +0800
Subject: [PATCH 192/207] add some paths

---
 .../deploy/pathplanner/autos/FourNote.auto    |  97 +++++++++++++++++
 .../deploy/pathplanner/autos/FourNoteFar.auto |  91 ++++++++++++++++
 .../pathplanner/autos/FourNoteTaxi.auto       | 103 ++++++++++++++++++
 .../deploy/pathplanner/autos/LeftOneNote.auto |  43 ++++++++
 .../deploy/pathplanner/autos/OneNote.auto     |  25 +++++
 .../deploy/pathplanner/autos/OneNoteTaxi.auto |  31 ++++++
 .../deploy/pathplanner/autos/ThreeNote.auto   |  73 +++++++++++++
 .../pathplanner/autos/ThreeNoteFar.auto       |  67 ++++++++++++
 .../pathplanner/autos/ThreeNoteTaxi.auto      |  73 +++++++++++++
 .../autos/{Taxi.auto => TwoNote.auto}         |   8 +-
 .../deploy/pathplanner/autos/TwoNoteTaxi.auto |  55 ++++++++++
 ...ackToStart.path => CenterBackToStart.path} |   0
 .../deploy/pathplanner/paths/CloseLeft.path   |  52 +++++++++
 .../pathplanner/paths/CloseLeftReturn.path    |  52 +++++++++
 .../deploy/pathplanner/paths/CloseRight.path  |  52 +++++++++
 .../pathplanner/paths/CloseRightReturn.path   |  52 +++++++++
 .../deploy/pathplanner/paths/FarLeft.path     |  52 +++++++++
 .../pathplanner/paths/FarLeftCenter.path      |  52 +++++++++
 .../paths/FarLeftCenterReturn.path            |  52 +++++++++
 .../pathplanner/paths/FarLeftReturn.path      |  52 +++++++++
 .../pathplanner/paths/LeftCloseLeft.path      |  52 +++++++++
 21 files changed, 1130 insertions(+), 4 deletions(-)
 create mode 100644 src/main/deploy/pathplanner/autos/FourNote.auto
 create mode 100644 src/main/deploy/pathplanner/autos/FourNoteFar.auto
 create mode 100644 src/main/deploy/pathplanner/autos/FourNoteTaxi.auto
 create mode 100644 src/main/deploy/pathplanner/autos/LeftOneNote.auto
 create mode 100644 src/main/deploy/pathplanner/autos/OneNote.auto
 create mode 100644 src/main/deploy/pathplanner/autos/OneNoteTaxi.auto
 create mode 100644 src/main/deploy/pathplanner/autos/ThreeNote.auto
 create mode 100644 src/main/deploy/pathplanner/autos/ThreeNoteFar.auto
 create mode 100644 src/main/deploy/pathplanner/autos/ThreeNoteTaxi.auto
 rename src/main/deploy/pathplanner/autos/{Taxi.auto => TwoNote.auto} (85%)
 create mode 100644 src/main/deploy/pathplanner/autos/TwoNoteTaxi.auto
 rename src/main/deploy/pathplanner/paths/{BackToStart.path => CenterBackToStart.path} (100%)
 create mode 100644 src/main/deploy/pathplanner/paths/CloseLeft.path
 create mode 100644 src/main/deploy/pathplanner/paths/CloseLeftReturn.path
 create mode 100644 src/main/deploy/pathplanner/paths/CloseRight.path
 create mode 100644 src/main/deploy/pathplanner/paths/CloseRightReturn.path
 create mode 100644 src/main/deploy/pathplanner/paths/FarLeft.path
 create mode 100644 src/main/deploy/pathplanner/paths/FarLeftCenter.path
 create mode 100644 src/main/deploy/pathplanner/paths/FarLeftCenterReturn.path
 create mode 100644 src/main/deploy/pathplanner/paths/FarLeftReturn.path
 create mode 100644 src/main/deploy/pathplanner/paths/LeftCloseLeft.path

diff --git a/src/main/deploy/pathplanner/autos/FourNote.auto b/src/main/deploy/pathplanner/autos/FourNote.auto
new file mode 100644
index 00000000..b452b4d8
--- /dev/null
+++ b/src/main/deploy/pathplanner/autos/FourNote.auto
@@ -0,0 +1,97 @@
+{
+  "version": 1.0,
+  "startingPose": {
+    "position": {
+      "x": 0.9789443299448194,
+      "y": 5.556063249807658
+    },
+    "rotation": 0
+  },
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "named",
+          "data": {
+            "name": "Shoot"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "FirstNote"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "IntakeNote"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "CenterBackToStart"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Shoot"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "CloseLeft"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "IntakeNote"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "CloseLeftReturn"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Shoot"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "CloseRight"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "IntakeNote"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "CloseRightReturn"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Shoot"
+          }
+        }
+      ]
+    }
+  },
+  "folder": null,
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/autos/FourNoteFar.auto b/src/main/deploy/pathplanner/autos/FourNoteFar.auto
new file mode 100644
index 00000000..7a3134a1
--- /dev/null
+++ b/src/main/deploy/pathplanner/autos/FourNoteFar.auto
@@ -0,0 +1,91 @@
+{
+  "version": 1.0,
+  "startingPose": {
+    "position": {
+      "x": 0.7850516674765234,
+      "y": 6.3027226484706595
+    },
+    "rotation": 0
+  },
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "named",
+          "data": {
+            "name": "Shoot"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "LeftCloseLeft"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "IntakeNote"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Shoot"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "FarLeft"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "IntakeNote"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "FarLeftReturn"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Shoot"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "FarLeftCenter"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "IntakeNote"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "FarLeftCenterReturn"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Shoot"
+          }
+        }
+      ]
+    }
+  },
+  "folder": null,
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/autos/FourNoteTaxi.auto b/src/main/deploy/pathplanner/autos/FourNoteTaxi.auto
new file mode 100644
index 00000000..2cd1b37a
--- /dev/null
+++ b/src/main/deploy/pathplanner/autos/FourNoteTaxi.auto
@@ -0,0 +1,103 @@
+{
+  "version": 1.0,
+  "startingPose": {
+    "position": {
+      "x": 0.9789443299448194,
+      "y": 5.556063249807658
+    },
+    "rotation": 0
+  },
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "named",
+          "data": {
+            "name": "Shoot"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "FirstNote"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "IntakeNote"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "CenterBackToStart"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Shoot"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "CloseLeft"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "IntakeNote"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "CloseLeftReturn"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Shoot"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "CloseRight"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "IntakeNote"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "CloseRightReturn"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Shoot"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "FirstNote"
+          }
+        }
+      ]
+    }
+  },
+  "folder": null,
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/autos/LeftOneNote.auto b/src/main/deploy/pathplanner/autos/LeftOneNote.auto
new file mode 100644
index 00000000..3757a645
--- /dev/null
+++ b/src/main/deploy/pathplanner/autos/LeftOneNote.auto
@@ -0,0 +1,43 @@
+{
+  "version": 1.0,
+  "startingPose": {
+    "position": {
+      "x": 0.7850516674765234,
+      "y": 6.3027226484706595
+    },
+    "rotation": 0
+  },
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "named",
+          "data": {
+            "name": "Shoot"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "LeftCloseLeft"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "IntakeNote"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Shoot"
+          }
+        }
+      ]
+    }
+  },
+  "folder": null,
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/autos/OneNote.auto b/src/main/deploy/pathplanner/autos/OneNote.auto
new file mode 100644
index 00000000..50f060f1
--- /dev/null
+++ b/src/main/deploy/pathplanner/autos/OneNote.auto
@@ -0,0 +1,25 @@
+{
+  "version": 1.0,
+  "startingPose": {
+    "position": {
+      "x": 1.37,
+      "y": 5.56
+    },
+    "rotation": 0.0
+  },
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "named",
+          "data": {
+            "name": "Shoot"
+          }
+        }
+      ]
+    }
+  },
+  "folder": null,
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/autos/OneNoteTaxi.auto b/src/main/deploy/pathplanner/autos/OneNoteTaxi.auto
new file mode 100644
index 00000000..f79c865a
--- /dev/null
+++ b/src/main/deploy/pathplanner/autos/OneNoteTaxi.auto
@@ -0,0 +1,31 @@
+{
+  "version": 1.0,
+  "startingPose": {
+    "position": {
+      "x": 0.9455266691330485,
+      "y": 5.5233478865961345
+    },
+    "rotation": 0
+  },
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "named",
+          "data": {
+            "name": "Shoot"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "FirstNote"
+          }
+        }
+      ]
+    }
+  },
+  "folder": null,
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/autos/ThreeNote.auto b/src/main/deploy/pathplanner/autos/ThreeNote.auto
new file mode 100644
index 00000000..6159ba17
--- /dev/null
+++ b/src/main/deploy/pathplanner/autos/ThreeNote.auto
@@ -0,0 +1,73 @@
+{
+  "version": 1.0,
+  "startingPose": {
+    "position": {
+      "x": 0.9789443299448194,
+      "y": 5.556063249807658
+    },
+    "rotation": 0
+  },
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "named",
+          "data": {
+            "name": "Shoot"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "FirstNote"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "IntakeNote"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "CenterBackToStart"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Shoot"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "CloseLeft"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "IntakeNote"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "CloseLeftReturn"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Shoot"
+          }
+        }
+      ]
+    }
+  },
+  "folder": null,
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/autos/ThreeNoteFar.auto b/src/main/deploy/pathplanner/autos/ThreeNoteFar.auto
new file mode 100644
index 00000000..6c448793
--- /dev/null
+++ b/src/main/deploy/pathplanner/autos/ThreeNoteFar.auto
@@ -0,0 +1,67 @@
+{
+  "version": 1.0,
+  "startingPose": {
+    "position": {
+      "x": 0.7850516674765234,
+      "y": 6.3027226484706595
+    },
+    "rotation": 0
+  },
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "named",
+          "data": {
+            "name": "Shoot"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "LeftCloseLeft"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "IntakeNote"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Shoot"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "FarLeft"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "IntakeNote"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "FarLeftReturn"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Shoot"
+          }
+        }
+      ]
+    }
+  },
+  "folder": null,
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/autos/ThreeNoteTaxi.auto b/src/main/deploy/pathplanner/autos/ThreeNoteTaxi.auto
new file mode 100644
index 00000000..6159ba17
--- /dev/null
+++ b/src/main/deploy/pathplanner/autos/ThreeNoteTaxi.auto
@@ -0,0 +1,73 @@
+{
+  "version": 1.0,
+  "startingPose": {
+    "position": {
+      "x": 0.9789443299448194,
+      "y": 5.556063249807658
+    },
+    "rotation": 0
+  },
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "named",
+          "data": {
+            "name": "Shoot"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "FirstNote"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "IntakeNote"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "CenterBackToStart"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Shoot"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "CloseLeft"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "IntakeNote"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "CloseLeftReturn"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Shoot"
+          }
+        }
+      ]
+    }
+  },
+  "folder": null,
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/autos/Taxi.auto b/src/main/deploy/pathplanner/autos/TwoNote.auto
similarity index 85%
rename from src/main/deploy/pathplanner/autos/Taxi.auto
rename to src/main/deploy/pathplanner/autos/TwoNote.auto
index faf982ba..a33df241 100644
--- a/src/main/deploy/pathplanner/autos/Taxi.auto
+++ b/src/main/deploy/pathplanner/autos/TwoNote.auto
@@ -2,10 +2,10 @@
   "version": 1.0,
   "startingPose": {
     "position": {
-      "x": 1.37,
-      "y": 5.56
+      "x": 0.9789443299448194,
+      "y": 5.556063249807658
     },
-    "rotation": 0.0
+    "rotation": 0
   },
   "command": {
     "type": "sequential",
@@ -32,7 +32,7 @@
         {
           "type": "path",
           "data": {
-            "pathName": "BackToStart"
+            "pathName": "CenterBackToStart"
           }
         },
         {
diff --git a/src/main/deploy/pathplanner/autos/TwoNoteTaxi.auto b/src/main/deploy/pathplanner/autos/TwoNoteTaxi.auto
new file mode 100644
index 00000000..bdc155b0
--- /dev/null
+++ b/src/main/deploy/pathplanner/autos/TwoNoteTaxi.auto
@@ -0,0 +1,55 @@
+{
+  "version": 1.0,
+  "startingPose": {
+    "position": {
+      "x": 0.9789443299448194,
+      "y": 5.556063249807658
+    },
+    "rotation": 0
+  },
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "named",
+          "data": {
+            "name": "Shoot"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "FirstNote"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "IntakeNote"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "CenterBackToStart"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Shoot"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "FirstNote"
+          }
+        }
+      ]
+    }
+  },
+  "folder": null,
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/BackToStart.path b/src/main/deploy/pathplanner/paths/CenterBackToStart.path
similarity index 100%
rename from src/main/deploy/pathplanner/paths/BackToStart.path
rename to src/main/deploy/pathplanner/paths/CenterBackToStart.path
diff --git a/src/main/deploy/pathplanner/paths/CloseLeft.path b/src/main/deploy/pathplanner/paths/CloseLeft.path
new file mode 100644
index 00000000..2f4ef29a
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/CloseLeft.path
@@ -0,0 +1,52 @@
+{
+  "version": 1.0,
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 0.9778908835444483,
+        "y": 5.52001197299496
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 1.536685674141276,
+        "y": 5.599313077022893
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 2.830961626597167,
+        "y": 7.010989778320136
+      },
+      "prevControl": {
+        "x": 1.8309616265971669,
+        "y": 7.010989778320136
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 0,
+    "rotateFast": false
+  },
+  "reversed": false,
+  "folder": null,
+  "previewStartingState": {
+    "rotation": 0,
+    "velocity": 0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/CloseLeftReturn.path b/src/main/deploy/pathplanner/paths/CloseLeftReturn.path
new file mode 100644
index 00000000..cf7376a2
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/CloseLeftReturn.path
@@ -0,0 +1,52 @@
+{
+  "version": 1.0,
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 2.830961626597167,
+        "y": 7.010989778320136
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.3897564171939947,
+        "y": 7.090290882348069
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 0.8831977571110939,
+        "y": 5.494436635385951
+      },
+      "prevControl": {
+        "x": -0.11680224288890606,
+        "y": 5.494436635385951
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 0,
+    "rotateFast": false
+  },
+  "reversed": false,
+  "folder": null,
+  "previewStartingState": {
+    "rotation": 0,
+    "velocity": 0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/CloseRight.path b/src/main/deploy/pathplanner/paths/CloseRight.path
new file mode 100644
index 00000000..f514f3fe
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/CloseRight.path
@@ -0,0 +1,52 @@
+{
+  "version": 1.0,
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 0.9464630659333784,
+        "y": 5.519836398594897
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 1.9464630659333793,
+        "y": 5.519836398594897
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 2.7818593193798713,
+        "y": 4.096396212493511
+      },
+      "prevControl": {
+        "x": 1.7818593193798713,
+        "y": 4.096396212493511
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 0,
+    "rotateFast": false
+  },
+  "reversed": false,
+  "folder": null,
+  "previewStartingState": {
+    "rotation": 0,
+    "velocity": 0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/CloseRightReturn.path b/src/main/deploy/pathplanner/paths/CloseRightReturn.path
new file mode 100644
index 00000000..28e75ef8
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/CloseRightReturn.path
@@ -0,0 +1,52 @@
+{
+  "version": 1.0,
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 2.7818593193798713,
+        "y": 4.096396212493511
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.7818593193798726,
+        "y": 4.096396212493511
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 0.973560048342923,
+        "y": 5.571630846613142
+      },
+      "prevControl": {
+        "x": -0.02643995165707702,
+        "y": 5.571630846613142
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 0,
+    "rotateFast": false
+  },
+  "reversed": false,
+  "folder": null,
+  "previewStartingState": {
+    "rotation": 0,
+    "velocity": 0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/FarLeft.path b/src/main/deploy/pathplanner/paths/FarLeft.path
new file mode 100644
index 00000000..43d0f42d
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/FarLeft.path
@@ -0,0 +1,52 @@
+{
+  "version": 1.0,
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 2.830961626597167,
+        "y": 7.010989778320136
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.3897564171939933,
+        "y": 7.090290882348069
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 8.215594377293828,
+        "y": 7.475676690483815
+      },
+      "prevControl": {
+        "x": 8.724373097296924,
+        "y": 7.4628597592793
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 0,
+    "rotateFast": false
+  },
+  "reversed": false,
+  "folder": null,
+  "previewStartingState": {
+    "rotation": 0,
+    "velocity": 0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/FarLeftCenter.path b/src/main/deploy/pathplanner/paths/FarLeftCenter.path
new file mode 100644
index 00000000..fcc9eb68
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/FarLeftCenter.path
@@ -0,0 +1,52 @@
+{
+  "version": 1.0,
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 2.830961626597167,
+        "y": 7.010989778320136
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.3897564171939933,
+        "y": 7.090290882348069
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 8.277396566115597,
+        "y": 5.782437176287395
+      },
+      "prevControl": {
+        "x": 8.786175286118693,
+        "y": 5.76962024508288
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 0,
+    "rotateFast": false
+  },
+  "reversed": false,
+  "folder": null,
+  "previewStartingState": {
+    "rotation": 0,
+    "velocity": 0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/FarLeftCenterReturn.path b/src/main/deploy/pathplanner/paths/FarLeftCenterReturn.path
new file mode 100644
index 00000000..32bfd197
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/FarLeftCenterReturn.path
@@ -0,0 +1,52 @@
+{
+  "version": 1.0,
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 8.277396566115597,
+        "y": 5.782437176287395
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 8.836191356712423,
+        "y": 5.861738280315328
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 2.800002007386261,
+        "y": 6.960014677502181
+      },
+      "prevControl": {
+        "x": 3.308780727389358,
+        "y": 6.9471977462976655
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 0,
+    "rotateFast": false
+  },
+  "reversed": false,
+  "folder": null,
+  "previewStartingState": {
+    "rotation": 0,
+    "velocity": 0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/FarLeftReturn.path b/src/main/deploy/pathplanner/paths/FarLeftReturn.path
new file mode 100644
index 00000000..0f73404a
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/FarLeftReturn.path
@@ -0,0 +1,52 @@
+{
+  "version": 1.0,
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 8.215594377293828,
+        "y": 7.475676690483815
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 8.774389167890654,
+        "y": 7.554977794511748
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 2.838452800999805,
+        "y": 7.002562207117168
+      },
+      "prevControl": {
+        "x": 3.3472315210029024,
+        "y": 6.989745275912653
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 0,
+    "rotateFast": false
+  },
+  "reversed": false,
+  "folder": null,
+  "previewStartingState": {
+    "rotation": 0,
+    "velocity": 0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/LeftCloseLeft.path b/src/main/deploy/pathplanner/paths/LeftCloseLeft.path
new file mode 100644
index 00000000..feb95c8f
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/LeftCloseLeft.path
@@ -0,0 +1,52 @@
+{
+  "version": 1.0,
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 0.7850516674765234,
+        "y": 6.3027226484706595
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 1.3438464580733505,
+        "y": 6.3820237524985926
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 2.830961626597167,
+        "y": 7.010989778320136
+      },
+      "prevControl": {
+        "x": 1.8309616265971669,
+        "y": 7.010989778320136
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 0,
+    "rotateFast": false
+  },
+  "reversed": false,
+  "folder": null,
+  "previewStartingState": {
+    "rotation": 0,
+    "velocity": 0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file

From fb921f9181a786495af138b68da4601583266a04 Mon Sep 17 00:00:00 2001
From: Isaac Turner <spacey_sooty@outlook.com>
Date: Tue, 5 Mar 2024 21:02:11 +0800
Subject: [PATCH 193/207] sorry

---
 src/main/cpp/Robot.cpp            | 7 ++++---
 src/main/cpp/ShooterBehaviour.cpp | 1 +
 2 files changed, 5 insertions(+), 3 deletions(-)

diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp
index 8b4962bd..382ed283 100644
--- a/src/main/cpp/Robot.cpp
+++ b/src/main/cpp/Robot.cpp
@@ -133,9 +133,10 @@ void Robot::AutonomousInit() {
 
   robotmap._builder = autos::InitCommands(_swerveDrive, shooter, intake, alphaArm);
 
-  if (m_autoSelected == "kTaxi") {
-    sched->Schedule(autos::Taxi(robotmap._builder));
-  }
+  //if (m_autoSelected == "kTaxi") {
+  //  sched->Schedule(autos::Taxi(robotmap._builder));
+  //}
+  sched->Schedule(robotmap._builder->GetAutoRoutine("OneNoteTaxi"));
 
   _swerveDrive->OnStart();
 }
diff --git a/src/main/cpp/ShooterBehaviour.cpp b/src/main/cpp/ShooterBehaviour.cpp
index b2daeab5..2765be4a 100644
--- a/src/main/cpp/ShooterBehaviour.cpp
+++ b/src/main/cpp/ShooterBehaviour.cpp
@@ -63,6 +63,7 @@ AutoShooter::AutoShooter(Shooter* shooter, Intake* intake, units::radians_per_se
 }
 
 void AutoShooter::OnTick(units::second_t dt) {
+  _goal = 300_rad_per_s;
   _shooter->SetState(ShooterState::kSpinUp);
   _shooter->SetPidGoal(_goal);
 

From 1c6bd2cd3a35c50d49d9fca0bb94153bd17fad2d Mon Sep 17 00:00:00 2001
From: Goanna247 <21496350@student.curtin.edu.au>
Date: Tue, 5 Mar 2024 22:46:44 +0800
Subject: [PATCH 194/207] got some stuff working

---
 .pathplanner/settings.json                    | 12 ++---
 src/main/cpp/Intake.cpp                       |  1 +
 src/main/cpp/IntakeBehaviour.cpp              |  1 +
 src/main/cpp/Robot.cpp                        |  2 +
 .../deploy/pathplanner/autos/OneNoteTaxi.auto | 14 ++----
 .../deploy/pathplanner/paths/Backwards.path   | 49 +++++++++++++++++++
 .../pathplanner/paths/CenterBackToStart.path  |  8 +--
 .../deploy/pathplanner/paths/CloseLeft.path   |  8 +--
 .../pathplanner/paths/CloseLeftReturn.path    |  8 +--
 .../deploy/pathplanner/paths/CloseRight.path  |  8 +--
 .../pathplanner/paths/CloseRightReturn.path   |  8 +--
 .../deploy/pathplanner/paths/FarLeft.path     |  8 +--
 .../pathplanner/paths/FarLeftCenter.path      |  8 +--
 .../paths/FarLeftCenterReturn.path            |  8 +--
 .../pathplanner/paths/FarLeftReturn.path      |  8 +--
 .../deploy/pathplanner/paths/FirstNote.path   | 28 +++++------
 .../pathplanner/paths/LeftCloseLeft.path      |  8 +--
 wombat/src/main/cpp/utils/Pathplanner.cpp     | 29 +++++------
 wombat/src/main/include/utils/Pathplanner.h   |  2 +-
 19 files changed, 133 insertions(+), 85 deletions(-)
 create mode 100644 src/main/deploy/pathplanner/paths/Backwards.path

diff --git a/.pathplanner/settings.json b/.pathplanner/settings.json
index e0540537..fd65c978 100644
--- a/.pathplanner/settings.json
+++ b/.pathplanner/settings.json
@@ -4,9 +4,9 @@
   "holonomicMode": true,
   "pathFolders": [],
   "autoFolders": [],
-  "defaultMaxVel": 3.0,
-  "defaultMaxAccel": 3.0,
-  "defaultMaxAngVel": 540.0,
-  "defaultMaxAngAccel": 720.0,
-  "maxModuleSpeed": 4.5
-}
+  "defaultMaxVel": 1.0,
+  "defaultMaxAccel": 1.5,
+  "defaultMaxAngVel": 100.0,
+  "defaultMaxAngAccel": 150.0,
+  "maxModuleSpeed": 1.0
+}
\ No newline at end of file
diff --git a/src/main/cpp/Intake.cpp b/src/main/cpp/Intake.cpp
index f1848623..e76dbe2b 100644
--- a/src/main/cpp/Intake.cpp
+++ b/src/main/cpp/Intake.cpp
@@ -51,6 +51,7 @@ void Intake::OnUpdate(units::second_t dt) {
 
     case IntakeState::kHold:
     {
+      std::cerr << "HEY FROM kHold" << std::endl;
       units::volt_t pidCalculate = 0_V;
       if (_config.intakeSensor->Get() == true && _config.passSensor->Get() == true) {
         SetState(IntakeState::kIdle);
diff --git a/src/main/cpp/IntakeBehaviour.cpp b/src/main/cpp/IntakeBehaviour.cpp
index cd67d1ac..38fca2d4 100644
--- a/src/main/cpp/IntakeBehaviour.cpp
+++ b/src/main/cpp/IntakeBehaviour.cpp
@@ -76,6 +76,7 @@ void IntakeNote::OnTick(units::second_t dt) {
   _intake->SetState(IntakeState::kIntake);
 
   if (_intake->GetState() == IntakeState::kHold) {
+    std::cerr << "EXITING" << std::endl;
     SetDone();
   }
 }
diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp
index 382ed283..9a76d89b 100644
--- a/src/main/cpp/Robot.cpp
+++ b/src/main/cpp/Robot.cpp
@@ -126,6 +126,8 @@ void Robot::RobotPeriodic() {
 void Robot::AutonomousInit() {
   loop.Clear();
   sched->InterruptAll();
+  _swerveDrive->MakeAtSetPoint();
+  // _swerveDrive->SetPose(frc::Pose2d());
 
   _swerveDrive->GetConfig().gyro->Reset();
 
diff --git a/src/main/deploy/pathplanner/autos/OneNoteTaxi.auto b/src/main/deploy/pathplanner/autos/OneNoteTaxi.auto
index f79c865a..38dc2cad 100644
--- a/src/main/deploy/pathplanner/autos/OneNoteTaxi.auto
+++ b/src/main/deploy/pathplanner/autos/OneNoteTaxi.auto
@@ -1,26 +1,20 @@
 {
   "version": 1.0,
-  "startingPose": {
-    "position": {
-      "x": 0.9455266691330485,
-      "y": 5.5233478865961345
-    },
-    "rotation": 0
-  },
+  "startingPose": null,
   "command": {
     "type": "sequential",
     "data": {
       "commands": [
         {
-          "type": "named",
+          "type": "path",
           "data": {
-            "name": "Shoot"
+            "pathName": "FirstNote"
           }
         },
         {
           "type": "path",
           "data": {
-            "pathName": "FirstNote"
+            "pathName": "Backwards"
           }
         }
       ]
diff --git a/src/main/deploy/pathplanner/paths/Backwards.path b/src/main/deploy/pathplanner/paths/Backwards.path
new file mode 100644
index 00000000..ac346ca2
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/Backwards.path
@@ -0,0 +1,49 @@
+{
+  "version": 1.0,
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 2.0,
+        "y": 0.0
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 0.6039125090180515,
+        "y": 1.7097140771607025e-16
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 0.0,
+        "y": 0.0
+      },
+      "prevControl": {
+        "x": 1.0,
+        "y": -1.2246467991473532e-16
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 1.0,
+    "maxAcceleration": 1.0,
+    "maxAngularVelocity": 100.0,
+    "maxAngularAcceleration": 150.0
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 0,
+    "rotateFast": false
+  },
+  "reversed": false,
+  "folder": null,
+  "previewStartingState": null,
+  "useDefaultConstraints": false
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/CenterBackToStart.path b/src/main/deploy/pathplanner/paths/CenterBackToStart.path
index fe5096a5..11caa43a 100644
--- a/src/main/deploy/pathplanner/paths/CenterBackToStart.path
+++ b/src/main/deploy/pathplanner/paths/CenterBackToStart.path
@@ -32,10 +32,10 @@
   "constraintZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 3.0,
-    "maxAcceleration": 3.0,
-    "maxAngularVelocity": 540.0,
-    "maxAngularAcceleration": 720.0
+    "maxVelocity": 1.0,
+    "maxAcceleration": 1.5,
+    "maxAngularVelocity": 100.0,
+    "maxAngularAcceleration": 150.0
   },
   "goalEndState": {
     "velocity": 0,
diff --git a/src/main/deploy/pathplanner/paths/CloseLeft.path b/src/main/deploy/pathplanner/paths/CloseLeft.path
index 2f4ef29a..c30a2487 100644
--- a/src/main/deploy/pathplanner/paths/CloseLeft.path
+++ b/src/main/deploy/pathplanner/paths/CloseLeft.path
@@ -32,10 +32,10 @@
   "constraintZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 3.0,
-    "maxAcceleration": 3.0,
-    "maxAngularVelocity": 540.0,
-    "maxAngularAcceleration": 720.0
+    "maxVelocity": 1.0,
+    "maxAcceleration": 1.5,
+    "maxAngularVelocity": 100.0,
+    "maxAngularAcceleration": 150.0
   },
   "goalEndState": {
     "velocity": 0,
diff --git a/src/main/deploy/pathplanner/paths/CloseLeftReturn.path b/src/main/deploy/pathplanner/paths/CloseLeftReturn.path
index cf7376a2..1916bdc1 100644
--- a/src/main/deploy/pathplanner/paths/CloseLeftReturn.path
+++ b/src/main/deploy/pathplanner/paths/CloseLeftReturn.path
@@ -32,10 +32,10 @@
   "constraintZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 3.0,
-    "maxAcceleration": 3.0,
-    "maxAngularVelocity": 540.0,
-    "maxAngularAcceleration": 720.0
+    "maxVelocity": 1.0,
+    "maxAcceleration": 1.5,
+    "maxAngularVelocity": 100.0,
+    "maxAngularAcceleration": 150.0
   },
   "goalEndState": {
     "velocity": 0,
diff --git a/src/main/deploy/pathplanner/paths/CloseRight.path b/src/main/deploy/pathplanner/paths/CloseRight.path
index f514f3fe..aa5aa48f 100644
--- a/src/main/deploy/pathplanner/paths/CloseRight.path
+++ b/src/main/deploy/pathplanner/paths/CloseRight.path
@@ -32,10 +32,10 @@
   "constraintZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 3.0,
-    "maxAcceleration": 3.0,
-    "maxAngularVelocity": 540.0,
-    "maxAngularAcceleration": 720.0
+    "maxVelocity": 1.0,
+    "maxAcceleration": 1.5,
+    "maxAngularVelocity": 100.0,
+    "maxAngularAcceleration": 150.0
   },
   "goalEndState": {
     "velocity": 0,
diff --git a/src/main/deploy/pathplanner/paths/CloseRightReturn.path b/src/main/deploy/pathplanner/paths/CloseRightReturn.path
index 28e75ef8..e23d8dd6 100644
--- a/src/main/deploy/pathplanner/paths/CloseRightReturn.path
+++ b/src/main/deploy/pathplanner/paths/CloseRightReturn.path
@@ -32,10 +32,10 @@
   "constraintZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 3.0,
-    "maxAcceleration": 3.0,
-    "maxAngularVelocity": 540.0,
-    "maxAngularAcceleration": 720.0
+    "maxVelocity": 1.0,
+    "maxAcceleration": 1.5,
+    "maxAngularVelocity": 100.0,
+    "maxAngularAcceleration": 150.0
   },
   "goalEndState": {
     "velocity": 0,
diff --git a/src/main/deploy/pathplanner/paths/FarLeft.path b/src/main/deploy/pathplanner/paths/FarLeft.path
index 43d0f42d..35bad1a4 100644
--- a/src/main/deploy/pathplanner/paths/FarLeft.path
+++ b/src/main/deploy/pathplanner/paths/FarLeft.path
@@ -32,10 +32,10 @@
   "constraintZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 3.0,
-    "maxAcceleration": 3.0,
-    "maxAngularVelocity": 540.0,
-    "maxAngularAcceleration": 720.0
+    "maxVelocity": 1.0,
+    "maxAcceleration": 1.5,
+    "maxAngularVelocity": 100.0,
+    "maxAngularAcceleration": 150.0
   },
   "goalEndState": {
     "velocity": 0,
diff --git a/src/main/deploy/pathplanner/paths/FarLeftCenter.path b/src/main/deploy/pathplanner/paths/FarLeftCenter.path
index fcc9eb68..747f6891 100644
--- a/src/main/deploy/pathplanner/paths/FarLeftCenter.path
+++ b/src/main/deploy/pathplanner/paths/FarLeftCenter.path
@@ -32,10 +32,10 @@
   "constraintZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 3.0,
-    "maxAcceleration": 3.0,
-    "maxAngularVelocity": 540.0,
-    "maxAngularAcceleration": 720.0
+    "maxVelocity": 1.0,
+    "maxAcceleration": 1.5,
+    "maxAngularVelocity": 100.0,
+    "maxAngularAcceleration": 150.0
   },
   "goalEndState": {
     "velocity": 0,
diff --git a/src/main/deploy/pathplanner/paths/FarLeftCenterReturn.path b/src/main/deploy/pathplanner/paths/FarLeftCenterReturn.path
index 32bfd197..50e3481c 100644
--- a/src/main/deploy/pathplanner/paths/FarLeftCenterReturn.path
+++ b/src/main/deploy/pathplanner/paths/FarLeftCenterReturn.path
@@ -32,10 +32,10 @@
   "constraintZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 3.0,
-    "maxAcceleration": 3.0,
-    "maxAngularVelocity": 540.0,
-    "maxAngularAcceleration": 720.0
+    "maxVelocity": 1.0,
+    "maxAcceleration": 1.5,
+    "maxAngularVelocity": 100.0,
+    "maxAngularAcceleration": 150.0
   },
   "goalEndState": {
     "velocity": 0,
diff --git a/src/main/deploy/pathplanner/paths/FarLeftReturn.path b/src/main/deploy/pathplanner/paths/FarLeftReturn.path
index 0f73404a..993de755 100644
--- a/src/main/deploy/pathplanner/paths/FarLeftReturn.path
+++ b/src/main/deploy/pathplanner/paths/FarLeftReturn.path
@@ -32,10 +32,10 @@
   "constraintZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 3.0,
-    "maxAcceleration": 3.0,
-    "maxAngularVelocity": 540.0,
-    "maxAngularAcceleration": 720.0
+    "maxVelocity": 1.0,
+    "maxAcceleration": 1.5,
+    "maxAngularVelocity": 100.0,
+    "maxAngularAcceleration": 150.0
   },
   "goalEndState": {
     "velocity": 0,
diff --git a/src/main/deploy/pathplanner/paths/FirstNote.path b/src/main/deploy/pathplanner/paths/FirstNote.path
index 226d93ee..596b7dcf 100644
--- a/src/main/deploy/pathplanner/paths/FirstNote.path
+++ b/src/main/deploy/pathplanner/paths/FirstNote.path
@@ -3,25 +3,25 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 1.37,
-        "y": 5.56
+        "x": 0.0,
+        "y": -0.0
       },
       "prevControl": null,
       "nextControl": {
-        "x": 2.37,
-        "y": 5.56
+        "x": 1.0,
+        "y": 0.0
       },
       "isLocked": false,
       "linkedName": null
     },
     {
       "anchor": {
-        "x": 2.91,
-        "y": 5.56
+        "x": 2.0,
+        "y": -0.0
       },
       "prevControl": {
-        "x": 1.9100000000000001,
-        "y": 5.56
+        "x": 1.0,
+        "y": 0.0
       },
       "nextControl": null,
       "isLocked": false,
@@ -32,14 +32,14 @@
   "constraintZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 3.0,
-    "maxAcceleration": 3.0,
-    "maxAngularVelocity": 540.0,
-    "maxAngularAcceleration": 720.0
+    "maxVelocity": 1.0,
+    "maxAcceleration": 1.5,
+    "maxAngularVelocity": 100.0,
+    "maxAngularAcceleration": 150.0
   },
   "goalEndState": {
     "velocity": 0,
-    "rotation": 0.31590706194753587,
+    "rotation": 0.0,
     "rotateFast": false
   },
   "reversed": false,
@@ -48,5 +48,5 @@
     "rotation": 0,
     "velocity": 0
   },
-  "useDefaultConstraints": true
+  "useDefaultConstraints": false
 }
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/LeftCloseLeft.path b/src/main/deploy/pathplanner/paths/LeftCloseLeft.path
index feb95c8f..a7b4b4a7 100644
--- a/src/main/deploy/pathplanner/paths/LeftCloseLeft.path
+++ b/src/main/deploy/pathplanner/paths/LeftCloseLeft.path
@@ -32,10 +32,10 @@
   "constraintZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 3.0,
-    "maxAcceleration": 3.0,
-    "maxAngularVelocity": 540.0,
-    "maxAngularAcceleration": 720.0
+    "maxVelocity": 1.0,
+    "maxAcceleration": 1.5,
+    "maxAngularVelocity": 100.0,
+    "maxAngularAcceleration": 150.0
   },
   "goalEndState": {
     "velocity": 0,
diff --git a/wombat/src/main/cpp/utils/Pathplanner.cpp b/wombat/src/main/cpp/utils/Pathplanner.cpp
index 835fcb8e..c001af3f 100644
--- a/wombat/src/main/cpp/utils/Pathplanner.cpp
+++ b/wombat/src/main/cpp/utils/Pathplanner.cpp
@@ -269,8 +269,8 @@ utils::FollowPath::FollowPath(drivetrain::SwerveDrive* swerve, std::string path,
   int i = 0;
   bool f = true;
 
-  nt::NetworkTableInstance::GetDefault().GetTable("pathplanner")->GetEntry("index").SetDouble(index);
-  nt::NetworkTableInstance::GetDefault().GetTable("pathplanner")->GetEntry("total").SetInteger(tot);
+  // nt::NetworkTableInstance::GetDefault().GetTable("pathplanner")->GetEntry("index").SetDouble(index);
+  // nt::NetworkTableInstance::GetDefault().GetTable("pathplanner")->GetEntry("total").SetInteger(tot);
 
   for (const pathplanner::PathPoint& point : points) {
     if (lastRot != nullptr) {
@@ -287,8 +287,9 @@ utils::FollowPath::FollowPath(drivetrain::SwerveDrive* swerve, std::string path,
     frc::Pose2d pose2 = frc::Pose2d(tr, rot);//.TransformBy(frc::Transform2d(-1.37_m, -5.56_m, 0_deg));
 
     if (i == index || i == tot || f) {
-      _poses.emplace_back(frc::Pose2d(pose2.Y() - 5.56_m, pose2.X() - 2.91_m, pose2.Rotation()));
-      i = 0;
+      _poses.emplace_back(frc::Pose2d(pose2.X(), pose2.Y(), pose2.Rotation()));
+      // _poses.emplace_back(frc::Pose2d(pose2.Y(), pose2.X(), pose2.Rotation()));
+      i = 0;//  - 5.56_m,  - 2.91_m
       f = false;
     }
 
@@ -326,14 +327,14 @@ utils::FollowPath::FollowPath(drivetrain::SwerveDrive* swerve, std::string path,
   //   WritePose2NT(nt::NetworkTableInstance::GetDefault().GetTable("pathplanner/" + std::to_string(i) +
   //   "/poses/prev"), p2);
   //
-  //   i++;
+    i++;
   // }
 
   i = 0;
   for (const frc::Pose2d pose : _poses) {
     WritePose2NT(nt::NetworkTableInstance::GetDefault().GetTable("pathplanner/poses/" + std::to_string(i)),
                  pose);
-    i++;
+    // i++;
   }
 
   CalcTimer();
@@ -354,7 +355,7 @@ void utils::FollowPath::CalcTimer() {
 
   _timer.Stop();
   _timer.Reset();
-  _time = units::second_t{std::abs(dist.value()) * 1.2 /*meters per second*/};
+  _time = units::second_t{std::abs(dist.value()) * 4 /*meters per second*/};
   // _time = 20_s;
   _timer.Start();
 }
@@ -428,13 +429,13 @@ void utils::AutoBuilder::SetAuto(std::string path) {
   wpi::json j = wpi::json::parse(cjson);
 
   _currentPath = &j;
-  _startingPose = &j["startingPose"];
+  // _startingPose = &j["startingPose"];
   _commands = &j["command"]["data"]["commands"];
 
   commands = std::vector<std::pair<std::string, std::string>>();
 
   nt::NetworkTableInstance::GetDefault().GetTable("json")->GetEntry("data").SetString(_currentPath->dump());
-  nt::NetworkTableInstance::GetDefault().GetTable("json")->GetEntry("start").SetString(_startingPose->dump());
+  // nt::NetworkTableInstance::GetDefault().GetTable("json")->GetEntry("start").SetString(_startingPose->dump());
   nt::NetworkTableInstance::GetDefault().GetTable("json")->GetEntry("commands").SetString(_commands->dump());
   nt::NetworkTableInstance::GetDefault()
       .GetTable("json")
@@ -448,9 +449,9 @@ void utils::AutoBuilder::SetAuto(std::string path) {
     if (c["type"] == "named") {
       commands.push_back(std::make_pair(c["type"], c["data"]["name"]));
     }
-    // if (c["type"] == "parallel") {
-      // commands.push_back(std::make_pair(c["type"], ""));
-    // }
+    if (c["type"] == "parallel") {
+      commands.push_back(std::make_pair(c["type"], ""));
+    }
   }
 
   nt::NetworkTableInstance::GetDefault().GetTable("commands")->GetEntry("length").SetInteger(commands.size());
@@ -505,8 +506,8 @@ void utils::AutoBuilder::SetAuto(std::string path) {
   nt::NetworkTableInstance::GetDefault().GetTable("commands")->GetEntry("PathAmt").SetInteger(pathamt);
   nt::NetworkTableInstance::GetDefault().GetTable("commands")->GetEntry("CommandAmt").SetInteger(commandamt);
 
-  WritePose2NT(nt::NetworkTableInstance::GetDefault().GetTable("startPose"),
-               JSONPoseToPose2d(*_startingPose));
+  // WritePose2NT(nt::NetworkTableInstance::GetDefault().GetTable("startPose"),
+  //              JSONPoseToPose2d(*_startingPose));
 
   // nt::NetworkTableInstance::GetDefault().GetTable("commands")->GetEntry("behaviours").SetStringArray(pathplan->GetName());
 }
diff --git a/wombat/src/main/include/utils/Pathplanner.h b/wombat/src/main/include/utils/Pathplanner.h
index f9980349..3d2fa729 100644
--- a/wombat/src/main/include/utils/Pathplanner.h
+++ b/wombat/src/main/include/utils/Pathplanner.h
@@ -293,7 +293,7 @@ class AutoBuilder {
   AutoCommands _commandsList;
 
   wpi::json* _currentPath;
-  wpi::json* _startingPose;
+  // wpi::json* _startingPose;
   wpi::json* _commands;
 
   behaviour::Behaviour::ptr pathplan;

From 6c267c3ebdc1e2feafb329839613c956f83a3b69 Mon Sep 17 00:00:00 2001
From: Goanna247 <21496350@student.curtin.edu.au>
Date: Wed, 6 Mar 2024 15:21:51 +0800
Subject: [PATCH 195/207] backwards + forwards

---
 src/main/cpp/Auto.cpp                         |  4 ++
 src/main/cpp/Robot.cpp                        | 17 ++++--
 .../deploy/pathplanner/autos/OneNoteTaxi.auto |  6 +-
 src/main/deploy/pathplanner/autos/Taxi.auto   | 56 +++++++++++++++++++
 .../deploy/pathplanner/paths/Backwards.path   |  8 +--
 src/main/include/Auto.h                       |  1 +
 src/main/include/Robot.h                      |  1 +
 wombat/src/main/cpp/utils/Pathplanner.cpp     | 29 ++++++----
 .../src/main/include/drivetrain/SwerveDrive.h |  2 +-
 9 files changed, 100 insertions(+), 24 deletions(-)
 create mode 100644 src/main/deploy/pathplanner/autos/Taxi.auto

diff --git a/src/main/cpp/Auto.cpp b/src/main/cpp/Auto.cpp
index 8c37a0e5..5051960f 100644
--- a/src/main/cpp/Auto.cpp
+++ b/src/main/cpp/Auto.cpp
@@ -36,6 +36,10 @@ std::shared_ptr<behaviour::Behaviour> autos::Taxi(wom::SwerveAutoBuilder* builde
   // behaviour::make<ArmToSetPoint>(_alphaArm, 1_deg);
 }
 
+std::shared_ptr<behaviour::Behaviour> autos::OneNoteTaxi(wom::SwerveAutoBuilder* builder) {
+  return builder->GetAutoRoutine("OneNoteTaxi");
+}
+
 // std::shared_ptr<behaviour::Behaviour> autos::QuadrupleClose(wom::drivetrain::SwerveDrive* _swerveDrive,
 //                                                             Shooter* _shooter, Intake* _intake,
 //                                                             AlphaArm* _alphaArm) {
diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp
index 9a76d89b..9f238d58 100644
--- a/src/main/cpp/Robot.cpp
+++ b/src/main/cpp/Robot.cpp
@@ -38,7 +38,14 @@ static units::second_t lastPeriodic;
 
 void Robot::RobotInit() {
   sched = wom::BehaviourScheduler::GetInstance();
-  m_chooser.SetDefaultOption("Default Auto", "Default Auto");
+  
+  frc::SmartDashboard::PutData("Auto Selector", &m_chooser);
+
+  m_chooser.SetDefaultOption(defaultAuto, defaultAuto);
+
+  for (auto& option : autoOptions) {
+    m_chooser.AddOption(option, option);
+  }
 
   _led = new LED();
 
@@ -135,10 +142,10 @@ void Robot::AutonomousInit() {
 
   robotmap._builder = autos::InitCommands(_swerveDrive, shooter, intake, alphaArm);
 
-  //if (m_autoSelected == "kTaxi") {
-  //  sched->Schedule(autos::Taxi(robotmap._builder));
-  //}
-  sched->Schedule(robotmap._builder->GetAutoRoutine("OneNoteTaxi"));
+  if (m_autoSelected == "kTaxi") {
+   sched->Schedule(autos::Taxi(robotmap._builder));
+  }
+  // sched->Schedule(robotmap._builder->GetAutoRoutine("OneNoteTaxi"));
 
   _swerveDrive->OnStart();
 }
diff --git a/src/main/deploy/pathplanner/autos/OneNoteTaxi.auto b/src/main/deploy/pathplanner/autos/OneNoteTaxi.auto
index 38dc2cad..61000c9e 100644
--- a/src/main/deploy/pathplanner/autos/OneNoteTaxi.auto
+++ b/src/main/deploy/pathplanner/autos/OneNoteTaxi.auto
@@ -8,13 +8,13 @@
         {
           "type": "path",
           "data": {
-            "pathName": "FirstNote"
+            "pathName": "Backwards"
           }
         },
         {
-          "type": "path",
+          "type": "named",
           "data": {
-            "pathName": "Backwards"
+            "name": null
           }
         }
       ]
diff --git a/src/main/deploy/pathplanner/autos/Taxi.auto b/src/main/deploy/pathplanner/autos/Taxi.auto
new file mode 100644
index 00000000..5387ddd5
--- /dev/null
+++ b/src/main/deploy/pathplanner/autos/Taxi.auto
@@ -0,0 +1,56 @@
+{
+  "version": 1.0,
+  "startingPose": null,
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "named",
+          "data": {
+            "name": "Shoot"
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "named",
+                "data": {
+                  "name": "IntakeNote"
+                }
+              },
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "Backwards"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "FirstNote"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Shoot"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Backwards"
+          }
+        }
+      ]
+    }
+  },
+  "folder": null,
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/Backwards.path b/src/main/deploy/pathplanner/paths/Backwards.path
index ac346ca2..f187dc44 100644
--- a/src/main/deploy/pathplanner/paths/Backwards.path
+++ b/src/main/deploy/pathplanner/paths/Backwards.path
@@ -8,8 +8,8 @@
       },
       "prevControl": null,
       "nextControl": {
-        "x": 0.6039125090180515,
-        "y": 1.7097140771607025e-16
+        "x": 1.993173655558508,
+        "y": 0.020888794958252705
       },
       "isLocked": false,
       "linkedName": null
@@ -20,8 +20,8 @@
         "y": 0.0
       },
       "prevControl": {
-        "x": 1.0,
-        "y": -1.2246467991473532e-16
+        "x": -1.0,
+        "y": 0.0
       },
       "nextControl": null,
       "isLocked": false,
diff --git a/src/main/include/Auto.h b/src/main/include/Auto.h
index 7e9f8df5..3133e715 100644
--- a/src/main/include/Auto.h
+++ b/src/main/include/Auto.h
@@ -24,6 +24,7 @@ std::shared_ptr<behaviour::Behaviour> AutoTest(wom::drivetrain::SwerveDrive* _sw
                                                Intake* _intake, AlphaArm* _alphaArm);
 
 std::shared_ptr<behaviour::Behaviour> Taxi(wom::utils::SwerveAutoBuilder* builder);
+std::shared_ptr<behaviour::Behaviour> OneNoteTaxi(wom::utils::SwerveAutoBuilder* builder);
 
 std::shared_ptr<behaviour::Behaviour> QuadrupleClose(wom::drivetrain::SwerveDrive* _swerveDrive,
                                                      Shooter* _shooter, Intake* _intake, AlphaArm* _alphaArm);
diff --git a/src/main/include/Robot.h b/src/main/include/Robot.h
index 68376038..d2d9e27c 100644
--- a/src/main/include/Robot.h
+++ b/src/main/include/Robot.h
@@ -91,6 +91,7 @@ class Robot : public frc::TimedRobot {
   AlphaArm* alphaArm;
   LED* _led;
 
+
   // ctre::phoenix6::hardware::TalonFX *frontLeft;
   // ctre::phoenix6::hardware::TalonFX *frontRight;
   // ctre::phoenix6::hardware::TalonFX *backLeft;
diff --git a/wombat/src/main/cpp/utils/Pathplanner.cpp b/wombat/src/main/cpp/utils/Pathplanner.cpp
index c001af3f..b912e6f5 100644
--- a/wombat/src/main/cpp/utils/Pathplanner.cpp
+++ b/wombat/src/main/cpp/utils/Pathplanner.cpp
@@ -246,6 +246,11 @@ utils::FollowPath::FollowPath(drivetrain::SwerveDrive* swerve, std::string path,
 
   std::ifstream file(filePath);
 
+  nt::NetworkTableInstance::GetDefault()
+      .GetTable("pathplanner")
+      ->GetEntry("Current path")
+      .SetString(filePath);
+
   std::string cdata;
   std::string cjson;
 
@@ -259,18 +264,19 @@ utils::FollowPath::FollowPath(drivetrain::SwerveDrive* swerve, std::string path,
   std::vector<pathplanner::PathPoint> points =
       pathplanner::PathPlannerPath::fromPathFile(path)->getAllPathPoints();
 
+
   pathplanner::RotationTarget* lastRot = nullptr;
   units::degree_t rot;
 
   int amt = 2;
   size_t tot = points.size();
-  int index = std::round((static_cast<double>(amt) / static_cast<int>(tot)) * 100);
+  int index = std::round((static_cast<double>(amt) / static_cast<double>(tot)) * 100);
   // double index = (amt / tot) * 100;
   int i = 0;
   bool f = true;
 
-  // nt::NetworkTableInstance::GetDefault().GetTable("pathplanner")->GetEntry("index").SetDouble(index);
-  // nt::NetworkTableInstance::GetDefault().GetTable("pathplanner")->GetEntry("total").SetInteger(tot);
+  nt::NetworkTableInstance::GetDefault().GetTable("pathplanner")->GetEntry("index").SetDouble(index);
+  nt::NetworkTableInstance::GetDefault().GetTable("pathplanner")->GetEntry("total").SetInteger(tot);
 
   for (const pathplanner::PathPoint& point : points) {
     if (lastRot != nullptr) {
@@ -285,8 +291,8 @@ utils::FollowPath::FollowPath(drivetrain::SwerveDrive* swerve, std::string path,
 
     frc::Translation2d tr = frc::Translation2d(point.position.X() * -1, point.position.Y() * -1);
     frc::Pose2d pose2 = frc::Pose2d(tr, rot);//.TransformBy(frc::Transform2d(-1.37_m, -5.56_m, 0_deg));
-
-    if (i == index || i == tot || f) {
+    //
+    if ( i == index || i == tot || f) {
       _poses.emplace_back(frc::Pose2d(pose2.X(), pose2.Y(), pose2.Rotation()));
       // _poses.emplace_back(frc::Pose2d(pose2.Y(), pose2.X(), pose2.Rotation()));
       i = 0;//  - 5.56_m,  - 2.91_m
@@ -327,10 +333,10 @@ utils::FollowPath::FollowPath(drivetrain::SwerveDrive* swerve, std::string path,
   //   WritePose2NT(nt::NetworkTableInstance::GetDefault().GetTable("pathplanner/" + std::to_string(i) +
   //   "/poses/prev"), p2);
   //
-    i++;
+    // i++;
   // }
 
-  i = 0;
+  // i = 0;
   for (const frc::Pose2d pose : _poses) {
     WritePose2NT(nt::NetworkTableInstance::GetDefault().GetTable("pathplanner/poses/" + std::to_string(i)),
                  pose);
@@ -355,8 +361,8 @@ void utils::FollowPath::CalcTimer() {
 
   _timer.Stop();
   _timer.Reset();
-  _time = units::second_t{std::abs(dist.value()) * 4 /*meters per second*/};
-  // _time = 20_s;
+  // _time = units::second_t{std::abs(dist.value()) * 3 /*meters per second ?No?*/};
+  _time = 20_s;
   _timer.Start();
 }
 
@@ -374,6 +380,7 @@ void utils::FollowPath::OnTick(units::second_t dt) {
       .GetTable("pathplanner")
       ->GetEntry("currentTime")
       .SetDouble(_timer.Get().value());
+  
   WritePose2NT(nt::NetworkTableInstance::GetDefault().GetTable("pathplanner/targetPose"),
                _poses[_currentPose]);
   WritePose2NT(nt::NetworkTableInstance::GetDefault().GetTable("pathplanner/currentPose"),
@@ -387,8 +394,8 @@ void utils::FollowPath::OnTick(units::second_t dt) {
   /*else*/ if (_swerve->IsAtSetPose() || _timer.Get() > _time) {
     if (_currentPose + 1 == static_cast<int>(_poses.size())) {
       // _swerve->MakeAtSetPoint();
-      _swerve->SetVelocity(frc::ChassisSpeeds());
-      _swerve->MakeAtSetPoint();
+      // _swerve->SetVelocity(frc::ChassisSpeeds());
+      // _swerve->MakeAtSetPoint();
       SetDone();
     } else {
       _currentPose++;
diff --git a/wombat/src/main/include/drivetrain/SwerveDrive.h b/wombat/src/main/include/drivetrain/SwerveDrive.h
index 6167fe58..3c7531e3 100644
--- a/wombat/src/main/include/drivetrain/SwerveDrive.h
+++ b/wombat/src/main/include/drivetrain/SwerveDrive.h
@@ -286,7 +286,7 @@ class PIDController : public wpi::Sendable, public wpi::SendableHelper<PIDContro
   double m_totalError = 0;
 
   // The error that is considered at setpoint.
-  double m_positionTolerance = 0.05;
+  double m_positionTolerance = 0.2;
   double m_velocityTolerance = std::numeric_limits<double>::infinity();
 
   double m_setpoint = 0;

From 904a512e5805d5d2724015b2a5dc661d9ce3d9f8 Mon Sep 17 00:00:00 2001
From: = <paul.hodges@student.education.wa.edu.au>
Date: Wed, 6 Mar 2024 16:29:47 +0800
Subject: [PATCH 196/207] ????

---
 src/main/deploy/pathplanner/autos/Taxi.auto    | 16 +++++-----------
 wombat/src/main/cpp/drivetrain/SwerveDrive.cpp |  4 ++--
 wombat/src/main/cpp/utils/Pathplanner.cpp      | 18 +++++++++---------
 3 files changed, 16 insertions(+), 22 deletions(-)

diff --git a/src/main/deploy/pathplanner/autos/Taxi.auto b/src/main/deploy/pathplanner/autos/Taxi.auto
index 5387ddd5..bd6319f4 100644
--- a/src/main/deploy/pathplanner/autos/Taxi.auto
+++ b/src/main/deploy/pathplanner/autos/Taxi.auto
@@ -16,15 +16,15 @@
           "data": {
             "commands": [
               {
-                "type": "named",
+                "type": "path",
                 "data": {
-                  "name": "IntakeNote"
+                  "pathName": "FirstNote"
                 }
               },
               {
-                "type": "path",
+                "type": "named",
                 "data": {
-                  "pathName": "Backwards"
+                  "name": "IntakeNote"
                 }
               }
             ]
@@ -33,7 +33,7 @@
         {
           "type": "path",
           "data": {
-            "pathName": "FirstNote"
+            "pathName": "Backwards"
           }
         },
         {
@@ -41,12 +41,6 @@
           "data": {
             "name": "Shoot"
           }
-        },
-        {
-          "type": "path",
-          "data": {
-            "pathName": "Backwards"
-          }
         }
       ]
     }
diff --git a/wombat/src/main/cpp/drivetrain/SwerveDrive.cpp b/wombat/src/main/cpp/drivetrain/SwerveDrive.cpp
index 11961907..0ab2f32d 100644
--- a/wombat/src/main/cpp/drivetrain/SwerveDrive.cpp
+++ b/wombat/src/main/cpp/drivetrain/SwerveDrive.cpp
@@ -596,7 +596,7 @@ void SwerveDrive::OnUpdate(units::second_t dt) {
   }
 
   _poseEstimator.Update(
-      _config.gyro->GetRotation2d() - 1_rad,
+      _config.gyro->GetRotation2d()/* - 1_rad*/,
       wpi::array<frc::SwerveModulePosition, 4>{_modules[3].GetPosition(), _modules[0].GetPosition(),
                                                _modules[1].GetPosition(), _modules[2].GetPosition()});
 
@@ -716,7 +716,7 @@ SwerveDriveState SwerveDrive::GetState() {
 
 void SwerveDrive::ResetPose(frc::Pose2d pose) {
   _poseEstimator.ResetPosition(
-      _config.gyro->GetRotation2d() - 1_rad,
+      _config.gyro->GetRotation2d()/* - 1_rad*/,
       wpi::array<frc::SwerveModulePosition, 4>{_modules[0].GetPosition(), _modules[1].GetPosition(),
                                                _modules[2].GetPosition(), _modules[3].GetPosition()},
       pose);
diff --git a/wombat/src/main/cpp/utils/Pathplanner.cpp b/wombat/src/main/cpp/utils/Pathplanner.cpp
index b912e6f5..9008a9b4 100644
--- a/wombat/src/main/cpp/utils/Pathplanner.cpp
+++ b/wombat/src/main/cpp/utils/Pathplanner.cpp
@@ -268,7 +268,7 @@ utils::FollowPath::FollowPath(drivetrain::SwerveDrive* swerve, std::string path,
   pathplanner::RotationTarget* lastRot = nullptr;
   units::degree_t rot;
 
-  int amt = 2;
+  int amt = 1;
   size_t tot = points.size();
   int index = std::round((static_cast<double>(amt) / static_cast<double>(tot)) * 100);
   // double index = (amt / tot) * 100;
@@ -292,7 +292,7 @@ utils::FollowPath::FollowPath(drivetrain::SwerveDrive* swerve, std::string path,
     frc::Translation2d tr = frc::Translation2d(point.position.X() * -1, point.position.Y() * -1);
     frc::Pose2d pose2 = frc::Pose2d(tr, rot);//.TransformBy(frc::Transform2d(-1.37_m, -5.56_m, 0_deg));
     //
-    if ( i == index || i == tot || f) {
+    if ( /*i == index ||*/ i == tot || f) {
       _poses.emplace_back(frc::Pose2d(pose2.X(), pose2.Y(), pose2.Rotation()));
       // _poses.emplace_back(frc::Pose2d(pose2.Y(), pose2.X(), pose2.Rotation()));
       i = 0;//  - 5.56_m,  - 2.91_m
@@ -361,8 +361,8 @@ void utils::FollowPath::CalcTimer() {
 
   _timer.Stop();
   _timer.Reset();
-  // _time = units::second_t{std::abs(dist.value()) * 3 /*meters per second ?No?*/};
-  _time = 20_s;
+  // _time = units::second_t{std::abs(dist.value()) / 0.7 /*meters per second ?No?*/};
+  _time = 15_s;
   _timer.Start();
 }
 
@@ -489,17 +489,17 @@ void utils::AutoBuilder::SetAuto(std::string path) {
       commandamt++;
     } else if (command["type"] == "parallel") {
       
-      std::shared_ptr<behaviour::Behaviour> nb = behaviour::make<behaviour::ConcurrentBehaviour>(behaviour::ConcurrentBehaviourReducer::ALL);
+      auto nb = behaviour::make<behaviour::ConcurrentBehaviour>(behaviour::ConcurrentBehaviourReducer::FIRST);
       int j = 0;
       for (auto c : command["data"]["commands"]) { 
-        nt::NetworkTableInstance::GetDefault().GetTable("commands/parallel/" + std::to_string(j))->GetEntry("type").SetString(static_cast<std::string>(c["type"]));
+        // nt::NetworkTableInstance::GetDefault().GetTable("commands/parallel/" + std::to_string(j))->GetEntry("type").SetString(static_cast<std::string>(c["type"]));
 
-        nt::NetworkTableInstance::GetDefault().GetTable("commands/parallel/" + std::to_string(j))->GetEntry("typeisstring").SetBoolean(c["type"].is_string());
+        // nt::NetworkTableInstance::GetDefault().GetTable("commands/parallel/" + std::to_string(j))->GetEntry("typeisstring").SetBoolean(c["type"].is_string());
         if (static_cast<std::string>(c["type"]) == "path") {
-          nb = nb &  behaviour::make<FollowPath>(_swerve, c["data"]["pathName"], _flip);
+          nb->Add(behaviour::make<FollowPath>(_swerve, c["data"]["pathName"], _flip));
           pathamt++;
         } else if (static_cast<std::string>(c["type"]) == "named") {
-          nb = nb & _commandsList.Run(c["data"]["name"]);
+          nb->Add(_commandsList.Run(c["data"]["name"]));
           commandamt++;
         }
         j++;

From 7754bf56340ce551158131785d538fb6a0bfcf42 Mon Sep 17 00:00:00 2001
From: = <paul.hodges@student.education.wa.edu.au>
Date: Wed, 6 Mar 2024 16:48:29 +0800
Subject: [PATCH 197/207] :skull: :pensive:

---
 src/main/cpp/vision/Vision.cpp            | 6 ++++--
 wombat/src/main/cpp/utils/Pathplanner.cpp | 4 ++--
 2 files changed, 6 insertions(+), 4 deletions(-)

diff --git a/src/main/cpp/vision/Vision.cpp b/src/main/cpp/vision/Vision.cpp
index 3360833d..3a6534a2 100644
--- a/src/main/cpp/vision/Vision.cpp
+++ b/src/main/cpp/vision/Vision.cpp
@@ -321,7 +321,7 @@ frc::Pose2d Vision::TurnToTarget(int target, wom::SwerveDrive* swerveDrive) {
 
   // std::cout << pose.Rotation().Degrees().value() << std::endl;
 
-  // swerveDrive->SetPose(pose);
+  swerveDrive->TurnToAngle(angle);
 
   return pose;
 }
@@ -339,7 +339,9 @@ frc::Pose2d Vision::TurnToTarget(VisionTarget target, wom::SwerveDrive* swerveDr
 
   std::cout << pose.Rotation().Degrees().value() << std::endl;
 
-  swerveDrive->SetPose(pose);
+  swerveDrive->TurnToAngle(angle);
+
+  return pose;
 }
 
 std::pair<frc::Pose2d, units::degree_t> Vision::GetAngleToObject(VisionTargetObjects object) {
diff --git a/wombat/src/main/cpp/utils/Pathplanner.cpp b/wombat/src/main/cpp/utils/Pathplanner.cpp
index 9008a9b4..2584ee8f 100644
--- a/wombat/src/main/cpp/utils/Pathplanner.cpp
+++ b/wombat/src/main/cpp/utils/Pathplanner.cpp
@@ -492,9 +492,9 @@ void utils::AutoBuilder::SetAuto(std::string path) {
       auto nb = behaviour::make<behaviour::ConcurrentBehaviour>(behaviour::ConcurrentBehaviourReducer::FIRST);
       int j = 0;
       for (auto c : command["data"]["commands"]) { 
-        // nt::NetworkTableInstance::GetDefault().GetTable("commands/parallel/" + std::to_string(j))->GetEntry("type").SetString(static_cast<std::string>(c["type"]));
+        nt::NetworkTableInstance::GetDefault().GetTable("commands/parallel/" + std::to_string(j))->GetEntry("type").SetString(static_cast<std::string>(c["type"]));
 
-        // nt::NetworkTableInstance::GetDefault().GetTable("commands/parallel/" + std::to_string(j))->GetEntry("typeisstring").SetBoolean(c["type"].is_string());
+        nt::NetworkTableInstance::GetDefault().GetTable("commands/parallel/" + std::to_string(j))->GetEntry("typeisstring").SetBoolean(c["type"].is_string());
         if (static_cast<std::string>(c["type"]) == "path") {
           nb->Add(behaviour::make<FollowPath>(_swerve, c["data"]["pathName"], _flip));
           pathamt++;

From e0632b1b39188deff69dcf8e1a32543c33cccde0 Mon Sep 17 00:00:00 2001
From: = <paul.hodges@student.education.wa.edu.au>
Date: Wed, 6 Mar 2024 19:02:25 +0800
Subject: [PATCH 198/207] :skull: :pensive:

---
 src/main/cpp/Auto.cpp                         |  2 +-
 src/main/cpp/IntakeBehaviour.cpp              | 15 ++++++-
 src/main/cpp/ShooterBehaviour.cpp             | 16 ++++---
 src/main/deploy/pathplanner/autos/Taxi.auto   | 23 ++++------
 src/main/include/IntakeBehaviour.h            |  1 +
 src/main/include/ShooterBehaviour.h           |  1 +
 wombat/src/main/cpp/behaviour/Behaviour.cpp   | 11 +++++
 wombat/src/main/cpp/utils/Pathplanner.cpp     | 45 ++++++++++++++-----
 wombat/src/main/include/behaviour/Behaviour.h |  2 +
 wombat/src/main/include/utils/Pathplanner.h   |  7 +--
 10 files changed, 87 insertions(+), 36 deletions(-)

diff --git a/src/main/cpp/Auto.cpp b/src/main/cpp/Auto.cpp
index 5051960f..4f255766 100644
--- a/src/main/cpp/Auto.cpp
+++ b/src/main/cpp/Auto.cpp
@@ -23,7 +23,7 @@ wom::SwerveAutoBuilder* autos::InitCommands(wom::drivetrain::SwerveDrive* _swerv
        {"IntakeNote", [_intake]() { return wom::make<IntakeNote>(_intake)->WithTimeout(1_s); }},
        {"PassNote", [_intake]() { return wom::make<PassNote>(_intake)->WithTimeout(1_s); }},
        {"EjectNote", [_intake]() { return wom::make<EjectNote>(_intake)->WithTimeout(1_s); }},
-       {"Shoot", [_shooter, _intake]() { return wom::make<AutoShooter>(_shooter, _intake, 1500_rad_per_s); }} 
+       {"Shoot", [_shooter, _intake]() { return wom::make<AutoShooter>(_shooter, _intake, 300_rad_per_s)->WithTimeout(2.5_s); }} 
       });
 
   return new wom::utils::SwerveAutoBuilder(_swerveDrive, "Taxi", c);
diff --git a/src/main/cpp/IntakeBehaviour.cpp b/src/main/cpp/IntakeBehaviour.cpp
index 38fca2d4..d9f78be3 100644
--- a/src/main/cpp/IntakeBehaviour.cpp
+++ b/src/main/cpp/IntakeBehaviour.cpp
@@ -5,6 +5,9 @@
 #include "IntakeBehaviour.h"
 
 #include <frc/XboxController.h>
+#include "Intake.h"
+#include "behaviour/Behaviour.h"
+#include "networktables/NetworkTableInstance.h"
 
 IntakeManualControl::IntakeManualControl(Intake* intake, AlphaArm *arm, frc::XboxController& codriver) : _intake(intake), _arm(arm), _codriver(codriver) {
   Controls(intake);
@@ -68,19 +71,27 @@ void AutoIntake::OnTick(units::second_t dt) {
   _intake->SetState(IntakeState::kIntake);
 }
 
-IntakeNote::IntakeNote(Intake* intake) : _intake(intake) {
+IntakeNote::IntakeNote(Intake* intake) : _intake(intake), behaviour::Behaviour("<Intake Note>") {
   Controls(intake);
 }
 
 void IntakeNote::OnTick(units::second_t dt) {
   _intake->SetState(IntakeState::kIntake);
 
-  if (_intake->GetState() == IntakeState::kHold) {
+  nt::NetworkTableInstance::GetDefault().GetTable("AutoIntake")->GetEntry("running").SetBoolean(true);
+
+  if (_intake->GetState() == IntakeState::kHold || _intake->GetState() == IntakeState::kIdle) {
+    _intake->SetState(IntakeState::kIdle);
     std::cerr << "EXITING" << std::endl;
     SetDone();
   }
 }
 
+void IntakeNote::OnStop() {
+  _intake->SetState(IntakeState::kIdle);
+    nt::NetworkTableInstance::GetDefault().GetTable("AutoIntake")->GetEntry("running").SetBoolean(false);
+}
+
 PassNote::PassNote(Intake* intake) : _intake(intake) {
   Controls(intake);
 }
diff --git a/src/main/cpp/ShooterBehaviour.cpp b/src/main/cpp/ShooterBehaviour.cpp
index 2765be4a..b4da99d8 100644
--- a/src/main/cpp/ShooterBehaviour.cpp
+++ b/src/main/cpp/ShooterBehaviour.cpp
@@ -5,6 +5,7 @@
 #include "ShooterBehaviour.h"
 #include "Intake.h"
 #include "Shooter.h"
+#include "behaviour/Behaviour.h"
 #include "units/angle.h"
 #include "units/angular_velocity.h"
 #include "units/time.h"
@@ -56,21 +57,26 @@ void ShooterManualControl::OnTick(units::second_t dt) {
   }
 }
 
-AutoShooter::AutoShooter(Shooter* shooter, Intake* intake, units::radians_per_second_t goal) : _shooter(shooter), _intake(intake), _goal(goal) {
+AutoShooter::AutoShooter(Shooter* shooter, Intake* intake, units::radians_per_second_t goal) : _shooter(shooter), _intake(intake), _goal(goal), behaviour::Behaviour("<Shoot>") {
   Controls(shooter);
 
-  _timer.Start();
 }
 
 void AutoShooter::OnTick(units::second_t dt) {
-  _goal = 300_rad_per_s;
+  if (!_timer_started) {
+    _timer.Start();
+    _timer_started = true;
+  }
+
   _shooter->SetState(ShooterState::kSpinUp);
+  _intake->SetState(IntakeState::kHold);
+
   _shooter->SetPidGoal(_goal);
 
-  if (_timer.Get() > 3_s) {
+  if (_timer.Get() > 1.5_s) {
     _intake->SetState(IntakeState::kPass);
 
-    if (_timer.Get() > 5_s) {
+    if (_timer.Get() > 2_s) {
       _intake->SetState(IntakeState::kIdle);
       _shooter->SetState(ShooterState::kIdle);
 
diff --git a/src/main/deploy/pathplanner/autos/Taxi.auto b/src/main/deploy/pathplanner/autos/Taxi.auto
index bd6319f4..69a5d8de 100644
--- a/src/main/deploy/pathplanner/autos/Taxi.auto
+++ b/src/main/deploy/pathplanner/autos/Taxi.auto
@@ -12,22 +12,15 @@
           }
         },
         {
-          "type": "parallel",
+          "type": "path",
+          "data": {
+            "pathName": "FirstNote"
+          }
+        },
+        {
+          "type": "named",
           "data": {
-            "commands": [
-              {
-                "type": "path",
-                "data": {
-                  "pathName": "FirstNote"
-                }
-              },
-              {
-                "type": "named",
-                "data": {
-                  "name": "IntakeNote"
-                }
-              }
-            ]
+            "name": "IntakeNote"
           }
         },
         {
diff --git a/src/main/include/IntakeBehaviour.h b/src/main/include/IntakeBehaviour.h
index eeb3e6e5..e7700f7f 100644
--- a/src/main/include/IntakeBehaviour.h
+++ b/src/main/include/IntakeBehaviour.h
@@ -36,6 +36,7 @@ class IntakeNote : public behaviour::Behaviour {
  public:
   explicit IntakeNote(Intake* intake);
   void OnTick(units::second_t dt) override;
+  void OnStop() override;
 
  private:
   Intake* _intake;
diff --git a/src/main/include/ShooterBehaviour.h b/src/main/include/ShooterBehaviour.h
index 6aa71f87..5e182772 100644
--- a/src/main/include/ShooterBehaviour.h
+++ b/src/main/include/ShooterBehaviour.h
@@ -44,4 +44,5 @@ class AutoShooter : public behaviour::Behaviour {
   units::radians_per_second_t _goal;
 
   frc::Timer _timer;
+  bool _timer_started = false;
 };
diff --git a/wombat/src/main/cpp/behaviour/Behaviour.cpp b/wombat/src/main/cpp/behaviour/Behaviour.cpp
index 7312c922..ce516f1e 100644
--- a/wombat/src/main/cpp/behaviour/Behaviour.cpp
+++ b/wombat/src/main/cpp/behaviour/Behaviour.cpp
@@ -5,6 +5,7 @@
 #include "behaviour/Behaviour.h"
 
 #include <iostream>
+#include <string>
 
 using namespace behaviour;
 
@@ -148,6 +149,16 @@ void SequentialBehaviour::OnStop() {
   }
 }
 
+std::vector<std::string> SequentialBehaviour::GetQueue() {
+  std::vector<std::string> names;
+
+  for (auto bh : _queue) {
+    names.push_back(bh->GetName());
+  }
+
+  return names;
+}
+
 // ConcurrentBehaviour
 ConcurrentBehaviour::ConcurrentBehaviour(ConcurrentBehaviourReducer reducer)
     : Behaviour(), _reducer(reducer) {}
diff --git a/wombat/src/main/cpp/utils/Pathplanner.cpp b/wombat/src/main/cpp/utils/Pathplanner.cpp
index 2584ee8f..4585cd59 100644
--- a/wombat/src/main/cpp/utils/Pathplanner.cpp
+++ b/wombat/src/main/cpp/utils/Pathplanner.cpp
@@ -232,8 +232,11 @@ frc::Trajectory utils::PathWeaver::getTrajectory(std::string_view path) {
 }
 
 // FollowPath implementation
-utils::FollowPath::FollowPath(drivetrain::SwerveDrive* swerve, std::string path, bool flip, std::string name)
-    : _swerve(swerve), behaviour::Behaviour(name) {
+utils::FollowPath::FollowPath(drivetrain::SwerveDrive* swerve, std::string path, bool flip)
+    : _swerve(swerve), behaviour::Behaviour("<Follow Path: " + path + ">") {
+  
+  Controls(swerve);
+
   _path = pathplanner::PathPlannerPath::fromPathFile(path);
 
   if (flip) {
@@ -343,9 +346,8 @@ utils::FollowPath::FollowPath(drivetrain::SwerveDrive* swerve, std::string path,
     // i++;
   }
 
-  CalcTimer();
 
-  _timer.Start();
+  _pathName = path;
 
   // _swerve->TurnToAngle(_poses[_currentPose].Rotation().Radians());
 }
@@ -361,11 +363,19 @@ void utils::FollowPath::CalcTimer() {
 
   _timer.Stop();
   _timer.Reset();
-  // _time = units::second_t{std::abs(dist.value()) / 0.7 /*meters per second ?No?*/};
+  // _time = units::second_t{std::abs(dist.value()) / 3 /*meters per second ?No?*/};
   _time = 15_s;
   _timer.Start();
 }
 
+void utils::FollowPath::OnStart() {
+  _timer.Reset();
+
+  CalcTimer();
+
+  _timer.Start();
+}
+
 void utils::FollowPath::OnTick(units::second_t dt) {
   
   nt::NetworkTableInstance::GetDefault()
@@ -385,6 +395,9 @@ void utils::FollowPath::OnTick(units::second_t dt) {
                _poses[_currentPose]);
   WritePose2NT(nt::NetworkTableInstance::GetDefault().GetTable("pathplanner/currentPose"),
                _swerve->GetPose());
+  nt::NetworkTableInstance::GetDefault().GetTable("pathplanner")->GetEntry("currentPoseNumber").SetInteger(_currentPose);
+    nt::NetworkTableInstance::GetDefault().GetTable("pathplanner")->GetEntry("amtPoses").SetInteger(static_cast<int>(_poses.size()));
+
 
   std::cout << "Following Path" << std::endl;
   
@@ -403,6 +416,16 @@ void utils::FollowPath::OnTick(units::second_t dt) {
       CalcTimer();
     }
   }
+
+  std::string filePath = frc::filesystem::GetDeployDirectory() + "/pathplanner/paths/" + _pathName + ".path";
+
+  std::cout << filePath << std::endl;
+
+  nt::NetworkTableInstance::GetDefault()
+      .GetTable("pathplanner")
+      ->GetEntry("Current path")
+      .SetString(filePath);
+
 }
 
 // AutoBuilder implementation
@@ -480,12 +503,10 @@ void utils::AutoBuilder::SetAuto(std::string path) {
         .SetString(static_cast<std::string>(command["data"].dump()));
 
     if (command["type"] == "path") {
-      auto b = behaviour::make<FollowPath>(_swerve, command["data"]["pathName"], _flip);
-      std::cout << b->GetName() << std::endl;
-      pathplan = pathplan << b;
+      pathplan->Add(behaviour::make<FollowPath>(_swerve, command["data"]["pathName"], _flip));
       pathamt++;
     } else if (command["type"] == "named") {
-      pathplan = pathplan << _commandsList.Run(command["data"]["name"]);
+      pathplan->Add(_commandsList.Run(command["data"]["name"]));
       commandamt++;
     } else if (command["type"] == "parallel") {
       
@@ -505,11 +526,15 @@ void utils::AutoBuilder::SetAuto(std::string path) {
         j++;
       }
       nt::NetworkTableInstance::GetDefault().GetTable("commands")->GetEntry("parallelcommandsamt").SetInteger(j);
-      pathplan = pathplan << nb;
+      pathplan->Add(nb);
     }
     i++;
   }
 
+  for (int i = 0; i < commandamt; i++) {
+    nt::NetworkTableInstance::GetDefault().GetTable("commands/behaviours")->GetEntry(std::to_string(i)).SetString(pathplan->GetQueue()[i]);
+  }
+
   nt::NetworkTableInstance::GetDefault().GetTable("commands")->GetEntry("PathAmt").SetInteger(pathamt);
   nt::NetworkTableInstance::GetDefault().GetTable("commands")->GetEntry("CommandAmt").SetInteger(commandamt);
 
diff --git a/wombat/src/main/include/behaviour/Behaviour.h b/wombat/src/main/include/behaviour/Behaviour.h
index 84ac8689..c1d9ba85 100644
--- a/wombat/src/main/include/behaviour/Behaviour.h
+++ b/wombat/src/main/include/behaviour/Behaviour.h
@@ -182,6 +182,8 @@ class SequentialBehaviour : public Behaviour {
   void OnTick(units::time::second_t dt) override;
   void OnStop() override;
 
+  std::vector<std::string> GetQueue();
+
  protected:
   std::deque<ptr> _queue;
 };
diff --git a/wombat/src/main/include/utils/Pathplanner.h b/wombat/src/main/include/utils/Pathplanner.h
index 3d2fa729..51327848 100644
--- a/wombat/src/main/include/utils/Pathplanner.h
+++ b/wombat/src/main/include/utils/Pathplanner.h
@@ -250,13 +250,14 @@ class PathWeaver {
 
 class FollowPath : public behaviour::Behaviour {
  public:
-  FollowPath(drivetrain::SwerveDrive* swerve, std::string path, bool flip = false,
-             std::string _name = "<Follow Path>");
+  FollowPath(drivetrain::SwerveDrive* swerve, std::string path, bool flip = false);
 
   void OnTick(units::time::second_t dt) override;
 
   void CalcTimer();
 
+  void OnStart() override;
+
  private:
   units::second_t _time;
   frc::Timer _timer;
@@ -296,7 +297,7 @@ class AutoBuilder {
   // wpi::json* _startingPose;
   wpi::json* _commands;
 
-  behaviour::Behaviour::ptr pathplan;
+  std::shared_ptr<behaviour::SequentialBehaviour> pathplan;
 
   std::vector<std::pair<std::string, std::string>> commands;
 };

From da0165367d45451f9105eabcb781015708800d41 Mon Sep 17 00:00:00 2001
From: = <paul.hodges@student.education.wa.edu.au>
Date: Wed, 6 Mar 2024 21:50:19 +0800
Subject: [PATCH 199/207] :skull: :pensive:

---
 src/main/cpp/Auto.cpp                         |  6 ++--
 src/main/cpp/Intake.cpp                       |  1 +
 src/main/cpp/IntakeBehaviour.cpp              | 19 +++++++----
 src/main/cpp/LED.cpp                          |  2 +-
 src/main/cpp/ShooterBehaviour.cpp             | 16 ++++++---
 src/main/deploy/pathplanner/autos/Taxi.auto   | 23 ++++++++-----
 src/main/include/Intake.h                     |  2 +-
 src/main/include/IntakeBehaviour.h            |  3 +-
 src/main/include/Shooter.h                    |  2 +-
 src/main/include/ShooterBehaviour.h           |  1 +
 wombat/src/main/cpp/behaviour/Behaviour.cpp   | 15 ++++++--
 .../src/main/cpp/drivetrain/SwerveDrive.cpp   |  6 ++--
 wombat/src/main/cpp/utils/Pathplanner.cpp     | 34 ++++++++++++-------
 wombat/src/main/include/behaviour/Behaviour.h |  5 ++-
 14 files changed, 91 insertions(+), 44 deletions(-)

diff --git a/src/main/cpp/Auto.cpp b/src/main/cpp/Auto.cpp
index 4f255766..c2bad320 100644
--- a/src/main/cpp/Auto.cpp
+++ b/src/main/cpp/Auto.cpp
@@ -20,10 +20,10 @@ wom::SwerveAutoBuilder* autos::InitCommands(wom::drivetrain::SwerveDrive* _swerv
   wom::AutoCommands c = *new wom::AutoCommands(
       {// {"ArmToSetPoint", [_alphaArm]() { return wom::make<ArmToSetPoint>(_alphaArm, 20_deg); }},
        // {"Shoot", [_shooter]() { return wom::make<AutoShoot>(_shooter); }},
-       {"IntakeNote", [_intake]() { return wom::make<IntakeNote>(_intake)->WithTimeout(1_s); }},
-       {"PassNote", [_intake]() { return wom::make<PassNote>(_intake)->WithTimeout(1_s); }},
+       {"IntakeNote", [_intake]() { return wom::make<IntakeNote>(_intake)/*->Until(wom::make<behaviour::WaitTime>(3_s))*/;}},
+       {"PassNote", [_intake]() { return wom::make<PassNote>(_intake)->Until(wom::make<behaviour::WaitTime>(1_s)); }},
        {"EjectNote", [_intake]() { return wom::make<EjectNote>(_intake)->WithTimeout(1_s); }},
-       {"Shoot", [_shooter, _intake]() { return wom::make<AutoShooter>(_shooter, _intake, 300_rad_per_s)->WithTimeout(2.5_s); }} 
+       {"Shoot", [_shooter, _intake]() { return wom::make<AutoShooter>(_shooter, _intake, 300_rad_per_s)->Until(wom::make<wom::WaitTime>(2_s)); }} 
       });
 
   return new wom::utils::SwerveAutoBuilder(_swerveDrive, "Taxi", c);
diff --git a/src/main/cpp/Intake.cpp b/src/main/cpp/Intake.cpp
index e76dbe2b..d3c6cce1 100644
--- a/src/main/cpp/Intake.cpp
+++ b/src/main/cpp/Intake.cpp
@@ -3,6 +3,7 @@
 // of the MIT License at the root of this project
 
 #include "Intake.h"
+#include "frc/RobotController.h"
 
 Intake::Intake(IntakeConfig config) : _config(config), _pid(frc::PIDController (0.0125, 0, 0, 0.05_s)), _pidPosition(frc::PIDController (1, 0, 0, 0.05_s)) {}
 
diff --git a/src/main/cpp/IntakeBehaviour.cpp b/src/main/cpp/IntakeBehaviour.cpp
index d9f78be3..fe72079a 100644
--- a/src/main/cpp/IntakeBehaviour.cpp
+++ b/src/main/cpp/IntakeBehaviour.cpp
@@ -75,21 +75,26 @@ IntakeNote::IntakeNote(Intake* intake) : _intake(intake), behaviour::Behaviour("
   Controls(intake);
 }
 
+void IntakeNote::OnStart() {
+  _timer.Reset();
+  _timer.Start();
+}
+
 void IntakeNote::OnTick(units::second_t dt) {
   _intake->SetState(IntakeState::kIntake);
 
-  nt::NetworkTableInstance::GetDefault().GetTable("AutoIntake")->GetEntry("running").SetBoolean(true);
+  // nt::NetworkTableInstance::GetDefault().GetTable("AutoIntake")->GetEntry("running").SetBoolean(true);
 
-  if (_intake->GetState() == IntakeState::kHold || _intake->GetState() == IntakeState::kIdle) {
-    _intake->SetState(IntakeState::kIdle);
-    std::cerr << "EXITING" << std::endl;
+  // if (_intake->GetState() == IntakeState::kHold || _intake->GetState() == IntakeState::kIdle || _timer.Get() > 3_s) {
+    // _intake->SetState(IntakeState::kIdle);
+    // std::cerr << "EXITING" << std::endl;
     SetDone();
-  }
+  // }
 }
 
 void IntakeNote::OnStop() {
-  _intake->SetState(IntakeState::kIdle);
-    nt::NetworkTableInstance::GetDefault().GetTable("AutoIntake")->GetEntry("running").SetBoolean(false);
+  // _intake->SetState(IntakeState::kIdle);
+    // nt::NetworkTableInstance::GetDefault().GetTable("AutoIntake")->GetEntry("running").SetBoolean(false);
 }
 
 PassNote::PassNote(Intake* intake) : _intake(intake) {
diff --git a/src/main/cpp/LED.cpp b/src/main/cpp/LED.cpp
index 579d4d6b..a6de4524 100644
--- a/src/main/cpp/LED.cpp
+++ b/src/main/cpp/LED.cpp
@@ -13,7 +13,7 @@ void LED::OnUpdate(units::second_t dt) {
     } break;
 
     case LEDState::kAiming: {
-      _led.frc::PWM::SetSpeed(0.27);
+      _led.frc::PWM::SetSpeed(0.21);
     } break;
     case LEDState::kAmpReady: {
       _led.frc::PWM::SetSpeed(0.87);
diff --git a/src/main/cpp/ShooterBehaviour.cpp b/src/main/cpp/ShooterBehaviour.cpp
index b4da99d8..336d1034 100644
--- a/src/main/cpp/ShooterBehaviour.cpp
+++ b/src/main/cpp/ShooterBehaviour.cpp
@@ -57,26 +57,29 @@ void ShooterManualControl::OnTick(units::second_t dt) {
   }
 }
 
-AutoShooter::AutoShooter(Shooter* shooter, Intake* intake, units::radians_per_second_t goal) : _shooter(shooter), _intake(intake), _goal(goal), behaviour::Behaviour("<Shoot>") {
+AutoShooter::AutoShooter(Shooter* shooter, Intake* intake, units::radians_per_second_t goal) : behaviour::Behaviour("<Shoot>"), _shooter(shooter), _intake(intake), _goal(goal) {
   Controls(shooter);
 
 }
 
 void AutoShooter::OnTick(units::second_t dt) {
   if (!_timer_started) {
+    _timer.Reset();
+    _timer.Restart();
     _timer.Start();
+
     _timer_started = true;
   }
 
   _shooter->SetState(ShooterState::kSpinUp);
-  _intake->SetState(IntakeState::kHold);
+  // _intake->SetState(IntakeState::kIntake);
 
   _shooter->SetPidGoal(_goal);
 
-  if (_timer.Get() > 1.5_s) {
+  if (_timer.Get() > 5_s) {
     _intake->SetState(IntakeState::kPass);
 
-    if (_timer.Get() > 2_s) {
+    if (_timer.Get() > 10_s) {
       _intake->SetState(IntakeState::kIdle);
       _shooter->SetState(ShooterState::kIdle);
 
@@ -84,3 +87,8 @@ void AutoShooter::OnTick(units::second_t dt) {
     }
   }
 }
+
+void AutoShooter::OnStop() {
+  _shooter->SetState(ShooterState::kIdle);
+  _intake->SetState(IntakeState::kIdle);
+}
diff --git a/src/main/deploy/pathplanner/autos/Taxi.auto b/src/main/deploy/pathplanner/autos/Taxi.auto
index 69a5d8de..bd6319f4 100644
--- a/src/main/deploy/pathplanner/autos/Taxi.auto
+++ b/src/main/deploy/pathplanner/autos/Taxi.auto
@@ -12,15 +12,22 @@
           }
         },
         {
-          "type": "path",
-          "data": {
-            "pathName": "FirstNote"
-          }
-        },
-        {
-          "type": "named",
+          "type": "parallel",
           "data": {
-            "name": "IntakeNote"
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "FirstNote"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "IntakeNote"
+                }
+              }
+            ]
           }
         },
         {
diff --git a/src/main/include/Intake.h b/src/main/include/Intake.h
index 87169e93..b2c1efea 100644
--- a/src/main/include/Intake.h
+++ b/src/main/include/Intake.h
@@ -22,7 +22,7 @@ struct IntakeConfig {
   wom::PIDConfig<units::radians_per_second, units::volt> pidConfig;
 };
 
-enum class IntakeState { kIdle, kRaw, kHold, kEject, kIntake, kPass};
+enum class IntakeState { kIdle, kRaw, kHold, kEject, kIntake, kPass };
 
 class Intake : public behaviour::HasBehaviour {
  public:
diff --git a/src/main/include/IntakeBehaviour.h b/src/main/include/IntakeBehaviour.h
index e7700f7f..e5884e55 100644
--- a/src/main/include/IntakeBehaviour.h
+++ b/src/main/include/IntakeBehaviour.h
@@ -37,10 +37,11 @@ class IntakeNote : public behaviour::Behaviour {
   explicit IntakeNote(Intake* intake);
   void OnTick(units::second_t dt) override;
   void OnStop() override;
+  void OnStart() override;
 
  private:
   Intake* _intake;
-
+  frc::Timer _timer;
 };
 
 class PassNote : public behaviour::Behaviour {
diff --git a/src/main/include/Shooter.h b/src/main/include/Shooter.h
index fa022180..b2a2ce9c 100644
--- a/src/main/include/Shooter.h
+++ b/src/main/include/Shooter.h
@@ -36,7 +36,7 @@ class Shooter : public behaviour::HasBehaviour {
  private:
   ShooterConfig _config;
   std::shared_ptr<nt::NetworkTable> table = nt::NetworkTableInstance::GetDefault().GetTable("Shooter");
-  ShooterState _state = ShooterState::kRaw;
+  ShooterState _state = ShooterState::kIdle;
   units::volt_t _rawVoltage;
   units::radians_per_second_t _goal;
   units::volt_t _setVoltage = 0_V;
diff --git a/src/main/include/ShooterBehaviour.h b/src/main/include/ShooterBehaviour.h
index 5e182772..5c6880b3 100644
--- a/src/main/include/ShooterBehaviour.h
+++ b/src/main/include/ShooterBehaviour.h
@@ -37,6 +37,7 @@ class AutoShooter : public behaviour::Behaviour {
   AutoShooter(Shooter* shooter, Intake* intake, units::radians_per_second_t goal);
 
   void OnTick(units::second_t dt) override;
+  void OnStop() override;
 
  private:
   Shooter* _shooter;
diff --git a/wombat/src/main/cpp/behaviour/Behaviour.cpp b/wombat/src/main/cpp/behaviour/Behaviour.cpp
index ce516f1e..ae7ce73d 100644
--- a/wombat/src/main/cpp/behaviour/Behaviour.cpp
+++ b/wombat/src/main/cpp/behaviour/Behaviour.cpp
@@ -129,7 +129,8 @@ void SequentialBehaviour::OnTick(units::time::second_t dt) {
     SetPeriod(_queue.front()->GetPeriod());
     _queue.front()->Tick();
     if (_queue.front()->IsFinished()) {
-      _queue.pop_front();
+      // _queue.pop_front();
+      _queue.erase(_queue.begin());
       if (_queue.empty())
         SetDone();
       else
@@ -144,7 +145,8 @@ void SequentialBehaviour::OnStop() {
   if (GetBehaviourState() != BehaviourState::DONE) {
     while (!_queue.empty()) {
       _queue.front()->Interrupt();
-      _queue.pop_front();
+      // _queue.pop_front();
+      _queue.erase(_queue.begin());
     }
   }
 }
@@ -240,6 +242,15 @@ void ConcurrentBehaviour::OnStop() {
   }
 }
 
+std::vector<std::string> ConcurrentBehaviour::GetQueue() {
+  std::vector<std::string> vec;
+
+  for (auto beh : _children) {
+    vec.push_back(beh->GetName());
+  }
+
+  return vec;
+}
 // If
 If::If(std::function<bool()> condition) : _condition(condition) {}
 If::If(bool v) : _condition([v]() { return v; }) {}
diff --git a/wombat/src/main/cpp/drivetrain/SwerveDrive.cpp b/wombat/src/main/cpp/drivetrain/SwerveDrive.cpp
index 0ab2f32d..26e0aafc 100644
--- a/wombat/src/main/cpp/drivetrain/SwerveDrive.cpp
+++ b/wombat/src/main/cpp/drivetrain/SwerveDrive.cpp
@@ -695,8 +695,8 @@ void SwerveDrive::SetPose(frc::Pose2d pose) {
 }
 
 bool SwerveDrive::IsAtSetPose() {
-  if (std::abs(_xPIDController.GetPositionError()) < 0.1 && std::abs(_yPIDController.GetPositionError()) < 0.1 && std::abs(_turnPIDController.GetPositionError()) < 0.1) {
-    if (std::abs(_xPIDController.GetVelocityError()) < 1 && std::abs(_yPIDController.GetVelocityError()) < 1 && std::abs(_turnPIDController.GetVelocityError()) < 1) {
+  if (std::abs(_xPIDController.GetPositionError()) < 0.1 && std::abs(_yPIDController.GetPositionError()) < 0.1) {
+    if (std::abs(_xPIDController.GetVelocityError()) < 1 && std::abs(_yPIDController.GetVelocityError()) < 1) {
       return false;
     }
   }
@@ -723,7 +723,7 @@ void SwerveDrive::ResetPose(frc::Pose2d pose) {
 }
 
 frc::Pose2d SwerveDrive::GetPose() {
-  return _poseEstimator.GetEstimatedPosition();
+  return frc::Pose2d(_poseEstimator.GetEstimatedPosition().X(), _poseEstimator.GetEstimatedPosition().Y(), _poseEstimator.GetEstimatedPosition().Rotation());
   // return frc::Pose2d(1_m, 1_m, 0_deg);
 }
 
diff --git a/wombat/src/main/cpp/utils/Pathplanner.cpp b/wombat/src/main/cpp/utils/Pathplanner.cpp
index 4585cd59..1cd44b07 100644
--- a/wombat/src/main/cpp/utils/Pathplanner.cpp
+++ b/wombat/src/main/cpp/utils/Pathplanner.cpp
@@ -295,7 +295,7 @@ utils::FollowPath::FollowPath(drivetrain::SwerveDrive* swerve, std::string path,
     frc::Translation2d tr = frc::Translation2d(point.position.X() * -1, point.position.Y() * -1);
     frc::Pose2d pose2 = frc::Pose2d(tr, rot);//.TransformBy(frc::Transform2d(-1.37_m, -5.56_m, 0_deg));
     //
-    if ( /*i == index ||*/ i == tot || f) {
+    if (i == index || i == static_cast<int>(tot - 1) || f) {
       _poses.emplace_back(frc::Pose2d(pose2.X(), pose2.Y(), pose2.Rotation()));
       // _poses.emplace_back(frc::Pose2d(pose2.Y(), pose2.X(), pose2.Rotation()));
       i = 0;//  - 5.56_m,  - 2.91_m
@@ -363,8 +363,8 @@ void utils::FollowPath::CalcTimer() {
 
   _timer.Stop();
   _timer.Reset();
-  // _time = units::second_t{std::abs(dist.value()) / 3 /*meters per second ?No?*/};
-  _time = 15_s;
+  _time = units::second_t{std::abs(dist.value()) / 1 /*meters per second ?No?*/};
+  // _time = 15_s;
   _timer.Start();
 }
 
@@ -405,7 +405,7 @@ void utils::FollowPath::OnTick(units::second_t dt) {
     _swerve->SetPose(_poses[_currentPose]);
   // }
   /*else*/ if (_swerve->IsAtSetPose() || _timer.Get() > _time) {
-    if (_currentPose + 1 == static_cast<int>(_poses.size())) {
+    if (_currentPose == static_cast<int>(_poses.size())) {
       // _swerve->MakeAtSetPoint();
       // _swerve->SetVelocity(frc::ChassisSpeeds());
       // _swerve->MakeAtSetPoint();
@@ -486,7 +486,7 @@ void utils::AutoBuilder::SetAuto(std::string path) {
 
   nt::NetworkTableInstance::GetDefault().GetTable("commands")->GetEntry("length").SetInteger(commands.size());
 
-  pathplan = behaviour::make<behaviour::SequentialBehaviour>();
+  auto _pathplan = behaviour::make<behaviour::SequentialBehaviour>();
 
   int i = 0;
   int pathamt = 0;
@@ -503,14 +503,16 @@ void utils::AutoBuilder::SetAuto(std::string path) {
         .SetString(static_cast<std::string>(command["data"].dump()));
 
     if (command["type"] == "path") {
-      pathplan->Add(behaviour::make<FollowPath>(_swerve, command["data"]["pathName"], _flip));
+      _pathplan->Add(behaviour::make<FollowPath>(_swerve, command["data"]["pathName"], _flip));
       pathamt++;
+          nt::NetworkTableInstance::GetDefault().GetTable("commands/" + std::to_string(i))->GetEntry("behaviours").SetStringArray(_pathplan->GetQueue());
     } else if (command["type"] == "named") {
-      pathplan->Add(_commandsList.Run(command["data"]["name"]));
+      _pathplan->Add(_commandsList.Run(command["data"]["name"]));
       commandamt++;
+          nt::NetworkTableInstance::GetDefault().GetTable("commands/" + std::to_string(i))->GetEntry("behaviours").SetStringArray(_pathplan->GetQueue());
     } else if (command["type"] == "parallel") {
       
-      auto nb = behaviour::make<behaviour::ConcurrentBehaviour>(behaviour::ConcurrentBehaviourReducer::FIRST);
+      auto nb = behaviour::make<behaviour::ConcurrentBehaviour>(behaviour::ConcurrentBehaviourReducer::ANY);
       int j = 0;
       for (auto c : command["data"]["commands"]) { 
         nt::NetworkTableInstance::GetDefault().GetTable("commands/parallel/" + std::to_string(j))->GetEntry("type").SetString(static_cast<std::string>(c["type"]));
@@ -523,17 +525,25 @@ void utils::AutoBuilder::SetAuto(std::string path) {
           nb->Add(_commandsList.Run(c["data"]["name"]));
           commandamt++;
         }
+        nb->Add(behaviour::make<behaviour::Print>("ok"));
         j++;
       }
       nt::NetworkTableInstance::GetDefault().GetTable("commands")->GetEntry("parallelcommandsamt").SetInteger(j);
-      pathplan->Add(nb);
+      nt::NetworkTableInstance::GetDefault().GetTable("commands")->GetEntry("parallel-" + std::to_string(i)).SetStringArray(nb->GetQueue());
+      _pathplan->Add(nb);
     }
+
+    _pathplan->Add(behaviour::make<behaviour::Print>("idk"));
+
+    pathplan = _pathplan;
+    nt::NetworkTableInstance::GetDefault().GetTable("commands/" + std::to_string(i))->GetEntry("currentbehaviours").SetStringArray(pathplan->GetQueue());
     i++;
   }
 
-  for (int i = 0; i < commandamt; i++) {
-    nt::NetworkTableInstance::GetDefault().GetTable("commands/behaviours")->GetEntry(std::to_string(i)).SetString(pathplan->GetQueue()[i]);
-  }
+  // pathplan->Add(behaviour::make<behaviour::Print>("test"));
+  //   pathplan->Add(behaviour::make<behaviour::Print>("test"));
+
+    nt::NetworkTableInstance::GetDefault().GetTable("commands/newbehaviours")->GetEntry(std::to_string(i)).SetStringArray(pathplan->GetQueue());
 
   nt::NetworkTableInstance::GetDefault().GetTable("commands")->GetEntry("PathAmt").SetInteger(pathamt);
   nt::NetworkTableInstance::GetDefault().GetTable("commands")->GetEntry("CommandAmt").SetInteger(commandamt);
diff --git a/wombat/src/main/include/behaviour/Behaviour.h b/wombat/src/main/include/behaviour/Behaviour.h
index c1d9ba85..3ae2d75d 100644
--- a/wombat/src/main/include/behaviour/Behaviour.h
+++ b/wombat/src/main/include/behaviour/Behaviour.h
@@ -185,7 +185,8 @@ class SequentialBehaviour : public Behaviour {
   std::vector<std::string> GetQueue();
 
  protected:
-  std::deque<ptr> _queue;
+  // std::deque<ptr> _queue;
+  std::vector<ptr> _queue;
 };
 
 inline std::shared_ptr<SequentialBehaviour> operator<<(Behaviour::ptr a, Behaviour::ptr b) {
@@ -229,6 +230,8 @@ class ConcurrentBehaviour : public Behaviour {
   void OnTick(units::time::second_t dt) override;
   void OnStop() override;
 
+  std::vector<std::string> GetQueue();
+
  private:
   ConcurrentBehaviourReducer _reducer;
   std::vector<std::shared_ptr<Behaviour>> _children;

From 977bcacf0549560c14d7f54c0eaae2f2d09a4e7b Mon Sep 17 00:00:00 2001
From: = <paul.hodges@student.education.wa.edu.au>
Date: Wed, 6 Mar 2024 21:56:21 +0800
Subject: [PATCH 200/207] :pensive: :skull:

---
 wombat/src/main/cpp/utils/Pathplanner.cpp | 4 ++++
 1 file changed, 4 insertions(+)

diff --git a/wombat/src/main/cpp/utils/Pathplanner.cpp b/wombat/src/main/cpp/utils/Pathplanner.cpp
index 1cd44b07..57f257fb 100644
--- a/wombat/src/main/cpp/utils/Pathplanner.cpp
+++ b/wombat/src/main/cpp/utils/Pathplanner.cpp
@@ -35,6 +35,7 @@
 #include "pathplanner/lib/path/PathPlannerPath.h"
 #include "pathplanner/lib/path/PathPoint.h"
 #include "pathplanner/lib/path/RotationTarget.h"
+#include "units/acceleration.h"
 #include "units/time.h"
 #include "utils/Util.h"
 #include "wpi/detail/conversions/to_json.h"
@@ -548,6 +549,9 @@ void utils::AutoBuilder::SetAuto(std::string path) {
   nt::NetworkTableInstance::GetDefault().GetTable("commands")->GetEntry("PathAmt").SetInteger(pathamt);
   nt::NetworkTableInstance::GetDefault().GetTable("commands")->GetEntry("CommandAmt").SetInteger(commandamt);
 
+  _swerve->SetAccelerationLimit(units::meters_per_second_squared_t{2});
+  _swerve->SetVoltageLimit(6_V);
+
   // WritePose2NT(nt::NetworkTableInstance::GetDefault().GetTable("startPose"),
   //              JSONPoseToPose2d(*_startingPose));
 

From 95136e25235b7ab9acb826875c61989098b9d0f5 Mon Sep 17 00:00:00 2001
From: = <paul.hodges@student.education.wa.edu.au>
Date: Wed, 6 Mar 2024 21:57:15 +0800
Subject: [PATCH 201/207] Added acceleration limits to swerve drive auto

---
 wombat/src/main/cpp/utils/Pathplanner.cpp | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/wombat/src/main/cpp/utils/Pathplanner.cpp b/wombat/src/main/cpp/utils/Pathplanner.cpp
index 57f257fb..5c13877a 100644
--- a/wombat/src/main/cpp/utils/Pathplanner.cpp
+++ b/wombat/src/main/cpp/utils/Pathplanner.cpp
@@ -552,6 +552,8 @@ void utils::AutoBuilder::SetAuto(std::string path) {
   _swerve->SetAccelerationLimit(units::meters_per_second_squared_t{2});
   _swerve->SetVoltageLimit(6_V);
 
+
+
   // WritePose2NT(nt::NetworkTableInstance::GetDefault().GetTable("startPose"),
   //              JSONPoseToPose2d(*_startingPose));
 

From baec689d7ae682cadd827d2a34e4556ca3b51a49 Mon Sep 17 00:00:00 2001
From: = <paul.hodges@student.education.wa.edu.au>
Date: Fri, 8 Mar 2024 17:25:19 +0800
Subject: [PATCH 202/207] make two note auto work

---
 src/main/cpp/Auto.cpp                         |  9 ++++
 src/main/cpp/Intake.cpp                       | 13 +++++
 src/main/cpp/ShooterBehaviour.cpp             |  5 +-
 .../pathplanner/autos/FourNoteTaxi.auto       |  4 +-
 src/main/deploy/pathplanner/autos/Taxi.auto   |  2 +-
 .../pathplanner/autos/ThreeNoteTaxi.auto      |  2 +-
 .../pathplanner/paths/BackwardsWorse.path     | 49 +++++++++++++++++
 .../deploy/pathplanner/paths/FirstNote.path   |  4 +-
 .../pathplanner/paths/FirstNoteGood.path      | 52 +++++++++++++++++++
 src/main/include/Auto.h                       |  2 +
 src/main/include/Intake.h                     |  3 ++
 src/main/include/RobotMap.h                   |  4 +-
 .../src/main/cpp/drivetrain/SwerveDrive.cpp   |  6 +--
 13 files changed, 141 insertions(+), 14 deletions(-)
 create mode 100644 src/main/deploy/pathplanner/paths/BackwardsWorse.path
 create mode 100644 src/main/deploy/pathplanner/paths/FirstNoteGood.path

diff --git a/src/main/cpp/Auto.cpp b/src/main/cpp/Auto.cpp
index c2bad320..d307cb28 100644
--- a/src/main/cpp/Auto.cpp
+++ b/src/main/cpp/Auto.cpp
@@ -40,6 +40,15 @@ std::shared_ptr<behaviour::Behaviour> autos::OneNoteTaxi(wom::SwerveAutoBuilder*
   return builder->GetAutoRoutine("OneNoteTaxi");
 }
 
+std::shared_ptr<behaviour::Behaviour> autos::FourNoteTaxi(wom::SwerveAutoBuilder* builder) {
+  return builder->GetAutoRoutine("FourNoteTaxi");
+}
+
+std::shared_ptr<behaviour::Behaviour> autos::ThreeNoteTaxi(wom::SwerveAutoBuilder* builder) {
+  return builder->GetAutoRoutine("ThreeNoteTaxi");
+}
+
+
 // std::shared_ptr<behaviour::Behaviour> autos::QuadrupleClose(wom::drivetrain::SwerveDrive* _swerveDrive,
 //                                                             Shooter* _shooter, Intake* _intake,
 //                                                             AlphaArm* _alphaArm) {
diff --git a/src/main/cpp/Intake.cpp b/src/main/cpp/Intake.cpp
index d3c6cce1..ccca1f65 100644
--- a/src/main/cpp/Intake.cpp
+++ b/src/main/cpp/Intake.cpp
@@ -3,7 +3,9 @@
 // of the MIT License at the root of this project
 
 #include "Intake.h"
+#include <algorithm>
 #include "frc/RobotController.h"
+#include "units/base.h"
 
 Intake::Intake(IntakeConfig config) : _config(config), _pid(frc::PIDController (0.0125, 0, 0, 0.05_s)), _pidPosition(frc::PIDController (1, 0, 0, 0.05_s)) {}
 
@@ -13,6 +15,8 @@ IntakeConfig Intake::GetConfig() {
 
 void Intake::OnStart() {
   _pid.Reset();
+
+  _timer.Start();
 }
 
 void Intake::OnUpdate(units::second_t dt) {
@@ -112,6 +116,15 @@ void Intake::OnUpdate(units::second_t dt) {
   _table->GetEntry("Encoder").SetDouble(_config.IntakeGearbox.encoder->GetEncoderPosition().value());
   _table->GetEntry("Shot Count").SetDouble(_noteShot);
   // _table->GetEntry("Encoder: ").SetDouble(_config.IntakeGearbox.encoder->GetEncoderPosition().value());
+  //
+  if (_timer.Get() < 4_s) {
+    _setVoltage = units::math::min(0_V, _setVoltage);
+  } else {
+    _timer.Stop();
+    _timer.Reset();
+    // _timer.Restart();
+    // _timer.Start();
+  }
 
   _config.IntakeGearbox.motorController->SetVoltage(_setVoltage);
 }
diff --git a/src/main/cpp/ShooterBehaviour.cpp b/src/main/cpp/ShooterBehaviour.cpp
index 336d1034..91cd64d5 100644
--- a/src/main/cpp/ShooterBehaviour.cpp
+++ b/src/main/cpp/ShooterBehaviour.cpp
@@ -59,7 +59,6 @@ void ShooterManualControl::OnTick(units::second_t dt) {
 
 AutoShooter::AutoShooter(Shooter* shooter, Intake* intake, units::radians_per_second_t goal) : behaviour::Behaviour("<Shoot>"), _shooter(shooter), _intake(intake), _goal(goal) {
   Controls(shooter);
-
 }
 
 void AutoShooter::OnTick(units::second_t dt) {
@@ -76,10 +75,10 @@ void AutoShooter::OnTick(units::second_t dt) {
 
   _shooter->SetPidGoal(_goal);
 
-  if (_timer.Get() > 5_s) {
+  if (_timer.Get() > 1_s) {
     _intake->SetState(IntakeState::kPass);
 
-    if (_timer.Get() > 10_s) {
+    if (_timer.Get() > 1.9_s) {
       _intake->SetState(IntakeState::kIdle);
       _shooter->SetState(ShooterState::kIdle);
 
diff --git a/src/main/deploy/pathplanner/autos/FourNoteTaxi.auto b/src/main/deploy/pathplanner/autos/FourNoteTaxi.auto
index 2cd1b37a..b2a19a5b 100644
--- a/src/main/deploy/pathplanner/autos/FourNoteTaxi.auto
+++ b/src/main/deploy/pathplanner/autos/FourNoteTaxi.auto
@@ -20,7 +20,7 @@
         {
           "type": "path",
           "data": {
-            "pathName": "FirstNote"
+            "pathName": "FirstNoteGood"
           }
         },
         {
@@ -92,7 +92,7 @@
         {
           "type": "path",
           "data": {
-            "pathName": "FirstNote"
+            "pathName": "FirstNoteGood"
           }
         }
       ]
diff --git a/src/main/deploy/pathplanner/autos/Taxi.auto b/src/main/deploy/pathplanner/autos/Taxi.auto
index bd6319f4..0976f300 100644
--- a/src/main/deploy/pathplanner/autos/Taxi.auto
+++ b/src/main/deploy/pathplanner/autos/Taxi.auto
@@ -33,7 +33,7 @@
         {
           "type": "path",
           "data": {
-            "pathName": "Backwards"
+            "pathName": "BackwardsWorse"
           }
         },
         {
diff --git a/src/main/deploy/pathplanner/autos/ThreeNoteTaxi.auto b/src/main/deploy/pathplanner/autos/ThreeNoteTaxi.auto
index 6159ba17..72836db4 100644
--- a/src/main/deploy/pathplanner/autos/ThreeNoteTaxi.auto
+++ b/src/main/deploy/pathplanner/autos/ThreeNoteTaxi.auto
@@ -20,7 +20,7 @@
         {
           "type": "path",
           "data": {
-            "pathName": "FirstNote"
+            "pathName": "FirstNoteGood"
           }
         },
         {
diff --git a/src/main/deploy/pathplanner/paths/BackwardsWorse.path b/src/main/deploy/pathplanner/paths/BackwardsWorse.path
new file mode 100644
index 00000000..0c86263a
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/BackwardsWorse.path
@@ -0,0 +1,49 @@
+{
+  "version": 1.0,
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 1.5,
+        "y": 0.0
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 1.493173655558508,
+        "y": 0.020888794958252705
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 0.0,
+        "y": 0.0
+      },
+      "prevControl": {
+        "x": -1.0,
+        "y": 0.0
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 1.0,
+    "maxAcceleration": 1.0,
+    "maxAngularVelocity": 100.0,
+    "maxAngularAcceleration": 150.0
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 0,
+    "rotateFast": false
+  },
+  "reversed": false,
+  "folder": null,
+  "previewStartingState": null,
+  "useDefaultConstraints": false
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/FirstNote.path b/src/main/deploy/pathplanner/paths/FirstNote.path
index 596b7dcf..58409395 100644
--- a/src/main/deploy/pathplanner/paths/FirstNote.path
+++ b/src/main/deploy/pathplanner/paths/FirstNote.path
@@ -16,11 +16,11 @@
     },
     {
       "anchor": {
-        "x": 2.0,
+        "x": 1.5,
         "y": -0.0
       },
       "prevControl": {
-        "x": 1.0,
+        "x": 0.5,
         "y": 0.0
       },
       "nextControl": null,
diff --git a/src/main/deploy/pathplanner/paths/FirstNoteGood.path b/src/main/deploy/pathplanner/paths/FirstNoteGood.path
new file mode 100644
index 00000000..8a27b13b
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/FirstNoteGood.path
@@ -0,0 +1,52 @@
+{
+  "version": 1.0,
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 1.325730468864645,
+        "y": 5.5040931765173085
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 2.325730468864644,
+        "y": 5.5040931765173085
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 2.918404239090931,
+        "y": 5.5040931765173085
+      },
+      "prevControl": {
+        "x": 1.9184042390909308,
+        "y": 5.5040931765173085
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 1.0,
+    "maxAcceleration": 1.5,
+    "maxAngularVelocity": 100.0,
+    "maxAngularAcceleration": 150.0
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 0.0,
+    "rotateFast": false
+  },
+  "reversed": false,
+  "folder": null,
+  "previewStartingState": {
+    "rotation": 0,
+    "velocity": 0
+  },
+  "useDefaultConstraints": false
+}
\ No newline at end of file
diff --git a/src/main/include/Auto.h b/src/main/include/Auto.h
index 3133e715..93fe9b69 100644
--- a/src/main/include/Auto.h
+++ b/src/main/include/Auto.h
@@ -25,6 +25,8 @@ std::shared_ptr<behaviour::Behaviour> AutoTest(wom::drivetrain::SwerveDrive* _sw
 
 std::shared_ptr<behaviour::Behaviour> Taxi(wom::utils::SwerveAutoBuilder* builder);
 std::shared_ptr<behaviour::Behaviour> OneNoteTaxi(wom::utils::SwerveAutoBuilder* builder);
+std::shared_ptr<behaviour::Behaviour> FourNoteTaxi(wom::utils::SwerveAutoBuilder* builder);
+std::shared_ptr<behaviour::Behaviour> ThreeNoteTaxi(wom::SwerveAutoBuilder* builder);
 
 std::shared_ptr<behaviour::Behaviour> QuadrupleClose(wom::drivetrain::SwerveDrive* _swerveDrive,
                                                      Shooter* _shooter, Intake* _intake, AlphaArm* _alphaArm);
diff --git a/src/main/include/Intake.h b/src/main/include/Intake.h
index b2c1efea..4765ccb9 100644
--- a/src/main/include/Intake.h
+++ b/src/main/include/Intake.h
@@ -12,6 +12,7 @@
 #include <string>
 
 #include "Wombat.h"
+#include "frc/Timer.h"
 
 struct IntakeConfig {
   std::string path;
@@ -53,4 +54,6 @@ class Intake : public behaviour::HasBehaviour {
   frc::PIDController _pidPosition;
 
   std::shared_ptr<nt::NetworkTable> _table = nt::NetworkTableInstance::GetDefault().GetTable("Intake");
+
+  frc::Timer _timer;
 };
diff --git a/src/main/include/RobotMap.h b/src/main/include/RobotMap.h
index 007b08dd..91ea79ad 100644
--- a/src/main/include/RobotMap.h
+++ b/src/main/include/RobotMap.h
@@ -66,8 +66,8 @@ struct RobotMap {
   struct IntakeSystem {
     rev::CANSparkMax intakeMotor{35, rev::CANSparkMax::MotorType::kBrushless};
     wom::CANSparkMaxEncoder intakeEncoder{&intakeMotor, 0.1_m};
-    frc::DigitalInput intakeSensor{5};
-    frc::DigitalInput passSensor{7};
+    frc::DigitalInput intakeSensor{7};
+    frc::DigitalInput passSensor{5};
     // frc::DigitalInput magSensor{0};
     // frc::DigitalInput shooterSensor{0};
 
diff --git a/wombat/src/main/cpp/drivetrain/SwerveDrive.cpp b/wombat/src/main/cpp/drivetrain/SwerveDrive.cpp
index 26e0aafc..382a7c49 100644
--- a/wombat/src/main/cpp/drivetrain/SwerveDrive.cpp
+++ b/wombat/src/main/cpp/drivetrain/SwerveDrive.cpp
@@ -717,9 +717,9 @@ SwerveDriveState SwerveDrive::GetState() {
 void SwerveDrive::ResetPose(frc::Pose2d pose) {
   _poseEstimator.ResetPosition(
       _config.gyro->GetRotation2d()/* - 1_rad*/,
-      wpi::array<frc::SwerveModulePosition, 4>{_modules[0].GetPosition(), _modules[1].GetPosition(),
-                                               _modules[2].GetPosition(), _modules[3].GetPosition()},
-      pose);
+      wpi::array<frc::SwerveModulePosition, 4>{_modules[3].GetPosition(), _modules[0].GetPosition(),
+                                               _modules[1].GetPosition(), _modules[2].GetPosition()},
+      frc::Pose2d(0_m, 0_m, 0_deg));
 }
 
 frc::Pose2d SwerveDrive::GetPose() {

From cd66c22cff5bd7b4de3378a9acadf3034f604189 Mon Sep 17 00:00:00 2001
From: = <paul.hodges@student.education.wa.edu.au>
Date: Fri, 8 Mar 2024 18:09:16 +0800
Subject: [PATCH 203/207] make two note auto work

---
 .pathplanner/settings.json                    |   2 +-
 src/main/cpp/AlphaArm.cpp                     | 131 +++++++++---------
 src/main/cpp/AlphaArmBehaviour.cpp            |  20 ++-
 src/main/cpp/Auto.cpp                         |  15 +-
 src/main/cpp/Climber.cpp                      |  14 +-
 src/main/cpp/ClimberBehaviour.cpp             |   2 +-
 src/main/cpp/Intake.cpp                       |  56 ++++----
 src/main/cpp/IntakeBehaviour.cpp              |  22 ++-
 src/main/cpp/Robot.cpp                        |  20 ++-
 .../deploy/pathplanner/autos/FourNote.auto    |   2 +-
 .../deploy/pathplanner/autos/FourNoteFar.auto |   2 +-
 .../pathplanner/autos/FourNoteTaxi.auto       |   2 +-
 .../deploy/pathplanner/autos/LeftOneNote.auto |   2 +-
 .../deploy/pathplanner/autos/OneNote.auto     |   2 +-
 .../deploy/pathplanner/autos/OneNoteTaxi.auto |   2 +-
 src/main/deploy/pathplanner/autos/Taxi.auto   |   2 +-
 .../deploy/pathplanner/autos/ThreeNote.auto   |   2 +-
 .../pathplanner/autos/ThreeNoteFar.auto       |   2 +-
 .../pathplanner/autos/ThreeNoteTaxi.auto      |   2 +-
 .../deploy/pathplanner/autos/TwoNote.auto     |   2 +-
 .../deploy/pathplanner/autos/TwoNoteTaxi.auto |   2 +-
 .../deploy/pathplanner/paths/Backwards.path   |   2 +-
 .../pathplanner/paths/BackwardsWorse.path     |   4 +-
 .../pathplanner/paths/CenterBackToStart.path  |   2 +-
 .../deploy/pathplanner/paths/CloseLeft.path   |   2 +-
 .../pathplanner/paths/CloseLeftReturn.path    |   2 +-
 .../deploy/pathplanner/paths/CloseRight.path  |   2 +-
 .../pathplanner/paths/CloseRightReturn.path   |   2 +-
 .../deploy/pathplanner/paths/FarLeft.path     |   2 +-
 .../pathplanner/paths/FarLeftCenter.path      |   2 +-
 .../paths/FarLeftCenterReturn.path            |   2 +-
 .../pathplanner/paths/FarLeftReturn.path      |   2 +-
 .../deploy/pathplanner/paths/FirstNote.path   |   4 +-
 .../pathplanner/paths/FirstNoteGood.path      |   2 +-
 .../pathplanner/paths/LeftCloseLeft.path      |   2 +-
 src/main/include/AlphaArm.h                   |  21 ++-
 src/main/include/AlphaArmBehaviour.h          |   7 +-
 src/main/include/Climber.h                    |   3 +-
 src/main/include/IntakeBehaviour.h            |   8 +-
 src/main/include/Robot.h                      |  15 +-
 src/main/include/ShooterBehaviour.h           |   2 +-
 .../src/main/cpp/drivetrain/SwerveDrive.cpp   |  57 ++++----
 .../behaviours/SwerveBehaviours.cpp           |   9 +-
 wombat/src/main/cpp/sim/Sim.cpp               |   3 +-
 wombat/src/main/cpp/utils/Encoder.cpp         |   1 +
 wombat/src/main/cpp/utils/Pathplanner.cpp     |  77 ++++++----
 46 files changed, 276 insertions(+), 265 deletions(-)

diff --git a/.pathplanner/settings.json b/.pathplanner/settings.json
index fd65c978..5cf1fe6a 100644
--- a/.pathplanner/settings.json
+++ b/.pathplanner/settings.json
@@ -9,4 +9,4 @@
   "defaultMaxAngVel": 100.0,
   "defaultMaxAngAccel": 150.0,
   "maxModuleSpeed": 1.0
-}
\ No newline at end of file
+}
diff --git a/src/main/cpp/AlphaArm.cpp b/src/main/cpp/AlphaArm.cpp
index 233fce50..96885b98 100644
--- a/src/main/cpp/AlphaArm.cpp
+++ b/src/main/cpp/AlphaArm.cpp
@@ -4,16 +4,16 @@
 
 #include "AlphaArm.h"
 
-
-AlphaArm::AlphaArm(AlphaArmConfig *config/*, frc::Rotation2d initialAngle, wom::vision::Limelight* vision */) : _config(config), _pidArm{frc::PIDController(1.2, 0.4, 0)}, _pidArmStates{frc::PIDController(37, 0.00070, 0.15)}, _pidIntakeState{frc::PIDController(30, 0.00015, 0.005)}, _pidClimberStates{frc::PIDController(25, 0.00015, 0.005)}
-{
-
-}
-
-void AlphaArm::OnStart(){
+AlphaArm::AlphaArm(AlphaArmConfig* config /*, frc::Rotation2d initialAngle, wom::vision::Limelight* vision */)
+    : _config(config),
+      _pidArm{frc::PIDController(1.2, 0.4, 0)},
+      _pidArmStates{frc::PIDController(37, 0.00070, 0.15)},
+      _pidIntakeState{frc::PIDController(30, 0.00015, 0.005)},
+      _pidClimberStates{frc::PIDController(25, 0.00015, 0.005)} {}
+
+void AlphaArm::OnStart() {
   _pidArmStates.Reset();
   _pidIntakeState.Reset();
-
 }
 
 void AlphaArm::OnUpdate(units::second_t dt) {
@@ -23,86 +23,90 @@ void AlphaArm::OnUpdate(units::second_t dt) {
       _setAlphaArmVoltage = 0_V;
       break;
     case AlphaArmState::kRaw:
-    std::cout << "RawControl" << std::endl;
-       _setAlphaArmVoltage = _rawArmVoltage;
+      std::cout << "RawControl" << std::endl;
+      _setAlphaArmVoltage = _rawArmVoltage;
 
       break;
 
-    case AlphaArmState::kHoldAngle:
-    {
+    case AlphaArmState::kHoldAngle: {
       _pidIntakeState.SetSetpoint(-_goal);
-       //_setAlphaArmVoltage = _pidArm.Calculate(_alphaArm->GetConfig().config->alphaArmEncoder.GetEncoderPosition());
-      _setAlphaArmVoltage = units::volt_t{_pidIntakeState.Calculate(-(_config->alphaArmEncoder.GetEncoderPosition().value() * (2 * 3.1415)))};
-    }
-    break;
+      //_setAlphaArmVoltage =
+      //_pidArm.Calculate(_alphaArm->GetConfig().config->alphaArmEncoder.GetEncoderPosition());
+      _setAlphaArmVoltage = units::volt_t{
+          _pidIntakeState.Calculate(-(_config->alphaArmEncoder.GetEncoderPosition().value() * (2 * 3.1415)))};
+    } break;
     case AlphaArmState::kAmpAngle:
       std::cout << "Amp Angle" << std::endl;
 
-       _pidArmStates.SetSetpoint(-2.12);
-       _setAlphaArmVoltage = units::volt_t{_pidArmStates.Calculate(-(_config->alphaArmEncoder.GetEncoderPosition().value() * (2 * 3.1415)))};
+      _pidArmStates.SetSetpoint(-2.12);
+      _setAlphaArmVoltage = units::volt_t{
+          _pidArmStates.Calculate(-(_config->alphaArmEncoder.GetEncoderPosition().value() * (2 * 3.1415)))};
 
-    break;
+      break;
 
     case AlphaArmState::kIntakeAngle:
-    std::cout << "Intake Angle" << std::endl;
-       _pidIntakeState.SetSetpoint(-0.50); //-0.48
-       _setAlphaArmVoltage = units::volt_t{_pidIntakeState.Calculate(-(_config->alphaArmEncoder.GetEncoderPosition().value() * (2 * 3.1415)))};
-    break;
+      std::cout << "Intake Angle" << std::endl;
+      _pidIntakeState.SetSetpoint(-0.50);  //-0.48
+      _setAlphaArmVoltage = units::volt_t{
+          _pidIntakeState.Calculate(-(_config->alphaArmEncoder.GetEncoderPosition().value() * (2 * 3.1415)))};
+      break;
     case AlphaArmState::kIntakedAngle:
-    std::cout << "Intake Angle" << std::endl;
-       _pidIntakeState.SetSetpoint(-0.55); //-0.48
-       _setAlphaArmVoltage = units::volt_t{_pidIntakeState.Calculate(-(_config->alphaArmEncoder.GetEncoderPosition().value() * (2 * 3.1415)))};
-    break;
+      std::cout << "Intake Angle" << std::endl;
+      _pidIntakeState.SetSetpoint(-0.55);  //-0.48
+      _setAlphaArmVoltage = units::volt_t{
+          _pidIntakeState.Calculate(-(_config->alphaArmEncoder.GetEncoderPosition().value() * (2 * 3.1415)))};
+      break;
     case AlphaArmState::kClimbAngle:
-    std::cout << "Climb Angle" << std::endl;
-       _pidArmStates.SetSetpoint(-2.00); //-0.48
-       _setAlphaArmVoltage = units::volt_t{_pidArmStates.Calculate(-(_config->alphaArmEncoder.GetEncoderPosition().value() * (2 * 3.1415)))};
-    break;
+      std::cout << "Climb Angle" << std::endl;
+      _pidArmStates.SetSetpoint(-2.00);  //-0.48
+      _setAlphaArmVoltage = units::volt_t{
+          _pidArmStates.Calculate(-(_config->alphaArmEncoder.GetEncoderPosition().value() * (2 * 3.1415)))};
+      break;
 
     case AlphaArmState::kClimbed:
-    std::cout << "Climb Angle" << std::endl;
-       _pidClimberStates.SetSetpoint(-0.5); //-0.48
-       _setAlphaArmVoltage = units::volt_t{_pidClimberStates.Calculate(-(_config->alphaArmEncoder.GetEncoderPosition().value() * (2 * 3.1415)))};
-    break;
+      std::cout << "Climb Angle" << std::endl;
+      _pidClimberStates.SetSetpoint(-0.5);  //-0.48
+      _setAlphaArmVoltage = units::volt_t{_pidClimberStates.Calculate(
+          -(_config->alphaArmEncoder.GetEncoderPosition().value() * (2 * 3.1415)))};
+      break;
 
     case AlphaArmState::kSpeakerAngle:
-    std::cout << "Speaker Angle" << std::endl;
-       _pidIntakeState.SetSetpoint(-0.82);
-       _setAlphaArmVoltage = units::volt_t{_pidIntakeState.Calculate(-(_config->alphaArmEncoder.GetEncoderPosition().value() * (2 * 3.1415)))};
+      std::cout << "Speaker Angle" << std::endl;
+      _pidIntakeState.SetSetpoint(-0.82);
+      _setAlphaArmVoltage = units::volt_t{
+          _pidIntakeState.Calculate(-(_config->alphaArmEncoder.GetEncoderPosition().value() * (2 * 3.1415)))};
 
-    break;
+      break;
 
-    // case AlphaArmState::kVisionAngle:
-    // std::cout << "Vision Angle" << std::endl;
+      // case AlphaArmState::kVisionAngle:
+      // std::cout << "Vision Angle" << std::endl;
 
-    // break;
+      // break;
 
     case AlphaArmState::kStowed:
-    std::cout << "Shuttling angle" << std::endl;
-       _pidIntakeState.SetSetpoint(-1.20);
-       _setAlphaArmVoltage = units::volt_t{_pidIntakeState.Calculate(-(_config->alphaArmEncoder.GetEncoderPosition().value() * (2 * 3.1415)))};
-      default:
+      std::cout << "Shuttling angle" << std::endl;
+      _pidIntakeState.SetSetpoint(-1.20);
+      _setAlphaArmVoltage = units::volt_t{
+          _pidIntakeState.Calculate(-(_config->alphaArmEncoder.GetEncoderPosition().value() * (2 * 3.1415)))};
+    default:
       std::cout << "Error: alphaArm in INVALID STATE" << std::endl;
       break;
-
   }
-   _config->alphaArmGearbox.motorController->SetVoltage(_setAlphaArmVoltage);
-   _config->alphaArmGearbox2.motorController->SetVoltage(_setAlphaArmVoltage);
-    std::cout << "Encoder Value: " << (_config->alphaArmEncoder.GetEncoderPosition().value() * (2 * 3.1415)) << std::endl;
-    _table->GetEntry("PID Error").SetDouble(_pidArm.GetPositionError());
-    _table->GetEntry("SetPoint").SetDouble(_pidArm.GetSetpoint());
-    _table->GetEntry("Input").SetDouble((_config->alphaArmEncoder.GetEncoderPosition().value() * (2 * 3.1415)));
+  _config->alphaArmGearbox.motorController->SetVoltage(_setAlphaArmVoltage);
+  _config->alphaArmGearbox2.motorController->SetVoltage(_setAlphaArmVoltage);
+  std::cout << "Encoder Value: " << (_config->alphaArmEncoder.GetEncoderPosition().value() * (2 * 3.1415))
+            << std::endl;
+  _table->GetEntry("PID Error").SetDouble(_pidArm.GetPositionError());
+  _table->GetEntry("SetPoint").SetDouble(_pidArm.GetSetpoint());
+  _table->GetEntry("Input").SetDouble((_config->alphaArmEncoder.GetEncoderPosition().value() * (2 * 3.1415)));
 
-    _table->GetEntry("PID Error State").SetDouble(_pidArmStates.GetPositionError());
-    _table->GetEntry("SetPoint State").SetDouble(_pidArmStates.GetSetpoint());
+  _table->GetEntry("PID Error State").SetDouble(_pidArmStates.GetPositionError());
+  _table->GetEntry("SetPoint State").SetDouble(_pidArmStates.GetSetpoint());
 
-    _table->GetEntry("Intake SetPoint State").SetDouble(_pidIntakeState.GetSetpoint());
-    _table->GetEntry("Intake PID Error State").SetDouble(_pidIntakeState.GetPositionError());
-
-
-
-     std::cout << "Voltage:" << _setAlphaArmVoltage.value() << std::endl;
+  _table->GetEntry("Intake SetPoint State").SetDouble(_pidIntakeState.GetSetpoint());
+  _table->GetEntry("Intake PID Error State").SetDouble(_pidIntakeState.GetPositionError());
 
+  std::cout << "Voltage:" << _setAlphaArmVoltage.value() << std::endl;
 }
 
 void AlphaArm::SetState(AlphaArmState state) {
@@ -113,11 +117,10 @@ void AlphaArm::SetArmRaw(units::volt_t voltage) {
   _rawArmVoltage = voltage;
 }
 
-
-void AlphaArm::SetGoal(double goal){
+void AlphaArm::SetGoal(double goal) {
   _goal = goal;
 }
 
-void AlphaArm::SetControllerRaw(units::volt_t voltage){
+void AlphaArm::SetControllerRaw(units::volt_t voltage) {
   _controlledRawVoltage = voltage;
 }
diff --git a/src/main/cpp/AlphaArmBehaviour.cpp b/src/main/cpp/AlphaArmBehaviour.cpp
index 2553e9b4..273e9449 100644
--- a/src/main/cpp/AlphaArmBehaviour.cpp
+++ b/src/main/cpp/AlphaArmBehaviour.cpp
@@ -8,7 +8,8 @@
 
 #include "vision/Vision.h"
 
-AlphaArmManualControl::AlphaArmManualControl(AlphaArm* alphaArm, Intake *intake, frc::XboxController* codriver)
+AlphaArmManualControl::AlphaArmManualControl(AlphaArm* alphaArm, Intake* intake,
+                                             frc::XboxController* codriver)
     : _alphaArm(alphaArm), _intake(intake), _codriver(codriver) {
   Controls(alphaArm);
 }
@@ -18,11 +19,9 @@ AlphaArmConfig AlphaArm::GetConfig() {
 }
 
 void AlphaArmManualControl::OnTick(units::second_t dt) {
-
   _table->GetEntry("State").SetBoolean(_rawControl);
   _table->GetEntry("Goal Value").SetBoolean(_gotValue);
 
-
   if (_codriver->GetBackButton()) {
     if (_rawControl == true) {
       _rawControl = false;
@@ -42,17 +41,18 @@ void AlphaArmManualControl::OnTick(units::second_t dt) {
     _table->GetEntry("CLIMBING:").SetBoolean(climbing);
     if (_codriver->GetPOV() == 90 || _codriver->GetPOV() == 180 || _codriver->GetPOV() == 270) {
       climbing = true;
-    } if (_codriver->GetPOV() == 0) {
+    }
+    if (_codriver->GetPOV() == 0) {
       climbing = false;
     } else {
       if (!climbing) {
-        if(_codriver->GetLeftTriggerAxis() > 0.1){
+        if (_codriver->GetLeftTriggerAxis() > 0.1) {
           _alphaArm->SetState(AlphaArmState::kIntakeAngle);
-        } else if (_codriver->GetLeftBumper()){
+        } else if (_codriver->GetLeftBumper()) {
           _alphaArm->SetState(AlphaArmState::kAmpAngle);
-        } else if(_codriver->GetAButton()){
+        } else if (_codriver->GetAButton()) {
           _alphaArm->SetState(AlphaArmState::kStowed);
-        } else if(_codriver->GetPOV() == 90){
+        } else if (_codriver->GetPOV() == 90) {
           _alphaArm->SetState(AlphaArmState::kClimbAngle);
         } else {
           if (_intake->GetState() == IntakeState::kHold) {
@@ -63,9 +63,5 @@ void AlphaArmManualControl::OnTick(units::second_t dt) {
         }
       }
     }
-
   }
-
 }
-
-
diff --git a/src/main/cpp/Auto.cpp b/src/main/cpp/Auto.cpp
index d307cb28..5fffd690 100644
--- a/src/main/cpp/Auto.cpp
+++ b/src/main/cpp/Auto.cpp
@@ -20,11 +20,17 @@ wom::SwerveAutoBuilder* autos::InitCommands(wom::drivetrain::SwerveDrive* _swerv
   wom::AutoCommands c = *new wom::AutoCommands(
       {// {"ArmToSetPoint", [_alphaArm]() { return wom::make<ArmToSetPoint>(_alphaArm, 20_deg); }},
        // {"Shoot", [_shooter]() { return wom::make<AutoShoot>(_shooter); }},
-       {"IntakeNote", [_intake]() { return wom::make<IntakeNote>(_intake)/*->Until(wom::make<behaviour::WaitTime>(3_s))*/;}},
-       {"PassNote", [_intake]() { return wom::make<PassNote>(_intake)->Until(wom::make<behaviour::WaitTime>(1_s)); }},
+       {"IntakeNote",
+        [_intake]() {
+          return wom::make<IntakeNote>(_intake) /*->Until(wom::make<behaviour::WaitTime>(3_s))*/;
+        }},
+       {"PassNote",
+        [_intake]() { return wom::make<PassNote>(_intake)->Until(wom::make<behaviour::WaitTime>(1_s)); }},
        {"EjectNote", [_intake]() { return wom::make<EjectNote>(_intake)->WithTimeout(1_s); }},
-       {"Shoot", [_shooter, _intake]() { return wom::make<AutoShooter>(_shooter, _intake, 300_rad_per_s)->Until(wom::make<wom::WaitTime>(2_s)); }} 
-      });
+       {"Shoot", [_shooter, _intake]() {
+          return wom::make<AutoShooter>(_shooter, _intake, 300_rad_per_s)
+              ->Until(wom::make<wom::WaitTime>(2_s));
+        }}});
 
   return new wom::utils::SwerveAutoBuilder(_swerveDrive, "Taxi", c);
 }
@@ -48,7 +54,6 @@ std::shared_ptr<behaviour::Behaviour> autos::ThreeNoteTaxi(wom::SwerveAutoBuilde
   return builder->GetAutoRoutine("ThreeNoteTaxi");
 }
 
-
 // std::shared_ptr<behaviour::Behaviour> autos::QuadrupleClose(wom::drivetrain::SwerveDrive* _swerveDrive,
 //                                                             Shooter* _shooter, Intake* _intake,
 //                                                             AlphaArm* _alphaArm) {
diff --git a/src/main/cpp/Climber.cpp b/src/main/cpp/Climber.cpp
index c0e263d7..dbe7dbd8 100644
--- a/src/main/cpp/Climber.cpp
+++ b/src/main/cpp/Climber.cpp
@@ -13,16 +13,13 @@ void Climber::OnUpdate(units::second_t dt) {
       _setVoltage = 0_V;
     } break;
 
-    case ClimberState::kRatchet:
-    {
+    case ClimberState::kRatchet: {
       _stringStateName = "Ratchet";
       _setVoltage = 0_V;
       // armServo.SetAngle(0);
-    }
-    break;
+    } break;
 
-    case ClimberState::kArmUp:
-    {
+    case ClimberState::kArmUp: {
       _stringStateName = "Arm Up";
       // _setVoltage = 8_V;
 
@@ -35,8 +32,9 @@ void Climber::OnUpdate(units::second_t dt) {
 
     case ClimberState::kArmDown: {
       _stringStateName = "Arm Down";
-      _pid.SetSetpoint(0.07*100);
-      _setVoltage = -units::volt_t{_pid.Calculate((-_config.climberGearbox.encoder->GetEncoderPosition().value()) * 100)};
+      _pid.SetSetpoint(0.07 * 100);
+      _setVoltage = -units::volt_t{
+          _pid.Calculate((-_config.climberGearbox.encoder->GetEncoderPosition().value()) * 100)};
       // armServo.SetAngle(180);
       // _setVoltage = -8_V;
     } break;
diff --git a/src/main/cpp/ClimberBehaviour.cpp b/src/main/cpp/ClimberBehaviour.cpp
index f958b2a2..113756c5 100644
--- a/src/main/cpp/ClimberBehaviour.cpp
+++ b/src/main/cpp/ClimberBehaviour.cpp
@@ -31,7 +31,7 @@ void ClimberManualControl::OnTick(units::second_t dt) {
       _arm->SetState(AlphaArmState::kClimbAngle);
     } else if (_codriver->GetPOV() == 180) {
       _climber->SetState(ClimberState::kArmDown);
-    } else  if (_codriver->GetPOV() == 270){
+    } else if (_codriver->GetPOV() == 270) {
       _climber->SetState(ClimberState::kRatchet);
       _arm->SetState(AlphaArmState::kClimbed);
     }
diff --git a/src/main/cpp/Intake.cpp b/src/main/cpp/Intake.cpp
index ccca1f65..dc657768 100644
--- a/src/main/cpp/Intake.cpp
+++ b/src/main/cpp/Intake.cpp
@@ -3,11 +3,16 @@
 // of the MIT License at the root of this project
 
 #include "Intake.h"
+
 #include <algorithm>
+
 #include "frc/RobotController.h"
 #include "units/base.h"
 
-Intake::Intake(IntakeConfig config) : _config(config), _pid(frc::PIDController (0.0125, 0, 0, 0.05_s)), _pidPosition(frc::PIDController (1, 0, 0, 0.05_s)) {}
+Intake::Intake(IntakeConfig config)
+    : _config(config),
+      _pid(frc::PIDController(0.02, 0, 0, 0.05_s)),
+      _pidPosition(frc::PIDController(1, 0, 0, 0.05_s)) {}
 
 IntakeConfig Intake::GetConfig() {
   return _config;
@@ -20,10 +25,8 @@ void Intake::OnStart() {
 }
 
 void Intake::OnUpdate(units::second_t dt) {
-
   switch (_state) {
-    case IntakeState::kIdle:
-    {
+    case IntakeState::kIdle: {
       if (_config.intakeSensor->Get() == false) {
         SetState(IntakeState::kHold);
       }
@@ -32,30 +35,24 @@ void Intake::OnUpdate(units::second_t dt) {
       _setVoltage = 0_V;
       _recordNote = false;
       hasValue = false;
-    }
-    break;
+    } break;
 
-    case IntakeState::kRaw:
-    {
+    case IntakeState::kRaw: {
       _stringStateName = "Raw";
       _pid.Reset();
       _setVoltage = _rawVoltage;
-    }
-    break;
+    } break;
 
-    case IntakeState::kEject:
-    {
+    case IntakeState::kEject: {
       if (_config.intakeSensor->Get() == true && _config.passSensor->Get() == true) {
         SetState(IntakeState::kIdle);
       }
       _stringStateName = "Eject";
       _setVoltage = 8_V;
       _pid.Reset();
-    }
-    break;
+    } break;
 
-    case IntakeState::kHold:
-    {
+    case IntakeState::kHold: {
       std::cerr << "HEY FROM kHold" << std::endl;
       units::volt_t pidCalculate = 0_V;
       if (_config.intakeSensor->Get() == true && _config.passSensor->Get() == true) {
@@ -64,48 +61,44 @@ void Intake::OnUpdate(units::second_t dt) {
       // units::volt_t pidCalculate =
       //     units::volt_t{_pid.Calculate(_config.IntakeGearbox.encoder->GetEncoderAngularVelocity().value())};
       if (_config.IntakeGearbox.encoder->GetEncoderPosition().value() < 0) {
-        pidCalculate = units::volt_t{-_pidPosition.Calculate(-_config.IntakeGearbox.encoder->GetEncoderPosition().value())};
+        pidCalculate = units::volt_t{
+            -_pidPosition.Calculate(-_config.IntakeGearbox.encoder->GetEncoderPosition().value())};
       } else {
-        pidCalculate = units::volt_t{_pidPosition.Calculate(_config.IntakeGearbox.encoder->GetEncoderPosition().value())};
+        pidCalculate = units::volt_t{
+            _pidPosition.Calculate(_config.IntakeGearbox.encoder->GetEncoderPosition().value())};
       }
 
       _setVoltage = pidCalculate;
       _stringStateName = "Hold";
-    }
-    break;
+    } break;
 
-    case IntakeState::kIntake:
-    {
+    case IntakeState::kIntake: {
       if (_config.intakeSensor->Get() == false) {
-
         if (_config.IntakeGearbox.encoder->GetEncoderPosition().value() < 0) {
           _pidPosition.SetSetpoint((-_config.IntakeGearbox.encoder->GetEncoderPosition().value()) - 0.5);
         } else {
-          _pidPosition.SetSetpoint(_config.IntakeGearbox.encoder->GetEncoderPosition().value() + 0.5 );
+          _pidPosition.SetSetpoint(_config.IntakeGearbox.encoder->GetEncoderPosition().value() + 0.5);
         }
 
         SetState(IntakeState::kHold);
       }
       _stringStateName = "Intake";
       _setVoltage = -10_V;
-    }
-    break;
+    } break;
 
-    case IntakeState::kPass:
-    {
+    case IntakeState::kPass: {
       if (_config.intakeSensor->Get() == true) {
         SetState(IntakeState::kIdle);
       }
 
       if (!_recordNote) {
-        _noteShot ++;
+        _noteShot++;
         _recordNote = true;
       }
 
       _stringStateName = "Pass";
       _setVoltage = -10_V;
-    }
-    break;
+    } break;
   }
   _table->GetEntry("State").SetString(_stringStateName);
   _table->GetEntry("Motor Voltage").SetDouble(_setVoltage.value());
@@ -130,7 +123,6 @@ void Intake::OnUpdate(units::second_t dt) {
 }
 
 void Intake::SetState(IntakeState state) {
-
   _state = state;
 }
 void Intake::SetRaw(units::volt_t voltage) {
diff --git a/src/main/cpp/IntakeBehaviour.cpp b/src/main/cpp/IntakeBehaviour.cpp
index fe72079a..d69dd44b 100644
--- a/src/main/cpp/IntakeBehaviour.cpp
+++ b/src/main/cpp/IntakeBehaviour.cpp
@@ -5,16 +5,17 @@
 #include "IntakeBehaviour.h"
 
 #include <frc/XboxController.h>
+
 #include "Intake.h"
 #include "behaviour/Behaviour.h"
 #include "networktables/NetworkTableInstance.h"
 
-IntakeManualControl::IntakeManualControl(Intake* intake, AlphaArm *arm, frc::XboxController& codriver) : _intake(intake), _arm(arm), _codriver(codriver) {
+IntakeManualControl::IntakeManualControl(Intake* intake, AlphaArm* arm, frc::XboxController& codriver)
+    : _intake(intake), _arm(arm), _codriver(codriver) {
   Controls(intake);
 }
 
 void IntakeManualControl::OnTick(units::second_t dt) {
-
   if (_codriver.GetBackButtonReleased()) {
     if (_rawControl) {
       _rawControl = false;
@@ -35,21 +36,19 @@ void IntakeManualControl::OnTick(units::second_t dt) {
       _intake->SetRaw(0_V);
     }
   } else {
-
-
     if (_codriver.GetXButton()) {
       if (_intake->GetState() == IntakeState::kIdle) {
         _intake->SetState(IntakeState::kIntake);
       }
     } else if (_codriver.GetRightBumper() || _codriver.GetRightTriggerAxis() > 0.1) {
       // if (_intake->GetState() == IntakeState::kHold) {
-        _intake->SetState(IntakeState::kPass);
+      _intake->SetState(IntakeState::kPass);
       // } else {
-        // _intake->SetState(IntakeState::kIdle);
+      // _intake->SetState(IntakeState::kIdle);
       // }
     } else if (_codriver.GetBButton()) {
       // if (_intake->GetState() == IntakeState::kHold) {
-        _intake->SetState(IntakeState::kEject);
+      _intake->SetState(IntakeState::kEject);
       // } else {
       //   _intake->SetState(IntakeState::kIdle);
       // }
@@ -85,16 +84,15 @@ void IntakeNote::OnTick(units::second_t dt) {
 
   // nt::NetworkTableInstance::GetDefault().GetTable("AutoIntake")->GetEntry("running").SetBoolean(true);
 
-  // if (_intake->GetState() == IntakeState::kHold || _intake->GetState() == IntakeState::kIdle || _timer.Get() > 3_s) {
-    // _intake->SetState(IntakeState::kIdle);
-    // std::cerr << "EXITING" << std::endl;
-    SetDone();
+  // if (_intake->GetState() == IntakeState::kHold || _intake->GetState() == IntakeState::kIdle ||
+  // _timer.Get() > 3_s) { _intake->SetState(IntakeState::kIdle); std::cerr << "EXITING" << std::endl;
+  SetDone();
   // }
 }
 
 void IntakeNote::OnStop() {
   // _intake->SetState(IntakeState::kIdle);
-    // nt::NetworkTableInstance::GetDefault().GetTable("AutoIntake")->GetEntry("running").SetBoolean(false);
+  // nt::NetworkTableInstance::GetDefault().GetTable("AutoIntake")->GetEntry("running").SetBoolean(false);
 }
 
 PassNote::PassNote(Intake* intake) : _intake(intake) {
diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp
index 9f238d58..60e0b1d9 100644
--- a/src/main/cpp/Robot.cpp
+++ b/src/main/cpp/Robot.cpp
@@ -3,8 +3,7 @@
 // of the MIT License at the root of this project
 
 #include "Robot.h"
-#include "Auto.h"
-#include "RobotMap.h"
+
 #include <frc/TimedRobot.h>
 #include <frc/Timer.h>
 #include <frc/controller/RamseteController.h>
@@ -15,6 +14,9 @@
 #include <units/acceleration.h>
 // #include <units/angle.h>
 
+#include "Auto.h"
+#include "RobotMap.h"
+
 // include units
 #include <units/velocity.h>
 #include <units/acceleration.h>
@@ -38,7 +40,7 @@ static units::second_t lastPeriodic;
 
 void Robot::RobotInit() {
   sched = wom::BehaviourScheduler::GetInstance();
-  
+
   frc::SmartDashboard::PutData("Auto Selector", &m_chooser);
 
   m_chooser.SetDefaultOption(defaultAuto, defaultAuto);
@@ -79,6 +81,7 @@ void Robot::RobotInit() {
   intake->SetDefaultBehaviour(
       [this]() { return wom::make<IntakeManualControl>(intake, alphaArm, robotmap.controllers.codriver); });
 
+
   robotmap.swerveBase.moduleConfigs[0].turnMotor.encoder->SetEncoderOffset(0.45229_rad);
   robotmap.swerveBase.moduleConfigs[1].turnMotor.encoder->SetEncoderOffset(2.6846_rad);
   robotmap.swerveBase.moduleConfigs[2].turnMotor.encoder->SetEncoderOffset(3.01121_rad);
@@ -95,7 +98,10 @@ void Robot::RobotPeriodic() {
   loop.Poll();
   wom::BehaviourScheduler::GetInstance()->Tick();
 
-        nt::NetworkTableInstance::GetDefault().GetTable("drivetrainpose")->GetEntry("state").SetInteger(static_cast<int>(_swerveDrive->GetState()));
+  nt::NetworkTableInstance::GetDefault()
+      .GetTable("drivetrainpose")
+      ->GetEntry("state")
+      .SetInteger(static_cast<int>(_swerveDrive->GetState()));
   // sched->Tick();
 
   // robotmap.swerveTable.swerveDriveTable->GetEntry("frontLeftEncoder")
@@ -133,7 +139,7 @@ void Robot::RobotPeriodic() {
 void Robot::AutonomousInit() {
   loop.Clear();
   sched->InterruptAll();
-  _swerveDrive->MakeAtSetPoint();
+  // _swerveDrive->MakeAtSetPoint();
   // _swerveDrive->SetPose(frc::Pose2d());
 
   _swerveDrive->GetConfig().gyro->Reset();
@@ -143,11 +149,13 @@ void Robot::AutonomousInit() {
   robotmap._builder = autos::InitCommands(_swerveDrive, shooter, intake, alphaArm);
 
   if (m_autoSelected == "kTaxi") {
-   sched->Schedule(autos::Taxi(robotmap._builder));
+    sched->Schedule(autos::Taxi(robotmap._builder));
   }
   // sched->Schedule(robotmap._builder->GetAutoRoutine("OneNoteTaxi"));
 
   _swerveDrive->OnStart();
+
+  intake->OnStart();
 }
 
 void Robot::AutonomousPeriodic() {
diff --git a/src/main/deploy/pathplanner/autos/FourNote.auto b/src/main/deploy/pathplanner/autos/FourNote.auto
index b452b4d8..e25e76c2 100644
--- a/src/main/deploy/pathplanner/autos/FourNote.auto
+++ b/src/main/deploy/pathplanner/autos/FourNote.auto
@@ -94,4 +94,4 @@
   },
   "folder": null,
   "choreoAuto": false
-}
\ No newline at end of file
+}
diff --git a/src/main/deploy/pathplanner/autos/FourNoteFar.auto b/src/main/deploy/pathplanner/autos/FourNoteFar.auto
index 7a3134a1..a88e8503 100644
--- a/src/main/deploy/pathplanner/autos/FourNoteFar.auto
+++ b/src/main/deploy/pathplanner/autos/FourNoteFar.auto
@@ -88,4 +88,4 @@
   },
   "folder": null,
   "choreoAuto": false
-}
\ No newline at end of file
+}
diff --git a/src/main/deploy/pathplanner/autos/FourNoteTaxi.auto b/src/main/deploy/pathplanner/autos/FourNoteTaxi.auto
index b2a19a5b..2ff4540e 100644
--- a/src/main/deploy/pathplanner/autos/FourNoteTaxi.auto
+++ b/src/main/deploy/pathplanner/autos/FourNoteTaxi.auto
@@ -100,4 +100,4 @@
   },
   "folder": null,
   "choreoAuto": false
-}
\ No newline at end of file
+}
diff --git a/src/main/deploy/pathplanner/autos/LeftOneNote.auto b/src/main/deploy/pathplanner/autos/LeftOneNote.auto
index 3757a645..9920e20f 100644
--- a/src/main/deploy/pathplanner/autos/LeftOneNote.auto
+++ b/src/main/deploy/pathplanner/autos/LeftOneNote.auto
@@ -40,4 +40,4 @@
   },
   "folder": null,
   "choreoAuto": false
-}
\ No newline at end of file
+}
diff --git a/src/main/deploy/pathplanner/autos/OneNote.auto b/src/main/deploy/pathplanner/autos/OneNote.auto
index 50f060f1..634bc54a 100644
--- a/src/main/deploy/pathplanner/autos/OneNote.auto
+++ b/src/main/deploy/pathplanner/autos/OneNote.auto
@@ -22,4 +22,4 @@
   },
   "folder": null,
   "choreoAuto": false
-}
\ No newline at end of file
+}
diff --git a/src/main/deploy/pathplanner/autos/OneNoteTaxi.auto b/src/main/deploy/pathplanner/autos/OneNoteTaxi.auto
index 61000c9e..e60adbc5 100644
--- a/src/main/deploy/pathplanner/autos/OneNoteTaxi.auto
+++ b/src/main/deploy/pathplanner/autos/OneNoteTaxi.auto
@@ -22,4 +22,4 @@
   },
   "folder": null,
   "choreoAuto": false
-}
\ No newline at end of file
+}
diff --git a/src/main/deploy/pathplanner/autos/Taxi.auto b/src/main/deploy/pathplanner/autos/Taxi.auto
index 0976f300..4a3cc737 100644
--- a/src/main/deploy/pathplanner/autos/Taxi.auto
+++ b/src/main/deploy/pathplanner/autos/Taxi.auto
@@ -47,4 +47,4 @@
   },
   "folder": null,
   "choreoAuto": false
-}
\ No newline at end of file
+}
diff --git a/src/main/deploy/pathplanner/autos/ThreeNote.auto b/src/main/deploy/pathplanner/autos/ThreeNote.auto
index 6159ba17..955deb9a 100644
--- a/src/main/deploy/pathplanner/autos/ThreeNote.auto
+++ b/src/main/deploy/pathplanner/autos/ThreeNote.auto
@@ -70,4 +70,4 @@
   },
   "folder": null,
   "choreoAuto": false
-}
\ No newline at end of file
+}
diff --git a/src/main/deploy/pathplanner/autos/ThreeNoteFar.auto b/src/main/deploy/pathplanner/autos/ThreeNoteFar.auto
index 6c448793..c9796575 100644
--- a/src/main/deploy/pathplanner/autos/ThreeNoteFar.auto
+++ b/src/main/deploy/pathplanner/autos/ThreeNoteFar.auto
@@ -64,4 +64,4 @@
   },
   "folder": null,
   "choreoAuto": false
-}
\ No newline at end of file
+}
diff --git a/src/main/deploy/pathplanner/autos/ThreeNoteTaxi.auto b/src/main/deploy/pathplanner/autos/ThreeNoteTaxi.auto
index 72836db4..d0190920 100644
--- a/src/main/deploy/pathplanner/autos/ThreeNoteTaxi.auto
+++ b/src/main/deploy/pathplanner/autos/ThreeNoteTaxi.auto
@@ -70,4 +70,4 @@
   },
   "folder": null,
   "choreoAuto": false
-}
\ No newline at end of file
+}
diff --git a/src/main/deploy/pathplanner/autos/TwoNote.auto b/src/main/deploy/pathplanner/autos/TwoNote.auto
index a33df241..a0689578 100644
--- a/src/main/deploy/pathplanner/autos/TwoNote.auto
+++ b/src/main/deploy/pathplanner/autos/TwoNote.auto
@@ -46,4 +46,4 @@
   },
   "folder": null,
   "choreoAuto": false
-}
\ No newline at end of file
+}
diff --git a/src/main/deploy/pathplanner/autos/TwoNoteTaxi.auto b/src/main/deploy/pathplanner/autos/TwoNoteTaxi.auto
index bdc155b0..1473d20a 100644
--- a/src/main/deploy/pathplanner/autos/TwoNoteTaxi.auto
+++ b/src/main/deploy/pathplanner/autos/TwoNoteTaxi.auto
@@ -52,4 +52,4 @@
   },
   "folder": null,
   "choreoAuto": false
-}
\ No newline at end of file
+}
diff --git a/src/main/deploy/pathplanner/paths/Backwards.path b/src/main/deploy/pathplanner/paths/Backwards.path
index f187dc44..f27afc50 100644
--- a/src/main/deploy/pathplanner/paths/Backwards.path
+++ b/src/main/deploy/pathplanner/paths/Backwards.path
@@ -46,4 +46,4 @@
   "folder": null,
   "previewStartingState": null,
   "useDefaultConstraints": false
-}
\ No newline at end of file
+}
diff --git a/src/main/deploy/pathplanner/paths/BackwardsWorse.path b/src/main/deploy/pathplanner/paths/BackwardsWorse.path
index 0c86263a..825b0b13 100644
--- a/src/main/deploy/pathplanner/paths/BackwardsWorse.path
+++ b/src/main/deploy/pathplanner/paths/BackwardsWorse.path
@@ -3,12 +3,12 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 1.5,
+        "x": 1.7,
         "y": 0.0
       },
       "prevControl": null,
       "nextControl": {
-        "x": 1.493173655558508,
+        "x": 1.693173655558508,
         "y": 0.020888794958252705
       },
       "isLocked": false,
diff --git a/src/main/deploy/pathplanner/paths/CenterBackToStart.path b/src/main/deploy/pathplanner/paths/CenterBackToStart.path
index 11caa43a..e3908cd4 100644
--- a/src/main/deploy/pathplanner/paths/CenterBackToStart.path
+++ b/src/main/deploy/pathplanner/paths/CenterBackToStart.path
@@ -49,4 +49,4 @@
     "velocity": 0
   },
   "useDefaultConstraints": true
-}
\ No newline at end of file
+}
diff --git a/src/main/deploy/pathplanner/paths/CloseLeft.path b/src/main/deploy/pathplanner/paths/CloseLeft.path
index c30a2487..c9e73a17 100644
--- a/src/main/deploy/pathplanner/paths/CloseLeft.path
+++ b/src/main/deploy/pathplanner/paths/CloseLeft.path
@@ -49,4 +49,4 @@
     "velocity": 0
   },
   "useDefaultConstraints": true
-}
\ No newline at end of file
+}
diff --git a/src/main/deploy/pathplanner/paths/CloseLeftReturn.path b/src/main/deploy/pathplanner/paths/CloseLeftReturn.path
index 1916bdc1..1c720683 100644
--- a/src/main/deploy/pathplanner/paths/CloseLeftReturn.path
+++ b/src/main/deploy/pathplanner/paths/CloseLeftReturn.path
@@ -49,4 +49,4 @@
     "velocity": 0
   },
   "useDefaultConstraints": true
-}
\ No newline at end of file
+}
diff --git a/src/main/deploy/pathplanner/paths/CloseRight.path b/src/main/deploy/pathplanner/paths/CloseRight.path
index aa5aa48f..2f23ffba 100644
--- a/src/main/deploy/pathplanner/paths/CloseRight.path
+++ b/src/main/deploy/pathplanner/paths/CloseRight.path
@@ -49,4 +49,4 @@
     "velocity": 0
   },
   "useDefaultConstraints": true
-}
\ No newline at end of file
+}
diff --git a/src/main/deploy/pathplanner/paths/CloseRightReturn.path b/src/main/deploy/pathplanner/paths/CloseRightReturn.path
index e23d8dd6..cf2db296 100644
--- a/src/main/deploy/pathplanner/paths/CloseRightReturn.path
+++ b/src/main/deploy/pathplanner/paths/CloseRightReturn.path
@@ -49,4 +49,4 @@
     "velocity": 0
   },
   "useDefaultConstraints": true
-}
\ No newline at end of file
+}
diff --git a/src/main/deploy/pathplanner/paths/FarLeft.path b/src/main/deploy/pathplanner/paths/FarLeft.path
index 35bad1a4..d0f89c01 100644
--- a/src/main/deploy/pathplanner/paths/FarLeft.path
+++ b/src/main/deploy/pathplanner/paths/FarLeft.path
@@ -49,4 +49,4 @@
     "velocity": 0
   },
   "useDefaultConstraints": true
-}
\ No newline at end of file
+}
diff --git a/src/main/deploy/pathplanner/paths/FarLeftCenter.path b/src/main/deploy/pathplanner/paths/FarLeftCenter.path
index 747f6891..1c7868b6 100644
--- a/src/main/deploy/pathplanner/paths/FarLeftCenter.path
+++ b/src/main/deploy/pathplanner/paths/FarLeftCenter.path
@@ -49,4 +49,4 @@
     "velocity": 0
   },
   "useDefaultConstraints": true
-}
\ No newline at end of file
+}
diff --git a/src/main/deploy/pathplanner/paths/FarLeftCenterReturn.path b/src/main/deploy/pathplanner/paths/FarLeftCenterReturn.path
index 50e3481c..b9faf6a5 100644
--- a/src/main/deploy/pathplanner/paths/FarLeftCenterReturn.path
+++ b/src/main/deploy/pathplanner/paths/FarLeftCenterReturn.path
@@ -49,4 +49,4 @@
     "velocity": 0
   },
   "useDefaultConstraints": true
-}
\ No newline at end of file
+}
diff --git a/src/main/deploy/pathplanner/paths/FarLeftReturn.path b/src/main/deploy/pathplanner/paths/FarLeftReturn.path
index 993de755..5b0b2b5c 100644
--- a/src/main/deploy/pathplanner/paths/FarLeftReturn.path
+++ b/src/main/deploy/pathplanner/paths/FarLeftReturn.path
@@ -49,4 +49,4 @@
     "velocity": 0
   },
   "useDefaultConstraints": true
-}
\ No newline at end of file
+}
diff --git a/src/main/deploy/pathplanner/paths/FirstNote.path b/src/main/deploy/pathplanner/paths/FirstNote.path
index 58409395..3dac4fc9 100644
--- a/src/main/deploy/pathplanner/paths/FirstNote.path
+++ b/src/main/deploy/pathplanner/paths/FirstNote.path
@@ -16,11 +16,11 @@
     },
     {
       "anchor": {
-        "x": 1.5,
+        "x": 1.7,
         "y": -0.0
       },
       "prevControl": {
-        "x": 0.5,
+        "x": 0.7,
         "y": 0.0
       },
       "nextControl": null,
diff --git a/src/main/deploy/pathplanner/paths/FirstNoteGood.path b/src/main/deploy/pathplanner/paths/FirstNoteGood.path
index 8a27b13b..6236212e 100644
--- a/src/main/deploy/pathplanner/paths/FirstNoteGood.path
+++ b/src/main/deploy/pathplanner/paths/FirstNoteGood.path
@@ -49,4 +49,4 @@
     "velocity": 0
   },
   "useDefaultConstraints": false
-}
\ No newline at end of file
+}
diff --git a/src/main/deploy/pathplanner/paths/LeftCloseLeft.path b/src/main/deploy/pathplanner/paths/LeftCloseLeft.path
index a7b4b4a7..c5219045 100644
--- a/src/main/deploy/pathplanner/paths/LeftCloseLeft.path
+++ b/src/main/deploy/pathplanner/paths/LeftCloseLeft.path
@@ -49,4 +49,4 @@
     "velocity": 0
   },
   "useDefaultConstraints": true
-}
\ No newline at end of file
+}
diff --git a/src/main/include/AlphaArm.h b/src/main/include/AlphaArm.h
index a3cfc953..d7f9a89a 100644
--- a/src/main/include/AlphaArm.h
+++ b/src/main/include/AlphaArm.h
@@ -4,14 +4,13 @@
 
 #pragma once
 #include <frc/DigitalInput.h>
-
+#include <frc/controller/PIDController.h>
+#include <units/angle.h>
+#include <units/voltage.h>
 
 #include "Wombat.h"
-//#include "vision/Vision.h"
 #include "utils/PID.h"
-#include <units/angle.h>
-#include <units/voltage.h>
-#include <frc/controller/PIDController.h>
+// #include "vision/Vision.h"
 
 struct AlphaArmConfig {
   wom::Gearbox alphaArmGearbox;
@@ -20,7 +19,6 @@ struct AlphaArmConfig {
 
   std::string path;
   // Vision *vision;
-
 };
 
 enum class AlphaArmState {
@@ -39,7 +37,7 @@ enum class AlphaArmState {
 
 class AlphaArm : public behaviour::HasBehaviour {
  public:
-  AlphaArm(AlphaArmConfig *config/*, frc::Rotation2d initialAngle, wom::vision::Limelight* vision*/);
+  AlphaArm(AlphaArmConfig* config /*, frc::Rotation2d initialAngle, wom::vision::Limelight* vision*/);
 
   void OnUpdate(units::second_t dt);
   void SetArmRaw(units::volt_t voltage);
@@ -47,17 +45,17 @@ class AlphaArm : public behaviour::HasBehaviour {
   void SetControllerRaw(units::volt_t voltage);
   void SetGoal(double goal);
   void OnStart();
-  AlphaArmConfig GetConfig(); //{ return _config; }
+  AlphaArmConfig GetConfig();  //{ return _config; }
   frc::PIDController GetPID();
 
  private:
   // units::radian_t CalcTargetAngle();
 
-  AlphaArmConfig *_config;
+  AlphaArmConfig* _config;
   // wom::vision::Limelight* _vision;
   AlphaArmState _state = AlphaArmState::kIdle;
-  //wom::utils::PIDController<units::degree, units::volt> _alphaArmPID;
-  //frc::DutyCycleEncoder armEncoder{4};
+  // wom::utils::PIDController<units::degree, units::volt> _alphaArmPID;
+  // frc::DutyCycleEncoder armEncoder{4};
   frc::PIDController _pidArm;
   frc::PIDController _pidArmStates;
   frc::PIDController _pidIntakeState;
@@ -69,5 +67,4 @@ class AlphaArm : public behaviour::HasBehaviour {
   units::volt_t _rawArmVoltage = 0_V;
   units::volt_t _testRawVoltage = 3_V;
   double _goal = 0;
-
 };
diff --git a/src/main/include/AlphaArmBehaviour.h b/src/main/include/AlphaArmBehaviour.h
index b72020ff..3016cdb0 100644
--- a/src/main/include/AlphaArmBehaviour.h
+++ b/src/main/include/AlphaArmBehaviour.h
@@ -3,12 +3,12 @@
 // of the MIT License at the root of this project
 
 #pragma once
-#include "vision/Vision.h"
 #include <frc/XboxController.h>
 
 #include "AlphaArm.h"
 #include "Intake.h"
 #include "Wombat.h"
+#include "vision/Vision.h"
 
 class AlphaArmManualControl : public behaviour::Behaviour {
  public:
@@ -17,11 +17,12 @@ class AlphaArmManualControl : public behaviour::Behaviour {
 
  private:
   AlphaArm* _alphaArm;
-  Intake *_intake;
+  Intake* _intake;
   frc::XboxController* _codriver;
   units::volt_t _setAlphaArmVoltage = 0_V;
   bool _rawControl = false;
   bool _gotValue = false;
-  std::shared_ptr<nt::NetworkTable> _table = nt::NetworkTableInstance::GetDefault().GetTable("AlphaArmSubsystem");
+  std::shared_ptr<nt::NetworkTable> _table =
+      nt::NetworkTableInstance::GetDefault().GetTable("AlphaArmSubsystem");
   bool climbing = false;
 };
diff --git a/src/main/include/Climber.h b/src/main/include/Climber.h
index b640fa62..4928f6e9 100644
--- a/src/main/include/Climber.h
+++ b/src/main/include/Climber.h
@@ -4,11 +4,12 @@
 
 #pragma once
 
+#include <frc/Servo.h>
+
 #include <memory>
 #include <string>
 
 #include "Wombat.h"
-#include <frc/Servo.h>
 
 struct ClimberConfig {
   wom::Gearbox climberGearbox;
diff --git a/src/main/include/IntakeBehaviour.h b/src/main/include/IntakeBehaviour.h
index e5884e55..fbbc4747 100644
--- a/src/main/include/IntakeBehaviour.h
+++ b/src/main/include/IntakeBehaviour.h
@@ -6,19 +6,19 @@
 
 #include <frc/XboxController.h>
 
-#include "Intake.h"
 #include "AlphaArm.h"
+#include "Intake.h"
 #include "Wombat.h"
 
 class IntakeManualControl : public behaviour::Behaviour {
  public:
-  explicit IntakeManualControl(Intake* intake, AlphaArm *arm, frc::XboxController& codriver);
+  explicit IntakeManualControl(Intake* intake, AlphaArm* arm, frc::XboxController& codriver);
 
   void OnTick(units::second_t dt) override;
 
  private:
   Intake* _intake;
-  AlphaArm *_arm;
+  AlphaArm* _arm;
   frc::XboxController& _codriver;
   bool _rawControl = false;
 };
@@ -51,7 +51,6 @@ class PassNote : public behaviour::Behaviour {
 
  private:
   Intake* _intake;
-
 };
 
 class EjectNote : public behaviour::Behaviour {
@@ -61,5 +60,4 @@ class EjectNote : public behaviour::Behaviour {
 
  private:
   Intake* _intake;
-
 };
diff --git a/src/main/include/Robot.h b/src/main/include/Robot.h
index d2d9e27c..e6c0dec6 100644
--- a/src/main/include/Robot.h
+++ b/src/main/include/Robot.h
@@ -5,9 +5,9 @@
 #pragma once
 
 #include <frc/Encoder.h>
+#include <frc/Servo.h>
 #include <frc/TimedRobot.h>
 #include <frc/Timer.h>
-#include "Auto.h"
 #include <frc/event/EventLoop.h>
 #include <frc/simulation/DifferentialDrivetrainSim.h>
 #include <frc/simulation/EncoderSim.h>
@@ -23,9 +23,9 @@
 
 #include "AlphaArm.h"
 #include "AlphaArmBehaviour.h"
+#include "Auto.h"
 #include "Climber.h"
 #include "ClimberBehaviour.h"
-#include <frc/Servo.h>
 #include "Intake.h"
 #include "IntakeBehaviour.h"
 #include "LED.h"
@@ -91,7 +91,6 @@ class Robot : public frc::TimedRobot {
   AlphaArm* alphaArm;
   LED* _led;
 
-
   // ctre::phoenix6::hardware::TalonFX *frontLeft;
   // ctre::phoenix6::hardware::TalonFX *frontRight;
   // ctre::phoenix6::hardware::TalonFX *backLeft;
@@ -99,14 +98,14 @@ class Robot : public frc::TimedRobot {
 
   // wom::SwerveDrive* _swerveDrive;
 
-  frc::Servo *armServo;
+  frc::Servo* armServo;
 
-  //ctre::phoenix6::hardware::TalonFX *frontLeft;
-  // ctre::phoenix6::hardware::TalonFX *frontRight;
-  // ctre::phoenix6::hardware::TalonFX *backLeft;
-  // ctre::phoenix6::hardware::TalonFX *backRight;
   // ctre::phoenix6::hardware::TalonFX *frontLeft;
   //  ctre::phoenix6::hardware::TalonFX *frontRight;
   //  ctre::phoenix6::hardware::TalonFX *backLeft;
   //  ctre::phoenix6::hardware::TalonFX *backRight;
+  //  ctre::phoenix6::hardware::TalonFX *frontLeft;
+  //   ctre::phoenix6::hardware::TalonFX *frontRight;
+  //   ctre::phoenix6::hardware::TalonFX *backLeft;
+  //   ctre::phoenix6::hardware::TalonFX *backRight;
 };
diff --git a/src/main/include/ShooterBehaviour.h b/src/main/include/ShooterBehaviour.h
index 5c6880b3..dfce34b8 100644
--- a/src/main/include/ShooterBehaviour.h
+++ b/src/main/include/ShooterBehaviour.h
@@ -5,6 +5,7 @@
 #pragma once
 
 #include <frc/XboxController.h>
+#include <units/angular_velocity.h>
 
 #include <memory>
 
@@ -13,7 +14,6 @@
 #include "Shooter.h"
 #include "Wombat.h"
 #include "frc/Timer.h"
-#include <units/angular_velocity.h>
 
 class ShooterManualControl : public behaviour::Behaviour {
  public:
diff --git a/wombat/src/main/cpp/drivetrain/SwerveDrive.cpp b/wombat/src/main/cpp/drivetrain/SwerveDrive.cpp
index 382a7c49..849a780d 100644
--- a/wombat/src/main/cpp/drivetrain/SwerveDrive.cpp
+++ b/wombat/src/main/cpp/drivetrain/SwerveDrive.cpp
@@ -439,7 +439,7 @@ SwerveDrive::SwerveDrive(SwerveDriveConfig config, frc::Pose2d initialPose)
   _anglePIDController.SetTolerance(360);
   _anglePIDController.EnableContinuousInput(0, 2 * 3.14159);
   // _anglePIDController.EnableContinuousInput(-3.14159, 3.14159);
-    
+
   int i = 1;
   for (auto cfg : _config.modules) {
     _modules.emplace_back(config.path + "/modules/" + std::to_string(i), cfg,
@@ -473,53 +473,41 @@ void SwerveDrive::OnUpdate(units::second_t dt) {
     case SwerveDriveState::kAngle: {
       _turnPIDController.SetSetpoint(_reqAngle.value());
 
-      double direction = _turnPIDController.Calculate(_config.gyro->GetRotation2d().Radians().value()); 
+      double direction = _turnPIDController.Calculate(_config.gyro->GetRotation2d().Radians().value());
 
       for (size_t i = 0; i < _modules.size(); i++) {
         switch (i) {
-          case 0:
-          {
+          case 0: {
             _modules[i].SetPID(135_deg, units::meters_per_second_t{-direction}, dt);
-          }
-          break;
+          } break;
 
-          case 1:
-          {
+          case 1: {
             _modules[i].SetPID(45_deg, units::meters_per_second_t{-direction}, dt);
-          }
-          break;
+          } break;
 
-          case 2:
-          {
+          case 2: {
             _modules[i].SetPID(135_deg, units::meters_per_second_t{direction}, dt);
-          }
-          break;
+          } break;
 
-          case 3:
-          {
+          case 3: {
             _modules[i].SetPID(45_deg, units::meters_per_second_t{direction}, dt);
-          }
-          break;
-
+          } break;
         }
       }
 
       _table->GetEntry("direction").SetDouble(direction);
       _table->GetEntry("_reqAngle").SetDouble(_reqAngle.value());
-    }
-    break;
+    } break;
     case SwerveDriveState::kPose: {
       // _target_fr_speeds.vx = _xPIDController.Calculate(GetPose().X(), dt);
       // _target_fr_speeds.vy = _yPIDController.Calculate(GetPose().Y(), dt);
       _target_fr_speeds.vx = units::meters_per_second_t{_xPIDController.Calculate(GetPose().X().value())};
       _target_fr_speeds.vy = units::meters_per_second_t{_yPIDController.Calculate(GetPose().Y().value())};
-      _target_fr_speeds.omega = 0_rad_per_s;
-          // units::radians_per_second_t{_anglePIDController.Calculate(GetPose().Rotation().Radians().value() - (3.14159))};
+      _target_fr_speeds.omega = units::radians_per_second_t{_anglePIDController.Calculate(GetPose().Rotation().Radians().value())};
 
       _table->GetEntry("Swerve vx").SetDouble(_target_fr_speeds.vx.value());
       _table->GetEntry("Swerve vy").SetDouble(_target_fr_speeds.vy.value());
       _table->GetEntry("Swerve omega").SetDouble(_target_fr_speeds.omega.value());
-
     }
       [[fallthrough]];
     case SwerveDriveState::kFieldRelativeVelocity:
@@ -553,11 +541,11 @@ void SwerveDrive::OnUpdate(units::second_t dt) {
       auto target_states = _kinematics.ToSwerveModuleStates(_target_speed);
       auto new_target_states = _kinematics.ToSwerveModuleStates(new_target_speed);
       for (size_t i = 0; i < _modules.size(); i++) {
-         if (i == 0 || i == 2 || i == 1) {
+        if (i == 0 || i == 2 || i == 1) {
           _modules[i].SetPID(new_target_states[i].angle.Radians(), new_target_states[i].speed, dt);
         } else {
           _modules[i].SetPID(target_states[i].angle.Radians(), target_states[i].speed, dt);
-        }         
+        }
       }
     } break;
     case SwerveDriveState::kIndividualTuning:
@@ -596,7 +584,7 @@ void SwerveDrive::OnUpdate(units::second_t dt) {
   }
 
   _poseEstimator.Update(
-      _config.gyro->GetRotation2d()/* - 1_rad*/,
+      _config.gyro->GetRotation2d() /* - 1_rad*/,
       wpi::array<frc::SwerveModulePosition, 4>{_modules[3].GetPosition(), _modules[0].GetPosition(),
                                                _modules[1].GetPosition(), _modules[2].GetPosition()});
 
@@ -695,8 +683,10 @@ void SwerveDrive::SetPose(frc::Pose2d pose) {
 }
 
 bool SwerveDrive::IsAtSetPose() {
-  if (std::abs(_xPIDController.GetPositionError()) < 0.1 && std::abs(_yPIDController.GetPositionError()) < 0.1) {
-    if (std::abs(_xPIDController.GetVelocityError()) < 1 && std::abs(_yPIDController.GetVelocityError()) < 1) {
+  if (std::abs(_xPIDController.GetPositionError()) < 0.1 &&
+      std::abs(_yPIDController.GetPositionError()) < 0.1) {
+    if (std::abs(_xPIDController.GetVelocityError()) < 1 &&
+        std::abs(_yPIDController.GetVelocityError()) < 1) {
       return false;
     }
   }
@@ -704,7 +694,8 @@ bool SwerveDrive::IsAtSetPose() {
 }
 
 bool SwerveDrive::IsAtSetAngle() {
-  if (std::abs(_turnPIDController.GetPositionError()) < 0.1 && std::abs(_turnPIDController.GetVelocityError()) < 0.1) {
+  if (std::abs(_turnPIDController.GetPositionError()) < 0.1 &&
+      std::abs(_turnPIDController.GetVelocityError()) < 0.1) {
     return true;
   }
   return false;
@@ -715,15 +706,17 @@ SwerveDriveState SwerveDrive::GetState() {
 }
 
 void SwerveDrive::ResetPose(frc::Pose2d pose) {
+  std::cout << "RESETTING POSE" << std::endl;
   _poseEstimator.ResetPosition(
-      _config.gyro->GetRotation2d()/* - 1_rad*/,
+      _config.gyro->GetRotation2d() /* - 1_rad*/,
       wpi::array<frc::SwerveModulePosition, 4>{_modules[3].GetPosition(), _modules[0].GetPosition(),
                                                _modules[1].GetPosition(), _modules[2].GetPosition()},
       frc::Pose2d(0_m, 0_m, 0_deg));
 }
 
 frc::Pose2d SwerveDrive::GetPose() {
-  return frc::Pose2d(_poseEstimator.GetEstimatedPosition().X(), _poseEstimator.GetEstimatedPosition().Y(), _poseEstimator.GetEstimatedPosition().Rotation());
+  return frc::Pose2d(_poseEstimator.GetEstimatedPosition().X(), _poseEstimator.GetEstimatedPosition().Y(),
+                     _poseEstimator.GetEstimatedPosition().Rotation());
   // return frc::Pose2d(1_m, 1_m, 0_deg);
 }
 
diff --git a/wombat/src/main/cpp/drivetrain/behaviours/SwerveBehaviours.cpp b/wombat/src/main/cpp/drivetrain/behaviours/SwerveBehaviours.cpp
index 42e058d3..0ce0e093 100644
--- a/wombat/src/main/cpp/drivetrain/behaviours/SwerveBehaviours.cpp
+++ b/wombat/src/main/cpp/drivetrain/behaviours/SwerveBehaviours.cpp
@@ -240,7 +240,7 @@ wom::drivetrain::behaviours::DrivebasePoseBehaviour::DrivebasePoseBehaviour(Swer
 }
 
 void wom::drivetrain::behaviours::DrivebasePoseBehaviour::OnStart() {
-    if (_voltageLimit >= (frc::RobotController::GetBatteryVoltage() - 0.5_V)) {
+  if (_voltageLimit >= (frc::RobotController::GetBatteryVoltage() - 0.5_V)) {
     _voltageLimit = frc::RobotController::GetBatteryVoltage() - 1_V;
   }
 
@@ -257,12 +257,11 @@ void wom::drivetrain::behaviours::DrivebasePoseBehaviour::OnStart() {
   _timer.Start();
 }
 
-
 // used in autonomous for going to set drive poses
 void wom::drivetrain::behaviours::DrivebasePoseBehaviour::OnTick(units::second_t deltaTime) {
-        nt::NetworkTableInstance::GetDefault().GetTable("drivetrainpose")->GetEntry("going").SetBoolean(true);
+  nt::NetworkTableInstance::GetDefault().GetTable("drivetrainpose")->GetEntry("going").SetBoolean(true);
   if (_timer.Get() > 1_s) {
-            nt::NetworkTableInstance::GetDefault().GetTable("drivetrainpose")->GetEntry("going").SetBoolean(false);
+    nt::NetworkTableInstance::GetDefault().GetTable("drivetrainpose")->GetEntry("going").SetBoolean(false);
     SetDone();
   }
   if (_swerveDrivebase->IsAtSetAngle() && _swerveDrivebase->GetState() == SwerveDriveState::kAngle) {
@@ -270,7 +269,7 @@ void wom::drivetrain::behaviours::DrivebasePoseBehaviour::OnTick(units::second_t
   } else {
     if (_swerveDrivebase->IsAtSetPose() && !_hold) {
       std::cout << "Exited..." << std::endl;
-        nt::NetworkTableInstance::GetDefault().GetTable("drivetrainpose")->GetEntry("going").SetBoolean(false);
+      nt::NetworkTableInstance::GetDefault().GetTable("drivetrainpose")->GetEntry("going").SetBoolean(false);
       SetDone();
     }
   }
diff --git a/wombat/src/main/cpp/sim/Sim.cpp b/wombat/src/main/cpp/sim/Sim.cpp
index 0234eaa6..914394de 100644
--- a/wombat/src/main/cpp/sim/Sim.cpp
+++ b/wombat/src/main/cpp/sim/Sim.cpp
@@ -16,7 +16,8 @@ wom::sim::SimSwerve::SimSwerve(wom::drivetrain::SwerveDrive* _swerve) : _swerve(
 
 void wom::sim::SimSwerve::OnTick() {
   _field.SetRobotPose(_swerve->GetPose());
-  utils::WritePose2NT(nt::NetworkTableInstance::GetDefault().GetTable("swerveSetpoint"), _swerve->GetSetpoint());
+  utils::WritePose2NT(nt::NetworkTableInstance::GetDefault().GetTable("swerveSetpoint"),
+                      _swerve->GetSetpoint());
 }
 
 void wom::sim::SimSwerve::OnTick(frc::Pose2d pose) {
diff --git a/wombat/src/main/cpp/utils/Encoder.cpp b/wombat/src/main/cpp/utils/Encoder.cpp
index 061ed30a..8f3b2c87 100644
--- a/wombat/src/main/cpp/utils/Encoder.cpp
+++ b/wombat/src/main/cpp/utils/Encoder.cpp
@@ -5,6 +5,7 @@
 #include "utils/Encoder.h"
 
 #include <rev/SparkRelativeEncoder.h>
+
 #include <cmath>
 
 wom::utils::Encoder::Encoder(double encoderTicksPerRotation, int type, units::meter_t wheelRadius,
diff --git a/wombat/src/main/cpp/utils/Pathplanner.cpp b/wombat/src/main/cpp/utils/Pathplanner.cpp
index 5c13877a..7954e07b 100644
--- a/wombat/src/main/cpp/utils/Pathplanner.cpp
+++ b/wombat/src/main/cpp/utils/Pathplanner.cpp
@@ -235,7 +235,6 @@ frc::Trajectory utils::PathWeaver::getTrajectory(std::string_view path) {
 // FollowPath implementation
 utils::FollowPath::FollowPath(drivetrain::SwerveDrive* swerve, std::string path, bool flip)
     : _swerve(swerve), behaviour::Behaviour("<Follow Path: " + path + ">") {
-  
   Controls(swerve);
 
   _path = pathplanner::PathPlannerPath::fromPathFile(path);
@@ -268,7 +267,6 @@ utils::FollowPath::FollowPath(drivetrain::SwerveDrive* swerve, std::string path,
   std::vector<pathplanner::PathPoint> points =
       pathplanner::PathPlannerPath::fromPathFile(path)->getAllPathPoints();
 
-
   pathplanner::RotationTarget* lastRot = nullptr;
   units::degree_t rot;
 
@@ -294,12 +292,12 @@ utils::FollowPath::FollowPath(drivetrain::SwerveDrive* swerve, std::string path,
     lastRot = &t;
 
     frc::Translation2d tr = frc::Translation2d(point.position.X() * -1, point.position.Y() * -1);
-    frc::Pose2d pose2 = frc::Pose2d(tr, rot);//.TransformBy(frc::Transform2d(-1.37_m, -5.56_m, 0_deg));
+    frc::Pose2d pose2 = frc::Pose2d(tr, rot);  //.TransformBy(frc::Transform2d(-1.37_m, -5.56_m, 0_deg));
     //
     if (i == index || i == static_cast<int>(tot - 1) || f) {
       _poses.emplace_back(frc::Pose2d(pose2.X(), pose2.Y(), pose2.Rotation()));
       // _poses.emplace_back(frc::Pose2d(pose2.Y(), pose2.X(), pose2.Rotation()));
-      i = 0;//  - 5.56_m,  - 2.91_m
+      i = 0;  //  - 5.56_m,  - 2.91_m
       f = false;
     }
 
@@ -337,7 +335,7 @@ utils::FollowPath::FollowPath(drivetrain::SwerveDrive* swerve, std::string path,
   //   WritePose2NT(nt::NetworkTableInstance::GetDefault().GetTable("pathplanner/" + std::to_string(i) +
   //   "/poses/prev"), p2);
   //
-    // i++;
+  // i++;
   // }
 
   // i = 0;
@@ -347,7 +345,6 @@ utils::FollowPath::FollowPath(drivetrain::SwerveDrive* swerve, std::string path,
     // i++;
   }
 
-
   _pathName = path;
 
   // _swerve->TurnToAngle(_poses[_currentPose].Rotation().Radians());
@@ -378,7 +375,6 @@ void utils::FollowPath::OnStart() {
 }
 
 void utils::FollowPath::OnTick(units::second_t dt) {
-  
   nt::NetworkTableInstance::GetDefault()
       .GetTable("pathplanner")
       ->GetEntry("atPose")
@@ -391,19 +387,24 @@ void utils::FollowPath::OnTick(units::second_t dt) {
       .GetTable("pathplanner")
       ->GetEntry("currentTime")
       .SetDouble(_timer.Get().value());
-  
+
   WritePose2NT(nt::NetworkTableInstance::GetDefault().GetTable("pathplanner/targetPose"),
                _poses[_currentPose]);
   WritePose2NT(nt::NetworkTableInstance::GetDefault().GetTable("pathplanner/currentPose"),
                _swerve->GetPose());
-  nt::NetworkTableInstance::GetDefault().GetTable("pathplanner")->GetEntry("currentPoseNumber").SetInteger(_currentPose);
-    nt::NetworkTableInstance::GetDefault().GetTable("pathplanner")->GetEntry("amtPoses").SetInteger(static_cast<int>(_poses.size()));
-
+  nt::NetworkTableInstance::GetDefault()
+      .GetTable("pathplanner")
+      ->GetEntry("currentPoseNumber")
+      .SetInteger(_currentPose);
+  nt::NetworkTableInstance::GetDefault()
+      .GetTable("pathplanner")
+      ->GetEntry("amtPoses")
+      .SetInteger(static_cast<int>(_poses.size()));
 
   std::cout << "Following Path" << std::endl;
-  
+
   // if (_swerve->IsAtSetAngle() && _swerve->GetState() == drivetrain::SwerveDriveState::kAngle) {
-    _swerve->SetPose(_poses[_currentPose]);
+  _swerve->SetPose(_poses[_currentPose]);
   // }
   /*else*/ if (_swerve->IsAtSetPose() || _timer.Get() > _time) {
     if (_currentPose == static_cast<int>(_poses.size())) {
@@ -426,7 +427,6 @@ void utils::FollowPath::OnTick(units::second_t dt) {
       .GetTable("pathplanner")
       ->GetEntry("Current path")
       .SetString(filePath);
-
 }
 
 // AutoBuilder implementation
@@ -455,7 +455,7 @@ void utils::AutoBuilder::SetAuto(std::string path) {
     cjson += cdata;
   }
 
-  // cjson.pop_back();
+  cjson.pop_back();
 
   wpi::json j = wpi::json::parse(cjson);
 
@@ -506,19 +506,30 @@ void utils::AutoBuilder::SetAuto(std::string path) {
     if (command["type"] == "path") {
       _pathplan->Add(behaviour::make<FollowPath>(_swerve, command["data"]["pathName"], _flip));
       pathamt++;
-          nt::NetworkTableInstance::GetDefault().GetTable("commands/" + std::to_string(i))->GetEntry("behaviours").SetStringArray(_pathplan->GetQueue());
+      nt::NetworkTableInstance::GetDefault()
+          .GetTable("commands/" + std::to_string(i))
+          ->GetEntry("behaviours")
+          .SetStringArray(_pathplan->GetQueue());
     } else if (command["type"] == "named") {
       _pathplan->Add(_commandsList.Run(command["data"]["name"]));
       commandamt++;
-          nt::NetworkTableInstance::GetDefault().GetTable("commands/" + std::to_string(i))->GetEntry("behaviours").SetStringArray(_pathplan->GetQueue());
+      nt::NetworkTableInstance::GetDefault()
+          .GetTable("commands/" + std::to_string(i))
+          ->GetEntry("behaviours")
+          .SetStringArray(_pathplan->GetQueue());
     } else if (command["type"] == "parallel") {
-      
       auto nb = behaviour::make<behaviour::ConcurrentBehaviour>(behaviour::ConcurrentBehaviourReducer::ANY);
       int j = 0;
-      for (auto c : command["data"]["commands"]) { 
-        nt::NetworkTableInstance::GetDefault().GetTable("commands/parallel/" + std::to_string(j))->GetEntry("type").SetString(static_cast<std::string>(c["type"]));
-
-        nt::NetworkTableInstance::GetDefault().GetTable("commands/parallel/" + std::to_string(j))->GetEntry("typeisstring").SetBoolean(c["type"].is_string());
+      for (auto c : command["data"]["commands"]) {
+        nt::NetworkTableInstance::GetDefault()
+            .GetTable("commands/parallel/" + std::to_string(j))
+            ->GetEntry("type")
+            .SetString(static_cast<std::string>(c["type"]));
+
+        nt::NetworkTableInstance::GetDefault()
+            .GetTable("commands/parallel/" + std::to_string(j))
+            ->GetEntry("typeisstring")
+            .SetBoolean(c["type"].is_string());
         if (static_cast<std::string>(c["type"]) == "path") {
           nb->Add(behaviour::make<FollowPath>(_swerve, c["data"]["pathName"], _flip));
           pathamt++;
@@ -529,22 +540,34 @@ void utils::AutoBuilder::SetAuto(std::string path) {
         nb->Add(behaviour::make<behaviour::Print>("ok"));
         j++;
       }
-      nt::NetworkTableInstance::GetDefault().GetTable("commands")->GetEntry("parallelcommandsamt").SetInteger(j);
-      nt::NetworkTableInstance::GetDefault().GetTable("commands")->GetEntry("parallel-" + std::to_string(i)).SetStringArray(nb->GetQueue());
+      nt::NetworkTableInstance::GetDefault()
+          .GetTable("commands")
+          ->GetEntry("parallelcommandsamt")
+          .SetInteger(j);
+      nt::NetworkTableInstance::GetDefault()
+          .GetTable("commands")
+          ->GetEntry("parallel-" + std::to_string(i))
+          .SetStringArray(nb->GetQueue());
       _pathplan->Add(nb);
     }
 
     _pathplan->Add(behaviour::make<behaviour::Print>("idk"));
 
     pathplan = _pathplan;
-    nt::NetworkTableInstance::GetDefault().GetTable("commands/" + std::to_string(i))->GetEntry("currentbehaviours").SetStringArray(pathplan->GetQueue());
+    nt::NetworkTableInstance::GetDefault()
+        .GetTable("commands/" + std::to_string(i))
+        ->GetEntry("currentbehaviours")
+        .SetStringArray(pathplan->GetQueue());
     i++;
   }
 
   // pathplan->Add(behaviour::make<behaviour::Print>("test"));
   //   pathplan->Add(behaviour::make<behaviour::Print>("test"));
 
-    nt::NetworkTableInstance::GetDefault().GetTable("commands/newbehaviours")->GetEntry(std::to_string(i)).SetStringArray(pathplan->GetQueue());
+  nt::NetworkTableInstance::GetDefault()
+      .GetTable("commands/newbehaviours")
+      ->GetEntry(std::to_string(i))
+      .SetStringArray(pathplan->GetQueue());
 
   nt::NetworkTableInstance::GetDefault().GetTable("commands")->GetEntry("PathAmt").SetInteger(pathamt);
   nt::NetworkTableInstance::GetDefault().GetTable("commands")->GetEntry("CommandAmt").SetInteger(commandamt);
@@ -552,8 +575,6 @@ void utils::AutoBuilder::SetAuto(std::string path) {
   _swerve->SetAccelerationLimit(units::meters_per_second_squared_t{2});
   _swerve->SetVoltageLimit(6_V);
 
-
-
   // WritePose2NT(nt::NetworkTableInstance::GetDefault().GetTable("startPose"),
   //              JSONPoseToPose2d(*_startingPose));
 

From 22755383ba28ff686acf0a94e30354e96fd117c7 Mon Sep 17 00:00:00 2001
From: = <paul.hodges@student.education.wa.edu.au>
Date: Fri, 8 Mar 2024 22:57:59 +0800
Subject: [PATCH 204/207] make three and four note auto work

---
 simgui.json                                   |   4 +-
 src/main/cpp/Auto.cpp                         |  20 ++-
 src/main/cpp/Intake.cpp                       |   2 +-
 src/main/cpp/Robot.cpp                        |   2 +-
 .../deploy/pathplanner/autos/FourNote.auto    |   2 +-
 .../deploy/pathplanner/autos/FourNoteFar.auto |   2 +-
 .../pathplanner/autos/FourNoteTaxi.auto       |   2 +-
 .../pathplanner/autos/FourNoteTaxiBad.auto    | 136 ++++++++++++++++++
 .../deploy/pathplanner/autos/LeftOneNote.auto |   2 +-
 .../deploy/pathplanner/autos/OneNote.auto     |   2 +-
 .../deploy/pathplanner/autos/OneNoteTaxi.auto |   2 +-
 src/main/deploy/pathplanner/autos/Taxi.auto   |   2 +-
 .../deploy/pathplanner/autos/ThreeNote.auto   |   2 +-
 .../pathplanner/autos/ThreeNoteFar.auto       |   2 +-
 .../pathplanner/autos/ThreeNoteTaxi.auto      |   2 +-
 .../pathplanner/autos/ThreeNoteTaxiBad.auto   |  81 +++++++++++
 .../deploy/pathplanner/autos/TwoNote.auto     |   2 +-
 .../deploy/pathplanner/autos/TwoNoteTaxi.auto |   2 +-
 .../pathplanner/paths/BackFromSecondNote.path |  52 +++++++
 .../pathplanner/paths/BackFromThirdNote.path  |  52 +++++++
 .../deploy/pathplanner/paths/FirstNote.path   |   4 +-
 .../pathplanner/paths/FirstNoteGood.path      |   6 +-
 .../pathplanner/paths/GoToSecondNote.path     |  52 +++++++
 .../pathplanner/paths/GoToThirdNote.path      |  52 +++++++
 .../src/main/cpp/drivetrain/SwerveDrive.cpp   |  21 ++-
 .../behaviours/SwerveBehaviours.cpp           |  18 +++
 wombat/src/main/cpp/utils/Pathplanner.cpp     |  61 ++++++--
 .../drivetrain/behaviours/SwerveBehaviours.h  |  14 ++
 wombat/src/main/include/utils/Pathplanner.h   |   8 +-
 29 files changed, 566 insertions(+), 43 deletions(-)
 create mode 100644 src/main/deploy/pathplanner/autos/FourNoteTaxiBad.auto
 create mode 100644 src/main/deploy/pathplanner/autos/ThreeNoteTaxiBad.auto
 create mode 100644 src/main/deploy/pathplanner/paths/BackFromSecondNote.path
 create mode 100644 src/main/deploy/pathplanner/paths/BackFromThirdNote.path
 create mode 100644 src/main/deploy/pathplanner/paths/GoToSecondNote.path
 create mode 100644 src/main/deploy/pathplanner/paths/GoToThirdNote.path

diff --git a/simgui.json b/simgui.json
index d2bce9a8..2c54b6e6 100644
--- a/simgui.json
+++ b/simgui.json
@@ -27,11 +27,13 @@
   "NetworkTables": {
     "transitory": {
       "drivetrain": {
-        "open": true,
         "pid": {
           "open": true
         }
       },
+      "pathplanner": {
+        "open": true
+      },
       "vision": {
         "open": true
       }
diff --git a/src/main/cpp/Auto.cpp b/src/main/cpp/Auto.cpp
index 5fffd690..b10693d6 100644
--- a/src/main/cpp/Auto.cpp
+++ b/src/main/cpp/Auto.cpp
@@ -11,6 +11,7 @@
 #include "ShooterBehaviour.h"
 #include "behaviour/Behaviour.h"
 #include "drivetrain/behaviours/SwerveBehaviours.h"
+#include "frc/geometry/Pose2d.h"
 #include "units/angle.h"
 #include "utils/Pathplanner.h"
 
@@ -20,17 +21,22 @@ wom::SwerveAutoBuilder* autos::InitCommands(wom::drivetrain::SwerveDrive* _swerv
   wom::AutoCommands c = *new wom::AutoCommands(
       {// {"ArmToSetPoint", [_alphaArm]() { return wom::make<ArmToSetPoint>(_alphaArm, 20_deg); }},
        // {"Shoot", [_shooter]() { return wom::make<AutoShoot>(_shooter); }},
-       {"IntakeNote",
+        {"IntakeNote",
         [_intake]() {
           return wom::make<IntakeNote>(_intake) /*->Until(wom::make<behaviour::WaitTime>(3_s))*/;
         }},
-       {"PassNote",
+        {"PassNote",
         [_intake]() { return wom::make<PassNote>(_intake)->Until(wom::make<behaviour::WaitTime>(1_s)); }},
-       {"EjectNote", [_intake]() { return wom::make<EjectNote>(_intake)->WithTimeout(1_s); }},
-       {"Shoot", [_shooter, _intake]() {
+        {"EjectNote", [_intake]() { return wom::make<EjectNote>(_intake)->WithTimeout(1_s); }},
+        {"Shoot", [_shooter, _intake]() {
           return wom::make<AutoShooter>(_shooter, _intake, 300_rad_per_s)
               ->Until(wom::make<wom::WaitTime>(2_s));
-        }}});
+        }},
+        {"TurnToThirdNote", [_swerveDrive]() { return wom::make<wom::TurnToAngleBehaviour>(_swerveDrive, -50_deg); }},
+        {"TurnToSpeakerFromThird", [_swerveDrive]() { return wom::make<wom::TurnToAngleBehaviour>(_swerveDrive, 15_deg); }},
+        {"TurnToSecondNote", [_swerveDrive]() { return wom::make<wom::TurnToAngleBehaviour>(_swerveDrive, 30_deg); }},
+        {"TurnToSpeakerFromSecond", [_swerveDrive]() { return wom::make<wom::TurnToAngleBehaviour>(_swerveDrive, -10_deg); }}
+      });
 
   return new wom::utils::SwerveAutoBuilder(_swerveDrive, "Taxi", c);
 }
@@ -47,11 +53,11 @@ std::shared_ptr<behaviour::Behaviour> autos::OneNoteTaxi(wom::SwerveAutoBuilder*
 }
 
 std::shared_ptr<behaviour::Behaviour> autos::FourNoteTaxi(wom::SwerveAutoBuilder* builder) {
-  return builder->GetAutoRoutine("FourNoteTaxi");
+  return builder->GetAutoRoutine("FourNoteTaxiBad");
 }
 
 std::shared_ptr<behaviour::Behaviour> autos::ThreeNoteTaxi(wom::SwerveAutoBuilder* builder) {
-  return builder->GetAutoRoutine("ThreeNoteTaxi");
+  return builder->GetAutoRoutine("ThreeNoteTaxiBad");
 }
 
 // std::shared_ptr<behaviour::Behaviour> autos::QuadrupleClose(wom::drivetrain::SwerveDrive* _swerveDrive,
diff --git a/src/main/cpp/Intake.cpp b/src/main/cpp/Intake.cpp
index dc657768..9b27395b 100644
--- a/src/main/cpp/Intake.cpp
+++ b/src/main/cpp/Intake.cpp
@@ -12,7 +12,7 @@
 Intake::Intake(IntakeConfig config)
     : _config(config),
       _pid(frc::PIDController(0.02, 0, 0, 0.05_s)),
-      _pidPosition(frc::PIDController(1, 0, 0, 0.05_s)) {}
+      _pidPosition(frc::PIDController(1.2, 0, 0, 0.05_s)) {}
 
 IntakeConfig Intake::GetConfig() {
   return _config;
diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp
index 60e0b1d9..ef227d90 100644
--- a/src/main/cpp/Robot.cpp
+++ b/src/main/cpp/Robot.cpp
@@ -149,7 +149,7 @@ void Robot::AutonomousInit() {
   robotmap._builder = autos::InitCommands(_swerveDrive, shooter, intake, alphaArm);
 
   if (m_autoSelected == "kTaxi") {
-    sched->Schedule(autos::Taxi(robotmap._builder));
+    sched->Schedule(autos::FourNoteTaxi(robotmap._builder));
   }
   // sched->Schedule(robotmap._builder->GetAutoRoutine("OneNoteTaxi"));
 
diff --git a/src/main/deploy/pathplanner/autos/FourNote.auto b/src/main/deploy/pathplanner/autos/FourNote.auto
index e25e76c2..b452b4d8 100644
--- a/src/main/deploy/pathplanner/autos/FourNote.auto
+++ b/src/main/deploy/pathplanner/autos/FourNote.auto
@@ -94,4 +94,4 @@
   },
   "folder": null,
   "choreoAuto": false
-}
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/autos/FourNoteFar.auto b/src/main/deploy/pathplanner/autos/FourNoteFar.auto
index a88e8503..7a3134a1 100644
--- a/src/main/deploy/pathplanner/autos/FourNoteFar.auto
+++ b/src/main/deploy/pathplanner/autos/FourNoteFar.auto
@@ -88,4 +88,4 @@
   },
   "folder": null,
   "choreoAuto": false
-}
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/autos/FourNoteTaxi.auto b/src/main/deploy/pathplanner/autos/FourNoteTaxi.auto
index 2ff4540e..b2a19a5b 100644
--- a/src/main/deploy/pathplanner/autos/FourNoteTaxi.auto
+++ b/src/main/deploy/pathplanner/autos/FourNoteTaxi.auto
@@ -100,4 +100,4 @@
   },
   "folder": null,
   "choreoAuto": false
-}
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/autos/FourNoteTaxiBad.auto b/src/main/deploy/pathplanner/autos/FourNoteTaxiBad.auto
new file mode 100644
index 00000000..21c3ab0f
--- /dev/null
+++ b/src/main/deploy/pathplanner/autos/FourNoteTaxiBad.auto
@@ -0,0 +1,136 @@
+{
+  "version": 1.0,
+  "startingPose": null,
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "named",
+          "data": {
+            "name": "Shoot"
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "FirstNote"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "IntakeNote"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "BackwardsWorse"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Shoot"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "TurnToSecondNote"
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "GoToSecondNote"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "IntakeNote"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "BackFromSecondNote"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "TurnToSpeakerFromSecond"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Shoot"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "TurnToThirdNote"
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "GoToThirdNote"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "IntakeNote"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "BackFromThirdNote"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "TurnToSpeakerFromThird"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Shoot"
+          }
+        }
+      ]
+    }
+  },
+  "folder": null,
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/autos/LeftOneNote.auto b/src/main/deploy/pathplanner/autos/LeftOneNote.auto
index 9920e20f..3757a645 100644
--- a/src/main/deploy/pathplanner/autos/LeftOneNote.auto
+++ b/src/main/deploy/pathplanner/autos/LeftOneNote.auto
@@ -40,4 +40,4 @@
   },
   "folder": null,
   "choreoAuto": false
-}
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/autos/OneNote.auto b/src/main/deploy/pathplanner/autos/OneNote.auto
index 634bc54a..50f060f1 100644
--- a/src/main/deploy/pathplanner/autos/OneNote.auto
+++ b/src/main/deploy/pathplanner/autos/OneNote.auto
@@ -22,4 +22,4 @@
   },
   "folder": null,
   "choreoAuto": false
-}
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/autos/OneNoteTaxi.auto b/src/main/deploy/pathplanner/autos/OneNoteTaxi.auto
index e60adbc5..61000c9e 100644
--- a/src/main/deploy/pathplanner/autos/OneNoteTaxi.auto
+++ b/src/main/deploy/pathplanner/autos/OneNoteTaxi.auto
@@ -22,4 +22,4 @@
   },
   "folder": null,
   "choreoAuto": false
-}
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/autos/Taxi.auto b/src/main/deploy/pathplanner/autos/Taxi.auto
index 4a3cc737..0976f300 100644
--- a/src/main/deploy/pathplanner/autos/Taxi.auto
+++ b/src/main/deploy/pathplanner/autos/Taxi.auto
@@ -47,4 +47,4 @@
   },
   "folder": null,
   "choreoAuto": false
-}
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/autos/ThreeNote.auto b/src/main/deploy/pathplanner/autos/ThreeNote.auto
index 955deb9a..6159ba17 100644
--- a/src/main/deploy/pathplanner/autos/ThreeNote.auto
+++ b/src/main/deploy/pathplanner/autos/ThreeNote.auto
@@ -70,4 +70,4 @@
   },
   "folder": null,
   "choreoAuto": false
-}
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/autos/ThreeNoteFar.auto b/src/main/deploy/pathplanner/autos/ThreeNoteFar.auto
index c9796575..6c448793 100644
--- a/src/main/deploy/pathplanner/autos/ThreeNoteFar.auto
+++ b/src/main/deploy/pathplanner/autos/ThreeNoteFar.auto
@@ -64,4 +64,4 @@
   },
   "folder": null,
   "choreoAuto": false
-}
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/autos/ThreeNoteTaxi.auto b/src/main/deploy/pathplanner/autos/ThreeNoteTaxi.auto
index d0190920..72836db4 100644
--- a/src/main/deploy/pathplanner/autos/ThreeNoteTaxi.auto
+++ b/src/main/deploy/pathplanner/autos/ThreeNoteTaxi.auto
@@ -70,4 +70,4 @@
   },
   "folder": null,
   "choreoAuto": false
-}
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/autos/ThreeNoteTaxiBad.auto b/src/main/deploy/pathplanner/autos/ThreeNoteTaxiBad.auto
new file mode 100644
index 00000000..35422857
--- /dev/null
+++ b/src/main/deploy/pathplanner/autos/ThreeNoteTaxiBad.auto
@@ -0,0 +1,81 @@
+{
+  "version": 1.0,
+  "startingPose": null,
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "named",
+          "data": {
+            "name": "Shoot"
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "FirstNote"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "IntakeNote"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "BackwardsWorse"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Shoot"
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "GoToSecondNote"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "IntakeNote"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "BackFromSecondNote"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Shoot"
+          }
+        }
+      ]
+    }
+  },
+  "folder": null,
+  "choreoAuto": false
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/autos/TwoNote.auto b/src/main/deploy/pathplanner/autos/TwoNote.auto
index a0689578..a33df241 100644
--- a/src/main/deploy/pathplanner/autos/TwoNote.auto
+++ b/src/main/deploy/pathplanner/autos/TwoNote.auto
@@ -46,4 +46,4 @@
   },
   "folder": null,
   "choreoAuto": false
-}
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/autos/TwoNoteTaxi.auto b/src/main/deploy/pathplanner/autos/TwoNoteTaxi.auto
index 1473d20a..bdc155b0 100644
--- a/src/main/deploy/pathplanner/autos/TwoNoteTaxi.auto
+++ b/src/main/deploy/pathplanner/autos/TwoNoteTaxi.auto
@@ -52,4 +52,4 @@
   },
   "folder": null,
   "choreoAuto": false
-}
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/BackFromSecondNote.path b/src/main/deploy/pathplanner/paths/BackFromSecondNote.path
new file mode 100644
index 00000000..cb0d9c08
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/BackFromSecondNote.path
@@ -0,0 +1,52 @@
+{
+  "version": 1.0,
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 1.2,
+        "y": 1.8
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 0.24924928537446073,
+        "y": 0.4117108365457816
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 0.0,
+        "y": 0.0
+      },
+      "prevControl": {
+        "x": 0.40057438174790605,
+        "y": 0.5504093176517296
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 1.0,
+    "maxAcceleration": 1.5,
+    "maxAngularVelocity": 100.0,
+    "maxAngularAcceleration": 150.0
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 0,
+    "rotateFast": false
+  },
+  "reversed": false,
+  "folder": null,
+  "previewStartingState": {
+    "rotation": 56.42020002146988,
+    "velocity": 0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/BackFromThirdNote.path b/src/main/deploy/pathplanner/paths/BackFromThirdNote.path
new file mode 100644
index 00000000..c25fa3f8
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/BackFromThirdNote.path
@@ -0,0 +1,52 @@
+{
+  "version": 1.0,
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 1.3,
+        "y": -1.8
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 0.3492492853744608,
+        "y": -3.1882891634542183
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 0.0,
+        "y": 0.0
+      },
+      "prevControl": {
+        "x": 0.40057438174790605,
+        "y": 0.5504093176517296
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 1.0,
+    "maxAcceleration": 1.5,
+    "maxAngularVelocity": 100.0,
+    "maxAngularAcceleration": 150.0
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 0,
+    "rotateFast": false
+  },
+  "reversed": false,
+  "folder": null,
+  "previewStartingState": {
+    "rotation": 56.42020002146988,
+    "velocity": 0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/FirstNote.path b/src/main/deploy/pathplanner/paths/FirstNote.path
index 3dac4fc9..6317d90a 100644
--- a/src/main/deploy/pathplanner/paths/FirstNote.path
+++ b/src/main/deploy/pathplanner/paths/FirstNote.path
@@ -16,11 +16,11 @@
     },
     {
       "anchor": {
-        "x": 1.7,
+        "x": 1.8,
         "y": -0.0
       },
       "prevControl": {
-        "x": 0.7,
+        "x": 0.8,
         "y": 0.0
       },
       "nextControl": null,
diff --git a/src/main/deploy/pathplanner/paths/FirstNoteGood.path b/src/main/deploy/pathplanner/paths/FirstNoteGood.path
index 6236212e..27f75bf1 100644
--- a/src/main/deploy/pathplanner/paths/FirstNoteGood.path
+++ b/src/main/deploy/pathplanner/paths/FirstNoteGood.path
@@ -3,12 +3,12 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 1.325730468864645,
+        "x": 1.33,
         "y": 5.5040931765173085
       },
       "prevControl": null,
       "nextControl": {
-        "x": 2.325730468864644,
+        "x": 2.329999999999999,
         "y": 5.5040931765173085
       },
       "isLocked": false,
@@ -49,4 +49,4 @@
     "velocity": 0
   },
   "useDefaultConstraints": false
-}
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/GoToSecondNote.path b/src/main/deploy/pathplanner/paths/GoToSecondNote.path
new file mode 100644
index 00000000..d4fd37d3
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/GoToSecondNote.path
@@ -0,0 +1,52 @@
+{
+  "version": 1.0,
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 0.0,
+        "y": 0.0
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 0.9744053724911999,
+        "y": 1.346746202764874
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 1.2,
+        "y": 1.8
+      },
+      "prevControl": {
+        "x": 0.4483335066527455,
+        "y": 0.7044817501903187
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 1.0,
+    "maxAcceleration": 1.5,
+    "maxAngularVelocity": 100.0,
+    "maxAngularAcceleration": 150.0
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 52.11278203254082,
+    "rotateFast": false
+  },
+  "reversed": false,
+  "folder": null,
+  "previewStartingState": {
+    "rotation": 50.75873366922985,
+    "velocity": 0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/GoToThirdNote.path b/src/main/deploy/pathplanner/paths/GoToThirdNote.path
new file mode 100644
index 00000000..cade06f7
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/GoToThirdNote.path
@@ -0,0 +1,52 @@
+{
+  "version": 1.0,
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 0.0,
+        "y": 0.0
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 1.2733841279990303,
+        "y": -1.0684961521509886
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 1.3,
+        "y": -1.8
+      },
+      "prevControl": {
+        "x": 0.2822377369416278,
+        "y": -0.945996060441566
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 1.0,
+    "maxAcceleration": 1.5,
+    "maxAngularVelocity": 100.0,
+    "maxAngularAcceleration": 150.0
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 52.11278203254082,
+    "rotateFast": false
+  },
+  "reversed": false,
+  "folder": null,
+  "previewStartingState": {
+    "rotation": 50.75873366922985,
+    "velocity": 0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/wombat/src/main/cpp/drivetrain/SwerveDrive.cpp b/wombat/src/main/cpp/drivetrain/SwerveDrive.cpp
index 849a780d..48020f5a 100644
--- a/wombat/src/main/cpp/drivetrain/SwerveDrive.cpp
+++ b/wombat/src/main/cpp/drivetrain/SwerveDrive.cpp
@@ -303,7 +303,7 @@ void SwerveModule::OnUpdate(units::second_t dt) {
   //     units::math::max(units::math::min(driveVoltage, voltageMax), voltageMin);
 
   driveVoltage = units::math::min(driveVoltage, 7_V);
-  turnVoltage = units::math::min(turnVoltage, 4_V);
+  turnVoltage = units::math::min(turnVoltage, 6_V);
 
   // driveVoltage = units::math::min(
   //     units::math::max(driveVoltage, -_driveModuleVoltageLimit),
@@ -431,7 +431,7 @@ SwerveDrive::SwerveDrive(SwerveDriveConfig config, frc::Pose2d initialPose)
       _anglePIDController{PIDController(0, 0, 0)},
       _xPIDController(PIDController(4, 0.1, 0)),
       _yPIDController(PIDController(4, 0.1, 0)),
-      _turnPIDController(PIDController(1, 0, 0)),
+      _turnPIDController(PIDController(7, 0, 0)),
       // _xPIDController(std::string path, config_t initialGains)
       // _xPIDController(config.path + "/pid/x", _config.posePositionPID),
       // _yPIDController(config.path + "/pid/y", _config.posePositionPID),
@@ -675,10 +675,23 @@ void SwerveDrive::SetFieldRelativeVelocity(FieldRelativeSpeeds speeds) {
 
 void SwerveDrive::SetPose(frc::Pose2d pose) {
   _state = SwerveDriveState::kPose;
-  _anglePIDController.SetSetpoint(pose.Rotation().Radians().value() - 3.14159);
+  if (pose.X() > 4_m) {
+      pose = frc::Pose2d(0_m, pose.Y(), pose.Rotation());
+    }
+    if (pose.Y() > 4_m) {
+      pose = frc::Pose2d(pose.X(), 0_m, pose.Rotation());
+    }
+
+    if (pose.X() < -4_m) {
+      pose = frc::Pose2d(0_m, pose.Y(), pose.Rotation());
+    }
+    if (pose.Y() < -4_m) {
+      pose = frc::Pose2d(pose.X(), 0_m, pose.Rotation());
+  }
+  _anglePIDController.SetSetpoint(pose.Rotation().Radians().value()/* - 3.14159*/);
   _xPIDController.SetSetpoint(pose.X().value());
   _yPIDController.SetSetpoint(pose.Y().value());
-
+  // 
   utils::WritePose2NT(nt::NetworkTableInstance::GetDefault().GetTable("swerveSetpoint"), GetSetpoint());
 }
 
diff --git a/wombat/src/main/cpp/drivetrain/behaviours/SwerveBehaviours.cpp b/wombat/src/main/cpp/drivetrain/behaviours/SwerveBehaviours.cpp
index 0ce0e093..5562f2dd 100644
--- a/wombat/src/main/cpp/drivetrain/behaviours/SwerveBehaviours.cpp
+++ b/wombat/src/main/cpp/drivetrain/behaviours/SwerveBehaviours.cpp
@@ -274,3 +274,21 @@ void wom::drivetrain::behaviours::DrivebasePoseBehaviour::OnTick(units::second_t
     }
   }
 }
+
+wom::drivetrain::behaviours::TurnToAngleBehaviour::TurnToAngleBehaviour(wom::drivetrain::SwerveDrive* swerve, units::radian_t angle) : _angle(angle), _swerve(swerve) {
+  Controls(swerve);
+}
+
+void wom::drivetrain::behaviours::TurnToAngleBehaviour::OnTick(units::second_t dt) {
+  _swerve->TurnToAngle(_angle);
+
+  nt::NetworkTableInstance::GetDefault().GetTable("drivetrain")->GetEntry("targetAngle").SetDouble(_angle.value());
+  nt::NetworkTableInstance::GetDefault().GetTable("drivetrain")->GetEntry("runningangle").SetBoolean(true);
+
+  if (units::math::abs(_swerve->GetPose().Rotation().Radians() - _angle) < 0.1_rad) {
+    nt::NetworkTableInstance::GetDefault().GetTable("drivetrain")->GetEntry("runningangle").SetBoolean(false);
+    SetDone();
+  }
+}
+
+
diff --git a/wombat/src/main/cpp/utils/Pathplanner.cpp b/wombat/src/main/cpp/utils/Pathplanner.cpp
index 7954e07b..6621363e 100644
--- a/wombat/src/main/cpp/utils/Pathplanner.cpp
+++ b/wombat/src/main/cpp/utils/Pathplanner.cpp
@@ -9,10 +9,10 @@
 #include <pathplanner/lib/util/HolonomicPathFollowerConfig.h>
 #include <pathplanner/lib/util/PIDConstants.h>
 #include <pathplanner/lib/util/ReplanningConfig.h>
-
 #include <algorithm>
 #include <array>
 #include <cmath>
+#include <cstdlib>
 #include <fstream>
 #include <initializer_list>
 #include <optional>
@@ -36,6 +36,7 @@
 #include "pathplanner/lib/path/PathPoint.h"
 #include "pathplanner/lib/path/RotationTarget.h"
 #include "units/acceleration.h"
+#include "units/base.h"
 #include "units/time.h"
 #include "utils/Util.h"
 #include "wpi/detail/conversions/to_json.h"
@@ -233,8 +234,8 @@ frc::Trajectory utils::PathWeaver::getTrajectory(std::string_view path) {
 }
 
 // FollowPath implementation
-utils::FollowPath::FollowPath(drivetrain::SwerveDrive* swerve, std::string path, bool flip)
-    : _swerve(swerve), behaviour::Behaviour("<Follow Path: " + path + ">") {
+utils::FollowPath::FollowPath(drivetrain::SwerveDrive* swerve, std::string path, bool flip, frc::Pose2d offset)
+    : _swerve(swerve), behaviour::Behaviour("<Follow Path: " + path + ">"), _offset(offset), _pathName(path) {
   Controls(swerve);
 
   _path = pathplanner::PathPlannerPath::fromPathFile(path);
@@ -267,6 +268,8 @@ utils::FollowPath::FollowPath(drivetrain::SwerveDrive* swerve, std::string path,
   std::vector<pathplanner::PathPoint> points =
       pathplanner::PathPlannerPath::fromPathFile(path)->getAllPathPoints();
 
+  points = CheckPoints(points);
+
   pathplanner::RotationTarget* lastRot = nullptr;
   units::degree_t rot;
 
@@ -293,9 +296,10 @@ utils::FollowPath::FollowPath(drivetrain::SwerveDrive* swerve, std::string path,
 
     frc::Translation2d tr = frc::Translation2d(point.position.X() * -1, point.position.Y() * -1);
     frc::Pose2d pose2 = frc::Pose2d(tr, rot);  //.TransformBy(frc::Transform2d(-1.37_m, -5.56_m, 0_deg));
-    //
+
     if (i == index || i == static_cast<int>(tot - 1) || f) {
-      _poses.emplace_back(frc::Pose2d(pose2.X(), pose2.Y(), pose2.Rotation()));
+      WritePose2NT(nt::NetworkTableInstance::GetDefault().GetTable("pathplanner/offset"), _offset); // -6 -6
+      _poses.emplace_back(frc::Pose2d(pose2.X() - _offset.X(), pose2.Y() - _offset.Y(), 0_deg));
       // _poses.emplace_back(frc::Pose2d(pose2.Y(), pose2.X(), pose2.Rotation()));
       i = 0;  //  - 5.56_m,  - 2.91_m
       f = false;
@@ -350,6 +354,26 @@ utils::FollowPath::FollowPath(drivetrain::SwerveDrive* swerve, std::string path,
   // _swerve->TurnToAngle(_poses[_currentPose].Rotation().Radians());
 }
 
+std::vector<pathplanner::PathPoint> utils::FollowPath::CheckPoints(std::vector<pathplanner::PathPoint> points) {
+  units::meter_t threshhold = 4_m;  
+  bool posesgood = true;
+
+  for (auto point : points) {
+    if (std::abs(point.position.X().value()) > threshhold.value() || std::abs(point.position.Y().value()) > threshhold.value()) {
+      posesgood = false;
+    }
+  }
+
+  if (!posesgood) {
+    std::vector<pathplanner::PathPoint> _points =
+      pathplanner::PathPlannerPath::fromPathFile(_pathName)->getAllPathPoints();
+
+    return CheckPoints(_points);
+  }
+
+  return points; 
+}
+
 void utils::FollowPath::CalcTimer() {
   frc::Pose2d current_pose = _swerve->GetPose();
   frc::Pose2d target_pose = _poses[_currentPose];
@@ -362,6 +386,9 @@ void utils::FollowPath::CalcTimer() {
   _timer.Stop();
   _timer.Reset();
   _time = units::second_t{std::abs(dist.value()) / 1 /*meters per second ?No?*/};
+
+  _time = units::math::min(_time, 4_s);
+
   // _time = 15_s;
   _timer.Start();
 }
@@ -402,6 +429,7 @@ void utils::FollowPath::OnTick(units::second_t dt) {
       .SetInteger(static_cast<int>(_poses.size()));
 
   std::cout << "Following Path" << std::endl;
+  _poses[_currentPose] = frc::Pose2d(_poses[_currentPose].X(), _poses[_currentPose].Y(), _swerve->GetPose().Rotation());    
 
   // if (_swerve->IsAtSetAngle() && _swerve->GetState() == drivetrain::SwerveDriveState::kAngle) {
   _swerve->SetPose(_poses[_currentPose]);
@@ -455,12 +483,14 @@ void utils::AutoBuilder::SetAuto(std::string path) {
     cjson += cdata;
   }
 
-  cjson.pop_back();
+  // cjson.pop_back();
+
+  std::cout << cjson << std::endl;
 
   wpi::json j = wpi::json::parse(cjson);
 
   _currentPath = &j;
-  // _startingPose = &j["startingPose"];
+  _startingPose = &j["startingPose"];
   _commands = &j["command"]["data"]["commands"];
 
   commands = std::vector<std::pair<std::string, std::string>>();
@@ -492,7 +522,16 @@ void utils::AutoBuilder::SetAuto(std::string path) {
   int i = 0;
   int pathamt = 0;
   int commandamt = 0;
-
+  
+  frc::Pose2d startPose; 
+
+  if (!_startingPose->is_null()) {
+    // startPose = JSONPoseToPose2d(*_startingPose);
+    startPose = frc::Pose2d();
+  } else {
+    startPose = frc::Pose2d();
+  }
+ 
   for (auto command : *_commands) {
     nt::NetworkTableInstance::GetDefault()
         .GetTable("commands/" + std::to_string(i))
@@ -504,7 +543,7 @@ void utils::AutoBuilder::SetAuto(std::string path) {
         .SetString(static_cast<std::string>(command["data"].dump()));
 
     if (command["type"] == "path") {
-      _pathplan->Add(behaviour::make<FollowPath>(_swerve, command["data"]["pathName"], _flip));
+      _pathplan->Add(behaviour::make<FollowPath>(_swerve, command["data"]["pathName"], _flip, startPose));
       pathamt++;
       nt::NetworkTableInstance::GetDefault()
           .GetTable("commands/" + std::to_string(i))
@@ -531,7 +570,7 @@ void utils::AutoBuilder::SetAuto(std::string path) {
             ->GetEntry("typeisstring")
             .SetBoolean(c["type"].is_string());
         if (static_cast<std::string>(c["type"]) == "path") {
-          nb->Add(behaviour::make<FollowPath>(_swerve, c["data"]["pathName"], _flip));
+          nb->Add(behaviour::make<FollowPath>(_swerve, c["data"]["pathName"], _flip, startPose));
           pathamt++;
         } else if (static_cast<std::string>(c["type"]) == "named") {
           nb->Add(_commandsList.Run(c["data"]["name"]));
@@ -589,6 +628,8 @@ frc::Pose2d utils::AutoBuilder::JSONPoseToPose2d(wpi::json j) {
   // std::cout << j["position"].is_object() << std::endl;
   // std::cout << j << std::endl;
 
+  std::cout << j.dump() << std::endl;
+
   return frc::Pose2d(units::meter_t{j["position"]["x"]}, units::meter_t{j["position"]["y"]},
                      units::degree_t{j["rotation"]});
   // return frc::Pose2d();
diff --git a/wombat/src/main/include/drivetrain/behaviours/SwerveBehaviours.h b/wombat/src/main/include/drivetrain/behaviours/SwerveBehaviours.h
index 7db5560c..62a0c7d7 100644
--- a/wombat/src/main/include/drivetrain/behaviours/SwerveBehaviours.h
+++ b/wombat/src/main/include/drivetrain/behaviours/SwerveBehaviours.h
@@ -16,7 +16,10 @@
 #include <string>
 
 #include "behaviour/Behaviour.h"
+#include "behaviour/HasBehaviour.h"
 #include "drivetrain/SwerveDrive.h"
+#include "units/angle.h"
+#include "units/time.h"
 #include "utils/Pathplanner.h"
 
 namespace wom {
@@ -235,6 +238,17 @@ class DrivebasePoseBehaviour : public behaviour::Behaviour {
   std::shared_ptr<nt::NetworkTable> _swerveDriveTable =
       nt::NetworkTableInstance::GetDefault().GetTable("swerve");
 };
+
+class TurnToAngleBehaviour : public behaviour::Behaviour {
+public:
+  TurnToAngleBehaviour(SwerveDrive* swerve, units::radian_t angle);
+
+  void OnTick(units::second_t dt) override;
+
+private:
+  units::radian_t _angle;
+  SwerveDrive* _swerve;
+};
 }  // namespace behaviours
 }  // namespace drivetrain
 }  // namespace wom
diff --git a/wombat/src/main/include/utils/Pathplanner.h b/wombat/src/main/include/utils/Pathplanner.h
index 51327848..d2481543 100644
--- a/wombat/src/main/include/utils/Pathplanner.h
+++ b/wombat/src/main/include/utils/Pathplanner.h
@@ -250,7 +250,7 @@ class PathWeaver {
 
 class FollowPath : public behaviour::Behaviour {
  public:
-  FollowPath(drivetrain::SwerveDrive* swerve, std::string path, bool flip = false);
+  FollowPath(drivetrain::SwerveDrive* swerve, std::string path, bool flip, frc::Pose2d offset);
 
   void OnTick(units::time::second_t dt) override;
 
@@ -259,6 +259,8 @@ class FollowPath : public behaviour::Behaviour {
   void OnStart() override;
 
  private:
+  std::vector<pathplanner::PathPoint> CheckPoints(std::vector<pathplanner::PathPoint> points);
+
   units::second_t _time;
   frc::Timer _timer;
 
@@ -269,6 +271,8 @@ class FollowPath : public behaviour::Behaviour {
   drivetrain::SwerveDrive* _swerve;
 
   int _currentPose = 0;
+
+  frc::Pose2d _offset;
 };
 
 class AutoBuilder {
@@ -294,7 +298,7 @@ class AutoBuilder {
   AutoCommands _commandsList;
 
   wpi::json* _currentPath;
-  // wpi::json* _startingPose;
+  wpi::json* _startingPose;
   wpi::json* _commands;
 
   std::shared_ptr<behaviour::SequentialBehaviour> pathplan;

From 4117047c4b2d14bb38f95dfa844e2fa7bcb5d5ca Mon Sep 17 00:00:00 2001
From: Isaac Turner <spacey_sooty@outlook.com>
Date: Sat, 9 Mar 2024 11:58:34 +0800
Subject: [PATCH 205/207] remove unnecessary network calls when competing

---
 build.gradle                                  | 48 +++++++++++++++++++
 debug.gradle                                  | 22 +++++++++
 src/main/cpp/AlphaArm.cpp                     | 20 ++++++++
 src/main/cpp/AlphaArmBehaviour.cpp            |  2 +
 src/main/cpp/Climber.cpp                      |  4 ++
 src/main/cpp/Intake.cpp                       |  5 +-
 src/main/cpp/Robot.cpp                        |  2 +
 src/main/cpp/Shooter.cpp                      | 16 +++++++
 src/main/cpp/vision/Vision.cpp                | 15 ++++++
 wombat/build.gradle                           | 24 ++++++++++
 .../src/main/cpp/drivetrain/SwerveDrive.cpp   | 22 ++++++++-
 .../behaviours/SwerveBehaviours.cpp           | 10 ++++
 wombat/src/main/cpp/subsystems/Shooter.cpp    |  2 +
 wombat/src/main/cpp/utils/Pathplanner.cpp     | 47 +++++++++++++++---
 wombat/src/main/cpp/vision/Limelight.cpp      |  2 +
 wombat/src/main/include/utils/PID.h           |  2 +
 wombat/src/main/include/utils/Util.h          |  4 ++
 17 files changed, 238 insertions(+), 9 deletions(-)
 create mode 100644 debug.gradle

diff --git a/build.gradle b/build.gradle
index 728f2b3b..f3055148 100644
--- a/build.gradle
+++ b/build.gradle
@@ -65,6 +65,30 @@ model {
             }
 
 	    binaries.all {
+        //apply from: 'debug.gradle'
+        if (!project.hasProperty("opt")) {
+          def opt;
+          if (opt == null) {
+            opt = "release"
+          }
+
+          if (!opt == "release") {
+            cppCompiler.define "DEBUG"
+          } else {
+            cppCompiler.define "RELEASE"
+          }
+        } else {
+          if (opt == null) {
+            opt = "release"
+          }
+
+          if (!opt == "release") {
+            cppCompiler.define "DEBUG"
+          } else {
+            cppCompiler.define "RELEASE"
+          }
+        }
+
 	    	lib project: ':wombat', library: 'Wombat', linkage: 'static'
 	    }
 
@@ -93,6 +117,30 @@ model {
             }
 
 	binaries.all {
+      //apply from: 'debug.gradle'
+      if (!project.hasProperty("opt")) {
+        def opt;
+        if (opt == null) {
+          opt = "release"
+        }
+
+        if (!opt == "release") {
+          cppCompiler.define "DEBUG"
+        } else {
+          cppCompiler.define "RELEASE"
+        }
+      } else {
+        if (opt == null) {
+          opt = "release"
+        }
+
+        if (!opt == "release") {
+          cppCompiler.define "DEBUG"
+        } else {
+          cppCompiler.define "RELEASE"
+        }
+      }
+
 	    lib project: ':wombat', library: 'Wombat', linkage: 'static'
 	}
 
diff --git a/debug.gradle b/debug.gradle
new file mode 100644
index 00000000..fa412367
--- /dev/null
+++ b/debug.gradle
@@ -0,0 +1,22 @@
+if (!project.hasProperty("opt")) {
+  def opt;
+  if (opt == null) {
+    opt = "release"
+  }
+
+  if (!opt == "release") {
+    cppCompiler.define "DEBUG"
+  } else {
+    cppCompiler.define "RELEASE"
+  }
+} else {
+  if (opt == null) {
+    opt = "release"
+  }
+
+  if (!opt == "release") {
+    cppCompiler.define "DEBUG"
+  } else {
+    cppCompiler.define "RELEASE"
+  }
+}
diff --git a/src/main/cpp/AlphaArm.cpp b/src/main/cpp/AlphaArm.cpp
index 96885b98..b08bf4cd 100644
--- a/src/main/cpp/AlphaArm.cpp
+++ b/src/main/cpp/AlphaArm.cpp
@@ -23,7 +23,9 @@ void AlphaArm::OnUpdate(units::second_t dt) {
       _setAlphaArmVoltage = 0_V;
       break;
     case AlphaArmState::kRaw:
+      #ifdef DEBUG
       std::cout << "RawControl" << std::endl;
+      #endif
       _setAlphaArmVoltage = _rawArmVoltage;
 
       break;
@@ -36,7 +38,9 @@ void AlphaArm::OnUpdate(units::second_t dt) {
           _pidIntakeState.Calculate(-(_config->alphaArmEncoder.GetEncoderPosition().value() * (2 * 3.1415)))};
     } break;
     case AlphaArmState::kAmpAngle:
+      #ifdef DEBUG
       std::cout << "Amp Angle" << std::endl;
+      #endif
 
       _pidArmStates.SetSetpoint(-2.12);
       _setAlphaArmVoltage = units::volt_t{
@@ -45,33 +49,43 @@ void AlphaArm::OnUpdate(units::second_t dt) {
       break;
 
     case AlphaArmState::kIntakeAngle:
+      #ifdef DEBUG
       std::cout << "Intake Angle" << std::endl;
+      #endif
       _pidIntakeState.SetSetpoint(-0.50);  //-0.48
       _setAlphaArmVoltage = units::volt_t{
           _pidIntakeState.Calculate(-(_config->alphaArmEncoder.GetEncoderPosition().value() * (2 * 3.1415)))};
       break;
     case AlphaArmState::kIntakedAngle:
+      #ifdef DEBUG
       std::cout << "Intake Angle" << std::endl;
+      #endif
       _pidIntakeState.SetSetpoint(-0.55);  //-0.48
       _setAlphaArmVoltage = units::volt_t{
           _pidIntakeState.Calculate(-(_config->alphaArmEncoder.GetEncoderPosition().value() * (2 * 3.1415)))};
       break;
     case AlphaArmState::kClimbAngle:
+      #ifdef DEBUG
       std::cout << "Climb Angle" << std::endl;
+      #endif
       _pidArmStates.SetSetpoint(-2.00);  //-0.48
       _setAlphaArmVoltage = units::volt_t{
           _pidArmStates.Calculate(-(_config->alphaArmEncoder.GetEncoderPosition().value() * (2 * 3.1415)))};
       break;
 
     case AlphaArmState::kClimbed:
+      #ifdef DEBUG
       std::cout << "Climb Angle" << std::endl;
+      #endif
       _pidClimberStates.SetSetpoint(-0.5);  //-0.48
       _setAlphaArmVoltage = units::volt_t{_pidClimberStates.Calculate(
           -(_config->alphaArmEncoder.GetEncoderPosition().value() * (2 * 3.1415)))};
       break;
 
     case AlphaArmState::kSpeakerAngle:
+      #ifdef DEBUG
       std::cout << "Speaker Angle" << std::endl;
+      #endif
       _pidIntakeState.SetSetpoint(-0.82);
       _setAlphaArmVoltage = units::volt_t{
           _pidIntakeState.Calculate(-(_config->alphaArmEncoder.GetEncoderPosition().value() * (2 * 3.1415)))};
@@ -84,7 +98,9 @@ void AlphaArm::OnUpdate(units::second_t dt) {
       // break;
 
     case AlphaArmState::kStowed:
+      #ifdef DEBUG
       std::cout << "Shuttling angle" << std::endl;
+      #endif
       _pidIntakeState.SetSetpoint(-1.20);
       _setAlphaArmVoltage = units::volt_t{
           _pidIntakeState.Calculate(-(_config->alphaArmEncoder.GetEncoderPosition().value() * (2 * 3.1415)))};
@@ -94,8 +110,10 @@ void AlphaArm::OnUpdate(units::second_t dt) {
   }
   _config->alphaArmGearbox.motorController->SetVoltage(_setAlphaArmVoltage);
   _config->alphaArmGearbox2.motorController->SetVoltage(_setAlphaArmVoltage);
+  #ifdef DEBUG
   std::cout << "Encoder Value: " << (_config->alphaArmEncoder.GetEncoderPosition().value() * (2 * 3.1415))
             << std::endl;
+  #endif
   _table->GetEntry("PID Error").SetDouble(_pidArm.GetPositionError());
   _table->GetEntry("SetPoint").SetDouble(_pidArm.GetSetpoint());
   _table->GetEntry("Input").SetDouble((_config->alphaArmEncoder.GetEncoderPosition().value() * (2 * 3.1415)));
@@ -106,7 +124,9 @@ void AlphaArm::OnUpdate(units::second_t dt) {
   _table->GetEntry("Intake SetPoint State").SetDouble(_pidIntakeState.GetSetpoint());
   _table->GetEntry("Intake PID Error State").SetDouble(_pidIntakeState.GetPositionError());
 
+  #ifdef DEBUG
   std::cout << "Voltage:" << _setAlphaArmVoltage.value() << std::endl;
+  #endif
 }
 
 void AlphaArm::SetState(AlphaArmState state) {
diff --git a/src/main/cpp/AlphaArmBehaviour.cpp b/src/main/cpp/AlphaArmBehaviour.cpp
index 273e9449..978dba07 100644
--- a/src/main/cpp/AlphaArmBehaviour.cpp
+++ b/src/main/cpp/AlphaArmBehaviour.cpp
@@ -19,8 +19,10 @@ AlphaArmConfig AlphaArm::GetConfig() {
 }
 
 void AlphaArmManualControl::OnTick(units::second_t dt) {
+  #ifdef DEBUG
   _table->GetEntry("State").SetBoolean(_rawControl);
   _table->GetEntry("Goal Value").SetBoolean(_gotValue);
+  #endif
 
   if (_codriver->GetBackButton()) {
     if (_rawControl == true) {
diff --git a/src/main/cpp/Climber.cpp b/src/main/cpp/Climber.cpp
index dbe7dbd8..1fdea0d0 100644
--- a/src/main/cpp/Climber.cpp
+++ b/src/main/cpp/Climber.cpp
@@ -26,8 +26,10 @@ void Climber::OnUpdate(units::second_t dt) {
       _pid.SetSetpoint(0.035 * 100);
       _setVoltage = -units::volt_t{
           _pid.Calculate((-_config.climberGearbox.encoder->GetEncoderPosition().value()) * 100)};
+  #ifdef DEBUG
       _table->GetEntry("PID OUTPUT")
           .SetDouble(_pid.Calculate((-_config.climberGearbox.encoder->GetEncoderPosition().value()) * 100));
+  #endif
     } break;
 
     case ClimberState::kArmDown: {
@@ -60,10 +62,12 @@ void Climber::OnUpdate(units::second_t dt) {
   }
   _config.climberGearbox.motorController->SetVoltage(_setVoltage);
 
+  #ifdef DEBUG
   _table->GetEntry("State: ").SetString(_stringStateName);
   _table->GetEntry("Motor Voltage: ").SetDouble(_setVoltage.value());
   _table->GetEntry("Encoder Output: ")
       .SetDouble(_config.climberGearbox.encoder->GetEncoderPosition().value() * 100);
+  #endif
 }
 
 void Climber::SetState(ClimberState state) {
diff --git a/src/main/cpp/Intake.cpp b/src/main/cpp/Intake.cpp
index 9b27395b..113b9b22 100644
--- a/src/main/cpp/Intake.cpp
+++ b/src/main/cpp/Intake.cpp
@@ -53,7 +53,9 @@ void Intake::OnUpdate(units::second_t dt) {
     } break;
 
     case IntakeState::kHold: {
+      #ifdef DEBUG
       std::cerr << "HEY FROM kHold" << std::endl;
+      #endif
       units::volt_t pidCalculate = 0_V;
       if (_config.intakeSensor->Get() == true && _config.passSensor->Get() == true) {
         SetState(IntakeState::kIdle);
@@ -100,6 +102,7 @@ void Intake::OnUpdate(units::second_t dt) {
       _setVoltage = -10_V;
     } break;
   }
+  #ifdef DEBUG
   _table->GetEntry("State").SetString(_stringStateName);
   _table->GetEntry("Motor Voltage").SetDouble(_setVoltage.value());
   _table->GetEntry("Intake Sensor").SetBoolean(_config.intakeSensor->Get());
@@ -109,7 +112,7 @@ void Intake::OnUpdate(units::second_t dt) {
   _table->GetEntry("Encoder").SetDouble(_config.IntakeGearbox.encoder->GetEncoderPosition().value());
   _table->GetEntry("Shot Count").SetDouble(_noteShot);
   // _table->GetEntry("Encoder: ").SetDouble(_config.IntakeGearbox.encoder->GetEncoderPosition().value());
-  //
+  #endif
   if (_timer.Get() < 4_s) {
     _setVoltage = units::math::min(0_V, _setVoltage);
   } else {
diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp
index ef227d90..a146a88a 100644
--- a/src/main/cpp/Robot.cpp
+++ b/src/main/cpp/Robot.cpp
@@ -98,10 +98,12 @@ void Robot::RobotPeriodic() {
   loop.Poll();
   wom::BehaviourScheduler::GetInstance()->Tick();
 
+  #ifdef DEBUG
   nt::NetworkTableInstance::GetDefault()
       .GetTable("drivetrainpose")
       ->GetEntry("state")
       .SetInteger(static_cast<int>(_swerveDrive->GetState()));
+  #endif
   // sched->Tick();
 
   // robotmap.swerveTable.swerveDriveTable->GetEntry("frontLeftEncoder")
diff --git a/src/main/cpp/Shooter.cpp b/src/main/cpp/Shooter.cpp
index 5fb03505..8f05aa43 100644
--- a/src/main/cpp/Shooter.cpp
+++ b/src/main/cpp/Shooter.cpp
@@ -11,19 +11,23 @@ void Shooter::OnStart() {
 }
 
 void Shooter::OnUpdate(units::second_t dt) {
+  #ifdef DEBUG
   table->GetEntry("Error").SetDouble(_pid.GetPositionError());
   table->GetEntry("SetPoint").SetDouble(_pid.GetSetpoint());
   table->GetEntry("Encoder Output")
       .SetDouble(_config.ShooterGearbox.encoder->GetEncoderAngularVelocity().value());
   table->GetEntry("Shooting").SetString(_statename);
   // table->GetEntry("PID CONTROLLER").Set(_pid);
+#endif
 
   switch (_state) {
     case ShooterState::kIdle: {
       _statename = "Idle";
       _pid.Reset();
       holdVoltage = 0_V;
+      #ifdef DEBUG
       std::cout << "KIdle" << std::endl;
+      #endif
       _setVoltage = 0_V;
       // if (_shooterSensor.Get()) {
       //   _state = ShooterState::kReverse;
@@ -31,11 +35,15 @@ void Shooter::OnUpdate(units::second_t dt) {
     } break;
     case ShooterState::kSpinUp: {
       _statename = "SpinUp";
+      #ifdef DEBUG
       std::cout << "KSpinUp" << std::endl;
+      #endif
       _pid.SetSetpoint(_goal.value());
       units::volt_t pidCalculate =
           units::volt_t{_pid.Calculate(_config.ShooterGearbox.encoder->GetEncoderAngularVelocity().value())};
+      #ifdef DEBUG
       std::cout << "KShooting" << std::endl;
+      #endif
 
       _setVoltage = pidCalculate;
 
@@ -57,11 +65,15 @@ void Shooter::OnUpdate(units::second_t dt) {
       _pid.SetSetpoint(_goal.value());
       units::volt_t pidCalculate =
           units::volt_t{_pid.Calculate(_config.ShooterGearbox.encoder->GetEncoderAngularVelocity().value())};
+      #ifdef DEBUG
       std::cout << "KShooting" << std::endl;
+      #endif
 
       if (_pid.GetPositionError() < 1 && _pid.GetVelocityError() < 1) {
         holdVoltage = pidCalculate;
+        #ifdef DEBUG
         std::cout << "STABLE" << std::endl;
+        #endif
       }
 
       if (holdVoltage.value() == 0) {
@@ -75,7 +87,9 @@ void Shooter::OnUpdate(units::second_t dt) {
       _statename = "Reverse";
       _pid.Reset();
       _setVoltage = -8_V;
+      #ifdef DEBUG
       std::cout << "KReverse" << std::endl;
+      #endif
       // if (!_shooterSensor.Get()) {
       //   SetState(ShooterState::kIdle);
       // }
@@ -85,7 +99,9 @@ void Shooter::OnUpdate(units::second_t dt) {
       holdVoltage = 0_V;
       _pid.Reset();
       _setVoltage = _rawVoltage;
+      #ifdef DEBUG
       std::cout << "KRaw" << std::endl;
+      #endif
       // if (_shooterSensor.Get()) {
       //   SetState(ShooterState::kRaw);
       // }
diff --git a/src/main/cpp/vision/Vision.cpp b/src/main/cpp/vision/Vision.cpp
index 3a6534a2..e7a11d1c 100644
--- a/src/main/cpp/vision/Vision.cpp
+++ b/src/main/cpp/vision/Vision.cpp
@@ -12,7 +12,9 @@
 #include "vision/Limelight.h"
 
 FMAP::FMAP(std::string path) : _path(path) {
+  #ifdef DEBUG
   std::cout << "Parsing FMAP" << std::endl;
+  #endif
 
   deploy_dir = frc::filesystem::GetDeployDirectory();
 
@@ -24,12 +26,16 @@ FMAP::FMAP(std::string path) : _path(path) {
 
   file >> j;
 
+  #ifdef DEBUG
   std::cout << j["fiducials"] << std::endl;
+  #endif
 
   // iterate through the json object and add each tag to the vector
 
   for (auto& fiducial : j["fiducials"]) {
+    #ifdef DEBUG
     std::cout << "Loading AprilTag " << fiducial["id"] << std::endl;
+    #endif
     AprilTag tag;
     tag.id = fiducial["id"];
     tag.size = fiducial["size"];
@@ -58,14 +64,21 @@ FMAP::FMAP(std::string path) : _path(path) {
 
   file.close();
 
+
+  #ifdef DEBUG
   std::cout << "Loaded " << _tags.size() << " tags" << std::endl;
+  #endif
 
   for (int i = 0; i < _tags.size(); i++) {
+  #ifdef DEBUG
     std::cout << "Tag " << _tags[i].id << " is at: X: " << _tags[i].pos.X().value()
               << " Y: " << _tags[i].pos.Y().value() << " Z: " << _tags[i].pos.Z().value() << std::endl;
+  #endif
   }
 
+  #ifdef DEBUG
   std::cout << "Loaded FMAP" << std::endl;
+  #endif
 }
 
 std::vector<AprilTag> FMAP::GetTags() {
@@ -337,7 +350,9 @@ frc::Pose2d Vision::TurnToTarget(VisionTarget target, wom::SwerveDrive* swerveDr
 
   frc::Pose2d pose = frc::Pose2d(current_pose.X(), current_pose.Y(), angle);
 
+  #ifdef DEBUG
   std::cout << pose.Rotation().Degrees().value() << std::endl;
+  #endif
 
   swerveDrive->TurnToAngle(angle);
 
diff --git a/wombat/build.gradle b/wombat/build.gradle
index a9b57267..3f2173a3 100644
--- a/wombat/build.gradle
+++ b/wombat/build.gradle
@@ -24,6 +24,30 @@ model {
       }
 
       binaries.all {
+        //apply from: '../debug.gradle'
+        if (!project.hasProperty("opt")) {
+          def opt;
+          if (opt == null) {
+            opt = "release"
+          }
+
+          if (!opt == "release") {
+            cppCompiler.define "DEBUG"
+          } else {
+            cppCompiler.define "RELEASE"
+          }
+        } else {
+          if (opt == null) {
+            opt = "release"
+          }
+
+          if (!opt == "release") {
+            cppCompiler.define "DEBUG"
+          } else {
+            cppCompiler.define "RELEASE"
+          }
+        }
+
         if (it.targetPlatform.name == "linuxathena")
           cppCompiler.define "PLATFORM_ROBORIO"
         else
diff --git a/wombat/src/main/cpp/drivetrain/SwerveDrive.cpp b/wombat/src/main/cpp/drivetrain/SwerveDrive.cpp
index 48020f5a..6e434724 100644
--- a/wombat/src/main/cpp/drivetrain/SwerveDrive.cpp
+++ b/wombat/src/main/cpp/drivetrain/SwerveDrive.cpp
@@ -229,9 +229,11 @@ void PIDController::InitSendable(wpi::SendableBuilder& builder) {
 }
 
 void SwerveModuleConfig::WriteNT(std::shared_ptr<nt::NetworkTable> table) const {
+  #ifdef DEBUG
   std::array<double, 2> pos{position.X().value(), position.Y().value()};
   table->GetEntry("position").SetDoubleArray(std::span(pos));
   table->GetEntry("wheelRadius").SetDouble(wheelRadius.value());
+  #endif
 }
 
 SwerveModule::SwerveModule(std::string path, SwerveModuleConfig config,
@@ -271,7 +273,9 @@ void SwerveModule::OnUpdate(units::second_t dt) {
       // units::radian_t input = units::math::fmod(_config.turnMotor.encoder->GetEncoderPosition(), (2_rad
       // * 3.1415));
       double input = _config.turnMotor.encoder->GetEncoderPosition().value();
+    #ifdef DEBUG
       _table->GetEntry("/testing/GetEncoderPos").SetDouble(input);
+    #endif
       // _velocityPIDController.SetSetpoint(3);
 
       driveVoltage = units::volt_t{_velocityPIDController.Calculate(GetSpeed().value())};
@@ -312,17 +316,21 @@ void SwerveModule::OnUpdate(units::second_t dt) {
   turnVoltage = units::math::min(units::math::max(turnVoltage, -turnVoltageMax), turnVoltageMax);
   // turnVoltage = units::math::min(units::math::max(turnVoltage, -7_V), 7_V);
 
+  #ifdef DEBUG
   _table->GetEntry("TurnVoltage").SetDouble(turnVoltage.value());
   _table->GetEntry("TurnSetpoint").SetDouble(_anglePIDController.GetSetpoint());
   _table->GetEntry("Demand").SetDouble(_config.turnMotor.encoder->GetEncoderPosition().value());
   _table->GetEntry("Error").SetDouble(_anglePIDController.GetPositionError());
+  #endif
 
   _config.driveMotor.motorController->SetVoltage(driveVoltage);
   _config.turnMotor.motorController->SetVoltage(turnVoltage);
 
+   #ifdef DEBUG
   _table->GetEntry("speed").SetDouble(GetSpeed().value());
   _table->GetEntry("angle").SetDouble(_config.turnMotor.encoder->GetEncoderPosition().value());
   _config.WriteNT(_table->GetSubTable("config"));
+  #endif
 }
 
 void SwerveModule::SetTurnOffsetForward() {
@@ -414,7 +422,9 @@ const SwerveModuleConfig& SwerveModule::GetConfig() const {
 }
 
 void SwerveDriveConfig::WriteNT(std::shared_ptr<nt::NetworkTable> table) {
+  #ifdef DEBUG
   table->GetEntry("mass").SetDouble(mass.value());
+  #endif
 }
 
 SwerveDrive::SwerveDrive(SwerveDriveConfig config, frc::Pose2d initialPose)
@@ -455,9 +465,11 @@ frc::ChassisSpeeds FieldRelativeSpeeds::ToChassisSpeeds(const units::radian_t ro
 }
 
 void SwerveDrive::OnUpdate(units::second_t dt) {
+  #ifdef DEBUG
   _table->GetEntry("/gryo/z").SetDouble(_config.gyro->GetRotation3d().Z().value());
   _table->GetEntry("/gryo/y").SetDouble(_config.gyro->GetRotation3d().Y().value());
   _table->GetEntry("/gryo/x").SetDouble(_config.gyro->GetRotation3d().X().value());
+  #endif
   switch (_state) {
     case SwerveDriveState::kZeroing:
       for (auto mod = _modules.begin(); mod < _modules.end(); mod++) {
@@ -495,8 +507,10 @@ void SwerveDrive::OnUpdate(units::second_t dt) {
         }
       }
 
+  #ifdef DEBUG
       _table->GetEntry("direction").SetDouble(direction);
       _table->GetEntry("_reqAngle").SetDouble(_reqAngle.value());
+#endif
     } break;
     case SwerveDriveState::kPose: {
       // _target_fr_speeds.vx = _xPIDController.Calculate(GetPose().X(), dt);
@@ -505,9 +519,11 @@ void SwerveDrive::OnUpdate(units::second_t dt) {
       _target_fr_speeds.vy = units::meters_per_second_t{_yPIDController.Calculate(GetPose().Y().value())};
       _target_fr_speeds.omega = units::radians_per_second_t{_anglePIDController.Calculate(GetPose().Rotation().Radians().value())};
 
+  #ifdef DEBUG
       _table->GetEntry("Swerve vx").SetDouble(_target_fr_speeds.vx.value());
       _table->GetEntry("Swerve vy").SetDouble(_target_fr_speeds.vy.value());
       _table->GetEntry("Swerve omega").SetDouble(_target_fr_speeds.omega.value());
+#endif
     }
       [[fallthrough]];
     case SwerveDriveState::kFieldRelativeVelocity:
@@ -519,9 +535,11 @@ void SwerveDrive::OnUpdate(units::second_t dt) {
       // _target_fr_speeds.vy.value() << std::endl;
       [[fallthrough]];
     case SwerveDriveState::kVelocity: {
+  #ifdef DEBUG
       _table->GetEntry("Swerve module VX").SetDouble(_target_speed.vx.value());
       _table->GetEntry("Swerve module VY").SetDouble(_target_speed.vy.value());
       _table->GetEntry("Swerve module Omega").SetDouble(_target_speed.omega.value());
+#endif
       if (_target_speed.omega.value() > 0) {
         // _modules[0].SetTurnOffsetForward();
         _modules[1].SetTurnOffsetForward();
@@ -691,7 +709,6 @@ void SwerveDrive::SetPose(frc::Pose2d pose) {
   _anglePIDController.SetSetpoint(pose.Rotation().Radians().value()/* - 3.14159*/);
   _xPIDController.SetSetpoint(pose.X().value());
   _yPIDController.SetSetpoint(pose.Y().value());
-  // 
   utils::WritePose2NT(nt::NetworkTableInstance::GetDefault().GetTable("swerveSetpoint"), GetSetpoint());
 }
 
@@ -719,7 +736,9 @@ SwerveDriveState SwerveDrive::GetState() {
 }
 
 void SwerveDrive::ResetPose(frc::Pose2d pose) {
+  #ifdef DEBUG
   std::cout << "RESETTING POSE" << std::endl;
+  #endif
   _poseEstimator.ResetPosition(
       _config.gyro->GetRotation2d() /* - 1_rad*/,
       wpi::array<frc::SwerveModulePosition, 4>{_modules[3].GetPosition(), _modules[0].GetPosition(),
@@ -750,7 +769,6 @@ void SwerveDrive::MakeAtSetPoint() {
 void SwerveDrive::TurnToAngle(units::radian_t angle) {
   _reqAngle = angle;
   _state = SwerveDriveState::kAngle;
-  std::cout << "awiodjauwohdjiowajdoiwjoaidjoiwJ" << std::endl;
 }
 }  // namespace drivetrain
 }  // namespace wom
diff --git a/wombat/src/main/cpp/drivetrain/behaviours/SwerveBehaviours.cpp b/wombat/src/main/cpp/drivetrain/behaviours/SwerveBehaviours.cpp
index 5562f2dd..bb792d00 100644
--- a/wombat/src/main/cpp/drivetrain/behaviours/SwerveBehaviours.cpp
+++ b/wombat/src/main/cpp/drivetrain/behaviours/SwerveBehaviours.cpp
@@ -259,17 +259,23 @@ void wom::drivetrain::behaviours::DrivebasePoseBehaviour::OnStart() {
 
 // used in autonomous for going to set drive poses
 void wom::drivetrain::behaviours::DrivebasePoseBehaviour::OnTick(units::second_t deltaTime) {
+#ifdef DEBUG
   nt::NetworkTableInstance::GetDefault().GetTable("drivetrainpose")->GetEntry("going").SetBoolean(true);
+#endif
   if (_timer.Get() > 1_s) {
+#ifdef DEBUG
     nt::NetworkTableInstance::GetDefault().GetTable("drivetrainpose")->GetEntry("going").SetBoolean(false);
+#endif
     SetDone();
   }
   if (_swerveDrivebase->IsAtSetAngle() && _swerveDrivebase->GetState() == SwerveDriveState::kAngle) {
     _swerveDrivebase->SetPose(frc::Pose2d(_pose.X(), _pose.Y(), 0_deg));
   } else {
     if (_swerveDrivebase->IsAtSetPose() && !_hold) {
+      #ifdef DEBUG
       std::cout << "Exited..." << std::endl;
       nt::NetworkTableInstance::GetDefault().GetTable("drivetrainpose")->GetEntry("going").SetBoolean(false);
+      #endif
       SetDone();
     }
   }
@@ -282,11 +288,15 @@ wom::drivetrain::behaviours::TurnToAngleBehaviour::TurnToAngleBehaviour(wom::dri
 void wom::drivetrain::behaviours::TurnToAngleBehaviour::OnTick(units::second_t dt) {
   _swerve->TurnToAngle(_angle);
 
+#ifdef DEBUG
   nt::NetworkTableInstance::GetDefault().GetTable("drivetrain")->GetEntry("targetAngle").SetDouble(_angle.value());
   nt::NetworkTableInstance::GetDefault().GetTable("drivetrain")->GetEntry("runningangle").SetBoolean(true);
+#endif
 
   if (units::math::abs(_swerve->GetPose().Rotation().Radians() - _angle) < 0.1_rad) {
+#ifdef DEBUG
     nt::NetworkTableInstance::GetDefault().GetTable("drivetrain")->GetEntry("runningangle").SetBoolean(false);
+#endif
     SetDone();
   }
 }
diff --git a/wombat/src/main/cpp/subsystems/Shooter.cpp b/wombat/src/main/cpp/subsystems/Shooter.cpp
index c851266b..209134d1 100644
--- a/wombat/src/main/cpp/subsystems/Shooter.cpp
+++ b/wombat/src/main/cpp/subsystems/Shooter.cpp
@@ -37,10 +37,12 @@ void wom::subsystems::Shooter::OnUpdate(units::second_t dt) {
 
   // _params.gearbox.motorController->SetVoltage(voltage);
 
+  #ifdef DEBUG
   _table->GetEntry("output_volts").SetDouble(voltage.value());
   _table->GetEntry("speed_rpm").SetDouble(currentSpeed.value());
   _table->GetEntry("setpoint_rpm").SetDouble(units::revolutions_per_minute_t{_pid.GetSetpoint()}.value());
   _table->GetEntry("stable").SetBoolean(_pid.IsStable());
+  #endif
 }
 
 void wom::subsystems::Shooter::SetManual(units::volt_t voltage) {
diff --git a/wombat/src/main/cpp/utils/Pathplanner.cpp b/wombat/src/main/cpp/utils/Pathplanner.cpp
index 6621363e..46bc4032 100644
--- a/wombat/src/main/cpp/utils/Pathplanner.cpp
+++ b/wombat/src/main/cpp/utils/Pathplanner.cpp
@@ -246,14 +246,18 @@ utils::FollowPath::FollowPath(drivetrain::SwerveDrive* swerve, std::string path,
 
   std::string filePath = frc::filesystem::GetDeployDirectory() + "/pathplanner/paths/" + path + ".path";
 
+  #ifdef DEBUG
   std::cout << filePath << std::endl;
+  #endif
 
   std::ifstream file(filePath);
 
+  #ifdef DEBUG
   nt::NetworkTableInstance::GetDefault()
       .GetTable("pathplanner")
       ->GetEntry("Current path")
       .SetString(filePath);
+  #endif
 
   std::string cdata;
   std::string cjson;
@@ -355,7 +359,7 @@ utils::FollowPath::FollowPath(drivetrain::SwerveDrive* swerve, std::string path,
 }
 
 std::vector<pathplanner::PathPoint> utils::FollowPath::CheckPoints(std::vector<pathplanner::PathPoint> points) {
-  units::meter_t threshhold = 4_m;  
+  units::meter_t threshhold = 4_m;
   bool posesgood = true;
 
   for (auto point : points) {
@@ -371,7 +375,7 @@ std::vector<pathplanner::PathPoint> utils::FollowPath::CheckPoints(std::vector<p
     return CheckPoints(_points);
   }
 
-  return points; 
+  return points;
 }
 
 void utils::FollowPath::CalcTimer() {
@@ -428,8 +432,10 @@ void utils::FollowPath::OnTick(units::second_t dt) {
       ->GetEntry("amtPoses")
       .SetInteger(static_cast<int>(_poses.size()));
 
+  #ifdef DEBUG
   std::cout << "Following Path" << std::endl;
-  _poses[_currentPose] = frc::Pose2d(_poses[_currentPose].X(), _poses[_currentPose].Y(), _swerve->GetPose().Rotation());    
+  #endif
+  _poses[_currentPose] = frc::Pose2d(_poses[_currentPose].X(), _poses[_currentPose].Y(), _swerve->GetPose().Rotation());
 
   // if (_swerve->IsAtSetAngle() && _swerve->GetState() == drivetrain::SwerveDriveState::kAngle) {
   _swerve->SetPose(_poses[_currentPose]);
@@ -449,12 +455,14 @@ void utils::FollowPath::OnTick(units::second_t dt) {
 
   std::string filePath = frc::filesystem::GetDeployDirectory() + "/pathplanner/paths/" + _pathName + ".path";
 
+  #ifdef DEBUG
   std::cout << filePath << std::endl;
 
   nt::NetworkTableInstance::GetDefault()
       .GetTable("pathplanner")
       ->GetEntry("Current path")
       .SetString(filePath);
+  #endif
 }
 
 // AutoBuilder implementation
@@ -471,7 +479,9 @@ void utils::AutoBuilder::SetAuto(std::string path) {
 
   fs::path location = deploy_directory / "pathplanner" / "autos" / path;
 
+  #ifdef DEBUG
   std::cout << location << std::endl;
+  #endif
 
   std::ifstream file(location);
 
@@ -485,7 +495,9 @@ void utils::AutoBuilder::SetAuto(std::string path) {
 
   // cjson.pop_back();
 
+  #ifdef DEBUG
   std::cout << cjson << std::endl;
+  #endif
 
   wpi::json j = wpi::json::parse(cjson);
 
@@ -495,6 +507,7 @@ void utils::AutoBuilder::SetAuto(std::string path) {
 
   commands = std::vector<std::pair<std::string, std::string>>();
 
+  #ifdef DEBUG
   nt::NetworkTableInstance::GetDefault().GetTable("json")->GetEntry("data").SetString(_currentPath->dump());
   // nt::NetworkTableInstance::GetDefault().GetTable("json")->GetEntry("start").SetString(_startingPose->dump());
   nt::NetworkTableInstance::GetDefault().GetTable("json")->GetEntry("commands").SetString(_commands->dump());
@@ -502,6 +515,7 @@ void utils::AutoBuilder::SetAuto(std::string path) {
       .GetTable("json")
       ->GetEntry("commands_type")
       .SetString(_commands->type_name());
+  #endif
 
   for (auto c : *_commands) {
     if (c["type"] == "path") {
@@ -515,15 +529,17 @@ void utils::AutoBuilder::SetAuto(std::string path) {
     }
   }
 
+  #ifdef DEBUG
   nt::NetworkTableInstance::GetDefault().GetTable("commands")->GetEntry("length").SetInteger(commands.size());
+  #endif
 
   auto _pathplan = behaviour::make<behaviour::SequentialBehaviour>();
 
   int i = 0;
   int pathamt = 0;
   int commandamt = 0;
-  
-  frc::Pose2d startPose; 
+
+  frc::Pose2d startPose;
 
   if (!_startingPose->is_null()) {
     // startPose = JSONPoseToPose2d(*_startingPose);
@@ -531,8 +547,10 @@ void utils::AutoBuilder::SetAuto(std::string path) {
   } else {
     startPose = frc::Pose2d();
   }
- 
+
   for (auto command : *_commands) {
+
+  #ifdef DEBUG
     nt::NetworkTableInstance::GetDefault()
         .GetTable("commands/" + std::to_string(i))
         ->GetEntry("type")
@@ -541,25 +559,31 @@ void utils::AutoBuilder::SetAuto(std::string path) {
         .GetTable("commands/" + std::to_string(i))
         ->GetEntry("data")
         .SetString(static_cast<std::string>(command["data"].dump()));
+  #endif
 
     if (command["type"] == "path") {
       _pathplan->Add(behaviour::make<FollowPath>(_swerve, command["data"]["pathName"], _flip, startPose));
       pathamt++;
+  #ifdef DEBUG
       nt::NetworkTableInstance::GetDefault()
           .GetTable("commands/" + std::to_string(i))
           ->GetEntry("behaviours")
           .SetStringArray(_pathplan->GetQueue());
+  #endif
     } else if (command["type"] == "named") {
       _pathplan->Add(_commandsList.Run(command["data"]["name"]));
       commandamt++;
+  #ifdef DEBUG
       nt::NetworkTableInstance::GetDefault()
           .GetTable("commands/" + std::to_string(i))
           ->GetEntry("behaviours")
           .SetStringArray(_pathplan->GetQueue());
+  #endif
     } else if (command["type"] == "parallel") {
       auto nb = behaviour::make<behaviour::ConcurrentBehaviour>(behaviour::ConcurrentBehaviourReducer::ANY);
       int j = 0;
       for (auto c : command["data"]["commands"]) {
+  #ifdef DEBUG
         nt::NetworkTableInstance::GetDefault()
             .GetTable("commands/parallel/" + std::to_string(j))
             ->GetEntry("type")
@@ -569,6 +593,7 @@ void utils::AutoBuilder::SetAuto(std::string path) {
             .GetTable("commands/parallel/" + std::to_string(j))
             ->GetEntry("typeisstring")
             .SetBoolean(c["type"].is_string());
+  #endif
         if (static_cast<std::string>(c["type"]) == "path") {
           nb->Add(behaviour::make<FollowPath>(_swerve, c["data"]["pathName"], _flip, startPose));
           pathamt++;
@@ -579,6 +604,7 @@ void utils::AutoBuilder::SetAuto(std::string path) {
         nb->Add(behaviour::make<behaviour::Print>("ok"));
         j++;
       }
+  #ifdef DEBUG
       nt::NetworkTableInstance::GetDefault()
           .GetTable("commands")
           ->GetEntry("parallelcommandsamt")
@@ -587,22 +613,26 @@ void utils::AutoBuilder::SetAuto(std::string path) {
           .GetTable("commands")
           ->GetEntry("parallel-" + std::to_string(i))
           .SetStringArray(nb->GetQueue());
+  #endif
       _pathplan->Add(nb);
     }
 
     _pathplan->Add(behaviour::make<behaviour::Print>("idk"));
 
     pathplan = _pathplan;
+  #ifdef DEBUG
     nt::NetworkTableInstance::GetDefault()
         .GetTable("commands/" + std::to_string(i))
         ->GetEntry("currentbehaviours")
         .SetStringArray(pathplan->GetQueue());
+  #endif
     i++;
   }
 
   // pathplan->Add(behaviour::make<behaviour::Print>("test"));
   //   pathplan->Add(behaviour::make<behaviour::Print>("test"));
 
+  #ifdef DEBUG
   nt::NetworkTableInstance::GetDefault()
       .GetTable("commands/newbehaviours")
       ->GetEntry(std::to_string(i))
@@ -610,6 +640,7 @@ void utils::AutoBuilder::SetAuto(std::string path) {
 
   nt::NetworkTableInstance::GetDefault().GetTable("commands")->GetEntry("PathAmt").SetInteger(pathamt);
   nt::NetworkTableInstance::GetDefault().GetTable("commands")->GetEntry("CommandAmt").SetInteger(commandamt);
+  #endif
 
   _swerve->SetAccelerationLimit(units::meters_per_second_squared_t{2});
   _swerve->SetVoltageLimit(6_V);
@@ -628,7 +659,9 @@ frc::Pose2d utils::AutoBuilder::JSONPoseToPose2d(wpi::json j) {
   // std::cout << j["position"].is_object() << std::endl;
   // std::cout << j << std::endl;
 
+  #ifdef DEBUG
   std::cout << j.dump() << std::endl;
+  #endif
 
   return frc::Pose2d(units::meter_t{j["position"]["x"]}, units::meter_t{j["position"]["y"]},
                      units::degree_t{j["rotation"]});
@@ -636,7 +669,9 @@ frc::Pose2d utils::AutoBuilder::JSONPoseToPose2d(wpi::json j) {
 }
 
 std::shared_ptr<behaviour::Behaviour> utils::AutoBuilder::GetAutoPath() {
+  #ifdef DEBUG
   std::cout << "Getting Path" << std::endl;
+  #endif
 
   return pathplan;
 }
diff --git a/wombat/src/main/cpp/vision/Limelight.cpp b/wombat/src/main/cpp/vision/Limelight.cpp
index 012a5e57..1bf77cd1 100644
--- a/wombat/src/main/cpp/vision/Limelight.cpp
+++ b/wombat/src/main/cpp/vision/Limelight.cpp
@@ -186,7 +186,9 @@ frc::Pose3d wom::vision::Limelight::GetPose() {
 }
 
 void wom::vision::Limelight::OnStart() {
+  #ifdef DEBUG
   std::cout << "starting" << std::endl;
+  #endif
 }
 
 void wom::vision::Limelight::OnUpdate(units::time::second_t dt) {
diff --git a/wombat/src/main/include/utils/PID.h b/wombat/src/main/include/utils/PID.h
index adf34f94..dcb9c766 100644
--- a/wombat/src/main/include/utils/PID.h
+++ b/wombat/src/main/include/utils/PID.h
@@ -134,7 +134,9 @@ class PIDController {
       out *= -1;
     }
 
+#ifdef DEBUG
     _table->GetEntry("error").SetDouble(out.value());
+#endif
 
     return out;
   }
diff --git a/wombat/src/main/include/utils/Util.h b/wombat/src/main/include/utils/Util.h
index 45e4d495..09448dca 100644
--- a/wombat/src/main/include/utils/Util.h
+++ b/wombat/src/main/include/utils/Util.h
@@ -39,7 +39,9 @@ class NTBound {
     _listener =
         table->AddListener(name, nt::EventFlags::kValueAll,
                            ([this](nt::NetworkTable* table, std::string_view key, const nt::Event& event) {
+                             #ifdef DEBUG
                              std::cout << "NT UPDATE" << std::endl;
+                             #endif
                              this->_onUpdate(event.GetValueEventData()->value);
                            }));
   }
@@ -49,7 +51,9 @@ class NTBound {
     _listener =
         _table->AddListener(_name, nt::EventFlags::kValueAll,
                             ([this](nt::NetworkTable* table, std::string_view key, const nt::Event& event) {
+                              #ifdef DEBUG
                               std::cout << "NT UPDATE" << std::endl;
+                              #endif
                               this->_onUpdate(event.GetValueEventData()->value);
                             }));
   }

From 39c51b5d95a0185e3642b823f59be9aa622de958 Mon Sep 17 00:00:00 2001
From: Isaac Turner <spacey_sooty@outlook.com>
Date: Sat, 9 Mar 2024 12:58:58 +0800
Subject: [PATCH 206/207] remove debug.gradle

---
 debug.gradle | 22 ----------------------
 1 file changed, 22 deletions(-)
 delete mode 100644 debug.gradle

diff --git a/debug.gradle b/debug.gradle
deleted file mode 100644
index fa412367..00000000
--- a/debug.gradle
+++ /dev/null
@@ -1,22 +0,0 @@
-if (!project.hasProperty("opt")) {
-  def opt;
-  if (opt == null) {
-    opt = "release"
-  }
-
-  if (!opt == "release") {
-    cppCompiler.define "DEBUG"
-  } else {
-    cppCompiler.define "RELEASE"
-  }
-} else {
-  if (opt == null) {
-    opt = "release"
-  }
-
-  if (!opt == "release") {
-    cppCompiler.define "DEBUG"
-  } else {
-    cppCompiler.define "RELEASE"
-  }
-}

From f64234ddd08a8e23b5f05fea76a21fc332abaf40 Mon Sep 17 00:00:00 2001
From: Isaac Turner <spacey_sooty@outlook.com>
Date: Sat, 9 Mar 2024 13:31:34 +0800
Subject: [PATCH 207/207] remove applies

---
 build.gradle        | 2 --
 wombat/build.gradle | 1 -
 2 files changed, 3 deletions(-)

diff --git a/build.gradle b/build.gradle
index f3055148..6524664b 100644
--- a/build.gradle
+++ b/build.gradle
@@ -65,7 +65,6 @@ model {
             }
 
 	    binaries.all {
-        //apply from: 'debug.gradle'
         if (!project.hasProperty("opt")) {
           def opt;
           if (opt == null) {
@@ -117,7 +116,6 @@ model {
             }
 
 	binaries.all {
-      //apply from: 'debug.gradle'
       if (!project.hasProperty("opt")) {
         def opt;
         if (opt == null) {
diff --git a/wombat/build.gradle b/wombat/build.gradle
index 3f2173a3..7c616f9d 100644
--- a/wombat/build.gradle
+++ b/wombat/build.gradle
@@ -24,7 +24,6 @@ model {
       }
 
       binaries.all {
-        //apply from: '../debug.gradle'
         if (!project.hasProperty("opt")) {
           def opt;
           if (opt == null) {