Khepera III 工具箱/编程提示
为 Khepera III 机器人编写程序非常简单。但是，良好的程序结构和第一次就做好事情会节省您的时间 - 大量的时间！因此，在本章中，我们将讨论在实现此类程序时的一些设计选择。
一个简单的程序将具有以下结构
// Program initialization
commandline_init();
...
commandline_parse(argc, argv);
...
khepera3_init();
// Algorithm initialization
algorithm.configuration.wall_threshold = 1000;
...
khepera3_drive_start();
// Main loop
while (true) {
// Read sensors
khepera3_infrared_proximity();
...
// Calculate actuator response
speed_left = ...
speed_right = ...
// Termination condition
if (...) {
break;
}
// Set actuators
khepera3_drive_set_speed(speed_left, speed_right);
...
// Sleep for a while
usleep(algorithm.configuration.wait_us);
}
// Algorithm termination/cleanup if necessary
khepera3_drive_set_speed(0, 0)
大多数程序由一个主循环组成，该循环在初始化阶段后执行。主循环实现了所谓的感知-动作循环 - 读取传感器值、推导出要采取的动作并相应地设置执行器（电机）的循环。主循环有时还包含一个终止条件，该条件在达到目标后立即退出程序。
如果您查看现有程序的代码（例如 motion_followline），您会注意到上述结构在不同的函数中实现。通常，该结构会分解成以下函数
- help：打印帮助文本。
- algorithm_init：初始化算法。
- algorithm_run：执行主循环。
- main：初始化程序并调用上述函数。
此外，还定义了一个结构来保存算法的配置和状态（如果需要）。
// Algorithm configuration and state variables
struct sAlgorithm {
struct {
int wall_threshold;
...
int verbosity;
} configuration;
struct {
int remaining_targets;
...
} state;
};
// Declare an instance of that structure
struct sAlgorithm algorithm;
// Access to the variables
algorithm.configuration.wall_threshold = ...
algorithm.state.remaining_targets = ...
即使乍一看这似乎很复杂，使用这种嵌套结构也有两个优点。首先，所有算法变量都在一个地方声明。如果您想在其他地方重用该算法，您一目了然地看到需要哪些变量。其次，将配置变量和状态变量分类将帮助您以干净的方式实现算法。配置变量只会在初始化阶段写入，并且只应该在主循环中读取。然而，状态变量在主循环中读写。
通常，程序不止一个状态。例如，一个参与搜索任务的机器人可能会有一个探索状态，在该状态下它会随机四处走动，以及一个梯度跟踪状态，在该状态下它会尝试向目标移动。
实现这一点的最简单方法是使用不同的主循环，即每个状态一个主循环。代码骨架如下所示（请注意，由 k3-create-program 脚本 创建的模板程序使用的是这个骨架）。
// Algorithm variables
struct sAlgorithm {
...
struct {
void (*hook)(); // Pointer to the current state function
...
} state;
};
// Forward declaration of states
void state_exploration();
void state_gradient_follow();
void state_success();
// Algorithm initialization
void algorithm_init() {
...
// Set the initial state
algorithm.state.hook = state_exploration;
}
// The algorithm just switches from state to state by calling the current state function
void algorithm_run() {
while (1) {
algorithm.state.hook();
}
}
void state_exploration() {
// State initialization
...
// Main loop
while (1) {
// Read sensors
...
// State change condition
if (...) {
algorithm.state.hook = state_gradient_follow; // Switch to the gradient follow state
return;
}
// Set actuators
...
// Sleep for a while
usleep(algorithm.configuration.wait_us);
}
}
void state_gradient_follow() {
// State initialization
...
// Main loop
while (1) {
// Read sensors
...
// State change condition
if (...) {
algorithm.state.hook = state_exploration; // Switch to the exploration state
return;
}
// Termination condition
if (...) {
algorithm.state.hook = state_success; // Switch to the success state, which terminates the program
return;
}
// Set actuators
...
// Sleep for a while
usleep(algorithm.configuration.wait_us);
}
}
void state_success() {
// We are done
exit(0);
}
不要害怕函数指针（algorithm.state.hook）！它的声明可能看起来很复杂，但它非常易于使用，如您在上面的状态函数中看到的。添加、移动或删除状态变得极其容易，因为您可以将状态视为一个连续的代码块。如果机器人在一个特定状态下没有按预期工作，您可以修改其代码，而不会影响其他状态（如果您将所有内容都放在一个带有if串联的主循环中，通常会发生这种情况）。
此外，如果您先在纸上绘制您的算法作为状态机，您就可以立即按照您在纸上的图表实现代码。每个状态将对应于一个函数，每个箭头将对应于一个状态变化条件。
从您的 控制理论 课程中，您可能知道感知-动作循环必须以一定的最小频率运行，以满足稳定性标准。另一方面，以更高的速度运行该循环也会消耗更多能量。
上面代码示例中显示的感知-动作循环的速度由三个因素决定
- 通信开销，即读取传感器和设置执行器所需的时间。
- 处理开销，即从当前感知推导出动作所需的时间。
- 等待时间（usleep(algorithm.configuration.wait_us)），可以随意调整。
通信开销只能在一定程度上优化。您需要在机器人和 Korebot 板（处理器板）之间传输的字节数有一个硬性限制，才能获得一定的信息，而通信通道（I2C 总线）的固定速度约为 10 KB/s。因此，调用khepera3_infrared_proximity 函数来获取红外接近传感器值将在大约 3.1 毫秒后返回（如果 I2C 总线空闲），因为它必须从微控制器传输 31 个字节到微控制器。每个函数传输的字节数可以在 khepera3 模块 的 .h 文件中找到。
类似地，为了在给定传感器读数和当前状态的情况下推导出动作，需要进行一定数量的操作。与通信开销相比，基本数学运算速度很快，但大量使用log、exp 或三角函数会减慢主循环速度。此外，所有必须写入或从磁盘读取的内容也会涉及大量的开销。最后但并非最不重要的是，printf会严重减慢您的程序速度，具体取决于输出的重定向位置。
虽然可以准确计算通信开销，但处理开销需要在程序运行的环境中进行测量（例如，将 stdout 重定向到将在真实实验中重定向到的位置）。然后可以调整等待时间以实现所需的感知-动作循环频率。
是否使用线程是计算机科学中的一场重大争论。Khepera III 工具箱不会强迫您使用或不使用线程，但它是在大多数程序不会使用多线程的想法下编写的。
如果您使用的是上面提供的代码骨架，那么您自然不会使用线程（或者如果您想的话，可以使用一个线程）。考虑到没有太多可并行化的地方，这是有道理的：机器人一次只能处于一种状态，并且必须以精确的顺序执行感知-动作循环的内部。
在编写等待某些数据到达打开的文件句柄的程序时，select 系统调用 将很有用。此系统调用获取文件句柄列表，并在其中一个文件句柄上可用数据或超时发生时返回。select 系统调用是解决大多数问题的方案，在这些问题中，程序员认为他们需要线程。
作为示例，请查看 motion_arrowkeys 程序，该程序（以非阻塞方式）等待标准输入上的用户输入，并同时执行主循环。
如果您出于某种原因需要使用多线程，则在访问机器人传感器和执行器时需要谨慎。基本上，您不允许以并发方式访问khepera3 模块或i2cal 模块，因为这些模块使用静态分配的变量，因此不是线程安全的。对此有两个解决方案
- 以这样一种方式设计您的程序，即只有主线程（或另一个专用线程）访问这些模块。在某些情况下，这很简单，但在其他情况下可能会变得非常复杂。
- 同步对khepera3 模块的所有调用以及所有i2cal 事务。它们需要使用相同的互斥锁进行同步，因为khepera3 模块使用i2cal 模块与传感器通信，即大多数khepera3 函数是 I2C 总线上的事务。
如果您想知道：当前的实现有意不是线程安全的，因为这会使函数调用对单线程应用程序来说更加复杂和容易出错。但是，将来可能会添加一个线程安全实现（例如，作为单独的模块）。
就像我们将算法的实现分解为每个实现一个状态的函数一样，我们也可以将它们拆分为不同的程序，每个程序实现一个状态子集，并按正确的顺序执行它们。这行得通，实际上在许多情况下都是一种非常好的技术！
假设你想要测试不同的算法来遍历一个迷宫。迷宫入口在你的竞技场的左侧，出口在右侧。由于你想要多次运行实验，所以你想要你的机器人一旦到达出口就自动回到入口，例如通过沿着地面的一条线。当然，你可以为所有算法添加一个新的状态（state_go_back_to_entrance），并在到达出口时切换到该状态。但是，你也可以将其实现为一个单独的程序（maze_go_back_to_entrance），在你的任何迷宫遍历程序之后立即调用它。
启动和退出程序会产生一些处理开销，但从操作系统的角度来看，这种开销很小。不过，你应该小心你的初始化阶段。例如，如果你必须在程序启动时加载大文件（例如地图等），这可能会产生一些明显的延迟。此外，如果你通过 WLAN 连接远程启动程序，需要考虑一个小的传输延迟（~ 10 毫秒）。
关于何时拆分以及在同一程序中实现多少内容，并没有明确的答案。但是，这里有一些想法
- 如果这有助于你测试和调试算法的各个部分，请进行拆分。
- 如果这避免了在多个程序中包含完全相同的代码片段（如上面的迷宫遍历示例），请进行拆分。
- 如果你认为可以将至少一部分代码重用于其他地方，请进行拆分。
- 如果两个部分之间没有任何共同之处，请进行拆分。
- 如果转换很复杂（例如，多个目标状态可能），请不要进行拆分。
- 如果精确的时间或低延迟是一个问题，请不要进行拆分。
- 如果两个部分共享状态变量或大量数据，请不要进行拆分。
此外，通常建议将算法实现为仅运行一次，然后退出。如果你想在多次运行中测试你的算法，只需多次启动该程序（例如使用脚本）并将输出重定向到每次运行的不同文件。这种方法更加灵活和健壮。
measure_real_speed 示例使用脚本多次运行同一个程序，但使用不同的参数。