Khepera III 工具箱/工具箱/模块/里程计 goto

odometry_goto 模块实现了一个简单的反馈算法，使差动驱动车辆移动到给定位置（$x$，$y$）。它使用 odometry_track 模块来估计机器人的当前位置。

概要

// Instantiate a track and a goto structure
struct sOdometryTrack ot;
struct sOdometryGoto og;

// Initialize the modules
odometry_track_init();
odometry_goto_init();

// Initializes tracking and goto structures
odometry_track_start(&ot);
odometry_goto_start(&og, &ot);

// Set a target position
odometry_goto_set_goal(&og, x, y);

// Move until the robot is close enough to the target position
while (og.result.atgoal == 0) {
	// Update position and calculate new speeds
	odometry_track_step(og.track);
	odometry_goto_step(&og);

	// Set the wheel speed
	khepera3_drive_set_speed(og.result.speed_left, og.result.speed_right);
}

// The final position (which may be slightly different from the target position)
... = og.track->result.x;
... = og.track->result.y;
... = og.track->result.theta;

描述

该模块始终与 odometry_track 模块结合使用。

odometry_goto_start 设置一个 sOdometryGoto 结构并为其分配一个 sOdometryTrack 结构。后者必须事先初始化（但可能已在更早时间初始化并使用过）。

目标位置使用 odometry_goto_set_goal 设置。在主循环中，odometry_goto_step（在 odometry_track_step 之后立即调用）计算新的轮速并将它们放入 result 结构中。这些速度值应设置为 khepera3_drive_set_speed。请注意，目标位置可以随时更改。odometry_goto_step 函数将针对当前配置的目标位置计算电机速度。

除了电机速度外，result 结构还提供了两个更有用的字段

• closetogoal：一旦机器人距离目标位置足够近以开始减速，就会设置该字段。
• atgoal：如果机器人已到达目标位置，则设置该字段。