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;