Cooper Liu
/
Eurobot2013_Co-Processor
working version with calibration done
Fork of Eurobot2013 by
Sonar/RFSRF05.cpp@11:5ba926692210, 2013-04-09 (annotated)
- Committer:
- xiaxia686
- Date:
- Tue Apr 09 15:32:47 2013 +0000
- Revision:
- 11:5ba926692210
- Parent:
- 10:2bd9f4e02b74
woking version (calibrated)
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
sv | 1:6799c07fe510 | 1 | #include "RFSRF05.h" |
sv | 1:6799c07fe510 | 2 | #include "mbed.h" |
sv | 1:6799c07fe510 | 3 | #include "globals.h" |
sv | 1:6799c07fe510 | 4 | #include "system.h" |
sv | 1:6799c07fe510 | 5 | |
sv | 1:6799c07fe510 | 6 | |
sv | 1:6799c07fe510 | 7 | RFSRF05::RFSRF05(PinName trigger, |
sv | 1:6799c07fe510 | 8 | PinName echo0, |
sv | 1:6799c07fe510 | 9 | PinName echo1, |
sv | 1:6799c07fe510 | 10 | PinName echo2, |
sv | 1:6799c07fe510 | 11 | PinName echo3, |
sv | 1:6799c07fe510 | 12 | PinName echo4, |
sv | 1:6799c07fe510 | 13 | PinName echo5, |
sv | 1:6799c07fe510 | 14 | PinName SDI, |
sv | 1:6799c07fe510 | 15 | PinName SDO, |
sv | 1:6799c07fe510 | 16 | PinName SCK, |
sv | 1:6799c07fe510 | 17 | PinName NCS, |
sv | 1:6799c07fe510 | 18 | PinName NIRQ) |
sv | 1:6799c07fe510 | 19 | : _rf(SDI,SDO,SCK,NCS,NIRQ), |
sv | 1:6799c07fe510 | 20 | _trigger(trigger), |
sv | 1:6799c07fe510 | 21 | _echo0(echo0), |
sv | 1:6799c07fe510 | 22 | _echo1(echo1), |
sv | 1:6799c07fe510 | 23 | _echo2(echo2), |
sv | 1:6799c07fe510 | 24 | _echo3(echo3), |
sv | 1:6799c07fe510 | 25 | _echo4(echo4), |
sv | 1:6799c07fe510 | 26 | _echo5(echo5) { |
sv | 1:6799c07fe510 | 27 | |
sv | 1:6799c07fe510 | 28 | // initialises codes |
sv | 1:6799c07fe510 | 29 | codes[0] = CODE0; |
sv | 1:6799c07fe510 | 30 | codes[1] = CODE1; |
sv | 1:6799c07fe510 | 31 | codes[2] = CODE2; |
sv | 1:6799c07fe510 | 32 | |
sv | 1:6799c07fe510 | 33 | //set callback execute to true |
sv | 1:6799c07fe510 | 34 | ValidPulse = false; |
sv | 1:6799c07fe510 | 35 | |
sv | 1:6799c07fe510 | 36 | // Attach interrupts |
sv | 1:6799c07fe510 | 37 | #ifdef SONAR_ECHO_INV |
sv | 1:6799c07fe510 | 38 | // inverted sonar inputs |
sv | 1:6799c07fe510 | 39 | _echo5.fall(this, &RFSRF05::_rising); |
sv | 1:6799c07fe510 | 40 | _echo0.rise(this, &RFSRF05::_falling); |
sv | 1:6799c07fe510 | 41 | _echo1.rise(this, &RFSRF05::_falling); |
sv | 1:6799c07fe510 | 42 | _echo2.rise(this, &RFSRF05::_falling); |
sv | 1:6799c07fe510 | 43 | _echo3.rise(this, &RFSRF05::_falling); |
sv | 1:6799c07fe510 | 44 | _echo4.rise(this, &RFSRF05::_falling); |
sv | 1:6799c07fe510 | 45 | _echo5.rise(this, &RFSRF05::_falling); |
sv | 1:6799c07fe510 | 46 | #else |
sv | 1:6799c07fe510 | 47 | _echo5.rise(this, &RFSRF05::_rising); |
sv | 1:6799c07fe510 | 48 | _echo0.fall(this, &RFSRF05::_falling); |
sv | 1:6799c07fe510 | 49 | _echo1.fall(this, &RFSRF05::_falling); |
sv | 1:6799c07fe510 | 50 | _echo2.fall(this, &RFSRF05::_falling); |
sv | 1:6799c07fe510 | 51 | _echo3.fall(this, &RFSRF05::_falling); |
sv | 1:6799c07fe510 | 52 | _echo4.fall(this, &RFSRF05::_falling); |
sv | 1:6799c07fe510 | 53 | _echo5.fall(this, &RFSRF05::_falling); |
sv | 1:6799c07fe510 | 54 | #endif |
sv | 1:6799c07fe510 | 55 | |
sv | 1:6799c07fe510 | 56 | |
sv | 1:6799c07fe510 | 57 | //init callabck function |
sv | 1:6799c07fe510 | 58 | callbackfunc = NULL; |
sv | 1:6799c07fe510 | 59 | callbackobj = NULL; |
sv | 1:6799c07fe510 | 60 | mcallbackfunc = NULL; |
sv | 1:6799c07fe510 | 61 | |
sv | 1:6799c07fe510 | 62 | // innitialises beacon counter |
sv | 1:6799c07fe510 | 63 | _beacon_counter = 0; |
sv | 1:6799c07fe510 | 64 | |
sv | 1:6799c07fe510 | 65 | #ifdef ROBOT_PRIMARY |
sv | 1:6799c07fe510 | 66 | //Interrupts every 50ms for primary robot |
sv | 1:6799c07fe510 | 67 | _ticker.attach(this, &RFSRF05::_startRange, 0.05); |
sv | 1:6799c07fe510 | 68 | #else |
sv | 1:6799c07fe510 | 69 | //attach callback |
sv | 1:6799c07fe510 | 70 | _rf.callbackobj = (DummyCT*)this; |
sv | 1:6799c07fe510 | 71 | _rf.mcallbackfunc = (void (DummyCT::*)(unsigned char rx_data)) &RFSRF05::startRange; |
sv | 1:6799c07fe510 | 72 | #endif |
sv | 1:6799c07fe510 | 73 | |
sv | 1:6799c07fe510 | 74 | } |
sv | 1:6799c07fe510 | 75 | |
sv | 1:6799c07fe510 | 76 | #ifdef ROBOT_PRIMARY |
sv | 1:6799c07fe510 | 77 | void RFSRF05::_startRange() { |
sv | 1:6799c07fe510 | 78 | |
sv | 1:6799c07fe510 | 79 | //printf("Srange\r\r"); |
sv | 1:6799c07fe510 | 80 | |
sv | 1:6799c07fe510 | 81 | // increments counter |
sv | 1:6799c07fe510 | 82 | _beacon_counter = (_beacon_counter + 1) % 3; |
sv | 1:6799c07fe510 | 83 | |
sv | 1:6799c07fe510 | 84 | |
sv | 1:6799c07fe510 | 85 | // set flags |
sv | 1:6799c07fe510 | 86 | ValidPulse = false; |
sv | 1:6799c07fe510 | 87 | expValidPulse = true; |
sv | 1:6799c07fe510 | 88 | |
sv | 1:6799c07fe510 | 89 | // writes code to RF port |
sv | 1:6799c07fe510 | 90 | _rf.write(codes[_beacon_counter]); |
sv | 1:6799c07fe510 | 91 | |
sv | 1:6799c07fe510 | 92 | // send a trigger pulse, 10uS long |
sv | 1:6799c07fe510 | 93 | _trigger = 1; |
sv | 1:6799c07fe510 | 94 | wait_us (10); |
sv | 1:6799c07fe510 | 95 | _trigger = 0; |
sv | 1:6799c07fe510 | 96 | |
sv | 1:6799c07fe510 | 97 | } |
sv | 1:6799c07fe510 | 98 | #else |
sv | 1:6799c07fe510 | 99 | |
sv | 1:6799c07fe510 | 100 | void RFSRF05::startRange(unsigned char rx_code) { |
sv | 1:6799c07fe510 | 101 | for (int i = 0; i < 3; i++) { |
sv | 1:6799c07fe510 | 102 | if (rx_code == codes[i]) { |
sv | 1:6799c07fe510 | 103 | |
sv | 1:6799c07fe510 | 104 | // assign beacon_counter |
sv | 1:6799c07fe510 | 105 | _beacon_counter = i; |
sv | 1:6799c07fe510 | 106 | |
sv | 1:6799c07fe510 | 107 | // set flags |
sv | 1:6799c07fe510 | 108 | ValidPulse = false; |
sv | 1:6799c07fe510 | 109 | expValidPulse = true; |
sv | 1:6799c07fe510 | 110 | |
sv | 1:6799c07fe510 | 111 | // send a trigger pulse, 10uS long |
sv | 1:6799c07fe510 | 112 | _trigger = 1; |
sv | 1:6799c07fe510 | 113 | wait_us (10); |
sv | 1:6799c07fe510 | 114 | _trigger = 0; |
sv | 1:6799c07fe510 | 115 | break; |
sv | 1:6799c07fe510 | 116 | } |
sv | 1:6799c07fe510 | 117 | } |
sv | 1:6799c07fe510 | 118 | } |
sv | 1:6799c07fe510 | 119 | #endif |
sv | 1:6799c07fe510 | 120 | |
sv | 1:6799c07fe510 | 121 | // Clear and start the timer at the begining of the echo pulse |
sv | 1:6799c07fe510 | 122 | void RFSRF05::_rising(void) { |
sv | 1:6799c07fe510 | 123 | |
sv | 1:6799c07fe510 | 124 | _timer.reset(); |
sv | 1:6799c07fe510 | 125 | _timer.start(); |
sv | 1:6799c07fe510 | 126 | |
sv | 1:6799c07fe510 | 127 | //Set callback execute to ture |
sv | 1:6799c07fe510 | 128 | if (expValidPulse) { |
sv | 1:6799c07fe510 | 129 | ValidPulse = true; |
sv | 1:6799c07fe510 | 130 | expValidPulse = false; |
sv | 1:6799c07fe510 | 131 | } |
sv | 1:6799c07fe510 | 132 | } |
sv | 1:6799c07fe510 | 133 | |
sv | 1:6799c07fe510 | 134 | // Stop and read the timer at the end of the pulse |
sv | 1:6799c07fe510 | 135 | void RFSRF05::_falling(void) { |
sv | 1:6799c07fe510 | 136 | _timer.stop(); |
sv | 1:6799c07fe510 | 137 | |
sv | 1:6799c07fe510 | 138 | if (ValidPulse) { |
sv | 1:6799c07fe510 | 139 | //printf("Validpulse trig!\r\n"); |
sv | 1:6799c07fe510 | 140 | ValidPulse = false; |
sv | 1:6799c07fe510 | 141 | |
sv | 1:6799c07fe510 | 142 | //Calucate distance |
xiaxia686 | 11:5ba926692210 | 143 | //true offset is about 100, we put 300 so circles overlap, not sure about the true speed-of-sound constant |
xiaxia686 | 11:5ba926692210 | 144 | _dist[_beacon_counter] = _timer.read_us()/2.8807f + 70; //2.8807f |
sv | 1:6799c07fe510 | 145 | |
sv | 1:6799c07fe510 | 146 | if (callbackfunc) |
xiaxia686 | 11:5ba926692210 | 147 | (*callbackfunc)(_beacon_counter, _dist[_beacon_counter], sonarvariance); |
sv | 1:6799c07fe510 | 148 | |
sv | 1:6799c07fe510 | 149 | if (callbackobj && mcallbackfunc) |
sv | 1:6799c07fe510 | 150 | (callbackobj->*mcallbackfunc)(_beacon_counter, _dist[_beacon_counter], sonarvariance); |
sv | 1:6799c07fe510 | 151 | |
sv | 1:6799c07fe510 | 152 | } |
sv | 1:6799c07fe510 | 153 | |
sv | 1:6799c07fe510 | 154 | } |
sv | 1:6799c07fe510 | 155 | |
sv | 1:6799c07fe510 | 156 | float RFSRF05::read0() { |
sv | 1:6799c07fe510 | 157 | // returns distance |
sv | 1:6799c07fe510 | 158 | return (_dist[0]); |
sv | 1:6799c07fe510 | 159 | } |
sv | 1:6799c07fe510 | 160 | |
sv | 1:6799c07fe510 | 161 | float RFSRF05::read1() { |
sv | 1:6799c07fe510 | 162 | // returns distance |
sv | 1:6799c07fe510 | 163 | return (_dist[1]); |
sv | 1:6799c07fe510 | 164 | } |
sv | 1:6799c07fe510 | 165 | |
sv | 1:6799c07fe510 | 166 | float RFSRF05::read2() { |
sv | 1:6799c07fe510 | 167 | // returns distance |
sv | 1:6799c07fe510 | 168 | return (_dist[2]); |
sv | 1:6799c07fe510 | 169 | } |
sv | 1:6799c07fe510 | 170 | |
sv | 1:6799c07fe510 | 171 | float RFSRF05::read(unsigned int beaconnum) { |
sv | 1:6799c07fe510 | 172 | // returns distance |
sv | 1:6799c07fe510 | 173 | return (_dist[beaconnum]); |
sv | 1:6799c07fe510 | 174 | } |
sv | 1:6799c07fe510 | 175 | |
sv | 1:6799c07fe510 | 176 | void RFSRF05::setCode(int code_index, unsigned char code) { |
sv | 1:6799c07fe510 | 177 | codes[code_index] = code; |
sv | 1:6799c07fe510 | 178 | } |
sv | 1:6799c07fe510 | 179 | |
sv | 1:6799c07fe510 | 180 | //SRF05::operator float() { |
sv | 1:6799c07fe510 | 181 | // return read(); |
sv | 1:6799c07fe510 | 182 | //} |