Unfinished v0.7, added bearing detection
Fork of Pi_Swarm_Library_v06_alpha by
piswarm.cpp
- Committer:
- jah128
- Date:
- 2014-02-02
- Revision:
- 3:4c0f2f3de33e
- Parent:
- 1:b067a08ff54e
- Child:
- 4:52b3e4c5a425
File content as of revision 3:4c0f2f3de33e:
/* 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(); } ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // Outer LED Functions ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // Returns the current setting for the outer LED colour. Returned colour is stored as 24-bit positive integer (R<<16 + G<<8 + B). Used by communication stack. int PiSwarm::get_oled_colour(){ return (oled_red << 16) + (oled_green << 8) + oled_blue; } // Returns the current enable state an individual LED. oled = LED to return enable state. Returns 0 for disabled, 1 for enabled char PiSwarm::get_oled_state(char oled){ if(oled_array[oled] == 1) return 1; return 0; } // Set the colour of the outer LED. Values for red, green and blue range from 0 (off) to 255 (maximum). 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); } // Returns the enable state of the center LED char PiSwarm::get_cled_state( void ){ return cled_enable; } // Set the state of an individual LED. oled = LED to enable. value = 0 for disable, 1 for enable. Use to change 1 LED without affecting others. void PiSwarm::set_oled(char oled, char value){ oled_array[oled]=value; } // Sets the state of all 10 LEDs. oled_x = 0 for disable, 1 for enable. Use to change all 10 LEDs at once 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(); } // Sets all outer LEDs to disabled and turns off LED power supply. 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); } // Set the outer LED brightness (total period of PWM output increases as brightness decreases). Ranges from 0 (minimum) to 255 (maximum) void PiSwarm::set_oled_brightness ( char brightness ) { if ( brightness > 100 ) brightness = 100; 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_g.period_us(oled_period * 2); else _oled_g.period_us(oled_period); _oled_b.period_us(oled_period); } // 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); } ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // Center LED Functions ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // Returns the current setting for the center LED colour. Returned colour is stored as 24-bit positive integer (R<<16 + G<<8 + B). Used by communication stack. int PiSwarm::get_cled_colour(){ return (cled_red << 16) + (cled_green << 8) + cled_blue; } // Set the colour of the center LED. Values for red, green and blue range from 0 (off) to 255 (maximum). 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); } // Turn on or off the center LED. enable=1 to turn on, 0 to turn off 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); } } // Set the center LED brightness (total period of PWM output increases as brightness decreases). Ranges from 0 (minimum) to 255 (maximum) 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); } ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // IR Sensor Functions ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // Estimates the distance to an obstacle from one of the IR sensors, defined by index (range 0-7). The value is converted to an approximate distance in millimetres, or 100.0 if no obstacle found. 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; } // Returns the illuminated raw sensor value for the IR sensor defined by index (range 0-7); turns on the emitters for a 500us pulse unsigned short PiSwarm::read_illuminated_raw_ir_value ( char index ){ //This function reads the IR strength when illuminated - used for PC system debugging purposes //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. 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); //3. Read the ADC value now IR emitter is on unsigned short strong_value = read_adc_value ( index ); //4. 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; } return strong_value; } // Returns the raw sensor value for the IR sensor defined by index (range 0-7). 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; } // Function enables or disables the LDO voltage regulators which supply power to the outer LEDs and the IR photo emitters. 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); } ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // MEMS Sensor Functions ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // Returns the yaw (clockwise) rotation in degrees-per-second reported by the LY3200 gyroscope. float PiSwarm::read_gyro ( void ){ // Gyro is a ST Micro LY3200ALH Yaw-rate Gyro: 0.67mV / d.p.s, 1.5V 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; } // Returns the acceleration in the X plane reported by the LIS332 accelerometer. Returned value is in mg. 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; } // Returns the acceleration in the Y plane reported by the LIS332 accelerometer. Returned value is in mg. 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; } // Returns the acceleration in the Z plane reported by the LIS332 accelerometer. Returned value is in mg. 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; } // Sends message to the magnetometer to read new values. Should be called before get_magnetometer_* when updated values are required 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; } // Returns the raw value for the X-plane magnetic field stored on the last call of read_magnetometer signed short PiSwarm::get_magnetometer_x ( void ){ return mag_values[0]; } // Returns the raw value for the Y-plane magnetic field stored on the last call of read_magnetometer signed short PiSwarm::get_magnetometer_y ( void ){ return mag_values[1]; } // Returns the raw value for the Z-plane magnetic field stored on the last call of read_magnetometer signed short PiSwarm::get_magnetometer_z ( void ){ return mag_values[2]; } ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // Other Sensor Functions ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // Returns the temperature reported by the MCP9701 sensor in degrees centigrade. 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; } // Returns the adjusted value (0-100) for the ambient light sensor 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::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; } } // Returns the uptime of the system (since initialisation) in seconds float PiSwarm::get_uptime (void) { return _system_timer.read(); } // Returns the battery level in millivolts 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); } // Returns the raw value for the variable resistor on the base of the platform. Ranges from 0 to 1024. float PiSwarm::pot_voltage(void) { int volt = 0; _ser.putc(SEND_TRIMPOT); volt = _ser.getc(); volt += _ser.getc() << 8; return(volt); } // Returns the ID of platform (set by the DIL switches above the display). Range 0-31 [0 reserved]. char PiSwarm::get_id () { return id; } // Return the current stored state for direction switch(es) pressed {1 = Center 2 = Right 4 = Left 8 = Down 16 = Up} char PiSwarm::get_switches () { return switches; } ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // Motor Functions ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // Returns the target speed of the left motor (-1.0 to 1.0) float PiSwarm::get_left_motor (){ return left_speed; } // Returns the target speed of the right motor (-1.0 to 1.0) float PiSwarm::get_right_motor (){ return right_speed; } // Sets the speed of the left motor (-1.0 to 1.0) void PiSwarm::left_motor (float speed) { motor(0,speed); } // Sets the speed of the right motor (-1.0 to 1.0) void PiSwarm::right_motor (float speed) { motor(1,speed); } // Drive forward at the given speed (-1.0 to 1.0) void PiSwarm::forward (float speed) { motor(0,speed); motor(1,speed); } // Drive backward at the given speed (-1.0 to 1.0) void PiSwarm::backward (float speed) { motor(0,-1.0*speed); motor(1,-1.0*speed); } // Turn on-the-spot left at the given speed (-1.0 to 1.0) void PiSwarm::left (float speed) { motor(0,speed); motor(1,-1.0*speed); } // Turn on-the-spot right at the given speed (-1.0 to 1.0) void PiSwarm::right (float speed) { motor(0,-1.0*speed); motor(1,speed); } // Stop both motors void PiSwarm::stop (void) { motor(0,0.0); motor(1,0.0); } ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // RF Transceiver Functions ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// void PiSwarm::send_rf_message(char* message, char length){ _rf.sendString(length, message); } ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // Sound Functions ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 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]); } } ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // EEPROM Functions ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 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; } ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // Setup and Calibration Functions ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 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); } 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; } 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; } 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); } 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 } void PiSwarm::calibrate_ticker_routine ( void ){ calibrate_led_state = 1 - calibrate_led_state; calibrate_led = calibrate_led_state; } 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::reset () { // _nrst = 0; wait (0.01); // _nrst = 1; wait (0.1); } 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::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); } 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(); } #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. * * * ********************************************************************************/