無刷云臺代碼分析
加入技術(shù)交流群
// {Serial.print(pitchAngleACC);Serial.print(" ");Serial.println(rollAngleACC);} // 調(diào)試時(shí)往串口發(fā)數(shù)據(jù) AngleACC角度控制數(shù)據(jù)
if(config.rcAbsolute==1) // Absolute RC control 絕對控制
{//方式1？
// Get SetpointfromRC-Channel if available.
// LPF on pitchSetpoint
if(updateRCPitch==true)//手動(dòng)修正判斷嗎？
{
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)//手動(dòng)修正判斷嗎？
{
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)//手動(dòng)修正判斷嗎？
{
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)//手動(dòng)修正判斷嗎？
{
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
//計(jì)算陀螺儀數(shù)據(jù)
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;//本文引用地址：http://www.ex-cimer.com/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;//加入加速度計(jì)算出的調(diào)節(jié)系數(shù)嗎？
// pitchSetpoint=constrain(pitchSetpoint,config.minRCPitch,config.maxRCPitch);
// rollSetpoint=constrain(rollSetpoint,config.minRCRoll,config.maxRCRoll);
//630-1130
//計(jì)算電機(jī)數(shù)據(jù)
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比例調(diào)節(jié)，I積分 預(yù)設(shè)置和反饋值之間的差值在時(shí)間上的累積，累積值大到一定時(shí)才處理，有滯后控制的作用。D微分項(xiàng)調(diào)節(jié)即根據(jù)趨勢作提前量調(diào)整，有提前控制的作用
float error = setPoint - in;//計(jì)算差值，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、比例調(diào)節(jié)算法P：當(dāng)前error*Kp（error為差值，Kp為P值即比例調(diào)節(jié)量，可進(jìn)行人工設(shè)置、基礎(chǔ)的調(diào)整速度只根據(jù)差值大小確定調(diào)整快慢）+2、積分調(diào)節(jié)算法I：總error*Ki*SampleTimeInSecs（差值積分總值*errorSum（是角度值嗎？）*調(diào)節(jié)因子Ki*時(shí)間變量+3、微分D 調(diào)節(jié)，即根據(jù)在一定時(shí)間內(nèi)的變化量來確定調(diào)整效果的快慢來作一個(gè)提前量調(diào)整。調(diào)整因子Kd*變化量(error - *errorOld)/時(shí)間
//D的作用就是在一定時(shí)間內(nèi)判斷差值的變化趨勢。越大就調(diào)的調(diào)的越快。越少調(diào)的越慢。
//I的算法好像有點(diǎn)問題，不像網(wǎng)上說的那樣嗎？
*errorOld = error;// &rollErrorOld=error 存本次的差值，以便和下一次角速度即差值比較
return constrain(out, -maxDegPerSecond ,maxDegPerSecond);//返回限幅后的數(shù)據(jù)out
}
*/
//1250-1700
pitchDevider = constrain(maxDegPerSecondPitch / (pitchPID + 0.000001), -15000,15000)*2;//調(diào)整信號
pitchDirection = sgn(pitchDevider) * config.dirMotorPitch;//計(jì)算電機(jī)輸出數(shù)據(jù)1、0、-1只轉(zhuǎn)動(dòng)一點(diǎn)點(diǎn) 0不轉(zhuǎn)
rollDevider = constrain(maxDegPerSecondRoll / (rollPID + 0.000001), -15000,15000)*2;//2、調(diào)整信號 constrain的作用是限幅，功能為如果maxDegPerSecondRoll / (rollPID + 0.000001)小于-15000則返回-15000，大于15000則返回15000。否則返回原來的值 2、+ 0.000001的作用是為了防止rollPID為0嗎？3、(rollPID + 0.000001)為角度值？不像但和角度相關(guān)
//maxDegPerSecondRoll = MOTORUPDATE_FREQ * 1000.0 / N_SIN / (config.nPolesMotorRoll/2) * 360.0;//轉(zhuǎn)動(dòng)的最大值嗎？
// Initialize Motor Movement （初始化電機(jī)運(yùn)動(dòng)） 最大值？
// 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;//計(jì)算電機(jī)輸出數(shù)據(jù)1、0、-1
//1400-1850
//Serial.println( (micros()-timer)/CC_FACTOR);
sCmd.readSerial();
}
評論