public void disconnect() {
if (controlAbstractionThread != null) {
controlAbstractionThread.interrupt();
try {
controlAbstractionThread.join();
} catch (InterruptedException e) {
Thread.currentThread().interrupt();
}
controlAbstractionThread = null;
}
vision.disconnect();
navdataChannel.disconnect();
controlCommandInterface.disconnect();
lastRotationVector = null;
integratedRotationVector[0] = 0;
integratedRotationVector[1] = 0;
integratedRotationVector[2] = 0;
speedVector[0] = 0;
speedVector[1] = 0;
speedVector[2] = 0;
}
public void connect(String remoteAddress) {
disconnect();
try {
InetAddress remoteInetAddress = InetAddress.getByName(remoteAddress);
controlCommandInterface.connect(remoteInetAddress);
try {
navdataChannel.connect(remoteInetAddress);
try {
vision.connect(remoteInetAddress, this);
} catch (IOException e) {
navdataChannel.disconnect();
throw e;
}
controlAbstractionThread = new Thread(this);
controlAbstractionThread.start();
} catch (IOException e) {
controlCommandInterface.disconnect();
throw e;
}
} catch (IOException e) {
e.printStackTrace();
}
}
