commit!
Embed:
(wiki syntax)
Show/hide line numbers
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 //}
Generated on Wed Jul 13 2022 17:39:34 by 1.7.2