初版

TOF測距センサVL53L0Xを３個使用した自律走行車の試作

製作品

# はじめに obnizのパーツライブラリに、TOF測距センサVL53L0Xがありますが残念ながら複数のインスタンスを生成することができないため、複数のVL53L0Xを使用することができません。このパーツライブラリのソースを参考に、複数のVL53L0Xからデータ取得可能なクラスライブラリを作成しました。 これを応用して、前、右、左の測距情報をもとに障害物を避けて走る自律走行車を試作してみました。 走行アルゴリズムをクラウド側で実行することにより、オンラインで実車を使ったハンズオン等に活用できると考えています。 現状、よちよち歩きですが、最終的には[RumiCar](https://www.rumicar.com/)への応用を考えています。 # デモ動画 試作した自律走行カーの走行デモです。 すごく遅い！ @[twitter](https://twitter.com/pokibon/status/1381459474259398658?s=20) センサーのデータはグラフ表示で可視化してみました。  # パーツリスト | 部品 |数量 | 備考| |:---:|:---:|:---:| | obniz 1Y|1個|3.3Vの電源端子が嬉しい| | TOF測距センサ VL53L0Xモジュール| 3個|SWITCH SCIENCE他| | ダブルギアボックス|1個|タミヤ| |トラックタイヤセット|1個|タミヤ| |モバイルバッテリー|1個|5V 1A以上| |シャーシ|1個|アクリル板の切れ端等| # 作り方 ## ハードウェア 必要なハードウェアは、obnizの公式制作例に掲載されている[[キッズプロジェクト] スマホで操作するラジコンをつくろう](https://blog.obniz.com/make/kids-project-simple-rc-car)がベースです。 これに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オブジェクトを共用する方法をこっそり教えて下さい！） ということで、変更したライブラリはパーツライブラリとしての登録はせずに、単なるクラスとして使用しています。 ```javascriput: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のピンを入力にしてハイインピーダンスにしています。 ```javascriput: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つのセンサの情報から、単純に進行方向を決定しています。 ```javascriput:自律走行アルゴリズム例 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(); } } }; ``` ### ソースリスト 全体のソースリストは、以下です。 ```javascript: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モジュールへの応用（測距スピードの問題を解決しないときついかな？）