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

 找回密码
 立即注册

QQ登录

只需一步，快速开始

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

搜索
»论坛 › 电子技术分类讨论区 › 四轴/航模/飞行器DIY › 四轴飞行器stc单片机程序
返回列表 发新帖
查看: 5530|回复: 3
打印 上一主题 下一主题
收起左侧

四轴飞行器stc单片机程序

[复制链接]
跳转到指定楼层
楼主
ID:230801 发表于 2017-9-4 10:14 | 只看该作者 |只看大图 回帖奖励 |倒序浏览 |阅读模式
stc也可以做四轴


单片机源程序如下:
  1. //========================================================================
  2. //        作者：xiaoliu
  3. //        邮箱：1042763631
  4. //        版本V1.0
  5. //========================================================================
  6. //                                 爱好者电子工作室
  7. //特别声明：此程序
  8. //         
  9. //
  10. //
  11. //                                 MCU工作频率12MHz
  12. //=========================================================================
  13. //1、如果需要更改工作频率，请修改本工程中的config.h头文件中 MAIN_Fosc的宏定义，
  14. //   延时函数都会保持一致，无需更改延迟的参数。
  15. //2、波特率为2400，如果需要更改，必须和STC-ISP最低波特率保持一致才能实现自动下载
  16. //3、IO口已被重新定义，IAP15W4K58S4最大封装为64脚，具有62个IO口，其中有8个模拟口，
  17. //   当然模拟口也可用作数字口，数字用D表示，模拟用A来表示，此定义专门为STC15W4K系列
  18. //   定义的IO，方便方便以后大家日后的使用，此定义方法类似arduino。
  19. //   使用数字IO口时，定义如下：
  20. //   P3.0-P3.7--->D0-D7   也可以直接使用0-7
  21. //   P2.0-P2.7--->D8-D15  也可以直接使用8-15
  22. //   P4.0-P4.7--->D16-D23 也可以直接使用16-23
  23. //   P5.0-P5.7--->D24-D31 也可以直接使用24-31
  24. //   P6.0-P6.7--->D32-D39 也可以直接使用32-39
  25. //   P7.0-P7.7--->D40-D47 也可以直接使用40-47
  26. //   P0.0-P0.7--->D48-D55 也可以直接使用48-55
  27. //   P1.0-P1.7--->D56-D63 也可以直接使用56-63 也可以使用A0-A7
  28. //   使用模拟IO口时，定义如下：
  29. //   P1.0-P1.7--->0-7
  30. //4、pinMode、digitalWrite必须使用数字IO定义方法，analogRead必须使用模拟IO定义方法
  31. //=============================================================================
  32. #include "config.h"
  33. #include "GPIO.h"
  34. #include "usart.h"
  35. #include "delay.h"
  36. #include "epwm.h"
  37. #include "eInterrupt.h"
  38. #include "IMU.h"
  39. #include "nrf24l01.h"
  40. #include "MPU6050.h"
  41. #include "eeprom.h"
  42. #define Q15(X) \
  43.    ((X < 0.0) ? (int)(32768*(X) - 0.5) : (int)(32767*(X) + 0.5))
  44. #define        EAXSFR()                P_SW2 |=  0x80        /* MOVX A,@DPTR/MOVX @DPTR,A指令的操作对象为扩展SFR(XSFR) */
  45. //PWM定义
  46. #define PWM_L 11 //左边的电机
  47. #define PWM_R 7  //右边的电机
  48. #define PWM_F 10 //前面的电机
  49. #define PWM_B 9  //后面的电机
  50. //mpu6050定义         
  51. #define SDA         22         //定义mpu6050的SDA的数字io
  52. #define SCL   48         //定义mpu6050的SCL的数字io


  55. #define CE    23         
  56. #define CSN   57
  57. #define SCK   56
  58. #define MOSI  34
  59. #define MISO  33
  60. #define IRQ   32

  62. #define LED_4   18
  63. #define LED_2   55
  64. #define LED_3   25
  65. #define LED_1   28

  67. u8 SW2_tmp;
  68. bit        B_8ms;
  69. u8 count;
  70. //******************************************************************************************************
  71. u8        xdata        TxBuf[20]={0};
  72. u8        xdata        RxBuf[20]={0};
  73. u8  xdata RxBuf_temp[20]={0};
  74. u8        xdata        tp[16];
  75. //******************************************************************************************************
  76. u8        YM=0;              //油门变化速度控制，不这样做的话快速变化油门时四轴会失速翻转

  78. int                data speed2=0,speed3=0,speed4=0,speed5=0;           //电机速度参数
  79. int                data PWM2=0,PWM3=0,PWM4=0,PWM5=0;        //加载至PWM模块的参数
  80. int                xdata g_x=0,g_y=0,g_z=0;                        //陀螺仪矫正参数
  81. int                xdata a_x,a_y;                                                //角度矫正参数

  83. long  xdata g_x_aver=0;
  84. long  xdata g_y_aver=0;
  85. long  xdata g_z_aver=0;
  86. int   delta_rc_x=0;
  87. int   delta_rc_y=0;
  88. int   delta_rc_z=0;

  90. u8 YM_last;
  91. int delta_Ax;
  92. int delta_Ay;
  93. int delta_Az;

  95. int Ax_real;
  96. int Ay_real;
  97. int Az_real;

  99. int delta_Ax_last;
  100. int delta_Ay_last;
  101. int delta_Az_last;
  102. //*****************角度参数*************************************************
  103. int xdata Angle_ax=0,Angle_ay=0,Angle_az=0;        //由加速度计算的加速度(弧度制)
  104. int idata Angle_gy=0,Angle_gx=0,Angle_gz=0;        //由角速度计算的角速率(角度制)
  105. int data  AngleX=0,AngleY=0,AngleZ=0;                                //四元数解算出的欧拉角
  106. float xdata  Anglezlate=0;                                //Z轴相关
  107. float xdata IMU_gz;
  108. int xdata  Ax=0,Ay=0;                                        //加入遥控器控制量后的角度
  109. int xdata  gx=0,gy=0;                                        //加入遥控器控制量后的角?
  110. u8 data lastR0=0,ZT=0; //上一次RxBuf[0]数据(RxBuf[0]数据在不断变动的)   状态标识
  111. //****************姿态处理和PID*********************************************
  112. int        data PID_Output;        //PID最终输出量

  114. float        xdata Last_Angle_gx=0;        //外环PI输出量  上一次陀螺仪数据
  115. long        xdata ERRORX_Out=0;        //外环P  外环I  外环误差积分
  116. long        xdata ERRORX_In=0;        //内环P  内环I   内环D  内环误差积分

  118. float        xdata Last_Angle_gy=0;
  119. long        xdata ERRORY_Out=0;
  120. long        xdata ERRORY_In=0;
  121. long        xdata ERRORZ_Out=0;

  123. int xdata Last_Ax=0;
  124. int xdata Last_Ay=0;

  126. unsigned int LED_num=0;

  128. int xdata Last_gx=0;
  129. int xdata Last_gy=0;
  130. long idata PID_P;
  131. long idata PID_I;
  132. long idata PID_D;
  133. #define        Out_XP        18.0f            //外环P
  134. #define        Out_XI        Q15(0.024)        //外环I                Q15(0.024)
  135. #define        Out_XD        3.0f            //外环D

  137. #define        In_XP        Q15(0.3) //内环P 720        0.3                                0.3                 0.5
  138. #define        In_XI        Q15(0.00)//内环I                0.024                   0                  0
  139. #define        In_XD        7.5f                 //内环D 720   5                  7.5                   8.0


  142. #define        In_YP        In_XP
  143. #define        In_YI        In_XI
  144. #define        In_YD        In_XD

  146. #define        Out_YP        Out_XP
  147. #define        Out_YI        Out_XI
  148. #define        Out_YD        Out_XD

  150. #define        ZP        5.0f                          //  5
  151. #define        ZD        8.0f        //自旋控制的PID                 8
  152. #define ZI  Q15(0.24)                        //  0.24

  154. #define        ERR_MAX        8000
  155. #define KALMAN_Q        Q15(0.20)
  156. #define KALMAN_R        Q15(0.80)
  157.          
  158.          
  159. int KalmanFilter_ax( int ResrcData,int ProcessNiose_Q,int MeasureNoise_R);
  160. int KalmanFilter_ay( int ResrcData,int ProcessNiose_Q,int MeasureNoise_R);
  161. int KalmanFilter_az( int ResrcData,int ProcessNiose_Q,int MeasureNoise_R);
  162. int KalmanFilter_gyrox( int ResrcData,int ProcessNiose_Q,int MeasureNoise_R);
  163. int KalmanFilter_gyroy( int ResrcData,int ProcessNiose_Q,int MeasureNoise_R);
  164. int KalmanFilter_gyroz( int ResrcData,int ProcessNiose_Q,int MeasureNoise_R);       
  165. void main()
  166. {               
  167.                 int i=1;
  168.                 u8 j;          
  169.                 Serialbegin(2400,1,COM1);//设置COM1的波特率为2400，将“最低波特率”的值设为2400即可实现自动下载
  170.                 pinMode(SDA,pullup);//设置为内部上拉模式
  171.                 pinMode(SCL,pullup);//设置为内部上拉模式
  172.        
  173.                 pinMode(PWM_L,OUTPP);//设置成推挽模式
  174.                 pinMode(PWM_R,OUTPP);//设置成推挽模式
  175.                 pinMode(PWM_F,OUTPP);//设置成推挽模式
  176.                 pinMode(PWM_B,OUTPP);//设置成推挽模式

  178.                 pinMode(LED_4,OUTPP);//设置成推挽模式       
  179.                 pinMode(LED_2,OUTPP);//设置成推挽模式                
  180.                 pinMode(LED_3,OUTPP);//设置成推挽模式
  181.                 pinMode(LED_1,OUTPP);//设置成推挽模式         

  183.                 pinMode(CE,pullup);//设置为内部上拉模式
  184.                 pinMode(CSN,pullup);//设置为内部上拉模式
  185.                 pinMode(SCK,pullup);//设置为内部上拉模式
  186.                 pinMode(MOSI,pullup);//设置为内部上拉模式
  187.                 pinMode(MISO,pullup);//设置为内部上拉模式
  188.                 pinMode(IRQ,pullup);//设置为内部上拉模式
  189.                
  190.                 //以下为PWM初始化       
  191.               epwmnTcolck(12);//系统时钟，不分频
  192.                 epwmTwide(20000);//设置PWM宽度为1001
  193.                 epwmStartADC(0);//不触发AD
  194.                 epwmFirstSignalLevel(PWM_L,0);//设置初始电平为低
  195.                 epwmFirstSignalLevel(PWM_R,0);//设置初始电平为低
  196.                 epwmFirstSignalLevel(PWM_F,0);//设置初始电平为低
  197.                 epwmFirstSignalLevel(PWM_B,0);//设置初始电平为低
  198.                
  199.                 epwmSetValue(PWM_L,19300,20000);//设置翻转值，第1次翻转为1000，第2次翻转为1001
  200.                 epwmSetValue(PWM_R,19300,20000);//设置翻转值，第1次翻转为1000，第2次翻转为1001
  201.                 epwmSetValue(PWM_F,19300,20000);//设置翻转值，第1次翻转为1000，第2次翻转为1001
  202.                 epwmSetValue(PWM_B,19300,20000);//设置翻转值，第1次翻转为1000，第2次翻转为1001
  203.                 epwmCRbegin();//启动PWM计数器
  204.                 init_NRF24L01();//初始化无线模块
  205.                 RxBuf[1] = 128;
  206.                 RxBuf[2] = 128;
  207.                 RxBuf[3] = 128;
  208.                 RxBuf[4] = 0;
  209.                 IAPRead();                //读取陀螺仪静差
  210.                 InitMPU6050();        //初始化MPU-6050
  211.                 Timer0_attachInterrupt(10000);//定时器0以10ms定时中断
  212.                 Timer3_attachInterrupt(4000);//
  213.                   loop()
  214.                 {                                                         
  215.                         YM = RxBuf[4];        //油门

  217.                             
  218.                        
  219.                         if((RxBuf[5] == 1)||(RxBuf[6] == 1))        //校准时，请按2次，第1次清除角速度，第2次清除角度
  220.                         {
  221.                                 RxBuf[5] = 0;
  222.                                 RxBuf[6] = 0;
  223.                                 RxBuf_temp[5] = 0;
  224.                                 RxBuf_temp[6] = 0;
  225.                                 if(RxBuf[4] < 20)
  226.                                 {               
  227.                                
  228.                                
  229.                                         EA = 0;
  230.                                         TR0=0;
  231.                                         g_x_aver=0;
  232.                                         g_y_aver=0;
  233.                                         g_z_aver=0;
  234.                                         for(j=0;j<64;j++)
  235.                                         {               
  236.                                                         Read_MPU6050(tp);
  237.                                                         g_x_aver=g_x_aver+(((int *)&tp)[4]);
  238.                                                         g_y_aver=g_y_aver+(((int *)&tp)[5]);
  239.                                                         g_z_aver=g_z_aver+(((int *)&tp)[6]);                                               
  240.                                         }               
  241.                                         g_x_aver=g_x_aver>>6;
  242.                                         g_y_aver=g_y_aver>>6;
  243.                                         g_z_aver=g_z_aver>>6;
  244.                                        
  245.                                         IAP_Gyro();
  246.    
  247.                                         epwmSetValue(PWM_L,19300,20000);//设置翻转值，第1次翻转为960，第2次翻转为1001
  248.                                         epwmSetValue(PWM_R,19300,20000);//设置翻转值，第1次翻转为960，第2次翻转为1001
  249.                                         epwmSetValue(PWM_F,19300,20000);//设置翻转值，第1次翻转为960，第2次翻转为1001
  250.                                         epwmSetValue(PWM_B,19300,20000);//设置翻转值，第1次翻转为960，第2次翻转为1001
  251.                                         delay_ms(100); //校准完毕滴一声

  253.                                         epwmSetValue(PWM_L,19300,20000);//设置翻转值，第1次翻转为1000，第2次翻转为1001
  254.                                         epwmSetValue(PWM_R,19300,20000);//设置翻转值，第1次翻转为1000，第2次翻转为1001
  255.                                         epwmSetValue(PWM_F,19300,20000);//设置翻转值，第1次翻转为1000，第2次翻转为1001
  256.                                         epwmSetValue(PWM_B,19300,20000);//设置翻转值，第1次翻转为1000，第2次翻转为1001
  257.                                         TR0=1;
  258.                                         EA = 1;
  259.                                 }
  260.                         }

  262.                 }
  263. }

  265. void timer0_int (void) interrupt TIMER0_VECTOR
  266. {
  267.         B_8ms = 1;
  268.         P52=1;

  270.          LED_num++;
  271.         if(LED_num>200)
  272.         {
  273.            LED_num=0;
  274. //           P51=1;                               
  275. //           P42=1;
  276. //           P07=1;
  277. //           P54=1;                  
  278.         }
  279.         if(LED_num==50)
  280.         {
  281. //           P51=0;                       
  282. //           P42=0;
  283. //           P07=0;
  284. //           P54=0;                  
  285.         }
  286.        
  287.         if(RxBuf[0] == lastR0)        //如果RxBuf[0]的数据没有收到 即失联
  288.         {
  289.                 if(++ZT >= 128)
  290.                 {
  291.                         ZT = 120;        //状态标识大于128即1秒没有收到数据，失控保护
  292.                        
  293.                         RxBuf[1] = 128;
  294.                         RxBuf_temp[1] = 128;
  295.                         RxBuf[2] = 128; //触发失控保护 ，缓慢下降，俯仰横滚方向舵归中
  296.                         RxBuf_temp[2]=128;
  297.                         RxBuf[3] = 128;
  298.                         RxBuf_temp[3]=128;
  299.                         if(RxBuf[4] != 0)       
  300.                         {
  301.                                 RxBuf[4]--;        //油门在原值逐渐减小
  302.                                 RxBuf_temp[4]=RxBuf[4];
  303.                         }
  304.                 }
  305.         }
  306.         else        ZT = 0;
  307.         lastR0 = RxBuf[0];

  309.         Read_MPU6050(tp);
  310.     Angle_ax=KalmanFilter_ax( ((int *)&tp)[0],KALMAN_Q,KALMAN_R);
  311.         Angle_ay=KalmanFilter_ay( ((int *)&tp)[1],KALMAN_Q,KALMAN_R);
  312.         Angle_az=KalmanFilter_az( ((int *)&tp)[2],KALMAN_Q,KALMAN_R);

  314.         Angle_gx = ((float)(((int *)&tp)[4] - g_x)) / 65.5;        //陀螺仪处理        结果单位是 +-度
  315.         Angle_gy = ((float)(((int *)&tp)[5] - g_y)) / 65.5;        //陀螺仪量程 +-500度/S, 1度/秒 对应读数 65.536
  316.         Angle_gz=KalmanFilter_gyroz( ((int *)&tp)[6],Q15(0.2),Q15(0.8))- g_z;
  317.         IMU_gz=Angle_gz/65.5;
  318.         Last_Angle_gx = Angle_gx;                //储存上一次角速度数据
  319.         Last_Angle_gy = Angle_gy;


  322.        
  323. //*********************************** 四元数解算 ***********************************
  324.         IMUupdate(Angle_gx*0.0174533f, Angle_gy*0.0174533f, IMU_gz*0.0174533f, Angle_ax,Angle_ay,Angle_az);//姿态解算，精度0.1度
  325. //将四轴飞行器的所有数据发到遥控上       
  326.         TxBuf[0]=AngleX-a_x;
  327.         TxBuf[1]=(AngleX-a_x)>>8;
  328.         TxBuf[2]=Angle_ax;
  329.         TxBuf[3]=Angle_ax>>8;
  330.         TxBuf[4]=((int *)&tp)[4]-g_x;
  331.         TxBuf[5]=(((int *)&tp)[4]-g_x)>>8;
  332.         TxBuf[6]=AngleY-a_y;
  333.         TxBuf[7]=(AngleY-a_y)>>8;       
  334.         TxBuf[8]=Angle_ay;
  335.         TxBuf[9]=Angle_ay>>8;       
  336.         TxBuf[10]=((int *)&tp)[5]-g_y;
  337.         TxBuf[11]=(((int *)&tp)[5]-g_y)>>8;       
  338.         TxBuf[12]=AngleZ;//z轴角度始终为0，因为不能从IMU中获取
  339.         TxBuf[13]=(AngleZ)>>8;
  340.         TxBuf[14]=Angle_az;
  341.         TxBuf[15]=Angle_az>>8;
  342.         TxBuf[16]=Angle_gz;
  343.         TxBuf[17]=Angle_gz>>8;
  344.         //0.174533为PI/180 目的是将角度转弧度        四元数计算出 俯仰(X轴): AngleX, 横滚(Y轴): AngleY                       
  345. //************** MPU6050 Y轴与水平倾角 ***********************************************************
  346.         if((RxBuf[2] > 113) && (RxBuf[2] < 143))        RxBuf[2] = 128;                 //113 143 128
  347.         delta_rc_y=128-RxBuf[2] ;
  348.         delta_rc_y=delta_rc_y*10/7;
  349.         //        Ay为误差，delta_rc_y为遥控给的角度作为基准，RxBuf[8]为微调量，a_y为偏差角度，AngleY为采样的角度
  350.         //一般的，在不考虑微调、偏移量，误差=基准-测量值
  351.         Ay  = delta_rc_y -(char)RxBuf[8]*2+a_y-AngleY;
  352.         if(YM > 20)                ERRORY_Out += Ay;                                //外环积分(油门小于某个值时不积分)
  353.         else                        ERRORY_Out = 0;                                        //油门小于定值时清除积分值
  354.         if(ERRORY_Out >  ERR_MAX)        ERRORY_Out =  ERR_MAX;
  355.         else if(ERRORY_Out < -ERR_MAX)        ERRORY_Out = -ERR_MAX;        //积分限幅
  356.         PID_P=(long)Ay*Out_YP;
  357.         PID_I=((long)ERRORY_Out*Out_YI)>>15;
  358.         PID_D=((Angle_gx+Last_Angle_gx)/2)*Out_YD;
  359.         PID_Output = (PID_P +PID_I+PID_D+5)/10;        //外环PID，“+5”为了四舍五入?
  360.         Last_Ay=Ay;
  361.        
  362.         gy=PID_Output - Angle_gx;
  363.         if(YM > 20)ERRORY_In +=gy;//内环积分(油门小于某个值时不积分)
  364.         else                        ERRORY_In = 0;                                                        //油门小于定值时清除积分值
  365.         if(ERRORY_In >  ERR_MAX)        ERRORY_In =  ERR_MAX;
  366.         else if(ERRORY_In < -ERR_MAX)        ERRORY_In = -ERR_MAX;        //积分限幅
  367.         PID_P=((long)gy*In_YP)>>15;
  368.         PID_I=((long)ERRORY_In*In_YI)>>15;
  369.         PID_D=((long)gy - Last_gy)*In_YD;
  370.         PID_Output =PID_P+PID_I+PID_D;
  371.         Last_gy=gy;
  372.         if(PID_Output >  19300)        PID_Output =  19300;  //输出量限幅
  373.         if(PID_Output < -19300)        PID_Output = -19300;
  374.         speed5 =  PID_Output,        speed2 =   -PID_Output;//加载到速度参数
  375. //**************MPU6050 X轴与水平倾角**************************************************
  376.         if((RxBuf[1] > 113) && (RxBuf[1] < 143))        RxBuf[1] = 128;       
  377.         delta_rc_x=RxBuf[1] - 128;
  378.         delta_rc_x=delta_rc_x*10/7;
  379.         //        Ax为误差，delta_rc_x为遥控给的角度作为基准，RxBuf[7]为微调量，a_x为偏差角度，AngleX为采样的角度
  380.         //一般的，在不考虑微调、偏移量，误差=基准-测量值
  381.         Ax = delta_rc_x+(char)RxBuf[7]*2+a_x-AngleX;
  382.         if(YM > 20)        ERRORX_Out += Ax;        //外环积分(油门小于某个值时不积分)
  383.         else                ERRORX_Out = 0;                //油门小于定值时清除积分值
  384.         if(ERRORX_Out >  ERR_MAX)        ERRORX_Out =  ERR_MAX;        //积分限幅
  385.         else if(ERRORX_Out < -ERR_MAX)        ERRORX_Out = -ERR_MAX;        //积分限幅       
  386.         PID_P=(long)Ax*Out_YP;
  387.         PID_I=((long)ERRORX_Out*Out_XI)>>15;
  388.         PID_D=((Angle_gy+Last_Angle_gy)/2)*Out_XD;
  389.         PID_Output = (PID_P+PID_I+PID_D+5)/10;        //外环PID
  390.         Last_Ax=Ax;
  391.         gx=PID_Output - Angle_gy;
  392.         if(YM > 20)        ERRORX_In += gx;        //内环积分(油门小于某个值时不积分)
  393.         else                ERRORX_In  = 0; //油门小于定值时清除积分值
  394.         if(ERRORX_In >  ERR_MAX)        ERRORX_In =  ERR_MAX;
  395.         else if(ERRORX_In < -ERR_MAX)        ERRORX_In = -ERR_MAX;        //积分限幅

  397.         PID_P=((long)gx*In_XP)>>15;
  398.         PID_I=((long)ERRORX_In*In_XI)>>15;
  399.         PID_D=((long)gx - Last_gx)*In_XD;
  400.         PID_Output =PID_P+PID_I+PID_D;
  401.        
  402.         Last_gx=gx;
  403.         if(PID_Output >  19300)        PID_Output =  19300;  //输出量限幅
  404.         if(PID_Output < -19300)        PID_Output = -19300;
  405.                        
  406.                                                                                 speed4 = 0 - PID_Output;
  407.        
  408.                                                                                 speed3 = 0 + PID_Output;
  409.        
  410. //************** MPU6050 Z轴指向 *****************************       
  411. //只做速度环即可
  412.         if((RxBuf[3] > 113) && (RxBuf[3] < 143))        RxBuf[3] = 128;
  413.         delta_rc_z = (RxBuf[3] - 128)-Angle_gz;        //操作量       
  414.         delta_rc_z = ((int)RxBuf[3] - 128)*64-Angle_gz;        //本来是乘与65.5的系数，乘与64也可以
  415.         if(YM > 20)                ERRORZ_Out += delta_rc_z;
  416.         else                        ERRORZ_Out  = 0;
  417.         if(ERRORZ_Out >  50000)        ERRORZ_Out =  50000;
  418.         else if(ERRORZ_Out < -50000)        ERRORZ_Out = -50000;        //积分限幅
  419.         PID_P=((long)delta_rc_z)*ZP;
  420.         PID_I=((long)ERRORZ_Out * ZI)>>15;
  421.         PID_D=((long)delta_rc_z - Anglezlate) * ZD;

  423.         PID_Output =( PID_P + PID_I + PID_D)>>6;                                                                                 
  424.        
  425.         Anglezlate = delta_rc_z;
  426.         if(PID_Output >  19300)        PID_Output =  19300;  //输出量限幅
  427.         if(PID_Output < -19300)        PID_Output = -19300;
  428.                 speed4 = speed4 + PID_Output;
  429.                 speed5 = speed5 - PID_Output;speed2 = speed2 - PID_Output;
  430.                 speed3 = speed3 + PID_Output;
  431.                
  432. //**************将速度参数加载至PWM模块*************************************************       
  433.         if(YM < 20)                PWM2 = 19300,        PWM3 = 19300,        PWM4 = 19300,        PWM5 = 19300;
  434.         else
  435.         {
  436.                 PWM2 = 19300 - (int)YM*4 - speed2;
  437.                          if(PWM2 > 19300)        PWM2 = 19300;        //速度参数控制，防止超过PWM参数范围0-1000
  438.                 else if(PWM2 < 0)                PWM2 = 0;

  440.                 PWM3 = 19300 - (int)YM*4 - speed3;
  441.                          if(PWM3 > 19300)        PWM3 = 19300;
  442.                 else if(PWM3 < 0)                PWM3 = 0;

  444.                 PWM4 = 19300 - (int)YM*4 - speed4;
  445.                          if(PWM4 > 19300)        PWM4 = 19300;
  446.                 else if(PWM4 < 0)                PWM4 = 0;

  448.                 PWM5 = 19300 - (int)YM*4 - speed5;
  449.                          if(PWM5 > 19300)        PWM5 = 19300;
  450.                 else if(PWM5 < 0)                PWM5 = 0;
  451.         }
  452.         epwmSetValue(PWM_L,PWM5,20000);//设置翻转值，第1次翻转为PWM5，第2次翻转为1001
  453.         epwmSetValue(PWM_R,PWM2,20000);//设置翻转值，第1次翻转为PWM2，第2次翻转为1001
  454.         epwmSetValue(PWM_F,PWM4,20000);//设置翻转值，第1次翻转为PWM4，第2次翻转为1001
  455.         epwmSetValue(PWM_B,PWM3,20000);//设置翻转值，第1次翻转为PWM3，第2次翻转为1001
  456.         P52=0;
  457.         }
  458. void timer3_int (void) interrupt 19
  459. {
  460.                  count++;
  461.                         if(count==1)
  462.                         {
  463.                                 nRF24L01_TxPacket(TxBuf);//发射数据
  464.                         }
  465.                         else if(count==3)
  466.                         {
  467.                                 SetRX_Mode();
  468.                         }
  469.                         else if(count==4)
  470.                         {                               
  471.                                 nRF24L01_RxPacket(RxBuf_temp);//接收数据
  472.                                 if((RxBuf_temp[9]-RxBuf_temp[8]-RxBuf_temp[7]-RxBuf_temp[6]-RxBuf_temp[5]-RxBuf_temp[4]-RxBuf_temp[3]-RxBuf_temp[2]-RxBuf_temp[1]-RxBuf_temp[0])==0)
  473.                                 {                                       
  474.                                         RxBuf[8]=RxBuf_temp[8];//左右微调
  475.                                         RxBuf[7]=RxBuf_temp[7];//前后微调
  476.                                         RxBuf[6]=RxBuf_temp[6];
  477.                                         RxBuf[5]=RxBuf_temp[5];
  478.                                         RxBuf[4]=RxBuf_temp[4];
  479.                                         RxBuf[3]=RxBuf_temp[3];
  480.                                         RxBuf[2]=RxBuf_temp[2];
  481.                                         RxBuf[1]=RxBuf_temp[1];
  482.                                         RxBuf[0]=RxBuf_temp[0];
  483.                                 }
  484.                                 count=0;
  485.                         }
  486. }
  487. int KalmanFilter_ax( int ResrcData,int ProcessNiose_Q,int MeasureNoise_R)
  488. {
  489.    int R = MeasureNoise_R;
  490.    int Q = ProcessNiose_Q;
  491.    static int ax_last;
  492.    int ax_mid = ax_last;
  493.    long ax_now;
  494.    static int ax_p_last;
  495.    long p_mid ;
  496.    long p_now;
  497.    int kg;        
  498.          long temp;
  499.    ax_mid=ax_last;
  500.    p_mid=ax_p_last+Q;
  501.          temp=p_mid<<15;
  502.          kg=(temp/((long)p_mid+R));
  503.    ax_now= ax_mid+(((long)kg*(ResrcData-ax_mid))>>15);
  504.    p_now=((long)p_mid*(32768-kg))>>15;      
  505.    ax_p_last = p_now;
  506.    ax_last = ax_now;
  507.    return ax_now;               
  508. }
  509. int KalmanFilter_ay( int ResrcData,int ProcessNiose_Q,int MeasureNoise_R)
  510. {
  511.    int R = MeasureNoise_R;
  512.    int Q = ProcessNiose_Q;
  513.    static int ay_last;
  514.    int ay_mid = ay_last;
  515.    long ay_now;
  516.    static int ay_p_last;
  517.    long p_mid ;
  518.    long p_now;
  519.    int kg;        
  520.          long temp;
  521.    ay_mid=ay_last;
  522.    p_mid=ay_p_last+Q;
  523.    temp=p_mid<<15;
  524.         kg=(temp/((long)p_mid+R));

  526.    ay_now= ay_mid+(((long)kg*(ResrcData-ay_mid))>>15);
  527.    p_now=((long)p_mid*(32768-kg))>>15;      
  528.    ay_p_last = p_now;
  529.    ay_last = ay_now;
  530.    return ay_now;               
  531. }
  532. int KalmanFilter_az( int ResrcData,int ProcessNiose_Q,int MeasureNoise_R)
  533. {
  534.    int R = MeasureNoise_R;
  535.    int Q = ProcessNiose_Q;
  536.    static int az_last;
  537.    int az_mid = az_last;
  538.    long az_now;
  539.    static int az_p_last;
  540.    long p_mid ;
  541.    long p_now;
  542.    int kg;        
  543.         long temp;
  544.    az_mid=az_last;
  545.    p_mid=az_p_last+Q;
  546.         temp=p_mid<<15;
  547.         kg=(temp/((long)p_mid+R));

  549.    az_now= az_mid+(((long)kg*(ResrcData-az_mid))>>15);
  550.    p_now=((long)p_mid*(32768-kg))>>15;      
  551.    az_p_last = p_now;
  552.    az_last = az_now;
  553.    return az_now;               
  554. }
  555. int KalmanFilter_gyrox( int ResrcData,int ProcessNiose_Q,int MeasureNoise_R)
  556. {
  557.    int R = MeasureNoise_R;
  558. ……………………

  560. …………限于本文篇幅 余下代码请从51黑下载附件…………
复制代码

所有资料51hei提供下载:
黑色板子四轴程序.zip (555.97 KB, 下载次数: 76)



分享到:  QQ好友和群QQ好友和群 QQ空间QQ空间 腾讯微博腾讯微博 腾讯朋友腾讯朋友
收藏收藏3 分享淘帖 顶顶 踩踩
回复

使用道具 举报

沙发
ID:233308 发表于 2017-9-15 15:30 来自手机 | 只看该作者
谢谢谢谢楼主
回复

使用道具 举报

板凳
ID:233308 发表于 2017-9-15 15:48 | 只看该作者
黑币不够下载了
回复

使用道具 举报

地板
ID:233308 发表于 2017-9-15 15:48 | 只看该作者
黑币不够下载了
回复

使用道具 举报

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

本版积分规则

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

Powered by 单片机教程网

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