单轴PID调节测试，距离做无人机又近了一步

经过几个星期的修改代码，单轴PID调节有点意思了。
硬件：
树莓派3b
mpu6050
pca9685
无刷电机 X 2
```
struct PID
{
float kp; //< proportional gain调整比例参数
float ki; //< integral gain调整积分参数
float kd; //< derivative gain调整微分参数
float pregyro;
//float desired; //< set point 期望值
float integ; //< integral积分参数
float iLimit; //< integral limit积分限制范围
float deriv; //< derivative微分参数
float preerror; //< previous error 上一次误差
float output;
float error; //< error 误差
float lastoutput;
} ;
PID Roll_Suit;

PID调节算法：measured是MPU6050的测量值， desired，是期望，这里期望是0.1， inital_Error是误差值0.38。

float Pid_Calc(PID &pidsuite,float measured,float desired,float Inital_Error)
{
    
    pidsuite.error = desired - measured + Inital_Error ;//偏差：期望-测量值
    pidsuite.error = pidsuite.error; 
    pidsuite.integ += pidsuite.error * IMU_UPDATE_DT;//偏差积分，IMU_UPDATE_DT也就是每调整漏斗大小的步辐
    
    if (pidsuite.integ >= pidsuite.iLimit)//作积分限制
    {
      pidsuite.integ = pidsuite.iLimit;
    }
    else if (pidsuite.integ < -pidsuite.iLimit)
    {
      pidsuite.integ = -pidsuite.iLimit;
    }
    
    pidsuite.deriv = (pidsuite.error - pidsuite.preerror) / 0.01;//微分     应该可用陀螺仪角速度代替
    pidsuite.preerror = pidsuite.error;//debug用 更新前一次偏差
    
    AngleSpeed[0] = pidsuite.deriv;
    
    pidsuite.output = (pidsuite.kp * pidsuite.error) + (pidsuite.ki * pidsuite.integ) + (pidsuite.kd * pidsuite.deriv);
    
    pid_in = pidsuite.output;
    
    pregyro = pidsuite.pregyro;
    if (pidsuite.output >  0.15)
    {
        pidsuite.output = 0.15;
    }
    if (pidsuite.output <  -0.15)
    {
        pidsuite.output = -0.15;
    }
    if (fabs(pidsuite.error) < 0.3 )
    {
        pidsuite.output = pidsuite.lastoutput * 0.5;
    }
    pidsuite.lastoutput = pidsuite.output;
    if(PID_ENABLE == 0)
    {
        return 0;
    }
    printf("%0.02f, %0.02f\n", measured, Pid_Roll);
    return pidsuite.output;
}

代码进入循环之后，会一直调用上面的算法进行调整。output就是输出的电机需要的值。

for(;;)
    {
        ms_update();
        Angle[0] = average_filter(ms_getRoll() ,Filter_Roll);//此为Roll
        //Angle[0] = ms_getRoll();
        Pid_Roll = Pid_Calc(Roll_Suit, Angle[0], 0.1, 0.28); 
        All_Count = All_Count + 1;
        Default_Acc = Default_Acc ;
        gettimeofday(&tv_now, NULL);
        if (abs(tv_now.tv_usec - tv_last.tv_usec) > 800)
        {    
            if(Default_Acc > 0.4)
            {
                Default_Acc = 0.46;
            }
            else
            {
                Default_Acc = 0.03;
            }
 
        }
        if (Default_Acc >= MAX_ACC)
        {
            Default_Acc = MAX_ACC;
        }
        if (Default_Acc <= 0.03)
        {
            Default_Acc = 0.03;
        }
        DutyCycle[0] = Default_Acc  + Pid_Roll ; 
        DutyCycle[1] = Default_Acc  - Pid_Roll ; 
        PWMOut(PinNumber1,DutyCycle[0]);
        PWMOut(PinNumber2,DutyCycle[1]);

下一步计划，计划做成四轴的形式，期待早日飞机起飞悬空