![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Navigate to a given point using the OGM and virtual forces
Dependencies: ISR_Mini-explorer mbed
Fork of VirtualForces by
main.cpp
- Committer:
- geotsam
- Date:
- 2017-04-20
- Revision:
- 26:b020cf253059
- Parent:
- 25:572c9e9a8809
- Child:
- 27:07bde633af72
File content as of revision 26:b020cf253059:
#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(); //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); const float pi=3.14159; //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 const float SECURITY_DISTANCE=150;//mm //those distance and angle are approximation in need of measurements 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) const float DEFAULT_X=WIDTH_ARENA/2; const float DEFAULT_Y=HEIGHT_ARENA/2; const float DEFAULT_THETA=pi/2; //used to create the map 250 represent the 250cm of the square where the robot is tested //float sizeCell=250/(float)SIZE_MAP; float sizeCellX=WIDTH_ARENA/(float)NB_CELL_WIDTH; float sizeCellY=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=500;//TODO TWEAK THE SPEED SO IT DOES NOT FUCK UP bool tooClose=false; bool turnFromObstacle=false; 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; 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<20; i++) { randomize_and_map(); wait(2); print_final_map(); } } //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)(WIDTH_ARENA*10))/10;//for decimal precision float target_y = (rand()%(int)(HEIGHT_ARENA*10))/10; float target_angle = ((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); } //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) { if(tooClose){ target_x=X; target_y=Y; target_angle=theta+pi/2; target_angle = atan(sin(target_angle)/cos(target_angle)); pc.printf("\n\rShould just turn now, new target_angle=%f\n\r", 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); beta = -alpha-theta+target_angle; //beta = atan(sin(beta)/cos(beta)); do { //pc.printf("\n\n\r entered while"); //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); update_sonar_values(leftMm, frontMm, rightMm); if ((leftMm < SECURITY_DISTANCE || frontMm < SECURITY_DISTANCE || rightMm < SECURITY_DISTANCE) && turnFromObstacle==false) { tooClose = true; turnFromObstacle = true; pc.printf("\n\r TOO CLOSE \n\r"); leftMotor(1,0); rightMotor(1,0); Odometria(); go_to_point_with_angle(X, Y, rad_angle_check(theta+pi)); break; } 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) && tooClose==false); //Stop at the end leftMotor(1,0); rightMotor(1,0); if(turnFromObstacle){ turnFromObstacle=false; tooClose=false; } } //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; } } 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(" 0 "); } else { if(currProba==0.5) pc.printf(" . "); else pc.printf(" + "); } } 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 heightMalus=-(3*sizeCellX/2); float heightBonus=sizeCellX/2; float widthMalus=-(3*sizeCellY/2); float widthBonus=sizeCellY/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(Xrobot >= (heightIndiceInOrthonormal+heightMalus) && Xrobot <= (heightIndiceInOrthonormal+heightBonus) && Yrobot >= (widthIndiceInOrthonormal+widthMalus) && Yrobot <= (widthIndiceInOrthonormal+widthBonus)) pc.printf(" R "); else{ currProba=log_to_proba(map[x][y]); if ( currProba < 0.5) pc.printf(" 0 "); else{ if(currProba==0.5) pc.printf(" . "); else pc.printf(" + "); } } } 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 Y; } float robot_center_y_in_orthonormal_y(){ return NB_CELL_WIDTH*sizeCellX-X; } float robot_sonar_front_x_in_orthonormal_x(){ return Y+DISTANCE_SONAR_FRONT_Y; } float robot_sonar_front_y_in_orthonormal_y(){ return NB_CELL_WIDTH*sizeCellX-X+DISTANCE_SONAR_FRONT_X; } float robot_sonar_right_x_in_orthonormal_x(){ return Y+DISTANCE_SONAR_RIGHT_Y; } float robot_sonar_right_y_in_orthonormal_y(){ return NB_CELL_WIDTH*sizeCellX-X+DISTANCE_SONAR_RIGHT_X; } float robot_sonar_left_x_in_orthonormal_x(){ return Y+DISTANCE_SONAR_LEFT_Y; } float robot_sonar_left_y_in_orthonormal_y(){ return NB_CELL_WIDTH*sizeCellX-X+DISTANCE_SONAR_LEFT_X; } float estimated_width_indice_in_orthonormal_x(int i){ return sizeCellX/2+i*sizeCellX; } float estimated_height_indice_in_orthonormal_y(int j){ return sizeCellY/2+j*sizeCellY; } 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; } }