Cooper Liu / Mbed 2 deprecated Eurobot2013_Co-Processor

working version with calibration done

Fork of Eurobot2013 by Oskar Weigl

Sonar/RFSRF05.cpp@11:5ba926692210, 2013-04-09 (annotated)

Committer:
xiaxia686
Date:
Tue Apr 09 15:32:47 2013 +0000
Revision:
11:5ba926692210
Parent:
10:2bd9f4e02b74 
woking version (calibrated)

Who changed what in which revision?

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