 		// Write back error
 		sprintf(i2c_buff, "ERROR\n", SonarReading[0]);
 	}
+	i2c_buffer_ready = 0; // Mark Arduino as ready
+	i2c_buffer_len = strlen(i2c_buff);
+
 	return;
 }
//Comment out to disable serial debugging
#define debugOn

// Sonar ISR
#include "sonarInterupt.h"
void SetupSonar(void);

	}

	// Parse Request
	i2c_buffer_ready = -1; // Mark Arduino as busy
	if(buff[0] == 's' && numBytes >= 2){
		// Read sonar id
		int sonar;
