Ejemplo n.º 1
0
  public PolygonObstacle getRobot() {
    Point2D.Double center = polymap.getRobotStart();
    PolygonObstacle robot = new PolygonObstacle();

    double x, y, angle;
    for (int i = 0; i < NUM_SIDES; i++) {
      angle = i * 2.0 * Math.PI / NUM_SIDES;
      x = RADIUS * Math.cos(angle);
      y = RADIUS * Math.sin(angle);
      robot.addVertex(x, y);
    }

    robot.close();

    return robot;
  }
Ejemplo n.º 2
0
  public void navigateRobot() {
    System.out.println("Path Index: " + pathIndex);
    System.out.println("goalPath Size: " + goalPath.size());
    System.out.println("locX, locY, locTheta" + locX + ", " + locY + ", " + locTheta);
    System.out.println("goalX, goalY, goalTheta: " + goalX + ", " + goalY + ", " + goalTheta);
    if (goalPath.size() > 0 && !done) {
      if (pathIndex < goalPath.size() - 1
          && Math.abs(goalX - locX) < 0.2
          && Math.abs(goalY - locY) < 0.2) {
        pathIndex++;
      }
      if (pathIndex == goalPath.size() - 1
          && Math.abs(goalX - locX) < 0.05
          && Math.abs(goalY - locY) < 0.05) {
        pathIndex++;
      }
      if (pathIndex >= goalPath.size()) {
        done = true;
        setVelocities(0.0, 0.0);
      }

      goalX = goalPath.get(pathIndex).x;
      goalY = goalPath.get(pathIndex).y;
      goalTheta = Math.atan2(goalY - locY, goalX - locX);
      if (goalTheta < 0) {
        goalTheta += 2 * Math.PI;
      }

      double error = goalTheta - locTheta;
      if (error > Math.PI) {
        error -= 2 * Math.PI;
      } else if (error < -Math.PI) {
        error += 2 * Math.PI;
      }
      if (Math.abs(error) < Math.PI / 8.0) {
        setVelocities(0.3, anglePID.update(error));
      } else {
        setVelocities(0, anglePID.update(error));
      }
      // setVelocities(0.2, anglePID.update(error));
    }
  }