Interface library for the Devantech SRF02/SRF08 ultrasonic i2c rangers. Depends on I2cRtosDriver lib!

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers Srf0208IFTest01.h Source File

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