Interface library for the Devantech SRF02/SRF08 ultrasonic i2c rangers. Depends on I2cRtosDriver lib!
Embed:
(wiki syntax)
Show/hide line numbers
Srf0208IFTest01.h
00001 // attempt to realize a triangulation using one srf08 (sender) and two srf02 (additional receivers) 00002 // Does not work yet ... doubt that it will ever work 00003 00004 #include "Srf0208IF.h" 00005 #include "I2CMasterRtos.h" 00006 #include "Serial.h" 00007 #include "us_ticker_api.h" 00008 #include "Thread.h" 00009 #include "math.h" 00010 00011 using namespace mbed; 00012 using namespace rtos; 00013 00014 Serial pc(USBTX, USBRX); 00015 00016 I2CMasterRtos i2c(p28, p27,100000); 00017 00018 Srf08IF mid(0xe0,i2c); 00019 Srf02IF right(0xe2,i2c); 00020 Srf02IF left(0xe4,i2c); 00021 00022 int doit() 00023 { 00024 Thread::wait(2000); 00025 pc.printf("\n\n\n########### STARTING ###############\n\n\n"); 00026 Thread::wait(1000); 00027 00028 while(1) { 00029 int t0 = us_ticker_read(); 00030 left.triggerEchoMeasurement(); 00031 int t1 = us_ticker_read(); 00032 right.triggerEchoMeasurement(); 00033 int t2 = us_ticker_read(); 00034 mid.triggerRanging(); 00035 int t3 = us_ticker_read(); 00036 00037 Thread::wait(100); 00038 00039 int tm=mid.readTransitTime_us(); 00040 int tl=left.readTransitTime_us(); 00041 int tr=right.readTransitTime_us(); 00042 00043 tl-=t3-t1; 00044 tr-=t3-t2; 00045 00046 const float B=73; // base in mm 00047 const float pi=3.141592654; 00048 const float magic=605.0; 00049 00050 float R = (tr-magic)*Srf02IF::sonicSpeed*1.0e-3; 00051 float L = (tl-magic)*Srf02IF::sonicSpeed*1.0e-3; 00052 float M = tm*Srf02IF::sonicSpeed*0.5e-3; 00053 00054 float m = (0.5*(R*R+L*L)-B*B) / (R+L); 00055 00056 float r = R-m; 00057 float l = L-m; 00058 float cosAr = (m*m+B*B-r*r) / (2.0*m*B); 00059 float cosAl = (B*B-R*L)*(L-R) / (B*(L*L+R*R-2.0*B*B)); 00060 float cosA = cosAr; 00061 float o = m * cosA; 00062 float d = sqrt(m*m-o*o); 00063 float a = 180.0/pi*(0.5*pi-acos(cosA)); 00064 00065 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 ", 00066 d, o, a, L, R, M, m, l, r, cosAr, cosAl); 00067 pc.printf("t01=%d t12=%d t23=%d ",t1-t0,t2-t1,t3-t2); 00068 pc.printf("tm=%4d tl=%4d tr=%4d \n",tm,tl,tr); 00069 00070 Thread::wait(100); 00071 } 00072 } 00073
Generated on Tue Jul 12 2022 20:48:48 by 1.7.2