Cooper Liu / Mbed 2 deprecated Eurobot2013_Co-Processor

working version with calibration done

Fork of Eurobot2013 by Oskar Weigl

Kalman/Sonar/RFSRF05.cpp@1:6799c07fe510, 2012-11-07 (annotated)

Committer:
sv
Date:
Wed Nov 07 14:37:35 2012 +0000
Revision:
1:6799c07fe510
Preliminary copy of 2012 code

Who changed what in which revision?

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