Diff: programs.cpp

Revision:

24:31707fec20a5

Parent:

23:e59040ac05c4

Child:

25:b5742873e02b

--- a/programs.cpp	Wed Jan 20 14:55:38 2016 +0000
+++ b/programs.cpp	Wed Jan 20 14:58:24 2016 +0000
@@ -728,14 +728,7 @@
 /// flocking_program
 
 void flocking_program()
-{    
-    // Do something on the first run of a program
-    if(program_run_init == 1)
-    {
-        // Initialisation code goes here...
-        program_run_init = 0;
-    }
-                 
+{                
     int average_heading = 0;
     int number_of_neighbours = 0;
     int number_of_flocking_headings = 0;
@@ -762,12 +755,6 @@
     
     cohesion_vector.distance /= number_of_neighbours; // Normalise
     
-//    float left_motor_speed = 0.5;
-//    float right_motor_speed = 0.5;
-
-    float left_motor_speed;
-    float right_motor_speed;
-    
     int target_heading = 0;
     int angles = 0;
     
@@ -788,20 +775,6 @@
         
     obstacle_vector.distance /= sensors_activated; // Normalise
     
-    char robot_nearby = 0;
-    
-    for(int i = 0; i < 8; i++)
-    {
-        if(robots_found[i])
-        {
-            if(robots_heading[i] > -90 && robots_heading[i] < 90)
-            {
-                robot_nearby = 1;
-                break;
-            }
-        }
-    }
-    
     if(obstacle_vector.distance > 150)
     {
         target_heading += obstacle_vector.angle + 180;
@@ -814,107 +787,37 @@
         angles++;
     }
     
-//    if(obstacle_vector.distance > 150 && number_of_neighbours == 0)
-//    {
-//        if(obstacle_vector.angle > 0 && obstacle_vector.angle < 67.5)
-//            left_motor_speed *= -1;
-//        else if(obstacle_vector.angle < 0 && obstacle_vector.angle > -67.5)
-//            right_motor_speed *= -1;        
-//    }
-//    else if(number_of_neighbours > 0)
-//    {
-        if(number_of_flocking_headings > 0)
-        {
-            average_heading /= number_of_flocking_headings;
-            
-            //out("beacon heading: %d\n", beacon_heading);
-//            out("average heading: %d\n", average_heading);
-//            out("difference: %d\n", beacon_heading - average_heading);
-
-            
-
-            int relative_flocking_heading = beacon_heading - average_heading;
-            
-            target_heading += relative_flocking_heading;
-            angles++;
+    if(number_of_flocking_headings > 0)
+    {
+        average_heading /= number_of_flocking_headings;            
 
-//            if(relative_flocking_heading > 0)
-//            {
-//                right_motor_speed = -0.25;
-//                left_motor_speed = 0.25;
-//            }
-//            else if(relative_flocking_heading < 0)
-//            {
-//                right_motor_speed = 0.25;
-//                left_motor_speed = -0.25;
-//            }
-            
-//            if(relative_flocking_heading > 22.5)
-//            {
-//                right_motor_speed = -0.5;
-//                left_motor_speed = 0.5;
-//            }
-//            else if(relative_flocking_heading < -22.5)
-//            {
-//                right_motor_speed = 0.5;
-//                left_motor_speed = -0.5;
-//            }
-        }
+        int relative_flocking_heading = beacon_heading - average_heading;
         
-        if(angles > 0)
-            target_heading /= angles; // Calculate average
+        target_heading += relative_flocking_heading;
+        angles++;
+    }
         
-        if(target_heading > 180)
-            target_heading -= 360;
-        if(target_heading < -180)
-            target_heading += 360;
-        
-        if(step_cycle == 0)
-            time_based_turn_degrees(1, target_heading, 1);
-        else
-            forward(0.25);
-        
-        
-//        if(cohesion_vector.angle > 22.5)
-//            right_motor_speed *= -1;
-//        else if(cohesion_vector.angle < -22.5)
-//            left_motor_speed *= -1;
-//    }
+    if(angles > 0)
+        target_heading /= angles; // Calculate average
+    
+    if(target_heading > 180)
+        target_heading -= 360;
+    if(target_heading < -180)
+        target_heading += 360;
     
-//    if(obstacle_vector.distance > 150 && robot_nearby == 0)
-//    {        
-//        if(obstacle_vector.angle > 0 && obstacle_vector.angle < 67.5)
-//            left_motor_speed *= -1;
-//        else if(obstacle_vector.angle < 0 && obstacle_vector.angle > -67.5)
-//            right_motor_speed *= -1;
-//    }
-//    else if(number_of_neighbours != 0)
-//    {
-//        average_heading /= number_of_flocking_headings; // Should this include self?
-//        out("average heading: %d\n", average_heading);
-//        
-//        // Turn towards the average of the cohesion and alignment angles?
-//        
-//        if(beacon_heading < average_heading)
-//            left_motor_speed = 0.25;
-//        else if (beacon_heading > average_heading)
-//            right_motor_speed = 0.25;
-//    }
-    
-//    set_left_motor_speed(left_motor_speed);
-//    set_right_motor_speed(right_motor_speed);
-    
-//    if(step_cycle == 0)
-//    {
-        // 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);
-        }
-//    }
+    if(step_cycle == 0)
+        time_based_turn_degrees(1, target_heading, 1);
+    else
+        forward(0.25);
+
+    // 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);
+    }
 }
 
 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////