M5Stackでできること 〜M5StackでToF測距センサユニットを使う

これまで超音波方式の測距センサは何度か使ったことがありますが、違う方式の測距センサも試してみたいと思い、こちらのセンサを購入しました。

M5Stack社が販売しているTime-of-Flight方式の測距センサです。

こちら の記事で、「M5Stack用超音波測距ユニット（RCWL-9620）」を使ってモノの動きを検知していますが、同じことを「M5Stack用ToF測距センサユニット」で試してみたいと思います。
センサからの距離を常時測定し、測定距離に変化が生じたときに「動きがあった」と判断することにします。

M5Stack BasicのGROVEポートに「M5Stack用ToF測距センサユニット」をつなぎます。

スケッチは以下のとおりです。

#include <M5Stack.h>

#define VL53L0X_REG_IDENTIFICATION_MODEL_ID         0xc0
#define VL53L0X_REG_IDENTIFICATION_REVISION_ID      0xc2
#define VL53L0X_REG_PRE_RANGE_CONFIG_VCSEL_PERIOD   0x50
#define VL53L0X_REG_FINAL_RANGE_CONFIG_VCSEL_PERIOD 0x70
#define VL53L0X_REG_SYSRANGE_START                  0x00
#define VL53L0X_REG_RESULT_INTERRUPT_STATUS         0x13
#define VL53L0X_REG_RESULT_RANGE_STATUS             0x14
#define address                                     0x29  // I2C address

byte gbuf[16];

uint16_t bswap(byte b[]) {
    // Big Endian unsigned short to little endian unsigned short
    uint16_t val = ((b[0] << 8) & b[1]);
    return val;
}

uint16_t makeuint16(int lsb, int msb) {
    return ((msb & 0xFF) << 8) | (lsb & 0xFF);
}

void write_byte_data(byte data) {
    Wire.beginTransmission(address);
    Wire.write(data);
    Wire.endTransmission();
}

void write_byte_data_at(byte reg, byte data) {
    // write data word at address and register
    Wire.beginTransmission(address);
    Wire.write(reg);
    Wire.write(data);
    Wire.endTransmission();
}

void write_word_data_at(byte reg, uint16_t data) {
    // write data word at address and register
    byte b0 = (data & 0xFF);
    byte b1 = ((data >> 8) && 0xFF);

    Wire.beginTransmission(address);
    Wire.write(reg);
    Wire.write(b0);
    Wire.write(b1);
    Wire.endTransmission();
}

byte read_byte_data() {
    Wire.requestFrom(address, 1);
    while (Wire.available() < 1) delay(1);
    byte b = Wire.read();
    return b;
}

byte read_byte_data_at(byte reg) {
    // write_byte_data((byte)0x00);
    write_byte_data(reg);
    Wire.requestFrom(address, 1);
    while (Wire.available() < 1) delay(1);
    byte b = Wire.read();
    return b;
}

uint16_t read_word_data_at(byte reg) {
    write_byte_data(reg);
    Wire.requestFrom(address, 2);
    while (Wire.available() < 2) delay(1);
    gbuf[0] = Wire.read();
    gbuf[1] = Wire.read();
    return bswap(gbuf);
}

void read_block_data_at(byte reg, int sz) {
    int i = 0;
    write_byte_data(reg);
    Wire.requestFrom(address, sz);
    for (i = 0; i < sz; i++) {
        while (Wire.available() < 1) delay(1);
        gbuf[i] = Wire.read();
    }
}

uint16_t VL53L0X_decode_vcsel_period(short vcsel_period_reg) {
    // Converts the encoded VCSEL period register value into the real
    // period in PLL clocks
    uint16_t vcsel_period_pclks = (vcsel_period_reg + 1) << 1;
    return vcsel_period_pclks;
}

void setup() {
  M5.begin();
  Wire.begin();
}

void loop() {
  write_byte_data_at(VL53L0X_REG_SYSRANGE_START, 0x01);
  byte val = 0;
  int  cnt = 0;
  while (cnt < 100) {  // 1 second waiting time max
    delay(10);
    val = read_byte_data_at(VL53L0X_REG_RESULT_RANGE_STATUS);
    if (val & 0x01) break;
    cnt++;
  }
  read_block_data_at(0x14, 12);
  uint16_t dist = makeuint16(gbuf[11], gbuf[10]);
  Serial.println(dist);
}

Arduino IDEの「ファイル」＞「スケッチ例」＞「M5Stack」＞「Unit」＞「ToF_VL53L0X」をほんの少しだけ変更したものです。
距離を測定し、シリアル通信で書き出しています。

Arduino IDEでシリアルプロッタを開くと、以下のようにモノとの距離がグラフ表示されます。

モノの動きを検知するためのスケッチは以下のとおりです。
「超音波測距ユニット」をつかった時のスケッチ（記事は こちら）に対して、センサで距離を測定する部分だけを変更したものです。

#include <M5Stack.h>

#define VL53L0X_REG_IDENTIFICATION_MODEL_ID         0xc0
#define VL53L0X_REG_IDENTIFICATION_REVISION_ID      0xc2
#define VL53L0X_REG_PRE_RANGE_CONFIG_VCSEL_PERIOD   0x50
#define VL53L0X_REG_FINAL_RANGE_CONFIG_VCSEL_PERIOD 0x70
#define VL53L0X_REG_SYSRANGE_START                  0x00
#define VL53L0X_REG_RESULT_INTERRUPT_STATUS         0x13
#define VL53L0X_REG_RESULT_RANGE_STATUS             0x14
#define address                                     0x29  // I2C address

byte gbuf[16];

uint16_t bswap(byte b[]) {
    // Big Endian unsigned short to little endian unsigned short
    uint16_t val = ((b[0] << 8) & b[1]);
    return val;
}

uint16_t makeuint16(int lsb, int msb) {
    return ((msb & 0xFF) << 8) | (lsb & 0xFF);
}

void write_byte_data(byte data) {
    Wire.beginTransmission(address);
    Wire.write(data);
    Wire.endTransmission();
}

void write_byte_data_at(byte reg, byte data) {
    // write data word at address and register
    Wire.beginTransmission(address);
    Wire.write(reg);
    Wire.write(data);
    Wire.endTransmission();
}

void write_word_data_at(byte reg, uint16_t data) {
    // write data word at address and register
    byte b0 = (data & 0xFF);
    byte b1 = ((data >> 8) && 0xFF);

    Wire.beginTransmission(address);
    Wire.write(reg);
    Wire.write(b0);
    Wire.write(b1);
    Wire.endTransmission();
}

byte read_byte_data() {
    Wire.requestFrom(address, 1);
    while (Wire.available() < 1) delay(1);
    byte b = Wire.read();
    return b;
}

byte read_byte_data_at(byte reg) {
    // write_byte_data((byte)0x00);
    write_byte_data(reg);
    Wire.requestFrom(address, 1);
    while (Wire.available() < 1) delay(1);
    byte b = Wire.read();
    return b;
}

uint16_t read_word_data_at(byte reg) {
    write_byte_data(reg);
    Wire.requestFrom(address, 2);
    while (Wire.available() < 2) delay(1);
    gbuf[0] = Wire.read();
    gbuf[1] = Wire.read();
    return bswap(gbuf);
}

void read_block_data_at(byte reg, int sz) {
    int i = 0;
    write_byte_data(reg);
    Wire.requestFrom(address, sz);
    for (i = 0; i < sz; i++) {
        while (Wire.available() < 1) delay(1);
        gbuf[i] = Wire.read();
    }
}

uint16_t VL53L0X_decode_vcsel_period(short vcsel_period_reg) {
    // Converts the encoded VCSEL period register value into the real
    // period in PLL clocks
    uint16_t vcsel_period_pclks = (vcsel_period_reg + 1) << 1;
    return vcsel_period_pclks;
}

void setup() {
  M5.begin();
  Wire.begin();
}

int           currentDist;         // 現在の検出距離
int           prevDist    = 0;     // 直近の検出距離
unsigned long currentMove = 0;     // 現在の動作回数
unsigned long prevMove    = 0;     // 直近の動作回数
unsigned long diffMove    = 0;     // 直近からの動作回数の増加分
unsigned long currentTime;         // 現在時刻
unsigned long prevTime    = 0;     // 直近の時刻
boolean       currentFlag = false; // 動作しているかどうかのフラグ

void loop() {
  currentTime = millis();

  write_byte_data_at(VL53L0X_REG_SYSRANGE_START, 0x01);
  byte val = 0;
  int  cnt = 0;
  while (cnt < 100) {
    delay(10);
    val = read_byte_data_at(VL53L0X_REG_RESULT_RANGE_STATUS);
    if (val & 0x01) break;
    cnt++;
  }
  read_block_data_at(0x14, 12);
  currentDist = makeuint16(gbuf[11], gbuf[10])/10; // 検出距離（単位:cm単位, 1cm単位で切り捨て)
  Serial.println(currentDist);
  if(abs(currentDist - prevDist) >= 10) {   // 検出距離の変化が10cm以上のとき
    currentMove++;                          // 動作回数を+1する
    prevDist = currentDist;
  }
  if(currentTime - prevTime >= 5000) {
    diffMove = currentMove - prevMove;      // 直近からの動作回数の増加分
    if(diffMove >= 5) currentFlag = true;   // 5秒間での動作回数の変化が5回以上なら、currentFlagをtrueにする
    if(diffMove == 0) currentFlag = false;  // 5秒間での動作回数の変化が0回なら、currentFlagをfalseにする
    if(currentFlag) M5.Lcd.fillScreen(GREEN);
    else            M5.Lcd.fillScreen(RED);
    prevMove = currentMove;
    prevTime = currentTime;
  }
}

センサからの距離を常時測定し、直近の測定結果からの変化が10cm以上のとき、動きがあったと判断し、動作回数をプラス1します。
誤検知や、対象物以外の動きにより検出距離が変化する場合もありますので、

• 5秒間に動きが5回以上あったら、モノが「動作中」と判断します。
• 5秒間に動きが全くなければ、モノが「停止中」と判断します。
• それ以外のときは、直近の判断結果を維持します。

これで、センサの前でモノが動いているかどうかを検知できました。

なお、「M5Stack用ToF測距センサユニット」は、インターフェースが「I2C」なので、GROVEケーブルを長くすると正常に動かなくなる可能性があります。
このため、長いGROVEケーブルを使っても正常動作するかどうか試してみました。
その結果、今回つかったデバイスについては、GROVEケーブルを2メートルのものに変更しても問題なく動作しました。

また、「M5Stack用ToF測距センサユニット」で距離を測定できる角度についても大雑把に調べてみました。
「M5Stack用ToF測距センサユニット」から20cm程度の距離で調べたところ、センサの正面に対して直径10cm（およそ27度）より外であれば、モノがあっても検知しませんでした。「超音波測距ユニット」よりも指向性が高いようです。
また、センサの真正面にモノがあると正しく検知します。
ただし、センサ正面から1〜5cmの距離にモノがあると、測定結果が大きく変動したり、中途半端な測定値になったりと、挙動が不安定です。
また、センサの真正面20cmのところに、鉛筆のような小さいモノを置くと、実際より若干遠い（30cmなど）と認識されたりして、測定時のふるまいは「超音波測距ユニット」とは結構ちがうようです。
そんな訳で、実際に使う際には用途や使い方を吟味する必要がありそうです。

ちなみに、センサを透明ケース（材質はポリカーボネート）の中に入れても測距できるか試してみましたが、ケースまでの距離を検知してしまい、ケースの外の目標物までの距離を測定することはできませんでした。

---