James Hilder
Unfinished v0.7, added bearing detection
Fork of
Pi_Swarm_Library_v06_alpha
by 
piswarm
Revision 11:5ebcb52726cf, committed 2014-06-30
- Comitter:
- jah128
- Date:
- Mon Jun 30 07:58:31 2014 +0000
- Parent:
- 10:5c111c07a2ed
- Commit message:
- Added prototype bearing detection to pi swarm library;
Changed in this revision
--- a/alpha433.cpp Tue Feb 18 21:05:32 2014 +0000 +++ b/alpha433.cpp Mon Jun 30 07:58:31 2014 +0000 @@ -4,7 +4,7 @@ * * (C) Dr James Hilder, Dept. Electronics & Computer Science, University of York * - * Version 0.6 February 2014 + * Version 0.7 May 2014 * * Designed for use with the Pi Swarm Board (enhanced MBED sensor board) v1.2 *
--- a/alpha433.h Tue Feb 18 21:05:32 2014 +0000 +++ b/alpha433.h Mon Jun 30 07:58:31 2014 +0000 @@ -4,7 +4,7 @@ * * (C) Dr James Hilder, Dept. Electronics & Computer Science, University of York * - * Version 0.6 February 2014 + * Version 0.7 May 2014 * * Designed for use with the Pi Swarm Board (enhanced MBED sensor board) v1.2 *
--- a/communications.cpp Tue Feb 18 21:05:32 2014 +0000 +++ b/communications.cpp Mon Jun 30 07:58:31 2014 +0000 @@ -4,7 +4,7 @@ * * (C) Dr James Hilder, Dept. Electronics & Computer Science, University of York * - * Version 0.6 February 2014 + * Version 0.7 May 2014 * * Designed for use with the Pi Swarm Board (enhanced MBED sensor board) v1.2 *
--- a/communications.h Tue Feb 18 21:05:32 2014 +0000 +++ b/communications.h Mon Jun 30 07:58:31 2014 +0000 @@ -4,7 +4,7 @@ * * (C) Dr James Hilder, Dept. Electronics & Computer Science, University of York * - * Version 0.6 February 2014 + * Version 0.7 May 2014 * * Designed for use with the Pi Swarm Board (enhanced MBED sensor board) v1.2 *
--- a/piswarm.cpp Tue Feb 18 21:05:32 2014 +0000
+++ b/piswarm.cpp Mon Jun 30 07:58:31 2014 +0000
@@ -3,10 +3,11 @@
* University of York Robot Lab Pi Swarm Robot Library
*
* (C) Dr James Hilder, Dept. Electronics & Computer Science, University of York
- *
+ *
* Version 0.6 February 2014
*
* Change notes:
+ * 0.7 : Added code for IR table beacon syncing and turning
* 0.6 : Added new IR sensor functions to improve efficiency (see manual for details)
* 0.5 : Initial release
*
@@ -14,20 +15,35 @@
*
******************************************************************************************/
-#include "piswarm.h"
+#include "piswarm.h"
// Variables
DigitalOut gpio_led (LED1);
DigitalOut calibrate_led (LED2);
-DigitalOut adc_led (LED3);
+DigitalOut beacon_led (LED3);
DigitalOut interrupt_led (LED4);
-InterruptIn expansion_interrupt (p29);
+InterruptIn expansion_interrupt (p29);
Timeout debounce_timeout;
Timeout led_timeout;
+Timeout motor_timeout;
Ticker calibrate_ticker;
+Ticker oled_ticker;
+Ticker beacon_ticker;
+
char id;
+float bearing = 0;
+char is_synced = 0;
+float beacon_confidence = 0;
+float bearing_confidence = 0;
+char saved_peak = 0;
+char miss_sync_count = 0;
+char hit_sync_count = 0;
char oled_array [10];
+char saved_oled_array [10];
+char saved_oled_red;
+char saved_oled_green;
+char saved_oled_blue;
char enable_ir_ldo = 0;
char enable_led_ldo = 0;
char calibrate_led_state = 1;
@@ -39,6 +55,8 @@
char oled_green = 0;
char oled_blue = 0;
char cled_enable = 0;
+char oled_ticker_step = 0;
+char bearing_strobe_step = 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;
@@ -55,7 +73,8 @@
char ir_values_stored = 0;
-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) {
+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();
}
@@ -64,19 +83,22 @@
// 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(){
+// 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){
+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 ){
+void PiSwarm::set_oled_colour( char red, char green, char blue )
+{
oled_red = red;
oled_green = green;
oled_blue = blue;
@@ -86,12 +108,35 @@
}
// 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){
+void PiSwarm::set_oled(char oled, char value)
+{
oled_array[oled]=value;
}
+// Save the current state and colour of OLEDs
+void PiSwarm::save_oled_state()
+{
+ for(int i=0; i<10; i++) {
+ saved_oled_array[i]=oled_array[i];
+ }
+ saved_oled_red=oled_red;
+ saved_oled_green=oled_green;
+ saved_oled_blue=oled_blue;
+}
+
+// Restore the saved colour and state of OLEDs
+void PiSwarm::restore_oled_state()
+{
+ for(int i=0; i<10; i++) {
+ oled_array[i]=saved_oled_array[i];
+ }
+ set_oled_colour(saved_oled_red,saved_oled_green,saved_oled_blue);
+ activate_oleds();
+}
+
// 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){
+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;
@@ -106,8 +151,9 @@
}
// 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++){
+void PiSwarm::turn_off_all_oleds()
+{
+ for(int i=0; i<10; i++) {
oled_array[i]=0;
}
activate_oleds();
@@ -117,17 +163,18 @@
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);
+ _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 ) {
+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;
+ 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);
@@ -136,8 +183,9 @@
}
// Sends the messages to enable/disable outer LEDs
-void PiSwarm::activate_oleds(){
- if(enable_led_ldo == 0){
+void PiSwarm::activate_oleds()
+{
+ if(enable_led_ldo == 0) {
enable_led_ldo = 1;
enable_ldo_outputs();
}
@@ -155,7 +203,7 @@
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);
+ _i2c.write(EXPANSION_IC_ADDRESS,data,3,false);
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
@@ -163,17 +211,20 @@
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// 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(){
+int PiSwarm::get_cled_colour()
+{
return (cled_red << 16) + (cled_green << 8) + cled_blue;
}
-// Returns the enable state of the center LED
-char PiSwarm::get_cled_state( void ){
+// Returns the enable state of the center LED
+char PiSwarm::get_cled_state( void )
+{
return cled_enable;
}
// 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 ){
+void PiSwarm::set_cled_colour( char red, char green, char blue )
+{
cled_red = red;
cled_green = green;
cled_blue = blue;
@@ -181,13 +232,14 @@
}
// Turn on or off the center LED. enable=1 to turn on, 0 to turn off
-void PiSwarm::enable_cled( char enable ){
+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{
+ } else {
_cled_r.pulsewidth_us(0);
_cled_g.pulsewidth_us(0);
_cled_b.pulsewidth_us(0);
@@ -195,7 +247,8 @@
}
// 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 ) {
+void PiSwarm::set_cled_brightness ( char brightness )
+{
if( brightness > 100 ) brightness = 100;
// Brightness is set by adjusting period of PWM. Period ranged from ~260uS at 100% to 1336uS at 0%
// When calibrate_colours = 1, red = 2x normal, green = 2x normal
@@ -204,9 +257,9 @@
cled_period *= cled_period;
cled_period /= 10;
cled_period += 255;
- if(CALIBRATE_COLOURS!=0)_cled_r.period_us(cled_period * 2);
+ 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);
+ if(CALIBRATE_COLOURS!=0)_cled_g.period_us(cled_period * 2);
else _cled_g.period_us(cled_period);
_cled_b.period_us(cled_period);
}
@@ -215,46 +268,59 @@
// IR Sensor Functions
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-// Estimates the distance to an obstacle from one of the IR sensors, defined by index (range 0-7).
+// 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.
// NB This function is preserved for code-compatability and cases where only a single reading is needed
// In many cases it is preferable to call store_reflected_ir_distances() to save all 8 values then read using get_reflected_ir_distance()
-float PiSwarm::read_reflected_ir_distance ( char index ){
+float PiSwarm::read_reflected_ir_distance ( char index )
+{
// Sanity check
if(index>7) return 0.0;
-
+
//1. Check that the IR LDO regulator is on, enable if it isn't
- if(enable_ir_ldo == 0){
+ if(enable_ir_ldo == 0) {
enable_ir_ldo = 1;
enable_ldo_outputs();
}
//2. Read the ADC value without IR emitter on; store in the background_ir_values[] array
background_ir_values [index] = 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;
+ 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);
-
+ wait_us(500);
+
//4. Read the ADC value now IR emitter is on; store in the illuminated_ir_values[] array
illuminated_ir_values [index] = 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;
+ 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 using calculate_reflected_distances(); store in reflected_ir_distances()
reflected_ir_distances [index] = calculate_reflected_distance( background_ir_values [index], illuminated_ir_values [index]);
ir_values_stored = 1;
@@ -264,141 +330,178 @@
// Returns the stored value of the reflected obstacle based on last call of either read_reflected_ir_distance or store_reflected_distances
// Introduced in API 0.6
-float PiSwarm::get_reflected_ir_distance ( char index ){
+float PiSwarm::get_reflected_ir_distance ( char index )
+{
// Sanity check: check range of index and that values have been stored
if (index>7 || ir_values_stored == 0) return 0.0;
- return reflected_ir_distances[index];
+ return reflected_ir_distances[index];
}
// Returns the stored value of the non-illuminated sensor based on last call of store_background_raw_ir_values
// Introduced in API 0.6
-unsigned short PiSwarm::get_background_raw_ir_value ( char index ){
+unsigned short PiSwarm::get_background_raw_ir_value ( char index )
+{
// Sanity check: check range of index and that values have been stored
if (index>7 || ir_values_stored == 0) return 0.0;
- return background_ir_values[index];
+ return background_ir_values[index];
}
-
+
// Returns the stored value of the illuminated sensor based on last call of store_illuminated_raw_ir_values
// Introduced in API 0.6
-unsigned short PiSwarm::get_illuminated_raw_ir_value ( char index ){
+unsigned short PiSwarm::get_illuminated_raw_ir_value ( char index )
+{
// Sanity check: check range of index and that values have been stored
if (index>7 || ir_values_stored == 0) return 0.0;
- return illuminated_ir_values[index];
+ return illuminated_ir_values[index];
}
-
+
// Stores the reflected distances for all 8 IR sensors
// Introduced in API 0.6
-void PiSwarm::store_reflected_ir_distances ( void ){
+void PiSwarm::store_reflected_ir_distances ( void )
+{
store_background_raw_ir_values();
store_illuminated_raw_ir_values();
- for(int i=0;i<8;i++){
- reflected_ir_distances [i] = calculate_reflected_distance( background_ir_values [i], illuminated_ir_values [i]);
- }
+ for(int i=0; i<8; i++) {
+ reflected_ir_distances [i] = calculate_reflected_distance( background_ir_values [i], illuminated_ir_values [i]);
+ }
}
-
+
// Stores the raw ADC values for all 8 IR sensors without enabling IR emitters
// Introduced in API 0.6
-void PiSwarm::store_background_raw_ir_values ( void ){
+void PiSwarm::store_background_raw_ir_values ( void )
+{
ir_values_stored = 1;
- for(int i=0;i<8;i++){
+ for(int i=0; i<8; i++) {
background_ir_values [i] = read_adc_value(i);
}
}
-
+
// Stores the raw ADC values for all 8 IR sensors with a 500us emitter pulse
// Introduced in API 0.6
-void PiSwarm::store_illuminated_raw_ir_values ( void ){
+void PiSwarm::store_illuminated_raw_ir_values ( void )
+{
//1. Check that the IR LDO regulator is on, enable if it isn't
- if(enable_ir_ldo == 0){
+ if(enable_ir_ldo == 0) {
enable_ir_ldo = 1;
enable_ldo_outputs();
}
-
- //2. Enable the front IR emitters and store the values
+
+ //2. Enable the front IR emitters and store the values
_irpulse_1 = 1;
- wait_us(500);
+ wait_us(500);
illuminated_ir_values [0] = read_adc_value(0);
illuminated_ir_values [1] = read_adc_value(1);
illuminated_ir_values [6] = read_adc_value(6);
illuminated_ir_values [7] = read_adc_value(7);
_irpulse_1 = 0;
-
- //3. Enable the rear+side IR emitters and store the values
+
+ //3. Enable the rear+side IR emitters and store the values
_irpulse_2 = 1;
- wait_us(500);
+ wait_us(500);
illuminated_ir_values [2] = read_adc_value(2);
illuminated_ir_values [3] = read_adc_value(3);
illuminated_ir_values [4] = read_adc_value(4);
illuminated_ir_values [5] = read_adc_value(5);
_irpulse_2 = 0;
}
-
+
// Converts a background and illuminated value into a distance
// Introduced in API 0.6 - used by read_reflected_ir_distance and store_reflected_ir_distances
-float PiSwarm::calculate_reflected_distance ( unsigned short background_value, unsigned short illuminated_value ){
+float PiSwarm::calculate_reflected_distance ( unsigned short background_value, unsigned short illuminated_value )
+{
float approximate_distance = 4000 + background_value - illuminated_value;
if(approximate_distance < 0) approximate_distance = 0;
-
+
// Very approximate: root value, divide by 2, approx distance in mm
approximate_distance = sqrt (approximate_distance) / 2.0;
-
+
// Then add adjustment value if value >27
if(approximate_distance > 27) {
float shift = pow(approximate_distance - 27,3);
approximate_distance += shift;
}
if(approximate_distance > 90) approximate_distance = 100.0;
- return approximate_distance;
+ return approximate_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 ){
+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){
+ 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;
+ 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);
+ 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;
+ 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 ){
+unsigned short PiSwarm::read_adc_value ( char index )
+{
short value = 0;
// Read a single value from the ADC
- if(index<8){
+ 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;
+ 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);
@@ -410,7 +513,8 @@
}
// 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 () {
+void PiSwarm::enable_ldo_outputs ()
+{
char data[2];
data[0] = 0x0A; //Address for PCA9505 Output Port 2
data[1] = 0;
@@ -424,36 +528,41 @@
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// 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 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)
+ 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
+// 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
+
+// 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 ){
+
+// 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 PiSwarm::read_magnetometer ( void )
+{
char read_data[7];
char success;
char command_data[1];
@@ -463,21 +572,24 @@
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;
+ 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 ){
+
+// 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 ){
+
+// 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 ){
+
+// 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];
}
@@ -486,7 +598,8 @@
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Returns the temperature reported by the MCP9701 sensor in degrees centigrade.
-float PiSwarm::read_temperature ( void ){
+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
@@ -495,7 +608,8 @@
}
// Returns the adjusted value (0-100) for the ambient light sensor
-float PiSwarm::read_light_sensor ( void ){
+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
@@ -504,7 +618,8 @@
}
-void PiSwarm::read_raw_sensors ( int * raw_ls_array ) {
+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();
@@ -514,12 +629,14 @@
}
// Returns the uptime of the system (since initialisation) in seconds
-float PiSwarm::get_uptime (void) {
+float PiSwarm::get_uptime (void)
+{
return _system_timer.read();
}
// Returns the battery level in millivolts
-float PiSwarm::battery() {
+float PiSwarm::battery()
+{
_ser.putc(SEND_BATTERY_MILLIVOLTS);
char lowbyte = _ser.getc();
char hibyte = _ser.getc();
@@ -528,7 +645,8 @@
}
// Returns the raw value for the variable resistor on the base of the platform. Ranges from 0 to 1024.
-float PiSwarm::pot_voltage(void) {
+float PiSwarm::pot_voltage(void)
+{
int volt = 0;
_ser.putc(SEND_TRIMPOT);
volt = _ser.getc();
@@ -537,12 +655,14 @@
}
// Returns the ID of platform (set by the DIL switches above the display). Range 0-31 [0 reserved].
-char PiSwarm::get_id () {
+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 () {
+char PiSwarm::get_switches ()
+{
return switches;
}
@@ -552,64 +672,114 @@
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Returns the target speed of the left motor (-1.0 to 1.0)
-float PiSwarm::get_left_motor (){
+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 (){
+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) {
+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) {
+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) {
+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) {
+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) {
+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) {
+void PiSwarm::right (float speed)
+{
motor(0,-1.0*speed);
motor(1,speed);
}
// Stop both motors
-void PiSwarm::stop (void) {
+void PiSwarm::stop (void)
+{
motor(0,0.0);
motor(1,0.0);
}
+// Turn at slow speed (0.15) to the set amount of degrees (clockwise)
+void PiSwarm::slow_turn (float degrees)
+{
+ float trunc = fmodf(degrees,360);
+ if(trunc<-180) trunc+=360;
+ if(trunc>180) trunc-=360;
+ if(trunc<0) {
+ float m_time = 1-trunc;
+ float delay = (m_time + logf(SLOW_TURN_LEFT_OFFSET + m_time + m_time)) * SLOW_TURN_LEFT_MULTIPLIER;
+ motor_timeout.attach_us(this,&PiSwarm::stop,(unsigned int) delay);
+ left(0.15);
+ } else if(trunc>0) {
+ float delay = (trunc + logf(SLOW_TURN_RIGHT_OFFSET + trunc + trunc)) * SLOW_TURN_RIGHT_MULTIPLIER;
+ motor_timeout.attach_us(this,&PiSwarm::stop,(unsigned int) delay);
+ right(0.15);
+ }
+}
+
+// Turn at fast speed (0.5) to the set amount of degrees (clockwise)
+void PiSwarm::fast_turn (float degrees)
+{
+ float trunc = fmodf(degrees,360);
+ if(trunc<-180) trunc+=360;
+ if(trunc>180) trunc-=360;
+ if(trunc<0) {
+ float m_time = 2.5-trunc;
+ float delay = m_time * FAST_TURN_LEFT_MULTIPLIER;
+ motor_timeout.attach_us(this,&PiSwarm::stop,(unsigned int) delay);
+ left(0.5);
+ } else if(trunc>0) {
+ float delay = (trunc + 2.5) * FAST_TURN_RIGHT_MULTIPLIER;
+ motor_timeout.attach_us(this,&PiSwarm::stop,(unsigned int) delay);
+ right(0.5);
+ }
+}
+
+void PiSwarm::test_turn_calibration(void)
+{
+}
+
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Sound Functions
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-void PiSwarm::play_tune (char * tune, int length) {
+void PiSwarm::play_tune (char * tune, int length)
+{
_ser.putc(DO_PLAY);
- _ser.putc(length);
+ _ser.putc(length);
for (int i = 0 ; i < length ; i++) {
- _ser.putc(tune[i]);
+ _ser.putc(tune[i]);
}
}
@@ -617,13 +787,15 @@
// Display Functions
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-void PiSwarm::locate(int x, int y) {
+void PiSwarm::locate(int x, int y)
+{
_ser.putc(DO_LCD_GOTO_XY);
_ser.putc(x);
_ser.putc(y);
}
-void PiSwarm::cls(void) {
+void PiSwarm::cls(void)
+{
_ser.putc(DO_CLEAR);
}
@@ -632,7 +804,8 @@
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-void PiSwarm::write_eeprom_byte ( int address, char data ){
+void PiSwarm::write_eeprom_byte ( int address, char data )
+{
char write_array[3];
write_array[0] = address / 256;
write_array[1] = address % 256;
@@ -641,8 +814,9 @@
//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){
+
+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
@@ -653,44 +827,45 @@
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];
- }
+ 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){
+
+ if(length > write_length) {
int page_count = (length + subpage) / 32;
int written = write_length;
- for(int p=0;p<page_count;p++){
+ 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++){
+ 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 PiSwarm::read_eeprom_byte ( int address )
+{
char address_array [2];
address_array[0] = address / 256;
address_array[1] = address % 256;
@@ -700,13 +875,15 @@
return data [0];
}
-char PiSwarm::read_next_eeprom_byte (){
+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 PiSwarm::read_eeprom_block ( int address, char length )
+{
char ret = 0;
return ret;
}
@@ -715,7 +892,8 @@
// RF Transceiver Functions
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-void PiSwarm::send_rf_message(char* message, char length){
+void PiSwarm::send_rf_message(char* message, char length)
+{
_rf.sendString(length, message);
}
@@ -723,22 +901,25 @@
// Setup and Calibration Functions
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-void PiSwarm::setup () {
+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();
+ beacon_led = setup_adc();
}
-void PiSwarm::setup_radio() {
+void PiSwarm::setup_radio()
+{
//Setup RF transceiver
_rf.rf_init();
_rf.setFrequency(RF_FREQUENCY);
_rf.setDatarate(RF_DATARATE);
}
-int PiSwarm::setup_expansion_ic () {
+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
@@ -752,16 +933,16 @@
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;
@@ -774,26 +955,28 @@
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
-
+ 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 ){
+char PiSwarm::setup_adc ( void )
+{
//ADC IC (for IR system) is a Analog Devices AD7997-BRUZ1
return 0;
}
-
-char PiSwarm::calibrate_gyro ( void ){
+
+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++){
+ for(int i=0; i<samples; i++) {
gyro_values[i] = _gyro.read();
wait(0.12);
}
@@ -801,23 +984,24 @@
float mean = 0;
float min = 3.5;
float max = 3.2;
- for(int i=0;i<samples;i++){
+ 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){
+ if(pass == 1) {
gyro_cal = mean / samples;
gyro_error = max - min;
calibrate_ticker.detach();
calibrate_led = 0;
}
return pass;
-}
+}
-char PiSwarm::calibrate_accelerometer ( void ){
+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];
@@ -825,7 +1009,7 @@
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++){
+ 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();
@@ -835,11 +1019,11 @@
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++){
+ 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;
@@ -847,13 +1031,13 @@
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;
+ 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 (z_test_value < z_min) z_min = z_test_value;
}
- if(pass == 1){
+ if(pass == 1) {
acc_x_cal = x_mean / samples;
acc_y_cal = y_mean / samples;
acc_z_cal = z_mean / samples;
@@ -861,15 +1045,16 @@
calibrate_led = 0;
}
return pass;
-}
+}
-char PiSwarm::calibrate_magnetometer ( void ){
+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]
+ 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);
}
@@ -877,7 +1062,8 @@
-void PiSwarm::interrupt_timeout_event ( void ){
+void PiSwarm::interrupt_timeout_event ( void )
+{
//Read switches
char data [1];
data [0] = 0x80; //Read input registers
@@ -888,26 +1074,30 @@
led_timeout.attach(this,&PiSwarm::led_timeout_event, 0.1); // Switch off the LED after 0.1s
}
-void PiSwarm::led_timeout_event ( void ){
+void PiSwarm::led_timeout_event ( void )
+{
interrupt_led = 0;
}
-void PiSwarm::expansion_interrupt_handler ( void ){
+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 ){
+void PiSwarm::calibrate_ticker_routine ( void )
+{
calibrate_led_state = 1 - calibrate_led_state;
calibrate_led = calibrate_led_state;
}
-void PiSwarm::test_oled(){
+void PiSwarm::test_oled()
+{
enable_led_ldo = 1;
enable_ldo_outputs();
set_oled_colour(100,100,100);
@@ -917,13 +1107,15 @@
_i2c.write(EXPANSION_IC_ADDRESS,data,2,false);
}
-void PiSwarm::reset () {
- // _nrst = 0;
+void PiSwarm::reset ()
+{
+ // _nrst = 0;
wait (0.01);
- // _nrst = 1;
+ // _nrst = 1;
wait (0.1);
}
-void PiSwarm::motor (int motor, float speed) {
+void PiSwarm::motor (int motor, float speed)
+{
char opcode = 0x0;
if (speed > 0.0) {
if (motor==1)
@@ -945,31 +1137,36 @@
}
-float PiSwarm::line_position() {
+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() {
+char PiSwarm::sensor_auto_calibrate()
+{
_ser.putc(AUTO_CALIBRATE);
return(_ser.getc());
}
-void PiSwarm::calibrate(void) {
+void PiSwarm::calibrate(void)
+{
_ser.putc(PI_CALIBRATE);
}
-void PiSwarm::reset_calibration() {
+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) {
+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);
@@ -977,50 +1174,58 @@
_ser.putc(d);
}
-void PiSwarm::PID_stop() {
+void PiSwarm::PID_stop()
+{
_ser.putc(STOP_PID);
}
-int PiSwarm::print (char* text, int length) {
- _ser.putc(DO_PRINT);
- _ser.putc(length);
+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]);
+ _ser.putc(text[i]);
}
return(0);
}
-int PiSwarm::_putc (int c) {
- _ser.putc(DO_PRINT);
- _ser.putc(0x1);
- _ser.putc(c);
+int PiSwarm::_putc (int c)
+{
+ _ser.putc(DO_PRINT);
+ _ser.putc(0x1);
+ _ser.putc(c);
wait (0.001);
return(c);
}
-int PiSwarm::_getc (void) {
+int PiSwarm::_getc (void)
+{
char r = 0;
return(r);
}
-int PiSwarm::putc (int c) {
+int PiSwarm::putc (int c)
+{
return(_ser.putc(c));
}
-int PiSwarm::getc (void) {
+int PiSwarm::getc (void)
+{
return(_ser.getc());
}
-void PiSwarm::start_system_timer(void) {
+void PiSwarm::start_system_timer(void)
+{
_system_timer.reset();
_system_timer.start();
}
#ifdef MBED_RPC
-const rpc_method *PiSwarm::get_rpc_methods() {
+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> },
@@ -1037,92 +1242,571 @@
}
#endif
-void display_system_time(){
- if(PISWARM_DEBUG == 1){
+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());
+ printf("Current system time:%s Uptime:%.1f\n",ctime(&system_time),piswarm.get_uptime());
+ }
+}
+
+void oled_bearing_strobe()
+{
+ bearing_strobe_step ++;
+ if(bearing_strobe_step > 10) {
+ oled_ticker.detach();
+ piswarm.restore_oled_state();
+ } else {
+ float position = (360 - bearing) / 36;
+ position -= 1;
+ if(position < 0) position += 10;
+ piswarm.set_oleds(0,0,0,0,0,0,0,0,0,0);
+ int fposition = floor(position);
+ int sposition = ceil(position);
+ if(sposition > 9) sposition = 0;
+ switch(bearing_strobe_step % 5) {
+ case 4:
+ piswarm.set_oled_colour(255,0,0);
+ if(sposition - position > 0.5) {
+ piswarm.set_oled(fposition,1);
+ } else piswarm.set_oled(sposition,1);
+ break;
+ case 3:
+ piswarm.set_oled_colour(155,30,0);
+ if(sposition - position > 0.5) {
+ piswarm.set_oled(fposition,1);
+ } else piswarm.set_oled(sposition,1);
+ break;
+ case 1:
+ case 2:
+ piswarm.set_oled_colour(100,25,0);
+ piswarm.set_oled(fposition,1);
+ piswarm.set_oled(sposition,1);
+ break;
+ case 0:
+ piswarm.set_oled_colour(60,10,0);
+ piswarm.set_oled(fposition,1);
+ piswarm.set_oled(sposition,1);
+ fposition --;
+ if(fposition<0)fposition = 9;
+ sposition ++;
+ if(sposition>9)sposition = 0;
+ piswarm.set_oled(fposition,1);
+ piswarm.set_oled(sposition,1);
+ break;
+ }
+ piswarm.activate_oleds();
+ }
+}
+
+void oled_ticker_update()
+{
+ oled_ticker_step++;
+ if(oled_ticker_step == 10) oled_ticker_step = 0;
+ switch(oled_ticker_step) {
+ case 0:
+ piswarm.set_oleds(1,1,0,0,0,0,0,0,0,0);
+ break;
+ case 1:
+ piswarm.set_oleds(0,1,1,0,0,0,0,0,0,0);
+ break;
+ case 2:
+ piswarm.set_oleds(0,0,1,1,0,0,0,0,0,0);
+ break;
+ case 3:
+ piswarm.set_oleds(0,0,0,1,1,0,0,0,0,0);
+ break;
+ case 4:
+ piswarm.set_oleds(0,0,0,0,1,1,0,0,0,0);
+ break;
+ case 5:
+ piswarm.set_oleds(0,0,0,0,0,1,1,0,0,0);
+ break;
+ case 6:
+ piswarm.set_oleds(0,0,0,0,0,0,1,1,0,0);
+ break;
+ case 7:
+ piswarm.set_oleds(0,0,0,0,0,0,0,1,1,0);
+ break;
+ case 8:
+ piswarm.set_oleds(0,0,0,0,0,0,0,0,1,1);
+ break;
+ case 9:
+ piswarm.set_oleds(1,0,0,0,0,0,0,0,0,1);
+ break;
+
}
}
-void init() {
+float synchronise_with_beacon()
+{
+ beacon_led = 1;
+ int offset_time = 0;
+ float confidence = -1;
+ float angle_confidence = 0;
+ float heading = 0;
+ float i_bearing = 0;
+ Timer ir_timer;
+ unsigned short ir_values[8][100];
+ unsigned int ir_totals[8];
+ ir_timer.start();
+ int target_time = 0;
+ for(int i=0; i<8; i++) {
+ ir_totals[i]=0;
+ }
+ for(int step = 0; step < 100; step ++) {
+ target_time += 10000 ;
+ piswarm.store_background_raw_ir_values ();
+ for(int sensor = 0; sensor < 8; sensor ++) {
+ ir_values[sensor][step] = piswarm.get_background_raw_ir_value(sensor);
+ ir_totals[sensor] += ir_values[sensor][step];
+ }
+ while(ir_timer.read_us() < target_time) wait_us(50);
+ }
+ int total = 0;
+ for(int i=0; i<8; i++) {
+ total += ir_totals[i];
+ }
+ total /= 800;
+ unsigned short s_pass [100];
+ int pass_count = 0;
+ for(int i=0; i<100; i++) {
+ signed short adjusted [8];
+ int pass = 0;
+ for(int k=0; k<8; k++) {
+ adjusted[k]=ir_values[k][i]-total;
+ if(adjusted[k] > 0) pass++;
+ }
+ s_pass [i] = pass;
+ if(pass>0) pass_count++;
+ }
+ if(pass_count < 2) {
+ confidence = -1;
+ } else {
+ //Find highest value
+ int highest = 0;
+ int highest_pos = 0;
+ for(int k=0; k<100; k++) {
+ if(s_pass[k] > highest) {
+ highest = s_pass[k];
+ highest_pos = k;
+ }
+ }
+ int length = 1;
+ //Adjust to find start: best length should be 2 or 3: compare previous value to
+ int pre = highest_pos -1;
+ if(pre < 0) pre=99;
+ int spost = highest_pos+1;
+ int post = highest_pos +2;
+ if(spost > 99) spost-=100;
+ if(post > 99) post-=100;
+ if(s_pass[pre] > 0 && s_pass[pre] >= s_pass[post]) {
+ highest_pos = pre;
+ length++;
+ }
+ if(s_pass[spost] > 0)length++;
+ if(s_pass[post] > s_pass[spost] && s_pass[spost] > 0)length++;
+ if(length>3) confidence = 0;
+ else if(length==1) confidence = 0;
+ else if(length==pass_count) confidence = 1;
+ else confidence = (float) length / (float) pass_count;
+ offset_time = highest_pos * 10;
+ if(length == 2) offset_time -= 5;
+ if(offset_time < 0) offset_time += 1000;
+ if(confidence > 0) {
+ unsigned short sums[8];
+ for(int i=0; i<8; i++) {
+ sums[i] = 0;
+ for(int l=0; l<length; l++) {
+ int sp = highest_pos + l;
+ if(sp>99)sp-=100;
+ signed short adj = ir_values[i][sp];
+ if(adj>0)sums[i]+=adj;
+ }
+ }
+ int peak_sum = 0;
+ int peak_position = 0;
+ for(int i=0; i<8; i++) {
+ if(sums[i]>peak_sum) {
+ peak_sum = sums[i];
+ peak_position = i;
+ }
+ }
+ float relative_sums [8];
+ float relative_total = 0;
+ for(int i=0; i<8; i++) {
+ relative_sums[i] = (float) sums[i] / (float) peak_sum;
+ relative_total += relative_sums[i];
+ }
+ int previous = peak_position - 1;
+ int d_prev = peak_position - 2;
+ int d_next = peak_position + 2;
+ if(d_prev < 0) d_prev += 8;
+ if(d_next > 7) d_next -= 8;
+ if(previous<0) previous+=8;
+ int next = peak_position + 1;
+ if(next > 7) next -= 8;
+ angle_confidence = (relative_sums[next] + relative_sums[previous] + 1) / relative_total;
+ heading = ((relative_sums[next] - relative_sums[previous] - relative_sums[d_prev] - relative_sums[d_prev] + relative_sums[d_next] + relative_sums[d_next]) * .5) + peak_position;
+ if(heading<0) heading+= 8;
+ if(heading>8) heading-= 8;
+ //Convert heading to bearing
+ int whole_part = (int) heading;
+ float remainder = heading - whole_part;
+ i_bearing = 0;
+ switch(whole_part) {
+ case 0:
+ i_bearing = 15 + (30*remainder);
+ break;
+ case 1:
+ i_bearing = 45 + (45*remainder);
+ break;
+ case 2:
+ i_bearing = 90 + (62*remainder);
+ break;
+ case 3:
+ i_bearing = 152 + (56*remainder);
+ break;
+ case 4:
+ i_bearing = 208 + (62*remainder);
+ break;
+ case 5:
+ i_bearing = 270 + (45 * remainder);
+ break;
+ case 6:
+ i_bearing = 315 + (30 * remainder);
+ break;
+ case 7:
+ i_bearing = 345 + (30 * remainder);
+ break;
+ }
+ if(i_bearing>=360)i_bearing-=360;
+ } else angle_confidence = 0;
+ }
+ if(angle_confidence > 0.4 && confidence > 0.4) {
+ int delay = 1990+offset_time;
+ if(delay > 2800) delay -= 1000;
+ bearing = 360-i_bearing;
+ is_synced = 1;
+ saved_peak = total;
+ while(ir_timer.read_ms() < delay) wait_us(50);
+ } else is_synced = 0;
+ beacon_led = 0;
+ hit_sync_count = 0;
+ miss_sync_count = 0;
+ beacon_confidence = confidence;
+ bearing_confidence = angle_confidence;
+ return confidence;
+}
+
+
+float get_bearing_from_beacon()
+{
+ beacon_led = 1;
+ float confidence = -1;
+ float angle_confidence = 0;
+ float heading = 0;
+ float i_bearing = 0;
+ Timer ir_timer;
+ unsigned short ir_values[8][5];
+ unsigned int ir_totals[8];
+ ir_timer.start();
+ int target_time = 0;
+ for(int i=0; i<8; i++) {
+ ir_totals[i]=0;
+ }
+ for(int step = 0; step < 5; step ++) {
+ target_time += 10000 ;
+ piswarm.store_background_raw_ir_values ();
+ for(int sensor = 0; sensor < 8; sensor ++) {
+ ir_values[sensor][step] = piswarm.get_background_raw_ir_value(sensor);
+ ir_totals[sensor] += ir_values[sensor][step];
+ }
+ while(ir_timer.read_us() < target_time) wait_us(50);
+ }
+ int total = 0;
+ for(int i=0; i<8; i++) {
+ total += ir_totals[i];
+ }
+ total /= 40;
+ unsigned short s_pass [5];
+ int pass_count = 0;
+ for(int i=0; i<5; i++) {
+ signed short adjusted [8];
+ int pass = 0;
+ for(int k=0; k<8; k++) {
+ adjusted[k]=ir_values[k][i]-total;
+ if(adjusted[k] > 0) pass++;
+ }
+ s_pass [i] = pass;
+ if(pass>0) pass_count++;
+ //pc.printf("I:%d 1:%d 2:%d 3:%d 4:%d 5:%d 6:%d 7:%d 8:%d T:%d\n",i,ir_values[0][i],ir_values[1][i],ir_values[2][i],ir_values[3][i],ir_values[4][i],ir_values[5][i],ir_values[6][i],ir_values[7][i],total);
+ }
+ if(pass_count < 2 || pass_count > 3 || total<(saved_peak / 4)) {
+ confidence = -1;
+ angle_confidence = 0;
+ } else {
+ confidence = 1;
+
+ unsigned short sums[8];
+ for(int i=0; i<8; i++) {
+ sums[i] = 0;
+ for(int l=1; l<4; l++) {
+ signed short adj = ir_values[i][l] - total;
+ if(adj>0)sums[i]+=adj;
+ }
+ }
+ int peak_sum = 0;
+ int peak_position = 0;
+ for(int i=0; i<8; i++) {
+ if(sums[i]>peak_sum) {
+ peak_sum = sums[i];
+ peak_position = i;
+ }
+ }
+ float relative_sums [8];
+ float relative_total = 0;
+ for(int i=0; i<8; i++) {
+ relative_sums[i] = (float) sums[i] / (float) peak_sum;
+ relative_total += relative_sums[i];
+ }
+ int previous = peak_position - 1;
+ int d_prev = peak_position - 2;
+ int d_next = peak_position + 2;
+ if(d_prev < 0) d_prev += 8;
+ if(d_next > 7) d_next -= 8;
+ if(previous<0) previous+=8;
+ int next = peak_position + 1;
+ if(next > 7) next -= 8;
+ angle_confidence = (relative_sums[next] + relative_sums[previous] + 1) / relative_total;
+ heading = ((relative_sums[next] - relative_sums[previous] - relative_sums[d_prev] - relative_sums[d_prev] + relative_sums[d_next] + relative_sums[d_next]) * .5) + peak_position;
+ if(heading<0) heading+= 8;
+ if(heading>8) heading-= 8;
+ //Convert heading to bearing
+ int whole_part = (int) heading;
+ float remainder = heading - whole_part;
+ i_bearing = 0;
+ switch(whole_part) {
+ case 0:
+ i_bearing = 15 + (30*remainder);
+ break;
+ case 1:
+ i_bearing = 45 + (45*remainder);
+ break;
+ case 2:
+ i_bearing = 90 + (62*remainder);
+ break;
+ case 3:
+ i_bearing = 152 + (56*remainder);
+ break;
+ case 4:
+ i_bearing = 208 + (62*remainder);
+ break;
+ case 5:
+ i_bearing = 270 + (45 * remainder);
+ break;
+ case 6:
+ i_bearing = 315 + (30 * remainder);
+ break;
+ case 7:
+ i_bearing = 345 + (30 * remainder);
+ break;
+ }
+ if(i_bearing>=360)i_bearing-=360;
+
+ }
+ if(angle_confidence > 0.4 && confidence==1) {
+ bearing = 360-i_bearing;
+ is_synced = 1;
+ hit_sync_count ++;
+ if(hit_sync_count > 4) {
+ miss_sync_count = 0;
+ }
+ } else is_synced = 0;
+ beacon_led = 0;
+ beacon_confidence = confidence;
+ bearing_confidence = angle_confidence;
+ return confidence;
+}
+
+void update_bearing()
+{
+ get_bearing_from_beacon();
+ if(is_synced == 1) {
+ if(DISPLAY_BEACON_READING == 1) display_bearing_info();
+ highlight_bearing();
+ } else {
+ miss_sync_count ++;
+ hit_sync_count = 0;
+ if(miss_sync_count > 3) {
+ beacon_ticker.detach();
+ if(DISPLAY_BEACON_READING == 1) {
+ piswarm.cls();
+ piswarm.printf("Lost ");
+ piswarm.locate(0,1);
+ piswarm.printf("Beacon ");
+ }
+ beacon_ticker.attach(resync_with_beacon,3.9);
+ }
+ oled_ticker.detach();
+ if(DISPLAY_BEACON_READING)piswarm.cls();
+ }
+}
+
+void resync_with_beacon()
+{
+ if(synchronise_with_beacon() > 0.4) {
+ beacon_ticker.detach();
+ beacon_ticker.attach(update_bearing,1);
+ if(DISPLAY_BEACON_READING == 1) display_bearing_info();
+ else {
+ piswarm.cls();
+ piswarm.printf("SYNCED");
+ piswarm.locate(0,1);
+ piswarm.printf("B:%3.1f",bearing);
+ }
+ highlight_bearing();
+ } else {
+ piswarm.cls();
+ piswarm.printf("NO SYNC");
+ piswarm.turn_off_all_oleds();
+ oled_ticker.detach();
+ }
+}
+
+void highlight_bearing()
+{
+ piswarm.save_oled_state();
+ bearing_strobe_step = 0;
+ piswarm.set_oled_colour(255,255,255);
+ oled_ticker.detach();
+ oled_ticker.attach(oled_bearing_strobe,0.02);
+}
+
+void display_bearing_info()
+{
+ piswarm.cls();
+ piswarm.printf("B:%3.1f",bearing);
+ piswarm.locate(0,1);
+ piswarm.printf("C:%3.1f",bearing_confidence*100);
+}
+
+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");
+ if(PISWARM_DEBUG == 1)pc.printf("PiSwarm Firmware 0.7 Initialisation...\n");
display_system_time();
- piswarm.play_tune("T298MSCG>C",10);
+ piswarm.play_tune("T596MSCEFGAB>C",14);
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);
+ oled_ticker.attach(oled_ticker_update,0.02);
+ wait(0.2);
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);
+ wait(0.2);
+ piswarm.set_cled_colour(0,0,200);
+ piswarm.set_oled_colour(0,0,200);
+ if(CALIBRATE_MEMS == 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);
+ if(PISWARM_DEBUG == 1)pc.printf("Test Magnetometer: ");
+ if(piswarm.calibrate_magnetometer() != 0) {
+ if(PISWARM_DEBUG == 1)pc.printf("FAILED\n");
+ 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");
+ } else {
+ wait(0.2);
+ }
+ oled_ticker.detach();
+ piswarm.turn_off_all_oleds();
+ if(USE_BEACON == 1) {
piswarm.cls();
- piswarm.locate(0,0);
- piswarm.printf("Gyro Cal");
+ piswarm.printf("LOCATING");
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.printf(" BEACON");
+ piswarm.set_cled_colour(0,200,0);
+ piswarm.set_oled_colour(150,200,150);
+ oled_ticker.attach(oled_ticker_update,0.1);
+ if(synchronise_with_beacon() > 0.4) {
+ beacon_ticker.attach(update_bearing,1);
+ if(DISPLAY_BEACON_READING == 1) display_bearing_info();
+ else {
+ piswarm.cls();
+ piswarm.printf("SYNCED");
+ piswarm.locate(0,1);
+ piswarm.printf("B:%3.1f",bearing);
+ }
+ highlight_bearing();
+ } else {
+ piswarm.cls();
+ piswarm.printf(" 2ND");
+ piswarm.locate(0,1);
+ piswarm.printf("ATTEMPT");
+ if(synchronise_with_beacon() > 0.4) {
+ beacon_ticker.attach(update_bearing,1);
+ if(DISPLAY_BEACON_READING == 1) display_bearing_info();
+ else {
+ piswarm.cls();
+ piswarm.printf("SYNCED");
+ piswarm.locate(0,1);
+ piswarm.printf("B:%3.1f",bearing);
+ }
+ highlight_bearing();
+ } else {
+ piswarm.cls();
+ piswarm.printf("NO SYNC");
+ piswarm.turn_off_all_oleds();
+ oled_ticker.detach();
+ beacon_ticker.attach(resync_with_beacon,10);
+ }
+ }
+ }
+ wait(0.5);
+ oled_ticker.detach();
piswarm.turn_off_all_oleds();
- wait(0.1);
piswarm.cls();
piswarm.set_cled_colour(0,0,0);
- if(START_RADIO_ON_BOOT == 1){
+ if(START_RADIO_ON_BOOT == 1) {
if(PISWARM_DEBUG == 1)pc.printf("Setting up 433MHz Transceiver\n");
piswarm.setup_radio();
}
@@ -1137,7 +1821,7 @@
* 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 *
+ * 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 *
--- a/piswarm.h Tue Feb 18 21:05:32 2014 +0000
+++ b/piswarm.h Mon Jun 30 07:58:31 2014 +0000
@@ -27,6 +27,9 @@
#endif
// The following defines are user parameters and may be edited
+// USE_BEACON - if Pi Swarm table with IR beacon is used, set to 1 to allow sync on boot.
+// DISPLAY_BEACON_READING - if value is 1, display shows beacon reading on each update
+// CALIBRATE_MEMS - set to 1 to calibrate MEMS sensors on boot
// SWARM_SIZE - size of swarm (+1). Max 32.
// USE_COMMUNICATION_STACK - 1 to enable, 0 to bypass
// CALIBRATE_COLOURS - 1 to increase blue intensity, 0 to not
@@ -44,8 +47,17 @@
// RF_FREQUENCY - Frequency of RF chip. See datasheet for details (default 435000000)
// RF_DATARATE - Baud rate of the RF chip. Max 115200, default 57600.
+#define USE_BEACON 1
+#define DISPLAY_BEACON_READING 1
+#define CALIBRATE_MEMS 0
+#define SLOW_TURN_LEFT_MULTIPLIER 5200
+#define SLOW_TURN_RIGHT_MULTIPLIER 5450
+#define SLOW_TURN_LEFT_OFFSET 1
+#define SLOW_TURN_RIGHT_OFFSET 4
+#define FAST_TURN_LEFT_MULTIPLIER 1450
+#define FAST_TURN_RIGHT_MULTIPLIER 1450
#define SWARM_SIZE 32
-#define USE_COMMUNICATION_STACK 1
+#define USE_COMMUNICATION_STACK 0
#define CALIBRATE_COLOURS 1
#define CENTER_LED_BRIGHTNESS 30
#define OUTER_LED_BRIGHTNESS 50
@@ -55,7 +67,7 @@
#define RF_USE_TDMA 1
#define RF_TDMA_TIME_PERIOD_US 15625
#define PISWARM_DEBUG 1
-#define RF_DEBUG 1
+#define RF_DEBUG 0
#define RF_VERBOSE 0
#define START_RADIO_ON_BOOT 1
#define RF_FREQUENCY 435000000
@@ -115,6 +127,12 @@
// Disable all the LEDs and the dedicated power supply
void turn_off_all_oleds(void);
+
+ // Save the current colour and state of OLEDs [introduced in API 0.7]
+ void save_oled_state(void);
+
+ // Restore the saved colour and state of OLEDs [introduced in API 0.7]
+ void restore_oled_state(void);
// Set brightness of outer LEDs (works by increasing total period for PWM)
void set_oled_brightness ( char brightness );
@@ -267,6 +285,14 @@
// Play a tune on the 3-pi buzzer (see notes at end of file for more details)
void play_tune (char * tune, int length);
+ // Turn at slow speed (0.15) to the set amount of degrees (clockwise)
+ void slow_turn (float degrees);
+
+ // Turn at fast speed (0.5) to the set amount of degrees (clockwise)
+ void fast_turn (float degrees);
+
+ void test_turn_calibration(void);
+
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// EEPROM Functions
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
@@ -398,6 +424,10 @@
};
+void display_bearing_info(void);
+void resync_with_beacon(void);
+void update_bearing(void);
+void highlight_bearing(void);
void display_system_time(void);
void init(void);
extern Serial pc;