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);
    }
 
}
Unless otherwise stated, the content of this page is licensed under Creative Commons Attribution-ShareAlike 3.0 License