Shuto Naruse
/
Eurobot_2012_Secondary
Eurobot_2012_Secondary
Diff: Kalman/Sonar/RFSRF05.cpp
- Revision:
- 0:fbfafa6bf5f9
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Kalman/Sonar/RFSRF05.cpp Fri Apr 20 21:32:24 2012 +0000 @@ -0,0 +1,139 @@ + +#include "RFSRF05.h" +#include "mbed.h" +#include "globals.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 + _code[0] = CODE0; + _code[1] = CODE1; + _code[2] = CODE2; + + //set callback execute to true + ValidPulse = false; + + // Attach interrupts + _echo0.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); + + + //init callabck function + callbackfunc = NULL; + callbackobj = NULL; + mcallbackfunc = NULL; + + // innitialises beacon counter + _beacon_counter = 0; + + //Interrupts every 50ms + _ticker.attach(this, &RFSRF05::_startRange, 0.05); +} + + +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(_code[_beacon_counter]); + + + + // send a trigger pulse, 10uS long + _trigger = 1; + wait_us (10); + _trigger = 0; + +} + + +// 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 + _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]); +} + +//SRF05::operator float() { +// return read(); +//}