James Hilder / Pi_Swarm_Library_7

Unfinished v0.7, added bearing detection

Fork of Pi_Swarm_Library_v06_alpha by piswarm

piswarm.cpp@1:b067a08ff54e, 2014-02-02 (annotated)

Committer:
jah128
Date:
Sun Feb 02 18:05:58 2014 +0000
Revision:
1:b067a08ff54e
Parent:
0:9ffe8ebd1c40
Child:
3:4c0f2f3de33e
updated comms library;

Who changed what in which revision?

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