Help us understand the problem. What is going on with this article?

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

■初めに

M5って次から次へと面白し周辺機器を出して来ますよね。今回はその中の一つのToF HATをobnizOSで使うための関数を作り、障害物回避Carに応用してみたので紹介します。

■(参考)ToF HATとM5StickCの接続は、このようになっているようです

image.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

前進中に近距離障害物を検知したら、左に後退して方向転換を行い再度前進する。。。という動作を繰り替えします。

デモ動画

車体

WIN_20191109_14_18_44_Pro.jpg
WIN_20191109_14_19_23_Pro.jpg
WIN_20191109_14_21_44_Pro.jpg
※コンパクトにするために、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の開発の利便性の両者の恩恵をうけるには結構体力使いますね。これからも、こつこつ移植していきます。

Why not register and get more from Qiita?
  1. We will deliver articles that match you
    By following users and tags, you can catch up information on technical fields that you are interested in as a whole
  2. you can read useful information later efficiently
    By "stocking" the articles you like, you can search right away
Comments
No comments
Sign up for free and join this conversation.
If you already have a Qiita account
Why do not you register as a user and use Qiita more conveniently?
You need to log in to use this function. Qiita can be used more conveniently after logging in.
You seem to be reading articles frequently this month. Qiita can be used more conveniently after logging in.
  1. We will deliver articles that match you
    By following users and tags, you can catch up information on technical fields that you are interested in as a whole
  2. you can read useful information later efficiently
    By "stocking" the articles you like, you can search right away
ユーザーは見つかりませんでした