James Hilder / Pi_Swarm_Library_7

Unfinished v0.7, added bearing detection

Fork of Pi_Swarm_Library_v06_alpha by piswarm

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
Child:
11:5ebcb52726cf
Draft v0.6 Library (unfinished)

Who changed what in which revision?

UserRevisionLine numberNew 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