M5StickCのToF HAT(レーザー距離センサー)をobnizOS I2C javaScriptで動かす(回転Servo 2WD 障害物回避Carへの応用)

#■初めに
M5って次から次へと面白し周辺機器を出して来ますよね。今回はその中の一つの***ToF HATをobnizOSで使うための関数***を作り、***障害物回避Carに応用***してみたので紹介します。
#■(参考)ToF HATとM5StickCの接続は、このようになっているようです
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/234533/c8ca7287-4c0c-818a-e94a-3ba391808bce.png)

#■コードの説明 (後述の応用例の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
##前進中に近距離障害物を検知したら、左に後退して方向転換を行い再度前進する。。。という動作を繰り替えします。
##デモ動画
<blockquote class="twitter-tweet"><p lang="ja" dir="ltr"><a href="https://twitter.com/hashtag/M5StickC?src=hash&amp;ref_src=twsrc%5Etfw">#M5StickC</a> のToF HAT(レーザー距離センサ)を <a href="https://twitter.com/hashtag/obnizOS?src=hash&amp;ref_src=twsrc%5Etfw">#obnizOS</a> のI2Cで動かしてみた。動画は回転Servo 2WD Carによる障害物回避。Qiita投稿準備中。 <a href="https://t.co/ZmQpTLB2g8">pic.twitter.com/ZmQpTLB2g8</a></p>&mdash; Google Homer (@google_homer_) <a href="https://twitter.com/google_homer_/status/1193033537760329728?ref_src=twsrc%5Etfw">November 9, 2019</a></blockquote> <script async src="https://platform.twitter.com/widgets.js" charset="utf-8"></script>
##車体
![WIN_20191109_14_18_44_Pro.jpg](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/234533/86701896-eeb6-5994-c66e-506585538e83.jpeg)
![WIN_20191109_14_19_23_Pro.jpg](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/234533/2fdf81cf-6238-b8ce-d221-7396402df597.jpeg)
![WIN_20191109_14_21_44_Pro.jpg](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/234533/635e5fd6-e556-703c-48f2-749edc393c75.jpeg)
***※コンパクトにするために、Servo及びGROVEの端子を切断して半田付けしています。***
##コード
```html
<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の開発の利便性の両者の恩恵をうけるには結構体力使いますね。これからも、こつこつ移植していきます。