A much simpler solution might be to call the i2c routines from a ticker object.  See below code. 
I had an I2C interface that would stop if my fingers got close to the wires ( adding capacitance? ) and I pulled my hair out trying to do a non-blocking read or implementing a timeout read.  I was calling the I2c routines from a loop in main().  When the I2C bus stopped, it froze, locked up, or otherwise blocked the processor.  (search keywords: i2c freeze lockup hang )
Using a ticker is much easier, and it recovers from a locked state well.  It also does not block the processing of the main loop, so your robot can take action (like stopping and putting the hazard lights on...)
This technique also works for serial devices, spi, etc. - basically anything that can stop your processor should be done in a ticker.  You won't need the complications of trying to run a real-time os or deal with watchdog timers.
moving read i2c sensors to a ticker
Timer ticktock;
int gGlobalSonarReading =0;
bool new_sonar_reading=false;
int last_sonar_reading_time = 0;
void my_read_sonar()
{
// send the commands to write then read the sonar here.....
//  aka  sonar.write(addr,cmd,2);
//  aka  sonar.read(read(readCmd, buffer, 6 ););
   last_read_sonar_time = ticktock.read_us();
   new_sonar_reading = true;
}
//////////////////////////////////////////////////////
//////////////////////  old main() 
///////////////////////////////////////////////////////
main()
{
  ticktock.start();  //for getting the current time...
  ...
    while(1) {
        l1=!l1;
        
       my_read_sonar();
        wait(0.2);
    }
 ...
}
//////////////////////////////////////////////////////
//////////////////////  change main to.... 
///////////////////////////////////////////////////////
Ticker read_i2c_sonar;
int desired_hertz = 10;  // 10 hz, ten times a second
int main()
{
   //attach the read function to the ticker, will automatically be called at the desired interval
  read_i2c_sonar.attach(&read_i2c_sonar, (1.0/desired_hertz));
 // ...
    while(1) {
       if ( new_sonar_reading ) {
          do_something_with_sonar_reading();  
          new_sonar_reading = false;
          // you can also check the timestamp the sonar reading when you read it, and check the timestamp against the current time
          // if the sonar timestamp is much smaller than the current time, perhaps there's a more serious problem
          // if ( (ticktock.read_us() - last_sonar_reading_time ) > 100000) { /* handle bad sensor... */ }
     }
        wait(0.2);
    }
// ...
}
 
                
             
        
I also had a regular I2C freeze while using an 24LC64 E2Prom and a FM24LC64 FRAM device on the same bus.
My solution which appears to have completely eliminated it is as follows.
int I2CJam; // global int for timeout handling //***************************************************************** // ADDITIONAL CODE FOR I2C ROUTINES //***************************************************************** I2CJam = I2CTimeoutInTimerTicks; // load timeout value ( i used approx 40ms in my application ) ..... DO an I2C read or write ..... eg. i2c.write(Device, cmd, 2, true)) eg. i2c.read(Device, cmd, len)) I2CJam = 0; // reset timeout when finished to prevent triggering //***************************************************** // CODE TO ADD TO YOUR TIMER INTERRUPT HANDLER //***************************************************** if (I2CJam > 0) { // if I2C timeout timer is active I2CJam--; // decrement if (I2CJam == 0) { // if timed out then i2c.stop(); // issue some stops and starts to un-jam the bus i2c.start(); i2c.stop(); i2c.start(); i2c.stop(); i2c.start(); i2c.stop(); i2c.start(); // I have probably gone a bit over the top with the quantity i2c.stop(); // but it is quick and is only triggered during a jam i2c.start(); i2c.stop(); i2c.start(); i2c.stop(); i2c.start(); i2c.stop(); i2c.start(); i2c.stop(); i2c.start(); } } //************************************************* There are some notes in the LPC1768 Datasheet UM10360 section 9.7.4 relating to bus jams.