Interface library for the Devantech SRF02/SRF08 ultrasonic i2c rangers. Depends on I2cRtosDriver lib!
Srf0208IFTest01.h
- Committer:
- humlet
- Date:
- 2013-06-01
- Revision:
- 3:70c946ba29cc
- Parent:
- 2:dfc8b09b4e3b
File content as of revision 3:70c946ba29cc:
// attempt to realize a triangulation using one srf08 (sender) and two srf02 (additional receivers) // Does not work yet ... doubt that it will ever work #include "Srf0208IF.h" #include "I2CMasterRtos.h" #include "Serial.h" #include "us_ticker_api.h" #include "Thread.h" #include "math.h" using namespace mbed; using namespace rtos; Serial pc(USBTX, USBRX); I2CMasterRtos i2c(p28, p27,100000); Srf08IF mid(0xe0,i2c); Srf02IF right(0xe2,i2c); Srf02IF left(0xe4,i2c); int doit() { Thread::wait(2000); pc.printf("\n\n\n########### STARTING ###############\n\n\n"); Thread::wait(1000); while(1) { int t0 = us_ticker_read(); left.triggerEchoMeasurement(); int t1 = us_ticker_read(); right.triggerEchoMeasurement(); int t2 = us_ticker_read(); mid.triggerRanging(); int t3 = us_ticker_read(); Thread::wait(100); int tm=mid.readTransitTime_us(); int tl=left.readTransitTime_us(); int tr=right.readTransitTime_us(); tl-=t3-t1; tr-=t3-t2; const float B=73; // base in mm const float pi=3.141592654; const float magic=605.0; float R = (tr-magic)*Srf02IF::sonicSpeed*1.0e-3; float L = (tl-magic)*Srf02IF::sonicSpeed*1.0e-3; float M = tm*Srf02IF::sonicSpeed*0.5e-3; float m = (0.5*(R*R+L*L)-B*B) / (R+L); float r = R-m; float l = L-m; float cosAr = (m*m+B*B-r*r) / (2.0*m*B); float cosAl = (B*B-R*L)*(L-R) / (B*(L*L+R*R-2.0*B*B)); float cosA = cosAr; float o = m * cosA; float d = sqrt(m*m-o*o); float a = 180.0/pi*(0.5*pi-acos(cosA)); pc.printf("d=%4.0f o=%3.0f a=%4.1f L=%4.0f R=%4.0f M=%4.0f m=%4.0f l=%4.0f r=%4.0f cosR=%f cosL=%f ", d, o, a, L, R, M, m, l, r, cosAr, cosAl); pc.printf("t01=%d t12=%d t23=%d ",t1-t0,t2-t1,t3-t2); pc.printf("tm=%4d tl=%4d tr=%4d \n",tm,tl,tr); Thread::wait(100); } }