Shuto Naruse / Mbed 2 deprecated Eurobot2012_Primary

Eurobot2012_Primary

Dependencies:   mbed Eurobot_2012_Primary

Kalman/Sonar/RFSRF05.cpp@0:f3bf6c7e2283, 2012-04-20 (annotated)

Committer:
narshu
Date:
Fri Apr 20 20:39:35 2012 +0000
Revision:
0:f3bf6c7e2283
Child:
1:bbabbd997d21
inverted sonar echo input at pin14 and pin15

Who changed what in which revision?

UserRevisionLine numberNew contents of line
narshu 0:f3bf6c7e2283 1
narshu 0:f3bf6c7e2283 2 #include "RFSRF05.h"
narshu 0:f3bf6c7e2283 3 #include "mbed.h"
narshu 0:f3bf6c7e2283 4 #include "globals.h"
narshu 0:f3bf6c7e2283 5 DigitalOut GLED3(LED3);
narshu 0:f3bf6c7e2283 6 DigitalOut GLED4(LED4);
narshu 0:f3bf6c7e2283 7
narshu 0:f3bf6c7e2283 8 RFSRF05::RFSRF05(PinName trigger,
narshu 0:f3bf6c7e2283 9 PinName echo0,
narshu 0:f3bf6c7e2283 10 PinName echo1,
narshu 0:f3bf6c7e2283 11 PinName echo2,
narshu 0:f3bf6c7e2283 12 PinName echo3,
narshu 0:f3bf6c7e2283 13 PinName echo4,
narshu 0:f3bf6c7e2283 14 PinName echo5,
narshu 0:f3bf6c7e2283 15 PinName SDI,
narshu 0:f3bf6c7e2283 16 PinName SDO,
narshu 0:f3bf6c7e2283 17 PinName SCK,
narshu 0:f3bf6c7e2283 18 PinName NCS,
narshu 0:f3bf6c7e2283 19 PinName NIRQ)
narshu 0:f3bf6c7e2283 20 : _rf(SDI,SDO,SCK,NCS,NIRQ),
narshu 0:f3bf6c7e2283 21 _trigger(trigger),
narshu 0:f3bf6c7e2283 22 _echo0(echo0),
narshu 0:f3bf6c7e2283 23 _echo1(echo1),
narshu 0:f3bf6c7e2283 24 _echo2(echo2),
narshu 0:f3bf6c7e2283 25 _echo3(echo3),
narshu 0:f3bf6c7e2283 26 _echo4(echo4),
narshu 0:f3bf6c7e2283 27 _echo5(echo5) {
narshu 0:f3bf6c7e2283 28
narshu 0:f3bf6c7e2283 29 // initialises codes
narshu 0:f3bf6c7e2283 30 _code[0] = CODE0;
narshu 0:f3bf6c7e2283 31 _code[1] = CODE1;
narshu 0:f3bf6c7e2283 32 _code[2] = CODE2;
narshu 0:f3bf6c7e2283 33
narshu 0:f3bf6c7e2283 34 //set callback execute to true
narshu 0:f3bf6c7e2283 35 ValidPulse = false;
narshu 0:f3bf6c7e2283 36
narshu 0:f3bf6c7e2283 37 // Attach interrupts
narshu 0:f3bf6c7e2283 38 _echo5.fall(this, &RFSRF05::_rising);
narshu 0:f3bf6c7e2283 39 _echo0.rise(this, &RFSRF05::_falling);
narshu 0:f3bf6c7e2283 40 _echo1.rise(this, &RFSRF05::_falling);
narshu 0:f3bf6c7e2283 41 _echo2.rise(this, &RFSRF05::_falling);
narshu 0:f3bf6c7e2283 42 _echo3.rise(this, &RFSRF05::_falling);
narshu 0:f3bf6c7e2283 43 _echo4.rise(this, &RFSRF05::_falling);
narshu 0:f3bf6c7e2283 44 _echo5.rise(this, &RFSRF05::_falling);
narshu 0:f3bf6c7e2283 45
narshu 0:f3bf6c7e2283 46
narshu 0:f3bf6c7e2283 47 //init callabck function
narshu 0:f3bf6c7e2283 48 callbackfunc = NULL;
narshu 0:f3bf6c7e2283 49 callbackobj = NULL;
narshu 0:f3bf6c7e2283 50 mcallbackfunc = NULL;
narshu 0:f3bf6c7e2283 51
narshu 0:f3bf6c7e2283 52 // innitialises beacon counter
narshu 0:f3bf6c7e2283 53 _beacon_counter = 0;
narshu 0:f3bf6c7e2283 54
narshu 0:f3bf6c7e2283 55 //Interrupts every 50ms
narshu 0:f3bf6c7e2283 56 _ticker.attach(this, &RFSRF05::_startRange, 0.05);
narshu 0:f3bf6c7e2283 57 }
narshu 0:f3bf6c7e2283 58
narshu 0:f3bf6c7e2283 59
narshu 0:f3bf6c7e2283 60 void RFSRF05::_startRange() {
narshu 0:f3bf6c7e2283 61
narshu 0:f3bf6c7e2283 62 //printf("Srange\r\r");
narshu 0:f3bf6c7e2283 63
narshu 0:f3bf6c7e2283 64 // increments counter
narshu 0:f3bf6c7e2283 65 _beacon_counter = (_beacon_counter + 1) % 3;
narshu 0:f3bf6c7e2283 66
narshu 0:f3bf6c7e2283 67 // writes code to RF port
narshu 0:f3bf6c7e2283 68 _rf.write(_code[_beacon_counter]);
narshu 0:f3bf6c7e2283 69
narshu 0:f3bf6c7e2283 70 // send a trigger pulse, 10uS long
narshu 0:f3bf6c7e2283 71 ValidPulse = false;
narshu 0:f3bf6c7e2283 72 expValidPulse = true;
narshu 0:f3bf6c7e2283 73
narshu 0:f3bf6c7e2283 74 _trigger = 1;
narshu 0:f3bf6c7e2283 75 wait_us (10);
narshu 0:f3bf6c7e2283 76 _trigger = 0;
narshu 0:f3bf6c7e2283 77 wait_us(50);
narshu 0:f3bf6c7e2283 78 }
narshu 0:f3bf6c7e2283 79
narshu 0:f3bf6c7e2283 80
narshu 0:f3bf6c7e2283 81 // Clear and start the timer at the begining of the echo pulse
narshu 0:f3bf6c7e2283 82 void RFSRF05::_rising(void) {
narshu 0:f3bf6c7e2283 83
narshu 0:f3bf6c7e2283 84 _timer.reset();
narshu 0:f3bf6c7e2283 85 _timer.start();
narshu 0:f3bf6c7e2283 86
narshu 0:f3bf6c7e2283 87 //Set callback execute to ture
narshu 0:f3bf6c7e2283 88 if (expValidPulse) {
narshu 0:f3bf6c7e2283 89 ValidPulse = true;
narshu 0:f3bf6c7e2283 90 expValidPulse = false;
narshu 0:f3bf6c7e2283 91 }
narshu 0:f3bf6c7e2283 92 }
narshu 0:f3bf6c7e2283 93
narshu 0:f3bf6c7e2283 94 // Stop and read the timer at the end of the pulse
narshu 0:f3bf6c7e2283 95 void RFSRF05::_falling(void) {
narshu 0:f3bf6c7e2283 96 _timer.stop();
narshu 0:f3bf6c7e2283 97
narshu 0:f3bf6c7e2283 98 if (ValidPulse) {
narshu 0:f3bf6c7e2283 99 //printf("Validpulse trig!\r\n");
narshu 0:f3bf6c7e2283 100 ValidPulse = false;
narshu 0:f3bf6c7e2283 101
narshu 0:f3bf6c7e2283 102 //Calucate distance
narshu 0:f3bf6c7e2283 103 _dist[_beacon_counter] = _timer.read_us()/2.9 + 300;
narshu 0:f3bf6c7e2283 104
narshu 0:f3bf6c7e2283 105 if (callbackfunc)
narshu 0:f3bf6c7e2283 106 (*callbackfunc)(_beacon_counter, _dist[_beacon_counter]);
narshu 0:f3bf6c7e2283 107
narshu 0:f3bf6c7e2283 108 if (callbackobj && mcallbackfunc)
narshu 0:f3bf6c7e2283 109 (callbackobj->*mcallbackfunc)(_beacon_counter, _dist[_beacon_counter], sonarvariance);
narshu 0:f3bf6c7e2283 110
narshu 0:f3bf6c7e2283 111 }
narshu 0:f3bf6c7e2283 112
narshu 0:f3bf6c7e2283 113 }
narshu 0:f3bf6c7e2283 114
narshu 0:f3bf6c7e2283 115 float RFSRF05::read0() {
narshu 0:f3bf6c7e2283 116 // returns distance
narshu 0:f3bf6c7e2283 117 return (_dist[0]);
narshu 0:f3bf6c7e2283 118 }
narshu 0:f3bf6c7e2283 119
narshu 0:f3bf6c7e2283 120 float RFSRF05::read1() {
narshu 0:f3bf6c7e2283 121 // returns distance
narshu 0:f3bf6c7e2283 122 return (_dist[1]);
narshu 0:f3bf6c7e2283 123 }
narshu 0:f3bf6c7e2283 124
narshu 0:f3bf6c7e2283 125 float RFSRF05::read2() {
narshu 0:f3bf6c7e2283 126 // returns distance
narshu 0:f3bf6c7e2283 127 return (_dist[2]);
narshu 0:f3bf6c7e2283 128 }
narshu 0:f3bf6c7e2283 129
narshu 0:f3bf6c7e2283 130 float RFSRF05::read(unsigned int beaconnum) {
narshu 0:f3bf6c7e2283 131 // returns distance
narshu 0:f3bf6c7e2283 132 return (_dist[beaconnum]);
narshu 0:f3bf6c7e2283 133 }
narshu 0:f3bf6c7e2283 134
narshu 0:f3bf6c7e2283 135 //SRF05::operator float() {
narshu 0:f3bf6c7e2283 136 // return read();
narshu 0:f3bf6c7e2283 137 //}