21ic电子技术开发论坛 › 单片机与嵌入式系统 › 新唐MCU › 无刷云台代码分析
返回列表 发新帖我要提问

[技术问答] 无刷云台代码分析

[复制链接]
1127|1
 楼主| ofsummer 发表于 2016-3-28 21:00 | 显示全部楼层 |阅读模式
代码, Motor, MOTO, PWM
1、_044.ino为主程序
void loop() 为主程序大循环
主要功能读取MPU6050平计算出相应数据
2、定时中断驱动电机转动
//用这个程序改多轴飞控一定很稳定。可以用它作为一个模块把算出的数据发给KK_C再进行控制。
复制
  1. /********************************/
    

  2. /* Motor Control Routines       */
    

  3. /********************************/
    

  4. ISR( TIMER1_OVF_vect )
    

  5. {//定时中断吗？在这里输出电机控制信号吗？
    

  6.   freqCounter++;
    

  7.   if(freqCounter==(CC_FACTOR/MOTORUPDATE_FREQ))
    

  8.   {//中断CC_FACTOR/MOTORUPDATE_FREQ次执行以下程序 即输出频率
    


  9. 

  10.     // Move pitch and roll Motor
    

  11.     deviderCountPitch++;//这里用计数的方式有什么作用喃？pitchDevider越大时延时会越长，好像不太好。直接执行不好吗？
    

  12.     //if(abs(pitchDevider)>=1)  //胥拟改进 大于或等于1说明有数据才进行调整，免得电机不断输出发热和抖动
    

  13.     if(deviderCountPitch  >= abs(pitchDevider))  //abs(pitchDevider)=计算参数的绝对值，即不算符号，只管数值
    

  14.        //分析如果pitchDevider=0时，每次都会执行它，用问题吗？=0时会不断输出到电机，有点问题哦！
    

  15.     {//Roll电机
    

  16.       fastMoveMotor(config.motorNumberRoll, rollDirection,pwmSinMotorRoll);
    

  17.       deviderCountRoll=0;
    

  18.     }
    

  19.     freqCounter=0;
    

  20.   }
    

  21. }
    

  22. //=======================================
    

  23. fastMoveMotor电机驱动子程序
    


  24. 

  25. // Hardware Abstraction for Motor connectors,
    

  26. // DO NOT CHANGE UNLES YOU KNOW WHAT YOU ARE DOING !!!
    

  27. #define PWM_A_MOTOR1 OCR2A
    

  28. #define PWM_B_MOTOR1 OCR1B
    

  29. #define PWM_C_MOTOR1 OCR1A
    


  30. 

  31. #define PWM_A_MOTOR0 OCR0A
    

  32. #define PWM_B_MOTOR0 OCR0B
    

  33. #define PWM_C_MOTOR0 OCR2B
    

  34. //以上是引脚定义吗？
    

  35. void fastMoveMotor(uint8_t motorNumber, int dirStep,uint8_t* pwmSin)
    

  36. {//fastMoveMotor(uint8_t motorNumber（电机选择？X轴/Y轴）, int dirStep（正转1、反转-1或不转0）,uint8_t* pwmSin（数据表首地址256字节）)
    

  37.   if (motorNumber == 0)
    

  38.   {//改这里就可以改成步进电机的了。:)
    

  39.     //用单片机写个步进电机驱动，再用两个端口进行控制。一个端口控制方向，一个端口控制步数
    

  40.     currentStepMotor0 += dirStep;//currentStepMotor0 为原来的位置 dirStep=（-1，0，1）
    

  41.     PWM_A_MOTOR0 = pwmSin[currentStepMotor0];//查表输出A
    

  42.     PWM_B_MOTOR0 = pwmSin[(uint8_t)(currentStepMotor0 + 85)];//查表输出B
    

  43.     PWM_C_MOTOR0 = pwmSin[(uint8_t)(currentStepMotor0 + 170)];//查表输出C 总步数85*3=255为一圈
    

  44.   }
    

  45.  
    

  46.   if (motorNumber == 1)
    

  47.   {
    

  48.     currentStepMotor1 += dirStep;
    

  49.     PWM_A_MOTOR1 = pwmSin[currentStepMotor1] ;
    

  50.     PWM_B_MOTOR1 = pwmSin[(uint8_t)(currentStepMotor1 + 85)] ;
    

  51.     PWM_C_MOTOR1 = pwmSin[(uint8_t)(currentStepMotor1 + 170)] ;
    

  52.   }
    

  53. }
    


  54. 

  55. //==================================================
    

  56.   pitchDevider = constrain(maxDegPerSecondPitch / (pitchPID + 0.000001), -15000,15000)*2;
    

  57.   pitchDirection = sgn(pitchDevider) * config.dirMotorPitch;//计算电机输出数据-1，0，1 只转一点点
    

  58.   rollDevider = constrain(maxDegPerSecondRoll / (rollPID + 0.000001), -15000,15000)*2;
    

  59.   rollDirection = sgn(rollDevider) * config.dirMotorRoll;//计算电机输出数据-1，0，1
    


  60. 


  61. 

  62. int8_t sgn(int val) {
    

  63.   if (val < 0) return -1;
    

  64.   if (val==0) return 0;
    

  65.   return 1;
    

  66. }


回复

收藏1 举报

相关帖子
 楼主| ofsummer 发表于 2016-3-28 21:01 | 显示全部楼层
//以下是主程序分析
//功能分析：除了通过陀螺仪和加速度仪数据运算调整两个电机移动以外还加入了外部控制的调整量
//主程序内只算出移动数据，在中断中才不断的进行输出动作
复制
  1. /**********************************************/
    

  2. /* Main Loop                                  */
    

  3. /**********************************************/
    

  4. int count=0;
    

  5. void loop()
    

  6. {
    

  7.  
    


  8. 

  9.   sampleTimePID = (micros()-timer)/1000000.0/CC_FACTOR; // in Seconds!  检测时间控制设置   通过CC_FACTOR调节因子调节大小？
    

  10.    //得到上次运行到本次运行的时间长短，用于PID算法
    

  11.   timer = micros();//存本次时间，用于和下次时间的比较。
    

  12.   //定时器会溢出吗？要进行相应处理吗？大约50天溢出一次，要进行确认！
    

  13.   
    

  14.   // Update raw Gyro //更新陀螺仪数据
    

  15.   updateRawGyroData(&gyroRoll,&gyroPitch);//读取陀螺仪数据
    

  16.    
    

  17.   // Update DMP data approximately at 50Hz to save calculation time.
    


  18. 


  19. 

  20.   if(config.useACC==1)//根据变量控制程序流程
    

  21.   {//流程1 执行频率不同
    

  22.    //周期长
    

  23.     count++;
    

  24.     // Update ACC data approximately at 50Hz to save calculation time.
    

  25.     if(count == 20)
    

  26.     {
    

  27.       sampleTimeACC = (micros()-timerACC)/1000.0/CC_FACTOR; // in Seconds * 1000.0 to account for factor 1000 in parameters
    

  28.       timerACC=timer;//计算时间差值
    

  29.       //{Serial.print(sampleTimeACC,5);Serial.print(" ");Serial.println(sampleTimePID,5);} 
    

  30.       mpu.getAcceleration(&x_val,&y_val,&z_val);//读三轴加速度值中吗？
    

  31.     }
    

  32.     if(count == 21)
    

  33.     //roll角度控制计算
    

  34.     rollAngleACC = 0.9 * rollAngleACC + 0.1 * ultraFastAtan2(-y_val,-z_val); //rollAngleACC = 0.8 * rollAngleACC + atan2(-y_val,-z_val)*57.2957795 * 0.2;
    

  35.     if(count == 22)
    

  36.     {//pitch角度控制计算
    

  37.       pitchAngleACC = 0.9 * pitchAngleACC + 0.1 * -ultraFastAtan2(-x_val,-z_val);//角度计算吗？
    

  38.       count=0;
    

  39.       if(config.accOutput==1){Serial.print(pitchAngleACC);Serial.print(" ACC ");Serial.println(rollAngleACC);}
    

  40. //      {Serial.print(gyroPitch);Serial.print(" ACC G ");Serial.println(gyroRoll);}
    

  41.     }
    

  42.   }
    

  43.   else // Use DMP
    

  44.   {//流程2
    

  45.    //周期短
    

  46.    //不进行加速度计算吗？
    

  47.     if(count == 2)
    

  48.     {//pitch角度控制计算
    

  49.       pitchAngleACC = -asin(-2.0*(q.x * q.z - q.w * q.y)) * 180.0/M_PI;//角度计算吗？
    

  50.       count=0;
    

  51.       if(config.dmpOutput==1){Serial.print(pitchAngleACC);Serial.print(" DMP ");Serial.println(rollAngleACC);}
    

  52. //      {Serial.print(gyroPitch);Serial.print(" DMP G ");Serial.println(gyroRoll);}
    

  53.     }
    

  54.     if(count == 1)
    

  55.     {//roll角度控制计算
    

  56.       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);
    

  57.       rollAngleACC = -1*(sgn(rollAngleACC) * 180.0 - rollAngleACC);//角度计算吗？
    

  58.       count++;
    

  59.     }
    

  60.     if(mpuInterrupt) 判断MPU 中断标志吗？
    

  61.     { //两个不同地方的sampleTimeACC有冲突吗？
    

  62.       sampleTimeACC = (micros()-timerACC)/1000.0/CC_FACTOR; // in Seconds * 1000.0 to account for factor 1000 in parameters
    

  63.       timerACC=timer;//计算时间差值通过CC_FACTOR调节因子调节大小？
    

  64.      //有中断产生时读取MPU6050的相应数据吗？
    

  65.       mpu.getFIFOBytes(fifoBuffer, 18); // I2C 800000L : 1300-1308 micros fo 42 bytes, ~540 micros for 16bytes
    

  66.       mpu.dmpGetQuaternion(&q, fifoBuffer); // I2C 800000L : 64-68 micros
    

  67.       mpuInterrupt = false;
    

  68.       count++;
    

  69.     }
    

  70.   }
    

  71.  
    

  72. //      {Serial.print(pitchAngleACC);Serial.print(" ");Serial.println(rollAngleACC);}   // 调试时往串口发数据 AngleACC角度控制数据
    

  73.  
    

  74.    
    

  75.   if(config.rcAbsolute==1) // Absolute RC control 绝对控制
    

  76.   {//方式1？
    

  77.     // Get Setpoint from RC-Channel if available.
    

  78.     // LPF on pitchSetpoint
    

  79.     if(updateRCPitch==true)//手动修正判断吗？
    

  80.     {
    

  81.       pulseInPWMPitch = constrain(pulseInPWMPitch,MIN_RC,MAX_RC);
    

  82.       pitchSetpoint = 0.025 * (config.minRCPitch + (float)(pulseInPWMPitch - MIN_RC)/(float)(MAX_RC - MIN_RC) * (config.maxRCPitch - config.minRCPitch)) + 0.975 * pitchSetpoint;
    

  83.       updateRCPitch=false;
    

  84.     }
    

  85.     if(updateRCRoll==true)//手动修正判断吗？
    

  86.     {
    

  87.       pulseInPWMRoll = constrain(pulseInPWMRoll,MIN_RC,MAX_RC);
    

  88.       rollSetpoint = 0.025 * (config.minRCRoll + (float)(pulseInPWMRoll - MIN_RC)/(float)(MAX_RC - MIN_RC) * (config.maxRCRoll - config.minRCRoll)) + 0.975 * rollSetpoint;
    

  89.       updateRCRoll=false;
    

  90.     }
    

  91.   }
    

  92.   else // Proportional RC control
    

  93.   {//方式2？
    

  94.     if(updateRCPitch==true)//手动修正判断吗？
    

  95.     {
    

  96.       pulseInPWMPitch = constrain(pulseInPWMPitch,MIN_RC,MAX_RC);
    

  97.       if(pulseInPWMPitch>=MID_RC+RC_DEADBAND)
    

  98.       {
    

  99.         pitchRCSpeed = 0.1 * (float)(pulseInPWMPitch - (MID_RC + RC_DEADBAND))/ (float)(MAX_RC - (MID_RC + RC_DEADBAND)) + 0.9 * pitchRCSpeed;
    

  100.       }
    

  101.       else if(pulseInPWMPitch<=MID_RC-RC_DEADBAND)
    

  102.       {
    

  103.         pitchRCSpeed = -0.1 * (float)((MID_RC - RC_DEADBAND) - pulseInPWMPitch)/ (float)((MID_RC - RC_DEADBAND)-MIN_RC) + 0.9 * pitchRCSpeed;
    

  104.       }
    

  105.       else pitchRCSpeed = 0.0;
    

  106.       updateRCPitch=false;
    

  107.     }
    

  108.     if(updateRCRoll==true)//手动修正判断吗？
    

  109.     {
    

  110.       pulseInPWMRoll = constrain(pulseInPWMRoll,MIN_RC,MAX_RC);
    

  111.       if(pulseInPWMRoll>=MID_RC+RC_DEADBAND)
    

  112.       {
    

  113.         rollRCSpeed = 0.1 * (float)(pulseInPWMRoll - (MID_RC + RC_DEADBAND))/ (float)(MAX_RC - (MID_RC + RC_DEADBAND)) + 0.9 * rollRCSpeed;
    

  114.       }
    

  115.       else if(pulseInPWMRoll<=MID_RC-RC_DEADBAND)
    

  116.       {
    

  117.         rollRCSpeed = -0.1 * (float)((MID_RC - RC_DEADBAND) - pulseInPWMRoll)/ (float)((MID_RC - RC_DEADBAND)-MIN_RC) + 0.9 * rollRCSpeed;
    

  118.       }
    

  119.       else rollRCSpeed = 0.0;
    

  120.       updateRCRoll=false;
    

  121.     }
    

  122.   }
    

  123.  
    

  124.  //480-900
    

  125. //计算陀螺仪数据
    

  126.   if((fabs(rollRCSpeed)>0.0)&& (rollAngleACC<config.maxRCRoll)&& (rollAngleACC>config.minRCRoll))//判断rollAngleACC是否在最大值和最小值之间同时 rollRCSpeed的绝对值>0
    

  127.   {//
    

  128.     gyroRoll = gyroRoll + config.accelWeight * rollRCSpeed * RC_GAIN;//config.accelWeight=15，config.accelWeight * rollRCSpeed * RC_GAIN为特性修正吗？还是？
    

  129.     rollSetpoint = rollAngleACC;
    

  130.   }
    

  131.   else//
    

  132.     gyroRoll = gyroRoll + config.accelWeight * (rollAngleACC - rollSetpoint) /sampleTimeACC;//
    


  133. 

  134.   if((fabs(pitchRCSpeed)>0.0)&&(pitchAngleACC<config.maxRCPitch)&&(pitchAngleACC>config.minRCPitch))
    

  135.   {
    

  136.     gyroPitch = gyroPitch + config.accelWeight * pitchRCSpeed * RC_GAIN;
    

  137.     pitchSetpoint = pitchAngleACC;
    

  138.   }
    

  139.   else
    

  140.     gyroPitch = gyroPitch + config.accelWeight * (pitchAngleACC - pitchSetpoint) /sampleTimeACC;//加入加速度计算出的调节系数吗？
    

  141.      
    


  142. 

  143. //     pitchSetpoint=constrain(pitchSetpoint,config.minRCPitch,config.maxRCPitch);
    

  144. //      rollSetpoint=constrain(rollSetpoint,config.minRCRoll,config.maxRCRoll);
    


  145. 

  146. //630-1130
    

  147. //计算电机数据
    

  148.   pitchPID = ComputePID(sampleTimePID,gyroPitch,0.0, &pitchErrorSum, &pitchErrorOld,config.gyroPitchKp,config.gyroPitchKi,config.gyroPitchKd,maxDegPerSecondPitch);
    

  149.   rollPID = ComputePID(sampleTimePID,gyroRoll,0.0, &rollErrorSum, &rollErrorOld,config.gyroRollKp,config.gyroRollKi,config.gyroRollKd,maxDegPerSecondRoll);
    

  150. //
    

  151. /*
    

  152. float ComputePID(float SampleTimeInSecs, float in, float setPoint, float *errorSum, float *errorOld, float Kp, float Ki, float Kd, float maxDegPerSecond)
    

  153. {
    

  154. //PID算法说明，PID 分为P比例调节，I积分 预设置和反馈值之间的差值在时间上的累积，累积值大到一定时才处理，有滞后控制的作用。D微分项调节即根据趋势作提前量调整，有提前控制的作用
    

  155.   float error = setPoint - in;//计算差值，0.0-gyroRoll
    

  156. //算法分析&rollErrorSum+=（0.0-gyroRoll）然后限幅
    

  157.   // Integrate Errors
    

  158.   *errorSum += error;//积分
    

  159.   *errorSum = constrain(*errorSum, -maxDegPerSecond ,maxDegPerSecond);//限幅
    

  160.  
    

  161.   /*Compute PID Output*/
    

  162. //PID算法代码
    

  163.   float out = (Kp * error + SampleTimeInSecs * Ki * *errorSum + Kd * (error - *errorOld) / (SampleTimeInSecs + 0.000001))/1000.0;
    

  164. //1、比例调节算法P：当前error*Kp（error为差值，Kp为P值即比例调节量，可进行人工设置、基础的调整速度只根据差值大小确定调整快慢）+2、积分调节算法I：总error*Ki*SampleTimeInSecs（差值积分总值*errorSum（是角度值吗？）*调节因子Ki*时间变量+3、微分D 调节，即根据在一定时间内的变化量来确定调整效果的快慢来作一个提前量调整。调整因子Kd*变化量(error - *errorOld)/时间
    

  165. //D的作用就是在一定时间内判断差值的变化趋势。越大就调的调的越快。越少调的越慢。
    

  166. //I的算法好像有点问题，不像网上说的那样吗？
    

  167.   *errorOld = error;// &rollErrorOld=error 存本次的差值，以便和下一次角速度即差值比较
    


  168. 

  169.   return constrain(out, -maxDegPerSecond ,maxDegPerSecond);//返回限幅后的数据out
    

  170. }
    

  171. */
    

  172. //1250-1700
    


  173. 

  174.   pitchDevider = constrain(maxDegPerSecondPitch / (pitchPID + 0.000001), -15000,15000)*2;//调整信号
    

  175.   pitchDirection = sgn(pitchDevider) * config.dirMotorPitch;//计算电机输出数据1、0、-1只转动一点点 0不转
    

  176.   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)为角度值？不像但和角度相关
    

  177.         //maxDegPerSecondRoll = MOTORUPDATE_FREQ * 1000.0 / N_SIN / (config.nPolesMotorRoll/2) * 360.0;//转动的最大值吗？
    

  178.   
    

  179.   // Initialize Motor Movement （初始化 电机 运动） 最大值？
    

  180.  // maxDegPerSecondPitch = MOTORUPDATE_FREQ * 1000.0 / N_SIN / (config.nPolesMotorPitch/2) * 360.0;
    

  181.   //maxDegPerSecondRoll = MOTORUPDATE_FREQ * 1000.0 / N_SIN / (config.nPolesMotorRoll/2) * 360.0;
    


  182. 

  183.   rollDirection = sgn(rollDevider) * config.dirMotorRoll;//计算电机输出数据1、0、-1
    

  184. //1400-1850
    


  185. 

  186.  
    


  187. 

  188. //Serial.println( (micros()-timer)/CC_FACTOR);
    

  189.   sCmd.readSerial();
    


  190. 

  191. }


回复

收藏1 举报

返回列表 发新帖
高级模式
B Color Image Link Quote Code Smilies
您需要登录后才可以回帖 登录 | 注册

本版积分规则

ofsummer

10

主题

71

帖子

1

粉丝
快速回复 在线客服 返回列表 返回顶部