Khepera III Toolbox/The Toolbox/Modules/odometry goto

From Wikibooks, open books for an open world
< Khepera III Toolbox‎ | The Toolbox‎ | Modules
Jump to: navigation, search

The odometry_goto module implements a simple feedback algorithm a differential-drive vehicle to move towards a given position (x, y). It uses the odometry_track module to estimate the current position of the robot.


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

// Initialize the modules

// Initializes tracking and goto structures
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

	// 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;


This module is always used in combination with the odometry_track module.

odometry_goto_start sets up a sOdometryGoto structure and assigned a sOdometryTrack structure to it. The latter must have been initialized beforehand (but could have been initialized much earlier and used already).

The target position is set with odometry_goto_set_goal. Inside the main loop, odometry_goto_step, called right after odometry_track_step, calculates the new wheel speeds and put them inside the result structure. These speed values should be set khepera3_drive_set_speed. Note that the target position may be changed at any time. The odometry_goto_step function will calculate the motor speeds for the currently configured target position.

In addition to the motor speeds, the result structure provides a two more useful fields:

  • closetogoal: Set as soon as the robot comes so close to the target position that it starts decelerating.
  • atgoal: Set if the robot has reached the target position.