Khepera III Toolbox/The Toolbox/Modules/odometry goto

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.

Synopsis

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

Description

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.