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

# やりたいこと

## 最終目標（[前回](http://qiita.com/mkoku/items/e04289eafd86ea64f28d)と同じ）
1. （[[3]](http://qiita.com/mkoku/items/e04289eafd86ea64f28d)で完了）~~二足歩行ロボット [Rapiro](http://www.rapiro.com/ja/) の制御を、標準の Arduino IDE（C/C++）ベースから、JavaScript（Node.js）ベースに移植し、Rapiro を IoT デバイスっぽくする~~
 * （[[1]]((http://qiita.com/mkoku/items/191ead1b62693003bf64))[[2]](http://qiita.com/mkoku/items/490606b5c70eeb9e5ea3)で完了）~~かつ、PCでの制御ではなく、Rapiro 内部に搭載した Raspberry Pi での制御とし、完全無線化を図る~~
 * （[[3]](http://qiita.com/mkoku/items/e04289eafd86ea64f28d)で完了）~~全機能の移植が難しくとも、最低限、10個の基本動作は移植・再現する~~
2. 可能な限り、[書籍「二足歩行ロボット 工作＆プログラミング（リックテレコム）」](http://www.ric.co.jp/book/contents/book_995.html)の改造内容も移植する
 * 距離センサの搭載
 * 静電容量タッチセンサの搭載など
3. この Rapiro をベースに、さらに賢そうな遊び方を模索する

## 今回 [4] の目標
* 距離センサ（[GP2Y0A21YK](https://www.switch-science.com/catalog/40/)）を搭載し、一定未満の範囲に障害物を検出したら動作を変え、障害物が無くなったらもとの動作に戻るようにする
 * [書籍「二足歩行ロボット 工作＆プログラミング（リックテレコム）」](http://www.ric.co.jp/book/contents/book_995.html)の第7章、7.1～7.3の内容に相当


# 方法

## 機材
* [Rapiro](http://www.rapiro.com/ja/)
 * 電源を入れる前に、Arduino IDE を用いて、制御ボードに StandardFirmataPlus を書き込んでおくが、__少々コツがある__ので以下で補足説明
* Raspberry Pi 3 Model B
 * Rapiro 内部に搭載済（[こちらの手順](http://qiita.com/mkoku/items/191ead1b62693003bf64)で）
 * Node.js インストール済（[こちらの手順](http://qiita.com/mkoku/items/111e6ec21395065f0c28)で）
 * npm で johnny-five, socket.io, express をインストール済
* 距離センサ [GP2Y0A21YK](https://www.switch-science.com/catalog/40/)
* 距離センサ接続用の部品（接続できればよく、以下全てが必須というわけではない）
    * [ピンヘッダ](https://www.switch-science.com/catalog/92/)（これは必須、Rapiro のメイン基板にはんだ付けしてアナログ入力A6・A7 および 5V・GND を引き出す）
        * はんだ、はんだごて
    * [3ピンJST型コネクタ付きジャンパワイヤ](https://www.switch-science.com/catalog/2421/)（距離センサ付属のケーブルに代えて使うと接続が容易）
    * [ジャンパワイヤ(メス～メス)](https://www.switch-science.com/catalog/2295/)（メイン基板のピンと距離センサを接続）
* エネループ（単3）5本 または [Rapiro 用 ACアダプタ](https://www.switch-science.com/catalog/1940/)
* PC（Windows10、Raspberry Pi に SSH や FTP できればなんでも良い）
* スマホまたはタブレット
* 無線LAN環境


## 距離センサの搭載
[Rapiro Wiki](http://wiki.rapiro.com/page/index/) にある「[RAPIROに近接センサを取り付ける](http://wiki.rapiro.com/page/installing_distance_sensor_ja/)」を参考にしました。さらに楽するために、[3ピンJST型コネクタ付きジャンパワイヤ](https://www.switch-science.com/catalog/2421/)と[ジャンパワイヤ(メス～メス)](https://www.switch-science.com/catalog/2295/)を使いました

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

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

* [二足歩行ロボット Rapiro を Node.js で制御 [3.5] A6/A7ピンを使う](http://qiita.com/mkoku/items/253b7b2869f0baba2fa2)

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


## コーディング

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

### 制御プログラム本体（app.js）
[前回](http://qiita.com/mkoku/items/e04289eafd86ea64f28d)からの主な改編部分は以下

* 27行目：障害物の検出閾値（距離[cm]）の定数
* 31行目：障害物の検出状態のフラグ
* 116～155行目：距離センサデータの取得と障害物が検出された時の処理

```js: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）

[前回](http://qiita.com/mkoku/items/e04289eafd86ea64f28d)からの主な改編部分は以下

* 25～28行目：距離センサの値を表示する場所
* 146～150行目：距離センサの値を表示

```html: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) で制御 － 距離センサ搭載](http://img.youtube.com/vi/nVEPS203CWc/0.jpg)](https://youtu.be/nVEPS203CWc)

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

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

最終目標の 2 のうち、距離センサの搭載は達成できました。[次回](http://qiita.com/mkoku/items/e2515c8398c9690fadf4)は、[書籍「二足歩行ロボット 工作＆プログラミング（リックテレコム）」](http://www.ric.co.jp/book/contents/book_995.html)の改造内容のうち、[I2C 静電容量タッチセンサ](https://www.switch-science.com/catalog/825/)の搭載（第7章、7.5の内容の移植）を試したい考えです。

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