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