5iMX.com 我爱模型 玩家论坛 ——专业遥控模型和无人机玩家论坛（玩模型就上我爱模型，创始于2003年）

标题: 一起来研究平民化百来块钱的MWC闭环云台控制器吧。。。。 [打印本页]
作者: long0001    时间: 2013-2-19 14:36
标题: 一起来研究平民化百来块钱的MWC闭环云台控制器吧。。。。
本帖最后由 long0001 于 2013-2-21 11:52 编辑

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    时间: 2013-2-19 14:40
沙发

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

作者: wmw    时间: 2013-2-19 17:46
日..........这标题...............
作者: kingofpop    时间: 2013-2-19 18:30
被标题骗了。。
作者: 困无间    时间: 2013-2-19 21:15
。。。    ...23456..10000
作者: 潜心学习    时间: 2013-2-19 21:51
路过，虽然不懂，但强烈支持
作者: scottzijiezhang    时间: 2013-2-19 22:17
看到这个我就觉得还是要好好上编程课的，然后才有机会来贡献。。。
作者: 疯狂的孙女    时间: 2013-2-20 22:55
看到这个~算是一个开源项目~不错~顶了~
作者: long0001    时间: 2013-2-21 13:46
顶起，看帖的人回个复，让更多人看到，让有能力的人参与进来，一起来打造我们的低成本闭环控制云台。
作者: 刘博    时间: 2013-2-21 16:01
代码是你自己写的？啥叫闭环？不就两个舵机吗？哪里来的控制器？
作者: long0001    时间: 2013-2-21 16:14
本帖最后由 long0001 于 2013-2-21 19:11 编辑
刘博 发表于 2013-2-21 16:01
代码是你自己写的？啥叫闭环？不就两个舵机吗？哪里来的控制器？


1、代码是MWC开源程序啊。大家可以一起来修改。
2、个人认为，闭环就是把传感器固定在云台上，反馈姿态进行精确控制。
3、控制两轴云台的话，就是两个舵机。
4、控制器就是MWC飞控板啊。本人的飞控板是海盗，所以可以刷MWC固件。
作者: long0001    时间: 2013-2-21 16:20
本帖最后由 long0001 于 2013-2-21 21:02 编辑

如果刷+4四轴模式固件，利用其中一个PITCH、ROLL的输出，去控制数字舵机，不知道可行否？如果把舵机换成双向电调，控制无刷电机，不知道会怎么样？
作者: 611055853    时间: 2013-2-21 21:51
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    时间: 2013-2-21 21:53
我的往后仰 补偿的角度不够 应该怎么改？
作者: long0001    时间: 2013-2-21 21:57
611055853 发表于 2013-2-21 21:51
servo[0] = constrain(axisPID  + rcData[TILT_PITCH_AUX_CH], TILT_PITCH_MIN, TILT_PITCH_MAX);
    ser ...

(, 下载次数: 0)                               
作者: long0001    时间: 2013-2-21 21:58
611055853 发表于 2013-2-21 21:53
我的往后仰 补偿的角度不够 应该怎么改？

你是开环还是闭环？
作者: long0001    时间: 2013-2-21 22:00
本帖最后由 long0001 于 2013-2-21 22:01 编辑

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

作者: long0001    时间: 2013-2-21 22:07
横滚、俯仰的舵机舵角位置最好一样长。
比如：横滚舵机转10°，云台也横滚10°，那么俯仰舵机转10°，云台也俯仰10°。
再比如：横滚舵机转10°，云台横滚7°，那么俯仰舵机转10°，云台也俯仰7°。
作者: scyscy    时间: 2013-2-21 22:16
作者: 611055853    时间: 2013-2-21 23:00
不知道什么是开环 闭环？那个我找到了 复制你的替换掉原来的么？
作者: 611055853    时间: 2013-2-21 23:02
我自己做的建议云台 只接了 俯仰 往一个方向补偿的 很少
舵角 舵量都改了微调也调了
作者: 611055853    时间: 2013-2-21 23:04
替换这里？  #ifdef GIMBAL
    servo[0] = constrain(TILT_PITCH_MIDDLE + TILT_PITCH_PROP * angle[PITCH] /16 + rcCommand[PITCH], TILT_PITCH_MIN, TILT_PITCH_MAX);
    servo[1] = constrain(TILT_ROLL_MIDDLE + TILT_ROLL_PROP   * angle[ROLL]  /16 + rcCommand[ROLL], TILT_ROLL_MIN, TILT_ROLL_MAX);
作者: 611055853    时间: 2013-2-21 23:04
替换这里？  #ifdef GIMBAL
    servo[0] = constrain(TILT_PITCH_MIDDLE + TILT_PITCH_PROP * angle[PITCH] /16 + rcCommand[PITCH], TILT_PITCH_MIN, TILT_PITCH_MAX);
    servo[1] = constrain(TILT_ROLL_MIDDLE + TILT_ROLL_PROP   * angle[ROLL]  /16 + rcCommand[ROLL], TILT_ROLL_MIN, TILT_ROLL_MAX);
作者: long0001    时间: 2013-2-21 23:13
是的，把这两行注释掉，添加那两行新代码上去。
作者: long0001    时间: 2013-2-21 23:20
611055853 发表于 2013-2-21 23:00
不知道什么是开环 闭环？那个我找到了 复制你的替换掉原来的么？

目前只有开环可以调得好，所谓开环，就是把你的飞控板固定在机架上。


作者: long0001    时间: 2013-2-21 23:36
刷好固件，连接电脑，打开调参软件，所有参数置0，点选ANGLE自稳模式，LEVEL的D值加到100，P值加到10，转动机架，如果云台补偿不够就继续加大P，补偿过度就减小P，调好P后，就可以调ROLL、PITCH的P了，调到速度足够快，最后慢慢加上D，看，不管你怎么转，转多快，云台都能跟上你的速度。
作者: long0001    时间: 2013-2-21 23:46
611055853 发表于 2013-2-21 23:02
我自己做的建议云台 只接了 俯仰 往一个方向补偿的 很少
舵角 舵量都改了微调也调了

先不要接遥控接收机，调好云台机械中位，调好PID参数，再接遥控，控的中位点设在1500，设不了就改程序代码，把程序中位点改成你的控的中位点。
作者: 611055853    时间: 2013-2-22 06:26
闭环就是飞控板只做为云台用？
作者: long0001    时间: 2013-2-22 07:50
现在讨论的都是飞控只作为云台用，闭环就是把传感器固定在云台上，无论你怎么转机架，传感器反馈的是云台的变化姿态。
而开环是把传感器固定在云台以外的其他地方，传感器反馈的是飞机的变化姿态，并不是云台的姿态。
作者: 611055853    时间: 2013-2-23 05:34
那两句代码已替换，云台还是无法手动介入，只能是设置在一个最高，中间或最低时打开云台增稳，增稳时无法介入！不增稳时旋钮调节起效，还有后仰已旧是一点，感觉府仰全布行程只有四十五度
作者: long0001    时间: 2013-2-23 13:34
拍张云台图片看看。
作者: huangjun    时间: 2013-2-25 20:18
long0001 发表于 2013-2-21 23:36
刷好固件，连接电脑，打开调参软件，所有参数置0，点选ANGLE自稳模式，LEVEL的D值加到100，P值加到10，转动 ...

这个需要解锁吧?我的想法是把普通的ouput代码copy到云台上?另外,用360度的电机是不是会方便点?
作者: 611055853    时间: 2013-2-25 22:42
long0001 发表于 2013-2-23 13:34
拍张云台图片看看。

(, 下载次数: 17)
作者: long0001    时间: 2013-2-25 23:47
611055853 发表于 2013-2-25 22:42

你这是MWC即作为飞控控制四轴，又作为云台控制吗？
作者: long0001    时间: 2013-2-25 23:49
huangjun 发表于 2013-2-25 20:18
这个需要解锁吧?我的想法是把普通的ouput代码copy到云台上?另外,用360度的电机是不是会方便点?

刷“云台模式”不需要解锁，，360 舵机应该可以。
作者: 611055853    时间: 2013-2-26 08:32
long0001 发表于 2013-2-25 23:49
刷“云台模式”不需要解锁，，360 舵机应该可以。

对我是即四轴，又云台增稳，不需要解锁！插电云台就起作用， X4模式最新固件， 现在就是无法在云台增稳的情况下，遥控辅助角度，必须设置旋钮 低中高随便一个位置是增稳，其他没有增稳，开启增稳的时候辅助调节无效。

作者: yiren1112    时间: 2013-2-26 20:32
太复杂，帮顶了
作者: long0001    时间: 2013-2-26 21:11
现在讨论的都是飞控只作为云台用
作者: kernel32    时间: 2013-2-27 16:41
MWC不是有闭环云台模式么
就是不能通过GUI调整PID
作者: long0001    时间: 2013-2-27 17:54
kernel32 发表于 2013-2-27 16:41
MWC不是有闭环云台模式么
就是不能通过GUI调整PID

请教闭环模式在哪里设置？？？
作者: lijungei    时间: 2013-2-28 12:59
好东西 谢谢楼主
作者: archfly    时间: 2013-3-24 01:16
学习刷了固件，就是云台抖得厉害

作者: JK3    时间: 2013-3-24 23:10
我的版回来啦。但我还不会接呀。也不知道在那里下 载软件，那位大大教一教我呀？
最好有家线图呀。
谢谢
作者: 畅游长江水    时间: 2013-4-20 20:12
高手！为大家作奉献，辛苦了，顶一把

作者: long0001    时间: 2013-4-21 08:57
其实舵机用于开环控制还是很不错滴。。。
作者: matthewspeaking    时间: 2013-4-21 22:41
加油，希望家了尘封的MWC还有用武之地
作者: gxhifi    时间: 2013-7-5 14:39
好可惜啊~可是先帮顶起里先!

真希望MWC的这个云台.可以不只是支持舵机电机.能支持无刷电机那该多好.
作者: 我爱远行    时间: 2013-7-5 16:29
新人学习，先顶后看，看帖必顶，回复是一种美德。
作者: 神舟100    时间: 2013-7-7 08:48
好帖，一定要顶
作者: 王牌猪头    时间: 2013-7-11 05:31
顶起！我也是MWC，最近想搞航模摄影，不知它本身自带的舵机云台效果如何？这个自带的，算是开环控制么？
作者: 炸香机    时间: 2013-9-1 00:10
必须顶起




欢迎光临 5iMX.com 我爱模型 玩家论坛 ——专业遥控模型和无人机玩家论坛（玩模型就上我爱模型，创始于2003年） (http://wz.5imx.com/) Powered by Discuz! X3.3