navigation updated with completed dijkstra's algo
Dependents: R5 2016 Robotics Team 1
Diff: navigation.cpp
- Revision:
- 12:6d37ea71da65
- Parent:
- 11:ea1809fcc27c
diff -r ea1809fcc27c -r 6d37ea71da65 navigation.cpp --- a/navigation.cpp Sat Apr 09 03:29:15 2016 +0000 +++ b/navigation.cpp Mon Feb 27 12:02:07 2017 +0000 @@ -208,8 +208,6 @@ /* send -(differenceAngle) to motors */ int returnVal = drive.move(0, ((-differenceAngle)*(PI / 180.0)) ); - pc.printf("\nChanged angle %f degrees to %f", differenceAngle, targetAngle); - // wait for move to complete while(!drive.isMoveDone()) { @@ -219,7 +217,6 @@ /* send targetDist to motors */ drive.move(targetDist, 0); - pc.printf("\nMoved forward a distance of %f to node %i", targetDist, target); // wait for move to complete while(!drive.isMoveDone()) { @@ -231,8 +228,7 @@ distLocalL = longRangeL.distInchesLOne(); distLocalR = longRangeR.distInchesROne(); - pc.printf("\n distLocalL = %f", distLocalL); - pc.printf("\n distLocalR = %f", distLocalR); + if (distLocalL <= 18.0) // wall in range { newLocalizeRight(); @@ -293,7 +289,6 @@ { wait(1e-6); } - pc.printf("\n Adjusted angle more right, in localizeRight"); } else if (distLocalL >= 6.9) // too close left, turn right @@ -304,7 +299,6 @@ { wait(1e-6); } - pc.printf("\n Adjusted angle right, in localizeRight"); } else if (distLocalL <= 5.5) // too close right, turn left @@ -315,13 +309,11 @@ { wait(1e-6); } - pc.printf("\n Adjusted angle left , in localizeRight"); } else { /* do nothing */ - pc.printf("\n No need to adjust, in localizeRight"); } } @@ -345,7 +337,6 @@ { wait(1e-6); } - pc.printf("\n Adjusted angle left, in localizeLeft"); } else if (distLocalR <= 8.3) // too close left, turn right @@ -356,13 +347,11 @@ { wait(1e-6); } - pc.printf("\n Adjusted angle right, in localizeLeft"); } else { /* do nothing */ - pc.printf("\n No need to adjust, in localizeLeft"); } } @@ -385,7 +374,6 @@ { wait(1e-6); } - pc.printf("\n Adjusted angle left, in localizeRightReverse"); } else if (distLocalL <= 6.2) // too close right, turn right @@ -396,13 +384,11 @@ { wait(1e-6); } - pc.printf("\n Adjusted angle right, in localizeRightReverse"); } else { /* do nothing */ - pc.printf("\n No need to adjust, in localizeRightReverse"); } } @@ -426,7 +412,6 @@ { wait(1e-6); } - pc.printf("\n Adjusted angle right, in localizeLeftReverse"); } else if (distLocalR <= 8.3) // too close left, turn left @@ -437,13 +422,11 @@ { wait(1e-6); } - pc.printf("\n Adjusted angle left, in localizeLeftReverse"); } else { /* do nothing */ - pc.printf("\n No need to adjust, in localizeLeftReverse"); } } @@ -467,7 +450,6 @@ wait(1e-6); } angle += angleAdjust; - pc.printf("\n Adjusted angle by %f in newLocalize", angleAdjust); } // FUNCTION: @@ -491,7 +473,6 @@ wait(1e-6); } //angle += (-1*angleAdjust); - pc.printf("\n Adjusted angle by %f in newLocalizeRight", -1*angleAdjust); } // FUNCTION: @@ -514,7 +495,6 @@ wait(1e-6); } //angle += (-1*angleAdjust); - pc.printf("\n Adjusted angle by %f in newLocalizeLeft", -1*angleAdjust); } // FUNCTION: @@ -537,7 +517,6 @@ wait(1e-6); } //angle += (-1*angleAdjust); - pc.printf("\n Adjusted angle by %f in newLocalizeLeftReverse", -1*angleAdjust); } // FUNCTION: @@ -561,6 +540,5 @@ wait(1e-6); } //angle += (-1*angleAdjust); - pc.printf("\n Adjusted angle by %f in newLocalizeRight", -1*angleAdjust); }