久久久久久久999_99精品久久精品一区二区爱城_成人欧美一区二区三区在线播放_国产精品日本一区二区不卡视频_国产午夜视频_欧美精品在线观看免费

<button id="wcyqg"></button>

<code id="wcyqg"><wbr id="wcyqg"></wbr></code>

標題: 完美的平衡車程序 卡爾曼濾波和PID及nrf24l01無線 [打印本頁]

---

作者: feerom    時間: 2017-11-30 08:34
標題: 完美的平衡車程序 卡爾曼濾波和PID及nrf24l01無線
完美的平衡車程序，完整的代碼，有卡爾曼濾波和PID


單片機源程序如下:

1. /***********************************************
   
2. 標題: mian.c
   
3. 日期: 2013/12/27
   
4. 版本：v1.0
   
5. MDK-ARM 版本: v4.12
   
6. ST 庫版本：v3.50  
   
7. 功能: 自平衡小車平衡及控制
   
8. 說明：可結合上位機校正六軸的補償值，使用藍牙或者NRF24L01控制
   
9. *************************************************/
   
10. #include "stm32f10x.h"
    
11. #include "iic.h"
    
12. #include "timer.h"
    
13. #include "usart.h"
    
14. #include "24l01.h"
    
15. #include "mpu3050.h"
    
16. #include "adxl345.h"
    
17. #include "filter.h"
    
18. #include "calculate.h"
    
19. #include "usb_lib.h"
    
20. #include "usb_desc.h"
    
21. #include "usb_istr.h"
    
22. #include "hw_config.h"
    
23. #include "usb_pwr.h"
    
24. #include "usb_pwr.h"
    
25. #include <stdio.h>
    
26. #include <math.h>
    
27. 
    
28. u8 receive_data;
    
29. u8 flg_get_senor_data;
    
30. u8 out[35]  ={0x5f, 0x60, 0};
    
31. static float angle, angle_dot, f_angle, f_angle_dot;
    
32. s16 temp;
    
33. s16 gx, gy, gz, ax ,ay, az, temperature;
    
34. s16 gx_offset, ax_offset, ay_offset, az_offset;
    
35. u8 tmp[2]={0};        
    
36. 
    
37. #define USB
    
38. #define FILTER_COUNT  32
    
39. s16 gx_buf[FILTER_COUNT], ax_buf[FILTER_COUNT], ay_buf[FILTER_COUNT],az_buf[FILTER_COUNT];
    
40. /******************************************************************************/
    
41. void delay(u32 count)   //延時函數
    
42. {
    
43.   for(; count != 0; count--);
    
44. }
    
45. /*************************************************
    
46. 
    
47. 名稱：void acc_filter(void)
    
48. 功能：加速度計數據濾波
    
49. 輸入參數：據濾波后的數據
    
50. 輸出參數：無
    
51. 返回值：  無
    
52. **************************************************/
    
53. void acc_filter(void)
    
54. {
    
55.   u8 i;
    
56.   s32 ax_sum = 0, ay_sum = 0, az_sum = 0;
    
57. 
    
58.   for(i = 1 ; i < FILTER_COUNT; i++)
    
59.   {
    
60.     ax_buf[i - 1] = ax_buf[i];
    
61.         ay_buf[i - 1] = ay_buf[i];
    
62.         az_buf[i - 1] = az_buf[i];
    
63.   }
    
64. 
    
65.   ax_buf[FILTER_COUNT - 1] = ax;
    
66.   ay_buf[FILTER_COUNT - 1] = ay;
    
67.   az_buf[FILTER_COUNT - 1] = az;
    
68. 
    
69.   for(i = 0 ; i < FILTER_COUNT; i++)
    
70.   {
    
71.     ax_sum += ax_buf[i];
    
72.         ay_sum += ay_buf[i];
    
73.         az_sum += az_buf[i];
    
74.   }
    
75. 
    
76.   ax = (s16)(ax_sum / FILTER_COUNT);
    
77.   ay = (s16)(ay_sum / FILTER_COUNT);
    
78.   az = (s16)(az_sum / FILTER_COUNT);
    
79. }
    
80. /*****************************************************************************/
    
81. int main(void)
    
82. {
    
83.   GPIO_InitTypeDef GPIO_InitStructure;
    
84.         
    
85. #ifdef USB //usb接入
    
86.   NVIC_InitTypeDef NVIC_InitStructure;
    
87. #endif
    
88. 
    
89.   u8 j, i=0;
    
90. 
    
91.   RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA|RCC_APB2Periph_GPIOB, ENABLE);
    
92.   
    
93.   GPIO_InitStructure.GPIO_Pin = GPIO_Pin_5;
    
94.   GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
    
95.   GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
    
96.    
    
97.   GPIO_Init(GPIOA, &GPIO_InitStructure);
    
98.   GPIO_SetBits(GPIOA,GPIO_Pin_5);  //blue led
    
99. 
    
100. #ifdef USB
     
101.   usb_system_init();
     
102. #endif
     
103.   
     
104.         //初始化
     
105.   usart_init();                                          
     
106.   iic_init();
     
107.   timer_init();
     
108.         
     
109.          NRF24L01_Init();    //初始化NRF24L01
     
110.   mpu3050_init();
     
111.   adxl345_init();
     
112. 
     
113.   motor_init();
     
114.         LED2OFF;//確認關閉LED2
     
115. while(NRF24L01_Check())//檢測不到24L01
     
116.         {
     
117.                 delay(3000);
     
118.                 LED2ON;  //LED2常亮
     
119.         }
     
120.         
     
121.   RX_Mode();   //24L01接收模式
     
122.           LED1ON;
     
123.   while (1)
     
124.   {  
     
125.     if(flg_get_senor_data)   //TIM3觸發
     
126.     {
     
127.       flg_get_senor_data = 0;
     
128. #ifdef USB
     
129.           NVIC_InitStructure.NVIC_IRQChannel = USB_LP_CAN1_RX0_IRQn;
     
130.           NVIC_InitStructure.NVIC_IRQChannelCmd = DISABLE;    //USB通訊關閉
     
131.           NVIC_Init(&NVIC_InitStructure);
     
132. #endif            
     
133.              //獲取六軸的原始值
     
134.       mpu3050_get_data(&gx, &gy, &gz, &temperature);
     
135.       adxl345_get_data(&ax, &ay ,&az);
     
136. 
     
137. #ifdef USB
     
138.           NVIC_InitStructure.NVIC_IRQChannel = USB_LP_CAN1_RX0_IRQn;
     
139.           NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;      //USB通訊啟動
     
140.           NVIC_Init(&NVIC_InitStructure);
     
141. #endif
     
142. 
     
143.           acc_filter();
     
144. 
     
145.           gx -= gx_offset;  //gx補償校正
     
146.                 ax -= ax_offset;  //ax補償校正
     
147.           ay -=        ay_offset;  //ay補償校正
     
148.           az -=        az_offset;  //az補償校正
     
149. 
     
150.           angle_dot = gx * GYRO_SCALE;  //+-2000  0.060975 °/LSB
     
151.       angle = atan(ay / sqrt(ax * ax + az * az ));
     
152.       angle = angle * 57.295780;    //180/pi
     
153. 
     
154.           kalman_filter(angle, angle_dot, &f_angle, &f_angle_dot);//卡爾曼濾波函數
     
155. 
     
156.     if(tmp[0]==0){ receive_parameter(receive_data);}//藍牙控制
     
157.                 //---------------------------------------------------------------
     
158.                
     
159.                 if(NRF24L01_RxPacket(tmp)==0)         //24L01控制
     
160.                         {
     
161.                                 receive_parameter(tmp[1]);
     
162.                                 LED1OFF;
     
163.       }        
     
164.                         
     
165.                
     
166. //------------------------------------------------------------
     
167.           pid(f_angle, f_angle_dot);   //PID調節
     
168. 
     
169. #ifdef USB    //USB上位機數據處理
     
170.   temp = (s16)(f_angle * 100);
     
171. 
     
172.           out[2] = (u8)(gx >> 8);
     
173.           out[3] = (u8)(gx);
     
174.           out[4] = (u8)(gy >> 8);
     
175.           out[5] = (u8)(gy);
     
176.           out[6] = (u8)(gz >> 8);
     
177.           out[7] = (u8)(gz);
     
178.           out[8] = (u8)(ax >> 8);
     
179.           out[9] = (u8)(ax);
     
180.           out[10] = (u8)(ay >> 8);
     
181.           out[11] = (u8)(ay);
     
182.           out[12] = (u8)(az >> 8);
     
183.           out[13] = (u8)(az);
     
184.     out[14] = (u8)(temp >> 8);
     
185.           out[15] =        (u8)(temp);
     
186. ……………………
     
187. 
     
188. …………限于本文篇幅 余下代碼請從51黑下載附件…………
     

復制代碼

所有資料51hei提供下載:
代碼.7z (698.94 KB, 下載次數: 119)

2022-12-13 04:14 上傳

點擊文件名下載附件
下載積分: 黑幣 -5

---

歡迎光臨 (http://m.zg4o1577.cn/bbs/)

Powered by Discuz! X3.1

感谢您访问我们的网站，您可能还对以下资源感兴趣：

久久久久久久999|99精品久久精品一区二区爱城|成人欧美一区二区三区在线播放|国产精品日本一区二区不卡视频|国产午夜视频|欧美精品在线观看免费

主站蜘蛛池模板： 国产伦精品一区二区三区四区 | 免费成人深夜夜行网站 | 成人网在线观看 | 国产做受网站 | 日韩精品大片 | 高hnp失禁3p小公主 | 欧美精品区 | www.一级片 | 九九在线免费视频 | 欧美a级成人淫片免费看 | 久久九九99| 美女一级片 | 三级在线看| 亚洲黄色一级 | 午夜av在线播放 | 北岛玲在线 | 中文字幕第8页 | 国产精品成人国产乱一区 | 五月天在线观看 | 黄色高清网站 | 久久精品2| 久久机热这里只有精品 | 欧美日日夜夜 | 日韩一区二区三区免费 | 丰满女人裸体淫交 | 色综合五月天 | 超碰国产在线 | 两性免费视频 | 国产三级午夜理伦三级 | 色爽 | 亚洲综合自拍 | 欧美激情五月 | 91狠狠干 | 成人午夜又粗又硬又大 | www.黄色在线 | 国产精品视屏 | 亚洲国产成人在线 | 欧美综合在线视频 | 中文在线免费观看 | 欧美三级韩国三级日本三斤在线观看 | 日韩欧美高清 |

<table id="sieya"></table>

<li id="sieya"><dl id="sieya"></dl></li>

<li id="sieya"></li>

<xmp id="sieya"><acronym id="sieya"></acronym></xmp>

<nav id="sieya"><center id="sieya"></center></nav>