Helmut Schmücker / Srf0208IF

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

Srf0208IFTest01.h@2:dfc8b09b4e3b, 2013-05-26 (annotated)

Committer:
humlet
Date:
Sun May 26 20:52:27 2013 +0000
Revision:
2:dfc8b09b4e3b
Child:
3:70c946ba29cc
initial

Who changed what in which revision?

UserRevisionLine numberNew contents of line
humlet 2:dfc8b09b4e3b 1 #include "Srf0208IF.h"
humlet 2:dfc8b09b4e3b 2 #include "I2CMasterRtos.h"
humlet 2:dfc8b09b4e3b 3 #include "Serial.h"
humlet 2:dfc8b09b4e3b 4 #include "us_ticker_api.h"
humlet 2:dfc8b09b4e3b 5 #include "Thread.h"
humlet 2:dfc8b09b4e3b 6 #include "math.h"
humlet 2:dfc8b09b4e3b 7
humlet 2:dfc8b09b4e3b 8 using namespace mbed;
humlet 2:dfc8b09b4e3b 9 using namespace rtos;
humlet 2:dfc8b09b4e3b 10
humlet 2:dfc8b09b4e3b 11 Serial pc(USBTX, USBRX);
humlet 2:dfc8b09b4e3b 12
humlet 2:dfc8b09b4e3b 13 I2CMasterRtos i2c(p28, p27,400000);
humlet 2:dfc8b09b4e3b 14
humlet 2:dfc8b09b4e3b 15 Srf08IF mid(0xe0,i2c);
humlet 2:dfc8b09b4e3b 16 Srf02IF right(0xe2,i2c);
humlet 2:dfc8b09b4e3b 17 Srf02IF left(0xe4,i2c);
humlet 2:dfc8b09b4e3b 18
humlet 2:dfc8b09b4e3b 19 int doit()
humlet 2:dfc8b09b4e3b 20 {
humlet 2:dfc8b09b4e3b 21 Thread::wait(2000);
humlet 2:dfc8b09b4e3b 22 pc.printf("\n\n\n########### STARTING ###############\n\n\n");
humlet 2:dfc8b09b4e3b 23 Thread::wait(1000);
humlet 2:dfc8b09b4e3b 24
humlet 2:dfc8b09b4e3b 25 while(1) {
humlet 2:dfc8b09b4e3b 26 int t0 = us_ticker_read();
humlet 2:dfc8b09b4e3b 27 left.triggerEchoMeasurement();
humlet 2:dfc8b09b4e3b 28 int t1 = us_ticker_read();
humlet 2:dfc8b09b4e3b 29 right.triggerEchoMeasurement();
humlet 2:dfc8b09b4e3b 30 int t2 = us_ticker_read();
humlet 2:dfc8b09b4e3b 31 mid.triggerRanging();
humlet 2:dfc8b09b4e3b 32 int t3 = us_ticker_read();
humlet 2:dfc8b09b4e3b 33
humlet 2:dfc8b09b4e3b 34 Thread::wait(100);
humlet 2:dfc8b09b4e3b 35
humlet 2:dfc8b09b4e3b 36 int tm=mid.readTransitTime_us();
humlet 2:dfc8b09b4e3b 37 int tl=left.readTransitTime_us();
humlet 2:dfc8b09b4e3b 38 int tr=right.readTransitTime_us();
humlet 2:dfc8b09b4e3b 39
humlet 2:dfc8b09b4e3b 40 tl-=t3-t1;
humlet 2:dfc8b09b4e3b 41 tr-=t3-t2;
humlet 2:dfc8b09b4e3b 42
humlet 2:dfc8b09b4e3b 43 const float B=73; // base in mm
humlet 2:dfc8b09b4e3b 44 const float pi=3.141592654;
humlet 2:dfc8b09b4e3b 45 const float magic=605.0;
humlet 2:dfc8b09b4e3b 46
humlet 2:dfc8b09b4e3b 47 float R = (tr-magic)*Srf02IF::sonicSpeed*1.0e-3;
humlet 2:dfc8b09b4e3b 48 float L = (tl-magic)*Srf02IF::sonicSpeed*1.0e-3;
humlet 2:dfc8b09b4e3b 49 float M = tm*Srf02IF::sonicSpeed*0.5e-3;
humlet 2:dfc8b09b4e3b 50
humlet 2:dfc8b09b4e3b 51 float m = (0.5*(R*R+L*L)-B*B) / (R+L);
humlet 2:dfc8b09b4e3b 52
humlet 2:dfc8b09b4e3b 53 float r = R-m;
humlet 2:dfc8b09b4e3b 54 float l = L-m;
humlet 2:dfc8b09b4e3b 55 float cosAr = (m*m+B*B-r*r) / (2.0*m*B);
humlet 2:dfc8b09b4e3b 56 float cosAl = (B*B-R*L)*(L-R) / (B*(L*L+R*R-2.0*B*B));
humlet 2:dfc8b09b4e3b 57 float cosA = cosAr;
humlet 2:dfc8b09b4e3b 58 float o = m * cosA;
humlet 2:dfc8b09b4e3b 59 float d = sqrt(m*m-o*o);
humlet 2:dfc8b09b4e3b 60 float a = 180.0/pi*(0.5*pi-acos(cosA));
humlet 2:dfc8b09b4e3b 61
humlet 2:dfc8b09b4e3b 62 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 ",
humlet 2:dfc8b09b4e3b 63 d, o, a, L, R, M, m, l, r, cosAr, cosAl);
humlet 2:dfc8b09b4e3b 64 pc.printf("t01=%d t12=%d t23=%d ",t1-t0,t2-t1,t3-t2);
humlet 2:dfc8b09b4e3b 65 pc.printf("tm=%4d tl=%4d tr=%4d \n",tm,tl,tr);
humlet 2:dfc8b09b4e3b 66
humlet 2:dfc8b09b4e3b 67 Thread::wait(100);
humlet 2:dfc8b09b4e3b 68 }
humlet 2:dfc8b09b4e3b 69 }
humlet 2:dfc8b09b4e3b 70