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