PennyNavigator.java
import javax.microedition.sensor.HeadingChannelInfo; import lejos.geom.Point; import lejos.nxt.Button; import lejos.nxt.LCD; import lejos.nxt.Motor; import lejos.robotics.localization.OdometryPoseProvider; import lejos.robotics.navigation.DifferentialPilot; import lejos.robotics.navigation.Pose; public class PennyNavigator { private static DifferentialPilot pilot = new DifferentialPilot(5.6, 11.1, Motor.C, Motor.B); private static OdometryPoseProvider poseProvider = new OdometryPoseProvider(pilot); private static int lineCount = 0; public static void main (String[] aArg) throws InterruptedException { Thread.sleep(500); LCD.drawString("Press enter", 0, 0); while(!Button.ENTER.isPressed() && !Button.ESCAPE.isPressed()); if (Button.ENTER.isPressed()) { LCD.drawString("Starting", 0, 0); Thread.sleep(1000); pilot.setTravelSpeed(40); pilot.setRotateSpeed(40); goTo(200,0); goTo(100,100); goTo(100,-50); goTo(0,0); Pose currentPose = poseProvider.getPose(); LCD.drawString("X" + (int)currentPose.getX() + " Y" + (int)currentPose.getY(), 0, lineCount+1); while(! Button.ENTER.isPressed()); } } public static void goTo(float x, float y) { Pose currentPose = poseProvider.getPose(); float heading = currentPose.relativeBearing(new Point(x, y)); float distance = currentPose.distanceTo(new Point(x, y)); LCD.drawString("X" + (int)currentPose.getX() + " Y" + (int)currentPose.getY() + " nH" + heading, 0, lineCount); LCD.drawString((new Point(x, y)).getX() + " " + (new Point(x, y)).getY(), 0, 7); lineCount++; pilot.rotate(heading); pilot.travel(distance); } }