Diff: programs.cpp

Revision:

16:976a1d0ea897

Parent:

14:f623db1e6184

Child:

19:b5788427db67

--- a/programs.cpp	Wed Oct 28 11:42:25 2015 +0000
+++ b/programs.cpp	Fri Oct 30 12:15:54 2015 +0000
@@ -560,6 +560,170 @@
 
 
 
+/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/// tag_game_program
+
+enum tag_game_role {hunter, hunted};
+enum tag_game_role role;
+int tag_distance = 500;
+char hunters[8]; // Memory of which robots have come within tag distance - assumed to be hunters
+Timeout hunter_timeout; // Timeout before robots switch back to being hunted
+char initial_hunter = 2; // Robot with this ID is permanently a hunter
+
+// Resets hunter robots to hunted after timeout
+void role_reset()
+{
+    role = hunted;
+    set_program_info("HUNTED");
+    // Clear memory of hunters
+    for(int i = 0; i < 8; i++)
+        hunters[i] = 0;
+}
+
+void tag_game_program()
+{
+    // Initialisation
+    if(program_run_init == 1) {
+        
+        // Set robot roles
+        if(robot_id == initial_hunter){
+            role = hunter;
+            set_program_info("FIRST HUNTER");
+        }
+        else {
+            role = hunted;
+            set_program_info("HUNTED");
+        }
+        // Clear memory of hunters    
+        for(int i = 0; i < 8; i++)
+            hunters[i] = 0;
+
+        program_run_init = 0;
+    }
+    
+    // Bias forward movement
+    float left_motor_speed = 0.5;
+    float right_motor_speed = 0.5;
+    
+    // Check the front sensors for obstacles
+    if(reflected_sensor_data[0] > obstacle_avoidance_threshold ||
+       reflected_sensor_data[1] > obstacle_avoidance_threshold ||
+       reflected_sensor_data[6] > obstacle_avoidance_threshold ||
+       reflected_sensor_data[7] > obstacle_avoidance_threshold)
+    {                
+        // Ignore the rear sensors when calculating the heading
+        reflected_sensor_data[2] = 0;
+        reflected_sensor_data[3] = 0;
+        reflected_sensor_data[4] = 0;
+        reflected_sensor_data[5] = 0;
+
+        // Get heading of sensed obstacle
+        int heading = get_bearing_from_ir_array(reflected_sensor_data);
+        
+        // Turn in opposite direction
+        if(heading > 0 && heading < 90)
+        {
+            left_motor_speed -= 0.75;
+            right_motor_speed += 0.5;
+        }
+        else if(heading < 0 && heading > -90)
+        {
+            left_motor_speed += 0.5;
+            right_motor_speed -= 0.75;
+        }
+        
+        set_left_motor_speed(left_motor_speed);
+        set_right_motor_speed(right_motor_speed);
+        
+        // Return early - obstacle avoidance is the top priority
+        return;
+    }
+        
+    int closest_robot = -1;
+    unsigned short shortest_distance = 0;
+
+    // Check whether there are any other robots within range
+    for(int i = 0; i < 8; i++)
+    {
+        if(robots_found[i])
+        {
+            if(robots_distance[i] > shortest_distance)
+            {
+                shortest_distance = robots_distance[i];
+                closest_robot = i;
+            }
+        }
+    }
+    
+    // If the closest robot is within tag distance, this robot has been tagged
+    if(shortest_distance > tag_distance)
+    {
+        // Switch role to hunter
+        if(role == hunted)
+        {
+            role = hunter;
+            set_program_info("NEW HUNTER");
+            // Reset to hunted after 10 seconds
+            hunter_timeout.attach(&role_reset, 10);
+        }
+        
+        // Keep a record of which robot tagged them, so hunters do not chase hunters
+        // Unless they are the initial hunter, who is aggresive towards everyone
+        if(robot_id != initial_hunter)
+            hunters[closest_robot] = 1;
+    }
+        
+    if(role == hunter)
+    {
+        // Set LEDS to red
+        set_leds(0x00, 0xFF);
+        set_center_led(1, 1);
+        
+        if(closest_robot >= 0 && !hunters[closest_robot]) // Ignore other hunters
+        {
+            // Turn towards closest hunted robot (unless it is straight ahead, or behind)            
+            if(robots_heading[closest_robot] > 22.5 && robots_heading[closest_robot] < 90)
+            {
+                left_motor_speed += 0.5;
+                right_motor_speed -= 0.5;
+            }
+            else if(robots_heading[closest_robot] < -22.5 && robots_heading[closest_robot] > -90)
+            {
+                left_motor_speed -= 0.5;
+                right_motor_speed += 0.5;
+            }
+        }
+        
+        set_left_motor_speed(left_motor_speed);
+        set_right_motor_speed(right_motor_speed);
+    }
+    else // role == hunted
+    {
+        // Set LEDs to green        
+        set_leds(0xFF, 0x00);
+        set_center_led(2, 1);
+        
+        // Avoid everyone
+        if(closest_robot >= 0)
+        {
+            // Turn away from closest robot
+            if(robots_heading[closest_robot] >= 0 && robots_heading[closest_robot] < 90)
+            {
+                left_motor_speed -= 0.5;
+                right_motor_speed += 0.5;
+            }
+            else if(robots_heading[closest_robot] < 0 && robots_heading[closest_robot] > -90)
+            {
+                left_motor_speed += 0.5;
+                right_motor_speed -= 0.5;
+            }
+        }
+        
+        set_left_motor_speed(left_motor_speed);
+        set_right_motor_speed(right_motor_speed);
+    }
+}
+
 
 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 /// generic_program - Framework for building typical programs