Diff: main.cpp

Revision:

2:98efd8dd9077

Parent:

0:13d8a77fb1d7

--- a/main.cpp	Sat Nov 21 03:20:07 2015 +0000
+++ b/main.cpp	Sat Nov 21 03:43:15 2015 +0000
@@ -68,6 +68,9 @@
 const float spin_enco_weight = 0.5;
 const float spin_gyro_weight = 1 - spin_enco_weight;
 
+const float frontWall = 0.7; //need to calibrate this threshold to a value where mouse can stop in time
+//something like this may be useful 
+
 volatile float enco_error;
 volatile float enco_pid;
 volatile float gyro_error;
@@ -82,6 +85,8 @@
 
 void systick()
 {
+    watchOut();
+    
     enco_error = l_enco.getPulses() - r_enco.getPulses();
     gyro_error = _gyro.read() - gyro_offset;
     
@@ -119,6 +124,28 @@
 }
 
 
+void stop()
+{
+    left_forward = 1;
+    left_reverse = 1;
+    right_forward = 1;
+    right_reverse = 1;
+    }
+    
+void watchOut()
+{
+    eRF = 1;
+    eLF = 1;
+    if(rRF > frontWall || rLF > frontWall)
+    {
+        eRF = 0;
+        eLF = 0;
+        stop();
+        return;
+    }
+}
+
+
 void setup()
 {
     pc.printf("Hello World\r\n");