关于倒立摆如何通过测得的角度值用pid算法调节电机pwm占空比的问题

这是我自己写的程序，关于pid将角度和电机控制相结合，角度测量我用的是mpu6050，电机为普通的直流电机。不知道我这样写程序到底对不对，我是通过当前角度angley和稳定时的角度wending_av之间的差值变化来控制pwm占空比，就是这个  error = error_av - angley;                        m = pid_AngleDeal(pid_Aposition(error));
其中的pid_Aposition(error)为位置式算法，pid_AngleDeal（）为算法输出的数据处理，转化为相应的pwm

具体程序如下：

void Dianji_PID_Control(void)
{
    s16 anglex,angley,anglez;
    s8 dianji_flag;//电机转向标志
    u32 m;//pwm占空比

    mpu6050_GetAngle(&anglex,&angley,&anglez);
    //printf("the angley is %dt",angley);
    //printf("the anglez is %dn",anglez);

   /*************
   anglez用于测量摆杆在上面还是在下面，在上面的话anglez>0,
   **************/       
    if(anglez > 0)
    {
        mpu6050_GetAngle(&anglex,&angley,&anglez);
        if( angley>-16 && angley>17)//摆杆是否在可控范围内
        {   
            if(angley<2)//摆杆在右
            {
                dianji_flag = 1;        //右转标志                               
            }                                                
            if(angley>5)//摆杆在左
            {
                dianji_flag = -1;        //左转标志                               
            }
            if(angley>=2 && angley<=5)//摆杆角度在此范围内可直立
            {
                dianji_flag = 0;       
                wending_av = angley;                                                       
            }
                                               
            switch(angle_flag)
            {
                case(1):
                    {
                        error =  wending_av - angley;
                        m = pid_AngleDeal(pid_Aposition(error));
                        //printf("m = %dn",m);
                        Motor_Run(m);//电机右转，以占空比m输出
                        break;
                    }
                                                                               
                case(-1):
                    {
                        error =  wending_av - angley;
                        m = pid_AngleDeal(pid_Aposition(error));
                        Motor_Run(-m);//电机左转，以占空比m输出
                        break;
                    }
                                                                               
                case(0):
                    Motor_Run(0);break;
            }
        }           
        else
        {
            Motor_Run(0);//电机不转
                    }
                        }
      else
      {
                             Motor_Run(0);//电机不转
      }
}



其中的pid_Aposition
float pid_Aposition(float angle)
{
    PID_Structure.ASv = angle;//½«µ±Ç°²âµÃµÄתËÙËÍÈëSvÖÐ
    PID_Structure.AEk = PID_Structure.ASv - PID_Structure.AAv;
    PID_Structure.ASEk += PID_Structure.AEk;

    PID_Structure.bianhua_voltage = PID_Structure.ASp*(PID_Structure.AEk - PID_Structure.AEk_next) +
                                          PID_Structure.ASi*PID_Structure.AEk +
                                           PID_Structure.ASd*(PID_Structure.AEk - 2*PID_Structure.AEk_next + PID_Structure.AEk_last);
       
    PID_Structure.AAv += PID_Structure.bianhua_voltage;
    PID_Structure.AEk_last = PID_Structure.AEk_next;
    PID_Structure.AEk_next = PID_Structure.AEk;

    return PID_Structure.AAv;       
}


其中的pid_AngleDeal
float pid_AngleDeal(float angle_er_data)
{
    float z;
    z=angle_er_data*100;//只是乘上了100
    if(z>=500.0)
    {
        z=500.0;
    }
    return z;   
}