File content as of revision 1:db5ac8bf9280:

#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波的输入


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() {
    //设定pwm波的周期 为1ms
    mypwm1.period_ms(PWM_PERIOD);
    mypwm2.period_ms(PWM_PERIOD);
    mypwm3.period_ms(PWM_PERIOD);
    mypwm4.period_ms(PWM_PERIOD);

    //初始化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)
    {
        //获取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;
}