1、_044.ino為主程序
void loop() 為主程序大循環
主要功能讀取MPU6050平計算出相應數據
2、定時中斷驅動電機轉動
//用這個程序改多軸飛控一定很穩定。可以用它作為一個模塊把算出的數據發給KK_C再進行控制。
/********************************/
/* Motor Control Routines */
/********************************/
ISR( TIMER1_OVF_vect )
{//定時中斷嗎？在這里輸出電機控制信號嗎？
freqCounter++;
if(freqCounter==(CC_FACTOR/MOTORUPDATE_FREQ))
{//中斷CC_FACTOR/MOTORUPDATE_FREQ次執行以下程序 即輸出頻率
// Move pitch and roll Motor
deviderCountPitch++;//這里用計數的方式有什么作用喃？pitchDevider越大時延時會越長，好像不太好。直接執行不好嗎？
//if(abs(pitchDevider)>=1) //胥擬改進 大于或等于1說明有數據才進行調整，免得電機不斷輸出發熱和抖動
if(deviderCountPitch >= abs(pitchDevider)) //abs(pitchDevider)=計算參數的絕對值，即不算符號，只管數值
//分析如果pitchDevider=0時，每次都會執行它，用問題嗎？=0時會不斷輸出到電機，有點問題哦！
{//Roll電機
fastMoveMotor(config.motorNumberRoll, rollDirection,pwmSinMotorRoll);
deviderCountRoll=0;
}
freqCounter=0;
}
}
//=======================================
fastMoveMotor電機驅動子程序
// 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（電機選擇？X軸/Y軸）, int dirStep（正轉1、反轉-1或不轉0）,uint8_t* pwmSin（數據表首地址256字節）)
if (motorNumber == 0)
{//改這里就可以改成步進電機的了。:)
//用單片機寫個步進電機驅動，再用兩個端口進行控制。一個端口控制方向，一個端口控制步數
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 總步數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;//計算電機輸出數據-1，0，1 只轉一點點
rollDevider = constrain(maxDegPerSecondRoll / (rollPID + 0.000001), -15000,15000)*2;
rollDirection = sgn(rollDevider) * config.dirMotorRoll;//計算電機輸出數據-1，0，1
int8_t sgn(int val) {
if (val < 0) return -1;
if (val==0) return 0;
return 1;
}
//以下是主程序分析
//功能分析：除了通過陀螺儀和加速度儀數據運算調整兩個電機移動以外還加入了外部控制的調整量
//主程序內只算出移動數據，在中斷中才不斷的進行輸出動作
/**********************************************/
/* Main Loop */
/**********************************************/
int count=0;
void loop()
{
sampleTimePID = (micros()-timer)/1000000.0/CC_FACTOR; // in Seconds! 檢測時間控制設置 通過CC_FACTOR調節因子調節大小？
//得到上次運行到本次運行的時間長短，用于PID算法
timer = micros();//存本次時間，用于和下次時間的比較。
//定時器會溢出嗎？要進行相應處理嗎？大約50天溢出一次，要進行確認！
// Update raw Gyro //更新陀螺儀數據
updateRawGyroData(&gyroRoll,&gyroPitch);//讀取陀螺儀數據
// Update DMP data approximately at 50Hz to save calculation time.
if(config.useACC==1)//根據變量控制程序流程
{//流程1 執行頻率不同
//周期長
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
//周期短
//不進行加速度計算嗎？
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 中斷標志嗎？
{ //兩個不同地方的sampleTimeACC有沖突嗎？
sampleTimeACC = (micros()-timerACC)/1000.0/CC_FACTOR; // in Seconds * 1000.0 to account for factor 1000 in parameters
timerACC=timer;//計算時間差值通過CC_FACTOR調節因子調節大小？
//有中斷產生時讀取MPU6050的相應數據嗎？
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);} // 調試時往串口發數據 AngleACC角度控制數據
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
//計算陀螺儀數據
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;//加入加速度計算出的調節系數嗎？
// 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();
}