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();
}