设为首页收藏本站
开启辅助访问 切换到宽版

 找回密码
 立即注册

QQ登录

只需一步，快速开始

热门： 51单片机 | 24小时必答区 | 单片机教程 | 单片机DIY制作 | STM32 | Cortex M3 | 模数电子 | 电子DIY制作 | 音响/功放 | 拆机乐园 | Arduino | 嵌入式OS | 程序设计

搜索
»论坛 › 电子技术分类讨论区 › 四轴/航模/飞行器DIY › 四轴串级PID程序源码 已用到项目中 很实用 分享给大家
返回列表 发新帖
查看: 5354|回复: 4
收起左侧

四轴串级PID程序源码 已用到项目中 很实用 分享给大家

[复制链接]
ID:204159 发表于 2017-5-24 09:01 | 显示全部楼层 |阅读模式
这是我做的PID算法，已用到项目中，很实用，现在分享给大家。

四轴串级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;                //遥控通道数据;

  8. extern u8 txbuf[4];         //发送缓冲
  9. extern u8 rxbuf[4];         //接收缓冲
  10. extern u16 test1[3]; //接收到NRf24L01数据
  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;                       //姿态角
  33. float Pitch_shell_out,Roll_shell_out,Yaw_shell_out;//外环总输出
  34.         //外环PID参数
  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;//内环单项输出
  48. float Pitch_core_out,Roll_core_out,Yaw_core_out;//内环总输出      
  49.       
  50. //内环PID参数
  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.         ////////////////////////外环角度环(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.         ////////////////////////内环角速度环(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, 下载次数: 73)
回复

使用道具 举报

ID:222327 发表于 2017-7-25 15:54 | 显示全部楼层
谢谢楼主啦~
回复

使用道具 举报

ID:235962 发表于 2017-9-28 09:33 | 显示全部楼层
好东西，赞赞赞
回复

使用道具 举报

ID:267230 发表于 2018-2-2 16:49 | 显示全部楼层
好东西，谢楼主
回复

使用道具 举报

返回列表 发新帖
高级模式
B Color Image Link Quote Code Smilies
您需要登录后才可以回帖 登录 | 立即注册

本版积分规则

手机版|小黑屋|51黑电子论坛 |51黑电子论坛6群 QQ 管理员QQ:125739409;技术交流QQ群281945664

Powered by 单片机教程网

快速回复 返回顶部 返回列表