GoTo
This article is a stub. You can help RoboWiki by expanding it. |
Some robots use the coordinate system to move. These robots are called GoTo robots and usually use a type of GoTo code. Such as a GoTo Surfer. By using a GoTo method, a robot can go directly to a set of desired coordinates.
Standard GoTo
This is your standard GoTo method. It is fairly accurate on a single call.
/**
* This method is very verbose to explain how things work.
* Do not obfuscate/optimize this sample.
*/
private void goTo(double x, double y) {
/* Transform our coordinates into a vector */
x -= getX();
y -= getY();
/* Calculate the angle to the target position */
double angleToTarget = Math.atan2(x, y);
/* Calculate the turn required get there */
double targetAngle = Utils.normalRelativeAngle(angleToTarget - getHeadingRadians());
/*
* The Java Hypot method is a quick way of getting the length
* of a vector. Which in this case is also the distance between
* our robot and the target location.
*/
double distance = Math.hypot(x, y);
/* This is a simple method of performing set front as back */
double turnAngle = Math.atan(Math.tan(targetAngle));
setTurnRightRadians(turnAngle);
if(targetAngle == turnAngle) {
setAhead(distance);
} else {
setBack(distance);
}
}
Advanced GoTo
This GoTo function uses cosine to prevent moving when not facing in the desired direction.
private void go(double x, double y) {
/* Calculate the difference bettwen the current position and the target position. */
x = x - getX();
y = y - getY();
/* Calculate the angle relative to the current heading. */
double goAngle = Utils.normalRelativeAngle(Math.atan2(x, y) - getHeadingRadians());
/*
* Apply a tangent to the turn this is a cheap way of achieving back to front turn angle as tangents period is PI.
* The output is very close to doing it correctly under most inputs. Applying the arctan will reverse the function
* back into a normal value, correcting the value. The arctan is not needed if code size is required, the error from
* tangent evening out over multiple turns.
*/
setTurnRightRadians(Math.atan(Math.tan(goAngle)));
/*
* The cosine call reduces the amount moved more the more perpendicular it is to the desired angle of travel. The
* hypot is a quick way of calculating the distance to move as it calculates the length of the given coordinates
* from 0.
*/
setAhead(Math.cos(goAngle) * Math.hypot(x, y));
}
Smallest Codesize GoTo
This GoTo method has been optimized for codesize. It will not start moving full speed until generally pointing in the correct direction, and depending on how far away the go-to point is. This method should be called every round.
private void goTo(int x, int y) {
double a;
setTurnRightRadians(Math.tan(
a = Math.atan2(x -= (int) getX(), y -= (int) getY())
- getHeadingRadians()));
setAhead(Math.hypot(x, y) * Math.cos(a));
}