Diff: programs.cpp

Revision:

18:5921c1853e8a

Parent:

17:da524989b637

Child:

19:77f7089dca87

--- a/programs.cpp	Thu Jan 07 17:58:07 2016 +0000
+++ b/programs.cpp	Fri Jan 08 19:10:52 2016 +0000
@@ -727,125 +727,141 @@
 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 /// 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;
+{
+    int average_heading = 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;
-            }
-        }
-    }
+        average_heading += flocking_headings[i];
+        
+    average_heading /= 8;
+    
+    out("average heading: %d\n", average_heading);
     
-    //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)
+    out("sending beacon heading: %d\n", beacon_heading);
+    bt.putc(beacon_heading);
+    //bt.printf("%d", beacon_heading);
+}
+
+//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(cohesion_vector.angle > 0)
-//            left_motor_speed = 0.25;
-//        else if(cohesion_vector.angle < 0)
-//            right_motor_speed = 0.25;
+//        if(robots_found[i])
+//        {
+//            number_of_neighbours++;
+//            cohesion_vector = addVector(cohesion_vector, robots_heading[i], robots_distance[i]);
+//        }
 //    }
-    else if(cohesion_vector.distance > 0)
-    {     
-//        int target_heading = (beacon_heading + flocking_heading) % 360;
+//    
+//    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(target_heading > 0)
-//            right_motor_speed = 0.25;
-//        else
+//        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;
-
-//    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);
-}
+//        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