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 void run() {
int count = 0;
while (true) {
LCD.drawString("" + Integer.toString(count++), 6, 0);
count %= 1000;
try {
Thread.sleep(100);
} catch (InterruptedException e) {
}
}
}
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() {
motor_left.resetTachoCount();
motor_left.regulateSpeed(true);
Movement.motor_left.smoothAcceleration(true);
int previousCommandCount = -1;
while (true) {
if (Movement.getCommandCount() == previousCommandCount) {
try {
Thread.sleep(10);
} catch (InterruptedException e) {
}
continue;
}
previousCommandCount = Movement.getCommandCount();
setToAngle(ControlCentre.getTargetSteeringAngleLeft());
int new_angle = getToAngle();
if (new_angle < 10) LCD.drawString(" ", 8, 1);
else if (new_angle < 100) LCD.drawString(" ", 9, 1);
LCD.drawString(Integer.toString(new_angle), 7, 1);
LCD.drawString("R", 11, 1);
int cur_angle = getCurrentSteeringAngle();
double delta = new_angle - cur_angle;
final double C = Movement.rotConstant;
double turn_angle = 0;
if (Math.abs(delta) < thresholdAngle / 2.0) {
continue;
} else if (Math.abs(delta) >= thresholdAngle / 2.0 && Math.abs(delta) < thresholdAngle) {
delta = thresholdAngle * delta / Math.abs(delta);
}
setCurrentSteeringAngle((int) (cur_angle + delta) % 360);
if (delta != 0 && Math.abs(delta) < 180) {
turn_angle = C * delta;
} else if (delta >= 180 && delta < 360) {
turn_angle = -C * (360 - delta);
} else if (delta <= -180) {
turn_angle = C * (360 + delta);
} else {
/* No turning needed */
continue;
}
motor_left.rotate((int) Math.round(turn_angle));
}
}
public void run() {
while (true) {
boolean kick = ControlCentre.getKickState();
if (kick) {
LCD.drawString("K,", 0, 1);
Movement.motor_kick.setSpeed(900);
Movement.motor_kick.rotate((-120 * (5 / 3)));
Movement.motor_kick.rotate((120 * (5 / 3)));
} else {
LCD.drawString("_,", 0, 1);
}
try {
Thread.sleep(100);
} catch (InterruptedException e) {
}
}
}
// 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();
}
public static void main(String[] args) throws Exception {
String connected = "Connected";
String waiting = "Waiting...";
String closing = "Closing...";
while (true) {
LCD.drawString(waiting, 0, 0);
LCD.refresh();
// BTConnection btc = Bluetooth.waitForConnection();
BTConnection btc = Bluetooth.waitForConnection(0, NXTConnection.RAW);
LCD.clear();
LCD.drawString(connected, 0, 0);
LCD.refresh();
DataInputStream dis = btc.openDataInputStream();
DataOutputStream dos = btc.openDataOutputStream();
for (int i = 0; i < 100; i++) {
int n = dis.readInt();
LCD.drawInt(n, 7, 0, 1);
LCD.refresh();
dos.writeInt(-n);
dos.flush();
}
dis.close();
dos.close();
Thread.sleep(100); // wait for data to drain
LCD.clear();
LCD.drawString(closing, 0, 0);
LCD.refresh();
btc.close();
LCD.clear();
}
}
public void run() {
Multiplexor chip = new Multiplexor(SensorPort.S4);
while (true) {
int targetLeft = ControlCentre.getTargetDriveLeftVal();
LCD.drawString(Integer.toString(targetLeft) + ",", 2, 1);
switch (targetLeft) {
case 0:
chip.setMotors(0, 0, 0);
break;
case 4:
chip.setMotors(0, 0, 0);
break;
case 1:
chip.setMotors(1, 1, 0);
break;
case 2:
chip.setMotors(1, 2, 0);
break;
case 3:
chip.setMotors(1, 3, 0);
break;
case 5:
chip.setMotors(-1, 1, 0);
break;
case 6:
chip.setMotors(-1, 2, 0);
break;
case 7:
chip.setMotors(-1, 3, 0);
break;
}
int targetRight = ControlCentre.getTargetDriveRightVal();
LCD.drawString(Integer.toString(targetRight) + " L", 4, 1);
switch (targetRight) {
case 0:
chip.setMotors(0, 0, 1);
break;
case 4:
chip.setMotors(0, 0, 1);
break;
case 1:
chip.setMotors(1, 1, 1);
break;
case 2:
chip.setMotors(1, 2, 1);
break;
case 3:
chip.setMotors(1, 3, 1);
break;
case 5:
chip.setMotors(-1, 1, 1);
break;
case 6:
chip.setMotors(-1, 2, 1);
break;
case 7:
chip.setMotors(-1, 3, 1);
break;
}
try {
Thread.sleep(10);
} 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();
}
static void joinSwarm() {
// start the f****n' swarmthread NOW!
swarmthread = new ClientSwarmThread();
swarmthread.start();
}