All the lab works are here!
Dependencies: ISR_Mini-explorer mbed
Fork of VirtualForces by
main.cpp
- Committer:
- geotsam
- Date:
- 2017-06-08
- Revision:
- 52:54b3fe68a4f2
- Parent:
- 51:b863fad80574
File content as of revision 52:54b3fe68a4f2:
#include "mbed.h" #include "robot.h" // Initializes the robot. This include should be used in all main.cpp! #include "math.h" using namespace std; //update angularLeft and angularRight float get_error_angles(float x, float y,float theta); void test_procedure_Lab_2(); void procedure_Lab_3(); void procedure_Lab_2(); void turn_to_target(float angleToTarget); void initialise_parameters(); //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(); //make the robot do a pi/2 flip void do_half_flip(); //go the the given position while updating the map void go_to_point_with_angle(float target_x, float target_y, float target_angle); void compute_angles_and_distance(float target_x, float target_y, float target_angle); void compute_linear_angular_velocities(); //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(); //print the map with the robot and the target marked on it void print_final_map_with_robot_position_and_target(); //go to a given line by updating angularLeft and angularRight void go_to_line(float* angularLeft,float* angularRight,float line_a, float line_b, float line_c); //calculate virtual force field and move void vff(bool* reached); void test_got_to_line(bool* reached); //Go to a given X,Y position, regardless of the final angle void go_to_point(float target_x, float target_y); //go to line and follow it, from lab 1 void go_to_line_lab_1(float line_a, float line_b, float line_c); //MATHS heavy functions float dist(float robot_x, float robot_y, float target_x, float target_y); float distFromLine(float robot_x, float robot_y, float line_a, float line_b, float line_c); //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 x_robot_in_orthonormal_x(float x, float y); float y_robot_in_orthonormal_y(float x, float y); 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); //update foceX and forceY if necessary void update_force(int widthIndice, int heightIndice, float range, float* forceX, float* forceY, float xRobotOrtho, float yRobotOrtho ); //compute the X and Y force void compute_forceX_and_forceY(float* forceX, float* forceY); //robotX and robotY are from Odometria, calculate line_a, line_b and line_c void calculate_line(float forceX, float forceY, float robotX, float robotY,float *line_a, float *line_b, float *line_c); //return 1 if positiv, -1 if negativ float sign1(float value); //return 1 if positiv, 0 if negativ int sign2(float value); //set target in ortho space, in reference to the robot (so if the robot is in the middle, you want to him to go 10cm down and 15 right, set_target_to(15,-10) void set_target_to(float x, float y); void try_to_reach_target(); void test_map_sonar(); //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 //TODO MEASURE THE DISTANCE on X and Y of the robot space, 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 //those distance and angle are approximation in need of measurements, in the orthonormal space 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=200;//cm const float HEIGHT_ARENA=200;//cm //const int SIZE_MAP=25; 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; float sizeCellWidth=WIDTH_ARENA/(float)NB_CELL_WIDTH; float sizeCellHeight=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; //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=20; 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=50;//TODO TWEAK THE SPEED SO IT DOES NOT FUCK UP const float KRHO=12, KA=30, KB=-13, KV=200, KH=200; //Kappa values //CONSTANT FORCE FIELD const float FORCE_CONSTANT_REPULSION=10000;//TODO tweak it const float FORCE_CONSTANT_ATTRACTION=1;//TODO tweak it const float RANGE_FORCE=30;//TODO tweak it float alpha; //angle error float rho; //distance from target float beta; float linear, angular, angular_left, angular_right; float dt=0.5; float temp; float d2; Timer t; int main(){ initialise_parameters(); procedure_Lab_3(); //uses force fields to reach target with set_terget //procedure_Lab_2(); //picks random targets and makes a map (SUCCESS - builds a more or less accurate map without colliding with obstacles) //theta=0; //X=0; //Y=0; //go_to_point(16.8, 78.6); //(x,y) in the global coordinates x cm on the x direction, y cm in the y direction form the 0,0 of the table //(SUCCESS - goes to the specified point) //go_to_line_lab_1(10, -10, 20); //a,b,c of a line: ax+by+c=0, again on the global coordinates of the table (SUCCESS - goes along the line) //go_to_point_with_angle(46.8, 78.6, 0);//(x,y,theta) in the global coordinates (SUCCESS - goes to the point, the angle is almost right as well) //test_procedure_Lab_2(); //starts from the middle of the arena, goes to a point with set_terget } void procedure_Lab_3(){ //try to reach the target is ortho space set_target_to(0,80);// 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(); } } void test_map_sonar(){ float leftMm; float frontMm; float rightMm; X=20; Y=WIDTH_ARENA/2; theta=0; while(1){ leftMm = get_distance_left_sensor(); frontMm = get_distance_front_sensor(); rightMm = get_distance_right_sensor(); pc.printf("%f, %f, %f",leftMm,frontMm,rightMm); //update the probabilities values update_sonar_values(leftMm, frontMm, rightMm); print_final_map(); } } void test_procedure_Lab_2(){ X=HEIGHT_ARENA/2; Y=WIDTH_ARENA/2; set_target_to(-100,50); print_final_map_with_robot_position_and_target(); go_to_point_with_angle(targetX, targetY, pi/2); print_final_map_with_robot_position_and_target(); } void procedure_Lab_2(){ for(int i=0;i<15;i++){ randomize_and_map(); print_final_map_with_robot_position(); wait(2); } while(1){ print_final_map_with_robot_position(); wait(10); } } void try_to_reach_target(){ //atan2 gives the angle beetween pi and -pi float angleToTarget=atan2(targetYOrhtoNotMap,targetXOrhtoNotMap); turn_to_target(angleToTarget); bool reached=false; int print=0; while (!reached) { vff(&reached); //test_got_to_line(&reached); if(print==10){ leftMotor(1,0); rightMotor(1,0); /* pc.printf("\r\n theta=%f", theta); 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); */ print_final_map_with_robot_position_and_target(); print=0; }else print+=1; } //Stop at the end leftMotor(1,0); rightMotor(1,0); pc.printf("\r\n target reached"); wait(3);// } //target in ortho space 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); targetXOrhtoNotMap=x; targetYOrhtoNotMap=y; } void initialise_parameters(){ 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 the map with the initial log values fill_initial_log_values(); theta=DEFAULT_THETA; X=DEFAULT_X; Y=DEFAULT_Y; } //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 movementOnX=rand()%(int)(WIDTH_ARENA/2); float movementOnY=rand()%(int)(HEIGHT_ARENA/2); float signOfX=rand()%2; if(signOfX < 1) signOfX=-1; float signOfY=rand()%2; if(signOfY < 1) signOfY=-1; float x = movementOnX*signOfX; float y = movementOnY*signOfY; float target_angle = 2*((float)(rand()%31416)-15708)/10000.0; set_target_to(x,y); go_to_point_with_angle(targetX, targetY, target_angle); } void do_half_flip(){ Odometria(); float theta_plus_h_pi=theta+pi/2;//theta is between -pi and pi if(theta_plus_h_pi > pi) theta_plus_h_pi=-(2*pi-theta_plus_h_pi); leftMotor(0,100); rightMotor(1,100); while(abs(theta_plus_h_pi-theta)>0.05){ Odometria(); // pc.printf("\n\r diff=%f", abs(theta_plus_pi-theta)); } leftMotor(1,0); rightMotor(1,0); } //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 distancePointToSonar=sqrt(pow(x-xs,2)+pow(y-ys,2)); 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; //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{ //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; } } } //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(" "); } else { if(currProba==0.5) pc.printf(" . "); else pc.printf(" X "); } } 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 widthMalus=-(3*sizeCellWidth/2); float widthBonus=sizeCellWidth/2; float heightMalus=-(3*sizeCellHeight/2); float heightBonus=sizeCellHeight/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(Yrobot >= (heightIndiceInOrthonormal+heightMalus) && Yrobot <= (heightIndiceInOrthonormal+heightBonus) && Xrobot >= (widthIndiceInOrthonormal+widthMalus) && Xrobot <= (widthIndiceInOrthonormal+widthBonus)) pc.printf(" R "); else{ currProba=log_to_proba(map[x][y]); if ( currProba < 0.5) pc.printf(" "); else{ if(currProba==0.5) pc.printf(" . "); else pc.printf(" X "); } } } pc.printf("\n\r"); } } void print_final_map_with_robot_position_and_target() { float currProba; Odometria(); float Xrobot=robot_center_x_in_orthonormal_x(); float Yrobot=robot_center_y_in_orthonormal_y(); float heightIndiceInOrthonormal; float widthIndiceInOrthonormal; float widthMalus=-(3*sizeCellWidth/2); float widthBonus=sizeCellWidth/2; float heightMalus=-(3*sizeCellHeight/2); float heightBonus=sizeCellHeight/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(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)) pc.printf(" T "); else{ currProba=log_to_proba(map[x][y]); if ( currProba < 0.5) pc.printf(" "); else{ if(currProba==0.5) pc.printf(" . "); else pc.printf(" X "); } } } } pc.printf("\n\r"); } } //MATHS heavy functions /**********************************************************************/ //Distance computation function float dist(float robot_x, float robot_y, float target_x, float target_y){ //pc.printf("%f, %f, %f, %f",robot_x,robot_y,target_x,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); } //return 1 if positiv, -1 if negativ float sign1(float value){ if(value>=0) return 1; else return -1; } //return 1 if positiv, 0 if negativ int sign2(float value){ if(value>=0) return 1; else return 0; } /* Robot space: orthonormal space: ^ ^ |x |y <- R O -> y x */ //Odometria must bu up to date float x_robot_in_orthonormal_x(float x, float y){ return robot_center_x_in_orthonormal_x()-y; } //Odometria must bu up to date float y_robot_in_orthonormal_y(float x, float y){ return robot_center_y_in_orthonormal_y()+x; } float robot_center_x_in_orthonormal_x(){ return NB_CELL_WIDTH*sizeCellWidth-Y; } float robot_center_y_in_orthonormal_y(){ return X; } float robot_sonar_front_x_in_orthonormal_x(){ return robot_center_x_in_orthonormal_x()+DISTANCE_SONAR_FRONT_X; } float robot_sonar_front_y_in_orthonormal_y(){ return robot_center_y_in_orthonormal_y()+DISTANCE_SONAR_FRONT_Y; } float robot_sonar_right_x_in_orthonormal_x(){ return robot_center_x_in_orthonormal_x()+DISTANCE_SONAR_RIGHT_X; } float robot_sonar_right_y_in_orthonormal_y(){ return robot_center_y_in_orthonormal_y()+DISTANCE_SONAR_RIGHT_Y; } float robot_sonar_left_x_in_orthonormal_x(){ return robot_center_x_in_orthonormal_x()+DISTANCE_SONAR_LEFT_X; } float robot_sonar_left_y_in_orthonormal_y(){ return robot_center_y_in_orthonormal_y()+DISTANCE_SONAR_LEFT_Y; } float estimated_width_indice_in_orthonormal_x(int i){ return sizeCellWidth/2+i*sizeCellWidth; } float estimated_height_indice_in_orthonormal_y(int j){ return sizeCellHeight/2+j*sizeCellHeight; } void update_force(int widthIndice, int heightIndice, float range, float* forceX, float* forceY, float xRobotOrtho, float yRobotOrtho ){ //get the coordonate of the map and the robot in the ortonormal space float xCenterCell=estimated_width_indice_in_orthonormal_x(widthIndice); float yCenterCell=estimated_height_indice_in_orthonormal_y(heightIndice); //compute the distance beetween the cell and the robot float distanceCellToRobot=sqrt(pow(xCenterCell-xRobotOrtho,2)+pow(yCenterCell-yRobotOrtho,2)); //check if the cell is in range if(distanceCellToRobot <= range) { /*float anglePointToRobot=compute_angle_between_vectors(xCenterCell,yCenterCell,xRobotOrtho,yRobotOrtho);//angle beetween the point and the sonar float alphaBeforeAdjustment=anglePointToRobot-theta; anglePointToRobot=rad_angle_check(alphaBeforeAdjustment); if(anglePointToRobot <= pi/2 || anglePointToRobot >= rad_angle_check(-pi/2)){ */ float probaCell=log_to_proba(map[widthIndice][heightIndice]); float xForceComputed=FORCE_CONSTANT_REPULSION*probaCell*(xCenterCell-xRobotOrtho)/pow(distanceCellToRobot,3); float yForceComputed=FORCE_CONSTANT_REPULSION*probaCell*(yCenterCell-yRobotOrtho)/pow(distanceCellToRobot,3); *forceX+=xForceComputed; *forceY+=yForceComputed; //} } } void test_got_to_line(bool* reached){ float line_a=1; float line_b=2; float line_c=-140; //we update the odometrie Odometria(); float angularRight=0; float angularLeft=0; go_to_line(&angularLeft,&angularRight,line_a,line_b,line_c); //pc.printf("\r\n line: %f x + %f y + %f =0", line_a, line_b, line_c); 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)); //wait(0.1); Odometria(); if(dist(robot_center_x_in_orthonormal_x(),robot_center_y_in_orthonormal_y(),targetXOrhto,targetYOrhto)<10) *reached=true; } void vff(bool* reached){ float line_a; float line_b; float line_c; //Updating X,Y and theta with the odometry values float forceX=0; float forceY=0; //we update the odometrie Odometria(); //we check the sensors float leftMm = get_distance_left_sensor(); float frontMm = get_distance_front_sensor(); float rightMm = get_distance_right_sensor(); float angularRight=0; float angularLeft=0; //update the probabilities values update_sonar_values(leftMm, frontMm, rightMm); //we compute the force on X and Y compute_forceX_and_forceY(&forceX, &forceY); //we compute a new ine calculate_line(forceX, forceY, X, Y,&line_a,&line_b,&line_c); go_to_line(&angularLeft,&angularRight,line_a,line_b,line_c); //Updating motor velocities leftMotor(sign2(angularLeft),abs(angularLeft)); rightMotor(sign2(angularRight),abs(angularRight)); //wait(0.1); Odometria(); if(dist(robot_center_x_in_orthonormal_x(),robot_center_y_in_orthonormal_y(),targetXOrhto,targetYOrhto)<10) *reached=true; } //robotX and robotY are from Odometria, calculate line_a, line_b and line_c void calculate_line(float forceX, float forceY, float robotX, float robotY,float *line_a, float *line_b, float *line_c){ /* in the teachers maths it is *line_a=forceY; *line_b=-forceX; because a*x+b*y+c=0 a impact the vertical and b the horizontal and he has to put them like this because Robot space: orthonormal space: ^ ^ |x |y <- R O -> y x but since our forceX, forceY are already computed in the orthonormal space I m not sure we need to */ *line_a=forceX; *line_b=forceY; //TODO check that //because the line computed always pass by the robot center we dont need lince_c //float xRobotOrtho=robot_center_x_in_orthonormal_x(); //float yRobotOrtho=robot_center_y_in_orthonormal_y(); //*line_c=forceX*yRobotOrtho+forceY*xRobotOrtho; *line_c=0; } /*angleToTarget is obtained through atan2 so it s: < 0 if the angle is bettween pi and 2pi on a trigo circle > 0 if it is between 0 and pi */ void turn_to_target(float angleToTarget){ Odometria(); float theta_plus_h_pi=theta+pi/2;//theta is between -pi and pi if(theta_plus_h_pi > pi) theta_plus_h_pi=-(2*pi-theta_plus_h_pi); if(angleToTarget>0){ leftMotor(0,1); rightMotor(1,1); }else{ leftMotor(1,1); rightMotor(0,1); } while(abs(angleToTarget-theta_plus_h_pi)>0.05){ Odometria(); theta_plus_h_pi=theta+pi/2;//theta is between -pi and pi if(theta_plus_h_pi > pi) theta_plus_h_pi=-(2*pi-theta_plus_h_pi); //pc.printf("\n\r diff=%f", abs(angleToTarget-theta_plus_h_pi)); } leftMotor(1,0); rightMotor(1,0); } //currently line_c is not used void go_to_line(float* angularLeft,float* angularRight,float line_a, float line_b, float line_c){ float lineAngle; float angleError; float linear; float angular; bool direction=true; float angleToTarget=atan2(targetYOrhtoNotMap,targetXOrhtoNotMap); bool inFront=true; if(angleToTarget < 0)//the target is in front inFront=false; 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 } } //pc.printf("\r\n Target is in front"); if(line_b!=0){ if(!direction) lineAngle=atan(-line_a/line_b); else lineAngle=atan(line_a/-line_b); } else{ lineAngle=1.5708; } //Computing angle error angleError = lineAngle-theta; angleError = atan(sin(angleError)/cos(angleError)); //Calculating velocities linear=KV*(3.1416); angular=KH*angleError; *angularLeft=(linear-0.5*DISTANCE_WHEELS*angular)/RADIUS_WHEELS; *angularRight=(linear+0.5*DISTANCE_WHEELS*angular)/RADIUS_WHEELS; float aL=*angularLeft; float aR=*angularRight; //Normalize speed for motors if(abs(*angularLeft)>abs(*angularRight)) { *angularRight=MAX_SPEED*abs(aR/aL)*sign1(aR); *angularLeft=MAX_SPEED*sign1(aL); } else { *angularLeft=MAX_SPEED*abs(aL/aR)*sign1(aL); *angularRight=MAX_SPEED*sign1(aR); } pc.printf("\r\n X=%f; Y=%f", robot_center_x_in_orthonormal_x(),robot_center_y_in_orthonormal_y()); } //compute the force on X and Y void compute_forceX_and_forceY(float* forceX, float* forceY){ //we put the position of the robot in an orthonormal space float xRobotOrtho=robot_center_x_in_orthonormal_x(); float yRobotOrtho=robot_center_y_in_orthonormal_y(); float forceRepulsionComputedX=0; float forceRepulsionComputedY=0; //for each cell of the map we compute a force of repulsion for(int i=0;i<NB_CELL_WIDTH;i++){ for(int j=0;j<NB_CELL_HEIGHT;j++){ update_force(i,j,RANGE_FORCE,&forceRepulsionComputedX,&forceRepulsionComputedY,xRobotOrtho,yRobotOrtho); } } //update with attraction force *forceX=+forceRepulsionComputedX; *forceY=+forceRepulsionComputedY; float distanceTargetRobot=sqrt(pow(targetXOrhto-xRobotOrtho,2)+pow(targetYOrhto-yRobotOrtho,2)); if(distanceTargetRobot != 0){ *forceX-=FORCE_CONSTANT_ATTRACTION*(targetXOrhto-xRobotOrtho)/distanceTargetRobot; *forceY-=FORCE_CONSTANT_ATTRACTION*(targetYOrhto-yRobotOrtho)/distanceTargetRobot; } float amplitude=sqrt(pow(*forceX,2)+pow(*forceY,2)); if(amplitude!=0){//avoid division by 0 if forceX and forceY == 0 *forceX=*forceX/amplitude; *forceY=*forceY/amplitude; } } //x and y passed are TargetNotMap float get_error_angles(float x, float y,float theta){ float angleBeetweenRobotAndTarget=atan2(y,x); if(y > 0){ if(angleBeetweenRobotAndTarget < pi/2)//up right angleBeetweenRobotAndTarget=-pi/2+angleBeetweenRobotAndTarget; else//up right angleBeetweenRobotAndTarget=angleBeetweenRobotAndTarget-pi/2; }else{ if(angleBeetweenRobotAndTarget > -pi/2)//lower right angleBeetweenRobotAndTarget=angleBeetweenRobotAndTarget-pi/2; else//lower left angleBeetweenRobotAndTarget=2*pi+angleBeetweenRobotAndTarget-pi/2; } return angleBeetweenRobotAndTarget-theta; } 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; //Normalize speed for motors if(angular_left>angular_right) { angular_right=MAX_SPEED*angular_right/angular_left; angular_left=MAX_SPEED; } else { angular_left=MAX_SPEED*angular_left/angular_right; angular_right=MAX_SPEED; } } //go the the given position while updating the map void go_to_point_with_angle(float target_x, float target_y, float target_angle) { Odometria(); 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)); bool keep_going=true; float leftMm; float frontMm; float rightMm; do { //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); //if in dangerzone if((frontMm < 150 && frontMm > 0)|| (leftMm <150 && leftMm > 0) || (rightMm <150 && rightMm > 0) ){ leftMotor(1,0); rightMotor(1,0); update_sonar_values(leftMm, frontMm, rightMm); //TODO Giorgos maybe you can also test the do_half_flip() function Odometria(); //do a flip TODO keep_going=false; do_half_flip(); }else{ //if not in danger zone continue as usual update_sonar_values(leftMm, frontMm, rightMm); 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(); pc.printf("\n\r dist to target= %f",d2); } } while(d2>1 && (abs(target_angle-theta)>0.01) && keep_going); //Stop at the end leftMotor(1,0); rightMotor(1,0); } //Updates sonar values void update_sonar_values(float leftMm, float frontMm, float rightMm){ if(leftMm==0) pc.printf("\n\r leftMm==0"); if(frontMm==0) pc.printf("\n\r frontMm==0"); if(rightMm==0) pc.printf("\n\r rightMm==0"); 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 space is within the range of the sonar (which has the coordinates xs, ys in the world space) //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]; } } } //Go to a given X,Y position, regardless of the final angle void go_to_point(float target_x, float target_y){ float angle_error; //angle error float d; //distance from target float k_linear=10, k_angular=200; do { //Updating X,Y and theta with the odometry values Odometria(); //Computing angle error and distance towards the target value angle_error = atan2((target_y-Y),(target_x-X))-theta; angle_error = atan(sin(angle_error)/cos(angle_error)); d=dist(X, Y, target_x, target_y); //Computing linear and angular velocities linear=k_linear*d; angular=k_angular*angle_error; angular_left=(linear-0.5*DISTANCE_WHEELS*angular)/RADIUS_WHEELS; angular_right=(linear+0.5*DISTANCE_WHEELS*angular)/RADIUS_WHEELS; //Normalize speed for motors if(angular_left>angular_right) { angular_right=MAX_SPEED*angular_right/angular_left; angular_left=MAX_SPEED; } else { angular_left=MAX_SPEED*angular_left/angular_right; angular_right=MAX_SPEED; } pc.printf("\n\r X=%f", X); pc.printf("\n\r Y=%f", Y); //Updating motor velocities leftMotor(1,angular_left); rightMotor(1,angular_right); wait(0.5); } while(d>1); //Stop at the end leftMotor(1,0); rightMotor(1,0); } //go to line and follow it, from lab 1 void go_to_line_lab_1(float line_a, float line_b, float line_c){ float angle_error; //angle error float d; //distance from line float kd=5, kh=200, kv=200; float linear, angular, angular_left, angular_right; float line_angle; //Check if b=0, if yes line_angle=90 if(line_b!=0){ line_angle=atan(-line_a/line_b); } else{ line_angle=1.5708; } do { pc.printf("\n\n\r entered while"); //Updating X,Y and theta with the odometry values Odometria(); //Computing angle error and distance from the target line angle_error = line_angle-theta; angle_error = atan(sin(angle_error)/cos(angle_error)); d=distFromLine(X, Y, line_a, line_b, line_c); pc.printf("\n\r d=%f", d); //Calculating velocities linear=kv*(3.1416-angle_error); angular=-kd*d+kh*angle_error; angular_left=(linear-0.5*DISTANCE_WHEELS*angular)/RADIUS_WHEELS; angular_right=(linear+0.5*DISTANCE_WHEELS*angular)/RADIUS_WHEELS; //Normalize MAX_SPEED for motors if(angular_left>angular_right) { angular_right=MAX_SPEED*angular_right/angular_left; angular_left=MAX_SPEED; } else { angular_left=MAX_SPEED*angular_left/angular_right; angular_right=MAX_SPEED; } //Updating motor velocities if(angular_left>0){ leftMotor(1,angular_left); } else{ leftMotor(0,-angular_left); } if(angular_right>0){ rightMotor(1,angular_right); } else{ rightMotor(0,-angular_right); } wait(0.5); } while(1); } float distFromLine(float robot_x, float robot_y, float line_a, float line_b, float line_c){ return abs((line_a*robot_x+line_b*robot_y+line_c)/sqrt(line_a*line_a+line_b*line_b)); }