Navigate to a given point using the OGM and virtual forces
Dependencies: ISR_Mini-explorer mbed
Fork of VirtualForces by
Diff: main.cpp
- Revision:
- 20:6a9062d54eb0
- Parent:
- 19:dbc5fbad4975
- Parent:
- 17:caf393b63e27
- Child:
- 21:62154d644531
diff -r dbc5fbad4975 -r 6a9062d54eb0 main.cpp --- a/main.cpp Mon Apr 03 17:01:13 2017 +0000 +++ b/main.cpp Mon Apr 03 17:07:37 2017 +0000 @@ -22,7 +22,7 @@ float temp; float d2; -bool tooClose = false; +//bool tooClose = false; int leftMm; int frontMm; @@ -34,11 +34,16 @@ int speed=999; // Max speed at beggining of movement -//Target example x,y values +//target exemple x y theta float target_x=46.8, target_y=78.6, target_angle=1.57; + //Timeout time; int main(){ + target_x=200*rand(); + target_y=200*rand(); + target_angle=3.1416*2*rand()-3.1416; + i2c1.frequency(100000); initRobot(); //Initializing the robot pc.baud(9600); // baud for the pc communication @@ -61,12 +66,10 @@ /*alpha = atan2((target_y-Y),(target_x-X))-theta; alpha = atan(sin(alpha)/cos(alpha)); rho = dist(X, Y, target_x, target_y); - beta = -alpha-theta+target_angle;*/ for (int i = 0; i<10; i++) { randomizeAndMap(); } - //Stop at the end leftMotor(1,0); rightMotor(1,0); @@ -124,14 +127,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); @@ -185,7 +188,7 @@ wait(0.2); //Timer stuff t.stop(); - } while(d2>1 & !tooClose); + } while(d2>1); if (tooClose) { computeObstacle();