navigation updated with completed dijkstra's algo
Dependents: R5 2016 Robotics Team 1
Diff: navigation.cpp
- Revision:
- 4:a5d44517c65c
- Parent:
- 3:27cc2bacd6af
- Child:
- 5:d0954e0aecc9
diff -r 27cc2bacd6af -r a5d44517c65c navigation.cpp --- a/navigation.cpp Wed Feb 17 16:47:15 2016 +0000 +++ b/navigation.cpp Tue Feb 23 23:36:03 2016 +0000 @@ -168,12 +168,12 @@ float targetAngle; float targetDist; float differenceAngle; - + while (!route.empty() ) { target = route.top(); route.pop(); - + for (int i = 0; i < graph[vertex].size(); i++) { if (graph[vertex][i].neighbor == target) @@ -183,35 +183,35 @@ break; } } - + if (targetAngle != angle) { differenceAngle = targetAngle - angle; - + if (abs(differenceAngle) > 180) { - if (differenceAngle < 0) // + if (differenceAngle < 0) // differenceAngle = 360 - abs(differenceAngle); else differenceAngle = -(360 - differenceAngle); } - + else { /* do nothing */ } - + /* send -(differenceAngle) to motors */ int returnVal = drive.move(0, ((-differenceAngle)*(PI / 180.0)) ); - pc.printf("\nChanged angle %f degrees to %f", differenceAngle, targetAngle); - + pc.printf("\nChanged angle %f degrees to %f", differenceAngle, targetAngle); + // wait for move to complete while(!drive.isMoveDone()) { wait(1e-6); } } - + /* send targetDist to motors */ int returnVal = drive.move(targetDist, 0); pc.printf("\nMoved forward a distance of %f to node %i", targetDist, target); @@ -221,7 +221,7 @@ { wait(1e-6); } - + vertex = target; angle = targetAngle; } @@ -254,4 +254,4 @@ { pc.printf("Node %i to %i = dist - %f, angle - %f\n", i, j, graph[i][j].distance, graph[i][j].angle); } -} +} \ No newline at end of file