File content as of revision 2:3a7eb05dbc72:

#include "mbed.h"
#include "mpu6050.h" 
#define PWM_PERIOD 1
#define RATE 0.01


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


PID PID_Para={0.000085,0.0,-0.0002};
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)
         {  
        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);}
}


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() {
    
    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;
    while(mpu_dmp_init())
    {
        wait(0.2);
        myled = !myled;
    }    
  
    toggle_led_ticker.attach(&make, RATE);
    while(1)
    {
       mpu_dmp_get_data(&pitch,&roll,&yaw);
    }   
}