UKESF Headstart Summer School / PsiSwarmLab-ScriptingBased

Modified version of the UKESF lab source which can be carried out with no knowledge of C

Fork of PsiSwarm-Headstart by UKESF Headstart Summer School

psiswarm.cpp@0:d6269d17c8cf, 2016-02-04 (annotated)

Committer:
jah128
Date:
Thu Feb 04 21:48:54 2016 +0000
Revision:
0:d6269d17c8cf
Child:
1:060690a934a9
PsiSwarm Library Version 0.4 - Initial Commit

Who changed what in which revision?

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