CalibDrive and CalibTurn states

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

platformio.ini

@@ -20,6 +20,7 @@ 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__

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();
+        }
     }
 }

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