Navigate to a given point using the OGM and virtual forces
Dependencies: ISR_Mini-explorer mbed
Fork of VirtualForces by
Revision 46:e57ebcf747dc, committed 2017-05-29
- Comitter:
- geotsam
- Date:
- Mon May 29 17:13:42 2017 +0000
- Parent:
- 45:fb07065a64a9
- Commit message:
- full of isnan() and other weird debugging stuff
Changed in this revision
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
diff -r fb07065a64a9 -r e57ebcf747dc main.cpp --- a/main.cpp Mon May 29 12:59:51 2017 +0000 +++ b/main.cpp Mon May 29 17:13:42 2017 +0000 @@ -4,6 +4,8 @@ using namespace std; +//makes the angle inAngle between pi and minus pi +float rad_angle_check_pi_and_minus_pi(float inAngle); void initialise_parameters(); //fill initialLogValues with the values we already know (here the bordurs) void fill_initial_log_values(); @@ -124,18 +126,20 @@ 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=300, KH=150; //Kappa values +const float KRHO=12, KA=30, KB=-13, KV=150, KH=150; //Kappa values //CONSTANT FORCE FIELD -const float FORCE_CONSTANT_REPULSION=50;//TODO tweak it +const float FORCE_CONSTANT_REPULSION=80;//TODO tweak it 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) +//those target are in comparison 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 -float targetXOrhto; -float targetYOrhto; +float targetXOrtho; +float targetYOrtho; + +bool direction; int main(){ initialise_parameters(); @@ -168,8 +172,8 @@ pc.printf("\r\n IN ORTHO:"); pc.printf("\r\n X Robot=%f", robot_center_x_in_orthonormal_x()); pc.printf("\r\n Y Robot=%f", robot_center_y_in_orthonormal_y()); - pc.printf("\r\n X Target=%f", targetXOrhto); - pc.printf("\r\n Y Target=%f", targetYOrhto); + pc.printf("\r\n X Target=%f", targetXOrtho); + pc.printf("\r\n Y Target=%f", targetYOrtho); print_final_map_with_robot_position_and_target(); print=0; }else @@ -185,8 +189,17 @@ void set_target_to(float x, float y){ targetX=y; targetY=-x; - targetXOrhto=x_robot_in_orthonormal_x(targetX,targetY); - targetYOrhto=y_robot_in_orthonormal_y(targetX,targetY); + targetXOrtho=x_robot_in_orthonormal_x(targetX,targetY); + targetYOrtho=y_robot_in_orthonormal_y(targetX,targetY); + + pc.printf("\r\nangletarget= %f", atan2(x,y)); + float angleError = atan2(x,y)-theta; + if(angleError>pi) angleError-=2*pi; + if(angleError<-pi) angleError+=2*pi; + + if(angleError<(pi/2) && angleError>(-pi/2)) direction=true; + else direction=false; + pc.printf("\r\nangleError= %f", angleError); } void initialise_parameters(){ @@ -249,8 +262,12 @@ set_target_to(target_x,target_y); Odometria(); float angleError = atan2((target_y-Y),(target_x-X))-theta; - angleError = atan(sin(angleError)/cos(angleError)); - float distanceFromTarget = dist(robot_center_x_in_orthonormal_x(),robot_center_y_in_orthonormal_y(),targetXOrhto,targetYOrhto); + if(!cos(angleError)) + angleError = atan(sin(angleError)/cos(angleError)); + else + angleError=pi/2; + if(isnan(angleError)) pc.printf("\r\n nan line 264"); + float distanceFromTarget = dist(robot_center_x_in_orthonormal_x(),robot_center_y_in_orthonormal_y(),targetXOrtho,targetYOrtho); float beta = -angleError-theta+target_angle; //beta = atan(sin(beta)/cos(beta)); bool keep_going=true; @@ -329,17 +346,34 @@ 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); + if(isnan(currProba)) pc.printf("\r\n currProba is nan"); 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); + if(isnan(currProba)) pc.printf("\r\n nan line 354"); map[i][j]=map[i][j]+proba_to_log(currProba)+initialLogValues[i][j]; + if(isnan(proba_to_log(currProba))) pc.printf("\r\nnan in line 355"); } } } +//makes the angle inAngle between pi and minus pi +float rad_angle_check_pi_and_minus_pi(float inAngle){ + //cout<<"before :"<<inAngle; + if(inAngle > 0){ + while(inAngle > (pi)) + inAngle-=pi; + }else{ + while(inAngle < 0) + inAngle+=pi; + } + //cout<<" after :"<<inAngle<<endl; + return inAngle; +} + //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){ @@ -347,6 +381,8 @@ 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 + alphaBeforeAdjustment=rad_angle_check_pi_and_minus_pi(alphaBeforeAdjustment); + //if(abs(alphaBeforeAdjustment)>ANGLE_SONAR/2) pc.printf("\r\n it is!"); 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 @@ -365,6 +401,8 @@ Er=1.f-pow((distancePointToSonar-RANGE_SONAR_MIN)/(distanceObstacleDetected-INCERTITUDE_SONAR-RANGE_SONAR_MIN),2); } /*****************************************************************************/ + if((1.f-Er*Ea)/2.f>1) pc.printf("\r\n E>1"); + if((1.f-Er*Ea)/2.f<0) pc.printf("\r\n E<0"); return (1.f-Er*Ea)/2.f; }else{ //probably occupied @@ -379,12 +417,13 @@ Or=0; } /*****************************************************************************/ + if((1+Or*Oa)/2>1) pc.printf("\r\n O>1"); + if((1+Or*Oa)/2<0) pc.printf("\r\n O<0"); 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() { @@ -467,7 +506,7 @@ if(Yrobot >= (heightIndiceInOrthonormal+heightMalus) && Yrobot <= (heightIndiceInOrthonormal+heightBonus) && Xrobot >= (widthIndiceInOrthonormal+widthMalus) && Xrobot <= (widthIndiceInOrthonormal+widthBonus)) pc.printf(" R "); else{ - if(targetYOrhto >= (heightIndiceInOrthonormal+heightMalus) && targetYOrhto <= (heightIndiceInOrthonormal+heightBonus) && targetXOrhto >= (widthIndiceInOrthonormal+widthMalus) && targetXOrhto <= (widthIndiceInOrthonormal+widthBonus)) + if(targetYOrtho >= (heightIndiceInOrthonormal+heightMalus) && targetYOrtho <= (heightIndiceInOrthonormal+heightBonus) && targetXOrtho >= (widthIndiceInOrthonormal+widthMalus) && targetXOrtho <= (widthIndiceInOrthonormal+widthBonus)) pc.printf(" T "); else{ currProba=log_to_proba(map[x][y]); @@ -493,14 +532,23 @@ 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 +//returns the probability [0,1] that the cell is occupied from the log value lt float log_to_proba(float lt){ - return 1-1/(1+exp(lt)); + float temp=1-1/(1+exp(lt)); + if(isnan(temp)){ + //pc.printf("\r\n nan in line 514"); + //pc.printf("\r\nlt= %f, 1+exp(lt)= %f", lt, 1+exp(lt)); + } + return temp; } //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)); + float temp; + if(p==1) temp=log(0.99/(1-0.99)); + else temp=log(p/(1-p)); + if(isnan(temp)) pc.printf("\r\n temp=%f, p=%f", temp,p); + return temp; } //returns the new log value @@ -597,8 +645,12 @@ //update angleError,distanceFromTarget,d2, beta void compute_angles_and_distance(float target_x, float target_y, float target_angle,float dt,float* angleError,float* distanceFromTarget,float* d2,float* beta){ *angleError = atan2((target_y-Y),(target_x-X))-theta; - *angleError = atan(sin(*angleError)/cos(*angleError)); - *distanceFromTarget = dist(robot_center_x_in_orthonormal_x(),robot_center_y_in_orthonormal_y(),targetXOrhto,targetYOrhto); + if(!cos(*angleError)) + *angleError = atan(sin(*angleError)/cos(*angleError)); + else + *angleError=pi/2; + if(isnan(*angleError)) pc.printf("\r\n nan line 613"); + *distanceFromTarget = dist(robot_center_x_in_orthonormal_x(),robot_center_y_in_orthonormal_y(),targetXOrtho,targetYOrtho); *d2 = *distanceFromTarget; *beta = -*angleError-theta+target_angle; @@ -650,8 +702,11 @@ //check if the cell is in range if(distanceCellToRobot <= range) { float probaCell=log_to_proba(map[widthIndice][heightIndice]); + //if(isnan(probaCell)) pc.printf("\r\nnan in probaCell"); float xForceComputed=FORCE_CONSTANT_REPULSION*probaCell*(xCenterCell-xRobotOrtho)/pow(distanceCellToRobot,3); + //if(isnan(xForceComputed)) pc.printf("\r\nnan in line 673"); float yForceComputed=FORCE_CONSTANT_REPULSION*probaCell*(yCenterCell-yRobotOrtho)/pow(distanceCellToRobot,3); + //if(isnan(yForceComputed)) pc.printf("\r\nnan in line 675"); *forceX+=xForceComputed; *forceY+=yForceComputed; } @@ -674,10 +729,10 @@ //update with attraction force *forceX=-forceRepulsionComputedX; *forceY=-forceRepulsionComputedY; - float distanceTargetRobot=sqrt(pow(targetXOrhto-xRobotOrtho,2)+pow(targetYOrhto-yRobotOrtho,2)); + float distanceTargetRobot=sqrt(pow(targetXOrtho-xRobotOrtho,2)+pow(targetYOrtho-yRobotOrtho,2)); if(distanceTargetRobot != 0){ - *forceX+=FORCE_CONSTANT_ATTRACTION*(targetXOrhto-xRobotOrtho)/distanceTargetRobot; - *forceY+=FORCE_CONSTANT_ATTRACTION*(targetYOrhto-yRobotOrtho)/distanceTargetRobot; + *forceX+=FORCE_CONSTANT_ATTRACTION*(targetXOrtho-xRobotOrtho)/distanceTargetRobot; + *forceY+=FORCE_CONSTANT_ATTRACTION*(targetYOrtho-yRobotOrtho)/distanceTargetRobot; } float amplitude=sqrt(pow(*forceX,2)+pow(*forceY,2)); if(amplitude!=0){//avoid division by 0 if forceX and forceY == 0 @@ -701,17 +756,17 @@ leftMotor(sign2(angularLeft),abs(angularLeft)); rightMotor(sign2(angularRight),abs(angularRight)); - pc.printf("\r\n dist=%f", dist(robot_center_x_in_orthonormal_x(),robot_center_y_in_orthonormal_y(),targetXOrhto,targetYOrhto)); + pc.printf("\r\n dist=%f", dist(robot_center_x_in_orthonormal_x(),robot_center_y_in_orthonormal_y(),targetXOrtho,targetYOrtho)); //wait(0.1); Odometria(); - if(dist(robot_center_x_in_orthonormal_x(),robot_center_y_in_orthonormal_y(),targetXOrhto,targetYOrhto)<10) + if(dist(robot_center_x_in_orthonormal_x(),robot_center_y_in_orthonormal_y(),targetXOrtho,targetYOrtho)<10) *reached=true; } void vff(bool* reached){ - float line_a; - float line_b; - float line_c; + float line_a=0; + float line_b=0; + float line_c=0; //Updating X,Y and theta with the odometry values float forceX=0; float forceY=0; @@ -738,7 +793,7 @@ //wait(0.1); Odometria(); - if(dist(robot_center_x_in_orthonormal_x(),robot_center_y_in_orthonormal_y(),targetXOrhto,targetYOrhto)<10) + if(dist(robot_center_x_in_orthonormal_x(),robot_center_y_in_orthonormal_y(),targetXOrtho,targetYOrtho)<10) *reached=true; } @@ -764,8 +819,12 @@ float angleError; float linear; float angular; + if(line_b!=0){ - lineAngle=atan(-line_a/line_b); + if(direction) + lineAngle=atan(-line_a/line_b); + else + lineAngle=atan(line_a/-line_b); } else{ lineAngle=1.5708; @@ -773,7 +832,11 @@ //Computing angle error angleError = lineAngle-theta; - angleError = atan(sin(angleError)/cos(angleError)); + if(!cos(angleError)) + angleError = atan(sin(angleError)/cos(angleError)); + else + angleError=pi/2; + if(isnan(angleError)) pc.printf("\r\n nan line 794"); //Calculating velocities linear=KV*(3.1416);