大家群策群力，一起来研究平民化百来块钱的MWC闭环云台控制器。。。

long0001 · 发表于 2013-2-19 14:30

本帖最后由 long0001 于 2013-2-21 11:56 编辑

crazyboyyy的帖子【抛砖引玉】mwc用于闭环云台(5楼有改进版代码)：http://5imx.3vwed.com/bbs/forum.php?mod=viewthread&tid=725888&extra=page%3D3

根据crazyboyyy帖子里的教程修改代码，
1、开环时速度可以控制的很快，即使用最差的舵机都可以很快，也就是把传感器固定在机架上。
调节ROLL，PITCH，LEVEL的各自PID参数，如果使用好的舵机的话，可以达到云台稳定要求。
需要遥控器介入调整角度就修改代码：
#ifdef GIMBAL
servo[0] = constrain(axisPID[PITCH]  + rcData[TILT_PITCH_AUX_CH], TILT_PITCH_MIN, TILT_PITCH_MAX);
servo[1] = constrain(axisPID[ROLL] + rcData[TILT_ROLL_AUX_CH], TILT_ROLL_MIN, TILT_ROLL_MAX);
使用舵机频率选择
      #define SERVO_RFR_50HZ
   //#define SERVO_RFR_160HZ
//#define SERVO_RFR_300HZ

2、闭环控制时速度就不够快了，也就是把传感器固定在云台上。
闭环时把LEVEL的PID参数调到最大都不够快。而且调节ROLL，PITCH的PID参数会使云台震荡。

另外，如果把飞控刷成#define AIRPLANE固定翼模式，云台舵机插到副翼（有两副翼，任选一个，如果反向就插另一个）和升降输出端，也可以控制云台。

我们的最终目标就是闭环控制，根据姿态的变化量输出控制量，希望有高手能够修改MWC程序的PID控制，利用GUI能够方便的调整PID。

个人认为以下代码就是PID控制代码，水平有限，苦研未果
//**** PITCH & ROLL & YAW PID ****
  int16_t prop;
  prop = max(abs(rcCommand[PITCH]),abs(rcCommand[ROLL])); // range [0;500]

  for(axis=0;axis<3;axis++) {
if ((f.ANGLE_MODE || f.HORIZON_MODE) && axis<2 ) { // MODE relying on ACC
   // 50 degrees max inclination
   errorAngle = constrain(2*rcCommand[axis] + GPS_angle[axis],-500,+500) - angle[axis] + conf.angleTrim[axis]; //16 bits is ok here
   PTermACC = (int32_t)errorAngle*conf.P8[PIDLEVEL]/100;                         // 32 bits is needed for calculation: errorAngle*P8[PIDLEVEL] could exceed 32768 16 bits is ok for result
   PTermACC = constrain(PTermACC,-conf.D8[PIDLEVEL]*5,+conf.D8[PIDLEVEL]*5);
   errorAngleI[axis]    = constrain(errorAngleI[axis]+errorAngle,-10000,+10000); // WindUp    //16 bits is ok here
   ITermACC             = ((int32_t)errorAngleI[axis]*conf.I8[PIDLEVEL])>>12;          // 32 bits is needed for calculation:10000*I8 could exceed 32768 16 bits is ok for result
}
if ( !f.ANGLE_MODE || f.HORIZON_MODE || axis == 2 ) { // MODE relying on GYRO or YAW axis
   if (abs(rcCommand[axis])<350) error =       rcCommand[axis]*10*8/conf.P8[axis] ; // 16 bits is needed for calculation: 350*10*8 = 28000    16 bits is ok for result if P8>2 (P>0.2)
                           else error = (int32_t)rcCommand[axis]*10*8/conf.P8[axis] ; // 32 bits is needed for calculation: 500*5*10*8 = 200000 16 bits is ok for result if P8>2 (P>0.2)
   error -= gyroData[axis];
   PTermGYRO = rcCommand[axis];

   errorGyroI[axis]  = constrain(errorGyroI[axis]+error,-16000,+16000);       // WindUp 16 bits is ok here
   if (abs(gyroData[axis])>640) errorGyroI[axis] = 0;
   ITermGYRO = (errorGyroI[axis]/125*conf.I8[axis])>>6;                                  // 16 bits is ok here 16000/125 = 128 ; 128*250 = 32000
}
if ( f.HORIZON_MODE && axis<2) {
   PTerm = ((int32_t)PTermACC*(500-prop) + (int32_t)PTermGYRO*prop)/500;
   ITerm = ((int32_t)ITermACC*(500-prop) + (int32_t)ITermGYRO*prop)/500;
} else {
   if ( f.ANGLE_MODE && axis<2) {
      PTerm = PTermACC;
      ITerm = ITermACC;
   } else {
      PTerm = PTermGYRO;
      ITerm = ITermGYRO;
   }
}
if (abs(gyroData[axis])<160) PTerm -=       gyroData[axis]*dynP8[axis]/10/8; // 16 bits is needed for calculation 160*200 = 32000       16 bits is ok for result
                        else PTerm -= (int32_t)gyroData[axis]*dynP8[axis]/10/8; // 32 bits is needed for calculation
delta       = gyroData[axis] - lastGyro[axis];                            // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800
lastGyro[axis] = gyroData[axis];
deltaSum    = delta1[axis]+delta2[axis]+delta;
delta2[axis] = delta1[axis];
delta1[axis] = delta;

if (abs(deltaSum)<640) DTerm = (deltaSum*dynD8[axis])>>5;                      // 16 bits is needed for calculation 640*50 = 32000          16 bits is ok for result
                  else DTerm = ((int32_t)deltaSum*dynD8[axis])>>5;             // 32 bits is needed for calculation

axisPID[axis] =  PTerm + ITerm - DTerm;
  }

真心希望大家群策群力，把自己会的贡献出来，造福基层模友。。。