Unfinished v0.7, added bearing detection
Fork of Pi_Swarm_Library_v06_alpha by
Diff: piswarm.cpp
- Revision:
- 0:9ffe8ebd1c40
- Child:
- 1:b067a08ff54e
diff -r 000000000000 -r 9ffe8ebd1c40 piswarm.cpp --- /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. * + * * + ********************************************************************************/ + +