Keegan Hu
/
balance_car
控制平衡车的代码
Fork of MPU6050_Driver_Balance by
Diff: main.cpp
- Revision:
- 1:f9658c7309ef
- Parent:
- 0:badebd32bd8b
- Child:
- 2:3a7eb05dbc72
diff -r badebd32bd8b -r f9658c7309ef main.cpp --- a/main.cpp Mon Apr 09 14:34:45 2018 +0000 +++ b/main.cpp Fri Apr 20 08:27:50 2018 +0000 @@ -1,12 +1,76 @@ #include "mbed.h" #include "mpu6050.h" +#define PWM_PERIOD 1 +#define RATE 0.01 -DigitalOut myled(LED1); -Serial pc(SERIAL_TX, SERIAL_RX); +float NTMAX=100.0; +struct PID +{ + float Kp; + float Ki; + float Kd; + float error; + float dif_error; + float last_error; + float int_error; +}; +//0.00005 + +//0.00015 +PID PID_Para={0.0,0.0,0.0005}; +Ticker toggle_led_ticker; //声明一个 Ticker 对象 +DigitalOut myled(PC_13); +//Serial pc(PA_9, PA_10); +float pitch,roll,yaw,co; //欧拉角 +PwmOut mypwm1(PA_7); +PwmOut mypwm2(PA_6); +PwmOut mypwm3(PB_0); +PwmOut mypwm4(PB_1); +float calculate(float input,float target); +void make() { //中断子程序,在平衡车和无人机的项目中,控制算法就写在中断子程序中:获取陀螺仪数据,根据陀螺仪数据通过PID算法得到电机的控制量(占空比和方向),在输出到单片机管脚 + co = calculate(roll, 0.0); + // pc.printf("pitch: %.2f roll: %.2f yaw: %.2f co: %.2f\r\n", pitch,roll,yaw,co); + // pc.printf("roll: %.2f co: %.2f\r\n",roll,co); + if (co >=0) + { + mypwm2.pulsewidth(co); + mypwm1.pulsewidth(0); + mypwm4.pulsewidth(co); + mypwm3.pulsewidth(0);} + else + { + mypwm2.pulsewidth(0); + mypwm1.pulsewidth(-co); + mypwm4.pulsewidth(0); + mypwm3.pulsewidth(-co);} +} + + +float calculate(float input,float target) +{ + PID_Para.error=target-input; + PID_Para.dif_error=PID_Para.error-PID_Para.last_error; + PID_Para.last_error=PID_Para.error; + + PID_Para.int_error+=PID_Para.error; + if(PID_Para.int_error >= NTMAX) + PID_Para.int_error = NTMAX; + if(PID_Para.int_error <= -NTMAX) + PID_Para.int_error = -NTMAX; + + float output; + output=PID_Para.Kp*PID_Para.error+PID_Para.Ki*PID_Para.int_error-PID_Para.Kd*PID_Para.dif_error; + return output; +} int main() { - float pitch,roll,yaw; //欧拉角 + + mypwm1.period_ms(PWM_PERIOD); + mypwm2.period_ms(PWM_PERIOD); + mypwm3.period_ms(PWM_PERIOD); + mypwm4.period_ms(PWM_PERIOD); + MPU_Init(); //初始化MPU6050 myled = 0; @@ -15,11 +79,10 @@ wait(0.2); myled = !myled; } + + toggle_led_ticker.attach(&make, RATE); while(1) { - if(mpu_dmp_get_data(&pitch,&roll,&yaw)==0)//获取欧拉角 - { - pc.printf("pitch: %.2f roll: %.2f yaw: %.2f\r\n", pitch,roll,yaw); - } + mpu_dmp_get_data(&pitch,&roll,&yaw); } }