// Aims to establish a conection over Bluetooth
private static void connect() {
Thread tryingDisplay = new ScreenWriter("Trying to connect", 7);
tryingDisplay.start();
// Wait until connected
connection = Bluetooth.waitForConnection();
Thread connectedDisplay = new ScreenWriter("Connected", 7);
connectedDisplay.start();
inputStream = connection.openDataInputStream();
outputStream = connection.openDataOutputStream();
Thread openConnDisplay = new ScreenWriter("Connection Opened", 7);
openConnDisplay.start();
}
static void init() throws InterruptedException {
Sound.beep();
try {
File f = new File("CalibrationData.dat");
FileInputStream is = new FileInputStream(f);
colors = RescueColors.readObject(is);
is.close();
LCD.drawString("File read", 0, 0);
LCD.drawString("sucessfully", 0, 0);
} catch (IOException e) {
LCD.drawString("Error reading", 0, 0);
LCD.drawString("file", 0, 1);
Thread.sleep(1000);
System.exit(-1);
}
colors.printToLCD();
robot = new RescueRobot(colors);
Button.ENTER.waitForPressAndRelease();
logger = new MovementLogger(robot);
Thread t = new Thread(logger);
t.setDaemon(true);
t.start();
}
private static void collectMessage() throws InterruptedException {
boolean atend = false;
int N = 0;
while (atend == false) {
N = N + 1; // % 100;
Movement.setCommandCount(N);
LCD.drawString("Recv:" + Integer.toString(N), 2, 2);
try {
// Bluetooth.getConnectionStatus();
int message = inputStream.readInt();
LCD.drawString("Rcvd:" + Integer.toString(N), 2, 3);
if (message >= (1 << 26)) {
LCD.drawString("end" + Integer.toString(N), 12, 2);
atend = true;
// Thread atendDisplay = new ScreenWriter(Integer.toString(message),7);
LCD.drawString(Integer.toString(message), 0, 7);
// atendDisplay.start();
// System.exit();
LCD.drawString("stopped" + message, 0, 2);
} else if (message < (1 << 26)) {
// Thread newMessageDisplay = new ScreenWriter(Integer.toString(message),6);
// LCD.drawString("display"+Integer.toString(N), 6, 0);
// newMessageDisplay.start();
LCD.drawString(" ", 5, 6);
LCD.drawString("Msg:" + Integer.toString(message), 0, 6);
// LCD.drawString("decode:"+Integer.toString(N), 6, 1);
parseMessage(message);
// LCD.drawString("decoded:"+Integer.toString(N), 6, 0);
}
// inputStream.close();
// inputStream = connection.openDataInputStream();
} catch (IOException e) {
Thread errorConnection = new ScreenWriter("Error - connect back up", 7);
errorConnection.start();
// connection = Bluetooth.waitForConnection();
Thread connectedDisplay = new ScreenWriter("Connection Opened", 7);
connectedDisplay.start();
}
}
}
public void run() {
connect();
try {
collectMessage();
} catch (InterruptedException e) {
Thread msgInterruptDisplay = new ScreenWriter("Msg Col Interupt", 7);
msgInterruptDisplay.start();
}
try {
Thread.sleep(100);
} catch (InterruptedException e) {
}
}
public static void main(String[] args) throws InterruptedException {
Thread mainCommunicator = new Communicator();
Thread kickThread = new KickThread();
Thread driveThread = new DriveThread();
Thread steeringLeftThread = new SteeringLeftThread();
Thread steeringRightThread = new SteeringRightThread();
Thread counterThread = new CounterThread();
mainCommunicator.start();
kickThread.start();
driveThread.start();
steeringLeftThread.start();
steeringRightThread.start();
counterThread.start();
}