All the lab works are here!
Dependencies: ISR_Mini-explorer mbed
Fork of VirtualForces by
Diff: main.cpp
- Revision:
- 47:f0fe58571e4a
- Parent:
- 45:fb07065a64a9
- Child:
- 48:cd3463dcca04
diff -r fb07065a64a9 -r f0fe58571e4a main.cpp --- a/main.cpp Mon May 29 12:59:51 2017 +0000 +++ b/main.cpp Tue May 30 15:17:19 2017 +0000 @@ -94,20 +94,12 @@ 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 float WIDTH_ARENA=200;//cm +const float HEIGHT_ARENA=200;//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 space -//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_X=20;//lower right -const float DEFAULT_Y=20;//lower right -const float DEFAULT_THETA=0; +const int NB_CELL_WIDTH=20; +const int NB_CELL_HEIGHT=20; //used to create the map 250 represent the 250cm of the square where the robot is tested //float sizeCell=250/(float)SIZE_MAP; @@ -121,10 +113,18 @@ const float RADIUS_WHEELS=3.25; const float DISTANCE_WHEELS=7.2; +//position and orientation of the robot when put on the map (ODOMETRY doesn't know those) it's in the robot space +//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_X=20;//lower right +//const float DEFAULT_Y=20;//lower right +const float DEFAULT_THETA=0; + 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=200, KH=200; //Kappa values //CONSTANT FORCE FIELD const float FORCE_CONSTANT_REPULSION=50;//TODO tweak it @@ -134,21 +134,24 @@ //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 - set_target_to(0,50);//up right + set_target_to(-50,-50);// print_final_map_with_robot_position_and_target(); try_to_reach_target(); - set_target_to(0,-50);//lower right - print_final_map_with_robot_position_and_target(); - try_to_reach_target(); - set_target_to(-50,50);//up left - print_final_map_with_robot_position_and_target(); - try_to_reach_target(); + //set_target_to(0,20);//lower right + //print_final_map_with_robot_position_and_target(); + //try_to_reach_target(); + //set_target_to(-20,-20);//up left + //print_final_map_with_robot_position_and_target(); + //try_to_reach_target(); //print the map forever while(1){ print_final_map_with_robot_position_and_target(); @@ -179,6 +182,7 @@ leftMotor(1,0); rightMotor(1,0); pc.printf("\r\n target reached"); + wait(3);// } //target in ortho space @@ -187,6 +191,8 @@ targetY=-x; targetXOrhto=x_robot_in_orthonormal_x(targetX,targetY); targetYOrhto=y_robot_in_orthonormal_y(targetX,targetY); + targetXOrhtoNotMap=x; + targetYOrhtoNotMap=y; } void initialise_parameters(){ @@ -365,6 +371,8 @@ 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{ //probably occupied @@ -379,6 +387,8 @@ 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{ @@ -522,6 +532,7 @@ 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 @@ -764,13 +775,58 @@ float angleError; float linear; float angular; + + bool inFront=true; + //atan2 gives the angle beetween pi and -pi + float angleToTarget=atan2(targetYOrhtoNotMap,targetXOrhtoNotMap); + pc.printf("angleToTarget=%f",angleToTarget); + if(angleToTarget < 0)//the target is in front + inFront=false; + + bool direction=true; + + 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 + } + } + + if(line_b!=0){ + if(!direction) + lineAngle=atan(-line_a/line_b); + else + lineAngle=atan(line_a/-line_b); + } + else{ + lineAngle=1.5708; + } + + /* if(line_b!=0){ lineAngle=atan(-line_a/line_b); } else{ lineAngle=1.5708; } - + */ //Computing angle error angleError = lineAngle-theta; angleError = atan(sin(angleError)/cos(angleError));