public void automatize(DataOutputStream dos, DataInputStream dis) {
int counter;
for (int walls = 0; walls < 6; walls++) {
counter = 0;
while (true) {
try {
if (counter == 0 && walls != 0)
dos.writeUTF("<s2r><move req=\"-5\"/><scan req=\"12\"/><rotate req=\"90\"/></s2r>");
else dos.writeUTF("<s2r><scan req=\"12\"/><move req=\"10\"/></s2r>");
dos.flush();
} catch (IOException e) {
System.out.println("Failed to send message:" + e);
}
try {
System.out.println("Receiving");
String newReply = dis.readUTF();
System.out.println(newReply);
RobotReplyMessage msg = parser.parse(newReply);
comm.addRobotReplyMessage(msg);
if (msg.getScanValueN(0) <= 25) {
break;
}
} catch (IOException e) {
System.out.println("Failed to read message:" + e);
}
counter++;
}
}
}
public void run() {
NXTConnector link;
boolean connected = false;
link = new NXTConnector();
while (true) {
connected = false;
while (!connected) {
// connected = link.connectTo("btspp://001653180896");
connected = link.connectTo("btspp://");
if (connected) System.out.println("Conntected to NXT.");
else {
System.out.println("Failed to connect to NXT.");
try {
Thread.sleep(1500);
} catch (InterruptedException e) {
System.out.println("Interrupted: " + e);
}
}
}
DataOutputStream dos = new DataOutputStream(link.getOutputStream());
DataInputStream dis = new DataInputStream(link.getInputStream());
RobotRequestMessage newMsg = null;
while (true) {
try {
while (newMsg == null) newMsg = comm.getRobotRequestMessage();
if (newMsg != null) {
dos.writeUTF(newMsg.getXML());
System.out.println("Forwarding client message: \n" + newMsg.getXML());
}
dos.flush();
} catch (IOException e) {
System.out.println("Failed to send message:" + e);
}
try {
System.out.println("Receiving");
String newReply = dis.readUTF();
System.out.println(newReply);
comm.addRobotReplyMessage(parser.parse(newReply));
} catch (IOException e) {
System.out.println("Failed to read message:" + e);
try {
link.close();
} catch (IOException e1) {
// TODO Auto-generated catch block
e1.printStackTrace();
}
break;
}
if (newMsg.automatic()) automatize(dos, dis);
newMsg = null;
}
}
}