James O'Keeffe
/
BeaconDemo_RobotCodeNew
ft. button press reset
Fork of BeaconDemo_RobotCode by
Diff: PsiSwarm/sensors.cpp
- Revision:
- 6:ff3c66f7372b
- Parent:
- 3:cd048f6e544e
- Child:
- 7:ef9ab01b9e26
diff -r 598298aa4900 -r ff3c66f7372b PsiSwarm/sensors.cpp --- 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; +} +