一起来研究平民化百来块钱的MWC闭环云台控制器吧。。。。

long0001

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


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

long0001

沙发

告别八百上万的云台控制器，大家一起来研究吧。。。。

wmw

日..........这标题...............

kingofpop

被标题骗了。。

困无间

。。。    ...23456..10000

潜心学习

路过，虽然不懂，但强烈支持

scottzijiezhang

看到这个我就觉得还是要好好上编程课的，然后才有机会来贡献。。。

疯狂的孙女

看到这个~算是一个开源项目~不错~顶了~

long0001

顶起，看帖的人回个复，让更多人看到，让有能力的人参与进来，一起来打造我们的低成本闭环控制云台。

刘博

代码是你自己写的？啥叫闭环？不就两个舵机吗？哪里来的控制器？

long0001

刘博 发表于 2013-2-21 16:01
代码是你自己写的？啥叫闭环？不就两个舵机吗？哪里来的控制器？

1、代码是MWC开源程序啊。大家可以一起来修改。
2、个人认为，闭环就是把传感器固定在云台上，反馈姿态进行精确控制。
3、控制两轴云台的话，就是两个舵机。
4、控制器就是MWC飞控板啊。本人的飞控板是海盗，所以可以刷MWC固件。

long0001

如果刷+4四轴模式固件，利用其中一个PITCH、ROLL的输出，去控制数字舵机，不知道可行否？如果把舵机换成双向电调，控制无刷电机，不知道会怎么样？

611055853

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);
这一句在哪 我找不到 有截图没？给我说说 谢谢了

611055853

我的往后仰 补偿的角度不够 应该怎么改？

long0001

611055853 发表于 2013-2-21 21:51
servo[0] = constrain(axisPID  + rcData[TILT_PITCH_AUX_CH], TILT_PITCH_MIN, TILT_PITCH_MAX);
    ser ...

                              

long0001

611055853 发表于 2013-2-21 21:53
我的往后仰 补偿的角度不够 应该怎么改？

你是开环还是闭环？

long0001

如果调整PID还不够补偿的话，只有调整舵角的位置了。。

long0001

横滚、俯仰的舵机舵角位置最好一样长。
比如：横滚舵机转10°，云台也横滚10°，那么俯仰舵机转10°，云台也俯仰10°。
再比如：横滚舵机转10°，云台横滚7°，那么俯仰舵机转10°，云台也俯仰7°。

scyscy

611055853

不知道什么是开环 闭环？那个我找到了 复制你的替换掉原来的么？