標(biāo)題: 四旋翼源碼第7版軟啟動(dòng)+藍(lán)牙源碼（串級(jí)PID控制） [打印本頁(yè)]
作者: hou123123    時(shí)間: 2018-5-12 11:21
標(biāo)題: 四旋翼源碼第7版軟啟動(dòng)+藍(lán)牙源碼（串級(jí)PID控制）
這是我們調(diào)小四軸的源碼。主要用的串級(jí)PID控制。


STM32-DMP移植單片機(jī)源程序如下:
  1. #include "delay.h"
  2. #include "sys.h"
  3. #include "usart.h"
  4. #include "anbt_dmp_fun.h"
  5. #include "anbt_i2c.h"
  6. #include "anbt_dmp_mpu6050.h"
  7. #include "anbt_dmp_driver.h"
  8. #include "debug.h"
  9. #include "PWM.h"
  10. #define BYTE0(dwTemp)       (*(char *)(&dwTemp))
  11. #define BYTE1(dwTemp)       (*((char *)(&dwTemp) + 1))
  12. #define BYTE2(dwTemp)       (*((char *)(&dwTemp) + 2))
  13. #define BYTE3(dwTemp)       (*((char *)(&dwTemp) + 3))

  15. #define q30  1073741824.0f

  17. void Data_Send_Status(float Pitch,float Roll,float Yaw,int16_t *gyro,int16_t *accel);
  18. void Send_Data(int16_t *Gyro,int16_t *Accel);


  21. static void DMP_Delay ( __IO uint32_t nCount )
  22. {
  23.   for ( ; nCount != 0; nCount -- );
  24.        
  25. }


  28. int main(void)
  29. {               


  32. int a = 0;
  33.   float error_pitch=0.0;
  34.         static float Pitch_I_out=0.0;
  35.         float  error_pitch_old=0.0;
  36.         static float Pitch_I_shell_out=0.0;
  37.   float error_pitch_shell=0.0;
  38.         float pitch_shell_out=0.0;
  39.         float pitch_out=0.0;
  40.        
  41.   float error_roll=0.0;
  42.         static float roll_I_out=0.0;
  43.         float  error_roll_old=0.0;
  44.         static float roll_I_shell_out=0.0;
  45.   float error_roll_shell=0.0;
  46.         float roll_shell_out=0.0;
  47.         float roll_out=0.0;
  48.        
  49.         static float yaw_I_out=0.0;
  50.         float error_yaw=0.0;
  51.         float error_yaw_old=0.0;
  52.         float yaw_out=0.0;
  53.         int i=1000000;
  54.         float  m1=0,m2=0,m3=0,m4=0,m5=0.0;
  55.         unsigned long sensor_timestamp;
  56.         short gyro[3], accel[3], sensors;//陀螺儀存放數(shù)組，加速度存放數(shù)組，返回狀態(tài)量
  57.         unsigned char more;
  58.         long quat[4];//四元數(shù)存放數(shù)組
  59.         float Yaw=0.00,Roll,Pitch;//歐拉角
  60.         float q0=1.0f,q1=0.0f,q2=0.0f,q3=0.0f;//計(jì)算姿態(tài)過(guò)程用到的變量

  62.    
  63. ////       
  64.     pitch_SET = 7;
  65.           P_pitch_shell =-25;//-55
  66.           P_pitch = -0.72;        //-1.2
  67.     I_pitch= -0.015;//        -0.032
  68.                 D_pitch=-0.85;//-1.2       
  69.           I_pitch_shell =-0.00012;//-0.0002
  70.                
  71.                
  72.                 roll_SET = 9;
  73.                 P_roll_shell= -25;//-55
  74.                 P_roll = 0.72;//1.2
  75.                 I_roll= 0.015;//0.032
  76.                 D_roll= 0.85;//1.2
  77.                 I_roll_shell = 0.00012;//0.0002
  78.        
  79.         NVIC_Configuration();//設(shè)置NVIC中斷分組2:2位搶占優(yōu)先級(jí)，2位響應(yīng)優(yōu)先級(jí)
  80.         uart_init(115200);         //串口初始化為115200
  81.         //引用圓點(diǎn)博士的I2C程序，這里跟我們平常沒(méi)有什么區(qū)別
  82.   PWM_Init();

  84.        
  85.         ANBT_I2C_Configuration();                //IIC初始化
  86. //        delay_ms(30);
  87.         DMP_Delay (300);//
  88.         AnBT_DMP_MPU6050_Init();                        //6050DMP初始化
  89.        
  90. //TIM2_Int_Init(4999,7199);//10Khz的計(jì)數(shù)頻率，計(jì)數(shù)到5000為500ms
  91. DMP_Delay (300);

  93. while(i--)
  94. {
  95.                 TIM_SetCompare3(TIM3,200);
  96.                 TIM_SetCompare4(TIM3,200);
  97.                 TIM_SetCompare3(TIM4,200);
  98.                 TIM_SetCompare4(TIM4,200);       
  99. }
  100. //a = ReceiveOneByte[0];

  102.         while(1)
  103.         {

  105.                 error_pitch_old = error_pitch;
  106.                 error_roll_old = error_roll;
  107.                 error_yaw_old = error_yaw;
  108.                  dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors,&more);       

  110.                  if ( sensors & INV_WXYZ_QUAT )
  111.                  {
  112.                          q0=quat[0] / q30;
  113.                          q1=quat[1] / q30;
  114.                          q2=quat[2] / q30;
  115.                          q3=quat[3] / q30;
  116.                          Pitch = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3; // pitch
  117.                          Roll = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // roll
  118.                          Yaw =         atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 57.3;                //感覺(jué)沒(méi)有價(jià)值，注掉
  119.                          
  120.                          

  122. //                         if(Pitch>15||Pitch<-15||Roll>15||Roll<-15)//stop
  123. //                         {
  124. //                                 TIM_SetCompare3(TIM3,0);
  125. //               TIM_SetCompare4(TIM3,0);
  126. //                TIM_SetCompare3(TIM4,0);
  127. //                TIM_SetCompare4(TIM4,0);
  128. //                                 while(1);
  129. //                         }
  130.                          //////////////////Pitch外環(huán)(PI)/////////////       
  131.                         error_pitch_shell = Pitch-pitch_SET;
  132.                         Pitch_I_shell_out+=error_pitch_shell;
  133.                          if(Pitch_I_shell_out>500)
  134.                          {
  135.                                  Pitch_I_shell_out = 500;
  136.                          }
  137.                          if(Pitch_I_shell_out<-500)
  138.                          {
  139.                                  Pitch_I_shell_out = -500;
  140.                          }
  141.                          pitch_shell_out = P_pitch_shell*error_pitch_shell+I_pitch_shell*Pitch_I_shell_out;
  142.                          //------------Pitch內(nèi)環(huán)PID gyro[1]---------------//
  143.                          error_pitch = gyro[1]-pitch_shell_out;
  144.                          Pitch_I_out+=error_pitch;
  145.                          if(Pitch_I_out>500)
  146.                          {
  147.                                  Pitch_I_out = 500;
  148.                          }
  149.                           if(Pitch_I_out<-500)
  150.                          {
  151.                                  Pitch_I_out = -500;
  152.                          }
  153.              pitch_out = P_pitch*error_pitch+I_pitch*Pitch_I_out+D_pitch*(error_pitch-error_pitch_old);
  154.                          m1=pitch_out;
  155.                        
  156.                          
  157.                          //////////////////roll外環(huán)(PI)gyro[0]/////////////       
  158.                    error_roll_shell = Roll - roll_SET;
  159.                          roll_I_shell_out+=error_roll_shell;
  160.                          if(roll_I_shell_out>1000)
  161.                          {
  162.                                  roll_I_shell_out = 1000;
  163.                          }
  164.               if(roll_I_shell_out<-1000)
  165.                          {
  166.                                  roll_I_shell_out = -1000;
  167.                          }
  168.                    roll_shell_out = P_roll_shell*error_roll_shell+I_roll_shell*roll_I_shell_out;
  169.                           //////////////////roll內(nèi)環(huán)PID/////////
  170.                          error_roll = gyro[0]-roll_shell_out;
  171.                          roll_I_out+=error_roll;
  172.                          if(roll_I_out>1000)
  173.                          {
  174.                                  roll_I_out = 1000;
  175.                          }
  176.                          if(roll_I_out<-1000)
  177.                          {
  178.                                  roll_I_out = -1000;
  179.                          }
  180.                          roll_out = P_roll*error_roll+I_roll*roll_I_out+D_roll*(error_roll-error_roll_old);
  181.                          m2 = roll_out;
  182.                          ////////////////gyro[2]  PD//////////
  183.                          error_yaw = gyro[2];
  184.                          yaw_out = 1.8*error_yaw+2.5*(error_yaw_old-error_yaw);
  185.                          
  186.                    m3 = yaw_out;
  187.                          /////////////////////yaw///////////
  188. //                                  error_yaw = Yaw;
  189. //                         yaw_I_out+=error_yaw;
  190. //                         if(yaw_I_out>200)
  191. //                         {
  192. //                                 yaw_I_out = 200;
  193. //                         }
  194. //                         if(yaw_I_out<-200)
  195. //                         {
  196. //                                 yaw_I_out = -200;
  197. //                         }
  198. //                        m4 = 0.0000025*error_yaw+0.000000025*yaw_I_out;
  199.                
  200.                
  201.                                     TIM_SetCompare3(TIM3,2500+m1+m2+m3+m4+m5);
  202.                TIM_SetCompare4(TIM3,2500+m1-m2-m3-m4+m5);
  203.                                  
  204.                TIM_SetCompare3(TIM4,2500-m1-m2+m3+m4+m5);
  205.                TIM_SetCompare4(TIM4,2500-m1+m2-m3-m4+m5);                  
  206.                        
  207.       Push(1,(int)pitch_out);
  208.                         Push(2,(int)Pitch);
  209.                          
  210.                         Push(3,(int)gyro[0]);
  211.                   Push(4,(int)gyro[1]);
  212.                   Push(5,(int)gyro[2]);
  213.                                 Push(6,(int)Yaw);
  214.        
  215.                        
  216.                        
  217.                        
  218.        SendDataToScope();       
  219.                        
  220.                  }          
  221.                
  222.         }
  223. }


  226. /*函數(shù)功能：根據(jù)匿名最新上位機(jī)協(xié)議寫(xiě)的顯示姿態(tài)的程序
  227. *具體原理看匿名的講解視頻


  230. 發(fā)送基本信息（姿態(tài)、鎖定狀態(tài)）


  233. */

  235. void Data_Send_Status(float Pitch,float Roll,float Yaw,int16_t *gyro,int16_t *accel)
  236. {
  237.         unsigned char i=0;
  238.         unsigned char _cnt=0,sum = 0;
  239.         unsigned int _temp;
  240.         u8 data_to_send[50];

  242.         data_to_send[_cnt++]=0xAA;
  243.         data_to_send[_cnt++]=0xAA;
  244.         data_to_send[_cnt++]=0x01;
  245.         data_to_send[_cnt++]=0;
  246.        
  247.         _temp = (int)(Roll*100);
  248.         data_to_send[_cnt++]=BYTE1(_temp);
  249.         data_to_send[_cnt++]=BYTE0(_temp);
  250.         _temp = 0-(int)(Pitch*100);
  251.         data_to_send[_cnt++]=BYTE1(_temp);
  252.         data_to_send[_cnt++]=BYTE0(_temp);
  253.         _temp = (int)(Yaw*100);
  254.         data_to_send[_cnt++]=BYTE1(_temp);
  255.         data_to_send[_cnt++]=BYTE0(_temp);
  256.        
  257.         data_to_send[3] = _cnt-4;
  258.         //和校驗(yàn)
  259.         for(i=0;i<_cnt;i++)
  260.                 sum+= data_to_send[i];
  261.         data_to_send[_cnt++]=sum;
  262.        
  263.         //串口發(fā)送數(shù)據(jù)
  264.         for(i=0;i<_cnt;i++)
  265.                 AnBT_Uart1_Send_Char(data_to_send[i]);
  266. }




  271. //發(fā)送傳感器數(shù)據(jù)
  272. void Send_Data(int16_t *Gyro,int16_t *Accel)
  273. {
  274.         unsigned char i=0;
  275.         unsigned char _cnt=0,sum = 0;
  276. //        unsigned int _temp;
  277.         u8 data_to_send[50];

  279. ……………………

  281. …………限于本文篇幅 余下代碼請(qǐng)從51黑下載附件…………
復(fù)制代碼


全部資料51hei下載地址：
第7版軟啟動(dòng)+藍(lán)牙.rar (396.1 KB, 下載次數(shù): 58)


作者: JIANGJING520    時(shí)間: 2018-11-30 17:22
下載試試看，最近在學(xué)
作者: liht1634    時(shí)間: 2019-8-29 12:57
最近在玩軟啟動(dòng)，了解一下。



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