Diff: BeautifulMeme/bmeme.cpp

Revision:

30:513457c1ad12

--- /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