这是我自己写的程序,关于pid将角度和电机控制相结合,角度测量我用的是mpu6050,电机为普通的直流电机。不知道我这样写程序到底对不对,我是通过当前角度angley和稳定时的角度wending_av之间的差值变化来控制pwm占空比,就是这个 error = error_av - angley; m = pid_AngleDeal(pid_Aposi
tion(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;
}
2