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