James O'Keeffe / Mbed 2 deprecated BeaconDemo_RobotCodeNew

ft. button press reset

Dependencies:   mbed

Fork of BeaconDemo_RobotCode by Science Memeseum

PsiSwarm/psiswarm.cpp@2:a6214fd156ff, 2015-10-05 (annotated)

Committer:
jah128
Date:
Mon Oct 05 14:42:16 2015 +0000
Revision:
2:a6214fd156ff
Parent:
1:f6356cf1cefc
Child:
3:cd048f6e544e
Changed nothing

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