Unfinished v0.7, added bearing detection
Fork of Pi_Swarm_Library_v06_alpha by
Diff: piswarm.cpp
- Revision:
- 1:b067a08ff54e
- Parent:
- 0:9ffe8ebd1c40
- Child:
- 3:4c0f2f3de33e
diff -r 9ffe8ebd1c40 -r b067a08ff54e piswarm.cpp --- a/piswarm.cpp Fri Jan 31 22:59:25 2014 +0000 +++ b/piswarm.cpp Sun Feb 02 18:05:58 2014 +0000 @@ -51,18 +51,43 @@ 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; } -int PiSwarm::get_cled_colour(){ - return (cled_red << 16) + (cled_green << 8) + cled_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; @@ -77,7 +102,7 @@ activate_oleds(); } -// Switches off all outer LEDs +// 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; @@ -92,6 +117,20 @@ _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(){ @@ -100,7 +139,6 @@ 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 @@ -117,29 +155,17 @@ _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); + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +// 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; } -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); -} - +// 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; @@ -147,6 +173,7 @@ 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) { @@ -160,79 +187,34 @@ } } -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); +// 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); } -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; -} +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +// IR Sensor Functions +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -char PiSwarm::setup_adc ( void ){ - //ADC IC (for IR system) is a Analog Devices AD7997-BRUZ1 - return 0; -} - +// 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 @@ -277,6 +259,7 @@ return app_distance; } +// 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 @@ -302,206 +285,51 @@ 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); +// 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); } - -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; -} +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +// MEMS Sensor Functions +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -void PiSwarm::calibrate_ticker_routine ( void ){ - calibrate_led_state = 1 - calibrate_led_state; - calibrate_led = calibrate_led_state; -} - +// 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 V offset + // 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 - + // 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; } - -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); -} - +// 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; @@ -515,18 +343,171 @@ 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; @@ -605,15 +586,205 @@ 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); -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; + // 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; @@ -621,65 +792,6 @@ // _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) { @@ -701,13 +813,6 @@ _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; @@ -745,13 +850,6 @@ _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); @@ -798,9 +896,6 @@ _system_timer.start(); } -float PiSwarm::get_uptime (void) { - return _system_timer.read(); -} #ifdef MBED_RPC const rpc_method *PiSwarm::get_rpc_methods() {