Bluetooth communication for flocking.
Fork of BeautifulMemeProject by
Diff: PsiSwarm/sensors.cpp
- Revision:
- 7:ef9ab01b9e26
- Parent:
- 6:ff3c66f7372b
diff -r ff3c66f7372b -r ef9ab01b9e26 PsiSwarm/sensors.cpp --- 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; }