Diff: programs.cpp

Revision:

26:3869ae5eddd7

Parent:

25:b5742873e02b

Child:

27:7eb032772bc2

--- a/programs.cpp	Mon Jan 25 15:58:58 2016 +0000
+++ b/programs.cpp	Mon Jan 25 16:36:54 2016 +0000
@@ -732,22 +732,27 @@
     int average_heading = 0;
     int number_of_neighbours = 0;
     int number_of_flocking_headings = 0;
+    int target_heading = 0;
+    int angles = 0;    
+    int ir_sensor_threshold = 600;
+    int sensors_activated = 0;
+    
+    struct FloatVector obstacle_vector;
+    obstacle_vector.angle = 0;
+    obstacle_vector.distance = 0;
     
     struct FloatVector cohesion_vector;
     cohesion_vector.angle = 0;
     cohesion_vector.distance = 0;
     
-//    out("flocking_headings: ");
-//    
-//    for(int i = 0; i < 8; i++)
-//    {
-//        out("%d, ", flocking_headings[i]);
-//    }
-//    
-//    out("\n");
-    
     for(int i = 0; i < 8; i++)
-    {            
+    {
+        if(reflected_sensor_data[i] > ir_sensor_threshold)
+        {            
+            obstacle_vector = addVector(obstacle_vector, get_bearing_from_ir_array(reflected_sensor_data), reflected_sensor_data[i]);
+            sensors_activated++;
+        }
+                
         if(robots_found[i])
         {
             // -180 degrees is reserved for "no byte received"
@@ -762,41 +767,9 @@
         }
     }
     
-//    out("number_of_flocking_headings: %d\n", number_of_flocking_headings);
-    
-    cohesion_vector.distance /= number_of_neighbours; // Normalise
-    
-    int target_heading = 0;
-    int angles = 0;
-    
-    int ir_sensor_threshold = 400;
-    
-    struct FloatVector obstacle_vector;
-    obstacle_vector.angle = 0;
-    obstacle_vector.distance = 0;
-    
-    int sensors_activated = 0;
-    
-//    out("reflected_sensor_data: ");
-    
-    for(int i = 0; i < 8; i++)
-    {
-//        out("%d, ", reflected_sensor_data[i]);
-        
-        if(reflected_sensor_data[i] > ir_sensor_threshold)
-        {            
-            obstacle_vector = addVector(obstacle_vector, get_bearing_from_ir_array(reflected_sensor_data), reflected_sensor_data[i]);
-            sensors_activated++;
-        }
-    }
-    
-//    out("\n");
-        
+    cohesion_vector.distance /= number_of_neighbours; // Normalise        
     obstacle_vector.distance /= sensors_activated; // Normalise
     
-//    out("obstacle_vector.distance: %f, sensors_activated: %d\n", obstacle_vector.distance, sensors_activated);
-    
-    //if(obstacle_vector.distance > 150)
     if(sensors_activated > 0)
     {
         int obstacle_avoidance_angle = obstacle_vector.angle + 180;
@@ -808,13 +781,11 @@
         
         target_heading += obstacle_avoidance_angle;
         angles++;
-        
-        out("obstacle_vector.angle: %f, target_heading: %d\n", obstacle_vector.angle, target_heading);
     }
     
-//    if(cohesion_vector.distance > 0)
-    if(number_of_neighbours > 0)
-    {
+    // 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(cohesion_angle > 180)
@@ -827,11 +798,7 @@
     }
     
     if(number_of_flocking_headings > 0)
-    {
-//        // Include own heading in flocking heading
-//        average_heading += beacon_heading;
-//        number_of_flocking_headings++;
-        
+    {        
         average_heading /= number_of_flocking_headings;            
 
         int relative_flocking_heading = beacon_heading - average_heading;
@@ -843,40 +810,13 @@
         
         target_heading += relative_flocking_heading;
         angles++;
-        
-        out("average_heading: %d, relative_flocking_heading: %d, target_heading: %d\n", average_heading, relative_flocking_heading, target_heading);
     }
         
     if(angles > 0)
         target_heading /= angles; // Calculate average
-    
-//    if(target_heading > 180)
-//        target_heading -= 360;
-//    if(target_heading < -180)
-//        target_heading += 360;
-        
-    out("normalised target_heading: %d\n", target_heading);
-    
-//    if(step_cycle == 0)
-//        time_based_turn_degrees(1, target_heading, 1);
-//    else
-//        forward(0.25);
 
     float left_motor_speed = 0.25;
     float right_motor_speed = 0.25;
-    
-//    // Only avoid obstacles if very close to a wall
-//    if(sensors_activated > 0 && obstacle_vector.distance > 1000)
-//    {
-//        int obstacle_avoidance_angle = obstacle_vector.angle + 180;
-//                
-//        if(obstacle_avoidance_angle > 180)
-//            obstacle_avoidance_angle -= 360;
-//        if(obstacle_avoidance_angle < -180)
-//            obstacle_avoidance_angle += 360;
-//        
-//        target_heading = obstacle_avoidance_angle;
-//    }
 
     if(target_heading > 22.5)
         right_motor_speed *= -1;