Diff: PsiSwarm/sensors.cpp

Revision:

7:ef9ab01b9e26

Parent:

6:ff3c66f7372b

--- a/PsiSwarm/sensors.cpp	Thu Oct 22 00:46:14 2015 +0000
+++ b/PsiSwarm/sensors.cpp	Thu Oct 22 13:28:17 2015 +0000
@@ -453,6 +453,8 @@
 
 
 int get_bearing_from_ir_array (unsigned short * ir_sensor_readings){
+    //out("Getting bearing from array: [%d][%d][%d][%d][%d][%d][%d][%d]\n",ir_sensor_readings[0],ir_sensor_readings[1],ir_sensor_readings[2],ir_sensor_readings[3],ir_sensor_readings[4],ir_sensor_readings[5],ir_sensor_readings[6],ir_sensor_readings[7]);
+    
     float degrees_per_radian = 57.295779513;
     
     // sin(IR sensor angle) and cos(IR sensor angle) LUT, for all 8 sensors
@@ -469,9 +471,11 @@
         cos_sum += ir_sensor_cos[i] * ir_sensor_readings[i];
     }
     
-    float bearing = atan2(sin_sum, cos_sum); // Calculate vector towards IR light source 
+     float bearing = atan2(sin_sum, cos_sum); // Calculate vector towards IR light source 
     bearing *= degrees_per_radian; // Convert to degrees
 
+    //out("Sin sum:%f Cos sum:%f Bearing:%f\n",sin_sum,cos_sum,bearing);
+
     return (int) bearing;
 }