|
本帖最后由 CCALM 于 2013-5-21 11:12 编辑
- float P=10.0,I=0,D=32.0; //灵敏 力度 3.9 2.9/0.954
- float rool=0,pitch=0;
- float roolb=0,pitchb=0; //上上一次角度
- float roola=0,pitcha=0; //上一次角度
- float Pmax=180;
- float Rmax=130;
- IMUupdate(GX,GY,GZ,AX,AY,AZ);//更新陀螺仪数据
- /*
- 俯仰角PID---------------------------------------------------------------------------------
- */
- pitch = P * Pitch + I * pitcha - D * GX ;
- pitchb=pitcha;
- pitcha=Pitch;
- if(pitch>Pmax) pitch=Pmax;
- if(pitch<-Pmax) pitch=-Pmax;
- /*
- 翻滚角PID---------------------------------------------------------------------------------
- */
- rool = P * Rool + I * roola - D * GY ;
- roolb=roola;
- roola=-Rool;
- if(Rool<1&&Rool>-1) rool=0;
- if(rool>Rmax) rool=Rmax;
- if(rool<-Rmax) rool=-Rmax;
- M1=pitch;
- M2=rool;
- M3=-pitch;
- M4=-rool;
复制代码 不知道代码有没有错,请大神鉴定
老是震荡,PID调很少也震荡,不然就是没反应 |
阿莫论坛20周年了!感谢大家的支持与爱护!!
知道什么是神吗?其实神本来也是人,只不过神做了人做不到的事情 所以才成了神。 (头文字D, 杜汶泽)
|