James Hilder
Unfinished v0.7, added bearing detection
Fork of
Pi_Swarm_Library_v06_alpha
by 
piswarm
piswarm.cpp
- Committer:
- jah128
- Date:
- 2014-06-30
- Revision:
- 11:5ebcb52726cf
- Parent:
- 9:7a4fc1d7e484
File content as of revision 11:5ebcb52726cf:
/*******************************************************************************************
*
* 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
*
* Designed for use with the Pi Swarm Board (enhanced MBED sensor board) v1.2
*
******************************************************************************************/
#include "piswarm.h"
// Variables
DigitalOut gpio_led (LED1);
DigitalOut calibrate_led (LED2);
DigitalOut beacon_led (LED3);
DigitalOut interrupt_led (LED4);
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;
char switches = 0;
char cled_red = 0;
char cled_green = 0;
char cled_blue = 0;
char oled_red = 0;
char oled_green = 0;
char oled_blue = 0;
char cled_enable = 0;
char 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;
float gyro_error = 0;
float acc_x_cal = 3350;
float acc_y_cal = 3350;
float acc_z_cal = 3350;
float left_speed = 0;
float right_speed = 0;
signed short mag_values [3];
unsigned short background_ir_values [8];
unsigned short illuminated_ir_values [8];
float reflected_ir_distances [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)
{
setup();
reset();
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Outer LED Functions
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Returns the current setting for the outer LED colour. Returned colour is stored as 24-bit positive integer (R<<16 + G<<8 + B). Used by communication stack.
int PiSwarm::get_oled_colour()
{
return (oled_red << 16) + (oled_green << 8) + oled_blue;
}
// Returns the current enable state an individual LED. oled = LED to return enable state. Returns 0 for disabled, 1 for enabled
char PiSwarm::get_oled_state(char oled)
{
if(oled_array[oled] == 1) return 1;
return 0;
}
// Set the colour of the outer LED. Values for red, green and blue range from 0 (off) to 255 (maximum).
void PiSwarm::set_oled_colour( char red, char green, char blue )
{
oled_red = red;
oled_green = green;
oled_blue = blue;
_oled_r.pulsewidth_us(red);
_oled_g.pulsewidth_us(green);
_oled_b.pulsewidth_us(blue);
}
// 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;
}
// 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)
{
oled_array[0]=oled_0;
oled_array[1]=oled_1;
oled_array[2]=oled_2;
oled_array[3]=oled_3;
oled_array[4]=oled_4;
oled_array[5]=oled_5;
oled_array[6]=oled_6;
oled_array[7]=oled_7;
oled_array[8]=oled_8;
oled_array[9]=oled_9;
activate_oleds();
}
// Sets all outer LEDs to disabled and turns off LED power supply.
void PiSwarm::turn_off_all_oleds()
{
for(int i=0; i<10; i++) {
oled_array[i]=0;
}
activate_oleds();
enable_led_ldo = 0;
enable_ldo_outputs();
char data[3];
data[0]=0x88; //Write to bank 1 then bank 2
data[1]=0x00; //Reset everything in bank 1
data[2]=0x00; //Reset everything in bank 2
_i2c.write(EXPANSION_IC_ADDRESS,data,3,false);
}
// Set the outer LED brightness (total period of PWM output increases as brightness decreases). Ranges from 0 (minimum) to 255 (maximum)
void PiSwarm::set_oled_brightness ( char brightness )
{
if ( brightness > 100 ) brightness = 100;
oled_brightness = brightness;
int oled_period = (104 - brightness);
oled_period *= oled_period;
oled_period /= 10;
oled_period += 255;
if(CALIBRATE_COLOURS!=0)_oled_r.period_us(oled_period * 2);
else _oled_r.period_us(oled_period);
if(CALIBRATE_COLOURS!=0)_oled_g.period_us(oled_period * 2);
else _oled_g.period_us(oled_period);
_oled_b.period_us(oled_period);
}
// Sends the messages to enable/disable outer LEDs
void PiSwarm::activate_oleds()
{
if(enable_led_ldo == 0) {
enable_led_ldo = 1;
enable_ldo_outputs();
}
char data[3];
data[0]=0x88; //Write to bank 1 then bank 2
data[1]=0x00; //Reset everything in bank 1
data[2]=0x00; //Reset everything in bank 2
if(oled_array[0]>0) data[1]+=1;
if(oled_array[1]>0) data[1]+=2;
if(oled_array[2]>0) data[1]+=4;
if(oled_array[3]>0) data[1]+=8;
if(oled_array[4]>0) data[1]+=16;
if(oled_array[5]>0) data[1]+=32;
if(oled_array[6]>0) data[1]+=64;
if(oled_array[7]>0) data[1]+=128;
if(oled_array[8]>0) data[2]+=1;
if(oled_array[9]>0) data[2]+=2;
_i2c.write(EXPANSION_IC_ADDRESS,data,3,false);
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Center LED Functions
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Returns the current setting for the center LED colour. Returned colour is stored as 24-bit positive integer (R<<16 + G<<8 + B). Used by communication stack.
int PiSwarm::get_cled_colour()
{
return (cled_red << 16) + (cled_green << 8) + cled_blue;
}
// 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 )
{
cled_red = red;
cled_green = green;
cled_blue = blue;
if(cled_enable != 0) enable_cled(1);
}
// Turn on or off the center LED. enable=1 to turn on, 0 to turn off
void PiSwarm::enable_cled( char enable )
{
cled_enable = enable;
if(enable != 0) {
_cled_r.pulsewidth_us(cled_red);
_cled_g.pulsewidth_us(cled_green);
_cled_b.pulsewidth_us(cled_blue);
} else {
_cled_r.pulsewidth_us(0);
_cled_g.pulsewidth_us(0);
_cled_b.pulsewidth_us(0);
}
}
// Set the center LED brightness (total period of PWM output increases as brightness decreases). Ranges from 0 (minimum) to 255 (maximum)
void PiSwarm::set_cled_brightness ( char brightness )
{
if( brightness > 100 ) brightness = 100;
// Brightness is set by adjusting period of PWM. Period ranged from ~260uS at 100% to 1336uS at 0%
// When calibrate_colours = 1, red = 2x normal, green = 2x normal
cled_brightness = brightness;
int cled_period = (104 - brightness);
cled_period *= cled_period;
cled_period /= 10;
cled_period += 255;
if(CALIBRATE_COLOURS!=0)_cled_r.period_us(cled_period * 2);
else _cled_r.period_us(cled_period);
if(CALIBRATE_COLOURS!=0)_cled_g.period_us(cled_period * 2);
else _cled_g.period_us(cled_period);
_cled_b.period_us(cled_period);
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// IR Sensor Functions
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Estimates the distance to an obstacle from one of the IR sensors, defined by index (range 0-7).
// The value is converted to an approximate distance in millimetres, or 100.0 if no obstacle found.
// 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 )
{
// 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) {
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;
}
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;
}
//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;
return reflected_ir_distances [index];
}
// 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 )
{
// 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];
}
// 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 )
{
// 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];
}
// 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 )
{
// 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];
}
// Stores the reflected distances for all 8 IR sensors
// Introduced in API 0.6
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]);
}
}
// 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 )
{
ir_values_stored = 1;
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 )
{
//1. Check that the IR LDO regulator is on, enable if it isn't
if(enable_ir_ldo == 0) {
enable_ir_ldo = 1;
enable_ldo_outputs();
}
//2. Enable the front IR emitters and store the values
_irpulse_1 = 1;
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
_irpulse_2 = 1;
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 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;
}
// Returns the illuminated raw sensor value for the IR sensor defined by index (range 0-7); turns on the emitters for a 500us pulse
unsigned short PiSwarm::read_illuminated_raw_ir_value ( char index )
{
//This function reads the IR strength when illuminated - used for PC system debugging purposes
//1. Check that the IR LDO regulator is on, enable if it isn't
if(enable_ir_ldo == 0) {
enable_ir_ldo = 1;
enable_ldo_outputs();
}
//2. Enable the relevant IR emitter by turning on its pulse output
switch(index) {
case 0:
case 1:
case 6:
case 7:
_irpulse_1 = 1;
break;
case 2:
case 3:
case 4:
case 5:
_irpulse_2 = 1;
break;
}
wait_us(500);
//3. Read the ADC value now IR emitter is on
unsigned short strong_value = read_adc_value ( index );
//4. Turn off IR emitter
switch(index) {
case 0:
case 1:
case 6:
case 7:
_irpulse_1 = 0;
break;
case 2:
case 3:
case 4:
case 5:
_irpulse_2 = 0;
break;
}
return strong_value;
}
// Returns the raw sensor value for the IR sensor defined by index (range 0-7).
unsigned short PiSwarm::read_adc_value ( char index )
{
short value = 0;
// Read a single value from the ADC
if(index<8) {
char apb[1];
char data[2];
switch(index) {
case 0:
apb[0]=0x80;
break;
case 1:
apb[0]=0x90;
break;
case 2:
apb[0]=0xA0;
break;
case 3:
apb[0]=0xB0;
break;
case 4:
apb[0]=0xC0;
break;
case 5:
apb[0]=0xD0;
break;
case 6:
apb[0]=0xE0;
break;
case 7:
apb[0]=0xF0;
break;
}
_i2c.write(ADC_ADDRESS,apb,1,false);
_i2c.read(ADC_ADDRESS,data,2,false);
value=((data[0] % 16)<<8)+data[1];
if(value > 4096) value=4096;
value=4096-value;
}
return value;
}
// Function enables or disables the LDO voltage regulators which supply power to the outer LEDs and the IR photo emitters.
void PiSwarm::enable_ldo_outputs ()
{
char data[2];
data[0] = 0x0A; //Address for PCA9505 Output Port 2
data[1] = 0;
if(enable_led_ldo) data[1] +=128;
if(enable_ir_ldo) data[1] +=64;
_i2c.write(EXPANSION_IC_ADDRESS,data,2,false);
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// MEMS Sensor Functions
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Returns the yaw (clockwise) rotation in degrees-per-second reported by the LY3200 gyroscope.
float PiSwarm::read_gyro ( void )
{
// Gyro is a ST Micro LY3200ALH Yaw-rate Gyro: 0.67mV / d.p.s, 1.5V offset
float r_gyro = _gyro.read();
r_gyro *= gyro_cal; // NB 3.4 seemed to work better that 3.3 but better to run calibration routine
r_gyro = 1.5 - r_gyro; // 1.5V off-set, invert sign
r_gyro *= 1492.5; // Convert to DPS (+ve = clockwise, -ve = c.clockwise)
return r_gyro;
}
// Returns the acceleration in the X plane reported by the LIS332 accelerometer. Returned value is in mg.
float PiSwarm::read_accelerometer_x ( void )
{
// LIS 332 AX Accelerometer - 0.363 V/g - 1.25V
float r_acc_x = (( _accel_x.read() * acc_x_cal ) - 1250.0 ) * 2.754; // Return value in mG
return r_acc_x;
}
// Returns the acceleration in the Y plane reported by the LIS332 accelerometer. Returned value is in mg.
float PiSwarm::read_accelerometer_y ( void )
{
float r_acc_y = (( _accel_y.read() * acc_y_cal ) - 1250.0 ) * 2.754; // Return value in mG
return r_acc_y;
}
// Returns the acceleration in the Z plane reported by the LIS332 accelerometer. Returned value is in mg.
float PiSwarm::read_accelerometer_z ( void )
{
float r_acc_z = (( _accel_z.read() * acc_z_cal ) - 1250.0 ) * 2.754; // Return value in mG
return r_acc_z;
}
// Sends message to the magnetometer to read new values. Should be called before get_magnetometer_* when updated values are required
char PiSwarm::read_magnetometer ( void )
{
char read_data[7];
char success;
char command_data[1];
command_data[0] = 0x00; //Auto-read from register 0x00 (status)
success = _i2c.write(MAGNETOMETER_ADDRESS, command_data, 1, false);
_i2c.read(MAGNETOMETER_ADDRESS, read_data, 7, false);
mag_values[0] = (read_data[1] << 8) + read_data[2];
mag_values[1] = (read_data[3] << 8) + read_data[4];
mag_values[2] = (read_data[5] << 8) + read_data[6];
return success;
}
// Returns the raw value for the X-plane magnetic field stored on the last call of read_magnetometer
signed short PiSwarm::get_magnetometer_x ( void )
{
return mag_values[0];
}
// Returns the raw value for the Y-plane magnetic field stored on the last call of read_magnetometer
signed short PiSwarm::get_magnetometer_y ( void )
{
return mag_values[1];
}
// Returns the raw value for the Z-plane magnetic field stored on the last call of read_magnetometer
signed short PiSwarm::get_magnetometer_z ( void )
{
return mag_values[2];
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Other Sensor Functions
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Returns the temperature reported by the MCP9701 sensor in degrees centigrade.
float PiSwarm::read_temperature ( void )
{
// Temperature Sensor is a Microchip MCP9701T-E/LT: 19.5mV/C 0C = 400mV
float r_temp = _temperature.read();
r_temp -= 0.1212f; // 0.4 / 3.3
r_temp *= 169.231f; // 3.3 / .0195
return r_temp;
}
// Returns the adjusted value (0-100) for the ambient light sensor
float PiSwarm::read_light_sensor ( void )
{
// Light sensor is a Avago APDS-9005 Ambient Light Sensor
//float r_light = (_light.read() * 141); //Gives a range of (approximately) 0=dark 100=max. light
float r_light = (sqrt(_light.read() * 1.41) * 100); //Non-linear and more CPU intensive but generally gives better low-light measures
if(r_light > 100) r_light = 100;
return r_light;
}
void PiSwarm::read_raw_sensors ( int * raw_ls_array )
{
_ser.putc(SEND_RAW_SENSOR_VALUES);
for (int i = 0; i < 5 ; i ++) {
int val = _ser.getc();
val += _ser.getc() << 8;
raw_ls_array [i] = val;
}
}
// Returns the uptime of the system (since initialisation) in seconds
float PiSwarm::get_uptime (void)
{
return _system_timer.read();
}
// Returns the battery level in millivolts
float PiSwarm::battery()
{
_ser.putc(SEND_BATTERY_MILLIVOLTS);
char lowbyte = _ser.getc();
char hibyte = _ser.getc();
float v = ((lowbyte + (hibyte << 8))/1000.0);
return(v);
}
// Returns the raw value for the variable resistor on the base of the platform. Ranges from 0 to 1024.
float PiSwarm::pot_voltage(void)
{
int volt = 0;
_ser.putc(SEND_TRIMPOT);
volt = _ser.getc();
volt += _ser.getc() << 8;
return(volt);
}
// Returns the ID of platform (set by the DIL switches above the display). Range 0-31 [0 reserved].
char PiSwarm::get_id ()
{
return id;
}
// Return the current stored state for direction switch(es) pressed {1 = Center 2 = Right 4 = Left 8 = Down 16 = Up}
char PiSwarm::get_switches ()
{
return switches;
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Motor Functions
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Returns the target speed of the left motor (-1.0 to 1.0)
float PiSwarm::get_left_motor ()
{
return left_speed;
}
// Returns the target speed of the right motor (-1.0 to 1.0)
float PiSwarm::get_right_motor ()
{
return right_speed;
}
// Sets the speed of the left motor (-1.0 to 1.0)
void PiSwarm::left_motor (float speed)
{
motor(0,speed);
}
// Sets the speed of the right motor (-1.0 to 1.0)
void PiSwarm::right_motor (float speed)
{
motor(1,speed);
}
// Drive forward at the given speed (-1.0 to 1.0)
void PiSwarm::forward (float speed)
{
motor(0,speed);
motor(1,speed);
}
// Drive backward at the given speed (-1.0 to 1.0)
void PiSwarm::backward (float speed)
{
motor(0,-1.0*speed);
motor(1,-1.0*speed);
}
// Turn on-the-spot left at the given speed (-1.0 to 1.0)
void PiSwarm::left (float speed)
{
motor(0,speed);
motor(1,-1.0*speed);
}
// Turn on-the-spot right at the given speed (-1.0 to 1.0)
void PiSwarm::right (float speed)
{
motor(0,-1.0*speed);
motor(1,speed);
}
// Stop both motors
void PiSwarm::stop (void)
{
motor(0,0.0);
motor(1,0.0);
}
// 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)
{
_ser.putc(DO_PLAY);
_ser.putc(length);
for (int i = 0 ; i < length ; i++) {
_ser.putc(tune[i]);
}
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Display Functions
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void PiSwarm::locate(int x, int y)
{
_ser.putc(DO_LCD_GOTO_XY);
_ser.putc(x);
_ser.putc(y);
}
void PiSwarm::cls(void)
{
_ser.putc(DO_CLEAR);
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// EEPROM Functions
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void PiSwarm::write_eeprom_byte ( int address, char data )
{
char write_array[3];
write_array[0] = address / 256;
write_array[1] = address % 256;
write_array[2] = data;
_i2c.write(EEPROM_ADDRESS, write_array, 3, false);
//Takes 5ms to write a page: ideally this could be done with a timer or RTOS
wait(0.005);
}
void PiSwarm::write_eeprom_block ( int address, char length, char * data)
{
/** Note from datasheet:
Page write operations are limited to writing bytes within a single physical page,
regardless of the number of bytes actually being written. Physical page
boundaries start at addresses that are integer multiples of the page buffer size (or
‘page size’) and end at addresses that are integer multiples of [page size – 1]. If a
Page Write command attempts to write across a physical page boundary, the
result is that the data wraps around to the beginning of the current page (overwriting
data previously stored there), instead of being written to the next page, as might be
expected. It is therefore necessary for the application software to prevent page write
operations that would attempt to cross a page boundary. */
//First check to see if first block starts at the start of a page
int subpage = address % 32;
//If subpage > 0 first page does not start at a page boundary
int write_length = 32 - subpage;
if( write_length > length ) write_length = length;
char firstpage_array [write_length+2];
firstpage_array[0] = address / 256;
firstpage_array[1] = address % 256;
for(int i=0; i<write_length; i++) {
firstpage_array[i+2] = data [i];
}
_i2c.write(EEPROM_ADDRESS, firstpage_array, write_length + 2, false);
wait(0.005);
if(length > write_length) {
int page_count = (length + subpage) / 32;
int written = write_length;
for(int p=0; p<page_count; p++) {
int to_write = length - written;
if (to_write > 32) {
to_write = 32;
}
char page_array [to_write + 2];
page_array[0] = (address + written) / 256;
page_array[1] = (address + written) % 256;
for(int i=0; i<to_write; i++) {
page_array[i+2] = data[written + i];
}
_i2c.write(EEPROM_ADDRESS, page_array, to_write + 2, false);
wait(0.005);
written += 32;
}
}
}
char PiSwarm::read_eeprom_byte ( int address )
{
char address_array [2];
address_array[0] = address / 256;
address_array[1] = address % 256;
char data [1];
_i2c.write(EEPROM_ADDRESS, address_array, 2, false);
_i2c.read(EEPROM_ADDRESS, data, 1, false);
return data [0];
}
char PiSwarm::read_next_eeprom_byte ()
{
char data [1];
_i2c.read(EEPROM_ADDRESS, data, 1, false);
return data [0];
}
char PiSwarm::read_eeprom_block ( int address, char length )
{
char ret = 0;
return ret;
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// RF Transceiver Functions
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void PiSwarm::send_rf_message(char* message, char length)
{
_rf.sendString(length, message);
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// 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();
beacon_led = setup_adc();
}
void PiSwarm::setup_radio()
{
//Setup RF transceiver
_rf.rf_init();
_rf.setFrequency(RF_FREQUENCY);
_rf.setDatarate(RF_DATARATE);
}
int PiSwarm::setup_expansion_ic ()
{
//Expansion IC is NXP PCA9505
//Address is 0x40 (defined by EXPANSION_IC_ADDRESS)
//Block IO 0 : 7-0 Are OLED Outputs
//Block IO 1 : 7 is NC. 6-2 are Cursor switches (interrupt inputs). 1 and 0 are OLED outputs.
//Block IO 2 : 7 and 6 are LDO enable outputs. 5 - 1 are ID switch. 0 is NC.
//Block IO 3 and 4 are for the expansion connector (not assigned here)
char data [4];
data [0] = 0x98; //Write to I/O configure register 0, using auto increment (allows multi-blocks in one command)
data [1] = 0x00; //All 8 pins in block 0 are outputs (0)
data [2] = 0xFC; //Pins 1 & 0 are outputs, rest are inputs (or NC)
data [3] = 0x3F; //Pins 7 & 6 are outputs, rest are inputs (or NC)
//If using the expansion port, assing its I/O here by increasing size of data[] and adding extra commands
int test_val = _i2c.write(EXPANSION_IC_ADDRESS,data,4,false);
// test_val should return 0 for correctly acknowledged response
//Invert the polarity for switch inputs (they connect to ground when pressed\on)
data [0] = 0x90; //Write to polarity inversion register using auto increment
data [1] = 0x00;
data [2] = 0x7C; //Invert pins 6-2 for cursor switches
data [3] = 0x3E; //Invert pins 5-1 for ID switch
_i2c.write(EXPANSION_IC_ADDRESS,data,4,false);
//Setup the interrupt masks. By default, only the direction switches trigger an interrupt
data [0] = 0xA0;
data [1] = 0xFF;
data [2] = 0x83; // Disable mask on pins 6-2 for cursor switches
data [3] = 0xFF;
_i2c.write(EXPANSION_IC_ADDRESS,data,4,false);
//Read the ID switches
data [0] = 0x80; //Read input registers
char read_data [5];
_i2c.write(EXPANSION_IC_ADDRESS, data, 1, false);
_i2c.read(EXPANSION_IC_ADDRESS, read_data, 5, false);
id = (read_data[2] & 0x3E) >> 1; //Mask pins 0, 6 and 7 then shift
//Setup interrupt handler
expansion_interrupt.mode(PullUp); // Pull Up by default (needed on v1 PCB - no pullup resistor)
expansion_interrupt.fall(this,&PiSwarm::expansion_interrupt_handler); // Add local handler on falling edge to read switch
return test_val;
}
char PiSwarm::setup_adc ( void )
{
//ADC IC (for IR system) is a Analog Devices AD7997-BRUZ1
return 0;
}
char PiSwarm::calibrate_gyro ( void )
{
//This routine attempts to calibrate the offset of the gyro
int samples = 8;
float gyro_values[samples];
calibrate_led = 1;
calibrate_ticker.attach(this,&PiSwarm::calibrate_ticker_routine,0.25);
for(int i=0; i<samples; i++) {
gyro_values[i] = _gyro.read();
wait(0.12);
}
char pass = 1;
float mean = 0;
float min = 3.5;
float max = 3.2;
for(int i=0; i<samples; i++) {
if((gyro_values[i] * 3.5 < 1.5) || (gyro_values[i] * 3.2 > 1.5)) pass = 0; // Calibration failed!
float test_value = 1.50 / gyro_values[i];
mean += test_value;
if (test_value > max) max = test_value;
if (test_value < min) min = test_value;
}
if(pass == 1) {
gyro_cal = mean / samples;
gyro_error = max - min;
calibrate_ticker.detach();
calibrate_led = 0;
}
return pass;
}
char PiSwarm::calibrate_accelerometer ( void )
{
//This routine attempts to calibrate the offset and multiplier of the accelerometer
int samples = 8;
float acc_x_values[samples];
float acc_y_values[samples];
float acc_z_values[samples];
calibrate_led = 1;
calibrate_ticker.attach(this,&PiSwarm::calibrate_ticker_routine,0.25);
for(int i=0; i<samples; i++) {
acc_x_values[i] = _accel_x.read();
acc_y_values[i] = _accel_y.read();
acc_z_values[i] = _accel_z.read();
wait(0.12);
}
char pass = 1;
float x_mean = 0, y_mean=0, z_mean=0;
float x_min = 3600, y_min=3600, z_min=3600;
float x_max = 3100, y_max=3100, z_max=3100;
for(int i=0; i<samples; i++) {
if((acc_x_values[i] * 3600 < 1250) || (acc_x_values[i] * 3100 > 1250)) pass = 0; // Calibration failed!
if((acc_y_values[i] * 3600 < 1250) || (acc_y_values[i] * 3100 > 1250)) pass = 0; // Calibration failed!
if((acc_z_values[i] * 3600 < 887) || (acc_z_values[i] * 3100 > 887)) pass = 0; // Calibration failed!
float x_test_value = 1250 / acc_x_values[i];
x_mean += x_test_value;
if (x_test_value > x_max) x_max = x_test_value;
if (x_test_value < x_min) x_min = x_test_value;
float y_test_value = 1250 / acc_y_values[i];
y_mean += y_test_value;
if (y_test_value > y_max) y_max = y_test_value;
if (y_test_value < y_min) y_min = y_test_value;
float z_test_value = 887 / acc_z_values[i];
z_mean += z_test_value;
if (z_test_value > z_max) z_max = z_test_value;
if (z_test_value < z_min) z_min = z_test_value;
}
if(pass == 1) {
acc_x_cal = x_mean / samples;
acc_y_cal = y_mean / samples;
acc_z_cal = z_mean / samples;
calibrate_ticker.detach();
calibrate_led = 0;
}
return pass;
}
char PiSwarm::calibrate_magnetometer ( void )
{
char command_data[2];
command_data[0] = 0x11; //Write to CTRL_REG2
command_data[1] = 0x80; // Enable auto resets
_i2c.write(MAGNETOMETER_ADDRESS, command_data, 2, false);
command_data[0] = 0x10; //Write to CTRL_REG1
command_data[1] = 0x29; // 001 01 0 0 1 [40HZ 32-OS NOT-FAST NORMAL ACTIVE MODE]
return _i2c.write(MAGNETOMETER_ADDRESS, command_data, 2, false);
}
void PiSwarm::interrupt_timeout_event ( void )
{
//Read switches
char data [1];
data [0] = 0x80; //Read input registers
char read_data [5];
_i2c.write(EXPANSION_IC_ADDRESS, data, 1, false);
_i2c.read(EXPANSION_IC_ADDRESS, read_data, 5, false);
switches = (read_data[1] & 0x7C) >> 2;
led_timeout.attach(this,&PiSwarm::led_timeout_event, 0.1); // Switch off the LED after 0.1s
}
void PiSwarm::led_timeout_event ( void )
{
interrupt_led = 0;
}
void PiSwarm::expansion_interrupt_handler ( void )
{
interrupt_led = 1;
debounce_timeout.attach_us(this,&PiSwarm::interrupt_timeout_event, 200); // Read the switches only after 200uS have passed to debounce
debounce_timeout.add_function(&switch_pressed); // Call the switch pressed routine (in main) when done
}
void PiSwarm::calibrate_ticker_routine ( void )
{
calibrate_led_state = 1 - calibrate_led_state;
calibrate_led = calibrate_led_state;
}
void PiSwarm::test_oled()
{
enable_led_ldo = 1;
enable_ldo_outputs();
set_oled_colour(100,100,100);
char data[2];
data[0] = 0x09; //Address for PCA9505 Output Port 1
data[1] = 3;
_i2c.write(EXPANSION_IC_ADDRESS,data,2,false);
}
void PiSwarm::reset ()
{
// _nrst = 0;
wait (0.01);
// _nrst = 1;
wait (0.1);
}
void PiSwarm::motor (int motor, float speed)
{
char opcode = 0x0;
if (speed > 0.0) {
if (motor==1)
opcode = M1_FORWARD;
else
opcode = M2_FORWARD;
} else {
if (motor==1)
opcode = M1_BACKWARD;
else
opcode = M2_BACKWARD;
}
if(motor==1)right_speed = speed;
else left_speed = speed;
unsigned char arg = 0x7f * abs(speed);
_ser.putc(opcode);
_ser.putc(arg);
}
float PiSwarm::line_position()
{
int pos = 0;
_ser.putc(SEND_LINE_POSITION);
pos = _ser.getc();
pos += _ser.getc() << 8;
float fpos = ((float)pos - 2048.0)/2048.0;
return(fpos);
}
char PiSwarm::sensor_auto_calibrate()
{
_ser.putc(AUTO_CALIBRATE);
return(_ser.getc());
}
void PiSwarm::calibrate(void)
{
_ser.putc(PI_CALIBRATE);
}
void PiSwarm::reset_calibration()
{
_ser.putc(LINE_SENSORS_RESET_CALIBRATION);
}
void PiSwarm::PID_start(int max_speed, int a, int b, int c, int d)
{
_ser.putc(max_speed);
_ser.putc(a);
_ser.putc(b);
_ser.putc(c);
_ser.putc(d);
}
void PiSwarm::PID_stop()
{
_ser.putc(STOP_PID);
}
int PiSwarm::print (char* text, int length)
{
_ser.putc(DO_PRINT);
_ser.putc(length);
for (int i = 0 ; i < length ; i++) {
_ser.putc(text[i]);
}
return(0);
}
int PiSwarm::_putc (int c)
{
_ser.putc(DO_PRINT);
_ser.putc(0x1);
_ser.putc(c);
wait (0.001);
return(c);
}
int PiSwarm::_getc (void)
{
char r = 0;
return(r);
}
int PiSwarm::putc (int c)
{
return(_ser.putc(c));
}
int PiSwarm::getc (void)
{
return(_ser.getc());
}
void PiSwarm::start_system_timer(void)
{
_system_timer.reset();
_system_timer.start();
}
#ifdef MBED_RPC
const rpc_method *PiSwarm::get_rpc_methods()
{
static const rpc_method rpc_methods[] = {{ "forward", rpc_method_caller<PiSwarm, float, &PiSwarm::forward> },
{ "backward", rpc_method_caller<PiSwarm, float, &PiSwarm::backward> },
{ "left", rpc_method_caller<PiSwarm, float, &PiSwarm::left> },
{ "right", rpc_method_caller<PiSwarm, float, &PiSwarm::right> },
{ "stop", rpc_method_caller<PiSwarm, &PiSwarm::stop> },
{ "left_motor", rpc_method_caller<PiSwarm, float, &PiSwarm::left_motor> },
{ "right_motor", rpc_method_caller<PiSwarm, float, &PiSwarm::right_motor> },
{ "battery", rpc_method_caller<float, PiSwarm, &PiSwarm::battery> },
{ "line_position", rpc_method_caller<float, PiSwarm, &PiSwarm::line_position> },
{ "sensor_auto_calibrate", rpc_method_caller<char, PiSwarm, &PiSwarm::sensor_auto_calibrate> },
RPC_METHOD_SUPER(Base)
};
return rpc_methods;
}
#endif
void display_system_time()
{
if(PISWARM_DEBUG == 1) {
time_t system_time = time(NULL);
printf("Current system time:%s Uptime:%.1f\n",ctime(&system_time),piswarm.get_uptime());
}
}
void 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;
}
}
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.7 Initialisation...\n");
display_system_time();
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.printf(" YORK ");
piswarm.locate(0,1);
piswarm.printf("ROBOTLAB");
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.cls();
piswarm.printf("Pi Swarm");
piswarm.locate(0,1);
piswarm.printf("ID : %d ",piswarm.get_id());
wait(0.2);
piswarm.set_cled_colour(0,0,200);
piswarm.set_oled_colour(0,0,200);
if(CALIBRATE_MEMS == 1) {
piswarm.cls();
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.printf("LOCATING");
piswarm.locate(0,1);
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();
piswarm.cls();
piswarm.set_cled_colour(0,0,0);
if(START_RADIO_ON_BOOT == 1) {
if(PISWARM_DEBUG == 1)pc.printf("Setting up 433MHz Transceiver\n");
piswarm.setup_radio();
}
}
/********************************************************************************
* COPYRIGHT NOTICE *
* *
* Parts of code based on the original m3pi library, Copyright (c) 2010 cstyles *
* *
* Permission is hereby granted, free of charge, to any person obtaining a copy *
* of this software and associated documentation files (the "Software"), to deal*
* in the Software without restriction, including without limitation the rights *
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell *
* copies of the Software, and to permit persons to whom the Software is *
* furnished to do so, subject to the following conditions: *
* *
* The above copyright notice and this permission notice shall be included in *
* all copies or substantial portions of the Software. *
* *
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR *
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, *
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE *
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER *
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,*
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN *
* THE SOFTWARE. *
* *
********************************************************************************/