Eurobot_shared - commit!

Users » narshu » Code » Eurobot_shared

commit!

Kalman/Sonar/RFSRF05.cpp@0:42026f893a2d, 2012-06-15 (annotated)

Committer:: narshu
Date:: Fri Jun 15 20:40:17 2012 +0000
Revision:: 0:42026f893a2d

Who changed what in which revision?

User	Revision	Line number	New contents of line
narshu	0:42026f893a2d	1
narshu	0:42026f893a2d	2	#include "RFSRF05.h"
narshu	0:42026f893a2d	3	#include "mbed.h"
narshu	0:42026f893a2d	4	#include "globals.h"
narshu	0:42026f893a2d	5	#include "system.h"
narshu	0:42026f893a2d	6
narshu	0:42026f893a2d	7
narshu	0:42026f893a2d	8	RFSRF05::RFSRF05(PinName trigger,
narshu	0:42026f893a2d	9	PinName echo0,
narshu	0:42026f893a2d	10	PinName echo1,
narshu	0:42026f893a2d	11	PinName echo2,
narshu	0:42026f893a2d	12	PinName echo3,
narshu	0:42026f893a2d	13	PinName echo4,
narshu	0:42026f893a2d	14	PinName echo5,
narshu	0:42026f893a2d	15	PinName SDI,
narshu	0:42026f893a2d	16	PinName SDO,
narshu	0:42026f893a2d	17	PinName SCK,
narshu	0:42026f893a2d	18	PinName NCS,
narshu	0:42026f893a2d	19	PinName NIRQ)
narshu	0:42026f893a2d	20	: _rf(SDI,SDO,SCK,NCS,NIRQ),
narshu	0:42026f893a2d	21	_trigger(trigger),
narshu	0:42026f893a2d	22	_echo0(echo0),
narshu	0:42026f893a2d	23	_echo1(echo1),
narshu	0:42026f893a2d	24	_echo2(echo2),
narshu	0:42026f893a2d	25	_echo3(echo3),
narshu	0:42026f893a2d	26	_echo4(echo4),
narshu	0:42026f893a2d	27	_echo5(echo5) {
narshu	0:42026f893a2d	28
narshu	0:42026f893a2d	29	// initialises codes
narshu	0:42026f893a2d	30	codes[0] = CODE0;
narshu	0:42026f893a2d	31	codes[1] = CODE1;
narshu	0:42026f893a2d	32	codes[2] = CODE2;
narshu	0:42026f893a2d	33
narshu	0:42026f893a2d	34	//set callback execute to true
narshu	0:42026f893a2d	35	ValidPulse = false;
narshu	0:42026f893a2d	36
narshu	0:42026f893a2d	37	// Attach interrupts
narshu	0:42026f893a2d	38	#ifdef SONAR_ECHO_INV
narshu	0:42026f893a2d	39	// inverted sonar inputs
narshu	0:42026f893a2d	40	_echo5.fall(this, &RFSRF05::_rising);
narshu	0:42026f893a2d	41	_echo0.rise(this, &RFSRF05::_falling);
narshu	0:42026f893a2d	42	_echo1.rise(this, &RFSRF05::_falling);
narshu	0:42026f893a2d	43	_echo2.rise(this, &RFSRF05::_falling);
narshu	0:42026f893a2d	44	_echo3.rise(this, &RFSRF05::_falling);
narshu	0:42026f893a2d	45	_echo4.rise(this, &RFSRF05::_falling);
narshu	0:42026f893a2d	46	_echo5.rise(this, &RFSRF05::_falling);
narshu	0:42026f893a2d	47	#else
narshu	0:42026f893a2d	48	_echo5.rise(this, &RFSRF05::_rising);
narshu	0:42026f893a2d	49	_echo0.fall(this, &RFSRF05::_falling);
narshu	0:42026f893a2d	50	_echo1.fall(this, &RFSRF05::_falling);
narshu	0:42026f893a2d	51	_echo2.fall(this, &RFSRF05::_falling);
narshu	0:42026f893a2d	52	_echo3.fall(this, &RFSRF05::_falling);
narshu	0:42026f893a2d	53	_echo4.fall(this, &RFSRF05::_falling);
narshu	0:42026f893a2d	54	_echo5.fall(this, &RFSRF05::_falling);
narshu	0:42026f893a2d	55	#endif
narshu	0:42026f893a2d	56
narshu	0:42026f893a2d	57
narshu	0:42026f893a2d	58	//init callabck function
narshu	0:42026f893a2d	59	callbackfunc = NULL;
narshu	0:42026f893a2d	60	callbackobj = NULL;
narshu	0:42026f893a2d	61	mcallbackfunc = NULL;
narshu	0:42026f893a2d	62
narshu	0:42026f893a2d	63	// innitialises beacon counter
narshu	0:42026f893a2d	64	_beacon_counter = 0;
narshu	0:42026f893a2d	65
narshu	0:42026f893a2d	66	#ifdef ROBOT_PRIMARY
narshu	0:42026f893a2d	67	//Interrupts every 50ms for primary robot
narshu	0:42026f893a2d	68	_ticker.attach(this, &RFSRF05::_startRange, 0.05);
narshu	0:42026f893a2d	69	#else
narshu	0:42026f893a2d	70	//attach callback
narshu	0:42026f893a2d	71	_rf.callbackobj = (DummyCT*)this;
narshu	0:42026f893a2d	72	_rf.mcallbackfunc = (void (DummyCT::*)(unsigned char rx_data)) &RFSRF05::startRange;
narshu	0:42026f893a2d	73	#endif
narshu	0:42026f893a2d	74
narshu	0:42026f893a2d	75	}
narshu	0:42026f893a2d	76
narshu	0:42026f893a2d	77	#ifdef ROBOT_PRIMARY
narshu	0:42026f893a2d	78	void RFSRF05::_startRange() {
narshu	0:42026f893a2d	79
narshu	0:42026f893a2d	80	//printf("Srange\r\r");
narshu	0:42026f893a2d	81
narshu	0:42026f893a2d	82	// increments counter
narshu	0:42026f893a2d	83	_beacon_counter = (_beacon_counter + 1) % 3;
narshu	0:42026f893a2d	84
narshu	0:42026f893a2d	85
narshu	0:42026f893a2d	86	// set flags
narshu	0:42026f893a2d	87	ValidPulse = false;
narshu	0:42026f893a2d	88	expValidPulse = true;
narshu	0:42026f893a2d	89
narshu	0:42026f893a2d	90	// writes code to RF port
narshu	0:42026f893a2d	91	_rf.write(codes[_beacon_counter]);
narshu	0:42026f893a2d	92
narshu	0:42026f893a2d	93	// send a trigger pulse, 10uS long
narshu	0:42026f893a2d	94	_trigger = 1;
narshu	0:42026f893a2d	95	wait_us (10);
narshu	0:42026f893a2d	96	_trigger = 0;
narshu	0:42026f893a2d	97
narshu	0:42026f893a2d	98	}
narshu	0:42026f893a2d	99	#else
narshu	0:42026f893a2d	100
narshu	0:42026f893a2d	101	void RFSRF05::startRange(unsigned char rx_code) {
narshu	0:42026f893a2d	102	for (int i = 0; i < 3; i++) {
narshu	0:42026f893a2d	103	if (rx_code == codes[i]) {
narshu	0:42026f893a2d	104
narshu	0:42026f893a2d	105	// assign beacon_counter
narshu	0:42026f893a2d	106	_beacon_counter = i;
narshu	0:42026f893a2d	107
narshu	0:42026f893a2d	108	// set flags
narshu	0:42026f893a2d	109	ValidPulse = false;
narshu	0:42026f893a2d	110	expValidPulse = true;
narshu	0:42026f893a2d	111
narshu	0:42026f893a2d	112	// send a trigger pulse, 10uS long
narshu	0:42026f893a2d	113	_trigger = 1;
narshu	0:42026f893a2d	114	wait_us (10);
narshu	0:42026f893a2d	115	_trigger = 0;
narshu	0:42026f893a2d	116	break;
narshu	0:42026f893a2d	117	}
narshu	0:42026f893a2d	118	}
narshu	0:42026f893a2d	119	}
narshu	0:42026f893a2d	120	#endif
narshu	0:42026f893a2d	121
narshu	0:42026f893a2d	122	// Clear and start the timer at the begining of the echo pulse
narshu	0:42026f893a2d	123	void RFSRF05::_rising(void) {
narshu	0:42026f893a2d	124
narshu	0:42026f893a2d	125	_timer.reset();
narshu	0:42026f893a2d	126	_timer.start();
narshu	0:42026f893a2d	127
narshu	0:42026f893a2d	128	//Set callback execute to ture
narshu	0:42026f893a2d	129	if (expValidPulse) {
narshu	0:42026f893a2d	130	ValidPulse = true;
narshu	0:42026f893a2d	131	expValidPulse = false;
narshu	0:42026f893a2d	132	}
narshu	0:42026f893a2d	133	}
narshu	0:42026f893a2d	134
narshu	0:42026f893a2d	135	// Stop and read the timer at the end of the pulse
narshu	0:42026f893a2d	136	void RFSRF05::_falling(void) {
narshu	0:42026f893a2d	137	_timer.stop();
narshu	0:42026f893a2d	138
narshu	0:42026f893a2d	139	if (ValidPulse) {
narshu	0:42026f893a2d	140	//printf("Validpulse trig!\r\n");
narshu	0:42026f893a2d	141	ValidPulse = false;
narshu	0:42026f893a2d	142
narshu	0:42026f893a2d	143	//Calucate distance
narshu	0:42026f893a2d	144	//true offset is about 100, we put 300 so circles overlap
narshu	0:42026f893a2d	145	_dist[_beacon_counter] = _timer.read_us()/2.9 + 300;
narshu	0:42026f893a2d	146
narshu	0:42026f893a2d	147	if (callbackfunc)
narshu	0:42026f893a2d	148	(*callbackfunc)(_beacon_counter, _dist[_beacon_counter]);
narshu	0:42026f893a2d	149
narshu	0:42026f893a2d	150	if (callbackobj && mcallbackfunc)
narshu	0:42026f893a2d	151	(callbackobj->*mcallbackfunc)(_beacon_counter, _dist[_beacon_counter], sonarvariance);
narshu	0:42026f893a2d	152
narshu	0:42026f893a2d	153	}
narshu	0:42026f893a2d	154
narshu	0:42026f893a2d	155	}
narshu	0:42026f893a2d	156
narshu	0:42026f893a2d	157	float RFSRF05::read0() {
narshu	0:42026f893a2d	158	// returns distance
narshu	0:42026f893a2d	159	return (_dist[0]);
narshu	0:42026f893a2d	160	}
narshu	0:42026f893a2d	161
narshu	0:42026f893a2d	162	float RFSRF05::read1() {
narshu	0:42026f893a2d	163	// returns distance
narshu	0:42026f893a2d	164	return (_dist[1]);
narshu	0:42026f893a2d	165	}
narshu	0:42026f893a2d	166
narshu	0:42026f893a2d	167	float RFSRF05::read2() {
narshu	0:42026f893a2d	168	// returns distance
narshu	0:42026f893a2d	169	return (_dist[2]);
narshu	0:42026f893a2d	170	}
narshu	0:42026f893a2d	171
narshu	0:42026f893a2d	172	float RFSRF05::read(unsigned int beaconnum) {
narshu	0:42026f893a2d	173	// returns distance
narshu	0:42026f893a2d	174	return (_dist[beaconnum]);
narshu	0:42026f893a2d	175	}
narshu	0:42026f893a2d	176
narshu	0:42026f893a2d	177	void RFSRF05::setCode(int code_index, unsigned char code) {
narshu	0:42026f893a2d	178	codes[code_index] = code;
narshu	0:42026f893a2d	179	}
narshu	0:42026f893a2d	180
narshu	0:42026f893a2d	181	//SRF05::operator float() {
narshu	0:42026f893a2d	182	// return read();
narshu	0:42026f893a2d	183	//}

Repository toolbox

Export to desktop IDE

Repository details

Type:	Library
Created:	28 Apr 2012
Imports:	1
Forks:	0
Commits:	1
Dependents:	0
Dependencies:	0
Followers:	1

Kalman/Sonar/RFSRF05.cpp@0:42026f893a2d, 2012-06-15 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning