Keegan Hu / Mbed 2 deprecated MPU6050_Driver_Balance

fsaf

Dependencies:   mbed

Fork of MPU6050_Driver_Balanceddddd by Keegan Hu

main.cpp@2:3a7eb05dbc72, 2018-04-20 (annotated)

Committer:
glintligo
Date:
Fri Apr 20 10:56:57 2018 +0000
Revision:
2:3a7eb05dbc72
Parent:
1:f9658c7309ef 
2018.4.20;

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 2:3a7eb05dbc72 20
glintligo 2:3a7eb05dbc72 21 PID PID_Para={0.000085,0.0,-0.0002};
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 2:3a7eb05dbc72 32 co = calculate(roll, 0.0);
glintligo 2:3a7eb05dbc72 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 2:3a7eb05dbc72 37 mypwm1.pulsewidth(co);
glintligo 2:3a7eb05dbc72 38 mypwm2.pulsewidth(0);
glintligo 2:3a7eb05dbc72 39 mypwm3.pulsewidth(co);
glintligo 2:3a7eb05dbc72 40 mypwm4.pulsewidth(0);}
glintligo 1:f9658c7309ef 41 else
glintligo 1:f9658c7309ef 42 {
glintligo 2:3a7eb05dbc72 43 mypwm1.pulsewidth(0);
glintligo 2:3a7eb05dbc72 44 mypwm2.pulsewidth(-co);
glintligo 2:3a7eb05dbc72 45 mypwm3.pulsewidth(0);
glintligo 2:3a7eb05dbc72 46 mypwm4.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 PID_Para.int_error+=PID_Para.error;
glintligo 1:f9658c7309ef 56 if(PID_Para.int_error >= NTMAX)
glintligo 1:f9658c7309ef 57 PID_Para.int_error = NTMAX;
glintligo 1:f9658c7309ef 58 if(PID_Para.int_error <= -NTMAX)
glintligo 1:f9658c7309ef 59 PID_Para.int_error = -NTMAX;
glintligo 1:f9658c7309ef 60
glintligo 1:f9658c7309ef 61 float output;
glintligo 1:f9658c7309ef 62 output=PID_Para.Kp*PID_Para.error+PID_Para.Ki*PID_Para.int_error-PID_Para.Kd*PID_Para.dif_error;
glintligo 1:f9658c7309ef 63 return output;
glintligo 1:f9658c7309ef 64 }
heroistired 0:badebd32bd8b 65
heroistired 0:badebd32bd8b 66 int main() {
glintligo 1:f9658c7309ef 67
glintligo 1:f9658c7309ef 68 mypwm1.period_ms(PWM_PERIOD);
glintligo 1:f9658c7309ef 69 mypwm2.period_ms(PWM_PERIOD);
glintligo 1:f9658c7309ef 70 mypwm3.period_ms(PWM_PERIOD);
glintligo 1:f9658c7309ef 71 mypwm4.period_ms(PWM_PERIOD);
glintligo 1:f9658c7309ef 72
heroistired 0:badebd32bd8b 73
heroistired 0:badebd32bd8b 74 MPU_Init(); //初始化MPU6050
heroistired 0:badebd32bd8b 75 myled = 0;
heroistired 0:badebd32bd8b 76 while(mpu_dmp_init())
heroistired 0:badebd32bd8b 77 {
heroistired 0:badebd32bd8b 78 wait(0.2);
heroistired 0:badebd32bd8b 79 myled = !myled;
heroistired 0:badebd32bd8b 80 }
glintligo 1:f9658c7309ef 81
glintligo 1:f9658c7309ef 82 toggle_led_ticker.attach(&make, RATE);
heroistired 0:badebd32bd8b 83 while(1)
heroistired 0:badebd32bd8b 84 {
glintligo 1:f9658c7309ef 85 mpu_dmp_get_data(&pitch,&roll,&yaw);
heroistired 0:badebd32bd8b 86 }
heroistired 0:badebd32bd8b 87 }