From a92cf5cb9c325162939a64430c2ccd31e816eb6f Mon Sep 17 00:00:00 2001
From: Alex Mikhalev <alexmikhalevalex@gmail.com>
Date: Wed, 8 May 2019 21:31:44 -0700
Subject: [PATCH] Improve state flow

---
 main/ugv_main.cc | 20 +++++++++++++-------
 1 file changed, 13 insertions(+), 7 deletions(-)

diff --git a/main/ugv_main.cc b/main/ugv_main.cc
index 775d9ce..e77c89b 100644
--- a/main/ugv_main.cc
+++ b/main/ugv_main.cc
@@ -162,6 +162,7 @@ struct State {
     comms->yaw_angle    = ahrs_.getYaw();
     UGV_State ugv_state = comms->ugv_state;
     comms->Unlock();
+    UGV_State next_state = ugv_state;
 
     switch (ugv_state) {
       default:
@@ -180,13 +181,13 @@ struct State {
         outputs.left_motor     = 0.0;
         outputs.right_motor    = 0.0;
         if (not_old && not_invalid) {
-          comms->ugv_state = UGV_State::STATE_DRIVING;
+          next_state = UGV_State::STATE_DRIVING;
         }
         break;
       }
       case UGV_State::STATE_DRIVING: {
         if (inputs.gps.fix_quality == io::GPS_FIX_INVALID) {
-          comms->ugv_state = UGV_State::STATE_AQUIRING;
+          next_state = UGV_State::STATE_AQUIRING;
           break;
         }
 
@@ -195,7 +196,7 @@ struct State {
 
         if (tgt_dist <= MIN_DIST) {
           ESP_LOGI(TAG, "Finished driving to target");
-          comms->ugv_state = UGV_State::STATE_FINISHED;
+          next_state = UGV_State::STATE_FINISHED;
           break;
         }
 
@@ -204,10 +205,11 @@ struct State {
         float angle_delta = tgt_bearing - cur_bearing;
         if (angle_delta < 180.f) angle_delta += 360.f;
         if (angle_delta > 180.f) angle_delta -= 360.f;
-        float angle_pwr = angle_delta * ANGLE_P;
+        float angle_pwr = clamp_mag(angle_delta * ANGLE_P, MAX_ANGLE_POWER);
+        float power     = DRIVE_POWER;
 
-        outputs.left_motor  = DRIVE_POWER + angle_pwr;
-        outputs.right_motor = DRIVE_POWER - angle_pwr;
+        outputs.left_motor  = power - angle_pwr;
+        outputs.right_motor = power + angle_pwr;
         break;
       }
       case UGV_State::STATE_TEST:
@@ -221,7 +223,7 @@ struct State {
         if (angle_delta < 180.f) angle_delta += 360.f;
         if (angle_delta > 180.f) angle_delta -= 360.f;
         float angle_pwr = clamp_mag(angle_delta * ANGLE_P, MAX_ANGLE_POWER);
-        float power = 0.0;
+        float power     = 0.0;
 
         outputs.left_motor  = power - angle_pwr;
         outputs.right_motor = power + angle_pwr;
@@ -229,6 +231,10 @@ struct State {
         break;
     }
     io->WriteOutputs(outputs);
+
+    comms->Lock();
+    comms->ugv_state = next_state;
+    comms->Unlock();
   }
 };