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; }
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)); } }