James O'Keeffe / Mbed 2 deprecated BeaconDemo_RobotCodeNew

ft. button press reset

Dependencies:   mbed

Fork of BeaconDemo_RobotCode by Science Memeseum

PsiSwarm/psiswarm.cpp@5:598298aa4900, 2015-10-13 (annotated)

Committer:
jah128
Date:
Tue Oct 13 11:47:14 2015 +0000
Revision:
5:598298aa4900
Parent:
4:25039ea5eb09
Child:
6:ff3c66f7372b
Added voltage regulator testing 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 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 5:598298aa4900 56
jah128 5:598298aa4900 57 char testing_voltage_regulators_flag = 1;
jah128 5:598298aa4900 58 char power_good_motor_left = 2;
jah128 5:598298aa4900 59 char power_good_motor_right = 2;
jah128 5:598298aa4900 60 char power_good_infrared = 2;
jah128 5:598298aa4900 61 char status_dc_in = 2;
jah128 5:598298aa4900 62 char status_charging = 2;
jah128 5:598298aa4900 63
jah128 0:8a5497a2e366 64 char switch_byte;
jah128 0:8a5497a2e366 65 char previous_switch_byte;
jah128 0:8a5497a2e366 66
jah128 0:8a5497a2e366 67 char debug_mode = DEBUG_MODE;
jah128 0:8a5497a2e366 68 char debug_output = DEBUG_OUTPUT_STREAM;
jah128 0:8a5497a2e366 69
jah128 2:a6214fd156ff 70 char firmware_bytes[21];
jah128 0:8a5497a2e366 71
jah128 0:8a5497a2e366 72 int base_colour_sensor_raw_values [4];
jah128 0:8a5497a2e366 73 int top_colour_sensor_raw_values [4];
jah128 0:8a5497a2e366 74
jah128 0:8a5497a2e366 75 char waiting_for_ultrasonic = 0;
jah128 0:8a5497a2e366 76 int ultrasonic_distance = 0;
jah128 0:8a5497a2e366 77 char ultrasonic_distance_updated = 0;
jah128 0:8a5497a2e366 78
jah128 3:cd048f6e544e 79
jah128 3:cd048f6e544e 80 float line_position = 0;
jah128 3:cd048f6e544e 81 char line_found = 0;
jah128 3:cd048f6e544e 82
jah128 0:8a5497a2e366 83 unsigned short background_ir_values [8];
jah128 0:8a5497a2e366 84 unsigned short illuminated_ir_values [8];
jah128 0:8a5497a2e366 85 float reflected_ir_distances [8];
jah128 0:8a5497a2e366 86 char ir_values_stored = 0;
jah128 0:8a5497a2e366 87 unsigned short background_base_ir_values [5];
jah128 0:8a5497a2e366 88 unsigned short illuminated_base_ir_values [5];
jah128 0:8a5497a2e366 89 char base_ir_values_stored = 0;
jah128 0:8a5497a2e366 90
jah128 0:8a5497a2e366 91 float motor_left_speed;
jah128 0:8a5497a2e366 92 float motor_right_speed;
jah128 0:8a5497a2e366 93 char motor_left_brake;
jah128 0:8a5497a2e366 94 char motor_right_brake;
jah128 0:8a5497a2e366 95
jah128 0:8a5497a2e366 96 char demo_on = 0;
jah128 0:8a5497a2e366 97 char event = 0;
jah128 0:8a5497a2e366 98 char change_id_event = 0;
jah128 0:8a5497a2e366 99 char encoder_event = 0;
jah128 0:8a5497a2e366 100 char switch_event = 0;
jah128 0:8a5497a2e366 101 char user_code_running = 0;
jah128 0:8a5497a2e366 102 char user_code_restore_mode = 0;
jah128 0:8a5497a2e366 103 char system_warnings = 0;
jah128 0:8a5497a2e366 104
jah128 0:8a5497a2e366 105 int ir_pulse_delay = DEFAULT_IR_PULSE_DELAY;
jah128 0:8a5497a2e366 106 int base_ir_pulse_delay = DEFAULT_BASE_IR_PULSE_DELAY;
jah128 0:8a5497a2e366 107
jah128 0:8a5497a2e366 108 /**
jah128 0:8a5497a2e366 109 * init()
jah128 0:8a5497a2e366 110 *
jah128 0:8a5497a2e366 111 * Main initialisation routine for the PsiSwarm robot
jah128 0:8a5497a2e366 112 *
jah128 0:8a5497a2e366 113 * Set up the GPIO expansion ICs, launch demo mode if button is held
jah128 0:8a5497a2e366 114 */
jah128 0:8a5497a2e366 115 void init()
jah128 0:8a5497a2e366 116 {
jah128 0:8a5497a2e366 117 firmware_version=0;
jah128 0:8a5497a2e366 118 timer_minute_count = 0;
jah128 0:8a5497a2e366 119 timer_ticker.attach(&IF_update_minutes, 300);
jah128 0:8a5497a2e366 120 uptime.start();
jah128 0:8a5497a2e366 121 primary_i2c.frequency(400000);
jah128 0:8a5497a2e366 122 IF_setup_serial_interfaces();
jah128 0:8a5497a2e366 123 debug("PsiSwarm Robot Library %1.2f\n\n",SOFTWARE_VERSION_CODE);
jah128 0:8a5497a2e366 124 debug("- Setting up serial interface\n");
jah128 0:8a5497a2e366 125 debug("- Reading firmware: ");
jah128 0:8a5497a2e366 126 if(read_firmware() == 1){
jah128 1:f6356cf1cefc 127 debug("Version %3.2f\n",firmware_version);
jah128 0:8a5497a2e366 128 }else debug("INVALID\n");
jah128 4:25039ea5eb09 129 debug("- Setting up PIC microcontroller\n");
jah128 4:25039ea5eb09 130 // IF_check_pic_firmware();
jah128 0:8a5497a2e366 131 debug("- Setting up LED drivers\n");
jah128 0:8a5497a2e366 132 IF_init_leds();
jah128 0:8a5497a2e366 133 if(IF_setup_led_expansion_ic() != 0) {
jah128 0:8a5497a2e366 134 debug("- WARNING: No I2C acknowledge for LED driver\n");
jah128 0:8a5497a2e366 135 system_warnings += 1;
jah128 0:8a5497a2e366 136 }
jah128 0:8a5497a2e366 137 debug("- Setting up motor drivers\n");
jah128 0:8a5497a2e366 138 IF_init_motors();
jah128 0:8a5497a2e366 139 debug("- Setting up GPIO expansion\n");
jah128 0:8a5497a2e366 140 reset_encoders();
jah128 0:8a5497a2e366 141 IF_setup_gpio_expansion_ic();
jah128 0:8a5497a2e366 142 debug("- Setting up temperature sensor\n");
jah128 0:8a5497a2e366 143 IF_setup_temperature_sensor();
jah128 0:8a5497a2e366 144 debug("- Setting up base colour sensor\n");
jah128 0:8a5497a2e366 145 IF_check_base_colour_sensor();
jah128 0:8a5497a2e366 146 debug("- Setting up ultrasonic sensor\n");
jah128 0:8a5497a2e366 147 //enable_ultrasonic_ticker();
jah128 0:8a5497a2e366 148 debug("- Robot ID: %d\n",robot_id);
jah128 0:8a5497a2e366 149 char switchstate = IF_get_switch_state();
jah128 0:8a5497a2e366 150 debug("- Switch State : %d\n",switchstate);
jah128 0:8a5497a2e366 151 debug("- Battery Voltage: %1.3fV\n",get_battery_voltage());
jah128 0:8a5497a2e366 152 debug("- DC Voltage : %1.3fV\n",get_dc_voltage());
jah128 0:8a5497a2e366 153 debug("- Current Draw : %1.3fA\n",get_current());
jah128 0:8a5497a2e366 154 debug("- Temperature : %1.3fC\n",get_temperature());
jah128 0:8a5497a2e366 155 char demo = 0;
jah128 0:8a5497a2e366 156 if(ENABLE_DEMO == 1 && switchstate > 0) demo=1;
jah128 0:8a5497a2e366 157 display.init_display(demo);
jah128 0:8a5497a2e366 158 event_handler.attach_us(&IF_handle_events, 1000);
jah128 0:8a5497a2e366 159 if(demo > 0){
jah128 0:8a5497a2e366 160 debug("- Demo mode button is pressed\n");
jah128 0:8a5497a2e366 161 wait(1.0);
jah128 0:8a5497a2e366 162 demo = IF_get_switch_state();
jah128 0:8a5497a2e366 163 if(demo > 0) demo_mode();
jah128 0:8a5497a2e366 164 display.init_display(0);
jah128 0:8a5497a2e366 165 }
jah128 0:8a5497a2e366 166 }
jah128 0:8a5497a2e366 167
jah128 0:8a5497a2e366 168 void IF_update_minutes(){
jah128 0:8a5497a2e366 169 uptime.reset();
jah128 0:8a5497a2e366 170 timer_minute_count += 5;
jah128 0:8a5497a2e366 171 }
jah128 0:8a5497a2e366 172
jah128 0:8a5497a2e366 173 void IF_handle_events()
jah128 0:8a5497a2e366 174 {
jah128 0:8a5497a2e366 175 // This is the main 'operating system' thread that handles events from robot stimuli
jah128 0:8a5497a2e366 176 // By default it is run every 1ms and checks if there are events to handle
jah128 0:8a5497a2e366 177 if(event > 0){
jah128 0:8a5497a2e366 178 // 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 179 if(encoder_event == 1){
jah128 0:8a5497a2e366 180 // The encoders have changed; update the encoder values
jah128 0:8a5497a2e366 181 IF_update_encoders();
jah128 0:8a5497a2e366 182 encoder_event = 0;
jah128 0:8a5497a2e366 183 event--;
jah128 0:8a5497a2e366 184 } else {
jah128 0:8a5497a2e366 185 if(switch_event == 1){
jah128 0:8a5497a2e366 186 IF_update_switch();
jah128 0:8a5497a2e366 187 switch_event = 0;
jah128 0:8a5497a2e366 188 event--;
jah128 0:8a5497a2e366 189 }
jah128 0:8a5497a2e366 190 if(change_id_event == 1){
jah128 0:8a5497a2e366 191 // The user ID for the robot has been changed
jah128 0:8a5497a2e366 192 IF_update_user_id();
jah128 0:8a5497a2e366 193 change_id_event = 0;
jah128 0:8a5497a2e366 194 event--;
jah128 0:8a5497a2e366 195 }
jah128 0:8a5497a2e366 196 }
jah128 0:8a5497a2e366 197 }
jah128 0:8a5497a2e366 198 }
jah128 0:8a5497a2e366 199
jah128 0:8a5497a2e366 200 void IF_update_encoders()
jah128 0:8a5497a2e366 201 {
jah128 0:8a5497a2e366 202 char rwep = previous_wheel_encoder_byte >> 2;
jah128 0:8a5497a2e366 203 char rwe = wheel_encoder_byte >> 2;
jah128 0:8a5497a2e366 204 char lwep = previous_wheel_encoder_byte % 4;
jah128 0:8a5497a2e366 205 char lwe = wheel_encoder_byte % 4;
jah128 0:8a5497a2e366 206 //pc.printf("L:%d P:%d R:%d P:%d \n",lwe,lwep,rwe,rwep);
jah128 0:8a5497a2e366 207 if(lwe == 0 && lwep==1) left_encoder++;
jah128 0:8a5497a2e366 208 if(lwe == 0 && lwep==2) left_encoder--;
jah128 0:8a5497a2e366 209 if(rwe == 0 && rwep==1) right_encoder++;
jah128 0:8a5497a2e366 210 if(rwe == 0 && rwep==2) right_encoder--;
jah128 4:25039ea5eb09 211 if(left_encoder % 100 == 0) pc.printf("L:%d\n",left_encoder);
jah128 0:8a5497a2e366 212 }
jah128 0:8a5497a2e366 213
jah128 0:8a5497a2e366 214 void IF_update_user_id()
jah128 0:8a5497a2e366 215 {
jah128 0:8a5497a2e366 216 }
jah128 0:8a5497a2e366 217
jah128 0:8a5497a2e366 218 void IF_update_switch()
jah128 0:8a5497a2e366 219 {
jah128 0:8a5497a2e366 220 // The user switch has changed state
jah128 0:8a5497a2e366 221 // In this implementation we will only act on positive changes (rising edges)
jah128 0:8a5497a2e366 222 // Subtracting new_state from (new_state & old_state) gives the positive changes
jah128 0:8a5497a2e366 223 char positive_change = switch_byte - (switch_byte & previous_switch_byte);
jah128 0:8a5497a2e366 224 if(demo_on) demo_handle_switch_event(positive_change);
jah128 0:8a5497a2e366 225 else handle_switch_event(positive_change);
jah128 0:8a5497a2e366 226 }
jah128 0:8a5497a2e366 227
jah128 0:8a5497a2e366 228 void reset_encoders()
jah128 0:8a5497a2e366 229 {
jah128 0:8a5497a2e366 230 left_encoder = 0;
jah128 0:8a5497a2e366 231 right_encoder = 0;
jah128 0:8a5497a2e366 232 }
jah128 0:8a5497a2e366 233
jah128 0:8a5497a2e366 234 void debug(const char* format, ...)
jah128 0:8a5497a2e366 235 {
jah128 0:8a5497a2e366 236 char buffer[256];
jah128 0:8a5497a2e366 237 if (debug_mode){
jah128 0:8a5497a2e366 238 va_list vl;
jah128 0:8a5497a2e366 239 va_start(vl, format);
jah128 0:8a5497a2e366 240 vsprintf(buffer,format,vl);
jah128 0:8a5497a2e366 241 if(debug_output & 2) bt.printf("%s", buffer);
jah128 0:8a5497a2e366 242 if(debug_output & 1) pc.printf("%s", buffer);
jah128 0:8a5497a2e366 243 if(debug_output & 4) display.debug_page(buffer,strlen(buffer));
jah128 0:8a5497a2e366 244 va_end(vl);
jah128 0:8a5497a2e366 245 }
jah128 0:8a5497a2e366 246 }
jah128 0:8a5497a2e366 247
jah128 0:8a5497a2e366 248 float get_uptime(void)
jah128 0:8a5497a2e366 249 {
jah128 0:8a5497a2e366 250 return uptime.read() + (timer_minute_count * 60);
jah128 0:8a5497a2e366 251 }
jah128 0:8a5497a2e366 252
jah128 0:8a5497a2e366 253 void pause_user_code(float period){
jah128 0:8a5497a2e366 254 user_code_restore_mode = user_code_running;
jah128 0:8a5497a2e366 255 user_code_running = 0;
jah128 0:8a5497a2e366 256 pause_usercode_timeout.attach(&IF_end_pause_user_code, period);
jah128 0:8a5497a2e366 257 }
jah128 0:8a5497a2e366 258
jah128 0:8a5497a2e366 259 void IF_end_pause_user_code(){
jah128 0:8a5497a2e366 260 user_code_running = user_code_restore_mode;
jah128 0:8a5497a2e366 261 }