psiswarm.cpp

00001 /* University of York Robotics Laboratory PsiSwarm Library: PsiSwarm Core Source File
00002  *
00003  * File: psiswarm.cpp
00004  *
00005  * (C) Dept. Electronics & Computer Science, University of York
00006  * James Hilder, Alan Millard, Alexander Horsfield, Homero Elizondo, Jon Timmis
00007  *
00008  * PsiSwarm Library Version: 0.41
00009  *
00010  * March 2016
00011  *
00012  *
00013  */
00014 
00015 #include "psiswarm.h"
00016 
00017 //Setup MBED connections to PsiSwarm Robot
00018 Serial pc(USBTX,USBRX);
00019 I2C primary_i2c (p9, p10);
00020 InterruptIn gpio_interrupt (p12);
00021 Serial bt(p13, p14);
00022 AnalogIn vin_current(p15);
00023 AnalogIn vin_battery(p16);
00024 AnalogIn vin_dc(p17);
00025 PwmOut motor_left_f (p21);
00026 PwmOut motor_left_r (p22);
00027 PwmOut motor_right_f(p23);
00028 PwmOut motor_right_r(p24);
00029 PwmOut center_led_red(p25);
00030 PwmOut center_led_green(p26);
00031 Display display;  //Connects to i2c(p28,p27), reset(p29), backlight(p30)
00032 DigitalOut mbed_led1(LED1);
00033 DigitalOut mbed_led2(LED2);
00034 DigitalOut mbed_led3(LED3);
00035 DigitalOut mbed_led4(LED4);
00036 
00037 
00038 float center_led_brightness;
00039 float backlight_brightness;
00040 
00041 Ticker event_handler;
00042 Timer uptime;
00043 Timeout pause_usercode_timeout;
00044 Ticker ultrasonic_ticker;
00045 Timeout ultrasonic_timeout;
00046 int timer_minute_count;
00047 Ticker timer_ticker;
00048 
00049 float firmware_version;
00050 float pcb_version;
00051 float serial_number;
00052 
00053 char has_compass=0;
00054 char has_side_ir=1;
00055 char has_base_ir=1;
00056 char has_base_colour_sensor=0;
00057 char has_top_colour_sensor=0;
00058 char has_wheel_encoders=0;
00059 char has_audio_pic=0;
00060 char has_ultrasonic_sensor=0;
00061 char has_temperature_sensor=0;
00062 char has_recharging_circuit=0;
00063 char has_433_radio=0;
00064 
00065 char robot_id;
00066 char previous_robot_id;
00067 
00068 char wheel_encoder_byte;
00069 char previous_wheel_encoder_byte;
00070 signed int left_encoder;
00071 signed int right_encoder;
00072 
00073 char time_based_motor_action = 0;
00074 
00075 char testing_voltage_regulators_flag = 1;
00076 char power_good_motor_left = 2;
00077 char power_good_motor_right = 2;
00078 char power_good_infrared = 2;
00079 char status_dc_in = 2;
00080 char status_charging = 2;
00081 
00082 char switch_byte;
00083 char previous_switch_byte;
00084 
00085 
00086 char use_motor_calibration = USE_MOTOR_CALIBRATION;
00087 char motor_calibration_set;
00088 float left_motor_calibration_value = 1.0;
00089 float right_motor_calibration_value = 1.0;
00090 
00091 char debug_mode = DEBUG_MODE;
00092 char debug_output = DEBUG_OUTPUT_STREAM;
00093 
00094 char firmware_bytes[21];
00095 
00096 int base_colour_sensor_raw_values [4];
00097 int top_colour_sensor_raw_values [4];
00098 
00099 char waiting_for_ultrasonic = 0;
00100 int ultrasonic_distance = 0;
00101 char ultrasonic_distance_updated = 0;
00102 
00103 
00104 float line_position = 0;
00105 char line_found = 0;
00106 
00107 unsigned short background_ir_values [8];
00108 unsigned short illuminated_ir_values [8];
00109 float reflected_ir_distances [8];
00110 char ir_values_stored = 0;
00111 unsigned short background_base_ir_values [5];
00112 unsigned short illuminated_base_ir_values [5];
00113 char base_ir_values_stored = 0;
00114 
00115 float motor_left_speed;
00116 float motor_right_speed;
00117 char motor_left_brake;
00118 char motor_right_brake;
00119 
00120 char demo_on = 0;
00121 char event = 0;
00122 char change_id_event = 0;
00123 char encoder_event = 0;
00124 char switch_event = 0;
00125 char user_code_running = 0;
00126 char user_code_restore_mode = 0;
00127 char system_warnings = 0;
00128 
00129 
00130 vector<string> basic_filenames; //filenames are stored in a vector string
00131 char psi_basic_file_count = 0;
00132 char use_flash_basic = 0;
00133 char file_transfer_mode = 0;
00134 
00135 int ir_pulse_delay = DEFAULT_IR_PULSE_DELAY;
00136 int base_ir_pulse_delay = DEFAULT_BASE_IR_PULSE_DELAY;
00137 
00138 
00139 
00140 
00141 /**
00142  * init()
00143  *
00144  * Main initialisation routine for the PsiSwarm robot
00145  *
00146  * Set up the GPIO expansion ICs, launch demo mode if button is held
00147  */
00148 void init()
00149 {
00150     firmware_version=0;
00151     timer_minute_count = 0;
00152     timer_ticker.attach(&IF_update_minutes, 300);
00153     uptime.start();
00154     primary_i2c.frequency(400000);
00155     IF_setup_serial_interfaces();
00156     debug("PsiSwarm Robot Library %1.2f\n\n",SOFTWARE_VERSION_CODE);
00157     debug("- Setting up serial interface\n");
00158     debug("- Reading firmware: ");
00159     if(read_firmware() == 1) {
00160         debug("Version %3.2f\n",firmware_version);
00161         IF_get_hardware_description();
00162         if(use_motor_calibration){
00163             if(!motor_calibration_set){
00164                 if(firmware_version < 1.1){
00165                     debug("- WARNING: This firmware is incompatible with motor calibration");
00166                     debug("- WARNING: Please update the firmware to use this feature.");
00167                     use_motor_calibration = 0;   
00168                 }   
00169                 else {
00170                     debug("- WARNING: Motor calibration values have not been stored in firmware");
00171                     debug("- WARNING: Run calibration routine to use this feature.");
00172                     use_motor_calibration = 0;   
00173                 }
00174             }
00175             else {
00176                 debug("- Motor calibration in use [LEFT:%0.4f RIGHT:%0.4f]",left_motor_calibration_value,right_motor_calibration_value);   
00177             }
00178         }
00179     } else {
00180         debug("INVALID\n");
00181         debug("- WARNING: Check firmware to enable robot features");
00182     }
00183     if(ENABLE_BASIC == 1) {
00184         read_list_of_file_names();
00185         if(psi_basic_file_count == 0) {
00186             debug("- No PsiBasic files found\n");
00187         } else use_flash_basic = 1;
00188     }
00189     debug("- Setting up PIC microcontroller\n");
00190     // IF_check_pic_firmware();
00191     debug("- Setting up LED drivers\n");
00192     IF_init_leds();
00193     if(IF_setup_led_expansion_ic() != 0) {
00194         debug("- WARNING: No I2C acknowledge for LED driver\n");
00195         system_warnings += 1;
00196     }
00197     debug("- Setting up motor drivers\n");
00198     IF_init_motors();
00199     debug("- Setting up GPIO expansion\n");
00200     reset_encoders();
00201     IF_setup_gpio_expansion_ic();
00202     if(has_temperature_sensor) {
00203         debug("- Setting up temperature sensor\n");
00204         IF_setup_temperature_sensor();
00205     }
00206     if(has_base_colour_sensor) {
00207         debug("- Setting up base colour sensor\n");
00208         IF_check_base_colour_sensor();
00209     }
00210     if(has_ultrasonic_sensor) {
00211         debug("- Setting up ultrasonic sensor\n");
00212         //enable_ultrasonic_ticker();
00213     }
00214 
00215     debug("- Robot ID: %d\n",robot_id);
00216     char switchstate = IF_get_switch_state();
00217     debug("- Switch State   : %d\n",switchstate);
00218     debug("- Battery Voltage: %1.3fV\n",get_battery_voltage());
00219     debug("- DC Voltage     : %1.3fV\n",get_dc_voltage());
00220     debug("- Current Draw   : %1.3fA\n",get_current());
00221     if(has_temperature_sensor){
00222     debug("- Temperature    : %1.3fC\n",get_temperature());
00223     }
00224     char demo = 0;
00225     if(ENABLE_DEMO == 1 && switchstate > 0) demo=1;
00226     display.init_display(demo);
00227     event_handler.attach_us(&IF_handle_events, 1000);
00228     if(demo > 0) {
00229         debug("- Demo mode button is pressed\n");
00230         wait(1.0);
00231         demo = IF_get_switch_state();
00232         if(demo > 0) demo_mode();
00233         display.init_display(0);
00234     }
00235     wait(1.0);
00236     display.clear_display();
00237     display.set_position(0,0);
00238     display.write_string(program_name);
00239     display.set_position(1,0);
00240     display.write_string(author_name);
00241     wait(0.1);
00242 }
00243 
00244 void IF_update_minutes()
00245 {
00246     uptime.reset();
00247     timer_minute_count += 5;
00248 }
00249 
00250 void IF_handle_events()
00251 {
00252     // This is the main 'operating system' thread that handles events from robot stimuli
00253     // By default it is run every 1ms and checks if there are events to handle
00254     if(event > 0) {
00255         // There are some events to handle.  We don't handle all events in every loop to keep the system responsive, instead they are priorised.
00256         if(encoder_event == 1) {
00257             // The encoders have changed; update the encoder values
00258             IF_update_encoders();
00259             encoder_event = 0;
00260             event--;
00261         }  else {
00262             if(switch_event == 1) {
00263                 IF_update_switch();
00264                 switch_event = 0;
00265                 event--;
00266             }
00267             if(change_id_event == 1) {
00268                 // The user ID for the robot has been changed
00269                 IF_update_user_id();
00270                 change_id_event = 0;
00271                 event--;
00272             }
00273         }
00274     }
00275 }
00276 
00277 void IF_update_encoders()
00278 {
00279     char rwep = previous_wheel_encoder_byte >> 2;
00280     char rwe = wheel_encoder_byte >> 2;
00281     char lwep = previous_wheel_encoder_byte % 4;
00282     char lwe = wheel_encoder_byte % 4;
00283     //pc.printf("L:%d P:%d   R:%d P:%d  \n",lwe,lwep,rwe,rwep);
00284     if(lwe == 0 && lwep==1) left_encoder++;
00285     if(lwe == 0 && lwep==2) left_encoder--;
00286     if(rwe == 0 && rwep==1) right_encoder++;
00287     if(rwe == 0 && rwep==2) right_encoder--;
00288     if(left_encoder % 100 == 0) pc.printf("L:%d\n",left_encoder);
00289 }
00290 
00291 void IF_update_user_id()
00292 {
00293 }
00294 
00295 void IF_update_switch()
00296 {
00297     // The user switch has changed state
00298     // In this implementation we will only act on positive changes (rising edges)
00299     // Subtracting new_state from (new_state & old_state) gives the positive changes
00300     char positive_change = switch_byte - (switch_byte & previous_switch_byte);
00301     if(demo_on) demo_handle_switch_event(positive_change);
00302     else handle_switch_event(positive_change);
00303 }
00304 
00305 void reset_encoders()
00306 {
00307     left_encoder = 0;
00308     right_encoder = 0;
00309 }
00310 
00311 void debug(const char* format, ...)
00312 {
00313     char buffer[256];
00314     if (debug_mode) {
00315         va_list vl;
00316         va_start(vl, format);
00317         vsprintf(buffer,format,vl);
00318         if(debug_output & 2) bt.printf("%s", buffer);
00319         if(debug_output & 1) pc.printf("%s", buffer);
00320         if(debug_output & 4) display.debug_page(buffer,strlen(buffer));
00321         va_end(vl);
00322     }
00323 }
00324 
00325 float get_uptime(void)
00326 {
00327     return uptime.read() + (timer_minute_count * 60);
00328 }
00329 
00330 void pause_user_code(float period)
00331 {
00332     user_code_restore_mode = user_code_running;
00333     user_code_running = 0;
00334     pause_usercode_timeout.attach(&IF_end_pause_user_code, period);
00335 }
00336 
00337 void IF_end_pause_user_code()
00338 {
00339     user_code_running = user_code_restore_mode;
00340 }
00341 
00342 void IF_get_hardware_description()
00343 {
00344         debug("- Robot serial number %1.2f\n",serial_number);
00345         debug("- PCB version %1.2f\n",pcb_version);
00346         debug("- Hardware: ");
00347         if(has_compass) debug("COMPASS, ");
00348         if(has_side_ir) debug("SIDE IR, ");
00349         if(has_base_ir) debug("BASE IR, ");
00350         if(has_base_colour_sensor) debug("BASE COLOUR, ");
00351         if(has_top_colour_sensor) debug("TOP COLOUR, ");
00352         if(has_wheel_encoders) debug("WHEEL ENC., ");
00353         if(has_audio_pic) debug("AUDIO, ");
00354         if(has_ultrasonic_sensor) debug("ULTRASONIC, ");
00355         if(has_temperature_sensor) debug("TEMPERATURE, ");
00356         if(has_recharging_circuit) debug("RECHARGING, ");
00357         if(has_433_radio) debug("433 RADIO.");
00358         debug("\n");
00359 }