下面描述了流程中使用的各个步骤和功能是如何工作的，以及如何对它们进行测试。
星路规划：
机器人的寻路系统基于A*算法，该算法能有效地计算出有障碍物环境中的最优路径。该系统实现为分布式架构，其中计算密集型寻路运行在机器人的树莓派4上，而机器人的TI板执行计算路径。
机器人使用2D网格表示在其环境中导航，其中地图(14英尺x 13英尺的单元)区分障碍物("x")和自由空间("0")。该映射存储在反映环境当前状态的字符数组(map和mapstart)中。当机器人需要计算新的路径时，它会将地图数据压缩成更有效的二进制格式进行传输。由于我们的项目是在走廊中执行的，所以我们需要重新创建一个不同于初始代码的新地图。走廊的90度角和预定义的盒子障碍物通过在地图上设置带有"x"的障碍物来模拟。
当机器人确定它需要一条新路径(由StartAstar标志触发)时，寻路过程开始。机器人将其当前位置、目的地坐标和压缩地图数据打包在SendAStarInfo结构中，并通过串行通信将其发送给树莓派4。在此处理期间，机器人暂停其运动(由AstarRunning标志控制)，直到接收到新的路径。
一旦外部系统计算出路径，它就会返回一系列避开障碍物的路径点。机器人处理这些信息，并将路径坐标存储在pathRow和pathCol数组中，系统最多支持40个路径点。使用statpos和numpts等变量跟踪机器人沿着这条路径的进程，并使用xy_control函数处理路径点之间的移动。
进行了广泛的测试，以调整寻路参数，确保在各种环境中可靠导航。另一个关键方面是在不同的坐标系之间进行转换。机器人在全局坐标系中运行，而a *算法使用基于网格的表示。这个转换是通过一个特定的映射过程来处理的，该过程解决了几个挑战：反转路径序列(因为a *返回从目的地到起点的路径)，将网格列直接转换为x坐标，并使用操作10 - pathRow翻转y坐标系统，以说明网格和机器人坐标系统之间的不同原点。如果没有这种仔细的坐标转换，机器人将试图导航到物理世界中不正确的位置。通过适当地桥接这些坐标系，代码确保当机器人执行xy_control()函数在路径点之间移动时，它准确地遵循由A*算法计算的最佳路径，同时成功地避开其环境中的障碍物。
机器人的目的地:
机器人阵列与我们在实验室里使用的非常相似。主要区别在于，Astar算法使用的是标准的robotdest数组，所以为了在Astar之后迭代路径点，我们需要创建一个新的robotdest数组，并在单独的xy_control中实现。为了实现第二个控件，我们使用了新的数组、一个新的statpos变量和一个路径选择标志来在控件之间切换。
球的收集和整理：
机器人的收球系统基于开关箱结构，使用多个开关箱来设置和控制系统的速度、转弯、电机控制和延迟。闸门和滑舌由伺服电机控制，伺服电机由控制变量Gangle和Tangle设置，如下图所示，每40毫秒检查和更新一次。
机器人通过使用状态机来控制机器人所处的状态以及根据条件采取的行动，从而在各种功能之间进行切换。对于避障、捡球和分拣功能，设置某些参数以触发机器人状态根据最小条件变化。机器人的第一个状态是检查状态，检查机器人上的任何传感器是否满足相应状态的最低要求。第一个检查是避障，如果机器人检测到前方6英寸的障碍物，则使用激光雷达传感器使机器人减速，并切换到右墙跟随状态。第二次和第三次检查，用相机扫描洋红色和绿色高尔夫球，用OpenMV处理图像，确定哪个在它前面。当机器人在2秒内检测到至少6个相应颜色的像素时，切换到粉红色和绿色，然后切换到案例20和30。
品红球的球收集和分类在案例20到26中完成，并遵循跟踪球、决定收集仓、抓取球、关闭门和重置收集器的过程。案例20，如下图所示，是跟踪案例，跟踪并将球居中于机器人前方。在这种状态下，机器人的相机视觉中心被定义为相机列值的中间的球心。然后，代码让机器人遵循一系列if语句，这些语句将设置vref和turn值来跟随并接近球的位置，直到它在机器人前面的设定距离(由它距离视觉范围顶部的像素数决定)。
一旦达到最小行阈值，机器人状态变为21，并遵循如下所示的结构。状态21是一个简单的延迟状态，它会让机器人停止3秒，让收集箱中的球沉淀下来，防止它们在门打开时滚出。状态22通过改变控制门的角度打开门，并设置左仓的舌角进行分拣，然后变为状态24。在这种情况下，机器人向前移动1秒，然后进入状态25，状态25继续向前移动机器人一秒，同时关闭它身后的门。最后，案例26用了另外2秒来重置舌和门的角度，然后继续前进并收集更多的球。第二个绿色球的收集过程与此相同，但使用为其设置的颜色阈值，并将收集到的球汇集到正确的容器中。
进行了多次测试，以调整收集、延迟和角度的阈值，并确保可靠和可重复的收集过程。颜色阈值是通过将高尔夫球放置在适当的环境中，并使用相机的视觉来捕获球的图像，并进行处理以隔离颜色，并考虑灯光和反射的眩光。收集测试具有多种用途，并且有助于对机器人进行故障排除以避免干扰。标准的测试程序是将两个彩色高尔夫球放在机器人的路径周围，看看它如何跟踪和收集它们。在最初的测试中，门的位置会阻挡摄像机对球的视线，阻止它离开控制状态，导致它卡在球的前面。为了说明这一点，打开状态的栅极角进一步打开，以清除相机视野。测试发现的另一个问题是，在收集过程中，由于球在垃圾箱中的动量，一些球会滚出来。因此，延迟情况被添加到收集步骤中，以确保球在收集过程中和门打开时不会继续向前滚动。为了进一步说明滚动情况，增加了25个程序，以关闭并舀回球到垃圾箱中，以防它们设法向前滚动。最后，当能够可靠地、重复地按照预期收集各种球，并进行分类，而不被卡住或溢出球时，就可以确定球的收集和分选完成。
四月标签检测：
为了允许机器人检测四月标记，提供了一个main.py文件。唯一需要做的更改是设置runtag=1和set tag。Id <=所需的标签数。
四月标签定位：
定位算法从检测航位推算箱结构中的四月标签开始。如果之前没有检测到四月标签，机器人切换到60号箱子，停止运动半秒，由计数变量计时。然后另一个开关将Robot设置为case 61，其中vref又被设置为1.0半秒(由count变量决定)。这是为了使机器人更靠近四月标签，以提高OpenMV相机内置的z距离和z角度四月标签检测的精度。最后，机器人切换到机箱62，这是四月标签和相机之间距离计算的位置。
机器人的速度和转弯速度设置为0。开始对相机和四月标签之间的z距离进行求和操作(该操作将为负值，因为它将四月标签视为初始点，而四月标签视为与该点之间的负距离)。这个总和将持续5秒，之后它将除以被计数的次数(5000次)，由count变量计时，并将比例因子设为12，以便以英尺为单位计算平均值。这个平均值减少了相机在z距离上产生的噪声误差。四月标签的预期x和y坐标在全局变量中设置，并且根据机器人是否面向全局正x或负x方向将计算机器人的当前位置。这些方程式的详细描述如下。机器人的当前位置设置为这些计算点，使用x = xpred[0][0]和y = xpred[1][0]来解释漂移，设置计数器变量，因此标签不会被重复检测，然后机器人切换回锯齿模式。
避障:
避障遵循一个简单的右墙跟随程序，以避免在其路径上的任何东西。当激光雷达在机器人前方6英寸处检测到障碍物时，它将一个名为right_wall_follow_state的标志设置为1，并切换到下图所示的情形10。当标志为1时，机器人将根据前方激光雷达距离向左转弯，一旦前方距离大于最小阈值，机器人将停止左转，标志将变为2。当它为2时，机器人将切换到右墙跟随，并根据设定的距离和右侧前方激光雷达的值平衡与右墙的距离。
旋转速度受到一个旋转阈值的限制，该阈值使数值饱和，并防止旋转过快。如果前方出现低于阈值的另一面墙，机器人将再次转向并重复该过程，直到整整5秒过去，并且与前方读数至少有1.5英尺的距离，然后返回状态1。通过在正方形测试区域中测试正确的墙壁，并在其路径中放置多个障碍物并测量其避开这些障碍物的性能，可以微调阈值和值。除了激光雷达的前方和右侧读数外，还定义了多个其他范围，如下图所示，以便进行其他避障过程或检查参数。这些值也可以用来调整机器人在走廊和通道的中心。
球的下降:
与大多数其他流程一样，创建了一个案例结构，这次我们在读取与丢弃区相对应的特定AprilTag时进入了案例结构。这些掉落区域是颜色协调的，所以每个标签都有自己的盒子结构，可以掉落绿色或品红的高尔夫球。该结构包括一个方法，一个延迟(防止高尔夫球滚动太多)，一个门打开和舌驱动，一个反向箱，和一个门关闭箱。每个case结构还需要一个标志为' 0 '，在丢弃序列期间设置为1。
