Position Estimation Program

The position estimation program implements gyroscope enhanced odometry. The program sends to the robot basic motion instructions using the available buttons, while it computes and displays its location and heading angle. The left and right buttons make the robot turn left and right. The center button serves multiple functions; a single click increases the linear speed of the robot, holding this button for about half a second will stop the robot and eventually reverse the direction it travels. The program computes and displays the states of the robot as it moves. Below is an excerpt of the full program.

void EstimatePosition(Pos &pos)
	//Get encoder information
	int right_cnt = MotorRotationCount(RIGHT_MOTOR);
	int left_cnt = MotorRotationCount(LEFT_MOTOR);
	//Estimate linear displacement
	pos.mDist = ((right_cnt-last_right_cnt)
		+ (left_cnt - last_left_cnt)) / 2 * ENCODER_SF;
	//Get gyro data and convert it to radians
	pos.mAng = XglReadAngle() / 100 * PI/180.0;
	//Compute position
	pos.mX += pos.mDist * cos(pos.mAng);
	pos.mY += pos.mDist * sin(pos.mAng);
	last_right_cnt = right_cnt;
	last_left_cnt = left_cnt;

Prev << Control
Next >> Drawing Robot