// CommToConsole.java
//
// A class that demonstrates how to communicate between the PC and the NXT
//
// Simon Parsons
// 26th October 2013
//
// Based on an example from Davide Grossi

import lejos.nxt.*;
import lejos.robotics.navigation.*;
import lejos.robotics.localization.*;
import java.util.Random;
import lejos.nxt.comm.*;

public class CommToConsole{
    public static void main (String[] args) throws Exception{
	// None of this is required for communication
	DifferentialPilot pilot = new DifferentialPilot(3.22, 19.5, Motor.C, Motor.B);
	OdometryPoseProvider opp = new OdometryPoseProvider(pilot);
	Random randomGenerator = new Random();
	int move;
	
	// Open a connection to the PC
	RConsole.open();
	Thread.sleep(2000);
	
	// Make some random movements
	for(int i=0; i<10; i++){
	    move = randomGenerator.nextInt(3);
	    switch(move){
	    case 0:
		System.out.println("Forward!");
		pilot.travel(10);
		pilot.stop();
		RConsole.println("pose = " + opp.getPose());
		break;
		
	    case 1:
		System.out.println("Anticlockwise!");
		pilot.rotate(90);
		pilot.stop();
		RConsole.println("pose = " + opp.getPose());
		break;
		
	    case 2:
		System.out.println("Clockwise!");
		pilot.rotate(-90);
		pilot.stop();
		RConsole.println("pose = " + opp.getPose());
		break;
	    }
	}
    }
}