// 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

    }
  }
Esempio n. 2
0
  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
  }
Esempio n. 5
0
 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
 }