写在前面

之前我已经写了2篇博客,介绍了如何进行树莓派和stm32芯片通信,传送门在下面:

这2篇博客都是从技术上实现了树莓派和stm32芯片的通信,那我们最终的目的是为了实现树莓派做为系统的总控来控制底盘,使其能够完成在整个地图中特定路线的循迹行进任务。下图是比赛场地图,我标出了5个点的位置,根据这5个点的位置规律,可以写出场上每一个点的坐标,并且我将根据这些坐标来完成整张地图的巡线逻辑。

逻辑分析

流程图

我们要目的就是按照如下的流程图所示,具体的底盘是如何运动的,例如抓取机构抓取货物的具体动作,底盘运动的具体循迹方法都是底盘主控芯片(底盘子系统)去完成的。
根据向上抽象的原则,底层所有的动作均向上抽象成一个命令供python调用。最终在主控树莓派中python根据任务逻辑的需求,动态的调用底层子系统的功能完成相应的动作即可。
流程图

自然语言

自然语言来描述上述过程,那就是首先我们得从起点出发,然后去连接购物车,购物车连接完毕之后购物机器人将按照D,C,B,A货架的顺序进行遍历,并且我将通过坐标点来确定购物机器人的路线,遍历时购物机器人走在靠货架外面的第二条线依靠摄像头去识别货架内的物品,如果是需要取的货物的话购物机器人就会走进去拿货物,拿到货物之后再退出来从而回到既定的轨迹上,当遍历完货架之后再遍历仓库,遍历仓库和遍历货架的方案是一样的,如果摄像头拍到有货物,就走进去拿东西,拿完再出来,如果没有就继续遍历,遍历完仓库之后就完成了整个任务,最后回到起点放下购物车即可。

程序实现

启动线程

因为整个购物车系统需要做识别,也需要做控制,因此开了2个线程,一个线程做图像识别,一个线程做控制,图像识别线程会在之后的博客里进行分享,现在着重讲解控制线程,可以看到robRobot类的一个对象,由rob对象的gogo()方法开始了整个系统的控制,那么这个gogo()里面到底做了什么呢?我们钻进去看看。

q1 = Queue()
q2 = Queue()
def robot(q1, q2):
    rob = Robot(q1, q2)
    rob.gogo()

if __name__ == '__main__':
    p1 = Process(target=robot, args=(q1, q2))
    p2 = Process(target=tf, args=(q1, q2))

    p1.start()
    p2.start()

    p1.join()
    p2.join()

可以看到在Robot类里面有gogo()这个函数,在这个函数里面分别执行3个动作

  • 1.走到起始位置(连接购物车)
  • 2.遍历货架
  • 3.遍历仓库

简单起见,我们着重来看走到起始位置run_to_start())里面的逻辑(遍历货架(task_shelf())和遍历仓库(task_shelf())的具体的的方法与起始位置(run_to_start())类似,但是因为这2个功能实现的一些细节操作比较繁琐因此在这里不再赘述,过段时间专门开一篇博客来讲)

class Robot(Control, Capture):
    def __init__(self, q1=None, q2=None):
        Control.__init__(self)
        Capture.__init__(self)
        self.location = (0, 2)
        self.q1 = q1
        self.q2 = q2
        self.tiers = [False, False]

    # 购物车拼接
    def start_init(self):
        self.run_to_start()
        self.location = (1, 1)
        print("shopping_car connect over")

    def set_location(self, location):
        self.location = location

    def get_location(self):
        print(self.location)

    def gogo(self):
        self.start_init()
        self.task_shelf()
        self.task_store()

运动控制

原来run_to_start()这个函数里面做的事请很简单,它就是将购物机器人的动作进行了分解,以特定方式走特定格数就设定为一个动作,并且将购物机器人从起点走到连接购物车的地方的路线分解成了以特定方式走特定格数的动作,走格子的动作都是通过move_by_grid()函数来实现的,那接下来看看这个函数里面到底干了啥吧

def move_by_grid(self, direction, distance=''):
        # print('move by grid')
        self.port_chassis.write((direction + str(distance)).encode())
        self.time1 = time.time()
        print(direction + str(distance))
        self.wait_for_act_end_signal_chassis(direction, distance)

嗷,原来是这样呀,看这两个函数是不是有点眼熟呢?如果没印象了的话请看这篇博客:
STM32F427IIH6芯片通过DMA+USART与树莓派进行双工通信
看到这里大家应该明白了吧,原来计算好了该走哪个格子,怎么走之后,python会通过串口发送命令到底盘主控芯片(底盘子系统)中去,由底盘子系统完成相应的动作,从而最终完成python控制购物机器人完成预定轨迹的行走

def run_to_start(self):
     self.move_by_grid(dir_up, 1)
     # self.move_by_grid(dir_back, 1)
     self.action_chassis(dir_left, 1)
     self.move_by_grid(dir_back, 1)
     self.action_chassis(dir_right, 1)
     self.move_by_grid(dir_up, 1)
     self.action_chassis(dir_left, 1)

def wait_for_act_end_signal_chassis(self, ret=None, distance=0):
        print('waiting chassis...')
        while True:
            sig = self.port_chassis.readline().decode()

            # print(self.time2 - self.time1)

            sig = sig[0:2]
            print(sig)
            #
            if sig == 'HI':
                self.time2 = time.time()
                print(self.time2 - self.time1)
                if self.time2 - self.time1 < 1.0 and self.flag == False and distance == 1:
                    print('1 step deal error')
                    self.flag = True
                    if (ret == dir_up or ret == dir_back):
                        self.move_by_grid(ret, 1)
                    else:
                        # left or right
                        self.move_by_grid(ret, 1)
                if self.time2 - self.time1 < 2.8 and self.flag == False and distance == 3:
                    print('3 step deal error')
                    self.flag = True
                    self.move_by_grid(ret, 1)
                print('chassis finished ^_^')
                self.flag = False
                break

在这里我再补充分析一下命令数据帧,以dir_up为例,发送'dir_up'即表示需要底盘子系统以模式A(后续博客会介绍模式A的相关内容,第二个数字0代表模式A,数字1代表模式B)方式进行前进(第一个数字1表示前进,2表示向左转,3表示向右转,4表示向后转),但是具体走多少格数呢?还是需要根据实际情况来确定的,因此在def move_by_grid()函数里有这么一个语句:self.port_chassis.write((direction + str(distance)).encode()),它就是为了在dir_up后面加上具体需要走的格数

dir_up = '@cmd 1 0 '
dir_left = '@cmd 2 0 '
dir_right = '@cmd 3 0 '
dir_back = '@cmd 4 0 '

(づ ̄3 ̄)づ╭❤~一键三连,这次一定(๑•̀ㅂ•́)و✧