04 跟上前面的车 - CoCube LEARN

在舞龙表演里，龙头先动，后面的身体一节一节跟上；在自然界里，尺蠖向前爬行时，身体后半段也会不断追上前半段。本案例要做一个类似的 leader-follower 队形：前面的 CoCube 是“领队”，后面的 CoCube 会根据前车的位置自动追上去。

这个程序使用 ESP-NOW 在机器人之间发送位置消息。每辆车都会持续广播自己的位置；如果某辆车发现自己应该跟随前一辆车，就会读取前车位置，并在距离过远时向前车移动。

准备两辆或更多 CoCube，下载同一个程序 leader_follower 在线程序。这个案例需要使用 CoCube 的地图定位功能，请把机器人放在可识别定位的地图上运行（足球地图、迷宫地图、透明地图或者其他定制地图）。

运行后，每辆车屏幕上会显示自己的编号：

按 A 键可以让编号减 1，按 B 键可以让编号加 1。建议从 1 开始编号，如果不小心调到 0，按 B 键调回即可。设置好编号后，把几辆车放在地图上。移动编号 1 的车，编号 2 的车会尝试跟上；如果有编号 3 的车，它会继续跟随编号 2，形成一条“车队长龙”。

完整程序

课堂演示可以按这个顺序进行：

这个案例的关键是给每辆车一个编号 ID。

每辆车都会不断通过 ESP-NOW 发送自己的位置：

发送内容：X坐标,Y坐标,方向
发送编号：自己的 ID

接收消息时，机器人会先判断这条消息是不是来自“前一辆车”：

如果 我的 ID == 收到的 ID + 1：
    这条消息来自我要跟随的前车

例如：

这样所有机器人都运行同一个程序，只需要设置不同编号，就能形成 leader-follower 队形。

程序启动时会做三件事：

ID = 1
D_limit = 60
设置 ESP-NOW 频道为 13
设置 ESP-NOW 分组为 255
广播 "send_pos"
显示当前 ID

D_limit 是跟车距离阈值。当前车和后车距离大于等于 60 时，后车开始追；距离小于 60 时，后车停止。

send_pos 脚本会每隔 50 毫秒发送一次当前位置：

重复执行：
    espNow 发送 pair：
        字符串 = X坐标,Y坐标,方向
        数字 = ID
    等待 50 毫秒

这里用 ESP-NOW 的 pair 消息有两个好处：

当机器人收到 ESP-NOW 消息后，会读取发送者的编号 id。

如果 我的 ID == id + 1：
    读取前车的位置
    计算我和前车之间的距离 D
    广播 "go!"

程序会把前车发来的字符串拆成三个数据：

robot_x = 前车 X 坐标
robot_y = 前车 Y 坐标
robot_theta = 前车方向

当前程序主要使用 robot_x 和 robot_y，也就是前车的位置。然后用两车坐标差计算距离：

dx = 前车 X - 我的 X
dy = 前车 Y - 我的 Y
D = sqrt(dx * dx + dy * dy)

如果 D 太大，说明后车落后了；如果 D 不大，说明后车已经跟得足够近。

收到 go! 广播后，机器人根据距离决定动作：

如果 D >= D_limit：
    move to target robot_x robot_y 50
否则：
    CoCube wheels break

也就是说，后车不是一直开动，而是只在距离超过阈值时，才向前车当前位置移动。

程序中包含了一个 跟随目标 的高级积木，需要打开高级模式后，才能在 CoCube 库中出现。它会让机器人先朝向目标点，再一边修正方向、一边向目标移动：

计算目标点距离
如果距离大于 3：
    朝向目标点
    重复直到距离小于 3：
        重新计算距离和角度误差
        根据角度误差调整左右轮速度

这样后车就不会只是直线乱冲，而是会根据目标点方向不断修正左右轮速度。相比于CoCube机器人库中的“移动到目标点”函数， 跟随目标 为非阻塞式的，即你可以不停地给机器人发送新的坐标点，机器人永远向最新的坐标点移动。