Diff: MiniExplorerCoimbra.cpp

Revision:

34:c208497dd079

Parent:

33:814bcd7d3cfe

Child:

35:68f9edbb3cff

--- a/MiniExplorerCoimbra.cpp	Fri Jun 09 00:28:32 2017 +0000
+++ b/MiniExplorerCoimbra.cpp	Fri Jun 09 14:30:21 2017 +0000
@@ -1,33 +1,34 @@
 #include "MiniExplorerCoimbra.hpp"
+#include "robot.h"
 
 #define PI 3.14159
 
-MiniExplorerCoimbra::	MiniExplorerCoimbra(float defaultX, float defaultY, float defaultTheta):map(200,200,20,20),sonarLeft(10*PI/36,-4,4),sonarFront(0,0,5),sonarRight(-10*PI/36,4,4){
-	i2c1.frequency(100000);
+MiniExplorerCoimbra::MiniExplorerCoimbra(float defaultX, float defaultY, float defaultTheta):map(200,200,20,20),sonarLeft(10*PI/36,-4,4),sonarFront(0,0,5),sonarRight(-10*PI/36,4,4){
+    i2c1.frequency(100000);
     initRobot(); //Initializing the robot
     pc.baud(9600); // baud for the pc communication
 
     measure_always_on();//TODO check if needed
 
-	this->setXYTheta(defaultX,defaultY,defaultTheta);
-	this->radiusWheels=3.25;
-	this->distanceWheels=7.2; 
-	
-	this->khro=12;
-	this->ka=30;
-	this->kb=-13;
-	this->kv=200;
-	this->kh=200;
+    this->setXYTheta(defaultX,defaultY,defaultTheta);
+    this->radiusWheels=3.25;
+    this->distanceWheels=7.2; 
+    
+    this->khro=12;
+    this->ka=30;
+    this->kb=-13;
+    this->kv=200;
+    this->kh=200;
 
-	this->rangeForce=30;
-	this->attractionConstantForce=10000;
-	this->repulsionConstantForce=1;
+    this->rangeForce=30;
+    this->attractionConstantForce=10000;
+    this->repulsionConstantForce=1;
 } 
 
 void MiniExplorerCoimbra::setXYTheta(float x, float y, float theta){
-	X=x;
-	Y=y;
-	theta=theta;
+    X=x;
+    Y=y;
+    theta=theta;
 }
 
 //generate a position randomly and makes the robot go there while updating the map
@@ -102,14 +103,16 @@
 void MiniExplorerCoimbra::update_sonar_values(float leftMm,float frontMm,float rightMm){
     float xWorldCell;
     float yWorldCell;
+    float xRobotWorld=this->get_converted_robot_X_into_world();
+    float yRobotWorld=this->get_converted_robot_Y_into_world();
     for(int i=0;i<this->map.nbCellWidth;i++){
         for(int j=0;j<this->map.nbCellHeight;j++){
         	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->get_converted_robot_X_into_world(),this->get_converted_robot_Y_into_world()));
-        	this->map.update_cell_value(i,j,this->sonarLeft.compute_probability_t(leftMm,xWorldCell,yWorldCell,this->get_converted_robot_X_into_world(),this->get_converted_robot_Y_into_world()));
-        	this->map.update_cell_value(i,j,this->sonarFront.compute_probability_t(frontMm,xWorldCell,yWorldCell,this->get_converted_robot_X_into_world(),this->get_converted_robot_Y_into_world()));
+        	this->map.update_cell_value(i,j,this->sonarRight.compute_probability_t(rightMm,xWorldCell,yWorldCell,xRobotWorld,yRobotWorld,theta));
+        	this->map.update_cell_value(i,j,this->sonarLeft.compute_probability_t(leftMm,xWorldCell,yWorldCell,xRobotWorld,yWorldCell,theta));
+        	this->map.update_cell_value(i,j,this->sonarFront.compute_probability_t(frontMm,xWorldCell,yWorldCell,xRobotWorld,yWorldCell,theta));
 
        	}
     }
@@ -210,7 +213,7 @@
             pc.printf("\r\n X Target=%f", targetXWorld);
             pc.printf("\r\n Y Target=%f", targetYWorld);
             */
-            print_final_map_with_robot_position_and_target();
+            this->print_final_map_with_robot_position_and_target(X,Y,targetXWorld,targetYWorld);
             print=0;
         }else
             print+=1;
@@ -456,3 +459,101 @@
     return angleBeetweenRobotAndTarget-theta;
 }
 */
+
+
+void MiniExplorerCoimbra::print_final_map_with_robot_position(float robot_x,float robot_y) {
+    float currProba;
+    float Xrobot=this->map.robot_x_coordinate_in_world(robot_x,robot_y);
+    float Yrobot=this->map.robot_y_coordinate_in_world(robot_x,robot_y);
+    
+    float heightIndiceInOrthonormal;
+    float widthIndiceInOrthonormal;
+    
+    float widthMalus=-(3*this->map.sizeCellWidth/2);
+    float widthBonus=this->map.sizeCellWidth/2;
+    
+    float heightMalus=-(3*this->map.sizeCellHeight/2);
+    float heightBonus=this->map.sizeCellHeight/2;
+
+    pc.printf("\n\r");
+    for (int y = this->map.nbCellHeight -1; y>-1; y--) {
+        for (int x= 0; x<this->map.nbCellWidth; x++) {
+            heightIndiceInOrthonormal=this->map.cell_height_coordinate_to_world(y);
+            widthIndiceInOrthonormal=this->map.cell_width_coordinate_to_world(x);
+            if(Yrobot >= (heightIndiceInOrthonormal+heightMalus) && Yrobot <= (heightIndiceInOrthonormal+heightBonus) && Xrobot >= (widthIndiceInOrthonormal+widthMalus) && Xrobot <= (widthIndiceInOrthonormal+widthBonus))                    
+                pc.printf(" R ");
+            else{
+                currProba=this->map.log_to_proba(this->map.cellsLogValues[x][y]);
+                if ( currProba < 0.5)
+                    pc.printf("   ");
+                else{
+                    if(currProba==0.5)
+                        pc.printf(" . ");
+                    else
+                        pc.printf(" X ");
+                } 
+            }
+        }
+        pc.printf("\n\r");
+    }
+}
+
+void MiniExplorerCoimbra::print_final_map_with_robot_position_and_target(float robot_x,float robot_y,float targetXWorld, float targetYWorld) {
+    float currProba;
+    float Xrobot=this->map.robot_x_coordinate_in_world(robot_x,robot_y);
+    float Yrobot=this->map.robot_y_coordinate_in_world(robot_x,robot_y);
+    
+    float heightIndiceInOrthonormal;
+    float widthIndiceInOrthonormal;
+    
+    float widthMalus=-(3*this->map.sizeCellWidth/2);
+    float widthBonus=this->map.sizeCellWidth/2;
+    
+    float heightMalus=-(3*this->map.sizeCellHeight/2);
+    float heightBonus=this->map.sizeCellHeight/2;
+
+    pc.printf("\n\r");
+    for (int y = this->map.nbCellHeight -1; y>-1; y--) {
+        for (int x= 0; x<this->map.nbCellWidth; x++) {
+            heightIndiceInOrthonormal=this->map.cell_height_coordinate_to_world(y);
+            widthIndiceInOrthonormal=this->map.cell_width_coordinate_to_world(x);
+            if(Yrobot >= (heightIndiceInOrthonormal+heightMalus) && Yrobot <= (heightIndiceInOrthonormal+heightBonus) && Xrobot >= (widthIndiceInOrthonormal+widthMalus) && Xrobot <= (widthIndiceInOrthonormal+widthBonus))
+                pc.printf(" R ");
+            else{
+                if(targetYWorld >= (heightIndiceInOrthonormal+heightMalus) && targetYWorld <= (heightIndiceInOrthonormal+heightBonus) && targetXWorld >= (widthIndiceInOrthonormal+widthMalus) && targetXWorld <= (widthIndiceInOrthonormal+widthBonus))                    
+                    pc.printf(" T ");
+                else{
+                    currProba=this->map.log_to_proba(this->map.cellsLogValues[x][y]);
+                    if ( currProba < 0.5)
+                        pc.printf("   ");
+                    else{
+                        if(currProba==0.5)
+                            pc.printf(" . ");
+                        else
+                            pc.printf(" X ");
+                    } 
+                }
+            }
+        }
+        pc.printf("\n\r");
+    }
+}
+
+void MiniExplorerCoimbra::print_final_map() {
+    float currProba;
+    pc.printf("\n\r");
+    for (int y = this->map.nbCellHeight -1; y>-1; y--) {
+        for (int x= 0; x<this->map.nbCellWidth; x++) {
+                currProba=this->map.log_to_proba(this->map.cellsLogValues[x][y]);
+            if ( currProba < 0.5) {
+                pc.printf("   ");
+            } else {
+                if(currProba==0.5)
+                    pc.printf(" . ");
+                else
+                    pc.printf(" X ");
+            }
+        }
+        pc.printf("\n\r");
+    }
+}