add better test for driving

main/ugv_io.cc

@@ -78,7 +78,7 @@ void IOClass::WriteOutputs(const Outputs &outputs) {
   ret = mcpwm_start(MCPWM_UNIT, MCPWM_TIMER_1);
   ERROR_CHECK(ret);
   bool left_dir, right_dir;
-  left_dir  = outputs.left_motor < 0.f;
+  left_dir  = outputs.left_motor > 0.f;
   right_dir = outputs.right_motor < 0.f;
 
   ret = gpio_set_level(MOTOR_LEFT_DIR, left_dir);

main/ugv_main.cc

@@ -38,9 +38,19 @@ static const float RAD_PER_DEG = PI / 180.f;
 // Radius of earth in meters
 static const float EARTH_RAD = 6372795.f;
 
-static const float DRIVE_POWER = 0.5;
+static const float DRIVE_POWER     = 0.5;
-static const float ANGLE_P     = 0.02;
+static const float ANGLE_P         = 0.05;
-static const float MIN_DIST    = 10.0;
+static const float MAX_ANGLE_POWER = 0.3;
+static const float MIN_DIST        = 10.0;
+
+float clamp_mag(float x, float max_mag) {
+  if (x > max_mag)
+    return max_mag;
+  else if (x < -max_mag)
+    return -max_mag;
+  else
+    return x;
+}
 
 struct LatLong {
  public:
@@ -149,6 +159,7 @@ struct State {
 
     comms->Lock();
     UpdateLocationFromGPS(comms->location, inputs.gps);
+    comms->yaw_angle    = ahrs_.getYaw();
     UGV_State ugv_state = comms->ugv_state;
     comms->Unlock();
 
@@ -174,6 +185,11 @@ struct State {
         break;
       }
       case UGV_State::STATE_DRIVING: {
+        if (inputs.gps.fix_quality == io::GPS_FIX_INVALID) {
+          comms->ugv_state = UGV_State::STATE_AQUIRING;
+          break;
+        }
+
         LatLong current_pos = {inputs.gps.latitude, inputs.gps.longitude};
         float   tgt_dist    = current_pos.distance_to(target);
 
@@ -195,8 +211,21 @@ struct State {
         break;
       }
       case UGV_State::STATE_TEST:
+#ifdef BASIC_TEST
         outputs.left_motor  = sinf(time_s * PI);
         outputs.right_motor = cosf(time_s * PI);
+#else
+        float tgt_bearing = 90.0;
+        float cur_bearing = ahrs_.getYaw();
+        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 = clamp_mag(angle_delta * ANGLE_P, MAX_ANGLE_POWER);
+        float power = 0.0;
+
+        outputs.left_motor  = power - angle_pwr;
+        outputs.right_motor = power + angle_pwr;
+#endif
         break;
     }
     io->WriteOutputs(outputs);