![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
最终代码
Fork of MPU6050_Driver_Balance by
Revision 1:db5ac8bf9280, committed 2018-05-20
- Comitter:
- glintligo
- Date:
- Sun May 20 07:02:38 2018 +0000
- Parent:
- 0:badebd32bd8b
- Commit message:
- ????
Changed in this revision
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
diff -r badebd32bd8b -r db5ac8bf9280 main.cpp --- a/main.cpp Mon Apr 09 14:34:45 2018 +0000 +++ b/main.cpp Sun May 20 07:02:38 2018 +0000 @@ -1,25 +1,125 @@ #include "mbed.h" #include "mpu6050.h" +#define PWM_PERIOD 1 //pwm波的周期 单位ms +#define RATE 0.01 //定时中断的周期,单位s + + +struct PID +{ + float Kp; + float Ki; + float Kd; + float error; + float dif_error; + float last_error; + float int_error; +}; + + +float NTMAX=100.0; //最大容错 +float t =0.0; //pid控制的目标 +PID PID_Para={0.00008,0.0,-0.00057}; //pid参数,kp,kp,kd +float pitch,roll,yaw; //欧拉角 +float co; //pid输出结果,作为pwm波的输入 -DigitalOut myled(LED1); -Serial pc(SERIAL_TX, SERIAL_RX); +Ticker toggle_time_ticker; //时间中断,用来控制转速的变化 +DigitalOut myled(PC_13); //检测mpu是否正常初始化,正常初始化后不会闪烁 +Serial pc(PA_9, PA_10); //蓝牙串口 +PwmOut mypwm1(PA_7); //控制电机的pwm波 +PwmOut mypwm2(PA_6); //控制电机的pwm波 +PwmOut mypwm3(PB_0); //控制电机的pwm波 +PwmOut mypwm4(PB_1); //控制电机的pwm波 + +/** + * [calculate 计算pid控制结果] + * @param input [pid的输入] + * @param target [pid控制目标] + * @return [pid的输出] + */ +float calculate(float input,float target); +/** + * [make 时间中断函数] + */ +void make(); + int main() { - float pitch,roll,yaw; //欧拉角 + //设定pwm波的周期 为1ms + mypwm1.period_ms(PWM_PERIOD); + mypwm2.period_ms(PWM_PERIOD); + mypwm3.period_ms(PWM_PERIOD); + mypwm4.period_ms(PWM_PERIOD); - MPU_Init(); //初始化MPU6050 + //初始化MPU6050,正常初始化后不会闪烁 + MPU_Init(); myled = 0; while(mpu_dmp_init()) { wait(0.2); myled = !myled; } + + //设置定时中断的周期为100hz,调用中断函数为void make() + toggle_time_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数据,roll为俯仰角 + mpu_dmp_get_data(&pitch,&roll,&yaw); + } + +} + +/** + * [make 时间中断函数] + */ +void make() { + //计算pid控制结果 + co = calculate(roll, t); + //控制电机 + if (co >=0) + { + mypwm1.pulsewidth(co); + mypwm2.pulsewidth(0); + mypwm3.pulsewidth(co); + mypwm4.pulsewidth(0); + } + else + { + mypwm1.pulsewidth(0); + mypwm2.pulsewidth(-co); + mypwm3.pulsewidth(0); + mypwm4.pulsewidth(-co); + } } + +/** + * [calculate 计算pid控制结果] + * @param input [pid的输入] + * @param target [pid控制目标] + * @return [pid的输出] + */ +float calculate(float input,float target) +{ + //计算输入与目标的偏差 + PID_Para.error=target-input; + //设置死位 + if(PID_Para.error<1.0 && PID_Para.error >-1.0) PID_Para.error=0.0; + //计算微分 + 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; + //计算pid结果 + output=PID_Para.Kp*PID_Para.error+PID_Para.Ki*PID_Para.int_error-PID_Para.Kd*PID_Para.dif_error; + return output; +}