mirror of https://github.com/TheAlgorithms/C
89 lines
2.9 KiB
C
89 lines
2.9 KiB
C
/**
|
|
* PID Controller
|
|
*
|
|
* The PID controller is a linear control algorithm that has three terms:
|
|
* - Proportional: A simple scaling of the error value by a gain kP
|
|
* - Integral: Integration of the error value over time, then multipled by gain
|
|
* kI
|
|
* - Derivative: Rate of change of the error value over time, multiplied by
|
|
* gain kD
|
|
*
|
|
* Terms of the controller can be removed by setting their gain to 0, creating a
|
|
* PI (kD = 0) or PD (kI = 0) controller. Depending on the control problem at
|
|
* hand, some terms may not increase the performance of the system, or may have
|
|
* a negative effect.
|
|
*
|
|
* For a more mathematical expanation of the PID Controller, see
|
|
* https://en.wikipedia.org/wiki/PID_controller
|
|
*
|
|
* Limitations of this implementation:
|
|
* - Since this implementation is just for demonstration, the pid_step function
|
|
* takes the dt as a parameter, and it can be provided by the user in main().
|
|
* This allows deterministic experimentation with the algorithm, rather than
|
|
* using time(NULL) which would make the function non-deterministic.
|
|
*
|
|
* Inputs: e(t) - Current error at time t. For example, how far a servo is off
|
|
* the desired angle Output: u(t) - Controller output at time t.
|
|
*/
|
|
#include <stdio.h>
|
|
|
|
struct pid
|
|
{
|
|
// Controller gains
|
|
float kP;
|
|
float kI;
|
|
float kD;
|
|
|
|
// State variables
|
|
float lastError;
|
|
float integral;
|
|
};
|
|
|
|
float pid_step(struct pid *controller, float dt, float error)
|
|
{
|
|
// Calculate p term
|
|
float p = error * controller->kP;
|
|
|
|
// Calculate i term
|
|
controller->integral += error * dt * controller->kI;
|
|
|
|
// Calculate d term, taking care to not divide by zero
|
|
float d =
|
|
dt == 0 ? 0 : ((error - controller->lastError) / dt) * controller->kD;
|
|
controller->lastError = error;
|
|
|
|
return p + controller->integral + d;
|
|
}
|
|
|
|
int main()
|
|
{
|
|
printf("PID Controller Example\n");
|
|
|
|
struct pid controller = {.lastError = 0, .integral = 0};
|
|
|
|
// Take the controller gains from the user
|
|
printf(
|
|
"Please enter controller gains in format kP, kI, KD. For example, "
|
|
"\"1.2 2.1 3.2\"\n> ");
|
|
scanf("%f %f %f", &controller.kP, &controller.kI, &controller.kD);
|
|
printf("Using kP: %f, kI: %f, kD: %f\n", controller.kP, controller.kI,
|
|
controller.kD);
|
|
|
|
// How often the pid_step algorithm expects to be called. In a real life
|
|
// scenario this would be provided by calling time(NULL) - last_time, or by
|
|
// calling the function reliably at X Hz (using a timer or RTOS etc) For
|
|
// demonstration of this algorithm though, it is defined below as 1 second,
|
|
// allowing easy testing of integral and derivative terms.
|
|
float time_step = 1;
|
|
|
|
float error_value;
|
|
while (1)
|
|
{
|
|
printf("Enter error value\n>");
|
|
scanf("%f", &error_value);
|
|
|
|
float output = pid_step(&controller, time_step, error_value);
|
|
printf("Output: %f\n", output);
|
|
}
|
|
}
|