無刷云臺代碼分析
加入技術交流群
// {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;//本文引用地址：http://www.104case.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;//加入加速度計算出的調節系數嗎？
// 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();
}
評論