Here's some code which should allow you to reproduce the bug:
- Code: Select all
import java.util.*;
import lejos.nxt.*;
import lejos.robotics.localization.*;
import lejos.robotics.navigation.*;
/**
* Main class and entry point for the Minestorm Robot.
*
* @author Krzysztof Dziemborowicz
* @version 2011.0920
*/
public class MinestormRobot {
// A blank line that is LCD.DISPLAY_CHAR_WIDTH long
private static final String BLANK_LINE = " ";
/**
* <code>MinestormRobot</code> entry point.
*
* @param args command line arguments (ignored).
*/
public static void main(String[] args) {
// Make controllers
DifferentialPilot pilot = new DifferentialPilot(5.6, 11.75, Motor.B, Motor.C);
OdometryPoseProvider poseProvider = new OdometryPoseProvider(pilot);
NavPathController pathController = new NavPathController(pilot, poseProvider);
// Set speed
pilot.setTravelSpeed(10.0);
pilot.setRotateSpeed(45.0);
// Make a long zig-zag path
ArrayList<WayPoint> waypoints = new ArrayList<WayPoint>(4);
int x = 0, y = 0;
for (int i = 0; i < 100; i++) {
if (i % 4 == 0) {
x += 20;
} else if (i % 4 == 2) {
x -= 20;
} else {
y += 20;
}
waypoints.add(new WayPoint(x, y));
}
// Trace the path
pathController.followRoute(waypoints, true);
long lastScreenRefresh = Long.MIN_VALUE;
while (pathController.isGoing()) {
// Hammer getPose()
Pose pose = pathController.getPoseProvider().getPose();
// Show pose on the screen every 100 ms
if (lastScreenRefresh + 100 < System.currentTimeMillis()) {
drawCenteredLine(Integer.toString((int) pose.getX()), 0);
drawCenteredLine(Integer.toString((int) pose.getY()), 1);
drawCenteredLine(Integer.toString((int) pose.getHeading()), 2);
lastScreenRefresh = System.currentTimeMillis();
}
}
Pose pose = pathController.getPoseProvider().getPose();
drawCenteredLine(Integer.toString((int) pose.getX()), 0);
drawCenteredLine(Integer.toString((int) pose.getY()), 1);
drawCenteredLine(Integer.toString((int) pose.getHeading()), 2);
Sound.twoBeeps();
Button.waitForPress();
}
/**
* Draws a centred string on the LCD on the specified line, clearing the line before doing so.
*
* @param s the string to draw.
* @param line the line number on which to draw the string.
*/
protected static void drawCenteredLine(String s, int line) {
LCD.drawString(BLANK_LINE, 0, line);
if (s != null) {
if (s.length() > LCD.DISPLAY_CHAR_WIDTH) {
s = s.substring(0, LCD.DISPLAY_CHAR_WIDTH);
}
LCD.drawString(s, (LCD.DISPLAY_CHAR_WIDTH - s.length()) / 2, line);
}
}
}
I've made an attempt at my own implementation of OdometryPoseProvider (based heavily on the one that came with leJOS NXJ 0.9.0) that appears to solve the problem for us:
- Code: Select all
import lejos.geom.*;
import lejos.robotics.localization.*;
import lejos.robotics.navigation.*;
/**
* Keeps track of the robot <code>Pose</code> using odometry (dead reckoning) data contained in a
* Move, which is supplied by a MoveProvider. This class is based on the OdometryPoseProvider
* supplied with leJOS, except that it works.
*
* @author Krzysztof Dziemborowicz
* @version 2011.0920
*/
public class OdometryPoseProvider implements PoseProvider, MoveListener {
// Current pose
private float x = 0, y = 0;
private float heading = 0;
// Current move
private MoveProvider mp;
private float baseAngle, baseDistance;
/**
* Creates a new <code>MinestormPoseProvider</code>.
*
* @param mp a <code>MoveProvider</code> to listen to.
*/
public OdometryPoseProvider(MoveProvider mp) {
mp.addMoveListener(this);
}
/**
* Returns the current estimate of the robot's <code>Pose</code>.
*
* @return the robot's <code>Pose</code>.
*/
@Override
public synchronized Pose getPose() {
if (mp != null) {
updatePose(mp.getMovement());
}
return new Pose(x, y, heading);
}
/**
* Sets the current <code>Pose</code>.
*
* @param pose the current <code>Pose</code>.
*/
@Override
public synchronized void setPose(Pose pose) {
Point loc = pose.getLocation();
x = loc.x;
y = loc.y;
heading = pose.getHeading();
}
/**
* Invoked whenever a movement starts.
*
* @param move the move that just started.
* @param mp the MoveProvider that invoked this method.
*/
@Override
public synchronized void moveStarted(Move move, MoveProvider mp) {
this.baseAngle = 0;
this.baseDistance = 0;
this.mp = mp;
}
/**
* Invoked whenever a movement stops.
*
* @param move the move that just started.
* @param mp the MoveProvider that invoked this method.
*/
@Override
public synchronized void moveStopped(Move move, MoveProvider mp) {
updatePose(move);
this.baseAngle = 0;
this.baseDistance = 0;
this.mp = null;
}
/**
* Updates the current pose with whatever incremental movement has occurred since the last move
* started.
*
* @param move the current move.
*/
private synchronized void updatePose(Move move) {
float angle = move.getAngleTurned() - baseAngle;
float distance = move.getDistanceTraveled() - baseDistance;
if (move.getMoveType() == Move.MoveType.TRAVEL || Math.abs(angle) < 0.2f) {
double headingRad = Math.toRadians(heading);
x += distance * Math.cos(headingRad);
y += distance * Math.sin(headingRad);
} else if (move.getMoveType() == Move.MoveType.ARC) {
double headingRad = Math.toRadians(heading);
double angleRad = Math.toRadians(angle);
double radius = distance / angleRad;
x += radius * (Math.sin(headingRad + angleRad) - Math.sin(headingRad));
y += radius * (Math.cos(headingRad) - Math.cos(headingRad + angleRad));
}
heading = normalise(heading + angle);
baseAngle = move.getAngleTurned();
baseDistance = move.getDistanceTraveled();
}
/**
* Returns the angle normalised, such that -180 < angle <= 180.
*
* @param angle an angle.
* @return the normalised angle.
*/
private static float normalise(float angle) {
while (angle > 180) {
angle -= 360;
}
while (angle <= -180) {
angle += 360;
}
return angle;
}
}
Can anyone else reproduce the problem? And can anyone confirm whether my analysis of what is going on here is correct? Or am I way off?
