サーボモータを滑らかに動かそう
電子工作やロボットでよく使われるサーボモータですが、どのように動かしていますか？多くの場合は角度を指定し、その角度に向けて動かすというのが基本かと思います。その場合、瞬時にその角度まで移動するか、一定の速度でその角度まで移動する、という形になると思います。一方でロボットなどに使う場合はカクカクとした機械的な動作ではなく、滑らかに、スムーズに動かしたい、と思うことがあります。
そのようなときに使えるテクニックがサーボイージング(Servo Easing)と呼ばれるものです。便利でありながら、簡単に実装することができます。
この記事ではサーボイージングとはなにか、そしてどのようにすれば実現できるかについて解説していきます。
実際に動作を見てみよう
早速ですがサーボイージングの実例を見てみましょう。
こちらで作成中のロボットを使った例です。
この例では首振り動作を3つの動かし方で動かしているものです。首振り動作はサーボモータによってコントロールされています。等速で動かすLinear, 動作の始めと終わりを緩やかに動かすEase In Out Cubic, 動作の終わりでまるで跳ね返るように動かしているEase Out Bounceの3つです。
Linearに比べてEase In Out Cubicは滑らかな動作に見えるかと思います。またEase Out Bounceもコミカルで面白い動きになっていますね。
こうした差が、複雑そうに見えて実は単純な仕組みで実現できます。このテクニックが”サーボイージング”と呼ばれるものです。
やり方を見てみよう！
こうしたサーボイージングの機能を持つライブラリが存在しています。
https://github.com/ArminJo/ServoEasing
しかし、こうしたサーボイージングの仕組みを自力で実装することも可能です。比較的簡単です。
この記事では簡単な実装例から見ていきましょう。
まずは図１のような回路を用意します。
マイコンボードとしてArduino R4 Minima、サーボモータにはTowerPro SG90を利用しています。5V出力とGND、及びPWM対応しているDIGITAL 9ピンを信号線としてSG90につなぎます。ArduinoにはUSBポート経由で給電するものとします。
実際の回路が図２です。左のオレンジと黒のものの中にSG90を取り付けてあります。今後の比較実験のためにこのようにしました。
SG90を取り付けている黒い部分は以前にSG90用固定パーツとして作成したものです。すぐに3Dプリンタで印刷できる3Dモデルがダウンロード可能ですのでぜひご利用ください。
Step 1:まずは動かしてみる
それではまず動かしてみましょう。以下のスケッチをArduinoに書き込みます。
#include <Servo.h>
unsigned int interval_ms = 2000;////loop内処理の実行間隔
unsigned int elapsed_ms, previous_ms, current_ms;
////サーボモータSG90の角度とパルス幅
static const int sg90_minAngle = 0;
static const int sg90_maxAngle = 180;
static const int sg90_minPulse = 500;
static const int sg90_maxPulse = 2400;
static const int NUM_PATTERN = 2;
int angle[NUM_PATTERN] = {10, 170};////指定角度パターン２つ
int currentPattern = 0;
Servo servo1;
////角度をパルス幅に変換する
int angle2Pulse(int angle, int minAngle, int maxAngle, int minPulse, int maxPulse) {
double angleRate = double(angle) / 180.0;
int pulse = angleRate * (sg90_maxPulse - sg90_minPulse) + sg90_minPulse;
return pulse;
}
void setup() {
previous_ms = 0;
servo1.attach(9);////Arduino Uno 9ピンにサーボを接続
}
void loop() {
current_ms = millis();////経過時間を取得
elapsed_ms = current_ms - previous_ms;////前回の実行からの経過時間を計算
if (elapsed_ms >= interval_ms) {////経過時間が実行間隔以上の場合、処理を実行
currentPattern = (currentPattern + 1) % NUM_PATTERN;////パターンを繰り返す
////サーボの角度をパルス幅に変換して指定
servo1.writeMicroseconds(angle2Pulse(angle[currentPattern], sg90_minAngle, sg90_maxAngle, sg90_minPulse, sg90_maxPulse));
previous_ms = current_ms;////現在の時間を記録
}
}このプログラムでは2秒ごとにSG90を角度10度と170度の間で行ったり来たりさせるプログラムです。
まずこのスケッチにはServoライブラリが必要ですので最初にインストールします。インストール済みの場合は必要ありません。(図２)
このプログラムではPWMのパルス幅で角度を指定するwriteMicrosecondsを利用しています。販売サイトなどでSG90のパルス幅は角度0から180度の間で0.5msから2.4msと記載されていますので、それを定数としてプログラム内に記述しています。（単位はmsなので2.4は2400となっています。）
static const int sg90_minAngle = 0;
static const int sg90_maxAngle = 180;
static const int sg90_minPulse = 500;
static const int sg90_maxPulse = 2400;特定の角度に対応するPWMパルス幅を計算するための関数
int angle2Pulse(int angle, int minAngle, int maxAngle, int minPulse, int maxPulse)
を用意したので、これを使ってパルス幅を計算します。(angle：指定角度、minAngle：最小角度、maxAngle：最大角度、minPulse：最小パルス幅、maxPulse：最大パルス幅、戻り値：指定角度に対応するパルス幅)
また多くのArduinoのスケッチのサンプルではloop()内でdelayを使って時間間隔を制御していますが、今回はタイミング調整の関係からdelayは用いず、Arduino起動時からの時間をミリ秒で返すmillis()を利用して実行時間間隔を制御しています。
実際の動作はこのようになるはずです。
Step 2:移動時間を設定できるようにする
Step1で作ったものは特定の角度へ瞬時に移動するものでした。しかし、状況によっては指定の角度までゆっくり動いてほしかったり、その速度を調整したい場合があります。
そこで今度はStep1でのスケッチを拡張して、次の角度までの移動時間を指定できるようにしましょう。
Step1でのスケッチを、次のように拡張します。
#include <Servo.h>
unsigned int interval_ms = 10;////loop内処理の実行間隔
unsigned int elapsed_ms, previous_ms, current_ms;
////サーボモータSG90の角度とパルス幅
static const int sg90_minAngle = 0;
static const int sg90_maxAngle = 180;
static const int sg90_minPulse = 500;
static const int sg90_maxPulse = 2400;
static const int NUM_PATTERN = 2;
int angle[NUM_PATTERN] = {10, 170};////指定角度パターン２つ
int currentPattern = 0, previousPattern = 0;
int operationTime[NUM_PATTERN] = {1000, 2000};////パターンごとの移動時間
int waitTime = 2000;////動作終了後の停止時間
int motionStartTime = 0, motionEndTime = 0;
////各種状態を表す
static const int MOTION_READY = 0;
static const int MOTION_OPERATING = 1;
static const int MOTION_WAITING = 2;
int currentMode = MOTION_READY;////現在の状態
Servo servo1;
////角度をパルス幅に変換する
int angle2Pulse(int angle, int minAngle, int maxAngle, int minPulse, int maxPulse)
{
double angleRate = double(angle) / 180.0;
int pulse = angleRate * (sg90_maxPulse - sg90_minPulse) + sg90_minPulse;
return pulse;
}
void setup()
{
previous_ms = 0;
servo1.attach(9);////Arduino Uno 9ピンにサーボを接続
}
void loop()
{
current_ms = millis();////経過時間を取得
elapsed_ms = current_ms - previous_ms;////前回の実行からの経過時間を計算
if (elapsed_ms >= interval_ms)////経過時間が実行間隔以上の場合、処理を実行
{
if (currentMode == MOTION_READY)////各パターンの開始時
{
previousPattern = currentPattern;
currentPattern = (currentPattern + 1) % NUM_PATTERN;////パターンを繰り返す
currentMode = MOTION_OPERATING;////動作モードに移行
motionStartTime = current_ms;////動作開始時間を記録
}
else if (currentMode == MOTION_OPERATING)////動作実行中
{
////経過時間から現在の動作が0から1のどこにあたるかを計算
double currentMotionRate = double(current_ms - motionStartTime) / operationTime[currentPattern];
if (currentMotionRate >= 1.0)////1.0以上ならば動作終了
{
currentMode = MOTION_WAITING;////動作終了モードに移行
motionEndTime = current_ms;////動作停止時間を記録
}
else
{
////経過時間から算出したcurrentMotionRateから現在の角度を計算
int currentAngle = double(angle[currentPattern] - angle[previousPattern]) * currentMotionRate + angle[previousPattern];
////サーボの角度をパルス幅に変換して指定
servo1.writeMicroseconds(angle2Pulse(currentAngle, sg90_minAngle, sg90_maxAngle, sg90_minPulse, sg90_maxPulse));
}
}
else if (currentMode == MOTION_WAITING)////動作終了時
{
if (current_ms - motionEndTime >= waitTime)////動作終了時の停止時間が経過したら次の動作へ移行
{
currentMode = MOTION_READY;
}
}
previous_ms = current_ms;////現在の時間を記録
}
}
実際の動作は次のようになるはずです。先ほどと同じ角度10度、170度の間をそれぞれ1秒、2秒かけて移動するようにしています。先ほどよりゆっくりとした動きで一定の速度で移動していますね。
大きな変更点として、まず動作間隔を変更しています。
unsigned int interval_ms = 10;////loop内処理の実行間隔次に、待ち状態、動作中、停止中の３つの状態を表す定数と変数を用意して状態を管理します。
////各種状態を表す
static const int MOTION_READY = 0;
static const int MOTION_OPERATING = 1;
static const int MOTION_WAITING = 2;
int currentMode = MOTION_READY;////現在の状態これら３つの状態の流れと役割を図３に示します。
まずMOTION_READYで次の動作角度、移動時間を決定し、動作開始時刻を記録します。そしてMOTION_OPERATINGに移行します。MOTION_OPERATINGでは記録した動作開始時刻と現在の時間、そして移動時間から、現在の動作が0から1のどこにあたるかの計算をします。0が動作開始時、1が動作終了時を意味します。
////経過時間から現在の動作が0から1のどこにあたるかを計算
double currentMotionRate = double(current_ms - motionStartTime) / operationTime[currentPattern];この値を使って現在の角度を決定しますが、実はこの実装が後のサーボイージングの導入時に効果を発揮します。
指定された角度まで到達したら、状態はMOTION_WAITINGに移行し、指定された時間だけ停止します。指定時間を過ぎたら、MOTION_READYに再度移行します。
後編へ続く！
ここまでで準備は完了です。後編では更に滑らかなサーボモータの動作を実現するため、サーボイージングを導入していきます。
