piswarm.cpp

00001 /*******************************************************************************************
00002  *
00003  * University of York Robot Lab Pi Swarm Robot Library
00004  *
00005  * (C) Dr James Hilder, Dept. Electronics & Computer Science, University of York
00006  * 
00007  * Version 0.5  February 2014
00008  *
00009  * Designed for use with the Pi Swarm Board (enhanced MBED sensor board) v1.2
00010  *
00011  ******************************************************************************************/
00012 
00013 #include "piswarm.h" 
00014 
00015 // Variables
00016 DigitalOut  gpio_led (LED1);
00017 DigitalOut  calibrate_led (LED2);
00018 DigitalOut  adc_led (LED3);
00019 DigitalOut  interrupt_led (LED4);
00020 InterruptIn expansion_interrupt (p29); 
00021 Timeout debounce_timeout;
00022 Timeout led_timeout;
00023 Ticker calibrate_ticker;
00024 char id;
00025 
00026 char oled_array [10];
00027 char enable_ir_ldo = 0;
00028 char enable_led_ldo = 0;
00029 char calibrate_led_state = 1;
00030 char switches = 0;
00031 char cled_red = 0;
00032 char cled_green = 0;
00033 char cled_blue = 0;
00034 char oled_red = 0;
00035 char oled_green = 0;
00036 char oled_blue = 0;
00037 char cled_enable = 0;
00038 char cled_brightness = CENTER_LED_BRIGHTNESS;  // The default brightness value:  must be between 0 (dimmest) and 100 (full brightness)
00039 char oled_brightness = OUTER_LED_BRIGHTNESS;  // The default brightness value:  must be between 0 (dimmest) and 100 (full brightness)
00040 float gyro_cal = 3.3;
00041 float gyro_error = 0;
00042 float acc_x_cal = 3350;
00043 float acc_y_cal = 3350;
00044 float acc_z_cal = 3350;
00045 float left_speed = 0;
00046 float right_speed = 0;
00047 signed short mag_values [3];
00048 
00049 
00050 PiSwarm::PiSwarm() :  Stream("PiSwarm"), _ser(p9, p10), _oled_r(p26), _oled_g(p25), _oled_b(p24), _cled_r(p23), _cled_g(p22), _cled_b(p21), _gyro(p16), _accel_x(p19), _accel_y(p18), _accel_z(p17), _temperature(p20), _light(p15), _i2c(p28,p27), _irpulse_1(p12), _irpulse_2(p13), _rf(p5,p6,p7,p8,p11)  {
00051     setup();
00052     reset();
00053 }
00054 
00055 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
00056 // Outer LED Functions
00057 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
00058 
00059 // Returns the current setting for the outer LED colour.  Returned colour is stored as 24-bit positive integer (R<<16 + G<<8 + B).  Used by communication stack. 
00060 int PiSwarm::get_oled_colour(){
00061     return (oled_red << 16) + (oled_green << 8) + oled_blue;
00062 }
00063 
00064 // Returns the current enable state an individual LED.  oled = LED to return enable state.  Returns 0 for disabled, 1 for enabled
00065 char PiSwarm::get_oled_state(char oled){
00066     if(oled_array[oled] == 1) return 1;
00067     return 0;
00068 }
00069 
00070 // Set the colour of the outer LED.  Values for red, green and blue range from 0 (off) to 255 (maximum).
00071 void PiSwarm::set_oled_colour( char red, char green, char blue ){
00072     oled_red = red;
00073     oled_green = green;
00074     oled_blue = blue;
00075     _oled_r.pulsewidth_us(red);
00076     _oled_g.pulsewidth_us(green);
00077     _oled_b.pulsewidth_us(blue);
00078 }
00079 
00080 // Set the state of an individual LED. oled = LED to enable. value = 0 for disable, 1 for enable. Use to change 1 LED without affecting others.
00081 void PiSwarm::set_oled(char oled, char value){
00082     oled_array[oled]=value;
00083 }
00084 
00085 // Sets the state of all 10 LEDs. oled_x = 0 for disable, 1 for enable. Use to change all 10 LEDs at once
00086 void PiSwarm::set_oleds(char oled_0, char oled_1, char oled_2, char oled_3, char oled_4, char oled_5, char oled_6, char oled_7, char oled_8, char oled_9){
00087     oled_array[0]=oled_0;
00088     oled_array[1]=oled_1;
00089     oled_array[2]=oled_2;
00090     oled_array[3]=oled_3;
00091     oled_array[4]=oled_4;
00092     oled_array[5]=oled_5;
00093     oled_array[6]=oled_6;
00094     oled_array[7]=oled_7;
00095     oled_array[8]=oled_8;
00096     oled_array[9]=oled_9;
00097     activate_oleds();
00098 }
00099 
00100 // Sets all outer LEDs to disabled and turns off LED power supply.
00101 void PiSwarm::turn_off_all_oleds(){
00102     for(int i=0;i<10;i++){
00103         oled_array[i]=0;
00104     }
00105     activate_oleds();
00106     enable_led_ldo = 0;
00107     enable_ldo_outputs();
00108     char data[3];
00109     data[0]=0x88;  //Write to bank 1 then bank 2
00110     data[1]=0x00;  //Reset everything in bank 1
00111     data[2]=0x00;  //Reset everything in bank 2
00112     _i2c.write(EXPANSION_IC_ADDRESS,data,3,false);    
00113 }
00114 
00115 // Set the outer LED brightness (total period of PWM output increases as brightness decreases). Ranges from 0 (minimum) to 255 (maximum)
00116 void PiSwarm::set_oled_brightness ( char brightness ) {
00117     if ( brightness > 100 ) brightness = 100;
00118     oled_brightness = brightness;
00119     int oled_period = (104 - brightness);
00120     oled_period *= oled_period;
00121     oled_period /= 10;
00122     oled_period += 255;    
00123     if(CALIBRATE_COLOURS!=0)_oled_r.period_us(oled_period * 2);
00124     else _oled_r.period_us(oled_period);
00125     if(CALIBRATE_COLOURS!=0)_oled_g.period_us(oled_period * 2);
00126     else _oled_g.period_us(oled_period);
00127     _oled_b.period_us(oled_period);
00128 }
00129 
00130 // Sends the messages to enable/disable outer LEDs
00131 void PiSwarm::activate_oleds(){
00132     if(enable_led_ldo == 0){
00133         enable_led_ldo = 1;
00134         enable_ldo_outputs();
00135     }
00136     char data[3];
00137     data[0]=0x88;  //Write to bank 1 then bank 2
00138     data[1]=0x00;  //Reset everything in bank 1
00139     data[2]=0x00;  //Reset everything in bank 2
00140     if(oled_array[0]>0) data[1]+=1;
00141     if(oled_array[1]>0) data[1]+=2;
00142     if(oled_array[2]>0) data[1]+=4;
00143     if(oled_array[3]>0) data[1]+=8;
00144     if(oled_array[4]>0) data[1]+=16;
00145     if(oled_array[5]>0) data[1]+=32;
00146     if(oled_array[6]>0) data[1]+=64;
00147     if(oled_array[7]>0) data[1]+=128;
00148     if(oled_array[8]>0) data[2]+=1;
00149     if(oled_array[9]>0) data[2]+=2;
00150     _i2c.write(EXPANSION_IC_ADDRESS,data,3,false);    
00151 }
00152 
00153 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
00154 // Center LED Functions
00155 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
00156 
00157 // Returns the current setting for the center LED colour.  Returned colour is stored as 24-bit positive integer (R<<16 + G<<8 + B).  Used by communication stack.
00158 int PiSwarm::get_cled_colour(){
00159     return (cled_red << 16) + (cled_green << 8) + cled_blue;
00160 }
00161 
00162 // Returns the enable state of the center LED 
00163 char PiSwarm::get_cled_state( void ){
00164     return cled_enable;
00165 }
00166 
00167 // Set the colour of the center LED.  Values for red, green and blue range from 0 (off) to 255 (maximum).
00168 void PiSwarm::set_cled_colour( char red, char green, char blue ){
00169     cled_red = red;
00170     cled_green = green;
00171     cled_blue = blue;
00172     if(cled_enable != 0) enable_cled(1);
00173 }
00174 
00175 // Turn on or off the center LED.  enable=1 to turn on, 0 to turn off
00176 void PiSwarm::enable_cled( char enable ){
00177     cled_enable = enable;
00178     if(enable != 0) {
00179         _cled_r.pulsewidth_us(cled_red);
00180         _cled_g.pulsewidth_us(cled_green);
00181         _cled_b.pulsewidth_us(cled_blue);
00182     }else{
00183         _cled_r.pulsewidth_us(0);
00184         _cled_g.pulsewidth_us(0);
00185         _cled_b.pulsewidth_us(0);
00186     }
00187 }
00188 
00189 // Set the center LED brightness (total period of PWM output increases as brightness decreases). Ranges from 0 (minimum) to 255 (maximum)
00190 void PiSwarm::set_cled_brightness ( char brightness ) {
00191     if( brightness > 100 ) brightness = 100;
00192     // Brightness is set by adjusting period of PWM.  Period ranged from ~260uS at 100% to 1336uS at 0%
00193     // When calibrate_colours = 1, red = 2x normal, green = 2x normal
00194     cled_brightness = brightness;
00195     int cled_period = (104 - brightness);
00196     cled_period *= cled_period;
00197     cled_period /= 10;
00198     cled_period += 255;
00199     if(CALIBRATE_COLOURS!=0)_cled_r.period_us(cled_period * 2); 
00200     else _cled_r.period_us(cled_period);
00201     if(CALIBRATE_COLOURS!=0)_cled_g.period_us(cled_period * 2); 
00202     else _cled_g.period_us(cled_period);
00203     _cled_b.period_us(cled_period);
00204 }
00205 
00206 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
00207 // IR Sensor Functions
00208 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
00209 
00210 // Estimates the distance to an obstacle from one of the IR sensors, defined by index (range 0-7).  The value is converted to an approximate distance in millimetres, or 100.0 if no obstacle found.
00211 float PiSwarm::read_reflected_ir_distance ( char index ){
00212     //This function attempts to read a real-world distance approximation using the IR proximity sensors
00213     //1.  Check that the IR LDO regulator is on, enable if it isn't
00214     if(enable_ir_ldo == 0){
00215         enable_ir_ldo = 1;
00216         enable_ldo_outputs();
00217     }
00218     //2.  Read the ADC value without IR emitter on
00219     unsigned short background_value = read_adc_value ( index );
00220     //3.  Enable the relevant IR emitter by turning on its pulse output
00221     switch(index){
00222         case 0: case 1: case 6: case 7:
00223         _irpulse_1 = 1;
00224         break;
00225         case 2: case 3: case 4: case 5:
00226         _irpulse_2 = 1;
00227         break;
00228     }
00229     wait_us(500);               
00230     //4.  Read the ADC value now IR emitter is on
00231     unsigned short strong_value = read_adc_value ( index );
00232     //5.  Turn off IR emitter
00233        switch(index){
00234         case 0: case 1: case 6: case 7:
00235         _irpulse_1 = 0;
00236         break;
00237         case 2: case 3: case 4: case 5:
00238         _irpulse_2 = 0;
00239         break;
00240     }
00241     //6.  Estimate distance based on 2 values
00242     float app_distance = 4000 + background_value - strong_value;
00243     if(app_distance < 0) app_distance = 0;
00244     // Very approximate: root value, divide by 2, approx distance in mm
00245     app_distance = sqrt (app_distance) / 2.0;
00246     // Then add adjustment value if value >27
00247     if(app_distance > 27) {
00248         float shift = pow(app_distance - 27,3);
00249         app_distance += shift;
00250     }
00251     if(app_distance > 90) app_distance = 100.0;
00252     return app_distance;
00253 }
00254 
00255 // Returns the illuminated raw sensor value for the IR sensor defined by index (range 0-7); turns on the emitters for a 500us pulse
00256 unsigned short PiSwarm::read_illuminated_raw_ir_value ( char index ){
00257     //This function reads the IR strength when illuminated - used for PC system debugging purposes
00258     //1.  Check that the IR LDO regulator is on, enable if it isn't
00259     if(enable_ir_ldo == 0){
00260         enable_ir_ldo = 1;
00261         enable_ldo_outputs();
00262     }
00263     //2.  Enable the relevant IR emitter by turning on its pulse output
00264     switch(index){
00265         case 0: case 1: case 6: case 7:
00266         _irpulse_1 = 1;
00267         break;
00268         case 2: case 3: case 4: case 5:
00269         _irpulse_2 = 1;
00270         break;
00271     }
00272     wait_us(500);               
00273     //3.  Read the ADC value now IR emitter is on
00274     unsigned short strong_value = read_adc_value ( index );
00275     //4.  Turn off IR emitter
00276        switch(index){
00277         case 0: case 1: case 6: case 7:
00278         _irpulse_1 = 0;
00279         break;
00280         case 2: case 3: case 4: case 5:
00281         _irpulse_2 = 0;
00282         break;
00283     }
00284     return strong_value;
00285 }
00286 
00287 // Returns the raw sensor value for the IR sensor defined by index (range 0-7).
00288 unsigned short PiSwarm::read_adc_value ( char index ){
00289     short value = 0;
00290     // Read a single value from the ADC
00291     if(index<8){
00292         char apb[1];
00293         char data[2];
00294         switch(index){
00295             case 0: apb[0]=0x80; break;
00296             case 1: apb[0]=0x90; break;
00297             case 2: apb[0]=0xA0; break;
00298             case 3: apb[0]=0xB0; break;
00299             case 4: apb[0]=0xC0; break;
00300             case 5: apb[0]=0xD0; break;
00301             case 6: apb[0]=0xE0; break;
00302             case 7: apb[0]=0xF0; break;
00303         }
00304         _i2c.write(ADC_ADDRESS,apb,1,false);
00305         _i2c.read(ADC_ADDRESS,data,2,false);
00306         value=((data[0] % 16)<<8)+data[1];
00307         if(value > 4096) value=4096;
00308         value=4096-value;
00309     }
00310     return value;
00311 }
00312 
00313 // Function enables or disables the LDO voltage regulators which supply power to the outer LEDs and the IR photo emitters.
00314 void PiSwarm::enable_ldo_outputs () {
00315     char data[2];
00316     data[0] = 0x0A;     //Address for PCA9505 Output Port 2
00317     data[1] = 0;
00318     if(enable_led_ldo) data[1] +=128;
00319     if(enable_ir_ldo) data[1] +=64;
00320     _i2c.write(EXPANSION_IC_ADDRESS,data,2,false);
00321 }
00322 
00323 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
00324 // MEMS Sensor Functions
00325 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
00326 
00327 // Returns the yaw (clockwise) rotation in degrees-per-second reported by the LY3200 gyroscope.
00328 float PiSwarm::read_gyro ( void ){
00329     // Gyro is a ST Micro LY3200ALH Yaw-rate Gyro: 0.67mV / d.p.s, 1.5V offset 
00330     float r_gyro = _gyro.read();
00331     r_gyro *= gyro_cal;     // NB 3.4 seemed to work better that 3.3 but better to run calibration routine
00332     r_gyro = 1.5 - r_gyro;  // 1.5V off-set, invert sign
00333     r_gyro *= 1492.5;       // Convert to DPS (+ve = clockwise, -ve = c.clockwise) 
00334     return r_gyro;
00335 }
00336 
00337 // Returns the acceleration in the X plane reported by the LIS332 accelerometer.  Returned value is in mg.    
00338 float PiSwarm::read_accelerometer_x ( void ){
00339     // LIS 332 AX Accelerometer - 0.363 V/g - 1.25V  
00340     float r_acc_x = (( _accel_x.read() * acc_x_cal ) - 1250.0 ) * 2.754;    // Return value in mG
00341     return r_acc_x;
00342 }
00343    
00344 // Returns the acceleration in the Y plane reported by the LIS332 accelerometer.  Returned value is in mg.    
00345 float PiSwarm::read_accelerometer_y ( void ){
00346     float r_acc_y = (( _accel_y.read() * acc_y_cal ) - 1250.0 ) * 2.754;    // Return value in mG 
00347     return r_acc_y;
00348 }
00349     
00350 // Returns the acceleration in the Z plane reported by the LIS332 accelerometer.  Returned value is in mg.     
00351 float PiSwarm::read_accelerometer_z ( void ){
00352     float r_acc_z = (( _accel_z.read() * acc_z_cal ) - 1250.0 ) * 2.754;    // Return value in mG
00353     return r_acc_z;
00354 }
00355 
00356 // Sends message to the magnetometer to read new values. Should be called before get_magnetometer_* when updated values are required
00357 char PiSwarm::read_magnetometer ( void ){
00358     char read_data[7];
00359     char success;
00360     char command_data[1];
00361     command_data[0] = 0x00; //Auto-read from register 0x00 (status)
00362     success = _i2c.write(MAGNETOMETER_ADDRESS, command_data, 1, false);
00363     _i2c.read(MAGNETOMETER_ADDRESS, read_data, 7, false);
00364     mag_values[0] = (read_data[1] << 8) + read_data[2];
00365     mag_values[1] = (read_data[3] << 8) + read_data[4];
00366     mag_values[2] = (read_data[5] << 8) + read_data[6];
00367     return success; 
00368 }
00369     
00370 // Returns the raw value for the X-plane magnetic field stored on the last call of read_magnetometer    
00371 signed short PiSwarm::get_magnetometer_x ( void ){
00372     return mag_values[0];
00373 }
00374     
00375 // Returns the raw value for the Y-plane magnetic field stored on the last call of read_magnetometer    
00376 signed short PiSwarm::get_magnetometer_y ( void ){
00377     return mag_values[1];
00378 }
00379     
00380 // Returns the raw value for the Z-plane magnetic field stored on the last call of read_magnetometer    
00381 signed short PiSwarm::get_magnetometer_z ( void ){
00382     return mag_values[2];
00383 }
00384 
00385 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
00386 // Other Sensor Functions
00387 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
00388 
00389 // Returns the temperature reported by the MCP9701 sensor in degrees centigrade.
00390 float PiSwarm::read_temperature ( void ){
00391     // Temperature Sensor is a Microchip MCP9701T-E/LT:  19.5mV/C  0C = 400mV
00392     float r_temp = _temperature.read();
00393     r_temp -= 0.1212f;      // 0.4 / 3.3
00394     r_temp *= 169.231f;  // 3.3 / .0195
00395     return r_temp;
00396 }
00397 
00398 // Returns the adjusted value (0-100) for the ambient light sensor
00399 float PiSwarm::read_light_sensor ( void ){
00400     // Light sensor is a Avago APDS-9005 Ambient Light Sensor
00401     //float r_light = (_light.read() * 141); //Gives a range of (approximately) 0=dark 100=max. light
00402     float r_light = (sqrt(_light.read() * 1.41) * 100); //Non-linear and more CPU intensive but generally gives better low-light measures
00403     if(r_light > 100) r_light = 100;
00404     return r_light;
00405 }
00406 
00407 
00408 void PiSwarm::read_raw_sensors ( int * raw_ls_array ) {
00409     _ser.putc(SEND_RAW_SENSOR_VALUES);
00410     for (int i = 0; i < 5 ; i ++) {
00411         int val = _ser.getc();
00412         val += _ser.getc() << 8;
00413         raw_ls_array [i] = val;
00414     }
00415 }
00416 
00417 // Returns the uptime of the system (since initialisation) in seconds
00418 float PiSwarm::get_uptime (void) {
00419     return _system_timer.read();
00420 }
00421 
00422 // Returns the battery level in millivolts
00423 float PiSwarm::battery() {
00424     _ser.putc(SEND_BATTERY_MILLIVOLTS);
00425     char lowbyte = _ser.getc();
00426     char hibyte  = _ser.getc();
00427     float v = ((lowbyte + (hibyte << 8))/1000.0);
00428     return(v);
00429 }
00430 
00431 // Returns the raw value for the variable resistor on the base of the platform.  Ranges from 0 to 1024.
00432 float PiSwarm::pot_voltage(void) {
00433     int volt = 0;
00434     _ser.putc(SEND_TRIMPOT);
00435     volt = _ser.getc();
00436     volt += _ser.getc() << 8;
00437     return(volt);
00438 }
00439 
00440 // Returns the ID of platform (set by the DIL switches above the display).  Range 0-31 [0 reserved].
00441 char PiSwarm::get_id () {
00442     return id;
00443 }
00444 
00445 // Return the current stored state for direction switch(es) pressed {1 = Center  2 = Right  4 = Left  8 = Down  16 = Up}
00446 char PiSwarm::get_switches () {
00447     return switches;
00448 }
00449 
00450 
00451 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
00452 // Motor Functions
00453 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
00454 
00455 // Returns the target speed of the left motor (-1.0 to 1.0)
00456 float PiSwarm::get_left_motor (){
00457     return left_speed;
00458 }
00459 
00460 // Returns the target speed of the right motor (-1.0 to 1.0)
00461 float PiSwarm::get_right_motor (){
00462     return right_speed;
00463 }
00464 
00465 // Sets the speed of the left motor (-1.0 to 1.0)
00466 void PiSwarm::left_motor (float speed) {
00467     motor(0,speed);
00468 }
00469 
00470 // Sets the speed of the right motor (-1.0 to 1.0)
00471 void PiSwarm::right_motor (float speed) {
00472     motor(1,speed);
00473 }
00474 
00475 // Drive forward at the given speed (-1.0 to 1.0)
00476 void PiSwarm::forward (float speed) {
00477     motor(0,speed);
00478     motor(1,speed);
00479 }
00480 
00481 // Drive backward at the given speed (-1.0 to 1.0)
00482 void PiSwarm::backward (float speed) {
00483     motor(0,-1.0*speed);
00484     motor(1,-1.0*speed);
00485 }
00486 
00487 // Turn on-the-spot left at the given speed (-1.0 to 1.0)
00488 void PiSwarm::left (float speed) {
00489     motor(0,speed);
00490     motor(1,-1.0*speed);
00491 }
00492 
00493 // Turn on-the-spot right at the given speed (-1.0 to 1.0)
00494 void PiSwarm::right (float speed) {
00495     motor(0,-1.0*speed);
00496     motor(1,speed);
00497 }
00498 
00499 // Stop both motors
00500 void PiSwarm::stop (void) {
00501     motor(0,0.0);
00502     motor(1,0.0);
00503 }
00504 
00505 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
00506 // Sound Functions
00507 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
00508 
00509 void PiSwarm::play_tune (char * tune, int length) {
00510     _ser.putc(DO_PLAY);
00511     _ser.putc(length);       
00512     for (int i = 0 ; i < length ; i++) {
00513         _ser.putc(tune[i]); 
00514     }
00515 }
00516 
00517 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
00518 // Display Functions
00519 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
00520 
00521 void PiSwarm::locate(int x, int y) {
00522     _ser.putc(DO_LCD_GOTO_XY);
00523     _ser.putc(x);
00524     _ser.putc(y);
00525 }
00526 
00527 void PiSwarm::cls(void) {
00528     _ser.putc(DO_CLEAR);
00529 }
00530 
00531 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
00532 // EEPROM Functions
00533 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
00534 
00535 
00536 void PiSwarm::write_eeprom_byte ( int address, char data ){
00537     char write_array[3];
00538     write_array[0] = address / 256;
00539     write_array[1] = address % 256;
00540     write_array[2] = data;
00541     _i2c.write(EEPROM_ADDRESS, write_array, 3, false);
00542     //Takes 5ms to write a page: ideally this could be done with a timer or RTOS
00543     wait(0.005);
00544 }
00545   
00546 void PiSwarm::write_eeprom_block ( int address, char length, char * data){
00547     /** Note from datasheet:
00548     Page write operations are limited to writing bytes within a single physical page,
00549     regardless of the number of bytes actually being written. Physical page
00550     boundaries start at addresses that are integer multiples of the page buffer size (or
00551     ‘page size’) and end at addresses that are integer multiples of [page size – 1]. If a
00552     Page Write command attempts to write across a physical page boundary, the
00553     result is that the data wraps around to the beginning of the current page (overwriting
00554     data previously stored there), instead of being written to the next page, as might be
00555     expected. It is therefore necessary for the application software to prevent page write
00556     operations that would attempt to cross a page boundary.   */
00557     
00558     //First check to see if first block starts at the start of a page
00559     int subpage = address % 32;
00560     
00561     //If subpage > 0 first page does not start at a page boundary
00562     int write_length = 32 - subpage;
00563     if( write_length > length ) write_length = length;
00564     char firstpage_array [write_length+2];
00565     firstpage_array[0] = address / 256;
00566     firstpage_array[1] = address % 256;
00567     for(int i=0;i<write_length;i++){
00568        firstpage_array[i+2] = data [i];
00569     } 
00570     _i2c.write(EEPROM_ADDRESS, firstpage_array, write_length + 2, false);
00571     wait(0.005);
00572     
00573     if(length > write_length){
00574         int page_count = (length + subpage) / 32;
00575         int written = write_length;
00576         for(int p=0;p<page_count;p++){
00577             int to_write = length - written;
00578             if (to_write > 32) {
00579                 to_write = 32;
00580             } 
00581             char page_array [to_write + 2];
00582             page_array[0] = (address + written) / 256;
00583             page_array[1] = (address + written) % 256;
00584             for(int i=0;i<to_write;i++){
00585                 page_array[i+2] = data[written + i];
00586             }
00587             _i2c.write(EEPROM_ADDRESS, page_array, to_write + 2, false);
00588             wait(0.005);
00589             written += 32;
00590         }
00591     } 
00592 }
00593     
00594 char PiSwarm::read_eeprom_byte ( int address ){
00595     char address_array [2];
00596     address_array[0] = address / 256;
00597     address_array[1] = address % 256;
00598     char data [1];
00599     _i2c.write(EEPROM_ADDRESS, address_array, 2, false);
00600     _i2c.read(EEPROM_ADDRESS, data, 1, false);
00601     return data [0];
00602 }
00603 
00604 char PiSwarm::read_next_eeprom_byte (){
00605     char data [1];
00606     _i2c.read(EEPROM_ADDRESS, data, 1, false);
00607     return data [0];
00608 }
00609     
00610 char PiSwarm::read_eeprom_block ( int address, char length ){
00611     char ret = 0;
00612     return ret;
00613 }
00614 
00615 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
00616 // RF Transceiver Functions
00617 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
00618 
00619 void PiSwarm::send_rf_message(char* message, char length){
00620     _rf.sendString(length, message);
00621 }
00622 
00623 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
00624 // Setup and Calibration Functions
00625 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
00626 
00627 void PiSwarm::setup () {
00628     _ser.baud(115200);
00629     set_oled_brightness (oled_brightness);
00630     set_cled_brightness (cled_brightness);
00631     gpio_led = setup_expansion_ic();
00632     adc_led = setup_adc();
00633 }
00634 
00635 void PiSwarm::setup_radio() {
00636     //Setup RF transceiver
00637     _rf.rf_init();
00638     _rf.setFrequency(RF_FREQUENCY);
00639     _rf.setDatarate(RF_DATARATE);
00640 }
00641 
00642 int PiSwarm::setup_expansion_ic () {
00643     //Expansion IC is NXP PCA9505
00644     //Address is 0x40 (defined by EXPANSION_IC_ADDRESS)
00645     //Block IO 0 : 7-0 Are OLED Outputs
00646     //Block IO 1 : 7 is NC.  6-2 are Cursor switches (interrupt inputs).  1 and 0 are OLED outputs.
00647     //Block IO 2 : 7 and 6 are LDO enable outputs.  5 - 1 are ID switch. 0 is NC.
00648     //Block IO 3 and 4 are for the expansion connector (not assigned here)
00649     char data [4];
00650     data [0] = 0x98;    //Write to I/O configure register 0, using auto increment (allows multi-blocks in one command)
00651     data [1] = 0x00;    //All 8 pins in block 0 are outputs (0)
00652     data [2] = 0xFC;    //Pins 1 & 0 are outputs, rest are inputs (or NC)
00653     data [3] = 0x3F;    //Pins 7 & 6 are outputs, rest are inputs (or NC)
00654     //If using the expansion port, assing its I/O here by increasing size of data[] and adding extra commands
00655     int test_val = _i2c.write(EXPANSION_IC_ADDRESS,data,4,false);
00656     
00657     // test_val should return 0 for correctly acknowledged response
00658    
00659     //Invert the polarity for switch inputs (they connect to ground when pressed\on)
00660     data [0] = 0x90;    //Write to polarity inversion register using auto increment
00661     data [1] = 0x00;
00662     data [2] = 0x7C;    //Invert pins 6-2 for cursor switches
00663     data [3] = 0x3E;    //Invert pins 5-1 for ID switch
00664     _i2c.write(EXPANSION_IC_ADDRESS,data,4,false);
00665     
00666     //Setup the interrupt masks.  By default, only the direction switches trigger an interrupt
00667     data [0] = 0xA0;
00668     data [1] = 0xFF;
00669     data [2] = 0x83;    // Disable mask on pins 6-2 for cursor switches
00670     data [3] = 0xFF;
00671     _i2c.write(EXPANSION_IC_ADDRESS,data,4,false);
00672 
00673     //Read the ID switches
00674     data [0] = 0x80;    //Read input registers
00675     char read_data [5];
00676     _i2c.write(EXPANSION_IC_ADDRESS, data, 1, false);
00677     _i2c.read(EXPANSION_IC_ADDRESS, read_data, 5, false);
00678     id = (read_data[2] & 0x3E) >> 1; //Mask pins 0, 6 and 7 then shift   
00679     
00680     //Setup interrupt handler
00681     expansion_interrupt.mode(PullUp);                                    // Pull Up by default (needed on v1 PCB - no pullup resistor)
00682     expansion_interrupt.fall(this,&PiSwarm::expansion_interrupt_handler);   // Add local handler on falling edge to read switch
00683     return test_val;
00684 }
00685 
00686 char PiSwarm::setup_adc ( void ){
00687     //ADC IC (for IR system) is a Analog Devices AD7997-BRUZ1
00688     return 0;
00689 }
00690   
00691 char PiSwarm::calibrate_gyro ( void ){
00692     //This routine attempts to calibrate the offset of the gyro
00693     int samples = 8;
00694     float gyro_values[samples];
00695     calibrate_led = 1;
00696     calibrate_ticker.attach(this,&PiSwarm::calibrate_ticker_routine,0.25);
00697     for(int i=0;i<samples;i++){
00698         gyro_values[i] = _gyro.read();
00699         wait(0.12);
00700     }
00701     char pass = 1;
00702     float mean = 0;
00703     float min = 3.5;
00704     float max = 3.2;
00705     for(int i=0;i<samples;i++){
00706         if((gyro_values[i] * 3.5 < 1.5) || (gyro_values[i] * 3.2 > 1.5)) pass = 0; // Calibration failed!
00707         float test_value = 1.50 / gyro_values[i];
00708         mean += test_value;
00709         if (test_value > max) max = test_value;
00710         if (test_value < min) min = test_value;
00711     }
00712     if(pass == 1){
00713         gyro_cal = mean / samples;
00714         gyro_error = max - min;
00715         calibrate_ticker.detach();
00716         calibrate_led = 0;
00717     }
00718     return pass;
00719 }    
00720 
00721 char PiSwarm::calibrate_accelerometer ( void ){
00722     //This routine attempts to calibrate the offset and multiplier of the accelerometer
00723     int samples = 8;
00724     float acc_x_values[samples];
00725     float acc_y_values[samples];
00726     float acc_z_values[samples];
00727     calibrate_led = 1;
00728     calibrate_ticker.attach(this,&PiSwarm::calibrate_ticker_routine,0.25);
00729     for(int i=0;i<samples;i++){
00730         acc_x_values[i] = _accel_x.read();
00731         acc_y_values[i] = _accel_y.read();
00732         acc_z_values[i] = _accel_z.read();
00733         wait(0.12);
00734     }
00735     char pass = 1;
00736     float x_mean = 0, y_mean=0, z_mean=0;
00737     float x_min = 3600, y_min=3600, z_min=3600;
00738     float x_max = 3100, y_max=3100, z_max=3100;
00739     for(int i=0;i<samples;i++){
00740         if((acc_x_values[i] * 3600 < 1250) || (acc_x_values[i] * 3100 > 1250)) pass = 0; // Calibration failed!
00741         if((acc_y_values[i] * 3600 < 1250) || (acc_y_values[i] * 3100 > 1250)) pass = 0; // Calibration failed!
00742         if((acc_z_values[i] * 3600 < 887) || (acc_z_values[i] * 3100 > 887)) pass = 0; // Calibration failed!
00743         
00744         float x_test_value = 1250 / acc_x_values[i];
00745         x_mean += x_test_value;
00746         if (x_test_value > x_max) x_max = x_test_value;
00747         if (x_test_value < x_min) x_min = x_test_value;
00748         float y_test_value = 1250 / acc_y_values[i];
00749         y_mean += y_test_value;
00750         if (y_test_value > y_max) y_max = y_test_value;
00751         if (y_test_value < y_min) y_min = y_test_value; 
00752         float z_test_value = 887 / acc_z_values[i];
00753         z_mean += z_test_value;
00754         if (z_test_value > z_max) z_max = z_test_value;
00755         if (z_test_value < z_min) z_min = z_test_value; 
00756     }
00757     if(pass == 1){
00758         acc_x_cal = x_mean / samples;
00759         acc_y_cal = y_mean / samples;
00760         acc_z_cal = z_mean / samples;
00761         calibrate_ticker.detach();
00762         calibrate_led = 0;
00763     }
00764     return pass;
00765 }   
00766 
00767 char PiSwarm::calibrate_magnetometer ( void ){
00768     char command_data[2];
00769     command_data[0] = 0x11; //Write to CTRL_REG2
00770     command_data[1] = 0x80; // Enable auto resets
00771     _i2c.write(MAGNETOMETER_ADDRESS, command_data, 2, false);
00772     command_data[0] = 0x10; //Write to CTRL_REG1
00773     command_data[1] = 0x29; // 001 01 0 0 1 [40HZ 32-OS  NOT-FAST  NORMAL  ACTIVE MODE] 
00774     return _i2c.write(MAGNETOMETER_ADDRESS, command_data, 2, false);
00775 }
00776 
00777 
00778 
00779 
00780 
00781 void PiSwarm::interrupt_timeout_event ( void ){
00782     //Read switches
00783     char data [1];
00784     data [0] = 0x80;    //Read input registers
00785     char read_data [5];
00786     _i2c.write(EXPANSION_IC_ADDRESS, data, 1, false);
00787     _i2c.read(EXPANSION_IC_ADDRESS, read_data, 5, false);
00788     switches = (read_data[1] & 0x7C) >> 2;
00789     led_timeout.attach(this,&PiSwarm::led_timeout_event, 0.1);               // Switch off the LED after 0.1s
00790 }
00791 
00792 void PiSwarm::led_timeout_event ( void ){
00793     interrupt_led = 0;
00794 }
00795 
00796 void PiSwarm::expansion_interrupt_handler ( void ){
00797     interrupt_led = 1;
00798     debounce_timeout.attach_us(this,&PiSwarm::interrupt_timeout_event, 200); // Read the switches only after 200uS have passed to debounce
00799     debounce_timeout.add_function(&switch_pressed);                       // Call the switch pressed routine (in main) when done
00800 }
00801 
00802 
00803     
00804  
00805 
00806 void PiSwarm::calibrate_ticker_routine ( void ){
00807     calibrate_led_state = 1 - calibrate_led_state;
00808     calibrate_led = calibrate_led_state;
00809 }
00810 
00811 void PiSwarm::test_oled(){
00812     enable_led_ldo = 1;
00813     enable_ldo_outputs();
00814     set_oled_colour(100,100,100);
00815     char data[2];
00816     data[0] = 0x09;     //Address for PCA9505 Output Port 1
00817     data[1] = 3;
00818     _i2c.write(EXPANSION_IC_ADDRESS,data,2,false);
00819 }
00820 
00821 void PiSwarm::reset () {
00822   //  _nrst = 0;
00823     wait (0.01);
00824    // _nrst = 1;
00825     wait (0.1);
00826 }
00827 void PiSwarm::motor (int motor, float speed) {
00828     char opcode = 0x0;
00829     if (speed > 0.0) {
00830         if (motor==1)
00831             opcode = M1_FORWARD;
00832         else
00833             opcode = M2_FORWARD;
00834     } else {
00835         if (motor==1)
00836             opcode = M1_BACKWARD;
00837         else
00838             opcode = M2_BACKWARD;
00839     }
00840     if(motor==1)right_speed = speed;
00841     else left_speed = speed;
00842     unsigned char arg = 0x7f * abs(speed);
00843 
00844     _ser.putc(opcode);
00845     _ser.putc(arg);
00846 }
00847 
00848 
00849 float PiSwarm::line_position() {
00850     int pos = 0;
00851     _ser.putc(SEND_LINE_POSITION);
00852     pos = _ser.getc();
00853     pos += _ser.getc() << 8;
00854     
00855     float fpos = ((float)pos - 2048.0)/2048.0;
00856     return(fpos);
00857 }
00858 
00859 char PiSwarm::sensor_auto_calibrate() {
00860     _ser.putc(AUTO_CALIBRATE);
00861     return(_ser.getc());
00862 }
00863 
00864 
00865 void PiSwarm::calibrate(void) {
00866     _ser.putc(PI_CALIBRATE);
00867 }
00868 
00869 void PiSwarm::reset_calibration() {
00870     _ser.putc(LINE_SENSORS_RESET_CALIBRATION);
00871 }
00872 
00873 void PiSwarm::PID_start(int max_speed, int a, int b, int c, int d) {
00874     _ser.putc(max_speed);
00875     _ser.putc(a);
00876     _ser.putc(b);
00877     _ser.putc(c);
00878     _ser.putc(d);
00879 }
00880 
00881 void PiSwarm::PID_stop() {
00882     _ser.putc(STOP_PID);
00883 }
00884 
00885 
00886 
00887 int PiSwarm::print (char* text, int length) {
00888     _ser.putc(DO_PRINT);  
00889     _ser.putc(length);       
00890     for (int i = 0 ; i < length ; i++) {
00891         _ser.putc(text[i]); 
00892     }
00893     return(0);
00894 }
00895 
00896 int PiSwarm::_putc (int c) {
00897     _ser.putc(DO_PRINT);  
00898     _ser.putc(0x1);       
00899     _ser.putc(c);         
00900     wait (0.001);
00901     return(c);
00902 }
00903 
00904 int PiSwarm::_getc (void) {
00905     char r = 0;
00906     return(r);
00907 }
00908 
00909 int PiSwarm::putc (int c) {
00910     return(_ser.putc(c));
00911 }
00912 
00913 int PiSwarm::getc (void) {
00914     return(_ser.getc());
00915 }
00916 
00917 void PiSwarm::start_system_timer(void) {
00918     _system_timer.reset();
00919     _system_timer.start();
00920 }
00921 
00922 
00923 #ifdef MBED_RPC
00924 const rpc_method *PiSwarm::get_rpc_methods() {
00925     static const rpc_method rpc_methods[] = {{ "forward", rpc_method_caller<PiSwarm, float, &PiSwarm::forward> },
00926         { "backward", rpc_method_caller<PiSwarm, float, &PiSwarm::backward> },
00927         { "left", rpc_method_caller<PiSwarm, float, &PiSwarm::left> },
00928         { "right", rpc_method_caller<PiSwarm, float, &PiSwarm::right> },
00929         { "stop", rpc_method_caller<PiSwarm, &PiSwarm::stop> },
00930         { "left_motor", rpc_method_caller<PiSwarm, float, &PiSwarm::left_motor> },
00931         { "right_motor", rpc_method_caller<PiSwarm, float, &PiSwarm::right_motor> },
00932         { "battery", rpc_method_caller<float, PiSwarm, &PiSwarm::battery> },
00933         { "line_position", rpc_method_caller<float, PiSwarm, &PiSwarm::line_position> },
00934         { "sensor_auto_calibrate", rpc_method_caller<char, PiSwarm, &PiSwarm::sensor_auto_calibrate> },
00935         RPC_METHOD_SUPER(Base)
00936     };
00937     return rpc_methods;
00938 }
00939 #endif
00940 
00941 void display_system_time(){
00942     if(PISWARM_DEBUG == 1){
00943         time_t system_time = time(NULL);
00944         printf("Current system time:%s Uptime:%.1f\n",ctime(&system_time),piswarm.get_uptime());   
00945     }
00946 }
00947 
00948 void init() {
00949     //Standard initialisation routine for Pi Swarm Robot
00950     //Displays a message on the screen and flashes the central LED
00951     //Calibrates the gyro and accelerometer
00952     //Make sure robot is flat and stationary when this code is run (calibration starts after about .5 second to allow robot to settle)
00953     piswarm.start_system_timer();
00954     pc.baud(PC_BAUD);
00955     if(PISWARM_DEBUG == 1)pc.printf("PiSwarm Firmware 0.4 Initialisation...\n");
00956     display_system_time();
00957     piswarm.play_tune("T298MSCG>C",10);
00958     piswarm.cls();
00959     piswarm.enable_cled(1);
00960     piswarm.set_cled_colour(200,0,0);
00961     piswarm.set_oled_colour(200,0,0);
00962     piswarm.set_oleds(1,0,1,0,1,0,1,0,1,0);
00963     piswarm.printf("  YORK  ");
00964     piswarm.locate(0,1);
00965     piswarm.printf("ROBOTLAB");
00966     wait(0.07);
00967     piswarm.set_oleds(0,1,0,1,0,1,0,1,0,1);
00968     wait(0.07);
00969     piswarm.set_cled_colour(0,200,0);
00970     piswarm.set_oled_colour(0,200,0);
00971     piswarm.set_oleds(1,0,1,0,1,0,1,0,1,0);
00972     wait(0.07);
00973     piswarm.set_oleds(0,1,0,1,0,1,0,1,0,1);
00974     wait(0.07);
00975     piswarm.set_cled_colour(0,0,200);
00976     piswarm.set_oled_colour(0,0,200);
00977     piswarm.set_oleds(1,0,1,0,1,0,1,0,1,0);
00978     wait(0.07);
00979     piswarm.set_oleds(0,1,0,1,0,1,0,1,0,1);
00980     wait(0.07);
00981     piswarm.set_oled_colour(20,150,200);
00982     piswarm.set_oleds(1,1,1,1,1,1,1,1,1,1);
00983     
00984     piswarm.cls();
00985     piswarm.set_cled_colour(20,20,20);
00986     piswarm.printf("Pi Swarm");
00987     piswarm.locate(0,1);
00988     piswarm.printf("ID : %d ",piswarm.get_id());
00989     if(PISWARM_DEBUG == 1)pc.printf("Test Magnetometer: ");
00990     if(piswarm.calibrate_magnetometer() != 0){
00991         if(PISWARM_DEBUG == 1)pc.printf("FAILED\n");
00992         wait(0.1);
00993         piswarm.cls();
00994         piswarm.locate(0,0);
00995         piswarm.printf("Mag Cal ");
00996         piswarm.locate(0,1);
00997         piswarm.printf("Failed  ");
00998         wait(0.1);
00999     }else if(PISWARM_DEBUG == 1)pc.printf("PASSED\n");
01000     if(PISWARM_DEBUG == 1)pc.printf("Test Gyroscope: ");
01001     if(piswarm.calibrate_gyro() == 0){
01002         if(PISWARM_DEBUG == 1)pc.printf("FAILED\n");
01003         wait(0.1);
01004         piswarm.cls();
01005         piswarm.locate(0,0);
01006         piswarm.printf("Gyro Cal");
01007         piswarm.locate(0,1);
01008         piswarm.printf("Failed  ");
01009         wait(0.1);
01010     }else if(PISWARM_DEBUG == 1)pc.printf("PASSED\n");
01011     if(PISWARM_DEBUG == 1)pc.printf("Test Accelerometer: ");
01012     if(piswarm.calibrate_accelerometer() == 0){
01013         if(PISWARM_DEBUG == 1)pc.printf("FAILED\n");
01014         wait(0.1);     
01015         piswarm.cls();
01016         piswarm.locate(0,0);
01017         piswarm.printf("Acc. Cal");
01018         piswarm.locate(0,1);
01019         piswarm.printf("Failed  ");
01020         wait(0.1);
01021     }else if(PISWARM_DEBUG == 1)pc.printf("PASSED\n");
01022     piswarm.turn_off_all_oleds();
01023     wait(0.1);
01024     piswarm.cls();
01025     piswarm.set_cled_colour(0,0,0);
01026     if(START_RADIO_ON_BOOT == 1){
01027         if(PISWARM_DEBUG == 1)pc.printf("Setting up 433MHz Transceiver\n");
01028         piswarm.setup_radio();
01029     }
01030 }
01031 
01032 /********************************************************************************
01033  * COPYRIGHT NOTICE                                                             *
01034  *                                                                              *
01035  * Parts of code based on the original m3pi library, Copyright (c) 2010 cstyles *
01036  *                                                                              *
01037  * Permission is hereby granted, free of charge, to any person obtaining a copy *
01038  * of this software and associated documentation files (the "Software"), to deal*
01039  * in the Software without restriction, including without limitation the rights *
01040  * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell    *
01041  * copies of the Software, and to permit persons to whom the Software is        * 
01042  * furnished to do so, subject to the following conditions:                     *
01043  *                                                                              *
01044  * The above copyright notice and this permission notice shall be included in   *
01045  * all copies or substantial portions of the Software.                          *
01046  *                                                                              *
01047  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR   *
01048  * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,     *
01049  * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE  *
01050  * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER       *
01051  * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,*
01052  * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN    *
01053  * THE SOFTWARE.                                                                *
01054  *                                                                              *
01055  ********************************************************************************/
01056 
01057