写在前面
之前的一些铺垫性内容已经写在我的另外几篇博客里面了,传送门在下面:
- STM32F427IIH6芯片通过DMA+USART(UART)与树莓派进行双工通信
- 购物机器人底盘控制之STM32F427IIH6芯片处理串口数据方案
- 树莓派中Python控制购物机器人完成预定轨迹行走
这篇博客讲的就是更加上层的逻辑了,那就是我们如何通过python的逻辑实现按照坐标遍历货架和仓库,我这里再放一张坐标图
程序分析
刚开始我们的位置是(0,0),我们需要走到(1,1)的位置去连接购物车,这一步的动作分解过程在函数run_to_start()
里面了,所以我们从(1,1)之后开始讲解。
首先我们会遍历货架,购物机器人遍历货架上时走的都是货架对应出来的第二条白线,按照行径路径来看,我是按照D,C,B,A来遍历的,D货架的第一个窗口对应出来在第二条白线上的坐标是(1,4),查下面的表也就是’D6’,但实际上在程序里是用’D6’来索引坐标(1,4)的,但是为了讲解方便我就用坐标索引位置了。
#这个是货柜的仓库坐标
MAP_DICT_SHELF = {
'A1': (0, 1), 'A2': (1, 1), 'A3': (2, 1), 'A4': (3, 1), 'A5': (4, 1), 'A6': (5, 1),
'B1': (8, 0), 'B2': (8, 1), 'B3': (8, 2), 'B4': (8, 3), 'B5': (8, 4), 'B6': (8, 5),
'C1': (9, 8), 'C2': (8, 8), 'C3': (7, 8), 'C4': (6, 8), 'C5': (5, 8), 'C6': (4, 8),
'D1': (1, 9), 'D2': (1, 8), 'D3': (1, 7), 'D4': (1, 6), 'D5': (1, 5), 'D6': (1, 4),
}
我们来看一下下面这个遍历货架的程序,enumerate
函数的作用是将一个可遍历的数据对象(这里是字符串s = ['D', 'C', 'B', 'A']
)组合为一个索引序列,同时列出数据和数据下标,下标就是index
,数据就是shelf_s
。
def task_shelf(self):
s = ['D', 'C', 'B', 'A']
for index, shelf_s in enumerate(s):
# if shelf_s == 'C':
# time.sleep(1)
self.run_to_goal(MAP_DICT_SHELF[shelf_s + '6'])
for i in range(1, 7):
shelf_p = shelf_s + str(7 - i)
tmp_dir = self.run_to_goal(MAP_DICT_SHELF[shelf_p])
self.q1.put(shelf_p)
detec_info = self.q2.get()
self.move_by_grid(dir_back, 1)
self.location = MAP_DICT_SHELF[shelf_s + str(2)]
if shelf_s == 'A':
self.move_by_grid(dir_back, 1)
self.location = MAP_DICT_SHELF[shelf_s + str(3)]
self.action_chassis(dir_right, 1)
每个货架开始遍历的地方分别是D6,C6,B6,A6,相当于是在每格的第6个位子开始遍历,所以需要MAP_DICT_SHELF[shelf_s + '6']
这个语句在遍历每一个新的货架的时候设置起点。设置好了起点之后就循环遍历6列窗口就可以啦。
def run_to_goal(self, goal):
move_calls = get_path_command_list(self.location, goal)
dir = dir_up
for i in move_calls:
dir = i[0]
direction = i[0]
total_distance = i[1]
self.move_by_grid(direction, total_distance)
time.sleep(0.3)
print('ok...... arrived at end point')
self.location = goal
print("Current location: " + str(self.location))
return dir
举个例子来说,假如我们现在的位置是(1,1),我们需要到‘D6’去,也就是坐标(1,4),我们就需要往车头方向前进3格,我们连接购物车的时候车头是跟D货柜平行,车头和A货柜在相反方向,因此我们只需要让购物机器人前进3格就可以啦,从之前的博客里面可以知道,我们需要发送到底盘子系统的串口命令里的3个数字就是1 0 3
,翻译成自然语言就是以模式A前进3格。用python语句来实现就是self.location = MAP_DICT_SHELF[shelf_s + str(3)]
这段程序里面放了一个嵌套的循环,外循环是为了走遍4个货柜,内循环是遍历每个货柜的6列窗口,查看是否有需要抓取的货物
从博客开始的图中可以很明显的看到,当走完一个货柜的之后,机器人的位置是在下一个货柜往外第一条白线的地方,这个对于接下来机器人的行进时很不利的,所以当购物机器人遍历完了一个货柜的6个窗口之后,需要让购物机器人后退一格,然后再右转,以便在下一个循环中购物机器人可以走到下一个货柜往外第二条白线的地方,同时每走完一步,购物机器人在场上的坐标位置都会更新,这也是下面这段代码的作用
self.move_by_grid(dir_back, 1)#后退一格
self.location = MAP_DICT_SHELF[shelf_s + str(2)]#更新购物机器人的位置
遍历完货柜之后就要遍历仓库了,遍历完货柜之后购物机器人的应该是在‘A2’也就是(1,1)点上的,而且车头方向是朝向起点,车身与A货架平行的,那么接下来要遍历仓库的话应该怎么走呢?思考一下
看下面这段代码,如果说A货柜也已经走完了,那么说明所有货架都已经遍历完毕,接下来准备遍历仓库了,因此购物机器人需要再退一格走到A3(2,1)上,以方便仓库的遍历,至此完成了货架的遍历
self.move_by_grid(dir_back, 1)#后退一格
self.location = MAP_DICT_SHELF[shelf_s + str(2)]#更新购物机器人的位置
if shelf_s == 'A':
self.move_by_grid(dir_back, 1)
self.location = MAP_DICT_SHELF[shelf_s + str(3)]
看下面这段task_store()
的开头,这就是仓库遍历的开始,在最开始的时候,购物机器人就会先以模式A前进1格,然后再右转,到达了(2,2),这个点也就是仓库遍历的起点,仓库遍历大体逻辑与货柜遍历相似,但是还有一点区别,仔细看这部分程序的话会看到有一个这个语句self.move_by_grid(dir_up, 0)
,让车子前进0步是什么意思呢?其实这个是在底层子系统里面做了区分,0步代表的就是以前面的光电传感器作为停车依据。
def task_store(self):
e = ['EA', 'EB', 'EC', 'ED']
self.move_by_grid(dir_up, 1)
self.move_by_grid(dir_right, 1)
for index, shelf_e in enumerate(e):
for i in range(1, 8):
shelf_e = shelf_e + str(i)
if i % 2 == 0:
self.move_by_grid(dir_up, 0)
else:
# save time
if i == 7:
self.move_by_grid(dir_up, 2)
else:
self.move_by_grid(dir_up, 1)
time.sleep(1)
self.q1.put(shelf_e)
detec_info = self.q2.get()
self.move_by_grid(dir_left, 1)
self.move_by_grid(dir_back, 3)
# over
(づ ̄3 ̄)づ╭❤~一键三连,这次一定(๑•̀ㅂ•́)و✧