![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Psi Swarm Code V0.41 [With Beautiful Meme program]
Dependencies: PsiSwarmLibrary mbed
Fork of BeautifulMemeProjectBT by
Revision 30:513457c1ad12, committed 2016-03-15
- Comitter:
- jah128
- Date:
- Tue Mar 15 00:58:43 2016 +0000
- Parent:
- 29:9756004e8499
- Commit message:
- Added serial handling for Psi Console
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/BeautifulMeme/beacon.cpp Tue Mar 15 00:58:43 2016 +0000 @@ -0,0 +1,376 @@ +/// PsiSwarm Beautiful Meme Project Source Code +/// Version 0.41 +/// James Hilder, Alan Millard, Homero Elizondo, Jon Timmis +/// University of York + +// beacon.cpp - Functions for detecting the beacon and taking IR readings of the robots + +#include "bmeme.h" + +int pulse_step = 1; //Pulse-step corresponds to which timeslot (0-9) is currently active, where beacon=0 and robots=2-8 +int low_threshold; //Set to be 2x mean background IR +int beacon_threshold; //Set to be 4x mean background IR +unsigned short ir_sensor_data[9][8]; // The raw sensor data from all 9x 50ms sample windows +Ticker ir_sample_ticker; // Ticker for the IR data sampling and processing; runs every 50ms in middle of timeslot +Ticker ir_emitter_ticker; // Ticker for turning on the IR emitters; runs every 50ms near start of timeslot +Timeout ir_emitter_timeout; // Timeout for turning off the IR emitters after 40ms +Timer beacon_debug_timer; // Timer for debug information only [remove later?] + +char show_ir_debug_info = 0; // Set to 1 to display (via PC) the list of IR readings & visible robots every timestep + +/// The locate beacon function samples the IR radiation from all 8 side sensors over a period of 1 second in [BEACON_PERIOD / 2.5] (20ms) blocks. +/// The infrared beacon is set to give a 50ms burst of IR every 500ms. We should thus see in the sampled radiation 2 blocks +/// of samples, 2 or 3 samples in duration, when a significant peak occurs; the blocks should be 25 samples apart. +void locate_beacon() +{ + int sample_period = (BEACON_PERIOD * 2) / 5; + out("1) Searching for IR beacon..."); + unsigned short samples[50][9]; + Timer beacon_timer; + beacon_timer.start(); + int offset = 0; + //This loop samples the background IR values at 50Hz for 1 second and stores in an array + for(int i=0; i<50; i++) { + store_background_raw_ir_values (); + if(i%2 == 0){ + set_center_led(1, 0.5); + set_leds(0xAA,0x55); + }else{ + set_center_led(2, 0.5); + set_leds(0x55,0xAA); + } + samples[i][8]=0; + for(int j=0; j<8; j++) { + samples[i][j] = get_background_raw_ir_value(j); + samples[i][8] += get_background_raw_ir_value(j); + } + offset+=sample_period; + while(beacon_timer.read_us() < offset) {} + } + + //Print values: for testing [comment out] + /* + for(int i=0; i<50; i++) { + out("IR %d:",i); + for(int j=0; j<8; j++) { + out("[%d:%d]",j,samples[i][j]); + } + out(" [SUM:%d]\n",samples[i][8]); + } + */ + + //Bubble sort sums to find (6) highest values + unsigned short sorted_array[50]; + for(int i=0; i<50; i++) { + sorted_array[i]=samples[i][8]; + } + for (int c = 0 ; c < 49; c++) { + for (int d = 0 ; d < (50-c-1); d++) { + if (sorted_array[d] > sorted_array[d+1]) { + unsigned short swap = sorted_array[d]; + sorted_array[d] = sorted_array[d+1]; + sorted_array[d+1] = swap; + } + } + } + + //Print sorted values: for testing [comment out] + /* + out("Sorted values:"); + for (int c = 0 ; c < 50 ; c++ ) { + out("%d", sorted_array[c]); + if(c<49)out(","); + } + out("\n"); + */ + + // Calculate mean background sum value by looking at 44 lowest sum values + int background_mean = 0; + for(int i=0;i<44;i++)background_mean += sorted_array[i]; + background_mean /= 44; + + //out("Background mean value: %d\n",background_mean); + + //Our beacon threshold will be 4x the background mean value; find all instances where this occurs + low_threshold = background_mean * 2; + beacon_threshold = background_mean * 4; + char beacon_detected_indices[50]; + for(int i=0;i<50;i++){ + if(samples[i][8] > beacon_threshold) beacon_detected_indices[i]=1; + else beacon_detected_indices[i]=0; + } + //Count and display matches + int beacon_detected_count = 0; + //char output_string[251] = ""; + for(int i=0;i<50;i++){ + if(beacon_detected_indices[i] == 1){ + beacon_detected_count++; + // char index_string[6]; + // sprintf(index_string,"[%d],",i); + // strcat(output_string,index_string); + } + } + //out("%d samples are above threshold:%s\n",beacon_detected_count,output_string); + + //We will use this array to store average values for each sensor when the beacon is detected + unsigned short beacon_averages[8]; + char beacon_averages_count = 0; + for(int i=0;i<8;i++)beacon_averages[i]=0; + + //Now determine if the beacon is correctly found: must adhere to a set of rules + //Firstly, we should have not less than 4 and not more than 6 positive matches + if(beacon_detected_count>3 && beacon_detected_count<7){ + // Now verify that the positive samples are in valid places... + // Find first positive sample + int first_index = 0; + //out("Here\n",first_index); + + while(beacon_detected_indices[first_index]==0)first_index ++; + + //out("First index:%d\n",first_index); + + + // Check if first index is zero: if so, we need to check index 49 (and 48) to see if they are also high + if(first_index == 0){ + if(beacon_detected_indices[49]>0)first_index = 49; + if(beacon_detected_indices[48]>0)first_index = 48; + } + + beacon_averages_count++; + for(int i=0;i<8;i++){beacon_averages[i]+=samples[first_index][i];} + + // Now count the length of the 'block' of positive hits: must be equal to 2 or 3 + char block_length = 1; + int end_index = first_index + 1; + if(end_index == 50) end_index = 0; + while(beacon_detected_indices[end_index]>0){ + beacon_averages_count++; + for(int i=0;i<8;i++){beacon_averages[i]+=samples[end_index][i];} + block_length ++; + end_index ++; + if(end_index == 50) end_index = 0; + } + if(block_length==2 || block_length == 3){ + //We have found the first correct block and it is valid; now calculate its mid-point and check that the second block is also present 500ms later + float mid_point; + char second_block_okay = 0; + if(block_length == 2){ + mid_point = first_index + 0.5; + char second_block_low = first_index + 25; + char second_block_high = first_index + 26; + if(second_block_low > 49) second_block_low -= 50; + if(second_block_high > 49) second_block_high -= 50; + beacon_averages_count+=2; + for(int i=0;i<8;i++){beacon_averages[i]+=samples[second_block_low][i]+samples[second_block_high][i];} + if(beacon_detected_indices[second_block_low]>0 && beacon_detected_indices[second_block_high]>0) second_block_okay = 1; + } + if(block_length == 3){ + mid_point = first_index + 1; + if(mid_point == 50) mid_point = 0; + char second_block_single = first_index + 26; + if(second_block_single > 49) second_block_single -= 50; + beacon_averages_count++; + for(int i=0;i<8;i++){beacon_averages[i]+=samples[second_block_single][i];} + if(beacon_detected_indices[second_block_single]>0) second_block_okay = 1; + } + if(second_block_okay >0){ + beacon_found = 1; + beacon_heading = get_bearing_from_ir_array(beacon_averages); + out("Found at %d degrees\n",beacon_heading); + //for(int i=0;i<8;i++){ + // beacon_averages[i] /= beacon_averages_count; + // out("[%d]",beacon_averages[i]); + //} + out("2) Synchronising...\n"); + // Calculate the offset to the expected start of the next beacon pulse + int microseconds_offset = (sample_period * mid_point) - sample_period - (sample_period / 4); + //out("MS Offset:%d Midpoint:%f\n Current Time:%d\n",microseconds_offset,mid_point,beacon_timer.read_us()); + int cycle_period = (BEACON_PERIOD * 10); + if(microseconds_offset < 0) microseconds_offset += cycle_period; + //If we have missed the start of the beacon this cycle, wait until the next cycle + while(beacon_timer.read_us()% (cycle_period) > microseconds_offset){}; + //Now wait until the start of the beacon pulse + while(beacon_timer.read_us()% (cycle_period) < microseconds_offset){}; + /* + out("Now:%d",beacon_timer.read_us()); + Timer test_timer; + test_timer.start(); + for(int i=0;i<50;i++){ + store_background_raw_ir_values (); + out("Time %d: %d\n",test_timer.read_ms(),get_background_raw_ir_value(2)); + while(test_timer.read_ms() % 10 < 9){}; + } + */ + }else{ + beacon_found = 0; + out("Beacon not found: a matching second block %dms after first block not detected\n",(BEACON_PERIOD / 100)); + } + }else{ + beacon_found = 0; + if(block_length == 1) out("Beacon not found: a single sample [%d] was high but not its neighbours\n",first_index); + if(block_length > 3) out("Beacon not found: a block of %d high samples was detected\n",block_length); + } + } else { + beacon_found = 0; + if(beacon_detected_count > 6) out("Beacon not found: too many high samples [%d]\n",beacon_detected_count); + else out("Beacon not found: too few high samples [%d]\n",beacon_detected_count); + } + if(beacon_found == 0){ + set_leds(0x00,0x00); + set_center_led(1, 1); + display.clear_display(); + display.set_position(0,0); + display.write_string("BEACON NOT FOUND"); + } +} + +// The start_infrared_timers() function is called as soon as the beacon has been detected and synchronised to +// It launches 2 tickers at offset times; the first is responsible for turning the robots IR emitters on in its proper timeslot +// The other reads the values given from the IR sensor in the middle of each timeslot and processes that information in the final timeslot +void start_infrared_timers() +{ + // At this point we should be exactly at the start of a beacon cycle. + // We want the emitter ticker to start in approx 5ms (this will let us set a 40ms pulse) + // We want the sample ticker to start in approx 25ms (this will let us sample in the middle each step + out("3) Starting TDMA infrared timers\n"); + beacon_debug_timer.start(); + wait_us(BEACON_PERIOD / 10); + ir_emitter_ticker.attach_us(emitter_ticker_block,BEACON_PERIOD); + wait_us(((BEACON_PERIOD * 4) / 10)); //Wait for middle of pulse + ir_sample_ticker.attach_us(sample_ticker_block,BEACON_PERIOD); +} + + +//Return the max value in IR array +unsigned short get_highest_sample(unsigned short * ir_array){ + unsigned short highest = 0; + for(int i=0;i<8;i++){ + if(ir_array[i]>highest) highest=ir_array[i]; + } + return highest; +} + +//Return the sum total of IR array +unsigned short get_sum_sample(unsigned short * ir_array){ + unsigned short sum = 0; + for(int i=0;i<8;i++){ + sum+=ir_array[i]; + } + return sum; +} + +//The emitter_ticker_block function runs every 50ms and turns the IR emitters on when pulse_step-1 matches the robot ID +//It then starts a timeout to run emitter_timeout_block after 40ms, which turns off the emitters +void emitter_ticker_block(){ + //If the time-step (-1) equals my ID, turn on my emitters for 40ms + if(pulse_step-1 == robot_id && disable_ir_emitters == 0){ + IF_set_IR_emitter_output(0, 1); + IF_set_IR_emitter_output(1, 1); + ir_emitter_timeout.attach_us(emitter_timeout_block,(BEACON_PERIOD * 8)/10); + } +} + +//Turn off the emitters +void emitter_timeout_block(){ + //Turn off IR emitters + IF_set_IR_emitter_output(0, 0); + IF_set_IR_emitter_output(1, 0); +} + +//The function sample_ticker_block() is called every 50ms, and should run close to the middle of every timeslot +//There are 10 time slots in each 500ms period +//Slot 0 is when the beacon is flashing +//Slot 1 should be IR-free and is used to measure background IR data, stored in background_sensor_data[] +//Slot 2-8 are for the 7 robots; slot-1 = robot_id +//In slot 9, the robot processes the data [and doesn't store and new readings] +//It checks if the beacon is visible, if any robots are, calculates their bearings if they are, and transfers the background and active IR data for the robot +void sample_ticker_block(){ + //If we are in time-step 0 to 8, store the background data in an array + if(pulse_step < 9){ + store_background_raw_ir_values (); + for(int i=0;i<8;i++)ir_sensor_data[pulse_step][i]=get_background_raw_ir_value(i); + }else{ + //If not, process the data + for(int i=0;i<9;i++){ + unsigned short sum = get_sum_sample(ir_sensor_data[i]); + unsigned short highest = get_highest_sample(ir_sensor_data[i]); + //Check if beacon is visible + if(i==0){ + if(sum > beacon_threshold){ + beacon_found = 1; + beacon_heading = get_bearing_from_ir_array (ir_sensor_data[0]); + }else beacon_found = 0; + //out("Beacon sum:%d 0:%d 4:%d\n",sum,ir_sensor_data[0][0],ir_sensor_data[0][4]); + } + if(i==1){ + for(int j=0;j<8;j++)background_sensor_data[j]=ir_sensor_data[1][j]; + } + if(i>1){ + char test_robot = i-1; + if(test_robot == robot_id){ + for(int j=0;j<8;j++)reflected_sensor_data[j]=ir_sensor_data[i][j]; + }else{ + if(sum > low_threshold){ + robots_found[test_robot] = 1; + //Debug-- + //out("Robot %d: [%d][%d][%d][%d][%d][%d][%d][%d]\n",test_robot,ir_sensor_data[i][0],ir_sensor_data[i][1],ir_sensor_data[i][2],ir_sensor_data[i][3],ir_sensor_data[i][4],ir_sensor_data[i][5],ir_sensor_data[i][6],ir_sensor_data[i][7]); + robots_heading[test_robot] = get_bearing_from_ir_array (ir_sensor_data[i]); + robots_distance[test_robot] = highest; + }else robots_found[test_robot] = 0; + } + } + } + if(show_ir_debug_info == 1)display_ir_readings(); + } + //Increment pulse step + pulse_step++; + if(pulse_step == 10) pulse_step = 0; +} + + +//Testing function to print out lines showing what robot can currently see in terms of beacon, other robots and obstacles +void display_ir_readings() +{ + out("____________________________________\nInfrared Detection at %d ms\n",beacon_debug_timer.read_ms()); + if(beacon_found==1){ + out("Beacon detected at %d degrees\n",beacon_heading); + } + for(int j=1;j<8;j++){ + if(robots_found[j])out("Robot %d detected at %d degrees, %d distance\n",j,robots_heading[j],robots_distance[j]); + } + out("Reflected values:"); + for(int i=0;i<8;i++){ + out("[%d,%d]",i,reflected_sensor_data[i]); + } + out("\nBackground values:"); + for(int i=0;i<8;i++){ + out("[%d,%d]",i,background_sensor_data[i]); + } + out("\n\n"); +} + +//Returns a 0 if turn is likely to complete in a single timestep, 1 if it is beyond range for single timestep and 2 if the beacon is not found so bearing unknown +char turn_to_bearing(int bearing) +{ + if(beacon_found == 0){ + out("Beacon not found: cannot turn to specific bearing\n"); + return 2; + }else{ + //First calculate the bearing using the angle of beacon relative to robot + int current_bearing = 360 - beacon_heading; + //Now work out turn needed to face intended heading + int target_turn = (bearing - current_bearing) % 360; + //Adjust to take 10% off turn, stops overshoot + target_turn = (target_turn * 9) / 10; + if(target_turn > 180) target_turn -= 360; + if(target_turn < -180) target_turn += 360; + //We can't reliably turn more than 280 degrees per second, so set a limit for the turn to that + char beyond_limit = 0; + int turn_limit = BEACON_PERIOD / 358; + if(target_turn > turn_limit) {target_turn = turn_limit; beyond_limit = 1;}; + if(target_turn < -turn_limit) {target_turn = -turn_limit; beyond_limit = 1;}; + out("Turning %d degrees\n",target_turn); + time_based_turn_degrees(1, target_turn,1); + return beyond_limit; + } +} \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/BeautifulMeme/beacon.h Tue Mar 15 00:58:43 2016 +0000 @@ -0,0 +1,20 @@ +/// PsiSwarm Beautiful Meme Project Source Code +/// Version 0.41 +/// James Hilder, Alan Millard, Homero Elizondo, Jon Timmis +/// University of York + +// beacon.h - Functions for detecting the beacon and taking IR readings of the robots + +#ifndef BEACON_H +#define BEACON_H + +void emitter_ticker_block(void); +void sample_ticker_block(void); +void emitter_timeout_block(void); +void locate_beacon(void); +void start_infrared_timers(void); +unsigned short get_highest_sample(unsigned short * ir_array); +unsigned short get_sum_sample(unsigned short * ir_array); +char turn_to_bearing(int bearing); + +#endif \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/BeautifulMeme/bmeme.cpp Tue Mar 15 00:58:43 2016 +0000 @@ -0,0 +1,306 @@ +/// PsiSwarm Beautiful Meme Project Source Code +/// Version 0.41 +/// James Hilder, Alan Millard, Homero Elizondo, Jon Timmis +/// University of York + +/// Include bmeme.h - this includes main.h, psiswarm.h all the other necessary core files + +#include "bmeme.h" + +// IMPORTANT!!! +// Do not call the IR functions at all as they will interfere with the correct operation of this program +// Instead, use the values held in the variables below; they are updated every 500ms + +char beacon_found = 0; // This will be a 1 when a beacon was detected during the previous 500ms window +int beacon_heading = 0; // This is the heading from the last time a beacon was detected +char robots_found[8]; // These will be a 1 when the respective robot [excluding self] was detected during the previous 500ms window +int robots_heading[8]; // These are the headings from the last time the respective robots were detected +unsigned short robots_distance[8]; // This is the maximum sensor value from the last time the respective robot was detected +unsigned short reflected_sensor_data[8]; // The reflected IR values when this robots emitters are on +unsigned short background_sensor_data[8];// The raw IR values when no robot (or beacon) should have its IR on + +char default_normal_program = 8; // The program to run on turn on (after 'face beacon' program) +char use_recharging_program = 0; // Set to 1 to force robot to run recharging program when battery voltage drops below a threshold +char user_code_debug = 1; // Set to 1 to show terminal messages from "out" function [specific to this code] +char display_debug_inf = 0; // Set to 1 to show debug info about beacon\robots on display [instead of running program info] +char main_program_state; // Index of the currently running program +char previous_program; // Used to hold previous running program when it is paused for switch press etc +char program_changed = 0; // Flag to update display when program is changed +char program_run_init = 0; // Flag to tell program to run its initialisation on first loop, if neccessary +char success_count = 0; // Flag to indicate the success of a program +char step_cycle = 0; // Alternates between 0 and 1 in successive time-steps +char target_reached = 0; // Flag to indicate if a program target has been reached +char prog_name [17]; // Stores the name of the running program [line 0 on the display] +char prog_info [17]; // Stores information about the current state of the program [line 1 on the display] +char disable_ir_emitters = 0; // Used to disable IR emission during charging etc [use with caution!] +char recharging_state = 0; // Stores the state of the recharging program (0 is not currently running) +char switch_held = 0; // Used for detected when the cursor switch is held to override program choice +char choose_program_mode = 0; +char program_count = 8; +char program_selection; +int flocking_headings[8]; // Beacon heading of each robot + +float battery_low_threshold = 3.60; // Threshold at which to interrupt program and start recharging routine: suggest 3.55 +float battery_high_threshold = 3.95; // Threshold at which to end battery recharging routine and resume normal program: suggest 4.0 + +Ticker main_loop_ticker; + +///This is the main loop for the Beautiful Meme code. The code block is run once every 250mS* [with 4Hz beacon] once all the IR samples have been collected. +void bmeme_main_loop() +{ + if(switch_held == 1)switch_held=2; + if(switch_held == 3 && choose_program_mode == 0) { + //The switch has been held right and then released: stop the current program + previous_program = main_program_state; + program_selection = previous_program; + choose_program_mode = 1; + set_program(255); + set_program_info(get_program_name(program_selection)); + } + if(use_recharging_program == 1)recharging_program(); + update_display(); + if(recharging_state == 0) { + switch(main_program_state) { + case 0: //Case 0 is the initial program: turn to face beacon + if(step_cycle == 0) { + char turn_status = turn_to_bearing(0); + if(turn_status == 0) { + success_count ++; + if(success_count > 1) set_program(default_normal_program); + } else success_count = 0; + } + break; + case 1: + target_reached = 0; + head_to_bearing_program(0); + if(target_reached == 1) set_program(2); + break; + case 2: + target_reached = 0; + head_to_bearing_program(180); + if(target_reached == 1) set_program(1); + break; + case 3: + curved_random_walk_with_interaction_program(); + break; + case 4: + straight_random_walk_with_interaction_program(); + break; + case 5: + find_space_program(1); + break; + case 6: + clustering_program(0,1); + break; + case 7: + tag_game_program(); + break; + case 8: + flocking_program(); + break; + case 255: + stop_program(); + break; + } + } + step_cycle=1-step_cycle; +} + +///Place user code here that should be run after initialisation but before the main loop +void bmeme_user_code_setup() +{ + program_name = "B-Meme"; + author_name = "YRL"; + version_name = "0.4"; + + display.clear_display(); + display.set_position(0,0); + display.write_string("BEAUTIFUL MEME"); + display.set_position(1,0); + display.write_string(" PROJECT"); + wait(0.2); + out("------------------------------------------------------\n"); + out("Beautiful Meme Project Demo Code \n"); + out("------------------------------------------------------\n"); + locate_beacon(); + while(beacon_found == 0) { + wait(0.5); + locate_beacon(); + } + start_infrared_timers(); + main_loop_ticker.attach_us(&bmeme_main_loop,BEACON_PERIOD * 10); + set_program(0); + set_leds(0x00,0x00); + set_center_led(3,0.5); + display.clear_display(); + display.set_position(0,0); + display.write_string("BEACON FOUND AT"); + display.set_position(1,0); + char degrees_string[16]; + sprintf(degrees_string,"%d DEGREES",beacon_heading); + display.write_string(degrees_string); +} + +/// Code goes here to handle what should happen when the user switch is pressed +void bmeme_handle_switch_event(char switch_state) +{ + /// Switch_state = 1 if up is pressed, 2 if down is pressed, 4 if left is pressed, 8 if right is pressed and 16 if the center button is pressed + /// NB For maximum compatability it is recommended to minimise reliance on center button press + if(choose_program_mode == 0) { + if(switch_state == 8) switch_held = 1; + else if(switch_state == 0 && switch_held == 2) switch_held = 3; + else switch_held = 0; + } else { + // We are in choose program mode + if(switch_state == 8) { + program_selection ++; + if(program_selection > program_count) program_selection = 0; + if(program_selection == program_count) set_program_info("RECHARGE"); + else set_program_info(get_program_name(program_selection)); + } + if(switch_state == 4) { + if(program_selection == 0) program_selection = program_count; + else program_selection --; + if(program_selection == program_count) set_program_info("RECHARGE"); + else set_program_info(get_program_name(program_selection)); + } + if(switch_state == 1 || switch_state == 2){ + if(program_selection == program_count){ + recharging_state = 1; + set_program(previous_program); + strcpy(prog_name,"CHARGING PROGRAM"); + set_program_info("HEAD TO BEACON"); + + } + else set_program(program_selection); + choose_program_mode = 0; + switch_held = 0; + } + } + //out("Switch:%d Switch_held:%d Program_Selection:%d Program_count:%d Prog_Info:%s\n",switch_state,switch_held,program_selection,program_count,prog_info); +} + +char * get_program_name(int index) +{ + char * ret_name = new char[17]; + switch(index) { + case 0: + strcpy(ret_name,"FACE BEACON"); + break; + case 1: + strcpy(ret_name,"HEAD TO BEACON"); + break; + case 2: + strcpy(ret_name,"HEAD TO SOUTH"); + break; + case 3: + strcpy(ret_name,"RANDOM WALK 1"); + break; + case 4: + strcpy(ret_name,"RANDOM WALK 2"); + break; + case 5: + strcpy(ret_name,"FIND SPACE"); + break; + case 6: + strcpy(ret_name,"CLUSTERING"); + break; + case 7: + strcpy(ret_name,"TAG GAME"); + break; + case 8: + strcpy(ret_name,"FLOCKING"); + break; + case 255: + strcpy(ret_name,"PROGRAM:"); + break; + } + return ret_name; +} + +void set_program(int index) +{ + main_program_state = index; + program_changed = 1; + program_run_init = 1; + strcpy(prog_info,""); + strcpy(prog_name,get_program_name(index)); +} + +void set_program_info(char * info) +{ + strcpy(prog_info,info); + program_changed = 1; +} + +void update_display() +{ + if(program_changed == 1) { + program_changed = 0; + display.clear_display(); + + if(display_debug_inf==1) display_debug_info(); + else { + display.set_position(0,0); + display.write_string(prog_name); + } + display.set_position(1,0); + display.write_string(prog_info); + } +} + +void display_debug_info() +{ + char disp_line[16] = "- - - - - - - -"; + if(beacon_found==1)disp_line[0]='B'; + for(int i=1; i<8; i++) { + if(robots_found[i])disp_line[((i)*2)]=48+i; + } + display.set_position(0,0); + display.write_string(disp_line); +} + +/// Verbose output +void out(const char* format, ...) +{ + char buffer[256]; + if (debug_mode) { + va_list vl; + va_start(vl, format); + vsprintf(buffer,format,vl); + if(user_code_debug == 1) pc.printf("%s", buffer); + va_end(vl); + } +} + + +void bmeme_handle_user_serial_message(char * message, char length, char interface) +{ + // This is where user code for handling a (non-system) serial message should go + // + // message = pointer to message char array + // length = length of message + // interface = 0 for PC serial connection, 1 for Bluetooth + + if(interface) { + if(length == 8) { + for(int i = 0; i < length; i++) { + // Convert single byte value into a beacon heading in the range +/-180 degrees + float beacon_heading = message[i]; + float degrees_per_value = 256.0f / 360.0f; + + if(beacon_heading != 0) + beacon_heading /= degrees_per_value; + + beacon_heading -= 180; + + flocking_headings[i] = beacon_heading; + + debug("%d, ", flocking_headings[i]); + //debug("%f, ", beacon_heading); + } + + debug("\n"); + } + } +} \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/BeautifulMeme/bmeme.h Tue Mar 15 00:58:43 2016 +0000 @@ -0,0 +1,58 @@ +/// PsiSwarm Beautiful Meme Project Source Code +/// Version 0.41 +/// James Hilder, Alan Millard, Homero Elizondo, Jon Timmis +/// University of York + + +#ifndef BMEME_H +#define BMEME_H + +#include "main.h" +#include "beacon.h" +#include "programs.h" +#include "vector.h" +#include <math.h> + +// Define the on-period for a IR pulse in microseconds (eg 50000 for 2Hz system, 25000 for a 4Hz system or 20000 for a 5Hz system) +#define BEACON_PERIOD 25000 + + +// Define the on-period for a IR pulse in microseconds (eg 50000 for 2Hz system, 25000 for a 4Hz system or 20000 for a 5Hz system) +#define BEACON_PERIOD 25000 + +extern char beacon_found; // This will be a 1 if beacon is detected, 0 if it isn't +extern int beacon_heading; // The heading from the last time the beacon was detected +extern char robots_found[8]; // These will be a 1 when the respective robot [excluding self] was detected during the previous 500ms window +extern int robots_heading[8]; // These are the headings from the last time the respective robots were detected +extern unsigned short robots_distance[8]; // This is the maximum sensor value from the last time the respective robot was detected +extern unsigned short reflected_sensor_data[8]; // The reflected IR values when this robots emitters are on +extern unsigned short background_sensor_data[8];// The raw IR values when no robot (or beacon) should have its IR on +extern char disable_ir_emitters; +extern char step_cycle; +extern char target_reached; +extern char recharging_state; +extern char prog_name [17]; +extern char prog_info [17]; +extern char main_program_state; +extern char program_changed; +extern char program_run_init; +extern float battery_low_threshold; +extern float battery_high_threshold; +extern int flocking_headings[8]; + +char * get_program_name(int index); +void set_program(int index); +void set_program_info(char * info); + +void update_display(void); +void display_debug_info(void); + +void bmeme_user_code_setup(void); +void bmeme_user_code_loop(void); +void bmeme_handle_switch_event(char switch_state); +void bmeme_handle_user_serial_message(char * message, char length, char interface); + +void display_ir_readings(void); +void out(const char* format, ...) ; + +#endif \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/BeautifulMeme/programs.cpp Tue Mar 15 00:58:43 2016 +0000 @@ -0,0 +1,869 @@ +/// PsiSwarm Beautiful Meme Project Source Code +/// Version 0.41 +/// James Hilder, Alan Millard, Homero Elizondo, Jon Timmis +/// University of York + +// programs.cpp - Various PsiSwarm Programs for Beautiful Meme Project + +#include "bmeme.h" + +int obstacle_avoidance_threshold = 300; +int robot_avoidance_threshold = 2000; + +///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +/// head_to_bearing_program +char was_turning = 0; + +///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 +void head_to_bearing_program(int target_bearing) +{ + if(step_cycle == 0 || was_turning == 0) { + // Check if we are heading in the right bearing (+- 25 degrees) + int current_bearing = (360 - beacon_heading) % 360; + // Current bearing should range from 0 to 359; target_bearing likewise; check the are within 25 degrees of each other + char bearing_ok = 0; + int lower_bound = target_bearing - 25; + int upper_bound = target_bearing + 25; + if(lower_bound < 0) { + if(current_bearing > (lower_bound + 360) || current_bearing < upper_bound) bearing_ok = 1; + } else if(upper_bound > 359) { + if(current_bearing > lower_bound || current_bearing < (upper_bound - 360)) bearing_ok = 1; + } else if(current_bearing > lower_bound && current_bearing < upper_bound) bearing_ok = 1; + // Check if there is an obstacle in front of us + if((reflected_sensor_data[7] > 1000 || reflected_sensor_data[0] > 1000) && bearing_ok == 1) target_reached = 1; + else { + // Now move forward if we are facing correct bearing, otherwise turn + if(bearing_ok == 1) { + //Check if anything is in front of us to determine speed - if it is, move slowly + int t_time = 6 * BEACON_PERIOD; + float t_speed = 1.0; + if(reflected_sensor_data[7] > 150 || reflected_sensor_data[0] > 150) { + t_time = 4 * BEACON_PERIOD; + t_speed = 0.6; + } + if(reflected_sensor_data[7] > 300 || reflected_sensor_data[0] > 300) { + t_time = 3 * BEACON_PERIOD; + t_speed = 0.4; + } + if(reflected_sensor_data[7] > 500 || reflected_sensor_data[0] > 500) { + t_time = 2 * BEACON_PERIOD; + t_speed = 0.2; + } + time_based_forward(t_speed,t_time,0); + was_turning = 0; + } else { + turn_to_bearing(target_bearing); + was_turning = 1; + } + } + } +} + + + + +///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +/// recharging_program + +char recharge_power_check_count = 0; +char battery_low_count = 0; + +void recharging_program() +{ + switch(recharging_state) { + case 0: + // We are not currently recharging, check the battery state + if(get_battery_voltage() < battery_low_threshold) { + // Battery is below low threshold + battery_low_count ++; + // 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 + if(battery_low_count > 3) { + // Set recharging state to 'looking for charger' + recharging_state = 1; + strcpy(prog_name,"CHARGING PROGRAM"); + set_program_info("HEAD TO BEACON"); + } + } else battery_low_count = 0; + break; + // State 1: Head to beacon [as this is where battery charger is] + case 1: + target_reached = 0; + head_to_bearing_program(0); + if(target_reached == 1) { + recharging_state = 2; + set_program_info("TURN 90 DEGREES"); + } + break; + // Stage 2: Turn 90 degrees to align with charging pads + case 2: + disable_ir_emitters = 1; + time_based_turn_degrees(0.8, 70.0, 1); + recharge_power_check_count = 0; + recharging_state = 3; + break; + // Stage 3: Wait for turn to complete + case 3: + if (time_based_motor_action != 1) { + recharging_state = 4; + set_program_info("CHECK FOR POWER"); + } + break; + // Stage 4: Check if charging + case 4: + recharge_power_check_count++; + if(get_dc_status() == 1) { + recharging_state = 5; + } else { + if(recharge_power_check_count < 10)recharging_state = 6; + else { + recharging_state = 7; + set_program_info("NO POWER - RETRY"); + } + } + break; + // Stage 5: Charging. Wait until threshold voltage exceeded + case 5: + if(get_battery_voltage() > battery_high_threshold) { + set_program_info("LEAVE CHARGER"); + recharging_state = 7; + } else { + char b_voltage[16]; + sprintf(b_voltage,"CHARGE: %1.3fV",get_battery_voltage()); + set_program_info(b_voltage); + } + break; + // Stage 6: We didn't find power, keep turning a couple of degrees at a time a recheck + case 6: + time_based_turn_degrees(0.5,4,1); + recharging_state = 4; + break; + // Stage 7: Charge may be finished. Turn 90 degrees then move away and resume previous program + case 7: + time_based_turn_degrees(0.8, 90.0, 1); + recharging_state = 8; + break; + + // Stage 8: Wait for turn to complete + case 8: + if (time_based_motor_action != 1) recharging_state = 9; + break; + // Stage 9: Move away + case 9: + time_based_forward(0.5, 1000000, 1); + recharging_state = 10; + break; + // Stage 10: Wait for move to complete + case 10: + if (time_based_motor_action != 1) recharging_state = 11; + break; + // Stage 11: Check if battery is below low threshold; if it is, start over, else end charge cycle + case 11: + disable_ir_emitters = 0; + if (get_battery_voltage() < battery_low_threshold) { + recharging_state = 1; + } else { + recharging_state = 0; + //Restore name of old program on display + set_program(main_program_state); + } + break; + } +} + + + + +///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +/// curved_walk_with_interaction_program (Alan's Random Walk\Obstacle Avoid and Robot Interaction Program) + +enum random_walk_state {random_walk, turn_towards, interact, turn_away, avoid_obstacle}; +enum random_walk_state internal_state = random_walk; +char action_timeout = 0; +char interaction_timeout = 0; +char random_walk_timeout = 0; +float previous_left_motor_speed = 0.5; +float previous_right_motor_speed = 0.5; + +void curved_random_walk_with_interaction_program() +{ + if(internal_state == random_walk) { + if(interaction_timeout < 4) + interaction_timeout++; + + int closest_robot = -1; + unsigned short shortest_distance = 0; + + // Check whether there are any other robots within range + for(int i = 0; i < 8; i++) { + if(robots_found[i]) { + if(robots_distance[i] > shortest_distance) { + shortest_distance = robots_distance[i]; + closest_robot = i; + } + } + } + + // Turn towards the closest robot + if(closest_robot >= 0 && shortest_distance > 300 && interaction_timeout >= 4) { + time_based_turn_degrees(1, robots_heading[closest_robot], 1); + + action_timeout = 0; + internal_state = turn_towards; + char temp_message[17]; + sprintf(temp_message,"FACE ROBOT %d",closest_robot); + set_program_info(temp_message); + } else { // Otherwise, do a random walk + // Check the front sensors for obstacles + if(reflected_sensor_data[0] > obstacle_avoidance_threshold || + reflected_sensor_data[1] > obstacle_avoidance_threshold || + reflected_sensor_data[6] > obstacle_avoidance_threshold || + reflected_sensor_data[7] > obstacle_avoidance_threshold) { + // Ignore the rear sensors when calculating the heading + reflected_sensor_data[2] = 0; + reflected_sensor_data[3] = 0; + reflected_sensor_data[4] = 0; + reflected_sensor_data[5] = 0; + + // Turn 180 degrees away from the sensed obstacle + int heading = get_bearing_from_ir_array (reflected_sensor_data) + 180; + + // Normalise + heading %= 360; + + if(heading < -180) + heading += 360; + + if(heading > 180) + heading -= 360; + set_program_info("AVOID OBSTACLE"); + time_based_turn_degrees(1, heading, 1); + + action_timeout = 0; + internal_state = turn_away; + } else { + // Change motor speeds every 1s + if(random_walk_timeout >= 2) { + float random_offset = (((float) rand() / (float) RAND_MAX) - 0.5) * 0.5; + + float left_motor_speed = previous_left_motor_speed - random_offset; + float right_motor_speed = previous_right_motor_speed + random_offset; + + float threshold = 0.25; + + if(left_motor_speed < threshold) + left_motor_speed = threshold; + + if(right_motor_speed < threshold) + right_motor_speed = threshold; + + set_left_motor_speed(left_motor_speed); + set_right_motor_speed(right_motor_speed); + + random_walk_timeout = 0; + } + + random_walk_timeout++; + } + } + } else if(internal_state == turn_towards) { + if(action_timeout < 4) + action_timeout++; + else { + set_program_info("SAY HELLO"); + vibrate(); + + action_timeout = 0; + internal_state = interact; + } + } else if(internal_state == interact) { + if(action_timeout < 4) + action_timeout++; + else { + set_program_info("TURN AROUND"); + time_based_turn_degrees(1, 180, 1); + + action_timeout = 0; + internal_state = turn_away; + } + + } else if(internal_state == turn_away) { + if(action_timeout < 4) + action_timeout++; + else { + set_program_info("RANDOM WALK"); + interaction_timeout = 0; + internal_state = random_walk; + } + } else if(internal_state == avoid_obstacle) { + if(action_timeout < 4) + action_timeout++; + else + set_program_info("RANDOM WALK"); + internal_state = random_walk; + } +} + + + + + +///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +/// straight_random_walk_with_interaction_program + +void straight_random_walk_with_interaction_program() +{ + +} + + + + + +///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +/// find_space_program + +char prog_debug = 1 ; +float target_wheel_speed; +int random_walk_bearing; + +void find_space_program(char bidirectional) +{ + // The find_space_program is a continuous turn-move vector program + + if(program_run_init == 1) { + // Setup the LEDs to red + set_leds(0x00,0xFF); + set_center_led(1,1); + program_run_init = 0; + random_walk_bearing = rand() % 360; + } + + // When step_cycle = 0 we calculate a vector to move to and a target distance + if(step_cycle == 0) { + struct FloatVector target_vector; + target_vector.angle = 0; + target_vector.distance = 0; + // Check for near robots within range + for(int i = 1; i < 8; i++) { + if(robots_found[i]) { + int res_bearing = robots_heading[i]; + res_bearing += 180; + if(res_bearing > 180) res_bearing -= 360; + target_vector = addVector(target_vector,res_bearing,robots_distance[i]); + 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); + } + } + if(target_vector.angle!=0){ + set_leds(0xFF,0xFF); + set_center_led(3,1); + } + // Check for obstacles + char obstacle = 0; + int peak_strength = 0; + for(int i=0; i<8; i++){ + if(reflected_sensor_data[i] > peak_strength) peak_strength = reflected_sensor_data[i]; + if(peak_strength > obstacle_avoidance_threshold) obstacle = 1; + } + if(obstacle){ + //Choose new random walk bearing + set_leds(0x00,0xFF); + set_center_led(1,1); + random_walk_bearing = rand() % 360; + int obstacle_bearing = get_bearing_from_ir_array (reflected_sensor_data); + int obs_bearing = obstacle_bearing + 180; + if(obs_bearing > 180) obs_bearing -= 360; + target_vector = addVector(target_vector,obs_bearing,peak_strength); + 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); + } + + if(target_vector.angle == 0 && target_vector.distance == 0){ + set_leds(0xFF,0x00); + set_center_led(2,1); + random_walk_bearing += 180; + if(random_walk_bearing > 360) random_walk_bearing -= 360; + target_vector.distance = 100; + int current_bearing = 360 - beacon_heading; + //Now work out turn needed to face intended heading + int target_turn = (random_walk_bearing - current_bearing) % 360; + if(target_turn > 180) target_turn -= 360; + if(target_turn < -180) target_turn += 360; + target_vector.angle = target_turn; + if(prog_debug) out("Random walk bearing:%d Current:%d Target:%f\n",random_walk_bearing,current_bearing,target_vector.angle); + } + char wheel_direction = 1; + if(bidirectional){ + // Allow reverse wheel direction + if(target_vector.angle < -90) {target_vector.angle += 180; wheel_direction = 0;} + else if(target_vector.angle > 90) {target_vector.angle -= 180; wheel_direction = 0;} + } + //Now turn to angle + float maximum_turn_angle = get_maximum_turn_angle(BEACON_PERIOD*10); + if(target_vector.angle < 0){ + if(target_vector.angle < -maximum_turn_angle){ + target_vector.angle = -maximum_turn_angle; + target_vector.distance = 100; + } + }else{ + if(target_vector.angle > maximum_turn_angle){ + target_vector.angle = maximum_turn_angle; + target_vector.distance = 100; + } + } + //Set the wheel speed for next action + if(target_vector.distance < 120) target_wheel_speed = 0.25; + else if(target_vector.distance < 240) target_wheel_speed = 0.35; + else if(target_vector.distance < 480) target_wheel_speed = 0.45; + else if(target_vector.distance < 960) target_wheel_speed = 0.55; + else target_wheel_speed = 0.65; + if(wheel_direction == 0) target_wheel_speed = 0-target_wheel_speed; + + //Now turn... + time_based_turn_degrees(1, (int) target_vector.angle, 1); + } else time_based_forward(target_wheel_speed,BEACON_PERIOD*6,0); +} + + + + + +///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +/// clustering_program + + +void clustering_program(char invert, char bidirectional) +{ + // The clustering program is a continuous turn-move vector program + // In normal mode (invert = 0) it is attracted to same-group robots and repels opposite-group, walls and very close same-group robots + // In invert mode (invert = 1) it avoids same-group and is attracted to opposite group + + // Store the robot group: even robots (0) are green, odd robots (1) are red + char group = robot_id % 2; + + if(program_run_init == 1) { + // Setup the LEDs based on robot_id + if(group == 0) { + set_leds(0xFF,0x00); + set_center_led(2,1); + } else { + set_leds(0x00,0xFF); + set_center_led(1,1); + } + program_run_init = 0; + random_walk_bearing = rand() % 360; + } + + // When step_cycle = 0 we calculate a vector to move to and a target distance + if(step_cycle == 0) { + char avoiding_friend = 0; + struct FloatVector target_vector; + target_vector.angle = 0; + target_vector.distance = 0; + // Check for near robots within range + for(int i = 1; i < 8; i++) { + if(robots_found[i]) { + // Determine if the robot is an attractor or a repellor + char attract = 0; + if((invert==0 && ((i%2) == group)) || (invert==1 && ((i%2) != group))) attract = 1; + // Avoid very close attractors to stop collisions + if(attract==1 && robots_distance[i] > robot_avoidance_threshold) {attract = 0; avoiding_friend = 1;} + int res_bearing = robots_heading[i]; + if(attract==0){ + res_bearing += 180; + if(res_bearing > 180) res_bearing -= 360; + } + target_vector = addVector(target_vector,res_bearing,robots_distance[i]); + if(prog_debug) { + if(attract) { + 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); + } else { + 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); + } + } + } + } + + // Check for obstacles + char obstacle = 0; + int peak_strength = 0; + for(int i=0; i<8; i++){ + if(reflected_sensor_data[i] > peak_strength) peak_strength = reflected_sensor_data[i]; + if(peak_strength > obstacle_avoidance_threshold) obstacle = 1; + } + if(obstacle){ + //Choose new random walk bearing + random_walk_bearing = rand() % 360; + int obstacle_bearing = get_bearing_from_ir_array (reflected_sensor_data); + int obs_bearing = obstacle_bearing + 180; + if(obs_bearing > 180) obs_bearing -= 360; + target_vector = addVector(target_vector,obs_bearing,peak_strength); + 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); + } + if(target_vector.angle == 0 && target_vector.distance == 0){ + //I have nothing attracting me so persist with random walk: with a 2% chance pick new bearing + if(rand() % 100 > 97) random_walk_bearing = rand() % 360; + target_vector.distance = 100; + int current_bearing = 360 - beacon_heading; + //Now work out turn needed to face intended heading + int target_turn = (random_walk_bearing - current_bearing) % 360; + if(target_turn > 180) target_turn -= 360; + if(target_turn < -180) target_turn += 360; + target_vector.angle = target_turn; + if(prog_debug) out("Random walk bearing:%d Current:%d Target:%f\n",random_walk_bearing,current_bearing,target_vector.angle); + } + char wheel_direction = 1; + if(bidirectional){ + // Allow reverse wheel direction + if(target_vector.angle < -90) {target_vector.angle += 180; wheel_direction = 0;} + else if(target_vector.angle > 90) {target_vector.angle -= 180; wheel_direction = 0;} + } + //Now turn to angle + float maximum_turn_angle = get_maximum_turn_angle(BEACON_PERIOD*10); + if(target_vector.angle < 0){ + if(target_vector.angle < -maximum_turn_angle){ + target_vector.angle = -maximum_turn_angle; + target_vector.distance = 100; + } + }else{ + if(target_vector.angle > maximum_turn_angle){ + target_vector.angle = maximum_turn_angle; + target_vector.distance = 100; + } + } + if(avoiding_friend) target_vector.distance = 100; + //Set the wheel speed for next action + if(target_vector.distance < 120) target_wheel_speed = 0.25; + else if(target_vector.distance < 240) target_wheel_speed = 0.35; + else if(target_vector.distance < 480) target_wheel_speed = 0.5; + else if(target_vector.distance < 960) target_wheel_speed = 0.65; + else target_wheel_speed = 0.85; + if(wheel_direction == 0) target_wheel_speed = 0-target_wheel_speed; + + //Now turn... + time_based_turn_degrees(1, (int) target_vector.angle, 1); + } else time_based_forward(target_wheel_speed,BEACON_PERIOD*7,0); +} + + +///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +/// stop_program - Pauses robot + +void stop_program() +{ + if(program_run_init == 1) { + save_led_states(); + set_leds(0,0); + set_center_led(0,0); + stop(); + program_run_init = 0; + } +} + + + +///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +/// tag_game_program + +enum tag_game_role {hunter, hunted}; +enum tag_game_role role; +int tag_distance = 500; +char hunters[8]; // Memory of which robots have come within tag distance - assumed to be hunters +Timeout hunter_timeout; // Timeout before robots switch back to being hunted +char initial_hunter = 2; // Robot with this ID is permanently a hunter + +// Resets hunter robots to hunted after timeout +void role_reset() +{ + role = hunted; + set_program_info("HUNTED"); + // Clear memory of hunters + for(int i = 0; i < 8; i++) + hunters[i] = 0; +} + +void tag_game_program() +{ + // Initialisation + if(program_run_init == 1) { + + // Set robot roles + if(robot_id == initial_hunter){ + role = hunter; + set_program_info("FIRST HUNTER"); + } + else { + role = hunted; + set_program_info("HUNTED"); + } + // Clear memory of hunters + for(int i = 0; i < 8; i++) + hunters[i] = 0; + + program_run_init = 0; + } + + // Bias forward movement + float left_motor_speed = 0.5; + float right_motor_speed = 0.5; + + // Check the front sensors for obstacles + if(reflected_sensor_data[0] > obstacle_avoidance_threshold || + reflected_sensor_data[1] > obstacle_avoidance_threshold || + reflected_sensor_data[6] > obstacle_avoidance_threshold || + reflected_sensor_data[7] > obstacle_avoidance_threshold) + { + // Ignore the rear sensors when calculating the heading + reflected_sensor_data[2] = 0; + reflected_sensor_data[3] = 0; + reflected_sensor_data[4] = 0; + reflected_sensor_data[5] = 0; + + // Get heading of sensed obstacle + int heading = get_bearing_from_ir_array(reflected_sensor_data); + + // Turn in opposite direction + if(heading > 0 && heading < 90) + { + left_motor_speed -= 0.75; + right_motor_speed += 0.5; + } + else if(heading < 0 && heading > -90) + { + left_motor_speed += 0.5; + right_motor_speed -= 0.75; + } + + set_left_motor_speed(left_motor_speed); + set_right_motor_speed(right_motor_speed); + + // Return early - obstacle avoidance is the top priority + return; + } + + int closest_robot = -1; + unsigned short shortest_distance = 0; + + // Check whether there are any other robots within range + for(int i = 0; i < 8; i++) + { + if(robots_found[i]) + { + if(robots_distance[i] > shortest_distance) + { + shortest_distance = robots_distance[i]; + closest_robot = i; + } + } + } + + // If the closest robot is within tag distance, this robot has been tagged + if(shortest_distance > tag_distance) + { + // Switch role to hunter + if(role == hunted) + { + role = hunter; + set_program_info("NEW HUNTER"); + // Reset to hunted after 10 seconds + hunter_timeout.attach(&role_reset, 10); + } + + // Keep a record of which robot tagged them, so hunters do not chase hunters + // Unless they are the initial hunter, who is aggresive towards everyone + if(robot_id != initial_hunter) + hunters[closest_robot] = 1; + } + + if(role == hunter) + { + // Set LEDS to red + set_leds(0x00, 0xFF); + set_center_led(1, 1); + + if(closest_robot >= 0 && !hunters[closest_robot]) // Ignore other hunters + { + // Turn towards closest hunted robot (unless it is straight ahead, or behind) + if(robots_heading[closest_robot] > 22.5 && robots_heading[closest_robot] < 90) + { + left_motor_speed += 0.5; + right_motor_speed -= 0.5; + } + else if(robots_heading[closest_robot] < -22.5 && robots_heading[closest_robot] > -90) + { + left_motor_speed -= 0.5; + right_motor_speed += 0.5; + } + } + + set_left_motor_speed(left_motor_speed); + set_right_motor_speed(right_motor_speed); + } + else // role == hunted + { + // Set LEDs to green + set_leds(0xFF, 0x00); + set_center_led(2, 1); + + // Avoid everyone + if(closest_robot >= 0) + { + // Turn away from closest robot + if(robots_heading[closest_robot] >= 0 && robots_heading[closest_robot] < 90) + { + left_motor_speed -= 0.5; + right_motor_speed += 0.5; + } + else if(robots_heading[closest_robot] < 0 && robots_heading[closest_robot] > -90) + { + left_motor_speed += 0.5; + right_motor_speed -= 0.5; + } + } + + set_left_motor_speed(left_motor_speed); + set_right_motor_speed(right_motor_speed); + } +} + +///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +/// flocking_program + +void flocking_program() +{ + char display_line[16] = " "; + int average_heading = 0; + int number_of_neighbours = 0; + int number_of_flocking_headings = 0; + int target_heading = 0; + int angles = 0; + int ir_sensor_threshold = 600; + int sensors_activated = 0; + + struct FloatVector obstacle_vector; + obstacle_vector.angle = 0; + obstacle_vector.distance = 0; + + struct FloatVector cohesion_vector; + cohesion_vector.angle = 0; + cohesion_vector.distance = 0; + + for(int i = 0; i < 8; i++) + { + if(reflected_sensor_data[i] > ir_sensor_threshold) + { + obstacle_vector = addVector(obstacle_vector, get_bearing_from_ir_array(reflected_sensor_data), reflected_sensor_data[i]); + sensors_activated++; + } + + if(robots_found[i]) + { + // -180 degrees is reserved for "no byte received" + if(flocking_headings[i] != -180) + { + average_heading += flocking_headings[i]; + number_of_flocking_headings++; + } + + cohesion_vector = addVector(cohesion_vector, robots_heading[i], robots_distance[i]); + number_of_neighbours++; + } + } + + cohesion_vector.distance /= number_of_neighbours; // Normalise + obstacle_vector.distance /= sensors_activated; // Normalise + + int obstacle_avoidance_angle; + + if(sensors_activated > 0) + { + obstacle_avoidance_angle = obstacle_vector.angle + 180; + + if(obstacle_avoidance_angle > 180) + obstacle_avoidance_angle -= 360; + if(obstacle_avoidance_angle < -180) + obstacle_avoidance_angle += 360; + + target_heading += obstacle_avoidance_angle; + angles++; + } + + int cohesion_angle; + + // Don't bother performing cohesion if robots are already close enough + if(number_of_neighbours > 0 && cohesion_vector.distance < 200) + { + cohesion_angle = cohesion_vector.angle; + + if(cohesion_angle > 180) + cohesion_angle -= 360; + if(cohesion_angle < -180) + cohesion_angle += 360; + + target_heading += cohesion_angle; + angles++; + } + + int relative_flocking_heading; + + if(number_of_flocking_headings > 0) + { + average_heading /= number_of_flocking_headings; + + relative_flocking_heading = beacon_heading - average_heading; + + if(relative_flocking_heading > 180) + relative_flocking_heading -= 360; + if(relative_flocking_heading < -180) + relative_flocking_heading += 360; + + target_heading += relative_flocking_heading; + angles++; + } + + if(angles > 0) + target_heading /= angles; // Calculate average + + float left_motor_speed = 0.25; + float right_motor_speed = 0.25; + + if(target_heading > 22.5) + right_motor_speed *= -1; + else if(target_heading < -22.5) + left_motor_speed *= -1; + + set_left_motor_speed(left_motor_speed); + set_right_motor_speed(right_motor_speed); + + // Only transmit beacon heading if the beacon is visible + if(beacon_found) + { + float degrees_per_value = 256.0f / 360.0f; + char beacon_heading_byte = (beacon_heading + 180) * degrees_per_value; // Convert beacon heading from +/-180 degrees into a single byte value + + bt.putc(beacon_heading_byte); + } + + sprintf(display_line, "%d %d %d %d", target_heading, obstacle_avoidance_angle, cohesion_angle, relative_flocking_heading); + + set_program_info(display_line); +} + +///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +/// generic_program - Framework for building typical programs + +void generic_program() +{ + // Do something on the first run of a program + if(program_run_init == 1) { + // Initialisation code goes here... + + program_run_init = 0; + } + + // 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) + if(step_cycle == 0) { + // Do something based on sensor data (eg turn) + } else { + // Do something ignoring sensor data (eg move, or nothing!) + } + +} \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/BeautifulMeme/programs.h Tue Mar 15 00:58:43 2016 +0000 @@ -0,0 +1,22 @@ +/// PsiSwarm Beautiful Meme Project Source Code +/// Version 0.41 +/// James Hilder, Alan Millard, Homero Elizondo, Jon Timmis +/// University of York + +// programs.h - Various PsiSwarm Programs for Beautiful Meme Project + +#ifndef PROGRAMS_H +#define PROGRAMS_H + +void recharging_program(void); +void head_to_bearing_program(int target_bearing); +void curved_random_walk_with_interaction_program(void); +void straight_random_walk_with_interaction_program(void); +void find_space_program(char bidirectional); +void clustering_program(char invert, char bidirectional); +void stop_program(void); +void role_reset(void); +void tag_game_program(void); +void flocking_program(void); + +#endif \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/BeautifulMeme/vector.cpp Tue Mar 15 00:58:43 2016 +0000 @@ -0,0 +1,23 @@ +/// PsiSwarm Beautiful Meme Project Source Code +/// Version 0.41 +/// James Hilder, Alan Millard, Homero Elizondo, Jon Timmis +/// University of York + +// vector.cpp - Functions for calculating floating point bearing vectors + +#include "bmeme.h" + + +struct FloatVector addVector(struct FloatVector in_vector, int angle, int distance){ + struct FloatVector out_vector; + float sin_component_old = sin(in_vector.angle * TO_RAD) * in_vector.distance; + float cos_component_old = cos(in_vector.angle * TO_RAD) * in_vector.distance; + float sin_component_new = sin(angle * TO_RAD) * distance; + float cos_component_new = cos(angle * TO_RAD) * distance; + float sin_component_sum = sin_component_old + sin_component_new; + float cos_component_sum = cos_component_old + cos_component_new; + out_vector.distance = sqrt((sin_component_sum * sin_component_sum) + (cos_component_sum * cos_component_sum)); + out_vector.angle = atan2(sin_component_sum ,cos_component_sum) * TO_DEG; + //out("Angle:%f Distance:%f\n",out_vector.angle,out_vector.distance); + return out_vector; +} \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/BeautifulMeme/vector.h Tue Mar 15 00:58:43 2016 +0000 @@ -0,0 +1,21 @@ +/// PsiSwarm Beautiful Meme Project Source Code +/// Version 0.41 +/// James Hilder, Alan Millard, Homero Elizondo, Jon Timmis +/// University of York + +// vector.h - Functions for calculating floating point bearing vectors + +#ifndef VECTOR_H +#define VECTOR_H + +#define TO_DEG 57.2958 +#define TO_RAD 0.017453 + +struct FloatVector{ + float angle; + float distance; +}; + +struct FloatVector addVector(struct FloatVector in_Vector, int angle, int distance); + +#endif \ No newline at end of file
--- a/PsiSwarm.lib Thu Mar 03 23:22:01 2016 +0000 +++ b/PsiSwarm.lib Tue Mar 15 00:58:43 2016 +0000 @@ -1,1 +1,1 @@ -https://developer.mbed.org/users/jah128/code/PsiSwarmLibrary/#060690a934a9 +https://developer.mbed.org/users/jah128/code/PsiSwarmLibrary/#7c0d1f581757
--- a/beacon.cpp Thu Mar 03 23:22:01 2016 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,376 +0,0 @@ -/// PsiSwarm Beautiful Meme Project Source Code -/// Version 0.4 -/// James Hilder, Alan Millard, Homero Elizondo, Jon Timmis -/// University of York - -// beacon.cpp - Functions for detecting the beacon and taking IR readings of the robots - -#include "main.h" - -int pulse_step = 1; //Pulse-step corresponds to which timeslot (0-9) is currently active, where beacon=0 and robots=2-8 -int low_threshold; //Set to be 2x mean background IR -int beacon_threshold; //Set to be 4x mean background IR -unsigned short ir_sensor_data[9][8]; // The raw sensor data from all 9x 50ms sample windows -Ticker ir_sample_ticker; // Ticker for the IR data sampling and processing; runs every 50ms in middle of timeslot -Ticker ir_emitter_ticker; // Ticker for turning on the IR emitters; runs every 50ms near start of timeslot -Timeout ir_emitter_timeout; // Timeout for turning off the IR emitters after 40ms -Timer beacon_debug_timer; // Timer for debug information only [remove later?] - -char show_ir_debug_info = 0; // Set to 1 to display (via PC) the list of IR readings & visible robots every timestep - -/// The locate beacon function samples the IR radiation from all 8 side sensors over a period of 1 second in [BEACON_PERIOD / 2.5] (20ms) blocks. -/// The infrared beacon is set to give a 50ms burst of IR every 500ms. We should thus see in the sampled radiation 2 blocks -/// of samples, 2 or 3 samples in duration, when a significant peak occurs; the blocks should be 25 samples apart. -void locate_beacon() -{ - int sample_period = (BEACON_PERIOD * 2) / 5; - out("1) Searching for IR beacon..."); - unsigned short samples[50][9]; - Timer beacon_timer; - beacon_timer.start(); - int offset = 0; - //This loop samples the background IR values at 50Hz for 1 second and stores in an array - for(int i=0; i<50; i++) { - store_background_raw_ir_values (); - if(i%2 == 0){ - set_center_led(1, 0.5); - set_leds(0xAA,0x55); - }else{ - set_center_led(2, 0.5); - set_leds(0x55,0xAA); - } - samples[i][8]=0; - for(int j=0; j<8; j++) { - samples[i][j] = get_background_raw_ir_value(j); - samples[i][8] += get_background_raw_ir_value(j); - } - offset+=sample_period; - while(beacon_timer.read_us() < offset) {} - } - - //Print values: for testing [comment out] - /* - for(int i=0; i<50; i++) { - out("IR %d:",i); - for(int j=0; j<8; j++) { - out("[%d:%d]",j,samples[i][j]); - } - out(" [SUM:%d]\n",samples[i][8]); - } - */ - - //Bubble sort sums to find (6) highest values - unsigned short sorted_array[50]; - for(int i=0; i<50; i++) { - sorted_array[i]=samples[i][8]; - } - for (int c = 0 ; c < 49; c++) { - for (int d = 0 ; d < (50-c-1); d++) { - if (sorted_array[d] > sorted_array[d+1]) { - unsigned short swap = sorted_array[d]; - sorted_array[d] = sorted_array[d+1]; - sorted_array[d+1] = swap; - } - } - } - - //Print sorted values: for testing [comment out] - /* - out("Sorted values:"); - for (int c = 0 ; c < 50 ; c++ ) { - out("%d", sorted_array[c]); - if(c<49)out(","); - } - out("\n"); - */ - - // Calculate mean background sum value by looking at 44 lowest sum values - int background_mean = 0; - for(int i=0;i<44;i++)background_mean += sorted_array[i]; - background_mean /= 44; - - //out("Background mean value: %d\n",background_mean); - - //Our beacon threshold will be 4x the background mean value; find all instances where this occurs - low_threshold = background_mean * 2; - beacon_threshold = background_mean * 4; - char beacon_detected_indices[50]; - for(int i=0;i<50;i++){ - if(samples[i][8] > beacon_threshold) beacon_detected_indices[i]=1; - else beacon_detected_indices[i]=0; - } - //Count and display matches - int beacon_detected_count = 0; - //char output_string[251] = ""; - for(int i=0;i<50;i++){ - if(beacon_detected_indices[i] == 1){ - beacon_detected_count++; - // char index_string[6]; - // sprintf(index_string,"[%d],",i); - // strcat(output_string,index_string); - } - } - //out("%d samples are above threshold:%s\n",beacon_detected_count,output_string); - - //We will use this array to store average values for each sensor when the beacon is detected - unsigned short beacon_averages[8]; - char beacon_averages_count = 0; - for(int i=0;i<8;i++)beacon_averages[i]=0; - - //Now determine if the beacon is correctly found: must adhere to a set of rules - //Firstly, we should have not less than 4 and not more than 6 positive matches - if(beacon_detected_count>3 && beacon_detected_count<7){ - // Now verify that the positive samples are in valid places... - // Find first positive sample - int first_index = 0; - //out("Here\n",first_index); - - while(beacon_detected_indices[first_index]==0)first_index ++; - - //out("First index:%d\n",first_index); - - - // Check if first index is zero: if so, we need to check index 49 (and 48) to see if they are also high - if(first_index == 0){ - if(beacon_detected_indices[49]>0)first_index = 49; - if(beacon_detected_indices[48]>0)first_index = 48; - } - - beacon_averages_count++; - for(int i=0;i<8;i++){beacon_averages[i]+=samples[first_index][i];} - - // Now count the length of the 'block' of positive hits: must be equal to 2 or 3 - char block_length = 1; - int end_index = first_index + 1; - if(end_index == 50) end_index = 0; - while(beacon_detected_indices[end_index]>0){ - beacon_averages_count++; - for(int i=0;i<8;i++){beacon_averages[i]+=samples[end_index][i];} - block_length ++; - end_index ++; - if(end_index == 50) end_index = 0; - } - if(block_length==2 || block_length == 3){ - //We have found the first correct block and it is valid; now calculate its mid-point and check that the second block is also present 500ms later - float mid_point; - char second_block_okay = 0; - if(block_length == 2){ - mid_point = first_index + 0.5; - char second_block_low = first_index + 25; - char second_block_high = first_index + 26; - if(second_block_low > 49) second_block_low -= 50; - if(second_block_high > 49) second_block_high -= 50; - beacon_averages_count+=2; - for(int i=0;i<8;i++){beacon_averages[i]+=samples[second_block_low][i]+samples[second_block_high][i];} - if(beacon_detected_indices[second_block_low]>0 && beacon_detected_indices[second_block_high]>0) second_block_okay = 1; - } - if(block_length == 3){ - mid_point = first_index + 1; - if(mid_point == 50) mid_point = 0; - char second_block_single = first_index + 26; - if(second_block_single > 49) second_block_single -= 50; - beacon_averages_count++; - for(int i=0;i<8;i++){beacon_averages[i]+=samples[second_block_single][i];} - if(beacon_detected_indices[second_block_single]>0) second_block_okay = 1; - } - if(second_block_okay >0){ - beacon_found = 1; - beacon_heading = get_bearing_from_ir_array(beacon_averages); - out("Found at %d degrees\n",beacon_heading); - //for(int i=0;i<8;i++){ - // beacon_averages[i] /= beacon_averages_count; - // out("[%d]",beacon_averages[i]); - //} - out("2) Synchronising...\n"); - // Calculate the offset to the expected start of the next beacon pulse - int microseconds_offset = (sample_period * mid_point) - sample_period - (sample_period / 4); - //out("MS Offset:%d Midpoint:%f\n Current Time:%d\n",microseconds_offset,mid_point,beacon_timer.read_us()); - int cycle_period = (BEACON_PERIOD * 10); - if(microseconds_offset < 0) microseconds_offset += cycle_period; - //If we have missed the start of the beacon this cycle, wait until the next cycle - while(beacon_timer.read_us()% (cycle_period) > microseconds_offset){}; - //Now wait until the start of the beacon pulse - while(beacon_timer.read_us()% (cycle_period) < microseconds_offset){}; - /* - out("Now:%d",beacon_timer.read_us()); - Timer test_timer; - test_timer.start(); - for(int i=0;i<50;i++){ - store_background_raw_ir_values (); - out("Time %d: %d\n",test_timer.read_ms(),get_background_raw_ir_value(2)); - while(test_timer.read_ms() % 10 < 9){}; - } - */ - }else{ - beacon_found = 0; - out("Beacon not found: a matching second block %dms after first block not detected\n",(BEACON_PERIOD / 100)); - } - }else{ - beacon_found = 0; - if(block_length == 1) out("Beacon not found: a single sample [%d] was high but not its neighbours\n",first_index); - if(block_length > 3) out("Beacon not found: a block of %d high samples was detected\n",block_length); - } - } else { - beacon_found = 0; - if(beacon_detected_count > 6) out("Beacon not found: too many high samples [%d]\n",beacon_detected_count); - else out("Beacon not found: too few high samples [%d]\n",beacon_detected_count); - } - if(beacon_found == 0){ - set_leds(0x00,0x00); - set_center_led(1, 1); - display.clear_display(); - display.set_position(0,0); - display.write_string("BEACON NOT FOUND"); - } -} - -// The start_infrared_timers() function is called as soon as the beacon has been detected and synchronised to -// It launches 2 tickers at offset times; the first is responsible for turning the robots IR emitters on in its proper timeslot -// The other reads the values given from the IR sensor in the middle of each timeslot and processes that information in the final timeslot -void start_infrared_timers() -{ - // At this point we should be exactly at the start of a beacon cycle. - // We want the emitter ticker to start in approx 5ms (this will let us set a 40ms pulse) - // We want the sample ticker to start in approx 25ms (this will let us sample in the middle each step - out("3) Starting TDMA infrared timers\n"); - beacon_debug_timer.start(); - wait_us(BEACON_PERIOD / 10); - ir_emitter_ticker.attach_us(emitter_ticker_block,BEACON_PERIOD); - wait_us(((BEACON_PERIOD * 4) / 10)); //Wait for middle of pulse - ir_sample_ticker.attach_us(sample_ticker_block,BEACON_PERIOD); -} - - -//Return the max value in IR array -unsigned short get_highest_sample(unsigned short * ir_array){ - unsigned short highest = 0; - for(int i=0;i<8;i++){ - if(ir_array[i]>highest) highest=ir_array[i]; - } - return highest; -} - -//Return the sum total of IR array -unsigned short get_sum_sample(unsigned short * ir_array){ - unsigned short sum = 0; - for(int i=0;i<8;i++){ - sum+=ir_array[i]; - } - return sum; -} - -//The emitter_ticker_block function runs every 50ms and turns the IR emitters on when pulse_step-1 matches the robot ID -//It then starts a timeout to run emitter_timeout_block after 40ms, which turns off the emitters -void emitter_ticker_block(){ - //If the time-step (-1) equals my ID, turn on my emitters for 40ms - if(pulse_step-1 == robot_id && disable_ir_emitters == 0){ - IF_set_IR_emitter_output(0, 1); - IF_set_IR_emitter_output(1, 1); - ir_emitter_timeout.attach_us(emitter_timeout_block,(BEACON_PERIOD * 8)/10); - } -} - -//Turn off the emitters -void emitter_timeout_block(){ - //Turn off IR emitters - IF_set_IR_emitter_output(0, 0); - IF_set_IR_emitter_output(1, 0); -} - -//The function sample_ticker_block() is called every 50ms, and should run close to the middle of every timeslot -//There are 10 time slots in each 500ms period -//Slot 0 is when the beacon is flashing -//Slot 1 should be IR-free and is used to measure background IR data, stored in background_sensor_data[] -//Slot 2-8 are for the 7 robots; slot-1 = robot_id -//In slot 9, the robot processes the data [and doesn't store and new readings] -//It checks if the beacon is visible, if any robots are, calculates their bearings if they are, and transfers the background and active IR data for the robot -void sample_ticker_block(){ - //If we are in time-step 0 to 8, store the background data in an array - if(pulse_step < 9){ - store_background_raw_ir_values (); - for(int i=0;i<8;i++)ir_sensor_data[pulse_step][i]=get_background_raw_ir_value(i); - }else{ - //If not, process the data - for(int i=0;i<9;i++){ - unsigned short sum = get_sum_sample(ir_sensor_data[i]); - unsigned short highest = get_highest_sample(ir_sensor_data[i]); - //Check if beacon is visible - if(i==0){ - if(sum > beacon_threshold){ - beacon_found = 1; - beacon_heading = get_bearing_from_ir_array (ir_sensor_data[0]); - }else beacon_found = 0; - //out("Beacon sum:%d 0:%d 4:%d\n",sum,ir_sensor_data[0][0],ir_sensor_data[0][4]); - } - if(i==1){ - for(int j=0;j<8;j++)background_sensor_data[j]=ir_sensor_data[1][j]; - } - if(i>1){ - char test_robot = i-1; - if(test_robot == robot_id){ - for(int j=0;j<8;j++)reflected_sensor_data[j]=ir_sensor_data[i][j]; - }else{ - if(sum > low_threshold){ - robots_found[test_robot] = 1; - //Debug-- - //out("Robot %d: [%d][%d][%d][%d][%d][%d][%d][%d]\n",test_robot,ir_sensor_data[i][0],ir_sensor_data[i][1],ir_sensor_data[i][2],ir_sensor_data[i][3],ir_sensor_data[i][4],ir_sensor_data[i][5],ir_sensor_data[i][6],ir_sensor_data[i][7]); - robots_heading[test_robot] = get_bearing_from_ir_array (ir_sensor_data[i]); - robots_distance[test_robot] = highest; - }else robots_found[test_robot] = 0; - } - } - } - if(show_ir_debug_info == 1)display_ir_readings(); - } - //Increment pulse step - pulse_step++; - if(pulse_step == 10) pulse_step = 0; -} - - -//Testing function to print out lines showing what robot can currently see in terms of beacon, other robots and obstacles -void display_ir_readings() -{ - out("____________________________________\nInfrared Detection at %d ms\n",beacon_debug_timer.read_ms()); - if(beacon_found==1){ - out("Beacon detected at %d degrees\n",beacon_heading); - } - for(int j=1;j<8;j++){ - if(robots_found[j])out("Robot %d detected at %d degrees, %d distance\n",j,robots_heading[j],robots_distance[j]); - } - out("Reflected values:"); - for(int i=0;i<8;i++){ - out("[%d,%d]",i,reflected_sensor_data[i]); - } - out("\nBackground values:"); - for(int i=0;i<8;i++){ - out("[%d,%d]",i,background_sensor_data[i]); - } - out("\n\n"); -} - -//Returns a 0 if turn is likely to complete in a single timestep, 1 if it is beyond range for single timestep and 2 if the beacon is not found so bearing unknown -char turn_to_bearing(int bearing) -{ - if(beacon_found == 0){ - out("Beacon not found: cannot turn to specific bearing\n"); - return 2; - }else{ - //First calculate the bearing using the angle of beacon relative to robot - int current_bearing = 360 - beacon_heading; - //Now work out turn needed to face intended heading - int target_turn = (bearing - current_bearing) % 360; - //Adjust to take 10% off turn, stops overshoot - target_turn = (target_turn * 9) / 10; - if(target_turn > 180) target_turn -= 360; - if(target_turn < -180) target_turn += 360; - //We can't reliably turn more than 280 degrees per second, so set a limit for the turn to that - char beyond_limit = 0; - int turn_limit = BEACON_PERIOD / 358; - if(target_turn > turn_limit) {target_turn = turn_limit; beyond_limit = 1;}; - if(target_turn < -turn_limit) {target_turn = -turn_limit; beyond_limit = 1;}; - out("Turning %d degrees\n",target_turn); - time_based_turn_degrees(1, target_turn,1); - return beyond_limit; - } -} \ No newline at end of file
--- a/beacon.h Thu Mar 03 23:22:01 2016 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,20 +0,0 @@ -/// PsiSwarm Beautiful Meme Project Source Code -/// Version 0.4 -/// James Hilder, Alan Millard, Homero Elizondo, Jon Timmis -/// University of York - -// beacon.h - Functions for detecting the beacon and taking IR readings of the robots - -#ifndef BEACON_H -#define BEACON_H - -void emitter_ticker_block(void); -void sample_ticker_block(void); -void emitter_timeout_block(void); -void locate_beacon(void); -void start_infrared_timers(void); -unsigned short get_highest_sample(unsigned short * ir_array); -unsigned short get_sum_sample(unsigned short * ir_array); -char turn_to_bearing(int bearing); - -#endif \ No newline at end of file
--- a/main.cpp Thu Mar 03 23:22:01 2016 +0000 +++ b/main.cpp Tue Mar 15 00:58:43 2016 +0000 @@ -9,149 +9,32 @@ **(C) Dr James Hilder - York Robotics Laboratory - University of York ** ***********************************************************************/ -/// PsiSwarm Beautiful Meme Project Source Code -/// Version 0.4 -/// James Hilder, Alan Millard, Homero Elizondo, Jon Timmis +/// PsiSwarm Blank Example Code +/// Version 0.41 +/// James Hilder, Alan Millard, Alexander Horsfield, Homero Elizondo, Jon Timmis /// University of York /// Include main.h - this includes psiswarm.h all the other necessary core files #include "main.h" -char * program_name = "B-Meme"; +char * program_name = "Blank"; char * author_name = "YRL"; -char * version_name = "0.4"; - -// IMPORTANT!!! -// Do not call the IR functions at all as they will interfere with the correct operation of this program -// Instead, use the values held in the variables below; they are updated every 500ms - -char beacon_found = 0; // This will be a 1 when a beacon was detected during the previous 500ms window -int beacon_heading = 0; // This is the heading from the last time a beacon was detected -char robots_found[8]; // These will be a 1 when the respective robot [excluding self] was detected during the previous 500ms window -int robots_heading[8]; // These are the headings from the last time the respective robots were detected -unsigned short robots_distance[8]; // This is the maximum sensor value from the last time the respective robot was detected -unsigned short reflected_sensor_data[8]; // The reflected IR values when this robots emitters are on -unsigned short background_sensor_data[8];// The raw IR values when no robot (or beacon) should have its IR on +char * version_name = "0.41"; -char default_normal_program = 8; // The program to run on turn on (after 'face beacon' program) -char use_recharging_program = 0; // Set to 1 to force robot to run recharging program when battery voltage drops below a threshold -char user_code_debug = 1; // Set to 1 to show terminal messages from "out" function [specific to this code] -char display_debug_inf = 0; // Set to 1 to show debug info about beacon\robots on display [instead of running program info] -char main_program_state; // Index of the currently running program -char previous_program; // Used to hold previous running program when it is paused for switch press etc -char program_changed = 0; // Flag to update display when program is changed -char program_run_init = 0; // Flag to tell program to run its initialisation on first loop, if neccessary -char success_count = 0; // Flag to indicate the success of a program -char step_cycle = 0; // Alternates between 0 and 1 in successive time-steps -char target_reached = 0; // Flag to indicate if a program target has been reached -char prog_name [17]; // Stores the name of the running program [line 0 on the display] -char prog_info [17]; // Stores information about the current state of the program [line 1 on the display] -char disable_ir_emitters = 0; // Used to disable IR emission during charging etc [use with caution!] -char recharging_state = 0; // Stores the state of the recharging program (0 is not currently running) -char switch_held = 0; // Used for detected when the cursor switch is held to override program choice -char choose_program_mode = 0; -char program_count = 8; -char program_selection; -int flocking_headings[8]; // Beacon heading of each robot - -float battery_low_threshold = 3.60; // Threshold at which to interrupt program and start recharging routine: suggest 3.55 -float battery_high_threshold = 3.95; // Threshold at which to end battery recharging routine and resume normal program: suggest 4.0 - -Ticker main_loop_ticker; - -///This is the main loop for the Beautiful Meme code. The code block is run once every 250mS* [with 4Hz beacon] once all the IR samples have been collected. -void main_loop() +void user_code_loop() { - if(switch_held == 1)switch_held=2; - if(switch_held == 3 && choose_program_mode == 0) { - //The switch has been held right and then released: stop the current program - previous_program = main_program_state; - program_selection = previous_program; - choose_program_mode = 1; - set_program(255); - set_program_info(get_program_name(program_selection)); - } - if(use_recharging_program == 1)recharging_program(); - update_display(); - if(recharging_state == 0) { - switch(main_program_state) { - case 0: //Case 0 is the initial program: turn to face beacon - if(step_cycle == 0) { - char turn_status = turn_to_bearing(0); - if(turn_status == 0) { - success_count ++; - if(success_count > 1) set_program(default_normal_program); - } else success_count = 0; - } - break; - case 1: - target_reached = 0; - head_to_bearing_program(0); - if(target_reached == 1) set_program(2); - break; - case 2: - target_reached = 0; - head_to_bearing_program(180); - if(target_reached == 1) set_program(1); - break; - case 3: - curved_random_walk_with_interaction_program(); - break; - case 4: - straight_random_walk_with_interaction_program(); - break; - case 5: - find_space_program(1); - break; - case 6: - clustering_program(0,1); - break; - case 7: - tag_game_program(); - break; - case 8: - flocking_program(); - break; - case 255: - stop_program(); - break; - } - } - step_cycle=1-step_cycle; + wait(1); } ///Place user code here that should be run after initialisation but before the main loop void user_code_setup() { - wait(0.8); - //Do nothing forever: - while(1)wait(1.0); + wait(1); display.clear_display(); display.set_position(0,0); - display.write_string("BEAUTIFUL MEME"); - display.set_position(1,0); - display.write_string(" PROJECT"); - wait(0.2); - out("------------------------------------------------------\n"); - out("Beautiful Meme Project Demo Code \n"); - out("------------------------------------------------------\n"); - locate_beacon(); - while(beacon_found == 0) { - wait(0.5); - locate_beacon(); - } - start_infrared_timers(); - main_loop_ticker.attach_us(&main_loop,BEACON_PERIOD * 10); - set_program(0); - set_leds(0x00,0x00); - set_center_led(3,0.5); - display.clear_display(); - display.set_position(0,0); - display.write_string("BEACON FOUND AT"); - display.set_position(1,0); - char degrees_string[16]; - sprintf(degrees_string,"%d DEGREES",beacon_heading); - display.write_string(degrees_string); + display.write_string("No Code"); + + //bmeme_user_code_setup(); } /// Code goes here to handle what should happen when the user switch is pressed @@ -159,38 +42,19 @@ { /// Switch_state = 1 if up is pressed, 2 if down is pressed, 4 if left is pressed, 8 if right is pressed and 16 if the center button is pressed /// NB For maximum compatability it is recommended to minimise reliance on center button press - if(choose_program_mode == 0) { - if(switch_state == 8) switch_held = 1; - else if(switch_state == 0 && switch_held == 2) switch_held = 3; - else switch_held = 0; - } else { - // We are in choose program mode - if(switch_state == 8) { - program_selection ++; - if(program_selection > program_count) program_selection = 0; - if(program_selection == program_count) set_program_info("RECHARGE"); - else set_program_info(get_program_name(program_selection)); - } - if(switch_state == 4) { - if(program_selection == 0) program_selection = program_count; - else program_selection --; - if(program_selection == program_count) set_program_info("RECHARGE"); - else set_program_info(get_program_name(program_selection)); - } - if(switch_state == 1 || switch_state == 2){ - if(program_selection == program_count){ - recharging_state = 1; - set_program(previous_program); - strcpy(prog_name,"CHARGING PROGRAM"); - set_program_info("HEAD TO BEACON"); - - } - else set_program(program_selection); - choose_program_mode = 0; - switch_held = 0; - } - } - //out("Switch:%d Switch_held:%d Program_Selection:%d Program_count:%d Prog_Info:%s\n",switch_state,switch_held,program_selection,program_count,prog_info); + + //bmeme_handle_switch_event(switch_state); +} + +void handle_user_serial_message(char * message, char length, char interface) +{ + // This is where user code for handling a (non-system) serial message should go + // + // message = pointer to message char array + // length = length of message + // interface = 0 for PC serial connection, 1 for Bluetooth + + //bmeme_handle_user_serial_message(message, length, interface); } /// The main routine: it is recommended to leave this function alone and add user code to the above functions @@ -201,99 +65,6 @@ user_code_setup(); user_code_running = 1; while(1) { - wait(1); - } -} - -char * get_program_name(int index) -{ - char * ret_name = new char[17]; - switch(index) { - case 0: - strcpy(ret_name,"FACE BEACON"); - break; - case 1: - strcpy(ret_name,"HEAD TO BEACON"); - break; - case 2: - strcpy(ret_name,"HEAD TO SOUTH"); - break; - case 3: - strcpy(ret_name,"RANDOM WALK 1"); - break; - case 4: - strcpy(ret_name,"RANDOM WALK 2"); - break; - case 5: - strcpy(ret_name,"FIND SPACE"); - break; - case 6: - strcpy(ret_name,"CLUSTERING"); - break; - case 7: - strcpy(ret_name,"TAG GAME"); - break; - case 8: - strcpy(ret_name,"FLOCKING"); - break; - case 255: - strcpy(ret_name,"PROGRAM:"); - break; - } - return ret_name; -} - -void set_program(int index) -{ - main_program_state = index; - program_changed = 1; - program_run_init = 1; - strcpy(prog_info,""); - strcpy(prog_name,get_program_name(index)); -} - -void set_program_info(char * info) -{ - strcpy(prog_info,info); - program_changed = 1; -} - -void update_display() -{ - if(program_changed == 1) { - program_changed = 0; - display.clear_display(); - - if(display_debug_inf==1) display_debug_info(); - else { - display.set_position(0,0); - display.write_string(prog_name); - } - display.set_position(1,0); - display.write_string(prog_info); - } -} - -void display_debug_info() -{ - char disp_line[16] = "- - - - - - - -"; - if(beacon_found==1)disp_line[0]='B'; - for(int i=1; i<8; i++) { - if(robots_found[i])disp_line[((i)*2)]=48+i; - } - display.set_position(0,0); - display.write_string(disp_line); -} - -/// Verbose output -void out(const char* format, ...) -{ - char buffer[256]; - if (debug_mode) { - va_list vl; - va_start(vl, format); - vsprintf(buffer,format,vl); - if(user_code_debug == 1) pc.printf("%s", buffer); - va_end(vl); + user_code_loop(); } } \ No newline at end of file
--- a/main.h Thu Mar 03 23:22:01 2016 +0000 +++ b/main.h Tue Mar 15 00:58:43 2016 +0000 @@ -9,58 +9,22 @@ **(C) Dr James Hilder - York Robotics Laboratory - University of York ** ***********************************************************************/ -/// PsiSwarm Beautiful Meme Project Source Code -/// Version 0.4 -/// James Hilder, Alan Millard, Homero Elizondo, Jon Timmis +/// PsiSwarm Blank Example Code +/// Version 0.41 +/// James Hilder, Alan Millard, Alexander Horsfield, Homero Elizondo, Jon Timmis /// University of York #ifndef MAIN_H #define MAIN_H #include "psiswarm.h" -#include "beacon.h" -#include "programs.h" -#include "vector.h" - -// Define the on-period for a IR pulse in microseconds (eg 50000 for 2Hz system, 25000 for a 4Hz system or 20000 for a 5Hz system) -#define BEACON_PERIOD 25000 +//#include "bmeme.h" -extern char beacon_found; // This will be a 1 if beacon is detected, 0 if it isn't -extern int beacon_heading; // The heading from the last time the beacon was detected -extern char robots_found[8]; // These will be a 1 when the respective robot [excluding self] was detected during the previous 500ms window -extern int robots_heading[8]; // These are the headings from the last time the respective robots were detected -extern unsigned short robots_distance[8]; // This is the maximum sensor value from the last time the respective robot was detected -extern unsigned short reflected_sensor_data[8]; // The reflected IR values when this robots emitters are on -extern unsigned short background_sensor_data[8];// The raw IR values when no robot (or beacon) should have its IR on -extern char disable_ir_emitters; -extern char step_cycle; -extern char target_reached; -extern char recharging_state; -extern char prog_name [17]; -extern char prog_info [17]; -extern char main_program_state; -extern char program_changed; -extern char program_run_init; -extern float battery_low_threshold; -extern float battery_high_threshold; -extern int flocking_headings[8]; - -char * get_program_name(int index); -void set_program(int index); -void set_program_info(char * info); - -void update_display(void); -void display_debug_info(void); - +int main(void); void user_code_setup(void); void user_code_loop(void); -void handle_switch_event(char switch_state); - -int main(void); +void handle_switch_event(char switch_state); +void handle_user_serial_message(char * message, char length, char interface); - -void display_ir_readings(void); -void out(const char* format, ...) ; - #endif \ No newline at end of file
--- a/programs.cpp Thu Mar 03 23:22:01 2016 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,869 +0,0 @@ -/// PsiSwarm Beautiful Meme Project Source Code -/// Version 0.4 -/// James Hilder, Alan Millard, Homero Elizondo, Jon Timmis -/// University of York - -// programs.cpp - Various PsiSwarm Programs for Beautiful Meme Project - -#include "main.h" - -int obstacle_avoidance_threshold = 300; -int robot_avoidance_threshold = 2000; - -///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -/// head_to_bearing_program -char was_turning = 0; - -///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 -void head_to_bearing_program(int target_bearing) -{ - if(step_cycle == 0 || was_turning == 0) { - // Check if we are heading in the right bearing (+- 25 degrees) - int current_bearing = (360 - beacon_heading) % 360; - // Current bearing should range from 0 to 359; target_bearing likewise; check the are within 25 degrees of each other - char bearing_ok = 0; - int lower_bound = target_bearing - 25; - int upper_bound = target_bearing + 25; - if(lower_bound < 0) { - if(current_bearing > (lower_bound + 360) || current_bearing < upper_bound) bearing_ok = 1; - } else if(upper_bound > 359) { - if(current_bearing > lower_bound || current_bearing < (upper_bound - 360)) bearing_ok = 1; - } else if(current_bearing > lower_bound && current_bearing < upper_bound) bearing_ok = 1; - // Check if there is an obstacle in front of us - if((reflected_sensor_data[7] > 1000 || reflected_sensor_data[0] > 1000) && bearing_ok == 1) target_reached = 1; - else { - // Now move forward if we are facing correct bearing, otherwise turn - if(bearing_ok == 1) { - //Check if anything is in front of us to determine speed - if it is, move slowly - int t_time = 6 * BEACON_PERIOD; - float t_speed = 1.0; - if(reflected_sensor_data[7] > 150 || reflected_sensor_data[0] > 150) { - t_time = 4 * BEACON_PERIOD; - t_speed = 0.6; - } - if(reflected_sensor_data[7] > 300 || reflected_sensor_data[0] > 300) { - t_time = 3 * BEACON_PERIOD; - t_speed = 0.4; - } - if(reflected_sensor_data[7] > 500 || reflected_sensor_data[0] > 500) { - t_time = 2 * BEACON_PERIOD; - t_speed = 0.2; - } - time_based_forward(t_speed,t_time,0); - was_turning = 0; - } else { - turn_to_bearing(target_bearing); - was_turning = 1; - } - } - } -} - - - - -///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -/// recharging_program - -char recharge_power_check_count = 0; -char battery_low_count = 0; - -void recharging_program() -{ - switch(recharging_state) { - case 0: - // We are not currently recharging, check the battery state - if(get_battery_voltage() < battery_low_threshold) { - // Battery is below low threshold - battery_low_count ++; - // 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 - if(battery_low_count > 3) { - // Set recharging state to 'looking for charger' - recharging_state = 1; - strcpy(prog_name,"CHARGING PROGRAM"); - set_program_info("HEAD TO BEACON"); - } - } else battery_low_count = 0; - break; - // State 1: Head to beacon [as this is where battery charger is] - case 1: - target_reached = 0; - head_to_bearing_program(0); - if(target_reached == 1) { - recharging_state = 2; - set_program_info("TURN 90 DEGREES"); - } - break; - // Stage 2: Turn 90 degrees to align with charging pads - case 2: - disable_ir_emitters = 1; - time_based_turn_degrees(0.8, 70.0, 1); - recharge_power_check_count = 0; - recharging_state = 3; - break; - // Stage 3: Wait for turn to complete - case 3: - if (time_based_motor_action != 1) { - recharging_state = 4; - set_program_info("CHECK FOR POWER"); - } - break; - // Stage 4: Check if charging - case 4: - recharge_power_check_count++; - if(get_dc_status() == 1) { - recharging_state = 5; - } else { - if(recharge_power_check_count < 10)recharging_state = 6; - else { - recharging_state = 7; - set_program_info("NO POWER - RETRY"); - } - } - break; - // Stage 5: Charging. Wait until threshold voltage exceeded - case 5: - if(get_battery_voltage() > battery_high_threshold) { - set_program_info("LEAVE CHARGER"); - recharging_state = 7; - } else { - char b_voltage[16]; - sprintf(b_voltage,"CHARGE: %1.3fV",get_battery_voltage()); - set_program_info(b_voltage); - } - break; - // Stage 6: We didn't find power, keep turning a couple of degrees at a time a recheck - case 6: - time_based_turn_degrees(0.5,4,1); - recharging_state = 4; - break; - // Stage 7: Charge may be finished. Turn 90 degrees then move away and resume previous program - case 7: - time_based_turn_degrees(0.8, 90.0, 1); - recharging_state = 8; - break; - - // Stage 8: Wait for turn to complete - case 8: - if (time_based_motor_action != 1) recharging_state = 9; - break; - // Stage 9: Move away - case 9: - time_based_forward(0.5, 1000000, 1); - recharging_state = 10; - break; - // Stage 10: Wait for move to complete - case 10: - if (time_based_motor_action != 1) recharging_state = 11; - break; - // Stage 11: Check if battery is below low threshold; if it is, start over, else end charge cycle - case 11: - disable_ir_emitters = 0; - if (get_battery_voltage() < battery_low_threshold) { - recharging_state = 1; - } else { - recharging_state = 0; - //Restore name of old program on display - set_program(main_program_state); - } - break; - } -} - - - - -///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -/// curved_walk_with_interaction_program (Alan's Random Walk\Obstacle Avoid and Robot Interaction Program) - -enum random_walk_state {random_walk, turn_towards, interact, turn_away, avoid_obstacle}; -enum random_walk_state internal_state = random_walk; -char action_timeout = 0; -char interaction_timeout = 0; -char random_walk_timeout = 0; -float previous_left_motor_speed = 0.5; -float previous_right_motor_speed = 0.5; - -void curved_random_walk_with_interaction_program() -{ - if(internal_state == random_walk) { - if(interaction_timeout < 4) - interaction_timeout++; - - int closest_robot = -1; - unsigned short shortest_distance = 0; - - // Check whether there are any other robots within range - for(int i = 0; i < 8; i++) { - if(robots_found[i]) { - if(robots_distance[i] > shortest_distance) { - shortest_distance = robots_distance[i]; - closest_robot = i; - } - } - } - - // Turn towards the closest robot - if(closest_robot >= 0 && shortest_distance > 300 && interaction_timeout >= 4) { - time_based_turn_degrees(1, robots_heading[closest_robot], 1); - - action_timeout = 0; - internal_state = turn_towards; - char temp_message[17]; - sprintf(temp_message,"FACE ROBOT %d",closest_robot); - set_program_info(temp_message); - } else { // Otherwise, do a random walk - // Check the front sensors for obstacles - if(reflected_sensor_data[0] > obstacle_avoidance_threshold || - reflected_sensor_data[1] > obstacle_avoidance_threshold || - reflected_sensor_data[6] > obstacle_avoidance_threshold || - reflected_sensor_data[7] > obstacle_avoidance_threshold) { - // Ignore the rear sensors when calculating the heading - reflected_sensor_data[2] = 0; - reflected_sensor_data[3] = 0; - reflected_sensor_data[4] = 0; - reflected_sensor_data[5] = 0; - - // Turn 180 degrees away from the sensed obstacle - int heading = get_bearing_from_ir_array (reflected_sensor_data) + 180; - - // Normalise - heading %= 360; - - if(heading < -180) - heading += 360; - - if(heading > 180) - heading -= 360; - set_program_info("AVOID OBSTACLE"); - time_based_turn_degrees(1, heading, 1); - - action_timeout = 0; - internal_state = turn_away; - } else { - // Change motor speeds every 1s - if(random_walk_timeout >= 2) { - float random_offset = (((float) rand() / (float) RAND_MAX) - 0.5) * 0.5; - - float left_motor_speed = previous_left_motor_speed - random_offset; - float right_motor_speed = previous_right_motor_speed + random_offset; - - float threshold = 0.25; - - if(left_motor_speed < threshold) - left_motor_speed = threshold; - - if(right_motor_speed < threshold) - right_motor_speed = threshold; - - set_left_motor_speed(left_motor_speed); - set_right_motor_speed(right_motor_speed); - - random_walk_timeout = 0; - } - - random_walk_timeout++; - } - } - } else if(internal_state == turn_towards) { - if(action_timeout < 4) - action_timeout++; - else { - set_program_info("SAY HELLO"); - vibrate(); - - action_timeout = 0; - internal_state = interact; - } - } else if(internal_state == interact) { - if(action_timeout < 4) - action_timeout++; - else { - set_program_info("TURN AROUND"); - time_based_turn_degrees(1, 180, 1); - - action_timeout = 0; - internal_state = turn_away; - } - - } else if(internal_state == turn_away) { - if(action_timeout < 4) - action_timeout++; - else { - set_program_info("RANDOM WALK"); - interaction_timeout = 0; - internal_state = random_walk; - } - } else if(internal_state == avoid_obstacle) { - if(action_timeout < 4) - action_timeout++; - else - set_program_info("RANDOM WALK"); - internal_state = random_walk; - } -} - - - - - -///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -/// straight_random_walk_with_interaction_program - -void straight_random_walk_with_interaction_program() -{ - -} - - - - - -///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -/// find_space_program - -char prog_debug = 1 ; -float target_wheel_speed; -int random_walk_bearing; - -void find_space_program(char bidirectional) -{ - // The find_space_program is a continuous turn-move vector program - - if(program_run_init == 1) { - // Setup the LEDs to red - set_leds(0x00,0xFF); - set_center_led(1,1); - program_run_init = 0; - random_walk_bearing = rand() % 360; - } - - // When step_cycle = 0 we calculate a vector to move to and a target distance - if(step_cycle == 0) { - struct FloatVector target_vector; - target_vector.angle = 0; - target_vector.distance = 0; - // Check for near robots within range - for(int i = 1; i < 8; i++) { - if(robots_found[i]) { - int res_bearing = robots_heading[i]; - res_bearing += 180; - if(res_bearing > 180) res_bearing -= 360; - target_vector = addVector(target_vector,res_bearing,robots_distance[i]); - 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); - } - } - if(target_vector.angle!=0){ - set_leds(0xFF,0xFF); - set_center_led(3,1); - } - // Check for obstacles - char obstacle = 0; - int peak_strength = 0; - for(int i=0; i<8; i++){ - if(reflected_sensor_data[i] > peak_strength) peak_strength = reflected_sensor_data[i]; - if(peak_strength > obstacle_avoidance_threshold) obstacle = 1; - } - if(obstacle){ - //Choose new random walk bearing - set_leds(0x00,0xFF); - set_center_led(1,1); - random_walk_bearing = rand() % 360; - int obstacle_bearing = get_bearing_from_ir_array (reflected_sensor_data); - int obs_bearing = obstacle_bearing + 180; - if(obs_bearing > 180) obs_bearing -= 360; - target_vector = addVector(target_vector,obs_bearing,peak_strength); - 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); - } - - if(target_vector.angle == 0 && target_vector.distance == 0){ - set_leds(0xFF,0x00); - set_center_led(2,1); - random_walk_bearing += 180; - if(random_walk_bearing > 360) random_walk_bearing -= 360; - target_vector.distance = 100; - int current_bearing = 360 - beacon_heading; - //Now work out turn needed to face intended heading - int target_turn = (random_walk_bearing - current_bearing) % 360; - if(target_turn > 180) target_turn -= 360; - if(target_turn < -180) target_turn += 360; - target_vector.angle = target_turn; - if(prog_debug) out("Random walk bearing:%d Current:%d Target:%f\n",random_walk_bearing,current_bearing,target_vector.angle); - } - char wheel_direction = 1; - if(bidirectional){ - // Allow reverse wheel direction - if(target_vector.angle < -90) {target_vector.angle += 180; wheel_direction = 0;} - else if(target_vector.angle > 90) {target_vector.angle -= 180; wheel_direction = 0;} - } - //Now turn to angle - float maximum_turn_angle = get_maximum_turn_angle(BEACON_PERIOD*10); - if(target_vector.angle < 0){ - if(target_vector.angle < -maximum_turn_angle){ - target_vector.angle = -maximum_turn_angle; - target_vector.distance = 100; - } - }else{ - if(target_vector.angle > maximum_turn_angle){ - target_vector.angle = maximum_turn_angle; - target_vector.distance = 100; - } - } - //Set the wheel speed for next action - if(target_vector.distance < 120) target_wheel_speed = 0.25; - else if(target_vector.distance < 240) target_wheel_speed = 0.35; - else if(target_vector.distance < 480) target_wheel_speed = 0.45; - else if(target_vector.distance < 960) target_wheel_speed = 0.55; - else target_wheel_speed = 0.65; - if(wheel_direction == 0) target_wheel_speed = 0-target_wheel_speed; - - //Now turn... - time_based_turn_degrees(1, (int) target_vector.angle, 1); - } else time_based_forward(target_wheel_speed,BEACON_PERIOD*6,0); -} - - - - - -///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -/// clustering_program - - -void clustering_program(char invert, char bidirectional) -{ - // The clustering program is a continuous turn-move vector program - // In normal mode (invert = 0) it is attracted to same-group robots and repels opposite-group, walls and very close same-group robots - // In invert mode (invert = 1) it avoids same-group and is attracted to opposite group - - // Store the robot group: even robots (0) are green, odd robots (1) are red - char group = robot_id % 2; - - if(program_run_init == 1) { - // Setup the LEDs based on robot_id - if(group == 0) { - set_leds(0xFF,0x00); - set_center_led(2,1); - } else { - set_leds(0x00,0xFF); - set_center_led(1,1); - } - program_run_init = 0; - random_walk_bearing = rand() % 360; - } - - // When step_cycle = 0 we calculate a vector to move to and a target distance - if(step_cycle == 0) { - char avoiding_friend = 0; - struct FloatVector target_vector; - target_vector.angle = 0; - target_vector.distance = 0; - // Check for near robots within range - for(int i = 1; i < 8; i++) { - if(robots_found[i]) { - // Determine if the robot is an attractor or a repellor - char attract = 0; - if((invert==0 && ((i%2) == group)) || (invert==1 && ((i%2) != group))) attract = 1; - // Avoid very close attractors to stop collisions - if(attract==1 && robots_distance[i] > robot_avoidance_threshold) {attract = 0; avoiding_friend = 1;} - int res_bearing = robots_heading[i]; - if(attract==0){ - res_bearing += 180; - if(res_bearing > 180) res_bearing -= 360; - } - target_vector = addVector(target_vector,res_bearing,robots_distance[i]); - if(prog_debug) { - if(attract) { - 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); - } else { - 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); - } - } - } - } - - // Check for obstacles - char obstacle = 0; - int peak_strength = 0; - for(int i=0; i<8; i++){ - if(reflected_sensor_data[i] > peak_strength) peak_strength = reflected_sensor_data[i]; - if(peak_strength > obstacle_avoidance_threshold) obstacle = 1; - } - if(obstacle){ - //Choose new random walk bearing - random_walk_bearing = rand() % 360; - int obstacle_bearing = get_bearing_from_ir_array (reflected_sensor_data); - int obs_bearing = obstacle_bearing + 180; - if(obs_bearing > 180) obs_bearing -= 360; - target_vector = addVector(target_vector,obs_bearing,peak_strength); - 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); - } - if(target_vector.angle == 0 && target_vector.distance == 0){ - //I have nothing attracting me so persist with random walk: with a 2% chance pick new bearing - if(rand() % 100 > 97) random_walk_bearing = rand() % 360; - target_vector.distance = 100; - int current_bearing = 360 - beacon_heading; - //Now work out turn needed to face intended heading - int target_turn = (random_walk_bearing - current_bearing) % 360; - if(target_turn > 180) target_turn -= 360; - if(target_turn < -180) target_turn += 360; - target_vector.angle = target_turn; - if(prog_debug) out("Random walk bearing:%d Current:%d Target:%f\n",random_walk_bearing,current_bearing,target_vector.angle); - } - char wheel_direction = 1; - if(bidirectional){ - // Allow reverse wheel direction - if(target_vector.angle < -90) {target_vector.angle += 180; wheel_direction = 0;} - else if(target_vector.angle > 90) {target_vector.angle -= 180; wheel_direction = 0;} - } - //Now turn to angle - float maximum_turn_angle = get_maximum_turn_angle(BEACON_PERIOD*10); - if(target_vector.angle < 0){ - if(target_vector.angle < -maximum_turn_angle){ - target_vector.angle = -maximum_turn_angle; - target_vector.distance = 100; - } - }else{ - if(target_vector.angle > maximum_turn_angle){ - target_vector.angle = maximum_turn_angle; - target_vector.distance = 100; - } - } - if(avoiding_friend) target_vector.distance = 100; - //Set the wheel speed for next action - if(target_vector.distance < 120) target_wheel_speed = 0.25; - else if(target_vector.distance < 240) target_wheel_speed = 0.35; - else if(target_vector.distance < 480) target_wheel_speed = 0.5; - else if(target_vector.distance < 960) target_wheel_speed = 0.65; - else target_wheel_speed = 0.85; - if(wheel_direction == 0) target_wheel_speed = 0-target_wheel_speed; - - //Now turn... - time_based_turn_degrees(1, (int) target_vector.angle, 1); - } else time_based_forward(target_wheel_speed,BEACON_PERIOD*7,0); -} - - -///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -/// stop_program - Pauses robot - -void stop_program() -{ - if(program_run_init == 1) { - save_led_states(); - set_leds(0,0); - set_center_led(0,0); - stop(); - program_run_init = 0; - } -} - - - -///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -/// tag_game_program - -enum tag_game_role {hunter, hunted}; -enum tag_game_role role; -int tag_distance = 500; -char hunters[8]; // Memory of which robots have come within tag distance - assumed to be hunters -Timeout hunter_timeout; // Timeout before robots switch back to being hunted -char initial_hunter = 2; // Robot with this ID is permanently a hunter - -// Resets hunter robots to hunted after timeout -void role_reset() -{ - role = hunted; - set_program_info("HUNTED"); - // Clear memory of hunters - for(int i = 0; i < 8; i++) - hunters[i] = 0; -} - -void tag_game_program() -{ - // Initialisation - if(program_run_init == 1) { - - // Set robot roles - if(robot_id == initial_hunter){ - role = hunter; - set_program_info("FIRST HUNTER"); - } - else { - role = hunted; - set_program_info("HUNTED"); - } - // Clear memory of hunters - for(int i = 0; i < 8; i++) - hunters[i] = 0; - - program_run_init = 0; - } - - // Bias forward movement - float left_motor_speed = 0.5; - float right_motor_speed = 0.5; - - // Check the front sensors for obstacles - if(reflected_sensor_data[0] > obstacle_avoidance_threshold || - reflected_sensor_data[1] > obstacle_avoidance_threshold || - reflected_sensor_data[6] > obstacle_avoidance_threshold || - reflected_sensor_data[7] > obstacle_avoidance_threshold) - { - // Ignore the rear sensors when calculating the heading - reflected_sensor_data[2] = 0; - reflected_sensor_data[3] = 0; - reflected_sensor_data[4] = 0; - reflected_sensor_data[5] = 0; - - // Get heading of sensed obstacle - int heading = get_bearing_from_ir_array(reflected_sensor_data); - - // Turn in opposite direction - if(heading > 0 && heading < 90) - { - left_motor_speed -= 0.75; - right_motor_speed += 0.5; - } - else if(heading < 0 && heading > -90) - { - left_motor_speed += 0.5; - right_motor_speed -= 0.75; - } - - set_left_motor_speed(left_motor_speed); - set_right_motor_speed(right_motor_speed); - - // Return early - obstacle avoidance is the top priority - return; - } - - int closest_robot = -1; - unsigned short shortest_distance = 0; - - // Check whether there are any other robots within range - for(int i = 0; i < 8; i++) - { - if(robots_found[i]) - { - if(robots_distance[i] > shortest_distance) - { - shortest_distance = robots_distance[i]; - closest_robot = i; - } - } - } - - // If the closest robot is within tag distance, this robot has been tagged - if(shortest_distance > tag_distance) - { - // Switch role to hunter - if(role == hunted) - { - role = hunter; - set_program_info("NEW HUNTER"); - // Reset to hunted after 10 seconds - hunter_timeout.attach(&role_reset, 10); - } - - // Keep a record of which robot tagged them, so hunters do not chase hunters - // Unless they are the initial hunter, who is aggresive towards everyone - if(robot_id != initial_hunter) - hunters[closest_robot] = 1; - } - - if(role == hunter) - { - // Set LEDS to red - set_leds(0x00, 0xFF); - set_center_led(1, 1); - - if(closest_robot >= 0 && !hunters[closest_robot]) // Ignore other hunters - { - // Turn towards closest hunted robot (unless it is straight ahead, or behind) - if(robots_heading[closest_robot] > 22.5 && robots_heading[closest_robot] < 90) - { - left_motor_speed += 0.5; - right_motor_speed -= 0.5; - } - else if(robots_heading[closest_robot] < -22.5 && robots_heading[closest_robot] > -90) - { - left_motor_speed -= 0.5; - right_motor_speed += 0.5; - } - } - - set_left_motor_speed(left_motor_speed); - set_right_motor_speed(right_motor_speed); - } - else // role == hunted - { - // Set LEDs to green - set_leds(0xFF, 0x00); - set_center_led(2, 1); - - // Avoid everyone - if(closest_robot >= 0) - { - // Turn away from closest robot - if(robots_heading[closest_robot] >= 0 && robots_heading[closest_robot] < 90) - { - left_motor_speed -= 0.5; - right_motor_speed += 0.5; - } - else if(robots_heading[closest_robot] < 0 && robots_heading[closest_robot] > -90) - { - left_motor_speed += 0.5; - right_motor_speed -= 0.5; - } - } - - set_left_motor_speed(left_motor_speed); - set_right_motor_speed(right_motor_speed); - } -} - -///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -/// flocking_program - -void flocking_program() -{ - char display_line[16] = " "; - int average_heading = 0; - int number_of_neighbours = 0; - int number_of_flocking_headings = 0; - int target_heading = 0; - int angles = 0; - int ir_sensor_threshold = 600; - int sensors_activated = 0; - - struct FloatVector obstacle_vector; - obstacle_vector.angle = 0; - obstacle_vector.distance = 0; - - struct FloatVector cohesion_vector; - cohesion_vector.angle = 0; - cohesion_vector.distance = 0; - - for(int i = 0; i < 8; i++) - { - if(reflected_sensor_data[i] > ir_sensor_threshold) - { - obstacle_vector = addVector(obstacle_vector, get_bearing_from_ir_array(reflected_sensor_data), reflected_sensor_data[i]); - sensors_activated++; - } - - if(robots_found[i]) - { - // -180 degrees is reserved for "no byte received" - if(flocking_headings[i] != -180) - { - average_heading += flocking_headings[i]; - number_of_flocking_headings++; - } - - cohesion_vector = addVector(cohesion_vector, robots_heading[i], robots_distance[i]); - number_of_neighbours++; - } - } - - cohesion_vector.distance /= number_of_neighbours; // Normalise - obstacle_vector.distance /= sensors_activated; // Normalise - - int obstacle_avoidance_angle; - - if(sensors_activated > 0) - { - obstacle_avoidance_angle = obstacle_vector.angle + 180; - - if(obstacle_avoidance_angle > 180) - obstacle_avoidance_angle -= 360; - if(obstacle_avoidance_angle < -180) - obstacle_avoidance_angle += 360; - - target_heading += obstacle_avoidance_angle; - angles++; - } - - int cohesion_angle; - - // Don't bother performing cohesion if robots are already close enough - if(number_of_neighbours > 0 && cohesion_vector.distance < 200) - { - cohesion_angle = cohesion_vector.angle; - - if(cohesion_angle > 180) - cohesion_angle -= 360; - if(cohesion_angle < -180) - cohesion_angle += 360; - - target_heading += cohesion_angle; - angles++; - } - - int relative_flocking_heading; - - if(number_of_flocking_headings > 0) - { - average_heading /= number_of_flocking_headings; - - relative_flocking_heading = beacon_heading - average_heading; - - if(relative_flocking_heading > 180) - relative_flocking_heading -= 360; - if(relative_flocking_heading < -180) - relative_flocking_heading += 360; - - target_heading += relative_flocking_heading; - angles++; - } - - if(angles > 0) - target_heading /= angles; // Calculate average - - float left_motor_speed = 0.25; - float right_motor_speed = 0.25; - - if(target_heading > 22.5) - right_motor_speed *= -1; - else if(target_heading < -22.5) - left_motor_speed *= -1; - - set_left_motor_speed(left_motor_speed); - set_right_motor_speed(right_motor_speed); - - // Only transmit beacon heading if the beacon is visible - if(beacon_found) - { - float degrees_per_value = 256.0f / 360.0f; - char beacon_heading_byte = (beacon_heading + 180) * degrees_per_value; // Convert beacon heading from +/-180 degrees into a single byte value - - bt.putc(beacon_heading_byte); - } - - sprintf(display_line, "%d %d %d %d", target_heading, obstacle_avoidance_angle, cohesion_angle, relative_flocking_heading); - - set_program_info(display_line); -} - -///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -/// generic_program - Framework for building typical programs - -void generic_program() -{ - // Do something on the first run of a program - if(program_run_init == 1) { - // Initialisation code goes here... - - program_run_init = 0; - } - - // 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) - if(step_cycle == 0) { - // Do something based on sensor data (eg turn) - } else { - // Do something ignoring sensor data (eg move, or nothing!) - } - -} \ No newline at end of file
--- a/programs.h Thu Mar 03 23:22:01 2016 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,22 +0,0 @@ -/// PsiSwarm Beautiful Meme Project Source Code -/// Version 0.4 -/// James Hilder, Alan Millard, Homero Elizondo, Jon Timmis -/// University of York - -// programs.h - Various PsiSwarm Programs for Beautiful Meme Project - -#ifndef PROGRAMS_H -#define PROGRAMS_H - -void recharging_program(void); -void head_to_bearing_program(int target_bearing); -void curved_random_walk_with_interaction_program(void); -void straight_random_walk_with_interaction_program(void); -void find_space_program(char bidirectional); -void clustering_program(char invert, char bidirectional); -void stop_program(void); -void role_reset(void); -void tag_game_program(void); -void flocking_program(void); - -#endif \ No newline at end of file
--- a/vector.cpp Thu Mar 03 23:22:01 2016 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,24 +0,0 @@ -/// PsiSwarm Beautiful Meme Project Source Code -/// Version 0.4 -/// James Hilder, Alan Millard, Homero Elizondo, Jon Timmis -/// University of York - -// vector.cpp - Functions for calculating floating point bearing vectors - -#include "main.h" -#include <math.h> - - -struct FloatVector addVector(struct FloatVector in_vector, int angle, int distance){ - struct FloatVector out_vector; - float sin_component_old = sin(in_vector.angle * TO_RAD) * in_vector.distance; - float cos_component_old = cos(in_vector.angle * TO_RAD) * in_vector.distance; - float sin_component_new = sin(angle * TO_RAD) * distance; - float cos_component_new = cos(angle * TO_RAD) * distance; - float sin_component_sum = sin_component_old + sin_component_new; - float cos_component_sum = cos_component_old + cos_component_new; - out_vector.distance = sqrt((sin_component_sum * sin_component_sum) + (cos_component_sum * cos_component_sum)); - out_vector.angle = atan2(sin_component_sum ,cos_component_sum) * TO_DEG; - //out("Angle:%f Distance:%f\n",out_vector.angle,out_vector.distance); - return out_vector; -} \ No newline at end of file
--- a/vector.h Thu Mar 03 23:22:01 2016 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,21 +0,0 @@ -/// PsiSwarm Beautiful Meme Project Source Code -/// Version 0.4 -/// James Hilder, Alan Millard, Homero Elizondo, Jon Timmis -/// University of York - -// vector.h - Functions for calculating floating point bearing vectors - -#ifndef VECTOR_H -#define VECTOR_H - -#define TO_DEG 57.2958 -#define TO_RAD 0.017453 - -struct FloatVector{ - float angle; - float distance; -}; - -struct FloatVector addVector(struct FloatVector in_Vector, int angle, int distance); - -#endif \ No newline at end of file