1、_044.ino為主程序
void loop() 為主程序大循環(huán)
主要功能讀取MPU6050平計算出相應(yīng)數(shù)據(jù)
2、定時中斷驅(qū)動電機(jī)轉(zhuǎn)動
//用這個程序改多軸飛控一定很穩(wěn)定�？梢杂盟鳛橐粋€模塊把算出的數(shù)據(jù)發(fā)給KK_C再進(jìn)行控制。
/********************************/
/* Motor Control Routines */
/********************************/
ISR( TIMER1_OVF_vect )
{//定時中斷嗎？在這里輸出電機(jī)控制信號嗎？
freqCounter++;
if(freqCounter==(CC_FACTOR/MOTORUPDATE_FREQ))
{//中斷CC_FACTOR/MOTORUPDATE_FREQ次執(zhí)行以下程序 即輸出頻率
// Move pitch and roll Motor
deviderCountPitch++;//這里用計數(shù)的方式有什么作用喃？pitchDevider越大時延時會越長，好像不太好。直接執(zhí)行不好嗎？
//if(abs(pitchDevider)>=1) //胥擬改進(jìn) 大于或等于1說明有數(shù)據(jù)才進(jìn)行調(diào)整，免得電機(jī)不斷輸出發(fā)熱和抖動
if(deviderCountPitch >= abs(pitchDevider)) //abs(pitchDevider)=計算參數(shù)的絕對值，即不算符號，只管數(shù)值
//分析如果pitchDevider=0時，每次都會執(zhí)行它，用問題嗎？=0時會不斷輸出到電機(jī)，有點問題哦！
{//Roll電機(jī)
fastMoveMotor(config.motorNumberRoll, rollDirection,pwmSinMotorRoll);
deviderCountRoll=0;
}
freqCounter=0;
}
}
//=======================================
fastMoveMotor電機(jī)驅(qū)動子程序
// Hardware Abstraction for Motor connectors,
// DO NOT CHANGE UNLES YOU KNOW WHAT YOU ARE DOING !!!
#define PWM_A_MOTOR1 OCR2A
#define PWM_B_MOTOR1 OCR1B
#define PWM_C_MOTOR1 OCR1A
#define PWM_A_MOTOR0 OCR0A
#define PWM_B_MOTOR0 OCR0B
#define PWM_C_MOTOR0 OCR2B
//以上是引腳定義嗎？
void fastMoveMotor(uint8_t motorNumber, int dirStep,uint8_t* pwmSin)
{//fastMoveMotor(uint8_t motorNumber（電機(jī)選擇？X軸/Y軸）, int dirStep（正轉(zhuǎn)1、反轉(zhuǎn)-1或不轉(zhuǎn)0）,uint8_t* pwmSin（數(shù)據(jù)表首地址256字節(jié)）)
if (motorNumber == 0)
{//改這里就可以改成步進(jìn)電機(jī)的了。:)
//用單片機(jī)寫個步進(jìn)電機(jī)驅(qū)動，再用兩個端口進(jìn)行控制。一個端口控制方向，一個端口控制步數(shù)
currentStepMotor0 += dirStep;//currentStepMotor0 為原來的位置 dirStep=（-1，0，1）
PWM_A_MOTOR0 = pwmSin[currentStepMotor0];//查表輸出A
PWM_B_MOTOR0 = pwmSin[(uint8_t)(currentStepMotor0 + 85)];//查表輸出B
PWM_C_MOTOR0 = pwmSin[(uint8_t)(currentStepMotor0 + 170)];//查表輸出C 總步數(shù)85*3=255為一圈
}
if (motorNumber == 1)
{
currentStepMotor1 += dirStep;
PWM_A_MOTOR1 = pwmSin[currentStepMotor1] ;
PWM_B_MOTOR1 = pwmSin[(uint8_t)(currentStepMotor1 + 85)] ;
PWM_C_MOTOR1 = pwmSin[(uint8_t)(currentStepMotor1 + 170)] ;
}
}
//==================================================
pitchDevider = constrain(maxDegPerSecondPitch / (pitchPID + 0.000001), -15000,15000)*2;
pitchDirection = sgn(pitchDevider) * config.dirMotorPitch;//計算電機(jī)輸出數(shù)據(jù)-1，0，1 只轉(zhuǎn)一點點
rollDevider = constrain(maxDegPerSecondRoll / (rollPID + 0.000001), -15000,15000)*2;
rollDirection = sgn(rollDevider) * config.dirMotorRoll;//計算電機(jī)輸出數(shù)據(jù)-1，0，1
int8_t sgn(int val) {
if (val < 0) return -1;
if (val==0) return 0;
return 1;
}
//以下是主程序分析
//功能分析：除了通過陀螺儀和加速度儀數(shù)據(jù)運(yùn)算調(diào)整兩個電機(jī)移動以外還加入了外部控制的調(diào)整量
//主程序內(nèi)只算出移動數(shù)據(jù)，在中斷中才不斷的進(jìn)行輸出動作
/**********************************************/
/* Main Loop */
/**********************************************/
int count=0;
void loop()
{
sampleTimePID = (micros()-timer)/1000000.0/CC_FACTOR; // in Seconds! 檢測時間控制設(shè)置 通過CC_FACTOR調(diào)節(jié)因子調(diào)節(jié)大�。�
//得到上次運(yùn)行到本次運(yùn)行的時間長短，用于PID算法
timer = micros();//存本次時間，用于和下次時間的比較。
//定時器會溢出嗎？要進(jìn)行相應(yīng)處理嗎？大約50天溢出一次，要進(jìn)行確認(rèn)！
// Update raw Gyro //更新陀螺儀數(shù)據(jù)
updateRawGyroData(&gyroRoll,&gyroPitch);//讀取陀螺儀數(shù)據(jù)
// Update DMP data approximately at 50Hz to save calculation time.
if(config.useACC==1)//根據(jù)變量控制程序流程
{//流程1 執(zhí)行頻率不同
//周期長
count++;
// Update ACC data approximately at 50Hz to save calculation time.
if(count == 20)
{
sampleTimeACC = (micros()-timerACC)/1000.0/CC_FACTOR; // in Seconds * 1000.0 to account for factor 1000 in parameters
timerACC=timer;//計算時間差值
//{Serial.print(sampleTimeACC,5);Serial.print(" ");Serial.println(sampleTimePID,5);}
mpu.getAcceleration(&x_val,&y_val,&z_val);//讀三軸加速度值中嗎？
}
if(count == 21)
//roll角度控制計算
rollAngleACC = 0.9 * rollAngleACC + 0.1 * ultraFastAtan2(-y_val,-z_val); //rollAngleACC = 0.8 * rollAngleACC + atan2(-y_val,-z_val)*57.2957795 * 0.2;
if(count == 22)
{//pitch角度控制計算
pitchAngleACC = 0.9 * pitchAngleACC + 0.1 * -ultraFastAtan2(-x_val,-z_val);//角度計算嗎？
count=0;
if(config.accOutput==1){Serial.print(pitchAngleACC);Serial.print(" ACC ");Serial.println(rollAngleACC);}
// {Serial.print(gyroPitch);Serial.print(" ACC G ");Serial.println(gyroRoll);}
}
}
else // Use DMP
{//流程2
//周期短
//不進(jìn)行加速度計算嗎？
if(count == 2)
{//pitch角度控制計算
pitchAngleACC = -asin(-2.0*(q.x * q.z - q.w * q.y)) * 180.0/M_PI;//角度計算嗎？
count=0;
if(config.dmpOutput==1){Serial.print(pitchAngleACC);Serial.print(" DMP ");Serial.println(rollAngleACC);}
// {Serial.print(gyroPitch);Serial.print(" DMP G ");Serial.println(gyroRoll);}
}
if(count == 1)
{//roll角度控制計算
rollAngleACC = ultraFastAtan2(2.0*(q.y * q.z + q.w * q.x), q.w * q.w - q.x * q.x - q.y * q.y + q.z * q.z);
rollAngleACC = -1*(sgn(rollAngleACC) * 180.0 - rollAngleACC);//角度計算嗎？
count++;
}
if(mpuInterrupt) 判斷MPU 中斷標(biāo)志嗎？
{ //兩個不同地方的sampleTimeACC有沖突嗎？
sampleTimeACC = (micros()-timerACC)/1000.0/CC_FACTOR; // in Seconds * 1000.0 to account for factor 1000 in parameters
timerACC=timer;//計算時間差值通過CC_FACTOR調(diào)節(jié)因子調(diào)節(jié)大小？
//有中斷產(chǎn)生時讀取MPU6050的相應(yīng)數(shù)據(jù)嗎？
mpu.getFIFOBytes(fifoBuffer, 18); // I2C 800000L : 1300-1308 micros fo 42 bytes, ~540 micros for 16bytes
mpu.dmpGetQuaternion(&q, fifoBuffer); // I2C 800000L : 64-68 micros
mpuInterrupt = false;
count++;
}
}
// {Serial.print(pitchAngleACC);Serial.print(" ");Serial.println(rollAngleACC);} // 調(diào)試時往串口發(fā)數(shù)據(jù) AngleACC角度控制數(shù)據(jù)
if(config.rcAbsolute==1) // Absolute RC control 絕對控制
{//方式1？
// Get Setpoint from RC-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
//計算陀螺儀數(shù)據(jù)
if((fabs(rollRCSpeed)>0.0)&& (rollAngleACC<config.maxRCRoll)&& (rollAngleACC>config.minRCRoll))//判斷rollAngleACC是否在最大值和最小值之間同時 rollRCSpeed的絕對值>0
{//
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;//
if((fabs(pitchRCSpeed)>0.0)&&(pitchAngleACC<config.maxRCPitch)&&(pitchAngleACC>config.minRCPitch))
{
gyroPitch = gyroPitch + config.accelWeight * pitchRCSpeed * RC_GAIN;
pitchSetpoint = pitchAngleACC;
}
else
gyroPitch = gyroPitch + config.accelWeight * (pitchAngleACC - pitchSetpoint) /sampleTimeACC;//加入加速度計算出的調(diào)節(jié)系數(shù)嗎？
// pitchSetpoint=constrain(pitchSetpoint,config.minRCPitch,config.maxRCPitch);
// rollSetpoint=constrain(rollSetpoint,config.minRCRoll,config.maxRCRoll);
//630-1130
//計算電機(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è)置和反饋值之間的差值在時間上的累積，累積值大到一定時才處理，有滯后控制的作用。D微分項調(diào)節(jié)即根據(jù)趨勢作提前量調(diào)整，有提前控制的作用
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、比例調(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*時間變量+3、微分D 調(diào)節(jié)，即根據(jù)在一定時間內(nèi)的變化量來確定調(diào)整效果的快慢來作一個提前量調(diào)整。調(diào)整因子Kd*變化量(error - *errorOld)/時間
//D的作用就是在一定時間內(nèi)判斷差值的變化趨勢。越大就調(diào)的調(diào)的越快。越少調(diào)的越慢。
//I的算法好像有點問題，不像網(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ī)輸出數(shù)據(jù)1、0、-1只轉(zhuǎ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)動的最大值嗎？
// Initialize Motor Movement （初始化 電機(jī) 運(yùn)動） 最大值？
// 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ī)輸出數(shù)據(jù)1、0、-1
//1400-1850
//Serial.println( (micros()-timer)/CC_FACTOR);
sCmd.readSerial();
}