0
0

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?

More than 5 years have passed since last update.

二足歩行ロボット Rapiro を Node.js で制御 [4] 距離センサの搭載

Last updated at Posted at 2017-01-22

やりたいこと

最終目標(前回と同じ)

  1. [3]で完了)二足歩行ロボット Rapiro の制御を、標準の Arduino IDE(C/C++)ベースから、JavaScript(Node.js)ベースに移植し、Rapiro を IoT デバイスっぽくする
  • [1][2]で完了)かつ、PCでの制御ではなく、Rapiro 内部に搭載した Raspberry Pi での制御とし、完全無線化を図る
  • [3]で完了)全機能の移植が難しくとも、最低限、10個の基本動作は移植・再現する
  1. 可能な限り、書籍「二足歩行ロボット 工作&プログラミング(リックテレコム)」の改造内容も移植する
  • 距離センサの搭載
  • 静電容量タッチセンサの搭載など
  1. この Rapiro をベースに、さらに賢そうな遊び方を模索する

今回 [4] の目標

方法

機材

  • Rapiro
  • 電源を入れる前に、Arduino IDE を用いて、制御ボードに StandardFirmataPlus を書き込んでおくが、__少々コツがある__ので以下で補足説明
  • Raspberry Pi 3 Model B
  • Rapiro 内部に搭載済(こちらの手順で)
  • Node.js インストール済(こちらの手順で)
  • npm で johnny-five, socket.io, express をインストール済
  • 距離センサ GP2Y0A21YK
  • 距離センサ接続用の部品(接続できればよく、以下全てが必須というわけではない)
  • エネループ(単3)5本 または Rapiro 用 ACアダプタ
  • PC(Windows10、Raspberry Pi に SSH や FTP できればなんでも良い)
  • スマホまたはタブレット
  • 無線LAN環境

距離センサの搭載

Rapiro Wiki にある「RAPIROに近接センサを取り付ける」を参考にしました。さらに楽するために、3ピンJST型コネクタ付きジャンパワイヤジャンパワイヤ(メス~メス)を使いました

  • メイン基板の「JP1」の4ピン分の穴にピンヘッダをはんだ付け
  • そのピンヘッダにメス~メスのジャンパワイヤをさす(例えば5Vに赤、A6に黄、A7に緑、GNDに黒のワイヤ)
  • 距離センサに3ピンJST型コネクタ付きジャンパワイヤをさす
  • そのワイヤを、上記のメス~メスジャンパワイヤにつなぐ

Firmata の書き込み(A6・A7ピンも使えるように)

Arduino IDE で 普通に Arduino Uno を選んで Firmata を書き込むと、Rapiro の JP1 から取り出した A6・A7 のアナログ入力の値は取得できませんでした。Arduino Uno は A5 までしかないからのようです。個人的な理解不足でいろいろハマりましたが… 以下の記事の方法で書き込むことでうまくいきました。

要は、Arduino IDE のボード定義ファイル(boards.txt)の Arduino Uno 用の部分を少し書き換えた Rapiro 用の定義(アナログ入力ピンを8個にしたもの)を加え、そのボードを選択した状態で StandardFirmataPlus を書き込みました。

コーディング

概要

  • 以下の三つのコードを入力し、同じ階層(e.g. /home/pi/rapiro/)に置く
    • rapiro-cfg.js : Rapiro の各種設定を格納するオブジェクト(前回と同一)
    • app.js : 制御プログラム本体
    • index.html : 操作インタフェース
      • 今回は app.js と index.html のみ前回から後述のように改編
  • 動作の概要
    • 距離センサの値は socket で送信されて、操作インタフェース上に常に表示
    • Rapiro が様々な動作・表情をしている途中で、Rapiro の前に障害物を置くと、別の動作・表情(今回の例では動作「blue」と表情「red」)に移行し、障害物がなくなると、元の動作に戻る

制御プログラム本体(app.js)

前回からの主な改編部分は以下

  • 27行目:障害物の検出閾値(距離[cm])の定数
  • 31行目:障害物の検出状態のフラグ
  • 116~155行目:距離センサデータの取得と障害物が検出された時の処理
app.js
// Rapiro を 内蔵 Raspberry Pi から Node.js + johnny-five で制御
// Rapiro標準の10モーション実装
// socket.ioによりブラウザから制御
// 距離センサ(GP2Y0A21YK)による動作制御
// 2017.01.21 by mkoku

'use strict';                                   // 厳格モードにする

// httpサーバとsocket.ioの設定
const express = require('express');             // expressモジュールを使う
const app     = express();                      // expressでアプリを作る
const server  = require('http').Server(app);    // httpサーバを起動しアプリをサーブ
const io      = require('socket.io')(server);   // サーバにsocket.ioをつなぐ
server.listen(3000);                            // サーバの3000番ポートをリッスン開始
app.use(express.static(__dirname));             // ホームdirにあるファイルを使えるようにする
app.get('/', function (req, res) {              // アクセス要求があったら
    res.sendFile(__dirname + '/index.html');    // index.htmlを送る
});
let   socket = null;                            // socket接続のインスタンス

// johnny-fiveの設定
const five  = require('johnny-five');           // johnny-fiveモジュールの読み込み
const cfg   = require('./rapiro-cfg');          // 設定ファイル'rapiro-cfg.js'の読み込み
const board = new five.Board({                  // Rapiro制御ボードのインスタンス
    port: '/dev/ttyAMA0'                        // シリアルポート名(環境による)
});
const pinServoDC = 17;                          // サーボへの電源供給ピン番号(17)
const obstacleThreshold = 9;                    // 障害物検出と判定する距離センサの閾値[cm]
const rapiro = {                                // Rapiroの設定や動作等を格納するオブジェクト
    ready: false,                               // Rapiroの準備状態(初期値false)
    obstacle: false                             // 障害物の検出状態(初期値false)
};


// 制御ボードの準備ができたら
board.on('ready', function() {

    // 各サーボのServoインスタンスを作成
    rapiro.head  = new five.Servo(cfg.servo.head);  // 頭
    rapiro.waist = new five.Servo(cfg.servo.waist); // 腰
    rapiro.r_s_r = new five.Servo(cfg.servo.r_s_r); // 右肩ロール(上下)
    rapiro.r_s_p = new five.Servo(cfg.servo.r_s_p); // 右肩ピッチ(開閉)
    rapiro.r_h_g = new five.Servo(cfg.servo.r_h_g); // 右手
    rapiro.l_s_r = new five.Servo(cfg.servo.l_s_r); // 左肩ロール(上下)
    rapiro.l_s_p = new five.Servo(cfg.servo.l_s_p); // 左肩ピッチ(開閉)
    rapiro.l_h_g = new five.Servo(cfg.servo.l_h_g); // 左手
    rapiro.r_f_y = new five.Servo(cfg.servo.r_f_y); // 右足ヨー(開閉)
    rapiro.r_f_p = new five.Servo(cfg.servo.r_f_p); // 右足ピッチ(内外)
    rapiro.l_f_y = new five.Servo(cfg.servo.l_f_y); // 左足ヨー(開閉)
    rapiro.l_f_p = new five.Servo(cfg.servo.l_f_p); // 左足ピッチ(内外)
    // 全サーボ(全身)をServosインスタンスに入れる
    rapiro.body = new five.Servos([
        rapiro.head,  rapiro.waist,                 // 頭・腰
        rapiro.r_s_r, rapiro.r_s_p, rapiro.r_h_g,   // 右腕
        rapiro.l_s_r, rapiro.l_s_p, rapiro.l_h_g,   // 左腕
        rapiro.r_f_y, rapiro.r_f_p,                 // 右足
        rapiro.l_f_y, rapiro.l_f_p                  // 左足
    ]);
    // 動作アニメーションのインスタンスを作成し、Servosを紐づける
    const bodyMotion = new five.Animation(rapiro.body);

    // 各LEDのインスタンスを作成
    rapiro.faceR = new five.Led(cfg.led.R);         // 赤
    rapiro.faceG = new five.Led(cfg.led.G);         // 緑
    rapiro.faceB = new five.Led(cfg.led.B);         // 青
    // 全LEDをLedsインスタンスに入れる
    rapiro.face  = new five.Leds([
        rapiro.faceR, rapiro.faceG, rapiro.faceB
    ]);
    // 表情アニメーションのインスタンスを作成し、Ledsを紐づける
    const facialExpression = new five.Animation(rapiro.face);

    // 動作アニメーションを実行する関数(引数:動作名の文字列)
    rapiro.execMotion = function(motionName) {
        const obj = cfg.motion;                                     // motionオブジェクトを取得
        for (let pname in obj) {                                    // motionオブジェクト中の全プロパティについて検討
            if (pname == motionName) {                              // 指定の動作名のプロパティがあったら
                rapiro.currentMotionName     = pname;               // 現在の動作名をその動作名に設定
                rapiro.currentMotionSequence = obj[pname];          // 現在の動作シーケンスにその動作を格納
                bodyMotion.enqueue(rapiro.currentMotionSequence);   // 動作アニメーションの開始
                return;                                             // あとは抜ける
            }
        }
        console.log('Error: unidentified motion argument');         // 指定の動作名のプロパティがなかったらエラー表示
    };

    // 表情アニメーションを実行する関数(引数:表情名の文字列)
    rapiro.execFace = function(faceName) {
        const obj = cfg.face;                                       // faceオブジェクトを取得
        for (let pname in obj) {                                    // faceオブジェクト中の全プロパティについて
            if (pname == faceName) {                                // 指定の表情名のプロパティがあったら
                rapiro.currentFaceName     = pname;                 // 現在の表情名をその表情名に設定
                rapiro.currentFaceSequence = obj[pname];            // 現在の表情シーケンスにその表情を格納
                facialExpression.enqueue(rapiro.currentFaceSequence);   // 表情アニメーションの開始
                return;                                             // あとは抜ける
            }
        }
        console.log('Error: unidentified face argument');           // 指定の表情名のプロパティがなかったらエラー表示
    };

    // サーボ電源をON
    this.pinMode(pinServoDC, five.Pin.OUTPUT);      // 電源供給ピンを出力モードに
    this.digitalWrite(pinServoDC, 1);               // 電源供給ピンに1を出力
    // 初期状態を作る
    rapiro.execMotion('stop');                      // 動作をstopに
    rapiro.execFace('white');                       // 表情をwhiteに
    // Rapiroの準備OK
    rapiro.ready = true;

    // デバッグ用:コマンドラインからモーションと表情を制御
    board.repl.inject({
        rapiro: rapiro
    });
    console.log("Type rapiro.execMotion('name') or rapiro.execFace('name')");

    // 距離センサ(GP2Y0A21YK)の処理
    // 汎用のアナログセンサ用のSensorクラスを使う場合(距離に反比例した値が得られる)
    // const proximity = new five.Sensor({
    //     pin: 'A6',
    //     freq: 250
    // });
    // 特定の距離センサ用のProximityクラスを使う場合(距離がcmまたはinで得られる)
    const proximity = new five.Proximity({
        controller: 'GP2Y0A21YK',   // コントローラに GP2Y0A21YK を指定
        pin: 'A6',                  // A6 ピンを指定
        freq: 250                   // サンプリング間隔[ms]
    });
    // 距離センサのデータが取得されたら
    proximity.on('data', function() {
        const dist = this.cm;                                       // cm or in(inch)
        // 障害物の処理
        if ((dist < obstacleThreshold) && (rapiro.obstacle == false)) {
            // 距離が閾値未満かつ現在障害物が未検出状態だったら
            rapiro.obstacle = true;                                 // 障害物検出状態に
            rapiro.previousMotionName = rapiro.currentMotionName;   // 現在の動作名を一時保管
            rapiro.previousFaceName = rapiro.currentFaceName;       // 現在の表情名を一時補間
            rapiro.execMotion('blue');                              // 動作blueを実行
            rapiro.execFace('red');                                 // 表情redを実行
        } else if ((dist >= obstacleThreshold) && (rapiro.obstacle == true)) {
            // 距離が閾値以上かつ現在障害物が検出状態だったら
            rapiro.obstacle = false;                                // 障害物未検出状態に
            rapiro.execMotion(rapiro.previousMotionName);           // 前の動作を再開
            rapiro.execFace(rapiro.previousFaceName);               // 前の表情を再開
        }
        // 距離の値と現状の動作・表情をsocketで送る
        if (socket != null) {
            socket.emit('proximity', {
                distance: dist
            });
            socket.emit('response', {
                motion: rapiro.currentMotionName,   // 動作名
                face:   rapiro.currentFaceName      // 表情名
            });
        }
    });

    // 終了時の処理
    this.on('exit', function() {
        this.digitalWrite(pinServoDC, 0);           // サーボの電源をOFF
    });
});

// WebSocketによる制御
io.on('connection', function(s) {
    socket = s;                                     // socket接続有り
    socket.on('request', function(data) {           // requestイベントが届いたら
        if (rapiro.ready == true) {                 // Rapiroの準備OKなら
            // アニメーションを実行
            rapiro.execMotion(data.motion);         // 動作アニメーション
            rapiro.execFace(data.face);             // 表情アニメーション
            // 現在の状態をresponseイベントとしてsocketで送る
            socket.emit('response', {
                motion: rapiro.currentMotionName,   // 動作名
                face:   rapiro.currentFaceName      // 表情名
            });
        }
    });
});

操作インタフェース(index.html)

前回からの主な改編部分は以下

  • 25~28行目:距離センサの値を表示する場所
  • 146~150行目:距離センサの値を表示
index.html
<!DOCTYPE html>
<!--
// Rapiro制御インタフェース
// Rapiro標準の10モーション実装
// socket.ioによりブラウザから制御
// 距離センサ(GP2Y0A21YK)による動作制御
// 2017.01.21 by mkoku
-->
<html>
<head>
    <meta charset='utf-8'>
    <meta name='viewport' content='width=device-width,initial-scale=1'>
    <title>Rapiro Proximity Sensor Test</title>
    <style>
        td.center {text-align: center}
    </style>
</head>

<body>
    <table>
        <!-- Rapiroからのレスポンスを表示する場所 -->
        <tr>
            <td class='center' colspan='3' id='msg'>motion / face</td>
        </tr>
        <!-- Rapiroの距離センサの値を表示する場所 -->
        <tr>
            <td class='center' colspan='3' id='prox'>proximity [cm]</td>
        </tr>
        <tr><td>&nbsp;</td></tr>
        <!-- 操作ボタン -->
        <tr>
            <td class='center'></td>
            <td class='center'><input type='button' value='↑' id='btnForward'></td>
            <td class='center'></td>
        </tr>
        <tr>
            <td class='center'><input type='button' value='←' id='btnLeft'></td>
            <td class='center'><input type='button' value='■' id='btnStop'></td>
            <td class='center'><input type='button' value='→' id='btnRight'></td>
        </tr>
        <tr>
            <td class='center'></td>
            <td class='center'><input type='button' value='↓' id='btnBack'></td>
            <td class='center'></td>
        </tr>
        <tr><td>&nbsp;</td></tr>
        <tr>
            <td class='center'><input type='button' value='R' id='btnRed'></td>
            <td class='center'><input type='button' value='G' id='btnGreen'></td>
            <td class='center'><input type='button' value='B' id='btnBlue'></td>
        </tr>
        <tr>
            <td class='center'><input type='button' value='Y' id='btnYellow'></td>
            <td class='center'><input type='button' value='P' id='btnPush'></td>
            <td class='center'></td>
        </tr>
    </table>

    <!-- socket.ioライブラリの読み込み(定型) -->
    <script src='/socket.io/socket.io.js'></script>

    <!-- メインスクリプト -->
    <script>
        var socket = io('http://192.168.43.200:3000');          // socket接続

        // 各ボタンオブジェクトの取得
        var btnStop    = document.getElementById('btnStop');    // stopボタン
        var btnForward = document.getElementById('btnForward'); // forwardボタン
        var btnBack    = document.getElementById('btnBack');    // backボタン
        var btnRight   = document.getElementById('btnRight');   // rightボタン
        var btnLeft    = document.getElementById('btnLeft');    // leftボタン
        var btnGreen   = document.getElementById('btnGreen');   // greenボタン
        var btnYellow  = document.getElementById('btnYellow');  // yellowボタン
        var btnBlue    = document.getElementById('btnBlue');    // blueボタン
        var btnRed     = document.getElementById('btnRed');     // redボタン
        var btnPush    = document.getElementById('btnPush');    // pushボタン

        // 各ボタンをクリックした時の処理
        btnStop.addEventListener('click', function() {      // stop
            socket.emit('request', {                        // requestイベントをsocketで送る
                motion: 'stop',                             // 動作名
                face:   'white'                             // 表情名
            });
        });
        btnForward.addEventListener('click', function() {   // forward
            socket.emit('request', {
                motion: 'forward',
                face:   'blue'
            });
        });
        btnBack.addEventListener('click', function() {      // back
            socket.emit('request', {
                motion: 'back',
                face:   'blue'
            });
        });
        btnRight.addEventListener('click', function() {     // right
            socket.emit('request', {
                motion: 'right',
                face:   'blue'
            });
        });
        btnLeft.addEventListener('click', function() {      // left
            socket.emit('request', {
                motion: 'left',
                face:   'blue'
            });
        });
        btnGreen.addEventListener('click', function() {     // green
            socket.emit('request', {
                motion: 'green',
                face:   'green'
            });
        });
        btnYellow.addEventListener('click', function() {    // yellow
            socket.emit('request', {
                motion: 'yellow',
                face:   'yellow'
            });
        });
        btnBlue.addEventListener('click', function() {      // blue
            socket.emit('request', {
                motion: 'blue',
                face:   'blue'
            });
        });
        btnRed.addEventListener('click', function() {       // red
            socket.emit('request', {
                motion: 'red',
                face:   'red'
            });
        });
        btnPush.addEventListener('click', function() {      // push
            socket.emit('request', {
                motion: 'push',
                face:   'blue'
            });
        });

        // Rapiroからのレスポンスの表示
        socket.on('response', function(data) {              // socketでresponseイベントが届いたら
            var msg = document.getElementById('msg');       // div要素を取得
            msg.innerHTML = data.motion + ' / ' + data.face;  // 動作と表情を表示
        });

        // Rapiroの距離センサの値の表示
        socket.on('proximity', function(data) {
            var prox = document.getElementById('prox');
            prox.innerHTML = data.distance + ' cm'
        });

    </script>
</body>
</html>

動作確認

  • 上記3つのコードを Rapiro 内部の Raspberry Pi に FTP
  • e.g. /home/pi/rapiro/ 内に
  • SSH で Rapiro 内部の Raspberry Pi にアクセスし、node で app.js を実行
  • 準備ができると Rapiro のサーボに通電され直立状態となり、目は白色にフェードする
pi@raspberrypi:~/rapiro $ node app.js
  • PC や スマホのブラウザで、Raspberry Pi のIPアドレスにアクセス
http://192.168.**.***:3000
  • 例えば以下のように振る舞えばOK

    • Rapiro を前進させておき
    • Rapiro の前に障害物を置くと、停止して腕を上げ下げし、目が赤くフェード
    • 障害物を取り除くと、再び前進

まとめ

以下の動画のようにできました。

Rapiro を node.js (johnny-five + socket.io) で制御 - 距離センサ搭載

今回、アナログの距離センサの値を取得するだけだからラクラク、と思って始めましたが、A6・A7ピンを使う部分で意外にハマってしまいました。Arduino でよく遊んではきましたが、それは、正規の Arduino と Arduino IDE で世にある事例で遊んでいただけのことで、少し凝ったことをしようとした時にはだいぶ知識・経験不足であることを思い知りました。しかし、おかげで、ボードの定義や、Arduino IDE がどのように動いているのかについて、少し勉強になりました。

いずれにせよ、これで、Rapiro にアナログのセンサを追加していく基本ができました。距離センサ以外にも、光(明るさ)、音、温度など、様々なアナログセンサに応用できます(おそらくしばらくは応用しないと思いますが)。

最終目標の 2 のうち、距離センサの搭載は達成できました。次回は、書籍「二足歩行ロボット 工作&プログラミング(リックテレコム)」の改造内容のうち、I2C 静電容量タッチセンサの搭載(第7章、7.5の内容の移植)を試したい考えです。

最新コードは以下にあります。
https://github.com/mkokubun/rapiro.js

0
0
0

Register as a new user and use Qiita more conveniently

  1. You get articles that match your needs
  2. You can efficiently read back useful information
  3. You can use dark theme
What you can do with signing up
0
0

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?