Kalman filter for Eurobot

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers RFSRF05.cpp Source File

RFSRF05.cpp

00001 /* mbed SRF05 Ultrasonic Rangefiner Library
00002  * Copyright (c) 2007-2010, cstyles, sford
00003  *
00004  * Permission is hereby granted, free of charge, to any person obtaining a copy
00005  * of this software and associated documentation files (the "Software"), to deal
00006  * in the Software without restriction, including without limitation the rights
00007  * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
00008  * copies of the Software, and to permit persons to whom the Software is
00009  * furnished to do so, subject to the following conditions:
00010  *
00011  * The above copyright notice and this permission notice shall be included in
00012  * all copies or substantial portions of the Software.
00013  *
00014  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
00015  * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
00016  * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
00017  * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
00018  * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
00019  * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
00020  * THE SOFTWARE.
00021  */
00022 
00023 #include "RFSRF05.h"
00024 #include "mbed.h"
00025 
00026 RFSRF05::RFSRF05(PinName trigger,
00027                  PinName echo0,
00028                  PinName echo1,
00029                  PinName echo2,
00030                  PinName echo3,
00031                  PinName echo4,
00032                  PinName echo5,
00033                  PinName SDI,
00034                  PinName SDO,
00035                  PinName SCK,
00036                  PinName NCS,
00037                  PinName NIRQ)
00038         : _trigger(trigger),
00039         _echo0(echo0),
00040         _echo1(echo1),
00041         _echo2(echo2),
00042         _echo3(echo3),
00043         _echo4(echo4),
00044         _echo5(echo5),
00045         _rf(SDI,SDO,SCK,NCS,NIRQ) {
00046 
00047 
00048     // initialises codes
00049     _code[0] = CODE0;
00050     _code[1] = CODE1;
00051     _code[2] = CODE2;
00052 
00053     //set callback execute to true
00054     ValidPulse = true;
00055 
00056     // Attach interrupts
00057     _echo0.rise(this, &RFSRF05::_rising);
00058     _echo0.fall(this, &RFSRF05::_falling);
00059     _echo1.fall(this, &RFSRF05::_falling);
00060     _echo2.fall(this, &RFSRF05::_falling);
00061     _echo3.fall(this, &RFSRF05::_falling);
00062     _echo4.fall(this, &RFSRF05::_falling);
00063     _echo5.fall(this, &RFSRF05::_falling);
00064 
00065     //init callabck function
00066     callbackfunc = NULL;
00067     callbackobj = NULL;
00068     mcallbackfunc = NULL;
00069 
00070     // innitialises beacon counter
00071     _beacon_counter = 0;
00072 
00073     //Interrupts every 50ms
00074     _ticker.attach(this, &RFSRF05::_startRange, 0.05);
00075 }
00076 
00077 void RFSRF05::_startRange() {
00078 
00079     // increments counter
00080     _beacon_counter = (_beacon_counter + 1) % 3;
00081 
00082     // writes code to RF port
00083     _rf.write(_code[_beacon_counter]);
00084 
00085     // send a trigger pulse, 10uS long
00086     ValidPulse = false;
00087     
00088     _trigger = 1;
00089     wait_us (10);
00090     _trigger = 0;
00091 }
00092 
00093 // Clear and start the timer at the begining of the echo pulse
00094 void RFSRF05::_rising(void) {
00095 
00096     _timer.reset();
00097     _timer.start();
00098     
00099     //Set callback execute to ture
00100     ValidPulse = true;
00101 }
00102 
00103 // Stop and read the timer at the end of the pulse
00104 void RFSRF05::_falling(void) {
00105     _timer.stop();
00106 
00107     if (ValidPulse) {
00108         ValidPulse = false;
00109     
00110         //Calucate distance
00111         _dist[_beacon_counter] =  _timer.read_us()/29.0;
00112     
00113         if (callbackfunc)
00114             (*callbackfunc)(_beacon_counter, _dist[_beacon_counter]);
00115             
00116         if (callbackobj && mcallbackfunc)
00117             (callbackobj->*mcallbackfunc)(_beacon_counter, _dist[_beacon_counter]);
00118 
00119     }
00120     
00121 }
00122 
00123 float RFSRF05::read0() {
00124     // returns distance
00125     return (_dist[0]);
00126 }
00127 
00128 float RFSRF05::read1() {
00129     // returns distance
00130     return (_dist[1]);
00131 }
00132 
00133 float RFSRF05::read2() {
00134     // returns distance
00135     return (_dist[2]);
00136 }
00137 
00138 //SRF05::operator float() {
00139 //    return read();
00140 //}