pwm uart gipo i2c

3rd/pid_control.c

@@ -0,0 +1,45 @@
+#include <stdint.h>
+
+#define  limit            6.3
+#define  Output_ramp      10000
+
+//限幅
+float  _constrain(float amt, float low, float high)    
+{
+	return ((amt<low)?(low):((amt)>(high)?(high):(amt)));
+}
+
+unsigned long Timestamp_Last = 0.0;
+float Last_Error = 0.0;
+float Last_intergration = 0.0;
+float Last_Output = 0.0;
+float PID_Controller(float Kp, float Ki, float Kd, float Error)
+{
+	float Ts = 0.0;
+	//TODO
+	uint32_t Timestamp = 0;
+	//uint32_t Timestamp = SysTick->VAL;
+  if(Timestamp < Timestamp_Last) Ts = (float)(Timestamp_Last - Timestamp)/9*1e-6;
+	else
+		Ts = (0xFFFFFF - Timestamp + Timestamp_Last)/9*1e-6;
+	
+	if(Ts<=0 || Ts > 0.05f) Ts = 0.001;
+	
+	float proportion = Kp * Error;//P环
+	
+	float intergration = Last_intergration + Ki * 0.5f * Ts * Error;//I环
+	intergration = _constrain(intergration, -limit, limit);
+	
+	float differential = Kd * (Error - Last_Error)/Ts;//D环
+	
+	float Output = proportion + intergration + differential;
+	Output = _constrain(Output, -limit, limit);
+	
+	Last_Error = Error;
+	Last_intergration = intergration;
+	Last_Output = Output;
+	Timestamp_Last = Timestamp;
+	
+	return Output;
+}
+