超音波センサー２個同時 サンプルスケッチ

//********************************************************************
//超音波センサを２個同時に使って距離をserialとLCD表示するプログラム
//計算を早くするため、cm以下の精度は出さないとした。
//音波が戻ってこない場合、0cm と出力する。LCD表示は「---」。
//Trigerパルスを２個同時に出す方式。ハードは２つのTrigerを接続すること
//計測のタイムアウトは40ms。タイマー割り込みで測定している。
//********************************************************************
#include <LiquidCrystal.h>
#include <MsTimer2.h>  // Timer interrupt

LiquidCrystal lcd(8,9,4,5,6,7); //lcdタイプの変数を定義
const long serialBps = 115200; //シリアル通信レート
//超音波センサー
const int sensor = 2; //センサーの数
const int pinTrig = 2; //トリガーパルス出力用ピン番号 
const int pinEcho[sensor] = {3,12}; //エコーパルス入力用ピン番号 [x]要素番号がセンサー番号
long Duration[sensor] ; //測定したエコーパルス幅 usec [x]要素番号がセンサー番号
long Distance[sensor] ; //測定した距離 mm [x]要素番号がセンサー番号
unsigned long StartTime; //パルス長を測り始める時間
int EchoState[sensor] ; //Echoポートの状態　０：LOW 初期状態　１：HIGH　２：LOW
volatile int CountDownTimer =0;  //１msのタイマー割り込みでカウントダウンされ、ゼロで止まるタイマー


//SETUP*****************************
void setup() {

  // LCDの行と列を設定
  lcd.begin(16, 2);

  //シリアルポート使用開始
  Serial.begin(serialBps);
  
  //pin modeの設定
  pinMode( pinTrig, OUTPUT);
  pinMode( pinEcho[0], INPUT);
  pinMode( pinEcho[1], INPUT);

  //タイマー割り込みの設定
  MsTimer2::set(1,count);    // 1ms period
  MsTimer2::start();            //タイマースタート

  //LCDへタイトルを表示
  lcd.home();               // カーソル位置を1行目に設定
  lcd.print("Ultrasonic X2-2");  
  
}

//MAIN LOOP*************************
void loop() {

  //=================================
  //超音波センサー　output = Distance[i]
  //=================================

  //超音波を出力（トリガーパルスの出力）
  digitalWrite( pinTrig, HIGH );     //HIGHを出す
  delayMicroseconds( 10 );           //10usec
  digitalWrite( pinTrig, LOW );      //LOWに戻す
  
  //Echoパルス幅を測る準備  
  for (int i=0; i<sensor; i++){
    EchoState[i]= 0;     //ポートのステータスを初期化
  }
  CountDownTimer = 40;   //４０msのカウントダウンタイマーをセット（タイムアウト用）

    
  //Echoパルス幅の時間を測定 usec単位
  while(true) {
      
       //各センサーのEchoの値（HIGH/LOW)をチェックし,状態を記憶する
       //HIGH-->LOWとなったら、パルス幅時間を求める
       for (int i=0; i<sensor; i++){
         int dR =digitalRead(pinEcho[i]);
         
         //HIGH?
         if (( dR == HIGH) && (EchoState[i] == 0)) {     // HIGH=1 , LOW=0
           EchoState[i] = 1;             //Echoポートの状態：０-->1に変更
           StartTime = micros();         //時間測定開始時間を記録
         }
         
         //LOW? HIGH-->LOW?
         if ((dR == LOW ) && (EchoState[i] == 1)) {
           EchoState[i] = 2;             //Echoポートの状態：1-->2に変更
       
           //パルス幅の時間を開始時間と現在時間から求める
           unsigned long ms =micros();
           if (ms > StartTime) {
             Duration[i]= ms-StartTime;
           }
           else {
             Duration[i] = 4294967295-StartTime+ms;
           }
         }
       }
       
       //全てのセンサーの計測が終了したら、whileループを抜ける。      
       int EchoStateSum = 0;
       for (int i=0; i<sensor; i++){
         EchoStateSum += EchoState[i];
       }
       if (EchoStateSum == 2*sensor) {   //全てのセンサーの計測が終了していたら、echoStateSumの値は２×センサー数となる。
        break;
       }

       //タイムアウトになったら、whileループを抜ける。
       //タイムアウトになったら、計測が終わっていないセンサーの計測値は全てzeroを入れる。
       if ( CountDownTimer == 0){
        for (int i=0; i<sensor; i++){
         if (EchoState[i] < 2){
          Duration[i] = 0;
         }
        }
        break;
       }
  }


  //距離(単位mm)を計算。
  for (int i=0; i<sensor; i++){ 
    Distance[i] = Duration[i]*340/2/10000;  // 音速を340m/sとする.往復距離を半分にする
  }
  

  //=================================
  //Serial 通信
  //=================================
  
   //シリアルに距離を出力 
  Serial.print("Distance:");
  Serial.print(Distance[0]);
  Serial.print("cm  ");
  Serial.print(Distance[1]); 
  Serial.println("cm");

  
  //LCDへ距離を出力     
  lcd.setCursor(0,1);          // カーソル位置を2行目に設定　
  lcd.print("              cm");
  
  if (Distance[0] == 0){ 
    lcd.setCursor(2,1);
    lcd.print("---");
    }
  else if (Distance[0]< 10) { 
    lcd.setCursor(4,1);
    lcd.print(Distance[0]);}
  else if(Distance[0] < 100) {
    lcd.setCursor(3,1);
    lcd.print(Distance[0]);}
  else {
    lcd.setCursor(2,1);
    lcd.print(Distance[0]);}

  if (Distance[1] == 0){
    lcd.setCursor(7,1);
    lcd.print("---");
  }
  else if (Distance[1]< 10) { 
    lcd.setCursor(9,1);
    lcd.print(Distance[1]);}
  else if(Distance[1] < 100) {
    lcd.setCursor(8,1);
    lcd.print(Distance[1]);}
  else {
    lcd.setCursor(7,1);
    lcd.print(Distance[1]);}
  
  //0.1秒毎に計測
  delay(100);    
}

//TIMER INTERRUPT**************************
void count() {
  //タイマーをカウントダウンする。ゼロで止める。
  if ( CountDownTimer != 0){
    CountDownTimer--;
  }
}