Navigate to a given point using the OGM and virtual forces

Dependencies:   ISR_Mini-explorer mbed

Fork of VirtualForces by Georgios Tsamis

Revision:
31:352be78e1aad
Parent:
30:95d8d3e2b81b
Child:
32:d51928b58645
--- a/main.cpp	Wed Apr 26 17:06:00 2017 +0000
+++ b/main.cpp	Sat Apr 29 07:29:30 2017 +0000
@@ -68,14 +68,15 @@
 const float DISTANCE_SONAR_FRONT_Y=5;
 
 //TODO adjust the size of the map for computation time (25*25?)
-const float WIDTH_ARENA=120;//cm
-const float HEIGHT_ARENA=90;//cm
+const float WIDTH_ARENA=250;//cm
+const float HEIGHT_ARENA=250;//cm
 
 //const int SIZE_MAP=25;
-const int NB_CELL_WIDTH=24;
-const int NB_CELL_HEIGHT=18;
+const int NB_CELL_WIDTH=20;
+const int NB_CELL_HEIGHT=20;
 
 //position and orientation of the robot when put on the map (ODOMETRY doesn't know those) it's in the robot frame
+//this configuration suppose that the robot is in the middle of the arena facing up (to be sure you can use print_final_map_with_robot_position
 const float DEFAULT_X=HEIGHT_ARENA/2;
 const float DEFAULT_Y=WIDTH_ARENA/2;
 const float DEFAULT_THETA=0;
@@ -121,7 +122,7 @@
     
     fill_initial_log_values();
 
-    theta=DEFAULT_THETA; //TODO
+    theta=DEFAULT_THETA;
     X=DEFAULT_X;
     Y=DEFAULT_Y;
 
@@ -140,7 +141,7 @@
 
 //fill initialLogValues with the values we already know (here the bordurs)
 void fill_initial_log_values(){
-     //Fill map, we know the border are occupied
+    //Fill map, we know the border are occupied
     for (int i = 0; i<NB_CELL_WIDTH; i++) {
         for (int j = 0; j<NB_CELL_HEIGHT; j++) {
             if(j==0 || j==NB_CELL_HEIGHT-1 || i==0 || i==NB_CELL_WIDTH-1)