V0.5 library for the Pi Swarm robot
piswarm.cpp@1:b067a08ff54e, 2014-02-02 (annotated)
- Committer:
- jah128
- Date:
- Sun Feb 02 18:05:58 2014 +0000
- Revision:
- 1:b067a08ff54e
- Parent:
- 0:9ffe8ebd1c40
- Child:
- 3:4c0f2f3de33e
updated comms library;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
jah128 | 0:9ffe8ebd1c40 | 1 | /* University of York Robot Lab Pi Swarm Robot Library |
jah128 | 0:9ffe8ebd1c40 | 2 | * |
jah128 | 0:9ffe8ebd1c40 | 3 | * (C) Dr James Hilder, Dept. Electronics & Computer Science, University of York |
jah128 | 0:9ffe8ebd1c40 | 4 | * |
jah128 | 0:9ffe8ebd1c40 | 5 | * Version 0.4 January 2014 |
jah128 | 0:9ffe8ebd1c40 | 6 | * |
jah128 | 0:9ffe8ebd1c40 | 7 | * Designed for use with the Pi Swarm Board (enhanced MBED sensor board) v1.2 |
jah128 | 0:9ffe8ebd1c40 | 8 | * |
jah128 | 0:9ffe8ebd1c40 | 9 | * Based on the original m3pi library, Copyright (c) 2007-2010 cstyles (see copyright notice at end of file) * |
jah128 | 0:9ffe8ebd1c40 | 10 | */ |
jah128 | 0:9ffe8ebd1c40 | 11 | |
jah128 | 0:9ffe8ebd1c40 | 12 | #include "piswarm.h" |
jah128 | 0:9ffe8ebd1c40 | 13 | |
jah128 | 0:9ffe8ebd1c40 | 14 | // Variables |
jah128 | 0:9ffe8ebd1c40 | 15 | DigitalOut gpio_led (LED1); |
jah128 | 0:9ffe8ebd1c40 | 16 | DigitalOut calibrate_led (LED2); |
jah128 | 0:9ffe8ebd1c40 | 17 | DigitalOut adc_led (LED3); |
jah128 | 0:9ffe8ebd1c40 | 18 | DigitalOut interrupt_led (LED4); |
jah128 | 0:9ffe8ebd1c40 | 19 | InterruptIn expansion_interrupt (p29); |
jah128 | 0:9ffe8ebd1c40 | 20 | Timeout debounce_timeout; |
jah128 | 0:9ffe8ebd1c40 | 21 | Timeout led_timeout; |
jah128 | 0:9ffe8ebd1c40 | 22 | Ticker calibrate_ticker; |
jah128 | 0:9ffe8ebd1c40 | 23 | char id; |
jah128 | 0:9ffe8ebd1c40 | 24 | |
jah128 | 0:9ffe8ebd1c40 | 25 | char oled_array [10]; |
jah128 | 0:9ffe8ebd1c40 | 26 | char enable_ir_ldo = 0; |
jah128 | 0:9ffe8ebd1c40 | 27 | char enable_led_ldo = 0; |
jah128 | 0:9ffe8ebd1c40 | 28 | char calibrate_led_state = 1; |
jah128 | 0:9ffe8ebd1c40 | 29 | char switches = 0; |
jah128 | 0:9ffe8ebd1c40 | 30 | char cled_red = 0; |
jah128 | 0:9ffe8ebd1c40 | 31 | char cled_green = 0; |
jah128 | 0:9ffe8ebd1c40 | 32 | char cled_blue = 0; |
jah128 | 0:9ffe8ebd1c40 | 33 | char oled_red = 0; |
jah128 | 0:9ffe8ebd1c40 | 34 | char oled_green = 0; |
jah128 | 0:9ffe8ebd1c40 | 35 | char oled_blue = 0; |
jah128 | 0:9ffe8ebd1c40 | 36 | char cled_enable = 0; |
jah128 | 0:9ffe8ebd1c40 | 37 | char cled_brightness = CENTER_LED_BRIGHTNESS; // The default brightness value: must be between 0 (dimmest) and 100 (full brightness) |
jah128 | 0:9ffe8ebd1c40 | 38 | char oled_brightness = OUTER_LED_BRIGHTNESS; // The default brightness value: must be between 0 (dimmest) and 100 (full brightness) |
jah128 | 0:9ffe8ebd1c40 | 39 | float gyro_cal = 3.3; |
jah128 | 0:9ffe8ebd1c40 | 40 | float gyro_error = 0; |
jah128 | 0:9ffe8ebd1c40 | 41 | float acc_x_cal = 3350; |
jah128 | 0:9ffe8ebd1c40 | 42 | float acc_y_cal = 3350; |
jah128 | 0:9ffe8ebd1c40 | 43 | float acc_z_cal = 3350; |
jah128 | 0:9ffe8ebd1c40 | 44 | float left_speed = 0; |
jah128 | 0:9ffe8ebd1c40 | 45 | float right_speed = 0; |
jah128 | 0:9ffe8ebd1c40 | 46 | signed short mag_values [3]; |
jah128 | 0:9ffe8ebd1c40 | 47 | |
jah128 | 0:9ffe8ebd1c40 | 48 | |
jah128 | 0:9ffe8ebd1c40 | 49 | 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) { |
jah128 | 0:9ffe8ebd1c40 | 50 | setup(); |
jah128 | 0:9ffe8ebd1c40 | 51 | reset(); |
jah128 | 0:9ffe8ebd1c40 | 52 | } |
jah128 | 0:9ffe8ebd1c40 | 53 | |
jah128 | 1:b067a08ff54e | 54 | ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// |
jah128 | 1:b067a08ff54e | 55 | // Outer LED Functions |
jah128 | 1:b067a08ff54e | 56 | ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// |
jah128 | 1:b067a08ff54e | 57 | |
jah128 | 1:b067a08ff54e | 58 | // 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. |
jah128 | 0:9ffe8ebd1c40 | 59 | int PiSwarm::get_oled_colour(){ |
jah128 | 0:9ffe8ebd1c40 | 60 | return (oled_red << 16) + (oled_green << 8) + oled_blue; |
jah128 | 0:9ffe8ebd1c40 | 61 | } |
jah128 | 0:9ffe8ebd1c40 | 62 | |
jah128 | 1:b067a08ff54e | 63 | // Returns the current enable state an individual LED. oled = LED to return enable state. Returns 0 for disabled, 1 for enabled |
jah128 | 1:b067a08ff54e | 64 | char PiSwarm::get_oled_state(char oled){ |
jah128 | 1:b067a08ff54e | 65 | if(oled_array[oled] == 1) return 1; |
jah128 | 1:b067a08ff54e | 66 | return 0; |
jah128 | 0:9ffe8ebd1c40 | 67 | } |
jah128 | 0:9ffe8ebd1c40 | 68 | |
jah128 | 1:b067a08ff54e | 69 | // Set the colour of the outer LED. Values for red, green and blue range from 0 (off) to 255 (maximum). |
jah128 | 1:b067a08ff54e | 70 | void PiSwarm::set_oled_colour( char red, char green, char blue ){ |
jah128 | 1:b067a08ff54e | 71 | oled_red = red; |
jah128 | 1:b067a08ff54e | 72 | oled_green = green; |
jah128 | 1:b067a08ff54e | 73 | oled_blue = blue; |
jah128 | 1:b067a08ff54e | 74 | _oled_r.pulsewidth_us(red); |
jah128 | 1:b067a08ff54e | 75 | _oled_g.pulsewidth_us(green); |
jah128 | 1:b067a08ff54e | 76 | _oled_b.pulsewidth_us(blue); |
jah128 | 1:b067a08ff54e | 77 | } |
jah128 | 1:b067a08ff54e | 78 | |
jah128 | 1:b067a08ff54e | 79 | |
jah128 | 1:b067a08ff54e | 80 | // Returns the enable state of the center LED |
jah128 | 1:b067a08ff54e | 81 | char PiSwarm::get_cled_state( void ){ |
jah128 | 1:b067a08ff54e | 82 | return cled_enable; |
jah128 | 1:b067a08ff54e | 83 | } |
jah128 | 1:b067a08ff54e | 84 | |
jah128 | 1:b067a08ff54e | 85 | // 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. |
jah128 | 0:9ffe8ebd1c40 | 86 | void PiSwarm::set_oled(char oled, char value){ |
jah128 | 0:9ffe8ebd1c40 | 87 | oled_array[oled]=value; |
jah128 | 0:9ffe8ebd1c40 | 88 | } |
jah128 | 0:9ffe8ebd1c40 | 89 | |
jah128 | 1:b067a08ff54e | 90 | // Sets the state of all 10 LEDs. oled_x = 0 for disable, 1 for enable. Use to change all 10 LEDs at once |
jah128 | 0:9ffe8ebd1c40 | 91 | 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){ |
jah128 | 0:9ffe8ebd1c40 | 92 | oled_array[0]=oled_0; |
jah128 | 0:9ffe8ebd1c40 | 93 | oled_array[1]=oled_1; |
jah128 | 0:9ffe8ebd1c40 | 94 | oled_array[2]=oled_2; |
jah128 | 0:9ffe8ebd1c40 | 95 | oled_array[3]=oled_3; |
jah128 | 0:9ffe8ebd1c40 | 96 | oled_array[4]=oled_4; |
jah128 | 0:9ffe8ebd1c40 | 97 | oled_array[5]=oled_5; |
jah128 | 0:9ffe8ebd1c40 | 98 | oled_array[6]=oled_6; |
jah128 | 0:9ffe8ebd1c40 | 99 | oled_array[7]=oled_7; |
jah128 | 0:9ffe8ebd1c40 | 100 | oled_array[8]=oled_8; |
jah128 | 0:9ffe8ebd1c40 | 101 | oled_array[9]=oled_9; |
jah128 | 0:9ffe8ebd1c40 | 102 | activate_oleds(); |
jah128 | 0:9ffe8ebd1c40 | 103 | } |
jah128 | 0:9ffe8ebd1c40 | 104 | |
jah128 | 1:b067a08ff54e | 105 | // Sets all outer LEDs to disabled and turns off LED power supply. |
jah128 | 0:9ffe8ebd1c40 | 106 | void PiSwarm::turn_off_all_oleds(){ |
jah128 | 0:9ffe8ebd1c40 | 107 | for(int i=0;i<10;i++){ |
jah128 | 0:9ffe8ebd1c40 | 108 | oled_array[i]=0; |
jah128 | 0:9ffe8ebd1c40 | 109 | } |
jah128 | 0:9ffe8ebd1c40 | 110 | activate_oleds(); |
jah128 | 0:9ffe8ebd1c40 | 111 | enable_led_ldo = 0; |
jah128 | 0:9ffe8ebd1c40 | 112 | enable_ldo_outputs(); |
jah128 | 0:9ffe8ebd1c40 | 113 | char data[3]; |
jah128 | 0:9ffe8ebd1c40 | 114 | data[0]=0x88; //Write to bank 1 then bank 2 |
jah128 | 0:9ffe8ebd1c40 | 115 | data[1]=0x00; //Reset everything in bank 1 |
jah128 | 0:9ffe8ebd1c40 | 116 | data[2]=0x00; //Reset everything in bank 2 |
jah128 | 0:9ffe8ebd1c40 | 117 | _i2c.write(EXPANSION_IC_ADDRESS,data,3,false); |
jah128 | 0:9ffe8ebd1c40 | 118 | } |
jah128 | 0:9ffe8ebd1c40 | 119 | |
jah128 | 1:b067a08ff54e | 120 | // Set the outer LED brightness (total period of PWM output increases as brightness decreases). Ranges from 0 (minimum) to 255 (maximum) |
jah128 | 1:b067a08ff54e | 121 | void PiSwarm::set_oled_brightness ( char brightness ) { |
jah128 | 1:b067a08ff54e | 122 | if ( brightness > 100 ) brightness = 100; |
jah128 | 1:b067a08ff54e | 123 | oled_brightness = brightness; |
jah128 | 1:b067a08ff54e | 124 | int oled_period = (104 - brightness); |
jah128 | 1:b067a08ff54e | 125 | oled_period *= oled_period; |
jah128 | 1:b067a08ff54e | 126 | oled_period /= 10; |
jah128 | 1:b067a08ff54e | 127 | oled_period += 255; |
jah128 | 1:b067a08ff54e | 128 | if(CALIBRATE_COLOURS!=0)_oled_r.period_us(oled_period * 2); |
jah128 | 1:b067a08ff54e | 129 | else _oled_r.period_us(oled_period); |
jah128 | 1:b067a08ff54e | 130 | if(CALIBRATE_COLOURS!=0)_oled_g.period_us(oled_period * 2); |
jah128 | 1:b067a08ff54e | 131 | else _oled_g.period_us(oled_period); |
jah128 | 1:b067a08ff54e | 132 | _oled_b.period_us(oled_period); |
jah128 | 1:b067a08ff54e | 133 | } |
jah128 | 0:9ffe8ebd1c40 | 134 | |
jah128 | 0:9ffe8ebd1c40 | 135 | // Sends the messages to enable/disable outer LEDs |
jah128 | 0:9ffe8ebd1c40 | 136 | void PiSwarm::activate_oleds(){ |
jah128 | 0:9ffe8ebd1c40 | 137 | if(enable_led_ldo == 0){ |
jah128 | 0:9ffe8ebd1c40 | 138 | enable_led_ldo = 1; |
jah128 | 0:9ffe8ebd1c40 | 139 | enable_ldo_outputs(); |
jah128 | 0:9ffe8ebd1c40 | 140 | } |
jah128 | 0:9ffe8ebd1c40 | 141 | char data[3]; |
jah128 | 0:9ffe8ebd1c40 | 142 | data[0]=0x88; //Write to bank 1 then bank 2 |
jah128 | 0:9ffe8ebd1c40 | 143 | data[1]=0x00; //Reset everything in bank 1 |
jah128 | 0:9ffe8ebd1c40 | 144 | data[2]=0x00; //Reset everything in bank 2 |
jah128 | 0:9ffe8ebd1c40 | 145 | if(oled_array[0]>0) data[1]+=1; |
jah128 | 0:9ffe8ebd1c40 | 146 | if(oled_array[1]>0) data[1]+=2; |
jah128 | 0:9ffe8ebd1c40 | 147 | if(oled_array[2]>0) data[1]+=4; |
jah128 | 0:9ffe8ebd1c40 | 148 | if(oled_array[3]>0) data[1]+=8; |
jah128 | 0:9ffe8ebd1c40 | 149 | if(oled_array[4]>0) data[1]+=16; |
jah128 | 0:9ffe8ebd1c40 | 150 | if(oled_array[5]>0) data[1]+=32; |
jah128 | 0:9ffe8ebd1c40 | 151 | if(oled_array[6]>0) data[1]+=64; |
jah128 | 0:9ffe8ebd1c40 | 152 | if(oled_array[7]>0) data[1]+=128; |
jah128 | 0:9ffe8ebd1c40 | 153 | if(oled_array[8]>0) data[2]+=1; |
jah128 | 0:9ffe8ebd1c40 | 154 | if(oled_array[9]>0) data[2]+=2; |
jah128 | 0:9ffe8ebd1c40 | 155 | _i2c.write(EXPANSION_IC_ADDRESS,data,3,false); |
jah128 | 0:9ffe8ebd1c40 | 156 | } |
jah128 | 0:9ffe8ebd1c40 | 157 | |
jah128 | 1:b067a08ff54e | 158 | |
jah128 | 1:b067a08ff54e | 159 | ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// |
jah128 | 1:b067a08ff54e | 160 | // Center LED Functions |
jah128 | 1:b067a08ff54e | 161 | ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// |
jah128 | 1:b067a08ff54e | 162 | |
jah128 | 1:b067a08ff54e | 163 | // 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. |
jah128 | 1:b067a08ff54e | 164 | int PiSwarm::get_cled_colour(){ |
jah128 | 1:b067a08ff54e | 165 | return (cled_red << 16) + (cled_green << 8) + cled_blue; |
jah128 | 0:9ffe8ebd1c40 | 166 | } |
jah128 | 0:9ffe8ebd1c40 | 167 | |
jah128 | 1:b067a08ff54e | 168 | // Set the colour of the center LED. Values for red, green and blue range from 0 (off) to 255 (maximum). |
jah128 | 0:9ffe8ebd1c40 | 169 | void PiSwarm::set_cled_colour( char red, char green, char blue ){ |
jah128 | 0:9ffe8ebd1c40 | 170 | cled_red = red; |
jah128 | 0:9ffe8ebd1c40 | 171 | cled_green = green; |
jah128 | 0:9ffe8ebd1c40 | 172 | cled_blue = blue; |
jah128 | 0:9ffe8ebd1c40 | 173 | if(cled_enable != 0) enable_cled(1); |
jah128 | 0:9ffe8ebd1c40 | 174 | } |
jah128 | 0:9ffe8ebd1c40 | 175 | |
jah128 | 1:b067a08ff54e | 176 | // Turn on or off the center LED. enable=1 to turn on, 0 to turn off |
jah128 | 0:9ffe8ebd1c40 | 177 | void PiSwarm::enable_cled( char enable ){ |
jah128 | 0:9ffe8ebd1c40 | 178 | cled_enable = enable; |
jah128 | 0:9ffe8ebd1c40 | 179 | if(enable != 0) { |
jah128 | 0:9ffe8ebd1c40 | 180 | _cled_r.pulsewidth_us(cled_red); |
jah128 | 0:9ffe8ebd1c40 | 181 | _cled_g.pulsewidth_us(cled_green); |
jah128 | 0:9ffe8ebd1c40 | 182 | _cled_b.pulsewidth_us(cled_blue); |
jah128 | 0:9ffe8ebd1c40 | 183 | }else{ |
jah128 | 0:9ffe8ebd1c40 | 184 | _cled_r.pulsewidth_us(0); |
jah128 | 0:9ffe8ebd1c40 | 185 | _cled_g.pulsewidth_us(0); |
jah128 | 0:9ffe8ebd1c40 | 186 | _cled_b.pulsewidth_us(0); |
jah128 | 0:9ffe8ebd1c40 | 187 | } |
jah128 | 0:9ffe8ebd1c40 | 188 | } |
jah128 | 0:9ffe8ebd1c40 | 189 | |
jah128 | 1:b067a08ff54e | 190 | // Set the center LED brightness (total period of PWM output increases as brightness decreases). Ranges from 0 (minimum) to 255 (maximum) |
jah128 | 1:b067a08ff54e | 191 | void PiSwarm::set_cled_brightness ( char brightness ) { |
jah128 | 1:b067a08ff54e | 192 | if( brightness > 100 ) brightness = 100; |
jah128 | 1:b067a08ff54e | 193 | // Brightness is set by adjusting period of PWM |
jah128 | 1:b067a08ff54e | 194 | // Max brightness is when total period = 256 uS |
jah128 | 1:b067a08ff54e | 195 | // When brightness is 90 total period = 274 uS |
jah128 | 1:b067a08ff54e | 196 | // When brightness is 50 total period = 546 uS |
jah128 | 1:b067a08ff54e | 197 | // When brightness is 10 total period = 1138 uS |
jah128 | 1:b067a08ff54e | 198 | // When brightness is 0 total period = 1336 uS |
jah128 | 1:b067a08ff54e | 199 | // When calibrate_colours = 1, red = 2x normal, green = 2x normal |
jah128 | 1:b067a08ff54e | 200 | cled_brightness = brightness; |
jah128 | 1:b067a08ff54e | 201 | int cled_period = (104 - brightness); |
jah128 | 1:b067a08ff54e | 202 | cled_period *= cled_period; |
jah128 | 1:b067a08ff54e | 203 | cled_period /= 10; |
jah128 | 1:b067a08ff54e | 204 | cled_period += 255; |
jah128 | 1:b067a08ff54e | 205 | if(CALIBRATE_COLOURS!=0)_cled_r.period_us(cled_period * 2); |
jah128 | 1:b067a08ff54e | 206 | else _cled_r.period_us(cled_period); |
jah128 | 1:b067a08ff54e | 207 | if(CALIBRATE_COLOURS!=0)_cled_g.period_us(cled_period * 2); |
jah128 | 1:b067a08ff54e | 208 | else _cled_g.period_us(cled_period); |
jah128 | 1:b067a08ff54e | 209 | _cled_b.period_us(cled_period); |
jah128 | 0:9ffe8ebd1c40 | 210 | } |
jah128 | 0:9ffe8ebd1c40 | 211 | |
jah128 | 0:9ffe8ebd1c40 | 212 | |
jah128 | 1:b067a08ff54e | 213 | ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// |
jah128 | 1:b067a08ff54e | 214 | // IR Sensor Functions |
jah128 | 1:b067a08ff54e | 215 | ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// |
jah128 | 0:9ffe8ebd1c40 | 216 | |
jah128 | 1:b067a08ff54e | 217 | // 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. |
jah128 | 0:9ffe8ebd1c40 | 218 | float PiSwarm::read_reflected_ir_distance ( char index ){ |
jah128 | 0:9ffe8ebd1c40 | 219 | //This function attempts to read a real-world distance approximation using the IR proximity sensors |
jah128 | 0:9ffe8ebd1c40 | 220 | //1. Check that the IR LDO regulator is on, enable if it isn't |
jah128 | 0:9ffe8ebd1c40 | 221 | if(enable_ir_ldo == 0){ |
jah128 | 0:9ffe8ebd1c40 | 222 | enable_ir_ldo = 1; |
jah128 | 0:9ffe8ebd1c40 | 223 | enable_ldo_outputs(); |
jah128 | 0:9ffe8ebd1c40 | 224 | } |
jah128 | 0:9ffe8ebd1c40 | 225 | //2. Read the ADC value without IR emitter on |
jah128 | 0:9ffe8ebd1c40 | 226 | unsigned short background_value = read_adc_value ( index ); |
jah128 | 0:9ffe8ebd1c40 | 227 | //3. Enable the relevant IR emitter by turning on its pulse output |
jah128 | 0:9ffe8ebd1c40 | 228 | switch(index){ |
jah128 | 0:9ffe8ebd1c40 | 229 | case 0: case 1: case 6: case 7: |
jah128 | 0:9ffe8ebd1c40 | 230 | _irpulse_1 = 1; |
jah128 | 0:9ffe8ebd1c40 | 231 | break; |
jah128 | 0:9ffe8ebd1c40 | 232 | case 2: case 3: case 4: case 5: |
jah128 | 0:9ffe8ebd1c40 | 233 | _irpulse_2 = 1; |
jah128 | 0:9ffe8ebd1c40 | 234 | break; |
jah128 | 0:9ffe8ebd1c40 | 235 | } |
jah128 | 0:9ffe8ebd1c40 | 236 | wait_us(500); |
jah128 | 0:9ffe8ebd1c40 | 237 | //4. Read the ADC value now IR emitter is on |
jah128 | 0:9ffe8ebd1c40 | 238 | unsigned short strong_value = read_adc_value ( index ); |
jah128 | 0:9ffe8ebd1c40 | 239 | //5. Turn off IR emitter |
jah128 | 0:9ffe8ebd1c40 | 240 | switch(index){ |
jah128 | 0:9ffe8ebd1c40 | 241 | case 0: case 1: case 6: case 7: |
jah128 | 0:9ffe8ebd1c40 | 242 | _irpulse_1 = 0; |
jah128 | 0:9ffe8ebd1c40 | 243 | break; |
jah128 | 0:9ffe8ebd1c40 | 244 | case 2: case 3: case 4: case 5: |
jah128 | 0:9ffe8ebd1c40 | 245 | _irpulse_2 = 0; |
jah128 | 0:9ffe8ebd1c40 | 246 | break; |
jah128 | 0:9ffe8ebd1c40 | 247 | } |
jah128 | 0:9ffe8ebd1c40 | 248 | //6. Estimate distance based on 2 values |
jah128 | 0:9ffe8ebd1c40 | 249 | float app_distance = 4000 + background_value - strong_value; |
jah128 | 0:9ffe8ebd1c40 | 250 | if(app_distance < 0) app_distance = 0; |
jah128 | 0:9ffe8ebd1c40 | 251 | // Very approximate: root value, divide by 2, approx distance in mm |
jah128 | 0:9ffe8ebd1c40 | 252 | app_distance = sqrt (app_distance) / 2.0; |
jah128 | 0:9ffe8ebd1c40 | 253 | // Then add adjustment value if value >27 |
jah128 | 0:9ffe8ebd1c40 | 254 | if(app_distance > 27) { |
jah128 | 0:9ffe8ebd1c40 | 255 | float shift = pow(app_distance - 27,3); |
jah128 | 0:9ffe8ebd1c40 | 256 | app_distance += shift; |
jah128 | 0:9ffe8ebd1c40 | 257 | } |
jah128 | 0:9ffe8ebd1c40 | 258 | if(app_distance > 90) app_distance = 100.0; |
jah128 | 0:9ffe8ebd1c40 | 259 | return app_distance; |
jah128 | 0:9ffe8ebd1c40 | 260 | } |
jah128 | 0:9ffe8ebd1c40 | 261 | |
jah128 | 1:b067a08ff54e | 262 | // Returns the raw sensor value for the IR sensor defined by index (range 0-7). |
jah128 | 0:9ffe8ebd1c40 | 263 | unsigned short PiSwarm::read_adc_value ( char index ){ |
jah128 | 0:9ffe8ebd1c40 | 264 | short value = 0; |
jah128 | 0:9ffe8ebd1c40 | 265 | // Read a single value from the ADC |
jah128 | 0:9ffe8ebd1c40 | 266 | if(index<8){ |
jah128 | 0:9ffe8ebd1c40 | 267 | char apb[1]; |
jah128 | 0:9ffe8ebd1c40 | 268 | char data[2]; |
jah128 | 0:9ffe8ebd1c40 | 269 | switch(index){ |
jah128 | 0:9ffe8ebd1c40 | 270 | case 0: apb[0]=0x80; break; |
jah128 | 0:9ffe8ebd1c40 | 271 | case 1: apb[0]=0x90; break; |
jah128 | 0:9ffe8ebd1c40 | 272 | case 2: apb[0]=0xA0; break; |
jah128 | 0:9ffe8ebd1c40 | 273 | case 3: apb[0]=0xB0; break; |
jah128 | 0:9ffe8ebd1c40 | 274 | case 4: apb[0]=0xC0; break; |
jah128 | 0:9ffe8ebd1c40 | 275 | case 5: apb[0]=0xD0; break; |
jah128 | 0:9ffe8ebd1c40 | 276 | case 6: apb[0]=0xE0; break; |
jah128 | 0:9ffe8ebd1c40 | 277 | case 7: apb[0]=0xF0; break; |
jah128 | 0:9ffe8ebd1c40 | 278 | } |
jah128 | 0:9ffe8ebd1c40 | 279 | _i2c.write(ADC_ADDRESS,apb,1,false); |
jah128 | 0:9ffe8ebd1c40 | 280 | _i2c.read(ADC_ADDRESS,data,2,false); |
jah128 | 0:9ffe8ebd1c40 | 281 | value=((data[0] % 16)<<8)+data[1]; |
jah128 | 0:9ffe8ebd1c40 | 282 | if(value > 4096) value=4096; |
jah128 | 0:9ffe8ebd1c40 | 283 | value=4096-value; |
jah128 | 0:9ffe8ebd1c40 | 284 | } |
jah128 | 0:9ffe8ebd1c40 | 285 | return value; |
jah128 | 0:9ffe8ebd1c40 | 286 | } |
jah128 | 0:9ffe8ebd1c40 | 287 | |
jah128 | 1:b067a08ff54e | 288 | // Function enables or disables the LDO voltage regulators which supply power to the outer LEDs and the IR photo emitters. |
jah128 | 1:b067a08ff54e | 289 | void PiSwarm::enable_ldo_outputs () { |
jah128 | 1:b067a08ff54e | 290 | char data[2]; |
jah128 | 1:b067a08ff54e | 291 | data[0] = 0x0A; //Address for PCA9505 Output Port 2 |
jah128 | 1:b067a08ff54e | 292 | data[1] = 0; |
jah128 | 1:b067a08ff54e | 293 | if(enable_led_ldo) data[1] +=128; |
jah128 | 1:b067a08ff54e | 294 | if(enable_ir_ldo) data[1] +=64; |
jah128 | 1:b067a08ff54e | 295 | _i2c.write(EXPANSION_IC_ADDRESS,data,2,false); |
jah128 | 0:9ffe8ebd1c40 | 296 | } |
jah128 | 0:9ffe8ebd1c40 | 297 | |
jah128 | 0:9ffe8ebd1c40 | 298 | |
jah128 | 1:b067a08ff54e | 299 | ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// |
jah128 | 1:b067a08ff54e | 300 | // MEMS Sensor Functions |
jah128 | 1:b067a08ff54e | 301 | ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// |
jah128 | 0:9ffe8ebd1c40 | 302 | |
jah128 | 1:b067a08ff54e | 303 | // Returns the yaw (clockwise) rotation in degrees-per-second reported by the LY3200 gyroscope. |
jah128 | 0:9ffe8ebd1c40 | 304 | float PiSwarm::read_gyro ( void ){ |
jah128 | 1:b067a08ff54e | 305 | // Gyro is a ST Micro LY3200ALH Yaw-rate Gyro: 0.67mV / d.p.s, 1.5V offset |
jah128 | 0:9ffe8ebd1c40 | 306 | float r_gyro = _gyro.read(); |
jah128 | 0:9ffe8ebd1c40 | 307 | r_gyro *= gyro_cal; // NB 3.4 seemed to work better that 3.3 but better to run calibration routine |
jah128 | 0:9ffe8ebd1c40 | 308 | r_gyro = 1.5 - r_gyro; // 1.5V off-set, invert sign |
jah128 | 0:9ffe8ebd1c40 | 309 | r_gyro *= 1492.5; // Convert to DPS (+ve = clockwise, -ve = c.clockwise) |
jah128 | 0:9ffe8ebd1c40 | 310 | return r_gyro; |
jah128 | 0:9ffe8ebd1c40 | 311 | } |
jah128 | 1:b067a08ff54e | 312 | |
jah128 | 1:b067a08ff54e | 313 | // Returns the acceleration in the X plane reported by the LIS332 accelerometer. Returned value is in mg. |
jah128 | 0:9ffe8ebd1c40 | 314 | float PiSwarm::read_accelerometer_x ( void ){ |
jah128 | 1:b067a08ff54e | 315 | // LIS 332 AX Accelerometer - 0.363 V/g - 1.25V |
jah128 | 0:9ffe8ebd1c40 | 316 | float r_acc_x = (( _accel_x.read() * acc_x_cal ) - 1250.0 ) * 2.754; // Return value in mG |
jah128 | 0:9ffe8ebd1c40 | 317 | return r_acc_x; |
jah128 | 0:9ffe8ebd1c40 | 318 | } |
jah128 | 1:b067a08ff54e | 319 | |
jah128 | 1:b067a08ff54e | 320 | // Returns the acceleration in the Y plane reported by the LIS332 accelerometer. Returned value is in mg. |
jah128 | 0:9ffe8ebd1c40 | 321 | float PiSwarm::read_accelerometer_y ( void ){ |
jah128 | 0:9ffe8ebd1c40 | 322 | float r_acc_y = (( _accel_y.read() * acc_y_cal ) - 1250.0 ) * 2.754; // Return value in mG |
jah128 | 0:9ffe8ebd1c40 | 323 | return r_acc_y; |
jah128 | 0:9ffe8ebd1c40 | 324 | } |
jah128 | 0:9ffe8ebd1c40 | 325 | |
jah128 | 1:b067a08ff54e | 326 | // Returns the acceleration in the Z plane reported by the LIS332 accelerometer. Returned value is in mg. |
jah128 | 0:9ffe8ebd1c40 | 327 | float PiSwarm::read_accelerometer_z ( void ){ |
jah128 | 0:9ffe8ebd1c40 | 328 | float r_acc_z = (( _accel_z.read() * acc_z_cal ) - 1250.0 ) * 2.754; // Return value in mG |
jah128 | 0:9ffe8ebd1c40 | 329 | return r_acc_z; |
jah128 | 0:9ffe8ebd1c40 | 330 | } |
jah128 | 0:9ffe8ebd1c40 | 331 | |
jah128 | 1:b067a08ff54e | 332 | // Sends message to the magnetometer to read new values. Should be called before get_magnetometer_* when updated values are required |
jah128 | 0:9ffe8ebd1c40 | 333 | char PiSwarm::read_magnetometer ( void ){ |
jah128 | 0:9ffe8ebd1c40 | 334 | char read_data[7]; |
jah128 | 0:9ffe8ebd1c40 | 335 | char success; |
jah128 | 0:9ffe8ebd1c40 | 336 | char command_data[1]; |
jah128 | 0:9ffe8ebd1c40 | 337 | command_data[0] = 0x00; //Auto-read from register 0x00 (status) |
jah128 | 0:9ffe8ebd1c40 | 338 | success = _i2c.write(MAGNETOMETER_ADDRESS, command_data, 1, false); |
jah128 | 0:9ffe8ebd1c40 | 339 | _i2c.read(MAGNETOMETER_ADDRESS, read_data, 7, false); |
jah128 | 0:9ffe8ebd1c40 | 340 | mag_values[0] = (read_data[1] << 8) + read_data[2]; |
jah128 | 0:9ffe8ebd1c40 | 341 | mag_values[1] = (read_data[3] << 8) + read_data[4]; |
jah128 | 0:9ffe8ebd1c40 | 342 | mag_values[2] = (read_data[5] << 8) + read_data[6]; |
jah128 | 0:9ffe8ebd1c40 | 343 | return success; |
jah128 | 0:9ffe8ebd1c40 | 344 | } |
jah128 | 0:9ffe8ebd1c40 | 345 | |
jah128 | 1:b067a08ff54e | 346 | // Returns the raw value for the X-plane magnetic field stored on the last call of read_magnetometer |
jah128 | 0:9ffe8ebd1c40 | 347 | signed short PiSwarm::get_magnetometer_x ( void ){ |
jah128 | 0:9ffe8ebd1c40 | 348 | return mag_values[0]; |
jah128 | 0:9ffe8ebd1c40 | 349 | } |
jah128 | 0:9ffe8ebd1c40 | 350 | |
jah128 | 1:b067a08ff54e | 351 | // Returns the raw value for the Y-plane magnetic field stored on the last call of read_magnetometer |
jah128 | 0:9ffe8ebd1c40 | 352 | signed short PiSwarm::get_magnetometer_y ( void ){ |
jah128 | 0:9ffe8ebd1c40 | 353 | return mag_values[1]; |
jah128 | 0:9ffe8ebd1c40 | 354 | } |
jah128 | 0:9ffe8ebd1c40 | 355 | |
jah128 | 1:b067a08ff54e | 356 | // Returns the raw value for the Z-plane magnetic field stored on the last call of read_magnetometer |
jah128 | 0:9ffe8ebd1c40 | 357 | signed short PiSwarm::get_magnetometer_z ( void ){ |
jah128 | 0:9ffe8ebd1c40 | 358 | return mag_values[2]; |
jah128 | 0:9ffe8ebd1c40 | 359 | } |
jah128 | 0:9ffe8ebd1c40 | 360 | |
jah128 | 1:b067a08ff54e | 361 | |
jah128 | 1:b067a08ff54e | 362 | ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// |
jah128 | 1:b067a08ff54e | 363 | // Other Sensor Functions |
jah128 | 1:b067a08ff54e | 364 | ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// |
jah128 | 1:b067a08ff54e | 365 | |
jah128 | 1:b067a08ff54e | 366 | // Returns the temperature reported by the MCP9701 sensor in degrees centigrade. |
jah128 | 1:b067a08ff54e | 367 | float PiSwarm::read_temperature ( void ){ |
jah128 | 1:b067a08ff54e | 368 | // Temperature Sensor is a Microchip MCP9701T-E/LT: 19.5mV/C 0C = 400mV |
jah128 | 1:b067a08ff54e | 369 | float r_temp = _temperature.read(); |
jah128 | 1:b067a08ff54e | 370 | r_temp -= 0.1212f; // 0.4 / 3.3 |
jah128 | 1:b067a08ff54e | 371 | r_temp *= 169.231f; // 3.3 / .0195 |
jah128 | 1:b067a08ff54e | 372 | return r_temp; |
jah128 | 1:b067a08ff54e | 373 | } |
jah128 | 1:b067a08ff54e | 374 | |
jah128 | 1:b067a08ff54e | 375 | // Returns the adjusted value (0-100) for the ambient light sensor |
jah128 | 1:b067a08ff54e | 376 | float PiSwarm::read_light_sensor ( void ){ |
jah128 | 1:b067a08ff54e | 377 | // Light sensor is a Avago APDS-9005 Ambient Light Sensor |
jah128 | 1:b067a08ff54e | 378 | //float r_light = (_light.read() * 141); //Gives a range of (approximately) 0=dark 100=max. light |
jah128 | 1:b067a08ff54e | 379 | float r_light = (sqrt(_light.read() * 1.41) * 100); //Non-linear and more CPU intensive but generally gives better low-light measures |
jah128 | 1:b067a08ff54e | 380 | if(r_light > 100) r_light = 100; |
jah128 | 1:b067a08ff54e | 381 | return r_light; |
jah128 | 1:b067a08ff54e | 382 | } |
jah128 | 1:b067a08ff54e | 383 | |
jah128 | 1:b067a08ff54e | 384 | |
jah128 | 1:b067a08ff54e | 385 | |
jah128 | 1:b067a08ff54e | 386 | void PiSwarm::read_raw_sensors ( int * raw_ls_array ) { |
jah128 | 1:b067a08ff54e | 387 | _ser.putc(SEND_RAW_SENSOR_VALUES); |
jah128 | 1:b067a08ff54e | 388 | for (int i = 0; i < 5 ; i ++) { |
jah128 | 1:b067a08ff54e | 389 | int val = _ser.getc(); |
jah128 | 1:b067a08ff54e | 390 | val += _ser.getc() << 8; |
jah128 | 1:b067a08ff54e | 391 | raw_ls_array [i] = val; |
jah128 | 1:b067a08ff54e | 392 | } |
jah128 | 1:b067a08ff54e | 393 | } |
jah128 | 1:b067a08ff54e | 394 | |
jah128 | 1:b067a08ff54e | 395 | // Returns the uptime of the system (since initialisation) in seconds |
jah128 | 1:b067a08ff54e | 396 | float PiSwarm::get_uptime (void) { |
jah128 | 1:b067a08ff54e | 397 | return _system_timer.read(); |
jah128 | 1:b067a08ff54e | 398 | } |
jah128 | 1:b067a08ff54e | 399 | |
jah128 | 1:b067a08ff54e | 400 | // Returns the battery level in millivolts |
jah128 | 1:b067a08ff54e | 401 | float PiSwarm::battery() { |
jah128 | 1:b067a08ff54e | 402 | _ser.putc(SEND_BATTERY_MILLIVOLTS); |
jah128 | 1:b067a08ff54e | 403 | char lowbyte = _ser.getc(); |
jah128 | 1:b067a08ff54e | 404 | char hibyte = _ser.getc(); |
jah128 | 1:b067a08ff54e | 405 | float v = ((lowbyte + (hibyte << 8))/1000.0); |
jah128 | 1:b067a08ff54e | 406 | return(v); |
jah128 | 1:b067a08ff54e | 407 | } |
jah128 | 1:b067a08ff54e | 408 | |
jah128 | 1:b067a08ff54e | 409 | // Returns the raw value for the variable resistor on the base of the platform. Ranges from 0 to 1024. |
jah128 | 1:b067a08ff54e | 410 | float PiSwarm::pot_voltage(void) { |
jah128 | 1:b067a08ff54e | 411 | int volt = 0; |
jah128 | 1:b067a08ff54e | 412 | _ser.putc(SEND_TRIMPOT); |
jah128 | 1:b067a08ff54e | 413 | volt = _ser.getc(); |
jah128 | 1:b067a08ff54e | 414 | volt += _ser.getc() << 8; |
jah128 | 1:b067a08ff54e | 415 | return(volt); |
jah128 | 1:b067a08ff54e | 416 | } |
jah128 | 1:b067a08ff54e | 417 | |
jah128 | 1:b067a08ff54e | 418 | // Returns the ID of platform (set by the DIL switches above the display). Range 0-31 [0 reserved]. |
jah128 | 1:b067a08ff54e | 419 | char PiSwarm::get_id () { |
jah128 | 1:b067a08ff54e | 420 | return id; |
jah128 | 1:b067a08ff54e | 421 | } |
jah128 | 1:b067a08ff54e | 422 | |
jah128 | 1:b067a08ff54e | 423 | // Return the current stored state for direction switch(es) pressed {1 = Center 2 = Right 4 = Left 8 = Down 16 = Up} |
jah128 | 1:b067a08ff54e | 424 | char PiSwarm::get_switches () { |
jah128 | 1:b067a08ff54e | 425 | return switches; |
jah128 | 1:b067a08ff54e | 426 | } |
jah128 | 1:b067a08ff54e | 427 | |
jah128 | 1:b067a08ff54e | 428 | |
jah128 | 1:b067a08ff54e | 429 | |
jah128 | 1:b067a08ff54e | 430 | |
jah128 | 1:b067a08ff54e | 431 | ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// |
jah128 | 1:b067a08ff54e | 432 | // Motor Functions |
jah128 | 1:b067a08ff54e | 433 | ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// |
jah128 | 1:b067a08ff54e | 434 | |
jah128 | 1:b067a08ff54e | 435 | // Returns the target speed of the left motor (-1.0 to 1.0) |
jah128 | 1:b067a08ff54e | 436 | float PiSwarm::get_left_motor (){ |
jah128 | 1:b067a08ff54e | 437 | return left_speed; |
jah128 | 1:b067a08ff54e | 438 | } |
jah128 | 1:b067a08ff54e | 439 | |
jah128 | 1:b067a08ff54e | 440 | // Returns the target speed of the right motor (-1.0 to 1.0) |
jah128 | 1:b067a08ff54e | 441 | float PiSwarm::get_right_motor (){ |
jah128 | 1:b067a08ff54e | 442 | return right_speed; |
jah128 | 1:b067a08ff54e | 443 | } |
jah128 | 1:b067a08ff54e | 444 | |
jah128 | 1:b067a08ff54e | 445 | // Sets the speed of the left motor (-1.0 to 1.0) |
jah128 | 1:b067a08ff54e | 446 | void PiSwarm::left_motor (float speed) { |
jah128 | 1:b067a08ff54e | 447 | motor(0,speed); |
jah128 | 1:b067a08ff54e | 448 | } |
jah128 | 1:b067a08ff54e | 449 | |
jah128 | 1:b067a08ff54e | 450 | // Sets the speed of the right motor (-1.0 to 1.0) |
jah128 | 1:b067a08ff54e | 451 | void PiSwarm::right_motor (float speed) { |
jah128 | 1:b067a08ff54e | 452 | motor(1,speed); |
jah128 | 1:b067a08ff54e | 453 | } |
jah128 | 1:b067a08ff54e | 454 | |
jah128 | 1:b067a08ff54e | 455 | // Drive forward at the given speed (-1.0 to 1.0) |
jah128 | 1:b067a08ff54e | 456 | void PiSwarm::forward (float speed) { |
jah128 | 1:b067a08ff54e | 457 | motor(0,speed); |
jah128 | 1:b067a08ff54e | 458 | motor(1,speed); |
jah128 | 1:b067a08ff54e | 459 | } |
jah128 | 1:b067a08ff54e | 460 | |
jah128 | 1:b067a08ff54e | 461 | // Drive backward at the given speed (-1.0 to 1.0) |
jah128 | 1:b067a08ff54e | 462 | void PiSwarm::backward (float speed) { |
jah128 | 1:b067a08ff54e | 463 | motor(0,-1.0*speed); |
jah128 | 1:b067a08ff54e | 464 | motor(1,-1.0*speed); |
jah128 | 1:b067a08ff54e | 465 | } |
jah128 | 1:b067a08ff54e | 466 | |
jah128 | 1:b067a08ff54e | 467 | // Turn on-the-spot left at the given speed (-1.0 to 1.0) |
jah128 | 1:b067a08ff54e | 468 | void PiSwarm::left (float speed) { |
jah128 | 1:b067a08ff54e | 469 | motor(0,speed); |
jah128 | 1:b067a08ff54e | 470 | motor(1,-1.0*speed); |
jah128 | 1:b067a08ff54e | 471 | } |
jah128 | 1:b067a08ff54e | 472 | |
jah128 | 1:b067a08ff54e | 473 | // Turn on-the-spot right at the given speed (-1.0 to 1.0) |
jah128 | 1:b067a08ff54e | 474 | void PiSwarm::right (float speed) { |
jah128 | 1:b067a08ff54e | 475 | motor(0,-1.0*speed); |
jah128 | 1:b067a08ff54e | 476 | motor(1,speed); |
jah128 | 1:b067a08ff54e | 477 | } |
jah128 | 1:b067a08ff54e | 478 | |
jah128 | 1:b067a08ff54e | 479 | // Stop both motors |
jah128 | 1:b067a08ff54e | 480 | void PiSwarm::stop (void) { |
jah128 | 1:b067a08ff54e | 481 | motor(0,0.0); |
jah128 | 1:b067a08ff54e | 482 | motor(1,0.0); |
jah128 | 1:b067a08ff54e | 483 | } |
jah128 | 1:b067a08ff54e | 484 | ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// |
jah128 | 1:b067a08ff54e | 485 | // RF Transceiver Functions |
jah128 | 1:b067a08ff54e | 486 | ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// |
jah128 | 1:b067a08ff54e | 487 | |
jah128 | 1:b067a08ff54e | 488 | void PiSwarm::send_rf_message(char* message, char length){ |
jah128 | 1:b067a08ff54e | 489 | _rf.sendString(length, message); |
jah128 | 1:b067a08ff54e | 490 | } |
jah128 | 1:b067a08ff54e | 491 | |
jah128 | 1:b067a08ff54e | 492 | |
jah128 | 1:b067a08ff54e | 493 | ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// |
jah128 | 1:b067a08ff54e | 494 | // Sound Functions |
jah128 | 1:b067a08ff54e | 495 | ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// |
jah128 | 1:b067a08ff54e | 496 | |
jah128 | 1:b067a08ff54e | 497 | void PiSwarm::play_tune (char * tune, int length) { |
jah128 | 1:b067a08ff54e | 498 | _ser.putc(DO_PLAY); |
jah128 | 1:b067a08ff54e | 499 | _ser.putc(length); |
jah128 | 1:b067a08ff54e | 500 | for (int i = 0 ; i < length ; i++) { |
jah128 | 1:b067a08ff54e | 501 | _ser.putc(tune[i]); |
jah128 | 1:b067a08ff54e | 502 | } |
jah128 | 1:b067a08ff54e | 503 | } |
jah128 | 1:b067a08ff54e | 504 | |
jah128 | 1:b067a08ff54e | 505 | |
jah128 | 1:b067a08ff54e | 506 | ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// |
jah128 | 1:b067a08ff54e | 507 | // EEPROM Functions |
jah128 | 1:b067a08ff54e | 508 | ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// |
jah128 | 1:b067a08ff54e | 509 | |
jah128 | 1:b067a08ff54e | 510 | |
jah128 | 0:9ffe8ebd1c40 | 511 | void PiSwarm::write_eeprom_byte ( int address, char data ){ |
jah128 | 0:9ffe8ebd1c40 | 512 | char write_array[3]; |
jah128 | 0:9ffe8ebd1c40 | 513 | write_array[0] = address / 256; |
jah128 | 0:9ffe8ebd1c40 | 514 | write_array[1] = address % 256; |
jah128 | 0:9ffe8ebd1c40 | 515 | write_array[2] = data; |
jah128 | 0:9ffe8ebd1c40 | 516 | _i2c.write(EEPROM_ADDRESS, write_array, 3, false); |
jah128 | 0:9ffe8ebd1c40 | 517 | //Takes 5ms to write a page: ideally this could be done with a timer or RTOS |
jah128 | 0:9ffe8ebd1c40 | 518 | wait(0.005); |
jah128 | 0:9ffe8ebd1c40 | 519 | } |
jah128 | 0:9ffe8ebd1c40 | 520 | |
jah128 | 0:9ffe8ebd1c40 | 521 | void PiSwarm::write_eeprom_block ( int address, char length, char * data){ |
jah128 | 0:9ffe8ebd1c40 | 522 | /** Note from datasheet: |
jah128 | 0:9ffe8ebd1c40 | 523 | Page write operations are limited to writing bytes within a single physical page, |
jah128 | 0:9ffe8ebd1c40 | 524 | regardless of the number of bytes actually being written. Physical page |
jah128 | 0:9ffe8ebd1c40 | 525 | boundaries start at addresses that are integer multiples of the page buffer size (or |
jah128 | 0:9ffe8ebd1c40 | 526 | ‘page size’) and end at addresses that are integer multiples of [page size – 1]. If a |
jah128 | 0:9ffe8ebd1c40 | 527 | Page Write command attempts to write across a physical page boundary, the |
jah128 | 0:9ffe8ebd1c40 | 528 | result is that the data wraps around to the beginning of the current page (overwriting |
jah128 | 0:9ffe8ebd1c40 | 529 | data previously stored there), instead of being written to the next page, as might be |
jah128 | 0:9ffe8ebd1c40 | 530 | expected. It is therefore necessary for the application software to prevent page write |
jah128 | 0:9ffe8ebd1c40 | 531 | operations that would attempt to cross a page boundary. */ |
jah128 | 0:9ffe8ebd1c40 | 532 | |
jah128 | 0:9ffe8ebd1c40 | 533 | //First check to see if first block starts at the start of a page |
jah128 | 0:9ffe8ebd1c40 | 534 | int subpage = address % 32; |
jah128 | 0:9ffe8ebd1c40 | 535 | |
jah128 | 0:9ffe8ebd1c40 | 536 | //If subpage > 0 first page does not start at a page boundary |
jah128 | 0:9ffe8ebd1c40 | 537 | int write_length = 32 - subpage; |
jah128 | 0:9ffe8ebd1c40 | 538 | if( write_length > length ) write_length = length; |
jah128 | 0:9ffe8ebd1c40 | 539 | char firstpage_array [write_length+2]; |
jah128 | 0:9ffe8ebd1c40 | 540 | firstpage_array[0] = address / 256; |
jah128 | 0:9ffe8ebd1c40 | 541 | firstpage_array[1] = address % 256; |
jah128 | 0:9ffe8ebd1c40 | 542 | for(int i=0;i<write_length;i++){ |
jah128 | 0:9ffe8ebd1c40 | 543 | firstpage_array[i+2] = data [i]; |
jah128 | 0:9ffe8ebd1c40 | 544 | } |
jah128 | 0:9ffe8ebd1c40 | 545 | _i2c.write(EEPROM_ADDRESS, firstpage_array, write_length + 2, false); |
jah128 | 0:9ffe8ebd1c40 | 546 | wait(0.005); |
jah128 | 0:9ffe8ebd1c40 | 547 | |
jah128 | 0:9ffe8ebd1c40 | 548 | if(length > write_length){ |
jah128 | 0:9ffe8ebd1c40 | 549 | int page_count = (length + subpage) / 32; |
jah128 | 0:9ffe8ebd1c40 | 550 | int written = write_length; |
jah128 | 0:9ffe8ebd1c40 | 551 | for(int p=0;p<page_count;p++){ |
jah128 | 0:9ffe8ebd1c40 | 552 | int to_write = length - written; |
jah128 | 0:9ffe8ebd1c40 | 553 | if (to_write > 32) { |
jah128 | 0:9ffe8ebd1c40 | 554 | to_write = 32; |
jah128 | 0:9ffe8ebd1c40 | 555 | } |
jah128 | 0:9ffe8ebd1c40 | 556 | char page_array [to_write + 2]; |
jah128 | 0:9ffe8ebd1c40 | 557 | page_array[0] = (address + written) / 256; |
jah128 | 0:9ffe8ebd1c40 | 558 | page_array[1] = (address + written) % 256; |
jah128 | 0:9ffe8ebd1c40 | 559 | for(int i=0;i<to_write;i++){ |
jah128 | 0:9ffe8ebd1c40 | 560 | page_array[i+2] = data[written + i]; |
jah128 | 0:9ffe8ebd1c40 | 561 | } |
jah128 | 0:9ffe8ebd1c40 | 562 | _i2c.write(EEPROM_ADDRESS, page_array, to_write + 2, false); |
jah128 | 0:9ffe8ebd1c40 | 563 | wait(0.005); |
jah128 | 0:9ffe8ebd1c40 | 564 | written += 32; |
jah128 | 0:9ffe8ebd1c40 | 565 | } |
jah128 | 0:9ffe8ebd1c40 | 566 | } |
jah128 | 0:9ffe8ebd1c40 | 567 | } |
jah128 | 0:9ffe8ebd1c40 | 568 | |
jah128 | 0:9ffe8ebd1c40 | 569 | char PiSwarm::read_eeprom_byte ( int address ){ |
jah128 | 0:9ffe8ebd1c40 | 570 | char address_array [2]; |
jah128 | 0:9ffe8ebd1c40 | 571 | address_array[0] = address / 256; |
jah128 | 0:9ffe8ebd1c40 | 572 | address_array[1] = address % 256; |
jah128 | 0:9ffe8ebd1c40 | 573 | char data [1]; |
jah128 | 0:9ffe8ebd1c40 | 574 | _i2c.write(EEPROM_ADDRESS, address_array, 2, false); |
jah128 | 0:9ffe8ebd1c40 | 575 | _i2c.read(EEPROM_ADDRESS, data, 1, false); |
jah128 | 0:9ffe8ebd1c40 | 576 | return data [0]; |
jah128 | 0:9ffe8ebd1c40 | 577 | } |
jah128 | 0:9ffe8ebd1c40 | 578 | |
jah128 | 0:9ffe8ebd1c40 | 579 | char PiSwarm::read_next_eeprom_byte (){ |
jah128 | 0:9ffe8ebd1c40 | 580 | char data [1]; |
jah128 | 0:9ffe8ebd1c40 | 581 | _i2c.read(EEPROM_ADDRESS, data, 1, false); |
jah128 | 0:9ffe8ebd1c40 | 582 | return data [0]; |
jah128 | 0:9ffe8ebd1c40 | 583 | } |
jah128 | 0:9ffe8ebd1c40 | 584 | |
jah128 | 0:9ffe8ebd1c40 | 585 | char PiSwarm::read_eeprom_block ( int address, char length ){ |
jah128 | 0:9ffe8ebd1c40 | 586 | char ret = 0; |
jah128 | 0:9ffe8ebd1c40 | 587 | return ret; |
jah128 | 0:9ffe8ebd1c40 | 588 | } |
jah128 | 1:b067a08ff54e | 589 | |
jah128 | 1:b067a08ff54e | 590 | |
jah128 | 1:b067a08ff54e | 591 | ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// |
jah128 | 1:b067a08ff54e | 592 | // Setup and Calibration Functions |
jah128 | 1:b067a08ff54e | 593 | ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// |
jah128 | 1:b067a08ff54e | 594 | |
jah128 | 1:b067a08ff54e | 595 | void PiSwarm::setup () { |
jah128 | 1:b067a08ff54e | 596 | _ser.baud(115200); |
jah128 | 1:b067a08ff54e | 597 | set_oled_brightness (oled_brightness); |
jah128 | 1:b067a08ff54e | 598 | set_cled_brightness (cled_brightness); |
jah128 | 1:b067a08ff54e | 599 | gpio_led = setup_expansion_ic(); |
jah128 | 1:b067a08ff54e | 600 | adc_led = setup_adc(); |
jah128 | 1:b067a08ff54e | 601 | } |
jah128 | 1:b067a08ff54e | 602 | |
jah128 | 1:b067a08ff54e | 603 | void PiSwarm::setup_radio() { |
jah128 | 1:b067a08ff54e | 604 | //Setup RF transceiver |
jah128 | 1:b067a08ff54e | 605 | _rf.rf_init(); |
jah128 | 1:b067a08ff54e | 606 | _rf.setFrequency(RF_FREQUENCY); |
jah128 | 1:b067a08ff54e | 607 | _rf.setDatarate(RF_DATARATE); |
jah128 | 1:b067a08ff54e | 608 | } |
jah128 | 1:b067a08ff54e | 609 | |
jah128 | 1:b067a08ff54e | 610 | int PiSwarm::setup_expansion_ic () { |
jah128 | 1:b067a08ff54e | 611 | //Expansion IC is NXP PCA9505 |
jah128 | 1:b067a08ff54e | 612 | //Address is 0x40 (defined by EXPANSION_IC_ADDRESS) |
jah128 | 1:b067a08ff54e | 613 | //Block IO 0 : 7-0 Are OLED Outputs |
jah128 | 1:b067a08ff54e | 614 | //Block IO 1 : 7 is NC. 6-2 are Cursor switches (interrupt inputs). 1 and 0 are OLED outputs. |
jah128 | 1:b067a08ff54e | 615 | //Block IO 2 : 7 and 6 are LDO enable outputs. 5 - 1 are ID switch. 0 is NC. |
jah128 | 1:b067a08ff54e | 616 | //Block IO 3 and 4 are for the expansion connector (not assigned here) |
jah128 | 1:b067a08ff54e | 617 | char data [4]; |
jah128 | 1:b067a08ff54e | 618 | data [0] = 0x98; //Write to I/O configure register 0, using auto increment (allows multi-blocks in one command) |
jah128 | 1:b067a08ff54e | 619 | data [1] = 0x00; //All 8 pins in block 0 are outputs (0) |
jah128 | 1:b067a08ff54e | 620 | data [2] = 0xFC; //Pins 1 & 0 are outputs, rest are inputs (or NC) |
jah128 | 1:b067a08ff54e | 621 | data [3] = 0x3F; //Pins 7 & 6 are outputs, rest are inputs (or NC) |
jah128 | 1:b067a08ff54e | 622 | //If using the expansion port, assing its I/O here by increasing size of data[] and adding extra commands |
jah128 | 1:b067a08ff54e | 623 | int test_val = _i2c.write(EXPANSION_IC_ADDRESS,data,4,false); |
jah128 | 0:9ffe8ebd1c40 | 624 | |
jah128 | 1:b067a08ff54e | 625 | // test_val should return 0 for correctly acknowledged response |
jah128 | 1:b067a08ff54e | 626 | |
jah128 | 1:b067a08ff54e | 627 | //Invert the polarity for switch inputs (they connect to ground when pressed\on) |
jah128 | 1:b067a08ff54e | 628 | data [0] = 0x90; //Write to polarity inversion register using auto increment |
jah128 | 1:b067a08ff54e | 629 | data [1] = 0x00; |
jah128 | 1:b067a08ff54e | 630 | data [2] = 0x7C; //Invert pins 6-2 for cursor switches |
jah128 | 1:b067a08ff54e | 631 | data [3] = 0x3E; //Invert pins 5-1 for ID switch |
jah128 | 1:b067a08ff54e | 632 | _i2c.write(EXPANSION_IC_ADDRESS,data,4,false); |
jah128 | 1:b067a08ff54e | 633 | |
jah128 | 1:b067a08ff54e | 634 | //Setup the interrupt masks. By default, only the direction switches trigger an interrupt |
jah128 | 1:b067a08ff54e | 635 | data [0] = 0xA0; |
jah128 | 1:b067a08ff54e | 636 | data [1] = 0xFF; |
jah128 | 1:b067a08ff54e | 637 | data [2] = 0x83; // Disable mask on pins 6-2 for cursor switches |
jah128 | 1:b067a08ff54e | 638 | data [3] = 0xFF; |
jah128 | 1:b067a08ff54e | 639 | _i2c.write(EXPANSION_IC_ADDRESS,data,4,false); |
jah128 | 1:b067a08ff54e | 640 | |
jah128 | 1:b067a08ff54e | 641 | //Read the ID switches |
jah128 | 1:b067a08ff54e | 642 | data [0] = 0x80; //Read input registers |
jah128 | 1:b067a08ff54e | 643 | char read_data [5]; |
jah128 | 1:b067a08ff54e | 644 | _i2c.write(EXPANSION_IC_ADDRESS, data, 1, false); |
jah128 | 1:b067a08ff54e | 645 | _i2c.read(EXPANSION_IC_ADDRESS, read_data, 5, false); |
jah128 | 1:b067a08ff54e | 646 | id = (read_data[2] & 0x3E) >> 1; //Mask pins 0, 6 and 7 then shift |
jah128 | 1:b067a08ff54e | 647 | |
jah128 | 1:b067a08ff54e | 648 | //Setup interrupt handler |
jah128 | 1:b067a08ff54e | 649 | expansion_interrupt.mode(PullUp); // Pull Up by default (needed on v1 PCB - no pullup resistor) |
jah128 | 1:b067a08ff54e | 650 | expansion_interrupt.fall(this,&PiSwarm::expansion_interrupt_handler); // Add local handler on falling edge to read switch |
jah128 | 1:b067a08ff54e | 651 | return test_val; |
jah128 | 0:9ffe8ebd1c40 | 652 | } |
jah128 | 0:9ffe8ebd1c40 | 653 | |
jah128 | 1:b067a08ff54e | 654 | char PiSwarm::setup_adc ( void ){ |
jah128 | 1:b067a08ff54e | 655 | //ADC IC (for IR system) is a Analog Devices AD7997-BRUZ1 |
jah128 | 1:b067a08ff54e | 656 | return 0; |
jah128 | 1:b067a08ff54e | 657 | } |
jah128 | 1:b067a08ff54e | 658 | |
jah128 | 1:b067a08ff54e | 659 | char PiSwarm::calibrate_gyro ( void ){ |
jah128 | 1:b067a08ff54e | 660 | //This routine attempts to calibrate the offset of the gyro |
jah128 | 1:b067a08ff54e | 661 | int samples = 8; |
jah128 | 1:b067a08ff54e | 662 | float gyro_values[samples]; |
jah128 | 1:b067a08ff54e | 663 | calibrate_led = 1; |
jah128 | 1:b067a08ff54e | 664 | calibrate_ticker.attach(this,&PiSwarm::calibrate_ticker_routine,0.25); |
jah128 | 1:b067a08ff54e | 665 | for(int i=0;i<samples;i++){ |
jah128 | 1:b067a08ff54e | 666 | gyro_values[i] = _gyro.read(); |
jah128 | 1:b067a08ff54e | 667 | wait(0.12); |
jah128 | 1:b067a08ff54e | 668 | } |
jah128 | 1:b067a08ff54e | 669 | char pass = 1; |
jah128 | 1:b067a08ff54e | 670 | float mean = 0; |
jah128 | 1:b067a08ff54e | 671 | float min = 3.5; |
jah128 | 1:b067a08ff54e | 672 | float max = 3.2; |
jah128 | 1:b067a08ff54e | 673 | for(int i=0;i<samples;i++){ |
jah128 | 1:b067a08ff54e | 674 | if((gyro_values[i] * 3.5 < 1.5) || (gyro_values[i] * 3.2 > 1.5)) pass = 0; // Calibration failed! |
jah128 | 1:b067a08ff54e | 675 | float test_value = 1.50 / gyro_values[i]; |
jah128 | 1:b067a08ff54e | 676 | mean += test_value; |
jah128 | 1:b067a08ff54e | 677 | if (test_value > max) max = test_value; |
jah128 | 1:b067a08ff54e | 678 | if (test_value < min) min = test_value; |
jah128 | 1:b067a08ff54e | 679 | } |
jah128 | 1:b067a08ff54e | 680 | if(pass == 1){ |
jah128 | 1:b067a08ff54e | 681 | gyro_cal = mean / samples; |
jah128 | 1:b067a08ff54e | 682 | gyro_error = max - min; |
jah128 | 1:b067a08ff54e | 683 | calibrate_ticker.detach(); |
jah128 | 1:b067a08ff54e | 684 | calibrate_led = 0; |
jah128 | 1:b067a08ff54e | 685 | } |
jah128 | 1:b067a08ff54e | 686 | return pass; |
jah128 | 1:b067a08ff54e | 687 | } |
jah128 | 1:b067a08ff54e | 688 | |
jah128 | 1:b067a08ff54e | 689 | char PiSwarm::calibrate_accelerometer ( void ){ |
jah128 | 1:b067a08ff54e | 690 | //This routine attempts to calibrate the offset and multiplier of the accelerometer |
jah128 | 1:b067a08ff54e | 691 | int samples = 8; |
jah128 | 1:b067a08ff54e | 692 | float acc_x_values[samples]; |
jah128 | 1:b067a08ff54e | 693 | float acc_y_values[samples]; |
jah128 | 1:b067a08ff54e | 694 | float acc_z_values[samples]; |
jah128 | 1:b067a08ff54e | 695 | calibrate_led = 1; |
jah128 | 1:b067a08ff54e | 696 | calibrate_ticker.attach(this,&PiSwarm::calibrate_ticker_routine,0.25); |
jah128 | 1:b067a08ff54e | 697 | for(int i=0;i<samples;i++){ |
jah128 | 1:b067a08ff54e | 698 | acc_x_values[i] = _accel_x.read(); |
jah128 | 1:b067a08ff54e | 699 | acc_y_values[i] = _accel_y.read(); |
jah128 | 1:b067a08ff54e | 700 | acc_z_values[i] = _accel_z.read(); |
jah128 | 1:b067a08ff54e | 701 | wait(0.12); |
jah128 | 1:b067a08ff54e | 702 | } |
jah128 | 1:b067a08ff54e | 703 | char pass = 1; |
jah128 | 1:b067a08ff54e | 704 | float x_mean = 0, y_mean=0, z_mean=0; |
jah128 | 1:b067a08ff54e | 705 | float x_min = 3600, y_min=3600, z_min=3600; |
jah128 | 1:b067a08ff54e | 706 | float x_max = 3100, y_max=3100, z_max=3100; |
jah128 | 1:b067a08ff54e | 707 | for(int i=0;i<samples;i++){ |
jah128 | 1:b067a08ff54e | 708 | if((acc_x_values[i] * 3600 < 1250) || (acc_x_values[i] * 3100 > 1250)) pass = 0; // Calibration failed! |
jah128 | 1:b067a08ff54e | 709 | if((acc_y_values[i] * 3600 < 1250) || (acc_y_values[i] * 3100 > 1250)) pass = 0; // Calibration failed! |
jah128 | 1:b067a08ff54e | 710 | if((acc_z_values[i] * 3600 < 887) || (acc_z_values[i] * 3100 > 887)) pass = 0; // Calibration failed! |
jah128 | 1:b067a08ff54e | 711 | |
jah128 | 1:b067a08ff54e | 712 | float x_test_value = 1250 / acc_x_values[i]; |
jah128 | 1:b067a08ff54e | 713 | x_mean += x_test_value; |
jah128 | 1:b067a08ff54e | 714 | if (x_test_value > x_max) x_max = x_test_value; |
jah128 | 1:b067a08ff54e | 715 | if (x_test_value < x_min) x_min = x_test_value; |
jah128 | 1:b067a08ff54e | 716 | float y_test_value = 1250 / acc_y_values[i]; |
jah128 | 1:b067a08ff54e | 717 | y_mean += y_test_value; |
jah128 | 1:b067a08ff54e | 718 | if (y_test_value > y_max) y_max = y_test_value; |
jah128 | 1:b067a08ff54e | 719 | if (y_test_value < y_min) y_min = y_test_value; |
jah128 | 1:b067a08ff54e | 720 | float z_test_value = 887 / acc_z_values[i]; |
jah128 | 1:b067a08ff54e | 721 | z_mean += z_test_value; |
jah128 | 1:b067a08ff54e | 722 | if (z_test_value > z_max) z_max = z_test_value; |
jah128 | 1:b067a08ff54e | 723 | if (z_test_value < z_min) z_min = z_test_value; |
jah128 | 1:b067a08ff54e | 724 | } |
jah128 | 1:b067a08ff54e | 725 | if(pass == 1){ |
jah128 | 1:b067a08ff54e | 726 | acc_x_cal = x_mean / samples; |
jah128 | 1:b067a08ff54e | 727 | acc_y_cal = y_mean / samples; |
jah128 | 1:b067a08ff54e | 728 | acc_z_cal = z_mean / samples; |
jah128 | 1:b067a08ff54e | 729 | calibrate_ticker.detach(); |
jah128 | 1:b067a08ff54e | 730 | calibrate_led = 0; |
jah128 | 1:b067a08ff54e | 731 | } |
jah128 | 1:b067a08ff54e | 732 | return pass; |
jah128 | 1:b067a08ff54e | 733 | } |
jah128 | 1:b067a08ff54e | 734 | |
jah128 | 1:b067a08ff54e | 735 | char PiSwarm::calibrate_magnetometer ( void ){ |
jah128 | 1:b067a08ff54e | 736 | char command_data[2]; |
jah128 | 1:b067a08ff54e | 737 | command_data[0] = 0x11; //Write to CTRL_REG2 |
jah128 | 1:b067a08ff54e | 738 | command_data[1] = 0x80; // Enable auto resets |
jah128 | 1:b067a08ff54e | 739 | _i2c.write(MAGNETOMETER_ADDRESS, command_data, 2, false); |
jah128 | 1:b067a08ff54e | 740 | command_data[0] = 0x10; //Write to CTRL_REG1 |
jah128 | 1:b067a08ff54e | 741 | command_data[1] = 0x29; // 001 01 0 0 1 [40HZ 32-OS NOT-FAST NORMAL ACTIVE MODE] |
jah128 | 1:b067a08ff54e | 742 | return _i2c.write(MAGNETOMETER_ADDRESS, command_data, 2, false); |
jah128 | 1:b067a08ff54e | 743 | } |
jah128 | 1:b067a08ff54e | 744 | |
jah128 | 1:b067a08ff54e | 745 | |
jah128 | 1:b067a08ff54e | 746 | |
jah128 | 1:b067a08ff54e | 747 | |
jah128 | 1:b067a08ff54e | 748 | |
jah128 | 1:b067a08ff54e | 749 | void PiSwarm::interrupt_timeout_event ( void ){ |
jah128 | 1:b067a08ff54e | 750 | //Read switches |
jah128 | 1:b067a08ff54e | 751 | char data [1]; |
jah128 | 1:b067a08ff54e | 752 | data [0] = 0x80; //Read input registers |
jah128 | 1:b067a08ff54e | 753 | char read_data [5]; |
jah128 | 1:b067a08ff54e | 754 | _i2c.write(EXPANSION_IC_ADDRESS, data, 1, false); |
jah128 | 1:b067a08ff54e | 755 | _i2c.read(EXPANSION_IC_ADDRESS, read_data, 5, false); |
jah128 | 1:b067a08ff54e | 756 | switches = (read_data[1] & 0x7C) >> 2; |
jah128 | 1:b067a08ff54e | 757 | led_timeout.attach(this,&PiSwarm::led_timeout_event, 0.1); // Switch off the LED after 0.1s |
jah128 | 1:b067a08ff54e | 758 | } |
jah128 | 1:b067a08ff54e | 759 | |
jah128 | 1:b067a08ff54e | 760 | void PiSwarm::led_timeout_event ( void ){ |
jah128 | 1:b067a08ff54e | 761 | interrupt_led = 0; |
jah128 | 1:b067a08ff54e | 762 | } |
jah128 | 1:b067a08ff54e | 763 | |
jah128 | 1:b067a08ff54e | 764 | void PiSwarm::expansion_interrupt_handler ( void ){ |
jah128 | 1:b067a08ff54e | 765 | interrupt_led = 1; |
jah128 | 1:b067a08ff54e | 766 | debounce_timeout.attach_us(this,&PiSwarm::interrupt_timeout_event, 200); // Read the switches only after 200uS have passed to debounce |
jah128 | 1:b067a08ff54e | 767 | debounce_timeout.add_function(&switch_pressed); // Call the switch pressed routine (in main) when done |
jah128 | 1:b067a08ff54e | 768 | } |
jah128 | 1:b067a08ff54e | 769 | |
jah128 | 1:b067a08ff54e | 770 | |
jah128 | 1:b067a08ff54e | 771 | |
jah128 | 1:b067a08ff54e | 772 | |
jah128 | 1:b067a08ff54e | 773 | |
jah128 | 1:b067a08ff54e | 774 | void PiSwarm::calibrate_ticker_routine ( void ){ |
jah128 | 1:b067a08ff54e | 775 | calibrate_led_state = 1 - calibrate_led_state; |
jah128 | 1:b067a08ff54e | 776 | calibrate_led = calibrate_led_state; |
jah128 | 1:b067a08ff54e | 777 | } |
jah128 | 1:b067a08ff54e | 778 | |
jah128 | 1:b067a08ff54e | 779 | void PiSwarm::test_oled(){ |
jah128 | 1:b067a08ff54e | 780 | enable_led_ldo = 1; |
jah128 | 1:b067a08ff54e | 781 | enable_ldo_outputs(); |
jah128 | 1:b067a08ff54e | 782 | set_oled_colour(100,100,100); |
jah128 | 1:b067a08ff54e | 783 | char data[2]; |
jah128 | 1:b067a08ff54e | 784 | data[0] = 0x09; //Address for PCA9505 Output Port 1 |
jah128 | 1:b067a08ff54e | 785 | data[1] = 3; |
jah128 | 1:b067a08ff54e | 786 | _i2c.write(EXPANSION_IC_ADDRESS,data,2,false); |
jah128 | 1:b067a08ff54e | 787 | } |
jah128 | 0:9ffe8ebd1c40 | 788 | |
jah128 | 0:9ffe8ebd1c40 | 789 | void PiSwarm::reset () { |
jah128 | 0:9ffe8ebd1c40 | 790 | // _nrst = 0; |
jah128 | 0:9ffe8ebd1c40 | 791 | wait (0.01); |
jah128 | 0:9ffe8ebd1c40 | 792 | // _nrst = 1; |
jah128 | 0:9ffe8ebd1c40 | 793 | wait (0.1); |
jah128 | 0:9ffe8ebd1c40 | 794 | } |
jah128 | 0:9ffe8ebd1c40 | 795 | void PiSwarm::motor (int motor, float speed) { |
jah128 | 0:9ffe8ebd1c40 | 796 | char opcode = 0x0; |
jah128 | 0:9ffe8ebd1c40 | 797 | if (speed > 0.0) { |
jah128 | 0:9ffe8ebd1c40 | 798 | if (motor==1) |
jah128 | 0:9ffe8ebd1c40 | 799 | opcode = M1_FORWARD; |
jah128 | 0:9ffe8ebd1c40 | 800 | else |
jah128 | 0:9ffe8ebd1c40 | 801 | opcode = M2_FORWARD; |
jah128 | 0:9ffe8ebd1c40 | 802 | } else { |
jah128 | 0:9ffe8ebd1c40 | 803 | if (motor==1) |
jah128 | 0:9ffe8ebd1c40 | 804 | opcode = M1_BACKWARD; |
jah128 | 0:9ffe8ebd1c40 | 805 | else |
jah128 | 0:9ffe8ebd1c40 | 806 | opcode = M2_BACKWARD; |
jah128 | 0:9ffe8ebd1c40 | 807 | } |
jah128 | 0:9ffe8ebd1c40 | 808 | if(motor==1)right_speed = speed; |
jah128 | 0:9ffe8ebd1c40 | 809 | else left_speed = speed; |
jah128 | 0:9ffe8ebd1c40 | 810 | unsigned char arg = 0x7f * abs(speed); |
jah128 | 0:9ffe8ebd1c40 | 811 | |
jah128 | 0:9ffe8ebd1c40 | 812 | _ser.putc(opcode); |
jah128 | 0:9ffe8ebd1c40 | 813 | _ser.putc(arg); |
jah128 | 0:9ffe8ebd1c40 | 814 | } |
jah128 | 0:9ffe8ebd1c40 | 815 | |
jah128 | 0:9ffe8ebd1c40 | 816 | |
jah128 | 0:9ffe8ebd1c40 | 817 | float PiSwarm::line_position() { |
jah128 | 0:9ffe8ebd1c40 | 818 | int pos = 0; |
jah128 | 0:9ffe8ebd1c40 | 819 | _ser.putc(SEND_LINE_POSITION); |
jah128 | 0:9ffe8ebd1c40 | 820 | pos = _ser.getc(); |
jah128 | 0:9ffe8ebd1c40 | 821 | pos += _ser.getc() << 8; |
jah128 | 0:9ffe8ebd1c40 | 822 | |
jah128 | 0:9ffe8ebd1c40 | 823 | float fpos = ((float)pos - 2048.0)/2048.0; |
jah128 | 0:9ffe8ebd1c40 | 824 | return(fpos); |
jah128 | 0:9ffe8ebd1c40 | 825 | } |
jah128 | 0:9ffe8ebd1c40 | 826 | |
jah128 | 0:9ffe8ebd1c40 | 827 | char PiSwarm::sensor_auto_calibrate() { |
jah128 | 0:9ffe8ebd1c40 | 828 | _ser.putc(AUTO_CALIBRATE); |
jah128 | 0:9ffe8ebd1c40 | 829 | return(_ser.getc()); |
jah128 | 0:9ffe8ebd1c40 | 830 | } |
jah128 | 0:9ffe8ebd1c40 | 831 | |
jah128 | 0:9ffe8ebd1c40 | 832 | |
jah128 | 0:9ffe8ebd1c40 | 833 | void PiSwarm::calibrate(void) { |
jah128 | 0:9ffe8ebd1c40 | 834 | _ser.putc(PI_CALIBRATE); |
jah128 | 0:9ffe8ebd1c40 | 835 | } |
jah128 | 0:9ffe8ebd1c40 | 836 | |
jah128 | 0:9ffe8ebd1c40 | 837 | void PiSwarm::reset_calibration() { |
jah128 | 0:9ffe8ebd1c40 | 838 | _ser.putc(LINE_SENSORS_RESET_CALIBRATION); |
jah128 | 0:9ffe8ebd1c40 | 839 | } |
jah128 | 0:9ffe8ebd1c40 | 840 | |
jah128 | 0:9ffe8ebd1c40 | 841 | void PiSwarm::PID_start(int max_speed, int a, int b, int c, int d) { |
jah128 | 0:9ffe8ebd1c40 | 842 | _ser.putc(max_speed); |
jah128 | 0:9ffe8ebd1c40 | 843 | _ser.putc(a); |
jah128 | 0:9ffe8ebd1c40 | 844 | _ser.putc(b); |
jah128 | 0:9ffe8ebd1c40 | 845 | _ser.putc(c); |
jah128 | 0:9ffe8ebd1c40 | 846 | _ser.putc(d); |
jah128 | 0:9ffe8ebd1c40 | 847 | } |
jah128 | 0:9ffe8ebd1c40 | 848 | |
jah128 | 0:9ffe8ebd1c40 | 849 | void PiSwarm::PID_stop() { |
jah128 | 0:9ffe8ebd1c40 | 850 | _ser.putc(STOP_PID); |
jah128 | 0:9ffe8ebd1c40 | 851 | } |
jah128 | 0:9ffe8ebd1c40 | 852 | |
jah128 | 0:9ffe8ebd1c40 | 853 | |
jah128 | 0:9ffe8ebd1c40 | 854 | void PiSwarm::locate(int x, int y) { |
jah128 | 0:9ffe8ebd1c40 | 855 | _ser.putc(DO_LCD_GOTO_XY); |
jah128 | 0:9ffe8ebd1c40 | 856 | _ser.putc(x); |
jah128 | 0:9ffe8ebd1c40 | 857 | _ser.putc(y); |
jah128 | 0:9ffe8ebd1c40 | 858 | } |
jah128 | 0:9ffe8ebd1c40 | 859 | |
jah128 | 0:9ffe8ebd1c40 | 860 | void PiSwarm::cls(void) { |
jah128 | 0:9ffe8ebd1c40 | 861 | _ser.putc(DO_CLEAR); |
jah128 | 0:9ffe8ebd1c40 | 862 | } |
jah128 | 0:9ffe8ebd1c40 | 863 | |
jah128 | 0:9ffe8ebd1c40 | 864 | int PiSwarm::print (char* text, int length) { |
jah128 | 0:9ffe8ebd1c40 | 865 | _ser.putc(DO_PRINT); |
jah128 | 0:9ffe8ebd1c40 | 866 | _ser.putc(length); |
jah128 | 0:9ffe8ebd1c40 | 867 | for (int i = 0 ; i < length ; i++) { |
jah128 | 0:9ffe8ebd1c40 | 868 | _ser.putc(text[i]); |
jah128 | 0:9ffe8ebd1c40 | 869 | } |
jah128 | 0:9ffe8ebd1c40 | 870 | return(0); |
jah128 | 0:9ffe8ebd1c40 | 871 | } |
jah128 | 0:9ffe8ebd1c40 | 872 | |
jah128 | 0:9ffe8ebd1c40 | 873 | int PiSwarm::_putc (int c) { |
jah128 | 0:9ffe8ebd1c40 | 874 | _ser.putc(DO_PRINT); |
jah128 | 0:9ffe8ebd1c40 | 875 | _ser.putc(0x1); |
jah128 | 0:9ffe8ebd1c40 | 876 | _ser.putc(c); |
jah128 | 0:9ffe8ebd1c40 | 877 | wait (0.001); |
jah128 | 0:9ffe8ebd1c40 | 878 | return(c); |
jah128 | 0:9ffe8ebd1c40 | 879 | } |
jah128 | 0:9ffe8ebd1c40 | 880 | |
jah128 | 0:9ffe8ebd1c40 | 881 | int PiSwarm::_getc (void) { |
jah128 | 0:9ffe8ebd1c40 | 882 | char r = 0; |
jah128 | 0:9ffe8ebd1c40 | 883 | return(r); |
jah128 | 0:9ffe8ebd1c40 | 884 | } |
jah128 | 0:9ffe8ebd1c40 | 885 | |
jah128 | 0:9ffe8ebd1c40 | 886 | int PiSwarm::putc (int c) { |
jah128 | 0:9ffe8ebd1c40 | 887 | return(_ser.putc(c)); |
jah128 | 0:9ffe8ebd1c40 | 888 | } |
jah128 | 0:9ffe8ebd1c40 | 889 | |
jah128 | 0:9ffe8ebd1c40 | 890 | int PiSwarm::getc (void) { |
jah128 | 0:9ffe8ebd1c40 | 891 | return(_ser.getc()); |
jah128 | 0:9ffe8ebd1c40 | 892 | } |
jah128 | 0:9ffe8ebd1c40 | 893 | |
jah128 | 0:9ffe8ebd1c40 | 894 | void PiSwarm::start_system_timer(void) { |
jah128 | 0:9ffe8ebd1c40 | 895 | _system_timer.reset(); |
jah128 | 0:9ffe8ebd1c40 | 896 | _system_timer.start(); |
jah128 | 0:9ffe8ebd1c40 | 897 | } |
jah128 | 0:9ffe8ebd1c40 | 898 | |
jah128 | 0:9ffe8ebd1c40 | 899 | |
jah128 | 0:9ffe8ebd1c40 | 900 | #ifdef MBED_RPC |
jah128 | 0:9ffe8ebd1c40 | 901 | const rpc_method *PiSwarm::get_rpc_methods() { |
jah128 | 0:9ffe8ebd1c40 | 902 | static const rpc_method rpc_methods[] = {{ "forward", rpc_method_caller<PiSwarm, float, &PiSwarm::forward> }, |
jah128 | 0:9ffe8ebd1c40 | 903 | { "backward", rpc_method_caller<PiSwarm, float, &PiSwarm::backward> }, |
jah128 | 0:9ffe8ebd1c40 | 904 | { "left", rpc_method_caller<PiSwarm, float, &PiSwarm::left> }, |
jah128 | 0:9ffe8ebd1c40 | 905 | { "right", rpc_method_caller<PiSwarm, float, &PiSwarm::right> }, |
jah128 | 0:9ffe8ebd1c40 | 906 | { "stop", rpc_method_caller<PiSwarm, &PiSwarm::stop> }, |
jah128 | 0:9ffe8ebd1c40 | 907 | { "left_motor", rpc_method_caller<PiSwarm, float, &PiSwarm::left_motor> }, |
jah128 | 0:9ffe8ebd1c40 | 908 | { "right_motor", rpc_method_caller<PiSwarm, float, &PiSwarm::right_motor> }, |
jah128 | 0:9ffe8ebd1c40 | 909 | { "battery", rpc_method_caller<float, PiSwarm, &PiSwarm::battery> }, |
jah128 | 0:9ffe8ebd1c40 | 910 | { "line_position", rpc_method_caller<float, PiSwarm, &PiSwarm::line_position> }, |
jah128 | 0:9ffe8ebd1c40 | 911 | { "sensor_auto_calibrate", rpc_method_caller<char, PiSwarm, &PiSwarm::sensor_auto_calibrate> }, |
jah128 | 0:9ffe8ebd1c40 | 912 | RPC_METHOD_SUPER(Base) |
jah128 | 0:9ffe8ebd1c40 | 913 | }; |
jah128 | 0:9ffe8ebd1c40 | 914 | return rpc_methods; |
jah128 | 0:9ffe8ebd1c40 | 915 | } |
jah128 | 0:9ffe8ebd1c40 | 916 | #endif |
jah128 | 0:9ffe8ebd1c40 | 917 | |
jah128 | 0:9ffe8ebd1c40 | 918 | void display_system_time(){ |
jah128 | 0:9ffe8ebd1c40 | 919 | if(PISWARM_DEBUG == 1){ |
jah128 | 0:9ffe8ebd1c40 | 920 | time_t system_time = time(NULL); |
jah128 | 0:9ffe8ebd1c40 | 921 | printf("Current system time:%s Uptime:%.1f\n",ctime(&system_time),piswarm.get_uptime()); |
jah128 | 0:9ffe8ebd1c40 | 922 | } |
jah128 | 0:9ffe8ebd1c40 | 923 | } |
jah128 | 0:9ffe8ebd1c40 | 924 | |
jah128 | 0:9ffe8ebd1c40 | 925 | void init() { |
jah128 | 0:9ffe8ebd1c40 | 926 | //Standard initialisation routine for Pi Swarm Robot |
jah128 | 0:9ffe8ebd1c40 | 927 | //Displays a message on the screen and flashes the central LED |
jah128 | 0:9ffe8ebd1c40 | 928 | //Calibrates the gyro and accelerometer |
jah128 | 0:9ffe8ebd1c40 | 929 | //Make sure robot is flat and stationary when this code is run (calibration starts after about .5 second to allow robot to settle) |
jah128 | 0:9ffe8ebd1c40 | 930 | piswarm.start_system_timer(); |
jah128 | 0:9ffe8ebd1c40 | 931 | pc.baud(PC_BAUD); |
jah128 | 0:9ffe8ebd1c40 | 932 | if(PISWARM_DEBUG == 1)pc.printf("PiSwarm Firmware 0.4 Initialisation...\n"); |
jah128 | 0:9ffe8ebd1c40 | 933 | display_system_time(); |
jah128 | 0:9ffe8ebd1c40 | 934 | piswarm.play_tune("T298MSCG>C",10); |
jah128 | 0:9ffe8ebd1c40 | 935 | piswarm.cls(); |
jah128 | 0:9ffe8ebd1c40 | 936 | piswarm.enable_cled(1); |
jah128 | 0:9ffe8ebd1c40 | 937 | piswarm.set_cled_colour(200,0,0); |
jah128 | 0:9ffe8ebd1c40 | 938 | piswarm.set_oled_colour(200,0,0); |
jah128 | 0:9ffe8ebd1c40 | 939 | piswarm.set_oleds(1,0,1,0,1,0,1,0,1,0); |
jah128 | 0:9ffe8ebd1c40 | 940 | piswarm.printf(" YORK "); |
jah128 | 0:9ffe8ebd1c40 | 941 | piswarm.locate(0,1); |
jah128 | 0:9ffe8ebd1c40 | 942 | piswarm.printf("ROBOTLAB"); |
jah128 | 0:9ffe8ebd1c40 | 943 | wait(0.07); |
jah128 | 0:9ffe8ebd1c40 | 944 | piswarm.set_oleds(0,1,0,1,0,1,0,1,0,1); |
jah128 | 0:9ffe8ebd1c40 | 945 | wait(0.07); |
jah128 | 0:9ffe8ebd1c40 | 946 | piswarm.set_cled_colour(0,200,0); |
jah128 | 0:9ffe8ebd1c40 | 947 | piswarm.set_oled_colour(0,200,0); |
jah128 | 0:9ffe8ebd1c40 | 948 | piswarm.set_oleds(1,0,1,0,1,0,1,0,1,0); |
jah128 | 0:9ffe8ebd1c40 | 949 | wait(0.07); |
jah128 | 0:9ffe8ebd1c40 | 950 | piswarm.set_oleds(0,1,0,1,0,1,0,1,0,1); |
jah128 | 0:9ffe8ebd1c40 | 951 | wait(0.07); |
jah128 | 0:9ffe8ebd1c40 | 952 | piswarm.set_cled_colour(0,0,200); |
jah128 | 0:9ffe8ebd1c40 | 953 | piswarm.set_oled_colour(0,0,200); |
jah128 | 0:9ffe8ebd1c40 | 954 | piswarm.set_oleds(1,0,1,0,1,0,1,0,1,0); |
jah128 | 0:9ffe8ebd1c40 | 955 | wait(0.07); |
jah128 | 0:9ffe8ebd1c40 | 956 | piswarm.set_oleds(0,1,0,1,0,1,0,1,0,1); |
jah128 | 0:9ffe8ebd1c40 | 957 | wait(0.07); |
jah128 | 0:9ffe8ebd1c40 | 958 | piswarm.set_oled_colour(20,150,200); |
jah128 | 0:9ffe8ebd1c40 | 959 | piswarm.set_oleds(1,1,1,1,1,1,1,1,1,1); |
jah128 | 0:9ffe8ebd1c40 | 960 | |
jah128 | 0:9ffe8ebd1c40 | 961 | piswarm.cls(); |
jah128 | 0:9ffe8ebd1c40 | 962 | piswarm.set_cled_colour(20,20,20); |
jah128 | 0:9ffe8ebd1c40 | 963 | piswarm.printf("Pi Swarm"); |
jah128 | 0:9ffe8ebd1c40 | 964 | piswarm.locate(0,1); |
jah128 | 0:9ffe8ebd1c40 | 965 | piswarm.printf("ID : %d ",piswarm.get_id()); |
jah128 | 0:9ffe8ebd1c40 | 966 | if(PISWARM_DEBUG == 1)pc.printf("Test Magnetometer: "); |
jah128 | 0:9ffe8ebd1c40 | 967 | if(piswarm.calibrate_magnetometer() != 0){ |
jah128 | 0:9ffe8ebd1c40 | 968 | if(PISWARM_DEBUG == 1)pc.printf("FAILED\n"); |
jah128 | 0:9ffe8ebd1c40 | 969 | wait(0.1); |
jah128 | 0:9ffe8ebd1c40 | 970 | piswarm.cls(); |
jah128 | 0:9ffe8ebd1c40 | 971 | piswarm.locate(0,0); |
jah128 | 0:9ffe8ebd1c40 | 972 | piswarm.printf("Mag Cal "); |
jah128 | 0:9ffe8ebd1c40 | 973 | piswarm.locate(0,1); |
jah128 | 0:9ffe8ebd1c40 | 974 | piswarm.printf("Failed "); |
jah128 | 0:9ffe8ebd1c40 | 975 | wait(0.1); |
jah128 | 0:9ffe8ebd1c40 | 976 | }else if(PISWARM_DEBUG == 1)pc.printf("PASSED\n"); |
jah128 | 0:9ffe8ebd1c40 | 977 | if(PISWARM_DEBUG == 1)pc.printf("Test Gyroscope: "); |
jah128 | 0:9ffe8ebd1c40 | 978 | if(piswarm.calibrate_gyro() == 0){ |
jah128 | 0:9ffe8ebd1c40 | 979 | if(PISWARM_DEBUG == 1)pc.printf("FAILED\n"); |
jah128 | 0:9ffe8ebd1c40 | 980 | wait(0.1); |
jah128 | 0:9ffe8ebd1c40 | 981 | piswarm.cls(); |
jah128 | 0:9ffe8ebd1c40 | 982 | piswarm.locate(0,0); |
jah128 | 0:9ffe8ebd1c40 | 983 | piswarm.printf("Gyro Cal"); |
jah128 | 0:9ffe8ebd1c40 | 984 | piswarm.locate(0,1); |
jah128 | 0:9ffe8ebd1c40 | 985 | piswarm.printf("Failed "); |
jah128 | 0:9ffe8ebd1c40 | 986 | wait(0.1); |
jah128 | 0:9ffe8ebd1c40 | 987 | }else if(PISWARM_DEBUG == 1)pc.printf("PASSED\n"); |
jah128 | 0:9ffe8ebd1c40 | 988 | if(PISWARM_DEBUG == 1)pc.printf("Test Accelerometer: "); |
jah128 | 0:9ffe8ebd1c40 | 989 | if(piswarm.calibrate_accelerometer() == 0){ |
jah128 | 0:9ffe8ebd1c40 | 990 | if(PISWARM_DEBUG == 1)pc.printf("FAILED\n"); |
jah128 | 0:9ffe8ebd1c40 | 991 | wait(0.1); |
jah128 | 0:9ffe8ebd1c40 | 992 | piswarm.cls(); |
jah128 | 0:9ffe8ebd1c40 | 993 | piswarm.locate(0,0); |
jah128 | 0:9ffe8ebd1c40 | 994 | piswarm.printf("Acc. Cal"); |
jah128 | 0:9ffe8ebd1c40 | 995 | piswarm.locate(0,1); |
jah128 | 0:9ffe8ebd1c40 | 996 | piswarm.printf("Failed "); |
jah128 | 0:9ffe8ebd1c40 | 997 | wait(0.1); |
jah128 | 0:9ffe8ebd1c40 | 998 | }else if(PISWARM_DEBUG == 1)pc.printf("PASSED\n"); |
jah128 | 0:9ffe8ebd1c40 | 999 | piswarm.turn_off_all_oleds(); |
jah128 | 0:9ffe8ebd1c40 | 1000 | wait(0.1); |
jah128 | 0:9ffe8ebd1c40 | 1001 | piswarm.cls(); |
jah128 | 0:9ffe8ebd1c40 | 1002 | piswarm.set_cled_colour(0,0,0); |
jah128 | 0:9ffe8ebd1c40 | 1003 | if(START_RADIO_ON_BOOT == 1){ |
jah128 | 0:9ffe8ebd1c40 | 1004 | if(PISWARM_DEBUG == 1)pc.printf("Setting up 433MHz Transceiver\n"); |
jah128 | 0:9ffe8ebd1c40 | 1005 | piswarm.setup_radio(); |
jah128 | 0:9ffe8ebd1c40 | 1006 | } |
jah128 | 0:9ffe8ebd1c40 | 1007 | } |
jah128 | 0:9ffe8ebd1c40 | 1008 | |
jah128 | 0:9ffe8ebd1c40 | 1009 | |
jah128 | 0:9ffe8ebd1c40 | 1010 | |
jah128 | 0:9ffe8ebd1c40 | 1011 | /******************************************************************************** |
jah128 | 0:9ffe8ebd1c40 | 1012 | * COPYRIGHT NOTICE * |
jah128 | 0:9ffe8ebd1c40 | 1013 | * * |
jah128 | 0:9ffe8ebd1c40 | 1014 | * Permission is hereby granted, free of charge, to any person obtaining a copy * |
jah128 | 0:9ffe8ebd1c40 | 1015 | * of this software and associated documentation files (the "Software"), to deal* |
jah128 | 0:9ffe8ebd1c40 | 1016 | * in the Software without restriction, including without limitation the rights * |
jah128 | 0:9ffe8ebd1c40 | 1017 | * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell * |
jah128 | 0:9ffe8ebd1c40 | 1018 | * copies of the Software, and to permit persons to whom the Software is * |
jah128 | 0:9ffe8ebd1c40 | 1019 | * furnished to do so, subject to the following conditions: * |
jah128 | 0:9ffe8ebd1c40 | 1020 | * * |
jah128 | 0:9ffe8ebd1c40 | 1021 | * The above copyright notice and this permission notice shall be included in * |
jah128 | 0:9ffe8ebd1c40 | 1022 | * all copies or substantial portions of the Software. * |
jah128 | 0:9ffe8ebd1c40 | 1023 | * * |
jah128 | 0:9ffe8ebd1c40 | 1024 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * |
jah128 | 0:9ffe8ebd1c40 | 1025 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, * |
jah128 | 0:9ffe8ebd1c40 | 1026 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * |
jah128 | 0:9ffe8ebd1c40 | 1027 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * |
jah128 | 0:9ffe8ebd1c40 | 1028 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,* |
jah128 | 0:9ffe8ebd1c40 | 1029 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN * |
jah128 | 0:9ffe8ebd1c40 | 1030 | * THE SOFTWARE. * |
jah128 | 0:9ffe8ebd1c40 | 1031 | * * |
jah128 | 0:9ffe8ebd1c40 | 1032 | ********************************************************************************/ |
jah128 | 0:9ffe8ebd1c40 | 1033 | |
jah128 | 0:9ffe8ebd1c40 | 1034 |