经过几个星期的修改代码,单轴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]);
下一步计划,计划做成四轴的形式,期待早日飞机起飞悬空