Henrik Klaus Martin

import java.io.IOException;


import lejos.navigation.CompassPilot;
import lejos.nxt.*;
//import lejos.navigation.*;


public class MCLcar {
    public static void main (String args []) throws IOException, InterruptedException {

        carBT bt = new carBT();

        LightSensor ls1 = new LightSensor(SensorPort.S1);
        LightSensor ls2 = new LightSensor(SensorPort.S2);
        LightSensor ls3 = new LightSensor(SensorPort.S3);

        final float wheelDiameter = 56; //mm
        final float trackWidth = 111; //mm

        CompassPilot myCompassPilot = new CompassPilot(SensorPort.S4, wheelDiameter, trackWidth, Motor.B, Motor.A, false);
        myOrders order = new myOrders();

        int localDirection;
        int line;

        myCompassPilot.calibrate();

        // Creating bt connection:
        LCD.clear();
        LCD.drawString("Enter to connect",0,0);
        LCD.refresh();
        while(! Button.ENTER.isPressed())
        {}
        bt.connect();

        // Put the map the right way.
        LCD.clear();
        LCD.drawString("Car face norht",0,0);
        LCD.drawString("on the map",0,1);
        LCD.drawString("and press ENTER",0,2);
        LCD.refresh();
        while(! Button.ENTER.isPressed())
        {}

        Thread.sleep(500); // break before next;

        while(! Button.ENTER.isPressed())
        {
            LCD.clear();
            LCD.drawString("Turn map and car",0,0);
            LCD.drawString("so angle is 0.", 0, 1);
            drawData("angle:", myCompassPilot.getAngle(), 5);
            Thread.sleep(300);
        }

        LCD.clear();
        LCD.drawString("Ready for orders.",0,0);

        while (! Button.ESCAPE.isPressed())
        {
            // Receive orders with bluetooth
            try {
                order = bt.receiveData();
            } catch (IOException e) {
                //e.printStackTrace();
            }
            // Clear display
            line = 0;
            LCD.clear();
            // write orderdata to display:
            drawData("Order T:", order.getTheta(), line++);
            drawData("Order D:", order.getDistance(), line++);

            // Turn
            localDirection = order.getTheta();
            if(localDirection != myCompassPilot.getAngle())
                myCompassPilot.rotateTo(localDirection, false);     //rotates to the specified angle/direction.
            // write new direction to LCD
            localDirection = myCompassPilot.getAngle();
            drawData("Compass:", localDirection, line);

            line++;

            // Reset Tacho counters / Distance
            myCompassPilot.resetTachoCount();

            // Drive strait forward
            myCompassPilot.travel(order.getDistance(), false);

            Thread.sleep(10); // ms

            // Write on the display.
            drawData("Distance", (int)myCompassPilot.getTravelDistance(), line);
            drawData("LS1:", ls1.readValue(), line+1);
            drawData("LS2:", ls2.readValue(), line+2);
            drawData("LS3:", ls3.readValue(), line+3);

            // Direction according to north
            localDirection = myCompassPilot.getAngle();
            //sends back measurements:
            drawData("Distance:", (int)myCompassPilot.getTravelDistance(), line+4);
            try {
                bt.sendData((int)-(Math.sin(myCompassPilot.getAngle()*Math.PI/180)*myCompassPilot.getTravelDistance()),
                            (int)-(Math.cos(myCompassPilot.getAngle()*Math.PI/180)*myCompassPilot.getTravelDistance()),
                            localDirection,
                            ls1.readValue(), ls2.readValue(), ls3.readValue());

            } catch (IOException e) {
                //e.printStackTrace();
            }
        }
    }

    private static void drawData(String s, int i, int LCDrow)
    {
        LCD.drawString(s,0, LCDrow);
        LCD.drawInt(i,s.length()+1, LCDrow);
        LCD.refresh();
    }
}