Diff: main.cpp

Revision:

29:224e9e686f7b

Parent:

28:f884979a02fa

Child:

30:95d8d3e2b81b

--- a/main.cpp	Wed Apr 26 10:55:54 2017 +0000
+++ b/main.cpp	Wed Apr 26 16:58:57 2017 +0000
@@ -8,8 +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();
+//make the robot do a pi/2 flip
+void do_half_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
@@ -126,11 +126,14 @@
     Y=DEFAULT_Y;
 
     
-    for (int i = 0; i<20; i++) {
-        randomize_and_map();
-        wait(2);
+    for (int i = 0; i<10; i++) {
+        randomize_and_map(); 
+        //print_final_map();
+        print_final_map_with_robot_position();   
+    }
+    while(1){
         print_final_map();
-        //print_final_map_with_robot_position();    
+        print_final_map_with_robot_position();
     }
 
 }
@@ -163,14 +166,17 @@
     go_to_point_with_angle(target_x, target_y, target_angle);
 }
 
-void do_a_flip(){
+
+void do_half_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);
+    float theta_plus_h_pi=theta+pi/2;//theta is between -pi and pi
+    if(theta_plus_h_pi > pi)
+        theta_plus_h_pi=-(2*pi-theta_plus_h_pi);
+    leftMotor(0,100);
+    rightMotor(1,100);
+    while(abs(theta_plus_h_pi-theta)>0.05){
         Odometria();
+       // pc.printf("\n\r diff=%f", abs(theta_plus_pi-theta));
     }
     leftMotor(1,0);
     rightMotor(1,0);    
@@ -203,34 +209,15 @@
         //pc.printf("\n\r rightMm=%f", rightMm);
     
         //if in dangerzone 
-        if(frontMm < 100 || leftMm <100 || rightMm <100){
+        if(frontMm < 120 || leftMm <120 || rightMm <120){
             leftMotor(1,0);
             rightMotor(1,0);
             update_sonar_values(leftMm, frontMm, rightMm);
-            //TODO Giorgos maybe you can also test the do_a_flip() function
+            //TODO Giorgos maybe you can also test the do_half_flip() function
             Odometria();
             //do a flip TODO
             keep_going=false;
-            target_angle=theta+pi;
-            target_y=Y;
-            target_x=X;
-            alpha = atan2((target_y-Y),(target_x-X))-theta;
-            alpha = atan(sin(alpha)/cos(alpha));
-            rho = dist(X, Y, target_x, target_y);
-            beta = -alpha-theta+target_angle;  
-            //keep going until the flip is done
-            do{
-                Odometria();
-                dt = t.read();
-                t.reset();
-                t.start();
-                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)
-                leftMotor(1,angular_left);
-                rightMotor(1,angular_right);
-                //Timer stuff
-                t.stop();
-            }while(d2>1 && (abs(target_angle-theta)>0.01));       
+            do_half_flip();   
         }else{
             //if not in danger zone continue as usual
             update_sonar_values(leftMm, frontMm, rightMm);
@@ -337,12 +324,12 @@
         for (int x= 0; x<NB_CELL_WIDTH; x++) {
                 currProba=log_to_proba(map[x][y]);
             if ( currProba < 0.5) {
-                pc.printf(" 0 ");
+                pc.printf("   ");
             } else {
                 if(currProba==0.5)
                     pc.printf(" . ");
                 else
-                    pc.printf(" + ");
+                    pc.printf(" X ");
             }
         }
         pc.printf("\n\r");
@@ -374,12 +361,12 @@
             else{
                 currProba=log_to_proba(map[x][y]);
                 if ( currProba < 0.5)
-                    pc.printf(" 0 ");
+                    pc.printf("   ");
                 else{
                     if(currProba==0.5)
                         pc.printf(" . ");
                     else
-                        pc.printf(" + ");
+                        pc.printf(" X ");
                 } 
             }
         }