.h
#ifndef _PID_H_ #define _PID_H_ #include "main.h" typedef struct { float SetSpeed; float ActualSeed; float Err; float Err_last; float Kp; float Ki; float Kd; float Voltage; float Integral; }PIDDef; extern void BSP_PID_Init(PIDDef *xPID); extern float PID_Realize(PIDDef *xPID, float speed); #endif
.c
#include "BSP_PID.h" void BSP_PID_Init(PIDDef *xPID) { xPID->SetSpeed = 0; xPID->ActualSeed = 0; xPID->Err = 0; xPID->Err_last = 0; xPID->Voltage = 0; xPID->Integral = 0; xPID->Kp = 0.1; xPID->Ki = 0.15; xPID->Kd = 0.1; } float PID_Realize(PIDDef *xPID, float speed) { xPID->SetSpeed = speed; xPID->Err = xPID->SetSpeed - xPID->ActualSeed; xPID->Integral += xPID->Err; xPID->Voltage = xPID->Kp * xPID->Err + xPID->Ki * xPID->Integral + xPID->Kd * (xPID->Err - xPID->Err_last); xPID->Err_last = xPID->Err; xPID->ActualSeed = xPID->Voltage; return xPID->ActualSeed; }