Diff: PsiSwarm/sensors.cpp

Revision:

6:ff3c66f7372b

Parent:

3:cd048f6e544e

Child:

7:ef9ab01b9e26

--- a/PsiSwarm/sensors.cpp	Tue Oct 13 11:47:14 2015 +0000
+++ b/PsiSwarm/sensors.cpp	Thu Oct 22 00:46:14 2015 +0000
@@ -2,11 +2,14 @@
  * 
  * File: sensors.cpp
  *
- * (C) Dr James Hilder, Dept. Electronics & Computer Science, University of York
+ * (C) Dept. Electronics & Computer Science, University of York
+ * James Hilder, Alan Millard, Homero Elizondo, Jon Timmis
  * 
- * PsiSwarm Library Version: 0.2
+ * Sensor bearing code by Alan Millard
  *
- * September 2015
+ * PsiSwarm Library Version: 0.3
+ *
+ * October 2015
  *
  */
 
@@ -446,4 +449,29 @@
          black_background[3],          black_active[3],  
          black_background[4],          black_active[4]);
     
-}
\ No newline at end of file
+}
+
+
+int get_bearing_from_ir_array (unsigned short * ir_sensor_readings){
+    float degrees_per_radian = 57.295779513;
+    
+    // sin(IR sensor angle) and cos(IR sensor angle) LUT, for all 8 sensors
+    float ir_sensor_sin[8] = {0.382683432, 0.923879533, 0.923879533, 0.382683432, -0.382683432, -0.923879533, -0.923879533, -0.382683432};
+    float ir_sensor_cos[8] = {0.923879533, 0.382683432, -0.382683432, -0.923879533, -0.923879533, -0.382683432, 0.382683432, 0.923879533};
+    
+    float sin_sum = 0;
+    float cos_sum = 0;
+    
+    for(int i = 0; i < 8; i++)
+    {        
+        // Use IR sensor reading to weight bearing vector
+        sin_sum += ir_sensor_sin[i] * ir_sensor_readings[i];
+        cos_sum += ir_sensor_cos[i] * ir_sensor_readings[i];
+    }
+    
+    float bearing = atan2(sin_sum, cos_sum); // Calculate vector towards IR light source 
+    bearing *= degrees_per_radian; // Convert to degrees
+
+    return (int) bearing;
+}    
+