Example #1
0
  private boolean hitTheWall(Pose pose) {
    float poseAngle = (float) Math.toRadians(Const.angleToByte(pose.getHeading()) * 90);
    byte sin = (byte) Math.round(Math.sin(poseAngle)), cos = (byte) Math.round(Math.cos(poseAngle));
    byte mapPoseX = (byte) (Const.roundToMapArea(Const.middlePointToBlock(pose.getX()), "X") + cos),
        mapPoseY = (byte) (Const.roundToMapArea(Const.middlePointToBlock(pose.getY()), "Y") + sin);

    // return true and update pose if it hit an obstacle
    if (mapPoseX < 0
        || mapPoseY < 0
        || mapPoseX > 8
        || mapPoseY > 5
        || wayClass.getOccupancyMap(mapPoseX, mapPoseY) > 0) {
      Rover.poseProvider.setPose(
          new Pose(
              cos != 0
                  ? (mapPoseX * Const.BLOCK_WIDTH + (cos > 0 ? -10 : 10 + Const.BLOCK_WIDTH))
                  : pose.getX(),
              sin != 0
                  ? (mapPoseY * Const.BLOCK_WIDTH + (sin > 0 ? -10 : 10 + Const.BLOCK_WIDTH))
                  : pose.getY(),
              cacheWayAngle));

      return true;
    }
    return false;
  }
 public static double setDistanceToPoint(Pose actualPose, double destX, double destY) {
   destX = -destX;
   double d =
       Math.sqrt(
           (Math.pow((destX - actualPose.getLocation().getX()), 2.0d)
               + (Math.pow((destY - actualPose.getLocation().getY()), 2.0d))));
   return d;
 }
 // ---- obie funkcje przyjmuja wartoœci dodatnie, zarówno dla x jak i y --//
 // zwraca wartoœæ o jak¹ ma obróciæ siê robot aby staæ przodem do docelowych wspó³rzênych x,y
 public static double setDirection(Pose actualPose, double destX, double destY) {
   double newAngle =
       (Math.atan2(
           actualPose.getLocation().getY() - destY, actualPose.getLocation().getX() + destX));
   System.out.println(
       -(Math.toDegrees(angleDiff(Math.toRadians(actualPose.getHeading()), newAngle))));
   return -(Math.toDegrees(angleDiff(Math.toRadians(actualPose.getHeading()), newAngle)));
 } // u¿ycie:
 /**
  * Returns a waypoint of the current location.
  *
  * @return Waypoint, current location
  */
 @Override
 public Waypoint getLocation() {
   PoseProvider provider = nav.getPoseProvider();
   Pose pose = provider.getPose();
   return (Waypoint) pose.getLocation();
 }
Example #5
0
  public void run() {

    while (true) {
      if (_suppress) {
        Thread.yield();
        continue;
      }
      Pose oldPose = cachePose;
      cachePose = Rover.poseProvider.getPose();

      // get angle and distance where to travel
      int[] way = wayClass.getPredefinedWay(oldPose, cachePose);
      cacheWayAngle = way[1];
      cacheWayDistance = way[0] * Const.BLOCK_WIDTH;

      float angle = Const.normalizeAngle(cacheWayAngle - cachePose.getHeading());

      if (Const.angleToByte(angle) == 2 && cacheWayDistance == Const.BLOCK_WIDTH) {
        // if the robot is supposed to rotate 180 deg and travel one block, back out
        Rover.pilot.travel(-cacheWayDistance, true);
      } else {
        // otherwise rotate and travel the distance
        if (Math.abs(angle) > 2) {
          // float cacheGyro = Rover.getGyro();
          boolean touched = false;

          Rover.pilot.rotate(angle, true);

          while (Rover.pilot.isMoving()) {
            boolean[] touch = Rover.getTouch();
            if (touch[0] || touch[1]) touched = true;
            Thread.yield();
          }
          Pose newPose = Rover.poseProvider.getPose();

          /*
          float angleGyro = Rover.getGyro() - cacheGyro;

          // if the turn is incomplete, rotate by the difference
          // TODO maybe turn only when the difference is bigger
          if (angle > angleGyro + 5 || angle < angleGyro - 5) {
          	Rover.pilot.travel(touched ? -5 : 5);
          	cacheWayDistance += touched ? 5 : -5;
          	Rover.pilot.rotate(angle - angleGyro);

          	newPose = Rover.poseProvider.getPose();
          } */

          // update pose with normalize heading
          Rover.poseProvider.setPose(new Pose(newPose.getX(), newPose.getY(), cacheWayAngle));
        }

        float blockAngle = (float) Math.toRadians(cacheWayAngle);
        byte sin = (byte) Math.round(Math.sin(blockAngle)),
            cos = (byte) Math.round(Math.cos(blockAngle));
        byte
            blockX =
                (byte)
                    (Const.roundToMapArea(Const.middlePointToBlock(cachePose.getX()), "X")
                        + cos * (way[0] + 1)),
            blockY =
                (byte)
                    (Const.roundToMapArea(Const.middlePointToBlock(cachePose.getY()), "Y")
                        + sin * (way[0] + 1));

        // extend the distance to hit a wall
        if (blockX < 0
            || blockY < 0
            || blockX > 8
            || blockY > 5
            || wayClass.getOccupancyMap(blockX, blockY) > 0) {

          cacheWayDistance += Const.BLOCK_WIDTH;
          // TODO no way of updating pose if it doesn't hit wall
        }
        Rover.pilot.travel(cacheWayDistance, true);
      }

      // while the rover is traveling
      while (Rover.pilot.isMoving()) {
        boolean[] touch = Rover.getTouch();
        if (touch[0] || touch[1]) {
          Rover.pilot.quickStop();
          float sonic = Rover.getSonic();

          Pose hitPose = Rover.poseProvider.getPose();

          if (hitTheWall(hitPose)) {
            Rover.pilot.travel(-4);
          } else if (sonic > 15 && !(touch[0] && touch[1])) {
            // if the rover touches the obstacle by just one sensor
            // and distance is more than 15 cm
            float distanceTraveled = Rover.pilot.getMovement().getDistanceTraveled();
            Rover.pilot.travel(-4);
            Rover.pilot.rotate(touch[0] ? -20 : 20);

            Pose newPose = Rover.poseProvider.getPose();
            Rover.poseProvider.setPose(new Pose(newPose.getX(), newPose.getY(), cacheWayAngle));

            cacheWayDistance -= distanceTraveled - 4;
            Rover.pilot.travel(cacheWayDistance, true);

          } else {
            // if the obstacle touched by both sensors
            // or the distance is less than 15 cm
            Pose p = Rover.poseProvider.getPose();

            byte blockX = Const.roundToMapArea(Const.middlePointToBlock(p.getX()), "X"),
                blockY = Const.roundToMapArea(Const.middlePointToBlock(p.getY()), "Y");
            float blockAngle = (float) Math.toRadians(Const.angleToByte(p.getHeading()) * 90);
            byte sin = (byte) Math.round(Math.sin(blockAngle)),
                cos = (byte) Math.round(Math.cos(blockAngle));

            boolean isFree;
            try {
              isFree = wayClass.getOccupancyMap((byte) (blockX + cos), (byte) (blockY + sin)) < 0;
            } catch (ArrayIndexOutOfBoundsException e) {
              isFree = false;
            }

            if (!(touch[0] && touch[1]) && isFree) {
              Rover.pilot.rotate(touch[0] ? -90 : 90);
              Rover.pilot.travel(15);
              Rover.pilot.rotate(touch[0] ? 90 : -90);

              Rover.poseProvider.setPose(
                  new Pose(
                      (blockX + 1 / 2) * Const.BLOCK_WIDTH,
                      (blockY + 1 / 2) * Const.BLOCK_WIDTH,
                      Const.angleToByte(p.getHeading()) * 90));

              float distanceTraveled = Rover.pilot.getMovement().getDistanceTraveled();
              cacheWayDistance -= distanceTraveled;

              Rover.pilot.travel(cacheWayDistance, true);
            } else {
              System.out.println("ERROR: TRAVEL CORRECTION");
              // This shouldn't happen any more
              Button.LEDPattern(5);
            }
          }
        }
        Thread.yield();
      }
    }
  }