久久国产成人av_抖音国产毛片_a片网站免费观看_A片无码播放手机在线观看,色五月在线观看,亚洲精品m在线观看,女人自慰的免费网址,悠悠在线观看精品视频,一级日本片免费的,亚洲精品久,国产精品成人久久久久久久

搜索
分享

【開源】分享一個(gè)經(jīng)典的串級(jí)PID算法,，附源代碼

 ExclusiveRR 2016-12-20


一直想搞一下四軸飛行器,，不過總是么有時(shí)間,，前段時(shí)間在家休息，好好研究一下四軸飛行器的PID算法?，F(xiàn)在湊個(gè)熱鬧,，分享傳說中的串級(jí)PID，內(nèi)環(huán)為角速度環(huán),，外環(huán)為角速度,。整定PID參數(shù)的時(shí)候先調(diào)外環(huán)，再調(diào)內(nèi)環(huán),。

不多說,，先上文件：
chuanPID.zip(1.75 KB)
(下載次數(shù): 1533, 2015-6-5 10:50 上傳)

下載積分: 積分 -1 分



想直接看的可以看看這個(gè)代碼，都是一樣的
  1. #include 'CONTROL.h'
  2. #include 'IMU1.h'      
  3. #include 'moto.h'
  4. #include 'RFdate.h'
  5. #include
  6. extern T_RC_Data                         Rc_D;                //遙控通道數(shù)據(jù);

  8. extern u8 txbuf[4];         //發(fā)送緩沖
  9. extern u8 rxbuf[4];         //接收緩沖
  10. extern u16 test1[3]; //接收到NRf24L01數(shù)據(jù)
  11. extern S_INT16_XYZ ACC_F,GYRO_F;

  13. PID PID_ROL,PID_PIT,PID_YAW;

  15. extern S_INT16_XYZ        MPU6050_ACC_LAST,MPU6050_GYRO_LAST;      


  18. int Motor_Ele=0;                                           //俯仰期望
  19. int Motor_Ail=0;                                           //橫滾期望

  21. //u8 ARMED = 0;

  23. //float rol_i=0,pit_i=0,yaw_p=0;
  24. float thr=0;

  26. S_FLOAT_XYZ EXP_ANGLE ,DIF_ANGLE;
  27. PID1 PID_Motor;
  28. /*********************************/
  29. float Pitch_i,Roll_i,Yaw_i;                                   //積分項(xiàng)
  30. float Pitch_old,Roll_old,Yaw_old;                 //角度保存
  31. float Pitch_d,Roll_d,Yaw_d;          //微分項(xiàng)
  32. float RC_Pitch,RC_Roll,RC_Yaw;                       //姿態(tài)角
  33. float Pitch_shell_out,Roll_shell_out,Yaw_shell_out;//外環(huán)總輸出
  34.         //外環(huán)PID參數(shù)
  35. float Pitch_shell_kp=280;//30 140
  36. float Pitch_shell_kd=0;//
  37. float Pitch_shell_ki=0;//
  38. /*********************************/
  39. float Roll_shell_kp=250;//30
  40. float Roll_shell_kd=0;//10                 
  41. float Roll_shell_ki=0;//0.08
  42. /*********************************/
  43. float Yaw_shell_kp=1.5;//10;//30
  44. float Yaw_shell_kd=0;//10                 
  45. float Yaw_shell_ki=0;//0.08;//0.08
  46. float Gyro_radian_old_x,Gyro_radian_old_y,Gyro_radian_old_z;//陀螺儀保存
  47. float pitch_core_kp_out,pitch_core_kd_out,Roll_core_kp_out,Roll_core_kd_out,Yaw_core_kp_out,Yaw_core_kd_out;//內(nèi)環(huán)單項(xiàng)輸出
  48. float Pitch_core_out,Roll_core_out,Yaw_core_out;//內(nèi)環(huán)總輸出      
  49.       
  50. //內(nèi)環(huán)PID參數(shù)
  51. //float Pitch_core_kp=0.040;
  52. //float Pitch_core_kd=0.008;////0.007;//0.07;
  53. float Pitch_core_kp=0.040;
  54. float Pitch_core_kd=0.002;////0.007;//0.07;

  56. float Roll_core_kp=0.040;//;
  57. float Roll_core_kd=0.002;////0.007;//06;//0.07;

  59. float Yaw_core_kp=0.046;//;
  60. float Yaw_core_kd=0.012;////0.007;//06;//0.07;


  63. int16_t moto1=0,moto2=0,moto3=0,moto4=0;

  65. float tempjd=0;
  66. void CONTROL(float rol, float pit, float yaw)
  67. {
  68.       
  69.          RC_Pitch=(Rc_D.PITCH-1500)/20;
  70.       
  71.         ////////////////////////外環(huán)角度環(huán)(PID)///////////////////////////////
  72.   Pitch_i+=(Q_ANGLE.Pitch-RC_Pitch);
  73. //-------------Pitch積分限幅----------------//
  74.   if(Pitch_i>300) Pitch_i=300;
  75.   else if(Pitch_i<-300) pitch_i="">
  76. //-------------Pitch微分--------------------//
  77.   Pitch_d=Q_ANGLE.Pitch-Pitch_old;
  78. //-------------Pitch  PID-------------------//
  79.   Pitch_shell_out = Pitch_shell_kp*(Q_ANGLE.Pitch-RC_Pitch) + Pitch_shell_ki*Pitch_i + Pitch_shell_kd*Pitch_d;
  80. //角度保存
  81.   Pitch_old=Q_ANGLE.Pitch;
  82. /*********************************************************/      
  83.       
  84.         RC_Roll=(Rc_D.ROLL-1500)/20;
  85.         Roll_i+=(Q_ANGLE.Rool-RC_Roll);
  86. //-------------Roll積分限幅----------------//
  87.   if(Roll_i>300) Roll_i=300;
  88.   else if(Roll_i<-300) roll_i="">
  89. //-------------Roll微分--------------------//
  90.   Roll_d=Q_ANGLE.Rool-Roll_old;
  91. //-------------Roll  PID-------------------//
  92.   Roll_shell_out  = Roll_shell_kp*(Q_ANGLE.Rool-RC_Roll) + Roll_shell_ki*Roll_i + Roll_shell_kd*Roll_d;
  93. //------------Roll角度保存------------------//
  94.   Roll_old=Q_ANGLE.Rool;
  95.       
  96.       
  97.         RC_Yaw=(Rc_D.YAW-1500)*10;
  98. //-------------Yaw微分--------------------//
  99.   Yaw_d=MPU6050_GYRO_LAST.Z-Yaw_old;
  100. //-------------Roll  PID-------------------//
  101.   Yaw_shell_out  = Yaw_shell_kp*(MPU6050_GYRO_LAST.Z-RC_Yaw) + Yaw_shell_ki*Yaw_i + Yaw_shell_kd*Yaw_d;
  102. //------------Roll角度保存------------------//
  103.   Yaw_old=MPU6050_GYRO_LAST.Z;
  104.       
  105.       
  106.         ////////////////////////內(nèi)環(huán)角速度環(huán)(PD)///////////////////////////////      
  107.   pitch_core_kp_out = Pitch_core_kp * (Pitch_shell_out + MPU6050_GYRO_LAST.Y * 3.5);
  108.   pitch_core_kd_out = Pitch_core_kd * (MPU6050_GYRO_LAST.Y   - Gyro_radian_old_y);

  110.   Roll_core_kp_out  = Roll_core_kp  * (Roll_shell_out  + MPU6050_GYRO_LAST.X *3.5);
  111.   Roll_core_kd_out  = Roll_core_kd  * (MPU6050_GYRO_LAST.X   - Gyro_radian_old_x);

  113.   Yaw_core_kp_out  = Yaw_core_kp  * (Yaw_shell_out  + MPU6050_GYRO_LAST.Z * 1);
  114.   Yaw_core_kd_out  = Yaw_core_kd  * (MPU6050_GYRO_LAST.Z   - Gyro_radian_old_z);
  115.       
  116.       
  117.         Pitch_core_out = pitch_core_kp_out + pitch_core_kd_out;
  118.   Roll_core_out  = Roll_core_kp_out  + Roll_core_kd_out;
  119.   Yaw_core_out   = Yaw_core_kp_out   + Yaw_core_kd_out;

  121.   Gyro_radian_old_y = MPU6050_GYRO_LAST.X;
  122.   Gyro_radian_old_x = MPU6050_GYRO_LAST.Y;
  123.   Gyro_radian_old_z = MPU6050_GYRO_LAST.Z;   //儲(chǔ)存歷史值
  124.       
  125. //--------------------將輸出值融合到四個(gè)電機(jī)--------------------------------//

  127.       
  128.         if(Rc_D.THROTTLE>1020)
  129.         {
  130.   thr=Rc_D.THROTTLE- 1000;

  132. //                if(Rc_D.THROTTLE<>
  133. //                {
  134. //  moto1=(int16_t)(thr  - Pitch_core_out);//- yaw);
  135. //        moto2=(int16_t)(thr  - Pitch_core_out);//+ yaw);      
  136. //        moto3=(int16_t)(thr  + Pitch_core_out);// - yaw);
  137. //        moto4=(int16_t)(thr  + Pitch_core_out);//+ yaw);      
  138.    
  139. //  moto1=(int16_t)(thr  - Roll_core_out);//- yaw);
  140. //        moto2=(int16_t)(thr  + Roll_core_out);//+ yaw);      
  141. //        moto3=(int16_t)(thr  + Roll_core_out);// - yaw);
  142. //        moto4=(int16_t)(thr  - Roll_core_out);//+ yaw);

  144. //  moto1=(int16_t)(thr  - Yaw_core_out);//- yaw);
  145. //        moto2=(int16_t)(thr  + Yaw_core_out);//+ yaw);      
  146. //        moto3=(int16_t)(thr  - Yaw_core_out);// - yaw);
  147. //        moto4=(int16_t)(thr  + Yaw_core_out);//+ yaw);                       
  148.                        
  149. //moto1=(int16_t)(thr - Roll_core_out - Pitch_core_out);
  150. //moto2=(int16_t)(thr + Roll_core_out - Pitch_core_out);      
  151. //moto3=(int16_t)(thr + Roll_core_out + Pitch_core_out);
  152. //moto4=(int16_t)(thr - Roll_core_out + Pitch_core_out);      
  153. //                       
  154.   moto1=(int16_t)(thr - Roll_core_out - Pitch_core_out- Yaw_core_out);
  155.         moto2=(int16_t)(thr + Roll_core_out - Pitch_core_out+ Yaw_core_out);      
  156.         moto3=(int16_t)(thr + Roll_core_out + Pitch_core_out- Yaw_core_out);
  157.         moto4=(int16_t)(thr - Roll_core_out + Pitch_core_out+ Yaw_core_out);                       
  158.                        
  159. //                }
  160.   }
  161.         else
  162.         {
  163.                 moto1 = 0;
  164.                 moto2 = 0;
  165.                 moto3 = 0;
  166.                 moto4 = 0;
  167.         }
  168.         MOTO_PWMRFLASH(moto1,moto2,moto3,moto4);//        Moto_PwmRflash(moto1,moto2,moto3,moto4);
  169. }

    本站是提供個(gè)人知識(shí)管理的網(wǎng)絡(luò)存儲(chǔ)空間,，所有內(nèi)容均由用戶發(fā)布,，不代表本站觀點(diǎn)。請(qǐng)注意甄別內(nèi)容中的聯(lián)系方式,、誘導(dǎo)購(gòu)買等信息,，謹(jǐn)防詐騙。如發(fā)現(xiàn)有害或侵權(quán)內(nèi)容,，請(qǐng)點(diǎn)擊一鍵舉報(bào),。
    轉(zhuǎn)藏 分享 獻(xiàn)花（0）

    0條評(píng)論

    發(fā)表

    請(qǐng)遵守用戶 評(píng)論公約

    類似文章 更多