import lejos.hardware.lcd.LCD;

// This code will test out a connection to the RTMS (Robot Tracking and Mapping System).
// You MUST get this working, otherwise you will not be able to complete some of the remaining labs.
public class TestRTMS {
    private static RBtoPCcom communicator;    // Class used to communicate with the RTMS
    
    public static void main(String[] args) {
        communicator = new RBtoPCcom(); // Open a BLUETOOTH connection to the RTMS
        
        communicator.waitForStart();    // Tell the PC you are ready now for the main loop
        
        Pose pose = communicator.requestPosition();  // Request the starting position from the PC
        
        LCD.drawString("x: " + pose.x, 0, 1);
        LCD.drawString("y: " + pose.y, 0, 2);
        LCD.drawString("angle: " + pose.angle, 0, 3);
        LCD.drawString("All Done", 0, 4);
        
        try{ Thread.sleep(5000); } catch(Exception e) {} // Wait 5 seconds
        
        communicator.closeConnection();
    }
}