やりたいこと
最終目標（前回と同じ）
- （[3]で完了）
二足歩行ロボット Rapiro の制御を、標準の Arduino IDE（C/C++）ベースから、JavaScript（Node.js）ベースに移植し、Rapiro を IoT デバイスっぽくする - 可能な限り、書籍「二足歩行ロボット 工作＆プログラミング（リックテレコム）」の改造内容も移植する
- 距離センサの搭載
- 静電容量タッチセンサの搭載など
- この Rapiro をベースに、さらに賢そうな遊び方を模索する
今回 [4] の目標
- 距離センサ（GP2Y0A21YK）を搭載し、一定未満の範囲に障害物を検出したら動作を変え、障害物が無くなったらもとの動作に戻るようにする
- 書籍「二足歩行ロボット 工作＆プログラミング（リックテレコム）」の第7章、7.1～7.3の内容に相当
方法
機材
-
Rapiro
- 電源を入れる前に、Arduino IDE を用いて、制御ボードに StandardFirmataPlus を書き込んでおくが、少々コツがあるので以下で補足説明
- Raspberry Pi 3 Model B
- 距離センサ GP2Y0A21YK
- 距離センサ接続用の部品（接続できればよく、以下全てが必須というわけではない）
-
ピンヘッダ（これは必須、Rapiro のメイン基板にはんだ付けしてアナログ入力A6・A7 および 5V・GND を引き出す）
- はんだ、はんだごて
- 3ピンJST型コネクタ付きジャンパワイヤ（距離センサ付属のケーブルに代えて使うと接続が容易）
- ジャンパワイヤ(メス～メス)（メイン基板のピンと距離センサを接続）
-
ピンヘッダ（これは必須、Rapiro のメイン基板にはんだ付けしてアナログ入力A6・A7 および 5V・GND を引き出す）
- エネループ（単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/）に置く
- 動作の概要
- 距離センサの値は 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> </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> </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 の前に障害物を置くと、停止して腕を上げ下げし、目が赤くフェード
- 障害物を取り除くと、再び前進
まとめ
以下の動画のようにできました。
今回、アナログの距離センサの値を取得するだけだからラクラク、と思って始めましたが、A6・A7ピンを使う部分で意外にハマってしまいました。Arduino でよく遊んではきましたが、それは、正規の Arduino と Arduino IDE で世にある事例で遊んでいただけのことで、少し凝ったことをしようとした時にはだいぶ知識・経験不足であることを思い知りました。しかし、おかげで、ボードの定義や、Arduino IDE がどのように動いているのかについて、少し勉強になりました。
いずれにせよ、これで、Rapiro にアナログのセンサを追加していく基本ができました。距離センサ以外にも、光（明るさ）、音、温度など、様々なアナログセンサに応用できます（おそらくしばらくは応用しないと思いますが）。
最終目標の 2 のうち、距離センサの搭載は達成できました。次回は、書籍「二足歩行ロボット 工作＆プログラミング（リックテレコム）」の改造内容のうち、I2C 静電容量タッチセンサの搭載（第7章、7.5の内容の移植）を試したい考えです。
最新コードは以下にあります。
https://github.com/mkokubun/rapiro.js