Navigate to a given point using the OGM and virtual forces
Dependencies: ISR_Mini-explorer mbed
Fork of VirtualForces by
Diff: main.cpp
- Revision:
- 28:f884979a02fa
- Parent:
- 27:07bde633af72
- Child:
- 29:224e9e686f7b
--- a/main.cpp Mon Apr 24 11:55:55 2017 +0000 +++ b/main.cpp Wed Apr 26 10:55:54 2017 +0000 @@ -8,6 +8,8 @@ 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 flip +void do_a_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 @@ -161,9 +163,23 @@ go_to_point_with_angle(target_x, target_y, target_angle); } +void do_a_flip(){ + Odometria(); + float theta_plus_pi=rad_angle_check(theta+pi); + while(abs(theta_plus_pi-theta)>0.01){ + leftMotor(1,500); + rightMotor(1,500); + wait(0.2); + Odometria(); + } + leftMotor(1,0); + rightMotor(1,0); +} + //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) { + Odometria(); alpha = atan2((target_y-Y),(target_x-X))-theta; alpha = atan(sin(alpha)/cos(alpha)); rho = dist(X, Y, target_x, target_y); @@ -191,6 +207,7 @@ leftMotor(1,0); rightMotor(1,0); update_sonar_values(leftMm, frontMm, rightMm); + //TODO Giorgos maybe you can also test the do_a_flip() function Odometria(); //do a flip TODO keep_going=false;