Shuto Naruse
Eurobot_2012_Secondary
Kalman/Sonar/RFSRF05.cpp@0:fbfafa6bf5f9, 2012-04-20 (annotated)
- Committer:
- narshu
- Date:
- Fri Apr 20 21:32:24 2012 +0000
- Revision:
- 0:fbfafa6bf5f9
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| narshu | 0:fbfafa6bf5f9 | 1 | |
| narshu | 0:fbfafa6bf5f9 | 2 | #include "RFSRF05.h" |
| narshu | 0:fbfafa6bf5f9 | 3 | #include "mbed.h" |
| narshu | 0:fbfafa6bf5f9 | 4 | #include "globals.h" |
| narshu | 0:fbfafa6bf5f9 | 5 | |
| narshu | 0:fbfafa6bf5f9 | 6 | RFSRF05::RFSRF05(PinName trigger, |
| narshu | 0:fbfafa6bf5f9 | 7 | PinName echo0, |
| narshu | 0:fbfafa6bf5f9 | 8 | PinName echo1, |
| narshu | 0:fbfafa6bf5f9 | 9 | PinName echo2, |
| narshu | 0:fbfafa6bf5f9 | 10 | PinName echo3, |
| narshu | 0:fbfafa6bf5f9 | 11 | PinName echo4, |
| narshu | 0:fbfafa6bf5f9 | 12 | PinName echo5, |
| narshu | 0:fbfafa6bf5f9 | 13 | PinName SDI, |
| narshu | 0:fbfafa6bf5f9 | 14 | PinName SDO, |
| narshu | 0:fbfafa6bf5f9 | 15 | PinName SCK, |
| narshu | 0:fbfafa6bf5f9 | 16 | PinName NCS, |
| narshu | 0:fbfafa6bf5f9 | 17 | PinName NIRQ) |
| narshu | 0:fbfafa6bf5f9 | 18 | : _rf(SDI,SDO,SCK,NCS,NIRQ), |
| narshu | 0:fbfafa6bf5f9 | 19 | _trigger(trigger), |
| narshu | 0:fbfafa6bf5f9 | 20 | _echo0(echo0), |
| narshu | 0:fbfafa6bf5f9 | 21 | _echo1(echo1), |
| narshu | 0:fbfafa6bf5f9 | 22 | _echo2(echo2), |
| narshu | 0:fbfafa6bf5f9 | 23 | _echo3(echo3), |
| narshu | 0:fbfafa6bf5f9 | 24 | _echo4(echo4), |
| narshu | 0:fbfafa6bf5f9 | 25 | _echo5(echo5) { |
| narshu | 0:fbfafa6bf5f9 | 26 | |
| narshu | 0:fbfafa6bf5f9 | 27 | // initialises codes |
| narshu | 0:fbfafa6bf5f9 | 28 | _code[0] = CODE0; |
| narshu | 0:fbfafa6bf5f9 | 29 | _code[1] = CODE1; |
| narshu | 0:fbfafa6bf5f9 | 30 | _code[2] = CODE2; |
| narshu | 0:fbfafa6bf5f9 | 31 | |
| narshu | 0:fbfafa6bf5f9 | 32 | //set callback execute to true |
| narshu | 0:fbfafa6bf5f9 | 33 | ValidPulse = false; |
| narshu | 0:fbfafa6bf5f9 | 34 | |
| narshu | 0:fbfafa6bf5f9 | 35 | // Attach interrupts |
| narshu | 0:fbfafa6bf5f9 | 36 | _echo0.rise(this, &RFSRF05::_rising); |
| narshu | 0:fbfafa6bf5f9 | 37 | _echo0.fall(this, &RFSRF05::_falling); |
| narshu | 0:fbfafa6bf5f9 | 38 | _echo1.fall(this, &RFSRF05::_falling); |
| narshu | 0:fbfafa6bf5f9 | 39 | _echo2.fall(this, &RFSRF05::_falling); |
| narshu | 0:fbfafa6bf5f9 | 40 | _echo3.fall(this, &RFSRF05::_falling); |
| narshu | 0:fbfafa6bf5f9 | 41 | _echo4.fall(this, &RFSRF05::_falling); |
| narshu | 0:fbfafa6bf5f9 | 42 | _echo5.fall(this, &RFSRF05::_falling); |
| narshu | 0:fbfafa6bf5f9 | 43 | |
| narshu | 0:fbfafa6bf5f9 | 44 | |
| narshu | 0:fbfafa6bf5f9 | 45 | //init callabck function |
| narshu | 0:fbfafa6bf5f9 | 46 | callbackfunc = NULL; |
| narshu | 0:fbfafa6bf5f9 | 47 | callbackobj = NULL; |
| narshu | 0:fbfafa6bf5f9 | 48 | mcallbackfunc = NULL; |
| narshu | 0:fbfafa6bf5f9 | 49 | |
| narshu | 0:fbfafa6bf5f9 | 50 | // innitialises beacon counter |
| narshu | 0:fbfafa6bf5f9 | 51 | _beacon_counter = 0; |
| narshu | 0:fbfafa6bf5f9 | 52 | |
| narshu | 0:fbfafa6bf5f9 | 53 | //Interrupts every 50ms |
| narshu | 0:fbfafa6bf5f9 | 54 | _ticker.attach(this, &RFSRF05::_startRange, 0.05); |
| narshu | 0:fbfafa6bf5f9 | 55 | } |
| narshu | 0:fbfafa6bf5f9 | 56 | |
| narshu | 0:fbfafa6bf5f9 | 57 | |
| narshu | 0:fbfafa6bf5f9 | 58 | void RFSRF05::_startRange() { |
| narshu | 0:fbfafa6bf5f9 | 59 | |
| narshu | 0:fbfafa6bf5f9 | 60 | //printf("Srange\r\r"); |
| narshu | 0:fbfafa6bf5f9 | 61 | |
| narshu | 0:fbfafa6bf5f9 | 62 | // increments counter |
| narshu | 0:fbfafa6bf5f9 | 63 | _beacon_counter = (_beacon_counter + 1) % 3; |
| narshu | 0:fbfafa6bf5f9 | 64 | |
| narshu | 0:fbfafa6bf5f9 | 65 | |
| narshu | 0:fbfafa6bf5f9 | 66 | // set flags |
| narshu | 0:fbfafa6bf5f9 | 67 | ValidPulse = false; |
| narshu | 0:fbfafa6bf5f9 | 68 | expValidPulse = true; |
| narshu | 0:fbfafa6bf5f9 | 69 | |
| narshu | 0:fbfafa6bf5f9 | 70 | // writes code to RF port |
| narshu | 0:fbfafa6bf5f9 | 71 | _rf.write(_code[_beacon_counter]); |
| narshu | 0:fbfafa6bf5f9 | 72 | |
| narshu | 0:fbfafa6bf5f9 | 73 | |
| narshu | 0:fbfafa6bf5f9 | 74 | |
| narshu | 0:fbfafa6bf5f9 | 75 | // send a trigger pulse, 10uS long |
| narshu | 0:fbfafa6bf5f9 | 76 | _trigger = 1; |
| narshu | 0:fbfafa6bf5f9 | 77 | wait_us (10); |
| narshu | 0:fbfafa6bf5f9 | 78 | _trigger = 0; |
| narshu | 0:fbfafa6bf5f9 | 79 | |
| narshu | 0:fbfafa6bf5f9 | 80 | } |
| narshu | 0:fbfafa6bf5f9 | 81 | |
| narshu | 0:fbfafa6bf5f9 | 82 | |
| narshu | 0:fbfafa6bf5f9 | 83 | // Clear and start the timer at the begining of the echo pulse |
| narshu | 0:fbfafa6bf5f9 | 84 | void RFSRF05::_rising(void) { |
| narshu | 0:fbfafa6bf5f9 | 85 | |
| narshu | 0:fbfafa6bf5f9 | 86 | _timer.reset(); |
| narshu | 0:fbfafa6bf5f9 | 87 | _timer.start(); |
| narshu | 0:fbfafa6bf5f9 | 88 | |
| narshu | 0:fbfafa6bf5f9 | 89 | //Set callback execute to ture |
| narshu | 0:fbfafa6bf5f9 | 90 | if (expValidPulse) { |
| narshu | 0:fbfafa6bf5f9 | 91 | ValidPulse = true; |
| narshu | 0:fbfafa6bf5f9 | 92 | expValidPulse = false; |
| narshu | 0:fbfafa6bf5f9 | 93 | } |
| narshu | 0:fbfafa6bf5f9 | 94 | } |
| narshu | 0:fbfafa6bf5f9 | 95 | |
| narshu | 0:fbfafa6bf5f9 | 96 | // Stop and read the timer at the end of the pulse |
| narshu | 0:fbfafa6bf5f9 | 97 | void RFSRF05::_falling(void) { |
| narshu | 0:fbfafa6bf5f9 | 98 | _timer.stop(); |
| narshu | 0:fbfafa6bf5f9 | 99 | |
| narshu | 0:fbfafa6bf5f9 | 100 | if (ValidPulse) { |
| narshu | 0:fbfafa6bf5f9 | 101 | //printf("Validpulse trig!\r\n"); |
| narshu | 0:fbfafa6bf5f9 | 102 | ValidPulse = false; |
| narshu | 0:fbfafa6bf5f9 | 103 | |
| narshu | 0:fbfafa6bf5f9 | 104 | //Calucate distance |
| narshu | 0:fbfafa6bf5f9 | 105 | _dist[_beacon_counter] = _timer.read_us()/2.9 + 300; |
| narshu | 0:fbfafa6bf5f9 | 106 | |
| narshu | 0:fbfafa6bf5f9 | 107 | if (callbackfunc) |
| narshu | 0:fbfafa6bf5f9 | 108 | (*callbackfunc)(_beacon_counter, _dist[_beacon_counter]); |
| narshu | 0:fbfafa6bf5f9 | 109 | |
| narshu | 0:fbfafa6bf5f9 | 110 | if (callbackobj && mcallbackfunc) |
| narshu | 0:fbfafa6bf5f9 | 111 | (callbackobj->*mcallbackfunc)(_beacon_counter, _dist[_beacon_counter], sonarvariance); |
| narshu | 0:fbfafa6bf5f9 | 112 | |
| narshu | 0:fbfafa6bf5f9 | 113 | } |
| narshu | 0:fbfafa6bf5f9 | 114 | |
| narshu | 0:fbfafa6bf5f9 | 115 | } |
| narshu | 0:fbfafa6bf5f9 | 116 | |
| narshu | 0:fbfafa6bf5f9 | 117 | float RFSRF05::read0() { |
| narshu | 0:fbfafa6bf5f9 | 118 | // returns distance |
| narshu | 0:fbfafa6bf5f9 | 119 | return (_dist[0]); |
| narshu | 0:fbfafa6bf5f9 | 120 | } |
| narshu | 0:fbfafa6bf5f9 | 121 | |
| narshu | 0:fbfafa6bf5f9 | 122 | float RFSRF05::read1() { |
| narshu | 0:fbfafa6bf5f9 | 123 | // returns distance |
| narshu | 0:fbfafa6bf5f9 | 124 | return (_dist[1]); |
| narshu | 0:fbfafa6bf5f9 | 125 | } |
| narshu | 0:fbfafa6bf5f9 | 126 | |
| narshu | 0:fbfafa6bf5f9 | 127 | float RFSRF05::read2() { |
| narshu | 0:fbfafa6bf5f9 | 128 | // returns distance |
| narshu | 0:fbfafa6bf5f9 | 129 | return (_dist[2]); |
| narshu | 0:fbfafa6bf5f9 | 130 | } |
| narshu | 0:fbfafa6bf5f9 | 131 | |
| narshu | 0:fbfafa6bf5f9 | 132 | float RFSRF05::read(unsigned int beaconnum) { |
| narshu | 0:fbfafa6bf5f9 | 133 | // returns distance |
| narshu | 0:fbfafa6bf5f9 | 134 | return (_dist[beaconnum]); |
| narshu | 0:fbfafa6bf5f9 | 135 | } |
| narshu | 0:fbfafa6bf5f9 | 136 | |
| narshu | 0:fbfafa6bf5f9 | 137 | //SRF05::operator float() { |
| narshu | 0:fbfafa6bf5f9 | 138 | // return read(); |
| narshu | 0:fbfafa6bf5f9 | 139 | //} |