Eurobot_shared pubulished from Eurobot Primary
Diff: Kalman/Sonar/RFSRF05.cpp
- Revision:
- 0:434fd09723be
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Kalman/Sonar/RFSRF05.cpp Tue Aug 07 10:25:53 2012 +0000 @@ -0,0 +1,183 @@ + +#include "RFSRF05.h" +#include "mbed.h" +#include "globals.h" +#include "system.h" + + +RFSRF05::RFSRF05(PinName trigger, + PinName echo0, + PinName echo1, + PinName echo2, + PinName echo3, + PinName echo4, + PinName echo5, + PinName SDI, + PinName SDO, + PinName SCK, + PinName NCS, + PinName NIRQ) + : _rf(SDI,SDO,SCK,NCS,NIRQ), + _trigger(trigger), + _echo0(echo0), + _echo1(echo1), + _echo2(echo2), + _echo3(echo3), + _echo4(echo4), + _echo5(echo5) { + + // initialises codes + codes[0] = CODE0; + codes[1] = CODE1; + codes[2] = CODE2; + + //set callback execute to true + ValidPulse = false; + + // Attach interrupts +#ifdef SONAR_ECHO_INV + // inverted sonar inputs + _echo5.fall(this, &RFSRF05::_rising); + _echo0.rise(this, &RFSRF05::_falling); + _echo1.rise(this, &RFSRF05::_falling); + _echo2.rise(this, &RFSRF05::_falling); + _echo3.rise(this, &RFSRF05::_falling); + _echo4.rise(this, &RFSRF05::_falling); + _echo5.rise(this, &RFSRF05::_falling); +#else + _echo5.rise(this, &RFSRF05::_rising); + _echo0.fall(this, &RFSRF05::_falling); + _echo1.fall(this, &RFSRF05::_falling); + _echo2.fall(this, &RFSRF05::_falling); + _echo3.fall(this, &RFSRF05::_falling); + _echo4.fall(this, &RFSRF05::_falling); + _echo5.fall(this, &RFSRF05::_falling); +#endif + + + //init callabck function + callbackfunc = NULL; + callbackobj = NULL; + mcallbackfunc = NULL; + + // innitialises beacon counter + _beacon_counter = 0; + +#ifdef ROBOT_PRIMARY + //Interrupts every 50ms for primary robot + _ticker.attach(this, &RFSRF05::_startRange, 0.05); +#else + //attach callback + _rf.callbackobj = (DummyCT*)this; + _rf.mcallbackfunc = (void (DummyCT::*)(unsigned char rx_data)) &RFSRF05::startRange; +#endif + +} + +#ifdef ROBOT_PRIMARY +void RFSRF05::_startRange() { + + //printf("Srange\r\r"); + + // increments counter + _beacon_counter = (_beacon_counter + 1) % 3; + + + // set flags + ValidPulse = false; + expValidPulse = true; + + // writes code to RF port + _rf.write(codes[_beacon_counter]); + + // send a trigger pulse, 10uS long + _trigger = 1; + wait_us (10); + _trigger = 0; + +} +#else + +void RFSRF05::startRange(unsigned char rx_code) { + for (int i = 0; i < 3; i++) { + if (rx_code == codes[i]) { + + // assign beacon_counter + _beacon_counter = i; + + // set flags + ValidPulse = false; + expValidPulse = true; + + // send a trigger pulse, 10uS long + _trigger = 1; + wait_us (10); + _trigger = 0; + break; + } + } +} +#endif + +// Clear and start the timer at the begining of the echo pulse +void RFSRF05::_rising(void) { + + _timer.reset(); + _timer.start(); + + //Set callback execute to ture + if (expValidPulse) { + ValidPulse = true; + expValidPulse = false; + } +} + +// Stop and read the timer at the end of the pulse +void RFSRF05::_falling(void) { + _timer.stop(); + + if (ValidPulse) { + //printf("Validpulse trig!\r\n"); + ValidPulse = false; + + //Calucate distance + //true offset is about 100, we put 300 so circles overlap + _dist[_beacon_counter] = _timer.read_us()/2.9 + 300; + + if (callbackfunc) + (*callbackfunc)(_beacon_counter, _dist[_beacon_counter]); + + if (callbackobj && mcallbackfunc) + (callbackobj->*mcallbackfunc)(_beacon_counter, _dist[_beacon_counter], sonarvariance); + + } + +} + +float RFSRF05::read0() { + // returns distance + return (_dist[0]); +} + +float RFSRF05::read1() { + // returns distance + return (_dist[1]); +} + +float RFSRF05::read2() { + // returns distance + return (_dist[2]); +} + +float RFSRF05::read(unsigned int beaconnum) { + // returns distance + return (_dist[beaconnum]); +} + +void RFSRF05::setCode(int code_index, unsigned char code) { + codes[code_index] = code; +} + +//SRF05::operator float() { +// return read(); +//}