Shuto Naruse / Mbed 2 deprecated Eurobot2012_Primary

Eurobot2012_Primary

Dependencies:   mbed Eurobot_2012_Primary

Eurobot_shared/Kalman/Sonar/RFSRF05.cpp@22:7ba09c0af0d0, 2012-05-03 (annotated)

Committer:
narshu
Date:
Thu May 03 14:20:04 2012 +0000
Revision:
22:7ba09c0af0d0
Parent:
17:bafcef1c3579 
added 90sec timer and tigger

Who changed what in which revision?

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