Diff: programs.cpp

Revision:

25:b5742873e02b

Parent:

24:31707fec20a5

Child:

26:3869ae5eddd7

--- a/programs.cpp	Wed Jan 20 14:58:24 2016 +0000
+++ b/programs.cpp	Mon Jan 25 15:58:58 2016 +0000
@@ -737,6 +737,15 @@
     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(robots_found[i])
@@ -753,62 +762,129 @@
         }
     }
     
+//    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;
     
-    for(int i = 1; i < 8; i++)
+//    out("reflected_sensor_data: ");
+    
+    for(int i = 0; i < 8; i++)
     {
-        if(reflected_sensor_data[i] > 0)
-        {
+//        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");
         
     obstacle_vector.distance /= sensors_activated; // Normalise
     
-    if(obstacle_vector.distance > 150)
+//    out("obstacle_vector.distance: %f, sensors_activated: %d\n", obstacle_vector.distance, sensors_activated);
+    
+    //if(obstacle_vector.distance > 150)
+    if(sensors_activated > 0)
     {
-        target_heading += obstacle_vector.angle + 180;
+        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;
         angles++;
+        
+        out("obstacle_vector.angle: %f, target_heading: %d\n", obstacle_vector.angle, target_heading);
     }
     
-    if(cohesion_vector.distance > 0)
+//    if(cohesion_vector.distance > 0)
+    if(number_of_neighbours > 0)
     {
-        target_heading += cohesion_vector.angle;
+        int cohesion_angle = cohesion_vector.angle;
+        
+        if(cohesion_angle > 180)
+            cohesion_angle -= 360;
+        if(cohesion_angle < -180)
+            cohesion_angle += 360;
+        
+        target_heading += cohesion_angle;
         angles++;
     }
     
     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;
         
+        if(relative_flocking_heading > 180)
+            relative_flocking_heading -= 360;
+        if(relative_flocking_heading < -180)
+            relative_flocking_heading += 360;
+        
         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;
+//    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;
     
-    if(step_cycle == 0)
-        time_based_turn_degrees(1, target_heading, 1);
-    else
-        forward(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;
+    else if(target_heading < -22.5)
+        left_motor_speed *= -1;
+        
+    set_left_motor_speed(left_motor_speed);
+    set_right_motor_speed(right_motor_speed);
 
     // Only transmit beacon heading if the beacon is visible
     if(beacon_found)