Mechatronics Laboratory Supplement
XY point-to-point control (A gradient descent algorithm)
Gradient descent is originally an optimization algorithm. The idea is simple: given a point on a function, move small steps along the negative of the function’s gradient to reach a local minimum. In robotics and automation, this same concept is used as a path-planning algorithm: given a robot’s current position, and a desired position (a local minimum), take small steps towards the desired position. The function xy_control implements this algorithm for the Mechatronics Lab’s mobile robots.
/*
* Name: xy_control
*
* Description:
* This function calculates velocity and turn commands for
* the mobile robot to move from its current coordinates
* to some desired coordinates.
*
* Parameters:
* float *vref_forxy, float *turn_forxy: parameters passed
* by reference. They are overwritten with the new velocity
* and turn commands necessary to command the robot to move
* towards its goal.
* float turn_thres: if the robot’s turn command is greater
* than this value it will start rotating in place to
* avoid big turns. Big turns add a lot of drift to the
* robot’s dead-reckoning measurement. (2 is a good value.)
* float x_pos, float y_pos: robot’s current coordinates.
* float x_desired, float y_desired: robot’s goal
* coordinates.
* float theta_abs: robot’s current heading angle.
* float target_radius: the radius of target
* area around x_desired and y_desired. The values
* *vref_forxy and *turn_forxy are set to zero when
* the robot enters this area.
* float target_radius_near: the radius of a threshold
* area surrounding x_desired and y_desired.
* If the robot enters this area, the function
* returns a 1, else 0.
*
* Returns:
* 1 if the robot is closer than target_radius_near
* to (x_desired,y_desired), 0 otherwise.
*/
int xy_control(float *vref_forxy, float *turn_forxy,
float turn_thres,
float x_pos, float y_pos,
float x_desired, float y_desired,
float theta_abs,
float target_radius,
float target_radius_near);
Example Code: moves the robot toward the goal coordinates (robotXdest,robotYdest), as shown in Figure 2.
// Note: be sure to include the file xy.h
#include "xy.h" // Note the "" instead of >. This is for including files
// that are in the current directory.
// get vref and turn values by passing them by reference to xy_control.
xy_control(&vref, &turn, turnThld, robotX, robotY, robotXdest, robotYdest, robotAng, targetR, targetRn);
// Apply the updated vref and turn commands to move the robot towards
// the destination coordinates
PIVelControl(vref, turn);
Figure 2. Figure 3.
Exercise: Write the code necessary to achieve the task in Figure 3.
GE423, Mechatronic Systems xy_control, Page 2 of 2