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