Diff: main.cpp

Revision:

11:e641aa08c92e

Parent:

8:109314be5b68

Child:

12:3c0ca2350624

--- a/main.cpp	Mon Mar 27 16:20:29 2017 +0000
+++ b/main.cpp	Mon Mar 27 16:34:43 2017 +0000
@@ -8,17 +8,23 @@
 
 int goToPointWithAngle(float target_x, float target_y, int theta);
 
+int updateSonarValues();
+
 float alpha; //angle error
 float rho; //distance from target
 float beta;
-//float k_linear=10, k_angular=200;
-//kb = -15  and ka = 30 tabom
-float kRho=12, ka=30, kb=-13;
+float kRho=12, ka=30, kb=-13; //Kappa values
 float linear, angular, angular_left, angular_right;
 float dt=0.5;
 float temp;
 float d2;
 
+bool tooClose = false;
+
+int leftMm;
+int frontMm;
+int rightMm;
+
 //Diameter of a wheel and distance between the 2
 float r=3.25, b=7.2;
 
@@ -58,6 +64,14 @@
     return sqrt(pow(target_y-robot_y,2) + pow(target_x-robot_x,2));
 }
 
+//Updates sonar values
+int updateSonarValues() {
+    leftMm = get_distance_left_sensor();
+    frontMm = get_distance_front_sensor();
+    rightMm = get_distance_right_sensor();
+    return 0;
+}
+
 int goToPointWithAngle(float target_x, float target_y, int theta) {
      do {
         pc.printf("\n\n\r entered while");
@@ -67,6 +81,11 @@
         t.reset();
         t.start();
         
+        updateSonarValues();
+        if (leftMm < 100 || frontMm < 100 || rightMm < 100) {
+            tooClose = true;    
+        }
+        
         //Updating X,Y and theta with the odometry values
         Odometria();
         
@@ -122,7 +141,7 @@
         wait(0.2);
         //Timer stuff
         t.stop();
-    } while(d2>1);
+    } while(d2>1 & !tooClose);
     
     return 0;
 }
\ No newline at end of file