James Hilder / Pi_Swarm_Library_7

Unfinished v0.7, added bearing detection

Fork of Pi_Swarm_Library_v06_alpha by piswarm

piswarm.cpp@0:9ffe8ebd1c40, 2014-01-31 (annotated)

Committer:
jah128
Date:
Fri Jan 31 22:59:25 2014 +0000
Revision:
0:9ffe8ebd1c40
Child:
1:b067a08ff54e
First commit

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