Mapping for TP2
Dependencies: ISR_Mini-explorer mbed
Fork of GoToPointWithAngle by
Diff: main.cpp
- Revision:
- 18:dbc5fbad4975
- Parent:
- 14:44ab4626f1ad
- Child:
- 19:6a9062d54eb0
--- a/main.cpp Mon Mar 27 16:44:37 2017 +0000 +++ b/main.cpp Mon Apr 03 17:01:13 2017 +0000 @@ -8,8 +8,11 @@ int goToPointWithAngle(float target_x, float target_y, float target_angle); +int randomizeAndMap(); int updateSonarValues(); +int computeObstacle(); +bool map[25][25]; float alpha; //angle error float rho; //distance from target float beta; @@ -25,13 +28,14 @@ int frontMm; int rightMm; + //Diameter of a wheel and distance between the 2 float r=3.25, b=7.2; int speed=999; // Max speed at beggining of movement //Target example x,y values -float target_x=46.8, target_y=78.6, target_angle=1.7; +float target_x=46.8, target_y=78.6, target_angle=1.57; //Timeout time; int main(){ @@ -41,20 +45,27 @@ measure_always_on(); wait_ms(50); + + //Fill map + for (int i = 0; i<25; i++) { + for (int j = 0; j<25; j++) { + map[i][j] = false; + } + } //Resetting coordinates before moving theta=0; X=0; Y=0; - alpha = atan2((target_y-Y),(target_x-X))-theta; + /*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; - //beta = atan(sin(beta)/cos(beta)); - - goToPointWithAngle(target_x, target_y, target_angle); + beta = -alpha-theta+target_angle;*/ + for (int i = 0; i<10; i++) { + randomizeAndMap(); + } //Stop at the end leftMotor(1,0); @@ -76,6 +87,34 @@ return 0; } +int randomizeAndMap() { + target_x = (rand()%2500)/10;//for decimal precision + target_y = (rand()%2500)/10; + target_angle = ((rand()%31416)-15708)/1000; + pc.printf("\n\r targ_X=%f", target_x); + pc.printf("\n\r targ_Y=%f", target_y); + pc.printf("\n\r targ_Angle=%f", target_angle); + goToPointWithAngle(target_x, target_y, target_angle); + return 0; +} + +int computeObstacle() { + //get the sensor values + //compute the probabilistic shit of empty/non-empty in the direction of the sensor below 10 cm + //update the map object with true where it's likely to be obstacled + int xObstacle, yObstacle; + if (leftMm < 10) { + + } else if (frontMm < 10) { + + } else if (rightMm < 10) { + + } + + map[xObstacle][yObstacle] = true; + return 0; +} + int goToPointWithAngle(float target_x, float target_y, float target_angle) { do { pc.printf("\n\n\r entered while"); @@ -97,9 +136,7 @@ alpha = atan(sin(alpha)/cos(alpha)); rho = dist(X, Y, target_x, target_y); d2 = rho; - beta = -alpha-theta+target_angle; - //beta = atan(sin(beta)/cos(beta)); - + beta = -alpha-theta+target_angle; //Computing angle error and distance towards the target value rho += dt*(-kRho*cos(alpha)*rho); @@ -150,5 +187,8 @@ t.stop(); } while(d2>1 & !tooClose); + if (tooClose) { + computeObstacle(); + } return 0; } \ No newline at end of file