pww999 发表于 2012-7-21 11:37
//卡耳漫滤波 PD融合控制代码:
#include "Wire.h"
您好:
請問您寫的PID和這個有甚麼不同啊??您的代碼看起來很複雜。
我看板上有人用這種PID也可以平衡,我用這種作四軸都無法平衡。
懇請大大教我{:3_59:}
float Input = f_angle, Input2 = f_angle2; // 輸入賦值
float error = Setpoint - Input, error2 = Setpoint - Input2; // 偏差值
ITerm+= (Ki*error);// * TimeCh); // 計算積分項
ITerm = constrain(ITerm, outMin, outMax); // 限定值域
float DTerm = (Input - lastError) / TimeCh; // 計算微分項
Output = Kp * error + Ki * ITerm - Kd * DTerm; // 計算輸出值
Output = constrain(Output, outMin, outMax); // 限定值域
謝謝 |