programs.cpp

00001 /// PsiSwarm Beautiful Meme Project Source Code
00002 /// Version 0.2
00003 /// James Hilder, Alan Millard, Homero Elizondo, Jon Timmis
00004 /// University of York
00005 
00006 // programs.cpp - Various PsiSwarm Programs for Beautiful Meme Project
00007 
00008 #include "main.h"
00009 
00010 int obstacle_avoidance_threshold = 300;
00011 int robot_avoidance_threshold = 2000;
00012 
00013 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
00014 /// head_to_bearing_program
00015 char was_turning = 0;
00016 
00017 ///The head to bearing program moves towards a given bearing (eg 0 for the beacon or 180 for the opposite wall) and keeps going until an obstacle is detected in front of it
00018 void head_to_bearing_program(int target_bearing)
00019 {
00020     if(step_cycle == 0 || was_turning == 0) {
00021         // Check if we are heading in the right bearing (+- 25 degrees)
00022         int current_bearing = (360 - beacon_heading) % 360;
00023         // Current bearing should range from 0 to 359; target_bearing likewise; check the are within 25 degrees of each other
00024         char bearing_ok = 0;
00025         int lower_bound = target_bearing - 25;
00026         int upper_bound = target_bearing + 25;
00027         if(lower_bound < 0) {
00028             if(current_bearing > (lower_bound + 360) || current_bearing < upper_bound) bearing_ok = 1;
00029         } else if(upper_bound > 359) {
00030             if(current_bearing > lower_bound || current_bearing < (upper_bound - 360)) bearing_ok = 1;
00031         } else if(current_bearing > lower_bound && current_bearing < upper_bound) bearing_ok = 1;
00032         // Check if there is an obstacle in front of us
00033         if((reflected_sensor_data[7] > 1000 || reflected_sensor_data[0] > 1000) && bearing_ok == 1) target_reached = 1;
00034         else {
00035             // Now move forward if we are facing correct bearing, otherwise turn
00036             if(bearing_ok == 1) {
00037                 //Check if anything is in front of us to determine speed - if it is, move slowly
00038                 int t_time = 6 * BEACON_PERIOD;
00039                 float t_speed = 1.0;
00040                 if(reflected_sensor_data[7] > 150 || reflected_sensor_data[0] > 150) {
00041                     t_time = 4 * BEACON_PERIOD;
00042                     t_speed = 0.6;
00043                 }
00044                 if(reflected_sensor_data[7] > 300 || reflected_sensor_data[0] > 300) {
00045                     t_time = 3 * BEACON_PERIOD;
00046                     t_speed = 0.4;
00047                 }
00048                 if(reflected_sensor_data[7] > 500 || reflected_sensor_data[0] > 500) {
00049                     t_time = 2 * BEACON_PERIOD;
00050                     t_speed = 0.2;
00051                 }
00052                 time_based_forward(t_speed,t_time,0);
00053                 was_turning = 0;
00054             } else {
00055                 turn_to_bearing(target_bearing);
00056                 was_turning = 1;
00057             }
00058         }
00059     }
00060 }
00061 
00062 
00063 
00064 
00065 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
00066 /// recharging_program
00067 
00068 char recharge_power_check_count = 0;
00069 char battery_low_count = 0;
00070 
00071 void recharging_program()
00072 {
00073     switch(recharging_state) {
00074         case 0:
00075             // We are not currently recharging, check the battery state
00076             if(get_battery_voltage() < battery_low_threshold) {
00077                 // Battery is below low threshold
00078                 battery_low_count ++;
00079                 // We don't immediately start recharging routine in case as battery level can fluctuate a little due to load; if we get a low value for 4 continuous timesteps we start recharging routine
00080                 if(battery_low_count > 3) {
00081                     // Set recharging state to 'looking for charger'
00082                     recharging_state = 1;
00083                     strcpy(prog_name,"CHARGING PROGRAM");
00084                     set_program_info("HEAD TO BEACON");
00085                 }
00086             } else battery_low_count = 0;
00087             break;
00088             // State 1:  Head to beacon [as this is where battery charger is]
00089         case 1:
00090             target_reached = 0;
00091             head_to_bearing_program(0);
00092             if(target_reached == 1) {
00093                 recharging_state = 2;
00094                 set_program_info("TURN 90 DEGREES");
00095             }
00096             break;
00097             // Stage 2:  Turn 90 degrees to align with charging pads
00098         case 2:
00099             disable_ir_emitters = 1;
00100             time_based_turn_degrees(0.8, 70.0, 1);
00101             recharge_power_check_count = 0;
00102             recharging_state = 3;
00103             break;
00104             // Stage 3:  Wait for turn to complete
00105         case 3:
00106             if (time_based_motor_action != 1) {
00107                 recharging_state = 4;
00108                 set_program_info("CHECK FOR POWER");
00109             }
00110             break;
00111             // Stage 4:  Check if charging
00112         case 4:
00113             recharge_power_check_count++;
00114             if(get_dc_status() == 1) {
00115                 recharging_state = 5;
00116             } else {
00117                 if(recharge_power_check_count < 10)recharging_state = 6;
00118                 else {
00119                     recharging_state = 7;
00120                     set_program_info("NO POWER - RETRY");
00121                 }
00122             }
00123             break;
00124             // Stage 5:  Charging.  Wait until threshold voltage exceeded
00125         case 5:
00126             if(get_battery_voltage() > battery_high_threshold) {
00127                 set_program_info("LEAVE CHARGER");
00128                 recharging_state = 7;
00129             } else {
00130                 char b_voltage[16];
00131                 sprintf(b_voltage,"CHARGE: %1.3fV",get_battery_voltage());
00132                 set_program_info(b_voltage);
00133             }
00134             break;
00135             // Stage 6:  We didn't find power, keep turning a couple of degrees at a time a recheck
00136         case 6:
00137             time_based_turn_degrees(0.5,4,1);
00138             recharging_state = 4;
00139             break;
00140             // Stage 7:  Charge may be finished.  Turn 90 degrees then move away and resume previous program
00141         case 7:
00142             time_based_turn_degrees(0.8, 90.0, 1);
00143             recharging_state = 8;
00144             break;
00145 
00146             // Stage 8:  Wait for turn to complete
00147         case 8:
00148             if (time_based_motor_action != 1) recharging_state = 9;
00149             break;
00150             // Stage 9:  Move away
00151         case 9:
00152             time_based_forward(0.5, 1000000, 1);
00153             recharging_state = 10;
00154             break;
00155             // Stage 10:  Wait for move to complete
00156         case 10:
00157             if (time_based_motor_action != 1) recharging_state = 11;
00158             break;
00159             // Stage 11:  Check if battery is below low threshold; if it is, start over, else end charge cycle
00160         case 11:
00161             disable_ir_emitters = 0;
00162             if (get_battery_voltage() < battery_low_threshold) {
00163                 recharging_state = 1;
00164             } else {
00165                 recharging_state = 0;
00166                 //Restore name of old program on display
00167                 set_program(main_program_state);
00168             }
00169             break;
00170     }
00171 }
00172 
00173 
00174 
00175 
00176 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
00177 /// curved_walk_with_interaction_program (Alan's Random Walk\Obstacle Avoid and Robot Interaction Program)
00178 
00179 enum random_walk_state {random_walk, turn_towards, interact, turn_away, avoid_obstacle};
00180 enum random_walk_state internal_state = random_walk;
00181 char action_timeout = 0;
00182 char interaction_timeout = 0;
00183 char random_walk_timeout = 0;
00184 float previous_left_motor_speed = 0.5;
00185 float previous_right_motor_speed = 0.5;
00186 
00187 void curved_random_walk_with_interaction_program()
00188 {
00189     if(internal_state == random_walk) {
00190         if(interaction_timeout < 4)
00191             interaction_timeout++;
00192 
00193         int closest_robot = -1;
00194         unsigned short shortest_distance = 0;
00195 
00196         // Check whether there are any other robots within range
00197         for(int i = 0; i < 8; i++) {
00198             if(robots_found[i]) {
00199                 if(robots_distance[i] > shortest_distance) {
00200                     shortest_distance = robots_distance[i];
00201                     closest_robot = i;
00202                 }
00203             }
00204         }
00205 
00206         // Turn towards the closest robot
00207         if(closest_robot >= 0 && shortest_distance > 300 && interaction_timeout >= 4) {
00208             time_based_turn_degrees(1, robots_heading[closest_robot], 1);
00209 
00210             action_timeout = 0;
00211             internal_state = turn_towards;
00212             char temp_message[17];
00213             sprintf(temp_message,"FACE ROBOT %d",closest_robot);
00214             set_program_info(temp_message);
00215         } else { // Otherwise, do a random walk
00216             // Check the front sensors for obstacles
00217             if(reflected_sensor_data[0] > obstacle_avoidance_threshold ||
00218                     reflected_sensor_data[1] > obstacle_avoidance_threshold ||
00219                     reflected_sensor_data[6] > obstacle_avoidance_threshold ||
00220                     reflected_sensor_data[7] > obstacle_avoidance_threshold) {
00221                 // Ignore the rear sensors when calculating the heading
00222                 reflected_sensor_data[2] = 0;
00223                 reflected_sensor_data[3] = 0;
00224                 reflected_sensor_data[4] = 0;
00225                 reflected_sensor_data[5] = 0;
00226 
00227                 // Turn 180 degrees away from the sensed obstacle
00228                 int heading = get_bearing_from_ir_array (reflected_sensor_data) + 180;
00229 
00230                 // Normalise
00231                 heading %= 360;
00232 
00233                 if(heading < -180)
00234                     heading += 360;
00235 
00236                 if(heading > 180)
00237                     heading -= 360;
00238                 set_program_info("AVOID OBSTACLE");
00239                 time_based_turn_degrees(1, heading, 1);
00240 
00241                 action_timeout = 0;
00242                 internal_state = turn_away;
00243             } else {
00244                 // Change motor speeds every 1s
00245                 if(random_walk_timeout >= 2) {
00246                     float random_offset = (((float) rand() / (float) RAND_MAX) - 0.5) * 0.5;
00247 
00248                     float left_motor_speed = previous_left_motor_speed - random_offset;
00249                     float right_motor_speed = previous_right_motor_speed + random_offset;
00250 
00251                     float threshold = 0.25;
00252 
00253                     if(left_motor_speed < threshold)
00254                         left_motor_speed = threshold;
00255 
00256                     if(right_motor_speed < threshold)
00257                         right_motor_speed = threshold;
00258 
00259                     set_left_motor_speed(left_motor_speed);
00260                     set_right_motor_speed(right_motor_speed);
00261 
00262                     random_walk_timeout = 0;
00263                 }
00264 
00265                 random_walk_timeout++;
00266             }
00267         }
00268     } else if(internal_state == turn_towards) {
00269         if(action_timeout < 4)
00270             action_timeout++;
00271         else {
00272             set_program_info("SAY HELLO");
00273             vibrate();
00274 
00275             action_timeout = 0;
00276             internal_state = interact;
00277         }
00278     } else if(internal_state == interact) {
00279         if(action_timeout < 4)
00280             action_timeout++;
00281         else {
00282             set_program_info("TURN AROUND");
00283             time_based_turn_degrees(1, 180, 1);
00284 
00285             action_timeout = 0;
00286             internal_state = turn_away;
00287         }
00288 
00289     } else if(internal_state == turn_away) {
00290         if(action_timeout < 4)
00291             action_timeout++;
00292         else {
00293             set_program_info("RANDOM WALK");
00294             interaction_timeout = 0;
00295             internal_state = random_walk;
00296         }
00297     } else if(internal_state == avoid_obstacle) {
00298         if(action_timeout < 4)
00299             action_timeout++;
00300         else
00301             set_program_info("RANDOM WALK");
00302         internal_state = random_walk;
00303     }
00304 }
00305 
00306 
00307 
00308 
00309 
00310 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
00311 /// straight_random_walk_with_interaction_program
00312 
00313 void straight_random_walk_with_interaction_program()
00314 {
00315 
00316 }
00317 
00318 
00319 
00320 
00321 
00322 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
00323 /// find_space_program
00324 
00325 char prog_debug = 1    ;
00326 float target_wheel_speed;
00327 int random_walk_bearing;
00328 
00329 void find_space_program(char bidirectional)
00330 {
00331     // The find_space_program is a continuous turn-move vector program
00332 
00333     if(program_run_init == 1) {
00334         // Setup the LEDs to red
00335         set_leds(0x00,0xFF);
00336         set_center_led(1,1);
00337         program_run_init = 0;
00338         random_walk_bearing = rand() % 360;
00339     }
00340 
00341     // When step_cycle = 0 we calculate a vector to move to and a target distance
00342     if(step_cycle == 0) {
00343         struct FloatVector target_vector;
00344         target_vector.angle = 0;
00345         target_vector.distance = 0;
00346         // Check for near robots within range
00347         for(int i = 1; i < 8; i++) {
00348             if(robots_found[i]) {
00349                 int res_bearing = robots_heading[i];
00350                 res_bearing += 180;
00351                 if(res_bearing > 180) res_bearing -= 360;   
00352                 target_vector = addVector(target_vector,res_bearing,robots_distance[i]);
00353                 if(prog_debug) out("Repelled from robot %d at bearing %d, strength %d,  resultant b:%f, d:%f\n",i,robots_heading[i],robots_distance[i],target_vector.angle,target_vector.distance);
00354             }
00355         }
00356         if(target_vector.angle!=0){
00357            set_leds(0xFF,0xFF);
00358            set_center_led(3,1);  
00359         }
00360         // Check for obstacles
00361         char obstacle = 0;
00362         int peak_strength = 0;
00363         for(int i=0; i<8; i++){
00364             if(reflected_sensor_data[i] > peak_strength) peak_strength = reflected_sensor_data[i];
00365             if(peak_strength > obstacle_avoidance_threshold) obstacle = 1;
00366         }
00367         if(obstacle){  
00368            //Choose new random walk bearing
00369            set_leds(0x00,0xFF);
00370            set_center_led(1,1);
00371            random_walk_bearing = rand() % 360;
00372            int obstacle_bearing = get_bearing_from_ir_array (reflected_sensor_data);
00373            int obs_bearing = obstacle_bearing + 180;
00374            if(obs_bearing > 180) obs_bearing -= 360;
00375            target_vector = addVector(target_vector,obs_bearing,peak_strength);
00376            if(prog_debug) out("Repelled from obstacle at bearing %d, strength %d,  resultant b:%f, d:%f\n",obstacle_bearing,peak_strength,target_vector.angle,target_vector.distance);
00377         }   
00378         
00379         if(target_vector.angle == 0 && target_vector.distance == 0){
00380             set_leds(0xFF,0x00);
00381             set_center_led(2,1);
00382             random_walk_bearing += 180;
00383             if(random_walk_bearing > 360) random_walk_bearing -= 360;
00384             target_vector.distance = 100;   
00385             int current_bearing = 360 - beacon_heading;
00386             //Now work out turn needed to face intended heading
00387             int target_turn = (random_walk_bearing - current_bearing) % 360;
00388             if(target_turn > 180) target_turn -= 360;
00389             if(target_turn < -180) target_turn += 360;
00390             target_vector.angle = target_turn;
00391             if(prog_debug) out("Random walk bearing:%d Current:%d Target:%f\n",random_walk_bearing,current_bearing,target_vector.angle);
00392         }
00393         char wheel_direction = 1;
00394         if(bidirectional){
00395             // Allow reverse wheel direction
00396             if(target_vector.angle < -90) {target_vector.angle += 180; wheel_direction = 0;}
00397             else if(target_vector.angle > 90) {target_vector.angle -= 180; wheel_direction = 0;}
00398         }
00399         //Now turn to angle
00400         float maximum_turn_angle = get_maximum_turn_angle(BEACON_PERIOD*10);
00401         if(target_vector.angle < 0){
00402             if(target_vector.angle < -maximum_turn_angle){
00403                    target_vector.angle = -maximum_turn_angle;
00404                    target_vector.distance = 100;
00405             }    
00406         }else{
00407             if(target_vector.angle > maximum_turn_angle){
00408                    target_vector.angle = maximum_turn_angle;
00409                    target_vector.distance = 100;
00410             } 
00411         }
00412         //Set the wheel speed for next action
00413         if(target_vector.distance < 120) target_wheel_speed = 0.25;
00414         else if(target_vector.distance < 240) target_wheel_speed = 0.35;
00415         else if(target_vector.distance < 480) target_wheel_speed = 0.45;
00416         else if(target_vector.distance < 960) target_wheel_speed = 0.55;
00417         else target_wheel_speed = 0.65;
00418         if(wheel_direction == 0) target_wheel_speed = 0-target_wheel_speed;
00419         
00420         //Now turn...
00421         time_based_turn_degrees(1, (int) target_vector.angle, 1);
00422     } else time_based_forward(target_wheel_speed,BEACON_PERIOD*6,0);
00423 }
00424 
00425 
00426 
00427 
00428 
00429 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
00430 /// clustering_program
00431 
00432 
00433 void clustering_program(char invert, char bidirectional)
00434 {
00435     // The clustering program is a continuous turn-move vector program
00436     // In normal mode (invert = 0) it is attracted to same-group robots and repels opposite-group, walls and very close same-group robots
00437     // In invert mode (invert = 1) it avoids same-group and is attracted to opposite group
00438 
00439     // Store the robot group: even robots (0) are green, odd robots (1) are red
00440     char group = robot_id % 2;
00441 
00442     if(program_run_init == 1) {
00443         // Setup the LEDs based on robot_id
00444         if(group == 0) {
00445             set_leds(0xFF,0x00);
00446             set_center_led(2,1);
00447         } else {
00448             set_leds(0x00,0xFF);
00449             set_center_led(1,1);
00450         }
00451         program_run_init = 0;
00452         random_walk_bearing = rand() % 360;
00453     }
00454 
00455     // When step_cycle = 0 we calculate a vector to move to and a target distance
00456     if(step_cycle == 0) {
00457         char avoiding_friend = 0;
00458         struct FloatVector target_vector;
00459         target_vector.angle = 0;
00460         target_vector.distance = 0;
00461         // Check for near robots within range
00462         for(int i = 1; i < 8; i++) {
00463             if(robots_found[i]) {
00464                 // Determine if the robot is an attractor or a repellor
00465                 char attract = 0;
00466                 if((invert==0 && ((i%2) == group)) || (invert==1 && ((i%2) != group))) attract = 1;
00467                 // Avoid very close attractors to stop collisions
00468                 if(attract==1 && robots_distance[i] > robot_avoidance_threshold) {attract = 0; avoiding_friend = 1;}
00469                 int res_bearing = robots_heading[i];
00470                 if(attract==0){
00471                     res_bearing += 180;
00472                     if(res_bearing > 180) res_bearing -= 360;   
00473                 }
00474                 target_vector = addVector(target_vector,res_bearing,robots_distance[i]);
00475                 if(prog_debug) {
00476                     if(attract) {
00477                         out("Attracted to robot %d at bearing %d, strength %d,  resultant b:%f, d:%f\n",i,robots_heading[i],robots_distance[i],target_vector.angle,target_vector.distance);
00478                     } else {
00479                         out("Repelled from robot %d at bearing %d, strength %d,  resultant b:%f, d:%f\n",i,robots_heading[i],robots_distance[i],target_vector.angle,target_vector.distance);
00480                     }
00481                 }
00482             }
00483         }
00484         
00485         // Check for obstacles
00486         char obstacle = 0;
00487         int peak_strength = 0;
00488         for(int i=0; i<8; i++){
00489             if(reflected_sensor_data[i] > peak_strength) peak_strength = reflected_sensor_data[i];
00490             if(peak_strength > obstacle_avoidance_threshold) obstacle = 1;
00491         }
00492         if(obstacle){  
00493            //Choose new random walk bearing
00494            random_walk_bearing = rand() % 360;
00495            int obstacle_bearing = get_bearing_from_ir_array (reflected_sensor_data);
00496            int obs_bearing = obstacle_bearing + 180;
00497            if(obs_bearing > 180) obs_bearing -= 360;
00498            target_vector = addVector(target_vector,obs_bearing,peak_strength);
00499            if(prog_debug) out("Repelled from obstacle at bearing %d, strength %d,  resultant b:%f, d:%f\n",obstacle_bearing,peak_strength,target_vector.angle,target_vector.distance);
00500         }   
00501         if(target_vector.angle == 0 && target_vector.distance == 0){
00502             //I have nothing attracting me so persist with random walk: with a 2% chance pick new bearing
00503             if(rand() % 100 > 97) random_walk_bearing = rand() % 360;
00504             target_vector.distance = 100;   
00505             int current_bearing = 360 - beacon_heading;
00506             //Now work out turn needed to face intended heading
00507             int target_turn = (random_walk_bearing - current_bearing) % 360;
00508             if(target_turn > 180) target_turn -= 360;
00509             if(target_turn < -180) target_turn += 360;
00510             target_vector.angle = target_turn;
00511             if(prog_debug) out("Random walk bearing:%d Current:%d Target:%f\n",random_walk_bearing,current_bearing,target_vector.angle);
00512         }
00513         char wheel_direction = 1;
00514         if(bidirectional){
00515             // Allow reverse wheel direction
00516             if(target_vector.angle < -90) {target_vector.angle += 180; wheel_direction = 0;}
00517             else if(target_vector.angle > 90) {target_vector.angle -= 180; wheel_direction = 0;}
00518         }
00519         //Now turn to angle
00520         float maximum_turn_angle = get_maximum_turn_angle(BEACON_PERIOD*10);
00521         if(target_vector.angle < 0){
00522             if(target_vector.angle < -maximum_turn_angle){
00523                    target_vector.angle = -maximum_turn_angle;
00524                    target_vector.distance = 100;
00525             }    
00526         }else{
00527             if(target_vector.angle > maximum_turn_angle){
00528                    target_vector.angle = maximum_turn_angle;
00529                    target_vector.distance = 100;
00530             } 
00531         }
00532         if(avoiding_friend) target_vector.distance = 100;
00533         //Set the wheel speed for next action
00534         if(target_vector.distance < 120) target_wheel_speed = 0.25;
00535         else if(target_vector.distance < 240) target_wheel_speed = 0.35;
00536         else if(target_vector.distance < 480) target_wheel_speed = 0.5;
00537         else if(target_vector.distance < 960) target_wheel_speed = 0.65;
00538         else target_wheel_speed = 0.85;
00539         if(wheel_direction == 0) target_wheel_speed = 0-target_wheel_speed;
00540         
00541         //Now turn...
00542         time_based_turn_degrees(1, (int) target_vector.angle, 1);
00543     } else time_based_forward(target_wheel_speed,BEACON_PERIOD*7,0);
00544 }
00545 
00546 
00547 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
00548 /// stop_program - Pauses robot
00549 
00550 void stop_program()
00551 {
00552     if(program_run_init == 1) {
00553         save_led_states();
00554         set_leds(0,0);
00555         set_center_led(0,0);
00556         stop();
00557         program_run_init = 0;
00558     }
00559 }
00560 
00561 
00562 
00563 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
00564 /// tag_game_program
00565 
00566 enum tag_game_role {hunter, hunted};
00567 enum tag_game_role role;
00568 int tag_distance = 500;
00569 char hunters[8]; // Memory of which robots have come within tag distance - assumed to be hunters
00570 Timeout hunter_timeout; // Timeout before robots switch back to being hunted
00571 char initial_hunter = 2; // Robot with this ID is permanently a hunter
00572 
00573 // Resets hunter robots to hunted after timeout
00574 void role_reset()
00575 {
00576     role = hunted;
00577     set_program_info("HUNTED");
00578     // Clear memory of hunters
00579     for(int i = 0; i < 8; i++)
00580         hunters[i] = 0;
00581 }
00582 
00583 void tag_game_program()
00584 {
00585     // Initialisation
00586     if(program_run_init == 1) {
00587         
00588         // Set robot roles
00589         if(robot_id == initial_hunter){
00590             role = hunter;
00591             set_program_info("FIRST HUNTER");
00592         }
00593         else {
00594             role = hunted;
00595             set_program_info("HUNTED");
00596         }
00597         // Clear memory of hunters    
00598         for(int i = 0; i < 8; i++)
00599             hunters[i] = 0;
00600 
00601         program_run_init = 0;
00602     }
00603     
00604     // Bias forward movement
00605     float left_motor_speed = 0.5;
00606     float right_motor_speed = 0.5;
00607     
00608     // Check the front sensors for obstacles
00609     if(reflected_sensor_data[0] > obstacle_avoidance_threshold ||
00610        reflected_sensor_data[1] > obstacle_avoidance_threshold ||
00611        reflected_sensor_data[6] > obstacle_avoidance_threshold ||
00612        reflected_sensor_data[7] > obstacle_avoidance_threshold)
00613     {                
00614         // Ignore the rear sensors when calculating the heading
00615         reflected_sensor_data[2] = 0;
00616         reflected_sensor_data[3] = 0;
00617         reflected_sensor_data[4] = 0;
00618         reflected_sensor_data[5] = 0;
00619 
00620         // Get heading of sensed obstacle
00621         int heading = get_bearing_from_ir_array(reflected_sensor_data);
00622         
00623         // Turn in opposite direction
00624         if(heading > 0 && heading < 90)
00625         {
00626             left_motor_speed -= 0.75;
00627             right_motor_speed += 0.5;
00628         }
00629         else if(heading < 0 && heading > -90)
00630         {
00631             left_motor_speed += 0.5;
00632             right_motor_speed -= 0.75;
00633         }
00634         
00635         set_left_motor_speed(left_motor_speed);
00636         set_right_motor_speed(right_motor_speed);
00637         
00638         // Return early - obstacle avoidance is the top priority
00639         return;
00640     }
00641         
00642     int closest_robot = -1;
00643     unsigned short shortest_distance = 0;
00644 
00645     // Check whether there are any other robots within range
00646     for(int i = 0; i < 8; i++)
00647     {
00648         if(robots_found[i])
00649         {
00650             if(robots_distance[i] > shortest_distance)
00651             {
00652                 shortest_distance = robots_distance[i];
00653                 closest_robot = i;
00654             }
00655         }
00656     }
00657     
00658     // If the closest robot is within tag distance, this robot has been tagged
00659     if(shortest_distance > tag_distance)
00660     {
00661         // Switch role to hunter
00662         if(role == hunted)
00663         {
00664             role = hunter;
00665             set_program_info("NEW HUNTER");
00666             // Reset to hunted after 10 seconds
00667             hunter_timeout.attach(&role_reset, 10);
00668         }
00669         
00670         // Keep a record of which robot tagged them, so hunters do not chase hunters
00671         // Unless they are the initial hunter, who is aggresive towards everyone
00672         if(robot_id != initial_hunter)
00673             hunters[closest_robot] = 1;
00674     }
00675         
00676     if(role == hunter)
00677     {
00678         // Set LEDS to red
00679         set_leds(0x00, 0xFF);
00680         set_center_led(1, 1);
00681         
00682         if(closest_robot >= 0 && !hunters[closest_robot]) // Ignore other hunters
00683         {
00684             // Turn towards closest hunted robot (unless it is straight ahead, or behind)            
00685             if(robots_heading[closest_robot] > 22.5 && robots_heading[closest_robot] < 90)
00686             {
00687                 left_motor_speed += 0.5;
00688                 right_motor_speed -= 0.5;
00689             }
00690             else if(robots_heading[closest_robot] < -22.5 && robots_heading[closest_robot] > -90)
00691             {
00692                 left_motor_speed -= 0.5;
00693                 right_motor_speed += 0.5;
00694             }
00695         }
00696         
00697         set_left_motor_speed(left_motor_speed);
00698         set_right_motor_speed(right_motor_speed);
00699     }
00700     else // role == hunted
00701     {
00702         // Set LEDs to green        
00703         set_leds(0xFF, 0x00);
00704         set_center_led(2, 1);
00705         
00706         // Avoid everyone
00707         if(closest_robot >= 0)
00708         {
00709             // Turn away from closest robot
00710             if(robots_heading[closest_robot] >= 0 && robots_heading[closest_robot] < 90)
00711             {
00712                 left_motor_speed -= 0.5;
00713                 right_motor_speed += 0.5;
00714             }
00715             else if(robots_heading[closest_robot] < 0 && robots_heading[closest_robot] > -90)
00716             {
00717                 left_motor_speed += 0.5;
00718                 right_motor_speed -= 0.5;
00719             }
00720         }
00721         
00722         set_left_motor_speed(left_motor_speed);
00723         set_right_motor_speed(right_motor_speed);
00724     }
00725 }
00726 
00727 
00728 // mock flocking program
00729 
00730 void mockFlocking()
00731 {
00732     float left_motor_speed = 0.5;
00733     float right_motor_speed = 0.5;
00734     char buffer[255];
00735     
00736         
00737     if (reflected_sensor_data[0] > 300){
00738         left_motor_speed = -0.5;
00739         set_left_motor_speed(left_motor_speed);
00740         set_right_motor_speed(right_motor_speed);      
00741     }
00742     else if (reflected_sensor_data[7] > 300){
00743         right_motor_speed = -0.5;
00744         set_left_motor_speed(left_motor_speed);
00745         set_right_motor_speed(right_motor_speed);      
00746     }
00747     else if (robot_id == 1) {
00748         display.clear_display();
00749         display.set_position(0,0);
00750         display.write_string("my Id is 1!");
00751         float left_motor_speed = 0.3;
00752         float right_motor_speed = 0.3;
00753         int turnChance = rand() %100;
00754         if (turnChance < 20) {
00755             left_motor_speed = 0;
00756         } else if (turnChance > 80) {
00757             right_motor_speed = 0;
00758         }
00759         
00760         set_left_motor_speed(left_motor_speed);
00761         set_right_motor_speed(right_motor_speed);
00762     }
00763     else {
00764         
00765         if (abs(robots_heading[1]) > 40 && robots_found[1] == 1) {
00766             time_based_turn_degrees(1, robots_heading[1], 1);
00767             sprintf(buffer,"%d > 40", robots_heading[1]);
00768             display.clear_display();
00769             display.set_position(0,0);
00770             display.write_string(buffer);
00771         } 
00772         else {
00773             //used for time based turn instead of location based
00774             set_left_motor_speed(left_motor_speed);
00775             set_right_motor_speed(right_motor_speed);
00776         }  
00777     }
00778 
00779 }
00780 
00781 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
00782 /// generic_program - Framework for building typical programs
00783 
00784 void generic_program()
00785 {
00786     // Do something on the first run of a program
00787     if(program_run_init == 1) {
00788         // Initialisation code goes here...
00789 
00790         program_run_init = 0;
00791     }
00792 
00793     // 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)
00794     if(step_cycle == 0) {
00795         // Do something based on sensor data (eg turn)
00796     } else {
00797         // Do something ignoring sensor data (eg move, or nothing!)
00798     }
00799 
00800 }