Navigate to a given point using the OGM and virtual forces
Dependencies: ISR_Mini-explorer mbed
Fork of VirtualForces by
Diff: main.cpp
- Revision:
- 40:f5e212d9f900
- Parent:
- 39:dd8326ec75ce
- Child:
- 41:39157b310975
diff -r dd8326ec75ce -r f5e212d9f900 main.cpp --- a/main.cpp Thu May 18 15:06:25 2017 +0000 +++ b/main.cpp Thu May 18 18:48:47 2017 +0000 @@ -99,8 +99,8 @@ //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=10;//lower right -const float DEFAULT_Y=10;//lower right +const float DEFAULT_X=20;//lower right +const float DEFAULT_Y=20;//lower right const float DEFAULT_THETA=0; //used to create the map 250 represent the 250cm of the square where the robot is tested @@ -131,36 +131,31 @@ int speed=MAX_SPEED; // Max speed at beggining of movement -float leftMm; -float frontMm; -float rightMm; - float line_a; float line_b; float line_c; //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*2/3;//this is in the robot frame top left -const float targetY=WIDTH_ARENA*2/3;//this is in the robot frame top left +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; bool reached=false; int main(){ + initialise_parameters(); - initialise_parameters(); - //try to reach the target while (!reached) { vff(); print_final_map_with_robot_position_and_target(); } - //Stop at the end leftMotor(1,0); rightMotor(1,0); - //print the map + + //print the map forever while(1){ print_final_map_with_robot_position_and_target(); } @@ -240,6 +235,9 @@ 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(); @@ -616,9 +614,7 @@ } } - void updateForce(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); @@ -651,10 +647,13 @@ //update with attraction force *forceX=-forceRepulsionComputedX; *forceY=-forceRepulsionComputedY; - *forceX+=FORCE_CONSTANT_ATTRACTION*(targetX_orhto-xRobotOrtho)/sqrt(pow(targetX_orhto-xRobotOrtho,2)+pow(targetY_orhto-yRobotOrtho,2)); - *forceY+=FORCE_CONSTANT_ATTRACTION*(targetY_orhto-yRobotOrtho)/sqrt(pow(targetX_orhto-xRobotOrtho,2)+pow(targetY_orhto-yRobotOrtho,2)); + float distanceTargetRobot=sqrt(pow(targetX_orhto-xRobotOrtho,2)+pow(targetY_orhto-yRobotOrtho,2)); + if(distanceTargetRobot != 0){ + *forceX+=FORCE_CONSTANT_ATTRACTION*(targetX_orhto-xRobotOrtho)/distanceTargetRobot; + *forceY+=FORCE_CONSTANT_ATTRACTION*(targetY_orhto-yRobotOrtho)/distanceTargetRobot; + } float amplitude=sqrt(pow(*forceX,2)+pow(*forceY,2)); - if(amplitude!=0){//avoid division by 0 + if(amplitude!=0){//avoid division by 0 if forceX and forceY == 0 *forceX=*forceX/amplitude; *forceY=*forceY/amplitude; } @@ -663,8 +662,11 @@ //robotX and robotY are from Odometria void calculate_line(float forceX, float forceY, float robotX, float robotY){ line_a=forceY; - line_b=-forceX; - line_c=forceX*robotY-forceY*robotX; + 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 vff(){ @@ -674,9 +676,9 @@ //we update the odometrie Odometria(); //we check the sensors - leftMm = get_distance_left_sensor(); - frontMm = get_distance_front_sensor(); - rightMm = get_distance_right_sensor(); + float leftMm = get_distance_left_sensor(); + float frontMm = get_distance_front_sensor(); + float rightMm = get_distance_right_sensor(); //update the probabilities values update_sonar_values(leftMm, frontMm, rightMm); //we compute the force on X and Y @@ -696,8 +698,7 @@ pc.printf("\r\n angL=%f", angular_left); pc.printf("\r\n dist=%f", dist(X,Y,targetX,targetY)); - - wait(0.1); + //wait(0.1); Odometria(); if(dist(X,Y,targetX,targetY)<10) reached=true;