// method runs when robot is in a dead end and has to back up and go around
public static void turnAround(boolean r, MapSystem ms, Movement mv) {
mv.turn(ms); // 180 degree turn
movRow(); // move forward
// check for empty adjacent space
boolean[] scanResults = new boolean[3];
scanResults = mapObj.scanAll();
// Move to said empty space
if (scanResults[0] && !scanResults[2]) { // Obstacle on the Left
mv.turn(scanResults[0], ms); // turn right
movRow();
} else { // Obstacle on the Right
mv.turn(!scanResults[2], ms); // turn left
movRow();
mv.turn(!scanResults[2], ms);
movRow();
// move forward 2 cells, scanning as we go
// if an obstacle is still on the same axis as the first
// class it as a different obstacle
// move forward a cell
// scan again - recursive
// move back to the correct column
// if at a 'limit cell', turn left instead of right
}
}
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));
}
}
// Method scans input and maps it; if obstacle is in front, left and right,
// backpedal. If there is a space adjacent but an obstacle at the front,
// turns to move around the obstacle using the available space.
public static void movRow() {
double nextCell = mapObj.basicProb(); // work out probability
// output to RConsole through BlueTooth
// btObj.stringToRCon("Object Probability in next Cell: %2f" + nextCell);
// mapObj.printMap(columns, numOfRowCells);
// sonar scan around of the robot
boolean[] scanResults = new boolean[3];
scanResults = mapObj.scanAll();
mapObj.updateMap(scanResults);
// btObj.stringToRCon("Scan Results: " + scanResults[0] + ", " + scanResults[1] + ", " +
// scanResults[2]);
// mapObj.printMap(columns, numOfRowCells);
if (scanResults[1]) { // if there's an obstacle in front
// if there's an obstacle to the left & right. i.e. a dead end
if (scanResults[0] && scanResults[2]) {
turnAround(scanResults[2], mapObj, movObj); // move backwards
} else {
// call movAround() method to navigate around the obstacle
movAround(scanResults[1], mapObj, movObj);
}
} else {
movObj.nextCell(mapObj); // move forward a cell
}
}
// Method to move around an obstacle - runs when obstacle is in front
public static void movAround(boolean r, MapSystem ms, Movement mv) {
mv.turn(r, ms); // true = right turn; false = left turn
movRow(); // scan and move forward if necessary
mv.turn(!r, ms); // invert the boolean to turn the other way
// move forward twice to pass the obstacle
movRow();
++loop2;
movRow();
++loop2;
mv.turn(!r, ms); // turn to face original column
boolean extend = mapObj.scanAhead(); // flag to check object is passed
while (extend) {
mv.turn(r, ms); // turn to move along the axis
movObj.nextCell(mapObj);
++loop2;
mv.turn(!r, ms); // face the original column
if (!mapObj.scanAhead()) { // if an obstacle is not found anymore
extend = false; // break the while loop
} else {
}
}
movRow(); // move into the original column
mv.turn(r, ms); // face the correct way to continue the patrol
}
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();
}
}
}
// Method to shift columns (X Axis)
public static void movCol(boolean b) {
movObj.turn(b, mapObj); // rotate 90 degrees - true = right; false = left
movRow(); // move forward a cell
movObj.turn(b, mapObj); // rotate 90 degrees - true = right; false = left
}
