import lejos.nxt.addon.*;
import lejos.nxt.*;
import lejos.robotics.navigation.*;

/**
    example of compass navigation? 

    @author Bagnall, course text, p. 326
    @author adapted by Sharon Tuttle
    @version 2015-04-03
*/

public class BlighbotCompass
{
    /**
        trying to use compass navigation??

        @param args not used
    */

    public static void main(String[] args)
    {
        System.out.println("MOOO!");
        CompassHTSensor compassSens = 
            new CompassHTSensor(SensorPort.S1);
        System.out.println("got compassSens?");

        CompassPilot pilot =
            new CompassPilot(compassSens, 4.32F, 15.5F, Motor.B,
                             Motor.C, true);
        System.out.println("got pilot?");

        Navigator robot = new Navigator(pilot);
        System.out.println("got robot?");

        robot.goTo(50, 0);  // 200
        System.out.println("part 1 done?");

        robot.goTo(25, 25);  // 100 100
        System.out.println("part 2 done?");

        robot.goTo(25, -12);  // 100 -50
        System.out.println("part 3 done?");

        robot.goTo(0, 0);
        System.out.println("part 4 done?");

        System.out.println("press button");
        Button.waitForAnyPress();
    }
}