RFSRF05.cpp

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