Revision 34:c208497dd079, committed 2017-06-09

Comitter:

Ludwigfr

Date:

Fri Jun 09 14:30:21 2017 +0000

Parent:

33:814bcd7d3cfe

Child:

35:68f9edbb3cff

Commit message:

okay it compiles

Changed in this revision

--- a/Map.cpp	Fri Jun 09 00:28:32 2017 +0000
+++ b/Map.cpp	Fri Jun 09 14:30:21 2017 +0000
@@ -1,12 +1,13 @@
 #include "Map.hpp"
 
-Map::Map(float widthRealMap, float HeightRealMap, int nbCellWidth, int nbCellHeight){
+
+Map::Map(float widthRealMap, float heightRealMap, int nbCellWidth, int nbCellHeight){
 	this->widthRealMap=widthRealMap;
-	this->HeightRealMap=HeightRealMap;
+	this->heightRealMap=heightRealMap;
 	this->nbCellWidth=nbCellWidth;
 	this->nbCellHeight=nbCellHeight;
 	this->sizeCellWidth=widthRealMap/(float)nbCellWidth;
-	this->sizeCellHeight=HeightRealMap/(float)nbCellHeight;
+	this->sizeCellHeight=heightRealMap/(float)nbCellHeight;
 	
 	this->cellsLogValues= new float*[nbCellWidth];
 	for(int i = 0; i < nbCellWidth; ++i)
@@ -23,11 +24,11 @@
 void Map::fill_initialLogValues(){
     //Fill map, we know the border are occupied
     for (int i = 0; i<this->nbCellWidth; i++) {
-        for (int j = 0; j<nbCellHeight; j++) {
-            if(j==0 || j==nbCellHeight-1 || i==0 || i==nbCellWidth-1)
-                this->initialLogValues[i][j] = proba_to_log(1);
+        for (int j = 0; j<this->nbCellHeight; j++) {
+            if(j==0 || j==this->nbCellHeight-1 || i==0 || i==this->nbCellWidth-1)
+                this->initialLogValues[i][j] = this->proba_to_log(1);
             else
-                initialLogValues[i][j] = proba_to_log(0.5);
+                this->initialLogValues[i][j] = this->proba_to_log(0.5);
         }
     }
 }
@@ -43,7 +44,33 @@
     return log(p/(1-p));
 }
 
-void Map::print_final_map() {
+void Map::update_cell_value(int widthIndice,int heightIndice ,float proba){
+    this->cellsLogValues[widthIndice][heightIndice]=this->cellsLogValues[widthIndice][heightIndice]+this->proba_to_log(proba)+this->initialLogValues[widthIndice][heightIndice];//map is filled as map[0][0] get the data for the point closest to the origin
+}
+
+float Map::robot_x_coordinate_in_world(float robot_x, float robot_y){
+    return this->nbCellWidth*this->sizeCellWidth-robot_y;
+}
+
+float Map::robot_y_coordinate_in_world(float robot_x, float robot_y){
+    return robot_x;
+}
+
+float Map::cell_width_coordinate_to_world(int i){
+	return this->sizeCellWidth/2+i*this->sizeCellWidth;
+}
+
+float Map::cell_height_coordinate_to_world(int j){
+	return this->sizeCellHeight/2+j*this->sizeCellHeight;
+}
+
+float Map::get_proba_cell(int widthIndice, int heightIndice){
+	return  this->log_to_proba(this->cellsLogValues[widthIndice][heightIndice]);
+}
+
+/*
+
+void MiniExplorerCoimbra::print_final_map() {
     float currProba;
     pc.printf("\n\r");
     for (int y = this->nbCellHeight -1; y>-1; y--) {
@@ -78,8 +105,8 @@
     float heightBonus=sizeCellHeight/2;
 
     pc.printf("\n\r");
-    for (int y = nbCellHeight -1; y>-1; y--) {
-        for (int x= 0; x<nbCellWidth; x++) {
+    for (int y = this->nbCellHeight -1; y>-1; y--) {
+        for (int x= 0; x<this->nbCellWidth; x++) {
             heightIndiceInOrthonormal=this->cell_height_coordinate_to_world(y);
             widthIndiceInOrthonormal=this->cell_width_coordinate_to_world(x);
             if(Yrobot >= (heightIndiceInOrthonormal+heightMalus) && Yrobot <= (heightIndiceInOrthonormal+heightBonus) && Xrobot >= (widthIndiceInOrthonormal+widthMalus) && Xrobot <= (widthIndiceInOrthonormal+widthBonus))                    
@@ -140,27 +167,4 @@
         pc.printf("\n\r");
     }
 }
-
-void Map::update_cell_value(int widthIndice,int heightIndice ,float proba){
-    this->cellsLogValues[widthIndice][heightIndice]=this->cellsLogValues[widthIndice][heightIndice]+this->proba_to_log(proba)+this->initialLogValues[widthIndice][heightIndice];//map is filled as map[0][0] get the data for the point closest to the origin
-}
-
-float Map::robot_x_coordinate_in_world(float robot_x, float robot_y){
-    return this->nbCellWidth*this->sizeCellWidth-robot_y;
-}
-
-float Map::robot_y_coordinate_in_world(float robot_x, float robot_y){
-    return robot_x;
-}
-
-float Map::cell_width_coordinate_to_world(int i){
-	return this->sizeCellWidth/2+i*this->sizeCellWidth;
-}
-
-float Map::cell_height_coordinate_to_world(int j){
-	return this->sizeCellHeight/2+j*this->sizeCellHeight;
-}
-
-float Map::get_proba_cell(int widthIndice, int heightIndice){
-	return  this->log_to_proba(this->cellsLogValues[widthIndice][heightIndice]);
-}
+*/
\ No newline at end of file

--- a/Map.hpp	Fri Jun 09 00:28:32 2017 +0000
+++ b/Map.hpp	Fri Jun 09 14:30:21 2017 +0000
@@ -1,9 +1,9 @@
 #ifndef MAP_HPP
 #define MAP_HPP
 
-#include "MiniExplorerCoimbra.hpp"
+#include<math.h>
 
-        /*
+/*
 Robot coordinate system:      World coordinate system:
       ^                                 ^
       |x                                |y
@@ -30,7 +30,7 @@
 
 public:
     float widthRealMap;
-    float HeightRealMap;
+    float heightRealMap;
     int nbCellWidth;
     int nbCellHeight;
     float sizeCellWidth;
@@ -38,16 +38,17 @@
     float** cellsLogValues;
     float** initialLogValues;
 
-    Map(float widthRealMap, float HeightRealMap, int nbCellWidth, int nbCellHeight);
-
-    void print_final_map();
+    Map(float widthRealMap, float heightRealMap, int nbCellWidth, int nbCellHeight);
+    
+    float cell_width_coordinate_to_world(int i);
 
-    void print_final_map_with_robot_position(float robot_x,float robot_y);
-
-    void print_final_map_with_robot_position_and_target(float robot_x,float robot_y,float targetXWolrd, float targetYWorld);
-
-    protected:
-
+    float cell_height_coordinate_to_world(int j);
+    
+    float get_proba_cell(int widthIndice, int heightIndice);
+    
+    //Updates map value
+    void update_cell_value(int widthIndice,int heightIndice ,float proba);
+    
     //returns the probability [0,1] that the cell is occupied from the log valAue lt
     float log_to_proba(float lt);
 
@@ -57,19 +58,17 @@
     //fill initialLogValues with the values we already know (here the bordurs)
     void fill_initialLogValues();
 
-    //Updates map value
-    void update_cell_value(int widthIndice,int heightIndice ,float proba);
-
     float robot_x_coordinate_in_world(float robot_x, float robot_y);
 
     float robot_y_coordinate_in_world(float robot_x, float robot_y);
-
-    float cell_width_coordinate_to_world(int i);
+    
+    /*
+    void print_final_map();
 
-    float cell_height_coordinate_to_world(int j);
+    void print_final_map_with_robot_position(float robot_x,float robot_y);
 
-    float get_proba_cell(int widthIndice, int heightIndice);
-
+    void print_final_map_with_robot_position_and_target(float robot_x,float robot_y,float targetXWolrd, float targetYWorld);
+    */
 }; 
 
 #endif

--- 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");
+    }
+}

--- a/MiniExplorerCoimbra.hpp	Fri Jun 09 00:28:32 2017 +0000
+++ b/MiniExplorerCoimbra.hpp	Fri Jun 09 14:30:21 2017 +0000
@@ -3,9 +3,8 @@
 
 #include "Map.hpp"
 #include "Sonar.hpp"
-#include "robot.h"
-#include "mbed.h"
-#include "math.h"
+#include<math.h>
+
 
 	/*
 Robot coordinate system:      World coordinate system:
@@ -16,9 +15,9 @@
 */
 
 class MiniExplorerCoimbra {
-
+	
 	public:
-
+	
 	Map map;
 	Sonar sonarLeft;
 	Sonar sonarFront;
@@ -85,7 +84,14 @@
 	> 0 if it is between 0 and PI
 	*/
 	void turn_to_target(float angleToTarget);
+	
+	void print_final_map();
+	
+	void print_final_map_with_robot_position(float robot_x,float robot_y);
+	
+	void print_final_map_with_robot_position_and_target(float robot_x,float robot_y,float targetXWorld, float targetYWorld);
 };
 
+
 #endif
 

--- a/Sonar.cpp	Fri Jun 09 00:28:32 2017 +0000
+++ b/Sonar.cpp	Fri Jun 09 14:30:21 2017 +0000
@@ -1,9 +1,11 @@
 #include "Sonar.hpp"
 
-Sonar::Sonar(float anlgeFromCenter, float distanceXRobot, float distanceYRobot ){
-	this->anlgeFromCenter=anlgeFromCenter;
-	this->distanceXRobot=-distanceYRobot;
-	this->distanceYRobot=distanceXRobot;
+#define PI 3.14159
+
+Sonar::Sonar(float angleFromCenter, float distanceXFromRobotCenter, float distanceYFromRobotCenter ){
+	this->angleFromCenter=angleFromCenter;
+	this->distanceX=-distanceYFromRobotCenter;
+	this->distanceY=distanceXFromRobotCenter;
 	this->maxRange=50;//cm
 	this->minRange=10;//Rmin cm
 	this->incertitudeRange=10;//cm
@@ -11,20 +13,20 @@
 }
 
 //ODOMETRIA MUST HAVE BEEN CALLED
-//function that check if a cell A(x,y) is in the range of the front sonar S(xs,ys) (with an angle depending on the sonar used, front 0, left pi/3, right -pi/3) returns the probability it's occupied/empty [0;1]
-float Sonar::compute_probability_t(float distanceObstacleDetected, float x, float y, float robotCoordinateXWorld, float robotCoordinateYWorld){
-	float xSonar=robotCoordinateXWorld+this->distanceX;
-	float ySonar=robotCoordinateYWorld+this->distanceY;
-    float distancePointToSonar=sqrt(pow(x-xSonar,2)+pow(y-ySonar,2));
+//function that check if a cell A(x,y) is in the range of the front sonar S(xs,ys) (with an angle depending on the sonar used, front 0, left PI/3, right -PI/3) returns the probability it's occuPIed/empty [0;1]
+float Sonar::compute_probability_t(float distanceObstacleDetected, float xCell, float yCell, float xRobotWorld, float yRobotWorld, float theta){
+	float xSonar=xRobotWorld+this->distanceX;
+	float ySonar=yRobotWorld+this->distanceY;
+    float distancePointToSonar=sqrt(pow(xCell-xSonar,2)+pow(yCell-ySonar,2));
     if( distancePointToSonar < this->maxRange){
-        float anglePointToSonar=this->compute_angle_between_vectors(x,y,xSonar,ySonar);//angle beetween the point and the sonar beam
-        float alphaBeforeAdjustment=anglePointToSonar-theta-angleFromSonarPosition;
+        float anglePointToSonar=this->compute_angle_between_vectors(xCell,yCell,xSonar,ySonar);//angle beetween the point and the sonar beam
+        float alphaBeforeAdjustment=anglePointToSonar-theta-this->angleFromCenter;
         anglePointToSonar=rad_angle_check(alphaBeforeAdjustment);//TODO I feel you don't need to do that but I m not sure
         
-        if(alphaBeforeAdjustment>pi)
-            alphaBeforeAdjustment=alphaBeforeAdjustment-2*pi;
-        if(alphaBeforeAdjustment<-pi)
-            alphaBeforeAdjustment=alphaBeforeAdjustment+2*pi;
+        if(alphaBeforeAdjustment>PI)
+            alphaBeforeAdjustment=alphaBeforeAdjustment-2*PI;
+        if(alphaBeforeAdjustment<-PI)
+            alphaBeforeAdjustment=alphaBeforeAdjustment+2*PI;
             
         //float anglePointToSonar2=atan2(y-ys,x-xs)-theta;
         
@@ -48,9 +50,9 @@
                 //    pc.printf("\n\r return value=%f,Er=%f,Ea=%f,alphaBeforeAdjustment=%f",(1.f-Er*Ea)/2.f,Er,Ea,alphaBeforeAdjustment);
                 return (1.f-Er*Ea)/2.f;
             }else{
-                //probably occupied
+                //probably occuPIed
             /*****************************************************************************/
-                float Oa=1.f-pow((2*alphaBeforeAdjustment)/ANGLE_SONAR,2);
+                float Oa=1.f-pow((2*alphaBeforeAdjustment)/this->angleRange,2);
                 float Or;
                 if( distancePointToSonar <= (distanceObstacleDetected + this->incertitudeRange)){
                     //point between distanceObstacleDetected +- INCERTITUDE_SONAR
@@ -88,15 +90,15 @@
         return acos(cosAlpha);
 }
 
-//makes the angle inAngle between 0 and 2pi
+//makes the angle inAngle between 0 and 2PI
 float Sonar::rad_angle_check(float inAngle){
     //cout<<"before :"<<inAngle;
     if(inAngle > 0){
-        while(inAngle > (2*pi))
-            inAngle-=2*pi;
+        while(inAngle > (2*PI))
+            inAngle-=2*PI;
     }else{
         while(inAngle < 0)
-            inAngle+=2*pi;
+            inAngle+=2*PI;
     }
     //cout<<" after :"<<inAngle<<endl;
     return inAngle;

--- a/Sonar.hpp	Fri Jun 09 00:28:32 2017 +0000
+++ b/Sonar.hpp	Fri Jun 09 14:30:21 2017 +0000
@@ -1,7 +1,7 @@
 #ifndef SONAR_HPP
 #define SONAR_HPP
 
-#include "MiniExplorerCoimbra.hpp"
+#include<math.h>
 
 class Sonar {
 
@@ -15,11 +15,11 @@
 	float distanceY;
 	
 
-	Sonar(float anlgeFromCenter, float distanceXRobot, float distanceYRobot );
+	Sonar(float anlgeFromCenter, float distanceXFromRobotCenter, float distanceYFromRobotCenter );
 
 	//ODOMETRIA MUST HAVE BEEN CALLED
 	//function that check if a cell A(x,y) is in the range of the front sonar S(xs,ys) (with an angle depending on the sonar used, front 0, left pi/3, right -pi/3) returns the probability it's occupied/empty [0;1]
-	float compute_probability_t(float distanceObstacleDetected, float x, float y, float robotCoordinateXWorld, float robotCoordinateYWorld);
+	float compute_probability_t(float distanceObstacleDetected, float xCell, float yCell, float xRobotWorld, float yRobotWorld, float theta);
 
 	//returns the angle between the vectors (x,y) and (xs,ys)
 	float compute_angle_between_vectors(float x, float y,float xs,float ys);

--- a/main.cpp	Fri Jun 09 00:28:32 2017 +0000
+++ b/main.cpp	Fri Jun 09 14:30:21 2017 +0000
@@ -1,8 +1,8 @@
 #include "MiniExplorerCoimbra.hpp"
 
-using namespace std;
+#define PI 3.14159
 
-int main(){
+int main(){ 
     MiniExplorerCoimbra myRobot(0,0,0);   
     return 0;
 }