RFSRF05.cpp

00001 
00002 #include "RFSRF05.h"
00003 #include "mbed.h"
00004 #include "globals.h"
00005 #include "system.h"
00006 
00007 
00008 RFSRF05::RFSRF05(PinName trigger,
00009                  PinName echo0,
00010                  PinName echo1,
00011                  PinName echo2,
00012                  PinName echo3,
00013                  PinName echo4,
00014                  PinName echo5,
00015                  PinName SDI,
00016                  PinName SDO,
00017                  PinName SCK,
00018                  PinName NCS,
00019                  PinName NIRQ)
00020         : _rf(SDI,SDO,SCK,NCS,NIRQ),
00021         _trigger(trigger),
00022         _echo0(echo0),
00023         _echo1(echo1),
00024         _echo2(echo2),
00025         _echo3(echo3),
00026         _echo4(echo4),
00027         _echo5(echo5) {
00028 
00029     // initialises codes
00030     codes[0] = CODE0;
00031     codes[1] = CODE1;
00032     codes[2] = CODE2;
00033 
00034     //set callback execute to true
00035     ValidPulse = false;
00036 
00037     // Attach interrupts
00038 #ifdef SONAR_ECHO_INV
00039     // inverted sonar inputs
00040     _echo5.fall(this, &RFSRF05::_rising);
00041     _echo0.rise(this, &RFSRF05::_falling);
00042     _echo1.rise(this, &RFSRF05::_falling);
00043     _echo2.rise(this, &RFSRF05::_falling);
00044     _echo3.rise(this, &RFSRF05::_falling);
00045     _echo4.rise(this, &RFSRF05::_falling);
00046     _echo5.rise(this, &RFSRF05::_falling);
00047 #else
00048     _echo5.rise(this, &RFSRF05::_rising);
00049     _echo0.fall(this, &RFSRF05::_falling);
00050     _echo1.fall(this, &RFSRF05::_falling);
00051     _echo2.fall(this, &RFSRF05::_falling);
00052     _echo3.fall(this, &RFSRF05::_falling);
00053     _echo4.fall(this, &RFSRF05::_falling);
00054     _echo5.fall(this, &RFSRF05::_falling);
00055 #endif
00056 
00057 
00058     //init callabck function
00059     callbackfunc = NULL;
00060     callbackobj = NULL;
00061     mcallbackfunc = NULL;
00062 
00063     // innitialises beacon counter
00064     _beacon_counter = 0;
00065 
00066 #ifdef ROBOT_PRIMARY
00067     //Interrupts every 50ms for primary robot
00068     _ticker.attach(this, &RFSRF05::_startRange, 0.05);
00069 #else
00070     //attach callback
00071     _rf.callbackobj = (DummyCT*)this;
00072     _rf.mcallbackfunc = (void (DummyCT::*)(unsigned char rx_data)) &RFSRF05::startRange;
00073 #endif
00074 
00075 }
00076 
00077 #ifdef ROBOT_PRIMARY
00078 void RFSRF05::_startRange() {
00079 
00080     //printf("Srange\r\r");
00081 
00082     // increments counter
00083     _beacon_counter = (_beacon_counter + 1) % 3;
00084 
00085 
00086     // set flags
00087     ValidPulse = false;
00088     expValidPulse = true;
00089     
00090     // writes code to RF port
00091     _rf.write(codes[_beacon_counter]);
00092 
00093     // send a trigger pulse, 10uS long
00094     _trigger = 1;
00095     wait_us (10);
00096     _trigger = 0;
00097 
00098 }
00099 #else
00100 
00101 void RFSRF05::startRange(unsigned char rx_code) {
00102     for (int i = 0; i < 3; i++) {
00103         if (rx_code == codes[i]) {
00104        
00105             // assign beacon_counter
00106             _beacon_counter = i;
00107 
00108             // set flags
00109             ValidPulse = false;
00110             expValidPulse = true;
00111 
00112             // send a trigger pulse, 10uS long
00113             _trigger = 1;
00114             wait_us (10);
00115             _trigger = 0;
00116             break;
00117         }
00118     }
00119 }
00120 #endif
00121 
00122 // Clear and start the timer at the begining of the echo pulse
00123 void RFSRF05::_rising(void) {
00124 
00125     _timer.reset();
00126     _timer.start();
00127 
00128     //Set callback execute to ture
00129     if (expValidPulse) {
00130         ValidPulse = true;
00131         expValidPulse = false;
00132     }
00133 }
00134 
00135 // Stop and read the timer at the end of the pulse
00136 void RFSRF05::_falling(void) {
00137     _timer.stop();
00138 
00139     if (ValidPulse) {
00140         //printf("Validpulse trig!\r\n");
00141         ValidPulse = false;
00142 
00143         //Calucate distance
00144         //true offset is about 100, we put 300 so circles overlap
00145         _dist[_beacon_counter] =  _timer.read_us()/2.9 + 300;
00146 
00147         if (callbackfunc)
00148             (*callbackfunc)(_beacon_counter, _dist[_beacon_counter]);
00149 
00150         if (callbackobj && mcallbackfunc)
00151             (callbackobj->*mcallbackfunc)(_beacon_counter, _dist[_beacon_counter], sonarvariance);
00152 
00153     }
00154 
00155 }
00156 
00157 float RFSRF05::read0() {
00158     // returns distance
00159     return (_dist[0]);
00160 }
00161 
00162 float RFSRF05::read1() {
00163     // returns distance
00164     return (_dist[1]);
00165 }
00166 
00167 float RFSRF05::read2() {
00168     // returns distance
00169     return (_dist[2]);
00170 }
00171 
00172 float RFSRF05::read(unsigned int beaconnum) {
00173     // returns distance
00174     return (_dist[beaconnum]);
00175 }
00176 
00177 void RFSRF05::setCode(int code_index, unsigned char code) {
00178     codes[code_index] = code;
00179 }
00180 
00181 //SRF05::operator float() {
00182 //    return read();
00183 //}