liang brain
/
Ex_MPU6050_Driver_Balance_chenhuan
asdf
Fork of MPU6050_Driver_Balance by
main.cpp@1:5e51ec875d34, 2018-05-09 (annotated)
- Committer:
- brainliang
- Date:
- Wed May 09 09:59:32 2018 +0000
- Revision:
- 1:5e51ec875d34
- Parent:
- 0:badebd32bd8b
adfsfdsa
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 | |
brainliang | 1:5e51ec875d34 | 5 | DigitalOut myled(PC_13); |
brainliang | 1:5e51ec875d34 | 6 | Serial pc(PA_2, PA_3); |
brainliang | 1:5e51ec875d34 | 7 | Ticker pid_ticker; //声明一个 Ticker 对象 |
brainliang | 1:5e51ec875d34 | 8 | |
brainliang | 1:5e51ec875d34 | 9 | void pid_calculator(); |
brainliang | 1:5e51ec875d34 | 10 | |
brainliang | 1:5e51ec875d34 | 11 | float pitch,roll,yaw; //欧拉角 |
brainliang | 1:5e51ec875d34 | 12 | unsigned int filure_counter = 0; |
brainliang | 1:5e51ec875d34 | 13 | unsigned int SystemTick = 0; |
brainliang | 1:5e51ec875d34 | 14 | |
brainliang | 1:5e51ec875d34 | 15 | void pid_calculator() //更新当前姿态 运行PID算法 |
brainliang | 1:5e51ec875d34 | 16 | { |
brainliang | 1:5e51ec875d34 | 17 | SystemTick++; |
brainliang | 1:5e51ec875d34 | 18 | if(SystemTick >= 5000) |
brainliang | 1:5e51ec875d34 | 19 | SystemTick = 0; |
brainliang | 1:5e51ec875d34 | 20 | if(mpu_dmp_get_data(&pitch,&roll,&yaw) != 0) |
brainliang | 1:5e51ec875d34 | 21 | filure_counter++; |
brainliang | 1:5e51ec875d34 | 22 | } |
brainliang | 1:5e51ec875d34 | 23 | |
heroistired | 0:badebd32bd8b | 24 | |
heroistired | 0:badebd32bd8b | 25 | int main() { |
brainliang | 1:5e51ec875d34 | 26 | |
brainliang | 1:5e51ec875d34 | 27 | int time_now = 0; |
brainliang | 1:5e51ec875d34 | 28 | |
brainliang | 1:5e51ec875d34 | 29 | pid_ticker.attach(&pid_calculator, 0.01); //中断跑在100Hz,因为6050也是以100Hz刷新,PID一定要写在中断pid_calculator()中 |
brainliang | 1:5e51ec875d34 | 30 | |
brainliang | 1:5e51ec875d34 | 31 | |
heroistired | 0:badebd32bd8b | 32 | MPU_Init(); //初始化MPU6050 |
heroistired | 0:badebd32bd8b | 33 | myled = 0; |
heroistired | 0:badebd32bd8b | 34 | while(mpu_dmp_init()) |
heroistired | 0:badebd32bd8b | 35 | { |
heroistired | 0:badebd32bd8b | 36 | wait(0.2); |
heroistired | 0:badebd32bd8b | 37 | myled = !myled; |
heroistired | 0:badebd32bd8b | 38 | } |
heroistired | 0:badebd32bd8b | 39 | while(1) |
heroistired | 0:badebd32bd8b | 40 | { |
brainliang | 1:5e51ec875d34 | 41 | //检测读取数据失败的次数 如感觉6050读数存在问题运行此段代码检查 串口打印的是每1s内 数据获取失败的次数 |
brainliang | 1:5e51ec875d34 | 42 | /*if((SystemTick - time_now) >= 100) |
brainliang | 1:5e51ec875d34 | 43 | { |
brainliang | 1:5e51ec875d34 | 44 | time_now = SystemTick; |
brainliang | 1:5e51ec875d34 | 45 | pc.printf("%d\r\n", filure_counter); |
brainliang | 1:5e51ec875d34 | 46 | filure_counter = 0; |
brainliang | 1:5e51ec875d34 | 47 | }*/ |
brainliang | 1:5e51ec875d34 | 48 | |
brainliang | 1:5e51ec875d34 | 49 | |
heroistired | 0:badebd32bd8b | 50 | pc.printf("pitch: %.2f roll: %.2f yaw: %.2f\r\n", pitch,roll,yaw); |
heroistired | 0:badebd32bd8b | 51 | } |
heroistired | 0:badebd32bd8b | 52 | } |
brainliang | 1:5e51ec875d34 | 53 |