Raspberry Piでブラウザからサーボモータをちゃんと制御する
前回、前々回と、Raspberry Piでブラウザ上からカメラのパン・チルト制御する準備をしてきたのですが、肝心の制御プログラムが今の所とてもイマイチで、サーボを操作するたびにサーボがガタガタと震えます。これじゃ使いものにならん、ということで、もっとキビキビとサーボが動くように修正していきます。
修正にあたっては、以下の本と、そのフォローアップサイトがとても参考になりました。とても良い本です。
フォローアップサイト：（追加コンテンツ）サーボモーターをPCやスマートフォンから角度制御する
上記の内容をベースにしつつ、色々自分が理解しやすいように書き換えていきます。
WebIOPiはすでにインストールしている前提で、サーボを精度良く制御するために必要な、精度の高いPWM信号を生成するためのライブラリである”wiringPi”をインストールします。
$ git clone git://git.drogon.net/wiringPi $ cd wiringPi $ ./build
次に、PythonからWiringPiを操作するための”WiringPi2-Python”をインストールします。
$ cd $ sudo apt-get update $ sudo apt-get install python-dev python-setuptools $ git clone https://github.com/Gadgetoid/WiringPi2-Python.git $ cd WiringPi2-Python $ sudo python setup.py install $ sudo python3 setup.py install
WebIOPiと連携させるために、Raspbyerry Pi標準のPython2.x系向けに加え、Python3.x系向けにもインストールしています。
これで事前準備は完了です。次に、WebIOPiのプロジェクトを作っていきます。
WebIOPiでブラウザにアクセスしたときのhtmlが読み込まれるディレクトリは”/etc/webiopi/config”内のdoc-root変数で指定できますが、ここでは
doc-root = /home/pi/projects/sample-servo/html
に設定したとして、以下のようにディレクトリとファイルを用意します。
sample-servo
|-html
| |- index.html
|-css
| |- jquery-ui.min.css ★
| |- styles.css
|-js
| |- jquery-1.11.3.min.js ★
| |- jquery-ui.min.js ★
| |- require.js ★
| |- sample-servo.js
|-python
|- sample-servo.py
★印がついているファイルについては、それぞれ以下から持ってきます。
では、ファイルを作っていきます。あ、WebIOPiを自動起動設定にしている人は、一旦切っておいた方がデバッグしやすいです。
$ sudo update-rc.d webiopi remove
では改めて、まずは”html/index.html”から。サーボコントロール用のスライダーを２個、ホームポジションに戻ってくるためのボタンを１個作ります。
<!DOCTYPE html>
<html>
<head lang="ja">
<meta charset="UTF-8">
<meta http-equiv="X-UA-Compatible" content="IE=edge">
<meta name="viewport" content="width=device-width, user-scalable=no">
<title>Servo Sample</title>
<script src="../js/jquery-1.11.3.min.js"></script>
<script src="../js/jquery-ui.min.js"></script>
<script src="../js/require.js"></script>
<script>
require(["/webiopi.js", "../js/sample-servo.js"], function(){
webiopi().ready(initialize_webiopi);
});
</script>
</head>
<body>
<div>
<span id="tilt-slider"></span>
<div id="pan-slider"></div>
<br>
<div id="controls"></div>
</div>
</body>
</html>
不要なものも入っているかもしれませんが、ご了承ください。require.jsを使う理由はよく理解していないのですが、多分”/webiopi.js”でWebIOPi関係のファイルが先に読み込まれることを保証するようにしないと、色々不具合が発生するからかなーと思っています（すみません、JavaScriptの世界は未だによく わかっていません。。。）
続いて”css/styles.css”です。
button {
display: block;
margin: 3px;
height: 45px;
width: 80px;
}
#pan-slider{
width: 300px;
margin: 0 10px;
}
#tilt-slider{
display: inline-block;
}
スライダーとボタンのCSSを定義しています。
続いて、サーボ動作のコアとなる”python/sample-servo.py”です。
import webiopi
import time
import wiringpi2 as wiringpi
SERVO_PAN = 12
SERVO_TILT = 13
# SERVO_PAN (Left)90 ... 0 ... -90(Right)
# SERVO_TILT (Down)90 ... 0 ... -90(UP)
SERVO_PAN_TRIM = 12 # degree
SERVO_TILT_TRIM = 0 # degree
SERVO_PAN_LEFT_LIMIT = 60 # degree
SERVO_PAN_RIGHT_LIMIT = -60 # degree
SERVO_TILT_DOWN_LIMIT = 40 # degree
SERVO_TILT_UP_LIMIT = -40 # degree
##### SERVO SPECIFICATION #####
SERVO_ANGLE_MIN = -90 # degree
SERVO_ANGLE_MAX = 90 # degree
SERVO_PULSE_MIN = 0.5 # ms
SERVO_PULSE_MAX = 2.4 # ms
SERVO_CYCLE = 20 # ms
###############################
#### WIRINGPI SPECIFICATION ####
PWM_WRITE_MIN = 0
PWM_WRITE_MAX = 1024
################################
SERVO_DUTY_MIN = SERVO_PULSE_MIN/SERVO_CYCLE
SERVO_DUTY_MAX = SERVO_PULSE_MAX/SERVO_CYCLE
SERVO_PAN_DUTY_MIN = (SERVO_DUTY_MAX - SERVO_DUTY_MIN) / (SERVO_ANGLE_MAX - SERVO_ANGLE_MIN) * ((SERVO_PAN_LEFT_LIMIT +SERVO_PAN_TRIM) - SERVO_ANGLE_MIN) + SERVO_DUTY_MIN
SERVO_PAN_DUTY_MAX = (SERVO_DUTY_MAX - SERVO_DUTY_MIN) / (SERVO_ANGLE_MAX - SERVO_ANGLE_MIN) * ((SERVO_PAN_RIGHT_LIMIT+SERVO_PAN_TRIM) - SERVO_ANGLE_MIN) + SERVO_DUTY_MIN
SERVO_TILT_DUTY_MIN = (SERVO_DUTY_MAX - SERVO_DUTY_MIN) / (SERVO_ANGLE_MAX - SERVO_ANGLE_MIN) * ((SERVO_TILT_DOWN_LIMIT+SERVO_TILT_TRIM) - SERVO_ANGLE_MIN) + SERVO_DUTY_MIN
SERVO_TILT_DUTY_MAX = (SERVO_DUTY_MAX - SERVO_DUTY_MIN) / (SERVO_ANGLE_MAX - SERVO_ANGLE_MIN) * ((SERVO_TILT_UP_LIMIT +SERVO_TILT_TRIM) - SERVO_ANGLE_MIN) + SERVO_DUTY_MIN
SERVO_PAN_PWM_WRITE_MIN = PWM_WRITE_MAX * SERVO_PAN_DUTY_MIN
SERVO_PAN_PWM_WRITE_MAX = PWM_WRITE_MAX * SERVO_PAN_DUTY_MAX
SERVO_TILT_PWM_WRITE_MIN = PWM_WRITE_MAX * SERVO_TILT_DUTY_MIN
SERVO_TILT_PWM_WRITE_MAX = PWM_WRITE_MAX * SERVO_TILT_DUTY_MAX
def getServoPanPWMvalue(val):
# This function returns 0 ... 1024
pwm_value = int((SERVO_PAN_PWM_WRITE_MAX - SERVO_PAN_PWM_WRITE_MIN) * val + SERVO_PAN_PWM_WRITE_MIN)
return pwm_value
def getServoTiltPWMvalue(val):
# This function returns 0 ... 1024
pwm_value = int((SERVO_TILT_PWM_WRITE_MAX - SERVO_TILT_PWM_WRITE_MIN) * val + SERVO_TILT_PWM_WRITE_MIN)
return pwm_value
webiopi.setDebug()
def setup():
webiopi.debug("Script with macros - Setup")
def loop():
webiopi.sleep(5)
def destroy():
webiopi.debug("Script with macros - Destroy")
@webiopi.macro
def setHwPWMforPan(duty, commandID):
wiringpi.pwmWrite(SERVO_PAN, getServoPanPWMvalue(float(duty)))
@webiopi.macro
def setHwPWMforTilt(duty, commandID):
wiringpi.pwmWrite(SERVO_TILT, getServoTiltPWMvalue(float(duty)))
サーボの制御方法を具体的に考えていきます。まず、今回使っているサーボSG90のデータシートを確認します。これによると、
- PWMの周波数は50Hz (=20ms周期)
- -90°⇔0.5ms Pulse（デューティ比: 2.5%）
- 0°⇔1.45ms Pulse（デューティ比: 7.25%）
- 90° ⇔2.4ms Pulse（デューティ比: 12%）
となっているようです。
実際にPWMのデューティ比を設定して出力するのはpwmWrite関数で、これの引数は、wiringpiのページを見てみると、0〜1024となっています。つまり、最終的には設定したいデューティ比を0〜1024にマッピングしてやればよいわけですね。
pwmSetMode(wiringpi.GPIO.PWM_MODE_MS)とpwmSetClock(375)で、周期20msのPWM信号を出力するよう設定しています。前者で周波数を固定するように命令し、後者でその具体的な値を設定しています。375というのは18750/周波数（50）から来ているようですが、「18750はどっから来たの？」というのは、こちらの方が解説してくださっているので、ご参考ください。
これで、0〜1024が0〜20ms（0〜100%）に対応づけられたことになります。ということは、例えば角度を0°にするためにデューティ比7.25%の信号を入力したければ、1024×7.25/100=74（小数点以下四捨五入）をpwmWriteの引数に入れてやればよいということになります。同様に計算すれば、-90°のときは26、90°のときは123を与えることになります。
以上の計算を一般化したものが、18行目〜52行目あたりです。あえて変数を長い名前にしているのでパッと見では見づらいですが、順に追っていけばどういう計算をしているのかわかると思います。
最終的にカメラの位置と動作範囲を調整する際には、10行目〜16行目の値だけを変更すればOKです。
11行目と12行目のTRIMは、カメラの初期位置がまっすぐになっていないとき、ここに角度を入れることでカメラの初期位置を微調整できます。
13行目〜16行目は、カメラの動作範囲を決めています。今回のようにカメラがマウントから随分前に張り出す形だと、特に下側の回転（チルト）角度を制限するようにしておかないと、カメラが台にぶつかっても無理に回ろうとしてサーボを痛めてしまうので、ここで動作範囲を設定するようにしています。
サーボがどっちに回るかをイメージしだすと結構混乱するので、まずは動作範囲を狭くして、少しずつ動かしながら動作可能な範囲を確かめていけばよいと思います。
最後に、操作UIを生成する”js/sample-servo.js”を作成します。
ar sliderMin = 0;
var sliderMax = 20;
var sliderStep = 1;
var sliderValue = sliderMax/2;
var commandID = 0;
function applyCustomCss(custom_css){
var head = document.getElementsByTagName('head')[0];
var style = document.createElement('link');
style.rel = "stylesheet";
style.type = "text/css";
style.href = custom_css;
head.appendChild(style);
}
function initialize_webiopi(){
applyCustomCss('../css/jquery-ui.min.css')
applyCustomCss('../css/styles.css')
webiopi().refreshGPIO(false);
}
$(function() {
var panSliderHandler = function(e, ui){
var ratio = ui.value/sliderMax;
webiopi().callMacro("setHwPWMforPan", [ratio, commandID++]);
};
var tiltSliderHandler = function(e, ui){
var ratio = ui.value/sliderMax;
webiopi().callMacro("setHwPWMforTilt", [ratio, commandID++]);
};
$("#pan-slider").slider({
min: sliderMin,
max: sliderMax,
step: sliderStep,
value: sliderValue,
//change: panSliderHandler,
slide: panSliderHandler
});
$("#tilt-slider").slider({
orientation: "vertical",
min: sliderMin,
max: sliderMax,
step: sliderStep,
value: sliderValue,
//change: tiltSliderHandler,
slide: tiltSliderHandler
});
var homeBtn = webiopi().createButton("homeBtn", "Home", function(){
console.log("JS:Home")
webiopi().callMacro("setHwPWMforPan", [0.5, commandID++]);
//$("#pan-slider").slider("value",sliderValue);
var tilt = setInterval(function(){
webiopi().callMacro("setHwPWMforTilt", [0.5, commandID++]);
//$("#tilt-slider").slider("value",sliderValue);
clearInterval(tilt);
}, 100);
});
$("#controls").append(homeBtn);
$("#tilt-slider").height(240);
});
htmlのタグの直書きではなく、わざわざJavaScriptの中で後から自作のstyels.cssとjquery-ui.min.cssを適用している理由は、この形にしておかないと、webiopiが読み込むwebiopi.jsやwebiopi.cssが上書き優先されて、ページ表示がおかしくなってしまうからです。実際、自分の場合はスライダーが表示されなくなってしまい、原因がわからず結構ハマッてしまいました。
スライダーのハンドラ関数 （panSliderHander, tiltSliderHandler）は、Pythonの関数（setHwPWMforPan, setHwPWMforTilt）に、スライダの上限値に対する現在の位置の割合を表すratio（0〜1）を渡しています。この割合値が、先に”python/sample-servo.py”で設定した角度の上限/下限内での位置にそのまま対応することになります。
それから、Homeボタンを押すことでカメラを初期位置に戻すようにしているのですが、残念なことに、Homeボタンを押すとカメラの位置は戻るのですが、スライダーのつまみが初期位置に戻ってくれません。結構試行錯誤したのですが、戻ってくれません。謎です。まー、ページをリロードすれば済む話ですし、そもそもスライダーにこだわらずにボタンUIにすれば良いだけですが。。。何か悔しい。
さて、お疲れ様でした。以上でソースの作成は終了です。早速実行。。。の前に、大事なことを言い忘れていました。Raspbyerry Pi B+およびPi 2では、正確なPWM信号は、最大２つまでしか同時に出すことができません（昔のRaspberry だと、１つだけ）。そんなことも知らずに今まで偉そうに（？）工作ブログを書いていて恥ずかしい限りなのですが、とにかくそういうことらしいので、配線は以下のように直しています。
GPIOの12か18のどちらか一つと、GPIOの13と19のどちらか一つを使います。ここではパン用にGPIO12、チルト用にGPIO13を接続しています。
では、改めて実行してみます。
$ sudo webiopi -d -c /etc/webiopi/config
起動後にブラウザで”http://（Raspberry PiのIPアドレス）:8000/”にアクセスすると、以下のような画面になります。
では、制御してみます。以下の動画の前半は前回時点でのプログラムによる制御で、後半が今回作成したプログラムによる制御です。
だいぶ安定して動作するようになっていることがわかるかと思います。
というわけで、サーボをブラウザからキビキビと動かすために色々やりました。丸一日かかってしまいましたが、勉強になりました。
ところで、「サーボ２個までは良いとして、３個以上のサーボを制御したい場合はどうするの？」というのが気になるところですが、自分がパッと思いつくやり方は、直接のサーボの制御はArduinoに任せて、Raspberry PiはArduinoに対してシリアルかI2Cで「どれをどれだけ回せ」というように指示させる方法ですかね。実際ラピロはそうなっていますし。
今の所はサーボはこのカメラのパン・チルト制御でしが使う予定がないので、もし追加機能でサーボを使う必要が出たら、やってみたいと思います。
というか、カメラを動かすところにこだわり過ぎて、肝心のカメラ撮影が何もできておりません。ということで、次回はカメラの撮影部分をなんやかんやしたいと思います。