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 /// flocking_program
00729 
00730 void flocking_program()
00731 {
00732     char display_line[16] = "               ";      
00733     int average_heading = 0;
00734     int number_of_neighbours = 0;
00735     int number_of_flocking_headings = 0;
00736     int target_heading = 0;
00737     int angles = 0;    
00738     int ir_sensor_threshold = 600;
00739     int sensors_activated = 0;
00740     
00741     struct FloatVector obstacle_vector;
00742     obstacle_vector.angle = 0;
00743     obstacle_vector.distance = 0;
00744     
00745     struct FloatVector cohesion_vector;
00746     cohesion_vector.angle = 0;
00747     cohesion_vector.distance = 0;
00748     
00749     for(int i = 0; i < 8; i++)
00750     {
00751         if(reflected_sensor_data[i] > ir_sensor_threshold)
00752         {            
00753             obstacle_vector = addVector(obstacle_vector, get_bearing_from_ir_array(reflected_sensor_data), reflected_sensor_data[i]);
00754             sensors_activated++;
00755         }
00756                 
00757         if(robots_found[i])
00758         {
00759             // -180 degrees is reserved for "no byte received"
00760             if(flocking_headings[i] != -180)
00761             {
00762                 average_heading += flocking_headings[i];
00763                 number_of_flocking_headings++;
00764             }
00765             
00766             cohesion_vector = addVector(cohesion_vector, robots_heading[i], robots_distance[i]);
00767             number_of_neighbours++;
00768         }
00769     }
00770     
00771     cohesion_vector.distance /= number_of_neighbours; // Normalise        
00772     obstacle_vector.distance /= sensors_activated; // Normalise
00773     
00774     int obstacle_avoidance_angle;
00775     
00776     if(sensors_activated > 0)
00777     {
00778         obstacle_avoidance_angle = obstacle_vector.angle + 180;
00779                 
00780         if(obstacle_avoidance_angle > 180)
00781             obstacle_avoidance_angle -= 360;
00782         if(obstacle_avoidance_angle < -180)
00783             obstacle_avoidance_angle += 360;
00784         
00785         target_heading += obstacle_avoidance_angle;
00786         angles++;
00787     }
00788 
00789     int cohesion_angle;
00790     
00791     // Don't bother performing cohesion if robots are already close enough
00792     if(number_of_neighbours > 0 && cohesion_vector.distance < 200)
00793     {            
00794         cohesion_angle = cohesion_vector.angle;
00795         
00796         if(cohesion_angle > 180)
00797             cohesion_angle -= 360;
00798         if(cohesion_angle < -180)
00799             cohesion_angle += 360;
00800         
00801         target_heading += cohesion_angle;
00802         angles++;
00803     }
00804     
00805     int relative_flocking_heading;
00806     
00807     if(number_of_flocking_headings > 0)
00808     {        
00809         average_heading /= number_of_flocking_headings;            
00810 
00811         relative_flocking_heading = beacon_heading - average_heading;
00812         
00813         if(relative_flocking_heading > 180)
00814             relative_flocking_heading -= 360;
00815         if(relative_flocking_heading < -180)
00816             relative_flocking_heading += 360;
00817         
00818         target_heading += relative_flocking_heading;
00819         angles++;
00820     }
00821         
00822     if(angles > 0)
00823         target_heading /= angles; // Calculate average
00824 
00825     float left_motor_speed = 0.25;
00826     float right_motor_speed = 0.25;
00827 
00828     if(target_heading > 22.5)
00829         right_motor_speed *= -1;
00830     else if(target_heading < -22.5)
00831         left_motor_speed *= -1;
00832         
00833     set_left_motor_speed(left_motor_speed);
00834     set_right_motor_speed(right_motor_speed);
00835 
00836     // Only transmit beacon heading if the beacon is visible
00837     if(beacon_found)
00838     {        
00839         float degrees_per_value = 256.0f / 360.0f;
00840         char beacon_heading_byte = (beacon_heading + 180) * degrees_per_value; // Convert beacon heading from +/-180 degrees into a single byte value
00841         
00842         bt.putc(beacon_heading_byte);
00843     }
00844     
00845     sprintf(display_line, "%d %d %d %d", target_heading, obstacle_avoidance_angle, cohesion_angle, relative_flocking_heading);
00846  
00847     set_program_info(display_line);
00848 }
00849 
00850 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
00851 /// generic_program - Framework for building typical programs
00852 
00853 void generic_program()
00854 {
00855     // Do something on the first run of a program
00856     if(program_run_init == 1) {
00857         // Initialisation code goes here...
00858 
00859         program_run_init = 0;
00860     }
00861 
00862     // 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)
00863     if(step_cycle == 0) {
00864         // Do something based on sensor data (eg turn)
00865     } else {
00866         // Do something ignoring sensor data (eg move, or nothing!)
00867     }
00868 
00869 }