Diff: Srf0208IFTest01.h

Revision:

2:dfc8b09b4e3b

Child:

3:70c946ba29cc

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Srf0208IFTest01.h	Sun May 26 20:52:27 2013 +0000
@@ -0,0 +1,70 @@
+#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,400000);
+
+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);
+    }
+}
+