Shuto Naruse / Mbed 2 deprecated Eurobot2012_Primary

Eurobot2012_Primary

Dependencies:   mbed Eurobot_2012_Primary

Eurobot_shared/Kalman/Sonar/RFSRF05.cpp@24:7a3906c2f5d5, 2012-05-04 (annotated)

Committer:
narshu
Date:
Fri May 04 05:23:45 2012 +0000
Revision:
24:7a3906c2f5d5
1st working version with accurate target acquisition.

Who changed what in which revision?

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