74 lines
2.2 KiB
C
74 lines
2.2 KiB
C
#include <math.h>
|
|
#include <stdio.h>
|
|
#include <stdlib.h>
|
|
#include <string.h>
|
|
|
|
#define BUFFER_SIZE 100
|
|
|
|
typedef struct {
|
|
float Kp, Ki, Kd; // PID gains
|
|
float error_buffer[BUFFER_SIZE]; // Circular buffer for integral calculation
|
|
int buffer_index; // Current index in the buffer
|
|
float integral_sum; // Running sum of integral terms
|
|
float prev_error; // Previous error for derivative term
|
|
float dt; // Sample time (seconds)
|
|
} PIDController;
|
|
|
|
// Initialize PID controller
|
|
void pid_init(PIDController *pid, float Kp, float Ki, float Kd, float dt) {
|
|
pid->Kp = Kp;
|
|
pid->Ki = Ki;
|
|
pid->Kd = Kd;
|
|
pid->dt = dt;
|
|
pid->buffer_index = 0;
|
|
pid->integral_sum = 0.0f;
|
|
pid->prev_error = 0.0f;
|
|
memset(pid->error_buffer, 0, sizeof(pid->error_buffer));
|
|
}
|
|
|
|
// Compute PID output
|
|
float pid_compute(PIDController *pid, float setpoint, float pv) {
|
|
float error = setpoint - pv;
|
|
|
|
// Update integral term using circular buffer
|
|
pid->integral_sum -=
|
|
pid->error_buffer[pid->buffer_index]; // Remove oldest value
|
|
pid->error_buffer[pid->buffer_index] =
|
|
error * pid->dt; // Store new integral term
|
|
pid->integral_sum += pid->error_buffer[pid->buffer_index]; // Add new value
|
|
|
|
// Advance circular buffer index
|
|
pid->buffer_index = (pid->buffer_index + 1) % BUFFER_SIZE;
|
|
|
|
// Compute derivative term
|
|
float derivative = (error - pid->prev_error) / pid->dt;
|
|
pid->prev_error = error;
|
|
|
|
// Compute PID output
|
|
float output = (pid->Kp * error) + (pid->Ki * pid->integral_sum) +
|
|
(pid->Kd * derivative);
|
|
|
|
return output;
|
|
}
|
|
|
|
int main() {
|
|
PIDController pid;
|
|
pid_init(&pid, 1.0f, 0.1f, 0.05f,
|
|
0.01f); // Kp, Ki, Kd, dt (10ms sample time)
|
|
|
|
float setpoint = 100.0f;
|
|
float pv = 90.0f;
|
|
|
|
for (int i = 0; i < 50; i++) {
|
|
float output = pid_compute(&pid, setpoint, pv);
|
|
printf("Iteration %d: Setpoint: %.2f, PV: %.2f, Output: %.2f\n", i,
|
|
setpoint, pv, output);
|
|
|
|
// Simulate process variable update
|
|
pv += output * 0.1f;
|
|
if (fabsf(output) < 0.4)
|
|
setpoint = 150;
|
|
}
|
|
|
|
return 0;
|
|
}
|