#include <stdio.h>
#include <math.h>
#include <stdlib.h>
typedef struct {
float SetSpeed;
float LastLocation;
float err_now_0;
float err_last_1;
float Kp;
float Ki;
float Kd;
float output;
float intergal_sum;
float umax;
float umin;
}PID;
PID pid;
void
PID_Config(PID *pid)
{
(*pid).SetSpeed = 0.0;
(*pid).LastLocation = 0.0;
(*pid).err_now_0 = 0.0;
(*pid).err_last_1 = 0.0;
(*pid).output = 0.0;
(*pid).intergal_sum = 0.0;
(*pid).Kp = 0.2;
(*pid).Ki = 0.1;
(*pid).Kd = 0.2;
(*pid).umax = 400;
(*pid).umin = -200;
}
float
PID_SetSpeed(PID *pid, float speed)
{
char index;
(*pid).SetSpeed = speed;
(*pid).err_now_0 = (*pid).SetSpeed-(*pid).LastLocation;
if ((*pid).LastLocation > (*pid).umax) { /*have arrived the max speed*/
if (abs((*pid).err_now_0) > 200) {
index = 0;
}else{
index = 1;
if ((*pid).err_now_0 < 0){
(*pid).intergal_sum += (*pid).err_now_0;
}
}
} else if ((*pid).LastLocation < (*pid).umin){ /*have arrived the min speed*/
if (abs((*pid).err_now_0) > 200) {
index = 0;
}else{
index = 1;
if ((*pid).err_now_0 > 0) {
(*pid).intergal_sum += (*pid).err_now_0;
}
}
} else{ /*not arrived the max/min speed*/
if (abs((*pid).err_now_0 > 200)){
index = 0;
} else {
index = 1;
(*pid).intergal_sum += (*pid).err_now_0;
}
}
(*pid).output = (*pid).Kp * (*pid).err_now_0 + index * (*pid).Ki * (*pid).intergal_sum / 2+ (*pid).Kd * ((*pid).err_now_0-(*pid).err_last_1);
(*pid).err_last_1 = (*pid).err_now_0;
(*pid).LastLocation = (*pid).output*1.0;
return (*pid).LastLocation;
}
int
main(int argc, char *argv[])
{
int count = 0;
float speed;
PID_Config(&pid);
while (count < 10000) {
speed = PID_SetSpeed(&pid, 200.0);
count++;
if (speed > 199.99) {
break;
}
}
printf("program have exe %d times ", count);
return 0;
}