Yo Fleyt
with class
Dependencies: ISR_Mini-explorer mbed
Fork of
VirtualForces
by 
Georgios Tsamis
Revision 33:814bcd7d3cfe, committed 2017-06-09
- Comitter:
- Ludwigfr
- Date:
- Fri Jun 09 00:28:32 2017 +0000
- Parent:
- 32:d51928b58645
- Child:
- 34:c208497dd079
- Commit message:
- version with class
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Map.cpp Fri Jun 09 00:28:32 2017 +0000
@@ -0,0 +1,166 @@
+#include "Map.hpp"
+
+Map::Map(float widthRealMap, float HeightRealMap, int nbCellWidth, int nbCellHeight){
+ this->widthRealMap=widthRealMap;
+ this->HeightRealMap=HeightRealMap;
+ this->nbCellWidth=nbCellWidth;
+ this->nbCellHeight=nbCellHeight;
+ this->sizeCellWidth=widthRealMap/(float)nbCellWidth;
+ this->sizeCellHeight=HeightRealMap/(float)nbCellHeight;
+
+ this->cellsLogValues= new float*[nbCellWidth];
+ for(int i = 0; i < nbCellWidth; ++i)
+ this->cellsLogValues[i] = new float[nbCellHeight];
+
+ this->initialLogValues= new float*[nbCellWidth];
+ for(int i = 0; i < nbCellWidth; ++i)
+ this->initialLogValues[i] = new float[nbCellHeight];
+
+ this->fill_initialLogValues();
+}
+
+//fill initialLogValues with the values we already know (here the bordurs)
+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);
+ else
+ initialLogValues[i][j] = proba_to_log(0.5);
+ }
+ }
+}
+
+//returns the probability [0,1] that the cell is occupied from the log valAue lt
+float Map::log_to_proba(float lt){
+ return 1-1/(1+exp(lt));
+}
+
+
+//returns the log value that the cell is occupied from the probability value [0,1]
+float Map::proba_to_log(float p){
+ return log(p/(1-p));
+}
+
+void Map::print_final_map() {
+ float currProba;
+ pc.printf("\n\r");
+ for (int y = this->nbCellHeight -1; y>-1; y--) {
+ for (int x= 0; x<this->nbCellWidth; x++) {
+ currProba=this->log_to_proba(this->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 Map::print_final_map_with_robot_position(float robot_x,float robot_y) {
+ float currProba;
+ float Xrobot=this->robot_x_coordinate_in_world(robot_x,robot_y);
+ float Yrobot=this->robot_y_coordinate_in_world(robot_x,robot_y);
+
+ float heightIndiceInOrthonormal;
+ float widthIndiceInOrthonormal;
+
+ float widthMalus=-(3*sizeCellWidth/2);
+ float widthBonus=sizeCellWidth/2;
+
+ float heightMalus=-(3*sizeCellHeight/2);
+ float heightBonus=sizeCellHeight/2;
+
+ pc.printf("\n\r");
+ for (int y = nbCellHeight -1; y>-1; y--) {
+ for (int x= 0; x<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))
+ pc.printf(" R ");
+ else{
+ currProba=this->log_to_proba(this->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 Map::print_final_map_with_robot_position_and_target(float robot_x,float robot_y,float targetXWorld, float targetYWorld) {
+ float currProba;
+ float Xrobot=this->robot_x_coordinate_in_world(robot_x,robot_y);
+ float Yrobot=this->robot_y_coordinate_in_world(robot_x,robot_y);
+
+ float heightIndiceInOrthonormal;
+ float widthIndiceInOrthonormal;
+
+ float widthMalus=-(3*sizeCellWidth/2);
+ float widthBonus=sizeCellWidth/2;
+
+ float heightMalus=-(3*sizeCellHeight/2);
+ float heightBonus=sizeCellHeight/2;
+
+ pc.printf("\n\r");
+ 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))
+ pc.printf(" R ");
+ else{
+ if(targetYWorld >= (heightIndiceInOrthonormal+heightMalus) && targetYWorld <= (heightIndiceInOrthonormal+heightBonus) && targetXWorld >= (widthIndiceInOrthonormal+widthMalus) && targetXWorld <= (widthIndiceInOrthonormal+widthBonus))
+ pc.printf(" T ");
+ else{
+ currProba=this->log_to_proba(this->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 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]);
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Map.hpp Fri Jun 09 00:28:32 2017 +0000
@@ -0,0 +1,76 @@
+#ifndef MAP_HPP
+#define MAP_HPP
+
+#include "MiniExplorerCoimbra.hpp"
+
+ /*
+Robot coordinate system: World coordinate system:
+ ^ ^
+ |x |y
+ <--R O-->
+ y x
+
+Screen coordinate system
+ x
+ O--->
+y|
+ v
+
+how the float[2] arrays stock the position
+Start at 0,0 end top right
+^
+|heightIndice
+|
+_____>
+widthIndice
+*/
+
+
+class Map {
+
+public:
+ float widthRealMap;
+ float HeightRealMap;
+ int nbCellWidth;
+ int nbCellHeight;
+ float sizeCellWidth;
+ float sizeCellHeight;
+ float** cellsLogValues;
+ float** initialLogValues;
+
+ Map(float widthRealMap, float HeightRealMap, int nbCellWidth, int nbCellHeight);
+
+ 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 targetXWolrd, float targetYWorld);
+
+ protected:
+
+ //returns the probability [0,1] that the cell is occupied from the log valAue lt
+ float log_to_proba(float lt);
+
+ //returns the log value that the cell is occupied from the probability value [0,1]
+ float proba_to_log(float p);
+
+ //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);
+
+ float cell_height_coordinate_to_world(int j);
+
+ float get_proba_cell(int widthIndice, int heightIndice);
+
+};
+
+#endif
+
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/MiniExplorerCoimbra.cpp Fri Jun 09 00:28:32 2017 +0000
@@ -0,0 +1,458 @@
+#include "MiniExplorerCoimbra.hpp"
+
+#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);
+ 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->rangeForce=30;
+ this->attractionConstantForce=10000;
+ this->repulsionConstantForce=1;
+}
+
+void MiniExplorerCoimbra::setXYTheta(float x, float y, float theta){
+ X=x;
+ Y=y;
+ theta=theta;
+}
+
+//generate a position randomly and makes the robot go there while updating the map
+void MiniExplorerCoimbra::randomize_and_map() {
+ //TODO check that it's aurelien's work
+ float movementOnX=rand()%(int)(this->map.widthRealMap/2);
+ float movementOnY=rand()%(int)(this->map.heightRealMap/2);
+
+ float signOfX=rand()%2;
+ if(signOfX < 1)
+ signOfX=-1;
+ float signOfY=rand()%2;
+ if(signOfY < 1)
+ signOfY=-1;
+
+ float targetX = movementOnX*signOfX;
+ float targetY = movementOnY*signOfY;
+ float targetAngle = 2*((float)(rand()%31416)-15708)/10000.0;
+ this->go_to_point_with_angle(targetX, targetY, targetAngle);
+}
+
+//go the the given position while updating the map
+void MiniExplorerCoimbra::go_to_point_with_angle(float targetX, float targetY, float targetAngle) {
+ bool keep_going=true;
+ float leftMm;
+ float frontMm;
+ float rightMm;
+ float dt;
+ Timer t;
+ float d2;
+ do {
+ //Timer stuff
+ dt = t.read();
+ t.reset();
+ t.start();
+
+ //Updating X,Y and theta with the odometry values
+ Odometria();
+ leftMm = get_distance_left_sensor();
+ frontMm = get_distance_front_sensor();
+ rightMm = get_distance_right_sensor();
+
+ //if in dangerzone
+ if((frontMm < 150 && frontMm > 0)|| (leftMm <150 && leftMm > 0) || (rightMm <150 && rightMm > 0) ){
+ leftMotor(1,0);
+ rightMotor(1,0);
+ this->update_sonar_values(leftMm, frontMm, rightMm);
+ //TODO Giorgos maybe you can also test the do_half_flip() function
+ Odometria();
+ //do a flip TODO
+ keep_going=false;
+ this->do_half_flip();
+ }else{
+ //if not in danger zone continue as usual
+ this->update_sonar_values(leftMm, frontMm, rightMm);
+
+ //Updating motor velocities
+ d2=this->update_angular_speed_wheels_go_to_point_with_angle(targetX,targetY,targetAngle,dt);
+
+ wait(0.2);
+ //Timer stuff
+ t.stop();
+ pc.printf("\n\r dist to target= %f",d2);
+ }
+ } while(d2>1 && (abs(targetAngle-theta)>0.01) && keep_going);
+
+ //Stop at the end
+ leftMotor(1,0);
+ rightMotor(1,0);
+}
+
+void MiniExplorerCoimbra::update_sonar_values(float leftMm,float frontMm,float rightMm){
+ float xWorldCell;
+ float yWorldCell;
+ 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()));
+
+ }
+ }
+}
+
+float MiniExplorerCoimbra::get_converted_robot_X_into_world(){
+ //x world coordinate
+ return this->map.nbCellWidth*this->map.sizeCellWidth-Y;
+}
+
+float MiniExplorerCoimbra::get_converted_robot_Y_into_world(){
+ //y worldcoordinate
+ return X;
+}
+
+void MiniExplorerCoimbra::do_half_flip(){
+ Odometria();
+ float theta_plus_h_pi=theta+PI/2;//theta is between -PI and PI
+ if(theta_plus_h_pi > PI)
+ theta_plus_h_pi=-(2*PI-theta_plus_h_pi);
+ leftMotor(0,100);
+ rightMotor(1,100);
+ while(abs(theta_plus_h_pi-theta)>0.05){
+ Odometria();
+ // pc.printf("\n\r diff=%f", abs(theta_plus_pi-theta));
+ }
+ leftMotor(1,0);
+ rightMotor(1,0);
+}
+
+//Distance computation function
+float MiniExplorerCoimbra::dist(float robotX, float robotY, float targetX, float targetY){
+ //pc.printf("%f, %f, %f, %f",robotX,robotY,targetX,targetY);
+ return sqrt(pow(targetY-robotY,2) + pow(targetX-robotX,2));
+}
+
+float MiniExplorerCoimbra::update_angular_speed_wheels_go_to_point_with_angle(float targetX, float targetY, float targetAngle, float dt){
+ //compute_angles_and_distance
+ float alpha = atan2((targetY-Y),(targetX-X))-theta;
+ alpha = atan(sin(alpha)/cos(alpha));
+ float rho = this->dist(X, Y, targetX, targetY);
+ float d2 = rho;
+ float beta = -alpha-theta+targetAngle;
+
+ //Computing angle error and distance towards the target value
+ rho += dt*(-this->khro*cos(alpha)*rho);
+ float temp = alpha;
+ alpha += dt*(this->khro*sin(alpha)-this->ka*alpha-this->kb*beta);
+ beta += dt*(-this->khro*sin(temp));
+
+ //Computing linear and angular velocities
+ float linear;
+ float angular;
+ if(alpha>=-1.5708 && alpha<=1.5708){
+ linear=this->khro*rho;
+ angular=this->ka*alpha+this->kb*beta;
+ }
+ else{
+ linear=-this->khro*rho;
+ angular=-this->ka*alpha-this->kb*beta;
+ }
+ float angular_left=(linear-0.5*this->distanceWheels*angular)/this->radiusWheels;
+ float angular_right=(linear+0.5*this->distanceWheels*angular)/this->radiusWheels;
+
+ //Normalize speed for motors
+ if(angular_left>angular_right) {
+ angular_right=this->speed*angular_right/angular_left;
+ angular_left=this->speed;
+ } else {
+ angular_left=this->speed*angular_left/angular_right;
+ angular_right=this->speed;
+ }
+
+ //compute_linear_angular_velocities
+ leftMotor(1,angular_left);
+ rightMotor(1,angular_right);
+
+ return d2;
+}
+
+void MiniExplorerCoimbra::try_to_reach_target(float targetXWorld,float targetYWorld){
+ //atan2 gives the angle beetween PI and -PI
+ float angleToTarget=atan2(targetXWorld,targetYWorld);
+ turn_to_target(angleToTarget);
+ bool reached=false;
+ int print=0;
+ while (!reached) {
+ vff(&reached,targetXWorld,targetYWorld);
+ //test_got_to_line(&reached);
+ if(print==10){
+ leftMotor(1,0);
+ rightMotor(1,0);
+ /*
+ pc.printf("\r\n theta=%f", theta);
+ pc.printf("\r\n IN ORTHO:");
+ pc.printf("\r\n X Robot=%f", this->get_converted_robot_X_into_world());
+ pc.printf("\r\n Y Robot=%f", this->get_converted_robot_Y_into_world());
+ pc.printf("\r\n X Target=%f", targetXWorld);
+ pc.printf("\r\n Y Target=%f", targetYWorld);
+ */
+ print_final_map_with_robot_position_and_target();
+ print=0;
+ }else
+ print+=1;
+ }
+ //Stop at the end
+ leftMotor(1,0);
+ rightMotor(1,0);
+ pc.printf("\r\n target reached");
+ wait(3);//
+}
+
+void MiniExplorerCoimbra::vff(bool* reached, float targetXWorld, float targetYWorld){
+ float line_a;
+ float line_b;
+ float line_c;
+ //Updating X,Y and theta with the odometry values
+ float forceX=0;
+ float forceY=0;
+ //we update the odometrie
+ Odometria();
+ //we check the sensors
+ float leftMm = get_distance_left_sensor();
+ float frontMm = get_distance_front_sensor();
+ float rightMm = get_distance_right_sensor();
+ //update the probabilities values
+ this->update_sonar_values(leftMm, frontMm, rightMm);
+ //we compute the force on X and Y
+ this->compute_forceX_and_forceY(&forceX, &forceY,targetXWorld,targetYWorld);
+ //we compute a new ine
+ this->calculate_line(forceX, forceY, &line_a,&line_b,&line_c);
+ //Updating motor velocities
+ this->go_to_line(line_a,line_b,line_c,targetXWorld,targetYWorld);
+
+ //wait(0.1);
+ Odometria();
+ if(dist(this->get_converted_robot_X_into_world(),this->get_converted_robot_Y_into_world(),targetXWorld,targetYWorld)<10)
+ *reached=true;
+}
+
+//compute the force on X and Y
+void MiniExplorerCoimbra::compute_forceX_and_forceY(float* forceX, float* forceY, float targetXWorld, float targetYWorld){
+ float xRobotWorld=this->get_converted_robot_X_into_world();
+ float yRobotWorld=this->get_converted_robot_Y_into_world();
+
+ float forceRepulsionComputedX=0;
+ float forceRepulsionComputedY=0;
+ //for each cell of the map we compute a force of repulsion
+ for(int i=0;i<this->map.nbCellWidth;i++){
+ for(int j=0;j<this->map.nbCellHeight;j++){
+ this->update_force(i,j,&forceRepulsionComputedX,&forceRepulsionComputedY,xRobotWorld,yRobotWorld);
+ }
+ }
+ //update with attraction force
+ *forceX=+forceRepulsionComputedX;
+ *forceY=+forceRepulsionComputedY;
+ float distanceTargetRobot=sqrt(pow(targetXWorld-xRobotWorld,2)+pow(targetYWorld-yRobotWorld,2));
+ if(distanceTargetRobot != 0){
+ *forceX-=this->attractionConstantForce*(targetXWorld-xRobotWorld)/distanceTargetRobot;
+ *forceY-=this->attractionConstantForce*(targetYWorld-yRobotWorld)/distanceTargetRobot;
+ }
+ float amplitude=sqrt(pow(*forceX,2)+pow(*forceY,2));
+ if(amplitude!=0){//avoid division by 0 if forceX and forceY == 0
+ *forceX=*forceX/amplitude;
+ *forceY=*forceY/amplitude;
+ }
+}
+
+void MiniExplorerCoimbra::update_force(int widthIndice, int heightIndice, float* forceX, float* forceY, float xRobotWorld, float yRobotWorld ){
+ //get the coordonate of the map and the robot in the ortonormal space
+ float xWorldCell=this->map.cell_width_coordinate_to_world(widthIndice);
+ float yWorldCell=this->map.cell_height_coordinate_to_world(heightIndice);
+
+ //compute the distance beetween the cell and the robot
+ float distanceCellToRobot=sqrt(pow(xWorldCell-xRobotWorld,2)+pow(yWorldCell-yRobotWorld,2));
+ //check if the cell is in range
+ if(distanceCellToRobot <= this->rangeForce) {
+ float probaCell=this->map.get_proba_cell(widthIndice,heightIndice);
+ float xForceComputed=this->repulsionConstantForce*probaCell*(xWorldCell-xRobotWorld)/pow(distanceCellToRobot,3);
+ float yForceComputed=this->repulsionConstantForce*probaCell*(yWorldCell-yRobotWorld)/pow(distanceCellToRobot,3);
+ *forceX+=xForceComputed;
+ *forceY+=yForceComputed;
+ }
+}
+
+//robotX and robotY are from Odometria, calculate line_a, line_b and line_c
+void MiniExplorerCoimbra::calculate_line(float forceX, float forceY, float *line_a, float *line_b, float *line_c){
+ /*
+ in the teachers maths it is
+
+ *line_a=forceY;
+ *line_b=-forceX;
+
+ because a*x+b*y+c=0
+ a impact the vertical and b the horizontal
+ and he has to put them like this because
+ Robot space: orthonormal space:
+ ^ ^
+ |x |y
+ <- R O ->
+ y x
+ but since our forceX, forceY are already computed in the orthonormal space I m not sure we need to
+ */
+ *line_a=forceX;
+ *line_b=forceY;
+ //because the line computed always pass by the robot center we dont need lince_c
+ //*line_c=forceX*yRobotWorld+forceY*xRobotWorld;
+ *line_c=0;
+}
+
+//currently line_c is not used
+void MiniExplorerCoimbra::go_to_line(float line_a, float line_b, float line_c,float targetXWorld, float targetYWorld){
+ float lineAngle;
+ float angleError;
+ float linear;
+ float angular;
+
+ bool direction=true;
+
+ //take as parameter how much the robot is supposed to move on x and y (world)
+ float angleToTarget=atan2(targetXWorld-this->get_converted_robot_X_into_world(),targetYWorld-this->get_converted_robot_Y_into_world());
+ bool inFront=true;
+ if(angleToTarget < 0)//the target is in front
+ inFront=false;
+
+ if(theta > 0){
+ //the robot is oriented to the left
+ if(theta > PI/2){
+ //the robot is oriented lower left
+ if(inFront)
+ direction=false;//robot and target not oriented the same way
+ }else{
+ //the robot is oriented upper left
+ if(!inFront)
+ direction=false;//robot and target not oriented the same way
+ }
+ }else{
+ //the robot is oriented to the right
+ if(theta < -PI/2){
+ //the robot is oriented lower right
+ if(inFront)
+ direction=false;//robot and target not oriented the same way
+ }else{
+ //the robot is oriented upper right
+ if(!inFront)
+ direction=false;//robot and target not oriented the same way
+ }
+ }
+ //pc.printf("\r\n Target is in front");
+
+ if(line_b!=0){
+ if(!direction)
+ lineAngle=atan(-line_a/line_b);
+ else
+ lineAngle=atan(line_a/-line_b);
+ }
+ else{
+ lineAngle=1.5708;
+ }
+
+ //Computing angle error
+ angleError = lineAngle-theta;
+ angleError = atan(sin(angleError)/cos(angleError));
+
+ //Calculating velocities
+ linear=this->kv*(3.1416);
+ angular=this->kh*angleError;
+
+ float angularLeft=(linear-0.5*this->distanceWheels*angular)/this->radiusWheels;
+ float angularRight=(linear+0.5*this->distanceWheels*angular)/this->radiusWheels;
+
+ //Normalize speed for motors
+ if(abs(angularLeft)>abs(angularRight)) {
+ angularRight=this->speed*abs(angularRight/angularLeft)*this->sign1(angularRight);
+ angularLeft=this->speed*this->sign1(angularLeft);
+ }
+ else {
+ angularLeft=this->speed*abs(angularLeft/angularRight)*this->sign1(angularLeft);
+ angularRight=this->speed*this->sign1(angularRight);
+ }
+ leftMotor(this->sign2(angularLeft),abs(angularLeft));
+ rightMotor(this->sign2(angularRight),abs(angularRight));
+}
+
+//return 1 if positiv, -1 if negativ
+float MiniExplorerCoimbra::sign1(float value){
+ if(value>=0)
+ return 1;
+ else
+ return -1;
+}
+
+//return 1 if positiv, 0 if negativ
+int MiniExplorerCoimbra::sign2(float value){
+ if(value>=0)
+ return 1;
+ else
+ return 0;
+}
+
+/*angleToTarget is obtained through atan2 so it s:
+< 0 if the angle is bettween PI and 2pi on a trigo circle
+> 0 if it is between 0 and PI
+*/
+void MiniExplorerCoimbra::turn_to_target(float angleToTarget){
+ Odometria();
+ float theta_plus_h_pi=theta+PI/2;//theta is between -PI and PI
+ if(theta_plus_h_pi > PI)
+ theta_plus_h_pi=-(2*PI-theta_plus_h_pi);
+ if(angleToTarget>0){
+ leftMotor(0,1);
+ rightMotor(1,1);
+ }else{
+ leftMotor(1,1);
+ rightMotor(0,1);
+ }
+ while(abs(angleToTarget-theta_plus_h_pi)>0.05){
+ Odometria();
+ theta_plus_h_pi=theta+PI/2;//theta is between -PI and PI
+ if(theta_plus_h_pi > PI)
+ theta_plus_h_pi=-(2*PI-theta_plus_h_pi);
+ //pc.printf("\n\r diff=%f", abs(angleToTarget-theta_plus_h_pi));
+ }
+ leftMotor(1,0);
+ rightMotor(1,0);
+}
+
+
+/*
+//x and y passed are TargetNotMap
+float get_error_angles(float x, float y,float theta){
+ float angleBeetweenRobotAndTarget=atan2(y,x);
+ if(y > 0){
+ if(angleBeetweenRobotAndTarget < PI/2)//up right
+ angleBeetweenRobotAndTarget=-PI/2+angleBeetweenRobotAndTarget;
+ else//up right
+ angleBeetweenRobotAndTarget=angleBeetweenRobotAndTarget-PI/2;
+ }else{
+ if(angleBeetweenRobotAndTarget > -PI/2)//lower right
+ angleBeetweenRobotAndTarget=angleBeetweenRobotAndTarget-PI/2;
+ else//lower left
+ angleBeetweenRobotAndTarget=2*PI+angleBeetweenRobotAndTarget-PI/2;
+ }
+ return angleBeetweenRobotAndTarget-theta;
+}
+*/
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/MiniExplorerCoimbra.hpp Fri Jun 09 00:28:32 2017 +0000
@@ -0,0 +1,91 @@
+#ifndef MINIEXPLORERCOIMBRA_HPP
+#define MINIEXPLORERCOIMBRA_HPP
+
+#include "Map.hpp"
+#include "Sonar.hpp"
+#include "robot.h"
+#include "mbed.h"
+#include "math.h"
+
+ /*
+Robot coordinate system: World coordinate system:
+ ^ ^
+ |x |y
+ <--R O-->
+ y x
+*/
+
+class MiniExplorerCoimbra {
+
+ public:
+
+ Map map;
+ Sonar sonarLeft;
+ Sonar sonarFront;
+ Sonar sonarRight;
+ float speed;
+ float radiusWheels;
+ float distanceWheels;
+ float khro;
+ float ka;
+ float kb;
+ float kv;
+ float kh;
+
+ float rangeForce;
+ float attractionConstantForce;
+ float repulsionConstantForce;
+
+ MiniExplorerCoimbra(float defaultX, float defaultY, float defaultTheta);
+
+ void setXYTheta(float x, float y, float theta);
+
+ //generate a position randomly and makes the robot go there while updating the map
+ void randomize_and_map();
+
+ //go the the given position while updating the map
+ void go_to_point_with_angle(float targetX, float targetY, float targetAngle);
+
+ void update_sonar_values(float leftMm,float frontMm,float rightMm);
+
+ float get_converted_robot_X_into_world();
+
+ float get_converted_robot_Y_into_world();
+
+ void do_half_flip();
+
+ //Distance computation function
+ float dist(float robotX, float robotY, float targetX, float targetY);
+
+ float update_angular_speed_wheels_go_to_point_with_angle(float targetX, float targetY, float targetAngle, float dt);
+
+ void try_to_reach_target(float targetXWorld,float targetYWorld);
+
+ void vff(bool* reached, float targetXWorld, float targetYWorld);
+
+ //compute the force on X and Y
+ void compute_forceX_and_forceY(float* forceX, float* forceY, float targetXWorld, float targetYWorld);
+
+ void update_force(int widthIndice, int heightIndice, float* forceX, float* forceY, float xRobotWorld, float yRobotWorld );
+
+ //robotX and robotY are from Odometria, calculate line_a, line_b and line_c
+ void calculate_line(float forceX, float forceY, float *line_a, float *line_b, float *line_c);
+
+ //currently line_c is not used
+ void go_to_line(float line_a, float line_b, float line_c,float targetXWorld, float targetYWorld);
+
+ //return 1 if positiv, -1 if negativ
+ float sign1(float value);
+
+ //return 1 if positiv, 0 if negativ
+ int sign2(float value);
+
+ /*angleToTarget is obtained through atan2 so it s:
+ < 0 if the angle is bettween PI and 2pi on a trigo circle
+ > 0 if it is between 0 and PI
+ */
+ void turn_to_target(float angleToTarget);
+};
+
+#endif
+
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Sonar.cpp Fri Jun 09 00:28:32 2017 +0000
@@ -0,0 +1,104 @@
+#include "Sonar.hpp"
+
+Sonar::Sonar(float anlgeFromCenter, float distanceXRobot, float distanceYRobot ){
+ this->anlgeFromCenter=anlgeFromCenter;
+ this->distanceXRobot=-distanceYRobot;
+ this->distanceYRobot=distanceXRobot;
+ this->maxRange=50;//cm
+ this->minRange=10;//Rmin cm
+ this->incertitudeRange=10;//cm
+ this->angleRange=3.14159/3;//Omega rad
+}
+
+//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));
+ 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;
+ 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;
+
+ //float anglePointToSonar2=atan2(y-ys,x-xs)-theta;
+
+ //check if the distance between the cell and the robot is within the circle of range RADIUS_WHEELS
+ //check if absolute difference between the angles is no more than Omega/2
+ if(anglePointToSonar <= this->angleRange/2 || anglePointToSonar >= this->rad_angle_check(-this->angleRange/2)){
+ if( distancePointToSonar < (distanceObstacleDetected - this->incertitudeRange)){
+ //point before obstacle, probably empty
+ /*****************************************************************************/
+ float Ea=1.f-pow((2*alphaBeforeAdjustment)/this->angleRange,2);
+ float Er;
+ if(distancePointToSonar < this->minRange){
+ //point before minimum sonar range
+ Er=0.f;
+ }else{
+ //point after minimum sonar range
+ Er=1.f-pow((distancePointToSonar-this->minRange)/(distanceObstacleDetected-this->incertitudeRange-this->minRange),2);
+ }
+ /*****************************************************************************/
+ //if((1.f-Er*Ea)/2.f >1 || (1.f-Er*Ea)/2.f < 0)
+ // 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
+ /*****************************************************************************/
+ float Oa=1.f-pow((2*alphaBeforeAdjustment)/ANGLE_SONAR,2);
+ float Or;
+ if( distancePointToSonar <= (distanceObstacleDetected + this->incertitudeRange)){
+ //point between distanceObstacleDetected +- INCERTITUDE_SONAR
+ Or=1-pow((distancePointToSonar-distanceObstacleDetected)/(this->incertitudeRange),2);
+ }else{
+ //point after in range of the sonar but after the zone detected
+ Or=0;
+ }
+ /*****************************************************************************/
+ //if((1+Or*Oa)/2 >1 || (1+Or*Oa)/2 < 0)
+ // pc.printf("\n\r return value=%f,Er=%f,Ea=%f,alphaBeforeAdjustment=%f",(1+Or*Oa)/2,Or,Oa,alphaBeforeAdjustment);
+ return (1+Or*Oa)/2;
+ }
+ }
+ }
+ //not checked by the sonar
+ return 0.5;
+}
+
+//returns the angle between the vectors (x,y) and (xs,ys)
+float Sonar::compute_angle_between_vectors(float x, float y,float xs,float ys){
+ //alpha angle between ->x and ->SA
+ //vector S to A ->SA
+ float vSAx=x-xs;
+ float vSAy=y-ys;
+ //norme SA
+ float normeSA=sqrt(pow(vSAx,2)+pow(vSAy,2));
+ //vector ->x (1,0)
+ float cosAlpha=1*vSAy/*+0*vSAx*//normeSA;;
+ //vector ->y (0,1)
+ float sinAlpha=/*0*vSAy+*/1*vSAx/normeSA;//+0*vSAx;
+ if (sinAlpha < 0)
+ return -acos(cosAlpha);
+ else
+ return acos(cosAlpha);
+}
+
+//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;
+ }else{
+ while(inAngle < 0)
+ inAngle+=2*pi;
+ }
+ //cout<<" after :"<<inAngle<<endl;
+ return inAngle;
+}
+
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Sonar.hpp Fri Jun 09 00:28:32 2017 +0000
@@ -0,0 +1,33 @@
+#ifndef SONAR_HPP
+#define SONAR_HPP
+
+#include "MiniExplorerCoimbra.hpp"
+
+class Sonar {
+
+public:
+ float maxRange;//cm
+ float minRange;//Rmin cm
+ float incertitudeRange;//cm
+ float angleRange;//Omega rad
+ float angleFromCenter;
+ float distanceX;
+ float distanceY;
+
+
+ Sonar(float anlgeFromCenter, float distanceXRobot, float distanceYRobot );
+
+ //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);
+
+ //returns the angle between the vectors (x,y) and (xs,ys)
+ float compute_angle_between_vectors(float x, float y,float xs,float ys);
+
+ //makes the angle inAngle between 0 and 2pi
+ float rad_angle_check(float inAngle);
+
+};
+
+#endif
+
--- a/main.cpp Wed May 03 16:46:43 2017 +0000
+++ b/main.cpp Fri Jun 09 00:28:32 2017 +0000
@@ -1,555 +1,8 @@
-#include "mbed.h"
-#include "robot.h" // Initializes the robot. This include should be used in all main.cpp!
-#include "math.h"
-
- using namespace std;
-
-//fill initialLogValues with the values we already know (here the bordurs)
-void fill_initial_log_values();
-//generate a position randomly and makes the robot go there while updating the map
-void randomize_and_map();
-//make the robot do a pi/2 flip
-void do_half_flip();
-//go the the given position while updating the map
-void go_to_point_with_angle(float target_x, float target_y, float target_angle);
-//Updates sonar values
-void update_sonar_values(float leftMm, float frontMm, float rightMm);
-//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 x, float y,float xs,float ys, float angleFromSonarPosition, float distanceObstacleDetected);
-//print the map
-void print_final_map();
-//print the map with the robot marked on it
-void print_final_map_with_robot_position();
-
-//MATHS heavy functions
-float dist(float robot_x, float robot_y, float target_x, float target_y);
-//returns the probability [0,1] that the cell is occupied from the log value lt
-float log_to_proba(float lt);
-//returns the log value that the cell is occupied from the probability value [0,1]
-float proba_to_log(float p);
-//returns the new log value
-float compute_log_estimation_lt(float previousLogValue,float currentProbability,float originalLogvalue );
-//makes the angle inAngle between 0 and 2pi
-float rad_angle_check(float inAngle);
-//returns the angle between the vectors (x,y) and (xs,ys)
-float compute_angle_between_vectors(float x, float y,float xs,float ys);
-float robot_center_x_in_orthonormal_x();
-float robot_center_y_in_orthonormal_y();
-float robot_sonar_front_x_in_orthonormal_x();
-float robot_sonar_front_y_in_orthonormal_y();
-float robot_sonar_right_x_in_orthonormal_x();
-float robot_sonar_right_y_in_orthonormal_y();
-float robot_sonar_left_x_in_orthonormal_x();
-float robot_sonar_left_y_in_orthonormal_y();
-float estimated_width_indice_in_orthonormal_x(int i);
-float estimated_height_indice_in_orthonormal_y(int j);
-void compute_angles_and_distance(float target_x, float target_y, float target_angle);
-void compute_linear_angular_velocities();
-//update foceX and forceY if necessary
-void updateForce(int widthIndice, int heightIndice, float range, float* forceX, float* forceY, float xRobotOrtho, float yRobotOrtho );
-//compute the X and Y force
-void compute_forceX_and_forceY(float targetX, float targetY,float forceX, float forceY);
-
-
-const float pi=3.14159;
-
-//CONSTANT FORCE FIELD
-const float FORCE_CONSTANT_REPULSION=10;//TODO tweak it
-const float FORCE_CONSTANT_ATTRACTION=10;//TODO tweak it
-const float RANGE_FORCE=50;//TODO tweak it
+#include "MiniExplorerCoimbra.hpp"
-//spec of the sonar
-//TODO MEASURE THE DISTANCE on X and Y of the robot frame, between each sonar and the center of the robot and add it to calculus in updateSonarValues
-const float RANGE_SONAR=50;//cm
-const float RANGE_SONAR_MIN=10;//Rmin cm
-const float INCERTITUDE_SONAR=10;//cm
-const float ANGLE_SONAR=pi/3;//Omega rad
-
-//those distance and angle are approximation in need of measurements, in the orthonormal frame
-const float ANGLE_FRONT_TO_LEFT=10*pi/36;//50 degrees
-const float DISTANCE_SONAR_LEFT_X=-4;
-const float DISTANCE_SONAR_LEFT_Y=4;
-
-const float ANGLE_FRONT_TO_RIGHT=-10*pi/36;//-50 degrees
-const float DISTANCE_SONAR_RIGHT_X=4;
-const float DISTANCE_SONAR_RIGHT_Y=4;
-
-const float ANGLE_FRONT_TO_FRONT=0;
-const float DISTANCE_SONAR_FRONT_X=0;
-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 int SIZE_MAP=25;
-const int NB_CELL_WIDTH=24;
-const int NB_CELL_HEIGHT=18;
-
-//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;
-
-//used to create the map 250 represent the 250cm of the square where the robot is tested
-//float sizeCell=250/(float)SIZE_MAP;
-float sizeCellWidth=WIDTH_ARENA/(float)NB_CELL_WIDTH;
-float sizeCellHeight=HEIGHT_ARENA/(float)NB_CELL_HEIGHT;
-
-float map[NB_CELL_WIDTH][NB_CELL_HEIGHT];//contains the log values for each cell
-float initialLogValues[NB_CELL_WIDTH][NB_CELL_HEIGHT];
-
-//Diameter of a wheel and distance between the 2
-const float RADIUS_WHEELS=3.25;
-const float DISTANCE_WHEELS=7.2;
-
-const int MAX_SPEED=250;//TODO TWEAK THE SPEED SO IT DOES NOT FUCK UP
-
-float alpha; //angle error
-float rho; //distance from target
-float beta;
-float kRho=12, ka=30, kb=-13; //Kappa values
-float linear, angular, angular_left, angular_right;
-float dt=0.5;
-float temp;
-float d2;
-Timer t;
-
-int speed=MAX_SPEED; // Max speed at beggining of movement
-
-float leftMm;
-float frontMm;
-float rightMm;
+using namespace std;
int main(){
-
- i2c1.frequency(100000);
- initRobot(); //Initializing the robot
- pc.baud(9600); // baud for the pc communication
-
- measure_always_on();//TODO check if needed
- wait(0.5);
-
- fill_initial_log_values();
-
- theta=DEFAULT_THETA;
- X=DEFAULT_X;
- Y=DEFAULT_Y;
-
-
- for (int i = 0; i<10; i++) {
- randomize_and_map();
- //print_final_map();
- print_final_map_with_robot_position();
- }
- while(1){
- print_final_map();
- print_final_map_with_robot_position();
- }
-
-}
-
-//fill initialLogValues with the values we already know (here the bordurs)
-void fill_initial_log_values(){
- //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)
- initialLogValues[i][j] = proba_to_log(1);
- else
- initialLogValues[i][j] = proba_to_log(0.5);
- }
- }
-}
-
-//generate a position randomly and makes the robot go there while updating the map
-void randomize_and_map() {
- //TODO check that it's aurelien's work
- float target_x = (rand()%(int)(HEIGHT_ARENA*10))/10;//for decimal precision
- float target_y = (rand()%(int)(WIDTH_ARENA*10))/10;
- float target_angle = 2*((float)(rand()%31416)-15708)/10000.0;
-
- //TODO comment that
- //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);
-
- go_to_point_with_angle(target_x, target_y, target_angle);
-}
-
-
-void do_half_flip(){
- Odometria();
- float theta_plus_h_pi=theta+pi/2;//theta is between -pi and pi
- if(theta_plus_h_pi > pi)
- theta_plus_h_pi=-(2*pi-theta_plus_h_pi);
- leftMotor(0,100);
- rightMotor(1,100);
- while(abs(theta_plus_h_pi-theta)>0.05){
- Odometria();
- // pc.printf("\n\r diff=%f", abs(theta_plus_pi-theta));
- }
- leftMotor(1,0);
- rightMotor(1,0);
-}
-
-//go the the given position while updating the map
-//TODO clean this procedure it's ugly as hell and too long
-void go_to_point_with_angle(float target_x, float target_y, float target_angle) {
- Odometria();
- 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;
- //beta = atan(sin(beta)/cos(beta));
- bool keep_going=true;
- do {
- //Timer stuff
- dt = t.read();
- t.reset();
- t.start();
-
- //Updating X,Y and theta with the odometry values
- Odometria();
- leftMm = get_distance_left_sensor();
- frontMm = get_distance_front_sensor();
- rightMm = get_distance_right_sensor();
-
- //pc.printf("\n\r leftMm=%f", leftMm);
- //pc.printf("\n\r frontMm=%f", frontMm);
- //pc.printf("\n\r rightMm=%f", rightMm);
-
- //if in dangerzone
- if(frontMm < 120 || leftMm <120 || rightMm <120){
- leftMotor(1,0);
- rightMotor(1,0);
- update_sonar_values(leftMm, frontMm, rightMm);
- //TODO Giorgos maybe you can also test the do_half_flip() function
- Odometria();
- //do a flip TODO
- keep_going=false;
- do_half_flip();
- }else{
- //if not in danger zone continue as usual
- update_sonar_values(leftMm, frontMm, rightMm);
- compute_angles_and_distance(target_x, target_y, target_angle); //Compute the angles and the distance from target
- compute_linear_angular_velocities(); //Using the angles and distance, compute the velocities needed (linear & angular)
-
- //pc.printf("\n\r X=%f", X);
- //pc.printf("\n\r Y=%f", Y);
-
- //pc.printf("\n\r a_r=%f", angular_right);
- //pc.printf("\n\r a_l=%f", angular_left);
-
- //Updating motor velocities
- leftMotor(1,angular_left);
- rightMotor(1,angular_right);
-
- wait(0.2);
- //Timer stuff
- t.stop();
- }
- } while(d2>1 && (abs(target_angle-theta)>0.01) && keep_going);
-
- //Stop at the end
- leftMotor(1,0);
- rightMotor(1,0);
-}
-
-//Updates sonar values
-void update_sonar_values(float leftMm, float frontMm, float rightMm){
- float currProba;
- float i_in_orthonormal;
- float j_in_orthonormal;
- for(int i=0;i<NB_CELL_WIDTH;i++){
- for(int j=0;j<NB_CELL_HEIGHT;j++){
- //check if the point A(x,y) in the world frame is within the range of the sonar (which has the coordinates xs, ys in the world frame)
- //check that again
- //compute for front sonar
- i_in_orthonormal=estimated_width_indice_in_orthonormal_x(i);
- j_in_orthonormal=estimated_height_indice_in_orthonormal_y(j);
-
- currProba=compute_probability_t(i_in_orthonormal,j_in_orthonormal,robot_sonar_front_x_in_orthonormal_x(),robot_sonar_front_y_in_orthonormal_y(),ANGLE_FRONT_TO_FRONT,frontMm/10);
- map[i][j]=map[i][j]+proba_to_log(currProba)+initialLogValues[i][j];//map is filled as map[0][0] get the data for the point closest to the origin
- //compute for right sonar
- currProba=compute_probability_t(i_in_orthonormal,j_in_orthonormal,robot_sonar_right_x_in_orthonormal_x(),robot_sonar_right_y_in_orthonormal_y(),ANGLE_FRONT_TO_RIGHT,rightMm/10);
- map[i][j]=map[i][j]+proba_to_log(currProba)+initialLogValues[i][j];
- //compute for left sonar
- currProba=compute_probability_t(i_in_orthonormal,j_in_orthonormal,robot_sonar_left_x_in_orthonormal_x(),robot_sonar_left_y_in_orthonormal_y(),ANGLE_FRONT_TO_LEFT,leftMm/10);
- map[i][j]=map[i][j]+proba_to_log(currProba)+initialLogValues[i][j];
- }
- }
-}
-
-//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 x, float y,float xs,float ys, float angleFromSonarPosition, float distanceObstacleDetected){
-
- float alpha=compute_angle_between_vectors(x,y,xs,ys);//angle beetween the point and the sonar beam
- float alphaBeforeAdjustment=alpha-theta-angleFromSonarPosition;
- alpha=rad_angle_check(alphaBeforeAdjustment);//TODO I feel you don't need to do that but I m not sure
- float distancePointToSonar=sqrt(pow(x-xs,2)+pow(y-ys,2));
-
- //check if the distance between the cell and the robot is within the circle of range RADIUS_WHEELS
- //check if absolute difference between the angles is no more than Omega/2
- if( distancePointToSonar < (RANGE_SONAR)&& (alpha <= ANGLE_SONAR/2 || alpha >= rad_angle_check(-ANGLE_SONAR/2))){
- if( distancePointToSonar < (distanceObstacleDetected - INCERTITUDE_SONAR)){
- //point before obstacle, probably empty
- /*****************************************************************************/
- float Ea=1.f-pow((2*alphaBeforeAdjustment)/ANGLE_SONAR,2);
- float Er;
- if(distancePointToSonar < RANGE_SONAR_MIN){
- //point before minimum sonar range
- Er=0.f;
- }else{
- //point after minimum sonar range
- Er=1.f-pow((distancePointToSonar-RANGE_SONAR_MIN)/(distanceObstacleDetected-INCERTITUDE_SONAR-RANGE_SONAR_MIN),2);
- }
- /*****************************************************************************/
- return (1.f-Er*Ea)/2.f;
- }else{
- //probably occupied
- /*****************************************************************************/
- float Oa=1.f-pow((2*alphaBeforeAdjustment)/ANGLE_SONAR,2);
- float Or;
- if( distancePointToSonar <= (distanceObstacleDetected + INCERTITUDE_SONAR)){
- //point between distanceObstacleDetected +- INCERTITUDE_SONAR
- Or=1-pow((distancePointToSonar-distanceObstacleDetected)/(INCERTITUDE_SONAR),2);
- }else{
- //point after in range of the sonar but after the zone detected
- Or=0;
- }
- /*****************************************************************************/
- return (1+Or*Oa)/2;
- }
- }else{
- //not checked by the sonar
- return 0.5;
- }
+ MiniExplorerCoimbra myRobot(0,0,0);
+ return 0;
}
-
-void print_final_map() {
- float currProba;
- pc.printf("\n\r");
- for (int y = NB_CELL_HEIGHT -1; y>-1; y--) {
- for (int x= 0; x<NB_CELL_WIDTH; x++) {
- currProba=log_to_proba(map[x][y]);
- if ( currProba < 0.5) {
- pc.printf(" ");
- } else {
- if(currProba==0.5)
- pc.printf(" . ");
- else
- pc.printf(" X ");
- }
- }
- pc.printf("\n\r");
- }
-}
-
-void print_final_map_with_robot_position() {
- float currProba;
- Odometria();
- float Xrobot=robot_center_x_in_orthonormal_x();
- float Yrobot=robot_center_y_in_orthonormal_y();
-
- float heightIndiceInOrthonormal;
- float widthIndiceInOrthonormal;
-
- float widthMalus=-(3*sizeCellWidth/2);
- float widthBonus=sizeCellWidth/2;
-
- float heightMalus=-(3*sizeCellHeight/2);
- float heightBonus=sizeCellHeight/2;
-
- pc.printf("\n\r");
- for (int y = NB_CELL_HEIGHT -1; y>-1; y--) {
- for (int x= 0; x<NB_CELL_WIDTH; x++) {
- heightIndiceInOrthonormal=estimated_height_indice_in_orthonormal_y(y);
- widthIndiceInOrthonormal=estimated_width_indice_in_orthonormal_x(x);
- if(Yrobot >= (heightIndiceInOrthonormal+heightMalus) && Yrobot <= (heightIndiceInOrthonormal+heightBonus) && Xrobot >= (widthIndiceInOrthonormal+widthMalus) && Xrobot <= (widthIndiceInOrthonormal+widthBonus))
- pc.printf(" R ");
- else{
- currProba=log_to_proba(map[x][y]);
- if ( currProba < 0.5)
- pc.printf(" ");
- else{
- if(currProba==0.5)
- pc.printf(" . ");
- else
- pc.printf(" X ");
- }
- }
- }
- pc.printf("\n\r");
- }
-}
-
-//MATHS heavy functions
-/**********************************************************************/
-//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));
-}
-
-//returns the probability [0,1] that the cell is occupied from the log valAue lt
-float log_to_proba(float lt){
- return 1-1/(1+exp(lt));
-}
-
-//returns the log value that the cell is occupied from the probability value [0,1]
-float proba_to_log(float p){
- return log(p/(1-p));
-}
-
-//returns the new log value
-float compute_log_estimation_lt(float previousLogValue,float currentProbability,float originalLogvalue ){
- return previousLogValue+proba_to_log(currentProbability)-originalLogvalue;
-}
-
-//makes the angle inAngle between 0 and 2pi
-float rad_angle_check(float inAngle){
- //cout<<"before :"<<inAngle;
- if(inAngle > 0){
- while(inAngle > (2*pi))
- inAngle-=2*pi;
- }else{
- while(inAngle < 0)
- inAngle+=2*pi;
- }
- //cout<<" after :"<<inAngle<<endl;
- return inAngle;
-}
-
-//returns the angle between the vectors (x,y) and (xs,ys)
-float compute_angle_between_vectors(float x, float y,float xs,float ys){
- //alpha angle between ->x and ->SA
- //vector S to A ->SA
- float vSAx=x-xs;
- float vSAy=y-ys;
- //norme SA
- float normeSA=sqrt(pow(vSAx,2)+pow(vSAy,2));
- //vector ->x (1,0)
- float cosAlpha=1*vSAy/*+0*vSAx*//normeSA;;
- //vector ->y (0,1)
- float sinAlpha=/*0*vSAy+*/1*vSAx/normeSA;//+0*vSAx;
- if (sinAlpha < 0)
- return -acos(cosAlpha);
- else
- return acos(cosAlpha);
-}
-
-float robot_center_x_in_orthonormal_x(){
- return NB_CELL_WIDTH*sizeCellWidth-Y;
-}
-
-float robot_center_y_in_orthonormal_y(){
- return X;
-}
-
-float robot_sonar_front_x_in_orthonormal_x(){
- return robot_center_x_in_orthonormal_x()+DISTANCE_SONAR_FRONT_X;
-}
-float robot_sonar_front_y_in_orthonormal_y(){
- return robot_center_y_in_orthonormal_y()+DISTANCE_SONAR_FRONT_Y;
-}
-
-float robot_sonar_right_x_in_orthonormal_x(){
- return robot_center_x_in_orthonormal_x()+DISTANCE_SONAR_RIGHT_X;
-}
-float robot_sonar_right_y_in_orthonormal_y(){
- return robot_center_y_in_orthonormal_y()+DISTANCE_SONAR_RIGHT_Y;
-}
-
-float robot_sonar_left_x_in_orthonormal_x(){
- return robot_center_x_in_orthonormal_x()+DISTANCE_SONAR_LEFT_X;
-}
-float robot_sonar_left_y_in_orthonormal_y(){
- return robot_center_y_in_orthonormal_y()+DISTANCE_SONAR_LEFT_Y;
-}
-
-float estimated_width_indice_in_orthonormal_x(int i){
- return sizeCellWidth/2+i*sizeCellWidth;
-}
-float estimated_height_indice_in_orthonormal_y(int j){
- return sizeCellHeight/2+j*sizeCellHeight;
-}
-
-void compute_angles_and_distance(float target_x, float target_y, float target_angle){
- alpha = atan2((target_y-Y),(target_x-X))-theta;
- alpha = atan(sin(alpha)/cos(alpha));
- rho = dist(X, Y, target_x, target_y);
- d2 = rho;
- beta = -alpha-theta+target_angle;
-
- //Computing angle error and distance towards the target value
- rho += dt*(-kRho*cos(alpha)*rho);
- temp = alpha;
- alpha += dt*(kRho*sin(alpha)-ka*alpha-kb*beta);
- beta += dt*(-kRho*sin(temp));
- //pc.printf("\n\r d2=%f", d2);
- //pc.printf("\n\r dt=%f", dt);
-}
-
-void compute_linear_angular_velocities(){
- //Computing linear and angular velocities
- if(alpha>=-1.5708 && alpha<=1.5708){
- linear=kRho*rho;
- angular=ka*alpha+kb*beta;
- }
- else{
- linear=-kRho*rho;
- angular=-ka*alpha-kb*beta;
- }
- angular_left=(linear-0.5*DISTANCE_WHEELS*angular)/RADIUS_WHEELS;
- angular_right=(linear+0.5*DISTANCE_WHEELS*angular)/RADIUS_WHEELS;
-
- //Slowing down at the end for more precision
- // if (d2<25) {
- // speed = d2*30;
- // }
-
- //Normalize speed for motors
- if(angular_left>angular_right) {
- angular_right=speed*angular_right/angular_left;
- angular_left=speed;
- } else {
- angular_left=speed*angular_left/angular_right;
- angular_right=speed;
- }
-}
-
-
-void updateForce(int widthIndice, int heightIndice, float range, float* forceX, float* forceY, float xRobotOrtho, float yRobotOrtho ){
-
- //get the coordonate of the map and the robot in the ortonormal frame
- float xCenterCell=estimated_width_indice_in_orthonormal_x(widthIndice);
- float yCenterCell=estimated_height_indice_in_orthonormal_y(heightIndice);
- //compute the distance beetween the cell and the robot
- float distanceCellToRobot=sqrt(pow(xCenterCell-xRobotOrtho,2)+pow(yCenterCell-yRobotOrtho,2));
- //check if the cell is in range
- if(distanceCellToRobot <= (range)) {
- float probaCell=log_to_proba(map[widthIndice][heightIndice]);
- float xForceComputed=FORCE_CONSTANT_REPULSION*probaCell*(xCenterCell-xRobotOrtho)/pow(distanceCellToRobot,3);
- float yForceComputed=FORCE_CONSTANT_REPULSION*probaCell*(yCenterCell-yRobotOrtho)/pow(distanceCellToRobot,3);
- *forceX+=xForceComputed;
- *forceY+=yForceComputed;
- }
-}
-
-void compute_forceX_and_forceY(float targetX, float targetY,float forceX, float forceY){
- float xRobotOrtho=robot_center_x_in_orthonormal_x();
- float yRobotOrtho=robot_center_y_in_orthonormal_y();
- for(int i=0;i<NB_CELL_WIDTH;i++){
- for(int j=0;j<NB_CELL_HEIGHT;j++){
- updateForce(i,j,RANGE_FORCE,&forceX,&forceY,xRobotOrtho,yRobotOrtho);
- }
- }
- //update with attraction force
- forceX+=FORCE_CONSTANT_ATTRACTION*(targetX-xRobotOrtho)/sqrt(pow(targetX-xRobotOrtho,2)+pow(targetY-yRobotOrtho,2));
- forceY+=FORCE_CONSTANT_ATTRACTION*(targetY-yRobotOrtho)/sqrt(pow(targetX-xRobotOrtho,2)+pow(targetY-yRobotOrtho,2));
- float amplitude=sqrt(pow(forceX,2)+pow(forceY,2));
- forceX=forceX/amplitude;
- forceY=forceY/amplitude;
-}
\ No newline at end of file