有人让我解释一下我程序里面PID计算的过程,这里就插一节吧,就是介绍下我的PID,当然,还很不完善,后期肯定还要经过很多改动才行,这里就先介绍下现在的"初级版"吧.
void PID_CAL(void) PID计算函数
{
static float thr=0,rool=0,pitch=0,yaw=0; 控制量
static float rool_i=0,pitch_i=0; 积分
int16_t Motor1,Motor2,Motor3,Motor4; 四个电机输出
IMU_TEST(); 计算姿态
GET_EXPRAD(); 获得控制量
rool = PID_RP.P * DIF_ANGLE.X; roll方向的P计算
if(Q_ANGLE.Rool>-0.1 && Q_ANGLE.Rool<0.1) 判断I是否需要清零
{
rool_i = 0;
}
rool_i -= PID_RP.I * Q_ANGLE.Rool; I计算
PID_RP.IMAX = DIF_ANGLE.X * 10; 积分限幅
if(PID_RP.IMAX<0)
{
PID_RP.IMAX = (-PID_RP.IMAX) + 100;
}
else
{
PID_RP.IMAX += 100;
}
if(rool_i>PID_RP.IMAX) rool_i = PID_RP.IMAX;
if(rool_i<-PID_RP.IMAX) rool_i = -PID_RP.IMAX;
rool += rool_i; 积分
rool -= PID_RP.D * GYRO_F.X; D计算
///
pitch = PID_RP.P * DIF_ANGLE.Y; 同上
if(Q_ANGLE.Pitch>-0.1 && Q_ANGLE.Pitch<0.1)
{
pitch_i = 0;
}
pitch_i -= PID_RP.I * Q_ANGLE.Pitch;
if(PID_RP.IMAX<0)
{
PID_RP.IMAX = (-PID_RP.IMAX) + 100;
}
else
{
PID_RP.IMAX += 100;
}
if(PID_RP.IMAX<0) PID_RP.IMAX = -PID_RP.IMAX;
if(pitch_i>PID_RP.IMAX) pitch_i = PID_RP.IMAX;
if(pitch_i<-PID_RP.IMAX) pitch_i = -PID_RP.IMAX;
pitch += pitch_i;
pitch -= PID_RP.D * GYRO_F.Y;
/
GYRO_I[0].Z += EXP_ANGLE.Z/3000; yaw方向就简单的用了陀螺的积分 PD
yaw = -10 * GYRO_I[0].Z;
yaw -= 3 * GYRO_F.Z;
//
thr = RC_DATA.THROTTLE+400;
将控制量输出给电机
Motor1=(int16_t)(thr + rool - pitch + yaw);
Motor2=(int16_t)(thr + rool + pitch - yaw);
Motor3=(int16_t)(thr - rool + pitch + yaw);
Motor4=(int16_t)(thr - rool - pitch - yaw);
if(FLY_ENABLE && (RC_DATA.THROTTLE>-400)) 和解锁有关,未解锁或油门太低电机禁止转动
MOTO_PWMRFLASH(Motor1,Motor2,Motor3,Motor4);
else
MOTO_PWMRFLASH(0,0,0,0);
}
void PID_CAL(void) PID计算函数
{
static float thr=0,rool=0,pitch=0,yaw=0; 控制量
static float rool_i=0,pitch_i=0; 积分
int16_t Motor1,Motor2,Motor3,Motor4; 四个电机输出
IMU_TEST(); 计算姿态
GET_EXPRAD(); 获得控制量
rool = PID_RP.P * DIF_ANGLE.X; roll方向的P计算
if(Q_ANGLE.Rool>-0.1 && Q_ANGLE.Rool<0.1) 判断I是否需要清零
{
rool_i = 0;
}
rool_i -= PID_RP.I * Q_ANGLE.Rool; I计算
PID_RP.IMAX = DIF_ANGLE.X * 10; 积分限幅
if(PID_RP.IMAX<0)
{
PID_RP.IMAX = (-PID_RP.IMAX) + 100;
}
else
{
PID_RP.IMAX += 100;
}
if(rool_i>PID_RP.IMAX) rool_i = PID_RP.IMAX;
if(rool_i<-PID_RP.IMAX) rool_i = -PID_RP.IMAX;
rool += rool_i; 积分
rool -= PID_RP.D * GYRO_F.X; D计算
///
pitch = PID_RP.P * DIF_ANGLE.Y; 同上
if(Q_ANGLE.Pitch>-0.1 && Q_ANGLE.Pitch<0.1)
{
pitch_i = 0;
}
pitch_i -= PID_RP.I * Q_ANGLE.Pitch;
if(PID_RP.IMAX<0)
{
PID_RP.IMAX = (-PID_RP.IMAX) + 100;
}
else
{
PID_RP.IMAX += 100;
}
if(PID_RP.IMAX<0) PID_RP.IMAX = -PID_RP.IMAX;
if(pitch_i>PID_RP.IMAX) pitch_i = PID_RP.IMAX;
if(pitch_i<-PID_RP.IMAX) pitch_i = -PID_RP.IMAX;
pitch += pitch_i;
pitch -= PID_RP.D * GYRO_F.Y;
/
GYRO_I[0].Z += EXP_ANGLE.Z/3000; yaw方向就简单的用了陀螺的积分 PD
yaw = -10 * GYRO_I[0].Z;
yaw -= 3 * GYRO_F.Z;
//
thr = RC_DATA.THROTTLE+400;
将控制量输出给电机
Motor1=(int16_t)(thr + rool - pitch + yaw);
Motor2=(int16_t)(thr + rool + pitch - yaw);
Motor3=(int16_t)(thr - rool + pitch + yaw);
Motor4=(int16_t)(thr - rool - pitch - yaw);
if(FLY_ENABLE && (RC_DATA.THROTTLE>-400)) 和解锁有关,未解锁或油门太低电机禁止转动
MOTO_PWMRFLASH(Motor1,Motor2,Motor3,Motor4);
else
MOTO_PWMRFLASH(0,0,0,0);
}
程序就这么简单,当然还需要很多改进,PID参数也需要慢慢调试才行
转载http://www.anotc.com/Articles/Browse/1