UKESF Headstart Summer School / Mbed 2 deprecated UKESF_Lab

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

main.cpp@6:ff3c66f7372b, 2015-10-22 (annotated)

Committer:
jah128
Date:
Thu Oct 22 00:46:14 2015 +0000
Revision:
6:ff3c66f7372b
Parent:
4:25039ea5eb09
Child:
7:ef9ab01b9e26
Initial version: beacon detection and sync. code, bearing estimation.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jah128 6:ff3c66f7372b 1 /***********************************************************************
jah128 0:8a5497a2e366 2 ** ██████╗ ███████╗██╗███████╗██╗ ██╗ █████╗ ██████╗ ███╗ ███╗ **
jah128 0:8a5497a2e366 3 ** ██╔══██╗██╔════╝██║██╔════╝██║ ██║██╔══██╗██╔══██╗████╗ ████║ **
jah128 0:8a5497a2e366 4 ** ██████╔╝███████╗██║███████╗██║ █╗ ██║███████║██████╔╝██╔████╔██║ **
jah128 0:8a5497a2e366 5 ** ██╔═══╝ ╚════██║██║╚════██║██║███╗██║██╔══██║██╔══██╗██║╚██╔╝██║ **
jah128 0:8a5497a2e366 6 ** ██║ ███████║██║███████║╚███╔███╔╝██║ ██║██║ ██║██║ ╚═╝ ██║ **
jah128 0:8a5497a2e366 7 ** ╚═╝ ╚══════╝╚═╝╚══════╝ ╚══╝╚══╝ ╚═╝ ╚═╝╚═╝ ╚═╝╚═╝ ╚═╝ **
jah128 0:8a5497a2e366 8 ************************************************************************
jah128 0:8a5497a2e366 9 **(C) Dr James Hilder - York Robotics Laboratory - University of York **
jah128 0:8a5497a2e366 10 ***********************************************************************/
jah128 0:8a5497a2e366 11
jah128 6:ff3c66f7372b 12 /// PsiSwarm Beautiful Meme Project Source Code
jah128 6:ff3c66f7372b 13 /// Version 0.1
jah128 6:ff3c66f7372b 14 /// James Hilder, Alan Millard, Homero Elizondo, Jon Timmis
jah128 6:ff3c66f7372b 15 /// University of York
jah128 0:8a5497a2e366 16
jah128 1:f6356cf1cefc 17 /// Include psiswarm.h - this includes all the other necessary core files
jah128 0:8a5497a2e366 18 #include "psiswarm.h"
jah128 0:8a5497a2e366 19
jah128 6:ff3c66f7372b 20 // IMPORTANT!!!
jah128 6:ff3c66f7372b 21 // Do not call the IR functions at all as they will interfere with the correct operation of this program
jah128 6:ff3c66f7372b 22 // Instead, use the values held in the variables below; they are updated every 500ms
jah128 6:ff3c66f7372b 23
jah128 6:ff3c66f7372b 24 char beacon_found = 0; // This will be a 1 when a beacon was detected during the previous 500ms window
jah128 6:ff3c66f7372b 25 int beacon_heading = 0; // This is the heading from the last time a beacon was detected
jah128 6:ff3c66f7372b 26 char robots_found[8]; // These will be a 1 when the respective robot [excluding self] was detected during the previous 500ms window
jah128 6:ff3c66f7372b 27 char robots_heading[8]; // These are the headings from the last time the respective robots were detected
jah128 6:ff3c66f7372b 28 unsigned short robots_distance[8]; // This is the maximum sensor value from the last time the respective robot was detected
jah128 6:ff3c66f7372b 29 unsigned short reflected_sensor_data[8]; // The reflected IR values when this robots emitters are on
jah128 6:ff3c66f7372b 30 unsigned short background_sensor_data[8];// The raw IR values when no robot (or beacon) should have its IR on
jah128 6:ff3c66f7372b 31
jah128 6:ff3c66f7372b 32 char * program_name = "B-Meme";
jah128 4:25039ea5eb09 33 char * author_name = "YRL";
jah128 1:f6356cf1cefc 34 char * version_name = "1.0";
jah128 0:8a5497a2e366 35
jah128 6:ff3c66f7372b 36 char user_code_debug = 1; // Set to 1 to show terminal messages from "out" function [specific to this code]
jah128 0:8a5497a2e366 37
jah128 0:8a5497a2e366 38
jah128 1:f6356cf1cefc 39 ///Place user code here that should be run after initialisation but before the main loop
jah128 6:ff3c66f7372b 40 void user_code_setup()
jah128 6:ff3c66f7372b 41 {
jah128 6:ff3c66f7372b 42 out("------------------------------------------------------\n");
jah128 6:ff3c66f7372b 43 out("Beautiful Meme Project Demo Code \n");
jah128 6:ff3c66f7372b 44 out("------------------------------------------------------\n");
jah128 6:ff3c66f7372b 45 locate_beacon();
jah128 6:ff3c66f7372b 46 while(beacon_found == 0) {
jah128 6:ff3c66f7372b 47 locate_beacon();
jah128 6:ff3c66f7372b 48 }
jah128 0:8a5497a2e366 49
jah128 6:ff3c66f7372b 50 start_infrared_timers();
jah128 0:8a5497a2e366 51 }
jah128 0:8a5497a2e366 52
jah128 1:f6356cf1cefc 53 ///This function is the loop where user code should be placed
jah128 6:ff3c66f7372b 54 void user_code_loop()
jah128 6:ff3c66f7372b 55 {
jah128 4:25039ea5eb09 56
jah128 1:f6356cf1cefc 57 ///Do not place code within a loop, but consider this function to be a loop that is always run
jah128 1:f6356cf1cefc 58 ///unless the user code is externally paused (such as by debug or recharging system)
jah128 6:ff3c66f7372b 59
jah128 6:ff3c66f7372b 60 //pc.printf("User loop code block\n");
jah128 6:ff3c66f7372b 61
jah128 6:ff3c66f7372b 62
jah128 6:ff3c66f7372b 63 wait(0.5);
jah128 0:8a5497a2e366 64 }
jah128 0:8a5497a2e366 65
jah128 0:8a5497a2e366 66
jah128 1:f6356cf1cefc 67 /// Code goes here to handle what should happen when the user switch is pressed
jah128 0:8a5497a2e366 68 void handle_switch_event(char switch_state)
jah128 0:8a5497a2e366 69 {
jah128 6:ff3c66f7372b 70 /// 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
jah128 1:f6356cf1cefc 71 /// NB For maximum compatability it is recommended to minimise reliance on center button press
jah128 6:ff3c66f7372b 72
jah128 6:ff3c66f7372b 73 //pc.printf("User switch code block: %d\n",switch_state);
jah128 0:8a5497a2e366 74 }
jah128 0:8a5497a2e366 75
jah128 1:f6356cf1cefc 76 /// The main routine: it is recommended to leave this function alone and add user code to the above functions
jah128 6:ff3c66f7372b 77 int main()
jah128 6:ff3c66f7372b 78 {
jah128 1:f6356cf1cefc 79 ///init() in psiswarm.cpp sets up the robot
jah128 0:8a5497a2e366 80 init();
jah128 0:8a5497a2e366 81 user_code_setup();
jah128 0:8a5497a2e366 82 user_code_running = 1;
jah128 0:8a5497a2e366 83 while(1) {
jah128 6:ff3c66f7372b 84 if(user_code_running)user_code_loop();
jah128 6:ff3c66f7372b 85 if(demo_on) demo_mode();
jah128 6:ff3c66f7372b 86 //wait_us(5);
jah128 6:ff3c66f7372b 87 }
jah128 6:ff3c66f7372b 88 }
jah128 6:ff3c66f7372b 89
jah128 6:ff3c66f7372b 90 /// Verbose output
jah128 6:ff3c66f7372b 91 void out(const char* format, ...)
jah128 6:ff3c66f7372b 92 {
jah128 6:ff3c66f7372b 93 char buffer[256];
jah128 6:ff3c66f7372b 94 if (debug_mode) {
jah128 6:ff3c66f7372b 95 va_list vl;
jah128 6:ff3c66f7372b 96 va_start(vl, format);
jah128 6:ff3c66f7372b 97 vsprintf(buffer,format,vl);
jah128 6:ff3c66f7372b 98 if(user_code_debug == 1) pc.printf("%s", buffer);
jah128 6:ff3c66f7372b 99 va_end(vl);
jah128 0:8a5497a2e366 100 }
jah128 0:8a5497a2e366 101 }