UKESF Headstart Summer School
Example code for the UKESF Headstart robotics lab; code to make the PsiSwarm robot move forward and turn.
Dependencies: PsiSwarm-Headstart mbed
Fork of
PsiSwarm_V41
by 
James Hilder
Revision 31:7fa2c47d73a2, committed 2016-06-20
- Comitter:
- jah128
- Date:
- Mon Jun 20 13:36:30 2016 +0000
- Parent:
- 30:513457c1ad12
- Commit message:
- PsiSwarm Headstart Lab
Changed in this revision
diff -r 513457c1ad12 -r 7fa2c47d73a2 BeautifulMeme/beacon.cpp
--- a/BeautifulMeme/beacon.cpp Tue Mar 15 00:58:43 2016 +0000
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,376 +0,0 @@
-/// 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 513457c1ad12 -r 7fa2c47d73a2 BeautifulMeme/beacon.h --- a/BeautifulMeme/beacon.h Tue Mar 15 00:58:43 2016 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,20 +0,0 @@ -/// 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 513457c1ad12 -r 7fa2c47d73a2 BeautifulMeme/bmeme.cpp
--- a/BeautifulMeme/bmeme.cpp Tue Mar 15 00:58:43 2016 +0000
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,306 +0,0 @@
-/// 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 513457c1ad12 -r 7fa2c47d73a2 BeautifulMeme/bmeme.h --- a/BeautifulMeme/bmeme.h Tue Mar 15 00:58:43 2016 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,58 +0,0 @@ -/// 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 513457c1ad12 -r 7fa2c47d73a2 BeautifulMeme/programs.cpp
--- a/BeautifulMeme/programs.cpp Tue Mar 15 00:58:43 2016 +0000
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,869 +0,0 @@
-/// 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 513457c1ad12 -r 7fa2c47d73a2 BeautifulMeme/programs.h --- a/BeautifulMeme/programs.h Tue Mar 15 00:58:43 2016 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,22 +0,0 @@ -/// 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 513457c1ad12 -r 7fa2c47d73a2 BeautifulMeme/vector.cpp
--- a/BeautifulMeme/vector.cpp Tue Mar 15 00:58:43 2016 +0000
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,23 +0,0 @@
-/// 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 513457c1ad12 -r 7fa2c47d73a2 BeautifulMeme/vector.h
--- a/BeautifulMeme/vector.h Tue Mar 15 00:58:43 2016 +0000
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,21 +0,0 @@
-/// 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 513457c1ad12 -r 7fa2c47d73a2 PsiSwarm.lib --- a/PsiSwarm.lib Tue Mar 15 00:58:43 2016 +0000 +++ b/PsiSwarm.lib Mon Jun 20 13:36:30 2016 +0000 @@ -1,1 +1,1 @@ -https://developer.mbed.org/users/jah128/code/PsiSwarmLibrary/#7c0d1f581757 +https://developer.mbed.org/teams/UKESF-Headstart-Summer-School/code/PsiSwarm-Headstart/#dc77a25f29de
diff -r 513457c1ad12 -r 7fa2c47d73a2 main.cpp
--- a/main.cpp Tue Mar 15 00:58:43 2016 +0000
+++ b/main.cpp Mon Jun 20 13:36:30 2016 +0000
@@ -1,70 +1,87 @@
-/***********************************************************************
-** ██████╗ ███████╗██╗███████╗██╗ ██╗ █████╗ ██████╗ ███╗ ███╗ **
-** ██╔══██╗██╔════╝██║██╔════╝██║ ██║██╔══██╗██╔══██╗████╗ ████║ **
-** ██████╔╝███████╗██║███████╗██║ █╗ ██║███████║██████╔╝██╔████╔██║ **
-** ██╔═══╝ ╚════██║██║╚════██║██║███╗██║██╔══██║██╔══██╗██║╚██╔╝██║ **
-** ██║ ███████║██║███████║╚███╔███╔╝██║ ██║██║ ██║██║ ╚═╝ ██║ **
-** ╚═╝ ╚══════╝╚═╝╚══════╝ ╚══╝╚══╝ ╚═╝ ╚═╝╚═╝ ╚═╝╚═╝ ╚═╝ **
-************************************************************************
-**(C) Dr James Hilder - York Robotics Laboratory - University of York **
-***********************************************************************/
+/**********************************************************
+* UKESF Headstart Summer School - Robot Programming Lab *
+**********************************************************/
-/// PsiSwarm Blank Example Code
-/// Version 0.41
-/// James Hilder, Alan Millard, Alexander Horsfield, Homero Elizondo, Jon Timmis
-/// University of York
+// Note: A line beginning with // is a 'comment' and is
+// ignored by the compiler [it is shown in green text in the IDE]
-/// Include main.h - this includes psiswarm.h all the other necessary core files
+// Include main.h - this contains the code needed to setup
+// the Psi-Swarm robot and its hardware
#include "main.h"
-char * program_name = "Blank";
+// You can change the author name to saomething else if you want - just make sure it is no more than 16 characters long
char * author_name = "YRL";
-char * version_name = "0.41";
-
-void user_code_loop()
-{
- wait(1);
-}
-
-///Place user code here that should be run after initialisation but before the main loop
-void user_code_setup()
-{
- wait(1);
- display.clear_display();
- display.set_position(0,0);
- display.write_string("No Code");
-
- //bmeme_user_code_setup();
-}
-
-/// Code goes here to handle what should happen when the user switch is pressed
-void 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
-
- //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
int main()
{
- ///init() in psiswarm.cpp sets up the robot
+ // init() in psiswarm.cpp sets up the robot - we need to run this line of code before anything else or the robot won't work.
init();
- user_code_setup();
- user_code_running = 1;
- while(1) {
- user_code_loop();
+
+ // We shall create a for-loop to try to move the robot in a 50cm square.
+ for(int i=0; i<4; i++){
+
+ // Move the robot forward 50cm
+ move_forward(50);
+
+ // Wait one second
+ wait(1.0);
+
+ // Turn the robot right 90 degrees
+ turn_right();
+
+ // Wait for one second
+ wait(1.0);
}
-}
\ No newline at end of file
+
+ // When we have finished our code, we want to leave the MBED in an endless loop.
+ while(1){}
+}
+
+/**
+* void move_forward(int distance)
+* This function should move the robot forward for the distance
+* (in centimeter) specified by distance
+*/
+void move_forward(int distance)
+{
+ float left_motor_speed = 0.4;
+ float right_motor_speed = 0.4;
+ float cm_per_second = 25.0;
+ float delay = distance / cm_per_second;
+ set_motor_speed(left_motor_speed, right_motor_speed);
+ wait(delay);
+ brake();
+}
+
+/**
+* void turn_right()
+* This function should turn the robot right by 90 degrees.
+*/
+void turn_right()
+{
+ float speed = 0.2;
+ float degrees_per_second = 150.0;
+ float delay = 90 / degrees_per_second;
+ turn(speed);
+ wait(delay);
+ brake();
+}
+
+/**
+* void turn_left()
+* This function should turn the robot left by 90 degrees. You will need to fill in the code here
+* HINT: You can use Ctrl-C and Ctrl-V to copy\paste blocks of code!
+*/
+void turn_left()
+{
+}
+
+////////////////////////////////////////////////////////////////////////////////////////////////////
+
+// You can ignore the code beyond this point: we need these variables and functions to make the code
+// compile without errors, but we aren't using the functions for this lab session.
+char * program_name = "Headstart";
+char * version_name = "0.5";
+void handle_switch_event(char switch_state){}
+void handle_user_serial_message(char * message, char length, char interface){}
\ No newline at end of file
diff -r 513457c1ad12 -r 7fa2c47d73a2 main.h --- a/main.h Tue Mar 15 00:58:43 2016 +0000 +++ b/main.h Mon Jun 20 13:36:30 2016 +0000 @@ -1,30 +1,17 @@ -/*********************************************************************** -** ██████╗ ███████╗██╗███████╗██╗ ██╗ █████╗ ██████╗ ███╗ ███╗ ** -** ██╔══██╗██╔════╝██║██╔════╝██║ ██║██╔══██╗██╔══██╗████╗ ████║ ** -** ██████╔╝███████╗██║███████╗██║ █╗ ██║███████║██████╔╝██╔████╔██║ ** -** ██╔═══╝ ╚════██║██║╚════██║██║███╗██║██╔══██║██╔══██╗██║╚██╔╝██║ ** -** ██║ ███████║██║███████║╚███╔███╔╝██║ ██║██║ ██║██║ ╚═╝ ██║ ** -** ╚═╝ ╚══════╝╚═╝╚══════╝ ╚══╝╚══╝ ╚═╝ ╚═╝╚═╝ ╚═╝╚═╝ ╚═╝ ** -************************************************************************ -**(C) Dr James Hilder - York Robotics Laboratory - University of York ** -***********************************************************************/ - -/// PsiSwarm Blank Example Code -/// Version 0.41 -/// James Hilder, Alan Millard, Alexander Horsfield, Homero Elizondo, Jon Timmis -/// University of York +/********************************************************** +* UKESF Headstart Summer School - Robot Programming Lab * +**********************************************************/ #ifndef MAIN_H #define MAIN_H #include "psiswarm.h" -//#include "bmeme.h" int main(void); -void user_code_setup(void); -void user_code_loop(void); void handle_switch_event(char switch_state); void handle_user_serial_message(char * message, char length, char interface); - +void move_forward(int distance); +void turn_right(); +void turn_left(); #endif \ No newline at end of file