liang brain / Mbed 2 deprecated Ex_MPU6050_Driver_Balance_chenhuan

asdf

Dependencies:   mbed

Fork of MPU6050_Driver_Balance by Chen Huan

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?

UserRevisionLine numberNew 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