#include <stdio.h>
typedef struct {
float SetLocation; /*you want to set location*/
float LastLocation; /*the last location*/
float err_now_0;
float err_last_1;
float err_sum;
float Kp;
float Ki;
float Kd;
float output;
}PID;
PID pid;
void
PID_Config(PID *pid)
{
(*pid).SetLocation = 0.0;
(*pid).LastLocation = 0.0;
(*pid).err_now_0 = 0.0;
(*pid).err_last_1= 0.0;
(*pid).err_sum = 0.0;
(*pid).Kp = 0.2;
(*pid).Ki = 0.015;
(*pid).Kd = 0.2;
(*pid).output = 0.0;
}
float
PID_SetLocation(PID *pid, float speed)
{
(*pid).SetLocation = speed;
(*pid).err_now_0 = (*pid).SetLocation-(*pid).LastLocation;
(*pid).err_sum += (*pid).err_now_0;
(*pid).output = (*pid).Kp * (*pid).err_now_0 + (*pid).Ki * (*pid).err_sum + (*pid).Kd * ((*pid).err_now_0-(*pid).err_last_1);
/*Uo(n) = P *e(n) + I *[e(n)+e(n-1)+...+e(0)]+ D *[e(n)-e(n-1)]*/
(*pid).err_last_1 = (*pid).err_now_0;
(*pid).LastLocation = (*pid).output;
return (*pid).output;
}
int
main(int argc, char *argv[])
{
int count = 0;
float speed;
PID_Config(&pid);
while (count < 10000) {
speed = PID_SetLocation(&pid, 200.0);
count++;
if (speed > 199.99) {
break;
}
}
printf("program had exe %d times \n", count);
return 0;
}
/**
program had exe 773 times
Process returned 0 (0x0) execution time : 0.165 s
*/