Add rotational outer loop to PidController

esp32/lib/PidController/PidController.cpp

@@ -20,7 +20,7 @@ double PidController::filtered_value = 0;
 double PidController::rotation_correction = 0;
 
 // Initialise PID parameters using known values
-PidParams PidController::params(1.00, 1.00, 1.00, 1.00, 1.00, 1.00, 1.00, 1.00, 1.00);
+PidParams PidController::params(0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00);
 PidDirection PidController::direction(0, KeyDirection::STOP);
 
 bool PidController::setup(IWifi &wifi, unsigned long timeout)
@@ -79,14 +79,20 @@ void PidController::loop()
     if (millis() > loop1Timer)
     {
         innerLoop();
-        loop1Timer += LOOP_INTERVAL;
+        loop1Timer += LOOP1_INTERVAL;
     }
 
     if (millis() > loop2Timer)
     {
         outerLoop();
         loop2Timer += LOOP2_INTERVAL;
     }
+
+    if (millis() > loop3Timer)
+    {
+        rotationLoop();
+        loop3Timer += LOOP3_INTERVAL;
+    }
 }
 
 void PidController::stabilizedLoop()
@@ -195,7 +201,6 @@ void PidController::innerLoop()
     float kp_i = params.kp_i;
     float ki_i = params.ki_i;
     float kd_i = params.kd_i;
-    float tilt_setpoint = params.tilt_setpoint;
     xSemaphoreGive(paramsMutex);
 
     // Fetch data from MPU6050
@@ -246,41 +251,39 @@ void PidController::innerLoop()
 
 void PidController::outerLoop()
 {
-    static double avgspeed = 0, avg_temp = 0, speed_error = 0, previous_speed_error = 0, integral_speed = 0, derivative_speed = 0, Pout_s = 0, Iout_s = 0, Dout_s = 0, ramp_rate = 0;
+    static double speed_error = 0;
+    static double previous_speed_error = 0;
+    static double integral_speed = 0;
+    static double derivative_speed = 0;
     static double current_speed_setpoint = 0;
     static double speed_setpoint = 0; // outer loop setpoint
-    static int speed_size = 0;
 
-    double max_ramp_rate = 0.5;
-    double time_constant = 2000;
-    double beta = 1 - exp(-LOOP2_INTERVAL / time_constant);
+    const double TIME_CONSTANT = 2000;
+    const double BETA = 1 - exp(-LOOP2_INTERVAL / TIME_CONSTANT);
 
     // Fetch PID parameters - take mutex and wait forever until it is available
     xSemaphoreTake(paramsMutex, portMAX_DELAY);
     float kp_o = params.kp_o;
     float ki_o = params.ki_o;
     float kd_o = params.kd_o;
     float balancing_setpoint = params.velocity_setpoint;
+    xSemaphoreGive(paramsMutex);
 
-    current_speed_setpoint += beta * (speed_setpoint - current_speed_setpoint);
+    current_speed_setpoint += BETA * (speed_setpoint - current_speed_setpoint);
 
     // filtered_value = (step1.getSpeedRad()-step2.getSpeedRad())/2;
     speed_error = -(current_speed_setpoint - filtered_value);
-
-    Pout_s = kp_o * speed_error;
-
+    double Pout_s = kp_o * speed_error;
     integral_speed = integral_speed + speed_error * (LOOP2_INTERVAL / 1000);
 
     if (ki_o == 0)
     {
         integral_speed = 0;
     }
 
-    Iout_s = ki_o * integral_speed;
-
+    double Iout_s = ki_o * integral_speed;
     derivative_speed = ((speed_error - previous_speed_error) / LOOP2_INTERVAL) * 1000;
-
-    Dout_s = kd_o * derivative_speed;
+    double Dout_s = kd_o * derivative_speed;
 
     angle_setpoint = Pout_s + Iout_s + Dout_s + balancing_setpoint;
 
@@ -313,6 +316,31 @@ void PidController::outerLoop()
 
 void PidController::rotationLoop()
 {
+    static double rotation_angle = 0;
+    static double rotation_target_angle = 0;
+    static double rotation_setpoint = 0;
+    static double previous_rotation_error;
+
+    const float KP_ROTATION = 3.00;
+    const float KD_ROTATION = 5.00;
+
+    sensors_event_t a, g, temp;
+    mpu.getEvent(&a, &g, &temp);
+
+    double rotationX = g.gyro.x - gyroXoffset;
+
+    // rotation_angle should not reset every time this loop is called.
+    rotation_angle = rotationX * (LOOP3_INTERVAL / 1000) + rotation_angle;
+    rotation_target_angle += (rotation_setpoint * 0.4 * LOOP3_INTERVAL) / 1000;
+
+    double rotation_error = -(rotation_target_angle - rotation_angle);
+    double Pout_r = KP_ROTATION * rotation_error;
+    double delta_error = rotation_error - previous_rotation_error;
+    double Dout_r = KD_ROTATION * (delta_error / LOOP3_INTERVAL) * 1000;
+
+    previous_rotation_error = rotation_error;
+
+    rotation_correction = Pout_r + Dout_r;
 }
 
 void PidController::calibrate()

esp32/lib/PidController/PidController.h

@@ -122,8 +122,9 @@ class PidController
     // Class constants
     static constexpr int STEPPER_INTERVAL_US = 10;
     static constexpr float MOTOR_ACCEL_RAD = 30.0;
-    static constexpr double LOOP_INTERVAL = 10;
-    static constexpr double LOOP2_INTERVAL = 200;
+    static constexpr double LOOP1_INTERVAL = 10;  // inner
+    static constexpr double LOOP2_INTERVAL = 200; // outer
+    static constexpr double LOOP3_INTERVAL = 20;  // rotation
 
     // Class variables
     static double accXoffset;

esp32/lib/WifiSetup/WifiSetup.cpp

@@ -227,7 +227,6 @@ void WifiSetup::callback(char *topic, byte *payload, unsigned int length)
             float kp_i = doc["kp_i"] | params.kp_i;
             float ki_i = doc["ki_i"] | params.ki_i;
             float kd_i = doc["kd_i"] | params.kd_i;
-            float setpoint_i = doc["setpoint_i"] | params.tilt_setpoint;
             float kp_o = doc["kp_o"] | params.kp_o;
             float ki_o = doc["ki_o"] | params.ki_o;
             float kd_o = doc["kd_o"] | params.kd_o;
@@ -245,7 +244,9 @@ void WifiSetup::callback(char *topic, byte *payload, unsigned int length)
                            ", rotation_setpoint: " + String(rotation_setpoint));
 
             // Use the callback given in the static class
-            PidController::setParams(PidParams(kp_i, ki_i, kd_i, tilt_setpoint, kp_o, ki_o, kd_o, velocity_setpoint, rotation_setpoint));
+            PidController::setParams(PidParams(
+                kp_i, ki_i, kd_i, tilt_setpoint,
+                kp_o, ki_o, kd_o, velocity_setpoint, rotation_setpoint));
         }
         else if (strcmp(topic, "esp32/cli") == 0)
         {