Unfinished v0.7, added bearing detection
Fork of Pi_Swarm_Library_v06_alpha by
piswarm.cpp@11:5ebcb52726cf, 2014-06-30 (annotated)
- Committer:
- jah128
- Date:
- Mon Jun 30 07:58:31 2014 +0000
- Revision:
- 11:5ebcb52726cf
- Parent:
- 9:7a4fc1d7e484
Added prototype bearing detection to pi swarm library;
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 | 11:5ebcb52726cf | 6 | * |
jah128 | 9:7a4fc1d7e484 | 7 | * Version 0.6 February 2014 |
jah128 | 9:7a4fc1d7e484 | 8 | * |
jah128 | 9:7a4fc1d7e484 | 9 | * Change notes: |
jah128 | 11:5ebcb52726cf | 10 | * 0.7 : Added code for IR table beacon syncing and turning |
jah128 | 9:7a4fc1d7e484 | 11 | * 0.6 : Added new IR sensor functions to improve efficiency (see manual for details) |
jah128 | 9:7a4fc1d7e484 | 12 | * 0.5 : Initial release |
jah128 | 0:9ffe8ebd1c40 | 13 | * |
jah128 | 0:9ffe8ebd1c40 | 14 | * Designed for use with the Pi Swarm Board (enhanced MBED sensor board) v1.2 |
jah128 | 0:9ffe8ebd1c40 | 15 | * |
jah128 | 4:52b3e4c5a425 | 16 | ******************************************************************************************/ |
jah128 | 0:9ffe8ebd1c40 | 17 | |
jah128 | 11:5ebcb52726cf | 18 | #include "piswarm.h" |
jah128 | 0:9ffe8ebd1c40 | 19 | |
jah128 | 0:9ffe8ebd1c40 | 20 | // Variables |
jah128 | 0:9ffe8ebd1c40 | 21 | DigitalOut gpio_led (LED1); |
jah128 | 0:9ffe8ebd1c40 | 22 | DigitalOut calibrate_led (LED2); |
jah128 | 11:5ebcb52726cf | 23 | DigitalOut beacon_led (LED3); |
jah128 | 0:9ffe8ebd1c40 | 24 | DigitalOut interrupt_led (LED4); |
jah128 | 11:5ebcb52726cf | 25 | InterruptIn expansion_interrupt (p29); |
jah128 | 0:9ffe8ebd1c40 | 26 | Timeout debounce_timeout; |
jah128 | 0:9ffe8ebd1c40 | 27 | Timeout led_timeout; |
jah128 | 11:5ebcb52726cf | 28 | Timeout motor_timeout; |
jah128 | 0:9ffe8ebd1c40 | 29 | Ticker calibrate_ticker; |
jah128 | 11:5ebcb52726cf | 30 | Ticker oled_ticker; |
jah128 | 11:5ebcb52726cf | 31 | Ticker beacon_ticker; |
jah128 | 11:5ebcb52726cf | 32 | |
jah128 | 0:9ffe8ebd1c40 | 33 | char id; |
jah128 | 0:9ffe8ebd1c40 | 34 | |
jah128 | 11:5ebcb52726cf | 35 | float bearing = 0; |
jah128 | 11:5ebcb52726cf | 36 | char is_synced = 0; |
jah128 | 11:5ebcb52726cf | 37 | float beacon_confidence = 0; |
jah128 | 11:5ebcb52726cf | 38 | float bearing_confidence = 0; |
jah128 | 11:5ebcb52726cf | 39 | char saved_peak = 0; |
jah128 | 11:5ebcb52726cf | 40 | char miss_sync_count = 0; |
jah128 | 11:5ebcb52726cf | 41 | char hit_sync_count = 0; |
jah128 | 0:9ffe8ebd1c40 | 42 | char oled_array [10]; |
jah128 | 11:5ebcb52726cf | 43 | char saved_oled_array [10]; |
jah128 | 11:5ebcb52726cf | 44 | char saved_oled_red; |
jah128 | 11:5ebcb52726cf | 45 | char saved_oled_green; |
jah128 | 11:5ebcb52726cf | 46 | char saved_oled_blue; |
jah128 | 0:9ffe8ebd1c40 | 47 | char enable_ir_ldo = 0; |
jah128 | 0:9ffe8ebd1c40 | 48 | char enable_led_ldo = 0; |
jah128 | 0:9ffe8ebd1c40 | 49 | char calibrate_led_state = 1; |
jah128 | 0:9ffe8ebd1c40 | 50 | char switches = 0; |
jah128 | 0:9ffe8ebd1c40 | 51 | char cled_red = 0; |
jah128 | 0:9ffe8ebd1c40 | 52 | char cled_green = 0; |
jah128 | 0:9ffe8ebd1c40 | 53 | char cled_blue = 0; |
jah128 | 0:9ffe8ebd1c40 | 54 | char oled_red = 0; |
jah128 | 0:9ffe8ebd1c40 | 55 | char oled_green = 0; |
jah128 | 0:9ffe8ebd1c40 | 56 | char oled_blue = 0; |
jah128 | 0:9ffe8ebd1c40 | 57 | char cled_enable = 0; |
jah128 | 11:5ebcb52726cf | 58 | char oled_ticker_step = 0; |
jah128 | 11:5ebcb52726cf | 59 | char bearing_strobe_step = 0; |
jah128 | 0:9ffe8ebd1c40 | 60 | char cled_brightness = CENTER_LED_BRIGHTNESS; // The default brightness value: must be between 0 (dimmest) and 100 (full brightness) |
jah128 | 0:9ffe8ebd1c40 | 61 | char oled_brightness = OUTER_LED_BRIGHTNESS; // The default brightness value: must be between 0 (dimmest) and 100 (full brightness) |
jah128 | 0:9ffe8ebd1c40 | 62 | float gyro_cal = 3.3; |
jah128 | 0:9ffe8ebd1c40 | 63 | float gyro_error = 0; |
jah128 | 0:9ffe8ebd1c40 | 64 | float acc_x_cal = 3350; |
jah128 | 0:9ffe8ebd1c40 | 65 | float acc_y_cal = 3350; |
jah128 | 0:9ffe8ebd1c40 | 66 | float acc_z_cal = 3350; |
jah128 | 0:9ffe8ebd1c40 | 67 | float left_speed = 0; |
jah128 | 0:9ffe8ebd1c40 | 68 | float right_speed = 0; |
jah128 | 0:9ffe8ebd1c40 | 69 | signed short mag_values [3]; |
jah128 | 9:7a4fc1d7e484 | 70 | unsigned short background_ir_values [8]; |
jah128 | 9:7a4fc1d7e484 | 71 | unsigned short illuminated_ir_values [8]; |
jah128 | 9:7a4fc1d7e484 | 72 | float reflected_ir_distances [8]; |
jah128 | 9:7a4fc1d7e484 | 73 | char ir_values_stored = 0; |
jah128 | 0:9ffe8ebd1c40 | 74 | |
jah128 | 0:9ffe8ebd1c40 | 75 | |
jah128 | 11:5ebcb52726cf | 76 | 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 | 11:5ebcb52726cf | 77 | { |
jah128 | 0:9ffe8ebd1c40 | 78 | setup(); |
jah128 | 0:9ffe8ebd1c40 | 79 | reset(); |
jah128 | 0:9ffe8ebd1c40 | 80 | } |
jah128 | 0:9ffe8ebd1c40 | 81 | |
jah128 | 1:b067a08ff54e | 82 | ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// |
jah128 | 1:b067a08ff54e | 83 | // Outer LED Functions |
jah128 | 1:b067a08ff54e | 84 | ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// |
jah128 | 1:b067a08ff54e | 85 | |
jah128 | 11:5ebcb52726cf | 86 | // 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 | 11:5ebcb52726cf | 87 | int PiSwarm::get_oled_colour() |
jah128 | 11:5ebcb52726cf | 88 | { |
jah128 | 0:9ffe8ebd1c40 | 89 | return (oled_red << 16) + (oled_green << 8) + oled_blue; |
jah128 | 0:9ffe8ebd1c40 | 90 | } |
jah128 | 0:9ffe8ebd1c40 | 91 | |
jah128 | 1:b067a08ff54e | 92 | // Returns the current enable state an individual LED. oled = LED to return enable state. Returns 0 for disabled, 1 for enabled |
jah128 | 11:5ebcb52726cf | 93 | char PiSwarm::get_oled_state(char oled) |
jah128 | 11:5ebcb52726cf | 94 | { |
jah128 | 1:b067a08ff54e | 95 | if(oled_array[oled] == 1) return 1; |
jah128 | 1:b067a08ff54e | 96 | return 0; |
jah128 | 0:9ffe8ebd1c40 | 97 | } |
jah128 | 0:9ffe8ebd1c40 | 98 | |
jah128 | 1:b067a08ff54e | 99 | // Set the colour of the outer LED. Values for red, green and blue range from 0 (off) to 255 (maximum). |
jah128 | 11:5ebcb52726cf | 100 | void PiSwarm::set_oled_colour( char red, char green, char blue ) |
jah128 | 11:5ebcb52726cf | 101 | { |
jah128 | 1:b067a08ff54e | 102 | oled_red = red; |
jah128 | 1:b067a08ff54e | 103 | oled_green = green; |
jah128 | 1:b067a08ff54e | 104 | oled_blue = blue; |
jah128 | 1:b067a08ff54e | 105 | _oled_r.pulsewidth_us(red); |
jah128 | 1:b067a08ff54e | 106 | _oled_g.pulsewidth_us(green); |
jah128 | 1:b067a08ff54e | 107 | _oled_b.pulsewidth_us(blue); |
jah128 | 1:b067a08ff54e | 108 | } |
jah128 | 1:b067a08ff54e | 109 | |
jah128 | 1:b067a08ff54e | 110 | // 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 | 11:5ebcb52726cf | 111 | void PiSwarm::set_oled(char oled, char value) |
jah128 | 11:5ebcb52726cf | 112 | { |
jah128 | 0:9ffe8ebd1c40 | 113 | oled_array[oled]=value; |
jah128 | 0:9ffe8ebd1c40 | 114 | } |
jah128 | 0:9ffe8ebd1c40 | 115 | |
jah128 | 11:5ebcb52726cf | 116 | // Save the current state and colour of OLEDs |
jah128 | 11:5ebcb52726cf | 117 | void PiSwarm::save_oled_state() |
jah128 | 11:5ebcb52726cf | 118 | { |
jah128 | 11:5ebcb52726cf | 119 | for(int i=0; i<10; i++) { |
jah128 | 11:5ebcb52726cf | 120 | saved_oled_array[i]=oled_array[i]; |
jah128 | 11:5ebcb52726cf | 121 | } |
jah128 | 11:5ebcb52726cf | 122 | saved_oled_red=oled_red; |
jah128 | 11:5ebcb52726cf | 123 | saved_oled_green=oled_green; |
jah128 | 11:5ebcb52726cf | 124 | saved_oled_blue=oled_blue; |
jah128 | 11:5ebcb52726cf | 125 | } |
jah128 | 11:5ebcb52726cf | 126 | |
jah128 | 11:5ebcb52726cf | 127 | // Restore the saved colour and state of OLEDs |
jah128 | 11:5ebcb52726cf | 128 | void PiSwarm::restore_oled_state() |
jah128 | 11:5ebcb52726cf | 129 | { |
jah128 | 11:5ebcb52726cf | 130 | for(int i=0; i<10; i++) { |
jah128 | 11:5ebcb52726cf | 131 | oled_array[i]=saved_oled_array[i]; |
jah128 | 11:5ebcb52726cf | 132 | } |
jah128 | 11:5ebcb52726cf | 133 | set_oled_colour(saved_oled_red,saved_oled_green,saved_oled_blue); |
jah128 | 11:5ebcb52726cf | 134 | activate_oleds(); |
jah128 | 11:5ebcb52726cf | 135 | } |
jah128 | 11:5ebcb52726cf | 136 | |
jah128 | 1:b067a08ff54e | 137 | // Sets the state of all 10 LEDs. oled_x = 0 for disable, 1 for enable. Use to change all 10 LEDs at once |
jah128 | 11:5ebcb52726cf | 138 | 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 | 11:5ebcb52726cf | 139 | { |
jah128 | 0:9ffe8ebd1c40 | 140 | oled_array[0]=oled_0; |
jah128 | 0:9ffe8ebd1c40 | 141 | oled_array[1]=oled_1; |
jah128 | 0:9ffe8ebd1c40 | 142 | oled_array[2]=oled_2; |
jah128 | 0:9ffe8ebd1c40 | 143 | oled_array[3]=oled_3; |
jah128 | 0:9ffe8ebd1c40 | 144 | oled_array[4]=oled_4; |
jah128 | 0:9ffe8ebd1c40 | 145 | oled_array[5]=oled_5; |
jah128 | 0:9ffe8ebd1c40 | 146 | oled_array[6]=oled_6; |
jah128 | 0:9ffe8ebd1c40 | 147 | oled_array[7]=oled_7; |
jah128 | 0:9ffe8ebd1c40 | 148 | oled_array[8]=oled_8; |
jah128 | 0:9ffe8ebd1c40 | 149 | oled_array[9]=oled_9; |
jah128 | 0:9ffe8ebd1c40 | 150 | activate_oleds(); |
jah128 | 0:9ffe8ebd1c40 | 151 | } |
jah128 | 0:9ffe8ebd1c40 | 152 | |
jah128 | 1:b067a08ff54e | 153 | // Sets all outer LEDs to disabled and turns off LED power supply. |
jah128 | 11:5ebcb52726cf | 154 | void PiSwarm::turn_off_all_oleds() |
jah128 | 11:5ebcb52726cf | 155 | { |
jah128 | 11:5ebcb52726cf | 156 | for(int i=0; i<10; i++) { |
jah128 | 0:9ffe8ebd1c40 | 157 | oled_array[i]=0; |
jah128 | 0:9ffe8ebd1c40 | 158 | } |
jah128 | 0:9ffe8ebd1c40 | 159 | activate_oleds(); |
jah128 | 0:9ffe8ebd1c40 | 160 | enable_led_ldo = 0; |
jah128 | 0:9ffe8ebd1c40 | 161 | enable_ldo_outputs(); |
jah128 | 0:9ffe8ebd1c40 | 162 | char data[3]; |
jah128 | 0:9ffe8ebd1c40 | 163 | data[0]=0x88; //Write to bank 1 then bank 2 |
jah128 | 0:9ffe8ebd1c40 | 164 | data[1]=0x00; //Reset everything in bank 1 |
jah128 | 0:9ffe8ebd1c40 | 165 | data[2]=0x00; //Reset everything in bank 2 |
jah128 | 11:5ebcb52726cf | 166 | _i2c.write(EXPANSION_IC_ADDRESS,data,3,false); |
jah128 | 0:9ffe8ebd1c40 | 167 | } |
jah128 | 0:9ffe8ebd1c40 | 168 | |
jah128 | 1:b067a08ff54e | 169 | // Set the outer LED brightness (total period of PWM output increases as brightness decreases). Ranges from 0 (minimum) to 255 (maximum) |
jah128 | 11:5ebcb52726cf | 170 | void PiSwarm::set_oled_brightness ( char brightness ) |
jah128 | 11:5ebcb52726cf | 171 | { |
jah128 | 1:b067a08ff54e | 172 | if ( brightness > 100 ) brightness = 100; |
jah128 | 1:b067a08ff54e | 173 | oled_brightness = brightness; |
jah128 | 1:b067a08ff54e | 174 | int oled_period = (104 - brightness); |
jah128 | 1:b067a08ff54e | 175 | oled_period *= oled_period; |
jah128 | 1:b067a08ff54e | 176 | oled_period /= 10; |
jah128 | 11:5ebcb52726cf | 177 | oled_period += 255; |
jah128 | 1:b067a08ff54e | 178 | if(CALIBRATE_COLOURS!=0)_oled_r.period_us(oled_period * 2); |
jah128 | 1:b067a08ff54e | 179 | else _oled_r.period_us(oled_period); |
jah128 | 1:b067a08ff54e | 180 | if(CALIBRATE_COLOURS!=0)_oled_g.period_us(oled_period * 2); |
jah128 | 1:b067a08ff54e | 181 | else _oled_g.period_us(oled_period); |
jah128 | 1:b067a08ff54e | 182 | _oled_b.period_us(oled_period); |
jah128 | 1:b067a08ff54e | 183 | } |
jah128 | 0:9ffe8ebd1c40 | 184 | |
jah128 | 0:9ffe8ebd1c40 | 185 | // Sends the messages to enable/disable outer LEDs |
jah128 | 11:5ebcb52726cf | 186 | void PiSwarm::activate_oleds() |
jah128 | 11:5ebcb52726cf | 187 | { |
jah128 | 11:5ebcb52726cf | 188 | if(enable_led_ldo == 0) { |
jah128 | 0:9ffe8ebd1c40 | 189 | enable_led_ldo = 1; |
jah128 | 0:9ffe8ebd1c40 | 190 | enable_ldo_outputs(); |
jah128 | 0:9ffe8ebd1c40 | 191 | } |
jah128 | 0:9ffe8ebd1c40 | 192 | char data[3]; |
jah128 | 0:9ffe8ebd1c40 | 193 | data[0]=0x88; //Write to bank 1 then bank 2 |
jah128 | 0:9ffe8ebd1c40 | 194 | data[1]=0x00; //Reset everything in bank 1 |
jah128 | 0:9ffe8ebd1c40 | 195 | data[2]=0x00; //Reset everything in bank 2 |
jah128 | 0:9ffe8ebd1c40 | 196 | if(oled_array[0]>0) data[1]+=1; |
jah128 | 0:9ffe8ebd1c40 | 197 | if(oled_array[1]>0) data[1]+=2; |
jah128 | 0:9ffe8ebd1c40 | 198 | if(oled_array[2]>0) data[1]+=4; |
jah128 | 0:9ffe8ebd1c40 | 199 | if(oled_array[3]>0) data[1]+=8; |
jah128 | 0:9ffe8ebd1c40 | 200 | if(oled_array[4]>0) data[1]+=16; |
jah128 | 0:9ffe8ebd1c40 | 201 | if(oled_array[5]>0) data[1]+=32; |
jah128 | 0:9ffe8ebd1c40 | 202 | if(oled_array[6]>0) data[1]+=64; |
jah128 | 0:9ffe8ebd1c40 | 203 | if(oled_array[7]>0) data[1]+=128; |
jah128 | 0:9ffe8ebd1c40 | 204 | if(oled_array[8]>0) data[2]+=1; |
jah128 | 0:9ffe8ebd1c40 | 205 | if(oled_array[9]>0) data[2]+=2; |
jah128 | 11:5ebcb52726cf | 206 | _i2c.write(EXPANSION_IC_ADDRESS,data,3,false); |
jah128 | 0:9ffe8ebd1c40 | 207 | } |
jah128 | 0:9ffe8ebd1c40 | 208 | |
jah128 | 1:b067a08ff54e | 209 | ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// |
jah128 | 1:b067a08ff54e | 210 | // Center LED Functions |
jah128 | 1:b067a08ff54e | 211 | ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// |
jah128 | 1:b067a08ff54e | 212 | |
jah128 | 1:b067a08ff54e | 213 | // 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 | 11:5ebcb52726cf | 214 | int PiSwarm::get_cled_colour() |
jah128 | 11:5ebcb52726cf | 215 | { |
jah128 | 1:b067a08ff54e | 216 | return (cled_red << 16) + (cled_green << 8) + cled_blue; |
jah128 | 0:9ffe8ebd1c40 | 217 | } |
jah128 | 0:9ffe8ebd1c40 | 218 | |
jah128 | 11:5ebcb52726cf | 219 | // Returns the enable state of the center LED |
jah128 | 11:5ebcb52726cf | 220 | char PiSwarm::get_cled_state( void ) |
jah128 | 11:5ebcb52726cf | 221 | { |
jah128 | 4:52b3e4c5a425 | 222 | return cled_enable; |
jah128 | 4:52b3e4c5a425 | 223 | } |
jah128 | 4:52b3e4c5a425 | 224 | |
jah128 | 1:b067a08ff54e | 225 | // Set the colour of the center LED. Values for red, green and blue range from 0 (off) to 255 (maximum). |
jah128 | 11:5ebcb52726cf | 226 | void PiSwarm::set_cled_colour( char red, char green, char blue ) |
jah128 | 11:5ebcb52726cf | 227 | { |
jah128 | 0:9ffe8ebd1c40 | 228 | cled_red = red; |
jah128 | 0:9ffe8ebd1c40 | 229 | cled_green = green; |
jah128 | 0:9ffe8ebd1c40 | 230 | cled_blue = blue; |
jah128 | 0:9ffe8ebd1c40 | 231 | if(cled_enable != 0) enable_cled(1); |
jah128 | 0:9ffe8ebd1c40 | 232 | } |
jah128 | 0:9ffe8ebd1c40 | 233 | |
jah128 | 1:b067a08ff54e | 234 | // Turn on or off the center LED. enable=1 to turn on, 0 to turn off |
jah128 | 11:5ebcb52726cf | 235 | void PiSwarm::enable_cled( char enable ) |
jah128 | 11:5ebcb52726cf | 236 | { |
jah128 | 0:9ffe8ebd1c40 | 237 | cled_enable = enable; |
jah128 | 0:9ffe8ebd1c40 | 238 | if(enable != 0) { |
jah128 | 0:9ffe8ebd1c40 | 239 | _cled_r.pulsewidth_us(cled_red); |
jah128 | 0:9ffe8ebd1c40 | 240 | _cled_g.pulsewidth_us(cled_green); |
jah128 | 0:9ffe8ebd1c40 | 241 | _cled_b.pulsewidth_us(cled_blue); |
jah128 | 11:5ebcb52726cf | 242 | } else { |
jah128 | 0:9ffe8ebd1c40 | 243 | _cled_r.pulsewidth_us(0); |
jah128 | 0:9ffe8ebd1c40 | 244 | _cled_g.pulsewidth_us(0); |
jah128 | 0:9ffe8ebd1c40 | 245 | _cled_b.pulsewidth_us(0); |
jah128 | 0:9ffe8ebd1c40 | 246 | } |
jah128 | 0:9ffe8ebd1c40 | 247 | } |
jah128 | 0:9ffe8ebd1c40 | 248 | |
jah128 | 1:b067a08ff54e | 249 | // Set the center LED brightness (total period of PWM output increases as brightness decreases). Ranges from 0 (minimum) to 255 (maximum) |
jah128 | 11:5ebcb52726cf | 250 | void PiSwarm::set_cled_brightness ( char brightness ) |
jah128 | 11:5ebcb52726cf | 251 | { |
jah128 | 1:b067a08ff54e | 252 | if( brightness > 100 ) brightness = 100; |
jah128 | 4:52b3e4c5a425 | 253 | // Brightness is set by adjusting period of PWM. Period ranged from ~260uS at 100% to 1336uS at 0% |
jah128 | 1:b067a08ff54e | 254 | // When calibrate_colours = 1, red = 2x normal, green = 2x normal |
jah128 | 1:b067a08ff54e | 255 | cled_brightness = brightness; |
jah128 | 1:b067a08ff54e | 256 | int cled_period = (104 - brightness); |
jah128 | 1:b067a08ff54e | 257 | cled_period *= cled_period; |
jah128 | 1:b067a08ff54e | 258 | cled_period /= 10; |
jah128 | 1:b067a08ff54e | 259 | cled_period += 255; |
jah128 | 11:5ebcb52726cf | 260 | if(CALIBRATE_COLOURS!=0)_cled_r.period_us(cled_period * 2); |
jah128 | 1:b067a08ff54e | 261 | else _cled_r.period_us(cled_period); |
jah128 | 11:5ebcb52726cf | 262 | if(CALIBRATE_COLOURS!=0)_cled_g.period_us(cled_period * 2); |
jah128 | 1:b067a08ff54e | 263 | else _cled_g.period_us(cled_period); |
jah128 | 1:b067a08ff54e | 264 | _cled_b.period_us(cled_period); |
jah128 | 0:9ffe8ebd1c40 | 265 | } |
jah128 | 0:9ffe8ebd1c40 | 266 | |
jah128 | 1:b067a08ff54e | 267 | ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// |
jah128 | 1:b067a08ff54e | 268 | // IR Sensor Functions |
jah128 | 1:b067a08ff54e | 269 | ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// |
jah128 | 0:9ffe8ebd1c40 | 270 | |
jah128 | 11:5ebcb52726cf | 271 | // Estimates the distance to an obstacle from one of the IR sensors, defined by index (range 0-7). |
jah128 | 9:7a4fc1d7e484 | 272 | // The value is converted to an approximate distance in millimetres, or 100.0 if no obstacle found. |
jah128 | 9:7a4fc1d7e484 | 273 | // NB This function is preserved for code-compatability and cases where only a single reading is needed |
jah128 | 9:7a4fc1d7e484 | 274 | // In many cases it is preferable to call store_reflected_ir_distances() to save all 8 values then read using get_reflected_ir_distance() |
jah128 | 11:5ebcb52726cf | 275 | float PiSwarm::read_reflected_ir_distance ( char index ) |
jah128 | 11:5ebcb52726cf | 276 | { |
jah128 | 9:7a4fc1d7e484 | 277 | // Sanity check |
jah128 | 9:7a4fc1d7e484 | 278 | if(index>7) return 0.0; |
jah128 | 11:5ebcb52726cf | 279 | |
jah128 | 0:9ffe8ebd1c40 | 280 | //1. Check that the IR LDO regulator is on, enable if it isn't |
jah128 | 11:5ebcb52726cf | 281 | if(enable_ir_ldo == 0) { |
jah128 | 0:9ffe8ebd1c40 | 282 | enable_ir_ldo = 1; |
jah128 | 0:9ffe8ebd1c40 | 283 | enable_ldo_outputs(); |
jah128 | 0:9ffe8ebd1c40 | 284 | } |
jah128 | 9:7a4fc1d7e484 | 285 | //2. Read the ADC value without IR emitter on; store in the background_ir_values[] array |
jah128 | 9:7a4fc1d7e484 | 286 | background_ir_values [index] = read_adc_value ( index ); |
jah128 | 11:5ebcb52726cf | 287 | |
jah128 | 0:9ffe8ebd1c40 | 288 | //3. Enable the relevant IR emitter by turning on its pulse output |
jah128 | 11:5ebcb52726cf | 289 | switch(index) { |
jah128 | 11:5ebcb52726cf | 290 | case 0: |
jah128 | 11:5ebcb52726cf | 291 | case 1: |
jah128 | 11:5ebcb52726cf | 292 | case 6: |
jah128 | 11:5ebcb52726cf | 293 | case 7: |
jah128 | 11:5ebcb52726cf | 294 | _irpulse_1 = 1; |
jah128 | 11:5ebcb52726cf | 295 | break; |
jah128 | 11:5ebcb52726cf | 296 | case 2: |
jah128 | 11:5ebcb52726cf | 297 | case 3: |
jah128 | 11:5ebcb52726cf | 298 | case 4: |
jah128 | 11:5ebcb52726cf | 299 | case 5: |
jah128 | 11:5ebcb52726cf | 300 | _irpulse_2 = 1; |
jah128 | 11:5ebcb52726cf | 301 | break; |
jah128 | 0:9ffe8ebd1c40 | 302 | } |
jah128 | 11:5ebcb52726cf | 303 | wait_us(500); |
jah128 | 11:5ebcb52726cf | 304 | |
jah128 | 9:7a4fc1d7e484 | 305 | //4. Read the ADC value now IR emitter is on; store in the illuminated_ir_values[] array |
jah128 | 9:7a4fc1d7e484 | 306 | illuminated_ir_values [index] = read_adc_value ( index ); |
jah128 | 11:5ebcb52726cf | 307 | |
jah128 | 0:9ffe8ebd1c40 | 308 | //5. Turn off IR emitter |
jah128 | 11:5ebcb52726cf | 309 | switch(index) { |
jah128 | 11:5ebcb52726cf | 310 | case 0: |
jah128 | 11:5ebcb52726cf | 311 | case 1: |
jah128 | 11:5ebcb52726cf | 312 | case 6: |
jah128 | 11:5ebcb52726cf | 313 | case 7: |
jah128 | 11:5ebcb52726cf | 314 | _irpulse_1 = 0; |
jah128 | 11:5ebcb52726cf | 315 | break; |
jah128 | 11:5ebcb52726cf | 316 | case 2: |
jah128 | 11:5ebcb52726cf | 317 | case 3: |
jah128 | 11:5ebcb52726cf | 318 | case 4: |
jah128 | 11:5ebcb52726cf | 319 | case 5: |
jah128 | 11:5ebcb52726cf | 320 | _irpulse_2 = 0; |
jah128 | 11:5ebcb52726cf | 321 | break; |
jah128 | 0:9ffe8ebd1c40 | 322 | } |
jah128 | 11:5ebcb52726cf | 323 | |
jah128 | 9:7a4fc1d7e484 | 324 | //6. Estimate distance based on 2 values using calculate_reflected_distances(); store in reflected_ir_distances() |
jah128 | 9:7a4fc1d7e484 | 325 | reflected_ir_distances [index] = calculate_reflected_distance( background_ir_values [index], illuminated_ir_values [index]); |
jah128 | 9:7a4fc1d7e484 | 326 | ir_values_stored = 1; |
jah128 | 9:7a4fc1d7e484 | 327 | return reflected_ir_distances [index]; |
jah128 | 9:7a4fc1d7e484 | 328 | } |
jah128 | 9:7a4fc1d7e484 | 329 | |
jah128 | 9:7a4fc1d7e484 | 330 | |
jah128 | 9:7a4fc1d7e484 | 331 | // Returns the stored value of the reflected obstacle based on last call of either read_reflected_ir_distance or store_reflected_distances |
jah128 | 9:7a4fc1d7e484 | 332 | // Introduced in API 0.6 |
jah128 | 11:5ebcb52726cf | 333 | float PiSwarm::get_reflected_ir_distance ( char index ) |
jah128 | 11:5ebcb52726cf | 334 | { |
jah128 | 9:7a4fc1d7e484 | 335 | // Sanity check: check range of index and that values have been stored |
jah128 | 9:7a4fc1d7e484 | 336 | if (index>7 || ir_values_stored == 0) return 0.0; |
jah128 | 11:5ebcb52726cf | 337 | return reflected_ir_distances[index]; |
jah128 | 9:7a4fc1d7e484 | 338 | } |
jah128 | 9:7a4fc1d7e484 | 339 | |
jah128 | 9:7a4fc1d7e484 | 340 | // Returns the stored value of the non-illuminated sensor based on last call of store_background_raw_ir_values |
jah128 | 9:7a4fc1d7e484 | 341 | // Introduced in API 0.6 |
jah128 | 11:5ebcb52726cf | 342 | unsigned short PiSwarm::get_background_raw_ir_value ( char index ) |
jah128 | 11:5ebcb52726cf | 343 | { |
jah128 | 9:7a4fc1d7e484 | 344 | // Sanity check: check range of index and that values have been stored |
jah128 | 9:7a4fc1d7e484 | 345 | if (index>7 || ir_values_stored == 0) return 0.0; |
jah128 | 11:5ebcb52726cf | 346 | return background_ir_values[index]; |
jah128 | 9:7a4fc1d7e484 | 347 | } |
jah128 | 11:5ebcb52726cf | 348 | |
jah128 | 9:7a4fc1d7e484 | 349 | // Returns the stored value of the illuminated sensor based on last call of store_illuminated_raw_ir_values |
jah128 | 9:7a4fc1d7e484 | 350 | // Introduced in API 0.6 |
jah128 | 11:5ebcb52726cf | 351 | unsigned short PiSwarm::get_illuminated_raw_ir_value ( char index ) |
jah128 | 11:5ebcb52726cf | 352 | { |
jah128 | 9:7a4fc1d7e484 | 353 | // Sanity check: check range of index and that values have been stored |
jah128 | 9:7a4fc1d7e484 | 354 | if (index>7 || ir_values_stored == 0) return 0.0; |
jah128 | 11:5ebcb52726cf | 355 | return illuminated_ir_values[index]; |
jah128 | 9:7a4fc1d7e484 | 356 | } |
jah128 | 11:5ebcb52726cf | 357 | |
jah128 | 9:7a4fc1d7e484 | 358 | // Stores the reflected distances for all 8 IR sensors |
jah128 | 9:7a4fc1d7e484 | 359 | // Introduced in API 0.6 |
jah128 | 11:5ebcb52726cf | 360 | void PiSwarm::store_reflected_ir_distances ( void ) |
jah128 | 11:5ebcb52726cf | 361 | { |
jah128 | 9:7a4fc1d7e484 | 362 | store_background_raw_ir_values(); |
jah128 | 9:7a4fc1d7e484 | 363 | store_illuminated_raw_ir_values(); |
jah128 | 11:5ebcb52726cf | 364 | for(int i=0; i<8; i++) { |
jah128 | 11:5ebcb52726cf | 365 | reflected_ir_distances [i] = calculate_reflected_distance( background_ir_values [i], illuminated_ir_values [i]); |
jah128 | 11:5ebcb52726cf | 366 | } |
jah128 | 9:7a4fc1d7e484 | 367 | } |
jah128 | 11:5ebcb52726cf | 368 | |
jah128 | 9:7a4fc1d7e484 | 369 | // Stores the raw ADC values for all 8 IR sensors without enabling IR emitters |
jah128 | 9:7a4fc1d7e484 | 370 | // Introduced in API 0.6 |
jah128 | 11:5ebcb52726cf | 371 | void PiSwarm::store_background_raw_ir_values ( void ) |
jah128 | 11:5ebcb52726cf | 372 | { |
jah128 | 9:7a4fc1d7e484 | 373 | ir_values_stored = 1; |
jah128 | 11:5ebcb52726cf | 374 | for(int i=0; i<8; i++) { |
jah128 | 9:7a4fc1d7e484 | 375 | background_ir_values [i] = read_adc_value(i); |
jah128 | 9:7a4fc1d7e484 | 376 | } |
jah128 | 9:7a4fc1d7e484 | 377 | } |
jah128 | 11:5ebcb52726cf | 378 | |
jah128 | 9:7a4fc1d7e484 | 379 | // Stores the raw ADC values for all 8 IR sensors with a 500us emitter pulse |
jah128 | 9:7a4fc1d7e484 | 380 | // Introduced in API 0.6 |
jah128 | 11:5ebcb52726cf | 381 | void PiSwarm::store_illuminated_raw_ir_values ( void ) |
jah128 | 11:5ebcb52726cf | 382 | { |
jah128 | 9:7a4fc1d7e484 | 383 | //1. Check that the IR LDO regulator is on, enable if it isn't |
jah128 | 11:5ebcb52726cf | 384 | if(enable_ir_ldo == 0) { |
jah128 | 9:7a4fc1d7e484 | 385 | enable_ir_ldo = 1; |
jah128 | 9:7a4fc1d7e484 | 386 | enable_ldo_outputs(); |
jah128 | 9:7a4fc1d7e484 | 387 | } |
jah128 | 11:5ebcb52726cf | 388 | |
jah128 | 11:5ebcb52726cf | 389 | //2. Enable the front IR emitters and store the values |
jah128 | 9:7a4fc1d7e484 | 390 | _irpulse_1 = 1; |
jah128 | 11:5ebcb52726cf | 391 | wait_us(500); |
jah128 | 9:7a4fc1d7e484 | 392 | illuminated_ir_values [0] = read_adc_value(0); |
jah128 | 9:7a4fc1d7e484 | 393 | illuminated_ir_values [1] = read_adc_value(1); |
jah128 | 9:7a4fc1d7e484 | 394 | illuminated_ir_values [6] = read_adc_value(6); |
jah128 | 9:7a4fc1d7e484 | 395 | illuminated_ir_values [7] = read_adc_value(7); |
jah128 | 9:7a4fc1d7e484 | 396 | _irpulse_1 = 0; |
jah128 | 11:5ebcb52726cf | 397 | |
jah128 | 11:5ebcb52726cf | 398 | //3. Enable the rear+side IR emitters and store the values |
jah128 | 9:7a4fc1d7e484 | 399 | _irpulse_2 = 1; |
jah128 | 11:5ebcb52726cf | 400 | wait_us(500); |
jah128 | 9:7a4fc1d7e484 | 401 | illuminated_ir_values [2] = read_adc_value(2); |
jah128 | 9:7a4fc1d7e484 | 402 | illuminated_ir_values [3] = read_adc_value(3); |
jah128 | 9:7a4fc1d7e484 | 403 | illuminated_ir_values [4] = read_adc_value(4); |
jah128 | 9:7a4fc1d7e484 | 404 | illuminated_ir_values [5] = read_adc_value(5); |
jah128 | 9:7a4fc1d7e484 | 405 | _irpulse_2 = 0; |
jah128 | 9:7a4fc1d7e484 | 406 | } |
jah128 | 11:5ebcb52726cf | 407 | |
jah128 | 9:7a4fc1d7e484 | 408 | // Converts a background and illuminated value into a distance |
jah128 | 9:7a4fc1d7e484 | 409 | // Introduced in API 0.6 - used by read_reflected_ir_distance and store_reflected_ir_distances |
jah128 | 11:5ebcb52726cf | 410 | float PiSwarm::calculate_reflected_distance ( unsigned short background_value, unsigned short illuminated_value ) |
jah128 | 11:5ebcb52726cf | 411 | { |
jah128 | 9:7a4fc1d7e484 | 412 | float approximate_distance = 4000 + background_value - illuminated_value; |
jah128 | 9:7a4fc1d7e484 | 413 | if(approximate_distance < 0) approximate_distance = 0; |
jah128 | 11:5ebcb52726cf | 414 | |
jah128 | 0:9ffe8ebd1c40 | 415 | // Very approximate: root value, divide by 2, approx distance in mm |
jah128 | 9:7a4fc1d7e484 | 416 | approximate_distance = sqrt (approximate_distance) / 2.0; |
jah128 | 11:5ebcb52726cf | 417 | |
jah128 | 0:9ffe8ebd1c40 | 418 | // Then add adjustment value if value >27 |
jah128 | 9:7a4fc1d7e484 | 419 | if(approximate_distance > 27) { |
jah128 | 9:7a4fc1d7e484 | 420 | float shift = pow(approximate_distance - 27,3); |
jah128 | 9:7a4fc1d7e484 | 421 | approximate_distance += shift; |
jah128 | 0:9ffe8ebd1c40 | 422 | } |
jah128 | 9:7a4fc1d7e484 | 423 | if(approximate_distance > 90) approximate_distance = 100.0; |
jah128 | 11:5ebcb52726cf | 424 | return approximate_distance; |
jah128 | 0:9ffe8ebd1c40 | 425 | } |
jah128 | 0:9ffe8ebd1c40 | 426 | |
jah128 | 3:4c0f2f3de33e | 427 | // 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 | 11:5ebcb52726cf | 428 | unsigned short PiSwarm::read_illuminated_raw_ir_value ( char index ) |
jah128 | 11:5ebcb52726cf | 429 | { |
jah128 | 3:4c0f2f3de33e | 430 | //This function reads the IR strength when illuminated - used for PC system debugging purposes |
jah128 | 3:4c0f2f3de33e | 431 | //1. Check that the IR LDO regulator is on, enable if it isn't |
jah128 | 11:5ebcb52726cf | 432 | if(enable_ir_ldo == 0) { |
jah128 | 3:4c0f2f3de33e | 433 | enable_ir_ldo = 1; |
jah128 | 3:4c0f2f3de33e | 434 | enable_ldo_outputs(); |
jah128 | 3:4c0f2f3de33e | 435 | } |
jah128 | 3:4c0f2f3de33e | 436 | //2. Enable the relevant IR emitter by turning on its pulse output |
jah128 | 11:5ebcb52726cf | 437 | switch(index) { |
jah128 | 11:5ebcb52726cf | 438 | case 0: |
jah128 | 11:5ebcb52726cf | 439 | case 1: |
jah128 | 11:5ebcb52726cf | 440 | case 6: |
jah128 | 11:5ebcb52726cf | 441 | case 7: |
jah128 | 11:5ebcb52726cf | 442 | _irpulse_1 = 1; |
jah128 | 11:5ebcb52726cf | 443 | break; |
jah128 | 11:5ebcb52726cf | 444 | case 2: |
jah128 | 11:5ebcb52726cf | 445 | case 3: |
jah128 | 11:5ebcb52726cf | 446 | case 4: |
jah128 | 11:5ebcb52726cf | 447 | case 5: |
jah128 | 11:5ebcb52726cf | 448 | _irpulse_2 = 1; |
jah128 | 11:5ebcb52726cf | 449 | break; |
jah128 | 3:4c0f2f3de33e | 450 | } |
jah128 | 11:5ebcb52726cf | 451 | wait_us(500); |
jah128 | 3:4c0f2f3de33e | 452 | //3. Read the ADC value now IR emitter is on |
jah128 | 3:4c0f2f3de33e | 453 | unsigned short strong_value = read_adc_value ( index ); |
jah128 | 3:4c0f2f3de33e | 454 | //4. Turn off IR emitter |
jah128 | 11:5ebcb52726cf | 455 | switch(index) { |
jah128 | 11:5ebcb52726cf | 456 | case 0: |
jah128 | 11:5ebcb52726cf | 457 | case 1: |
jah128 | 11:5ebcb52726cf | 458 | case 6: |
jah128 | 11:5ebcb52726cf | 459 | case 7: |
jah128 | 11:5ebcb52726cf | 460 | _irpulse_1 = 0; |
jah128 | 11:5ebcb52726cf | 461 | break; |
jah128 | 11:5ebcb52726cf | 462 | case 2: |
jah128 | 11:5ebcb52726cf | 463 | case 3: |
jah128 | 11:5ebcb52726cf | 464 | case 4: |
jah128 | 11:5ebcb52726cf | 465 | case 5: |
jah128 | 11:5ebcb52726cf | 466 | _irpulse_2 = 0; |
jah128 | 11:5ebcb52726cf | 467 | break; |
jah128 | 3:4c0f2f3de33e | 468 | } |
jah128 | 3:4c0f2f3de33e | 469 | return strong_value; |
jah128 | 3:4c0f2f3de33e | 470 | } |
jah128 | 3:4c0f2f3de33e | 471 | |
jah128 | 1:b067a08ff54e | 472 | // Returns the raw sensor value for the IR sensor defined by index (range 0-7). |
jah128 | 11:5ebcb52726cf | 473 | unsigned short PiSwarm::read_adc_value ( char index ) |
jah128 | 11:5ebcb52726cf | 474 | { |
jah128 | 0:9ffe8ebd1c40 | 475 | short value = 0; |
jah128 | 0:9ffe8ebd1c40 | 476 | // Read a single value from the ADC |
jah128 | 11:5ebcb52726cf | 477 | if(index<8) { |
jah128 | 0:9ffe8ebd1c40 | 478 | char apb[1]; |
jah128 | 0:9ffe8ebd1c40 | 479 | char data[2]; |
jah128 | 11:5ebcb52726cf | 480 | switch(index) { |
jah128 | 11:5ebcb52726cf | 481 | case 0: |
jah128 | 11:5ebcb52726cf | 482 | apb[0]=0x80; |
jah128 | 11:5ebcb52726cf | 483 | break; |
jah128 | 11:5ebcb52726cf | 484 | case 1: |
jah128 | 11:5ebcb52726cf | 485 | apb[0]=0x90; |
jah128 | 11:5ebcb52726cf | 486 | break; |
jah128 | 11:5ebcb52726cf | 487 | case 2: |
jah128 | 11:5ebcb52726cf | 488 | apb[0]=0xA0; |
jah128 | 11:5ebcb52726cf | 489 | break; |
jah128 | 11:5ebcb52726cf | 490 | case 3: |
jah128 | 11:5ebcb52726cf | 491 | apb[0]=0xB0; |
jah128 | 11:5ebcb52726cf | 492 | break; |
jah128 | 11:5ebcb52726cf | 493 | case 4: |
jah128 | 11:5ebcb52726cf | 494 | apb[0]=0xC0; |
jah128 | 11:5ebcb52726cf | 495 | break; |
jah128 | 11:5ebcb52726cf | 496 | case 5: |
jah128 | 11:5ebcb52726cf | 497 | apb[0]=0xD0; |
jah128 | 11:5ebcb52726cf | 498 | break; |
jah128 | 11:5ebcb52726cf | 499 | case 6: |
jah128 | 11:5ebcb52726cf | 500 | apb[0]=0xE0; |
jah128 | 11:5ebcb52726cf | 501 | break; |
jah128 | 11:5ebcb52726cf | 502 | case 7: |
jah128 | 11:5ebcb52726cf | 503 | apb[0]=0xF0; |
jah128 | 11:5ebcb52726cf | 504 | break; |
jah128 | 0:9ffe8ebd1c40 | 505 | } |
jah128 | 0:9ffe8ebd1c40 | 506 | _i2c.write(ADC_ADDRESS,apb,1,false); |
jah128 | 0:9ffe8ebd1c40 | 507 | _i2c.read(ADC_ADDRESS,data,2,false); |
jah128 | 0:9ffe8ebd1c40 | 508 | value=((data[0] % 16)<<8)+data[1]; |
jah128 | 0:9ffe8ebd1c40 | 509 | if(value > 4096) value=4096; |
jah128 | 0:9ffe8ebd1c40 | 510 | value=4096-value; |
jah128 | 0:9ffe8ebd1c40 | 511 | } |
jah128 | 0:9ffe8ebd1c40 | 512 | return value; |
jah128 | 0:9ffe8ebd1c40 | 513 | } |
jah128 | 0:9ffe8ebd1c40 | 514 | |
jah128 | 1:b067a08ff54e | 515 | // Function enables or disables the LDO voltage regulators which supply power to the outer LEDs and the IR photo emitters. |
jah128 | 11:5ebcb52726cf | 516 | void PiSwarm::enable_ldo_outputs () |
jah128 | 11:5ebcb52726cf | 517 | { |
jah128 | 1:b067a08ff54e | 518 | char data[2]; |
jah128 | 1:b067a08ff54e | 519 | data[0] = 0x0A; //Address for PCA9505 Output Port 2 |
jah128 | 1:b067a08ff54e | 520 | data[1] = 0; |
jah128 | 1:b067a08ff54e | 521 | if(enable_led_ldo) data[1] +=128; |
jah128 | 1:b067a08ff54e | 522 | if(enable_ir_ldo) data[1] +=64; |
jah128 | 1:b067a08ff54e | 523 | _i2c.write(EXPANSION_IC_ADDRESS,data,2,false); |
jah128 | 0:9ffe8ebd1c40 | 524 | } |
jah128 | 0:9ffe8ebd1c40 | 525 | |
jah128 | 1:b067a08ff54e | 526 | ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// |
jah128 | 1:b067a08ff54e | 527 | // MEMS Sensor Functions |
jah128 | 1:b067a08ff54e | 528 | ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// |
jah128 | 0:9ffe8ebd1c40 | 529 | |
jah128 | 1:b067a08ff54e | 530 | // Returns the yaw (clockwise) rotation in degrees-per-second reported by the LY3200 gyroscope. |
jah128 | 11:5ebcb52726cf | 531 | float PiSwarm::read_gyro ( void ) |
jah128 | 11:5ebcb52726cf | 532 | { |
jah128 | 11:5ebcb52726cf | 533 | // Gyro is a ST Micro LY3200ALH Yaw-rate Gyro: 0.67mV / d.p.s, 1.5V offset |
jah128 | 0:9ffe8ebd1c40 | 534 | float r_gyro = _gyro.read(); |
jah128 | 0:9ffe8ebd1c40 | 535 | r_gyro *= gyro_cal; // NB 3.4 seemed to work better that 3.3 but better to run calibration routine |
jah128 | 0:9ffe8ebd1c40 | 536 | r_gyro = 1.5 - r_gyro; // 1.5V off-set, invert sign |
jah128 | 11:5ebcb52726cf | 537 | r_gyro *= 1492.5; // Convert to DPS (+ve = clockwise, -ve = c.clockwise) |
jah128 | 0:9ffe8ebd1c40 | 538 | return r_gyro; |
jah128 | 0:9ffe8ebd1c40 | 539 | } |
jah128 | 1:b067a08ff54e | 540 | |
jah128 | 11:5ebcb52726cf | 541 | // Returns the acceleration in the X plane reported by the LIS332 accelerometer. Returned value is in mg. |
jah128 | 11:5ebcb52726cf | 542 | float PiSwarm::read_accelerometer_x ( void ) |
jah128 | 11:5ebcb52726cf | 543 | { |
jah128 | 11:5ebcb52726cf | 544 | // LIS 332 AX Accelerometer - 0.363 V/g - 1.25V |
jah128 | 0:9ffe8ebd1c40 | 545 | float r_acc_x = (( _accel_x.read() * acc_x_cal ) - 1250.0 ) * 2.754; // Return value in mG |
jah128 | 0:9ffe8ebd1c40 | 546 | return r_acc_x; |
jah128 | 0:9ffe8ebd1c40 | 547 | } |
jah128 | 11:5ebcb52726cf | 548 | |
jah128 | 11:5ebcb52726cf | 549 | // Returns the acceleration in the Y plane reported by the LIS332 accelerometer. Returned value is in mg. |
jah128 | 11:5ebcb52726cf | 550 | float PiSwarm::read_accelerometer_y ( void ) |
jah128 | 11:5ebcb52726cf | 551 | { |
jah128 | 11:5ebcb52726cf | 552 | float r_acc_y = (( _accel_y.read() * acc_y_cal ) - 1250.0 ) * 2.754; // Return value in mG |
jah128 | 0:9ffe8ebd1c40 | 553 | return r_acc_y; |
jah128 | 0:9ffe8ebd1c40 | 554 | } |
jah128 | 11:5ebcb52726cf | 555 | |
jah128 | 11:5ebcb52726cf | 556 | // Returns the acceleration in the Z plane reported by the LIS332 accelerometer. Returned value is in mg. |
jah128 | 11:5ebcb52726cf | 557 | float PiSwarm::read_accelerometer_z ( void ) |
jah128 | 11:5ebcb52726cf | 558 | { |
jah128 | 0:9ffe8ebd1c40 | 559 | float r_acc_z = (( _accel_z.read() * acc_z_cal ) - 1250.0 ) * 2.754; // Return value in mG |
jah128 | 0:9ffe8ebd1c40 | 560 | return r_acc_z; |
jah128 | 0:9ffe8ebd1c40 | 561 | } |
jah128 | 0:9ffe8ebd1c40 | 562 | |
jah128 | 1:b067a08ff54e | 563 | // Sends message to the magnetometer to read new values. Should be called before get_magnetometer_* when updated values are required |
jah128 | 11:5ebcb52726cf | 564 | char PiSwarm::read_magnetometer ( void ) |
jah128 | 11:5ebcb52726cf | 565 | { |
jah128 | 0:9ffe8ebd1c40 | 566 | char read_data[7]; |
jah128 | 0:9ffe8ebd1c40 | 567 | char success; |
jah128 | 0:9ffe8ebd1c40 | 568 | char command_data[1]; |
jah128 | 0:9ffe8ebd1c40 | 569 | command_data[0] = 0x00; //Auto-read from register 0x00 (status) |
jah128 | 0:9ffe8ebd1c40 | 570 | success = _i2c.write(MAGNETOMETER_ADDRESS, command_data, 1, false); |
jah128 | 0:9ffe8ebd1c40 | 571 | _i2c.read(MAGNETOMETER_ADDRESS, read_data, 7, false); |
jah128 | 0:9ffe8ebd1c40 | 572 | mag_values[0] = (read_data[1] << 8) + read_data[2]; |
jah128 | 0:9ffe8ebd1c40 | 573 | mag_values[1] = (read_data[3] << 8) + read_data[4]; |
jah128 | 0:9ffe8ebd1c40 | 574 | mag_values[2] = (read_data[5] << 8) + read_data[6]; |
jah128 | 11:5ebcb52726cf | 575 | return success; |
jah128 | 0:9ffe8ebd1c40 | 576 | } |
jah128 | 11:5ebcb52726cf | 577 | |
jah128 | 11:5ebcb52726cf | 578 | // Returns the raw value for the X-plane magnetic field stored on the last call of read_magnetometer |
jah128 | 11:5ebcb52726cf | 579 | signed short PiSwarm::get_magnetometer_x ( void ) |
jah128 | 11:5ebcb52726cf | 580 | { |
jah128 | 0:9ffe8ebd1c40 | 581 | return mag_values[0]; |
jah128 | 0:9ffe8ebd1c40 | 582 | } |
jah128 | 11:5ebcb52726cf | 583 | |
jah128 | 11:5ebcb52726cf | 584 | // Returns the raw value for the Y-plane magnetic field stored on the last call of read_magnetometer |
jah128 | 11:5ebcb52726cf | 585 | signed short PiSwarm::get_magnetometer_y ( void ) |
jah128 | 11:5ebcb52726cf | 586 | { |
jah128 | 0:9ffe8ebd1c40 | 587 | return mag_values[1]; |
jah128 | 0:9ffe8ebd1c40 | 588 | } |
jah128 | 11:5ebcb52726cf | 589 | |
jah128 | 11:5ebcb52726cf | 590 | // Returns the raw value for the Z-plane magnetic field stored on the last call of read_magnetometer |
jah128 | 11:5ebcb52726cf | 591 | signed short PiSwarm::get_magnetometer_z ( void ) |
jah128 | 11:5ebcb52726cf | 592 | { |
jah128 | 0:9ffe8ebd1c40 | 593 | return mag_values[2]; |
jah128 | 0:9ffe8ebd1c40 | 594 | } |
jah128 | 0:9ffe8ebd1c40 | 595 | |
jah128 | 1:b067a08ff54e | 596 | ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// |
jah128 | 1:b067a08ff54e | 597 | // Other Sensor Functions |
jah128 | 1:b067a08ff54e | 598 | ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// |
jah128 | 1:b067a08ff54e | 599 | |
jah128 | 1:b067a08ff54e | 600 | // Returns the temperature reported by the MCP9701 sensor in degrees centigrade. |
jah128 | 11:5ebcb52726cf | 601 | float PiSwarm::read_temperature ( void ) |
jah128 | 11:5ebcb52726cf | 602 | { |
jah128 | 1:b067a08ff54e | 603 | // Temperature Sensor is a Microchip MCP9701T-E/LT: 19.5mV/C 0C = 400mV |
jah128 | 1:b067a08ff54e | 604 | float r_temp = _temperature.read(); |
jah128 | 1:b067a08ff54e | 605 | r_temp -= 0.1212f; // 0.4 / 3.3 |
jah128 | 1:b067a08ff54e | 606 | r_temp *= 169.231f; // 3.3 / .0195 |
jah128 | 1:b067a08ff54e | 607 | return r_temp; |
jah128 | 1:b067a08ff54e | 608 | } |
jah128 | 1:b067a08ff54e | 609 | |
jah128 | 1:b067a08ff54e | 610 | // Returns the adjusted value (0-100) for the ambient light sensor |
jah128 | 11:5ebcb52726cf | 611 | float PiSwarm::read_light_sensor ( void ) |
jah128 | 11:5ebcb52726cf | 612 | { |
jah128 | 1:b067a08ff54e | 613 | // Light sensor is a Avago APDS-9005 Ambient Light Sensor |
jah128 | 1:b067a08ff54e | 614 | //float r_light = (_light.read() * 141); //Gives a range of (approximately) 0=dark 100=max. light |
jah128 | 1:b067a08ff54e | 615 | 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 | 616 | if(r_light > 100) r_light = 100; |
jah128 | 1:b067a08ff54e | 617 | return r_light; |
jah128 | 1:b067a08ff54e | 618 | } |
jah128 | 1:b067a08ff54e | 619 | |
jah128 | 1:b067a08ff54e | 620 | |
jah128 | 11:5ebcb52726cf | 621 | void PiSwarm::read_raw_sensors ( int * raw_ls_array ) |
jah128 | 11:5ebcb52726cf | 622 | { |
jah128 | 1:b067a08ff54e | 623 | _ser.putc(SEND_RAW_SENSOR_VALUES); |
jah128 | 1:b067a08ff54e | 624 | for (int i = 0; i < 5 ; i ++) { |
jah128 | 1:b067a08ff54e | 625 | int val = _ser.getc(); |
jah128 | 1:b067a08ff54e | 626 | val += _ser.getc() << 8; |
jah128 | 1:b067a08ff54e | 627 | raw_ls_array [i] = val; |
jah128 | 1:b067a08ff54e | 628 | } |
jah128 | 1:b067a08ff54e | 629 | } |
jah128 | 1:b067a08ff54e | 630 | |
jah128 | 1:b067a08ff54e | 631 | // Returns the uptime of the system (since initialisation) in seconds |
jah128 | 11:5ebcb52726cf | 632 | float PiSwarm::get_uptime (void) |
jah128 | 11:5ebcb52726cf | 633 | { |
jah128 | 1:b067a08ff54e | 634 | return _system_timer.read(); |
jah128 | 1:b067a08ff54e | 635 | } |
jah128 | 1:b067a08ff54e | 636 | |
jah128 | 1:b067a08ff54e | 637 | // Returns the battery level in millivolts |
jah128 | 11:5ebcb52726cf | 638 | float PiSwarm::battery() |
jah128 | 11:5ebcb52726cf | 639 | { |
jah128 | 1:b067a08ff54e | 640 | _ser.putc(SEND_BATTERY_MILLIVOLTS); |
jah128 | 1:b067a08ff54e | 641 | char lowbyte = _ser.getc(); |
jah128 | 1:b067a08ff54e | 642 | char hibyte = _ser.getc(); |
jah128 | 1:b067a08ff54e | 643 | float v = ((lowbyte + (hibyte << 8))/1000.0); |
jah128 | 1:b067a08ff54e | 644 | return(v); |
jah128 | 1:b067a08ff54e | 645 | } |
jah128 | 1:b067a08ff54e | 646 | |
jah128 | 1:b067a08ff54e | 647 | // Returns the raw value for the variable resistor on the base of the platform. Ranges from 0 to 1024. |
jah128 | 11:5ebcb52726cf | 648 | float PiSwarm::pot_voltage(void) |
jah128 | 11:5ebcb52726cf | 649 | { |
jah128 | 1:b067a08ff54e | 650 | int volt = 0; |
jah128 | 1:b067a08ff54e | 651 | _ser.putc(SEND_TRIMPOT); |
jah128 | 1:b067a08ff54e | 652 | volt = _ser.getc(); |
jah128 | 1:b067a08ff54e | 653 | volt += _ser.getc() << 8; |
jah128 | 1:b067a08ff54e | 654 | return(volt); |
jah128 | 1:b067a08ff54e | 655 | } |
jah128 | 1:b067a08ff54e | 656 | |
jah128 | 1:b067a08ff54e | 657 | // Returns the ID of platform (set by the DIL switches above the display). Range 0-31 [0 reserved]. |
jah128 | 11:5ebcb52726cf | 658 | char PiSwarm::get_id () |
jah128 | 11:5ebcb52726cf | 659 | { |
jah128 | 1:b067a08ff54e | 660 | return id; |
jah128 | 1:b067a08ff54e | 661 | } |
jah128 | 1:b067a08ff54e | 662 | |
jah128 | 1:b067a08ff54e | 663 | // Return the current stored state for direction switch(es) pressed {1 = Center 2 = Right 4 = Left 8 = Down 16 = Up} |
jah128 | 11:5ebcb52726cf | 664 | char PiSwarm::get_switches () |
jah128 | 11:5ebcb52726cf | 665 | { |
jah128 | 1:b067a08ff54e | 666 | return switches; |
jah128 | 1:b067a08ff54e | 667 | } |
jah128 | 1:b067a08ff54e | 668 | |
jah128 | 1:b067a08ff54e | 669 | |
jah128 | 1:b067a08ff54e | 670 | ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// |
jah128 | 1:b067a08ff54e | 671 | // Motor Functions |
jah128 | 1:b067a08ff54e | 672 | ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// |
jah128 | 1:b067a08ff54e | 673 | |
jah128 | 1:b067a08ff54e | 674 | // Returns the target speed of the left motor (-1.0 to 1.0) |
jah128 | 11:5ebcb52726cf | 675 | float PiSwarm::get_left_motor () |
jah128 | 11:5ebcb52726cf | 676 | { |
jah128 | 1:b067a08ff54e | 677 | return left_speed; |
jah128 | 1:b067a08ff54e | 678 | } |
jah128 | 1:b067a08ff54e | 679 | |
jah128 | 1:b067a08ff54e | 680 | // Returns the target speed of the right motor (-1.0 to 1.0) |
jah128 | 11:5ebcb52726cf | 681 | float PiSwarm::get_right_motor () |
jah128 | 11:5ebcb52726cf | 682 | { |
jah128 | 1:b067a08ff54e | 683 | return right_speed; |
jah128 | 1:b067a08ff54e | 684 | } |
jah128 | 1:b067a08ff54e | 685 | |
jah128 | 1:b067a08ff54e | 686 | // Sets the speed of the left motor (-1.0 to 1.0) |
jah128 | 11:5ebcb52726cf | 687 | void PiSwarm::left_motor (float speed) |
jah128 | 11:5ebcb52726cf | 688 | { |
jah128 | 1:b067a08ff54e | 689 | motor(0,speed); |
jah128 | 1:b067a08ff54e | 690 | } |
jah128 | 1:b067a08ff54e | 691 | |
jah128 | 1:b067a08ff54e | 692 | // Sets the speed of the right motor (-1.0 to 1.0) |
jah128 | 11:5ebcb52726cf | 693 | void PiSwarm::right_motor (float speed) |
jah128 | 11:5ebcb52726cf | 694 | { |
jah128 | 1:b067a08ff54e | 695 | motor(1,speed); |
jah128 | 1:b067a08ff54e | 696 | } |
jah128 | 1:b067a08ff54e | 697 | |
jah128 | 1:b067a08ff54e | 698 | // Drive forward at the given speed (-1.0 to 1.0) |
jah128 | 11:5ebcb52726cf | 699 | void PiSwarm::forward (float speed) |
jah128 | 11:5ebcb52726cf | 700 | { |
jah128 | 1:b067a08ff54e | 701 | motor(0,speed); |
jah128 | 1:b067a08ff54e | 702 | motor(1,speed); |
jah128 | 1:b067a08ff54e | 703 | } |
jah128 | 1:b067a08ff54e | 704 | |
jah128 | 1:b067a08ff54e | 705 | // Drive backward at the given speed (-1.0 to 1.0) |
jah128 | 11:5ebcb52726cf | 706 | void PiSwarm::backward (float speed) |
jah128 | 11:5ebcb52726cf | 707 | { |
jah128 | 1:b067a08ff54e | 708 | motor(0,-1.0*speed); |
jah128 | 1:b067a08ff54e | 709 | motor(1,-1.0*speed); |
jah128 | 1:b067a08ff54e | 710 | } |
jah128 | 1:b067a08ff54e | 711 | |
jah128 | 1:b067a08ff54e | 712 | // Turn on-the-spot left at the given speed (-1.0 to 1.0) |
jah128 | 11:5ebcb52726cf | 713 | void PiSwarm::left (float speed) |
jah128 | 11:5ebcb52726cf | 714 | { |
jah128 | 1:b067a08ff54e | 715 | motor(0,speed); |
jah128 | 1:b067a08ff54e | 716 | motor(1,-1.0*speed); |
jah128 | 1:b067a08ff54e | 717 | } |
jah128 | 1:b067a08ff54e | 718 | |
jah128 | 1:b067a08ff54e | 719 | // Turn on-the-spot right at the given speed (-1.0 to 1.0) |
jah128 | 11:5ebcb52726cf | 720 | void PiSwarm::right (float speed) |
jah128 | 11:5ebcb52726cf | 721 | { |
jah128 | 1:b067a08ff54e | 722 | motor(0,-1.0*speed); |
jah128 | 1:b067a08ff54e | 723 | motor(1,speed); |
jah128 | 1:b067a08ff54e | 724 | } |
jah128 | 1:b067a08ff54e | 725 | |
jah128 | 1:b067a08ff54e | 726 | // Stop both motors |
jah128 | 11:5ebcb52726cf | 727 | void PiSwarm::stop (void) |
jah128 | 11:5ebcb52726cf | 728 | { |
jah128 | 1:b067a08ff54e | 729 | motor(0,0.0); |
jah128 | 1:b067a08ff54e | 730 | motor(1,0.0); |
jah128 | 1:b067a08ff54e | 731 | } |
jah128 | 1:b067a08ff54e | 732 | |
jah128 | 11:5ebcb52726cf | 733 | // Turn at slow speed (0.15) to the set amount of degrees (clockwise) |
jah128 | 11:5ebcb52726cf | 734 | void PiSwarm::slow_turn (float degrees) |
jah128 | 11:5ebcb52726cf | 735 | { |
jah128 | 11:5ebcb52726cf | 736 | float trunc = fmodf(degrees,360); |
jah128 | 11:5ebcb52726cf | 737 | if(trunc<-180) trunc+=360; |
jah128 | 11:5ebcb52726cf | 738 | if(trunc>180) trunc-=360; |
jah128 | 11:5ebcb52726cf | 739 | if(trunc<0) { |
jah128 | 11:5ebcb52726cf | 740 | float m_time = 1-trunc; |
jah128 | 11:5ebcb52726cf | 741 | float delay = (m_time + logf(SLOW_TURN_LEFT_OFFSET + m_time + m_time)) * SLOW_TURN_LEFT_MULTIPLIER; |
jah128 | 11:5ebcb52726cf | 742 | motor_timeout.attach_us(this,&PiSwarm::stop,(unsigned int) delay); |
jah128 | 11:5ebcb52726cf | 743 | left(0.15); |
jah128 | 11:5ebcb52726cf | 744 | } else if(trunc>0) { |
jah128 | 11:5ebcb52726cf | 745 | float delay = (trunc + logf(SLOW_TURN_RIGHT_OFFSET + trunc + trunc)) * SLOW_TURN_RIGHT_MULTIPLIER; |
jah128 | 11:5ebcb52726cf | 746 | motor_timeout.attach_us(this,&PiSwarm::stop,(unsigned int) delay); |
jah128 | 11:5ebcb52726cf | 747 | right(0.15); |
jah128 | 11:5ebcb52726cf | 748 | } |
jah128 | 11:5ebcb52726cf | 749 | } |
jah128 | 11:5ebcb52726cf | 750 | |
jah128 | 11:5ebcb52726cf | 751 | // Turn at fast speed (0.5) to the set amount of degrees (clockwise) |
jah128 | 11:5ebcb52726cf | 752 | void PiSwarm::fast_turn (float degrees) |
jah128 | 11:5ebcb52726cf | 753 | { |
jah128 | 11:5ebcb52726cf | 754 | float trunc = fmodf(degrees,360); |
jah128 | 11:5ebcb52726cf | 755 | if(trunc<-180) trunc+=360; |
jah128 | 11:5ebcb52726cf | 756 | if(trunc>180) trunc-=360; |
jah128 | 11:5ebcb52726cf | 757 | if(trunc<0) { |
jah128 | 11:5ebcb52726cf | 758 | float m_time = 2.5-trunc; |
jah128 | 11:5ebcb52726cf | 759 | float delay = m_time * FAST_TURN_LEFT_MULTIPLIER; |
jah128 | 11:5ebcb52726cf | 760 | motor_timeout.attach_us(this,&PiSwarm::stop,(unsigned int) delay); |
jah128 | 11:5ebcb52726cf | 761 | left(0.5); |
jah128 | 11:5ebcb52726cf | 762 | } else if(trunc>0) { |
jah128 | 11:5ebcb52726cf | 763 | float delay = (trunc + 2.5) * FAST_TURN_RIGHT_MULTIPLIER; |
jah128 | 11:5ebcb52726cf | 764 | motor_timeout.attach_us(this,&PiSwarm::stop,(unsigned int) delay); |
jah128 | 11:5ebcb52726cf | 765 | right(0.5); |
jah128 | 11:5ebcb52726cf | 766 | } |
jah128 | 11:5ebcb52726cf | 767 | } |
jah128 | 11:5ebcb52726cf | 768 | |
jah128 | 11:5ebcb52726cf | 769 | void PiSwarm::test_turn_calibration(void) |
jah128 | 11:5ebcb52726cf | 770 | { |
jah128 | 11:5ebcb52726cf | 771 | } |
jah128 | 11:5ebcb52726cf | 772 | |
jah128 | 1:b067a08ff54e | 773 | ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// |
jah128 | 1:b067a08ff54e | 774 | // Sound Functions |
jah128 | 1:b067a08ff54e | 775 | ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// |
jah128 | 1:b067a08ff54e | 776 | |
jah128 | 11:5ebcb52726cf | 777 | void PiSwarm::play_tune (char * tune, int length) |
jah128 | 11:5ebcb52726cf | 778 | { |
jah128 | 1:b067a08ff54e | 779 | _ser.putc(DO_PLAY); |
jah128 | 11:5ebcb52726cf | 780 | _ser.putc(length); |
jah128 | 1:b067a08ff54e | 781 | for (int i = 0 ; i < length ; i++) { |
jah128 | 11:5ebcb52726cf | 782 | _ser.putc(tune[i]); |
jah128 | 1:b067a08ff54e | 783 | } |
jah128 | 1:b067a08ff54e | 784 | } |
jah128 | 1:b067a08ff54e | 785 | |
jah128 | 4:52b3e4c5a425 | 786 | ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// |
jah128 | 4:52b3e4c5a425 | 787 | // Display Functions |
jah128 | 4:52b3e4c5a425 | 788 | ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// |
jah128 | 4:52b3e4c5a425 | 789 | |
jah128 | 11:5ebcb52726cf | 790 | void PiSwarm::locate(int x, int y) |
jah128 | 11:5ebcb52726cf | 791 | { |
jah128 | 4:52b3e4c5a425 | 792 | _ser.putc(DO_LCD_GOTO_XY); |
jah128 | 4:52b3e4c5a425 | 793 | _ser.putc(x); |
jah128 | 4:52b3e4c5a425 | 794 | _ser.putc(y); |
jah128 | 4:52b3e4c5a425 | 795 | } |
jah128 | 4:52b3e4c5a425 | 796 | |
jah128 | 11:5ebcb52726cf | 797 | void PiSwarm::cls(void) |
jah128 | 11:5ebcb52726cf | 798 | { |
jah128 | 4:52b3e4c5a425 | 799 | _ser.putc(DO_CLEAR); |
jah128 | 4:52b3e4c5a425 | 800 | } |
jah128 | 1:b067a08ff54e | 801 | |
jah128 | 1:b067a08ff54e | 802 | ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// |
jah128 | 1:b067a08ff54e | 803 | // EEPROM Functions |
jah128 | 1:b067a08ff54e | 804 | ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// |
jah128 | 1:b067a08ff54e | 805 | |
jah128 | 1:b067a08ff54e | 806 | |
jah128 | 11:5ebcb52726cf | 807 | void PiSwarm::write_eeprom_byte ( int address, char data ) |
jah128 | 11:5ebcb52726cf | 808 | { |
jah128 | 0:9ffe8ebd1c40 | 809 | char write_array[3]; |
jah128 | 0:9ffe8ebd1c40 | 810 | write_array[0] = address / 256; |
jah128 | 0:9ffe8ebd1c40 | 811 | write_array[1] = address % 256; |
jah128 | 0:9ffe8ebd1c40 | 812 | write_array[2] = data; |
jah128 | 0:9ffe8ebd1c40 | 813 | _i2c.write(EEPROM_ADDRESS, write_array, 3, false); |
jah128 | 0:9ffe8ebd1c40 | 814 | //Takes 5ms to write a page: ideally this could be done with a timer or RTOS |
jah128 | 0:9ffe8ebd1c40 | 815 | wait(0.005); |
jah128 | 0:9ffe8ebd1c40 | 816 | } |
jah128 | 11:5ebcb52726cf | 817 | |
jah128 | 11:5ebcb52726cf | 818 | void PiSwarm::write_eeprom_block ( int address, char length, char * data) |
jah128 | 11:5ebcb52726cf | 819 | { |
jah128 | 0:9ffe8ebd1c40 | 820 | /** Note from datasheet: |
jah128 | 0:9ffe8ebd1c40 | 821 | Page write operations are limited to writing bytes within a single physical page, |
jah128 | 0:9ffe8ebd1c40 | 822 | regardless of the number of bytes actually being written. Physical page |
jah128 | 0:9ffe8ebd1c40 | 823 | boundaries start at addresses that are integer multiples of the page buffer size (or |
jah128 | 0:9ffe8ebd1c40 | 824 | ‘page size’) and end at addresses that are integer multiples of [page size – 1]. If a |
jah128 | 0:9ffe8ebd1c40 | 825 | Page Write command attempts to write across a physical page boundary, the |
jah128 | 0:9ffe8ebd1c40 | 826 | result is that the data wraps around to the beginning of the current page (overwriting |
jah128 | 0:9ffe8ebd1c40 | 827 | data previously stored there), instead of being written to the next page, as might be |
jah128 | 0:9ffe8ebd1c40 | 828 | expected. It is therefore necessary for the application software to prevent page write |
jah128 | 0:9ffe8ebd1c40 | 829 | operations that would attempt to cross a page boundary. */ |
jah128 | 11:5ebcb52726cf | 830 | |
jah128 | 0:9ffe8ebd1c40 | 831 | //First check to see if first block starts at the start of a page |
jah128 | 0:9ffe8ebd1c40 | 832 | int subpage = address % 32; |
jah128 | 11:5ebcb52726cf | 833 | |
jah128 | 0:9ffe8ebd1c40 | 834 | //If subpage > 0 first page does not start at a page boundary |
jah128 | 0:9ffe8ebd1c40 | 835 | int write_length = 32 - subpage; |
jah128 | 0:9ffe8ebd1c40 | 836 | if( write_length > length ) write_length = length; |
jah128 | 0:9ffe8ebd1c40 | 837 | char firstpage_array [write_length+2]; |
jah128 | 0:9ffe8ebd1c40 | 838 | firstpage_array[0] = address / 256; |
jah128 | 0:9ffe8ebd1c40 | 839 | firstpage_array[1] = address % 256; |
jah128 | 11:5ebcb52726cf | 840 | for(int i=0; i<write_length; i++) { |
jah128 | 11:5ebcb52726cf | 841 | firstpage_array[i+2] = data [i]; |
jah128 | 11:5ebcb52726cf | 842 | } |
jah128 | 0:9ffe8ebd1c40 | 843 | _i2c.write(EEPROM_ADDRESS, firstpage_array, write_length + 2, false); |
jah128 | 0:9ffe8ebd1c40 | 844 | wait(0.005); |
jah128 | 11:5ebcb52726cf | 845 | |
jah128 | 11:5ebcb52726cf | 846 | if(length > write_length) { |
jah128 | 0:9ffe8ebd1c40 | 847 | int page_count = (length + subpage) / 32; |
jah128 | 0:9ffe8ebd1c40 | 848 | int written = write_length; |
jah128 | 11:5ebcb52726cf | 849 | for(int p=0; p<page_count; p++) { |
jah128 | 0:9ffe8ebd1c40 | 850 | int to_write = length - written; |
jah128 | 0:9ffe8ebd1c40 | 851 | if (to_write > 32) { |
jah128 | 0:9ffe8ebd1c40 | 852 | to_write = 32; |
jah128 | 11:5ebcb52726cf | 853 | } |
jah128 | 0:9ffe8ebd1c40 | 854 | char page_array [to_write + 2]; |
jah128 | 0:9ffe8ebd1c40 | 855 | page_array[0] = (address + written) / 256; |
jah128 | 0:9ffe8ebd1c40 | 856 | page_array[1] = (address + written) % 256; |
jah128 | 11:5ebcb52726cf | 857 | for(int i=0; i<to_write; i++) { |
jah128 | 0:9ffe8ebd1c40 | 858 | page_array[i+2] = data[written + i]; |
jah128 | 0:9ffe8ebd1c40 | 859 | } |
jah128 | 0:9ffe8ebd1c40 | 860 | _i2c.write(EEPROM_ADDRESS, page_array, to_write + 2, false); |
jah128 | 0:9ffe8ebd1c40 | 861 | wait(0.005); |
jah128 | 0:9ffe8ebd1c40 | 862 | written += 32; |
jah128 | 0:9ffe8ebd1c40 | 863 | } |
jah128 | 11:5ebcb52726cf | 864 | } |
jah128 | 0:9ffe8ebd1c40 | 865 | } |
jah128 | 11:5ebcb52726cf | 866 | |
jah128 | 11:5ebcb52726cf | 867 | char PiSwarm::read_eeprom_byte ( int address ) |
jah128 | 11:5ebcb52726cf | 868 | { |
jah128 | 0:9ffe8ebd1c40 | 869 | char address_array [2]; |
jah128 | 0:9ffe8ebd1c40 | 870 | address_array[0] = address / 256; |
jah128 | 0:9ffe8ebd1c40 | 871 | address_array[1] = address % 256; |
jah128 | 0:9ffe8ebd1c40 | 872 | char data [1]; |
jah128 | 0:9ffe8ebd1c40 | 873 | _i2c.write(EEPROM_ADDRESS, address_array, 2, false); |
jah128 | 0:9ffe8ebd1c40 | 874 | _i2c.read(EEPROM_ADDRESS, data, 1, false); |
jah128 | 0:9ffe8ebd1c40 | 875 | return data [0]; |
jah128 | 0:9ffe8ebd1c40 | 876 | } |
jah128 | 0:9ffe8ebd1c40 | 877 | |
jah128 | 11:5ebcb52726cf | 878 | char PiSwarm::read_next_eeprom_byte () |
jah128 | 11:5ebcb52726cf | 879 | { |
jah128 | 0:9ffe8ebd1c40 | 880 | char data [1]; |
jah128 | 0:9ffe8ebd1c40 | 881 | _i2c.read(EEPROM_ADDRESS, data, 1, false); |
jah128 | 0:9ffe8ebd1c40 | 882 | return data [0]; |
jah128 | 0:9ffe8ebd1c40 | 883 | } |
jah128 | 11:5ebcb52726cf | 884 | |
jah128 | 11:5ebcb52726cf | 885 | char PiSwarm::read_eeprom_block ( int address, char length ) |
jah128 | 11:5ebcb52726cf | 886 | { |
jah128 | 0:9ffe8ebd1c40 | 887 | char ret = 0; |
jah128 | 0:9ffe8ebd1c40 | 888 | return ret; |
jah128 | 0:9ffe8ebd1c40 | 889 | } |
jah128 | 1:b067a08ff54e | 890 | |
jah128 | 4:52b3e4c5a425 | 891 | ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// |
jah128 | 4:52b3e4c5a425 | 892 | // RF Transceiver Functions |
jah128 | 4:52b3e4c5a425 | 893 | ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// |
jah128 | 4:52b3e4c5a425 | 894 | |
jah128 | 11:5ebcb52726cf | 895 | void PiSwarm::send_rf_message(char* message, char length) |
jah128 | 11:5ebcb52726cf | 896 | { |
jah128 | 4:52b3e4c5a425 | 897 | _rf.sendString(length, message); |
jah128 | 4:52b3e4c5a425 | 898 | } |
jah128 | 1:b067a08ff54e | 899 | |
jah128 | 1:b067a08ff54e | 900 | ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// |
jah128 | 1:b067a08ff54e | 901 | // Setup and Calibration Functions |
jah128 | 1:b067a08ff54e | 902 | ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// |
jah128 | 1:b067a08ff54e | 903 | |
jah128 | 11:5ebcb52726cf | 904 | void PiSwarm::setup () |
jah128 | 11:5ebcb52726cf | 905 | { |
jah128 | 1:b067a08ff54e | 906 | _ser.baud(115200); |
jah128 | 1:b067a08ff54e | 907 | set_oled_brightness (oled_brightness); |
jah128 | 1:b067a08ff54e | 908 | set_cled_brightness (cled_brightness); |
jah128 | 1:b067a08ff54e | 909 | gpio_led = setup_expansion_ic(); |
jah128 | 11:5ebcb52726cf | 910 | beacon_led = setup_adc(); |
jah128 | 1:b067a08ff54e | 911 | } |
jah128 | 1:b067a08ff54e | 912 | |
jah128 | 11:5ebcb52726cf | 913 | void PiSwarm::setup_radio() |
jah128 | 11:5ebcb52726cf | 914 | { |
jah128 | 1:b067a08ff54e | 915 | //Setup RF transceiver |
jah128 | 1:b067a08ff54e | 916 | _rf.rf_init(); |
jah128 | 1:b067a08ff54e | 917 | _rf.setFrequency(RF_FREQUENCY); |
jah128 | 1:b067a08ff54e | 918 | _rf.setDatarate(RF_DATARATE); |
jah128 | 1:b067a08ff54e | 919 | } |
jah128 | 1:b067a08ff54e | 920 | |
jah128 | 11:5ebcb52726cf | 921 | int PiSwarm::setup_expansion_ic () |
jah128 | 11:5ebcb52726cf | 922 | { |
jah128 | 1:b067a08ff54e | 923 | //Expansion IC is NXP PCA9505 |
jah128 | 1:b067a08ff54e | 924 | //Address is 0x40 (defined by EXPANSION_IC_ADDRESS) |
jah128 | 1:b067a08ff54e | 925 | //Block IO 0 : 7-0 Are OLED Outputs |
jah128 | 1:b067a08ff54e | 926 | //Block IO 1 : 7 is NC. 6-2 are Cursor switches (interrupt inputs). 1 and 0 are OLED outputs. |
jah128 | 1:b067a08ff54e | 927 | //Block IO 2 : 7 and 6 are LDO enable outputs. 5 - 1 are ID switch. 0 is NC. |
jah128 | 1:b067a08ff54e | 928 | //Block IO 3 and 4 are for the expansion connector (not assigned here) |
jah128 | 1:b067a08ff54e | 929 | char data [4]; |
jah128 | 1:b067a08ff54e | 930 | data [0] = 0x98; //Write to I/O configure register 0, using auto increment (allows multi-blocks in one command) |
jah128 | 1:b067a08ff54e | 931 | data [1] = 0x00; //All 8 pins in block 0 are outputs (0) |
jah128 | 1:b067a08ff54e | 932 | data [2] = 0xFC; //Pins 1 & 0 are outputs, rest are inputs (or NC) |
jah128 | 1:b067a08ff54e | 933 | data [3] = 0x3F; //Pins 7 & 6 are outputs, rest are inputs (or NC) |
jah128 | 1:b067a08ff54e | 934 | //If using the expansion port, assing its I/O here by increasing size of data[] and adding extra commands |
jah128 | 1:b067a08ff54e | 935 | int test_val = _i2c.write(EXPANSION_IC_ADDRESS,data,4,false); |
jah128 | 11:5ebcb52726cf | 936 | |
jah128 | 1:b067a08ff54e | 937 | // test_val should return 0 for correctly acknowledged response |
jah128 | 11:5ebcb52726cf | 938 | |
jah128 | 1:b067a08ff54e | 939 | //Invert the polarity for switch inputs (they connect to ground when pressed\on) |
jah128 | 1:b067a08ff54e | 940 | data [0] = 0x90; //Write to polarity inversion register using auto increment |
jah128 | 1:b067a08ff54e | 941 | data [1] = 0x00; |
jah128 | 1:b067a08ff54e | 942 | data [2] = 0x7C; //Invert pins 6-2 for cursor switches |
jah128 | 1:b067a08ff54e | 943 | data [3] = 0x3E; //Invert pins 5-1 for ID switch |
jah128 | 1:b067a08ff54e | 944 | _i2c.write(EXPANSION_IC_ADDRESS,data,4,false); |
jah128 | 11:5ebcb52726cf | 945 | |
jah128 | 1:b067a08ff54e | 946 | //Setup the interrupt masks. By default, only the direction switches trigger an interrupt |
jah128 | 1:b067a08ff54e | 947 | data [0] = 0xA0; |
jah128 | 1:b067a08ff54e | 948 | data [1] = 0xFF; |
jah128 | 1:b067a08ff54e | 949 | data [2] = 0x83; // Disable mask on pins 6-2 for cursor switches |
jah128 | 1:b067a08ff54e | 950 | data [3] = 0xFF; |
jah128 | 1:b067a08ff54e | 951 | _i2c.write(EXPANSION_IC_ADDRESS,data,4,false); |
jah128 | 1:b067a08ff54e | 952 | |
jah128 | 1:b067a08ff54e | 953 | //Read the ID switches |
jah128 | 1:b067a08ff54e | 954 | data [0] = 0x80; //Read input registers |
jah128 | 1:b067a08ff54e | 955 | char read_data [5]; |
jah128 | 1:b067a08ff54e | 956 | _i2c.write(EXPANSION_IC_ADDRESS, data, 1, false); |
jah128 | 1:b067a08ff54e | 957 | _i2c.read(EXPANSION_IC_ADDRESS, read_data, 5, false); |
jah128 | 11:5ebcb52726cf | 958 | id = (read_data[2] & 0x3E) >> 1; //Mask pins 0, 6 and 7 then shift |
jah128 | 11:5ebcb52726cf | 959 | |
jah128 | 1:b067a08ff54e | 960 | //Setup interrupt handler |
jah128 | 1:b067a08ff54e | 961 | expansion_interrupt.mode(PullUp); // Pull Up by default (needed on v1 PCB - no pullup resistor) |
jah128 | 1:b067a08ff54e | 962 | expansion_interrupt.fall(this,&PiSwarm::expansion_interrupt_handler); // Add local handler on falling edge to read switch |
jah128 | 1:b067a08ff54e | 963 | return test_val; |
jah128 | 0:9ffe8ebd1c40 | 964 | } |
jah128 | 0:9ffe8ebd1c40 | 965 | |
jah128 | 11:5ebcb52726cf | 966 | char PiSwarm::setup_adc ( void ) |
jah128 | 11:5ebcb52726cf | 967 | { |
jah128 | 1:b067a08ff54e | 968 | //ADC IC (for IR system) is a Analog Devices AD7997-BRUZ1 |
jah128 | 1:b067a08ff54e | 969 | return 0; |
jah128 | 1:b067a08ff54e | 970 | } |
jah128 | 11:5ebcb52726cf | 971 | |
jah128 | 11:5ebcb52726cf | 972 | char PiSwarm::calibrate_gyro ( void ) |
jah128 | 11:5ebcb52726cf | 973 | { |
jah128 | 1:b067a08ff54e | 974 | //This routine attempts to calibrate the offset of the gyro |
jah128 | 1:b067a08ff54e | 975 | int samples = 8; |
jah128 | 1:b067a08ff54e | 976 | float gyro_values[samples]; |
jah128 | 1:b067a08ff54e | 977 | calibrate_led = 1; |
jah128 | 1:b067a08ff54e | 978 | calibrate_ticker.attach(this,&PiSwarm::calibrate_ticker_routine,0.25); |
jah128 | 11:5ebcb52726cf | 979 | for(int i=0; i<samples; i++) { |
jah128 | 1:b067a08ff54e | 980 | gyro_values[i] = _gyro.read(); |
jah128 | 1:b067a08ff54e | 981 | wait(0.12); |
jah128 | 1:b067a08ff54e | 982 | } |
jah128 | 1:b067a08ff54e | 983 | char pass = 1; |
jah128 | 1:b067a08ff54e | 984 | float mean = 0; |
jah128 | 1:b067a08ff54e | 985 | float min = 3.5; |
jah128 | 1:b067a08ff54e | 986 | float max = 3.2; |
jah128 | 11:5ebcb52726cf | 987 | for(int i=0; i<samples; i++) { |
jah128 | 1:b067a08ff54e | 988 | if((gyro_values[i] * 3.5 < 1.5) || (gyro_values[i] * 3.2 > 1.5)) pass = 0; // Calibration failed! |
jah128 | 1:b067a08ff54e | 989 | float test_value = 1.50 / gyro_values[i]; |
jah128 | 1:b067a08ff54e | 990 | mean += test_value; |
jah128 | 1:b067a08ff54e | 991 | if (test_value > max) max = test_value; |
jah128 | 1:b067a08ff54e | 992 | if (test_value < min) min = test_value; |
jah128 | 1:b067a08ff54e | 993 | } |
jah128 | 11:5ebcb52726cf | 994 | if(pass == 1) { |
jah128 | 1:b067a08ff54e | 995 | gyro_cal = mean / samples; |
jah128 | 1:b067a08ff54e | 996 | gyro_error = max - min; |
jah128 | 1:b067a08ff54e | 997 | calibrate_ticker.detach(); |
jah128 | 1:b067a08ff54e | 998 | calibrate_led = 0; |
jah128 | 1:b067a08ff54e | 999 | } |
jah128 | 1:b067a08ff54e | 1000 | return pass; |
jah128 | 11:5ebcb52726cf | 1001 | } |
jah128 | 1:b067a08ff54e | 1002 | |
jah128 | 11:5ebcb52726cf | 1003 | char PiSwarm::calibrate_accelerometer ( void ) |
jah128 | 11:5ebcb52726cf | 1004 | { |
jah128 | 1:b067a08ff54e | 1005 | //This routine attempts to calibrate the offset and multiplier of the accelerometer |
jah128 | 1:b067a08ff54e | 1006 | int samples = 8; |
jah128 | 1:b067a08ff54e | 1007 | float acc_x_values[samples]; |
jah128 | 1:b067a08ff54e | 1008 | float acc_y_values[samples]; |
jah128 | 1:b067a08ff54e | 1009 | float acc_z_values[samples]; |
jah128 | 1:b067a08ff54e | 1010 | calibrate_led = 1; |
jah128 | 1:b067a08ff54e | 1011 | calibrate_ticker.attach(this,&PiSwarm::calibrate_ticker_routine,0.25); |
jah128 | 11:5ebcb52726cf | 1012 | for(int i=0; i<samples; i++) { |
jah128 | 1:b067a08ff54e | 1013 | acc_x_values[i] = _accel_x.read(); |
jah128 | 1:b067a08ff54e | 1014 | acc_y_values[i] = _accel_y.read(); |
jah128 | 1:b067a08ff54e | 1015 | acc_z_values[i] = _accel_z.read(); |
jah128 | 1:b067a08ff54e | 1016 | wait(0.12); |
jah128 | 1:b067a08ff54e | 1017 | } |
jah128 | 1:b067a08ff54e | 1018 | char pass = 1; |
jah128 | 1:b067a08ff54e | 1019 | float x_mean = 0, y_mean=0, z_mean=0; |
jah128 | 1:b067a08ff54e | 1020 | float x_min = 3600, y_min=3600, z_min=3600; |
jah128 | 1:b067a08ff54e | 1021 | float x_max = 3100, y_max=3100, z_max=3100; |
jah128 | 11:5ebcb52726cf | 1022 | for(int i=0; i<samples; i++) { |
jah128 | 1:b067a08ff54e | 1023 | if((acc_x_values[i] * 3600 < 1250) || (acc_x_values[i] * 3100 > 1250)) pass = 0; // Calibration failed! |
jah128 | 1:b067a08ff54e | 1024 | if((acc_y_values[i] * 3600 < 1250) || (acc_y_values[i] * 3100 > 1250)) pass = 0; // Calibration failed! |
jah128 | 1:b067a08ff54e | 1025 | if((acc_z_values[i] * 3600 < 887) || (acc_z_values[i] * 3100 > 887)) pass = 0; // Calibration failed! |
jah128 | 11:5ebcb52726cf | 1026 | |
jah128 | 1:b067a08ff54e | 1027 | float x_test_value = 1250 / acc_x_values[i]; |
jah128 | 1:b067a08ff54e | 1028 | x_mean += x_test_value; |
jah128 | 1:b067a08ff54e | 1029 | if (x_test_value > x_max) x_max = x_test_value; |
jah128 | 1:b067a08ff54e | 1030 | if (x_test_value < x_min) x_min = x_test_value; |
jah128 | 1:b067a08ff54e | 1031 | float y_test_value = 1250 / acc_y_values[i]; |
jah128 | 1:b067a08ff54e | 1032 | y_mean += y_test_value; |
jah128 | 1:b067a08ff54e | 1033 | if (y_test_value > y_max) y_max = y_test_value; |
jah128 | 11:5ebcb52726cf | 1034 | if (y_test_value < y_min) y_min = y_test_value; |
jah128 | 1:b067a08ff54e | 1035 | float z_test_value = 887 / acc_z_values[i]; |
jah128 | 1:b067a08ff54e | 1036 | z_mean += z_test_value; |
jah128 | 1:b067a08ff54e | 1037 | if (z_test_value > z_max) z_max = z_test_value; |
jah128 | 11:5ebcb52726cf | 1038 | if (z_test_value < z_min) z_min = z_test_value; |
jah128 | 1:b067a08ff54e | 1039 | } |
jah128 | 11:5ebcb52726cf | 1040 | if(pass == 1) { |
jah128 | 1:b067a08ff54e | 1041 | acc_x_cal = x_mean / samples; |
jah128 | 1:b067a08ff54e | 1042 | acc_y_cal = y_mean / samples; |
jah128 | 1:b067a08ff54e | 1043 | acc_z_cal = z_mean / samples; |
jah128 | 1:b067a08ff54e | 1044 | calibrate_ticker.detach(); |
jah128 | 1:b067a08ff54e | 1045 | calibrate_led = 0; |
jah128 | 1:b067a08ff54e | 1046 | } |
jah128 | 1:b067a08ff54e | 1047 | return pass; |
jah128 | 11:5ebcb52726cf | 1048 | } |
jah128 | 1:b067a08ff54e | 1049 | |
jah128 | 11:5ebcb52726cf | 1050 | char PiSwarm::calibrate_magnetometer ( void ) |
jah128 | 11:5ebcb52726cf | 1051 | { |
jah128 | 1:b067a08ff54e | 1052 | char command_data[2]; |
jah128 | 1:b067a08ff54e | 1053 | command_data[0] = 0x11; //Write to CTRL_REG2 |
jah128 | 1:b067a08ff54e | 1054 | command_data[1] = 0x80; // Enable auto resets |
jah128 | 1:b067a08ff54e | 1055 | _i2c.write(MAGNETOMETER_ADDRESS, command_data, 2, false); |
jah128 | 1:b067a08ff54e | 1056 | command_data[0] = 0x10; //Write to CTRL_REG1 |
jah128 | 11:5ebcb52726cf | 1057 | command_data[1] = 0x29; // 001 01 0 0 1 [40HZ 32-OS NOT-FAST NORMAL ACTIVE MODE] |
jah128 | 1:b067a08ff54e | 1058 | return _i2c.write(MAGNETOMETER_ADDRESS, command_data, 2, false); |
jah128 | 1:b067a08ff54e | 1059 | } |
jah128 | 1:b067a08ff54e | 1060 | |
jah128 | 1:b067a08ff54e | 1061 | |
jah128 | 1:b067a08ff54e | 1062 | |
jah128 | 1:b067a08ff54e | 1063 | |
jah128 | 1:b067a08ff54e | 1064 | |
jah128 | 11:5ebcb52726cf | 1065 | void PiSwarm::interrupt_timeout_event ( void ) |
jah128 | 11:5ebcb52726cf | 1066 | { |
jah128 | 1:b067a08ff54e | 1067 | //Read switches |
jah128 | 1:b067a08ff54e | 1068 | char data [1]; |
jah128 | 1:b067a08ff54e | 1069 | data [0] = 0x80; //Read input registers |
jah128 | 1:b067a08ff54e | 1070 | char read_data [5]; |
jah128 | 1:b067a08ff54e | 1071 | _i2c.write(EXPANSION_IC_ADDRESS, data, 1, false); |
jah128 | 1:b067a08ff54e | 1072 | _i2c.read(EXPANSION_IC_ADDRESS, read_data, 5, false); |
jah128 | 1:b067a08ff54e | 1073 | switches = (read_data[1] & 0x7C) >> 2; |
jah128 | 1:b067a08ff54e | 1074 | led_timeout.attach(this,&PiSwarm::led_timeout_event, 0.1); // Switch off the LED after 0.1s |
jah128 | 1:b067a08ff54e | 1075 | } |
jah128 | 1:b067a08ff54e | 1076 | |
jah128 | 11:5ebcb52726cf | 1077 | void PiSwarm::led_timeout_event ( void ) |
jah128 | 11:5ebcb52726cf | 1078 | { |
jah128 | 1:b067a08ff54e | 1079 | interrupt_led = 0; |
jah128 | 1:b067a08ff54e | 1080 | } |
jah128 | 1:b067a08ff54e | 1081 | |
jah128 | 11:5ebcb52726cf | 1082 | void PiSwarm::expansion_interrupt_handler ( void ) |
jah128 | 11:5ebcb52726cf | 1083 | { |
jah128 | 1:b067a08ff54e | 1084 | interrupt_led = 1; |
jah128 | 1:b067a08ff54e | 1085 | debounce_timeout.attach_us(this,&PiSwarm::interrupt_timeout_event, 200); // Read the switches only after 200uS have passed to debounce |
jah128 | 1:b067a08ff54e | 1086 | debounce_timeout.add_function(&switch_pressed); // Call the switch pressed routine (in main) when done |
jah128 | 1:b067a08ff54e | 1087 | } |
jah128 | 1:b067a08ff54e | 1088 | |
jah128 | 1:b067a08ff54e | 1089 | |
jah128 | 11:5ebcb52726cf | 1090 | |
jah128 | 11:5ebcb52726cf | 1091 | |
jah128 | 1:b067a08ff54e | 1092 | |
jah128 | 11:5ebcb52726cf | 1093 | void PiSwarm::calibrate_ticker_routine ( void ) |
jah128 | 11:5ebcb52726cf | 1094 | { |
jah128 | 1:b067a08ff54e | 1095 | calibrate_led_state = 1 - calibrate_led_state; |
jah128 | 1:b067a08ff54e | 1096 | calibrate_led = calibrate_led_state; |
jah128 | 1:b067a08ff54e | 1097 | } |
jah128 | 1:b067a08ff54e | 1098 | |
jah128 | 11:5ebcb52726cf | 1099 | void PiSwarm::test_oled() |
jah128 | 11:5ebcb52726cf | 1100 | { |
jah128 | 1:b067a08ff54e | 1101 | enable_led_ldo = 1; |
jah128 | 1:b067a08ff54e | 1102 | enable_ldo_outputs(); |
jah128 | 1:b067a08ff54e | 1103 | set_oled_colour(100,100,100); |
jah128 | 1:b067a08ff54e | 1104 | char data[2]; |
jah128 | 1:b067a08ff54e | 1105 | data[0] = 0x09; //Address for PCA9505 Output Port 1 |
jah128 | 1:b067a08ff54e | 1106 | data[1] = 3; |
jah128 | 1:b067a08ff54e | 1107 | _i2c.write(EXPANSION_IC_ADDRESS,data,2,false); |
jah128 | 1:b067a08ff54e | 1108 | } |
jah128 | 0:9ffe8ebd1c40 | 1109 | |
jah128 | 11:5ebcb52726cf | 1110 | void PiSwarm::reset () |
jah128 | 11:5ebcb52726cf | 1111 | { |
jah128 | 11:5ebcb52726cf | 1112 | // _nrst = 0; |
jah128 | 0:9ffe8ebd1c40 | 1113 | wait (0.01); |
jah128 | 11:5ebcb52726cf | 1114 | // _nrst = 1; |
jah128 | 0:9ffe8ebd1c40 | 1115 | wait (0.1); |
jah128 | 0:9ffe8ebd1c40 | 1116 | } |
jah128 | 11:5ebcb52726cf | 1117 | void PiSwarm::motor (int motor, float speed) |
jah128 | 11:5ebcb52726cf | 1118 | { |
jah128 | 0:9ffe8ebd1c40 | 1119 | char opcode = 0x0; |
jah128 | 0:9ffe8ebd1c40 | 1120 | if (speed > 0.0) { |
jah128 | 0:9ffe8ebd1c40 | 1121 | if (motor==1) |
jah128 | 0:9ffe8ebd1c40 | 1122 | opcode = M1_FORWARD; |
jah128 | 0:9ffe8ebd1c40 | 1123 | else |
jah128 | 0:9ffe8ebd1c40 | 1124 | opcode = M2_FORWARD; |
jah128 | 0:9ffe8ebd1c40 | 1125 | } else { |
jah128 | 0:9ffe8ebd1c40 | 1126 | if (motor==1) |
jah128 | 0:9ffe8ebd1c40 | 1127 | opcode = M1_BACKWARD; |
jah128 | 0:9ffe8ebd1c40 | 1128 | else |
jah128 | 0:9ffe8ebd1c40 | 1129 | opcode = M2_BACKWARD; |
jah128 | 0:9ffe8ebd1c40 | 1130 | } |
jah128 | 0:9ffe8ebd1c40 | 1131 | if(motor==1)right_speed = speed; |
jah128 | 0:9ffe8ebd1c40 | 1132 | else left_speed = speed; |
jah128 | 0:9ffe8ebd1c40 | 1133 | unsigned char arg = 0x7f * abs(speed); |
jah128 | 0:9ffe8ebd1c40 | 1134 | |
jah128 | 0:9ffe8ebd1c40 | 1135 | _ser.putc(opcode); |
jah128 | 0:9ffe8ebd1c40 | 1136 | _ser.putc(arg); |
jah128 | 0:9ffe8ebd1c40 | 1137 | } |
jah128 | 0:9ffe8ebd1c40 | 1138 | |
jah128 | 0:9ffe8ebd1c40 | 1139 | |
jah128 | 11:5ebcb52726cf | 1140 | float PiSwarm::line_position() |
jah128 | 11:5ebcb52726cf | 1141 | { |
jah128 | 0:9ffe8ebd1c40 | 1142 | int pos = 0; |
jah128 | 0:9ffe8ebd1c40 | 1143 | _ser.putc(SEND_LINE_POSITION); |
jah128 | 0:9ffe8ebd1c40 | 1144 | pos = _ser.getc(); |
jah128 | 0:9ffe8ebd1c40 | 1145 | pos += _ser.getc() << 8; |
jah128 | 11:5ebcb52726cf | 1146 | |
jah128 | 0:9ffe8ebd1c40 | 1147 | float fpos = ((float)pos - 2048.0)/2048.0; |
jah128 | 0:9ffe8ebd1c40 | 1148 | return(fpos); |
jah128 | 0:9ffe8ebd1c40 | 1149 | } |
jah128 | 0:9ffe8ebd1c40 | 1150 | |
jah128 | 11:5ebcb52726cf | 1151 | char PiSwarm::sensor_auto_calibrate() |
jah128 | 11:5ebcb52726cf | 1152 | { |
jah128 | 0:9ffe8ebd1c40 | 1153 | _ser.putc(AUTO_CALIBRATE); |
jah128 | 0:9ffe8ebd1c40 | 1154 | return(_ser.getc()); |
jah128 | 0:9ffe8ebd1c40 | 1155 | } |
jah128 | 0:9ffe8ebd1c40 | 1156 | |
jah128 | 0:9ffe8ebd1c40 | 1157 | |
jah128 | 11:5ebcb52726cf | 1158 | void PiSwarm::calibrate(void) |
jah128 | 11:5ebcb52726cf | 1159 | { |
jah128 | 0:9ffe8ebd1c40 | 1160 | _ser.putc(PI_CALIBRATE); |
jah128 | 0:9ffe8ebd1c40 | 1161 | } |
jah128 | 0:9ffe8ebd1c40 | 1162 | |
jah128 | 11:5ebcb52726cf | 1163 | void PiSwarm::reset_calibration() |
jah128 | 11:5ebcb52726cf | 1164 | { |
jah128 | 0:9ffe8ebd1c40 | 1165 | _ser.putc(LINE_SENSORS_RESET_CALIBRATION); |
jah128 | 0:9ffe8ebd1c40 | 1166 | } |
jah128 | 0:9ffe8ebd1c40 | 1167 | |
jah128 | 11:5ebcb52726cf | 1168 | void PiSwarm::PID_start(int max_speed, int a, int b, int c, int d) |
jah128 | 11:5ebcb52726cf | 1169 | { |
jah128 | 0:9ffe8ebd1c40 | 1170 | _ser.putc(max_speed); |
jah128 | 0:9ffe8ebd1c40 | 1171 | _ser.putc(a); |
jah128 | 0:9ffe8ebd1c40 | 1172 | _ser.putc(b); |
jah128 | 0:9ffe8ebd1c40 | 1173 | _ser.putc(c); |
jah128 | 0:9ffe8ebd1c40 | 1174 | _ser.putc(d); |
jah128 | 0:9ffe8ebd1c40 | 1175 | } |
jah128 | 0:9ffe8ebd1c40 | 1176 | |
jah128 | 11:5ebcb52726cf | 1177 | void PiSwarm::PID_stop() |
jah128 | 11:5ebcb52726cf | 1178 | { |
jah128 | 0:9ffe8ebd1c40 | 1179 | _ser.putc(STOP_PID); |
jah128 | 0:9ffe8ebd1c40 | 1180 | } |
jah128 | 0:9ffe8ebd1c40 | 1181 | |
jah128 | 0:9ffe8ebd1c40 | 1182 | |
jah128 | 0:9ffe8ebd1c40 | 1183 | |
jah128 | 11:5ebcb52726cf | 1184 | int PiSwarm::print (char* text, int length) |
jah128 | 11:5ebcb52726cf | 1185 | { |
jah128 | 11:5ebcb52726cf | 1186 | _ser.putc(DO_PRINT); |
jah128 | 11:5ebcb52726cf | 1187 | _ser.putc(length); |
jah128 | 0:9ffe8ebd1c40 | 1188 | for (int i = 0 ; i < length ; i++) { |
jah128 | 11:5ebcb52726cf | 1189 | _ser.putc(text[i]); |
jah128 | 0:9ffe8ebd1c40 | 1190 | } |
jah128 | 0:9ffe8ebd1c40 | 1191 | return(0); |
jah128 | 0:9ffe8ebd1c40 | 1192 | } |
jah128 | 0:9ffe8ebd1c40 | 1193 | |
jah128 | 11:5ebcb52726cf | 1194 | int PiSwarm::_putc (int c) |
jah128 | 11:5ebcb52726cf | 1195 | { |
jah128 | 11:5ebcb52726cf | 1196 | _ser.putc(DO_PRINT); |
jah128 | 11:5ebcb52726cf | 1197 | _ser.putc(0x1); |
jah128 | 11:5ebcb52726cf | 1198 | _ser.putc(c); |
jah128 | 0:9ffe8ebd1c40 | 1199 | wait (0.001); |
jah128 | 0:9ffe8ebd1c40 | 1200 | return(c); |
jah128 | 0:9ffe8ebd1c40 | 1201 | } |
jah128 | 0:9ffe8ebd1c40 | 1202 | |
jah128 | 11:5ebcb52726cf | 1203 | int PiSwarm::_getc (void) |
jah128 | 11:5ebcb52726cf | 1204 | { |
jah128 | 0:9ffe8ebd1c40 | 1205 | char r = 0; |
jah128 | 0:9ffe8ebd1c40 | 1206 | return(r); |
jah128 | 0:9ffe8ebd1c40 | 1207 | } |
jah128 | 0:9ffe8ebd1c40 | 1208 | |
jah128 | 11:5ebcb52726cf | 1209 | int PiSwarm::putc (int c) |
jah128 | 11:5ebcb52726cf | 1210 | { |
jah128 | 0:9ffe8ebd1c40 | 1211 | return(_ser.putc(c)); |
jah128 | 0:9ffe8ebd1c40 | 1212 | } |
jah128 | 0:9ffe8ebd1c40 | 1213 | |
jah128 | 11:5ebcb52726cf | 1214 | int PiSwarm::getc (void) |
jah128 | 11:5ebcb52726cf | 1215 | { |
jah128 | 0:9ffe8ebd1c40 | 1216 | return(_ser.getc()); |
jah128 | 0:9ffe8ebd1c40 | 1217 | } |
jah128 | 0:9ffe8ebd1c40 | 1218 | |
jah128 | 11:5ebcb52726cf | 1219 | void PiSwarm::start_system_timer(void) |
jah128 | 11:5ebcb52726cf | 1220 | { |
jah128 | 0:9ffe8ebd1c40 | 1221 | _system_timer.reset(); |
jah128 | 0:9ffe8ebd1c40 | 1222 | _system_timer.start(); |
jah128 | 0:9ffe8ebd1c40 | 1223 | } |
jah128 | 0:9ffe8ebd1c40 | 1224 | |
jah128 | 0:9ffe8ebd1c40 | 1225 | |
jah128 | 0:9ffe8ebd1c40 | 1226 | #ifdef MBED_RPC |
jah128 | 11:5ebcb52726cf | 1227 | const rpc_method *PiSwarm::get_rpc_methods() |
jah128 | 11:5ebcb52726cf | 1228 | { |
jah128 | 0:9ffe8ebd1c40 | 1229 | static const rpc_method rpc_methods[] = {{ "forward", rpc_method_caller<PiSwarm, float, &PiSwarm::forward> }, |
jah128 | 0:9ffe8ebd1c40 | 1230 | { "backward", rpc_method_caller<PiSwarm, float, &PiSwarm::backward> }, |
jah128 | 0:9ffe8ebd1c40 | 1231 | { "left", rpc_method_caller<PiSwarm, float, &PiSwarm::left> }, |
jah128 | 0:9ffe8ebd1c40 | 1232 | { "right", rpc_method_caller<PiSwarm, float, &PiSwarm::right> }, |
jah128 | 0:9ffe8ebd1c40 | 1233 | { "stop", rpc_method_caller<PiSwarm, &PiSwarm::stop> }, |
jah128 | 0:9ffe8ebd1c40 | 1234 | { "left_motor", rpc_method_caller<PiSwarm, float, &PiSwarm::left_motor> }, |
jah128 | 0:9ffe8ebd1c40 | 1235 | { "right_motor", rpc_method_caller<PiSwarm, float, &PiSwarm::right_motor> }, |
jah128 | 0:9ffe8ebd1c40 | 1236 | { "battery", rpc_method_caller<float, PiSwarm, &PiSwarm::battery> }, |
jah128 | 0:9ffe8ebd1c40 | 1237 | { "line_position", rpc_method_caller<float, PiSwarm, &PiSwarm::line_position> }, |
jah128 | 0:9ffe8ebd1c40 | 1238 | { "sensor_auto_calibrate", rpc_method_caller<char, PiSwarm, &PiSwarm::sensor_auto_calibrate> }, |
jah128 | 0:9ffe8ebd1c40 | 1239 | RPC_METHOD_SUPER(Base) |
jah128 | 0:9ffe8ebd1c40 | 1240 | }; |
jah128 | 0:9ffe8ebd1c40 | 1241 | return rpc_methods; |
jah128 | 0:9ffe8ebd1c40 | 1242 | } |
jah128 | 0:9ffe8ebd1c40 | 1243 | #endif |
jah128 | 0:9ffe8ebd1c40 | 1244 | |
jah128 | 11:5ebcb52726cf | 1245 | void display_system_time() |
jah128 | 11:5ebcb52726cf | 1246 | { |
jah128 | 11:5ebcb52726cf | 1247 | if(PISWARM_DEBUG == 1) { |
jah128 | 0:9ffe8ebd1c40 | 1248 | time_t system_time = time(NULL); |
jah128 | 11:5ebcb52726cf | 1249 | printf("Current system time:%s Uptime:%.1f\n",ctime(&system_time),piswarm.get_uptime()); |
jah128 | 11:5ebcb52726cf | 1250 | } |
jah128 | 11:5ebcb52726cf | 1251 | } |
jah128 | 11:5ebcb52726cf | 1252 | |
jah128 | 11:5ebcb52726cf | 1253 | void oled_bearing_strobe() |
jah128 | 11:5ebcb52726cf | 1254 | { |
jah128 | 11:5ebcb52726cf | 1255 | bearing_strobe_step ++; |
jah128 | 11:5ebcb52726cf | 1256 | if(bearing_strobe_step > 10) { |
jah128 | 11:5ebcb52726cf | 1257 | oled_ticker.detach(); |
jah128 | 11:5ebcb52726cf | 1258 | piswarm.restore_oled_state(); |
jah128 | 11:5ebcb52726cf | 1259 | } else { |
jah128 | 11:5ebcb52726cf | 1260 | float position = (360 - bearing) / 36; |
jah128 | 11:5ebcb52726cf | 1261 | position -= 1; |
jah128 | 11:5ebcb52726cf | 1262 | if(position < 0) position += 10; |
jah128 | 11:5ebcb52726cf | 1263 | piswarm.set_oleds(0,0,0,0,0,0,0,0,0,0); |
jah128 | 11:5ebcb52726cf | 1264 | int fposition = floor(position); |
jah128 | 11:5ebcb52726cf | 1265 | int sposition = ceil(position); |
jah128 | 11:5ebcb52726cf | 1266 | if(sposition > 9) sposition = 0; |
jah128 | 11:5ebcb52726cf | 1267 | switch(bearing_strobe_step % 5) { |
jah128 | 11:5ebcb52726cf | 1268 | case 4: |
jah128 | 11:5ebcb52726cf | 1269 | piswarm.set_oled_colour(255,0,0); |
jah128 | 11:5ebcb52726cf | 1270 | if(sposition - position > 0.5) { |
jah128 | 11:5ebcb52726cf | 1271 | piswarm.set_oled(fposition,1); |
jah128 | 11:5ebcb52726cf | 1272 | } else piswarm.set_oled(sposition,1); |
jah128 | 11:5ebcb52726cf | 1273 | break; |
jah128 | 11:5ebcb52726cf | 1274 | case 3: |
jah128 | 11:5ebcb52726cf | 1275 | piswarm.set_oled_colour(155,30,0); |
jah128 | 11:5ebcb52726cf | 1276 | if(sposition - position > 0.5) { |
jah128 | 11:5ebcb52726cf | 1277 | piswarm.set_oled(fposition,1); |
jah128 | 11:5ebcb52726cf | 1278 | } else piswarm.set_oled(sposition,1); |
jah128 | 11:5ebcb52726cf | 1279 | break; |
jah128 | 11:5ebcb52726cf | 1280 | case 1: |
jah128 | 11:5ebcb52726cf | 1281 | case 2: |
jah128 | 11:5ebcb52726cf | 1282 | piswarm.set_oled_colour(100,25,0); |
jah128 | 11:5ebcb52726cf | 1283 | piswarm.set_oled(fposition,1); |
jah128 | 11:5ebcb52726cf | 1284 | piswarm.set_oled(sposition,1); |
jah128 | 11:5ebcb52726cf | 1285 | break; |
jah128 | 11:5ebcb52726cf | 1286 | case 0: |
jah128 | 11:5ebcb52726cf | 1287 | piswarm.set_oled_colour(60,10,0); |
jah128 | 11:5ebcb52726cf | 1288 | piswarm.set_oled(fposition,1); |
jah128 | 11:5ebcb52726cf | 1289 | piswarm.set_oled(sposition,1); |
jah128 | 11:5ebcb52726cf | 1290 | fposition --; |
jah128 | 11:5ebcb52726cf | 1291 | if(fposition<0)fposition = 9; |
jah128 | 11:5ebcb52726cf | 1292 | sposition ++; |
jah128 | 11:5ebcb52726cf | 1293 | if(sposition>9)sposition = 0; |
jah128 | 11:5ebcb52726cf | 1294 | piswarm.set_oled(fposition,1); |
jah128 | 11:5ebcb52726cf | 1295 | piswarm.set_oled(sposition,1); |
jah128 | 11:5ebcb52726cf | 1296 | break; |
jah128 | 11:5ebcb52726cf | 1297 | } |
jah128 | 11:5ebcb52726cf | 1298 | piswarm.activate_oleds(); |
jah128 | 11:5ebcb52726cf | 1299 | } |
jah128 | 11:5ebcb52726cf | 1300 | } |
jah128 | 11:5ebcb52726cf | 1301 | |
jah128 | 11:5ebcb52726cf | 1302 | void oled_ticker_update() |
jah128 | 11:5ebcb52726cf | 1303 | { |
jah128 | 11:5ebcb52726cf | 1304 | oled_ticker_step++; |
jah128 | 11:5ebcb52726cf | 1305 | if(oled_ticker_step == 10) oled_ticker_step = 0; |
jah128 | 11:5ebcb52726cf | 1306 | switch(oled_ticker_step) { |
jah128 | 11:5ebcb52726cf | 1307 | case 0: |
jah128 | 11:5ebcb52726cf | 1308 | piswarm.set_oleds(1,1,0,0,0,0,0,0,0,0); |
jah128 | 11:5ebcb52726cf | 1309 | break; |
jah128 | 11:5ebcb52726cf | 1310 | case 1: |
jah128 | 11:5ebcb52726cf | 1311 | piswarm.set_oleds(0,1,1,0,0,0,0,0,0,0); |
jah128 | 11:5ebcb52726cf | 1312 | break; |
jah128 | 11:5ebcb52726cf | 1313 | case 2: |
jah128 | 11:5ebcb52726cf | 1314 | piswarm.set_oleds(0,0,1,1,0,0,0,0,0,0); |
jah128 | 11:5ebcb52726cf | 1315 | break; |
jah128 | 11:5ebcb52726cf | 1316 | case 3: |
jah128 | 11:5ebcb52726cf | 1317 | piswarm.set_oleds(0,0,0,1,1,0,0,0,0,0); |
jah128 | 11:5ebcb52726cf | 1318 | break; |
jah128 | 11:5ebcb52726cf | 1319 | case 4: |
jah128 | 11:5ebcb52726cf | 1320 | piswarm.set_oleds(0,0,0,0,1,1,0,0,0,0); |
jah128 | 11:5ebcb52726cf | 1321 | break; |
jah128 | 11:5ebcb52726cf | 1322 | case 5: |
jah128 | 11:5ebcb52726cf | 1323 | piswarm.set_oleds(0,0,0,0,0,1,1,0,0,0); |
jah128 | 11:5ebcb52726cf | 1324 | break; |
jah128 | 11:5ebcb52726cf | 1325 | case 6: |
jah128 | 11:5ebcb52726cf | 1326 | piswarm.set_oleds(0,0,0,0,0,0,1,1,0,0); |
jah128 | 11:5ebcb52726cf | 1327 | break; |
jah128 | 11:5ebcb52726cf | 1328 | case 7: |
jah128 | 11:5ebcb52726cf | 1329 | piswarm.set_oleds(0,0,0,0,0,0,0,1,1,0); |
jah128 | 11:5ebcb52726cf | 1330 | break; |
jah128 | 11:5ebcb52726cf | 1331 | case 8: |
jah128 | 11:5ebcb52726cf | 1332 | piswarm.set_oleds(0,0,0,0,0,0,0,0,1,1); |
jah128 | 11:5ebcb52726cf | 1333 | break; |
jah128 | 11:5ebcb52726cf | 1334 | case 9: |
jah128 | 11:5ebcb52726cf | 1335 | piswarm.set_oleds(1,0,0,0,0,0,0,0,0,1); |
jah128 | 11:5ebcb52726cf | 1336 | break; |
jah128 | 11:5ebcb52726cf | 1337 | |
jah128 | 0:9ffe8ebd1c40 | 1338 | } |
jah128 | 0:9ffe8ebd1c40 | 1339 | } |
jah128 | 0:9ffe8ebd1c40 | 1340 | |
jah128 | 11:5ebcb52726cf | 1341 | float synchronise_with_beacon() |
jah128 | 11:5ebcb52726cf | 1342 | { |
jah128 | 11:5ebcb52726cf | 1343 | beacon_led = 1; |
jah128 | 11:5ebcb52726cf | 1344 | int offset_time = 0; |
jah128 | 11:5ebcb52726cf | 1345 | float confidence = -1; |
jah128 | 11:5ebcb52726cf | 1346 | float angle_confidence = 0; |
jah128 | 11:5ebcb52726cf | 1347 | float heading = 0; |
jah128 | 11:5ebcb52726cf | 1348 | float i_bearing = 0; |
jah128 | 11:5ebcb52726cf | 1349 | Timer ir_timer; |
jah128 | 11:5ebcb52726cf | 1350 | unsigned short ir_values[8][100]; |
jah128 | 11:5ebcb52726cf | 1351 | unsigned int ir_totals[8]; |
jah128 | 11:5ebcb52726cf | 1352 | ir_timer.start(); |
jah128 | 11:5ebcb52726cf | 1353 | int target_time = 0; |
jah128 | 11:5ebcb52726cf | 1354 | for(int i=0; i<8; i++) { |
jah128 | 11:5ebcb52726cf | 1355 | ir_totals[i]=0; |
jah128 | 11:5ebcb52726cf | 1356 | } |
jah128 | 11:5ebcb52726cf | 1357 | for(int step = 0; step < 100; step ++) { |
jah128 | 11:5ebcb52726cf | 1358 | target_time += 10000 ; |
jah128 | 11:5ebcb52726cf | 1359 | piswarm.store_background_raw_ir_values (); |
jah128 | 11:5ebcb52726cf | 1360 | for(int sensor = 0; sensor < 8; sensor ++) { |
jah128 | 11:5ebcb52726cf | 1361 | ir_values[sensor][step] = piswarm.get_background_raw_ir_value(sensor); |
jah128 | 11:5ebcb52726cf | 1362 | ir_totals[sensor] += ir_values[sensor][step]; |
jah128 | 11:5ebcb52726cf | 1363 | } |
jah128 | 11:5ebcb52726cf | 1364 | while(ir_timer.read_us() < target_time) wait_us(50); |
jah128 | 11:5ebcb52726cf | 1365 | } |
jah128 | 11:5ebcb52726cf | 1366 | int total = 0; |
jah128 | 11:5ebcb52726cf | 1367 | for(int i=0; i<8; i++) { |
jah128 | 11:5ebcb52726cf | 1368 | total += ir_totals[i]; |
jah128 | 11:5ebcb52726cf | 1369 | } |
jah128 | 11:5ebcb52726cf | 1370 | total /= 800; |
jah128 | 11:5ebcb52726cf | 1371 | unsigned short s_pass [100]; |
jah128 | 11:5ebcb52726cf | 1372 | int pass_count = 0; |
jah128 | 11:5ebcb52726cf | 1373 | for(int i=0; i<100; i++) { |
jah128 | 11:5ebcb52726cf | 1374 | signed short adjusted [8]; |
jah128 | 11:5ebcb52726cf | 1375 | int pass = 0; |
jah128 | 11:5ebcb52726cf | 1376 | for(int k=0; k<8; k++) { |
jah128 | 11:5ebcb52726cf | 1377 | adjusted[k]=ir_values[k][i]-total; |
jah128 | 11:5ebcb52726cf | 1378 | if(adjusted[k] > 0) pass++; |
jah128 | 11:5ebcb52726cf | 1379 | } |
jah128 | 11:5ebcb52726cf | 1380 | s_pass [i] = pass; |
jah128 | 11:5ebcb52726cf | 1381 | if(pass>0) pass_count++; |
jah128 | 11:5ebcb52726cf | 1382 | } |
jah128 | 11:5ebcb52726cf | 1383 | if(pass_count < 2) { |
jah128 | 11:5ebcb52726cf | 1384 | confidence = -1; |
jah128 | 11:5ebcb52726cf | 1385 | } else { |
jah128 | 11:5ebcb52726cf | 1386 | //Find highest value |
jah128 | 11:5ebcb52726cf | 1387 | int highest = 0; |
jah128 | 11:5ebcb52726cf | 1388 | int highest_pos = 0; |
jah128 | 11:5ebcb52726cf | 1389 | for(int k=0; k<100; k++) { |
jah128 | 11:5ebcb52726cf | 1390 | if(s_pass[k] > highest) { |
jah128 | 11:5ebcb52726cf | 1391 | highest = s_pass[k]; |
jah128 | 11:5ebcb52726cf | 1392 | highest_pos = k; |
jah128 | 11:5ebcb52726cf | 1393 | } |
jah128 | 11:5ebcb52726cf | 1394 | } |
jah128 | 11:5ebcb52726cf | 1395 | int length = 1; |
jah128 | 11:5ebcb52726cf | 1396 | //Adjust to find start: best length should be 2 or 3: compare previous value to |
jah128 | 11:5ebcb52726cf | 1397 | int pre = highest_pos -1; |
jah128 | 11:5ebcb52726cf | 1398 | if(pre < 0) pre=99; |
jah128 | 11:5ebcb52726cf | 1399 | int spost = highest_pos+1; |
jah128 | 11:5ebcb52726cf | 1400 | int post = highest_pos +2; |
jah128 | 11:5ebcb52726cf | 1401 | if(spost > 99) spost-=100; |
jah128 | 11:5ebcb52726cf | 1402 | if(post > 99) post-=100; |
jah128 | 11:5ebcb52726cf | 1403 | if(s_pass[pre] > 0 && s_pass[pre] >= s_pass[post]) { |
jah128 | 11:5ebcb52726cf | 1404 | highest_pos = pre; |
jah128 | 11:5ebcb52726cf | 1405 | length++; |
jah128 | 11:5ebcb52726cf | 1406 | } |
jah128 | 11:5ebcb52726cf | 1407 | if(s_pass[spost] > 0)length++; |
jah128 | 11:5ebcb52726cf | 1408 | if(s_pass[post] > s_pass[spost] && s_pass[spost] > 0)length++; |
jah128 | 11:5ebcb52726cf | 1409 | if(length>3) confidence = 0; |
jah128 | 11:5ebcb52726cf | 1410 | else if(length==1) confidence = 0; |
jah128 | 11:5ebcb52726cf | 1411 | else if(length==pass_count) confidence = 1; |
jah128 | 11:5ebcb52726cf | 1412 | else confidence = (float) length / (float) pass_count; |
jah128 | 11:5ebcb52726cf | 1413 | offset_time = highest_pos * 10; |
jah128 | 11:5ebcb52726cf | 1414 | if(length == 2) offset_time -= 5; |
jah128 | 11:5ebcb52726cf | 1415 | if(offset_time < 0) offset_time += 1000; |
jah128 | 11:5ebcb52726cf | 1416 | if(confidence > 0) { |
jah128 | 11:5ebcb52726cf | 1417 | unsigned short sums[8]; |
jah128 | 11:5ebcb52726cf | 1418 | for(int i=0; i<8; i++) { |
jah128 | 11:5ebcb52726cf | 1419 | sums[i] = 0; |
jah128 | 11:5ebcb52726cf | 1420 | for(int l=0; l<length; l++) { |
jah128 | 11:5ebcb52726cf | 1421 | int sp = highest_pos + l; |
jah128 | 11:5ebcb52726cf | 1422 | if(sp>99)sp-=100; |
jah128 | 11:5ebcb52726cf | 1423 | signed short adj = ir_values[i][sp]; |
jah128 | 11:5ebcb52726cf | 1424 | if(adj>0)sums[i]+=adj; |
jah128 | 11:5ebcb52726cf | 1425 | } |
jah128 | 11:5ebcb52726cf | 1426 | } |
jah128 | 11:5ebcb52726cf | 1427 | int peak_sum = 0; |
jah128 | 11:5ebcb52726cf | 1428 | int peak_position = 0; |
jah128 | 11:5ebcb52726cf | 1429 | for(int i=0; i<8; i++) { |
jah128 | 11:5ebcb52726cf | 1430 | if(sums[i]>peak_sum) { |
jah128 | 11:5ebcb52726cf | 1431 | peak_sum = sums[i]; |
jah128 | 11:5ebcb52726cf | 1432 | peak_position = i; |
jah128 | 11:5ebcb52726cf | 1433 | } |
jah128 | 11:5ebcb52726cf | 1434 | } |
jah128 | 11:5ebcb52726cf | 1435 | float relative_sums [8]; |
jah128 | 11:5ebcb52726cf | 1436 | float relative_total = 0; |
jah128 | 11:5ebcb52726cf | 1437 | for(int i=0; i<8; i++) { |
jah128 | 11:5ebcb52726cf | 1438 | relative_sums[i] = (float) sums[i] / (float) peak_sum; |
jah128 | 11:5ebcb52726cf | 1439 | relative_total += relative_sums[i]; |
jah128 | 11:5ebcb52726cf | 1440 | } |
jah128 | 11:5ebcb52726cf | 1441 | int previous = peak_position - 1; |
jah128 | 11:5ebcb52726cf | 1442 | int d_prev = peak_position - 2; |
jah128 | 11:5ebcb52726cf | 1443 | int d_next = peak_position + 2; |
jah128 | 11:5ebcb52726cf | 1444 | if(d_prev < 0) d_prev += 8; |
jah128 | 11:5ebcb52726cf | 1445 | if(d_next > 7) d_next -= 8; |
jah128 | 11:5ebcb52726cf | 1446 | if(previous<0) previous+=8; |
jah128 | 11:5ebcb52726cf | 1447 | int next = peak_position + 1; |
jah128 | 11:5ebcb52726cf | 1448 | if(next > 7) next -= 8; |
jah128 | 11:5ebcb52726cf | 1449 | angle_confidence = (relative_sums[next] + relative_sums[previous] + 1) / relative_total; |
jah128 | 11:5ebcb52726cf | 1450 | heading = ((relative_sums[next] - relative_sums[previous] - relative_sums[d_prev] - relative_sums[d_prev] + relative_sums[d_next] + relative_sums[d_next]) * .5) + peak_position; |
jah128 | 11:5ebcb52726cf | 1451 | if(heading<0) heading+= 8; |
jah128 | 11:5ebcb52726cf | 1452 | if(heading>8) heading-= 8; |
jah128 | 11:5ebcb52726cf | 1453 | //Convert heading to bearing |
jah128 | 11:5ebcb52726cf | 1454 | int whole_part = (int) heading; |
jah128 | 11:5ebcb52726cf | 1455 | float remainder = heading - whole_part; |
jah128 | 11:5ebcb52726cf | 1456 | i_bearing = 0; |
jah128 | 11:5ebcb52726cf | 1457 | switch(whole_part) { |
jah128 | 11:5ebcb52726cf | 1458 | case 0: |
jah128 | 11:5ebcb52726cf | 1459 | i_bearing = 15 + (30*remainder); |
jah128 | 11:5ebcb52726cf | 1460 | break; |
jah128 | 11:5ebcb52726cf | 1461 | case 1: |
jah128 | 11:5ebcb52726cf | 1462 | i_bearing = 45 + (45*remainder); |
jah128 | 11:5ebcb52726cf | 1463 | break; |
jah128 | 11:5ebcb52726cf | 1464 | case 2: |
jah128 | 11:5ebcb52726cf | 1465 | i_bearing = 90 + (62*remainder); |
jah128 | 11:5ebcb52726cf | 1466 | break; |
jah128 | 11:5ebcb52726cf | 1467 | case 3: |
jah128 | 11:5ebcb52726cf | 1468 | i_bearing = 152 + (56*remainder); |
jah128 | 11:5ebcb52726cf | 1469 | break; |
jah128 | 11:5ebcb52726cf | 1470 | case 4: |
jah128 | 11:5ebcb52726cf | 1471 | i_bearing = 208 + (62*remainder); |
jah128 | 11:5ebcb52726cf | 1472 | break; |
jah128 | 11:5ebcb52726cf | 1473 | case 5: |
jah128 | 11:5ebcb52726cf | 1474 | i_bearing = 270 + (45 * remainder); |
jah128 | 11:5ebcb52726cf | 1475 | break; |
jah128 | 11:5ebcb52726cf | 1476 | case 6: |
jah128 | 11:5ebcb52726cf | 1477 | i_bearing = 315 + (30 * remainder); |
jah128 | 11:5ebcb52726cf | 1478 | break; |
jah128 | 11:5ebcb52726cf | 1479 | case 7: |
jah128 | 11:5ebcb52726cf | 1480 | i_bearing = 345 + (30 * remainder); |
jah128 | 11:5ebcb52726cf | 1481 | break; |
jah128 | 11:5ebcb52726cf | 1482 | } |
jah128 | 11:5ebcb52726cf | 1483 | if(i_bearing>=360)i_bearing-=360; |
jah128 | 11:5ebcb52726cf | 1484 | } else angle_confidence = 0; |
jah128 | 11:5ebcb52726cf | 1485 | } |
jah128 | 11:5ebcb52726cf | 1486 | if(angle_confidence > 0.4 && confidence > 0.4) { |
jah128 | 11:5ebcb52726cf | 1487 | int delay = 1990+offset_time; |
jah128 | 11:5ebcb52726cf | 1488 | if(delay > 2800) delay -= 1000; |
jah128 | 11:5ebcb52726cf | 1489 | bearing = 360-i_bearing; |
jah128 | 11:5ebcb52726cf | 1490 | is_synced = 1; |
jah128 | 11:5ebcb52726cf | 1491 | saved_peak = total; |
jah128 | 11:5ebcb52726cf | 1492 | while(ir_timer.read_ms() < delay) wait_us(50); |
jah128 | 11:5ebcb52726cf | 1493 | } else is_synced = 0; |
jah128 | 11:5ebcb52726cf | 1494 | beacon_led = 0; |
jah128 | 11:5ebcb52726cf | 1495 | hit_sync_count = 0; |
jah128 | 11:5ebcb52726cf | 1496 | miss_sync_count = 0; |
jah128 | 11:5ebcb52726cf | 1497 | beacon_confidence = confidence; |
jah128 | 11:5ebcb52726cf | 1498 | bearing_confidence = angle_confidence; |
jah128 | 11:5ebcb52726cf | 1499 | return confidence; |
jah128 | 11:5ebcb52726cf | 1500 | } |
jah128 | 11:5ebcb52726cf | 1501 | |
jah128 | 11:5ebcb52726cf | 1502 | |
jah128 | 11:5ebcb52726cf | 1503 | float get_bearing_from_beacon() |
jah128 | 11:5ebcb52726cf | 1504 | { |
jah128 | 11:5ebcb52726cf | 1505 | beacon_led = 1; |
jah128 | 11:5ebcb52726cf | 1506 | float confidence = -1; |
jah128 | 11:5ebcb52726cf | 1507 | float angle_confidence = 0; |
jah128 | 11:5ebcb52726cf | 1508 | float heading = 0; |
jah128 | 11:5ebcb52726cf | 1509 | float i_bearing = 0; |
jah128 | 11:5ebcb52726cf | 1510 | Timer ir_timer; |
jah128 | 11:5ebcb52726cf | 1511 | unsigned short ir_values[8][5]; |
jah128 | 11:5ebcb52726cf | 1512 | unsigned int ir_totals[8]; |
jah128 | 11:5ebcb52726cf | 1513 | ir_timer.start(); |
jah128 | 11:5ebcb52726cf | 1514 | int target_time = 0; |
jah128 | 11:5ebcb52726cf | 1515 | for(int i=0; i<8; i++) { |
jah128 | 11:5ebcb52726cf | 1516 | ir_totals[i]=0; |
jah128 | 11:5ebcb52726cf | 1517 | } |
jah128 | 11:5ebcb52726cf | 1518 | for(int step = 0; step < 5; step ++) { |
jah128 | 11:5ebcb52726cf | 1519 | target_time += 10000 ; |
jah128 | 11:5ebcb52726cf | 1520 | piswarm.store_background_raw_ir_values (); |
jah128 | 11:5ebcb52726cf | 1521 | for(int sensor = 0; sensor < 8; sensor ++) { |
jah128 | 11:5ebcb52726cf | 1522 | ir_values[sensor][step] = piswarm.get_background_raw_ir_value(sensor); |
jah128 | 11:5ebcb52726cf | 1523 | ir_totals[sensor] += ir_values[sensor][step]; |
jah128 | 11:5ebcb52726cf | 1524 | } |
jah128 | 11:5ebcb52726cf | 1525 | while(ir_timer.read_us() < target_time) wait_us(50); |
jah128 | 11:5ebcb52726cf | 1526 | } |
jah128 | 11:5ebcb52726cf | 1527 | int total = 0; |
jah128 | 11:5ebcb52726cf | 1528 | for(int i=0; i<8; i++) { |
jah128 | 11:5ebcb52726cf | 1529 | total += ir_totals[i]; |
jah128 | 11:5ebcb52726cf | 1530 | } |
jah128 | 11:5ebcb52726cf | 1531 | total /= 40; |
jah128 | 11:5ebcb52726cf | 1532 | unsigned short s_pass [5]; |
jah128 | 11:5ebcb52726cf | 1533 | int pass_count = 0; |
jah128 | 11:5ebcb52726cf | 1534 | for(int i=0; i<5; i++) { |
jah128 | 11:5ebcb52726cf | 1535 | signed short adjusted [8]; |
jah128 | 11:5ebcb52726cf | 1536 | int pass = 0; |
jah128 | 11:5ebcb52726cf | 1537 | for(int k=0; k<8; k++) { |
jah128 | 11:5ebcb52726cf | 1538 | adjusted[k]=ir_values[k][i]-total; |
jah128 | 11:5ebcb52726cf | 1539 | if(adjusted[k] > 0) pass++; |
jah128 | 11:5ebcb52726cf | 1540 | } |
jah128 | 11:5ebcb52726cf | 1541 | s_pass [i] = pass; |
jah128 | 11:5ebcb52726cf | 1542 | if(pass>0) pass_count++; |
jah128 | 11:5ebcb52726cf | 1543 | //pc.printf("I:%d 1:%d 2:%d 3:%d 4:%d 5:%d 6:%d 7:%d 8:%d T:%d\n",i,ir_values[0][i],ir_values[1][i],ir_values[2][i],ir_values[3][i],ir_values[4][i],ir_values[5][i],ir_values[6][i],ir_values[7][i],total); |
jah128 | 11:5ebcb52726cf | 1544 | } |
jah128 | 11:5ebcb52726cf | 1545 | if(pass_count < 2 || pass_count > 3 || total<(saved_peak / 4)) { |
jah128 | 11:5ebcb52726cf | 1546 | confidence = -1; |
jah128 | 11:5ebcb52726cf | 1547 | angle_confidence = 0; |
jah128 | 11:5ebcb52726cf | 1548 | } else { |
jah128 | 11:5ebcb52726cf | 1549 | confidence = 1; |
jah128 | 11:5ebcb52726cf | 1550 | |
jah128 | 11:5ebcb52726cf | 1551 | unsigned short sums[8]; |
jah128 | 11:5ebcb52726cf | 1552 | for(int i=0; i<8; i++) { |
jah128 | 11:5ebcb52726cf | 1553 | sums[i] = 0; |
jah128 | 11:5ebcb52726cf | 1554 | for(int l=1; l<4; l++) { |
jah128 | 11:5ebcb52726cf | 1555 | signed short adj = ir_values[i][l] - total; |
jah128 | 11:5ebcb52726cf | 1556 | if(adj>0)sums[i]+=adj; |
jah128 | 11:5ebcb52726cf | 1557 | } |
jah128 | 11:5ebcb52726cf | 1558 | } |
jah128 | 11:5ebcb52726cf | 1559 | int peak_sum = 0; |
jah128 | 11:5ebcb52726cf | 1560 | int peak_position = 0; |
jah128 | 11:5ebcb52726cf | 1561 | for(int i=0; i<8; i++) { |
jah128 | 11:5ebcb52726cf | 1562 | if(sums[i]>peak_sum) { |
jah128 | 11:5ebcb52726cf | 1563 | peak_sum = sums[i]; |
jah128 | 11:5ebcb52726cf | 1564 | peak_position = i; |
jah128 | 11:5ebcb52726cf | 1565 | } |
jah128 | 11:5ebcb52726cf | 1566 | } |
jah128 | 11:5ebcb52726cf | 1567 | float relative_sums [8]; |
jah128 | 11:5ebcb52726cf | 1568 | float relative_total = 0; |
jah128 | 11:5ebcb52726cf | 1569 | for(int i=0; i<8; i++) { |
jah128 | 11:5ebcb52726cf | 1570 | relative_sums[i] = (float) sums[i] / (float) peak_sum; |
jah128 | 11:5ebcb52726cf | 1571 | relative_total += relative_sums[i]; |
jah128 | 11:5ebcb52726cf | 1572 | } |
jah128 | 11:5ebcb52726cf | 1573 | int previous = peak_position - 1; |
jah128 | 11:5ebcb52726cf | 1574 | int d_prev = peak_position - 2; |
jah128 | 11:5ebcb52726cf | 1575 | int d_next = peak_position + 2; |
jah128 | 11:5ebcb52726cf | 1576 | if(d_prev < 0) d_prev += 8; |
jah128 | 11:5ebcb52726cf | 1577 | if(d_next > 7) d_next -= 8; |
jah128 | 11:5ebcb52726cf | 1578 | if(previous<0) previous+=8; |
jah128 | 11:5ebcb52726cf | 1579 | int next = peak_position + 1; |
jah128 | 11:5ebcb52726cf | 1580 | if(next > 7) next -= 8; |
jah128 | 11:5ebcb52726cf | 1581 | angle_confidence = (relative_sums[next] + relative_sums[previous] + 1) / relative_total; |
jah128 | 11:5ebcb52726cf | 1582 | heading = ((relative_sums[next] - relative_sums[previous] - relative_sums[d_prev] - relative_sums[d_prev] + relative_sums[d_next] + relative_sums[d_next]) * .5) + peak_position; |
jah128 | 11:5ebcb52726cf | 1583 | if(heading<0) heading+= 8; |
jah128 | 11:5ebcb52726cf | 1584 | if(heading>8) heading-= 8; |
jah128 | 11:5ebcb52726cf | 1585 | //Convert heading to bearing |
jah128 | 11:5ebcb52726cf | 1586 | int whole_part = (int) heading; |
jah128 | 11:5ebcb52726cf | 1587 | float remainder = heading - whole_part; |
jah128 | 11:5ebcb52726cf | 1588 | i_bearing = 0; |
jah128 | 11:5ebcb52726cf | 1589 | switch(whole_part) { |
jah128 | 11:5ebcb52726cf | 1590 | case 0: |
jah128 | 11:5ebcb52726cf | 1591 | i_bearing = 15 + (30*remainder); |
jah128 | 11:5ebcb52726cf | 1592 | break; |
jah128 | 11:5ebcb52726cf | 1593 | case 1: |
jah128 | 11:5ebcb52726cf | 1594 | i_bearing = 45 + (45*remainder); |
jah128 | 11:5ebcb52726cf | 1595 | break; |
jah128 | 11:5ebcb52726cf | 1596 | case 2: |
jah128 | 11:5ebcb52726cf | 1597 | i_bearing = 90 + (62*remainder); |
jah128 | 11:5ebcb52726cf | 1598 | break; |
jah128 | 11:5ebcb52726cf | 1599 | case 3: |
jah128 | 11:5ebcb52726cf | 1600 | i_bearing = 152 + (56*remainder); |
jah128 | 11:5ebcb52726cf | 1601 | break; |
jah128 | 11:5ebcb52726cf | 1602 | case 4: |
jah128 | 11:5ebcb52726cf | 1603 | i_bearing = 208 + (62*remainder); |
jah128 | 11:5ebcb52726cf | 1604 | break; |
jah128 | 11:5ebcb52726cf | 1605 | case 5: |
jah128 | 11:5ebcb52726cf | 1606 | i_bearing = 270 + (45 * remainder); |
jah128 | 11:5ebcb52726cf | 1607 | break; |
jah128 | 11:5ebcb52726cf | 1608 | case 6: |
jah128 | 11:5ebcb52726cf | 1609 | i_bearing = 315 + (30 * remainder); |
jah128 | 11:5ebcb52726cf | 1610 | break; |
jah128 | 11:5ebcb52726cf | 1611 | case 7: |
jah128 | 11:5ebcb52726cf | 1612 | i_bearing = 345 + (30 * remainder); |
jah128 | 11:5ebcb52726cf | 1613 | break; |
jah128 | 11:5ebcb52726cf | 1614 | } |
jah128 | 11:5ebcb52726cf | 1615 | if(i_bearing>=360)i_bearing-=360; |
jah128 | 11:5ebcb52726cf | 1616 | |
jah128 | 11:5ebcb52726cf | 1617 | } |
jah128 | 11:5ebcb52726cf | 1618 | if(angle_confidence > 0.4 && confidence==1) { |
jah128 | 11:5ebcb52726cf | 1619 | bearing = 360-i_bearing; |
jah128 | 11:5ebcb52726cf | 1620 | is_synced = 1; |
jah128 | 11:5ebcb52726cf | 1621 | hit_sync_count ++; |
jah128 | 11:5ebcb52726cf | 1622 | if(hit_sync_count > 4) { |
jah128 | 11:5ebcb52726cf | 1623 | miss_sync_count = 0; |
jah128 | 11:5ebcb52726cf | 1624 | } |
jah128 | 11:5ebcb52726cf | 1625 | } else is_synced = 0; |
jah128 | 11:5ebcb52726cf | 1626 | beacon_led = 0; |
jah128 | 11:5ebcb52726cf | 1627 | beacon_confidence = confidence; |
jah128 | 11:5ebcb52726cf | 1628 | bearing_confidence = angle_confidence; |
jah128 | 11:5ebcb52726cf | 1629 | return confidence; |
jah128 | 11:5ebcb52726cf | 1630 | } |
jah128 | 11:5ebcb52726cf | 1631 | |
jah128 | 11:5ebcb52726cf | 1632 | void update_bearing() |
jah128 | 11:5ebcb52726cf | 1633 | { |
jah128 | 11:5ebcb52726cf | 1634 | get_bearing_from_beacon(); |
jah128 | 11:5ebcb52726cf | 1635 | if(is_synced == 1) { |
jah128 | 11:5ebcb52726cf | 1636 | if(DISPLAY_BEACON_READING == 1) display_bearing_info(); |
jah128 | 11:5ebcb52726cf | 1637 | highlight_bearing(); |
jah128 | 11:5ebcb52726cf | 1638 | } else { |
jah128 | 11:5ebcb52726cf | 1639 | miss_sync_count ++; |
jah128 | 11:5ebcb52726cf | 1640 | hit_sync_count = 0; |
jah128 | 11:5ebcb52726cf | 1641 | if(miss_sync_count > 3) { |
jah128 | 11:5ebcb52726cf | 1642 | beacon_ticker.detach(); |
jah128 | 11:5ebcb52726cf | 1643 | if(DISPLAY_BEACON_READING == 1) { |
jah128 | 11:5ebcb52726cf | 1644 | piswarm.cls(); |
jah128 | 11:5ebcb52726cf | 1645 | piswarm.printf("Lost "); |
jah128 | 11:5ebcb52726cf | 1646 | piswarm.locate(0,1); |
jah128 | 11:5ebcb52726cf | 1647 | piswarm.printf("Beacon "); |
jah128 | 11:5ebcb52726cf | 1648 | } |
jah128 | 11:5ebcb52726cf | 1649 | beacon_ticker.attach(resync_with_beacon,3.9); |
jah128 | 11:5ebcb52726cf | 1650 | } |
jah128 | 11:5ebcb52726cf | 1651 | oled_ticker.detach(); |
jah128 | 11:5ebcb52726cf | 1652 | if(DISPLAY_BEACON_READING)piswarm.cls(); |
jah128 | 11:5ebcb52726cf | 1653 | } |
jah128 | 11:5ebcb52726cf | 1654 | } |
jah128 | 11:5ebcb52726cf | 1655 | |
jah128 | 11:5ebcb52726cf | 1656 | void resync_with_beacon() |
jah128 | 11:5ebcb52726cf | 1657 | { |
jah128 | 11:5ebcb52726cf | 1658 | if(synchronise_with_beacon() > 0.4) { |
jah128 | 11:5ebcb52726cf | 1659 | beacon_ticker.detach(); |
jah128 | 11:5ebcb52726cf | 1660 | beacon_ticker.attach(update_bearing,1); |
jah128 | 11:5ebcb52726cf | 1661 | if(DISPLAY_BEACON_READING == 1) display_bearing_info(); |
jah128 | 11:5ebcb52726cf | 1662 | else { |
jah128 | 11:5ebcb52726cf | 1663 | piswarm.cls(); |
jah128 | 11:5ebcb52726cf | 1664 | piswarm.printf("SYNCED"); |
jah128 | 11:5ebcb52726cf | 1665 | piswarm.locate(0,1); |
jah128 | 11:5ebcb52726cf | 1666 | piswarm.printf("B:%3.1f",bearing); |
jah128 | 11:5ebcb52726cf | 1667 | } |
jah128 | 11:5ebcb52726cf | 1668 | highlight_bearing(); |
jah128 | 11:5ebcb52726cf | 1669 | } else { |
jah128 | 11:5ebcb52726cf | 1670 | piswarm.cls(); |
jah128 | 11:5ebcb52726cf | 1671 | piswarm.printf("NO SYNC"); |
jah128 | 11:5ebcb52726cf | 1672 | piswarm.turn_off_all_oleds(); |
jah128 | 11:5ebcb52726cf | 1673 | oled_ticker.detach(); |
jah128 | 11:5ebcb52726cf | 1674 | } |
jah128 | 11:5ebcb52726cf | 1675 | } |
jah128 | 11:5ebcb52726cf | 1676 | |
jah128 | 11:5ebcb52726cf | 1677 | void highlight_bearing() |
jah128 | 11:5ebcb52726cf | 1678 | { |
jah128 | 11:5ebcb52726cf | 1679 | piswarm.save_oled_state(); |
jah128 | 11:5ebcb52726cf | 1680 | bearing_strobe_step = 0; |
jah128 | 11:5ebcb52726cf | 1681 | piswarm.set_oled_colour(255,255,255); |
jah128 | 11:5ebcb52726cf | 1682 | oled_ticker.detach(); |
jah128 | 11:5ebcb52726cf | 1683 | oled_ticker.attach(oled_bearing_strobe,0.02); |
jah128 | 11:5ebcb52726cf | 1684 | } |
jah128 | 11:5ebcb52726cf | 1685 | |
jah128 | 11:5ebcb52726cf | 1686 | void display_bearing_info() |
jah128 | 11:5ebcb52726cf | 1687 | { |
jah128 | 11:5ebcb52726cf | 1688 | piswarm.cls(); |
jah128 | 11:5ebcb52726cf | 1689 | piswarm.printf("B:%3.1f",bearing); |
jah128 | 11:5ebcb52726cf | 1690 | piswarm.locate(0,1); |
jah128 | 11:5ebcb52726cf | 1691 | piswarm.printf("C:%3.1f",bearing_confidence*100); |
jah128 | 11:5ebcb52726cf | 1692 | } |
jah128 | 11:5ebcb52726cf | 1693 | |
jah128 | 11:5ebcb52726cf | 1694 | void init() |
jah128 | 11:5ebcb52726cf | 1695 | { |
jah128 | 0:9ffe8ebd1c40 | 1696 | //Standard initialisation routine for Pi Swarm Robot |
jah128 | 0:9ffe8ebd1c40 | 1697 | //Displays a message on the screen and flashes the central LED |
jah128 | 0:9ffe8ebd1c40 | 1698 | //Calibrates the gyro and accelerometer |
jah128 | 0:9ffe8ebd1c40 | 1699 | //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 | 1700 | piswarm.start_system_timer(); |
jah128 | 0:9ffe8ebd1c40 | 1701 | pc.baud(PC_BAUD); |
jah128 | 11:5ebcb52726cf | 1702 | if(PISWARM_DEBUG == 1)pc.printf("PiSwarm Firmware 0.7 Initialisation...\n"); |
jah128 | 0:9ffe8ebd1c40 | 1703 | display_system_time(); |
jah128 | 11:5ebcb52726cf | 1704 | piswarm.play_tune("T596MSCEFGAB>C",14); |
jah128 | 0:9ffe8ebd1c40 | 1705 | piswarm.cls(); |
jah128 | 0:9ffe8ebd1c40 | 1706 | piswarm.enable_cled(1); |
jah128 | 0:9ffe8ebd1c40 | 1707 | piswarm.set_cled_colour(200,0,0); |
jah128 | 0:9ffe8ebd1c40 | 1708 | piswarm.set_oled_colour(200,0,0); |
jah128 | 0:9ffe8ebd1c40 | 1709 | piswarm.printf(" YORK "); |
jah128 | 0:9ffe8ebd1c40 | 1710 | piswarm.locate(0,1); |
jah128 | 0:9ffe8ebd1c40 | 1711 | piswarm.printf("ROBOTLAB"); |
jah128 | 11:5ebcb52726cf | 1712 | oled_ticker.attach(oled_ticker_update,0.02); |
jah128 | 11:5ebcb52726cf | 1713 | wait(0.2); |
jah128 | 0:9ffe8ebd1c40 | 1714 | piswarm.set_cled_colour(0,200,0); |
jah128 | 0:9ffe8ebd1c40 | 1715 | piswarm.set_oled_colour(0,200,0); |
jah128 | 0:9ffe8ebd1c40 | 1716 | piswarm.cls(); |
jah128 | 0:9ffe8ebd1c40 | 1717 | piswarm.printf("Pi Swarm"); |
jah128 | 0:9ffe8ebd1c40 | 1718 | piswarm.locate(0,1); |
jah128 | 0:9ffe8ebd1c40 | 1719 | piswarm.printf("ID : %d ",piswarm.get_id()); |
jah128 | 11:5ebcb52726cf | 1720 | wait(0.2); |
jah128 | 11:5ebcb52726cf | 1721 | piswarm.set_cled_colour(0,0,200); |
jah128 | 11:5ebcb52726cf | 1722 | piswarm.set_oled_colour(0,0,200); |
jah128 | 11:5ebcb52726cf | 1723 | if(CALIBRATE_MEMS == 1) { |
jah128 | 0:9ffe8ebd1c40 | 1724 | piswarm.cls(); |
jah128 | 11:5ebcb52726cf | 1725 | if(PISWARM_DEBUG == 1)pc.printf("Test Magnetometer: "); |
jah128 | 11:5ebcb52726cf | 1726 | if(piswarm.calibrate_magnetometer() != 0) { |
jah128 | 11:5ebcb52726cf | 1727 | if(PISWARM_DEBUG == 1)pc.printf("FAILED\n"); |
jah128 | 11:5ebcb52726cf | 1728 | piswarm.cls(); |
jah128 | 11:5ebcb52726cf | 1729 | piswarm.locate(0,0); |
jah128 | 11:5ebcb52726cf | 1730 | piswarm.printf("Mag Cal "); |
jah128 | 11:5ebcb52726cf | 1731 | piswarm.locate(0,1); |
jah128 | 11:5ebcb52726cf | 1732 | piswarm.printf("Failed "); |
jah128 | 11:5ebcb52726cf | 1733 | wait(0.1); |
jah128 | 11:5ebcb52726cf | 1734 | } else if(PISWARM_DEBUG == 1)pc.printf("PASSED\n"); |
jah128 | 11:5ebcb52726cf | 1735 | if(PISWARM_DEBUG == 1)pc.printf("Test Gyroscope: "); |
jah128 | 11:5ebcb52726cf | 1736 | if(piswarm.calibrate_gyro() == 0) { |
jah128 | 11:5ebcb52726cf | 1737 | if(PISWARM_DEBUG == 1)pc.printf("FAILED\n"); |
jah128 | 11:5ebcb52726cf | 1738 | wait(0.1); |
jah128 | 11:5ebcb52726cf | 1739 | piswarm.cls(); |
jah128 | 11:5ebcb52726cf | 1740 | piswarm.locate(0,0); |
jah128 | 11:5ebcb52726cf | 1741 | piswarm.printf("Gyro Cal"); |
jah128 | 11:5ebcb52726cf | 1742 | piswarm.locate(0,1); |
jah128 | 11:5ebcb52726cf | 1743 | piswarm.printf("Failed "); |
jah128 | 11:5ebcb52726cf | 1744 | wait(0.1); |
jah128 | 11:5ebcb52726cf | 1745 | } else if(PISWARM_DEBUG == 1)pc.printf("PASSED\n"); |
jah128 | 11:5ebcb52726cf | 1746 | if(PISWARM_DEBUG == 1)pc.printf("Test Accelerometer: "); |
jah128 | 11:5ebcb52726cf | 1747 | if(piswarm.calibrate_accelerometer() == 0) { |
jah128 | 11:5ebcb52726cf | 1748 | if(PISWARM_DEBUG == 1)pc.printf("FAILED\n"); |
jah128 | 11:5ebcb52726cf | 1749 | wait(0.1); |
jah128 | 11:5ebcb52726cf | 1750 | piswarm.cls(); |
jah128 | 11:5ebcb52726cf | 1751 | piswarm.locate(0,0); |
jah128 | 11:5ebcb52726cf | 1752 | piswarm.printf("Acc. Cal"); |
jah128 | 11:5ebcb52726cf | 1753 | piswarm.locate(0,1); |
jah128 | 11:5ebcb52726cf | 1754 | piswarm.printf("Failed "); |
jah128 | 11:5ebcb52726cf | 1755 | wait(0.1); |
jah128 | 11:5ebcb52726cf | 1756 | } else if(PISWARM_DEBUG == 1)pc.printf("PASSED\n"); |
jah128 | 11:5ebcb52726cf | 1757 | } else { |
jah128 | 11:5ebcb52726cf | 1758 | wait(0.2); |
jah128 | 11:5ebcb52726cf | 1759 | } |
jah128 | 11:5ebcb52726cf | 1760 | oled_ticker.detach(); |
jah128 | 11:5ebcb52726cf | 1761 | piswarm.turn_off_all_oleds(); |
jah128 | 11:5ebcb52726cf | 1762 | if(USE_BEACON == 1) { |
jah128 | 0:9ffe8ebd1c40 | 1763 | piswarm.cls(); |
jah128 | 11:5ebcb52726cf | 1764 | piswarm.printf("LOCATING"); |
jah128 | 0:9ffe8ebd1c40 | 1765 | piswarm.locate(0,1); |
jah128 | 11:5ebcb52726cf | 1766 | piswarm.printf(" BEACON"); |
jah128 | 11:5ebcb52726cf | 1767 | piswarm.set_cled_colour(0,200,0); |
jah128 | 11:5ebcb52726cf | 1768 | piswarm.set_oled_colour(150,200,150); |
jah128 | 11:5ebcb52726cf | 1769 | oled_ticker.attach(oled_ticker_update,0.1); |
jah128 | 11:5ebcb52726cf | 1770 | if(synchronise_with_beacon() > 0.4) { |
jah128 | 11:5ebcb52726cf | 1771 | beacon_ticker.attach(update_bearing,1); |
jah128 | 11:5ebcb52726cf | 1772 | if(DISPLAY_BEACON_READING == 1) display_bearing_info(); |
jah128 | 11:5ebcb52726cf | 1773 | else { |
jah128 | 11:5ebcb52726cf | 1774 | piswarm.cls(); |
jah128 | 11:5ebcb52726cf | 1775 | piswarm.printf("SYNCED"); |
jah128 | 11:5ebcb52726cf | 1776 | piswarm.locate(0,1); |
jah128 | 11:5ebcb52726cf | 1777 | piswarm.printf("B:%3.1f",bearing); |
jah128 | 11:5ebcb52726cf | 1778 | } |
jah128 | 11:5ebcb52726cf | 1779 | highlight_bearing(); |
jah128 | 11:5ebcb52726cf | 1780 | } else { |
jah128 | 11:5ebcb52726cf | 1781 | piswarm.cls(); |
jah128 | 11:5ebcb52726cf | 1782 | piswarm.printf(" 2ND"); |
jah128 | 11:5ebcb52726cf | 1783 | piswarm.locate(0,1); |
jah128 | 11:5ebcb52726cf | 1784 | piswarm.printf("ATTEMPT"); |
jah128 | 11:5ebcb52726cf | 1785 | if(synchronise_with_beacon() > 0.4) { |
jah128 | 11:5ebcb52726cf | 1786 | beacon_ticker.attach(update_bearing,1); |
jah128 | 11:5ebcb52726cf | 1787 | if(DISPLAY_BEACON_READING == 1) display_bearing_info(); |
jah128 | 11:5ebcb52726cf | 1788 | else { |
jah128 | 11:5ebcb52726cf | 1789 | piswarm.cls(); |
jah128 | 11:5ebcb52726cf | 1790 | piswarm.printf("SYNCED"); |
jah128 | 11:5ebcb52726cf | 1791 | piswarm.locate(0,1); |
jah128 | 11:5ebcb52726cf | 1792 | piswarm.printf("B:%3.1f",bearing); |
jah128 | 11:5ebcb52726cf | 1793 | } |
jah128 | 11:5ebcb52726cf | 1794 | highlight_bearing(); |
jah128 | 11:5ebcb52726cf | 1795 | } else { |
jah128 | 11:5ebcb52726cf | 1796 | piswarm.cls(); |
jah128 | 11:5ebcb52726cf | 1797 | piswarm.printf("NO SYNC"); |
jah128 | 11:5ebcb52726cf | 1798 | piswarm.turn_off_all_oleds(); |
jah128 | 11:5ebcb52726cf | 1799 | oled_ticker.detach(); |
jah128 | 11:5ebcb52726cf | 1800 | beacon_ticker.attach(resync_with_beacon,10); |
jah128 | 11:5ebcb52726cf | 1801 | } |
jah128 | 11:5ebcb52726cf | 1802 | } |
jah128 | 11:5ebcb52726cf | 1803 | } |
jah128 | 11:5ebcb52726cf | 1804 | wait(0.5); |
jah128 | 11:5ebcb52726cf | 1805 | oled_ticker.detach(); |
jah128 | 0:9ffe8ebd1c40 | 1806 | piswarm.turn_off_all_oleds(); |
jah128 | 0:9ffe8ebd1c40 | 1807 | piswarm.cls(); |
jah128 | 0:9ffe8ebd1c40 | 1808 | piswarm.set_cled_colour(0,0,0); |
jah128 | 11:5ebcb52726cf | 1809 | if(START_RADIO_ON_BOOT == 1) { |
jah128 | 0:9ffe8ebd1c40 | 1810 | if(PISWARM_DEBUG == 1)pc.printf("Setting up 433MHz Transceiver\n"); |
jah128 | 0:9ffe8ebd1c40 | 1811 | piswarm.setup_radio(); |
jah128 | 0:9ffe8ebd1c40 | 1812 | } |
jah128 | 0:9ffe8ebd1c40 | 1813 | } |
jah128 | 0:9ffe8ebd1c40 | 1814 | |
jah128 | 0:9ffe8ebd1c40 | 1815 | /******************************************************************************** |
jah128 | 0:9ffe8ebd1c40 | 1816 | * COPYRIGHT NOTICE * |
jah128 | 0:9ffe8ebd1c40 | 1817 | * * |
jah128 | 4:52b3e4c5a425 | 1818 | * Parts of code based on the original m3pi library, Copyright (c) 2010 cstyles * |
jah128 | 4:52b3e4c5a425 | 1819 | * * |
jah128 | 0:9ffe8ebd1c40 | 1820 | * Permission is hereby granted, free of charge, to any person obtaining a copy * |
jah128 | 0:9ffe8ebd1c40 | 1821 | * of this software and associated documentation files (the "Software"), to deal* |
jah128 | 0:9ffe8ebd1c40 | 1822 | * in the Software without restriction, including without limitation the rights * |
jah128 | 0:9ffe8ebd1c40 | 1823 | * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell * |
jah128 | 11:5ebcb52726cf | 1824 | * copies of the Software, and to permit persons to whom the Software is * |
jah128 | 0:9ffe8ebd1c40 | 1825 | * furnished to do so, subject to the following conditions: * |
jah128 | 0:9ffe8ebd1c40 | 1826 | * * |
jah128 | 0:9ffe8ebd1c40 | 1827 | * The above copyright notice and this permission notice shall be included in * |
jah128 | 0:9ffe8ebd1c40 | 1828 | * all copies or substantial portions of the Software. * |
jah128 | 0:9ffe8ebd1c40 | 1829 | * * |
jah128 | 0:9ffe8ebd1c40 | 1830 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * |
jah128 | 0:9ffe8ebd1c40 | 1831 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, * |
jah128 | 0:9ffe8ebd1c40 | 1832 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * |
jah128 | 0:9ffe8ebd1c40 | 1833 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * |
jah128 | 0:9ffe8ebd1c40 | 1834 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,* |
jah128 | 0:9ffe8ebd1c40 | 1835 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN * |
jah128 | 0:9ffe8ebd1c40 | 1836 | * THE SOFTWARE. * |
jah128 | 0:9ffe8ebd1c40 | 1837 | * * |
jah128 | 0:9ffe8ebd1c40 | 1838 | ********************************************************************************/ |
jah128 | 0:9ffe8ebd1c40 | 1839 | |
jah128 | 0:9ffe8ebd1c40 | 1840 |