#include <M5Stack.h>
int trig = 22; // Trig
int echo = 21; // Echo
double duration = 0;
double distance = 0;

// the setup routine runs once when M5Stack starts up
void setup() {
  pinMode(trig,OUTPUT);
  pinMode(echo,INPUT);

  // initialize the M5Stack object
  M5.begin();
  // text print
  M5.Lcd.fillScreen(BLACK);
  M5.Lcd.setCursor(0, 0);
  M5.Lcd.setTextColor(WHITE);
  M5.Lcd.setTextSize(4);

}

// the loop routine runs over and over again forever
void loop(){
  digitalWrite(trig,LOW);
  delayMicroseconds(1);
  digitalWrite(trig,HIGH);
  delayMicroseconds(11);
  digitalWrite(trig,LOW);
  duration = pulseIn(echo,HIGH);
  distance = duration*0.017;// = 340 * 100 / 1000000 / 2
  M5.Lcd.setCursor(0, 0);
  M5.Lcd.fillScreen(BLACK);
  M5.Lcd.printf("%2.2f cm\n",distance);
  delay(500);
  M5.update();
}