pid controller + unitest

pid_controller.c

#include <stdio.h>
#include <time.h>

typedef struct {
    double Kp;
    double Ki;
    double Kd;
    double sample_time;
    double current_time;
    double last_time;
    double SetPoint;
    double PTerm;
    double ITerm;
    double DTerm;
    double last_error;
    double int_error;
    double windup_guard;
    double output;
} PID;

void PID_clear(PID *pid);

void PID_init(PID *pid, double P, double I, double D) {
    pid->Kp = P;
    pid->Ki = I;
    pid->Kd = D;
    pid->sample_time = 0.0;
    pid->current_time = (double)clock() / CLOCKS_PER_SEC;
    pid->last_time = pid->current_time;
    PID_clear(pid);
}

void PID_clear(PID *pid) {
    pid->SetPoint = 0.0;
    pid->PTerm = 0.0;
    pid->ITerm = 0.0;
    pid->DTerm = 0.0;
    pid->last_error = 0.0;
    pid->int_error = 0.0;
    pid->windup_guard = 100.0;
    pid->output = 0.0;
}

void PID_update(PID *pid, double feedback_value) {
    double error = feedback_value - pid->SetPoint;
    double delta_time = pid->sample_time;
    double delta_error = error - pid->last_error;

    pid->PTerm = pid->Kp * error;
    if (pid->PTerm > 90.0) {
        pid->PTerm = 90.0;
    } else if (pid->PTerm < -90.0) {
        pid->PTerm = -90.0;
    }

    pid->ITerm += pid->Ki * error * delta_time;

    pid->DTerm = pid->Kd * delta_error / delta_time;
    if (pid->DTerm > 90.0) {
        pid->DTerm = 90.0;
    } else if (pid->DTerm < -90.0) {
        pid->DTerm = -90.0;
    }

    pid->last_error = error;

    double Output = pid->PTerm + pid->ITerm + pid->DTerm;

    if (Output > 90.0) {
        pid->output = 90.0;
    } else if (Output < -90.0) {
        pid->output = -90.0;
    } else {
        pid->output = Output;
    }
}

void PID_setKp(PID *pid, double Proportional_gain) {
    pid->Kp = Proportional_gain;
}

void PID_setKi(PID *pid, double Integral_gain) {
    pid->Ki = Integral_gain;
}

void PID_setKd(PID *pid, double derivative_gain) {
    pid->Kd = derivative_gain;
}

void PID_setSampleTime(PID *pid, double sample_time) {
    pid->sample_time = sample_time;
}

void PID_setSetPoint(PID *pid, double setpoint) {
    pid->SetPoint = setpoint;
}

// Unit Test
int main() {
    PID pid;
    PID_init(&pid, 0.2, 0.0, 0.0);  // Initialize with P = 0.2, I = 0, D = 0
    PID_setSampleTime(&pid, 1.0);   // Set sample time to 1 second
    PID_setSetPoint(&pid, 50.0);    // Set the setpoint to 50.0

    // Simulate a process with feedback
    double feedback = 0.0;
    for (int i = 0; i < 10; i++) {
        feedback += 2.0;  // Example: Increase feedback by 2 each iteration
        PID_update(&pid, feedback);
        printf("Iteration %d: Output = %.2f\n", i + 1, pid.output);
    }

    return 0;
}

 //===== Compile the executable =======
// gcc -o pid_controller pid_controller.c
//======Run the executable: After compiling=====
// ./pid_controller