Helmut Schmücker / Srf0208IF

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

Srf0208IFTest01.h@3:70c946ba29cc, 2013-06-01 (annotated)

Committer:
humlet
Date:
Sat Jun 01 07:40:00 2013 +0000
Revision:
3:70c946ba29cc
Parent:
2:dfc8b09b4e3b 
commitNpublish

Who changed what in which revision?

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