//DistanceSenSor_DualVL53l0xTest01.ino//

#include <M5Stack.h>
#include <Wire.h>
#include <VL53L0X.h>

//https://github.com/pololu/vl53l0x-arduino
//https://klab.hateblo.jp/entry/2021/04/18/094705
//2つのVL53L0Xを同時に使いますが，defaultでは2つとも同じI2Cアドレス0x29を持っています。
//アドレスの競合を避けるために，2台目センサのアドレスを0x2bに変更します。
//VL53L0XにはXSHUTという端子があり，これをLにするとそのVL53L0Xは無効になります。
//この仕組みを利用して1台目センサを無効にしておいて，そのすきに2台目センサと通信してI2Cアドレスを変更します。
//GPIO26を1台目センサのXSHUTに，GPIO19を2台目センサのXSHUTに接続しておきます。
//
//2台のセンサのXSHUTをLにすると，センサは停止状態＝初期状態になります。（以前設定されていたI2Cアドレスも消去されます）
//1台目センサのXSHUTをLに保ったまま2台目センサのXSHUTをHにすると，defaultアドレスで有効なのは2台目センサだけになります
//この状況で2台目センサのアドレス変更作業を行います。
//作業が終わったら，1台目センサのXSHUTおHに戻して1台目センサを復活させます。
//ここで，VL53L0XのXSHUTは2.8V信号で，2.8Vでプルアップされているため，XSHUTがつながっているGPIO26，19はOpenDrain設定にしておく必要があります。

//SingleMode   (not ContinuousMode)
//必要なときに1回だけ測距 ただし，1つのセンサが測距し終わったら次のセンサの測距が始まる直列駆動

VL53L0X sensor1;
VL53L0X sensor2;

int measurementstatus1 = 0; //0:何もしていない　1:リクエスト中
int measurementstatus2 = 0; //0:何もしていない　1:リクエスト中

const int SDApin = 21; //SDAピン番号
const int SCLpin = 22; //SCLピン番号

const int XSHUTofSensor1pin = 26; //1台目センサのXSHUTのピン番号
const int XSHUTofSensor2pin = 19; //2台目センサのXSHUTのピン番号
const uint8_t Sensor2I2Caddress = 0x2b; //変更後の2台目センサのI2Cアドレス

void checkI2CAddress() {
  int address, error;
  //見つかったaddressの対になるadrressは+0x80にある
  for (address = 1; address<128; address++) {
    Wire.beginTransmission(address);
    error = Wire.endTransmission();
    if(error==0){
      M5.Lcd.print(address,HEX);M5.Lcd.print(" ");
    }
    delay(1);
  }
  M5.Lcd.println();  
}

void setup()
{
  M5.begin();
  M5.Power.begin();
  M5.Lcd.setTextSize(2); //高さ16　26文字/行

  pinMode(XSHUTofSensor1pin, OUTPUT_OPEN_DRAIN);//オープンドレイン設定
  pinMode(XSHUTofSensor2pin, OUTPUT_OPEN_DRAIN);//オープンドレイン設定
  digitalWrite(XSHUTofSensor1pin, LOW); //1台目センサを無効状態にする
  digitalWrite(XSHUTofSensor2pin, LOW); //2台目センサを無効状態にする
  delay(100); //無効状態＝初期化を確実にする

  Wire.begin(SDApin,SCLpin); //ここをGray用に設定しないと通信に失敗する
  Wire.setClock(400000); // use 400 kHz I2C

  M5.Lcd.setCursor(0, 120);
  //I2Cアドレス使用状況の検証
  checkI2CAddress();

  digitalWrite(XSHUTofSensor2pin, HIGH); //2台目センサを有効状態に戻す
  sensor2.setTimeout(500); //[msec]
  if (!sensor2.init())
  {
    M5.Lcd.println("Sensor2 not initialized");
    while (1);
  }
  sensor2.setAddress((uint8_t)Sensor2I2Caddress); //2台目センサーのアドレス変更
  M5.Lcd.println("Sensor2 initialized");

  digitalWrite(XSHUTofSensor1pin, HIGH); //1台目センサを有効状態に戻す

  if (!sensor1.init())
  {
    M5.Lcd.println("Sensor1 not initialized");
    while (1);
  }
  M5.Lcd.println("Sensor1 initialized");

  //I2Cアドレス使用状況の検証
  checkI2CAddress();

  sensor1.setMeasurementTimingBudget(50000); //測定タイミングバジェット(1回の距離測定)に許容される時間[micros]
  sensor2.setMeasurementTimingBudget(50000); //測定タイミングバジェット(1回の距離測定)に許容される時間[micros]
  M5.Lcd.setCursor(0, 0);
  M5.Lcd.println("Dual VL53L0X Started");
}


void loop()
{
  int distance1 = sensor1.readRangeSingleMillimeters();
  if (sensor1.timeoutOccurred()) {
    M5.Lcd.setCursor(0, 80);
    M5.Lcd.print("SENSOR1 TIMEOUT");
  } else {
    M5.Lcd.setCursor(0, 80);
    M5.Lcd.print("        ");
  }
  M5.Lcd.setCursor(0, 30);
  M5.Lcd.print("                          ");
  M5.Lcd.setCursor(0, 30);
  M5.Lcd.printf("S1 distance[mm]:    %5d  \n",distance1);

  int distance2 = sensor2.readRangeSingleMillimeters();
  if (sensor2.timeoutOccurred()) {
    M5.Lcd.setCursor(0, 100);
    M5.Lcd.print("SENSOR2 TIMEOUT");
  } else {
    M5.Lcd.setCursor(0, 100);
    M5.Lcd.print("        ");
  }
  M5.Lcd.setCursor(0, 50);
  M5.Lcd.print("                          ");
  M5.Lcd.setCursor(0, 50);
  M5.Lcd.printf("S2 distance[mm]:    %5d  \n",distance2);

}