James Wilson / Mbed 2 deprecated PsiSwarm-BeaconDemo_Bluetooth

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

Fork of PsiSwarm-flockingAddedBluetooth by James Wilson

PsiSwarm/psiswarm.cpp@0:8a5497a2e366, 2015-10-03 (annotated)

Committer:
jah128
Date:
Sat Oct 03 22:48:50 2015 +0000
Revision:
0:8a5497a2e366
Child:
1:f6356cf1cefc
Initial commit of PsiSwarm API and example code

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jah128 0:8a5497a2e366 1 /* University of York Robotics Laboratory PsiSwarm Library: PsiSwarm Core Source File
jah128 0:8a5497a2e366 2 *
jah128 0:8a5497a2e366 3 * File: psiswarm.cpp
jah128 0:8a5497a2e366 4 *
jah128 0:8a5497a2e366 5 * (C) Dr James Hilder, Dept. Electronics & Computer Science, University of York
jah128 0:8a5497a2e366 6 *
jah128 0:8a5497a2e366 7 * PsiSwarm Library Version: 0.2
jah128 0:8a5497a2e366 8 *
jah128 0:8a5497a2e366 9 * September 2015
jah128 0:8a5497a2e366 10 *
jah128 0:8a5497a2e366 11 */
jah128 0:8a5497a2e366 12
jah128 0:8a5497a2e366 13 #include "psiswarm.h"
jah128 0:8a5497a2e366 14
jah128 0:8a5497a2e366 15 Display display;
jah128 0:8a5497a2e366 16 Serial pc(USBTX,USBRX);
jah128 0:8a5497a2e366 17 Serial bt(p13, p14);
jah128 0:8a5497a2e366 18
jah128 0:8a5497a2e366 19 DigitalOut mbed_led1(LED1);
jah128 0:8a5497a2e366 20 DigitalOut mbed_led2(LED2);
jah128 0:8a5497a2e366 21 DigitalOut mbed_led3(LED3);
jah128 0:8a5497a2e366 22 DigitalOut mbed_led4(LED4);
jah128 0:8a5497a2e366 23 AnalogIn vin_current(p15);
jah128 0:8a5497a2e366 24 AnalogIn vin_battery(p16);
jah128 0:8a5497a2e366 25 AnalogIn vin_dc(p17);
jah128 0:8a5497a2e366 26 I2C primary_i2c (p9, p10);
jah128 0:8a5497a2e366 27 InterruptIn gpio_interrupt (p12);
jah128 0:8a5497a2e366 28 PwmOut motor_left_f (p21);
jah128 0:8a5497a2e366 29 PwmOut motor_left_r (p22);
jah128 0:8a5497a2e366 30 PwmOut motor_right_f(p23);
jah128 0:8a5497a2e366 31 PwmOut motor_right_r(p24);
jah128 0:8a5497a2e366 32 PwmOut center_led_red(p25);
jah128 0:8a5497a2e366 33 PwmOut center_led_green(p26);
jah128 0:8a5497a2e366 34 float center_led_brightness;
jah128 0:8a5497a2e366 35 float backlight_brightness;
jah128 0:8a5497a2e366 36
jah128 0:8a5497a2e366 37 Ticker event_handler;
jah128 0:8a5497a2e366 38 Timer uptime;
jah128 0:8a5497a2e366 39 Timeout pause_usercode_timeout;
jah128 0:8a5497a2e366 40 Ticker ultrasonic_ticker;
jah128 0:8a5497a2e366 41 Timeout ultrasonic_timeout;
jah128 0:8a5497a2e366 42 int timer_minute_count;
jah128 0:8a5497a2e366 43 Ticker timer_ticker;
jah128 0:8a5497a2e366 44
jah128 0:8a5497a2e366 45 float firmware_version;
jah128 0:8a5497a2e366 46 char robot_id;
jah128 0:8a5497a2e366 47 char previous_robot_id;
jah128 0:8a5497a2e366 48
jah128 0:8a5497a2e366 49 char wheel_encoder_byte;
jah128 0:8a5497a2e366 50 char previous_wheel_encoder_byte;
jah128 0:8a5497a2e366 51 signed int left_encoder;
jah128 0:8a5497a2e366 52 signed int right_encoder;
jah128 0:8a5497a2e366 53
jah128 0:8a5497a2e366 54 char switch_byte;
jah128 0:8a5497a2e366 55 char previous_switch_byte;
jah128 0:8a5497a2e366 56
jah128 0:8a5497a2e366 57 char debug_mode = DEBUG_MODE;
jah128 0:8a5497a2e366 58 char debug_output = DEBUG_OUTPUT_STREAM;
jah128 0:8a5497a2e366 59
jah128 0:8a5497a2e366 60 char firmware_bytes[20];
jah128 0:8a5497a2e366 61
jah128 0:8a5497a2e366 62 int base_colour_sensor_raw_values [4];
jah128 0:8a5497a2e366 63 int top_colour_sensor_raw_values [4];
jah128 0:8a5497a2e366 64
jah128 0:8a5497a2e366 65 char waiting_for_ultrasonic = 0;
jah128 0:8a5497a2e366 66 int ultrasonic_distance = 0;
jah128 0:8a5497a2e366 67 char ultrasonic_distance_updated = 0;
jah128 0:8a5497a2e366 68
jah128 0:8a5497a2e366 69 unsigned short background_ir_values [8];
jah128 0:8a5497a2e366 70 unsigned short illuminated_ir_values [8];
jah128 0:8a5497a2e366 71 float reflected_ir_distances [8];
jah128 0:8a5497a2e366 72 char ir_values_stored = 0;
jah128 0:8a5497a2e366 73 unsigned short background_base_ir_values [5];
jah128 0:8a5497a2e366 74 unsigned short illuminated_base_ir_values [5];
jah128 0:8a5497a2e366 75 char base_ir_values_stored = 0;
jah128 0:8a5497a2e366 76
jah128 0:8a5497a2e366 77 float motor_left_speed;
jah128 0:8a5497a2e366 78 float motor_right_speed;
jah128 0:8a5497a2e366 79 char motor_left_brake;
jah128 0:8a5497a2e366 80 char motor_right_brake;
jah128 0:8a5497a2e366 81
jah128 0:8a5497a2e366 82 char demo_on = 0;
jah128 0:8a5497a2e366 83 char event = 0;
jah128 0:8a5497a2e366 84 char change_id_event = 0;
jah128 0:8a5497a2e366 85 char encoder_event = 0;
jah128 0:8a5497a2e366 86 char switch_event = 0;
jah128 0:8a5497a2e366 87 char user_code_running = 0;
jah128 0:8a5497a2e366 88 char user_code_restore_mode = 0;
jah128 0:8a5497a2e366 89 char system_warnings = 0;
jah128 0:8a5497a2e366 90
jah128 0:8a5497a2e366 91 int ir_pulse_delay = DEFAULT_IR_PULSE_DELAY;
jah128 0:8a5497a2e366 92 int base_ir_pulse_delay = DEFAULT_BASE_IR_PULSE_DELAY;
jah128 0:8a5497a2e366 93
jah128 0:8a5497a2e366 94 /**
jah128 0:8a5497a2e366 95 * init()
jah128 0:8a5497a2e366 96 *
jah128 0:8a5497a2e366 97 * Main initialisation routine for the PsiSwarm robot
jah128 0:8a5497a2e366 98 *
jah128 0:8a5497a2e366 99 * Set up the GPIO expansion ICs, launch demo mode if button is held
jah128 0:8a5497a2e366 100 */
jah128 0:8a5497a2e366 101 void init()
jah128 0:8a5497a2e366 102 {
jah128 0:8a5497a2e366 103 firmware_version=0;
jah128 0:8a5497a2e366 104 timer_minute_count = 0;
jah128 0:8a5497a2e366 105 timer_ticker.attach(&IF_update_minutes, 300);
jah128 0:8a5497a2e366 106 uptime.start();
jah128 0:8a5497a2e366 107 primary_i2c.frequency(400000);
jah128 0:8a5497a2e366 108 IF_setup_serial_interfaces();
jah128 0:8a5497a2e366 109 debug("PsiSwarm Robot Library %1.2f\n\n",SOFTWARE_VERSION_CODE);
jah128 0:8a5497a2e366 110 debug("- Setting up serial interface\n");
jah128 0:8a5497a2e366 111 debug("- Reading firmware: ");
jah128 0:8a5497a2e366 112 if(read_firmware() == 1){
jah128 0:8a5497a2e366 113 debug("Version %d.%d\n",firmware_bytes[9],firmware_bytes[10]);
jah128 0:8a5497a2e366 114 }else debug("INVALID\n");
jah128 0:8a5497a2e366 115 debug("- Setting up LED drivers\n");
jah128 0:8a5497a2e366 116 IF_init_leds();
jah128 0:8a5497a2e366 117 if(IF_setup_led_expansion_ic() != 0) {
jah128 0:8a5497a2e366 118 debug("- WARNING: No I2C acknowledge for LED driver\n");
jah128 0:8a5497a2e366 119 system_warnings += 1;
jah128 0:8a5497a2e366 120 }
jah128 0:8a5497a2e366 121 debug("- Setting up motor drivers\n");
jah128 0:8a5497a2e366 122 IF_init_motors();
jah128 0:8a5497a2e366 123 debug("- Setting up GPIO expansion\n");
jah128 0:8a5497a2e366 124 reset_encoders();
jah128 0:8a5497a2e366 125 IF_setup_gpio_expansion_ic();
jah128 0:8a5497a2e366 126 debug("- Setting up temperature sensor\n");
jah128 0:8a5497a2e366 127 IF_setup_temperature_sensor();
jah128 0:8a5497a2e366 128 debug("- Setting up base colour sensor\n");
jah128 0:8a5497a2e366 129 IF_check_base_colour_sensor();
jah128 0:8a5497a2e366 130 debug("- Setting up ultrasonic sensor\n");
jah128 0:8a5497a2e366 131 //enable_ultrasonic_ticker();
jah128 0:8a5497a2e366 132 debug("- Robot ID: %d\n",robot_id);
jah128 0:8a5497a2e366 133 char switchstate = IF_get_switch_state();
jah128 0:8a5497a2e366 134 debug("- Switch State : %d\n",switchstate);
jah128 0:8a5497a2e366 135 debug("- Battery Voltage: %1.3fV\n",get_battery_voltage());
jah128 0:8a5497a2e366 136 debug("- DC Voltage : %1.3fV\n",get_dc_voltage());
jah128 0:8a5497a2e366 137 debug("- Current Draw : %1.3fA\n",get_current());
jah128 0:8a5497a2e366 138 debug("- Temperature : %1.3fC\n",get_temperature());
jah128 0:8a5497a2e366 139 char demo = 0;
jah128 0:8a5497a2e366 140 if(ENABLE_DEMO == 1 && switchstate > 0) demo=1;
jah128 0:8a5497a2e366 141 display.init_display(demo);
jah128 0:8a5497a2e366 142 event_handler.attach_us(&IF_handle_events, 1000);
jah128 0:8a5497a2e366 143 if(demo > 0){
jah128 0:8a5497a2e366 144 debug("- Demo mode button is pressed\n");
jah128 0:8a5497a2e366 145 wait(1.0);
jah128 0:8a5497a2e366 146 demo = IF_get_switch_state();
jah128 0:8a5497a2e366 147 if(demo > 0) demo_mode();
jah128 0:8a5497a2e366 148 display.init_display(0);
jah128 0:8a5497a2e366 149 }
jah128 0:8a5497a2e366 150 }
jah128 0:8a5497a2e366 151
jah128 0:8a5497a2e366 152 void IF_update_minutes(){
jah128 0:8a5497a2e366 153 uptime.reset();
jah128 0:8a5497a2e366 154 timer_minute_count += 5;
jah128 0:8a5497a2e366 155 }
jah128 0:8a5497a2e366 156
jah128 0:8a5497a2e366 157 void IF_handle_events()
jah128 0:8a5497a2e366 158 {
jah128 0:8a5497a2e366 159 // This is the main 'operating system' thread that handles events from robot stimuli
jah128 0:8a5497a2e366 160 // By default it is run every 1ms and checks if there are events to handle
jah128 0:8a5497a2e366 161 if(event > 0){
jah128 0:8a5497a2e366 162 // There are some events to handle. We don't handle all events in every loop to keep the system responsive, instead they are priorised.
jah128 0:8a5497a2e366 163 if(encoder_event == 1){
jah128 0:8a5497a2e366 164 // The encoders have changed; update the encoder values
jah128 0:8a5497a2e366 165 IF_update_encoders();
jah128 0:8a5497a2e366 166 encoder_event = 0;
jah128 0:8a5497a2e366 167 event--;
jah128 0:8a5497a2e366 168 } else {
jah128 0:8a5497a2e366 169 if(switch_event == 1){
jah128 0:8a5497a2e366 170 IF_update_switch();
jah128 0:8a5497a2e366 171 switch_event = 0;
jah128 0:8a5497a2e366 172 event--;
jah128 0:8a5497a2e366 173 }
jah128 0:8a5497a2e366 174 if(change_id_event == 1){
jah128 0:8a5497a2e366 175 // The user ID for the robot has been changed
jah128 0:8a5497a2e366 176 IF_update_user_id();
jah128 0:8a5497a2e366 177 change_id_event = 0;
jah128 0:8a5497a2e366 178 event--;
jah128 0:8a5497a2e366 179 }
jah128 0:8a5497a2e366 180 }
jah128 0:8a5497a2e366 181 }
jah128 0:8a5497a2e366 182 }
jah128 0:8a5497a2e366 183
jah128 0:8a5497a2e366 184 void IF_update_encoders()
jah128 0:8a5497a2e366 185 {
jah128 0:8a5497a2e366 186 char rwep = previous_wheel_encoder_byte >> 2;
jah128 0:8a5497a2e366 187 char rwe = wheel_encoder_byte >> 2;
jah128 0:8a5497a2e366 188 char lwep = previous_wheel_encoder_byte % 4;
jah128 0:8a5497a2e366 189 char lwe = wheel_encoder_byte % 4;
jah128 0:8a5497a2e366 190 //pc.printf("L:%d P:%d R:%d P:%d \n",lwe,lwep,rwe,rwep);
jah128 0:8a5497a2e366 191 if(lwe == 0 && lwep==1) left_encoder++;
jah128 0:8a5497a2e366 192 if(lwe == 0 && lwep==2) left_encoder--;
jah128 0:8a5497a2e366 193 if(rwe == 0 && rwep==1) right_encoder++;
jah128 0:8a5497a2e366 194 if(rwe == 0 && rwep==2) right_encoder--;
jah128 0:8a5497a2e366 195 }
jah128 0:8a5497a2e366 196
jah128 0:8a5497a2e366 197 void IF_update_user_id()
jah128 0:8a5497a2e366 198 {
jah128 0:8a5497a2e366 199 }
jah128 0:8a5497a2e366 200
jah128 0:8a5497a2e366 201 void IF_update_switch()
jah128 0:8a5497a2e366 202 {
jah128 0:8a5497a2e366 203 // The user switch has changed state
jah128 0:8a5497a2e366 204 // In this implementation we will only act on positive changes (rising edges)
jah128 0:8a5497a2e366 205 // Subtracting new_state from (new_state & old_state) gives the positive changes
jah128 0:8a5497a2e366 206 char positive_change = switch_byte - (switch_byte & previous_switch_byte);
jah128 0:8a5497a2e366 207 if(demo_on) demo_handle_switch_event(positive_change);
jah128 0:8a5497a2e366 208 else handle_switch_event(positive_change);
jah128 0:8a5497a2e366 209 }
jah128 0:8a5497a2e366 210
jah128 0:8a5497a2e366 211 void reset_encoders()
jah128 0:8a5497a2e366 212 {
jah128 0:8a5497a2e366 213 left_encoder = 0;
jah128 0:8a5497a2e366 214 right_encoder = 0;
jah128 0:8a5497a2e366 215 }
jah128 0:8a5497a2e366 216
jah128 0:8a5497a2e366 217 void debug(const char* format, ...)
jah128 0:8a5497a2e366 218 {
jah128 0:8a5497a2e366 219 char buffer[256];
jah128 0:8a5497a2e366 220 if (debug_mode){
jah128 0:8a5497a2e366 221 va_list vl;
jah128 0:8a5497a2e366 222 va_start(vl, format);
jah128 0:8a5497a2e366 223 vsprintf(buffer,format,vl);
jah128 0:8a5497a2e366 224 if(debug_output & 2) bt.printf("%s", buffer);
jah128 0:8a5497a2e366 225 if(debug_output & 1) pc.printf("%s", buffer);
jah128 0:8a5497a2e366 226 if(debug_output & 4) display.debug_page(buffer,strlen(buffer));
jah128 0:8a5497a2e366 227 va_end(vl);
jah128 0:8a5497a2e366 228 }
jah128 0:8a5497a2e366 229 }
jah128 0:8a5497a2e366 230
jah128 0:8a5497a2e366 231 float get_uptime(void)
jah128 0:8a5497a2e366 232 {
jah128 0:8a5497a2e366 233 return uptime.read() + (timer_minute_count * 60);
jah128 0:8a5497a2e366 234 }
jah128 0:8a5497a2e366 235
jah128 0:8a5497a2e366 236 void pause_user_code(float period){
jah128 0:8a5497a2e366 237 user_code_restore_mode = user_code_running;
jah128 0:8a5497a2e366 238 user_code_running = 0;
jah128 0:8a5497a2e366 239 pause_usercode_timeout.attach(&IF_end_pause_user_code, period);
jah128 0:8a5497a2e366 240 }
jah128 0:8a5497a2e366 241
jah128 0:8a5497a2e366 242 void IF_end_pause_user_code(){
jah128 0:8a5497a2e366 243 user_code_running = user_code_restore_mode;
jah128 0:8a5497a2e366 244 }