main.cpp

00001 #include "mbed.h"
00002 #include "mpu6050.h" 
00003 #define PWM_PERIOD 1    //pwm波的周期  单位ms
00004 #define RATE 0.01       //定时中断的周期，单位s
00005 
00006 
00007 struct PID
00008 {
00009     float Kp;
00010     float Ki;
00011     float Kd;
00012     float error;
00013     float dif_error;
00014     float last_error;
00015     float int_error;
00016 };
00017 
00018                            
00019 float NTMAX=100.0;                          //最大容错
00020 float t =0.0;                               //pid控制的目标
00021 PID PID_Para={0.00008,0.0,-0.00057};        //pid参数,kp,kp,kd
00022 float pitch,roll,yaw;                       //欧拉角
00023 float co;                                   //pid输出结果，作为pwm波的输入
00024 
00025 
00026 Ticker toggle_time_ticker;                  //时间中断，用来控制转速的变化
00027 DigitalOut myled(PC_13);                    //检测mpu是否正常初始化，正常初始化后不会闪烁
00028 Serial pc(PA_9, PA_10);                     //蓝牙串口
00029 PwmOut mypwm1(PA_7);                        //控制电机的pwm波
00030 PwmOut mypwm2(PA_6);                        //控制电机的pwm波
00031 PwmOut mypwm3(PB_0);                        //控制电机的pwm波
00032 PwmOut mypwm4(PB_1);                        //控制电机的pwm波
00033 
00034 /**
00035  * [calculate 计算pid控制结果]
00036  * @param  input  [pid的输入]
00037  * @param  target [pid控制目标]
00038  * @return        [pid的输出]
00039  */
00040 float calculate(float input,float target);
00041 /**
00042  * [make 时间中断函数]
00043  */
00044 void make();
00045 
00046 
00047 int main() {
00048     //设定pwm波的周期 为1ms
00049     mypwm1.period_ms(PWM_PERIOD);
00050     mypwm2.period_ms(PWM_PERIOD);
00051     mypwm3.period_ms(PWM_PERIOD);
00052     mypwm4.period_ms(PWM_PERIOD);
00053 
00054     //初始化MPU6050，正常初始化后不会闪烁
00055     MPU_Init();                 
00056     myled = 0;
00057     while(mpu_dmp_init())
00058     {
00059         wait(0.2);
00060         myled = !myled;
00061     }    
00062 
00063     //设置定时中断的周期为100hz，调用中断函数为void make()
00064     toggle_time_ticker.attach(&make, RATE);
00065 
00066     while(1)
00067     {
00068         //获取mpu数据，roll为俯仰角
00069         mpu_dmp_get_data(&pitch,&roll,&yaw);
00070     }  
00071 
00072 }
00073 
00074 /**
00075  * [make 时间中断函数]
00076  */
00077 void make() {
00078     //计算pid控制结果        
00079     co = calculate(roll, t); 
00080     //控制电机
00081     if (co >=0)
00082     {  
00083        mypwm1.pulsewidth(co);
00084        mypwm2.pulsewidth(0);
00085        mypwm3.pulsewidth(co);
00086        mypwm4.pulsewidth(0);
00087     }
00088     else
00089     {
00090        mypwm1.pulsewidth(0);
00091        mypwm2.pulsewidth(-co);
00092        mypwm3.pulsewidth(0);
00093        mypwm4.pulsewidth(-co);
00094     }
00095 }
00096 
00097 /**
00098  * [calculate 计算pid控制结果]
00099  * @param  input  [pid的输入]
00100  * @param  target [pid控制目标]
00101  * @return        [pid的输出]
00102  */
00103 float calculate(float input,float target)
00104 {
00105     //计算输入与目标的偏差
00106     PID_Para.error=target-input;
00107     //设置死位
00108     if(PID_Para.error<1.0 && PID_Para.error >-1.0) PID_Para.error=0.0;
00109     //计算微分
00110     PID_Para.dif_error=PID_Para.error-PID_Para.last_error;
00111     //后一位偏差
00112     PID_Para.last_error=PID_Para.error;
00113     //计算积分
00114     PID_Para.int_error+=PID_Para.error;
00115     //限制积分范围
00116     if(PID_Para.int_error >= NTMAX)
00117         PID_Para.int_error = NTMAX;
00118     if(PID_Para.int_error <= -NTMAX)
00119         PID_Para.int_error = -NTMAX;
00120     
00121     float output;
00122     //计算pid结果
00123     output=PID_Para.Kp*PID_Para.error+PID_Para.Ki*PID_Para.int_error-PID_Para.Kd*PID_Para.dif_error;
00124     return output;
00125 }