无刷云台代码分析
加入技术交流群
// {Serial.print(pitchAngleACC);Serial.print(" ");Serial.println(rollAngleACC);} // 调试时往串口发数据 AngleACC角度控制数据
if(config.rcAbsolute==1) // Absolute RC control 绝对控制
{//方式1？
// Get SetpointfromRC-Channel if available.
// LPF on pitchSetpoint
if(updateRCPitch==true)//手动修正判断吗？
{
pulseInPWMPitch = constrain(pulseInPWMPitch,MIN_RC,MAX_RC);
pitchSetpoint = 0.025 * (config.minRCPitch + (float)(pulseInPWMPitch - MIN_RC)/(float)(MAX_RC - MIN_RC) * (config.maxRCPitch - config.minRCPitch)) + 0.975 * pitchSetpoint;
updateRCPitch=false;
}
if(updateRCRoll==true)//手动修正判断吗？
{
pulseInPWMRoll = constrain(pulseInPWMRoll,MIN_RC,MAX_RC);
rollSetpoint = 0.025 * (config.minRCRoll + (float)(pulseInPWMRoll - MIN_RC)/(float)(MAX_RC - MIN_RC) * (config.maxRCRoll - config.minRCRoll)) + 0.975 * rollSetpoint;
updateRCRoll=false;
}
}
else // Proportional RC control
{//方式2？
if(updateRCPitch==true)//手动修正判断吗？
{
pulseInPWMPitch = constrain(pulseInPWMPitch,MIN_RC,MAX_RC);
if(pulseInPWMPitch>=MID_RC+RC_DEADBAND)
{
pitchRCSpeed = 0.1 * (float)(pulseInPWMPitch - (MID_RC + RC_DEADBAND))/ (float)(MAX_RC - (MID_RC + RC_DEADBAND)) + 0.9 * pitchRCSpeed;
}
else if(pulseInPWMPitch<=MID_RC-RC_DEADBAND)
{
pitchRCSpeed = -0.1 * (float)((MID_RC - RC_DEADBAND) - pulseInPWMPitch)/ (float)((MID_RC - RC_DEADBAND)-MIN_RC) + 0.9 * pitchRCSpeed;
}
else pitchRCSpeed = 0.0;
updateRCPitch=false;
}
if(updateRCRoll==true)//手动修正判断吗？
{
pulseInPWMRoll = constrain(pulseInPWMRoll,MIN_RC,MAX_RC);
if(pulseInPWMRoll>=MID_RC+RC_DEADBAND)
{
rollRCSpeed = 0.1 * (float)(pulseInPWMRoll - (MID_RC + RC_DEADBAND))/ (float)(MAX_RC - (MID_RC + RC_DEADBAND)) + 0.9 * rollRCSpeed;
}
else if(pulseInPWMRoll<=MID_RC-RC_DEADBAND)
{
rollRCSpeed = -0.1 * (float)((MID_RC - RC_DEADBAND) - pulseInPWMRoll)/ (float)((MID_RC - RC_DEADBAND)-MIN_RC) + 0.9 * rollRCSpeed;
}
else rollRCSpeed = 0.0;
updateRCRoll=false;
}
}
//480-900
//计算陀螺仪数据
if((fabs(rollRCSpeed)>0.0)&& (rollAngleACC
{//
gyroRoll = gyroRoll + config.accelWeight * rollRCSpeed * RC_GAIN;//config.accelWeight=15，config.accelWeight * rollRCSpeed * RC_GAIN为特性修正吗？还是？
rollSetpoint = rollAngleACC;
}
else//
gyroRoll = gyroRoll + config.accelWeight * (rollAngleACC - rollSetpoint) /sampleTimeACC;//本文引用地址：https://www.eepw.com.cn/article/201611/323783.htm
if((fabs(pitchRCSpeed)>0.0)&&(pitchAngleACC
{
gyroPitch = gyroPitch + config.accelWeight * pitchRCSpeed * RC_GAIN;
pitchSetpoint = pitchAngleACC;
}
else
gyroPitch = gyroPitch + config.accelWeight * (pitchAngleACC - pitchSetpoint) /sampleTimeACC;//加入加速度计算出的调节系数吗？
// pitchSetpoint=constrain(pitchSetpoint,config.minRCPitch,config.maxRCPitch);
// rollSetpoint=constrain(rollSetpoint,config.minRCRoll,config.maxRCRoll);
//630-1130
//计算电机数据
pitchPID = ComputePID(sampleTimePID,gyroPitch,0.0, &pitchErrorSum, &pitchErrorOld,config.gyroPitchKp,config.gyroPitchKi,config.gyroPitchKd,maxDegPerSecondPitch);
rollPID = ComputePID(sampleTimePID,gyroRoll,0.0, &rollErrorSum, &rollErrorOld,config.gyroRollKp,config.gyroRollKi,config.gyroRollKd,maxDegPerSecondRoll);
//
/*
float ComputePID(float SampleTimeInSecs, float in, float setPoint, float *errorSum, float *errorOld, float Kp, float Ki, float Kd, float maxDegPerSecond)
{
//PID算法说明，PID 分为P比例调节，I积分 预设置和反馈值之间的差值在时间上的累积，累积值大到一定时才处理，有滞后控制的作用。D微分项调节即根据趋势作提前量调整，有提前控制的作用
float error = setPoint - in;//计算差值，0.0-gyroRoll
//算法分析&rollErrorSum+=（0.0-gyroRoll）然后限幅
// Integrate Errors
*errorSum += error;//积分
*errorSum = constrain(*errorSum, -maxDegPerSecond ,maxDegPerSecond);//限幅
/*Compute PID Output*/
//PID算法代码
float out = (Kp * error + SampleTimeInSecs * Ki * *errorSum + Kd * (error - *errorOld) / (SampleTimeInSecs + 0.000001))/1000.0;
//1、比例调节算法P：当前error*Kp（error为差值，Kp为P值即比例调节量，可进行人工设置、基础的调整速度只根据差值大小确定调整快慢）+2、积分调节算法I：总error*Ki*SampleTimeInSecs（差值积分总值*errorSum（是角度值吗？）*调节因子Ki*时间变量+3、微分D 调节，即根据在一定时间内的变化量来确定调整效果的快慢来作一个提前量调整。调整因子Kd*变化量(error - *errorOld)/时间
//D的作用就是在一定时间内判断差值的变化趋势。越大就调的调的越快。越少调的越慢。
//I的算法好像有点问题，不像网上说的那样吗？
*errorOld = error;// &rollErrorOld=error 存本次的差值，以便和下一次角速度即差值比较
return constrain(out, -maxDegPerSecond ,maxDegPerSecond);//返回限幅后的数据out
}
*/
//1250-1700
pitchDevider = constrain(maxDegPerSecondPitch / (pitchPID + 0.000001), -15000,15000)*2;//调整信号
pitchDirection = sgn(pitchDevider) * config.dirMotorPitch;//计算电机输出数据1、0、-1只转动一点点 0不转
rollDevider = constrain(maxDegPerSecondRoll / (rollPID + 0.000001), -15000,15000)*2;//2、调整信号 constrain的作用是限幅，功能为如果maxDegPerSecondRoll / (rollPID + 0.000001)小于-15000则返回-15000，大于15000则返回15000。否则返回原来的值 2、+ 0.000001的作用是为了防止rollPID为0吗？3、(rollPID + 0.000001)为角度值？不像但和角度相关
//maxDegPerSecondRoll = MOTORUPDATE_FREQ * 1000.0 / N_SIN / (config.nPolesMotorRoll/2) * 360.0;//转动的最大值吗？
// Initialize Motor Movement （初始化电机运动） 最大值？
// maxDegPerSecondPitch = MOTORUPDATE_FREQ * 1000.0 / N_SIN / (config.nPolesMotorPitch/2) * 360.0;
//maxDegPerSecondRoll = MOTORUPDATE_FREQ * 1000.0 / N_SIN / (config.nPolesMotorRoll/2) * 360.0;
rollDirection = sgn(rollDevider) * config.dirMotorRoll;//计算电机输出数据1、0、-1
//1400-1850
//Serial.println( (micros()-timer)/CC_FACTOR);
sCmd.readSerial();
}
评论