Diff: programs.cpp

Revision:

27:7eb032772bc2

Parent:

26:3869ae5eddd7

--- a/programs.cpp	Mon Jan 25 16:36:54 2016 +0000
+++ b/programs.cpp	Sun Jan 31 15:14:54 2016 +0000
@@ -728,7 +728,8 @@
 /// flocking_program
 
 void flocking_program()
-{                
+{
+    char display_line[16] = "               ";      
     int average_heading = 0;
     int number_of_neighbours = 0;
     int number_of_flocking_headings = 0;
@@ -770,9 +771,11 @@
     cohesion_vector.distance /= number_of_neighbours; // Normalise        
     obstacle_vector.distance /= sensors_activated; // Normalise
     
+    int obstacle_avoidance_angle;
+    
     if(sensors_activated > 0)
     {
-        int obstacle_avoidance_angle = obstacle_vector.angle + 180;
+        obstacle_avoidance_angle = obstacle_vector.angle + 180;
                 
         if(obstacle_avoidance_angle > 180)
             obstacle_avoidance_angle -= 360;
@@ -782,11 +785,13 @@
         target_heading += obstacle_avoidance_angle;
         angles++;
     }
+
+    int cohesion_angle;
     
     // Don't bother performing cohesion if robots are already close enough
-    if(number_of_neighbours > 0 && cohesion_vector.distance < 400)
-    {        
-        int cohesion_angle = cohesion_vector.angle;
+    if(number_of_neighbours > 0 && cohesion_vector.distance < 200)
+    {            
+        cohesion_angle = cohesion_vector.angle;
         
         if(cohesion_angle > 180)
             cohesion_angle -= 360;
@@ -797,11 +802,13 @@
         angles++;
     }
     
+    int relative_flocking_heading;
+    
     if(number_of_flocking_headings > 0)
     {        
         average_heading /= number_of_flocking_headings;            
 
-        int relative_flocking_heading = beacon_heading - average_heading;
+        relative_flocking_heading = beacon_heading - average_heading;
         
         if(relative_flocking_heading > 180)
             relative_flocking_heading -= 360;
@@ -828,12 +835,16 @@
 
     // Only transmit beacon heading if the beacon is visible
     if(beacon_found)
-    {
+    {        
         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
         
         bt.putc(beacon_heading_byte);
     }
+    
+    sprintf(display_line, "%d %d %d %d", target_heading, obstacle_avoidance_angle, cohesion_angle, relative_flocking_heading);
+ 
+    set_program_info(display_line);
 }
 
 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////