解魔方机器人[三]-魔方扫描

拥有了基本动作这些工具之后,回想最开始的方案设计,我们现在该着手解决系统中的“输入”了。输入即EV3对魔方进行扫描,通过计算机语言数字化的对魔方进行表示。仍然本着任务分解的思路,我们将扫描魔方这个任务继续分解为更小的任务:扫描色块、扫描单面、扫描六面。

3.1 扫描色块

上一章已经介绍,扫描魔方时色彩传感器为RGB模式,因此在扫描后会得到R、G、B值,每个色块的颜色就需要采用包含3个值的数组来存储。例如:color[0]=R、color[1]=G、color[2]=B。

程序设计

# 这个示例仅为说明RGB采集,并非实际应用于机器人中的程序
for i in range(3):
  color[i] = cs.value(i)

3.2 扫描单面

EV3每次扫描最上面的面,我们按照先中间再四周的顺序进行扫描,单面一共9个色块,扫描流程为:

  1. 扫描中间块
  2. 顺时针转45度
  3. 扫描角块
  4. 顺时针转45度
  5. 扫描边块
  6. 顺时针转45度
  7. 扫描角块
  8. 顺时针转45度
  9. 扫描边块
  10. 顺时针转45度
  11. 扫描角块
  12. 顺时针转45度
  13. 扫描边块
  14. 顺时针转45度
  15. 扫描角块
  16. 顺时针转45度
  17. 扫描边块
  18. 探头收回

从机器人的正面俯视,容易知道每个色块的扫描顺序。

======+-----+  +-----+
      [	    ]  |3 2 1|   +=============
hand motor  ]  |4 0 8|  [X]color sensor
      [	    ]  |5 6 7|   +=============
======+-----+  +-----+

程序设计

# 扫描魔方单面
def scan_one_side():
  # 定义一个9x3的二维数组,存放9个色块的RGB值
  ev3_scan_side = np.zeros((9,3), np.uint8)
  # 扫描中间块
  head_center()
  for i in range(3):
    cv = cs.value(i)
    if cv<=255:
      ev3_scan_side[0,i] = cv
    else:
      ev3_scan_side[0,i] = 255
  # 扫描四周
  for j in range(8):
    base_step()
    if j%2 == 0:
      head_side_coner()
    else:
      head_side_edge()
    for i in range(3):
      cv = cs.value(i)
      if cv<=255:
        ev3_scan_side[j+1,i] = cv
      else:
        ev3_scan_side[j+1,i] = 255
  # end_of_mission()函数实际为hand_raise()和head_back()函数的组合,确保机器人恢复初始化状态
  end_of_mission()

  # 返回扫描单面后的二维数组
  return ev3_scan_side

3.3 扫描六面

扫描六面基本思想就是扫描完一面后,将魔方翻转,再扫描另一面。需要注意的是:面向机器人的魔方正面和背面在扫描的时候,需要先进行底座旋转,并且此时扫描后得到的色块顺序相对于其他面也是不一样的。尤其是色块顺序,在之后对魔方进行建模时非常重要,其实也并不难,只是需要足够的耐心来认真确认。扫描流程:

  1. 扫描单面
  2. 翻动魔方
  3. 扫描单面
  4. 翻动魔方
  5. 扫描单面
  6. 翻动魔方
  7. 扫描单面
  8. 底座逆时针转90度
  9. 翻动魔方
  10. 扫描单面
  11. 翻动魔方
  12. 翻动魔方
  13. 扫描单面
  14. 底座顺时针转90度
  15. 翻动魔方
  16. 底座顺时针转90度

定义从机器人的正面俯视魔方的面为第1个面,得到魔方各个面及色块的扫描顺序:

                   +--5--+
                   |5 4 3|
                   |6 0 2|
                   |7 8 1|
 +--0--+--1--+--2--+--3--+
 |3 2 1|3 2 1|3 2 1|3 2 1|
 |4 0 8|4 0 8|4 0 8|4 0 8|
 |5 6 7|5 6 7|5 6 7|5 6 7|
 +-----+-----+-----+--4--+
                   |5 4 3|
                   |6 0 2|
                   |7 8 1|
                   +-----+

程序设计

def scan_every_side():
  定义一个6x9x3的三维数组保存魔方6个面每个面9个色块的RGB值
  ev3_every_side = np.zeros((6,9,3), np.uint8)
  for i in range(6):

    # 扫描第1个面
    if i==0:
      ev3_every_side[i,:] = scan_one_side()

    # 扫描第2-4个面
    elif i>0 and i< 4:
      hand_turn()
      hand_raise()
      ev3_every_side[i,:] = scan_one_side()

    # 扫描第5个面
    elif i==4:
      base_anticlockwise()
      hand_turn()
      hand_raise()
      ev3_every_side[i,:] = scan_one_side()

    # 扫描第6个面
    elif i==5:
      hand_turn()
      hand_turn()
      hand_raise()
      ev3_every_side[i,:] = scan_one_side()
      base_clockwise()
      hand_turn()
      hand_raise()
      base_clockwise()

  # 返回保存扫描结果的三维数组
  return ev3_every_side

发表评论

电子邮件地址不会被公开。 必填项已用*标注