Diff: sensors.cpp

Revision:

13:5f08195456a4

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/sensors.cpp	Tue Dec 15 08:56:36 2015 +0000
@@ -0,0 +1,85 @@
+#include "sensors.h"
+#include "mbed.h"
+#include "mouse.h"
+#include "pinouts.h"
+
+// updates PID values.
+void Mouse::scan()
+{
+    eRS = 1;
+    wait_us(50);
+    rs_val = rRS.read();
+    eRS = 0;
+    
+    eRF = 1;
+    wait_us(50);
+    rf_val = rRF.read();
+    
+    eLS  = 1;
+    wait_us(50);
+    ls_val = rLS.read();
+    eLS = 0;
+    
+    eLF = 1;
+    wait_us(50);
+    lf_val = rLF.read();
+}
+
+bool Mouse::forward_wall_detected()
+{
+    return lf_val > thresh_LF;
+}
+
+bool Mouse::side_opening_detected()
+{
+    if (rs_val < open_right) 
+    {
+        mouse_state = MOVECELL;
+        turn_direction = RIGHT;
+        return true;
+    } 
+    else if (ls_val < open_left) 
+    {
+        mouse_state = MOVECELL;
+        turn_direction = LEFT;
+        return true;
+    }
+        
+    return false;
+}
+
+// computes gyro_offset
+// uses a "weighted" average. 
+// idea is that the current gyro offset is weighted more than previous ones. 
+    // uses the following y(n) = 1/2 * y(n-1) + 1/2 * x(n). 
+    // (therefore y(n) = sum of x(i)/2^i from i from 0 to n.)
+    // this maintains that there will be some influence from previous factors, but keeps the current value at a higher weight.
+// currently this is only in the setup function. we can run this when the mouse is running in a line
+// when we figure out good line running pid.
+void Mouse::offsetCalc()
+{
+    gyro_offset = gyro_offset / 2 + _gyro.read() / 2;
+}
+
+void Mouse::sensor_reset()
+{
+    l_enco.reset();
+    r_enco.reset();
+}
+
+void Mouse::sensor_init()
+{
+    gyro_offset = 0;
+    
+    lf_val = 0;
+    rf_val = 0;
+    ls_val = 0;
+    rs_val = 0;
+    
+    eLF = 0;
+    eRF = 0;
+    eLS = 0;
+    eRS = 0;
+    
+    sensor_reset();
+}
\ No newline at end of file