Mapping for TP2
Dependencies: ISR_Mini-explorer mbed
Fork of GoToPointWithAngle by
Diff: main.cpp
- Revision:
- 20:62154d644531
- Parent:
- 19:6a9062d54eb0
- Child:
- 21:ebb37a249b5f
--- a/main.cpp Mon Apr 03 17:07:37 2017 +0000 +++ b/main.cpp Fri Apr 07 15:47:51 2017 +0000 @@ -11,6 +11,7 @@ int randomizeAndMap(); int updateSonarValues(); int computeObstacle(); +int printFinalMap(); bool map[25][25]; float alpha; //angle error @@ -22,7 +23,7 @@ float temp; float d2; -//bool tooClose = false; +bool tooClose = false; int leftMm; int frontMm; @@ -40,16 +41,16 @@ //Timeout time; int main(){ - target_x=200*rand(); + /*target_x=200*rand(); target_y=200*rand(); - target_angle=3.1416*2*rand()-3.1416; + target_angle=3.1416*2*rand()-3.1416;*/ i2c1.frequency(100000); initRobot(); //Initializing the robot pc.baud(9600); // baud for the pc communication - measure_always_on(); - wait_ms(50); + //measure_always_on(); + //wait_ms(50); //Fill map for (int i = 0; i<25; i++) { @@ -60,23 +61,37 @@ //Resetting coordinates before moving theta=0; - X=0; - Y=0; + X=125; + Y=125; /*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++) { + for (int i = 0; i<25; i++) { randomizeAndMap(); } //Stop at the end leftMotor(1,0); rightMotor(1,0); - pc.printf("\n\r %f -- arrived!", rho); + //pc.printf("\n\r %f -- arrived!", rho); + printFinalMap(); } +int printFinalMap() { + for (int i = 0; i<25; i++) { + for (int j = 0; j<25; j++) { + if (map[i][j]) { + pc.printf("X "); + } else { + pc.printf("O "); + } + } + pc.printf("\n\r"); + } + return 0; +} //Distance computation function float dist(float robot_x, float robot_y, float target_x, float target_y){ return sqrt(pow(target_y-robot_y,2) + pow(target_x-robot_x,2)); @@ -84,8 +99,11 @@ //Updates sonar values int updateSonarValues() { + //start_read_left_sensor(); leftMm = get_distance_left_sensor(); + //start_read_front_sensor(); frontMm = get_distance_front_sensor(); + //start_read_right_sensor(); rightMm = get_distance_right_sensor(); return 0; } @@ -93,7 +111,7 @@ int randomizeAndMap() { target_x = (rand()%2500)/10;//for decimal precision target_y = (rand()%2500)/10; - target_angle = ((rand()%31416)-15708)/1000; + target_angle = ((float)(rand()%31416)-15708)/10000.0; 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); @@ -107,11 +125,11 @@ //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; @@ -132,6 +150,7 @@ updateSonarValues(); if (leftMm < 100 || frontMm < 100 || rightMm < 100) { + tooClose = true; break; }