はじめに

obnizのパーツライブラリに、TOF測距センサVL53L0Xがありますが残念ながら複数のインスタンスを生成することができないため、複数のVL53L0Xを使用することができません。このパーツライブラリのソースを参考に、複数のVL53L0Xからデータ取得可能なクラスライブラリを作成しました。
これを応用して、前、右、左の測距情報をもとに障害物を避けて走る自律走行車を試作してみました。
走行アルゴリズムをクラウド側で実行することにより、オンラインで実車を使ったハンズオン等に活用できると考えています。
現状、よちよち歩きですが、最終的にはRumiCarへの応用を考えています。

デモ動画

試作した自律走行カーの走行デモです。
すごく遅い！

センサーのデータはグラフ表示で可視化してみました。

パーツリスト

作り方

ハードウェア

必要なハードウェアは、obnizの公式制作例に掲載されている[キッズプロジェクト] スマホで操作するラジコンをつくろうがベースです。
これにVL53L0Xのセンサを3個搭載しています。VL53L0Xの電源は、obniz 1Yの3.3V出力を使用しています。obnizの場合は、I/O端子から取得できる3.3V出力の電流容量が足りないため、別途三端子レギュレータ等で5Vから3.3Vを作る必要がありますのでちょっと面倒です。
3個のセンサは、左前方、正面、右前方を検出するように配置しています。

ソフトウェア

クラスライブラリ

VL53L0Xでは、同一のI2Cチャネルで複数使用するためI2Cアドレスを変更する機能がありますが、obniz標準のパーツライブラリでは、このアドレスを変更する機能がサポートされていません。
このため、パーツライブラリのソースをベースに新たにsetAddress()メソッドを追加しました。

また、標準のVL53L0Xパーツライブラリでは、インスタンスを生成する毎にI2Cオブジェクトを作成しますが、2回め以降はI2Cのリソースがすでにないためエラーになります。このため、2つ目のインスタンス生成からはi2cのオブジェクトは生成せず、1つ目のオブジェクトをコピーしています。この処理はパーツライブラリのクラスの外側で行っています。ここはもう少しスマートが手段があると思いますが、javascriptに不慣れなせいで手抜きです。（誰か複数のインスタンスの中でi2cオブジェクトを共用する方法をこっそり教えて下さい！）

ということで、変更したライブラリはパーツライブラリとしての登録はせずに、単なるクラスとして使用しています。

VL53L0Xクラスライブラリ
class VL53L0X2 {
          constructor() {
              this.requiredKeys = [];
              this.keys = ["vcc", "gnd", "sda", "scl", "i2c"];
              this.address = 0x29;
              this.regs = {
                  IDENTIFICATION_MODEL_ID: 0xc0,
                  IDENTIFICATION_REVISION_ID: 0xc2,
                  PRE_RANGE_CONFIG_VCSEL_PERIOD: 0x50,
                  FINAL_RANGE_CONFIG_VCSEL_PERIOD: 0x70,
                  SYSRANGE_START: 0x00,
                  RESULT_INTERRUPT_STATUS: 0x13,
                  RESULT_RANGE_STATUS: 0x14,
                  I2C_SLAVE_DEVICE_ADDRESS: 0x8a,　
              };
              this.acnt = 0;
              this.scnt = 0;
              this.status = 0;
          }
          static info() {
              return {
                  name: "VL53L0X2",
              };
          }
          wired(obniz) {
              this.obniz = obniz;
              this.obniz.setVccGnd(this.params.vcc, this.params.gnd, "3v");
              this.obniz.wait(100);
              this.params.clock = 400000;
//              this.params.pull = "3v";
              this.params.mode = "master";
              this.i2c = obniz.getI2CWithConfig(this.params);
          }
          async getWait() {
              this.i2c.write(this.address, [this.regs.SYSRANGE_START, 0x01]);
              let val = [0];
              let cnt = 0;
              while (cnt < 10) {
                  await obniz.wait(10);
                  this.i2c.write(this.address, [this.regs.RESULT_RANGE_STATUS]);
                  val = await this.i2c.readWait(this.address, 1);
                  if (val[0] & 0x01) {
                      break;
                  }
                  else {
                      cnt++;
                  }
              }
              if (!(val[0] & 0x01)) {
                  return null;
              } // sensor not ready
              this.i2c.write(this.address, [0x14]);
              const gbuf = await this.i2c.readWait(this.address, 12);
              this.acnt = this.makeuint16(gbuf[7], gbuf[6]);
              this.scnt = this.makeuint16(gbuf[9], gbuf[8]);
              const dist = this.makeuint16(gbuf[11], gbuf[10]);
              this.status = (gbuf[0] & 0x78) >> 3;
              return dist;
          }
          makeuint16(lsb, msb) {
              return ((msb & 0xff) << 8) | (lsb & 0xff);
          }
          setAddress(newAddress) {
            this.i2c.write(this.address, [this.regs.I2C_SLAVE_DEVICE_ADDRESS, newAddress & 0x7f]);
                  this.address = newAddress & 0x7f;
          }
      }

複数のVL53L0Xの初期化

複数のVL53L0Xを使用するには、それぞれにI2Cアドレスを割り当てる必要があります。
最初はどれも同じI2Cアドレス0X29が割り当てられていますので、XSHUT端子を利用して1つずつI2Cアドレスを変更して行きます。
VL53L0XのSCL, SDA, XSHUT端子は、モジュール側でプルアップされていますので、obniz側のプルアップは不要です。特にXSHUT端子をアクティブにするには、obnizのピンを入力にしてハイインピーダンスにしています。

VL53L0X初期化
async function init() {
        var value;
        obniz.io1.output(false);      // io1 : gnd
        obniz.io4.output(false);      // io4 : sensor0 xshut to Low(disable)
        obniz.io5.output(false);      // io5 : sensor1 xshut to Low(disable)
        obniz.io6.output(false);      // io6 : sensor2 xshut to Low(disable)

        obniz.io4.input(value);       // io4 : sensor0 xshut0 to High-Z
        sensor0 = new VL53L0X2;
        sensor0.params = {scl:2, sda:3};
        sensor0.wired(obniz);
        await obniz.wait(100);
        sensor0.setAddress(0x30);     // change i2cAddress

        obniz.io5.input(value);       // io5 : sensor1 xshut0 to High-Z 
        sensor1 = new VL53L0X2;
        sensor1.i2c = sensor0.i2c;
        await obniz.wait(100);
        sensor1.setAddress(0x31);     // change i2cAddress
        
        obniz.io6.input(value);       // io6 : sensor2 xshut0 to High-Z
        sensor2 = new VL53L0X2;
        sensor2.i2c = sensor0.i2c;
        await obniz.wait(100);
        sensor2.setAddress(0x32);     // change i2cAddress 

        motorL = obniz.wired("DCMotor", {forward:9, back:8});
        motorR = obniz.wired("DCMotor", {forward:11, back:10});
      }

自律走行

3つのセンサの情報から、単純に進行方向を決定しています。

自律走行アルゴリズム例
obniz.onloop = async function() {
          var distance0 = await sensor0.getWait();
          var distance1 = await sensor1.getWait();
          var distance2 = await sensor2.getWait();

          if (distance1 >= 150) {
            forward();
          } else  {
            reverse();
          }
          if (distance0 < 150 || distance2 < 150) {
            var delta = distance2 - distance0;
            if (delta > 100)  {
              right();
            } else  if (delta < -100) {
              left();
            }
          }
        };

ソースリスト

全体のソースリストは、以下です。

lv53l0x_demo
<html>
  <head>
    <meta charset="utf-8" />
    <meta name="viewport" content="width=device-width, initial-scale=1" />
    <link rel="stylesheet" href="https://stackpath.bootstrapcdn.com/bootstrap/4.3.1/css/bootstrap.min.css" />
    <script src="https://code.jquery.com/jquery-3.2.1.min.js"></script>
    <script src="https://cdn.jsdelivr.net/npm/moment@2.24.0/min/moment.min.js"></script>
    <script src="https://cdn.jsdelivr.net/npm/chart.js@2.8.0"></script>
    <script src="https://cdn.jsdelivr.net/npm/chartjs-plugin-streaming@1.8.0"></script>
    <script src="https://unpkg.com/obniz@3.x/obniz.js" crossorigin="anonymous" ></script>
  </head>
  <body>
    <div id="obniz-debug"></div>
    <div class="container">
      <div class="text-center">
        <button id="Start" class="btn btn-warning" style="width:45%;height:100px;font-size:50px;">
          Start
        </button>
        <button id="Stop" class="btn btn-warning" style="width:45%;height:100px;font-size:50px;">
          Stop
        </button>
      </div>
      <div class="text-center">
        <div class="chart-container">
          <canvas id="myChart"></canvas>
       </div>
      </div>

    <script>
      var obniz = new Obniz("OBNIZ_ID_HERE");
      var sensor0;
      var sensor1;
      var sensor2;
      var motorR;
      var motorL;
      var maxPower = 150;
      var pow = 0;
      //==============================
      // for display graph
      //==============================     
      var distance = {
        left:   0,
        center: 0,
        right:  0
      }
      //chart color setting
      var chartColors = {
        left:   'rgb(0, 164, 227)',
        center: 'rgb(252, 109, 83)',
        right:  'rgb(255, 197, 34)'
      }

      //refresh
      function barOnRefresh(chart){

      }

      function lineOnRefresh(chart){
        chart.config.data.datasets.forEach(async function(dataset){
          dataset.data.push({
            x: Date.now(),
            y: distance[dataset.label]
          });
        });
      }

      //chart config
      var barConfig = {
        type: "bar",
        data: {
        }
      }
      var lineConfig = {
        type: "line",
        data: {
          datasets: [{
            label: "left",
            borderWidth: 1,
            borderColor: chartColors.left,
            backgroundColor: chartColors.left,
            pointRadius: 2,
            lineTension: 0,
            fill: false,
            data: []
          },
          {
            label: "center",
            borderWidth: 1,
            borderColor: chartColors.center,
            backgroundColor: chartColors.center,
            pointRadius: 2,
            lineTension: 0,
            fill: false,
            data: []
          },
          {
            label: "right",
            borderWidth: 1,
            borderColor: chartColors.right,
            backgroundColor: chartColors.right,
            pointRadius: 2,
            lineTension: 0,
            fill: false,
            data: []
          }]
        },
        //chart options
        options: {
          scales: {
            yAxes: [{
              ticks: {
                beginAtZero: true,
                suggestedMax: 500.0,
                suggestedMin: 0.0,
                stepSize: 50.0
              }
            }],
            xAxes: [{
              type: "realtime",
              realtime:{
                duration: 15000,
                refresh: 200,
                delay: 1000,
                onRefresh: lineOnRefresh
              },
              scaleLabel:{
                display: true,
                labelString: "Time",
                fontSize: 15
              }
            }]
          },
          legend: {
            display: true,
            position: "top",
            labels: {fontSize: 10}
          },
          responsive: true
        }
      };

      //==============================
      // car control functions
      //==============================  
//      Obniz.PartsRegistrate(VL53L0X2);            // regist original parts
      function forward() {
        motorR.power(pow);            // set speed
        motorL.power(pow);            // set speed
        motorR.move(true);            // start 
        motorL.move(true);            // start 
      }
      function reverse() {
        motorR.power(pow);            // set speed
        motorL.power(pow);            // set speed
        motorR.move(false);           // start 
        motorL.move(false);           // start 
      }   
      function stop() {
        motorR.stop();
        motorL.stop();
      }
      function right() {
        motorL.stop();
        motorR.power(pow);            // set speed
        motorR.move(true);            // start 
      }   
      function left() {
        motorR.stop();
        motorL.power(pow);            // set speed
        motorL.move(true);            // start 
      }   
      //==============================
      // init vl53l0x X 3
      //==============================  
      async function init() {
        var value;
        obniz.io1.output(false);      // io1 : gnd
        obniz.io4.output(false);      // io4 : sensor0 xshut0 to Low(disable)
        obniz.io5.output(false);      // io5 : sensor1 xshut0 to Low(disable)
        obniz.io6.output(false);      // io6 : sensor2 xshut0 to Low(disable)

        obniz.io4.input(value);       // io4 : sensor0 xshut0 to High-Z
        sensor0 = new VL53L0X2;
        sensor0.params = {scl:2, sda:3};
        sensor0.wired(obniz);
        await obniz.wait(100);
        sensor0.setAddress(0x30);     // change i2cAddress

        obniz.io5.input(value);       // io5 : sensor1 xshut0 to High-Z 
        sensor1 = new VL53L0X2;
        sensor1.i2c = sensor0.i2c;
        await obniz.wait(100);
        sensor1.setAddress(0x31);     // change i2cAddress
        
        obniz.io6.input(value);       // io6 : sensor2 xshut0 to High-Z
        sensor2 = new VL53L0X2;
        sensor2.i2c = sensor0.i2c;
        await obniz.wait(100);
        sensor2.setAddress(0x32);     // change i2cAddress 

        motorL = obniz.wired("DCMotor", {forward:9, back:8});
        motorR = obniz.wired("DCMotor", {forward:11, back:10});

      }

      // called on online
      obniz.onconnect = async function() {        
        init();
        $("#Start").on("touchstart mousedown", () => {
          // Start motor
          pow = maxPower;
        });
        $("#Stop").on("touchstart mousedown", () => {
          // Start motor
          pow = 0;
        });
        var canvas = $('#myChart');
        var ctx = canvas[0].getContext('2d');
        window.myChart = new Chart(ctx, lineConfig);

        // called while online.                                     
        obniz.onloop = async function() {
          // JavaScript Examples
          var distance0 = await sensor0.getWait();
          var distance1 = await sensor1.getWait();
          var distance2 = await sensor2.getWait();

          distance.left   = distance0;
          distance.center = distance1;
          distance.right  = distance2;

          obniz.display.clear();        // clear LCD
          obniz.display.print("Left  :" + distance0);
          obniz.display.print("Center:" + distance1);
          obniz.display.print("Right :" + distance2);

          if (distance1 >= 150) {
            forward();
          } else  {
            reverse();
          }
          if (distance0 < 150 || distance2 < 150) {
            var delta = distance2 - distance0;
            if (delta > 100)  {
              right();
            } else  if (delta < -100) {
              left();
            }
          }
        };
      };
      
      // called on offline
      obniz.onclose = async function() {
        
      };
//==============================
// VL53L0X2 : for Multi VL53L0X2 
//==============================  
      class VL53L0X2 {
          constructor() {
              this.requiredKeys = [];
              this.keys = ["vcc", "gnd", "sda", "scl", "i2c"];
              this.address = 0x29;
              this.regs = {
                  IDENTIFICATION_MODEL_ID: 0xc0,
                  IDENTIFICATION_REVISION_ID: 0xc2,
                  PRE_RANGE_CONFIG_VCSEL_PERIOD: 0x50,
                  FINAL_RANGE_CONFIG_VCSEL_PERIOD: 0x70,
                  SYSRANGE_START: 0x00,
                  RESULT_INTERRUPT_STATUS: 0x13,
                  RESULT_RANGE_STATUS: 0x14,
                  I2C_SLAVE_DEVICE_ADDRESS: 0x8a,　
              };
              this.acnt = 0;
              this.scnt = 0;
              this.status = 0;
          }
          static info() {
              return {
                  name: "VL53L0X2",
              };
          }
          wired(obniz) {
              this.obniz = obniz;
              this.obniz.setVccGnd(this.params.vcc, this.params.gnd, "3v");
              this.obniz.wait(100);
              this.params.clock = 400000;
//              this.params.pull = "3v";
              this.params.mode = "master";
              this.i2c = obniz.getI2CWithConfig(this.params);
          }
          async getWait() {
              this.i2c.write(this.address, [this.regs.SYSRANGE_START, 0x01]);
              let val = [0];
              let cnt = 0;
              while (cnt < 10) {
                  await obniz.wait(10);
                  this.i2c.write(this.address, [this.regs.RESULT_RANGE_STATUS]);
                  val = await this.i2c.readWait(this.address, 1);
                  if (val[0] & 0x01) {
                      break;
                  }
                  else {
                      cnt++;
                  }
              }
              if (!(val[0] & 0x01)) {
                  return null;
              } // sensor not ready
              this.i2c.write(this.address, [0x14]);
              const gbuf = await this.i2c.readWait(this.address, 12);
              this.acnt = this.makeuint16(gbuf[7], gbuf[6]);
              this.scnt = this.makeuint16(gbuf[9], gbuf[8]);
              const dist = this.makeuint16(gbuf[11], gbuf[10]);
              this.status = (gbuf[0] & 0x78) >> 3;
              return dist;
          }
          makeuint16(lsb, msb) {
              return ((msb & 0xff) << 8) | (lsb & 0xff);
          }
          setAddress(newAddress) {
            this.i2c.write(this.address, [this.regs.I2C_SLAVE_DEVICE_ADDRESS, newAddress & 0x7f]);
                  this.address = newAddress & 0x7f;
          }
      }
    </script>
  </body>
</html>
}

今後の課題

• VL53L0Xの測距サイクルが2Hz程度と遅い。実力は33〜50Hzぐらい出せるはずだが、obnizからの制御と、VL53L0Xの最低限の設定しか実施していない影響で、フル性能を引き出せていない。
• タミヤのギアボックスのギア比を間違えて344.2:1に設定してしまったので、ものすごく遅い。（でもセンサーデータの取得も遅いのでちょうど良いかも）
• RumiCarのCPUモジュールへの応用（測距スピードの問題を解決しないときついかな？）