James Hilder / Pi_Swarm_Library_7

Unfinished v0.7, added bearing detection

Fork of Pi_Swarm_Library_v06_alpha by piswarm

piswarm.cpp@4:52b3e4c5a425, 2014-02-02 (annotated)

Committer:
jah128
Date:
Sun Feb 02 22:30:47 2014 +0000
Revision:
4:52b3e4c5a425
Parent:
3:4c0f2f3de33e
Child:
9:7a4fc1d7e484
Fixed warnings;

Who changed what in which revision?

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