About the “Goal Seeker” Challenge
In order to complete the goal seeker challenge, it is essential to give your robot a sense of location, i.e. to ensure that it knows where it is, where it is heading, and how to get to an arbitrary (x,y) point.
Here is, therefore, and outline and the details for a program to solve the previous “coordinate system” challenge so you can create your “goals seeker” program based on this one.
Please note that people reported problems using the compass sensor. The code below, therefore, provides functions with and without compass sensor use, you should pick whichever works better for you.
My own experience is:
- using a compass does improve turning accuracy
- using a compass to go straight does not work so well
- the compass should be mounted parallel to the droving plane and at least 10-15 cm away from the motors
- you need to calibrate the compass before you run the robot in a new physical location (use the “calibrate” function listed below)
Anyway, here are the pieces for a successful “coordinate system” program.
The Traveler Program – Outline
publicclass Traveler
/*
* This program allows the robot to keep track of its position
* in the world. We assume that the robot is initially positioned
* at the origin, facing north along the y-axis (a 90 degree angle
* to x-axis.
*
* All distances are assumed to be in cm, all angles in degrees. All
* angles are counted in math orientation, i.e. counter-clockwise.
*/
// constants
finalstaticfloatCORRECTION = 3.0f;
finalstaticintMAX_TURN_CORRECTIONS = 20;
finalstaticfloatCONVERT_DIST = 20.46f;
finalstaticfloatCONVERT_TURN = 1.93f;
finalstaticintSPEED_DRIVE = 500;
finalstaticintSPEED_TURN = 150;
finalstaticintSPEED_CALIBRATE = 30;
// state variables
static Motor leftMotor = Motor.A;
static Motor rightMotor = Motor.C;
static CompassSensor compass = new CompassSensor(SensorPort.S4);
staticintxLocation = 0; // in cm
staticintyLocation = 0; // in cm
staticintheading = 90; // in (counter-clockwise) degrees
/*The standard main method to begin the program.
*/
publicstaticvoid main(String args[]) throws Exception
/*This function moves the robot to the new (x,y)
* coordinates and updates its position and heading.
*/
publicstaticvoid goTo(int x, int y) throws Exception
/* This function computes the shortest angle (in
* degrees) between the current location and heading
* and the (x,y) coordinates.
*
* It will return an integer between -179 and 180.
*/
publicstaticint findAngleTo(double x, double y)
/* This function computes the distance between the
* current location and the (x,y) coordinates.
*
* It will return a positive integer
*/
publicstaticint findDistanceTo(int x, int y)
/* Finds the current compass heading as an integer
* between -179 and 180 instead of the standard compass
* heading from 0 to 360.
*/
publicstaticint findCompassHeading()
/* Turns the robot from it current heading by the
* specified degrees, using a compass sensor to verify the
* rotation. If 'angle' is positive, it
* turns counter-clockwise, if 'angle' is negative,
* it turns clockwise. If 'angle' is zero, nothing
* will happen.
*
* Note: Does NOT update the state variables!
*/
publicstaticvoid turnWithCompass(int angle) throws Exception
/* Turns the robot from it current heading by the
* specified degrees. If 'angle' is positive, it
* turns counter-clockwise, if 'angle' is negative,
* it turns clockwise. If 'angle' is zero, nothing
* will happen.
*
* Note: Does NOT update the state variables!
*/
publicstaticvoid turn(int angle)
/* Drives robot forward by specified distance. If 'distance'
* is not positive, nothing will happen. This method uses the
* compass sensor to correct deviations from a straight line.
*
* Note: Does NOT update the state variables!
*/
publicstaticvoid travelWithCompass(int distance)
/*Drives robot forward by specified distance. If 'distance'
* is not positive, nothing will happen.
*
* Note: Does NOT update the state variables!
*/
publicstaticvoid travel(int distance)
/* Method to calibrate compass by rotating *at least
* twice* very slowly. Requires two input variables:
*
* turnSpeed: int speed at which to turn. Must be slow
* so that one full turn takes 20 secs
* or more.
* convert: float conversion factor to convert degrees
* of wheel rotation into robot rotation
* degrees. Depends on wheel size and axis
* length. Try approx. 2.0f and see if that
* gives you at least two rotations.
*/
publicstaticvoid calibrate(int speed, float convert) throws Exception
/* Converts the input (degrees) to radians
*/
publicstaticfloat toRadians(int degree)
/*Converts the input (radians) to degrees
*/
publicstaticint toDegrees(double radians)
How to change “turn” to “turnWithCompass”
publicstaticvoid turn(int angle)
{
if (angle == 0)
return;
int numDegrees = (int)Math.round(Math.abs(angle)*CONVERT_TURN);
// set motors up for counter-clockwise rotation
Motor forwardMotor = leftMotor;
Motor backwardMotor = rightMotor;
// if angle is negative, switch motors for clockwise rotation
if (angle < 0)
{
forwardMotor = rightMotor;
backwardMotor = leftMotor;
}
forwardMotor.setSpeed(SPEED_TURN);
backwardMotor.setSpeed(SPEED_TURN);
forwardMotor.resetTachoCount();
backwardMotor.resetTachoCount();
forwardMotor.forward();
backwardMotor.backward();
while ((forwardMotor.getTachoCount() <= numDegrees) ||
(backwardMotor.getTachoCount() >= -numDegrees))
{
if (forwardMotor.getTachoCount() > numDegrees)
forwardMotor.stop();
if (backwardMotor.getTachoCount() < -numDegrees)
backwardMotor.stop();
}
forwardMotor.stop();
backwardMotor.stop();
}
Turning with Compass Sensor by adding a second function to use “turn” but correct mistakes if necessary:
publicstaticvoid turnWithCompass(int angle) throws Exception
{
int counter = 0;
compass.resetCartesianZero();
int error = angle;
while ((Math.abs(error) >= 1) &
(counter < MAX_TURN_CORRECTIONS))
{
turn(error);
error = angle - findCompassHeading();
if (error > 180)
error = error - 360;
elseif (error < -180)
error = error + 360;
counter++;
}
}
How to change “travel” to “travelWithCompass”
publicstaticvoid travel(int distance)
{
if (distance <= 0)
return;
int numDegrees = (int)Math.round(distance*CONVERT_DIST);
leftMotor.setSpeed(SPEED_DRIVE);
rightMotor.setSpeed(SPEED_DRIVE);
leftMotor.resetTachoCount();
rightMotor.resetTachoCount();
leftMotor.forward();
rightMotor.forward();
while ((leftMotor.getTachoCount() <= numDegrees) ||
(rightMotor.getTachoCount() <= numDegrees))
{
if (leftMotor.getTachoCount() > numDegrees)
leftMotor.stop();
if (rightMotor.getTachoCount() > numDegrees)
rightMotor.stop();
}
leftMotor.stop();
rightMotor.stop();
}
Traveling with Compass Sensor by adding proportional error correction:
publicstaticvoid travelWithCompass(int distance)
{
if (distance <= 0)
return;
int numDegrees = (int)Math.round(distance*CONVERT_DIST);
leftMotor.setSpeed(SPEED_DRIVE);
rightMotor.setSpeed(SPEED_DRIVE);
leftMotor.resetTachoCount();
rightMotor.resetTachoCount();
leftMotor.forward();
rightMotor.forward();
compass.resetCartesianZero();
while ((leftMotor.getTachoCount() <= numDegrees) ||
(rightMotor.getTachoCount() <= numDegrees))
{
int error = (int)Math.round(CORRECTION * findCompassHeading());
leftMotor.setSpeed(SPEED_DRIVE - error);
rightMotor.setSpeed(SPEED_DRIVE + error);
}
leftMotor.stop();
rightMotor.stop();
}
Utility Functions for Coordinate System challenge
publicstaticint findAngleTo(double x, double y)
{
double dx = (x - xLocation);
double dy = (y - yLocation);
int angle = toDegrees(Math.atan2(dy, dx));
int difference = angle - heading;
if (difference > 180)
return 360 - difference;
elseif (difference < -180)
return difference + 360;
else
return difference;
}
publicstaticint findDistanceTo(int x, int y)
{
int dX = x - xLocation;
int dY = y - yLocation;
return (int)Math.round((float)Math.sqrt(dX*dX + dY*dY));
}
publicstaticint findCompassHeading()
{
int degrees = (int)compass.getDegreesCartesian();
if (degrees > 180)
return degrees - 360;
else
return degrees;
}
publicstaticfloat toRadians(int degree)
{
return (float)(degree / 180.0 * Math.PI);
}
publicstaticint toDegrees(double radians)
{
return (int)Math.round((float)(radians/Math.PI * 180.0));
}
The function to put everything together:
publicstaticvoid goTo(int x, int y) throws Exception
{
int angle = findAngleTo(x, y);
int dist = findDistanceTo(x, y);
turnWithCompass(angle);
travelWithCompass(dist);
xLocation = x;
yLocation = y;
heading = heading + angle;
}
To travel to a goal while avoiding obstacles you clearly need to modify the “travel” function: you no longer can go forward for a fixed distance but you must be prepared to stop as you encounter an obstacle. So, copy your “traveler” code you just put together into a new “Seeker” class, add an Ultrasonic sensor as a state variable, and add a “travel carefully” function (you could remove the “travel” and “travelWithCompass”). That function travels forward by a given distance, unless it sees an obstacle ahead, in which case it stops. So it either moves forward by d, or moves forward by some distance and stopped before an obstacle. It also updates the (x,y) location of your robot to take the actually traveled distance into account.
How to travel and avoid obstacles
publicstaticvoid travelCarefully(float distance)
{
if (distance <= 0)
return;
int numDegrees = (int)Math.round(distance*CONVERT_DIST);
leftMotor.setSpeed(SPEED_DRIVE);
rightMotor.setSpeed(SPEED_DRIVE);
leftMotor.resetTachoCount();
rightMotor.resetTachoCount();
leftMotor.forward();
rightMotor.forward();
boolean obstacle = (sonar.getDistance() < MIN_DISTANCE);
boolean drive_on = ((leftMotor.getTachoCount() <= numDegrees) ||
(rightMotor.getTachoCount() <= numDegrees));
while ((drive_on) & (!obstacle))
{
if (leftMotor.getTachoCount() > numDegrees)
leftMotor.stop();
if (rightMotor.getTachoCount() > numDegrees)
rightMotor.stop();
obstacle = (sonar.getDistance() < MIN_DISTANCE);
drive_on = ((leftMotor.getTachoCount() <= numDegrees) ||
(rightMotor.getTachoCount() <= numDegrees));
}
leftMotor.stop();
rightMotor.stop();
distance = leftMotor.getTachoCount() / CONVERT_DIST;
xLocation = xLocation + (int)(distance *Math.cos(toRadians(heading)));
yLocation = yLocation + (int)(distance * Math.sin(toRadians(heading)));
LCD.drawString("driven:" + distance, 1, 4);
}
The previous “goTo” function also needs to change. It still computes the heading to turn to, the distance to move, but then attempts to move to the given coordinates. When it is done, either the robot arrived at (x, y) or it did stop in front of an obstacle. In either case, the state variables are updated appropriately so the robot still knows where it is.
publicstaticvoid goTo(int x, int y) throws Exception
{
int angle = findAngleTo(x, y);
int dist = findDistanceTo(x, y);
turnWithCompass(angle);
heading = heading + angle;
travelCarefully(dist);
}
To make use of this function, you could to put together a main function following this algorithm:
Calibrate compass sensor
(1)goTo the goal
if you reached the goal, we’re done. Otherwise
avoid obstacle
go to step (1)
Your avoidance strategy could make use of the fact that
- there are at most 3 obstacles
- they do not touch
- each obstacle is rectangular with length at least 20cm
So, it is till up to you to construct a suitable main function – best of luck!