James O'Keeffe / Mbed 2 deprecated BeaconDemo_RobotCodeNew

ft. button press reset

Dependencies:   mbed

Fork of BeaconDemo_RobotCode by Science Memeseum

PsiSwarm/psiswarm.cpp@8:00558287a4ef, 2015-10-22 (annotated)

Committer:
jah128
Date:
Thu Oct 22 15:36:16 2015 +0000
Revision:
8:00558287a4ef
Parent:
6:ff3c66f7372b 
Added time_based_turn_degrees

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