James Hilder
Psi Swarm Code V0.41 [With Beautiful Meme program]
Dependencies: PsiSwarmLibrary mbed
Fork of
BeautifulMemeProjectBT
by 
Alan Millard
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
diff -r 9756004e8499 -r 513457c1ad12 BeautifulMeme/beacon.cpp
--- /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
diff -r 9756004e8499 -r 513457c1ad12 BeautifulMeme/beacon.h --- /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
diff -r 9756004e8499 -r 513457c1ad12 BeautifulMeme/bmeme.cpp
--- /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
diff -r 9756004e8499 -r 513457c1ad12 BeautifulMeme/bmeme.h --- /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
diff -r 9756004e8499 -r 513457c1ad12 BeautifulMeme/programs.cpp
--- /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
diff -r 9756004e8499 -r 513457c1ad12 BeautifulMeme/programs.h --- /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
diff -r 9756004e8499 -r 513457c1ad12 BeautifulMeme/vector.cpp
--- /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
diff -r 9756004e8499 -r 513457c1ad12 BeautifulMeme/vector.h
--- /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
diff -r 9756004e8499 -r 513457c1ad12 PsiSwarm.lib --- 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
diff -r 9756004e8499 -r 513457c1ad12 beacon.cpp
--- 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
diff -r 9756004e8499 -r 513457c1ad12 beacon.h --- 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
diff -r 9756004e8499 -r 513457c1ad12 main.cpp
--- 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
diff -r 9756004e8499 -r 513457c1ad12 main.h --- 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
diff -r 9756004e8499 -r 513457c1ad12 programs.cpp
--- 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
diff -r 9756004e8499 -r 513457c1ad12 programs.h --- 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
diff -r 9756004e8499 -r 513457c1ad12 vector.cpp
--- 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
diff -r 9756004e8499 -r 513457c1ad12 vector.h
--- 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