/*
	Timer 割り込みは必ずしも必要ないが、使用例として
	Arduinoのハードウェアシリアル端子は、IDEから書き込む際に
	他の回路が接続されているとエラーになることがあるので
	SoftwareSerial で Pin 8/9 を使用
	TOFの出力データは ASCII数字 5桁＋\r\n で送られてくる
*/
// include the library code:
#include <LiquidCrystal.h>
#include <MsTimer2.h>
#include <SoftwareSerial.h>

const int rs = 12, en = 11, d4 = 5, d5 = 4, d6 = 3, d7 = 2;
const int LED = 13;
long tma = 0;
LiquidCrystal lcd(rs, en, d4, d5, d6, d7);
SoftwareSerial softSerial(8,9);   // set Rx, Tx

void timerFire() {      // timer interupt 20ms period
  tma++;
}

void setup() {
  //timer fire every 20ms
  MsTimer2::set(20, timerFire);
  MsTimer2::start();

  pinMode(LED, OUTPUT);
  // set up the LCD's number of columns and rows:
  lcd.begin(16, 2);
  
  // initialize the software serial communications:
  softSerial.begin(19200);
}

void loop() {
  String stra;
  softSerial.listen();
  if((tma % 5) == 0){
    if (softSerial.available()){		// シリアルデータ受信
      stra = softSerial.readStringUntil('\n');
      softSerial.flush();
    }
    lcd.setCursor(0, 0);    
    if(stra.length() > 5){
      stra = stra.substring(stra.length() - 5, stra.length() - 1);
    }
    lcd.setCursor(5, 0);
    lcd.print(stra);		// 測距結果表示
    lcd.setCursor(10, 0);
    lcd.print("mm");
  }
    if((tma % 25) == 0){ 
      digitalWrite(LED, HIGH);
    }else{
      digitalWrite(LED, LOW);
    }

  if((tma % 10) == 0){ 		// 1秒毎に経過タイム更新
    lcd.setCursor(12, 1);
      // print the number of seconds since reset:
    lcd.print(tma / 50);
  }
}