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