James Hilder
Unfinished v0.7, added bearing detection
Fork of
Pi_Swarm_Library_v06_alpha
by 
piswarm
Diff: piswarm.cpp
- Revision:
- 1:b067a08ff54e
- Parent:
- 0:9ffe8ebd1c40
- Child:
- 3:4c0f2f3de33e
--- 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() {