Oskar Weigl / Kalman

Kalman filter for Eurobot

RFSRF05/RFSRF05.cpp@0:a0285293f6a6, 2012-03-20 (annotated)

Committer:
madcowswe
Date:
Tue Mar 20 12:43:16 2012 +0000
Revision:
0:a0285293f6a6
Algo done, vars not done

Who changed what in which revision?

UserRevisionLine numberNew contents of line
madcowswe 0:a0285293f6a6 1 /* mbed SRF05 Ultrasonic Rangefiner Library
madcowswe 0:a0285293f6a6 2 * Copyright (c) 2007-2010, cstyles, sford
madcowswe 0:a0285293f6a6 3 *
madcowswe 0:a0285293f6a6 4 * Permission is hereby granted, free of charge, to any person obtaining a copy
madcowswe 0:a0285293f6a6 5 * of this software and associated documentation files (the "Software"), to deal
madcowswe 0:a0285293f6a6 6 * in the Software without restriction, including without limitation the rights
madcowswe 0:a0285293f6a6 7 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
madcowswe 0:a0285293f6a6 8 * copies of the Software, and to permit persons to whom the Software is
madcowswe 0:a0285293f6a6 9 * furnished to do so, subject to the following conditions:
madcowswe 0:a0285293f6a6 10 *
madcowswe 0:a0285293f6a6 11 * The above copyright notice and this permission notice shall be included in
madcowswe 0:a0285293f6a6 12 * all copies or substantial portions of the Software.
madcowswe 0:a0285293f6a6 13 *
madcowswe 0:a0285293f6a6 14 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
madcowswe 0:a0285293f6a6 15 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
madcowswe 0:a0285293f6a6 16 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
madcowswe 0:a0285293f6a6 17 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
madcowswe 0:a0285293f6a6 18 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
madcowswe 0:a0285293f6a6 19 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
madcowswe 0:a0285293f6a6 20 * THE SOFTWARE.
madcowswe 0:a0285293f6a6 21 */
madcowswe 0:a0285293f6a6 22
madcowswe 0:a0285293f6a6 23 #include "RFSRF05.h"
madcowswe 0:a0285293f6a6 24 #include "mbed.h"
madcowswe 0:a0285293f6a6 25
madcowswe 0:a0285293f6a6 26 RFSRF05::RFSRF05(PinName trigger,
madcowswe 0:a0285293f6a6 27 PinName echo0,
madcowswe 0:a0285293f6a6 28 PinName echo1,
madcowswe 0:a0285293f6a6 29 PinName echo2,
madcowswe 0:a0285293f6a6 30 PinName echo3,
madcowswe 0:a0285293f6a6 31 PinName echo4,
madcowswe 0:a0285293f6a6 32 PinName echo5,
madcowswe 0:a0285293f6a6 33 PinName SDI,
madcowswe 0:a0285293f6a6 34 PinName SDO,
madcowswe 0:a0285293f6a6 35 PinName SCK,
madcowswe 0:a0285293f6a6 36 PinName NCS,
madcowswe 0:a0285293f6a6 37 PinName NIRQ)
madcowswe 0:a0285293f6a6 38 : _trigger(trigger),
madcowswe 0:a0285293f6a6 39 _echo0(echo0),
madcowswe 0:a0285293f6a6 40 _echo1(echo1),
madcowswe 0:a0285293f6a6 41 _echo2(echo2),
madcowswe 0:a0285293f6a6 42 _echo3(echo3),
madcowswe 0:a0285293f6a6 43 _echo4(echo4),
madcowswe 0:a0285293f6a6 44 _echo5(echo5),
madcowswe 0:a0285293f6a6 45 _rf(SDI,SDO,SCK,NCS,NIRQ) {
madcowswe 0:a0285293f6a6 46
madcowswe 0:a0285293f6a6 47
madcowswe 0:a0285293f6a6 48 // initialises codes
madcowswe 0:a0285293f6a6 49 _code[0] = CODE0;
madcowswe 0:a0285293f6a6 50 _code[1] = CODE1;
madcowswe 0:a0285293f6a6 51 _code[2] = CODE2;
madcowswe 0:a0285293f6a6 52
madcowswe 0:a0285293f6a6 53 //set callback execute to true
madcowswe 0:a0285293f6a6 54 ValidPulse = true;
madcowswe 0:a0285293f6a6 55
madcowswe 0:a0285293f6a6 56 // Attach interrupts
madcowswe 0:a0285293f6a6 57 _echo0.rise(this, &RFSRF05::_rising);
madcowswe 0:a0285293f6a6 58 _echo0.fall(this, &RFSRF05::_falling);
madcowswe 0:a0285293f6a6 59 _echo1.fall(this, &RFSRF05::_falling);
madcowswe 0:a0285293f6a6 60 _echo2.fall(this, &RFSRF05::_falling);
madcowswe 0:a0285293f6a6 61 _echo3.fall(this, &RFSRF05::_falling);
madcowswe 0:a0285293f6a6 62 _echo4.fall(this, &RFSRF05::_falling);
madcowswe 0:a0285293f6a6 63 _echo5.fall(this, &RFSRF05::_falling);
madcowswe 0:a0285293f6a6 64
madcowswe 0:a0285293f6a6 65 //init callabck function
madcowswe 0:a0285293f6a6 66 callbackfunc = NULL;
madcowswe 0:a0285293f6a6 67 callbackobj = NULL;
madcowswe 0:a0285293f6a6 68 mcallbackfunc = NULL;
madcowswe 0:a0285293f6a6 69
madcowswe 0:a0285293f6a6 70 // innitialises beacon counter
madcowswe 0:a0285293f6a6 71 _beacon_counter = 0;
madcowswe 0:a0285293f6a6 72
madcowswe 0:a0285293f6a6 73 //Interrupts every 50ms
madcowswe 0:a0285293f6a6 74 _ticker.attach(this, &RFSRF05::_startRange, 0.05);
madcowswe 0:a0285293f6a6 75 }
madcowswe 0:a0285293f6a6 76
madcowswe 0:a0285293f6a6 77 void RFSRF05::_startRange() {
madcowswe 0:a0285293f6a6 78
madcowswe 0:a0285293f6a6 79 // increments counter
madcowswe 0:a0285293f6a6 80 _beacon_counter = (_beacon_counter + 1) % 3;
madcowswe 0:a0285293f6a6 81
madcowswe 0:a0285293f6a6 82 // writes code to RF port
madcowswe 0:a0285293f6a6 83 _rf.write(_code[_beacon_counter]);
madcowswe 0:a0285293f6a6 84
madcowswe 0:a0285293f6a6 85 // send a trigger pulse, 10uS long
madcowswe 0:a0285293f6a6 86 ValidPulse = false;
madcowswe 0:a0285293f6a6 87
madcowswe 0:a0285293f6a6 88 _trigger = 1;
madcowswe 0:a0285293f6a6 89 wait_us (10);
madcowswe 0:a0285293f6a6 90 _trigger = 0;
madcowswe 0:a0285293f6a6 91 }
madcowswe 0:a0285293f6a6 92
madcowswe 0:a0285293f6a6 93 // Clear and start the timer at the begining of the echo pulse
madcowswe 0:a0285293f6a6 94 void RFSRF05::_rising(void) {
madcowswe 0:a0285293f6a6 95
madcowswe 0:a0285293f6a6 96 _timer.reset();
madcowswe 0:a0285293f6a6 97 _timer.start();
madcowswe 0:a0285293f6a6 98
madcowswe 0:a0285293f6a6 99 //Set callback execute to ture
madcowswe 0:a0285293f6a6 100 ValidPulse = true;
madcowswe 0:a0285293f6a6 101 }
madcowswe 0:a0285293f6a6 102
madcowswe 0:a0285293f6a6 103 // Stop and read the timer at the end of the pulse
madcowswe 0:a0285293f6a6 104 void RFSRF05::_falling(void) {
madcowswe 0:a0285293f6a6 105 _timer.stop();
madcowswe 0:a0285293f6a6 106
madcowswe 0:a0285293f6a6 107 if (ValidPulse) {
madcowswe 0:a0285293f6a6 108 ValidPulse = false;
madcowswe 0:a0285293f6a6 109
madcowswe 0:a0285293f6a6 110 //Calucate distance
madcowswe 0:a0285293f6a6 111 _dist[_beacon_counter] = _timer.read_us()/29.0;
madcowswe 0:a0285293f6a6 112
madcowswe 0:a0285293f6a6 113 if (callbackfunc)
madcowswe 0:a0285293f6a6 114 (*callbackfunc)(_beacon_counter, _dist[_beacon_counter]);
madcowswe 0:a0285293f6a6 115
madcowswe 0:a0285293f6a6 116 if (callbackobj && mcallbackfunc)
madcowswe 0:a0285293f6a6 117 (callbackobj->*mcallbackfunc)(_beacon_counter, _dist[_beacon_counter]);
madcowswe 0:a0285293f6a6 118
madcowswe 0:a0285293f6a6 119 }
madcowswe 0:a0285293f6a6 120
madcowswe 0:a0285293f6a6 121 }
madcowswe 0:a0285293f6a6 122
madcowswe 0:a0285293f6a6 123 float RFSRF05::read0() {
madcowswe 0:a0285293f6a6 124 // returns distance
madcowswe 0:a0285293f6a6 125 return (_dist[0]);
madcowswe 0:a0285293f6a6 126 }
madcowswe 0:a0285293f6a6 127
madcowswe 0:a0285293f6a6 128 float RFSRF05::read1() {
madcowswe 0:a0285293f6a6 129 // returns distance
madcowswe 0:a0285293f6a6 130 return (_dist[1]);
madcowswe 0:a0285293f6a6 131 }
madcowswe 0:a0285293f6a6 132
madcowswe 0:a0285293f6a6 133 float RFSRF05::read2() {
madcowswe 0:a0285293f6a6 134 // returns distance
madcowswe 0:a0285293f6a6 135 return (_dist[2]);
madcowswe 0:a0285293f6a6 136 }
madcowswe 0:a0285293f6a6 137
madcowswe 0:a0285293f6a6 138 //SRF05::operator float() {
madcowswe 0:a0285293f6a6 139 // return read();
madcowswe 0:a0285293f6a6 140 //}