关于倒立摆如何通过测得的角度值用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 %d\t",angley);
    //printf("the anglez is %d\n",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 = %d\n",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;//??μ±?°2aμ?μ?×a?ù?íè?Sv?D
    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;   
}

在用到实际的时候电机转的非常不稳定，控制电机的pwm占空比也很不正常，求大神讲解一下，或者谈谈你们对此的好看法，欢迎讨论！

小编发到问答求助版块吧 坛友关注度高

前段时间刚练习了这个题目。我们采用的是增量式编码器作为角度传感器来使用（你这里用mup6050也一样，反正都是为了得到当前的角度值）。这个倒立摆需要双环控制，角度环和电机环，角度环监测当前角度并且实时控制，电机环用的是光电编码器控制电机所控制动臂的位置（后面转圈那一问必须有这一环控制）。双环叠加双环控制。说一下具体实现：通过监测当前角度来控制电机正反转，摆杆往哪偏离电机往那个方向转，以此控制电机来保证摆杆倒立平衡。具体电机控制就用到PID算法了。那么电机环的作用就是既要保证平衡又要使电机所转动达到的位置能控制，比如在平衡的时候摆杆定点平衡，始终在这一点平衡，不会自主转圈。当需要转圈的时候控制电机转动来转圈。这两个环分别独立，然后叠加调试。单环调试相对简单，在调试的时候效果明显，当加上电机环的时候调试相对困难。再说一下我们转圈实现的方法：电机的光电编码器通过正交解码可以得到当前电机是正转还是反转，也可以知道转速。那么假设我们一直更改光电编码器上面的零点，让这个零点循环360度移动，而程序里面让电机一直追随这个零点，那么电机就实现转圈了。相应的速度也可控。（说的有点乱，大体上就是这个样子吧，）说的不好，勿喷，本人也是小小白哈哈。

调节PWM的问题，比如倒立状态的最高点为零点（或者180度点），用当前实际角度值和平衡点的设定值做差，利用差值进行比例，积分，微分三项相加计算出PWM值。

本人没有接触过这方面的知识  感觉坛友讲的不错  学习了

谢谢，学习了