From 2442452df057d3f58bf3ff18c2b0f0e1ff9d2a7c Mon Sep 17 00:00:00 2001
From: Paul Hodges <paul.hodges@student.education.wa.edu.au>
Date: Tue, 22 Aug 2023 20:50:13 +0800
Subject: [PATCH] Tuned Swerve PIDs

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

diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp
index ce899b6..d57433c 100644
--- a/src/main/cpp/Robot.cpp
+++ b/src/main/cpp/Robot.cpp
@@ -216,6 +216,10 @@ void Robot::TeleopPeriodic() {
   if (map.controllers.test.GetBButtonPressed()){
     swerve->GetActiveBehaviour()->Interrupt();
   }
+
+  if (map.controllers.test.GetAButtonPressed()){
+    swerve->SetIndividualTuning(1, 270_deg, 0_m/1_s);
+  }
 }
 
 
diff --git a/src/main/include/RobotMap.h b/src/main/include/RobotMap.h
index 8343bc3..f954777 100644
--- a/src/main/include/RobotMap.h
+++ b/src/main/include/RobotMap.h
@@ -149,11 +149,11 @@ struct RobotMap {
     // Setting the PID path and values to be used for SwerveDrive and SwerveModules
     wom::SwerveModule::angle_pid_conf_t anglePID {
       "/drivetrain/pid/angle/config",
-      14_V / 180_deg,
+      13.2_V / 180_deg,
       0.0_V / (100_deg * 1_s),
-      0_V / (100_deg / 1_s),
-      1_deg,
-      0.5_deg / 2_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",