From dd538d7185c13cdc5f76300c57eb3ea7aa1d217c Mon Sep 17 00:00:00 2001
From: Andy Killorin <37423245+Speedy6451@users.noreply.github.com>
Date: Wed, 4 Feb 2026 14:40:08 -0500
Subject: [PATCH] CalibDrive and CalibTurn states

---
 lib/Chassis/src/chassis.h |  4 +++
 platformio.ini            |  3 +-
 src/robot.cpp             | 60 ++++++++++++++++++++++++++++++++++++++-
 src/robot.h               |  6 ++++
 4 files changed, 71 insertions(+), 2 deletions(-)

diff --git a/lib/Chassis/src/chassis.h b/lib/Chassis/src/chassis.h
index 51bb0e0..8b58196 100644
--- a/lib/Chassis/src/chassis.h
+++ b/lib/Chassis/src/chassis.h
@@ -1,6 +1,7 @@
 #pragma once
 
 #include <Arduino.h>
+#include <Romi32U4.h>
 
 #include "chassis-params.h"
 #include "utils.h"
@@ -36,6 +37,9 @@ public:
     static void Timer4OverflowISRHandler(void);
 
 public:
+    Romi32U4ButtonB buttonB;
+    Romi32U4ButtonC buttonC;
+
     Pose CalcOdomFromWheelMotion(void);
 
 
diff --git a/platformio.ini b/platformio.ini
index c12b564..9253259 100644
--- a/platformio.ini
+++ b/platformio.ini
@@ -20,9 +20,10 @@ lib_deps =
     https://github.com/gcl8a/Button 
     https://github.com/gcl8a/event_timer
     https://github.com/WPIRoboticsEngineering/Romi-32u4-utilities
+    wpi-32u4-library   
 
 build_flags = 
 ;    -D__MOTOR_DEBUG__
 ;    -D__LOOP_DEBUG__
 ;    -D__IMU_DEBUG__
-    -D__NAV_DEBUG__
+     -D__NAV_DEBUG__
diff --git a/src/robot.cpp b/src/robot.cpp
index cf064a8..27f8895 100644
--- a/src/robot.cpp
+++ b/src/robot.cpp
@@ -18,6 +18,48 @@ void Robot::EnterIdleState(void)
     robotState = ROBOT_IDLE;
 }
 
+void Robot::EnterCalibTurn(void)
+{
+    chassis.Stop();
+
+    Serial.println("-> Calibration Turn");
+    currPose = Pose(); // clear pose
+    robotState = ROBOT_CALIB_TURN;
+}
+
+void Robot::EnterCalibDrive(void)
+{
+    chassis.Stop();
+
+    Serial.println("-> Calibration Drive");
+    currPose = Pose(); // clear pose
+    robotState = ROBOT_CALIB_DRIVE;
+}
+
+void Robot::CalibTurn(void)
+{
+    if(robotState == ROBOT_CALIB_TURN)
+    {
+        chassis.SetMotorEfforts(-90,70);
+        if (currPose.theta * 180.0 / PI > 360.0) {
+            chassis.Stop();
+            EnterIdleState();
+        }
+    }
+}
+
+void Robot::CalibDrive(void)
+{
+    if(robotState == ROBOT_CALIB_DRIVE)
+    {
+        chassis.SetMotorEfforts(70,60);
+        if (currPose.x > 60.0) {
+            chassis.Stop();
+            EnterIdleState();
+        }
+    }
+}
+
 /**
  * The main loop for your robot. Process both synchronous events (motor control),
  * and asynchronous events (distance readings, etc.).
@@ -32,7 +74,14 @@ void Robot::RobotLoop(void)
     {
         // We do FK regardless of state
         UpdatePose(delta);
-        chassis.SetMotorEfforts(220,-220);
+        //chassis.SetMotorEfforts(220,-220);
+
+        if (chassis.buttonB.isPressed()) {
+            EnterCalibTurn();
+        }
+        if (chassis.buttonC.isPressed()) {
+            EnterCalibDrive();
+        }
         
         /**
          * Here, we break with tradition and only call these functions if we're in the 
@@ -46,5 +95,14 @@ void Robot::RobotLoop(void)
             DriveToPoint();
             if(CheckReachedDestination()) HandleDestination();
         }
+
+        if(robotState == ROBOT_CALIB_TURN)
+        {
+            CalibTurn();
+        }
+        if(robotState == ROBOT_CALIB_DRIVE)
+        {
+            CalibDrive();
+        }
     }
 }
diff --git a/src/robot.h b/src/robot.h
index 494f51b..4193edc 100644
--- a/src/robot.h
+++ b/src/robot.h
@@ -13,6 +13,8 @@ protected:
     {
         ROBOT_IDLE,
         ROBOT_DRIVE_TO_POINT,
+        ROBOT_CALIB_TURN,
+        ROBOT_CALIB_DRIVE,
     };
     ROBOT_STATE robotState = ROBOT_IDLE;
 
@@ -36,11 +38,15 @@ public:
 protected:
     /* State changes */    
     void EnterIdleState(void);
+    void EnterCalibTurn(void);
+    void EnterCalibDrive(void);
 
     // /* Navigation methods.*/
     void UpdatePose(const Pose& u);
     void SetDestination(const Pose& destination);
     void DriveToPoint(void);
+    void CalibTurn(void);
+    void CalibDrive(void);
     bool CheckReachedDestination(void);
     void HandleDestination(void);
 };