diff --git a/esp32/lib/PidController/PidController.cpp b/esp32/lib/PidController/PidController.cpp index e9f0b81..585d589 100644 --- a/esp32/lib/PidController/PidController.cpp +++ b/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,7 +79,7 @@ void PidController::loop() if (millis() > loop1Timer) { innerLoop(); - loop1Timer += LOOP_INTERVAL; + loop1Timer += LOOP1_INTERVAL; } if (millis() > loop2Timer) @@ -87,6 +87,12 @@ void PidController::loop() 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,14 +251,15 @@ 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); @@ -261,14 +267,13 @@ void PidController::outerLoop() 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) @@ -276,11 +281,9 @@ void PidController::outerLoop() 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() diff --git a/esp32/lib/PidController/PidController.h b/esp32/lib/PidController/PidController.h index e3eab7e..4e6c5b2 100644 --- a/esp32/lib/PidController/PidController.h +++ b/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; diff --git a/esp32/lib/WifiSetup/WifiSetup.cpp b/esp32/lib/WifiSetup/WifiSetup.cpp index 25462d6..aed36c5 100644 --- a/esp32/lib/WifiSetup/WifiSetup.cpp +++ b/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) {