Esempio n. 1
0
  private void elevatorCollision() {
    LCD.clear();
    LCD.drawString("Collision: " + collision, 0, 4);
    Delay.msDelay(3000);
    float[] dist = new float[sonicSensor.getDistanceMode().sampleSize()];
    sonicSensor.getDistanceMode().fetchSample(dist, 0);

    if ((dist[0] - DISTANCE_TO_WALL) > 0.005) {
      drive.turnRight(5, false);
    } else {
      drive.turnLeft(5, false);
    }
    if (collision == "Wall") {
      drive.stop();
    } else if (collision == "Left Wall") {
      drive.turnRight(5, false);
    } else if (collision == "Right Wall") {
      drive.moveDistance(drive.maxSpeed(), -2);
      drive.turnLeft(20, false);
    }
  }
Esempio n. 2
0
  /**
   * Estimates the collision object depending on current program
   *
   * @param program Current program running
   * @param delay Time to wait in ms
   * @param destructionMode If collision object is a robot KILL IT ^^
   * @return collision Estimated collision object
   */
  public String estimateCollision(String program, int delay) {

    float[] leftSample = new float[leftSensor.sampleSize()];
    float[] rightSample = new float[rightSensor.sampleSize()];
    int leftTouch = 0;
    int rightTouch = 0;
    int touchCount = 0;
    boolean leftTouched = false;
    boolean rightTouched = false;
    collision = "none";
    int run = 0;

    while (run < 4) {
      leftSensor.fetchSample(leftSample, 0);
      leftSensor.fetchSample(leftSample, 0);
      leftTouch = (int) leftSample[0];
      rightTouch = (int) rightSample[0];
      LCD.drawString("Left: " + String.valueOf(leftTouch), 0, 2);
      LCD.drawString("right: " + String.valueOf(rightSample), 0, 3);
      // for Bridge, ChainBridge, Elevator, Rolls, Seesaw
      // Elevator dürfte nicht vorkommen
      // TODO: Collision mit einem Sensor Roboter
      if (leftTouch != 0 && rightTouch != 0) {
        drive.stop();
        Delay.msDelay(delay);
        touchCount++;
        if (touchCount == 2) {
          collision = "Wall";
          break;
        }
      } else if (leftTouch != 0) {
        leftTouched = true;
        drive.stop();
        Delay.msDelay(delay);
        if (!rightTouched) {
          touchCount++;
        } else {
          rightTouched = false;
        }
        if (touchCount == 2) {
          collision = "Left Wall";
          break;
        }
      } else if (rightTouch != 0) {
        rightTouched = true;
        drive.stop();
        Delay.msDelay(delay);
        if (!leftTouched) {
          touchCount++;
        } else {
          leftTouched = false;
        }
        if (touchCount == 2) {
          collision = "Right Wall";
          break;
        }
      } else if (touchCount < 2 && touchCount != 0) {
        touchCount = 0;
        collision = "Robot";
        break;
      }
      run++;
    }
    if ((program == "ChainBridge"
            || program == "LineFollowing"
            || program == "Rolls"
            || program == "Seesaw"
            || program == "Maze")
        && collision == "Robot") {
      if (destructionMode) {
        killThemAll("");
      } else {
        drive.stop();
        Delay.msDelay(TIME_TO_WAIT);
      }
    } else if (program == "Elevator") {
      elevatorCollision();
    } else if (program == "Bridge") {

    } else if (program == "EndBoss") {

    }
    return collision;
  }