Shuto Naruse / Sonar

RFSRF05.cpp@0:7583de124698, 2012-03-30 (annotated)

Committer:
narshu
Date:
Fri Mar 30 19:44:25 2012 +0000
Revision:
0:7583de124698
Child:
1:0370ea94b64a
Sonar Library for EUROBOT 2012

Who changed what in which revision?

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