Diff: programs.cpp

Revision:

17:da524989b637

Parent:

16:976a1d0ea897

Child:

18:5921c1853e8a

--- a/programs.cpp	Fri Oct 30 12:15:54 2015 +0000
+++ b/programs.cpp	Thu Jan 07 17:58:07 2016 +0000
@@ -724,6 +724,128 @@
     }
 }
 
+/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/// 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();
+
+        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