Diff: programs.cpp

Revision:

21:e5ab8c56a769

Parent:

20:e08376c0b4ea

Child:

22:fd75afdadfb2

--- a/programs.cpp	Fri Jan 15 14:42:42 2016 +0000
+++ b/programs.cpp	Fri Jan 15 18:38:59 2016 +0000
@@ -744,18 +744,30 @@
         timer.reset();
              
         int average_heading = 0;
+        int number_of_neighbours = 0;
         
         for(int i = 0; i < 8; i++)
-            average_heading += flocking_headings[i];
-            
-        average_heading /= 8;
+        {            
+            if(robots_found[i])
+            {
+                average_heading += flocking_headings[i];
+                number_of_neighbours++;
+            }
+        }
         
-//        out("average heading: %d\n", average_heading);
+        if(number_of_neighbours != 0)
+        {
+            average_heading /= number_of_neighbours;
         
-        float degrees_per_value = 256.0f / 360.0f;    
+            out("average heading: %d\n", average_heading);
+            
+            turn_to_bearing(average_heading + 180);
+        }
+        
+        float degrees_per_value = 256.0f / 360.0f;
         char beacon_heading_byte = (beacon_heading + 180) * degrees_per_value; // Convert beacon heading from +/-180 degrees into a single byte value
         
-        out("sending beacon heading: %d\n", beacon_heading);
+//        out("sending beacon heading: %d\n", beacon_heading);
         bt.putc(beacon_heading_byte);
         
 //        out("robot_id: %d\n", robot_id);