Oskar Weigl
/
Eurobot2013
We are going to win! wohoo
Diff: Kalman/Sonar/RFSRF05.cpp
- Revision:
- 4:698a3c538482
- Parent:
- 3:8c78c15a92e3
- Child:
- 8:a8693e2daa00
diff -r 8c78c15a92e3 -r 698a3c538482 Kalman/Sonar/RFSRF05.cpp --- a/Kalman/Sonar/RFSRF05.cpp Wed Nov 14 15:48:20 2012 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,183 +0,0 @@ - -#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(); -//}