Keegan Hu / Mbed 2 deprecated MPU6050_Driver_Balance

fsaf

Dependencies:   mbed

Fork of MPU6050_Driver_Balanceddddd by Keegan Hu

main.cpp@1:f9658c7309ef, 2018-04-20 (annotated)

Committer:
glintligo
Date:
Fri Apr 20 08:27:50 2018 +0000
Revision:
1:f9658c7309ef
Parent:
0:badebd32bd8b
Child:
2:3a7eb05dbc72
fdd

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"
glintligo 1:f9658c7309ef 3 #define PWM_PERIOD 1
glintligo 1:f9658c7309ef 4 #define RATE 0.01
heroistired 0:badebd32bd8b 5
heroistired 0:badebd32bd8b 6
glintligo 1:f9658c7309ef 7 float NTMAX=100.0;
glintligo 1:f9658c7309ef 8 struct PID
glintligo 1:f9658c7309ef 9 {
glintligo 1:f9658c7309ef 10 float Kp;
glintligo 1:f9658c7309ef 11 float Ki;
glintligo 1:f9658c7309ef 12 float Kd;
glintligo 1:f9658c7309ef 13 float error;
glintligo 1:f9658c7309ef 14 float dif_error;
glintligo 1:f9658c7309ef 15 float last_error;
glintligo 1:f9658c7309ef 16 float int_error;
glintligo 1:f9658c7309ef 17 };
glintligo 1:f9658c7309ef 18 //0.00005
glintligo 1:f9658c7309ef 19
glintligo 1:f9658c7309ef 20 //0.00015
glintligo 1:f9658c7309ef 21 PID PID_Para={0.0,0.0,0.0005};
glintligo 1:f9658c7309ef 22 Ticker toggle_led_ticker; //声明一个 Ticker 对象
glintligo 1:f9658c7309ef 23 DigitalOut myled(PC_13);
glintligo 1:f9658c7309ef 24 //Serial pc(PA_9, PA_10);
glintligo 1:f9658c7309ef 25 float pitch,roll,yaw,co; //欧拉角
glintligo 1:f9658c7309ef 26 PwmOut mypwm1(PA_7);
glintligo 1:f9658c7309ef 27 PwmOut mypwm2(PA_6);
glintligo 1:f9658c7309ef 28 PwmOut mypwm3(PB_0);
glintligo 1:f9658c7309ef 29 PwmOut mypwm4(PB_1);
glintligo 1:f9658c7309ef 30 float calculate(float input,float target);
glintligo 1:f9658c7309ef 31 void make() { //中断子程序,在平衡车和无人机的项目中,控制算法就写在中断子程序中:获取陀螺仪数据,根据陀螺仪数据通过PID算法得到电机的控制量(占空比和方向),在输出到单片机管脚
glintligo 1:f9658c7309ef 32 co = calculate(roll, 0.0);
glintligo 1:f9658c7309ef 33 // pc.printf("pitch: %.2f roll: %.2f yaw: %.2f co: %.2f\r\n", pitch,roll,yaw,co);
glintligo 1:f9658c7309ef 34 // pc.printf("roll: %.2f co: %.2f\r\n",roll,co);
glintligo 1:f9658c7309ef 35 if (co >=0)
glintligo 1:f9658c7309ef 36 {
glintligo 1:f9658c7309ef 37 mypwm2.pulsewidth(co);
glintligo 1:f9658c7309ef 38 mypwm1.pulsewidth(0);
glintligo 1:f9658c7309ef 39 mypwm4.pulsewidth(co);
glintligo 1:f9658c7309ef 40 mypwm3.pulsewidth(0);}
glintligo 1:f9658c7309ef 41 else
glintligo 1:f9658c7309ef 42 {
glintligo 1:f9658c7309ef 43 mypwm2.pulsewidth(0);
glintligo 1:f9658c7309ef 44 mypwm1.pulsewidth(-co);
glintligo 1:f9658c7309ef 45 mypwm4.pulsewidth(0);
glintligo 1:f9658c7309ef 46 mypwm3.pulsewidth(-co);}
glintligo 1:f9658c7309ef 47 }
glintligo 1:f9658c7309ef 48
glintligo 1:f9658c7309ef 49
glintligo 1:f9658c7309ef 50 float calculate(float input,float target)
glintligo 1:f9658c7309ef 51 {
glintligo 1:f9658c7309ef 52 PID_Para.error=target-input;
glintligo 1:f9658c7309ef 53 PID_Para.dif_error=PID_Para.error-PID_Para.last_error;
glintligo 1:f9658c7309ef 54 PID_Para.last_error=PID_Para.error;
glintligo 1:f9658c7309ef 55
glintligo 1:f9658c7309ef 56 PID_Para.int_error+=PID_Para.error;
glintligo 1:f9658c7309ef 57 if(PID_Para.int_error >= NTMAX)
glintligo 1:f9658c7309ef 58 PID_Para.int_error = NTMAX;
glintligo 1:f9658c7309ef 59 if(PID_Para.int_error <= -NTMAX)
glintligo 1:f9658c7309ef 60 PID_Para.int_error = -NTMAX;
glintligo 1:f9658c7309ef 61
glintligo 1:f9658c7309ef 62 float output;
glintligo 1:f9658c7309ef 63 output=PID_Para.Kp*PID_Para.error+PID_Para.Ki*PID_Para.int_error-PID_Para.Kd*PID_Para.dif_error;
glintligo 1:f9658c7309ef 64 return output;
glintligo 1:f9658c7309ef 65 }
heroistired 0:badebd32bd8b 66
heroistired 0:badebd32bd8b 67 int main() {
glintligo 1:f9658c7309ef 68
glintligo 1:f9658c7309ef 69 mypwm1.period_ms(PWM_PERIOD);
glintligo 1:f9658c7309ef 70 mypwm2.period_ms(PWM_PERIOD);
glintligo 1:f9658c7309ef 71 mypwm3.period_ms(PWM_PERIOD);
glintligo 1:f9658c7309ef 72 mypwm4.period_ms(PWM_PERIOD);
glintligo 1:f9658c7309ef 73
heroistired 0:badebd32bd8b 74
heroistired 0:badebd32bd8b 75 MPU_Init(); //初始化MPU6050
heroistired 0:badebd32bd8b 76 myled = 0;
heroistired 0:badebd32bd8b 77 while(mpu_dmp_init())
heroistired 0:badebd32bd8b 78 {
heroistired 0:badebd32bd8b 79 wait(0.2);
heroistired 0:badebd32bd8b 80 myled = !myled;
heroistired 0:badebd32bd8b 81 }
glintligo 1:f9658c7309ef 82
glintligo 1:f9658c7309ef 83 toggle_led_ticker.attach(&make, RATE);
heroistired 0:badebd32bd8b 84 while(1)
heroistired 0:badebd32bd8b 85 {
glintligo 1:f9658c7309ef 86 mpu_dmp_get_data(&pitch,&roll,&yaw);
heroistired 0:badebd32bd8b 87 }
heroistired 0:badebd32bd8b 88 }