Navigate to a given point using the OGM and virtual forces
Dependencies: ISR_Mini-explorer mbed
Fork of VirtualForces by
Diff: main.cpp
- Revision:
- 14:d58f2bdbf42e
- Parent:
- 13:41f75c132135
- Child:
- 16:ff73cc7b3156
--- a/main.cpp Mon Mar 27 16:42:53 2017 +0000 +++ b/main.cpp Mon Mar 27 16:49:49 2017 +0000 @@ -19,7 +19,7 @@ float temp; float d2; -bool tooClose = false; +//bool tooClose = false; int leftMm; int frontMm; @@ -85,14 +85,14 @@ t.reset(); t.start(); + //Updating X,Y and theta with the odometry values + Odometria(); + updateSonarValues(); if (leftMm < 100 || frontMm < 100 || rightMm < 100) { - tooClose = true; + break; } - //Updating X,Y and theta with the odometry values - Odometria(); - alpha = atan2((target_y-Y),(target_x-X))-theta; alpha = atan(sin(alpha)/cos(alpha)); rho = dist(X, Y, target_x, target_y); @@ -145,7 +145,7 @@ wait(0.2); //Timer stuff t.stop(); - } while(d2>1 & !tooClose); + } while(d2>1); return 0; } \ No newline at end of file