■初めに
M5って次から次へと面白し周辺機器を出して来ますよね。今回はその中の一つのToF HATをobnizOSで使うための関数を作り、障害物回避Carに応用してみたので紹介します。
■(参考)ToF HATとM5StickCの接続は、このようになっているようです
■コードの説明 (後述の応用例のToF_READ関数)
M5オフィシャルの下記のコードを参考に、obnizのJacaScriptのI2Cライブラリーを使ってToF_READという関数を作成しました。returnで計測距離(mm)を返します。ライブラリ化のやり方がわからなかったので外出しに出来ていません。コードについては後述の応用例のインラインを参照下さい*。
I2Cの細かい事はさっぱりわかっていません。オフィシャルのSampleコードから気合でエッセンスを抜き出しただけです。すいません。参考としてコメントに何をやっているか推測を書きました。
M5オフィシャルのToF HAT Sampleコード
https://github.com/m5stack/M5-ProductExampleCodes/blob/master/Hat/tof-hat/Arduino/ToF_Count/ToF_Count.ino
■ToF HATを使ってみた感じ
以下のような少し怪しい動きをしました。個体の問題なのか、私のプログラムの問題なのか不明です。
・仕様上の射程距離は0～2mとなっているが、40cm前後でしか動かない。
・実際よりも10cm程度大きい距離が返ってくる。
・実際の距離にかかわらず、時々20mmという意味不明な値が返ってくる。
■(応用例)回転Servo 2WD 障害物回避Car
前進中に近距離障害物を検知したら、左に後退して方向転換を行い再度前進する。。。という動作を繰り替えします。
デモ動画
#M5StickC のToF HAT(レーザー距離センサ)を #obnizOS のI2Cで動かしてみた。動画は回転Servo 2WD Carによる障害物回避。Qiita投稿準備中。 pic.twitter.com/ZmQpTLB2g8
— Google Homer (@google_homer_) November 9, 2019
車体
※コンパクトにするために、Servo及びGROVEの端子を切断して半田付けしています。
コード
<html>
<head>
<meta charset="utf-8">
<link rel="stylesheet" href="https://stackpath.bootstrapcdn.com/bootstrap/4.3.1/css/bootstrap.min.css">
<script src="https://obniz.io/js/jquery-3.2.1.min.js"></script>
<script src="https://unpkg.com/obniz@2.4.0/obniz.js" crossorigin="anonymous"></script>
<script src="https://unpkg.com/m5stickcjs/m5stickc.js"></script>
</head>
<body>
<div id="obniz-debug"></div><br>
<font size="7">
2WD Servo Car By ToF HAT<br>
距離：
<span id="DISTANCE"></span>
</font>
<script>
//回転サーボ停止angleの個体補正(仕様上は90°で停止となっているが)
const Lhome = 96;
const Rhome = 96;
var obniz = new M5StickC('OBNIZ_ID_HERE');
obniz.onconnect = async function () {
//GROVE端子にR/Lの回転サーボを接続
var Servo_L = obniz.wired("ServoMotor" , {signal:33} );
var Servo_R = obniz.wired("ServoMotor" , {signal:32} );
//ToF HAT I2C
obniz.i2c0.start({mode:"master", sda:0, scl:26, clock:400000});
await STOP();
obniz.led.on();
obniz.repeat(async function(){
//壁までの距離計測
var distance = await ToF_READ();
$("#DISTANCE").text(distance);
//壁に接近したら2秒間後退(少し方向変換)
if ( distance > 100 ){
if ( distance < 180 ){
await BACK();
await obniz.wait(2000);
}
await FORWARD();
}
},10)
/*********************************/
/* M5StickC ToF HAT 距離計測関数 */
/*********************************/
async function ToF_READ(){
//計測開始
obniz.i2c0.write(0x29, [0x00,0x01]);
//計測完了待ち合わせ
var cnt = 0;
while( cnt < 10 ){
await obniz.wait(10);
//計測ステータス転送要求
obniz.i2c0.write(0x29, [0x14]);
//計測ステータス読み出し
var status = await obniz.i2c0.readWait(0x29, 1); //status 1Byte read
if ( status & 0x01 ) cnt = 99; else cnt++;
}
//計測データ転送要求
obniz.i2c0.write(0x29, [0x14]);
//計測データ読み出し
var data = await obniz.i2c0.readWait(0x29, 12); //data 12Byte read
//バイナリの距離データ(符号付き･2バイト･リトルエイディアン)を整数に変換
var dist = (data[10]*256)+data[11];if (dist & 0x8000) dist=-((dist-1)^0xffff);
return dist;
}
/*************************/
/* Servo Motion Controll */
/*************************/
//左右前転で直進(R/L等速となるようangle補正)
async function FORWARD(){
Servo_L.angle(Lhome+10);
Servo_R.angle(Rhome-15);
}
//左寄りに後退し少しだけ方向転換(補正により、たまたま同一angle)
async function BACK(){
Servo_L.angle(Lhome-15);
Servo_R.angle(Rhome+15);
}
async function STOP(){
Servo_L.angle(Lhome);
Servo_R.angle(Rhome);
}
}
</script>
</body>
</html>
■まとめ
M5StickCの豊富な周辺機器とobnizOSの開発の利便性の両者の恩恵をうけるには結構体力使いますね。これからも、こつこつ移植していきます。