James Hilder
Unfinished v0.7, added bearing detection
Fork of
Pi_Swarm_Library_v06_alpha
by 
piswarm
Diff: piswarm.cpp
- Revision:
- 0:9ffe8ebd1c40
- Child:
- 1:b067a08ff54e
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/piswarm.cpp Fri Jan 31 22:59:25 2014 +0000
@@ -0,0 +1,939 @@
+/* University of York Robot Lab Pi Swarm Robot Library
+ *
+ * (C) Dr James Hilder, Dept. Electronics & Computer Science, University of York
+ *
+ * Version 0.4 January 2014
+ *
+ * Designed for use with the Pi Swarm Board (enhanced MBED sensor board) v1.2
+ *
+ * Based on the original m3pi library, Copyright (c) 2007-2010 cstyles (see copyright notice at end of file) *
+ */
+
+#include "piswarm.h"
+
+// Variables
+DigitalOut gpio_led (LED1);
+DigitalOut calibrate_led (LED2);
+DigitalOut adc_led (LED3);
+DigitalOut interrupt_led (LED4);
+InterruptIn expansion_interrupt (p29);
+Timeout debounce_timeout;
+Timeout led_timeout;
+Ticker calibrate_ticker;
+char id;
+
+char oled_array [10];
+char enable_ir_ldo = 0;
+char enable_led_ldo = 0;
+char calibrate_led_state = 1;
+char switches = 0;
+char cled_red = 0;
+char cled_green = 0;
+char cled_blue = 0;
+char oled_red = 0;
+char oled_green = 0;
+char oled_blue = 0;
+char cled_enable = 0;
+char cled_brightness = CENTER_LED_BRIGHTNESS; // The default brightness value: must be between 0 (dimmest) and 100 (full brightness)
+char oled_brightness = OUTER_LED_BRIGHTNESS; // The default brightness value: must be between 0 (dimmest) and 100 (full brightness)
+float gyro_cal = 3.3;
+float gyro_error = 0;
+float acc_x_cal = 3350;
+float acc_y_cal = 3350;
+float acc_z_cal = 3350;
+float left_speed = 0;
+float right_speed = 0;
+signed short mag_values [3];
+
+
+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) {
+ setup();
+ reset();
+}
+
+int PiSwarm::get_oled_colour(){
+ return (oled_red << 16) + (oled_green << 8) + oled_blue;
+}
+
+int PiSwarm::get_cled_colour(){
+ return (cled_red << 16) + (cled_green << 8) + cled_blue;
+}
+
+void PiSwarm::set_oled(char oled, char value){
+ oled_array[oled]=value;
+}
+
+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){
+ oled_array[0]=oled_0;
+ oled_array[1]=oled_1;
+ oled_array[2]=oled_2;
+ oled_array[3]=oled_3;
+ oled_array[4]=oled_4;
+ oled_array[5]=oled_5;
+ oled_array[6]=oled_6;
+ oled_array[7]=oled_7;
+ oled_array[8]=oled_8;
+ oled_array[9]=oled_9;
+ activate_oleds();
+}
+
+// Switches off all outer LEDs
+void PiSwarm::turn_off_all_oleds(){
+ for(int i=0;i<10;i++){
+ oled_array[i]=0;
+ }
+ activate_oleds();
+ enable_led_ldo = 0;
+ enable_ldo_outputs();
+ char data[3];
+ data[0]=0x88; //Write to bank 1 then bank 2
+ data[1]=0x00; //Reset everything in bank 1
+ data[2]=0x00; //Reset everything in bank 2
+ _i2c.write(EXPANSION_IC_ADDRESS,data,3,false);
+}
+
+
+// Sends the messages to enable/disable outer LEDs
+void PiSwarm::activate_oleds(){
+ if(enable_led_ldo == 0){
+ enable_led_ldo = 1;
+ enable_ldo_outputs();
+ }
+ char data[3];
+
+ data[0]=0x88; //Write to bank 1 then bank 2
+ data[1]=0x00; //Reset everything in bank 1
+ data[2]=0x00; //Reset everything in bank 2
+ if(oled_array[0]>0) data[1]+=1;
+ if(oled_array[1]>0) data[1]+=2;
+ if(oled_array[2]>0) data[1]+=4;
+ if(oled_array[3]>0) data[1]+=8;
+ if(oled_array[4]>0) data[1]+=16;
+ if(oled_array[5]>0) data[1]+=32;
+ if(oled_array[6]>0) data[1]+=64;
+ if(oled_array[7]>0) data[1]+=128;
+ if(oled_array[8]>0) data[2]+=1;
+ if(oled_array[9]>0) data[2]+=2;
+ _i2c.write(EXPANSION_IC_ADDRESS,data,3,false);
+}
+
+void PiSwarm::test_oled(){
+ enable_led_ldo = 1;
+ enable_ldo_outputs();
+ set_oled_colour(100,100,100);
+ char data[2];
+ data[0] = 0x09; //Address for PCA9505 Output Port 1
+ data[1] = 3;
+ _i2c.write(EXPANSION_IC_ADDRESS,data,2,false);
+}
+
+void PiSwarm::send_rf_message(char* message, char length){
+ _rf.sendString(length, message);
+}
+
+void PiSwarm::set_oled_colour( char red, char green, char blue ){
+ oled_red = red;
+ oled_green = green;
+ oled_blue = blue;
+ _oled_r.pulsewidth_us(red);
+ _oled_g.pulsewidth_us(green);
+ _oled_b.pulsewidth_us(blue);
+}
+
+void PiSwarm::set_cled_colour( char red, char green, char blue ){
+ cled_red = red;
+ cled_green = green;
+ cled_blue = blue;
+ if(cled_enable != 0) enable_cled(1);
+}
+
+void PiSwarm::enable_cled( char enable ){
+ cled_enable = enable;
+ if(enable != 0) {
+ _cled_r.pulsewidth_us(cled_red);
+ _cled_g.pulsewidth_us(cled_green);
+ _cled_b.pulsewidth_us(cled_blue);
+ }else{
+ _cled_r.pulsewidth_us(0);
+ _cled_g.pulsewidth_us(0);
+ _cled_b.pulsewidth_us(0);
+ }
+}
+
+void PiSwarm::setup () {
+ _ser.baud(115200);
+ set_oled_brightness (oled_brightness);
+ set_cled_brightness (cled_brightness);
+ gpio_led = setup_expansion_ic();
+ adc_led = setup_adc();
+}
+
+void PiSwarm::setup_radio() {
+ //Setup RF transceiver
+ _rf.rf_init();
+ _rf.setFrequency(RF_FREQUENCY);
+ _rf.setDatarate(RF_DATARATE);
+}
+
+void PiSwarm::enable_ldo_outputs () {
+ char data[2];
+ data[0] = 0x0A; //Address for PCA9505 Output Port 2
+ data[1] = 0;
+ if(enable_led_ldo) data[1] +=128;
+ if(enable_ir_ldo) data[1] +=64;
+ _i2c.write(EXPANSION_IC_ADDRESS,data,2,false);
+}
+
+int PiSwarm::setup_expansion_ic () {
+ //Expansion IC is NXP PCA9505
+ //Address is 0x40 (defined by EXPANSION_IC_ADDRESS)
+ //Block IO 0 : 7-0 Are OLED Outputs
+ //Block IO 1 : 7 is NC. 6-2 are Cursor switches (interrupt inputs). 1 and 0 are OLED outputs.
+ //Block IO 2 : 7 and 6 are LDO enable outputs. 5 - 1 are ID switch. 0 is NC.
+ //Block IO 3 and 4 are for the expansion connector (not assigned here)
+ char data [4];
+ data [0] = 0x98; //Write to I/O configure register 0, using auto increment (allows multi-blocks in one command)
+ data [1] = 0x00; //All 8 pins in block 0 are outputs (0)
+ data [2] = 0xFC; //Pins 1 & 0 are outputs, rest are inputs (or NC)
+ data [3] = 0x3F; //Pins 7 & 6 are outputs, rest are inputs (or NC)
+ //If using the expansion port, assing its I/O here by increasing size of data[] and adding extra commands
+ int test_val = _i2c.write(EXPANSION_IC_ADDRESS,data,4,false);
+
+ // test_val should return 0 for correctly acknowledged response
+
+ //Invert the polarity for switch inputs (they connect to ground when pressed\on)
+ data [0] = 0x90; //Write to polarity inversion register using auto increment
+ data [1] = 0x00;
+ data [2] = 0x7C; //Invert pins 6-2 for cursor switches
+ data [3] = 0x3E; //Invert pins 5-1 for ID switch
+ _i2c.write(EXPANSION_IC_ADDRESS,data,4,false);
+
+ //Setup the interrupt masks. By default, only the direction switches trigger an interrupt
+ data [0] = 0xA0;
+ data [1] = 0xFF;
+ data [2] = 0x83; // Disable mask on pins 6-2 for cursor switches
+ data [3] = 0xFF;
+ _i2c.write(EXPANSION_IC_ADDRESS,data,4,false);
+
+ //Read the ID switches
+ data [0] = 0x80; //Read input registers
+ char read_data [5];
+ _i2c.write(EXPANSION_IC_ADDRESS, data, 1, false);
+ _i2c.read(EXPANSION_IC_ADDRESS, read_data, 5, false);
+ id = (read_data[2] & 0x3E) >> 1; //Mask pins 0, 6 and 7 then shift
+
+ //Setup interrupt handler
+ expansion_interrupt.mode(PullUp); // Pull Up by default (needed on v1 PCB - no pullup resistor)
+ expansion_interrupt.fall(this,&PiSwarm::expansion_interrupt_handler); // Add local handler on falling edge to read switch
+ return test_val;
+}
+
+char PiSwarm::setup_adc ( void ){
+ //ADC IC (for IR system) is a Analog Devices AD7997-BRUZ1
+ return 0;
+}
+
+float PiSwarm::read_reflected_ir_distance ( char index ){
+ //This function attempts to read a real-world distance approximation using the IR proximity sensors
+ //1. Check that the IR LDO regulator is on, enable if it isn't
+ if(enable_ir_ldo == 0){
+ enable_ir_ldo = 1;
+ enable_ldo_outputs();
+ }
+ //2. Read the ADC value without IR emitter on
+ unsigned short background_value = read_adc_value ( index );
+ //3. Enable the relevant IR emitter by turning on its pulse output
+ switch(index){
+ case 0: case 1: case 6: case 7:
+ _irpulse_1 = 1;
+ break;
+ case 2: case 3: case 4: case 5:
+ _irpulse_2 = 1;
+ break;
+ }
+ wait_us(500);
+ //4. Read the ADC value now IR emitter is on
+ unsigned short strong_value = read_adc_value ( index );
+ //5. Turn off IR emitter
+ switch(index){
+ case 0: case 1: case 6: case 7:
+ _irpulse_1 = 0;
+ break;
+ case 2: case 3: case 4: case 5:
+ _irpulse_2 = 0;
+ break;
+ }
+ //6. Estimate distance based on 2 values
+ float app_distance = 4000 + background_value - strong_value;
+ if(app_distance < 0) app_distance = 0;
+ // Very approximate: root value, divide by 2, approx distance in mm
+ app_distance = sqrt (app_distance) / 2.0;
+ // Then add adjustment value if value >27
+ if(app_distance > 27) {
+ float shift = pow(app_distance - 27,3);
+ app_distance += shift;
+ }
+ if(app_distance > 90) app_distance = 100.0;
+ return app_distance;
+}
+
+unsigned short PiSwarm::read_adc_value ( char index ){
+ short value = 0;
+ // Read a single value from the ADC
+ if(index<8){
+ char apb[1];
+ char data[2];
+ switch(index){
+ case 0: apb[0]=0x80; break;
+ case 1: apb[0]=0x90; break;
+ case 2: apb[0]=0xA0; break;
+ case 3: apb[0]=0xB0; break;
+ case 4: apb[0]=0xC0; break;
+ case 5: apb[0]=0xD0; break;
+ case 6: apb[0]=0xE0; break;
+ case 7: apb[0]=0xF0; break;
+ }
+ _i2c.write(ADC_ADDRESS,apb,1,false);
+ _i2c.read(ADC_ADDRESS,data,2,false);
+ value=((data[0] % 16)<<8)+data[1];
+ if(value > 4096) value=4096;
+ value=4096-value;
+ }
+ return value;
+}
+
+void PiSwarm::interrupt_timeout_event ( void ){
+ //Read switches
+ char data [1];
+ data [0] = 0x80; //Read input registers
+ char read_data [5];
+ _i2c.write(EXPANSION_IC_ADDRESS, data, 1, false);
+ _i2c.read(EXPANSION_IC_ADDRESS, read_data, 5, false);
+ switches = (read_data[1] & 0x7C) >> 2;
+ led_timeout.attach(this,&PiSwarm::led_timeout_event, 0.1); // Switch off the LED after 0.1s
+}
+
+void PiSwarm::led_timeout_event ( void ){
+ interrupt_led = 0;
+}
+
+void PiSwarm::expansion_interrupt_handler ( void ){
+ interrupt_led = 1;
+ debounce_timeout.attach_us(this,&PiSwarm::interrupt_timeout_event, 200); // Read the switches only after 200uS have passed to debounce
+ debounce_timeout.add_function(&switch_pressed); // Call the switch pressed routine (in main) when done
+}
+
+char PiSwarm::get_id () {
+ return id;
+}
+
+char PiSwarm::get_switches () {
+ return switches;
+}
+
+void PiSwarm::set_cled_brightness ( char brightness ) {
+ if( brightness > 100 ) brightness = 100;
+ // Brightness is set by adjusting period of PWM
+ // Max brightness is when total period = 256 uS
+ // When brightness is 90 total period = 274 uS
+ // When brightness is 50 total period = 546 uS
+ // When brightness is 10 total period = 1138 uS
+ // When brightness is 0 total period = 1336 uS
+ // When calibrate_colours = 1, red = 2x normal, green = 2x normal
+ cled_brightness = brightness;
+ int cled_period = (104 - brightness);
+ cled_period *= cled_period;
+ cled_period /= 10;
+ cled_period += 255;
+ if(CALIBRATE_COLOURS!=0)_cled_r.period_us(cled_period * 2);
+ else _cled_r.period_us(cled_period);
+ if(CALIBRATE_COLOURS!=0)_cled_g.period_us(cled_period * 2);
+ else _cled_g.period_us(cled_period);
+ _cled_b.period_us(cled_period);
+}
+
+void PiSwarm::set_oled_brightness ( char brightness ) {
+ if ( brightness > 100 ) brightness = 100;
+ // Brightness is set by adjusting period of PWM
+ // Max brightness is when total period = 256 uS
+ // When brightness is 90 total period = 274 uS
+ // When brightness is 50 total period = 546 uS
+ // When brightness is 10 total period = 1138 uS
+ // When brightness is 0 total period = 1336 uS
+ oled_brightness = brightness;
+ int oled_period = (104 - brightness);
+ oled_period *= oled_period;
+ oled_period /= 10;
+ oled_period += 255;
+ if(CALIBRATE_COLOURS!=0)_oled_r.period_us(oled_period * 2);
+ else _oled_r.period_us(oled_period);
+ if(CALIBRATE_COLOURS!=0)_oled_r.period_us(oled_period * 2);
+ else _oled_g.period_us(oled_period);
+ _oled_b.period_us(oled_period);
+}
+
+
+float PiSwarm::read_temperature ( void ){
+ // Temperature Sensor is a Microchip MCP9701T-E/LT
+ // 19.5mV/C 0C = 400mV
+ float r_temp = _temperature.read();
+ r_temp -= 0.1212f; // 0.4 / 3.3
+ r_temp *= 169.231f; // 3.3 / .0195
+ return r_temp;
+}
+
+char PiSwarm::calibrate_gyro ( void ){
+ //This routine attempts to calibrate the offset of the gyro
+ int samples = 8;
+ float gyro_values[samples];
+ calibrate_led = 1;
+ calibrate_ticker.attach(this,&PiSwarm::calibrate_ticker_routine,0.25);
+ for(int i=0;i<samples;i++){
+ gyro_values[i] = _gyro.read();
+ wait(0.12);
+ }
+ char pass = 1;
+ float mean = 0;
+ float min = 3.5;
+ float max = 3.2;
+ for(int i=0;i<samples;i++){
+ if((gyro_values[i] * 3.5 < 1.5) || (gyro_values[i] * 3.2 > 1.5)) pass = 0; // Calibration failed!
+ float test_value = 1.50 / gyro_values[i];
+ mean += test_value;
+ if (test_value > max) max = test_value;
+ if (test_value < min) min = test_value;
+ }
+ if(pass == 1){
+ gyro_cal = mean / samples;
+ gyro_error = max - min;
+ calibrate_ticker.detach();
+ calibrate_led = 0;
+ }
+ return pass;
+}
+
+char PiSwarm::calibrate_accelerometer ( void ){
+ //This routine attempts to calibrate the offset and multiplier of the accelerometer
+ int samples = 8;
+ float acc_x_values[samples];
+ float acc_y_values[samples];
+ float acc_z_values[samples];
+ calibrate_led = 1;
+ calibrate_ticker.attach(this,&PiSwarm::calibrate_ticker_routine,0.25);
+ for(int i=0;i<samples;i++){
+ acc_x_values[i] = _accel_x.read();
+ acc_y_values[i] = _accel_y.read();
+ acc_z_values[i] = _accel_z.read();
+ wait(0.12);
+ }
+ char pass = 1;
+ float x_mean = 0, y_mean=0, z_mean=0;
+ float x_min = 3600, y_min=3600, z_min=3600;
+ float x_max = 3100, y_max=3100, z_max=3100;
+ for(int i=0;i<samples;i++){
+ if((acc_x_values[i] * 3600 < 1250) || (acc_x_values[i] * 3100 > 1250)) pass = 0; // Calibration failed!
+ if((acc_y_values[i] * 3600 < 1250) || (acc_y_values[i] * 3100 > 1250)) pass = 0; // Calibration failed!
+ if((acc_z_values[i] * 3600 < 887) || (acc_z_values[i] * 3100 > 887)) pass = 0; // Calibration failed!
+
+ float x_test_value = 1250 / acc_x_values[i];
+ x_mean += x_test_value;
+ if (x_test_value > x_max) x_max = x_test_value;
+ if (x_test_value < x_min) x_min = x_test_value;
+ float y_test_value = 1250 / acc_y_values[i];
+ y_mean += y_test_value;
+ if (y_test_value > y_max) y_max = y_test_value;
+ if (y_test_value < y_min) y_min = y_test_value;
+ float z_test_value = 887 / acc_z_values[i];
+ z_mean += z_test_value;
+ if (z_test_value > z_max) z_max = z_test_value;
+ if (z_test_value < z_min) z_min = z_test_value;
+ }
+ if(pass == 1){
+ acc_x_cal = x_mean / samples;
+ acc_y_cal = y_mean / samples;
+ acc_z_cal = z_mean / samples;
+ calibrate_ticker.detach();
+ calibrate_led = 0;
+ }
+ return pass;
+}
+
+void PiSwarm::calibrate_ticker_routine ( void ){
+ calibrate_led_state = 1 - calibrate_led_state;
+ calibrate_led = calibrate_led_state;
+}
+
+float PiSwarm::read_gyro ( void ){
+ // Gyro is a ST Micro LY3200ALH Yaw-rate Gyro
+ // 0.67mV / d.p.s.
+ // 1.5V V offset
+ float r_gyro = _gyro.read();
+ r_gyro *= gyro_cal; // NB 3.4 seemed to work better that 3.3 but better to run calibration routine
+ r_gyro = 1.5 - r_gyro; // 1.5V off-set, invert sign
+ r_gyro *= 1492.5; // Convert to DPS (+ve = clockwise, -ve = c.clockwise)
+ return r_gyro;
+}
+
+float PiSwarm::read_accelerometer_x ( void ){
+ // LIS 332 AX Accelerometer - 0.363 V/g - 1.25V
+
+ float r_acc_x = (( _accel_x.read() * acc_x_cal ) - 1250.0 ) * 2.754; // Return value in mG
+ return r_acc_x;
+}
+
+float PiSwarm::read_accelerometer_y ( void ){
+ float r_acc_y = (( _accel_y.read() * acc_y_cal ) - 1250.0 ) * 2.754; // Return value in mG
+ return r_acc_y;
+}
+
+float PiSwarm::read_accelerometer_z ( void ){
+ float r_acc_z = (( _accel_z.read() * acc_z_cal ) - 1250.0 ) * 2.754; // Return value in mG
+ return r_acc_z;
+}
+
+
+char PiSwarm::calibrate_magnetometer ( void ){
+ char command_data[2];
+ command_data[0] = 0x11; //Write to CTRL_REG2
+ command_data[1] = 0x80; // Enable auto resets
+ _i2c.write(MAGNETOMETER_ADDRESS, command_data, 2, false);
+ command_data[0] = 0x10; //Write to CTRL_REG1
+ command_data[1] = 0x29; // 001 01 0 0 1 [40HZ 32-OS NOT-FAST NORMAL ACTIVE MODE]
+ return _i2c.write(MAGNETOMETER_ADDRESS, command_data, 2, false);
+}
+
+char PiSwarm::read_magnetometer ( void ){
+ char read_data[7];
+ char success;
+ char command_data[1];
+ command_data[0] = 0x00; //Auto-read from register 0x00 (status)
+ success = _i2c.write(MAGNETOMETER_ADDRESS, command_data, 1, false);
+ _i2c.read(MAGNETOMETER_ADDRESS, read_data, 7, false);
+ mag_values[0] = (read_data[1] << 8) + read_data[2];
+ mag_values[1] = (read_data[3] << 8) + read_data[4];
+ mag_values[2] = (read_data[5] << 8) + read_data[6];
+ return success;
+}
+
+signed short PiSwarm::get_magnetometer_x ( void ){
+ return mag_values[0];
+}
+
+signed short PiSwarm::get_magnetometer_y ( void ){
+ return mag_values[1];
+}
+
+signed short PiSwarm::get_magnetometer_z ( void ){
+ return mag_values[2];
+}
+
+void PiSwarm::write_eeprom_byte ( int address, char data ){
+ char write_array[3];
+ write_array[0] = address / 256;
+ write_array[1] = address % 256;
+ write_array[2] = data;
+ _i2c.write(EEPROM_ADDRESS, write_array, 3, false);
+ //Takes 5ms to write a page: ideally this could be done with a timer or RTOS
+ wait(0.005);
+}
+
+void PiSwarm::write_eeprom_block ( int address, char length, char * data){
+ /** Note from datasheet:
+ Page write operations are limited to writing bytes within a single physical page,
+ regardless of the number of bytes actually being written. Physical page
+ boundaries start at addresses that are integer multiples of the page buffer size (or
+ ‘page size’) and end at addresses that are integer multiples of [page size – 1]. If a
+ Page Write command attempts to write across a physical page boundary, the
+ result is that the data wraps around to the beginning of the current page (overwriting
+ data previously stored there), instead of being written to the next page, as might be
+ expected. It is therefore necessary for the application software to prevent page write
+ operations that would attempt to cross a page boundary. */
+
+ //First check to see if first block starts at the start of a page
+ int subpage = address % 32;
+
+ //If subpage > 0 first page does not start at a page boundary
+ int write_length = 32 - subpage;
+ if( write_length > length ) write_length = length;
+ char firstpage_array [write_length+2];
+ firstpage_array[0] = address / 256;
+ firstpage_array[1] = address % 256;
+ for(int i=0;i<write_length;i++){
+ firstpage_array[i+2] = data [i];
+ }
+ _i2c.write(EEPROM_ADDRESS, firstpage_array, write_length + 2, false);
+ wait(0.005);
+
+ if(length > write_length){
+ int page_count = (length + subpage) / 32;
+ int written = write_length;
+ for(int p=0;p<page_count;p++){
+ int to_write = length - written;
+ if (to_write > 32) {
+ to_write = 32;
+ }
+ char page_array [to_write + 2];
+ page_array[0] = (address + written) / 256;
+ page_array[1] = (address + written) % 256;
+ for(int i=0;i<to_write;i++){
+ page_array[i+2] = data[written + i];
+ }
+ _i2c.write(EEPROM_ADDRESS, page_array, to_write + 2, false);
+ wait(0.005);
+ written += 32;
+ }
+ }
+}
+
+char PiSwarm::read_eeprom_byte ( int address ){
+ char address_array [2];
+ address_array[0] = address / 256;
+ address_array[1] = address % 256;
+ char data [1];
+ _i2c.write(EEPROM_ADDRESS, address_array, 2, false);
+ _i2c.read(EEPROM_ADDRESS, data, 1, false);
+ return data [0];
+}
+
+char PiSwarm::read_next_eeprom_byte (){
+ char data [1];
+ _i2c.read(EEPROM_ADDRESS, data, 1, false);
+ return data [0];
+}
+
+char PiSwarm::read_eeprom_block ( int address, char length ){
+ char ret = 0;
+ return ret;
+}
+
+float PiSwarm::read_light_sensor ( void ){
+ // Light sensor is a Avago APDS-9005 Ambient Light Sensor
+ //float r_light = (_light.read() * 141); //Gives a range of (approximately) 0=dark 100=max. light
+ float r_light = (sqrt(_light.read() * 1.41) * 100); //Non-linear and more CPU intensive but generally gives better low-light measures
+ if(r_light > 100) r_light = 100;
+ return r_light;
+}
+
+
+void PiSwarm::reset () {
+ // _nrst = 0;
+ wait (0.01);
+ // _nrst = 1;
+ wait (0.1);
+}
+
+void PiSwarm::play_tune (char * tune, int length) {
+ _ser.putc(DO_PLAY);
+ _ser.putc(length);
+ for (int i = 0 ; i < length ; i++) {
+ _ser.putc(tune[i]);
+ }
+}
+
+void PiSwarm::read_raw_sensors ( int * raw_ls_array ) {
+ _ser.putc(SEND_RAW_SENSOR_VALUES);
+ for (int i = 0; i < 5 ; i ++) {
+ int val = _ser.getc();
+ val += _ser.getc() << 8;
+ raw_ls_array [i] = val;
+ }
+}
+
+float PiSwarm::get_left_motor (){
+ return left_speed;
+}
+
+float PiSwarm::get_right_motor (){
+ return right_speed;
+}
+
+void PiSwarm::left_motor (float speed) {
+ motor(0,speed);
+}
+
+void PiSwarm::right_motor (float speed) {
+ motor(1,speed);
+}
+
+void PiSwarm::forward (float speed) {
+ motor(0,speed);
+ motor(1,speed);
+}
+
+void PiSwarm::backward (float speed) {
+ motor(0,-1.0*speed);
+ motor(1,-1.0*speed);
+}
+
+void PiSwarm::left (float speed) {
+ motor(0,speed);
+ motor(1,-1.0*speed);
+}
+
+void PiSwarm::right (float speed) {
+ motor(0,-1.0*speed);
+ motor(1,speed);
+}
+
+void PiSwarm::stop (void) {
+ motor(0,0.0);
+ motor(1,0.0);
+}
+
+void PiSwarm::motor (int motor, float speed) {
+ char opcode = 0x0;
+ if (speed > 0.0) {
+ if (motor==1)
+ opcode = M1_FORWARD;
+ else
+ opcode = M2_FORWARD;
+ } else {
+ if (motor==1)
+ opcode = M1_BACKWARD;
+ else
+ opcode = M2_BACKWARD;
+ }
+ if(motor==1)right_speed = speed;
+ else left_speed = speed;
+ unsigned char arg = 0x7f * abs(speed);
+
+ _ser.putc(opcode);
+ _ser.putc(arg);
+}
+
+float PiSwarm::battery() {
+ _ser.putc(SEND_BATTERY_MILLIVOLTS);
+ char lowbyte = _ser.getc();
+ char hibyte = _ser.getc();
+ float v = ((lowbyte + (hibyte << 8))/1000.0);
+ return(v);
+}
+
+float PiSwarm::line_position() {
+ int pos = 0;
+ _ser.putc(SEND_LINE_POSITION);
+ pos = _ser.getc();
+ pos += _ser.getc() << 8;
+
+ float fpos = ((float)pos - 2048.0)/2048.0;
+ return(fpos);
+}
+
+char PiSwarm::sensor_auto_calibrate() {
+ _ser.putc(AUTO_CALIBRATE);
+ return(_ser.getc());
+}
+
+
+void PiSwarm::calibrate(void) {
+ _ser.putc(PI_CALIBRATE);
+}
+
+void PiSwarm::reset_calibration() {
+ _ser.putc(LINE_SENSORS_RESET_CALIBRATION);
+}
+
+void PiSwarm::PID_start(int max_speed, int a, int b, int c, int d) {
+ _ser.putc(max_speed);
+ _ser.putc(a);
+ _ser.putc(b);
+ _ser.putc(c);
+ _ser.putc(d);
+}
+
+void PiSwarm::PID_stop() {
+ _ser.putc(STOP_PID);
+}
+
+float PiSwarm::pot_voltage(void) {
+ int volt = 0;
+ _ser.putc(SEND_TRIMPOT);
+ volt = _ser.getc();
+ volt += _ser.getc() << 8;
+ return(volt);
+}
+
+void PiSwarm::locate(int x, int y) {
+ _ser.putc(DO_LCD_GOTO_XY);
+ _ser.putc(x);
+ _ser.putc(y);
+}
+
+void PiSwarm::cls(void) {
+ _ser.putc(DO_CLEAR);
+}
+
+int PiSwarm::print (char* text, int length) {
+ _ser.putc(DO_PRINT);
+ _ser.putc(length);
+ for (int i = 0 ; i < length ; i++) {
+ _ser.putc(text[i]);
+ }
+ return(0);
+}
+
+int PiSwarm::_putc (int c) {
+ _ser.putc(DO_PRINT);
+ _ser.putc(0x1);
+ _ser.putc(c);
+ wait (0.001);
+ return(c);
+}
+
+int PiSwarm::_getc (void) {
+ char r = 0;
+ return(r);
+}
+
+int PiSwarm::putc (int c) {
+ return(_ser.putc(c));
+}
+
+int PiSwarm::getc (void) {
+ return(_ser.getc());
+}
+
+void PiSwarm::start_system_timer(void) {
+ _system_timer.reset();
+ _system_timer.start();
+}
+
+float PiSwarm::get_uptime (void) {
+ return _system_timer.read();
+}
+
+#ifdef MBED_RPC
+const rpc_method *PiSwarm::get_rpc_methods() {
+ static const rpc_method rpc_methods[] = {{ "forward", rpc_method_caller<PiSwarm, float, &PiSwarm::forward> },
+ { "backward", rpc_method_caller<PiSwarm, float, &PiSwarm::backward> },
+ { "left", rpc_method_caller<PiSwarm, float, &PiSwarm::left> },
+ { "right", rpc_method_caller<PiSwarm, float, &PiSwarm::right> },
+ { "stop", rpc_method_caller<PiSwarm, &PiSwarm::stop> },
+ { "left_motor", rpc_method_caller<PiSwarm, float, &PiSwarm::left_motor> },
+ { "right_motor", rpc_method_caller<PiSwarm, float, &PiSwarm::right_motor> },
+ { "battery", rpc_method_caller<float, PiSwarm, &PiSwarm::battery> },
+ { "line_position", rpc_method_caller<float, PiSwarm, &PiSwarm::line_position> },
+ { "sensor_auto_calibrate", rpc_method_caller<char, PiSwarm, &PiSwarm::sensor_auto_calibrate> },
+ RPC_METHOD_SUPER(Base)
+ };
+ return rpc_methods;
+}
+#endif
+
+void display_system_time(){
+ if(PISWARM_DEBUG == 1){
+ time_t system_time = time(NULL);
+ printf("Current system time:%s Uptime:%.1f\n",ctime(&system_time),piswarm.get_uptime());
+ }
+}
+
+void init() {
+ //Standard initialisation routine for Pi Swarm Robot
+ //Displays a message on the screen and flashes the central LED
+ //Calibrates the gyro and accelerometer
+ //Make sure robot is flat and stationary when this code is run (calibration starts after about .5 second to allow robot to settle)
+ piswarm.start_system_timer();
+ pc.baud(PC_BAUD);
+ if(PISWARM_DEBUG == 1)pc.printf("PiSwarm Firmware 0.4 Initialisation...\n");
+ display_system_time();
+ piswarm.play_tune("T298MSCG>C",10);
+ piswarm.cls();
+ piswarm.enable_cled(1);
+ piswarm.set_cled_colour(200,0,0);
+ piswarm.set_oled_colour(200,0,0);
+ piswarm.set_oleds(1,0,1,0,1,0,1,0,1,0);
+ piswarm.printf(" YORK ");
+ piswarm.locate(0,1);
+ piswarm.printf("ROBOTLAB");
+ wait(0.07);
+ piswarm.set_oleds(0,1,0,1,0,1,0,1,0,1);
+ wait(0.07);
+ piswarm.set_cled_colour(0,200,0);
+ piswarm.set_oled_colour(0,200,0);
+ piswarm.set_oleds(1,0,1,0,1,0,1,0,1,0);
+ wait(0.07);
+ piswarm.set_oleds(0,1,0,1,0,1,0,1,0,1);
+ wait(0.07);
+ piswarm.set_cled_colour(0,0,200);
+ piswarm.set_oled_colour(0,0,200);
+ piswarm.set_oleds(1,0,1,0,1,0,1,0,1,0);
+ wait(0.07);
+ piswarm.set_oleds(0,1,0,1,0,1,0,1,0,1);
+ wait(0.07);
+ piswarm.set_oled_colour(20,150,200);
+ piswarm.set_oleds(1,1,1,1,1,1,1,1,1,1);
+
+ piswarm.cls();
+ piswarm.set_cled_colour(20,20,20);
+ piswarm.printf("Pi Swarm");
+ piswarm.locate(0,1);
+ piswarm.printf("ID : %d ",piswarm.get_id());
+ if(PISWARM_DEBUG == 1)pc.printf("Test Magnetometer: ");
+ if(piswarm.calibrate_magnetometer() != 0){
+ if(PISWARM_DEBUG == 1)pc.printf("FAILED\n");
+ wait(0.1);
+ piswarm.cls();
+ piswarm.locate(0,0);
+ piswarm.printf("Mag Cal ");
+ piswarm.locate(0,1);
+ piswarm.printf("Failed ");
+ wait(0.1);
+ }else if(PISWARM_DEBUG == 1)pc.printf("PASSED\n");
+ if(PISWARM_DEBUG == 1)pc.printf("Test Gyroscope: ");
+ if(piswarm.calibrate_gyro() == 0){
+ if(PISWARM_DEBUG == 1)pc.printf("FAILED\n");
+ wait(0.1);
+ piswarm.cls();
+ piswarm.locate(0,0);
+ piswarm.printf("Gyro Cal");
+ piswarm.locate(0,1);
+ piswarm.printf("Failed ");
+ wait(0.1);
+ }else if(PISWARM_DEBUG == 1)pc.printf("PASSED\n");
+ if(PISWARM_DEBUG == 1)pc.printf("Test Accelerometer: ");
+ if(piswarm.calibrate_accelerometer() == 0){
+ if(PISWARM_DEBUG == 1)pc.printf("FAILED\n");
+ wait(0.1);
+ piswarm.cls();
+ piswarm.locate(0,0);
+ piswarm.printf("Acc. Cal");
+ piswarm.locate(0,1);
+ piswarm.printf("Failed ");
+ wait(0.1);
+ }else if(PISWARM_DEBUG == 1)pc.printf("PASSED\n");
+ piswarm.turn_off_all_oleds();
+ wait(0.1);
+ piswarm.cls();
+ piswarm.set_cled_colour(0,0,0);
+ if(START_RADIO_ON_BOOT == 1){
+ if(PISWARM_DEBUG == 1)pc.printf("Setting up 433MHz Transceiver\n");
+ piswarm.setup_radio();
+ }
+}
+
+
+
+/********************************************************************************
+ * COPYRIGHT NOTICE *
+ * *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy *
+ * of this software and associated documentation files (the "Software"), to deal*
+ * in the Software without restriction, including without limitation the rights *
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell *
+ * copies of the Software, and to permit persons to whom the Software is *
+ * furnished to do so, subject to the following conditions: *
+ * *
+ * The above copyright notice and this permission notice shall be included in *
+ * all copies or substantial portions of the Software. *
+ * *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR *
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, *
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE *
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER *
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,*
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN *
+ * THE SOFTWARE. *
+ * *
+ ********************************************************************************/
+
+