Navigate to a given point using the OGM and virtual forces
Dependencies: ISR_Mini-explorer mbed
Fork of VirtualForces by
Diff: main.cpp
- Revision:
- 33:78139f82ea74
- Parent:
- 32:d51928b58645
- Child:
- 34:128fc7aed957
--- a/main.cpp Wed May 03 16:46:43 2017 +0000 +++ b/main.cpp Thu May 04 15:21:24 2017 +0000 @@ -20,6 +20,10 @@ void print_final_map(); //print the map with the robot marked on it void print_final_map_with_robot_position(); +//go to a given line +void go_to_line(); +//calculate virtual force field and move +void vff(); //MATHS heavy functions float dist(float robot_x, float robot_y, float target_x, float target_y); @@ -109,7 +113,7 @@ float alpha; //angle error float rho; //distance from target float beta; -float kRho=12, ka=30, kb=-13; //Kappa values +float kRho=12, ka=30, kb=-13, kv=200, kh=200; //Kappa values //TODO check kv, kh for go_to_line float linear, angular, angular_left, angular_right; float dt=0.5; float temp; @@ -120,7 +124,16 @@ float leftMm; float frontMm; -float rightMm; +float rightMm; + +float line_a; +float line_b; +float line_c; + +float targetX; +float targetY; + +bool reached=false; int main(){ @@ -136,18 +149,19 @@ theta=DEFAULT_THETA; X=DEFAULT_X; Y=DEFAULT_Y; - - - for (int i = 0; i<10; i++) { - randomize_and_map(); - //print_final_map(); - print_final_map_with_robot_position(); - } - while(1){ - print_final_map(); + + while (!reached) { + vff(); print_final_map_with_robot_position(); } - + + //Stop at the end + leftMotor(1,0); + rightMotor(1,0); + + while(1){ + print_final_map_with_robot_position(); + } } //fill initialLogValues with the values we already know (here the bordurs) @@ -552,4 +566,64 @@ float amplitude=sqrt(pow(forceX,2)+pow(forceY,2)); forceX=forceX/amplitude; forceY=forceY/amplitude; +} + +void calculate_line(float forceX, float forceY, float robotX, float robotY){ + line_a=forceY; + line_b=-forceX; + line_c=forceX*robotY-forceY*robotX; +} + +void vff(){ + //Updating X,Y and theta with the odometry values + float forceX, forceY; + Odometria(); + + leftMm = get_distance_left_sensor(); + frontMm = get_distance_front_sensor(); + rightMm = get_distance_right_sensor(); + update_sonar_values(leftMm, frontMm, rightMm); + + updateForce(WIDTH_ARENA, HEIGHT_ARENA, 20, &forceX, &forceY, X, Y); //TODO check range value, I randomly put 20 + compute_forceX_and_forceY(targetX, targetY,forceX, forceY); + calculate_line(forceX, forceY, X, Y); + + //Updating motor velocities + leftMotor(1,angular_left); + rightMotor(1,angular_right); + + wait(0.2); + Odometria(); + if(dist(X,Y,targetX,targetY)<0.5) + reached=true; +} + +void go_to_line(){ + float line_angle, angle_error; + if(line_b!=0){ + line_angle=atan(-line_a/line_b); + } + else{ + line_angle=1.5708; + } + + //Computing angle error + angle_error = line_angle-theta; + angle_error = atan(sin(angle_error)/cos(angle_error)); + + //Calculating velocities + linear=kv*(3.1416); + angular=kh*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=speed*angular_right/angular_left; + angular_left=speed; + } + else { + angular_left=speed*angular_left/angular_right; + angular_right=speed; + } } \ No newline at end of file