標題: 四軸串級PID程序源碼 已用到項目中 很實用 分享給大家 [打印本頁]
作者: chencccc    時間: 2017-5-24 09:01
標題: 四軸串級PID程序源碼 已用到項目中 很實用 分享給大家
這是我做的PID算法，已用到項目中，很實用，現(xiàn)在分享給大家。

四軸串級PID單片機源程序如下:
  1. #include "CONTROL.h"
  2. #include "IMU1.h"      
  3. #include "moto.h"
  4. #include "RFdate.h"
  5. #include <math.h>
  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;                                   //積分項
  30. float Pitch_old,Roll_old,Yaw_old;                 //角度保存
  31. float Pitch_d,Roll_d,Yaw_d;          //微分項
  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;//內環(huán)單項輸出
  48. float Pitch_core_out,Roll_core_out,Yaw_core_out;//內環(huán)總輸出      
  49.       
  50. //內環(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=-300;
  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=-300;
  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.         ////////////////////////內環(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;   //儲存歷史值
  124.       
  125. //--------------------將輸出值融合到四個電機--------------------------------//

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

  132. //                if(Rc_D.THROTTLE<=2000)
  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. ……………………

  152. …………限于本文篇幅 余下代碼請從51黑下載附件…………
復制代碼

所有資料51hei提供下載:
chuanPID.zip (1.75 KB, 下載次數(shù): 79)


作者: 琳芙斯    時間: 2017-7-25 15:54
謝謝樓主啦~
作者: e介草民    時間: 2017-9-28 09:33
好東西，贊贊贊
作者: liupudong    時間: 2018-2-2 16:49
好東西，謝樓主



歡迎光臨 (http://www.torrancerestoration.com/bbs/) Powered by Discuz! X3.1