import lejos.nxt.*;
import lejos.navigation.*;

/**
 * Testing the compass pilot.
 * @author Roger
 */
public class CompassPilotTst
{

    public static void main(String[] args)
    {
        System.out.println("CompassPilot Test");
        Button.waitForPress();
        CompassPilot pilot = new CompassPilot(SensorPort.S3, 2.25f, 4.8f, Motor.A, Motor.C);
        pilot.calibrate();
        LCD.drawInt(pilot.getAngle(), 4, 0, 0);
        Button.waitForPress();
        pilot.rotateTo(90);
        pilot.getCompass().resetCartesianZero();
        pilot.travel(20);
        LCD.drawInt((int) pilot.getTravelDistance(), 4, 0, 1);
        LCD.drawInt(pilot.getAngle(), 5, 8, 1);
        pilot.rotateTo(180);
        pilot.travel(-20);
        LCD.drawInt((int) pilot.getTravelDistance(), 4, 0, 2);
        LCD.drawInt(pilot.getAngle(), 5, 8, 2);
        Button.waitForPress();
    }
}