Diff: programs.cpp

Revision:

22:fd75afdadfb2

Parent:

21:e5ab8c56a769

Child:

23:e59040ac05c4

--- a/programs.cpp	Fri Jan 15 18:38:59 2016 +0000
+++ b/programs.cpp	Sat Jan 16 12:01:49 2016 +0000
@@ -727,173 +727,103 @@
 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 /// flocking_program
 
-Timer timer;
-
 void flocking_program()
 {    
     // Do something on the first run of a program
     if(program_run_init == 1)
     {
-        // Initialisation code goes here...        
-        timer.start();
+        // Initialisation code goes here...
         program_run_init = 0;
     }
+                 
+    int average_heading = 0;
+    int number_of_neighbours = 0;
+    int number_of_flocking_headings = 0;
     
-    if(timer.read() > 1)
-    {
-        timer.reset();
-             
-        int average_heading = 0;
-        int number_of_neighbours = 0;
-        
-        for(int i = 0; i < 8; i++)
-        {            
-            if(robots_found[i])
+    struct FloatVector cohesion_vector;
+    cohesion_vector.angle = 0;
+    cohesion_vector.distance = 0;
+    
+    for(int i = 0; i < 8; i++)
+    {            
+        if(robots_found[i])
+        {
+            // -180 degrees is reserved for "no byte received"
+            if(flocking_headings[i] != -180)
             {
                 average_heading += flocking_headings[i];
-                number_of_neighbours++;
+                number_of_flocking_headings++;
+            }
+            
+            cohesion_vector = addVector(cohesion_vector, robots_heading[i], robots_distance[i]);
+            number_of_neighbours++;
+        }
+    }
+    
+    cohesion_vector.distance /= number_of_neighbours; // Normalise
+    
+    float left_motor_speed = 0.5;
+    float right_motor_speed = 0.5;
+    
+    struct FloatVector obstacle_vector;
+    obstacle_vector.angle = 0;
+    obstacle_vector.distance = 0;
+    
+    for(int i = 1; i < 8; i++)
+        obstacle_vector = addVector(obstacle_vector, get_bearing_from_ir_array(reflected_sensor_data), reflected_sensor_data[i]);
+        
+    obstacle_vector.distance /= 8; // 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 && 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);
         
-        if(number_of_neighbours != 0)
+        // 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)
         {
-            average_heading /= number_of_neighbours;
-        
-            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
             
-            turn_to_bearing(average_heading + 180);
+            bt.putc(beacon_heading_byte);
         }
-        
-        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);
-        bt.putc(beacon_heading_byte);
-        
-//        out("robot_id: %d\n", robot_id);
     }
 }
 
-//Timer timer;
-//
-//void flocking_program()
-//{    
-//    // Do something on the first run of a program
-//    if(program_run_init == 1) {
-//        // Initialisation code goes here...
-//        
-//        timer.start();
-//
-//        program_run_init = 0;
-//    } 
-//
-//    // step_cycle is either zero or one; use this avoid estimating bearings on the cycle after a turn (as the results will be skewed by the turn)
-//    if(step_cycle == 0) {
-//        // Do something based on sensor data (eg turn)
-//    } else {
-//        // Do something ignoring sensor data (eg move, or nothing!)
-//    }
-//    
-//    // Separation
-//    // Alignment
-//    // Cohesion
-//    
-//    float left_motor_speed = 0.5;
-//    float right_motor_speed = 0.5;
-//    
-//    struct FloatVector obstacle_vector;
-//    obstacle_vector.angle = 0;
-//    obstacle_vector.distance = 0;
-//    
-//    for(int i = 1; i < 8; i++)
-//        obstacle_vector = addVector(obstacle_vector, get_bearing_from_ir_array(reflected_sensor_data), reflected_sensor_data[i]);
-//        
-//    obstacle_vector.distance /= 8; // Normalise
-//    
-//    struct FloatVector cohesion_vector;
-//    cohesion_vector.angle = 0;
-//    cohesion_vector.distance = 0;
-//    
-//    int number_of_neighbours = 0;
-//
-//    for(int i = 1; i < 8; i++)
-//    {
-//        if(robots_found[i])
-//        {
-//            number_of_neighbours++;
-//            cohesion_vector = addVector(cohesion_vector, robots_heading[i], robots_distance[i]);
-//        }
-//    }
-//    
-//    cohesion_vector.distance /= number_of_neighbours; // Normalise
-//    
-////    out("distance: %f, angle: %f\n", cohesion_vector.distance, cohesion_vector.angle);    
-//    //out("distance: %f, angle: %f\n", obstacle_vector.distance, obstacle_vector.angle);
-//    
-//    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 && robot_nearby == 0)
-//    if(obstacle_vector.distance > 150)
-//    {
-////        out("avoiding obstacle\n");
-//        
-//        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(cohesion_vector.distance > 400)
-////    {
-////        if(cohesion_vector.angle > 0)
-////            left_motor_speed = 0.25;
-////        else if(cohesion_vector.angle < 0)
-////            right_motor_speed = 0.25;
-////    }
-//    else if(cohesion_vector.distance > 0)
-//    {     
-////        int target_heading = (beacon_heading + flocking_heading) % 360;
-////        
-////        if(target_heading > 0)
-////            right_motor_speed = 0.25;
-////        else
-////            left_motor_speed = 0.25;
-//
-////    out("beacon: %d, flocking: %d, target: %d, mod: %d\n", beacon_heading, flocking_heading, beacon_heading + flocking_heading, (beacon_heading + flocking_heading) % 360);
-//       
-//        if(cohesion_vector.angle < 0)
-//            left_motor_speed = 0.25;
-//        else if(cohesion_vector.angle > 0)
-//            right_motor_speed = 0.25;
-//            
-//        // Set LEDs to green        
-//        set_leds(0xFF, 0x00);
-//        set_center_led(2, 1);
-//    }
-//    else
-//    {
-//        // Set LEDS to red
-//        set_leds(0x00, 0xFF);
-//        set_center_led(1, 1);
-//    }
-//    
-////    out("left_motor_speed: %f, right_motor_speed: %f\n", left_motor_speed, right_motor_speed);
-//    
-//    set_left_motor_speed(left_motor_speed);
-//    set_right_motor_speed(right_motor_speed);
-//}
-
 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 /// generic_program - Framework for building typical programs