Diff: programs.cpp

Revision:

19:77f7089dca87

Parent:

18:5921c1853e8a

Child:

20:e08376c0b4ea

--- a/programs.cpp	Fri Jan 08 19:10:52 2016 +0000
+++ b/programs.cpp	Tue Jan 12 17:28:12 2016 +0000
@@ -727,20 +727,41 @@
 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 /// flocking_program
 
+Timer timer;
+
 void flocking_program()
-{
-    int average_heading = 0;
+{    
+    // Do something on the first run of a program
+    if(program_run_init == 1)
+    {
+        // Initialisation code goes here...        
+        timer.start();
+        program_run_init = 0;
+    }
     
-    for(int i = 0; i < 8; i++)
-        average_heading += flocking_headings[i];
+    if(timer.read() > 1)
+    {
+        timer.reset();
+             
+        int average_heading = 0;
         
-    average_heading /= 8;
-    
-    out("average heading: %d\n", average_heading);
-    
-    out("sending beacon heading: %d\n", beacon_heading);
-    bt.putc(beacon_heading);
-    //bt.printf("%d", beacon_heading);
+        for(int i = 0; i < 8; i++)
+            average_heading += flocking_headings[i];
+            
+        average_heading /= 8;
+        
+//        out("average heading: %d\n", average_heading);
+        
+        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("beacon heading byte: %d\n", beacon_heading_byte);
+//        out("ground truth: %f\n", (beacon_heading + 180) * degrees_per_value);
+    //    bt.putc(beacon_heading);
+        bt.putc(beacon_heading_byte);
+    //    bt.printf("%d\n", beacon_heading);
+    }   
 }
 
 //Timer timer;