Chen Huan
/
mpu
平衡车的MPU6050驱动 C.H.
Fork of MPU6050_Driver_Balance by
main.cpp@1:588d4df02e56, 2018-05-02 (annotated)
- Committer:
- heroistired
- Date:
- Wed May 02 01:32:39 2018 +0000
- Revision:
- 1:588d4df02e56
- Parent:
- 0:badebd32bd8b
d
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
heroistired | 0:badebd32bd8b | 1 | #include "mbed.h" |
heroistired | 0:badebd32bd8b | 2 | #include "mpu6050.h" |
heroistired | 0:badebd32bd8b | 3 | |
heroistired | 0:badebd32bd8b | 4 | |
heroistired | 0:badebd32bd8b | 5 | DigitalOut myled(LED1); |
heroistired | 0:badebd32bd8b | 6 | Serial pc(SERIAL_TX, SERIAL_RX); |
heroistired | 1:588d4df02e56 | 7 | Ticker pid_ticker; //声明一个 Ticker 对象 |
heroistired | 1:588d4df02e56 | 8 | |
heroistired | 1:588d4df02e56 | 9 | void pid_calculator(); |
heroistired | 1:588d4df02e56 | 10 | |
heroistired | 1:588d4df02e56 | 11 | float pitch,roll,yaw; //欧拉角 |
heroistired | 1:588d4df02e56 | 12 | unsigned int filure_counter = 0; |
heroistired | 1:588d4df02e56 | 13 | unsigned int SystemTick = 0; |
heroistired | 0:badebd32bd8b | 14 | |
heroistired | 0:badebd32bd8b | 15 | int main() { |
heroistired | 1:588d4df02e56 | 16 | |
heroistired | 1:588d4df02e56 | 17 | int time_now = 0; |
heroistired | 1:588d4df02e56 | 18 | |
heroistired | 1:588d4df02e56 | 19 | pid_ticker.attach(&pid_calculator, 0.01); //中断跑在100Hz,因为6050也是以100Hz刷新 |
heroistired | 1:588d4df02e56 | 20 | |
heroistired | 1:588d4df02e56 | 21 | |
heroistired | 0:badebd32bd8b | 22 | MPU_Init(); //初始化MPU6050 |
heroistired | 0:badebd32bd8b | 23 | myled = 0; |
heroistired | 0:badebd32bd8b | 24 | while(mpu_dmp_init()) |
heroistired | 0:badebd32bd8b | 25 | { |
heroistired | 0:badebd32bd8b | 26 | wait(0.2); |
heroistired | 0:badebd32bd8b | 27 | myled = !myled; |
heroistired | 0:badebd32bd8b | 28 | } |
heroistired | 0:badebd32bd8b | 29 | while(1) |
heroistired | 0:badebd32bd8b | 30 | { |
heroistired | 1:588d4df02e56 | 31 | //检测读取数据失败的次数 如感觉6050读数存在问题运行此段代码检查 串口打印的是每1s内 数据获取失败的次数 |
heroistired | 1:588d4df02e56 | 32 | /*if((SystemTick - time_now) >= 100) |
heroistired | 1:588d4df02e56 | 33 | { |
heroistired | 1:588d4df02e56 | 34 | time_now = SystemTick; |
heroistired | 1:588d4df02e56 | 35 | pc.printf("%d\r\n", filure_counter); |
heroistired | 1:588d4df02e56 | 36 | filure_counter = 0; |
heroistired | 1:588d4df02e56 | 37 | }*/ |
heroistired | 1:588d4df02e56 | 38 | |
heroistired | 1:588d4df02e56 | 39 | |
heroistired | 0:badebd32bd8b | 40 | pc.printf("pitch: %.2f roll: %.2f yaw: %.2f\r\n", pitch,roll,yaw); |
heroistired | 0:badebd32bd8b | 41 | } |
heroistired | 0:badebd32bd8b | 42 | } |
heroistired | 1:588d4df02e56 | 43 | |
heroistired | 1:588d4df02e56 | 44 | void pid_calculator() //更新当前姿态 运行PID算法 |
heroistired | 1:588d4df02e56 | 45 | { |
heroistired | 1:588d4df02e56 | 46 | SystemTick++; |
heroistired | 1:588d4df02e56 | 47 | if(SystemTick >= 5000) |
heroistired | 1:588d4df02e56 | 48 | SystemTick = 0; |
heroistired | 1:588d4df02e56 | 49 | if(mpu_dmp_get_data(&pitch,&roll,&yaw) != 0) |
heroistired | 1:588d4df02e56 | 50 | filure_counter++; |
heroistired | 1:588d4df02e56 | 51 | } |