14241浏览
查看: 14241|回复: 20

[建造日志] 如何用Arduino做一个可编程混控

[复制链接]
一、前言
  在铁甲雄心的比赛中, 饶厂长虽然比赛机器人整体设计缺陷较多, 但是对于行走的控制非常自豪, 想怎么混控怎么混控, 想设置什么功能设置什么功能。 像我的这台机器, 我用两个通道分别控制前后行走与左右行走, 一个通道控制角度旋转, 一个通道控制点火动作,一个通道控制陀螺仪矫正的积分与增益开关, 一个通道控制机器翻转后的控制反向与增益反向。因此开关陀螺仪矫正, 切换正反行走, 同时控制三个轮子平移又自旋, 完全不在话下。这些复杂逻辑如果想靠调遥控器混控来完成的话, 怕是要把人急死也搞不定, 但如果用Arduino这种单片机完成的话, 还是比较容易得。
如何用Arduino做一个可编程混控图1如何用Arduino做一个可编程混控图2
  图中的机器人正在绕场地中心横移, 而这看起来比较老练的操作是我从未拿过遥控器的队友很快就掌握了的。 虽然在比赛中没用出来就被铲起来了, 但这并不妨碍我对我的混控方法洋洋自得。

  事实证明, 这套独立供电的系统可扩展性强, 抗干扰性强, 稳定性也不错, 受王大师的委托, 今天拿出来分享一下。
二、先修知识
  为了能够比较顺畅的完成第三部分正文的内容, 你可能需要了解一些先修必备知识。 当然, 如果你已经对这些内容非常了解了, 你可以直接去copy我的程序(估计你并不需要)。 另外, 不了解这部分内容并不会让你做不出来, 只是会让你的调试更困难, 你也可以直接进行烧录和接线, 试试你的运气。 不过我还是建议想要用Arduino做这件事的你仔细阅读这部分内容, 以免有些关键问题被忽略。


  1. PPM/舵机信号
  在开始做这一切之前, 如果搞不清楚你的接收机是什么信号, 你的驱动板是什么信号, 那做混控什么的简直就是噩梦了。 常玩航模的朋友们对接收机的信号肯定是很清楚的, 这是典型的PPM信号, 舵机、电调等航模常用器件的输入都是这种信号, 使得舵机、电调的信号线可以直接接到接收机上, 这样的安装很方便。
  
标准PPM刷新率是50Hz(周期20ms),但也遇到过60Hz(周期约16ms)的接收机,如GR3E 3通道接收机。7通道接收机WFR07的周期是21.2ms。 我们的主角PPM长这样:
  如何用Arduino做一个可编程混控图3
   最上面的是多路PPM调制的信号, 是接收机关心的, 我们不需要了解。 中间几路就是我们要的PPM信号。 如果我们有示波器, 或是单片机能够串口输出的话, 你就能够实时的拨动遥控器, 看一看每一路的变化了。 以六通道为例, 遥控器的摇杆、拨码开关、旋钮都对应着固定的通道号(有的也可以设置),因此每一路通道的变化都可以通过PPM的高电平宽度来侦测到。  

   PPM信号是一个周期性的方波信号,周期为20ms,也就是50Hz的刷新频率。PPM每一周期中的高电平持续时间为1~2ms(1000us~2000us)。以双手回中的遥控器为例, 在摇杆的自然位置, 这四路通道的脉宽通通为1500us左右。 如果偏差较远, 可以通过微调校准中心。

   2. 脉宽捕获/外部中断与定时器
   我们用示波器这种设备可以观测信号的脉宽, 那如何让单片机做到呢? 有的单片机有脉宽捕获功能, 如STM8, STM32等等, 可以通过配置寄存器设置引脚直接捕获脉宽。 Arduino据我所知貌似没有这种功能, 但是有外部中断与定时器。 单片机(Arduino)的引脚能够被脉宽跳变所触发, 当脉宽跳变且跳为高的时候, 开始计时, 当跳变为低的时候, 停止计时, 则脉宽被捕获到。
   如何用Arduino做一个可编程混控图4
   有的同学说, Arduino 有pulseIn函数, 可以直接测脉宽, 当然可以, 但不建议, 因为这不是硬件触发, 执行效率较低, 在没有操作系统调度的情况下不建议使用。
   如何用Arduino做一个可编程混控图5
   有的同学问, Arduino Uno行不行? mega行不行? 理论上说, 都行, 但是注意, uno只有两路外部中断接口, mega只有6路而且mega体积比较大, 如果你要3路信号接上的话, uno就没戏了。 我本人使用的是Arduino 的M0这款, 有13路中断, 主频48M还不错, 我6路做输入6路做输出刚刚满足。 最好别用3.3v电平的主控, 会造成一些麻烦, 建议使用M0即可。

   3. 混控的数学
   混控该怎么说呢, 要复杂可以很复杂, 要简单可以非常简单。 我自己的三轮万向轮混控就是一个公式解决了问题。 当然, 每个人的思维习惯不同, 我们搞控制的以及习惯了用数学模型思考了, 可能反而搞得很复杂。 实际上混控愿意如何实现主要看自己的手感和习惯, 当然了解其基本原理也是有些价值的。
如何用Arduino做一个可编程混控图6
以两轮行走系统为例, 上面三个式子就是我们所要的一切了。
Vr Vl分别是右轮和左轮的线速度, v为车辆整体沿运动方向的速度, w为方向角速度(l为两轮的距离, 是个常量)。 我们混控就靠这俩公式即可。 比如, 我希望我能达到如下效果:
如何用Arduino做一个可编程混控图7
  理解这步很关键!假设左边上下动杆为通道1, 右边左右动杆为通道2, 则我捕获通道1的脉宽则与v对应, 而捕获的通道2的脉宽与w对应。 接下来再通过v与w值, 直接求取Vr和Vl, 并直接输出给驱动板即可。
例子1:
   我左边推到最上, 右边不动, 则通道一脉宽为2000us, 则v = 2000 - 1500;
右杆在中间, 则通道2脉宽为1500, 则w = 1500-1500 = 0; 求解得到Vr = 500, Vl = 500;(二元一次方程组)
那么, 对应两个轮子的脉宽为, Pwr = Vr+1500; Pwl = Vl + 1500;
   如果求解出给驱动的脉宽超过1000-2000这个范围怎么办? 这是一个关键点, 究竟是限制速度保证转速还是限制转速保证速度, 就看个人手感了。 理解了这一点, 混控的程序就非常简单了。

三、开始设计我们的混控
  什么? 你觉得上面都懂就OK了? 图样了, 你以为我说的可靠, 稳定什么的不得靠点手段吗?mini格斗群里讨论的驱动板的问题难道不是个例子吗? 上面这点知识能让机器人动不假, 要稳定还真有些别的注意事项。

  1. 硬件
   硬件讲起来注意事项比较多,①接线: Arduino直接接接收机怎么接? 一根一根杜邦线的吗?容易掉吧。 接上然后粘起来? 额, 怎么调试呢。 最好是专门设计一块转接板, 将3pin的接口设计在上面, 把电源、信号以及gnd都排布好, 方便直接连接。 Arduino连接电调、驱动同理。

   ②隔离:我们不是很确定购买的电调、驱动什么的是否做了光耦隔离, 假如没有的话很容易造成电流反向烧毁主板, 造成不必要的麻烦, 在设计PCB的时候就把管沟隔离设计到输出信号端。
   
   ③供电:主控怎么供电? 直接电调引出来5v? 电池直接? better not。 最好的方式就是隔离供电。 假如电池电压较高, 如24v、 48v等, 建议使用稳压芯片给主控供电。 假如电池电压本身就不高, 建议使用一块比较小的电池单独供电。 当然, 电池质量好且不超过12v的话电池直接给arduino Vin口供电即可。

   ④上电顺序: 在给你的驱动、电调什么的上电前, 确保主控是通电的。 先开主控, 再开遥控, 最后给武器、行走系统上电。 不这么做你可能会丢掉性命!!!!!!!!!


  2. 软件
  软件的注意事项更多:
  ①滤波: 如果不希望自己的轮子在原地一抖一抖的话, 加个滤波是你的第一选择。 中值滤波, 专抗尖峰噪声, 低延时, 完美解决此问题。
如何用Arduino做一个可编程混控图8
  ②看门狗: 如果不希望万一失控后机子再也控不住的话, 你最好加看门狗。 如果你不知道看门狗是什么的话真的要恶补一下单片机知识了, 这个我就不解释了, 狗年大吉。

   ③用定时中断或操作系统: 如果你不希望控制周期时长时短或者卡在什么莫名其妙的地方的话, 用定时中断或操作系统吧, 千万别跑在loop里。
如何用Arduino做一个可编程混控图9
   
   3. 代码

[mw_shl_code=cpp,true]#define CH1 8
#define CH2 9

#define CH3 13
#define CH4 12


#include <Servo.h>
Servo Left_motor;
Servo Right_motor;
Servo Relay;
int PW_CH1 = 1500;
int PW_CH1_last = 1500;
int PW_CH1_sample = 1500;
int start_CH1 = 0;
int PW_CH2 = 1500;
int PW_CH2_last = 1500;
int PW_CH2_sample = 1500;
int start_CH2 = 0;
int PW_CH3 = 1500;

int start_CH3 = 0;
int PW_CH4 = 1500;
int start_CH4 = 0;
void setup() {
  // put your setup code here, to run once:
  SerialUSB.begin(9600);
  Left_motor.attach(0);
  Right_motor.attach(1);
  Relay.attach(3);
  pinMode(3, OUTPUT);
  pinMode(CH1, INPUT_PULLUP);
  attachInterrupt(digitalPinToInterrupt(CH1), ISR_CH1, CHANGE);
  pinMode(CH2, INPUT_PULLUP);
  attachInterrupt(digitalPinToInterrupt(CH2), ISR_CH2, CHANGE);
  pinMode(CH3, INPUT_PULLUP);
  attachInterrupt(digitalPinToInterrupt(CH3), ISR_CH3, CHANGE);
  pinMode(CH4, INPUT_PULLUP);
  attachInterrupt(digitalPinToInterrupt(CH4), ISR_CH4, CHANGE);
}

void ISR_CH1() {
  //SerialUSB.println("test");
  if (digitalRead(CH1) == HIGH) {
    start_CH1 = micros();
  }
  if (digitalRead(CH1) == LOW) {
    PW_CH1 = micros() - start_CH1;
  }
}

void ISR_CH2() {
  //SerialUSB.println("test");
  if (digitalRead(CH2) == HIGH) {
    start_CH2 = micros();
  }
  if (digitalRead(CH2) == LOW) {
    PW_CH2 = micros() - start_CH2;
  }
}

void ISR_CH3() {
  //SerialUSB.println("test");
  if (digitalRead(CH3) == HIGH) {
    start_CH3 = micros();
  }
  if (digitalRead(CH3) == LOW) {
    PW_CH3 = micros() - start_CH3;
  }
}

void ISR_CH4() {
  //SerialUSB.println("test");
  if (digitalRead(CH4) == HIGH) {
    start_CH4 = micros();
  }
  if (digitalRead(CH4) == LOW) {
    PW_CH4 = micros() - start_CH4;
  }
}
int filter1[5] = {1500, 1500, 1500, 1500, 1500};
int filter2[5] = {1500, 1500, 1500, 1500, 1500};
int temp[5];
int mid1(void) {
  int i, j, t;
  temp[0] = filter1[0];
  temp[1] = filter1[1];
  temp[2] = filter1[2];
  temp[3] = filter1[3];
  temp[4] = filter1[4];

  for (j = 0; j < 4; j++)
    for (i = 0; i < 4 - j; i++)
    {
      if (temp > temp[i + 1])
      {
        t = temp;
        temp = temp[i + 1];
        temp[i + 1] = t;
      }
    }
    return temp[2];
}

int mid2(void) {
  int i, j, t;
  temp[0] = filter2[0];
  temp[1] = filter2[1];
  temp[2] = filter2[2];
  temp[3] = filter2[3];
  temp[4] = filter2[4];

  for (j = 0; j < 4; j++)
    for (i = 0; i < 4 - j; i++)
    {
      if (temp > temp[i + 1])
      {
        t = temp;
        temp = temp[i + 1];
        temp[i + 1] = t;
      }
    }
    return temp[2];
}


void loop() {
  // put your main code here, to run repeatedly:
  static float speed_advance = 0;
  static float rotation_speed = 0;
  float test_flag = 0;
  //if(PW_CH2>=1500){
  float v1 = 0;
  float v2 = 0;
  static float v3 = 0;
  float raw_speed_advance;
  float raw_rotation_speed;

  filter1[0] = filter1[1];
  filter1[1] = filter1[2];
  filter1[2] = filter1[3];
  filter1[3] = filter1[4];
  filter1[4] = PW_CH1;
  filter2[0] = filter2[1];
  filter2[1] = filter2[2];
  filter2[2] = filter2[3];
  filter2[3] = filter2[4];
  filter2[4] = PW_CH2;

  PW_CH1_last = mid1();
  PW_CH2_last = mid2();
  //filter1[4] = PW_CH1;
  //filter2[4] = PW_CH2;

  test_flag = 0;

/*
  SerialUSB.print(filter1[0]);
  SerialUSB.print(" ");

  SerialUSB.print(filter1[1]);
  SerialUSB.print(" ");
  SerialUSB.print(filter1[2]);
  SerialUSB.print(" ");
  SerialUSB.print(filter1[3]);
  SerialUSB.print(" ");
  SerialUSB.println(filter1[4]);
  //SerialUSB.print(" ");
  //SerialUSB.println(PW_CH2);
  */

  raw_speed_advance = (PW_CH1_last - 1500) * (1 - abs(PW_CH1_last - 1500) / 1000.0);
  raw_speed_advance = -1 * raw_speed_advance;
  speed_advance = raw_speed_advance;

  if (abs(raw_speed_advance) <= 30) {
    raw_speed_advance = 0;
  }
  speed_advance = 0.9 * speed_advance + 0.1 * raw_speed_advance;
  raw_rotation_speed = (PW_CH2_last - 1500) / 2.0;

  if (abs(raw_rotation_speed) <= 30) {
    raw_rotation_speed = 0;
  }
  rotation_speed = 0.9 * rotation_speed + 0.1 * raw_rotation_speed;
  rotation_speed = raw_rotation_speed;

  if (PW_CH3 >= 1800) {

    speed_advance *= -1;
    rotation_speed *= -1;
  } else if ((PW_CH3 <= 1700) && (PW_CH3 >= 1300)) {
   
  }

  if (PW_CH4 > 1700) {
    v3 = 0.99 * v3 + 0.01 * 50;
    Relay.write(160);
  } else {
    v3 = 0.99 * v3 + 0.01 * 0;
    Relay.write( 20);
  }

   SerialUSB.print(speed_advance);
   SerialUSB.print(" ");
  SerialUSB.print(rotation_speed);
  SerialUSB.print(" ");
  v1 = ((speed_advance - rotation_speed) + 1500);
  v2 = ((speed_advance + rotation_speed) + 1500);

  if (abs(v1 - 1500) <= 15) {
    v1 = 1500;
  }
  if (abs(v2 - 1500) <= 15) {
    v2 = 1500;
  }
  
    SerialUSB.print(v1);
    SerialUSB.print(" ");
    SerialUSB.print(v2);
    SerialUSB.print(" ");
  //  SerialUSB.print(v3);
    //SerialUSB.print(" ");
  
  Left_motor.writeMicroseconds(v1+20);
  Right_motor.writeMicroseconds(v2+20);

  /*
    Left_motor.write(((speed_advance - rotation_speed)+500)/5.56);
    Right_motor.write(((speed_advance + rotation_speed)+500)/5.56);

    SerialUSB.print(((speed_advance - rotation_speed)+500)/5.56);
    SerialUSB.print(" ");
    SerialUSB.println(((speed_advance + rotation_speed)+500)/5.56);
  */
  
    SerialUSB.print(PW_CH1_last);
    SerialUSB.print(" ");
    SerialUSB.print(PW_CH2_last);
    SerialUSB.println(" ");
  //  SerialUSB.print(PW_CH3);
   // SerialUSB.print(" ");
    //SerialUSB.println(PW_CH4);
  

  delay(10);
}[/mw_shl_code]

   
怎么说呢, 这段代码写的。。。。 有失水准, 我也不想解读了, 我怕被同事看见。 注意哦, 这个代码是M0的, 只能在M0上用, Uno什么的是编译不了的。


四、效果
如何用Arduino做一个可编程混控图10如何用Arduino做一个可编程混控图11



洋葱_FMB  中级技师 来自手机

发表于 2018-2-23 00:08:27

饶厂长 发表于 2018-2-22 23:56
一、前言
  在铁甲雄心的比赛中, 饶厂长虽然比赛机器人整体设计缺陷较多, 但是对于行走的控制非常自豪,  ...

好疯骚的漂移啊。
回复

使用道具 举报

xborot  中级技师 来自手机

发表于 2018-2-23 00:34:08

饶厂长 发表于 2018-2-22 23:56
一、前言
  在铁甲雄心的比赛中, 饶厂长虽然比赛机器人整体设计缺陷较多, 但是对于行走的控制非常自豪,  ...

我用的是花科尔D10,刷的5.0版,一个摇杆就控制了前后左右四个方向,比这种两个摇杆的控制起来方便很多
回复

使用道具 举报

hnyzcj  版主

发表于 2018-2-23 07:57:44


好疯骚的漂移啊。:lol
回复

使用道具 举报

svw  初级技匠

发表于 2018-2-23 08:51:45

太精彩了,而且居然看懂了
回复

使用道具 举报

饶厂长  初级技匠
 楼主|

发表于 2018-2-23 09:41:28

xborot 发表于 2018-2-23 00:34
我用的是花科尔D10,刷的5.0版,一个摇杆就控制了前后左右四个方向,比这种两个摇杆的控制起来方便很多 ...

这个图只是我的操作习惯, 你也可以设置为单手控制
回复

使用道具 举报

饶厂长  初级技匠
 楼主|

发表于 2018-2-23 09:42:12

洋葱_FMB 发表于 2018-2-23 00:08
好疯骚的漂移啊。

没你疯, 哈哈哈哈哈
回复

使用道具 举报

饶厂长  初级技匠
 楼主|

发表于 2018-2-23 09:42:41

hnyzcj 发表于 2018-2-23 07:57
好疯骚的漂移啊。

疯, 哈哈哈
回复

使用道具 举报

svw  初级技匠

发表于 2018-2-23 10:54:10

https://www.dfrobot.com.cn/goods-1519.html

dfduino的m0可以实现混控功能吗?
回复

使用道具 举报

饶厂长  初级技匠
 楼主|

发表于 2018-2-23 11:08:48


这个只有四个中断,  就看你觉得够不够了
回复

使用道具 举报

xborot  中级技师

发表于 2018-2-23 17:42:31

饶厂长 发表于 2018-2-23 09:41
这个图只是我的操作习惯, 你也可以设置为单手控制

哦,那不错
回复

使用道具 举报

swanglei  中级技神

发表于 2018-2-23 19:32:34

咦?程序呢?:lol:lol:lol:lol:lol:lol
回复

使用道具 举报

饶厂长  初级技匠
 楼主|

发表于 2018-2-23 19:55:52


今晚更新
回复

使用道具 举报

洋葱_FMB  中级技师 来自手机

发表于 2018-2-24 08:06:33

饶厂长 发表于 2018-2-22 23:56
一、前言
  在铁甲雄心的比赛中, 饶厂长虽然比赛机器人整体设计缺陷较多, 但是对于行走的控制非常自豪,  ...

你说的关机顺序是错误的。不管什么情况,应该
第一打开遥控器。然后给机器设备供电。
下电也是机器下电,然后关闭遥控器。
不然真可能小命不保。
小日本队这个细节做的是最好的一个队了

他们是扶她爸遥控器,配合工业控制器的失控应该很小。
但是我们入了维修场地准备上场前面,遥控器都一直打开。
回复

使用道具 举报

饶厂长  初级技匠
 楼主|

发表于 2018-2-24 10:27:51

洋葱_FMB 发表于 2018-2-24 08:06
你说的关机顺序是错误的。不管什么情况,应该
第一打开遥控器。然后给机器设备供电。
下电也是机器下电, ...

你说的没错。 由于自己搞的解码主控, 所以主控上电有个启动时间, 并非上电就工作。  主控上电后再开遥控器,  或是先开遥控器再开主控没有分别。 确认两个都上电后, 再开启机器行走部分电源, 这个顺序和你说的没有冲突。  所以我是先开主控, 再开遥控, 最后开机。
回复

使用道具 举报

饶厂长  初级技匠
 楼主|

发表于 2018-2-24 10:32:51

洋葱_FMB 发表于 2018-2-24 08:06
你说的关机顺序是错误的。不管什么情况,应该
第一打开遥控器。然后给机器设备供电。
下电也是机器下电, ...

如果主控是和机器其他部分同时上电的话, 就按你说的先开控, 再开机。
回复

使用道具 举报

Aslm  学徒

发表于 2018-7-29 22:52:48

你好,这种要怎么跟陀螺仪搭配做辅助控制呢?
回复

使用道具 举报

gada888  版主

发表于 2018-8-29 09:46:05

这个牛
回复

使用道具 举报

帅猫  高级技师

发表于 2020-2-25 10:51:46

我的接收机是R9DS的,直接用UNO的pulseIn()读取PWM就可以了。买一个好的接收机是多么重要:lol
回复

使用道具 举报

饶厂长  初级技匠
 楼主|

发表于 2020-2-27 19:14:04

帅猫 发表于 2020-2-25 10:51
我的接收机是R9DS的,直接用UNO的pulseIn()读取PWM就可以了。买一个好的接收机是多么重要:lol ...

同时4路用pulsein可以吗?
回复

使用道具 举报

帅猫  高级技师

发表于 2020-2-27 19:48:55

饶厂长 发表于 2020-2-27 19:14
同时4路用pulsein可以吗?

只用了两路,因为是两个马达也用不到那么多pluseIn
回复

使用道具 举报

您需要登录后才可以回帖 登录 | 立即注册

本版积分规则

为本项目制作心愿单
购买心愿单
心愿单 编辑
[[wsData.name]]

硬件清单

  • [[d.name]]
btnicon
我也要做!
点击进入购买页面
上海智位机器人股份有限公司 沪ICP备09038501号-4

© 2013-2024 Comsenz Inc. Powered by Discuz! X3.4 Licensed

mail