/**
   * * Move the robot and update the distribution with the action model
   *
   * @param distance
   * @param _heading
   * @param _sensorModel
   */
  private void move(
      float distance, Heading _heading, ActionModel _actionModel, SensorModel _sensorModel) {
    // move robot
    m_robot.translate(m_translationAmount);

    // now update estimate of position using the action model
    m_distribution = _actionModel.updateAfterMove(m_distribution, _heading);

    // if visualising, update the shown distribution
    if (m_mapVis != null) {
      m_mapVis.setDistribution(m_distribution);
    }

    // A short delay so we can see what's going on
    Delay.msDelay(1000);

    m_distribution =
        _sensorModel.updateAfterSensing(m_distribution, _heading, m_robot.getRangeValues());

    // if visualising, update the shown distribution
    if (m_mapVis != null) {
      m_mapVis.setDistribution(m_distribution);
    }

    // A short delay so we can see what's going on
    Delay.msDelay(1000);
  }
 public void run() {
   while (true) {
     connectToDevice();
     reportToDevice();
     closeConnection();
     Delay.msDelay(waitBetweenSends);
   }
 }
  // circle method: traces a semi-circle and a quarter circle
  // and takes two DifferentialPilot objects as method input
  public static void circle(DifferentialPilot Pilot1, DifferentialPilot Pilot2) {
    // Semi-Circle
    Pilot1.setTravelSpeed(5); // first drawing the arc for the semi-circle
    Pilot1.arc(36, 180); // robot arcs left for 180 degrees, guided by a 36 inch radius
    Pilot1.stop();
    Delay.msDelay(300);
    Pilot1.setRotateSpeed(30);
    Pilot1.rotate(90); // robot makes a 90 degree left turn
    Pilot1.stop();
    Delay.msDelay(300);
    Pilot1.setTravelSpeed(5);
    Pilot1.travel(
        72); // here, we're closing up the semi-circle (that is, tracing diameter of the circle)

    Delay.msDelay(1300);

    // Quarter-Circle
    Pilot2.setTravelSpeed(5);
    Pilot2.arc(12, 90); // robot arcs right for 90 degree (so forms 1/4 of circle)
    Pilot2.stop();
    Delay.msDelay(300);

    // loop to close up the quarter circle
    for (int l = 1; l <= 2; l++) {
      Pilot2.setRotateSpeed(30);
      Pilot2.rotate(90); // robot makes a 90 degree right turn
      Pilot2.stop();
      Delay.msDelay(300);
      Pilot2.setTravelSpeed(5);
      Pilot2.travel(12); // robot goes forward for 12 in (the radius of circle)
      Pilot2.stop();
      Delay.msDelay(300);
    }
  }
  // polygon method: traces a triangle and a pentagon, and takes a DifferentialPilot object as
  // method input
  public static void polygon(DifferentialPilot Pilot1) {
    // Triangle: having three sides, so for-loop goes from 1 to 3
    for (int i = 1; i <= 3; i++) {
      Pilot1.setTravelSpeed(5); // setting the travel and rotation speed for the robot
      Pilot1.setRotateSpeed(30);
      Pilot1.travel(24); // robot travels 2 feet forward
      Pilot1.stop();
      Delay.msDelay(300);
      Pilot1.rotate(120); // robot rotates left to form a 60 degree interior angle
      Pilot1.stop();
      Delay.msDelay(300);
    }
    Delay.msDelay(1300);

    // Pentagon: having five sides, so for-loop goes from 1 to 5
    for (int i = 1; i <= 5; i++) {
      Pilot1.setTravelSpeed(5); // setting the travel and rotation speed for the robot
      Pilot1.setRotateSpeed(30);
      Pilot1.travel(24); // robot travels 2 feet forward
      Pilot1.stop();
      Delay.msDelay(300);
      Pilot1.rotate(72); // robot rotates left to form 108 interior angle of pentagon
      Pilot1.stop();
      Delay.msDelay(300);
    }
    Delay.msDelay(1300);
  }
  /** Run. */
  public void run() {
    Motor.A.setSpeed(90);
    Motor.C.setSpeed(90);

    while (true) {
      moveForward();

      waitForReading();
      Motor.A.backward();
      Motor.C.backward();
      Delay.msDelay(1000);
      rotation(TURN_90_DEGREES, TURN_LEFT);
    }
  }
  private void connectToDevice() {
    try {
      btc = Bluetooth.waitForConnection();
      if (btc == null) {
        RConsole.println("[BT] Connection error!");
        throw new BluetoothStateException("Connection error!");
      }

      dos = btc.openDataOutputStream();

    } catch (BluetoothStateException e) {
      e.printStackTrace();
      Delay.msDelay(waitBetweenSends);
      connectToDevice();
    }
  }
 /**
  * If an abyss is detected, the robot will rotate 10 degrees, until the light sensor sees normal
  * ground again.
  */
 @Override
 public void action() {
   System.out.println("Abys");
   suppressed = false;
   while (lightSensor.getLightValue() < threshold && !suppressed) {
     pilot.rotate(10, true);
     if (!pilot.isMoving()) {
       Settings.whipAndBridgeCounter++;
     }
     Delay.msDelay(10);
   }
   while (pilot.isMoving() && !suppressed) {
     Thread.yield();
   }
   pilot.stop();
 }
 public static boolean findline() {
   TachoPilot p = robot.pilot;
   int[] angles = new int[] {30, -30, 60, -60, 90, -90, 0};
   int lastangle = 0;
   for (int angle : angles) {
     p.rotate((lastangle - angle) * 4, true);
     while (p.isMoving()) {
       if (linefound()) {
         Sound.beepSequence();
         p.stop();
         return true;
       }
       Thread.yield();
     }
     p.stop();
     lastangle = angle;
     Delay.msDelay(250);
   }
   return false;
 }
  private void reportToDevice() {
    short sendInt;

    if (bdetect.isBox()) {
      sendInt = stationIsOccupied;
    } else {
      sendInt = stationIsNotOccupied;
    }

    try {
      dos.writeInt(sendInt);
    } catch (IOException e) {
      RConsole.println("[BT] Send failed!");
      Delay.msDelay(waitBetweenSends);
      closeConnection();
      connectToDevice();
      reportToDevice();
    }

    RConsole.println("[BT] Status " + sendInt + " successfully reported!");
  }
  private int waitForReading() {
    int lightLevelReading;
    int distanceToObject;

    do {
      try {
        Thread.sleep(100);
      } catch (InterruptedException ex) {
      }

      lightLevelReading = lightSensorDown.readValue();
    } while (lightLevelReading > BLACK_WHITE_THRESHOLD);

    distanceToObject = ultrasonicSensor.getDistance();

    if (distanceToObject < 20) {
      Motor.A.setSpeed(1000);
      Motor.C.setSpeed(1000);
      pilot.travel(300);
      Delay.msDelay(10000);
    }

    return (lightLevelReading);
  }