Beispiel #1
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));
    }
  }
  /**
   * The run() method tracks the distance between an object and the robot and the angle at which the
   * object is detected. If the distance between the obstacle and the robot is less than 3 feet or
   * the left/right touch sensors are pressed, then turn boolean _detected=false to _detected=true.
   * This is constantly tracked via a while loop.
   */
  public void run() {
    int
        obstacle_distance; // initialize obstacle_distance, the distance between the robot and an
                           // object
    _detected = false; // set boolean _detected to be false initially

    // this loop will be constantly running
    while (true) {
      obstacle_distance =
          _ear.getDistance(); // obstacle_distance gets the distance b/w robot and object
      _angle = motor.getTachoCount(); // _angle retrieves the angle of the motor at detection time
      /*if object is less than three feet away and the absolute value of _angle is less than 40,
       * or when the touch sensors are pressed, then change _detected to be true and this will affect
       * the methods in Racer.
       */
      if ((obstacle_distance < 35 && Math.abs(_angle) < 40)
          || _leftHand.isPressed()
          || _rightHand.isPressed()) {
        _detected = true;
      }
    }
  }
  /**
   * This recursive method is responsible for finding the blue styrofoam block and returning it to
   * the green/red zone. The search and detect process will begin when the robot has reached the
   * green/red zone.
   *
   * @param x The x position of where the robot will start the search for the blue styrofoam block
   * @param y The y position of where the robot will start search for the blue styrofoam block
   */
  public void doSearchAndDetect(double x, double y) { // Searching inside the green/red zone
    if (firstTry) {
      odox = odometer.getX();
      odoy = odometer.getY();
      odotheta = odometer.getTheta();
      dashboard.turnLeft(40);
      try {
        Thread.sleep(750);
      } catch (Exception e) {
      }
    } else if (Math.abs(odox - odometer.getX()) > 15 && Math.abs(odoy - odometer.getY()) > 15) {
      odox = odometer.getX();
      odoy = odometer.getY();
      odotheta = odometer.getTheta();
      dashboard.turnLeft(40);
      try {
        Thread.sleep(750);
      } catch (Exception e) {
      }
    }
    firstTry = false;
    // Initializing the heading angle when a robot detects something 35 cm away
    detectionAngle = odometer.getTheta();
    Point greenZone = new Point((int) x, (int) y); // Remembering green zone
    dashboard.turnLeft(70); // Beginning the 360 degrees scan
    boolean clawIsDown = false;

    // The boolean found never becomes true so the robot will keep spinning until
    // it finds something then moves to a different zone or it will complete 1 full
    // revolution from its starting angle then move to a different zone.
    while (!found) {
      // Condition for the robot to move to a different area to search since it has
      // completed one full revolution
      if (Math.abs(odox - odometer.getX()) < 10
          && Math.abs(odoy - odometer.getY()) < 10
          && Math.abs(odotheta - odometer.getTheta())
              < 5) { // Case where no objects in search radius
        if (directionCounter
            == 1) { // First time around, go to a point 15 cm south west of the green zone bottom
                    // left corner
          directionCounter++;
          navigation.travelTo(greenZone.getX() - 15, greenZone.getY() - 15, false);
        } else if (directionCounter
            == 2) { // Second time around, go below the middle of the green zone
          directionCounter++;
          navigation.travelTo(greenZone.getX() + 30, greenZone.getY() - 15, false);
        } else if (directionCounter == 3) { // Third time, go north easy of of the green zone
          directionCounter++;
          navigation.travelTo(greenZone.getX() + 30, greenZone.getY() + 45, false);

        } else if (directionCounter
            == 4) { // Last time around, go to the middle of the playing field
          directionCounter = 1;
          navigation.travelTo(180, 180, false);
        }
        // After traveling to the new scan zone, perform the search and scan
        doSearchAndDetect(greenZone.getX(), greenZone.getY());
      } else {
        // If the ultrasonic sensor senses something within 35 centimeters, go to the object
        if (usLow.getFilteredDistance() <= 35
            && Math.abs(odometer.getTheta() - badHeading) > 30) { // Range to go check out object
          detectionAngle = odometer.getTheta(); // Saving heading angle when the object is detected
          blockAngle = odometer.getTheta();
          previousDistance = usLow.getFilteredDistance();
          navigation.turnTo(
              detectionAngle
                  - 30.0); // Turning to past where object was detected so it can analyze the full
                           // block
          Stopwatch stopwatch = new Stopwatch();
          // Scanning whole object for 3 seconds to find shortest point of the object to robot
          while (stopwatch.elapsed() < (3 * Math.pow(10, 3))) {
            dashboard.turnRight(
                70); // Turning to the right to analyze object since it overturned initially
            if (usLow.getFilteredDistance() < previousDistance) {
              dashboard.stop();
              previousDistance = usLow.getFilteredDistance();
              blockAngle = odometer.getTheta();
            }
          }

          dashboard.stop();
          navigation.turnTo(blockAngle); // Turn to block
          // Remembering tacho count in case object is a wooden block
          // so it will back up to exactly the same spot
          originalTacho = dashboard.getRightTacho();
          Stopwatch watch = new Stopwatch();
          while (usLow.getFilteredDistance() >= 10) {
            // If robot hasn't reached object in 8 seconds, most likely nothing there and false
            // positive
            if (watch.elapsed() > 9 * Math.pow(10, 3)) {
              badHeading = odometer.getTheta();
              newTacho = dashboard.getRightTacho();
              dashboard.rotateMotor(-(newTacho - originalTacho), -(newTacho - originalTacho), 200);
              dashboard.stop();
              doSearchAndDetect(greenZone.getX(), greenZone.getY());
            }
            // Travel to the object by going forward
            dashboard.goForward(200);
          }
          // Making a check for a wooden block by using the top ultrasonic sensor
          if (usHigh.getFilteredDistance()
              < 25) { // Brown Block so back up distance traveled to the block
            dashboard.stop();
            badHeading =
                odometer
                    .getTheta(); // Saving the angle of the wooden block so it doesn't go back when
                                 // it sees it again
            newTacho = dashboard.getRightTacho();
            dashboard.rotateMotor(-(newTacho - originalTacho), -(newTacho - originalTacho), 200);
            dashboard.stop();
            doSearchAndDetect(greenZone.getX(), greenZone.getY());
          } else { // Go forward a bit more to get closer to the styrofoam block
            for (int i = 0; i < 200; i++) {
              dashboard.goForward(70);
            }
          }
          // 2nd check for brown block to make sure when closer to object
          if (usHigh.getFilteredDistance() < 25) {
            dashboard.stop();
            badHeading = odometer.getTheta();
            newTacho = dashboard.getRightTacho();
            dashboard.rotateMotor(-(newTacho - originalTacho), -(newTacho - originalTacho), 200);
            dashboard.stop();
            doSearchAndDetect(greenZone.getX(), greenZone.getY());
          } else { // Return to dropzone then drop off and re-do scan
            newTacho = dashboard.getRightTacho();
            // If doing search but the block is in the green/red zone, don't pick it up again
            if ((odometer.getX() > greenZone.getX() && odometer.getY() > greenZone.getY())
                && (odometer.getX() < greenZone.getX() + 30
                    && odometer.getY() < greenZone.getY() + 30)) {
              badHeading = odometer.getTheta();
              dashboard.rotateMotor(-(newTacho - originalTacho), -(newTacho - originalTacho), 200);
              dashboard.stop();
              doSearchAndDetect(greenZone.getX(), greenZone.getY());
            }
            dashboard.stop();
            // Reached a good distance from the styrofoam block so back up
            // and get ready for the juggle.
            Stopwatch secondWatch = new Stopwatch();
            while (secondWatch.elapsed() < 3 * Math.pow(10, 3)) {
              dashboard.goBackward(100);
            }
            dashboard.stop();
            if (!clawIsDown) {
              // Arms are currently up so drop them since the block is
              // detected and close by
              lifter.dropClaw();
              clawIsDown = true;
            }
            // Driving forward until the block is within 7 cm
            while (usLow.getFilteredDistance() >= 7) {
              dashboard.goForward(100);
            }
            juggle(); // Juggle to reposition the block so it is easier to pick
            dashboard.stop();
            // Drive forward 8 cm so the block will be picked up perfectly
            dashboard.goForward(8, false);
            // Remember where the block was picked up so the robot
            // will return here after dropping off in the designated zone
            previousPickUp.setLocation(odometer.getX(), odometer.getY());
            dashboard.stop();
            lifter.lift(); // Picking up the block
            isCarrying = true;
            // Driving back to the green zone (a bit further to account) for the arms
            // and at this distance, the block is dropped off in the middle of the zone
            navigation.travelTo(greenZone.getX() + 45, greenZone.getY() + 45, false);
            // If it is the nth*2 time, robot will stack.
            if (!firstIteration) {
              lifter.stackLift();
            }
            navigation.turnTo(180); // Turning so the block will end up in the middle of the zone
            dashboard.goForward(
                2,
                false); // Going forward 2 cm so even closer to block (used for stacking purposes)
            lifter.dropBlock();
            Stopwatch secondStop = new Stopwatch();
            // After dropping it off, back up then lift arms
            while (secondStop.elapsed() < 3.5 * Math.pow(10, 3)) {
              dashboard.goBackward(100);
            }
            dashboard.stop();
            lifter.postLift();
            firstIteration = !firstIteration; // Only making stacks of 2 so reverse boolean
            navigation.travelTo(
                greenZone.getX() - 15,
                greenZone.getY(),
                false); // Travel to side of green zone so blocks are not touched when navigating
                        // out of the zone
            navigation.travelTo(
                previousPickUp.getX(),
                previousPickUp.getY(),
                false); // Return to previous position of where styrofoam block was found
            doSearchAndDetect(
                greenZone.getX(), greenZone.getY()); // Do search and detect at the new position
          }
        }
      }
    }
  }
 private void randomTurn() {
   int rnd = (int) Math.round(Math.random() * (double) 4);
   Motor.A.rotate(turnValue[rnd]);
 }