ExclusiveRR / 文件夹1 / 【开源】分享一个经典的串级PID算法,附源...

分享

   

【开源】分享一个经典的串级PID算法,附源代码

2016-12-20  Exclusive...


一直想搞一下四轴飞行器,不过总是么有时间,前段时间在家休息,好好研究一下四轴飞行器的PID算法。现在凑个热闹,分享传说中的串级PID,内环为角速度环,外环为角速度。整定PID参数的时候先调外环,再调内环。

不多说,先上文件:
chuanPID.zip(1.75 KB)
(下载次数: 1533, 2015-6-5 10:50 上传)

下载积分: 积分 -1 分



想直接看的可以看看这个代码,都是一样的
  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;                //遥控通道数据;

  7. extern u8 txbuf[4];         //发送缓冲
  8. extern u8 rxbuf[4];         //接收缓冲
  9. extern u16 test1[3]; //接收到NRf24L01数据
  10. extern S_INT16_XYZ ACC_F,GYRO_F;

  11. PID PID_ROL,PID_PIT,PID_YAW;

  12. extern S_INT16_XYZ        MPU6050_ACC_LAST,MPU6050_GYRO_LAST;      


  13. int Motor_Ele=0;                                           //俯仰期望
  14. int Motor_Ail=0;                                           //横滚期望

  15. //u8 ARMED = 0;

  16. //float rol_i=0,pit_i=0,yaw_p=0;
  17. float thr=0;

  18. S_FLOAT_XYZ EXP_ANGLE ,DIF_ANGLE;
  19. PID1 PID_Motor;
  20. /*********************************/
  21. float Pitch_i,Roll_i,Yaw_i;                                   //积分项
  22. float Pitch_old,Roll_old,Yaw_old;                 //角度保存
  23. float Pitch_d,Roll_d,Yaw_d;          //微分项
  24. float RC_Pitch,RC_Roll,RC_Yaw;                       //姿态角
  25. float Pitch_shell_out,Roll_shell_out,Yaw_shell_out;//外环总输出
  26.         //外环PID参数
  27. float Pitch_shell_kp=280;//30 140
  28. float Pitch_shell_kd=0;//
  29. float Pitch_shell_ki=0;//
  30. /*********************************/
  31. float Roll_shell_kp=250;//30
  32. float Roll_shell_kd=0;//10                 
  33. float Roll_shell_ki=0;//0.08
  34. /*********************************/
  35. float Yaw_shell_kp=1.5;//10;//30
  36. float Yaw_shell_kd=0;//10                 
  37. float Yaw_shell_ki=0;//0.08;//0.08
  38. float Gyro_radian_old_x,Gyro_radian_old_y,Gyro_radian_old_z;//陀螺仪保存
  39. 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;//内环单项输出
  40. float Pitch_core_out,Roll_core_out,Yaw_core_out;//内环总输出      
  41.       
  42. //内环PID参数
  43. //float Pitch_core_kp=0.040;
  44. //float Pitch_core_kd=0.008;////0.007;//0.07;
  45. float Pitch_core_kp=0.040;
  46. float Pitch_core_kd=0.002;////0.007;//0.07;

  47. float Roll_core_kp=0.040;//;
  48. float Roll_core_kd=0.002;////0.007;//06;//0.07;

  49. float Yaw_core_kp=0.046;//;
  50. float Yaw_core_kd=0.012;////0.007;//06;//0.07;


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

  52. float tempjd=0;
  53. void CONTROL(float rol, float pit, float yaw)
  54. {
  55.       
  56.          RC_Pitch=(Rc_D.PITCH-1500)/20;
  57.       
  58.         ////////////////////////外环角度环(PID)///////////////////////////////
  59.   Pitch_i+=(Q_ANGLE.Pitch-RC_Pitch);
  60. //-------------Pitch积分限幅----------------//
  61.   if(Pitch_i>300) Pitch_i=300;
  62.   else if(Pitch_i<-300) pitch_i="">
  63. //-------------Pitch微分--------------------//
  64.   Pitch_d=Q_ANGLE.Pitch-Pitch_old;
  65. //-------------Pitch  PID-------------------//
  66.   Pitch_shell_out = Pitch_shell_kp*(Q_ANGLE.Pitch-RC_Pitch) + Pitch_shell_ki*Pitch_i + Pitch_shell_kd*Pitch_d;
  67. //角度保存
  68.   Pitch_old=Q_ANGLE.Pitch;
  69. /*********************************************************/      
  70.       
  71.         RC_Roll=(Rc_D.ROLL-1500)/20;
  72.         Roll_i+=(Q_ANGLE.Rool-RC_Roll);
  73. //-------------Roll积分限幅----------------//
  74.   if(Roll_i>300) Roll_i=300;
  75.   else if(Roll_i<-300) roll_i="">
  76. //-------------Roll微分--------------------//
  77.   Roll_d=Q_ANGLE.Rool-Roll_old;
  78. //-------------Roll  PID-------------------//
  79.   Roll_shell_out  = Roll_shell_kp*(Q_ANGLE.Rool-RC_Roll) + Roll_shell_ki*Roll_i + Roll_shell_kd*Roll_d;
  80. //------------Roll角度保存------------------//
  81.   Roll_old=Q_ANGLE.Rool;
  82.       
  83.       
  84.         RC_Yaw=(Rc_D.YAW-1500)*10;
  85. //-------------Yaw微分--------------------//
  86.   Yaw_d=MPU6050_GYRO_LAST.Z-Yaw_old;
  87. //-------------Roll  PID-------------------//
  88.   Yaw_shell_out  = Yaw_shell_kp*(MPU6050_GYRO_LAST.Z-RC_Yaw) + Yaw_shell_ki*Yaw_i + Yaw_shell_kd*Yaw_d;
  89. //------------Roll角度保存------------------//
  90.   Yaw_old=MPU6050_GYRO_LAST.Z;
  91.       
  92.       
  93.         ////////////////////////内环角速度环(PD)///////////////////////////////      
  94.   pitch_core_kp_out = Pitch_core_kp * (Pitch_shell_out + MPU6050_GYRO_LAST.Y * 3.5);
  95.   pitch_core_kd_out = Pitch_core_kd * (MPU6050_GYRO_LAST.Y   - Gyro_radian_old_y);

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

  98.   Yaw_core_kp_out  = Yaw_core_kp  * (Yaw_shell_out  + MPU6050_GYRO_LAST.Z * 1);
  99.   Yaw_core_kd_out  = Yaw_core_kd  * (MPU6050_GYRO_LAST.Z   - Gyro_radian_old_z);
  100.       
  101.       
  102.         Pitch_core_out = pitch_core_kp_out + pitch_core_kd_out;
  103.   Roll_core_out  = Roll_core_kp_out  + Roll_core_kd_out;
  104.   Yaw_core_out   = Yaw_core_kp_out   + Yaw_core_kd_out;

  105.   Gyro_radian_old_y = MPU6050_GYRO_LAST.X;
  106.   Gyro_radian_old_x = MPU6050_GYRO_LAST.Y;
  107.   Gyro_radian_old_z = MPU6050_GYRO_LAST.Z;   //储存历史值
  108.       
  109. //--------------------将输出值融合到四个电机--------------------------------//

  110.       
  111.         if(Rc_D.THROTTLE>1020)
  112.         {
  113.   thr=Rc_D.THROTTLE- 1000;

  114. //                if(Rc_D.THROTTLE<>
  115. //                {
  116. //  moto1=(int16_t)(thr  - Pitch_core_out);//- yaw);
  117. //        moto2=(int16_t)(thr  - Pitch_core_out);//+ yaw);      
  118. //        moto3=(int16_t)(thr  + Pitch_core_out);// - yaw);
  119. //        moto4=(int16_t)(thr  + Pitch_core_out);//+ yaw);      
  120.    
  121. //  moto1=(int16_t)(thr  - Roll_core_out);//- yaw);
  122. //        moto2=(int16_t)(thr  + Roll_core_out);//+ yaw);      
  123. //        moto3=(int16_t)(thr  + Roll_core_out);// - yaw);
  124. //        moto4=(int16_t)(thr  - Roll_core_out);//+ yaw);

  125. //  moto1=(int16_t)(thr  - Yaw_core_out);//- yaw);
  126. //        moto2=(int16_t)(thr  + Yaw_core_out);//+ yaw);      
  127. //        moto3=(int16_t)(thr  - Yaw_core_out);// - yaw);
  128. //        moto4=(int16_t)(thr  + Yaw_core_out);//+ yaw);                       
  129.                        
  130. //moto1=(int16_t)(thr - Roll_core_out - Pitch_core_out);
  131. //moto2=(int16_t)(thr + Roll_core_out - Pitch_core_out);      
  132. //moto3=(int16_t)(thr + Roll_core_out + Pitch_core_out);
  133. //moto4=(int16_t)(thr - Roll_core_out + Pitch_core_out);      
  134. //                       
  135.   moto1=(int16_t)(thr - Roll_core_out - Pitch_core_out- Yaw_core_out);
  136.         moto2=(int16_t)(thr + Roll_core_out - Pitch_core_out+ Yaw_core_out);      
  137.         moto3=(int16_t)(thr + Roll_core_out + Pitch_core_out- Yaw_core_out);
  138.         moto4=(int16_t)(thr - Roll_core_out + Pitch_core_out+ Yaw_core_out);                       
  139.                        
  140. //                }
  141.   }
  142.         else
  143.         {
  144.                 moto1 = 0;
  145.                 moto2 = 0;
  146.                 moto3 = 0;
  147.                 moto4 = 0;
  148.         }
  149.         MOTO_PWMRFLASH(moto1,moto2,moto3,moto4);//        Moto_PwmRflash(moto1,moto2,moto3,moto4);
  150. }



    本站是提供个人知识管理的网络存储空间,所有内容均由用户发布,不代表本站观点。请注意甄别内容中的联系方式、诱导购买等信息,谨防诈骗。如发现有害或侵权内容,请点击一键举报。

    0条评论

    发表

    请遵守用户 评论公约

    类似文章 更多
    喜欢该文的人也喜欢 更多

    ×
    ×

    ¥.00

    微信或支付宝扫码支付:

    开通即同意《个图VIP服务协议》

    全部>>