Pi_Swarm_Library_7 - Unfinished v0.7, added bearing detection

Users » jah128 » Code » Pi_Swarm_Library_7

Unfinished v0.7, added bearing detection

Fork of Pi_Swarm_Library_v06_alpha by piswarm

piswarm.cpp@11:5ebcb52726cf, 2014-06-30 (annotated)

Committer:: jah128
Date:: Mon Jun 30 07:58:31 2014 +0000
Revision:: 11:5ebcb52726cf
Parent:: 9:7a4fc1d7e484

Added prototype bearing detection to pi swarm library;

Who changed what in which revision?

User	Revision	Line number	New contents of line
jah128	4:52b3e4c5a425	1	/*******************************************************************************************
jah128	4:52b3e4c5a425	2	*
jah128	4:52b3e4c5a425	3	* University of York Robot Lab Pi Swarm Robot Library
jah128	0:9ffe8ebd1c40	4	*
jah128	0:9ffe8ebd1c40	5	* (C) Dr James Hilder, Dept. Electronics & Computer Science, University of York
jah128	11:5ebcb52726cf	6	*
jah128	9:7a4fc1d7e484	7	* Version 0.6 February 2014
jah128	9:7a4fc1d7e484	8	*
jah128	9:7a4fc1d7e484	9	* Change notes:
jah128	11:5ebcb52726cf	10	* 0.7 : Added code for IR table beacon syncing and turning
jah128	9:7a4fc1d7e484	11	* 0.6 : Added new IR sensor functions to improve efficiency (see manual for details)
jah128	9:7a4fc1d7e484	12	* 0.5 : Initial release
jah128	0:9ffe8ebd1c40	13	*
jah128	0:9ffe8ebd1c40	14	* Designed for use with the Pi Swarm Board (enhanced MBED sensor board) v1.2
jah128	0:9ffe8ebd1c40	15	*
jah128	4:52b3e4c5a425	16	******************************************************************************************/
jah128	0:9ffe8ebd1c40	17
jah128	11:5ebcb52726cf	18	#include "piswarm.h"
jah128	0:9ffe8ebd1c40	19
jah128	0:9ffe8ebd1c40	20	// Variables
jah128	0:9ffe8ebd1c40	21	DigitalOut gpio_led (LED1);
jah128	0:9ffe8ebd1c40	22	DigitalOut calibrate_led (LED2);
jah128	11:5ebcb52726cf	23	DigitalOut beacon_led (LED3);
jah128	0:9ffe8ebd1c40	24	DigitalOut interrupt_led (LED4);
jah128	11:5ebcb52726cf	25	InterruptIn expansion_interrupt (p29);
jah128	0:9ffe8ebd1c40	26	Timeout debounce_timeout;
jah128	0:9ffe8ebd1c40	27	Timeout led_timeout;
jah128	11:5ebcb52726cf	28	Timeout motor_timeout;
jah128	0:9ffe8ebd1c40	29	Ticker calibrate_ticker;
jah128	11:5ebcb52726cf	30	Ticker oled_ticker;
jah128	11:5ebcb52726cf	31	Ticker beacon_ticker;
jah128	11:5ebcb52726cf	32
jah128	0:9ffe8ebd1c40	33	char id;
jah128	0:9ffe8ebd1c40	34
jah128	11:5ebcb52726cf	35	float bearing = 0;
jah128	11:5ebcb52726cf	36	char is_synced = 0;
jah128	11:5ebcb52726cf	37	float beacon_confidence = 0;
jah128	11:5ebcb52726cf	38	float bearing_confidence = 0;
jah128	11:5ebcb52726cf	39	char saved_peak = 0;
jah128	11:5ebcb52726cf	40	char miss_sync_count = 0;
jah128	11:5ebcb52726cf	41	char hit_sync_count = 0;
jah128	0:9ffe8ebd1c40	42	char oled_array [10];
jah128	11:5ebcb52726cf	43	char saved_oled_array [10];
jah128	11:5ebcb52726cf	44	char saved_oled_red;
jah128	11:5ebcb52726cf	45	char saved_oled_green;
jah128	11:5ebcb52726cf	46	char saved_oled_blue;
jah128	0:9ffe8ebd1c40	47	char enable_ir_ldo = 0;
jah128	0:9ffe8ebd1c40	48	char enable_led_ldo = 0;
jah128	0:9ffe8ebd1c40	49	char calibrate_led_state = 1;
jah128	0:9ffe8ebd1c40	50	char switches = 0;
jah128	0:9ffe8ebd1c40	51	char cled_red = 0;
jah128	0:9ffe8ebd1c40	52	char cled_green = 0;
jah128	0:9ffe8ebd1c40	53	char cled_blue = 0;
jah128	0:9ffe8ebd1c40	54	char oled_red = 0;
jah128	0:9ffe8ebd1c40	55	char oled_green = 0;
jah128	0:9ffe8ebd1c40	56	char oled_blue = 0;
jah128	0:9ffe8ebd1c40	57	char cled_enable = 0;
jah128	11:5ebcb52726cf	58	char oled_ticker_step = 0;
jah128	11:5ebcb52726cf	59	char bearing_strobe_step = 0;
jah128	0:9ffe8ebd1c40	60	char cled_brightness = CENTER_LED_BRIGHTNESS; // The default brightness value: must be between 0 (dimmest) and 100 (full brightness)
jah128	0:9ffe8ebd1c40	61	char oled_brightness = OUTER_LED_BRIGHTNESS; // The default brightness value: must be between 0 (dimmest) and 100 (full brightness)
jah128	0:9ffe8ebd1c40	62	float gyro_cal = 3.3;
jah128	0:9ffe8ebd1c40	63	float gyro_error = 0;
jah128	0:9ffe8ebd1c40	64	float acc_x_cal = 3350;
jah128	0:9ffe8ebd1c40	65	float acc_y_cal = 3350;
jah128	0:9ffe8ebd1c40	66	float acc_z_cal = 3350;
jah128	0:9ffe8ebd1c40	67	float left_speed = 0;
jah128	0:9ffe8ebd1c40	68	float right_speed = 0;
jah128	0:9ffe8ebd1c40	69	signed short mag_values [3];
jah128	9:7a4fc1d7e484	70	unsigned short background_ir_values [8];
jah128	9:7a4fc1d7e484	71	unsigned short illuminated_ir_values [8];
jah128	9:7a4fc1d7e484	72	float reflected_ir_distances [8];
jah128	9:7a4fc1d7e484	73	char ir_values_stored = 0;
jah128	0:9ffe8ebd1c40	74
jah128	0:9ffe8ebd1c40	75
jah128	11:5ebcb52726cf	76	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)
jah128	11:5ebcb52726cf	77	{
jah128	0:9ffe8ebd1c40	78	setup();
jah128	0:9ffe8ebd1c40	79	reset();
jah128	0:9ffe8ebd1c40	80	}
jah128	0:9ffe8ebd1c40	81
jah128	1:b067a08ff54e	82	//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
jah128	1:b067a08ff54e	83	// Outer LED Functions
jah128	1:b067a08ff54e	84	//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
jah128	1:b067a08ff54e	85
jah128	11:5ebcb52726cf	86	// 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.
jah128	11:5ebcb52726cf	87	int PiSwarm::get_oled_colour()
jah128	11:5ebcb52726cf	88	{
jah128	0:9ffe8ebd1c40	89	return (oled_red << 16) + (oled_green << 8) + oled_blue;
jah128	0:9ffe8ebd1c40	90	}
jah128	0:9ffe8ebd1c40	91
jah128	1:b067a08ff54e	92	// Returns the current enable state an individual LED. oled = LED to return enable state. Returns 0 for disabled, 1 for enabled
jah128	11:5ebcb52726cf	93	char PiSwarm::get_oled_state(char oled)
jah128	11:5ebcb52726cf	94	{
jah128	1:b067a08ff54e	95	if(oled_array[oled] == 1) return 1;
jah128	1:b067a08ff54e	96	return 0;
jah128	0:9ffe8ebd1c40	97	}
jah128	0:9ffe8ebd1c40	98
jah128	1:b067a08ff54e	99	// Set the colour of the outer LED. Values for red, green and blue range from 0 (off) to 255 (maximum).
jah128	11:5ebcb52726cf	100	void PiSwarm::set_oled_colour( char red, char green, char blue )
jah128	11:5ebcb52726cf	101	{
jah128	1:b067a08ff54e	102	oled_red = red;
jah128	1:b067a08ff54e	103	oled_green = green;
jah128	1:b067a08ff54e	104	oled_blue = blue;
jah128	1:b067a08ff54e	105	_oled_r.pulsewidth_us(red);
jah128	1:b067a08ff54e	106	_oled_g.pulsewidth_us(green);
jah128	1:b067a08ff54e	107	_oled_b.pulsewidth_us(blue);
jah128	1:b067a08ff54e	108	}
jah128	1:b067a08ff54e	109
jah128	1:b067a08ff54e	110	// 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.
jah128	11:5ebcb52726cf	111	void PiSwarm::set_oled(char oled, char value)
jah128	11:5ebcb52726cf	112	{
jah128	0:9ffe8ebd1c40	113	oled_array[oled]=value;
jah128	0:9ffe8ebd1c40	114	}
jah128	0:9ffe8ebd1c40	115
jah128	11:5ebcb52726cf	116	// Save the current state and colour of OLEDs
jah128	11:5ebcb52726cf	117	void PiSwarm::save_oled_state()
jah128	11:5ebcb52726cf	118	{
jah128	11:5ebcb52726cf	119	for(int i=0; i<10; i++) {
jah128	11:5ebcb52726cf	120	saved_oled_array[i]=oled_array[i];
jah128	11:5ebcb52726cf	121	}
jah128	11:5ebcb52726cf	122	saved_oled_red=oled_red;
jah128	11:5ebcb52726cf	123	saved_oled_green=oled_green;
jah128	11:5ebcb52726cf	124	saved_oled_blue=oled_blue;
jah128	11:5ebcb52726cf	125	}
jah128	11:5ebcb52726cf	126
jah128	11:5ebcb52726cf	127	// Restore the saved colour and state of OLEDs
jah128	11:5ebcb52726cf	128	void PiSwarm::restore_oled_state()
jah128	11:5ebcb52726cf	129	{
jah128	11:5ebcb52726cf	130	for(int i=0; i<10; i++) {
jah128	11:5ebcb52726cf	131	oled_array[i]=saved_oled_array[i];
jah128	11:5ebcb52726cf	132	}
jah128	11:5ebcb52726cf	133	set_oled_colour(saved_oled_red,saved_oled_green,saved_oled_blue);
jah128	11:5ebcb52726cf	134	activate_oleds();
jah128	11:5ebcb52726cf	135	}
jah128	11:5ebcb52726cf	136
jah128	1:b067a08ff54e	137	// Sets the state of all 10 LEDs. oled_x = 0 for disable, 1 for enable. Use to change all 10 LEDs at once
jah128	11:5ebcb52726cf	138	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)
jah128	11:5ebcb52726cf	139	{
jah128	0:9ffe8ebd1c40	140	oled_array[0]=oled_0;
jah128	0:9ffe8ebd1c40	141	oled_array[1]=oled_1;
jah128	0:9ffe8ebd1c40	142	oled_array[2]=oled_2;
jah128	0:9ffe8ebd1c40	143	oled_array[3]=oled_3;
jah128	0:9ffe8ebd1c40	144	oled_array[4]=oled_4;
jah128	0:9ffe8ebd1c40	145	oled_array[5]=oled_5;
jah128	0:9ffe8ebd1c40	146	oled_array[6]=oled_6;
jah128	0:9ffe8ebd1c40	147	oled_array[7]=oled_7;
jah128	0:9ffe8ebd1c40	148	oled_array[8]=oled_8;
jah128	0:9ffe8ebd1c40	149	oled_array[9]=oled_9;
jah128	0:9ffe8ebd1c40	150	activate_oleds();
jah128	0:9ffe8ebd1c40	151	}
jah128	0:9ffe8ebd1c40	152
jah128	1:b067a08ff54e	153	// Sets all outer LEDs to disabled and turns off LED power supply.
jah128	11:5ebcb52726cf	154	void PiSwarm::turn_off_all_oleds()
jah128	11:5ebcb52726cf	155	{
jah128	11:5ebcb52726cf	156	for(int i=0; i<10; i++) {
jah128	0:9ffe8ebd1c40	157	oled_array[i]=0;
jah128	0:9ffe8ebd1c40	158	}
jah128	0:9ffe8ebd1c40	159	activate_oleds();
jah128	0:9ffe8ebd1c40	160	enable_led_ldo = 0;
jah128	0:9ffe8ebd1c40	161	enable_ldo_outputs();
jah128	0:9ffe8ebd1c40	162	char data[3];
jah128	0:9ffe8ebd1c40	163	data[0]=0x88; //Write to bank 1 then bank 2
jah128	0:9ffe8ebd1c40	164	data[1]=0x00; //Reset everything in bank 1
jah128	0:9ffe8ebd1c40	165	data[2]=0x00; //Reset everything in bank 2
jah128	11:5ebcb52726cf	166	_i2c.write(EXPANSION_IC_ADDRESS,data,3,false);
jah128	0:9ffe8ebd1c40	167	}
jah128	0:9ffe8ebd1c40	168
jah128	1:b067a08ff54e	169	// Set the outer LED brightness (total period of PWM output increases as brightness decreases). Ranges from 0 (minimum) to 255 (maximum)
jah128	11:5ebcb52726cf	170	void PiSwarm::set_oled_brightness ( char brightness )
jah128	11:5ebcb52726cf	171	{
jah128	1:b067a08ff54e	172	if ( brightness > 100 ) brightness = 100;
jah128	1:b067a08ff54e	173	oled_brightness = brightness;
jah128	1:b067a08ff54e	174	int oled_period = (104 - brightness);
jah128	1:b067a08ff54e	175	oled_period *= oled_period;
jah128	1:b067a08ff54e	176	oled_period /= 10;
jah128	11:5ebcb52726cf	177	oled_period += 255;
jah128	1:b067a08ff54e	178	if(CALIBRATE_COLOURS!=0)_oled_r.period_us(oled_period * 2);
jah128	1:b067a08ff54e	179	else _oled_r.period_us(oled_period);
jah128	1:b067a08ff54e	180	if(CALIBRATE_COLOURS!=0)_oled_g.period_us(oled_period * 2);
jah128	1:b067a08ff54e	181	else _oled_g.period_us(oled_period);
jah128	1:b067a08ff54e	182	_oled_b.period_us(oled_period);
jah128	1:b067a08ff54e	183	}
jah128	0:9ffe8ebd1c40	184
jah128	0:9ffe8ebd1c40	185	// Sends the messages to enable/disable outer LEDs
jah128	11:5ebcb52726cf	186	void PiSwarm::activate_oleds()
jah128	11:5ebcb52726cf	187	{
jah128	11:5ebcb52726cf	188	if(enable_led_ldo == 0) {
jah128	0:9ffe8ebd1c40	189	enable_led_ldo = 1;
jah128	0:9ffe8ebd1c40	190	enable_ldo_outputs();
jah128	0:9ffe8ebd1c40	191	}
jah128	0:9ffe8ebd1c40	192	char data[3];
jah128	0:9ffe8ebd1c40	193	data[0]=0x88; //Write to bank 1 then bank 2
jah128	0:9ffe8ebd1c40	194	data[1]=0x00; //Reset everything in bank 1
jah128	0:9ffe8ebd1c40	195	data[2]=0x00; //Reset everything in bank 2
jah128	0:9ffe8ebd1c40	196	if(oled_array[0]>0) data[1]+=1;
jah128	0:9ffe8ebd1c40	197	if(oled_array[1]>0) data[1]+=2;
jah128	0:9ffe8ebd1c40	198	if(oled_array[2]>0) data[1]+=4;
jah128	0:9ffe8ebd1c40	199	if(oled_array[3]>0) data[1]+=8;
jah128	0:9ffe8ebd1c40	200	if(oled_array[4]>0) data[1]+=16;
jah128	0:9ffe8ebd1c40	201	if(oled_array[5]>0) data[1]+=32;
jah128	0:9ffe8ebd1c40	202	if(oled_array[6]>0) data[1]+=64;
jah128	0:9ffe8ebd1c40	203	if(oled_array[7]>0) data[1]+=128;
jah128	0:9ffe8ebd1c40	204	if(oled_array[8]>0) data[2]+=1;
jah128	0:9ffe8ebd1c40	205	if(oled_array[9]>0) data[2]+=2;
jah128	11:5ebcb52726cf	206	_i2c.write(EXPANSION_IC_ADDRESS,data,3,false);
jah128	0:9ffe8ebd1c40	207	}
jah128	0:9ffe8ebd1c40	208
jah128	1:b067a08ff54e	209	//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
jah128	1:b067a08ff54e	210	// Center LED Functions
jah128	1:b067a08ff54e	211	//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
jah128	1:b067a08ff54e	212
jah128	1:b067a08ff54e	213	// 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.
jah128	11:5ebcb52726cf	214	int PiSwarm::get_cled_colour()
jah128	11:5ebcb52726cf	215	{
jah128	1:b067a08ff54e	216	return (cled_red << 16) + (cled_green << 8) + cled_blue;
jah128	0:9ffe8ebd1c40	217	}
jah128	0:9ffe8ebd1c40	218
jah128	11:5ebcb52726cf	219	// Returns the enable state of the center LED
jah128	11:5ebcb52726cf	220	char PiSwarm::get_cled_state( void )
jah128	11:5ebcb52726cf	221	{
jah128	4:52b3e4c5a425	222	return cled_enable;
jah128	4:52b3e4c5a425	223	}
jah128	4:52b3e4c5a425	224
jah128	1:b067a08ff54e	225	// Set the colour of the center LED. Values for red, green and blue range from 0 (off) to 255 (maximum).
jah128	11:5ebcb52726cf	226	void PiSwarm::set_cled_colour( char red, char green, char blue )
jah128	11:5ebcb52726cf	227	{
jah128	0:9ffe8ebd1c40	228	cled_red = red;
jah128	0:9ffe8ebd1c40	229	cled_green = green;
jah128	0:9ffe8ebd1c40	230	cled_blue = blue;
jah128	0:9ffe8ebd1c40	231	if(cled_enable != 0) enable_cled(1);
jah128	0:9ffe8ebd1c40	232	}
jah128	0:9ffe8ebd1c40	233
jah128	1:b067a08ff54e	234	// Turn on or off the center LED. enable=1 to turn on, 0 to turn off
jah128	11:5ebcb52726cf	235	void PiSwarm::enable_cled( char enable )
jah128	11:5ebcb52726cf	236	{
jah128	0:9ffe8ebd1c40	237	cled_enable = enable;
jah128	0:9ffe8ebd1c40	238	if(enable != 0) {
jah128	0:9ffe8ebd1c40	239	_cled_r.pulsewidth_us(cled_red);
jah128	0:9ffe8ebd1c40	240	_cled_g.pulsewidth_us(cled_green);
jah128	0:9ffe8ebd1c40	241	_cled_b.pulsewidth_us(cled_blue);
jah128	11:5ebcb52726cf	242	} else {
jah128	0:9ffe8ebd1c40	243	_cled_r.pulsewidth_us(0);
jah128	0:9ffe8ebd1c40	244	_cled_g.pulsewidth_us(0);
jah128	0:9ffe8ebd1c40	245	_cled_b.pulsewidth_us(0);
jah128	0:9ffe8ebd1c40	246	}
jah128	0:9ffe8ebd1c40	247	}
jah128	0:9ffe8ebd1c40	248
jah128	1:b067a08ff54e	249	// Set the center LED brightness (total period of PWM output increases as brightness decreases). Ranges from 0 (minimum) to 255 (maximum)
jah128	11:5ebcb52726cf	250	void PiSwarm::set_cled_brightness ( char brightness )
jah128	11:5ebcb52726cf	251	{
jah128	1:b067a08ff54e	252	if( brightness > 100 ) brightness = 100;
jah128	4:52b3e4c5a425	253	// Brightness is set by adjusting period of PWM. Period ranged from ~260uS at 100% to 1336uS at 0%
jah128	1:b067a08ff54e	254	// When calibrate_colours = 1, red = 2x normal, green = 2x normal
jah128	1:b067a08ff54e	255	cled_brightness = brightness;
jah128	1:b067a08ff54e	256	int cled_period = (104 - brightness);
jah128	1:b067a08ff54e	257	cled_period *= cled_period;
jah128	1:b067a08ff54e	258	cled_period /= 10;
jah128	1:b067a08ff54e	259	cled_period += 255;
jah128	11:5ebcb52726cf	260	if(CALIBRATE_COLOURS!=0)_cled_r.period_us(cled_period * 2);
jah128	1:b067a08ff54e	261	else _cled_r.period_us(cled_period);
jah128	11:5ebcb52726cf	262	if(CALIBRATE_COLOURS!=0)_cled_g.period_us(cled_period * 2);
jah128	1:b067a08ff54e	263	else _cled_g.period_us(cled_period);
jah128	1:b067a08ff54e	264	_cled_b.period_us(cled_period);
jah128	0:9ffe8ebd1c40	265	}
jah128	0:9ffe8ebd1c40	266
jah128	1:b067a08ff54e	267	//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
jah128	1:b067a08ff54e	268	// IR Sensor Functions
jah128	1:b067a08ff54e	269	//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
jah128	0:9ffe8ebd1c40	270
jah128	11:5ebcb52726cf	271	// Estimates the distance to an obstacle from one of the IR sensors, defined by index (range 0-7).
jah128	9:7a4fc1d7e484	272	// The value is converted to an approximate distance in millimetres, or 100.0 if no obstacle found.
jah128	9:7a4fc1d7e484	273	// NB This function is preserved for code-compatability and cases where only a single reading is needed
jah128	9:7a4fc1d7e484	274	// In many cases it is preferable to call store_reflected_ir_distances() to save all 8 values then read using get_reflected_ir_distance()
jah128	11:5ebcb52726cf	275	float PiSwarm::read_reflected_ir_distance ( char index )
jah128	11:5ebcb52726cf	276	{
jah128	9:7a4fc1d7e484	277	// Sanity check
jah128	9:7a4fc1d7e484	278	if(index>7) return 0.0;
jah128	11:5ebcb52726cf	279
jah128	0:9ffe8ebd1c40	280	//1. Check that the IR LDO regulator is on, enable if it isn't
jah128	11:5ebcb52726cf	281	if(enable_ir_ldo == 0) {
jah128	0:9ffe8ebd1c40	282	enable_ir_ldo = 1;
jah128	0:9ffe8ebd1c40	283	enable_ldo_outputs();
jah128	0:9ffe8ebd1c40	284	}
jah128	9:7a4fc1d7e484	285	//2. Read the ADC value without IR emitter on; store in the background_ir_values[] array
jah128	9:7a4fc1d7e484	286	background_ir_values [index] = read_adc_value ( index );
jah128	11:5ebcb52726cf	287
jah128	0:9ffe8ebd1c40	288	//3. Enable the relevant IR emitter by turning on its pulse output
jah128	11:5ebcb52726cf	289	switch(index) {
jah128	11:5ebcb52726cf	290	case 0:
jah128	11:5ebcb52726cf	291	case 1:
jah128	11:5ebcb52726cf	292	case 6:
jah128	11:5ebcb52726cf	293	case 7:
jah128	11:5ebcb52726cf	294	_irpulse_1 = 1;
jah128	11:5ebcb52726cf	295	break;
jah128	11:5ebcb52726cf	296	case 2:
jah128	11:5ebcb52726cf	297	case 3:
jah128	11:5ebcb52726cf	298	case 4:
jah128	11:5ebcb52726cf	299	case 5:
jah128	11:5ebcb52726cf	300	_irpulse_2 = 1;
jah128	11:5ebcb52726cf	301	break;
jah128	0:9ffe8ebd1c40	302	}
jah128	11:5ebcb52726cf	303	wait_us(500);
jah128	11:5ebcb52726cf	304
jah128	9:7a4fc1d7e484	305	//4. Read the ADC value now IR emitter is on; store in the illuminated_ir_values[] array
jah128	9:7a4fc1d7e484	306	illuminated_ir_values [index] = read_adc_value ( index );
jah128	11:5ebcb52726cf	307
jah128	0:9ffe8ebd1c40	308	//5. Turn off IR emitter
jah128	11:5ebcb52726cf	309	switch(index) {
jah128	11:5ebcb52726cf	310	case 0:
jah128	11:5ebcb52726cf	311	case 1:
jah128	11:5ebcb52726cf	312	case 6:
jah128	11:5ebcb52726cf	313	case 7:
jah128	11:5ebcb52726cf	314	_irpulse_1 = 0;
jah128	11:5ebcb52726cf	315	break;
jah128	11:5ebcb52726cf	316	case 2:
jah128	11:5ebcb52726cf	317	case 3:
jah128	11:5ebcb52726cf	318	case 4:
jah128	11:5ebcb52726cf	319	case 5:
jah128	11:5ebcb52726cf	320	_irpulse_2 = 0;
jah128	11:5ebcb52726cf	321	break;
jah128	0:9ffe8ebd1c40	322	}
jah128	11:5ebcb52726cf	323
jah128	9:7a4fc1d7e484	324	//6. Estimate distance based on 2 values using calculate_reflected_distances(); store in reflected_ir_distances()
jah128	9:7a4fc1d7e484	325	reflected_ir_distances [index] = calculate_reflected_distance( background_ir_values [index], illuminated_ir_values [index]);
jah128	9:7a4fc1d7e484	326	ir_values_stored = 1;
jah128	9:7a4fc1d7e484	327	return reflected_ir_distances [index];
jah128	9:7a4fc1d7e484	328	}
jah128	9:7a4fc1d7e484	329
jah128	9:7a4fc1d7e484	330
jah128	9:7a4fc1d7e484	331	// Returns the stored value of the reflected obstacle based on last call of either read_reflected_ir_distance or store_reflected_distances
jah128	9:7a4fc1d7e484	332	// Introduced in API 0.6
jah128	11:5ebcb52726cf	333	float PiSwarm::get_reflected_ir_distance ( char index )
jah128	11:5ebcb52726cf	334	{
jah128	9:7a4fc1d7e484	335	// Sanity check: check range of index and that values have been stored
jah128	9:7a4fc1d7e484	336	if (index>7 \|\| ir_values_stored == 0) return 0.0;
jah128	11:5ebcb52726cf	337	return reflected_ir_distances[index];
jah128	9:7a4fc1d7e484	338	}
jah128	9:7a4fc1d7e484	339
jah128	9:7a4fc1d7e484	340	// Returns the stored value of the non-illuminated sensor based on last call of store_background_raw_ir_values
jah128	9:7a4fc1d7e484	341	// Introduced in API 0.6
jah128	11:5ebcb52726cf	342	unsigned short PiSwarm::get_background_raw_ir_value ( char index )
jah128	11:5ebcb52726cf	343	{
jah128	9:7a4fc1d7e484	344	// Sanity check: check range of index and that values have been stored
jah128	9:7a4fc1d7e484	345	if (index>7 \|\| ir_values_stored == 0) return 0.0;
jah128	11:5ebcb52726cf	346	return background_ir_values[index];
jah128	9:7a4fc1d7e484	347	}
jah128	11:5ebcb52726cf	348
jah128	9:7a4fc1d7e484	349	// Returns the stored value of the illuminated sensor based on last call of store_illuminated_raw_ir_values
jah128	9:7a4fc1d7e484	350	// Introduced in API 0.6
jah128	11:5ebcb52726cf	351	unsigned short PiSwarm::get_illuminated_raw_ir_value ( char index )
jah128	11:5ebcb52726cf	352	{
jah128	9:7a4fc1d7e484	353	// Sanity check: check range of index and that values have been stored
jah128	9:7a4fc1d7e484	354	if (index>7 \|\| ir_values_stored == 0) return 0.0;
jah128	11:5ebcb52726cf	355	return illuminated_ir_values[index];
jah128	9:7a4fc1d7e484	356	}
jah128	11:5ebcb52726cf	357
jah128	9:7a4fc1d7e484	358	// Stores the reflected distances for all 8 IR sensors
jah128	9:7a4fc1d7e484	359	// Introduced in API 0.6
jah128	11:5ebcb52726cf	360	void PiSwarm::store_reflected_ir_distances ( void )
jah128	11:5ebcb52726cf	361	{
jah128	9:7a4fc1d7e484	362	store_background_raw_ir_values();
jah128	9:7a4fc1d7e484	363	store_illuminated_raw_ir_values();
jah128	11:5ebcb52726cf	364	for(int i=0; i<8; i++) {
jah128	11:5ebcb52726cf	365	reflected_ir_distances [i] = calculate_reflected_distance( background_ir_values [i], illuminated_ir_values [i]);
jah128	11:5ebcb52726cf	366	}
jah128	9:7a4fc1d7e484	367	}
jah128	11:5ebcb52726cf	368
jah128	9:7a4fc1d7e484	369	// Stores the raw ADC values for all 8 IR sensors without enabling IR emitters
jah128	9:7a4fc1d7e484	370	// Introduced in API 0.6
jah128	11:5ebcb52726cf	371	void PiSwarm::store_background_raw_ir_values ( void )
jah128	11:5ebcb52726cf	372	{
jah128	9:7a4fc1d7e484	373	ir_values_stored = 1;
jah128	11:5ebcb52726cf	374	for(int i=0; i<8; i++) {
jah128	9:7a4fc1d7e484	375	background_ir_values [i] = read_adc_value(i);
jah128	9:7a4fc1d7e484	376	}
jah128	9:7a4fc1d7e484	377	}
jah128	11:5ebcb52726cf	378
jah128	9:7a4fc1d7e484	379	// Stores the raw ADC values for all 8 IR sensors with a 500us emitter pulse
jah128	9:7a4fc1d7e484	380	// Introduced in API 0.6
jah128	11:5ebcb52726cf	381	void PiSwarm::store_illuminated_raw_ir_values ( void )
jah128	11:5ebcb52726cf	382	{
jah128	9:7a4fc1d7e484	383	//1. Check that the IR LDO regulator is on, enable if it isn't
jah128	11:5ebcb52726cf	384	if(enable_ir_ldo == 0) {
jah128	9:7a4fc1d7e484	385	enable_ir_ldo = 1;
jah128	9:7a4fc1d7e484	386	enable_ldo_outputs();
jah128	9:7a4fc1d7e484	387	}
jah128	11:5ebcb52726cf	388
jah128	11:5ebcb52726cf	389	//2. Enable the front IR emitters and store the values
jah128	9:7a4fc1d7e484	390	_irpulse_1 = 1;
jah128	11:5ebcb52726cf	391	wait_us(500);
jah128	9:7a4fc1d7e484	392	illuminated_ir_values [0] = read_adc_value(0);
jah128	9:7a4fc1d7e484	393	illuminated_ir_values [1] = read_adc_value(1);
jah128	9:7a4fc1d7e484	394	illuminated_ir_values [6] = read_adc_value(6);
jah128	9:7a4fc1d7e484	395	illuminated_ir_values [7] = read_adc_value(7);
jah128	9:7a4fc1d7e484	396	_irpulse_1 = 0;
jah128	11:5ebcb52726cf	397
jah128	11:5ebcb52726cf	398	//3. Enable the rear+side IR emitters and store the values
jah128	9:7a4fc1d7e484	399	_irpulse_2 = 1;
jah128	11:5ebcb52726cf	400	wait_us(500);
jah128	9:7a4fc1d7e484	401	illuminated_ir_values [2] = read_adc_value(2);
jah128	9:7a4fc1d7e484	402	illuminated_ir_values [3] = read_adc_value(3);
jah128	9:7a4fc1d7e484	403	illuminated_ir_values [4] = read_adc_value(4);
jah128	9:7a4fc1d7e484	404	illuminated_ir_values [5] = read_adc_value(5);
jah128	9:7a4fc1d7e484	405	_irpulse_2 = 0;
jah128	9:7a4fc1d7e484	406	}
jah128	11:5ebcb52726cf	407
jah128	9:7a4fc1d7e484	408	// Converts a background and illuminated value into a distance
jah128	9:7a4fc1d7e484	409	// Introduced in API 0.6 - used by read_reflected_ir_distance and store_reflected_ir_distances
jah128	11:5ebcb52726cf	410	float PiSwarm::calculate_reflected_distance ( unsigned short background_value, unsigned short illuminated_value )
jah128	11:5ebcb52726cf	411	{
jah128	9:7a4fc1d7e484	412	float approximate_distance = 4000 + background_value - illuminated_value;
jah128	9:7a4fc1d7e484	413	if(approximate_distance < 0) approximate_distance = 0;
jah128	11:5ebcb52726cf	414
jah128	0:9ffe8ebd1c40	415	// Very approximate: root value, divide by 2, approx distance in mm
jah128	9:7a4fc1d7e484	416	approximate_distance = sqrt (approximate_distance) / 2.0;
jah128	11:5ebcb52726cf	417
jah128	0:9ffe8ebd1c40	418	// Then add adjustment value if value >27
jah128	9:7a4fc1d7e484	419	if(approximate_distance > 27) {
jah128	9:7a4fc1d7e484	420	float shift = pow(approximate_distance - 27,3);
jah128	9:7a4fc1d7e484	421	approximate_distance += shift;
jah128	0:9ffe8ebd1c40	422	}
jah128	9:7a4fc1d7e484	423	if(approximate_distance > 90) approximate_distance = 100.0;
jah128	11:5ebcb52726cf	424	return approximate_distance;
jah128	0:9ffe8ebd1c40	425	}
jah128	0:9ffe8ebd1c40	426
jah128	3:4c0f2f3de33e	427	// Returns the illuminated raw sensor value for the IR sensor defined by index (range 0-7); turns on the emitters for a 500us pulse
jah128	11:5ebcb52726cf	428	unsigned short PiSwarm::read_illuminated_raw_ir_value ( char index )
jah128	11:5ebcb52726cf	429	{
jah128	3:4c0f2f3de33e	430	//This function reads the IR strength when illuminated - used for PC system debugging purposes
jah128	3:4c0f2f3de33e	431	//1. Check that the IR LDO regulator is on, enable if it isn't
jah128	11:5ebcb52726cf	432	if(enable_ir_ldo == 0) {
jah128	3:4c0f2f3de33e	433	enable_ir_ldo = 1;
jah128	3:4c0f2f3de33e	434	enable_ldo_outputs();
jah128	3:4c0f2f3de33e	435	}
jah128	3:4c0f2f3de33e	436	//2. Enable the relevant IR emitter by turning on its pulse output
jah128	11:5ebcb52726cf	437	switch(index) {
jah128	11:5ebcb52726cf	438	case 0:
jah128	11:5ebcb52726cf	439	case 1:
jah128	11:5ebcb52726cf	440	case 6:
jah128	11:5ebcb52726cf	441	case 7:
jah128	11:5ebcb52726cf	442	_irpulse_1 = 1;
jah128	11:5ebcb52726cf	443	break;
jah128	11:5ebcb52726cf	444	case 2:
jah128	11:5ebcb52726cf	445	case 3:
jah128	11:5ebcb52726cf	446	case 4:
jah128	11:5ebcb52726cf	447	case 5:
jah128	11:5ebcb52726cf	448	_irpulse_2 = 1;
jah128	11:5ebcb52726cf	449	break;
jah128	3:4c0f2f3de33e	450	}
jah128	11:5ebcb52726cf	451	wait_us(500);
jah128	3:4c0f2f3de33e	452	//3. Read the ADC value now IR emitter is on
jah128	3:4c0f2f3de33e	453	unsigned short strong_value = read_adc_value ( index );
jah128	3:4c0f2f3de33e	454	//4. Turn off IR emitter
jah128	11:5ebcb52726cf	455	switch(index) {
jah128	11:5ebcb52726cf	456	case 0:
jah128	11:5ebcb52726cf	457	case 1:
jah128	11:5ebcb52726cf	458	case 6:
jah128	11:5ebcb52726cf	459	case 7:
jah128	11:5ebcb52726cf	460	_irpulse_1 = 0;
jah128	11:5ebcb52726cf	461	break;
jah128	11:5ebcb52726cf	462	case 2:
jah128	11:5ebcb52726cf	463	case 3:
jah128	11:5ebcb52726cf	464	case 4:
jah128	11:5ebcb52726cf	465	case 5:
jah128	11:5ebcb52726cf	466	_irpulse_2 = 0;
jah128	11:5ebcb52726cf	467	break;
jah128	3:4c0f2f3de33e	468	}
jah128	3:4c0f2f3de33e	469	return strong_value;
jah128	3:4c0f2f3de33e	470	}
jah128	3:4c0f2f3de33e	471
jah128	1:b067a08ff54e	472	// Returns the raw sensor value for the IR sensor defined by index (range 0-7).
jah128	11:5ebcb52726cf	473	unsigned short PiSwarm::read_adc_value ( char index )
jah128	11:5ebcb52726cf	474	{
jah128	0:9ffe8ebd1c40	475	short value = 0;
jah128	0:9ffe8ebd1c40	476	// Read a single value from the ADC
jah128	11:5ebcb52726cf	477	if(index<8) {
jah128	0:9ffe8ebd1c40	478	char apb[1];
jah128	0:9ffe8ebd1c40	479	char data[2];
jah128	11:5ebcb52726cf	480	switch(index) {
jah128	11:5ebcb52726cf	481	case 0:
jah128	11:5ebcb52726cf	482	apb[0]=0x80;
jah128	11:5ebcb52726cf	483	break;
jah128	11:5ebcb52726cf	484	case 1:
jah128	11:5ebcb52726cf	485	apb[0]=0x90;
jah128	11:5ebcb52726cf	486	break;
jah128	11:5ebcb52726cf	487	case 2:
jah128	11:5ebcb52726cf	488	apb[0]=0xA0;
jah128	11:5ebcb52726cf	489	break;
jah128	11:5ebcb52726cf	490	case 3:
jah128	11:5ebcb52726cf	491	apb[0]=0xB0;
jah128	11:5ebcb52726cf	492	break;
jah128	11:5ebcb52726cf	493	case 4:
jah128	11:5ebcb52726cf	494	apb[0]=0xC0;
jah128	11:5ebcb52726cf	495	break;
jah128	11:5ebcb52726cf	496	case 5:
jah128	11:5ebcb52726cf	497	apb[0]=0xD0;
jah128	11:5ebcb52726cf	498	break;
jah128	11:5ebcb52726cf	499	case 6:
jah128	11:5ebcb52726cf	500	apb[0]=0xE0;
jah128	11:5ebcb52726cf	501	break;
jah128	11:5ebcb52726cf	502	case 7:
jah128	11:5ebcb52726cf	503	apb[0]=0xF0;
jah128	11:5ebcb52726cf	504	break;
jah128	0:9ffe8ebd1c40	505	}
jah128	0:9ffe8ebd1c40	506	_i2c.write(ADC_ADDRESS,apb,1,false);
jah128	0:9ffe8ebd1c40	507	_i2c.read(ADC_ADDRESS,data,2,false);
jah128	0:9ffe8ebd1c40	508	value=((data[0] % 16)<<8)+data[1];
jah128	0:9ffe8ebd1c40	509	if(value > 4096) value=4096;
jah128	0:9ffe8ebd1c40	510	value=4096-value;
jah128	0:9ffe8ebd1c40	511	}
jah128	0:9ffe8ebd1c40	512	return value;
jah128	0:9ffe8ebd1c40	513	}
jah128	0:9ffe8ebd1c40	514
jah128	1:b067a08ff54e	515	// Function enables or disables the LDO voltage regulators which supply power to the outer LEDs and the IR photo emitters.
jah128	11:5ebcb52726cf	516	void PiSwarm::enable_ldo_outputs ()
jah128	11:5ebcb52726cf	517	{
jah128	1:b067a08ff54e	518	char data[2];
jah128	1:b067a08ff54e	519	data[0] = 0x0A; //Address for PCA9505 Output Port 2
jah128	1:b067a08ff54e	520	data[1] = 0;
jah128	1:b067a08ff54e	521	if(enable_led_ldo) data[1] +=128;
jah128	1:b067a08ff54e	522	if(enable_ir_ldo) data[1] +=64;
jah128	1:b067a08ff54e	523	_i2c.write(EXPANSION_IC_ADDRESS,data,2,false);
jah128	0:9ffe8ebd1c40	524	}
jah128	0:9ffe8ebd1c40	525
jah128	1:b067a08ff54e	526	//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
jah128	1:b067a08ff54e	527	// MEMS Sensor Functions
jah128	1:b067a08ff54e	528	//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
jah128	0:9ffe8ebd1c40	529
jah128	1:b067a08ff54e	530	// Returns the yaw (clockwise) rotation in degrees-per-second reported by the LY3200 gyroscope.
jah128	11:5ebcb52726cf	531	float PiSwarm::read_gyro ( void )
jah128	11:5ebcb52726cf	532	{
jah128	11:5ebcb52726cf	533	// Gyro is a ST Micro LY3200ALH Yaw-rate Gyro: 0.67mV / d.p.s, 1.5V offset
jah128	0:9ffe8ebd1c40	534	float r_gyro = _gyro.read();
jah128	0:9ffe8ebd1c40	535	r_gyro *= gyro_cal; // NB 3.4 seemed to work better that 3.3 but better to run calibration routine
jah128	0:9ffe8ebd1c40	536	r_gyro = 1.5 - r_gyro; // 1.5V off-set, invert sign
jah128	11:5ebcb52726cf	537	r_gyro *= 1492.5; // Convert to DPS (+ve = clockwise, -ve = c.clockwise)
jah128	0:9ffe8ebd1c40	538	return r_gyro;
jah128	0:9ffe8ebd1c40	539	}
jah128	1:b067a08ff54e	540
jah128	11:5ebcb52726cf	541	// Returns the acceleration in the X plane reported by the LIS332 accelerometer. Returned value is in mg.
jah128	11:5ebcb52726cf	542	float PiSwarm::read_accelerometer_x ( void )
jah128	11:5ebcb52726cf	543	{
jah128	11:5ebcb52726cf	544	// LIS 332 AX Accelerometer - 0.363 V/g - 1.25V
jah128	0:9ffe8ebd1c40	545	float r_acc_x = (( _accel_x.read() * acc_x_cal ) - 1250.0 ) * 2.754; // Return value in mG
jah128	0:9ffe8ebd1c40	546	return r_acc_x;
jah128	0:9ffe8ebd1c40	547	}
jah128	11:5ebcb52726cf	548
jah128	11:5ebcb52726cf	549	// Returns the acceleration in the Y plane reported by the LIS332 accelerometer. Returned value is in mg.
jah128	11:5ebcb52726cf	550	float PiSwarm::read_accelerometer_y ( void )
jah128	11:5ebcb52726cf	551	{
jah128	11:5ebcb52726cf	552	float r_acc_y = (( _accel_y.read() * acc_y_cal ) - 1250.0 ) * 2.754; // Return value in mG
jah128	0:9ffe8ebd1c40	553	return r_acc_y;
jah128	0:9ffe8ebd1c40	554	}
jah128	11:5ebcb52726cf	555
jah128	11:5ebcb52726cf	556	// Returns the acceleration in the Z plane reported by the LIS332 accelerometer. Returned value is in mg.
jah128	11:5ebcb52726cf	557	float PiSwarm::read_accelerometer_z ( void )
jah128	11:5ebcb52726cf	558	{
jah128	0:9ffe8ebd1c40	559	float r_acc_z = (( _accel_z.read() * acc_z_cal ) - 1250.0 ) * 2.754; // Return value in mG
jah128	0:9ffe8ebd1c40	560	return r_acc_z;
jah128	0:9ffe8ebd1c40	561	}
jah128	0:9ffe8ebd1c40	562
jah128	1:b067a08ff54e	563	// Sends message to the magnetometer to read new values. Should be called before get_magnetometer_* when updated values are required
jah128	11:5ebcb52726cf	564	char PiSwarm::read_magnetometer ( void )
jah128	11:5ebcb52726cf	565	{
jah128	0:9ffe8ebd1c40	566	char read_data[7];
jah128	0:9ffe8ebd1c40	567	char success;
jah128	0:9ffe8ebd1c40	568	char command_data[1];
jah128	0:9ffe8ebd1c40	569	command_data[0] = 0x00; //Auto-read from register 0x00 (status)
jah128	0:9ffe8ebd1c40	570	success = _i2c.write(MAGNETOMETER_ADDRESS, command_data, 1, false);
jah128	0:9ffe8ebd1c40	571	_i2c.read(MAGNETOMETER_ADDRESS, read_data, 7, false);
jah128	0:9ffe8ebd1c40	572	mag_values[0] = (read_data[1] << 8) + read_data[2];
jah128	0:9ffe8ebd1c40	573	mag_values[1] = (read_data[3] << 8) + read_data[4];
jah128	0:9ffe8ebd1c40	574	mag_values[2] = (read_data[5] << 8) + read_data[6];
jah128	11:5ebcb52726cf	575	return success;
jah128	0:9ffe8ebd1c40	576	}
jah128	11:5ebcb52726cf	577
jah128	11:5ebcb52726cf	578	// Returns the raw value for the X-plane magnetic field stored on the last call of read_magnetometer
jah128	11:5ebcb52726cf	579	signed short PiSwarm::get_magnetometer_x ( void )
jah128	11:5ebcb52726cf	580	{
jah128	0:9ffe8ebd1c40	581	return mag_values[0];
jah128	0:9ffe8ebd1c40	582	}
jah128	11:5ebcb52726cf	583
jah128	11:5ebcb52726cf	584	// Returns the raw value for the Y-plane magnetic field stored on the last call of read_magnetometer
jah128	11:5ebcb52726cf	585	signed short PiSwarm::get_magnetometer_y ( void )
jah128	11:5ebcb52726cf	586	{
jah128	0:9ffe8ebd1c40	587	return mag_values[1];
jah128	0:9ffe8ebd1c40	588	}
jah128	11:5ebcb52726cf	589
jah128	11:5ebcb52726cf	590	// Returns the raw value for the Z-plane magnetic field stored on the last call of read_magnetometer
jah128	11:5ebcb52726cf	591	signed short PiSwarm::get_magnetometer_z ( void )
jah128	11:5ebcb52726cf	592	{
jah128	0:9ffe8ebd1c40	593	return mag_values[2];
jah128	0:9ffe8ebd1c40	594	}
jah128	0:9ffe8ebd1c40	595
jah128	1:b067a08ff54e	596	//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
jah128	1:b067a08ff54e	597	// Other Sensor Functions
jah128	1:b067a08ff54e	598	//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
jah128	1:b067a08ff54e	599
jah128	1:b067a08ff54e	600	// Returns the temperature reported by the MCP9701 sensor in degrees centigrade.
jah128	11:5ebcb52726cf	601	float PiSwarm::read_temperature ( void )
jah128	11:5ebcb52726cf	602	{
jah128	1:b067a08ff54e	603	// Temperature Sensor is a Microchip MCP9701T-E/LT: 19.5mV/C 0C = 400mV
jah128	1:b067a08ff54e	604	float r_temp = _temperature.read();
jah128	1:b067a08ff54e	605	r_temp -= 0.1212f; // 0.4 / 3.3
jah128	1:b067a08ff54e	606	r_temp *= 169.231f; // 3.3 / .0195
jah128	1:b067a08ff54e	607	return r_temp;
jah128	1:b067a08ff54e	608	}
jah128	1:b067a08ff54e	609
jah128	1:b067a08ff54e	610	// Returns the adjusted value (0-100) for the ambient light sensor
jah128	11:5ebcb52726cf	611	float PiSwarm::read_light_sensor ( void )
jah128	11:5ebcb52726cf	612	{
jah128	1:b067a08ff54e	613	// Light sensor is a Avago APDS-9005 Ambient Light Sensor
jah128	1:b067a08ff54e	614	//float r_light = (_light.read() * 141); //Gives a range of (approximately) 0=dark 100=max. light
jah128	1:b067a08ff54e	615	float r_light = (sqrt(_light.read() * 1.41) * 100); //Non-linear and more CPU intensive but generally gives better low-light measures
jah128	1:b067a08ff54e	616	if(r_light > 100) r_light = 100;
jah128	1:b067a08ff54e	617	return r_light;
jah128	1:b067a08ff54e	618	}
jah128	1:b067a08ff54e	619
jah128	1:b067a08ff54e	620
jah128	11:5ebcb52726cf	621	void PiSwarm::read_raw_sensors ( int * raw_ls_array )
jah128	11:5ebcb52726cf	622	{
jah128	1:b067a08ff54e	623	_ser.putc(SEND_RAW_SENSOR_VALUES);
jah128	1:b067a08ff54e	624	for (int i = 0; i < 5 ; i ++) {
jah128	1:b067a08ff54e	625	int val = _ser.getc();
jah128	1:b067a08ff54e	626	val += _ser.getc() << 8;
jah128	1:b067a08ff54e	627	raw_ls_array [i] = val;
jah128	1:b067a08ff54e	628	}
jah128	1:b067a08ff54e	629	}
jah128	1:b067a08ff54e	630
jah128	1:b067a08ff54e	631	// Returns the uptime of the system (since initialisation) in seconds
jah128	11:5ebcb52726cf	632	float PiSwarm::get_uptime (void)
jah128	11:5ebcb52726cf	633	{
jah128	1:b067a08ff54e	634	return _system_timer.read();
jah128	1:b067a08ff54e	635	}
jah128	1:b067a08ff54e	636
jah128	1:b067a08ff54e	637	// Returns the battery level in millivolts
jah128	11:5ebcb52726cf	638	float PiSwarm::battery()
jah128	11:5ebcb52726cf	639	{
jah128	1:b067a08ff54e	640	_ser.putc(SEND_BATTERY_MILLIVOLTS);
jah128	1:b067a08ff54e	641	char lowbyte = _ser.getc();
jah128	1:b067a08ff54e	642	char hibyte = _ser.getc();
jah128	1:b067a08ff54e	643	float v = ((lowbyte + (hibyte << 8))/1000.0);
jah128	1:b067a08ff54e	644	return(v);
jah128	1:b067a08ff54e	645	}
jah128	1:b067a08ff54e	646
jah128	1:b067a08ff54e	647	// Returns the raw value for the variable resistor on the base of the platform. Ranges from 0 to 1024.
jah128	11:5ebcb52726cf	648	float PiSwarm::pot_voltage(void)
jah128	11:5ebcb52726cf	649	{
jah128	1:b067a08ff54e	650	int volt = 0;
jah128	1:b067a08ff54e	651	_ser.putc(SEND_TRIMPOT);
jah128	1:b067a08ff54e	652	volt = _ser.getc();
jah128	1:b067a08ff54e	653	volt += _ser.getc() << 8;
jah128	1:b067a08ff54e	654	return(volt);
jah128	1:b067a08ff54e	655	}
jah128	1:b067a08ff54e	656
jah128	1:b067a08ff54e	657	// Returns the ID of platform (set by the DIL switches above the display). Range 0-31 [0 reserved].
jah128	11:5ebcb52726cf	658	char PiSwarm::get_id ()
jah128	11:5ebcb52726cf	659	{
jah128	1:b067a08ff54e	660	return id;
jah128	1:b067a08ff54e	661	}
jah128	1:b067a08ff54e	662
jah128	1:b067a08ff54e	663	// Return the current stored state for direction switch(es) pressed {1 = Center 2 = Right 4 = Left 8 = Down 16 = Up}
jah128	11:5ebcb52726cf	664	char PiSwarm::get_switches ()
jah128	11:5ebcb52726cf	665	{
jah128	1:b067a08ff54e	666	return switches;
jah128	1:b067a08ff54e	667	}
jah128	1:b067a08ff54e	668
jah128	1:b067a08ff54e	669
jah128	1:b067a08ff54e	670	//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
jah128	1:b067a08ff54e	671	// Motor Functions
jah128	1:b067a08ff54e	672	//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
jah128	1:b067a08ff54e	673
jah128	1:b067a08ff54e	674	// Returns the target speed of the left motor (-1.0 to 1.0)
jah128	11:5ebcb52726cf	675	float PiSwarm::get_left_motor ()
jah128	11:5ebcb52726cf	676	{
jah128	1:b067a08ff54e	677	return left_speed;
jah128	1:b067a08ff54e	678	}
jah128	1:b067a08ff54e	679
jah128	1:b067a08ff54e	680	// Returns the target speed of the right motor (-1.0 to 1.0)
jah128	11:5ebcb52726cf	681	float PiSwarm::get_right_motor ()
jah128	11:5ebcb52726cf	682	{
jah128	1:b067a08ff54e	683	return right_speed;
jah128	1:b067a08ff54e	684	}
jah128	1:b067a08ff54e	685
jah128	1:b067a08ff54e	686	// Sets the speed of the left motor (-1.0 to 1.0)
jah128	11:5ebcb52726cf	687	void PiSwarm::left_motor (float speed)
jah128	11:5ebcb52726cf	688	{
jah128	1:b067a08ff54e	689	motor(0,speed);
jah128	1:b067a08ff54e	690	}
jah128	1:b067a08ff54e	691
jah128	1:b067a08ff54e	692	// Sets the speed of the right motor (-1.0 to 1.0)
jah128	11:5ebcb52726cf	693	void PiSwarm::right_motor (float speed)
jah128	11:5ebcb52726cf	694	{
jah128	1:b067a08ff54e	695	motor(1,speed);
jah128	1:b067a08ff54e	696	}
jah128	1:b067a08ff54e	697
jah128	1:b067a08ff54e	698	// Drive forward at the given speed (-1.0 to 1.0)
jah128	11:5ebcb52726cf	699	void PiSwarm::forward (float speed)
jah128	11:5ebcb52726cf	700	{
jah128	1:b067a08ff54e	701	motor(0,speed);
jah128	1:b067a08ff54e	702	motor(1,speed);
jah128	1:b067a08ff54e	703	}
jah128	1:b067a08ff54e	704
jah128	1:b067a08ff54e	705	// Drive backward at the given speed (-1.0 to 1.0)
jah128	11:5ebcb52726cf	706	void PiSwarm::backward (float speed)
jah128	11:5ebcb52726cf	707	{
jah128	1:b067a08ff54e	708	motor(0,-1.0*speed);
jah128	1:b067a08ff54e	709	motor(1,-1.0*speed);
jah128	1:b067a08ff54e	710	}
jah128	1:b067a08ff54e	711
jah128	1:b067a08ff54e	712	// Turn on-the-spot left at the given speed (-1.0 to 1.0)
jah128	11:5ebcb52726cf	713	void PiSwarm::left (float speed)
jah128	11:5ebcb52726cf	714	{
jah128	1:b067a08ff54e	715	motor(0,speed);
jah128	1:b067a08ff54e	716	motor(1,-1.0*speed);
jah128	1:b067a08ff54e	717	}
jah128	1:b067a08ff54e	718
jah128	1:b067a08ff54e	719	// Turn on-the-spot right at the given speed (-1.0 to 1.0)
jah128	11:5ebcb52726cf	720	void PiSwarm::right (float speed)
jah128	11:5ebcb52726cf	721	{
jah128	1:b067a08ff54e	722	motor(0,-1.0*speed);
jah128	1:b067a08ff54e	723	motor(1,speed);
jah128	1:b067a08ff54e	724	}
jah128	1:b067a08ff54e	725
jah128	1:b067a08ff54e	726	// Stop both motors
jah128	11:5ebcb52726cf	727	void PiSwarm::stop (void)
jah128	11:5ebcb52726cf	728	{
jah128	1:b067a08ff54e	729	motor(0,0.0);
jah128	1:b067a08ff54e	730	motor(1,0.0);
jah128	1:b067a08ff54e	731	}
jah128	1:b067a08ff54e	732
jah128	11:5ebcb52726cf	733	// Turn at slow speed (0.15) to the set amount of degrees (clockwise)
jah128	11:5ebcb52726cf	734	void PiSwarm::slow_turn (float degrees)
jah128	11:5ebcb52726cf	735	{
jah128	11:5ebcb52726cf	736	float trunc = fmodf(degrees,360);
jah128	11:5ebcb52726cf	737	if(trunc<-180) trunc+=360;
jah128	11:5ebcb52726cf	738	if(trunc>180) trunc-=360;
jah128	11:5ebcb52726cf	739	if(trunc<0) {
jah128	11:5ebcb52726cf	740	float m_time = 1-trunc;
jah128	11:5ebcb52726cf	741	float delay = (m_time + logf(SLOW_TURN_LEFT_OFFSET + m_time + m_time)) * SLOW_TURN_LEFT_MULTIPLIER;
jah128	11:5ebcb52726cf	742	motor_timeout.attach_us(this,&PiSwarm::stop,(unsigned int) delay);
jah128	11:5ebcb52726cf	743	left(0.15);
jah128	11:5ebcb52726cf	744	} else if(trunc>0) {
jah128	11:5ebcb52726cf	745	float delay = (trunc + logf(SLOW_TURN_RIGHT_OFFSET + trunc + trunc)) * SLOW_TURN_RIGHT_MULTIPLIER;
jah128	11:5ebcb52726cf	746	motor_timeout.attach_us(this,&PiSwarm::stop,(unsigned int) delay);
jah128	11:5ebcb52726cf	747	right(0.15);
jah128	11:5ebcb52726cf	748	}
jah128	11:5ebcb52726cf	749	}
jah128	11:5ebcb52726cf	750
jah128	11:5ebcb52726cf	751	// Turn at fast speed (0.5) to the set amount of degrees (clockwise)
jah128	11:5ebcb52726cf	752	void PiSwarm::fast_turn (float degrees)
jah128	11:5ebcb52726cf	753	{
jah128	11:5ebcb52726cf	754	float trunc = fmodf(degrees,360);
jah128	11:5ebcb52726cf	755	if(trunc<-180) trunc+=360;
jah128	11:5ebcb52726cf	756	if(trunc>180) trunc-=360;
jah128	11:5ebcb52726cf	757	if(trunc<0) {
jah128	11:5ebcb52726cf	758	float m_time = 2.5-trunc;
jah128	11:5ebcb52726cf	759	float delay = m_time * FAST_TURN_LEFT_MULTIPLIER;
jah128	11:5ebcb52726cf	760	motor_timeout.attach_us(this,&PiSwarm::stop,(unsigned int) delay);
jah128	11:5ebcb52726cf	761	left(0.5);
jah128	11:5ebcb52726cf	762	} else if(trunc>0) {
jah128	11:5ebcb52726cf	763	float delay = (trunc + 2.5) * FAST_TURN_RIGHT_MULTIPLIER;
jah128	11:5ebcb52726cf	764	motor_timeout.attach_us(this,&PiSwarm::stop,(unsigned int) delay);
jah128	11:5ebcb52726cf	765	right(0.5);
jah128	11:5ebcb52726cf	766	}
jah128	11:5ebcb52726cf	767	}
jah128	11:5ebcb52726cf	768
jah128	11:5ebcb52726cf	769	void PiSwarm::test_turn_calibration(void)
jah128	11:5ebcb52726cf	770	{
jah128	11:5ebcb52726cf	771	}
jah128	11:5ebcb52726cf	772
jah128	1:b067a08ff54e	773	//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
jah128	1:b067a08ff54e	774	// Sound Functions
jah128	1:b067a08ff54e	775	//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
jah128	1:b067a08ff54e	776
jah128	11:5ebcb52726cf	777	void PiSwarm::play_tune (char * tune, int length)
jah128	11:5ebcb52726cf	778	{
jah128	1:b067a08ff54e	779	_ser.putc(DO_PLAY);
jah128	11:5ebcb52726cf	780	_ser.putc(length);
jah128	1:b067a08ff54e	781	for (int i = 0 ; i < length ; i++) {
jah128	11:5ebcb52726cf	782	_ser.putc(tune[i]);
jah128	1:b067a08ff54e	783	}
jah128	1:b067a08ff54e	784	}
jah128	1:b067a08ff54e	785
jah128	4:52b3e4c5a425	786	//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
jah128	4:52b3e4c5a425	787	// Display Functions
jah128	4:52b3e4c5a425	788	//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
jah128	4:52b3e4c5a425	789
jah128	11:5ebcb52726cf	790	void PiSwarm::locate(int x, int y)
jah128	11:5ebcb52726cf	791	{
jah128	4:52b3e4c5a425	792	_ser.putc(DO_LCD_GOTO_XY);
jah128	4:52b3e4c5a425	793	_ser.putc(x);
jah128	4:52b3e4c5a425	794	_ser.putc(y);
jah128	4:52b3e4c5a425	795	}
jah128	4:52b3e4c5a425	796
jah128	11:5ebcb52726cf	797	void PiSwarm::cls(void)
jah128	11:5ebcb52726cf	798	{
jah128	4:52b3e4c5a425	799	_ser.putc(DO_CLEAR);
jah128	4:52b3e4c5a425	800	}
jah128	1:b067a08ff54e	801
jah128	1:b067a08ff54e	802	//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
jah128	1:b067a08ff54e	803	// EEPROM Functions
jah128	1:b067a08ff54e	804	//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
jah128	1:b067a08ff54e	805
jah128	1:b067a08ff54e	806
jah128	11:5ebcb52726cf	807	void PiSwarm::write_eeprom_byte ( int address, char data )
jah128	11:5ebcb52726cf	808	{
jah128	0:9ffe8ebd1c40	809	char write_array[3];
jah128	0:9ffe8ebd1c40	810	write_array[0] = address / 256;
jah128	0:9ffe8ebd1c40	811	write_array[1] = address % 256;
jah128	0:9ffe8ebd1c40	812	write_array[2] = data;
jah128	0:9ffe8ebd1c40	813	_i2c.write(EEPROM_ADDRESS, write_array, 3, false);
jah128	0:9ffe8ebd1c40	814	//Takes 5ms to write a page: ideally this could be done with a timer or RTOS
jah128	0:9ffe8ebd1c40	815	wait(0.005);
jah128	0:9ffe8ebd1c40	816	}
jah128	11:5ebcb52726cf	817
jah128	11:5ebcb52726cf	818	void PiSwarm::write_eeprom_block ( int address, char length, char * data)
jah128	11:5ebcb52726cf	819	{
jah128	0:9ffe8ebd1c40	820	/** Note from datasheet:
jah128	0:9ffe8ebd1c40	821	Page write operations are limited to writing bytes within a single physical page,
jah128	0:9ffe8ebd1c40	822	regardless of the number of bytes actually being written. Physical page
jah128	0:9ffe8ebd1c40	823	boundaries start at addresses that are integer multiples of the page buffer size (or
jah128	0:9ffe8ebd1c40	824	‘page size’) and end at addresses that are integer multiples of [page size – 1]. If a
jah128	0:9ffe8ebd1c40	825	Page Write command attempts to write across a physical page boundary, the
jah128	0:9ffe8ebd1c40	826	result is that the data wraps around to the beginning of the current page (overwriting
jah128	0:9ffe8ebd1c40	827	data previously stored there), instead of being written to the next page, as might be
jah128	0:9ffe8ebd1c40	828	expected. It is therefore necessary for the application software to prevent page write
jah128	0:9ffe8ebd1c40	829	operations that would attempt to cross a page boundary. */
jah128	11:5ebcb52726cf	830
jah128	0:9ffe8ebd1c40	831	//First check to see if first block starts at the start of a page
jah128	0:9ffe8ebd1c40	832	int subpage = address % 32;
jah128	11:5ebcb52726cf	833
jah128	0:9ffe8ebd1c40	834	//If subpage > 0 first page does not start at a page boundary
jah128	0:9ffe8ebd1c40	835	int write_length = 32 - subpage;
jah128	0:9ffe8ebd1c40	836	if( write_length > length ) write_length = length;
jah128	0:9ffe8ebd1c40	837	char firstpage_array [write_length+2];
jah128	0:9ffe8ebd1c40	838	firstpage_array[0] = address / 256;
jah128	0:9ffe8ebd1c40	839	firstpage_array[1] = address % 256;
jah128	11:5ebcb52726cf	840	for(int i=0; i<write_length; i++) {
jah128	11:5ebcb52726cf	841	firstpage_array[i+2] = data [i];
jah128	11:5ebcb52726cf	842	}
jah128	0:9ffe8ebd1c40	843	_i2c.write(EEPROM_ADDRESS, firstpage_array, write_length + 2, false);
jah128	0:9ffe8ebd1c40	844	wait(0.005);
jah128	11:5ebcb52726cf	845
jah128	11:5ebcb52726cf	846	if(length > write_length) {
jah128	0:9ffe8ebd1c40	847	int page_count = (length + subpage) / 32;
jah128	0:9ffe8ebd1c40	848	int written = write_length;
jah128	11:5ebcb52726cf	849	for(int p=0; p<page_count; p++) {
jah128	0:9ffe8ebd1c40	850	int to_write = length - written;
jah128	0:9ffe8ebd1c40	851	if (to_write > 32) {
jah128	0:9ffe8ebd1c40	852	to_write = 32;
jah128	11:5ebcb52726cf	853	}
jah128	0:9ffe8ebd1c40	854	char page_array [to_write + 2];
jah128	0:9ffe8ebd1c40	855	page_array[0] = (address + written) / 256;
jah128	0:9ffe8ebd1c40	856	page_array[1] = (address + written) % 256;
jah128	11:5ebcb52726cf	857	for(int i=0; i<to_write; i++) {
jah128	0:9ffe8ebd1c40	858	page_array[i+2] = data[written + i];
jah128	0:9ffe8ebd1c40	859	}
jah128	0:9ffe8ebd1c40	860	_i2c.write(EEPROM_ADDRESS, page_array, to_write + 2, false);
jah128	0:9ffe8ebd1c40	861	wait(0.005);
jah128	0:9ffe8ebd1c40	862	written += 32;
jah128	0:9ffe8ebd1c40	863	}
jah128	11:5ebcb52726cf	864	}
jah128	0:9ffe8ebd1c40	865	}
jah128	11:5ebcb52726cf	866
jah128	11:5ebcb52726cf	867	char PiSwarm::read_eeprom_byte ( int address )
jah128	11:5ebcb52726cf	868	{
jah128	0:9ffe8ebd1c40	869	char address_array [2];
jah128	0:9ffe8ebd1c40	870	address_array[0] = address / 256;
jah128	0:9ffe8ebd1c40	871	address_array[1] = address % 256;
jah128	0:9ffe8ebd1c40	872	char data [1];
jah128	0:9ffe8ebd1c40	873	_i2c.write(EEPROM_ADDRESS, address_array, 2, false);
jah128	0:9ffe8ebd1c40	874	_i2c.read(EEPROM_ADDRESS, data, 1, false);
jah128	0:9ffe8ebd1c40	875	return data [0];
jah128	0:9ffe8ebd1c40	876	}
jah128	0:9ffe8ebd1c40	877
jah128	11:5ebcb52726cf	878	char PiSwarm::read_next_eeprom_byte ()
jah128	11:5ebcb52726cf	879	{
jah128	0:9ffe8ebd1c40	880	char data [1];
jah128	0:9ffe8ebd1c40	881	_i2c.read(EEPROM_ADDRESS, data, 1, false);
jah128	0:9ffe8ebd1c40	882	return data [0];
jah128	0:9ffe8ebd1c40	883	}
jah128	11:5ebcb52726cf	884
jah128	11:5ebcb52726cf	885	char PiSwarm::read_eeprom_block ( int address, char length )
jah128	11:5ebcb52726cf	886	{
jah128	0:9ffe8ebd1c40	887	char ret = 0;
jah128	0:9ffe8ebd1c40	888	return ret;
jah128	0:9ffe8ebd1c40	889	}
jah128	1:b067a08ff54e	890
jah128	4:52b3e4c5a425	891	//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
jah128	4:52b3e4c5a425	892	// RF Transceiver Functions
jah128	4:52b3e4c5a425	893	//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
jah128	4:52b3e4c5a425	894
jah128	11:5ebcb52726cf	895	void PiSwarm::send_rf_message(char* message, char length)
jah128	11:5ebcb52726cf	896	{
jah128	4:52b3e4c5a425	897	_rf.sendString(length, message);
jah128	4:52b3e4c5a425	898	}
jah128	1:b067a08ff54e	899
jah128	1:b067a08ff54e	900	//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
jah128	1:b067a08ff54e	901	// Setup and Calibration Functions
jah128	1:b067a08ff54e	902	//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
jah128	1:b067a08ff54e	903
jah128	11:5ebcb52726cf	904	void PiSwarm::setup ()
jah128	11:5ebcb52726cf	905	{
jah128	1:b067a08ff54e	906	_ser.baud(115200);
jah128	1:b067a08ff54e	907	set_oled_brightness (oled_brightness);
jah128	1:b067a08ff54e	908	set_cled_brightness (cled_brightness);
jah128	1:b067a08ff54e	909	gpio_led = setup_expansion_ic();
jah128	11:5ebcb52726cf	910	beacon_led = setup_adc();
jah128	1:b067a08ff54e	911	}
jah128	1:b067a08ff54e	912
jah128	11:5ebcb52726cf	913	void PiSwarm::setup_radio()
jah128	11:5ebcb52726cf	914	{
jah128	1:b067a08ff54e	915	//Setup RF transceiver
jah128	1:b067a08ff54e	916	_rf.rf_init();
jah128	1:b067a08ff54e	917	_rf.setFrequency(RF_FREQUENCY);
jah128	1:b067a08ff54e	918	_rf.setDatarate(RF_DATARATE);
jah128	1:b067a08ff54e	919	}
jah128	1:b067a08ff54e	920
jah128	11:5ebcb52726cf	921	int PiSwarm::setup_expansion_ic ()
jah128	11:5ebcb52726cf	922	{
jah128	1:b067a08ff54e	923	//Expansion IC is NXP PCA9505
jah128	1:b067a08ff54e	924	//Address is 0x40 (defined by EXPANSION_IC_ADDRESS)
jah128	1:b067a08ff54e	925	//Block IO 0 : 7-0 Are OLED Outputs
jah128	1:b067a08ff54e	926	//Block IO 1 : 7 is NC. 6-2 are Cursor switches (interrupt inputs). 1 and 0 are OLED outputs.
jah128	1:b067a08ff54e	927	//Block IO 2 : 7 and 6 are LDO enable outputs. 5 - 1 are ID switch. 0 is NC.
jah128	1:b067a08ff54e	928	//Block IO 3 and 4 are for the expansion connector (not assigned here)
jah128	1:b067a08ff54e	929	char data [4];
jah128	1:b067a08ff54e	930	data [0] = 0x98; //Write to I/O configure register 0, using auto increment (allows multi-blocks in one command)
jah128	1:b067a08ff54e	931	data [1] = 0x00; //All 8 pins in block 0 are outputs (0)
jah128	1:b067a08ff54e	932	data [2] = 0xFC; //Pins 1 & 0 are outputs, rest are inputs (or NC)
jah128	1:b067a08ff54e	933	data [3] = 0x3F; //Pins 7 & 6 are outputs, rest are inputs (or NC)
jah128	1:b067a08ff54e	934	//If using the expansion port, assing its I/O here by increasing size of data[] and adding extra commands
jah128	1:b067a08ff54e	935	int test_val = _i2c.write(EXPANSION_IC_ADDRESS,data,4,false);
jah128	11:5ebcb52726cf	936
jah128	1:b067a08ff54e	937	// test_val should return 0 for correctly acknowledged response
jah128	11:5ebcb52726cf	938
jah128	1:b067a08ff54e	939	//Invert the polarity for switch inputs (they connect to ground when pressed\on)
jah128	1:b067a08ff54e	940	data [0] = 0x90; //Write to polarity inversion register using auto increment
jah128	1:b067a08ff54e	941	data [1] = 0x00;
jah128	1:b067a08ff54e	942	data [2] = 0x7C; //Invert pins 6-2 for cursor switches
jah128	1:b067a08ff54e	943	data [3] = 0x3E; //Invert pins 5-1 for ID switch
jah128	1:b067a08ff54e	944	_i2c.write(EXPANSION_IC_ADDRESS,data,4,false);
jah128	11:5ebcb52726cf	945
jah128	1:b067a08ff54e	946	//Setup the interrupt masks. By default, only the direction switches trigger an interrupt
jah128	1:b067a08ff54e	947	data [0] = 0xA0;
jah128	1:b067a08ff54e	948	data [1] = 0xFF;
jah128	1:b067a08ff54e	949	data [2] = 0x83; // Disable mask on pins 6-2 for cursor switches
jah128	1:b067a08ff54e	950	data [3] = 0xFF;
jah128	1:b067a08ff54e	951	_i2c.write(EXPANSION_IC_ADDRESS,data,4,false);
jah128	1:b067a08ff54e	952
jah128	1:b067a08ff54e	953	//Read the ID switches
jah128	1:b067a08ff54e	954	data [0] = 0x80; //Read input registers
jah128	1:b067a08ff54e	955	char read_data [5];
jah128	1:b067a08ff54e	956	_i2c.write(EXPANSION_IC_ADDRESS, data, 1, false);
jah128	1:b067a08ff54e	957	_i2c.read(EXPANSION_IC_ADDRESS, read_data, 5, false);
jah128	11:5ebcb52726cf	958	id = (read_data[2] & 0x3E) >> 1; //Mask pins 0, 6 and 7 then shift
jah128	11:5ebcb52726cf	959
jah128	1:b067a08ff54e	960	//Setup interrupt handler
jah128	1:b067a08ff54e	961	expansion_interrupt.mode(PullUp); // Pull Up by default (needed on v1 PCB - no pullup resistor)
jah128	1:b067a08ff54e	962	expansion_interrupt.fall(this,&PiSwarm::expansion_interrupt_handler); // Add local handler on falling edge to read switch
jah128	1:b067a08ff54e	963	return test_val;
jah128	0:9ffe8ebd1c40	964	}
jah128	0:9ffe8ebd1c40	965
jah128	11:5ebcb52726cf	966	char PiSwarm::setup_adc ( void )
jah128	11:5ebcb52726cf	967	{
jah128	1:b067a08ff54e	968	//ADC IC (for IR system) is a Analog Devices AD7997-BRUZ1
jah128	1:b067a08ff54e	969	return 0;
jah128	1:b067a08ff54e	970	}
jah128	11:5ebcb52726cf	971
jah128	11:5ebcb52726cf	972	char PiSwarm::calibrate_gyro ( void )
jah128	11:5ebcb52726cf	973	{
jah128	1:b067a08ff54e	974	//This routine attempts to calibrate the offset of the gyro
jah128	1:b067a08ff54e	975	int samples = 8;
jah128	1:b067a08ff54e	976	float gyro_values[samples];
jah128	1:b067a08ff54e	977	calibrate_led = 1;
jah128	1:b067a08ff54e	978	calibrate_ticker.attach(this,&PiSwarm::calibrate_ticker_routine,0.25);
jah128	11:5ebcb52726cf	979	for(int i=0; i<samples; i++) {
jah128	1:b067a08ff54e	980	gyro_values[i] = _gyro.read();
jah128	1:b067a08ff54e	981	wait(0.12);
jah128	1:b067a08ff54e	982	}
jah128	1:b067a08ff54e	983	char pass = 1;
jah128	1:b067a08ff54e	984	float mean = 0;
jah128	1:b067a08ff54e	985	float min = 3.5;
jah128	1:b067a08ff54e	986	float max = 3.2;
jah128	11:5ebcb52726cf	987	for(int i=0; i<samples; i++) {
jah128	1:b067a08ff54e	988	if((gyro_values[i] * 3.5 < 1.5) \|\| (gyro_values[i] * 3.2 > 1.5)) pass = 0; // Calibration failed!
jah128	1:b067a08ff54e	989	float test_value = 1.50 / gyro_values[i];
jah128	1:b067a08ff54e	990	mean += test_value;
jah128	1:b067a08ff54e	991	if (test_value > max) max = test_value;
jah128	1:b067a08ff54e	992	if (test_value < min) min = test_value;
jah128	1:b067a08ff54e	993	}
jah128	11:5ebcb52726cf	994	if(pass == 1) {
jah128	1:b067a08ff54e	995	gyro_cal = mean / samples;
jah128	1:b067a08ff54e	996	gyro_error = max - min;
jah128	1:b067a08ff54e	997	calibrate_ticker.detach();
jah128	1:b067a08ff54e	998	calibrate_led = 0;
jah128	1:b067a08ff54e	999	}
jah128	1:b067a08ff54e	1000	return pass;
jah128	11:5ebcb52726cf	1001	}
jah128	1:b067a08ff54e	1002
jah128	11:5ebcb52726cf	1003	char PiSwarm::calibrate_accelerometer ( void )
jah128	11:5ebcb52726cf	1004	{
jah128	1:b067a08ff54e	1005	//This routine attempts to calibrate the offset and multiplier of the accelerometer
jah128	1:b067a08ff54e	1006	int samples = 8;
jah128	1:b067a08ff54e	1007	float acc_x_values[samples];
jah128	1:b067a08ff54e	1008	float acc_y_values[samples];
jah128	1:b067a08ff54e	1009	float acc_z_values[samples];
jah128	1:b067a08ff54e	1010	calibrate_led = 1;
jah128	1:b067a08ff54e	1011	calibrate_ticker.attach(this,&PiSwarm::calibrate_ticker_routine,0.25);
jah128	11:5ebcb52726cf	1012	for(int i=0; i<samples; i++) {
jah128	1:b067a08ff54e	1013	acc_x_values[i] = _accel_x.read();
jah128	1:b067a08ff54e	1014	acc_y_values[i] = _accel_y.read();
jah128	1:b067a08ff54e	1015	acc_z_values[i] = _accel_z.read();
jah128	1:b067a08ff54e	1016	wait(0.12);
jah128	1:b067a08ff54e	1017	}
jah128	1:b067a08ff54e	1018	char pass = 1;
jah128	1:b067a08ff54e	1019	float x_mean = 0, y_mean=0, z_mean=0;
jah128	1:b067a08ff54e	1020	float x_min = 3600, y_min=3600, z_min=3600;
jah128	1:b067a08ff54e	1021	float x_max = 3100, y_max=3100, z_max=3100;
jah128	11:5ebcb52726cf	1022	for(int i=0; i<samples; i++) {
jah128	1:b067a08ff54e	1023	if((acc_x_values[i] * 3600 < 1250) \|\| (acc_x_values[i] * 3100 > 1250)) pass = 0; // Calibration failed!
jah128	1:b067a08ff54e	1024	if((acc_y_values[i] * 3600 < 1250) \|\| (acc_y_values[i] * 3100 > 1250)) pass = 0; // Calibration failed!
jah128	1:b067a08ff54e	1025	if((acc_z_values[i] * 3600 < 887) \|\| (acc_z_values[i] * 3100 > 887)) pass = 0; // Calibration failed!
jah128	11:5ebcb52726cf	1026
jah128	1:b067a08ff54e	1027	float x_test_value = 1250 / acc_x_values[i];
jah128	1:b067a08ff54e	1028	x_mean += x_test_value;
jah128	1:b067a08ff54e	1029	if (x_test_value > x_max) x_max = x_test_value;
jah128	1:b067a08ff54e	1030	if (x_test_value < x_min) x_min = x_test_value;
jah128	1:b067a08ff54e	1031	float y_test_value = 1250 / acc_y_values[i];
jah128	1:b067a08ff54e	1032	y_mean += y_test_value;
jah128	1:b067a08ff54e	1033	if (y_test_value > y_max) y_max = y_test_value;
jah128	11:5ebcb52726cf	1034	if (y_test_value < y_min) y_min = y_test_value;
jah128	1:b067a08ff54e	1035	float z_test_value = 887 / acc_z_values[i];
jah128	1:b067a08ff54e	1036	z_mean += z_test_value;
jah128	1:b067a08ff54e	1037	if (z_test_value > z_max) z_max = z_test_value;
jah128	11:5ebcb52726cf	1038	if (z_test_value < z_min) z_min = z_test_value;
jah128	1:b067a08ff54e	1039	}
jah128	11:5ebcb52726cf	1040	if(pass == 1) {
jah128	1:b067a08ff54e	1041	acc_x_cal = x_mean / samples;
jah128	1:b067a08ff54e	1042	acc_y_cal = y_mean / samples;
jah128	1:b067a08ff54e	1043	acc_z_cal = z_mean / samples;
jah128	1:b067a08ff54e	1044	calibrate_ticker.detach();
jah128	1:b067a08ff54e	1045	calibrate_led = 0;
jah128	1:b067a08ff54e	1046	}
jah128	1:b067a08ff54e	1047	return pass;
jah128	11:5ebcb52726cf	1048	}
jah128	1:b067a08ff54e	1049
jah128	11:5ebcb52726cf	1050	char PiSwarm::calibrate_magnetometer ( void )
jah128	11:5ebcb52726cf	1051	{
jah128	1:b067a08ff54e	1052	char command_data[2];
jah128	1:b067a08ff54e	1053	command_data[0] = 0x11; //Write to CTRL_REG2
jah128	1:b067a08ff54e	1054	command_data[1] = 0x80; // Enable auto resets
jah128	1:b067a08ff54e	1055	_i2c.write(MAGNETOMETER_ADDRESS, command_data, 2, false);
jah128	1:b067a08ff54e	1056	command_data[0] = 0x10; //Write to CTRL_REG1
jah128	11:5ebcb52726cf	1057	command_data[1] = 0x29; // 001 01 0 0 1 [40HZ 32-OS NOT-FAST NORMAL ACTIVE MODE]
jah128	1:b067a08ff54e	1058	return _i2c.write(MAGNETOMETER_ADDRESS, command_data, 2, false);
jah128	1:b067a08ff54e	1059	}
jah128	1:b067a08ff54e	1060
jah128	1:b067a08ff54e	1061
jah128	1:b067a08ff54e	1062
jah128	1:b067a08ff54e	1063
jah128	1:b067a08ff54e	1064
jah128	11:5ebcb52726cf	1065	void PiSwarm::interrupt_timeout_event ( void )
jah128	11:5ebcb52726cf	1066	{
jah128	1:b067a08ff54e	1067	//Read switches
jah128	1:b067a08ff54e	1068	char data [1];
jah128	1:b067a08ff54e	1069	data [0] = 0x80; //Read input registers
jah128	1:b067a08ff54e	1070	char read_data [5];
jah128	1:b067a08ff54e	1071	_i2c.write(EXPANSION_IC_ADDRESS, data, 1, false);
jah128	1:b067a08ff54e	1072	_i2c.read(EXPANSION_IC_ADDRESS, read_data, 5, false);
jah128	1:b067a08ff54e	1073	switches = (read_data[1] & 0x7C) >> 2;
jah128	1:b067a08ff54e	1074	led_timeout.attach(this,&PiSwarm::led_timeout_event, 0.1); // Switch off the LED after 0.1s
jah128	1:b067a08ff54e	1075	}
jah128	1:b067a08ff54e	1076
jah128	11:5ebcb52726cf	1077	void PiSwarm::led_timeout_event ( void )
jah128	11:5ebcb52726cf	1078	{
jah128	1:b067a08ff54e	1079	interrupt_led = 0;
jah128	1:b067a08ff54e	1080	}
jah128	1:b067a08ff54e	1081
jah128	11:5ebcb52726cf	1082	void PiSwarm::expansion_interrupt_handler ( void )
jah128	11:5ebcb52726cf	1083	{
jah128	1:b067a08ff54e	1084	interrupt_led = 1;
jah128	1:b067a08ff54e	1085	debounce_timeout.attach_us(this,&PiSwarm::interrupt_timeout_event, 200); // Read the switches only after 200uS have passed to debounce
jah128	1:b067a08ff54e	1086	debounce_timeout.add_function(&switch_pressed); // Call the switch pressed routine (in main) when done
jah128	1:b067a08ff54e	1087	}
jah128	1:b067a08ff54e	1088
jah128	1:b067a08ff54e	1089
jah128	11:5ebcb52726cf	1090
jah128	11:5ebcb52726cf	1091
jah128	1:b067a08ff54e	1092
jah128	11:5ebcb52726cf	1093	void PiSwarm::calibrate_ticker_routine ( void )
jah128	11:5ebcb52726cf	1094	{
jah128	1:b067a08ff54e	1095	calibrate_led_state = 1 - calibrate_led_state;
jah128	1:b067a08ff54e	1096	calibrate_led = calibrate_led_state;
jah128	1:b067a08ff54e	1097	}
jah128	1:b067a08ff54e	1098
jah128	11:5ebcb52726cf	1099	void PiSwarm::test_oled()
jah128	11:5ebcb52726cf	1100	{
jah128	1:b067a08ff54e	1101	enable_led_ldo = 1;
jah128	1:b067a08ff54e	1102	enable_ldo_outputs();
jah128	1:b067a08ff54e	1103	set_oled_colour(100,100,100);
jah128	1:b067a08ff54e	1104	char data[2];
jah128	1:b067a08ff54e	1105	data[0] = 0x09; //Address for PCA9505 Output Port 1
jah128	1:b067a08ff54e	1106	data[1] = 3;
jah128	1:b067a08ff54e	1107	_i2c.write(EXPANSION_IC_ADDRESS,data,2,false);
jah128	1:b067a08ff54e	1108	}
jah128	0:9ffe8ebd1c40	1109
jah128	11:5ebcb52726cf	1110	void PiSwarm::reset ()
jah128	11:5ebcb52726cf	1111	{
jah128	11:5ebcb52726cf	1112	// _nrst = 0;
jah128	0:9ffe8ebd1c40	1113	wait (0.01);
jah128	11:5ebcb52726cf	1114	// _nrst = 1;
jah128	0:9ffe8ebd1c40	1115	wait (0.1);
jah128	0:9ffe8ebd1c40	1116	}
jah128	11:5ebcb52726cf	1117	void PiSwarm::motor (int motor, float speed)
jah128	11:5ebcb52726cf	1118	{
jah128	0:9ffe8ebd1c40	1119	char opcode = 0x0;
jah128	0:9ffe8ebd1c40	1120	if (speed > 0.0) {
jah128	0:9ffe8ebd1c40	1121	if (motor==1)
jah128	0:9ffe8ebd1c40	1122	opcode = M1_FORWARD;
jah128	0:9ffe8ebd1c40	1123	else
jah128	0:9ffe8ebd1c40	1124	opcode = M2_FORWARD;
jah128	0:9ffe8ebd1c40	1125	} else {
jah128	0:9ffe8ebd1c40	1126	if (motor==1)
jah128	0:9ffe8ebd1c40	1127	opcode = M1_BACKWARD;
jah128	0:9ffe8ebd1c40	1128	else
jah128	0:9ffe8ebd1c40	1129	opcode = M2_BACKWARD;
jah128	0:9ffe8ebd1c40	1130	}
jah128	0:9ffe8ebd1c40	1131	if(motor==1)right_speed = speed;
jah128	0:9ffe8ebd1c40	1132	else left_speed = speed;
jah128	0:9ffe8ebd1c40	1133	unsigned char arg = 0x7f * abs(speed);
jah128	0:9ffe8ebd1c40	1134
jah128	0:9ffe8ebd1c40	1135	_ser.putc(opcode);
jah128	0:9ffe8ebd1c40	1136	_ser.putc(arg);
jah128	0:9ffe8ebd1c40	1137	}
jah128	0:9ffe8ebd1c40	1138
jah128	0:9ffe8ebd1c40	1139
jah128	11:5ebcb52726cf	1140	float PiSwarm::line_position()
jah128	11:5ebcb52726cf	1141	{
jah128	0:9ffe8ebd1c40	1142	int pos = 0;
jah128	0:9ffe8ebd1c40	1143	_ser.putc(SEND_LINE_POSITION);
jah128	0:9ffe8ebd1c40	1144	pos = _ser.getc();
jah128	0:9ffe8ebd1c40	1145	pos += _ser.getc() << 8;
jah128	11:5ebcb52726cf	1146
jah128	0:9ffe8ebd1c40	1147	float fpos = ((float)pos - 2048.0)/2048.0;
jah128	0:9ffe8ebd1c40	1148	return(fpos);
jah128	0:9ffe8ebd1c40	1149	}
jah128	0:9ffe8ebd1c40	1150
jah128	11:5ebcb52726cf	1151	char PiSwarm::sensor_auto_calibrate()
jah128	11:5ebcb52726cf	1152	{
jah128	0:9ffe8ebd1c40	1153	_ser.putc(AUTO_CALIBRATE);
jah128	0:9ffe8ebd1c40	1154	return(_ser.getc());
jah128	0:9ffe8ebd1c40	1155	}
jah128	0:9ffe8ebd1c40	1156
jah128	0:9ffe8ebd1c40	1157
jah128	11:5ebcb52726cf	1158	void PiSwarm::calibrate(void)
jah128	11:5ebcb52726cf	1159	{
jah128	0:9ffe8ebd1c40	1160	_ser.putc(PI_CALIBRATE);
jah128	0:9ffe8ebd1c40	1161	}
jah128	0:9ffe8ebd1c40	1162
jah128	11:5ebcb52726cf	1163	void PiSwarm::reset_calibration()
jah128	11:5ebcb52726cf	1164	{
jah128	0:9ffe8ebd1c40	1165	_ser.putc(LINE_SENSORS_RESET_CALIBRATION);
jah128	0:9ffe8ebd1c40	1166	}
jah128	0:9ffe8ebd1c40	1167
jah128	11:5ebcb52726cf	1168	void PiSwarm::PID_start(int max_speed, int a, int b, int c, int d)
jah128	11:5ebcb52726cf	1169	{
jah128	0:9ffe8ebd1c40	1170	_ser.putc(max_speed);
jah128	0:9ffe8ebd1c40	1171	_ser.putc(a);
jah128	0:9ffe8ebd1c40	1172	_ser.putc(b);
jah128	0:9ffe8ebd1c40	1173	_ser.putc(c);
jah128	0:9ffe8ebd1c40	1174	_ser.putc(d);
jah128	0:9ffe8ebd1c40	1175	}
jah128	0:9ffe8ebd1c40	1176
jah128	11:5ebcb52726cf	1177	void PiSwarm::PID_stop()
jah128	11:5ebcb52726cf	1178	{
jah128	0:9ffe8ebd1c40	1179	_ser.putc(STOP_PID);
jah128	0:9ffe8ebd1c40	1180	}
jah128	0:9ffe8ebd1c40	1181
jah128	0:9ffe8ebd1c40	1182
jah128	0:9ffe8ebd1c40	1183
jah128	11:5ebcb52726cf	1184	int PiSwarm::print (char* text, int length)
jah128	11:5ebcb52726cf	1185	{
jah128	11:5ebcb52726cf	1186	_ser.putc(DO_PRINT);
jah128	11:5ebcb52726cf	1187	_ser.putc(length);
jah128	0:9ffe8ebd1c40	1188	for (int i = 0 ; i < length ; i++) {
jah128	11:5ebcb52726cf	1189	_ser.putc(text[i]);
jah128	0:9ffe8ebd1c40	1190	}
jah128	0:9ffe8ebd1c40	1191	return(0);
jah128	0:9ffe8ebd1c40	1192	}
jah128	0:9ffe8ebd1c40	1193
jah128	11:5ebcb52726cf	1194	int PiSwarm::_putc (int c)
jah128	11:5ebcb52726cf	1195	{
jah128	11:5ebcb52726cf	1196	_ser.putc(DO_PRINT);
jah128	11:5ebcb52726cf	1197	_ser.putc(0x1);
jah128	11:5ebcb52726cf	1198	_ser.putc(c);
jah128	0:9ffe8ebd1c40	1199	wait (0.001);
jah128	0:9ffe8ebd1c40	1200	return(c);
jah128	0:9ffe8ebd1c40	1201	}
jah128	0:9ffe8ebd1c40	1202
jah128	11:5ebcb52726cf	1203	int PiSwarm::_getc (void)
jah128	11:5ebcb52726cf	1204	{
jah128	0:9ffe8ebd1c40	1205	char r = 0;
jah128	0:9ffe8ebd1c40	1206	return(r);
jah128	0:9ffe8ebd1c40	1207	}
jah128	0:9ffe8ebd1c40	1208
jah128	11:5ebcb52726cf	1209	int PiSwarm::putc (int c)
jah128	11:5ebcb52726cf	1210	{
jah128	0:9ffe8ebd1c40	1211	return(_ser.putc(c));
jah128	0:9ffe8ebd1c40	1212	}
jah128	0:9ffe8ebd1c40	1213
jah128	11:5ebcb52726cf	1214	int PiSwarm::getc (void)
jah128	11:5ebcb52726cf	1215	{
jah128	0:9ffe8ebd1c40	1216	return(_ser.getc());
jah128	0:9ffe8ebd1c40	1217	}
jah128	0:9ffe8ebd1c40	1218
jah128	11:5ebcb52726cf	1219	void PiSwarm::start_system_timer(void)
jah128	11:5ebcb52726cf	1220	{
jah128	0:9ffe8ebd1c40	1221	_system_timer.reset();
jah128	0:9ffe8ebd1c40	1222	_system_timer.start();
jah128	0:9ffe8ebd1c40	1223	}
jah128	0:9ffe8ebd1c40	1224
jah128	0:9ffe8ebd1c40	1225
jah128	0:9ffe8ebd1c40	1226	#ifdef MBED_RPC
jah128	11:5ebcb52726cf	1227	const rpc_method *PiSwarm::get_rpc_methods()
jah128	11:5ebcb52726cf	1228	{
jah128	0:9ffe8ebd1c40	1229	static const rpc_method rpc_methods[] = {{ "forward", rpc_method_caller<PiSwarm, float, &PiSwarm::forward> },
jah128	0:9ffe8ebd1c40	1230	{ "backward", rpc_method_caller<PiSwarm, float, &PiSwarm::backward> },
jah128	0:9ffe8ebd1c40	1231	{ "left", rpc_method_caller<PiSwarm, float, &PiSwarm::left> },
jah128	0:9ffe8ebd1c40	1232	{ "right", rpc_method_caller<PiSwarm, float, &PiSwarm::right> },
jah128	0:9ffe8ebd1c40	1233	{ "stop", rpc_method_caller<PiSwarm, &PiSwarm::stop> },
jah128	0:9ffe8ebd1c40	1234	{ "left_motor", rpc_method_caller<PiSwarm, float, &PiSwarm::left_motor> },
jah128	0:9ffe8ebd1c40	1235	{ "right_motor", rpc_method_caller<PiSwarm, float, &PiSwarm::right_motor> },
jah128	0:9ffe8ebd1c40	1236	{ "battery", rpc_method_caller<float, PiSwarm, &PiSwarm::battery> },
jah128	0:9ffe8ebd1c40	1237	{ "line_position", rpc_method_caller<float, PiSwarm, &PiSwarm::line_position> },
jah128	0:9ffe8ebd1c40	1238	{ "sensor_auto_calibrate", rpc_method_caller<char, PiSwarm, &PiSwarm::sensor_auto_calibrate> },
jah128	0:9ffe8ebd1c40	1239	RPC_METHOD_SUPER(Base)
jah128	0:9ffe8ebd1c40	1240	};
jah128	0:9ffe8ebd1c40	1241	return rpc_methods;
jah128	0:9ffe8ebd1c40	1242	}
jah128	0:9ffe8ebd1c40	1243	#endif
jah128	0:9ffe8ebd1c40	1244
jah128	11:5ebcb52726cf	1245	void display_system_time()
jah128	11:5ebcb52726cf	1246	{
jah128	11:5ebcb52726cf	1247	if(PISWARM_DEBUG == 1) {
jah128	0:9ffe8ebd1c40	1248	time_t system_time = time(NULL);
jah128	11:5ebcb52726cf	1249	printf("Current system time:%s Uptime:%.1f\n",ctime(&system_time),piswarm.get_uptime());
jah128	11:5ebcb52726cf	1250	}
jah128	11:5ebcb52726cf	1251	}
jah128	11:5ebcb52726cf	1252
jah128	11:5ebcb52726cf	1253	void oled_bearing_strobe()
jah128	11:5ebcb52726cf	1254	{
jah128	11:5ebcb52726cf	1255	bearing_strobe_step ++;
jah128	11:5ebcb52726cf	1256	if(bearing_strobe_step > 10) {
jah128	11:5ebcb52726cf	1257	oled_ticker.detach();
jah128	11:5ebcb52726cf	1258	piswarm.restore_oled_state();
jah128	11:5ebcb52726cf	1259	} else {
jah128	11:5ebcb52726cf	1260	float position = (360 - bearing) / 36;
jah128	11:5ebcb52726cf	1261	position -= 1;
jah128	11:5ebcb52726cf	1262	if(position < 0) position += 10;
jah128	11:5ebcb52726cf	1263	piswarm.set_oleds(0,0,0,0,0,0,0,0,0,0);
jah128	11:5ebcb52726cf	1264	int fposition = floor(position);
jah128	11:5ebcb52726cf	1265	int sposition = ceil(position);
jah128	11:5ebcb52726cf	1266	if(sposition > 9) sposition = 0;
jah128	11:5ebcb52726cf	1267	switch(bearing_strobe_step % 5) {
jah128	11:5ebcb52726cf	1268	case 4:
jah128	11:5ebcb52726cf	1269	piswarm.set_oled_colour(255,0,0);
jah128	11:5ebcb52726cf	1270	if(sposition - position > 0.5) {
jah128	11:5ebcb52726cf	1271	piswarm.set_oled(fposition,1);
jah128	11:5ebcb52726cf	1272	} else piswarm.set_oled(sposition,1);
jah128	11:5ebcb52726cf	1273	break;
jah128	11:5ebcb52726cf	1274	case 3:
jah128	11:5ebcb52726cf	1275	piswarm.set_oled_colour(155,30,0);
jah128	11:5ebcb52726cf	1276	if(sposition - position > 0.5) {
jah128	11:5ebcb52726cf	1277	piswarm.set_oled(fposition,1);
jah128	11:5ebcb52726cf	1278	} else piswarm.set_oled(sposition,1);
jah128	11:5ebcb52726cf	1279	break;
jah128	11:5ebcb52726cf	1280	case 1:
jah128	11:5ebcb52726cf	1281	case 2:
jah128	11:5ebcb52726cf	1282	piswarm.set_oled_colour(100,25,0);
jah128	11:5ebcb52726cf	1283	piswarm.set_oled(fposition,1);
jah128	11:5ebcb52726cf	1284	piswarm.set_oled(sposition,1);
jah128	11:5ebcb52726cf	1285	break;
jah128	11:5ebcb52726cf	1286	case 0:
jah128	11:5ebcb52726cf	1287	piswarm.set_oled_colour(60,10,0);
jah128	11:5ebcb52726cf	1288	piswarm.set_oled(fposition,1);
jah128	11:5ebcb52726cf	1289	piswarm.set_oled(sposition,1);
jah128	11:5ebcb52726cf	1290	fposition --;
jah128	11:5ebcb52726cf	1291	if(fposition<0)fposition = 9;
jah128	11:5ebcb52726cf	1292	sposition ++;
jah128	11:5ebcb52726cf	1293	if(sposition>9)sposition = 0;
jah128	11:5ebcb52726cf	1294	piswarm.set_oled(fposition,1);
jah128	11:5ebcb52726cf	1295	piswarm.set_oled(sposition,1);
jah128	11:5ebcb52726cf	1296	break;
jah128	11:5ebcb52726cf	1297	}
jah128	11:5ebcb52726cf	1298	piswarm.activate_oleds();
jah128	11:5ebcb52726cf	1299	}
jah128	11:5ebcb52726cf	1300	}
jah128	11:5ebcb52726cf	1301
jah128	11:5ebcb52726cf	1302	void oled_ticker_update()
jah128	11:5ebcb52726cf	1303	{
jah128	11:5ebcb52726cf	1304	oled_ticker_step++;
jah128	11:5ebcb52726cf	1305	if(oled_ticker_step == 10) oled_ticker_step = 0;
jah128	11:5ebcb52726cf	1306	switch(oled_ticker_step) {
jah128	11:5ebcb52726cf	1307	case 0:
jah128	11:5ebcb52726cf	1308	piswarm.set_oleds(1,1,0,0,0,0,0,0,0,0);
jah128	11:5ebcb52726cf	1309	break;
jah128	11:5ebcb52726cf	1310	case 1:
jah128	11:5ebcb52726cf	1311	piswarm.set_oleds(0,1,1,0,0,0,0,0,0,0);
jah128	11:5ebcb52726cf	1312	break;
jah128	11:5ebcb52726cf	1313	case 2:
jah128	11:5ebcb52726cf	1314	piswarm.set_oleds(0,0,1,1,0,0,0,0,0,0);
jah128	11:5ebcb52726cf	1315	break;
jah128	11:5ebcb52726cf	1316	case 3:
jah128	11:5ebcb52726cf	1317	piswarm.set_oleds(0,0,0,1,1,0,0,0,0,0);
jah128	11:5ebcb52726cf	1318	break;
jah128	11:5ebcb52726cf	1319	case 4:
jah128	11:5ebcb52726cf	1320	piswarm.set_oleds(0,0,0,0,1,1,0,0,0,0);
jah128	11:5ebcb52726cf	1321	break;
jah128	11:5ebcb52726cf	1322	case 5:
jah128	11:5ebcb52726cf	1323	piswarm.set_oleds(0,0,0,0,0,1,1,0,0,0);
jah128	11:5ebcb52726cf	1324	break;
jah128	11:5ebcb52726cf	1325	case 6:
jah128	11:5ebcb52726cf	1326	piswarm.set_oleds(0,0,0,0,0,0,1,1,0,0);
jah128	11:5ebcb52726cf	1327	break;
jah128	11:5ebcb52726cf	1328	case 7:
jah128	11:5ebcb52726cf	1329	piswarm.set_oleds(0,0,0,0,0,0,0,1,1,0);
jah128	11:5ebcb52726cf	1330	break;
jah128	11:5ebcb52726cf	1331	case 8:
jah128	11:5ebcb52726cf	1332	piswarm.set_oleds(0,0,0,0,0,0,0,0,1,1);
jah128	11:5ebcb52726cf	1333	break;
jah128	11:5ebcb52726cf	1334	case 9:
jah128	11:5ebcb52726cf	1335	piswarm.set_oleds(1,0,0,0,0,0,0,0,0,1);
jah128	11:5ebcb52726cf	1336	break;
jah128	11:5ebcb52726cf	1337
jah128	0:9ffe8ebd1c40	1338	}
jah128	0:9ffe8ebd1c40	1339	}
jah128	0:9ffe8ebd1c40	1340
jah128	11:5ebcb52726cf	1341	float synchronise_with_beacon()
jah128	11:5ebcb52726cf	1342	{
jah128	11:5ebcb52726cf	1343	beacon_led = 1;
jah128	11:5ebcb52726cf	1344	int offset_time = 0;
jah128	11:5ebcb52726cf	1345	float confidence = -1;
jah128	11:5ebcb52726cf	1346	float angle_confidence = 0;
jah128	11:5ebcb52726cf	1347	float heading = 0;
jah128	11:5ebcb52726cf	1348	float i_bearing = 0;
jah128	11:5ebcb52726cf	1349	Timer ir_timer;
jah128	11:5ebcb52726cf	1350	unsigned short ir_values[8][100];
jah128	11:5ebcb52726cf	1351	unsigned int ir_totals[8];
jah128	11:5ebcb52726cf	1352	ir_timer.start();
jah128	11:5ebcb52726cf	1353	int target_time = 0;
jah128	11:5ebcb52726cf	1354	for(int i=0; i<8; i++) {
jah128	11:5ebcb52726cf	1355	ir_totals[i]=0;
jah128	11:5ebcb52726cf	1356	}
jah128	11:5ebcb52726cf	1357	for(int step = 0; step < 100; step ++) {
jah128	11:5ebcb52726cf	1358	target_time += 10000 ;
jah128	11:5ebcb52726cf	1359	piswarm.store_background_raw_ir_values ();
jah128	11:5ebcb52726cf	1360	for(int sensor = 0; sensor < 8; sensor ++) {
jah128	11:5ebcb52726cf	1361	ir_values[sensor][step] = piswarm.get_background_raw_ir_value(sensor);
jah128	11:5ebcb52726cf	1362	ir_totals[sensor] += ir_values[sensor][step];
jah128	11:5ebcb52726cf	1363	}
jah128	11:5ebcb52726cf	1364	while(ir_timer.read_us() < target_time) wait_us(50);
jah128	11:5ebcb52726cf	1365	}
jah128	11:5ebcb52726cf	1366	int total = 0;
jah128	11:5ebcb52726cf	1367	for(int i=0; i<8; i++) {
jah128	11:5ebcb52726cf	1368	total += ir_totals[i];
jah128	11:5ebcb52726cf	1369	}
jah128	11:5ebcb52726cf	1370	total /= 800;
jah128	11:5ebcb52726cf	1371	unsigned short s_pass [100];
jah128	11:5ebcb52726cf	1372	int pass_count = 0;
jah128	11:5ebcb52726cf	1373	for(int i=0; i<100; i++) {
jah128	11:5ebcb52726cf	1374	signed short adjusted [8];
jah128	11:5ebcb52726cf	1375	int pass = 0;
jah128	11:5ebcb52726cf	1376	for(int k=0; k<8; k++) {
jah128	11:5ebcb52726cf	1377	adjusted[k]=ir_values[k][i]-total;
jah128	11:5ebcb52726cf	1378	if(adjusted[k] > 0) pass++;
jah128	11:5ebcb52726cf	1379	}
jah128	11:5ebcb52726cf	1380	s_pass [i] = pass;
jah128	11:5ebcb52726cf	1381	if(pass>0) pass_count++;
jah128	11:5ebcb52726cf	1382	}
jah128	11:5ebcb52726cf	1383	if(pass_count < 2) {
jah128	11:5ebcb52726cf	1384	confidence = -1;
jah128	11:5ebcb52726cf	1385	} else {
jah128	11:5ebcb52726cf	1386	//Find highest value
jah128	11:5ebcb52726cf	1387	int highest = 0;
jah128	11:5ebcb52726cf	1388	int highest_pos = 0;
jah128	11:5ebcb52726cf	1389	for(int k=0; k<100; k++) {
jah128	11:5ebcb52726cf	1390	if(s_pass[k] > highest) {
jah128	11:5ebcb52726cf	1391	highest = s_pass[k];
jah128	11:5ebcb52726cf	1392	highest_pos = k;
jah128	11:5ebcb52726cf	1393	}
jah128	11:5ebcb52726cf	1394	}
jah128	11:5ebcb52726cf	1395	int length = 1;
jah128	11:5ebcb52726cf	1396	//Adjust to find start: best length should be 2 or 3: compare previous value to
jah128	11:5ebcb52726cf	1397	int pre = highest_pos -1;
jah128	11:5ebcb52726cf	1398	if(pre < 0) pre=99;
jah128	11:5ebcb52726cf	1399	int spost = highest_pos+1;
jah128	11:5ebcb52726cf	1400	int post = highest_pos +2;
jah128	11:5ebcb52726cf	1401	if(spost > 99) spost-=100;
jah128	11:5ebcb52726cf	1402	if(post > 99) post-=100;
jah128	11:5ebcb52726cf	1403	if(s_pass[pre] > 0 && s_pass[pre] >= s_pass[post]) {
jah128	11:5ebcb52726cf	1404	highest_pos = pre;
jah128	11:5ebcb52726cf	1405	length++;
jah128	11:5ebcb52726cf	1406	}
jah128	11:5ebcb52726cf	1407	if(s_pass[spost] > 0)length++;
jah128	11:5ebcb52726cf	1408	if(s_pass[post] > s_pass[spost] && s_pass[spost] > 0)length++;
jah128	11:5ebcb52726cf	1409	if(length>3) confidence = 0;
jah128	11:5ebcb52726cf	1410	else if(length==1) confidence = 0;
jah128	11:5ebcb52726cf	1411	else if(length==pass_count) confidence = 1;
jah128	11:5ebcb52726cf	1412	else confidence = (float) length / (float) pass_count;
jah128	11:5ebcb52726cf	1413	offset_time = highest_pos * 10;
jah128	11:5ebcb52726cf	1414	if(length == 2) offset_time -= 5;
jah128	11:5ebcb52726cf	1415	if(offset_time < 0) offset_time += 1000;
jah128	11:5ebcb52726cf	1416	if(confidence > 0) {
jah128	11:5ebcb52726cf	1417	unsigned short sums[8];
jah128	11:5ebcb52726cf	1418	for(int i=0; i<8; i++) {
jah128	11:5ebcb52726cf	1419	sums[i] = 0;
jah128	11:5ebcb52726cf	1420	for(int l=0; l<length; l++) {
jah128	11:5ebcb52726cf	1421	int sp = highest_pos + l;
jah128	11:5ebcb52726cf	1422	if(sp>99)sp-=100;
jah128	11:5ebcb52726cf	1423	signed short adj = ir_values[i][sp];
jah128	11:5ebcb52726cf	1424	if(adj>0)sums[i]+=adj;
jah128	11:5ebcb52726cf	1425	}
jah128	11:5ebcb52726cf	1426	}
jah128	11:5ebcb52726cf	1427	int peak_sum = 0;
jah128	11:5ebcb52726cf	1428	int peak_position = 0;
jah128	11:5ebcb52726cf	1429	for(int i=0; i<8; i++) {
jah128	11:5ebcb52726cf	1430	if(sums[i]>peak_sum) {
jah128	11:5ebcb52726cf	1431	peak_sum = sums[i];
jah128	11:5ebcb52726cf	1432	peak_position = i;
jah128	11:5ebcb52726cf	1433	}
jah128	11:5ebcb52726cf	1434	}
jah128	11:5ebcb52726cf	1435	float relative_sums [8];
jah128	11:5ebcb52726cf	1436	float relative_total = 0;
jah128	11:5ebcb52726cf	1437	for(int i=0; i<8; i++) {
jah128	11:5ebcb52726cf	1438	relative_sums[i] = (float) sums[i] / (float) peak_sum;
jah128	11:5ebcb52726cf	1439	relative_total += relative_sums[i];
jah128	11:5ebcb52726cf	1440	}
jah128	11:5ebcb52726cf	1441	int previous = peak_position - 1;
jah128	11:5ebcb52726cf	1442	int d_prev = peak_position - 2;
jah128	11:5ebcb52726cf	1443	int d_next = peak_position + 2;
jah128	11:5ebcb52726cf	1444	if(d_prev < 0) d_prev += 8;
jah128	11:5ebcb52726cf	1445	if(d_next > 7) d_next -= 8;
jah128	11:5ebcb52726cf	1446	if(previous<0) previous+=8;
jah128	11:5ebcb52726cf	1447	int next = peak_position + 1;
jah128	11:5ebcb52726cf	1448	if(next > 7) next -= 8;
jah128	11:5ebcb52726cf	1449	angle_confidence = (relative_sums[next] + relative_sums[previous] + 1) / relative_total;
jah128	11:5ebcb52726cf	1450	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;
jah128	11:5ebcb52726cf	1451	if(heading<0) heading+= 8;
jah128	11:5ebcb52726cf	1452	if(heading>8) heading-= 8;
jah128	11:5ebcb52726cf	1453	//Convert heading to bearing
jah128	11:5ebcb52726cf	1454	int whole_part = (int) heading;
jah128	11:5ebcb52726cf	1455	float remainder = heading - whole_part;
jah128	11:5ebcb52726cf	1456	i_bearing = 0;
jah128	11:5ebcb52726cf	1457	switch(whole_part) {
jah128	11:5ebcb52726cf	1458	case 0:
jah128	11:5ebcb52726cf	1459	i_bearing = 15 + (30*remainder);
jah128	11:5ebcb52726cf	1460	break;
jah128	11:5ebcb52726cf	1461	case 1:
jah128	11:5ebcb52726cf	1462	i_bearing = 45 + (45*remainder);
jah128	11:5ebcb52726cf	1463	break;
jah128	11:5ebcb52726cf	1464	case 2:
jah128	11:5ebcb52726cf	1465	i_bearing = 90 + (62*remainder);
jah128	11:5ebcb52726cf	1466	break;
jah128	11:5ebcb52726cf	1467	case 3:
jah128	11:5ebcb52726cf	1468	i_bearing = 152 + (56*remainder);
jah128	11:5ebcb52726cf	1469	break;
jah128	11:5ebcb52726cf	1470	case 4:
jah128	11:5ebcb52726cf	1471	i_bearing = 208 + (62*remainder);
jah128	11:5ebcb52726cf	1472	break;
jah128	11:5ebcb52726cf	1473	case 5:
jah128	11:5ebcb52726cf	1474	i_bearing = 270 + (45 * remainder);
jah128	11:5ebcb52726cf	1475	break;
jah128	11:5ebcb52726cf	1476	case 6:
jah128	11:5ebcb52726cf	1477	i_bearing = 315 + (30 * remainder);
jah128	11:5ebcb52726cf	1478	break;
jah128	11:5ebcb52726cf	1479	case 7:
jah128	11:5ebcb52726cf	1480	i_bearing = 345 + (30 * remainder);
jah128	11:5ebcb52726cf	1481	break;
jah128	11:5ebcb52726cf	1482	}
jah128	11:5ebcb52726cf	1483	if(i_bearing>=360)i_bearing-=360;
jah128	11:5ebcb52726cf	1484	} else angle_confidence = 0;
jah128	11:5ebcb52726cf	1485	}
jah128	11:5ebcb52726cf	1486	if(angle_confidence > 0.4 && confidence > 0.4) {
jah128	11:5ebcb52726cf	1487	int delay = 1990+offset_time;
jah128	11:5ebcb52726cf	1488	if(delay > 2800) delay -= 1000;
jah128	11:5ebcb52726cf	1489	bearing = 360-i_bearing;
jah128	11:5ebcb52726cf	1490	is_synced = 1;
jah128	11:5ebcb52726cf	1491	saved_peak = total;
jah128	11:5ebcb52726cf	1492	while(ir_timer.read_ms() < delay) wait_us(50);
jah128	11:5ebcb52726cf	1493	} else is_synced = 0;
jah128	11:5ebcb52726cf	1494	beacon_led = 0;
jah128	11:5ebcb52726cf	1495	hit_sync_count = 0;
jah128	11:5ebcb52726cf	1496	miss_sync_count = 0;
jah128	11:5ebcb52726cf	1497	beacon_confidence = confidence;
jah128	11:5ebcb52726cf	1498	bearing_confidence = angle_confidence;
jah128	11:5ebcb52726cf	1499	return confidence;
jah128	11:5ebcb52726cf	1500	}
jah128	11:5ebcb52726cf	1501
jah128	11:5ebcb52726cf	1502
jah128	11:5ebcb52726cf	1503	float get_bearing_from_beacon()
jah128	11:5ebcb52726cf	1504	{
jah128	11:5ebcb52726cf	1505	beacon_led = 1;
jah128	11:5ebcb52726cf	1506	float confidence = -1;
jah128	11:5ebcb52726cf	1507	float angle_confidence = 0;
jah128	11:5ebcb52726cf	1508	float heading = 0;
jah128	11:5ebcb52726cf	1509	float i_bearing = 0;
jah128	11:5ebcb52726cf	1510	Timer ir_timer;
jah128	11:5ebcb52726cf	1511	unsigned short ir_values[8][5];
jah128	11:5ebcb52726cf	1512	unsigned int ir_totals[8];
jah128	11:5ebcb52726cf	1513	ir_timer.start();
jah128	11:5ebcb52726cf	1514	int target_time = 0;
jah128	11:5ebcb52726cf	1515	for(int i=0; i<8; i++) {
jah128	11:5ebcb52726cf	1516	ir_totals[i]=0;
jah128	11:5ebcb52726cf	1517	}
jah128	11:5ebcb52726cf	1518	for(int step = 0; step < 5; step ++) {
jah128	11:5ebcb52726cf	1519	target_time += 10000 ;
jah128	11:5ebcb52726cf	1520	piswarm.store_background_raw_ir_values ();
jah128	11:5ebcb52726cf	1521	for(int sensor = 0; sensor < 8; sensor ++) {
jah128	11:5ebcb52726cf	1522	ir_values[sensor][step] = piswarm.get_background_raw_ir_value(sensor);
jah128	11:5ebcb52726cf	1523	ir_totals[sensor] += ir_values[sensor][step];
jah128	11:5ebcb52726cf	1524	}
jah128	11:5ebcb52726cf	1525	while(ir_timer.read_us() < target_time) wait_us(50);
jah128	11:5ebcb52726cf	1526	}
jah128	11:5ebcb52726cf	1527	int total = 0;
jah128	11:5ebcb52726cf	1528	for(int i=0; i<8; i++) {
jah128	11:5ebcb52726cf	1529	total += ir_totals[i];
jah128	11:5ebcb52726cf	1530	}
jah128	11:5ebcb52726cf	1531	total /= 40;
jah128	11:5ebcb52726cf	1532	unsigned short s_pass [5];
jah128	11:5ebcb52726cf	1533	int pass_count = 0;
jah128	11:5ebcb52726cf	1534	for(int i=0; i<5; i++) {
jah128	11:5ebcb52726cf	1535	signed short adjusted [8];
jah128	11:5ebcb52726cf	1536	int pass = 0;
jah128	11:5ebcb52726cf	1537	for(int k=0; k<8; k++) {
jah128	11:5ebcb52726cf	1538	adjusted[k]=ir_values[k][i]-total;
jah128	11:5ebcb52726cf	1539	if(adjusted[k] > 0) pass++;
jah128	11:5ebcb52726cf	1540	}
jah128	11:5ebcb52726cf	1541	s_pass [i] = pass;
jah128	11:5ebcb52726cf	1542	if(pass>0) pass_count++;
jah128	11:5ebcb52726cf	1543	//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);
jah128	11:5ebcb52726cf	1544	}
jah128	11:5ebcb52726cf	1545	if(pass_count < 2 \|\| pass_count > 3 \|\| total<(saved_peak / 4)) {
jah128	11:5ebcb52726cf	1546	confidence = -1;
jah128	11:5ebcb52726cf	1547	angle_confidence = 0;
jah128	11:5ebcb52726cf	1548	} else {
jah128	11:5ebcb52726cf	1549	confidence = 1;
jah128	11:5ebcb52726cf	1550
jah128	11:5ebcb52726cf	1551	unsigned short sums[8];
jah128	11:5ebcb52726cf	1552	for(int i=0; i<8; i++) {
jah128	11:5ebcb52726cf	1553	sums[i] = 0;
jah128	11:5ebcb52726cf	1554	for(int l=1; l<4; l++) {
jah128	11:5ebcb52726cf	1555	signed short adj = ir_values[i][l] - total;
jah128	11:5ebcb52726cf	1556	if(adj>0)sums[i]+=adj;
jah128	11:5ebcb52726cf	1557	}
jah128	11:5ebcb52726cf	1558	}
jah128	11:5ebcb52726cf	1559	int peak_sum = 0;
jah128	11:5ebcb52726cf	1560	int peak_position = 0;
jah128	11:5ebcb52726cf	1561	for(int i=0; i<8; i++) {
jah128	11:5ebcb52726cf	1562	if(sums[i]>peak_sum) {
jah128	11:5ebcb52726cf	1563	peak_sum = sums[i];
jah128	11:5ebcb52726cf	1564	peak_position = i;
jah128	11:5ebcb52726cf	1565	}
jah128	11:5ebcb52726cf	1566	}
jah128	11:5ebcb52726cf	1567	float relative_sums [8];
jah128	11:5ebcb52726cf	1568	float relative_total = 0;
jah128	11:5ebcb52726cf	1569	for(int i=0; i<8; i++) {
jah128	11:5ebcb52726cf	1570	relative_sums[i] = (float) sums[i] / (float) peak_sum;
jah128	11:5ebcb52726cf	1571	relative_total += relative_sums[i];
jah128	11:5ebcb52726cf	1572	}
jah128	11:5ebcb52726cf	1573	int previous = peak_position - 1;
jah128	11:5ebcb52726cf	1574	int d_prev = peak_position - 2;
jah128	11:5ebcb52726cf	1575	int d_next = peak_position + 2;
jah128	11:5ebcb52726cf	1576	if(d_prev < 0) d_prev += 8;
jah128	11:5ebcb52726cf	1577	if(d_next > 7) d_next -= 8;
jah128	11:5ebcb52726cf	1578	if(previous<0) previous+=8;
jah128	11:5ebcb52726cf	1579	int next = peak_position + 1;
jah128	11:5ebcb52726cf	1580	if(next > 7) next -= 8;
jah128	11:5ebcb52726cf	1581	angle_confidence = (relative_sums[next] + relative_sums[previous] + 1) / relative_total;
jah128	11:5ebcb52726cf	1582	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;
jah128	11:5ebcb52726cf	1583	if(heading<0) heading+= 8;
jah128	11:5ebcb52726cf	1584	if(heading>8) heading-= 8;
jah128	11:5ebcb52726cf	1585	//Convert heading to bearing
jah128	11:5ebcb52726cf	1586	int whole_part = (int) heading;
jah128	11:5ebcb52726cf	1587	float remainder = heading - whole_part;
jah128	11:5ebcb52726cf	1588	i_bearing = 0;
jah128	11:5ebcb52726cf	1589	switch(whole_part) {
jah128	11:5ebcb52726cf	1590	case 0:
jah128	11:5ebcb52726cf	1591	i_bearing = 15 + (30*remainder);
jah128	11:5ebcb52726cf	1592	break;
jah128	11:5ebcb52726cf	1593	case 1:
jah128	11:5ebcb52726cf	1594	i_bearing = 45 + (45*remainder);
jah128	11:5ebcb52726cf	1595	break;
jah128	11:5ebcb52726cf	1596	case 2:
jah128	11:5ebcb52726cf	1597	i_bearing = 90 + (62*remainder);
jah128	11:5ebcb52726cf	1598	break;
jah128	11:5ebcb52726cf	1599	case 3:
jah128	11:5ebcb52726cf	1600	i_bearing = 152 + (56*remainder);
jah128	11:5ebcb52726cf	1601	break;
jah128	11:5ebcb52726cf	1602	case 4:
jah128	11:5ebcb52726cf	1603	i_bearing = 208 + (62*remainder);
jah128	11:5ebcb52726cf	1604	break;
jah128	11:5ebcb52726cf	1605	case 5:
jah128	11:5ebcb52726cf	1606	i_bearing = 270 + (45 * remainder);
jah128	11:5ebcb52726cf	1607	break;
jah128	11:5ebcb52726cf	1608	case 6:
jah128	11:5ebcb52726cf	1609	i_bearing = 315 + (30 * remainder);
jah128	11:5ebcb52726cf	1610	break;
jah128	11:5ebcb52726cf	1611	case 7:
jah128	11:5ebcb52726cf	1612	i_bearing = 345 + (30 * remainder);
jah128	11:5ebcb52726cf	1613	break;
jah128	11:5ebcb52726cf	1614	}
jah128	11:5ebcb52726cf	1615	if(i_bearing>=360)i_bearing-=360;
jah128	11:5ebcb52726cf	1616
jah128	11:5ebcb52726cf	1617	}
jah128	11:5ebcb52726cf	1618	if(angle_confidence > 0.4 && confidence==1) {
jah128	11:5ebcb52726cf	1619	bearing = 360-i_bearing;
jah128	11:5ebcb52726cf	1620	is_synced = 1;
jah128	11:5ebcb52726cf	1621	hit_sync_count ++;
jah128	11:5ebcb52726cf	1622	if(hit_sync_count > 4) {
jah128	11:5ebcb52726cf	1623	miss_sync_count = 0;
jah128	11:5ebcb52726cf	1624	}
jah128	11:5ebcb52726cf	1625	} else is_synced = 0;
jah128	11:5ebcb52726cf	1626	beacon_led = 0;
jah128	11:5ebcb52726cf	1627	beacon_confidence = confidence;
jah128	11:5ebcb52726cf	1628	bearing_confidence = angle_confidence;
jah128	11:5ebcb52726cf	1629	return confidence;
jah128	11:5ebcb52726cf	1630	}
jah128	11:5ebcb52726cf	1631
jah128	11:5ebcb52726cf	1632	void update_bearing()
jah128	11:5ebcb52726cf	1633	{
jah128	11:5ebcb52726cf	1634	get_bearing_from_beacon();
jah128	11:5ebcb52726cf	1635	if(is_synced == 1) {
jah128	11:5ebcb52726cf	1636	if(DISPLAY_BEACON_READING == 1) display_bearing_info();
jah128	11:5ebcb52726cf	1637	highlight_bearing();
jah128	11:5ebcb52726cf	1638	} else {
jah128	11:5ebcb52726cf	1639	miss_sync_count ++;
jah128	11:5ebcb52726cf	1640	hit_sync_count = 0;
jah128	11:5ebcb52726cf	1641	if(miss_sync_count > 3) {
jah128	11:5ebcb52726cf	1642	beacon_ticker.detach();
jah128	11:5ebcb52726cf	1643	if(DISPLAY_BEACON_READING == 1) {
jah128	11:5ebcb52726cf	1644	piswarm.cls();
jah128	11:5ebcb52726cf	1645	piswarm.printf("Lost ");
jah128	11:5ebcb52726cf	1646	piswarm.locate(0,1);
jah128	11:5ebcb52726cf	1647	piswarm.printf("Beacon ");
jah128	11:5ebcb52726cf	1648	}
jah128	11:5ebcb52726cf	1649	beacon_ticker.attach(resync_with_beacon,3.9);
jah128	11:5ebcb52726cf	1650	}
jah128	11:5ebcb52726cf	1651	oled_ticker.detach();
jah128	11:5ebcb52726cf	1652	if(DISPLAY_BEACON_READING)piswarm.cls();
jah128	11:5ebcb52726cf	1653	}
jah128	11:5ebcb52726cf	1654	}
jah128	11:5ebcb52726cf	1655
jah128	11:5ebcb52726cf	1656	void resync_with_beacon()
jah128	11:5ebcb52726cf	1657	{
jah128	11:5ebcb52726cf	1658	if(synchronise_with_beacon() > 0.4) {
jah128	11:5ebcb52726cf	1659	beacon_ticker.detach();
jah128	11:5ebcb52726cf	1660	beacon_ticker.attach(update_bearing,1);
jah128	11:5ebcb52726cf	1661	if(DISPLAY_BEACON_READING == 1) display_bearing_info();
jah128	11:5ebcb52726cf	1662	else {
jah128	11:5ebcb52726cf	1663	piswarm.cls();
jah128	11:5ebcb52726cf	1664	piswarm.printf("SYNCED");
jah128	11:5ebcb52726cf	1665	piswarm.locate(0,1);
jah128	11:5ebcb52726cf	1666	piswarm.printf("B:%3.1f",bearing);
jah128	11:5ebcb52726cf	1667	}
jah128	11:5ebcb52726cf	1668	highlight_bearing();
jah128	11:5ebcb52726cf	1669	} else {
jah128	11:5ebcb52726cf	1670	piswarm.cls();
jah128	11:5ebcb52726cf	1671	piswarm.printf("NO SYNC");
jah128	11:5ebcb52726cf	1672	piswarm.turn_off_all_oleds();
jah128	11:5ebcb52726cf	1673	oled_ticker.detach();
jah128	11:5ebcb52726cf	1674	}
jah128	11:5ebcb52726cf	1675	}
jah128	11:5ebcb52726cf	1676
jah128	11:5ebcb52726cf	1677	void highlight_bearing()
jah128	11:5ebcb52726cf	1678	{
jah128	11:5ebcb52726cf	1679	piswarm.save_oled_state();
jah128	11:5ebcb52726cf	1680	bearing_strobe_step = 0;
jah128	11:5ebcb52726cf	1681	piswarm.set_oled_colour(255,255,255);
jah128	11:5ebcb52726cf	1682	oled_ticker.detach();
jah128	11:5ebcb52726cf	1683	oled_ticker.attach(oled_bearing_strobe,0.02);
jah128	11:5ebcb52726cf	1684	}
jah128	11:5ebcb52726cf	1685
jah128	11:5ebcb52726cf	1686	void display_bearing_info()
jah128	11:5ebcb52726cf	1687	{
jah128	11:5ebcb52726cf	1688	piswarm.cls();
jah128	11:5ebcb52726cf	1689	piswarm.printf("B:%3.1f",bearing);
jah128	11:5ebcb52726cf	1690	piswarm.locate(0,1);
jah128	11:5ebcb52726cf	1691	piswarm.printf("C:%3.1f",bearing_confidence*100);
jah128	11:5ebcb52726cf	1692	}
jah128	11:5ebcb52726cf	1693
jah128	11:5ebcb52726cf	1694	void init()
jah128	11:5ebcb52726cf	1695	{
jah128	0:9ffe8ebd1c40	1696	//Standard initialisation routine for Pi Swarm Robot
jah128	0:9ffe8ebd1c40	1697	//Displays a message on the screen and flashes the central LED
jah128	0:9ffe8ebd1c40	1698	//Calibrates the gyro and accelerometer
jah128	0:9ffe8ebd1c40	1699	//Make sure robot is flat and stationary when this code is run (calibration starts after about .5 second to allow robot to settle)
jah128	0:9ffe8ebd1c40	1700	piswarm.start_system_timer();
jah128	0:9ffe8ebd1c40	1701	pc.baud(PC_BAUD);
jah128	11:5ebcb52726cf	1702	if(PISWARM_DEBUG == 1)pc.printf("PiSwarm Firmware 0.7 Initialisation...\n");
jah128	0:9ffe8ebd1c40	1703	display_system_time();
jah128	11:5ebcb52726cf	1704	piswarm.play_tune("T596MSCEFGAB>C",14);
jah128	0:9ffe8ebd1c40	1705	piswarm.cls();
jah128	0:9ffe8ebd1c40	1706	piswarm.enable_cled(1);
jah128	0:9ffe8ebd1c40	1707	piswarm.set_cled_colour(200,0,0);
jah128	0:9ffe8ebd1c40	1708	piswarm.set_oled_colour(200,0,0);
jah128	0:9ffe8ebd1c40	1709	piswarm.printf(" YORK ");
jah128	0:9ffe8ebd1c40	1710	piswarm.locate(0,1);
jah128	0:9ffe8ebd1c40	1711	piswarm.printf("ROBOTLAB");
jah128	11:5ebcb52726cf	1712	oled_ticker.attach(oled_ticker_update,0.02);
jah128	11:5ebcb52726cf	1713	wait(0.2);
jah128	0:9ffe8ebd1c40	1714	piswarm.set_cled_colour(0,200,0);
jah128	0:9ffe8ebd1c40	1715	piswarm.set_oled_colour(0,200,0);
jah128	0:9ffe8ebd1c40	1716	piswarm.cls();
jah128	0:9ffe8ebd1c40	1717	piswarm.printf("Pi Swarm");
jah128	0:9ffe8ebd1c40	1718	piswarm.locate(0,1);
jah128	0:9ffe8ebd1c40	1719	piswarm.printf("ID : %d ",piswarm.get_id());
jah128	11:5ebcb52726cf	1720	wait(0.2);
jah128	11:5ebcb52726cf	1721	piswarm.set_cled_colour(0,0,200);
jah128	11:5ebcb52726cf	1722	piswarm.set_oled_colour(0,0,200);
jah128	11:5ebcb52726cf	1723	if(CALIBRATE_MEMS == 1) {
jah128	0:9ffe8ebd1c40	1724	piswarm.cls();
jah128	11:5ebcb52726cf	1725	if(PISWARM_DEBUG == 1)pc.printf("Test Magnetometer: ");
jah128	11:5ebcb52726cf	1726	if(piswarm.calibrate_magnetometer() != 0) {
jah128	11:5ebcb52726cf	1727	if(PISWARM_DEBUG == 1)pc.printf("FAILED\n");
jah128	11:5ebcb52726cf	1728	piswarm.cls();
jah128	11:5ebcb52726cf	1729	piswarm.locate(0,0);
jah128	11:5ebcb52726cf	1730	piswarm.printf("Mag Cal ");
jah128	11:5ebcb52726cf	1731	piswarm.locate(0,1);
jah128	11:5ebcb52726cf	1732	piswarm.printf("Failed ");
jah128	11:5ebcb52726cf	1733	wait(0.1);
jah128	11:5ebcb52726cf	1734	} else if(PISWARM_DEBUG == 1)pc.printf("PASSED\n");
jah128	11:5ebcb52726cf	1735	if(PISWARM_DEBUG == 1)pc.printf("Test Gyroscope: ");
jah128	11:5ebcb52726cf	1736	if(piswarm.calibrate_gyro() == 0) {
jah128	11:5ebcb52726cf	1737	if(PISWARM_DEBUG == 1)pc.printf("FAILED\n");
jah128	11:5ebcb52726cf	1738	wait(0.1);
jah128	11:5ebcb52726cf	1739	piswarm.cls();
jah128	11:5ebcb52726cf	1740	piswarm.locate(0,0);
jah128	11:5ebcb52726cf	1741	piswarm.printf("Gyro Cal");
jah128	11:5ebcb52726cf	1742	piswarm.locate(0,1);
jah128	11:5ebcb52726cf	1743	piswarm.printf("Failed ");
jah128	11:5ebcb52726cf	1744	wait(0.1);
jah128	11:5ebcb52726cf	1745	} else if(PISWARM_DEBUG == 1)pc.printf("PASSED\n");
jah128	11:5ebcb52726cf	1746	if(PISWARM_DEBUG == 1)pc.printf("Test Accelerometer: ");
jah128	11:5ebcb52726cf	1747	if(piswarm.calibrate_accelerometer() == 0) {
jah128	11:5ebcb52726cf	1748	if(PISWARM_DEBUG == 1)pc.printf("FAILED\n");
jah128	11:5ebcb52726cf	1749	wait(0.1);
jah128	11:5ebcb52726cf	1750	piswarm.cls();
jah128	11:5ebcb52726cf	1751	piswarm.locate(0,0);
jah128	11:5ebcb52726cf	1752	piswarm.printf("Acc. Cal");
jah128	11:5ebcb52726cf	1753	piswarm.locate(0,1);
jah128	11:5ebcb52726cf	1754	piswarm.printf("Failed ");
jah128	11:5ebcb52726cf	1755	wait(0.1);
jah128	11:5ebcb52726cf	1756	} else if(PISWARM_DEBUG == 1)pc.printf("PASSED\n");
jah128	11:5ebcb52726cf	1757	} else {
jah128	11:5ebcb52726cf	1758	wait(0.2);
jah128	11:5ebcb52726cf	1759	}
jah128	11:5ebcb52726cf	1760	oled_ticker.detach();
jah128	11:5ebcb52726cf	1761	piswarm.turn_off_all_oleds();
jah128	11:5ebcb52726cf	1762	if(USE_BEACON == 1) {
jah128	0:9ffe8ebd1c40	1763	piswarm.cls();
jah128	11:5ebcb52726cf	1764	piswarm.printf("LOCATING");
jah128	0:9ffe8ebd1c40	1765	piswarm.locate(0,1);
jah128	11:5ebcb52726cf	1766	piswarm.printf(" BEACON");
jah128	11:5ebcb52726cf	1767	piswarm.set_cled_colour(0,200,0);
jah128	11:5ebcb52726cf	1768	piswarm.set_oled_colour(150,200,150);
jah128	11:5ebcb52726cf	1769	oled_ticker.attach(oled_ticker_update,0.1);
jah128	11:5ebcb52726cf	1770	if(synchronise_with_beacon() > 0.4) {
jah128	11:5ebcb52726cf	1771	beacon_ticker.attach(update_bearing,1);
jah128	11:5ebcb52726cf	1772	if(DISPLAY_BEACON_READING == 1) display_bearing_info();
jah128	11:5ebcb52726cf	1773	else {
jah128	11:5ebcb52726cf	1774	piswarm.cls();
jah128	11:5ebcb52726cf	1775	piswarm.printf("SYNCED");
jah128	11:5ebcb52726cf	1776	piswarm.locate(0,1);
jah128	11:5ebcb52726cf	1777	piswarm.printf("B:%3.1f",bearing);
jah128	11:5ebcb52726cf	1778	}
jah128	11:5ebcb52726cf	1779	highlight_bearing();
jah128	11:5ebcb52726cf	1780	} else {
jah128	11:5ebcb52726cf	1781	piswarm.cls();
jah128	11:5ebcb52726cf	1782	piswarm.printf(" 2ND");
jah128	11:5ebcb52726cf	1783	piswarm.locate(0,1);
jah128	11:5ebcb52726cf	1784	piswarm.printf("ATTEMPT");
jah128	11:5ebcb52726cf	1785	if(synchronise_with_beacon() > 0.4) {
jah128	11:5ebcb52726cf	1786	beacon_ticker.attach(update_bearing,1);
jah128	11:5ebcb52726cf	1787	if(DISPLAY_BEACON_READING == 1) display_bearing_info();
jah128	11:5ebcb52726cf	1788	else {
jah128	11:5ebcb52726cf	1789	piswarm.cls();
jah128	11:5ebcb52726cf	1790	piswarm.printf("SYNCED");
jah128	11:5ebcb52726cf	1791	piswarm.locate(0,1);
jah128	11:5ebcb52726cf	1792	piswarm.printf("B:%3.1f",bearing);
jah128	11:5ebcb52726cf	1793	}
jah128	11:5ebcb52726cf	1794	highlight_bearing();
jah128	11:5ebcb52726cf	1795	} else {
jah128	11:5ebcb52726cf	1796	piswarm.cls();
jah128	11:5ebcb52726cf	1797	piswarm.printf("NO SYNC");
jah128	11:5ebcb52726cf	1798	piswarm.turn_off_all_oleds();
jah128	11:5ebcb52726cf	1799	oled_ticker.detach();
jah128	11:5ebcb52726cf	1800	beacon_ticker.attach(resync_with_beacon,10);
jah128	11:5ebcb52726cf	1801	}
jah128	11:5ebcb52726cf	1802	}
jah128	11:5ebcb52726cf	1803	}
jah128	11:5ebcb52726cf	1804	wait(0.5);
jah128	11:5ebcb52726cf	1805	oled_ticker.detach();
jah128	0:9ffe8ebd1c40	1806	piswarm.turn_off_all_oleds();
jah128	0:9ffe8ebd1c40	1807	piswarm.cls();
jah128	0:9ffe8ebd1c40	1808	piswarm.set_cled_colour(0,0,0);
jah128	11:5ebcb52726cf	1809	if(START_RADIO_ON_BOOT == 1) {
jah128	0:9ffe8ebd1c40	1810	if(PISWARM_DEBUG == 1)pc.printf("Setting up 433MHz Transceiver\n");
jah128	0:9ffe8ebd1c40	1811	piswarm.setup_radio();
jah128	0:9ffe8ebd1c40	1812	}
jah128	0:9ffe8ebd1c40	1813	}
jah128	0:9ffe8ebd1c40	1814
jah128	0:9ffe8ebd1c40	1815	/********************************************************************************
jah128	0:9ffe8ebd1c40	1816	* COPYRIGHT NOTICE *
jah128	0:9ffe8ebd1c40	1817	* *
jah128	4:52b3e4c5a425	1818	* Parts of code based on the original m3pi library, Copyright (c) 2010 cstyles *
jah128	4:52b3e4c5a425	1819	* *
jah128	0:9ffe8ebd1c40	1820	* Permission is hereby granted, free of charge, to any person obtaining a copy *
jah128	0:9ffe8ebd1c40	1821	* of this software and associated documentation files (the "Software"), to deal*
jah128	0:9ffe8ebd1c40	1822	* in the Software without restriction, including without limitation the rights *
jah128	0:9ffe8ebd1c40	1823	* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell *
jah128	11:5ebcb52726cf	1824	* copies of the Software, and to permit persons to whom the Software is *
jah128	0:9ffe8ebd1c40	1825	* furnished to do so, subject to the following conditions: *
jah128	0:9ffe8ebd1c40	1826	* *
jah128	0:9ffe8ebd1c40	1827	* The above copyright notice and this permission notice shall be included in *
jah128	0:9ffe8ebd1c40	1828	* all copies or substantial portions of the Software. *
jah128	0:9ffe8ebd1c40	1829	* *
jah128	0:9ffe8ebd1c40	1830	* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR *
jah128	0:9ffe8ebd1c40	1831	* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, *
jah128	0:9ffe8ebd1c40	1832	* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE *
jah128	0:9ffe8ebd1c40	1833	* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER *
jah128	0:9ffe8ebd1c40	1834	* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,*
jah128	0:9ffe8ebd1c40	1835	* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN *
jah128	0:9ffe8ebd1c40	1836	* THE SOFTWARE. *
jah128	0:9ffe8ebd1c40	1837	* *
jah128	0:9ffe8ebd1c40	1838	********************************************************************************/
jah128	0:9ffe8ebd1c40	1839
jah128	0:9ffe8ebd1c40	1840

Repository toolbox

Export to desktop IDE

Repository details

Type:	Library
Created:	30 Jun 2014
Imports:	9
Forks:	0
Commits:	12
Dependents:	0
Dependencies:	0
Followers:	2

piswarm.cpp@11:5ebcb52726cf, 2014-06-30 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning