main.cpp

00001 #include "mbed.h"
00002 #include "robot.h" // Initializes the robot. This include should be used in all main.cpp!
00003 #include "math.h"
00004 
00005  using namespace std;
00006 
00007 //fill initialLogValues with the values we already know (here the bordurs)
00008 void fill_initial_log_values();
00009 //generate a position randomly and makes the robot go there while updating the map
00010 void randomize_and_map();
00011 //make the robot do a pi/2 flip
00012 void do_half_flip();
00013 //go the the given position while updating the map
00014 void go_to_point_with_angle(float target_x, float target_y, float target_angle);
00015 //Updates sonar values
00016 void update_sonar_values(float leftMm, float frontMm, float rightMm);
00017 //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]
00018 float compute_probability_t(float x, float y,float xs,float ys, float angleFromSonarPosition, float distanceObstacleDetected);
00019 //print the map
00020 void print_final_map();
00021 //print the map with the robot marked on it
00022 void print_final_map_with_robot_position();
00023 
00024 //MATHS heavy functions
00025 float dist(float robot_x, float robot_y, float target_x, float target_y);
00026 //returns the probability [0,1] that the cell is occupied from the log value lt
00027 float log_to_proba(float lt);
00028 //returns the log value that the cell is occupied from the probability value [0,1]
00029 float proba_to_log(float p);
00030 //returns the new log value
00031 float compute_log_estimation_lt(float previousLogValue,float currentProbability,float originalLogvalue );
00032 //makes the angle inAngle between 0 and 2pi
00033 float rad_angle_check(float inAngle);
00034 //returns the angle between the vectors (x,y) and (xs,ys)
00035 float compute_angle_between_vectors(float x, float y,float xs,float ys);
00036 float robot_center_x_in_orthonormal_x();
00037 float robot_center_y_in_orthonormal_y();
00038 float robot_sonar_front_x_in_orthonormal_x();
00039 float robot_sonar_front_y_in_orthonormal_y();
00040 float robot_sonar_right_x_in_orthonormal_x();
00041 float robot_sonar_right_y_in_orthonormal_y();
00042 float robot_sonar_left_x_in_orthonormal_x();
00043 float robot_sonar_left_y_in_orthonormal_y();
00044 float estimated_width_indice_in_orthonormal_x(int i);
00045 float estimated_height_indice_in_orthonormal_y(int j);
00046 void compute_angles_and_distance(float target_x, float target_y, float target_angle);
00047 void compute_linear_angular_velocities();
00048 
00049 const float pi=3.14159;
00050 //spec of the sonar
00051 //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
00052 const float RANGE_SONAR=50;//cm
00053 const float RANGE_SONAR_MIN=10;//Rmin cm
00054 const float INCERTITUDE_SONAR=10;//cm
00055 const float ANGLE_SONAR=pi/3;//Omega rad
00056 
00057 //those distance and angle are approximation in need of measurements, in the orthonormal frame
00058 const float ANGLE_FRONT_TO_LEFT=10*pi/36;//50 degrees
00059 const float DISTANCE_SONAR_LEFT_X=-4;
00060 const float DISTANCE_SONAR_LEFT_Y=4;
00061 
00062 const float ANGLE_FRONT_TO_RIGHT=-10*pi/36;//-50 degrees
00063 const float DISTANCE_SONAR_RIGHT_X=4;
00064 const float DISTANCE_SONAR_RIGHT_Y=4;
00065 
00066 const float ANGLE_FRONT_TO_FRONT=0;
00067 const float DISTANCE_SONAR_FRONT_X=0;
00068 const float DISTANCE_SONAR_FRONT_Y=5;
00069 
00070 //TODO adjust the size of the map for computation time (25*25?)
00071 const float WIDTH_ARENA=250;//cm
00072 const float HEIGHT_ARENA=250;//cm
00073 
00074 //const int SIZE_MAP=25;
00075 const int NB_CELL_WIDTH=20;
00076 const int NB_CELL_HEIGHT=20;
00077 
00078 //position and orientation of the robot when put on the map (ODOMETRY doesn't know those) it's in the robot frame
00079 //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
00080 const float DEFAULT_X=HEIGHT_ARENA/2;
00081 const float DEFAULT_Y=WIDTH_ARENA/2;
00082 const float DEFAULT_THETA=0;
00083 
00084 //used to create the map 250 represent the 250cm of the square where the robot is tested
00085 //float sizeCell=250/(float)SIZE_MAP;
00086 float sizeCellWidth=WIDTH_ARENA/(float)NB_CELL_WIDTH;
00087 float sizeCellHeight=HEIGHT_ARENA/(float)NB_CELL_HEIGHT;
00088 
00089 float map[NB_CELL_WIDTH][NB_CELL_HEIGHT];//contains the log values for each cell
00090 float initialLogValues[NB_CELL_WIDTH][NB_CELL_HEIGHT];
00091 
00092 //Diameter of a wheel and distance between the 2
00093 const float RADIUS_WHEELS=3.25;
00094 const float DISTANCE_WHEELS=7.2;
00095 
00096 const int MAX_SPEED=250;//TODO TWEAK THE SPEED SO IT DOES NOT FUCK UP
00097 
00098 float alpha; //angle error
00099 float rho; //distance from target
00100 float beta;
00101 float kRho=12, ka=30, kb=-13; //Kappa values
00102 float linear, angular, angular_left, angular_right;
00103 float dt=0.5;
00104 float temp;
00105 float d2;
00106 Timer t;
00107 
00108 int speed=MAX_SPEED; // Max speed at beggining of movement
00109 
00110 float leftMm;
00111 float frontMm;
00112 float rightMm; 
00113 
00114 int main(){
00115     
00116     i2c1.frequency(100000);
00117     initRobot(); //Initializing the robot
00118     pc.baud(9600); // baud for the pc communication
00119 
00120     measure_always_on();//TODO check if needed
00121     wait(0.5);
00122     
00123     fill_initial_log_values();
00124 
00125     theta=DEFAULT_THETA;
00126     X=DEFAULT_X;
00127     Y=DEFAULT_Y;
00128 
00129     
00130     for (int i = 0; i<10; i++) {
00131         randomize_and_map(); 
00132         //print_final_map();
00133         print_final_map_with_robot_position();   
00134     }
00135     while(1){
00136         print_final_map();
00137         print_final_map_with_robot_position();
00138     }
00139 
00140 }
00141 
00142 //fill initialLogValues with the values we already know (here the bordurs)
00143 void fill_initial_log_values(){
00144     //Fill map, we know the border are occupied
00145     for (int i = 0; i<NB_CELL_WIDTH; i++) {
00146         for (int j = 0; j<NB_CELL_HEIGHT; j++) {
00147             if(j==0 || j==NB_CELL_HEIGHT-1 || i==0 || i==NB_CELL_WIDTH-1)
00148                 initialLogValues[i][j] = proba_to_log(1);
00149             else
00150                 initialLogValues[i][j] = proba_to_log(0.5);
00151         }
00152     }
00153 }
00154 
00155 //generate a position randomly and makes the robot go there while updating the map
00156 void randomize_and_map() {
00157     //TODO check that it's aurelien's work
00158     float target_x = (rand()%(int)(HEIGHT_ARENA*10))/10;//for decimal precision
00159     float target_y = (rand()%(int)(WIDTH_ARENA*10))/10;
00160     float target_angle = 2*((float)(rand()%31416)-15708)/10000.0;
00161     
00162     //TODO comment that
00163     //pc.printf("\n\r targ_X=%f", target_x);
00164     //pc.printf("\n\r targ_Y=%f", target_y);
00165     //pc.printf("\n\r targ_Angle=%f", target_angle);
00166     
00167     go_to_point_with_angle(target_x, target_y, target_angle);
00168 }
00169 
00170 
00171 void do_half_flip(){
00172     Odometria();
00173     float theta_plus_h_pi=theta+pi/2;//theta is between -pi and pi
00174     if(theta_plus_h_pi > pi)
00175         theta_plus_h_pi=-(2*pi-theta_plus_h_pi);
00176     leftMotor(0,100);
00177     rightMotor(1,100);
00178     while(abs(theta_plus_h_pi-theta)>0.05){
00179         Odometria();
00180        // pc.printf("\n\r diff=%f", abs(theta_plus_pi-theta));
00181     }
00182     leftMotor(1,0);
00183     rightMotor(1,0);    
00184 }
00185 
00186 //go the the given position while updating the map
00187 //TODO clean this procedure it's ugly as hell and too long
00188 void go_to_point_with_angle(float target_x, float target_y, float target_angle) {
00189     Odometria();
00190     alpha = atan2((target_y-Y),(target_x-X))-theta;
00191     alpha = atan(sin(alpha)/cos(alpha));
00192     rho = dist(X, Y, target_x, target_y);
00193     beta = -alpha-theta+target_angle;
00194     //beta = atan(sin(beta)/cos(beta));
00195     bool keep_going=true;
00196     do {
00197         //Timer stuff
00198         dt = t.read();
00199         t.reset();
00200         t.start();
00201         
00202         //Updating X,Y and theta with the odometry values
00203         Odometria();
00204         leftMm = get_distance_left_sensor();
00205         frontMm = get_distance_front_sensor();
00206         rightMm = get_distance_right_sensor();
00207 
00208         //pc.printf("\n\r leftMm=%f", leftMm);
00209         //pc.printf("\n\r frontMm=%f", frontMm);
00210         //pc.printf("\n\r rightMm=%f", rightMm);
00211     
00212         //if in dangerzone 
00213         if(frontMm < 120 || leftMm <120 || rightMm <120){
00214             leftMotor(1,0);
00215             rightMotor(1,0);
00216             update_sonar_values(leftMm, frontMm, rightMm);
00217             //TODO Giorgos maybe you can also test the do_half_flip() function
00218             Odometria();
00219             //do a flip TODO
00220             keep_going=false;
00221             do_half_flip();   
00222         }else{
00223             //if not in danger zone continue as usual
00224             update_sonar_values(leftMm, frontMm, rightMm);
00225             compute_angles_and_distance(target_x, target_y, target_angle); //Compute the angles and the distance from target
00226             compute_linear_angular_velocities(); //Using the angles and distance, compute the velocities needed (linear & angular)
00227     
00228             //pc.printf("\n\r X=%f", X);
00229             //pc.printf("\n\r Y=%f", Y);
00230     
00231             //pc.printf("\n\r a_r=%f", angular_right);
00232             //pc.printf("\n\r a_l=%f", angular_left);
00233     
00234             //Updating motor velocities
00235             leftMotor(1,angular_left);
00236             rightMotor(1,angular_right);
00237     
00238             wait(0.2);
00239             //Timer stuff
00240             t.stop();
00241         }
00242     } while(d2>1 && (abs(target_angle-theta)>0.01) && keep_going);
00243 
00244     //Stop at the end
00245     leftMotor(1,0);
00246     rightMotor(1,0);
00247 }
00248 
00249 //Updates sonar values
00250 void update_sonar_values(float leftMm, float frontMm, float rightMm){
00251     float currProba;
00252     float i_in_orthonormal;
00253     float j_in_orthonormal;
00254     for(int i=0;i<NB_CELL_WIDTH;i++){
00255         for(int j=0;j<NB_CELL_HEIGHT;j++){
00256                 //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)
00257             //check that again
00258             //compute for front sonar
00259             i_in_orthonormal=estimated_width_indice_in_orthonormal_x(i);
00260             j_in_orthonormal=estimated_height_indice_in_orthonormal_y(j);
00261 
00262             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);
00263             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
00264             //compute for right sonar
00265             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);
00266             map[i][j]=map[i][j]+proba_to_log(currProba)+initialLogValues[i][j];
00267              //compute for left sonar
00268             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);
00269             map[i][j]=map[i][j]+proba_to_log(currProba)+initialLogValues[i][j];
00270         }
00271     }
00272 }
00273 
00274 //ODOMETRIA MUST HAVE BEEN CALLED
00275 //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]
00276 float compute_probability_t(float x, float y,float xs,float ys, float angleFromSonarPosition, float distanceObstacleDetected){
00277 
00278     float alpha=compute_angle_between_vectors(x,y,xs,ys);//angle beetween the point and the sonar beam
00279     float alphaBeforeAdjustment=alpha-theta-angleFromSonarPosition;
00280     alpha=rad_angle_check(alphaBeforeAdjustment);//TODO I feel you don't need to do that but I m not sure
00281     float distancePointToSonar=sqrt(pow(x-xs,2)+pow(y-ys,2));
00282 
00283     //check if the distance between the cell and the robot is within the circle of range RADIUS_WHEELS
00284     //check if absolute difference between the angles is no more than Omega/2
00285     if( distancePointToSonar < (RANGE_SONAR)&& (alpha <= ANGLE_SONAR/2 || alpha >= rad_angle_check(-ANGLE_SONAR/2))){
00286         if( distancePointToSonar < (distanceObstacleDetected - INCERTITUDE_SONAR)){
00287         //point before obstacle, probably empty
00288         /*****************************************************************************/
00289             float Ea=1.f-pow((2*alphaBeforeAdjustment)/ANGLE_SONAR,2);
00290             float Er;
00291             if(distancePointToSonar < RANGE_SONAR_MIN){
00292                 //point before minimum sonar range
00293                 Er=0.f;
00294             }else{
00295                 //point after minimum sonar range
00296                 Er=1.f-pow((distancePointToSonar-RANGE_SONAR_MIN)/(distanceObstacleDetected-INCERTITUDE_SONAR-RANGE_SONAR_MIN),2);
00297             }
00298          /*****************************************************************************/
00299             return (1.f-Er*Ea)/2.f;
00300         }else{
00301             //probably occupied
00302         /*****************************************************************************/
00303             float Oa=1.f-pow((2*alphaBeforeAdjustment)/ANGLE_SONAR,2);
00304             float Or;
00305             if( distancePointToSonar <= (distanceObstacleDetected + INCERTITUDE_SONAR)){
00306                 //point between distanceObstacleDetected +- INCERTITUDE_SONAR
00307                 Or=1-pow((distancePointToSonar-distanceObstacleDetected)/(INCERTITUDE_SONAR),2);
00308             }else{
00309                 //point after in range of the sonar but after the zone detected
00310                 Or=0;
00311             }
00312         /*****************************************************************************/
00313             return (1+Or*Oa)/2;
00314         }
00315     }else{
00316         //not checked by the sonar
00317         return 0.5;
00318     }
00319 }
00320 
00321 void print_final_map() {
00322     float currProba;
00323     pc.printf("\n\r");
00324     for (int y = NB_CELL_HEIGHT -1; y>-1; y--) {
00325         for (int x= 0; x<NB_CELL_WIDTH; x++) {
00326                 currProba=log_to_proba(map[x][y]);
00327             if ( currProba < 0.5) {
00328                 pc.printf("   ");
00329             } else {
00330                 if(currProba==0.5)
00331                     pc.printf(" . ");
00332                 else
00333                     pc.printf(" X ");
00334             }
00335         }
00336         pc.printf("\n\r");
00337     }
00338 }
00339 
00340 void print_final_map_with_robot_position() {
00341     float currProba;
00342     Odometria();
00343     float Xrobot=robot_center_x_in_orthonormal_x();
00344     float Yrobot=robot_center_y_in_orthonormal_y();
00345     
00346     float heightIndiceInOrthonormal;
00347     float widthIndiceInOrthonormal;
00348     
00349     float widthMalus=-(3*sizeCellWidth/2);
00350     float widthBonus=sizeCellWidth/2;
00351     
00352     float heightMalus=-(3*sizeCellHeight/2);
00353     float heightBonus=sizeCellHeight/2;
00354 
00355     pc.printf("\n\r");
00356     for (int y = NB_CELL_HEIGHT -1; y>-1; y--) {
00357         for (int x= 0; x<NB_CELL_WIDTH; x++) {
00358             heightIndiceInOrthonormal=estimated_height_indice_in_orthonormal_y(y);
00359             widthIndiceInOrthonormal=estimated_width_indice_in_orthonormal_x(x);
00360             if(Yrobot >= (heightIndiceInOrthonormal+heightMalus) && Yrobot <= (heightIndiceInOrthonormal+heightBonus) && Xrobot >= (widthIndiceInOrthonormal+widthMalus) && Xrobot <= (widthIndiceInOrthonormal+widthBonus))                    
00361                 pc.printf(" R ");
00362             else{
00363                 currProba=log_to_proba(map[x][y]);
00364                 if ( currProba < 0.5)
00365                     pc.printf("   ");
00366                 else{
00367                     if(currProba==0.5)
00368                         pc.printf(" . ");
00369                     else
00370                         pc.printf(" X ");
00371                 } 
00372             }
00373         }
00374         pc.printf("\n\r");
00375     }
00376 }
00377 
00378 //MATHS heavy functions
00379 /**********************************************************************/
00380 //Distance computation function
00381 float dist(float robot_x, float robot_y, float target_x, float target_y){
00382     return sqrt(pow(target_y-robot_y,2) + pow(target_x-robot_x,2));
00383 }
00384 
00385 //returns the probability [0,1] that the cell is occupied from the log valAue lt
00386 float log_to_proba(float lt){
00387     return 1-1/(1+exp(lt));
00388 }
00389 
00390 //returns the log value that the cell is occupied from the probability value [0,1]
00391 float proba_to_log(float p){
00392     return log(p/(1-p));
00393 }
00394 
00395 //returns the new log value
00396 float compute_log_estimation_lt(float previousLogValue,float currentProbability,float originalLogvalue ){
00397     return previousLogValue+proba_to_log(currentProbability)-originalLogvalue;
00398 }
00399 
00400 //makes the angle inAngle between 0 and 2pi
00401 float rad_angle_check(float inAngle){
00402     //cout<<"before :"<<inAngle;
00403     if(inAngle > 0){
00404         while(inAngle > (2*pi))
00405             inAngle-=2*pi;
00406     }else{
00407         while(inAngle < 0)
00408             inAngle+=2*pi;
00409     }
00410     //cout<<" after :"<<inAngle<<endl;
00411     return inAngle;
00412 }
00413 
00414 //returns the angle between the vectors (x,y) and (xs,ys)
00415 float compute_angle_between_vectors(float x, float y,float xs,float ys){
00416     //alpha angle between ->x and ->SA
00417     //vector S to A ->SA
00418     float vSAx=x-xs;
00419     float vSAy=y-ys;
00420     //norme SA
00421     float normeSA=sqrt(pow(vSAx,2)+pow(vSAy,2));
00422     //vector ->x (1,0)
00423     float cosAlpha=1*vSAy/*+0*vSAx*//normeSA;;
00424     //vector ->y (0,1)
00425     float sinAlpha=/*0*vSAy+*/1*vSAx/normeSA;//+0*vSAx;
00426     if (sinAlpha < 0)
00427         return -acos(cosAlpha);
00428     else
00429         return acos(cosAlpha);
00430 }
00431 
00432 float robot_center_x_in_orthonormal_x(){
00433     return NB_CELL_WIDTH*sizeCellWidth-Y;
00434 }
00435 
00436 float robot_center_y_in_orthonormal_y(){
00437     return X;
00438 }
00439 
00440 float robot_sonar_front_x_in_orthonormal_x(){
00441     return robot_center_x_in_orthonormal_x()+DISTANCE_SONAR_FRONT_X;
00442 }
00443 float robot_sonar_front_y_in_orthonormal_y(){
00444     return robot_center_y_in_orthonormal_y()+DISTANCE_SONAR_FRONT_Y;
00445 }
00446 
00447 float robot_sonar_right_x_in_orthonormal_x(){
00448     return robot_center_x_in_orthonormal_x()+DISTANCE_SONAR_RIGHT_X;
00449 }
00450 float robot_sonar_right_y_in_orthonormal_y(){
00451     return robot_center_y_in_orthonormal_y()+DISTANCE_SONAR_RIGHT_Y;
00452 }
00453 
00454 float robot_sonar_left_x_in_orthonormal_x(){
00455     return robot_center_x_in_orthonormal_x()+DISTANCE_SONAR_LEFT_X;
00456 }
00457 float robot_sonar_left_y_in_orthonormal_y(){
00458     return robot_center_y_in_orthonormal_y()+DISTANCE_SONAR_LEFT_Y;
00459 }
00460 
00461 float estimated_width_indice_in_orthonormal_x(int i){
00462     return sizeCellWidth/2+i*sizeCellWidth;
00463 }
00464 float estimated_height_indice_in_orthonormal_y(int j){
00465     return sizeCellHeight/2+j*sizeCellHeight;
00466 }
00467 
00468 void compute_angles_and_distance(float target_x, float target_y, float target_angle){
00469     alpha = atan2((target_y-Y),(target_x-X))-theta;
00470     alpha = atan(sin(alpha)/cos(alpha));
00471     rho = dist(X, Y, target_x, target_y);
00472     d2 = rho;
00473     beta = -alpha-theta+target_angle;        
00474     
00475     //Computing angle error and distance towards the target value
00476     rho += dt*(-kRho*cos(alpha)*rho);
00477     temp = alpha;
00478     alpha += dt*(kRho*sin(alpha)-ka*alpha-kb*beta);
00479     beta += dt*(-kRho*sin(temp));
00480    //pc.printf("\n\r d2=%f", d2);
00481     //pc.printf("\n\r dt=%f", dt);
00482 }
00483 
00484 void compute_linear_angular_velocities(){
00485     //Computing linear and angular velocities
00486     if(alpha>=-1.5708 && alpha<=1.5708){
00487         linear=kRho*rho;
00488         angular=ka*alpha+kb*beta;
00489     }
00490     else{
00491         linear=-kRho*rho;
00492         angular=-ka*alpha-kb*beta;
00493     }
00494     angular_left=(linear-0.5*DISTANCE_WHEELS*angular)/RADIUS_WHEELS;
00495     angular_right=(linear+0.5*DISTANCE_WHEELS*angular)/RADIUS_WHEELS;
00496     
00497     //Slowing down at the end for more precision
00498     // if (d2<25) {
00499     //     speed = d2*30;
00500     // }
00501     
00502     //Normalize speed for motors
00503     if(angular_left>angular_right) {
00504         angular_right=speed*angular_right/angular_left;
00505         angular_left=speed;
00506     } else {
00507         angular_left=speed*angular_left/angular_right;
00508         angular_right=speed;
00509     }
00510 }