Shuto Naruse / Mbed 2 deprecated Eurobot2012_Primary

Eurobot2012_Primary

Dependencies:   mbed Eurobot_2012_Primary

Eurobot_shared/Kalman/Sonar/RFSRF05.cpp@9:377560539b74, 2012-04-28 (annotated)

Committer:
narshu
Date:
Sat Apr 28 17:21:24 2012 +0000
Revision:
9:377560539b74
Parent:
Kalman/Sonar/RFSRF05.cpp@8:ffc7d8af2d5a
Child:
15:acae5c0e9ca8
Restructured project to have a single shared lib; Also raised the RF baud rate

Who changed what in which revision?

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