Thư viện PID - C Language
/* PID Library Copyright @2013 vidieukhien.net forum */ #ifndef _PID_H_ #define _PID_H_ #include <stdint.h> #define PID_DIRECT 0 #define PID_REVERSE 1 typedef struct { double kp; double ki; double kd; double *input; double *output; double *setPoint; double outMin; double outMax; double lastInput; double lastProcessValue; double sampleTime; } PID; void PID_Init(PID *pid, double *input, double *output, double *setPoint, double kp, double ki, double kd, double sampleTimeMs, uint8_t direction); void PID_SetOutputLimits(PID *pid, double min, double max); void PID_Compute(PID *pid); void PID_SetTunings(PID *pid, double kp, double ki, double kd, uint8_t direction); #endif
/* PID Library Copyright @2013 vidieukhien.net forum */ #include "pid.h" void PID_SetOutputLimits(PID *pid, double min, double max) { pid->outMin = min; pid->outMax = max; if(pid->lastProcessValue < min) pid->lastProcessValue = min; if(pid->lastProcessValue > max) pid->lastProcessValue = max; if(*pid->output < min) *pid->output = min; if(*pid->output > max) *pid->output = max; } void PID_SetTunings(PID *pid, double kp, double ki, double kd, uint8_t direction) { float sampleTimeInSec = pid->sampleTime/1000.0; pid->kp = kp; pid->ki = ki * sampleTimeInSec; pid->kd = kd / sampleTimeInSec; if(direction == PID_REVERSE) { pid->kp = -pid->kp; pid->ki = -pid->ki; pid->kd = -pid->kd; } } void PID_Init(PID *pid, double *input, double *output, double *setPoint, double kp, double ki, double kd, double sampleTimeMs, uint8_t direction) { pid->input = input; pid->output = output; pid->setPoint = setPoint; pid->lastInput = *input; pid->lastProcessValue = *output; pid->sampleTime = sampleTimeMs; PID_SetTunings(pid, kp, ki, kd, direction); PID_SetOutputLimits(pid, 100, 1000); } void PID_Compute(PID *pid) { double input, error, dinput, output; input = *pid->input; error = *pid->setPoint - input; pid->lastProcessValue += (pid->ki * error); if(pid->lastProcessValue > pid->outMax) pid->lastProcessValue = pid->outMax; if(pid->lastProcessValue < pid->outMin) pid->lastProcessValue = pid->outMin; dinput = input - pid->lastInput; output = pid->kp * error + pid->lastProcessValue - pid->kd * dinput; if(output > pid->outMax) output = pid->outMax; if(output < pid->outMin) output = pid->outMin; *pid->output = output; pid->lastInput = input; }
/*app examle */ #include "pid.h" PID pid; double vInput, vSetpoint, pwmOutput; double kp = 3, ki = 7, kd = 0.1, sampleTime = 100; /* initilize pid in initilization function */ void APP_Init() { vInput = SENSOR_Read(); vSetpoint = 30; /*Set voltage at 30V*/ PID_Init(&pid, &vInput, &pwmOutput, &vSetpoint, kp, ki, kd, sampleTime, PID_REVERSE); } /*Call every sample time (in milisecond)*/ void TaskEvery100ms() { //Read sensor to vInput vInput = SENSOR_Read(); //Calculate PID PID_Compute(&pid); //Set PWM duty PWM_SetDuty(pwmOutput); } void APP_SetVref(double volt) { vSetpoint = volt; }
[[youtube-{A64S2gVdDH0}-{688}x{387}]]