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_tmotorNumber（电机挑选？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 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;//核算时刻差值
//{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++;
}
}
