STM32

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

2019-07-14 19:23发布

站内问答 / STM32/STM8
6784 6 968
这是我自己写的程序，关于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 ",angley);
    //printf("the anglez is %d ",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 ",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;   
}
友情提示: 此问题已得到解决，问题已经关闭,关闭后问题禁止继续编辑，回答。
6条回答
皮卡丘zl
2019-07-15 09:50
jhw4530037 发表于 2017-8-1 16:41
当时你们如何将角度值转化为适当的pwm输出

传感器会得到一个实测值，通过PD算法对实测值与预定值进行处理。
其实大同小异，就是调kp和kd

一周热门 更多>