Kalman filter for Eurobot
Embed:
(wiki syntax)
Show/hide line numbers
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 //}
Generated on Wed Jul 13 2022 22:04:07 by 1.7.2