![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Mapping for TP2
Dependencies: ISR_Mini-explorer mbed
Fork of GoToPointWithAngle by
Diff: main.cpp
- Revision:
- 23:901fc468b8a7
- Parent:
- 21:ebb37a249b5f
- Child:
- 24:8f4b820d8de8
--- a/main.cpp Tue Apr 18 17:48:26 2017 +0000 +++ b/main.cpp Wed Apr 19 10:45:09 2017 +0000 @@ -16,6 +16,11 @@ float compute_probability_t(float x, float y,float xs,float ys, float angleFromSonarPosition, float distanceObstacleDetected); //print the map void print_final_map(); +//compute the angles and distance used for the velocities +void compute_angles_and_distance(float target_x, float target_y, float target_angle); +//compute the linear and 2 angular velocities +void compute_linear_angular_velocities(); + //MATHS heavy functions float dist(float robot_x, float robot_y, float target_x, float target_y); @@ -133,7 +138,7 @@ //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) { - bool tooClose=false; + bool tooClose=false; //binary variable to know if an obstacle is <10cm away do { pc.printf("\n\n\r entered while"); @@ -145,53 +150,19 @@ //Updating X,Y and theta with the odometry values Odometria(); + //Updare the values from the sonar update_sonar_values(); + //Check if one of the sonars finds an obstacle that is too close if (leftMm < 100 || frontMm < 100 || rightMm < 100) { tooClose = true; break; } - 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); + 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) - //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; - - //Slowing down at the end for more precision - if (d2<25) { - speed = d2*30; - } - - //Normalize speed for motors - if(angular_left>angular_right) { - angular_right=speed*angular_right/angular_left; - angular_left=speed; - } else { - angular_left=speed*angular_left/angular_right; - angular_right=speed; - } - + //Printing values for debugging purposes pc.printf("\n\r X=%f", X); pc.printf("\n\r Y=%f", Y); pc.printf("\n\r leftMm=%f", leftMm); @@ -353,3 +324,46 @@ return acos(cosAlpha); } +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; + + //Slowing down at the end for more precision + if (d2<25) { + speed = d2*30; + } + + //Normalize speed for motors + if(angular_left>angular_right) { + angular_right=speed*angular_right/angular_left; + angular_left=speed; + } else { + angular_left=speed*angular_left/angular_right; + angular_right=speed; + } +}