#include <SoftwareSerial.h>
#include <Wire.h>
#include <Adafruit_SSD1306.h>
#include <Adafruit_GFX.h>
#define OLED_ADDR   0x3C    // OLED display TWI address
Adafruit_SSD1306 display(-1);
#define RXpin 6
#define TXpin 7
SoftwareSerial mySerial(RXpin,TXpin); // RX, TX connected to US sensor
unsigned char data[4]={};
float distance = 0.00;

void setup() {
 pinMode(RXpin, INPUT);
 pinMode(TXpin, OUTPUT);
 Wire.begin( );                // démarre la communication I2C
 Serial.begin(9600);
 mySerial.begin(9600); 
 display.begin(SSD1306_SWITCHCAPVCC, OLED_ADDR); // initialize
 display.clearDisplay();     //and clear display
 display.display(); 
}

void loop() {
 mySerial.flush();
 delay(30);
 digitalWrite(TXpin,HIGH);
 delay(30);
 digitalWrite(TXpin,LOW);
 delay(30);
  digitalWrite(TXpin,HIGH);
 delay(60);
     for(int i=0;i<4;i++)  {
       data[i]=mySerial.read();
     }
 distance = (data[1]*256)+data[2];
 display.clearDisplay();
 display.display();  
 display.setTextColor(WHITE);
 display.setTextSize(1);
 display.setCursor(35,1);    
 display.print("DISTANCE" );
 display.display(); // actually display all of the above 
 display.setTextSize(2);
 display.setCursor(10,17);    
 display.print(distance);
 display.setCursor(85,17);    
 display.print(" mm");
 display.display();
 delay(5000);
}