All the lab works are here!
Dependencies: ISR_Mini-explorer mbed
Fork of VirtualForces by
Diff: main.cpp
- Revision:
- 50:a970cf7889d3
- Parent:
- 49:d61da2bc8882
- Child:
- 51:b863fad80574
diff -r d61da2bc8882 -r a970cf7889d3 main.cpp --- a/main.cpp Tue May 30 16:09:25 2017 +0000 +++ b/main.cpp Tue May 30 17:18:55 2017 +0000 @@ -71,6 +71,15 @@ void set_target_to(float x, float y); void try_to_reach_target(); +//those target are in comparaison to the robot (for exemple a robot in 50,50 with a taget of 0,0 would not need to move) +float targetX;//this is in the robot space top left +float targetY;//this is in the robot space top left +//Ortho but for the map (i.e x and Y are > 0) +float targetXOrhto; +float targetYOrhto; +float targetXOrhtoNotMap; +float targetYOrhtoNotMap; + const float pi=3.14159; //spec of the sonar @@ -94,12 +103,12 @@ const float DISTANCE_SONAR_FRONT_Y=5; //TODO adjust the size of the map for computation time (25*25?) -const float WIDTH_ARENA=200;//cm -const float HEIGHT_ARENA=200;//cm +const float WIDTH_ARENA=120;//cm +const float HEIGHT_ARENA=90;//cm //const int SIZE_MAP=25; -const int NB_CELL_WIDTH=20; -const int NB_CELL_HEIGHT=20; +const int NB_CELL_WIDTH=24; +const int NB_CELL_HEIGHT=18; //used to create the map 250 represent the 250cm of the square where the robot is tested //float sizeCell=250/(float)SIZE_MAP; @@ -123,7 +132,6 @@ const int MAX_SPEED=200;//TODO TWEAK THE SPEED SO IT DOES NOT FUCK UP -//TODO all those global variables are making me sad const float KRHO=12, KA=30, KB=-13, KV=200, KH=200; //Kappa values //CONSTANT FORCE FIELD @@ -131,15 +139,6 @@ const float FORCE_CONSTANT_ATTRACTION=25;//TODO tweak it const float RANGE_FORCE=50;//TODO tweak it -//those target are in comparaison to the robot (for exemple a robot in 50,50 with a taget of 0,0 would not need to move) -float targetX;//this is in the robot space top left -float targetY;//this is in the robot space top left -//Ortho but for the map (i.e x and Y are > 0) -float targetXOrhto; -float targetYOrhto; -float targetXOrhtoNotMap; -float targetYOrhtoNotMap; - int main(){ initialise_parameters(); //try to reach the target @@ -164,7 +163,7 @@ while (!reached) { vff(&reached); //test_got_to_line(&reached); - if(print==10){ + if(print==30){ leftMotor(1,0); rightMotor(1,0); pc.printf("\r\n theta=%f", theta); @@ -335,13 +334,16 @@ 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 + if(currProba!=0.5) + 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]; + if(currProba!=0.5) + 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]; + if(currProba!=0.5) + map[i][j]=map[i][j]+proba_to_log(currProba)+initialLogValues[i][j]; } } } @@ -350,58 +352,59 @@ //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 anglePointToSonar=compute_angle_between_vectors(x,y,xs,ys);//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 float distancePointToSonar=sqrt(pow(x-xs,2)+pow(y-ys,2)); - - if(alphaBeforeAdjustment>pi) - alphaBeforeAdjustment=alphaBeforeAdjustment-2*pi; - if(alphaBeforeAdjustment<-pi) - alphaBeforeAdjustment=alphaBeforeAdjustment+2*pi; + if( distancePointToSonar < RANGE_SONAR){ + float anglePointToSonar=compute_angle_between_vectors(x,y,xs,ys);//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; - //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( distancePointToSonar < (RANGE_SONAR)&& (anglePointToSonar <= ANGLE_SONAR/2 || anglePointToSonar >= 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; + //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 <= ANGLE_SONAR/2 || anglePointToSonar >= 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); + } + /*****************************************************************************/ + //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{ - //point after minimum sonar range - Er=1.f-pow((distancePointToSonar-RANGE_SONAR_MIN)/(distanceObstacleDetected-INCERTITUDE_SONAR-RANGE_SONAR_MIN),2); + //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; + } + /*****************************************************************************/ + //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; } - /*****************************************************************************/ - 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 + 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; - } - /*****************************************************************************/ - 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; - } - }else{ - //not checked by the sonar - return 0.5; + } } + //not checked by the sonar + return 0.5; } void print_final_map() {