more PID

main/ugv_main.cc

@@ -29,12 +29,13 @@ 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 ANGLE_P         = 0.05;
-static const float ANGLE_I         = 0.00;
-static const float ANGLE_D         = -0.05;
-static const float MAX_ANGLE_POWER = 0.5;
-static const float MIN_DIST        = 10.0;
+static const float DRIVE_POWER       = 0.5;
+static const float ANGLE_P           = 0.02;
+static const float ANGLE_I           = 0.000005;
+static const float ANGLE_D           = -0.01;
+static const float MAX_ANGLE_POWER   = 0.3;
+static const float MAX_ANGLE_I_ERROR = 15.0;
+static const float MIN_DIST          = 10.0;
 
 extern "C" void OnTimeout(void *arg);
 
@@ -97,6 +98,9 @@ class PIDController {
   void  MaxOutput(float max_output) { max_output_ = max_output; }
   float MaxOutput() const { return max_output_; }
 
+  void  MaxIError(float max_i_error) { max_i_error_ = max_i_error; }
+  float MaxIError() const { return max_i_error_; }
+
   void  Setpoint(float setpoint) { setpoint_ = setpoint; }
   float Setpoint() const { return setpoint_; }
 
@@ -129,6 +133,7 @@ class PIDController {
   float ki_;
   float kd_;
   float max_output_;
+  float max_i_error_;
 
   bool  enabled_;
   float setpoint_;
@@ -154,6 +159,7 @@ PIDController::PIDController(float dt, float kp, float ki, float kd)
       ki_(ki),
       kd_(kd),
       max_output_(INFINITY),
+      max_i_error_(INFINITY),
       enabled_(false),
       setpoint_(0),
       input_(0),
@@ -177,7 +183,11 @@ float PIDController::Update() {
   }
   float error = Error();
 
-  integral_ += error * dt_;
+  if (fabsf(error) > max_i_error_) {
+    integral_ = 0.;
+  } else {
+    integral_ += error * dt_;
+  }
 
   output_ += kp_ * error;
   output_ += ki_ * integral_;
@@ -216,8 +226,10 @@ struct State {
 
   State()
       : target{34.069022, -118.443067},
-        angle_controller_(LOOP_PERIOD_S, ANGLE_P) {
+        angle_controller_(LOOP_PERIOD_S, ANGLE_P, ANGLE_I, ANGLE_D) {
     angle_controller_.MaxOutput(MAX_ANGLE_POWER);
+    angle_controller_.MaxIError(MAX_ANGLE_I_ERROR);
+
     comms   = new CommsClass();
     io      = new IOClass();
     display = new DisplayClass(comms);
@@ -262,6 +274,7 @@ struct State {
                inputs.mpu.mag.z);
       ESP_LOGD(TAG, "ahrs: yaw=%f, pitch=%f, roll=%f", ahrs_.getYaw(),
                ahrs_.getPitch(), ahrs_.getRoll());
+      ESP_LOGD(TAG, "PID: error: %f", angle_controller_.Error());
       last_print = time_us;
     }
 
@@ -352,8 +365,8 @@ struct State {
 
     if (angle_controller_.Enabled()) {
       float angle_pwr     = angle_controller_.Update();
-      outputs.left_motor  = drive_power + angle_pwr;
-      outputs.right_motor = drive_power - angle_pwr;
+      outputs.left_motor  = drive_power - angle_pwr;
+      outputs.right_motor = drive_power + angle_pwr;
     }
 
     io->WriteOutputs(outputs);