Diff: programs.cpp

Revision:

23:e59040ac05c4

Parent:

22:fd75afdadfb2

Child:

24:31707fec20a5

--- a/programs.cpp	Sat Jan 16 12:01:49 2016 +0000
+++ b/programs.cpp	Wed Jan 20 14:55:38 2016 +0000
@@ -762,17 +762,31 @@
     
     cohesion_vector.distance /= number_of_neighbours; // Normalise
     
-    float left_motor_speed = 0.5;
-    float right_motor_speed = 0.5;
+//    float left_motor_speed = 0.5;
+//    float right_motor_speed = 0.5;
+
+    float left_motor_speed;
+    float right_motor_speed;
+    
+    int target_heading = 0;
+    int angles = 0;
     
     struct FloatVector obstacle_vector;
     obstacle_vector.angle = 0;
     obstacle_vector.distance = 0;
     
+    int sensors_activated = 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]);
+    {
+        if(reflected_sensor_data[i] > 0)
+        {
+            obstacle_vector = addVector(obstacle_vector, get_bearing_from_ir_array(reflected_sensor_data), reflected_sensor_data[i]);
+            sensors_activated++;
+        }
+    }
         
-    obstacle_vector.distance /= 8; // Normalise
+    obstacle_vector.distance /= sensors_activated; // Normalise
     
     char robot_nearby = 0;
     
@@ -788,31 +802,110 @@
         }
     }
     
-    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)
+    if(obstacle_vector.distance > 150)
     {
-        average_heading /= number_of_flocking_headings; // Should this include self?
-        out("average heading: %d\n", average_heading);
-        
-        // 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;
+        target_heading += obstacle_vector.angle + 180;
+        angles++;
+    }
+    
+    if(cohesion_vector.distance > 0)
+    {
+        target_heading += cohesion_vector.angle;
+        angles++;
     }
     
-    set_left_motor_speed(left_motor_speed);
-    set_right_motor_speed(right_motor_speed);
+//    if(obstacle_vector.distance > 150 && number_of_neighbours == 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)
+//    {
+        if(number_of_flocking_headings > 0)
+        {
+            average_heading /= number_of_flocking_headings;
+            
+            //out("beacon heading: %d\n", beacon_heading);
+//            out("average heading: %d\n", average_heading);
+//            out("difference: %d\n", beacon_heading - average_heading);
+
+            
+
+            int relative_flocking_heading = beacon_heading - average_heading;
+            
+            target_heading += relative_flocking_heading;
+            angles++;
+
+//            if(relative_flocking_heading > 0)
+//            {
+//                right_motor_speed = -0.25;
+//                left_motor_speed = 0.25;
+//            }
+//            else if(relative_flocking_heading < 0)
+//            {
+//                right_motor_speed = 0.25;
+//                left_motor_speed = -0.25;
+//            }
+            
+//            if(relative_flocking_heading > 22.5)
+//            {
+//                right_motor_speed = -0.5;
+//                left_motor_speed = 0.5;
+//            }
+//            else if(relative_flocking_heading < -22.5)
+//            {
+//                right_motor_speed = 0.5;
+//                left_motor_speed = -0.5;
+//            }
+        }
+        
+        if(angles > 0)
+            target_heading /= angles; // Calculate average
+        
+        if(target_heading > 180)
+            target_heading -= 360;
+        if(target_heading < -180)
+            target_heading += 360;
+        
+        if(step_cycle == 0)
+            time_based_turn_degrees(1, target_heading, 1);
+        else
+            forward(0.25);
+        
+        
+//        if(cohesion_vector.angle > 22.5)
+//            right_motor_speed *= -1;
+//        else if(cohesion_vector.angle < -22.5)
+//            left_motor_speed *= -1;
+//    }
     
-    if(step_cycle == 0)
-    {
+//    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);
+//        
+//        // 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)
         {
@@ -821,7 +914,7 @@
             
             bt.putc(beacon_heading_byte);
         }
-    }
+//    }
 }
 
 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////