Shuto Naruse / Mbed 2 deprecated Eurobot2012_Secondary

Eurobot2012_Secondary

Fork of Eurobot_2012_Secondary by Shuto Naruse

Eurobot_shared/Kalman/Sonar/RFSRF05.cpp@1:cc2a9eb0bd55, 2012-10-17 (annotated)

Committer:
narshu
Date:
Wed Oct 17 22:25:31 2012 +0000
Revision:
1:cc2a9eb0bd55
Commit before publishing

Who changed what in which revision?

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