psiswarm.cpp

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