All the lab works are here!
Dependencies: ISR_Mini-explorer mbed
Fork of VirtualForces by
Diff: main.cpp
- Revision:
- 43:ffd5a6d4dd48
- Parent:
- 42:ab25bffdc32b
- Child:
- 44:e2bd06f94dc0
diff -r ab25bffdc32b -r ffd5a6d4dd48 main.cpp --- a/main.cpp Mon May 22 13:25:20 2017 +0000 +++ b/main.cpp Mon May 22 18:20:08 2017 +0000 @@ -13,7 +13,7 @@ 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); -//Updates sonar values +//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); @@ -27,6 +27,7 @@ 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); //MATHS heavy functions float dist(float robot_x, float robot_y, float target_x, float target_y); @@ -57,19 +58,19 @@ //update angularLeft and angularRight void compute_linear_angular_velocities(float angleError,float distanceFromTarget,float beta, float* angularLeft, float* angularRight); //update foceX and forceY if necessary -void updateForce(int widthIndice, int heightIndice, float range, float* forceX, float* forceY, float xRobotOrtho, float yRobotOrtho ); +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); +void set_target_to(float x, float y); const float pi=3.14159; -//CONSTANT FORCE FIELD -const float FORCE_CONSTANT_REPULSION=50;//TODO tweak it -const float FORCE_CONSTANT_ATTRACTION=10;//TODO tweak it -const float RANGE_FORCE=50;//TODO tweak it - //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 @@ -121,13 +122,18 @@ 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 +const float KRHO=12, KA=30, KB=-13, KV=300, KH=150; //Kappa values + +//CONSTANT FORCE FIELD +const float FORCE_CONSTANT_REPULSION=50;//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) -const float targetX=HEIGHT_ARENA-X-20;//this is in the robot frame top left -const float targetY=WIDTH_ARENA+Y-20;//this is in the robot frame top left -float targetX_orhto=0; -float targetY_orhto=0; +float targetX;//this is in the robot frame top left +float targetY;//this is in the robot frame top left +float targetXOrhto; +float targetYOrhto; int main(){ initialise_parameters(); @@ -135,7 +141,8 @@ //try to reach the target bool reached=false; while (!reached) { - vff(&reached); + vff(&reached); + //test_got_to_line(&reached); print_final_map_with_robot_position_and_target(); } //Stop at the end @@ -149,6 +156,13 @@ } +void set_target_to(float x, float y){ + targetX=x; + targetY=y; + targetXOrhto=x_robot_in_orthonormal_x(targetX,targetY); + targetYOrhto=y_robot_in_orthonormal_y(targetX,targetY); +} + void initialise_parameters(){ i2c1.frequency(100000); initRobot(); //Initializing the robot @@ -162,8 +176,7 @@ theta=DEFAULT_THETA; X=DEFAULT_X; Y=DEFAULT_Y; - targetX_orhto=x_robot_in_orthonormal_x(targetX,targetY); - targetY_orhto=y_robot_in_orthonormal_y(targetX,targetY); + set_target_to(HEIGHT_ARENA-40,WIDTH_ARENA-40);//50 cm up and 80 on the right } //fill initialLogValues with the values we already know (here the bordurs) @@ -186,12 +199,7 @@ float target_y = (rand()%(int)(WIDTH_ARENA*10))/10; float target_angle = 2*((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_to_point_with_angle(targetX, targetY, target_angle); } @@ -213,10 +221,11 @@ //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) { + 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(X, Y, target_x, target_y); + float distanceFromTarget = dist(robot_center_x_in_orthonormal_x(),robot_center_y_in_orthonormal_y(),targetXOrhto,targetYOrhto); float beta = -angleError-theta+target_angle; //beta = atan(sin(beta)/cos(beta)); bool keep_going=true; @@ -260,14 +269,6 @@ compute_angles_and_distance(target_x, target_y, target_angle,dt,&angleError,&distanceFromTarget,&d2,&beta);//Compute the angles and the distance from target compute_linear_angular_velocities(angleError,distanceFromTarget,beta,&angularLeft,&angularRight); //Using the angles and distance, compute the velocities needed (linear & angular) - //Normalize speed for motors - if(angularLeft>angularRight) { - angularRight=MAX_SPEED*angularRight/angularLeft; - angularLeft=MAX_SPEED; - } else { - angularLeft=MAX_SPEED*angularLeft/angularRight; - angularRight=MAX_SPEED; - } //pc.printf("\n\r X=%f", X); //pc.printf("\n\r Y=%f", Y); @@ -275,8 +276,8 @@ //pc.printf("\n\r a_l=%f", angularLeft); //Updating motor velocities - leftMotor(1,angularLeft); - rightMotor(1,angularRight); + leftMotor(sign2(angularLeft),abs(angularLeft)); + rightMotor(sign2(angularRight),abs(angularRight)); wait(0.2); //Timer stuff @@ -441,7 +442,7 @@ if(Yrobot >= (heightIndiceInOrthonormal+heightMalus) && Yrobot <= (heightIndiceInOrthonormal+heightBonus) && Xrobot >= (widthIndiceInOrthonormal+widthMalus) && Xrobot <= (widthIndiceInOrthonormal+widthBonus)) pc.printf(" R "); else{ - if(targetY_orhto >= (heightIndiceInOrthonormal+heightMalus) && targetY_orhto <= (heightIndiceInOrthonormal+heightBonus) && targetX_orhto >= (widthIndiceInOrthonormal+widthMalus) && targetX_orhto <= (widthIndiceInOrthonormal+widthBonus)) + if(targetYOrhto >= (heightIndiceInOrthonormal+heightMalus) && targetYOrhto <= (heightIndiceInOrthonormal+heightBonus) && targetXOrhto >= (widthIndiceInOrthonormal+widthMalus) && targetXOrhto <= (widthIndiceInOrthonormal+widthBonus)) pc.printf(" T "); else{ currProba=log_to_proba(map[x][y]); @@ -572,7 +573,7 @@ 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(X, Y, target_x, target_y); + *distanceFromTarget = dist(robot_center_x_in_orthonormal_x(),robot_center_y_in_orthonormal_y(),targetXOrhto,targetYOrhto); *d2 = *distanceFromTarget; *beta = -*angleError-theta+target_angle; @@ -598,18 +599,31 @@ linear=-KRHO*distanceFromTarget; angular=-KA*angleError-KB*beta; } + //TODO check those signs *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); + } } -void updateForce(int widthIndice, int heightIndice, float range, float* forceX, float* forceY, float xRobotOrtho, float yRobotOrtho ){ +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 frame 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)) { + if(distanceCellToRobot <= range) { 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); @@ -629,16 +643,16 @@ //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++){ - updateForce(i,j,RANGE_FORCE,&forceRepulsionComputedX,&forceRepulsionComputedY,xRobotOrtho,yRobotOrtho); + update_force(i,j,RANGE_FORCE,&forceRepulsionComputedX,&forceRepulsionComputedY,xRobotOrtho,yRobotOrtho); } } //update with attraction force *forceX=-forceRepulsionComputedX; *forceY=-forceRepulsionComputedY; - float distanceTargetRobot=sqrt(pow(targetX_orhto-xRobotOrtho,2)+pow(targetY_orhto-yRobotOrtho,2)); + float distanceTargetRobot=sqrt(pow(targetXOrhto-xRobotOrtho,2)+pow(targetYOrhto-yRobotOrtho,2)); if(distanceTargetRobot != 0){ - *forceX+=FORCE_CONSTANT_ATTRACTION*(targetX_orhto-xRobotOrtho)/distanceTargetRobot; - *forceY+=FORCE_CONSTANT_ATTRACTION*(targetY_orhto-yRobotOrtho)/distanceTargetRobot; + *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 @@ -650,13 +664,37 @@ //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){ *line_a=forceY; - *line_b=forceX; + *line_b=-forceX; //TODO check in what referentiel it needs to go float xRobotOrtho=robot_center_x_in_orthonormal_x(); float yRobotOrtho=robot_center_y_in_orthonormal_y(); *line_c=forceX*yRobotOrtho-forceY*xRobotOrtho; } +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 angR=%f", angularRight); + pc.printf("\r\n angL=%f", angularLeft); + 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; @@ -679,30 +717,23 @@ //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); - //Normalize speed for motors - if(angularLeft>angularRight) { - angularRight=MAX_SPEED*angularRight/angularLeft; - angularLeft=MAX_SPEED; - } - else { - angularLeft=MAX_SPEED*angularLeft/angularRight; - angularRight=MAX_SPEED; - } + pc.printf("\r\n forceX=%f", forceX); pc.printf("\r\n forceY=%f", forceY); pc.printf("\r\n line: %f x + %f y + %f =0", line_a, line_b, line_c); //Updating motor velocities - leftMotor(1,angularLeft); - rightMotor(1,angularRight); + + leftMotor(sign2(angularLeft),abs(angularLeft)); + rightMotor(sign2(angularRight),abs(angularRight)); pc.printf("\r\n angR=%f", angularRight); pc.printf("\r\n angL=%f", angularLeft); - pc.printf("\r\n dist=%f", dist(X,Y,targetX,targetY)); + 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(X,Y,targetX,targetY)<10) + if(dist(robot_center_x_in_orthonormal_x(),robot_center_y_in_orthonormal_y(),targetXOrhto,targetYOrhto)<10) *reached=true; } @@ -717,7 +748,7 @@ else{ lineAngle=1.5708; } - + //Computing angle error angleError = lineAngle-theta; angleError = atan(sin(angleError)/cos(angleError)); @@ -725,6 +756,42 @@ //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; + //TODO if we put it like the poly says it fails, if we switch the plus and minus it works ... + *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 theta=%f", theta); + pc.printf("\r\n angleError=%f", angleError); + pc.printf("\r\n X=%f", robot_center_x_in_orthonormal_x()); + pc.printf("\r\n Y=%f", robot_center_y_in_orthonormal_y()); + pc.printf("\r\n TX=%f", targetXOrhto); + pc.printf("\r\n TY=%f", targetYOrhto); +} + +//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; } \ No newline at end of file