Diff: MiniExplorerCoimbra.cpp

Revision:

37:b4c45e43ad29

Parent:

35:68f9edbb3cff

Child:

38:5ed7c79fb724

--- a/MiniExplorerCoimbra.cpp	Sun Jun 11 22:53:59 2017 +0000
+++ b/MiniExplorerCoimbra.cpp	Sun Jun 11 23:22:28 2017 +0000
@@ -3,7 +3,7 @@
 
 #define PI 3.14159
 
-MiniExplorerCoimbra::MiniExplorerCoimbra(float defaultXWorld, float defaultYWorld, float defaultThetaWorld):map(200,200,20,20),sonarLeft(10*PI/36,-4,4),sonarFront(0,0,5),sonarRight(-10*PI/36,4,4){
+MiniExplorerCoimbra::MiniExplorerCoimbra(float defaultXWorld, float defaultYWorld, float defaultThetaWorld):map(200,200,20,20),sonarLeft(PI/2+10*PI/36,-4,4),sonarFront(PI/2,0,5),sonarRight(PI/2-10*PI/36,4,4){
     i2c1.frequency(100000);
     initRobot(); //Initializing the robot
     pc.baud(9600); // baud for the pc communication
@@ -171,9 +171,9 @@
         	xWorldCell=this->map.cell_width_coordinate_to_world(i);
             yWorldCell=this->map.cell_height_coordinate_to_world(j);
             //compute_probability_t(float distanceObstacleDetected, float x, float y, float[2] robotCoordinatesInWorld)
-        	this->map.update_cell_value(i,j,this->sonarRight.compute_probability_t(rightMm,xWorldCell,yWorldCell,this->xWorld,this->yWorld,theta));
-        	this->map.update_cell_value(i,j,this->sonarLeft.compute_probability_t(leftMm,xWorldCell,yWorldCell,this->xWorld,yWorldCell,theta));
-        	this->map.update_cell_value(i,j,this->sonarFront.compute_probability_t(frontMm,xWorldCell,yWorldCell,this->xWorld,yWorldCell,theta));
+        	this->map.update_cell_value(i,j,this->sonarRight.compute_probability_t(rightMm,xWorldCell,yWorldCell,this->xWorld,this->yWorld,this->thetaWorld));
+        	this->map.update_cell_value(i,j,this->sonarLeft.compute_probability_t(leftMm,xWorldCell,yWorldCell,this->xWorld,yWorldCell,this->thetaWorld));
+        	this->map.update_cell_value(i,j,this->sonarFront.compute_probability_t(frontMm,xWorldCell,yWorldCell,this->xWorld,yWorldCell,this->thetaWorld));
 
        	}
     }
@@ -192,7 +192,7 @@
     float deplacementOnXWorld=targetXWorld-this->xWorld;
     float deplacementOnYWorld=targetYWorld-this->yWorld;
     */
-    float angleToTarget=atan2(targetXWorld-this->xWorld,targetYWorld-this->yWorld);
+    float angleToTarget=atan2(targetYWorld-this->yWorld,targetXWorld-this->xWorld);
     turn_to_target(angleToTarget);
     bool reached=false;
     int print=0;
@@ -202,7 +202,7 @@
         if(print==10){
             leftMotor(1,0);
             rightMotor(1,0);
-            this->print_final_map_with_robot_position_and_target(targetXWorld,targetYWorld);
+            this->print_map_with_robot_position_and_target(targetXWorld,targetYWorld);
             print=0;
         }else
             print+=1;
@@ -314,7 +314,7 @@
     float angular; 
     
     //atan2 use the deplacement on X and the deplacement on Y
-    float angleToTarget=atan2(targetXWorld-this->xWorld,targetYWorld-this->yWorld);
+    float angleToTarget=atan2(targetYWorld-this->yWorld,targetXWorld-this->xWorld);
     bool aligned=false;
     
     //this condition is passed if the target is in the same direction as the robot orientation
@@ -404,7 +404,7 @@
     rightMotor(1,0);    
 }
 
-void MiniExplorerCoimbra::print_final_map_with_robot_position() {
+void MiniExplorerCoimbra::print_map_with_robot_position() {
     float currProba;
     
     float heightIndiceInOrthonormal;
@@ -439,7 +439,7 @@
     }
 }
 
-void MiniExplorerCoimbra::print_final_map_with_robot_position_and_target(float targetXWorld, float targetYWorld) {
+void MiniExplorerCoimbra::print_map_with_robot_position_and_target(float targetXWorld, float targetYWorld) {
     float currProba;
     
     float heightIndiceInOrthonormal;
@@ -478,7 +478,7 @@
     }
 }
 
-void MiniExplorerCoimbra::print_final_map() {
+void MiniExplorerCoimbra::print_map() {
     float currProba;
     pc.printf("\n\r");
     for (int y = this->map.nbCellHeight -1; y>-1; y--) {