標題: stm32F1+mpu6050平衡車源代碼 [打印本頁]
作者: 韓高飛    時間: 2019-1-25 14:31
標題: stm32F1+mpu6050平衡車源代碼
stm32F1 平衡車源代碼，采用亞博智能小車拓展板，采用MPU6050  INT腳中斷，本代碼只有直立環(huán)，后續(xù)代碼稍后上傳

單片機源程序如下:
  1. #include "mpu6050.h"
  2. #include "sys.h"
  3. #include "delay.h"
  4. #include "usart.h"   

  6. //初始化MPU6050
  7. //返回值:0,成功
  8. //    其他,錯誤代碼
  9. u8 MPU_Init(void)
  10. {
  11.         u8 res;
  12.   GPIO_InitTypeDef  GPIO_InitStructure;
  13.         
  14.         RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO,ENABLE);//使能AFIO時鐘
  15.         RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA,ENABLE);//先使能外設(shè)IO PORTA時鐘
  16.         
  17.   GPIO_InitStructure.GPIO_Pin = GPIO_Pin_15;         // 端口配置
  18.   GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;        //推挽輸出
  19.   GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;                 //IO口速度為50MHz
  20.   GPIO_Init(GPIOA, &GPIO_InitStructure);                                         //根據(jù)設(shè)定參數(shù)初始化GPIOA

  22.         GPIO_PinRemapConfig(GPIO_Remap_SWJ_JTAGDisable,ENABLE);//禁止JTAG,從而PA15可以做普通IO使用,否則PA15不能做普通IO!!!
  23.         
  24.         MPU_AD0_CTRL=0;                        //控制MPU6050的AD0腳為低電平,從機地址為:0X68
  25.         
  26.         MPU_IIC_Init(); //初始化IIC總線
  27.         MPU_Write_Byte(MPU_PWR_MGMT1_REG,0X80);        //復(fù)位MPU6050
  28.   delay_ms(100);
  29.         MPU_Write_Byte(MPU_PWR_MGMT1_REG,0X00);        //喚醒MPU6050
  30.         MPU_Set_Gyro_Fsr(3);                                        //陀螺儀傳感器,±2000dps
  31.         MPU_Set_Accel_Fsr(0);                                        //加速度傳感器,±2g
  32.         MPU_Set_Rate(200);                                                //設(shè)置采樣率50Hz
  33.         MPU_Write_Byte(MPU_INT_EN_REG,0X00);        //關(guān)閉所有中斷
  34.         MPU_Write_Byte(MPU_USER_CTRL_REG,0X00);        //I2C主模式關(guān)閉
  35.         MPU_Write_Byte(MPU_FIFO_EN_REG,0X00);        //關(guān)閉FIFO
  36.         MPU_Write_Byte(MPU_INTBP_CFG_REG,0X80);//INT引腳低電平有效
  37.         res=MPU_Read_Byte(MPU_DEVICE_ID_REG);
  38.         if(res==MPU_ADDR)//器件ID正確
  39.         {
  40.                 MPU_Write_Byte(MPU_PWR_MGMT1_REG,0X01);        //設(shè)置CLKSEL,PLL X軸為參考
  41.                 MPU_Write_Byte(MPU_PWR_MGMT2_REG,0X00);        //加速度與陀螺儀都工作
  42.                 MPU_Set_Rate(200);                                                //設(shè)置采樣率為50Hz
  43.          }else return 1;
  44.         return 0;
  45. }
  46. //設(shè)置MPU6050陀螺儀傳感器滿量程范圍
  47. //fsr:0,±250dps;1,±500dps;2,±1000dps;3,±2000dps
  48. //返回值:0,設(shè)置成功
  49. //    其他,設(shè)置失敗
  50. u8 MPU_Set_Gyro_Fsr(u8 fsr)
  51. {
  52.         return MPU_Write_Byte(MPU_GYRO_CFG_REG,fsr<<3);//設(shè)置陀螺儀滿量程范圍  
  53. }
  54. //設(shè)置MPU6050加速度傳感器滿量程范圍
  55. //fsr:0,±2g;1,±4g;2,±8g;3,±16g
  56. //返回值:0,設(shè)置成功
  57. //    其他,設(shè)置失敗
  58. u8 MPU_Set_Accel_Fsr(u8 fsr)
  59. {
  60.         return MPU_Write_Byte(MPU_ACCEL_CFG_REG,fsr<<3);//設(shè)置加速度傳感器滿量程范圍  
  61. }
  62. //設(shè)置MPU6050的數(shù)字低通濾波器
  63. //lpf:數(shù)字低通濾波頻率(Hz)
  64. //返回值:0,設(shè)置成功
  65. //    其他,設(shè)置失敗
  66. u8 MPU_Set_LPF(u16 lpf)
  67. {
  68.         u8 data=0;
  69.         if(lpf>=188)data=1;
  70.         else if(lpf>=98)data=2;
  71.         else if(lpf>=42)data=3;
  72.         else if(lpf>=20)data=4;
  73.         else if(lpf>=10)data=5;
  74.         else data=6;
  75.         return MPU_Write_Byte(MPU_CFG_REG,data);//設(shè)置數(shù)字低通濾波器  
  76. }
  77. //設(shè)置MPU6050的采樣率(假定Fs=1KHz)
  78. //rate:4~1000(Hz)
  79. //返回值:0,設(shè)置成功
  80. //    其他,設(shè)置失敗
  81. u8 MPU_Set_Rate(u16 rate)
  82. {
  83.         u8 data;
  84.         if(rate>1000)rate=1000;
  85.         if(rate<4)rate=4;
  86.         data=1000/rate-1;
  87.         data=MPU_Write_Byte(MPU_SAMPLE_RATE_REG,data);        //設(shè)置數(shù)字低通濾波器
  88.          return MPU_Set_LPF(rate/2);        //自動設(shè)置LPF為采樣率的一半
  89. }

  91. //得到溫度值
  92. //返回值:溫度值(擴大了100倍)
  93. short MPU_Get_Temperature(void)
  94. {
  95.     u8 buf[2];
  96.     short raw;
  97.         float temp;
  98.         MPU_Read_Len(MPU_ADDR,MPU_TEMP_OUTH_REG,2,buf);
  99.     raw=((u16)buf[0]<<8)|buf[1];  
  100.     temp=36.53+((double)raw)/340;  
  101.     return temp*100;;
  102. }
  103. //得到陀螺儀值(原始值)
  104. //gx,gy,gz:陀螺儀x,y,z軸的原始讀數(shù)(帶符號)
  105. //返回值:0,成功
  106. //    其他,錯誤代碼
  107. u8 MPU_Get_Gyroscope(short *gx,short *gy,short *gz)
  108. {
  109.     u8 buf[6],res;  
  110.         res=MPU_Read_Len(MPU_ADDR,MPU_GYRO_XOUTH_REG,6,buf);
  111.         if(res==0)
  112.         {
  113.                 *gx=((u16)buf[0]<<8)|buf[1];  
  114.                 *gy=((u16)buf[2]<<8)|buf[3];  
  115.                 *gz=((u16)buf[4]<<8)|buf[5];
  116.         }         
  117.     return res;;
  118. }
  119. //得到加速度值(原始值)
  120. //gx,gy,gz:陀螺儀x,y,z軸的原始讀數(shù)(帶符號)
  121. //返回值:0,成功
  122. //    其他,錯誤代碼
  123. u8 MPU_Get_Accelerometer(short *ax,short *ay,short *az)
  124. {
  125.     u8 buf[6],res;  
  126.         res=MPU_Read_Len(MPU_ADDR,MPU_ACCEL_XOUTH_REG,6,buf);
  127.         if(res==0)
  128.         {
  129.                 *ax=((u16)buf[0]<<8)|buf[1];  
  130.                 *ay=((u16)buf[2]<<8)|buf[3];  
  131.                 *az=((u16)buf[4]<<8)|buf[5];
  132.         }         
  133.     return res;;
  134. }
  135. //IIC連續(xù)寫
  136. //addr:器件地址
  137. //reg:寄存器地址
  138. //len:寫入長度
  139. //buf:數(shù)據(jù)區(qū)
  140. //返回值:0,正常
  141. //    其他,錯誤代碼
  142. u8 MPU_Write_Len(u8 addr,u8 reg,u8 len,u8 *buf)
  143. {
  144.         u8 i;
  145.     MPU_IIC_Start();
  146.         MPU_IIC_Send_Byte((addr<<1)|0);//發(fā)送器件地址+寫命令        
  147.         if(MPU_IIC_Wait_Ack())        //等待應(yīng)答
  148.         {
  149.                 MPU_IIC_Stop();                 
  150.                 return 1;               
  151.         }
  152.     MPU_IIC_Send_Byte(reg);        //寫寄存器地址
  153.     MPU_IIC_Wait_Ack();                //等待應(yīng)答
  154.         for(i=0;i<len;i++)
  155.         {
  156.                 MPU_IIC_Send_Byte(buf[i]);        //發(fā)送數(shù)據(jù)
  157.                 if(MPU_IIC_Wait_Ack())                //等待ACK
  158.                 {
  159.                         MPU_IIC_Stop();         
  160.                         return 1;                 
  161.                 }               
  162.         }   
  163.     MPU_IIC_Stop();         
  164.         return 0;        
  165. }
  166. //IIC連續(xù)讀
  167. //addr:器件地址
  168. //reg:要讀取的寄存器地址
  169. //len:要讀取的長度
  170. //buf:讀取到的數(shù)據(jù)存儲區(qū)
  171. //返回值:0,正常
  172. //    其他,錯誤代碼
  173. u8 MPU_Read_Len(u8 addr,u8 reg,u8 len,u8 *buf)
  174. {
  175.          MPU_IIC_Start();
  176.         MPU_IIC_Send_Byte((addr<<1)|0);//發(fā)送器件地址+寫命令        
  177.         if(MPU_IIC_Wait_Ack())        //等待應(yīng)答
  178.         {
  179.                 MPU_IIC_Stop();                 
  180.                 return 1;               
  181.         }
  182.     MPU_IIC_Send_Byte(reg);        //寫寄存器地址
  183.     MPU_IIC_Wait_Ack();                //等待應(yīng)答
  184.     MPU_IIC_Start();
  185.         MPU_IIC_Send_Byte((addr<<1)|1);//發(fā)送器件地址+讀命令        
  186.     MPU_IIC_Wait_Ack();                //等待應(yīng)答
  187.         while(len)
  188.         {
  189.                 if(len==1)*buf=MPU_IIC_Read_Byte(0);//讀數(shù)據(jù),發(fā)送nACK
  190.                 else *buf=MPU_IIC_Read_Byte(1);                //讀數(shù)據(jù),發(fā)送ACK  
  191.                 len--;
  192.                 buf++;
  193.         }   
  194.     MPU_IIC_Stop();        //產(chǎn)生一個停止條件
  195.         return 0;        
  196. }
  197. //IIC寫一個字節(jié)
  198. //reg:寄存器地址
  199. //data:數(shù)據(jù)
  200. //返回值:0,正常
  201. //    其他,錯誤代碼
  202. u8 MPU_Write_Byte(u8 reg,u8 data)                                 
  203. {
  204.     MPU_IIC_Start();
  205.         MPU_IIC_Send_Byte((MPU_ADDR<<1)|0);//發(fā)送器件地址+寫命令        
  206.         if(MPU_IIC_Wait_Ack())        //等待應(yīng)答
  207.         {
  208.                 MPU_IIC_Stop();                 
  209.                 return 1;               
  210.         }
  211.     MPU_IIC_Send_Byte(reg);        //寫寄存器地址
  212.     MPU_IIC_Wait_Ack();                //等待應(yīng)答
  213.         MPU_IIC_Send_Byte(data);//發(fā)送數(shù)據(jù)
  214.         if(MPU_IIC_Wait_Ack())        //等待ACK
  215.         {
  216.                 MPU_IIC_Stop();         
  217.                 return 1;                 
  218.         }                 
  219.     MPU_IIC_Stop();         
  220.         return 0;
  221. }
  222. //IIC讀一個字節(jié)
  223. //reg:寄存器地址
  224. //返回值:讀到的數(shù)據(jù)
  225. u8 MPU_Read_Byte(u8 reg)
  226. {
  227.         u8 res;
  228.     MPU_IIC_Start();
  229.         MPU_IIC_Send_Byte((MPU_ADDR<<1)|0);//發(fā)送器件地址+寫命令        
  230.         MPU_IIC_Wait_Ack();                //等待應(yīng)答
  231.     MPU_IIC_Send_Byte(reg);        //寫寄存器地址
  232.     MPU_IIC_Wait_Ack();                //等待應(yīng)答
  233.     MPU_IIC_Start();
  234.         MPU_IIC_Send_Byte((MPU_ADDR<<1)|1);//發(fā)送器件地址+讀命令        
  235.     MPU_IIC_Wait_Ack();                //等待應(yīng)答
  236.         res=MPU_IIC_Read_Byte(0);//讀取數(shù)據(jù),發(fā)送nACK
  237.     MPU_IIC_Stop();                        //產(chǎn)生一個停止條件
  238.         return res;               
  239. }
復(fù)制代碼
  1. #include "sys.h"        
  2. #include "delay.h"        
  3. #include "led.h"
  4. #include "Motor.h"
  5. #include "usart.h"
  6. #include "timer.h"
  7. #include "pid.h"
  8. #include "oled.h"
  9. #include "mpu6050.h"
  10. #include "usmart.h"   
  11. #include "inv_mpu.h"
  12. #include "inv_mpu_dmp_motion_driver.h"

  14. //串口1發(fā)送1個字符
  15. //c:要發(fā)送的字符
  16. u8 a = 0;
  17. u16 Kms;

  19. float pitch;
  20. float roll,yaw;                 //歐拉角         
  21. short gyrox,gyroy,gyroz;        //陀螺儀原始數(shù)據(jù)
  22. short aacx,aacy,aacz;                //加速度傳感器原始數(shù)據(jù)

  24. void usart1_send_char(u8 c)
  25. {           
  26.         while(USART_GetFlagStatus(USART1,USART_FLAG_TC)==RESET); //循環(huán)發(fā)送,直到發(fā)送完畢   
  27.         USART_SendData(USART1,c);  
  28. }
  29. //傳送數(shù)據(jù)給匿名四軸上位機軟件(V2.6版本)
  30. //fun:功能字. 0XA0~0XAF
  31. //data:數(shù)據(jù)緩存區(qū),最多28字節(jié)!!
  32. //len:data區(qū)有效數(shù)據(jù)個數(shù)
  33. void usart1_niming_report(u8 fun,u8*data,u8 len)
  34. {
  35.         u8 send_buf[32];
  36.         u8 i;
  37.         if(len>28)return;        //最多28字節(jié)數(shù)據(jù)
  38.         send_buf[len+3]=0;        //校驗數(shù)置零
  39.         send_buf[0]=0X88;        //幀頭
  40.         send_buf[1]=fun;        //功能字
  41.         send_buf[2]=len;        //數(shù)據(jù)長度
  42.         for(i=0;i<len;i++)send_buf[3+i]=data[i];                        //復(fù)制數(shù)據(jù)
  43.         for(i=0;i<len+3;i++)send_buf[len+3]+=send_buf[i];        //計算校驗和        
  44.         for(i=0;i<len+4;i++)usart1_send_char(send_buf[i]);        //發(fā)送數(shù)據(jù)到串口1
  45. }
  46. //發(fā)送加速度傳感器數(shù)據(jù)和陀螺儀數(shù)據(jù)
  47. //aacx,aacy,aacz:x,y,z三個方向上面的加速度值
  48. //gyrox,gyroy,gyroz:x,y,z三個方向上面的陀螺儀值
  49. void mpu6050_send_data(short aacx,short aacy,short aacz,short gyrox,short gyroy,short gyroz)
  50. {
  51.         u8 tbuf[12];
  52.         tbuf[0]=(aacx>>8)&0XFF;
  53.         tbuf[1]=aacx&0XFF;
  54.         tbuf[2]=(aacy>>8)&0XFF;
  55.         tbuf[3]=aacy&0XFF;
  56.         tbuf[4]=(aacz>>8)&0XFF;
  57.         tbuf[5]=aacz&0XFF;
  58.         tbuf[6]=(gyrox>>8)&0XFF;
  59.         tbuf[7]=gyrox&0XFF;
  60.         tbuf[8]=(gyroy>>8)&0XFF;
  61.         tbuf[9]=gyroy&0XFF;
  62.         tbuf[10]=(gyroz>>8)&0XFF;
  63.         tbuf[11]=gyroz&0XFF;
  64.         usart1_niming_report(0XA1,tbuf,12);//自定義幀,0XA1
  65. }        
  66. //通過串口1上報結(jié)算后的姿態(tài)數(shù)據(jù)給電腦
  67. //aacx,aacy,aacz:x,y,z三個方向上面的加速度值
  68. //gyrox,gyroy,gyroz:x,y,z三個方向上面的陀螺儀值
  69. //roll:橫滾角.單位0.01度。 -18000 -> 18000 對應(yīng) -180.00  ->  180.00度
  70. //pitch:俯仰角.單位 0.01度。-9000 - 9000 對應(yīng) -90.00 -> 90.00 度
  71. //yaw:航向角.單位為0.1度 0 -> 3600  對應(yīng) 0 -> 360.0度
  72. void usart1_report_imu(short aacx,short aacy,short aacz,short gyrox,short gyroy,short gyroz,short roll,short pitch,short yaw)
  73. {
  74.         u8 tbuf[28];
  75.         u8 i;
  76.         for(i=0;i<28;i++)tbuf[i]=0;//清0
  77.         tbuf[0]=(aacx>>8)&0XFF;
  78.         tbuf[1]=aacx&0XFF;
  79.         tbuf[2]=(aacy>>8)&0XFF;
  80.         tbuf[3]=aacy&0XFF;
  81.         tbuf[4]=(aacz>>8)&0XFF;
  82.         tbuf[5]=aacz&0XFF;
  83.         tbuf[6]=(gyrox>>8)&0XFF;
  84.         tbuf[7]=gyrox&0XFF;
  85.         tbuf[8]=(gyroy>>8)&0XFF;
  86.         tbuf[9]=gyroy&0XFF;
  87.         tbuf[10]=(gyroz>>8)&0XFF;
  88.         tbuf[11]=gyroz&0XFF;        
  89.         tbuf[18]=(roll>>8)&0XFF;
  90.         tbuf[19]=roll&0XFF;
  91.         tbuf[20]=(pitch>>8)&0XFF;
  92.         tbuf[21]=pitch&0XFF;
  93.         tbuf[22]=(yaw>>8)&0XFF;
  94.         tbuf[23]=yaw&0XFF;
  95.         usart1_niming_report(0XAF,tbuf,28);//飛控顯示幀,0XAF
  96. }  


  99. int main(void)
  100. {        
  101.          
  102.         u8 i,report=1;
  103.   
  104.         
  105.         delay_init();                     //延時函數(shù)初始化        
  106.    NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2); //設(shè)置NVIC中斷分組2:2位搶占優(yōu)先級，2位響應(yīng)優(yōu)先級                 
  107.         uart_init(115200);         //串口初始化為115200
  108.      delay_ms(100);
  109.           MPU_Init();                                        //初始化MPU6050
  110.         
  111.         
  112.             Motor_Init();        
  113.    
  114.            TIM2_PWM_Init(7199,0);         //不分頻。PWM頻率=72000000/900=80Khz
  115.       PID_Init();
  116.           LED_Init();//LED端口初始化
  117.          

  119.      mpu_dmp_init();
  120.                
  121.         MPU6050_EXTI_Init();//外部中斷初始化
  122.          
  123.         
  124.          
  125. while(1)
  126.         {
  127.                         
  128.                
  129.         }         
  130. }

復(fù)制代碼

所有資料51hei提供下載:
平衡版.7z (277.58 KB, 下載次數(shù): 89)
BST STM32智能平衡小車.pdf (40 KB, 下載次數(shù): 63)

作者: admin    時間: 2019-1-25 15:23
本帖需要重新編輯補全電路原理圖,源碼,詳細說明與圖片即可獲得100+黑幣(帖子下方有編輯按鈕)
作者: 小分子    時間: 2020-2-19 19:59
黑51這個網(wǎng)站是比較厲害了。這個資料都出來了。
作者: littlequan    時間: 2020-4-16 09:39
不錯不錯



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