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, Homero Elizondo, Jon Timmis
00007  * 
00008  * PsiSwarm Library Version: 0.3
00009  *
00010  * October 2015
00011  *
00012  */ 
00013 
00014 #include "psiswarm.h"
00015 
00016 //Setup MBED connections to PsiSwarm Robot
00017 Serial pc(USBTX,USBRX);
00018 I2C primary_i2c (p9, p10);
00019 InterruptIn gpio_interrupt (p12);
00020 Serial bt(p13, p14);
00021 AnalogIn vin_current(p15);
00022 AnalogIn vin_battery(p16);
00023 AnalogIn vin_dc(p17);
00024 PwmOut motor_left_f (p21);
00025 PwmOut motor_left_r (p22);
00026 PwmOut motor_right_f(p23);
00027 PwmOut motor_right_r(p24);
00028 PwmOut center_led_red(p25);
00029 PwmOut center_led_green(p26);
00030 Display display;  //Connects to i2c(p28,p27), reset(p29), backlight(p30)
00031 DigitalOut mbed_led1(LED1);
00032 DigitalOut mbed_led2(LED2);
00033 DigitalOut mbed_led3(LED3);
00034 DigitalOut mbed_led4(LED4);
00035 
00036 
00037 float center_led_brightness;
00038 float backlight_brightness;
00039 
00040 Ticker event_handler;
00041 Timer uptime;
00042 Timeout pause_usercode_timeout;
00043 Ticker ultrasonic_ticker;
00044 Timeout ultrasonic_timeout;
00045 int timer_minute_count;
00046 Ticker timer_ticker;
00047 
00048 float firmware_version;
00049 char robot_id;
00050 char previous_robot_id;
00051 
00052 char wheel_encoder_byte;
00053 char previous_wheel_encoder_byte;
00054 signed int left_encoder;
00055 signed int right_encoder;
00056 
00057 char time_based_motor_action = 0;
00058 
00059 char testing_voltage_regulators_flag = 1;
00060 char power_good_motor_left = 2;
00061 char power_good_motor_right = 2;
00062 char power_good_infrared = 2;
00063 char status_dc_in = 2;
00064 char status_charging = 2;
00065 
00066 char switch_byte;
00067 char previous_switch_byte;
00068 
00069 char debug_mode = DEBUG_MODE;
00070 char debug_output = DEBUG_OUTPUT_STREAM;
00071  
00072 char firmware_bytes[21];
00073 
00074 int base_colour_sensor_raw_values [4];
00075 int top_colour_sensor_raw_values [4];
00076 
00077 char waiting_for_ultrasonic = 0;
00078 int ultrasonic_distance = 0;
00079 char ultrasonic_distance_updated = 0;
00080 
00081 
00082 float line_position = 0;
00083 char line_found = 0;
00084 
00085 unsigned short background_ir_values [8];
00086 unsigned short illuminated_ir_values [8];
00087 float reflected_ir_distances [8];
00088 char ir_values_stored = 0;
00089 unsigned short background_base_ir_values [5];
00090 unsigned short illuminated_base_ir_values [5];
00091 char base_ir_values_stored = 0;
00092 
00093 float motor_left_speed;
00094 float motor_right_speed;
00095 char motor_left_brake;
00096 char motor_right_brake;
00097 
00098 char demo_on = 0;
00099 char event = 0;
00100 char change_id_event = 0;
00101 char encoder_event = 0;
00102 char switch_event = 0;
00103 char user_code_running = 0;
00104 char user_code_restore_mode = 0;
00105 char system_warnings = 0;
00106 
00107 int ir_pulse_delay = DEFAULT_IR_PULSE_DELAY;
00108 int base_ir_pulse_delay = DEFAULT_BASE_IR_PULSE_DELAY;
00109 
00110 /**
00111  * init()
00112  *
00113  * Main initialisation routine for the PsiSwarm robot
00114  *
00115  * Set up the GPIO expansion ICs, launch demo mode if button is held
00116  */
00117 void init()
00118 {
00119     firmware_version=0;
00120     timer_minute_count = 0;
00121     timer_ticker.attach(&IF_update_minutes, 300);
00122     uptime.start();
00123     primary_i2c.frequency(400000);
00124     IF_setup_serial_interfaces();
00125     debug("PsiSwarm Robot Library %1.2f\n\n",SOFTWARE_VERSION_CODE);
00126     debug("- Setting up serial interface\n");
00127     debug("- Reading firmware: ");
00128     if(read_firmware() == 1){
00129         debug("Version %3.2f\n",firmware_version);   
00130     }else debug("INVALID\n");
00131     debug("- Setting up PIC microcontroller\n");
00132    // IF_check_pic_firmware();
00133     debug("- Setting up LED drivers\n");
00134     IF_init_leds();    
00135     if(IF_setup_led_expansion_ic() != 0) {
00136         debug("- WARNING: No I2C acknowledge for LED driver\n");   
00137         system_warnings += 1; 
00138     }
00139     debug("- Setting up motor drivers\n");
00140     IF_init_motors();   
00141     debug("- Setting up GPIO expansion\n");
00142     reset_encoders();
00143     IF_setup_gpio_expansion_ic();
00144     debug("- Setting up temperature sensor\n");
00145     IF_setup_temperature_sensor();
00146     debug("- Setting up base colour sensor\n");
00147     IF_check_base_colour_sensor();
00148     debug("- Setting up ultrasonic sensor\n");
00149     //enable_ultrasonic_ticker();
00150     debug("- Robot ID: %d\n",robot_id);
00151     char switchstate = IF_get_switch_state(); 
00152     debug("- Switch State   : %d\n",switchstate);
00153     debug("- Battery Voltage: %1.3fV\n",get_battery_voltage());
00154     debug("- DC Voltage     : %1.3fV\n",get_dc_voltage());
00155     debug("- Current Draw   : %1.3fA\n",get_current());
00156     debug("- Temperature    : %1.3fC\n",get_temperature());
00157     char demo = 0;
00158     if(ENABLE_DEMO == 1 && switchstate > 0) demo=1;
00159     display.init_display(demo);
00160     event_handler.attach_us(&IF_handle_events, 1000);
00161     if(demo > 0){      
00162         debug("- Demo mode button is pressed\n");
00163         wait(1.0);
00164         demo = IF_get_switch_state();
00165         if(demo > 0) demo_mode();
00166         display.init_display(0);  
00167     }
00168 }
00169 
00170 void IF_update_minutes(){
00171     uptime.reset();
00172     timer_minute_count += 5;
00173 }
00174 
00175 void IF_handle_events()
00176 {
00177     // This is the main 'operating system' thread that handles events from robot stimuli
00178     // By default it is run every 1ms and checks if there are events to handle
00179     if(event > 0){
00180         // There are some events to handle.  We don't handle all events in every loop to keep the system responsive, instead they are priorised.
00181         if(encoder_event == 1){
00182             // The encoders have changed; update the encoder values
00183             IF_update_encoders();
00184             encoder_event = 0; 
00185             event--;  
00186         }  else {
00187             if(switch_event == 1){
00188                IF_update_switch();
00189                switch_event = 0;
00190                event--; 
00191             }
00192             if(change_id_event == 1){
00193                 // The user ID for the robot has been changed
00194                 IF_update_user_id();
00195                 change_id_event = 0;
00196                 event--;
00197             }
00198         }
00199     }   
00200 }
00201 
00202 void IF_update_encoders()
00203 {
00204     char rwep = previous_wheel_encoder_byte >> 2;
00205     char rwe = wheel_encoder_byte >> 2;
00206     char lwep = previous_wheel_encoder_byte % 4;
00207     char lwe = wheel_encoder_byte % 4;
00208     //pc.printf("L:%d P:%d   R:%d P:%d  \n",lwe,lwep,rwe,rwep);
00209     if(lwe == 0 && lwep==1) left_encoder++;
00210     if(lwe == 0 && lwep==2) left_encoder--;
00211     if(rwe == 0 && rwep==1) right_encoder++;
00212     if(rwe == 0 && rwep==2) right_encoder--;
00213     if(left_encoder % 100 == 0) pc.printf("L:%d\n",left_encoder);
00214 }
00215 
00216 void IF_update_user_id()
00217 {
00218 }
00219 
00220 void IF_update_switch()
00221 {
00222     // The user switch has changed state
00223     // In this implementation we will only act on positive changes (rising edges)
00224     // Subtracting new_state from (new_state & old_state) gives the positive changes
00225     char positive_change = switch_byte - (switch_byte & previous_switch_byte);
00226     if(demo_on) demo_handle_switch_event(positive_change);
00227     else handle_switch_event(positive_change);
00228 }
00229 
00230 void reset_encoders()
00231 {
00232     left_encoder = 0;
00233     right_encoder = 0;    
00234 }
00235 
00236 void debug(const char* format, ...) 
00237 {
00238   char buffer[256];
00239   if (debug_mode){
00240   va_list vl;
00241   va_start(vl, format);
00242   vsprintf(buffer,format,vl);
00243   if(debug_output & 2) bt.printf("%s", buffer);
00244   if(debug_output & 1) pc.printf("%s", buffer);
00245   if(debug_output & 4) display.debug_page(buffer,strlen(buffer));
00246   va_end(vl);
00247   }
00248 }
00249 
00250 float get_uptime(void)
00251 {
00252     return uptime.read() + (timer_minute_count * 60);   
00253 }
00254 
00255 void pause_user_code(float period){
00256     user_code_restore_mode = user_code_running;
00257     user_code_running = 0;
00258     pause_usercode_timeout.attach(&IF_end_pause_user_code, period);
00259 }
00260 
00261 void IF_end_pause_user_code(){
00262      user_code_running = user_code_restore_mode;
00263 }