File content as of revision 2:dfc8b09b4e3b:

#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);
    }
}