無(wú)刷云臺(tái)代碼分析

void loop() 為主程序大循環(huán)
主要功能讀取MPU6050平計(jì)算出相應(yīng)數(shù)據(jù)
2、定時(shí)中斷驅(qū)動(dòng)電機(jī)轉(zhuǎn)動(dòng)

//用這個(gè)程序改多軸飛控一定很穩(wěn)定?？梢杂盟鳛橐粋€(gè)模塊把算出的數(shù)據(jù)發(fā)給KK_C再進(jìn)行控制。
/********************************/
/* Motor Control Routines */
/********************************/
ISR( TIMER1_OVF_vect )
{//定時(shí)中斷嗎？在這里輸出電機(jī)控制信號(hào)嗎？
freqCounter++;
if(freqCounter==(CC_FACTOR/MOTORUPDATE_FREQ))
{//中斷CC_FACTOR/MOTORUPDATE_FREQ次執(zhí)行以下程序 即輸出頻率

// Move pitch and roll Motor
deviderCountPitch++;//這里用計(jì)數(shù)的方式有什么作用喃？pitchDevider越大時(shí)延時(shí)會(huì)越長(zhǎng)，好像不太好。直接執(zhí)行不好嗎？
//if(abs(pitchDevider)>=1) //胥擬改進(jìn) 大于或等于1說(shuō)明有數(shù)據(jù)才進(jìn)行調(diào)整，免得電機(jī)不斷輸出發(fā)熱和抖動(dòng)
if(deviderCountPitch >= abs(pitchDevider)) //abs(pitchDevider)=計(jì)算參數(shù)的絕對(duì)值，即不算符號(hào)，只管數(shù)值
//分析如果pitchDevider=0時(shí)，每次都會(huì)執(zhí)行它，用問(wèn)題嗎？=0時(shí)會(huì)不斷輸出到電機(jī)，有點(diǎn)問(wèn)題哦！
{//Roll電機(jī)
fastMoveMotor(config.motorNumberRoll, rollDirection,pwmSinMotorRoll);
deviderCountRoll=0;
}
freqCounter=0;
}
}
//=======================================
fastMoveMotor電機(jī)驅(qū)動(dòng)子程序

// 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_tmotorNumber（電機(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ī)寫(xiě)個(gè)步進(jìn)電機(jī)驅(qū)動(dòng)，再用兩個(gè)端口進(jìn)行控制。一個(gè)端口控制方向，一個(gè)端口控制步數(shù)
currentStepMotor0 += dirStep;//currentStepMotor0 為原來(lái)的位置 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ì)算電機(jī)輸出數(shù)據(jù)-1，0，1 只轉(zhuǎn)一點(diǎn)點(diǎn)
rollDevider = constrain(maxDegPerSecondRoll / (rollPID + 0.000001), -15000,15000)*2;
rollDirection = sgn(rollDevider) * config.dirMotorRoll;//計(jì)算電機(jī)輸出數(shù)據(jù)-1，0，1

int8_t sgn(int val) {
if (val < 0) return -1;
if (val==0) return 0;
return 1;
}
//以下是主程序分析
//功能分析：除了通過(guò)陀螺儀和加速度儀數(shù)據(jù)運(yùn)算調(diào)整兩個(gè)電機(jī)移動(dòng)以外還加入了外部控制的調(diào)整量
//主程序內(nèi)只算出移動(dòng)數(shù)據(jù)，在中斷中才不斷的進(jìn)行輸出動(dòng)作
/**********************************************/
/* Main Loop */
/**********************************************/
int count=0;
void loop()
{

sampleTimePID = (micros()-timer)/1000000.0/CC_FACTOR; // in Seconds! 檢測(cè)時(shí)間控制設(shè)置 通過(guò)CC_FACTOR調(diào)節(jié)因子調(diào)節(jié)大??？
//得到上次運(yùn)行到本次運(yùn)行的時(shí)間長(zhǎng)短，用于PID算法
timer = micros();//存本次時(shí)間，用于和下次時(shí)間的比較。
//定時(shí)器會(huì)溢出嗎？要進(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í)行頻率不同
//周期長(zhǎng)
count++;
// Update ACCdataapproximately 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;//計(jì)算時(shí)間差值
//{Serial.print(sampleTimeACC,5);Serial.print(" ");Serial.println(sampleTimePID,5);}
mpu.getAcceleration(&x_val,&y_val,&z_val);//讀三軸加速度值中嗎？
}
if(count == 21)
//roll角度控制計(jì)算
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角度控制計(jì)算
pitchAngleACC = 0.9 * pitchAngleACC + 0.1 * -ultraFastAtan2(-x_val,-z_val);//角度計(jì)算嗎？
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)行加速度計(jì)算嗎？
if(count == 2)
{//pitch角度控制計(jì)算
pitchAngleACC = -asin(-2.0*(q.x * q.z - q.w * q.y)) * 180.0/M_PI;//角度計(jì)算嗎？
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角度控制計(jì)算
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);//角度計(jì)算嗎？
count++;
}
if(mpuInterrupt) 判斷MPU 中斷標(biāo)志嗎？
{ //兩個(gè)不同地方的sampleTimeACC有沖突嗎？
sampleTimeACC = (micros()-timerACC)/1000.0/CC_FACTOR; // in Seconds * 1000.0 to account for factor 1000 in parameters
timerACC=timer;//計(jì)算時(shí)間差值通過(guò)CC_FACTOR調(diào)節(jié)因子調(diào)節(jié)大??？
//有中斷產(chǎn)生時(shí)讀取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++;
}
}