モーションを作成する
ロボットが動く仕組み
最初に、サーボモータ制御のロボットがどのように動くか簡単に説明します。
Arduinoでのサーボモータの制御方法は基本的にServoライブラリを使うことになります。サーボモータを制御するでも見たとおり、Servoライブラリでできることはサーボモータの角度を決めることだけです。
そこで、パラパラ漫画の要領で、短い時間間隔で角度を連続してずらしていき、動いているように見せます。このようにして作った一連の動きをモーションといいます。(デモプログラムではサーボモータが原点（可動域の中央）にある場合の角度を0°として、‐90°〜90°の範囲で角度を変化させます。）
首サーボモータ：0°
首サーボモータ：-15°
首サーボモータ：-30°
首サーボモータ：-45°
首サーボモータ：-60°
それでは、実際にデモプログラムを触りながら、モーションの調整やオリジナルモーションの作り方を覚えていきます。
デモプログラムを見てみよう
以下がデモプログラムの151行目から始まるメイン関数です。ここではシリアル通信で入力された値に応じてモーションを呼び出しています。playMotion()がモーションを再生する関数で、引数に実行したいモーションとそのモーションのポーズ数を指定します。ポーズ数に関しては後ほど説明します。
例えば、'w'が入力されるとモーションmove_forward(前進）が実行されます。また、'j'が入力されるとモーションlook_around(首ふり）が実行されます。
//====================================================================================================================
//Main
//====================================================================================================================
void setup(){
initServo();
Serial.begin(9600);
}
void loop(){
if(Serial.available()){
char cmd = Serial.read();
switch (cmd){
case 'w':
playMotion(move_forward, 6);
break;
case 's':
playMotion(move_back, 6);
break;
case 'a':
playMotion(turn_left, 7);
break;
case 'd':
playMotion(turn_right, 7);
break;
case 'j':
playMotion(look_around, 5);
break;
case 'k':
playMotion(home_position, 1);
break;
}
}
delay(100);
}
次に、デモプログラムの21行目から始まるモーションの定義を見てみます。各モーションは2次元配列であり、配列内の要素はあるポーズの時の{腰サーボの角度、右足サーボの角度、左足サーボの角度、首サーボの角度、遷移時間（ミリ秒）}を表します。これを複数登録することにより、連続してポーズを変えていき、モーションとしています。
（例）look_around
ポーズ1：look_around[0]={0,0,0,0,200} 現在の姿勢から、200ミリ秒間で、全てのサーボの角度を0°にします（直立姿勢）
ポーズ2：look_around[1]={0,0,0,60,200} 直立姿勢から、200ミリ秒で、首サーボの角度を60°にします。（右を向く）
ポーズ3：look_around[2]={0,0,0,0,200} 右を向いた姿勢から、200ミリ秒で、首サーボの角度を0°にします。（直立姿勢に戻る）
ポーズ4：look_around[3]={0,0,0,-60,200} 直立姿勢から、200のミリ秒で、首サーボの角度を‐60°にします。（左を向く）
ポーズ5：look_around[4]={0,0,0,0,200} 左を向いた姿勢から、200ミリ秒で、首サーボの角度を0°にします。（直立姿勢に戻る）
ポーズ数が5なので、実行するきはplayMotion(look_around, 5);となります。モーションlook_aroundを実行するときは、全部で5個のポーズを順番にとっていくことになります。
//====================================================================================================================
//Motion
//====================================================================================================================
//motion1 look around
int look_around[5][SERV_NUM+1] ={{0,0,0,0,200},{0,0,0,60,300},{0,0,0,0,300},{0,0,0,-60,300},{0,0,0,0,300}};
//motion2 move forward
int move_forward[6][SERV_NUM+1] ={{35,0,0,0,200},{35,-15,-15,0,200},{0,-15,-15,0,600},{-35,-15,-15,0,200},{-35,15,15,0,400},{0,15,15,0,600}};
//motion3 move back
int move_back[6][SERV_NUM+1] ={{-35,0,0,0,200},{-35,-15,-15,0,200},{0,-15,-15,0,600},{35,-15,-15,0,200},{35,15,15,0,400},{0,15,15,0,600}};
//motion4 home position
int home_position[1][SERV_NUM+1] ={{0,0,0,0,300}};
//motion5 turn right
int turn_right[7][SERV_NUM+1] ={{35,0,0,0,200},{35,-15,0,0,200},{0,-15,0,0,600},{-35,-15,0,0,200},{-35,-15,-15,0,200},{-35,0,0,0,200},{0,0,0,0,600}};
//motion6 turn left
int turn_left[7][SERV_NUM+1] ={{-35,0,0,0,200},{-35,0,15,0,200},{0,0,15,0,600},{35,0,15,0,200},{35,15,15,0,200},{35,0,0,0,200},{0,0,0,0,600}};
オリジナルのモーションを作ってみよう。
7つ目のモーションoriginalを作ってみます。ポーズ数は5です。
- ポーズ1：original[0]={0,0,0,0,200} 現在のポーズから200ミリ秒で、ポーズ1（直立姿勢）をとる
- ポーズ2：original[1]={45,0,0,60,300} ポーズ1から、300ミリ秒で、ポーズ2（右足を上げて、首を右にふる）をとる
- ポーズ3：original[2]={0,0,0,0,1000} ポーズ2から、1000ミリ秒で、ポーズ3（直立姿勢）をとる
- ポーズ4：original[3]={-45,0,0,-60,300} ポーズ3から、300ミリ秒で、ポーズ4（左足を上げて、首を左にふる）をとる
- ポーズ5：original[4]={0,0,0,0,1000} ポーズ4から、1000ミリ秒で、ポーズ5（直立姿勢）をとる
デモプログラムのモーションの定義部分に以下のように7つ目のモーションoriginalを追加してください。
//====================================================================================================================
//Motion
//====================================================================================================================
//motion1 look around
int look_around[5][SERV_NUM+1] ={{0,0,0,0,200},{0,0,0,60,300},{0,0,0,0,300},{0,0,0,-60,300},{0,0,0,0,300}};
//motion2 move forward
int move_forward[6][SERV_NUM+1] ={{35,0,0,0,200},{35,-15,-15,0,200},{0,-15,-15,0,600},{-35,-15,-15,0,200},{-35,15,15,0,400},{0,15,15,0,600}};
//motion3 move back
int move_back[6][SERV_NUM+1] ={{-35,0,0,0,200},{-35,-15,-15,0,200},{0,-15,-15,0,600},{35,-15,-15,0,200},{35,15,15,0,400},{0,15,15,0,600}};
//motion4 home position
int home_position[1][SERV_NUM+1] ={{0,0,0,0,300}};
//motion5 turn right
int turn_right[7][SERV_NUM+1] ={{35,0,0,0,200},{35,-15,0,0,200},{0,-15,0,0,600},{-35,-15,0,0,200},{-35,-15,-15,0,200},{-35,0,0,0,200},{0,0,0,0,600}};
//motion6 turn left
int turn_left[7][SERV_NUM+1] ={{-35,0,0,0,200},{-35,0,15,0,200},{0,0,15,0,600},{35,0,15,0,200},{35,15,15,0,200},{35,0,0,0,200},{0,0,0,0,600}};
//motion7 original motion
int original[5][SERV_NUM+1] ={{0,0,0,0,200},{45,0,0,60,300},{0,0,0,0,1000},{-45,0,0,-60,300},{0,0,0,0,1000}};
シリアル通信で'l'を受信したときにモーションoriginalを実行するようにメイン関数を以下のように書き換えます。
//====================================================================================================================
//Main
//====================================================================================================================
void setup(){
initServo();
Serial.begin(9600);
}
void loop(){
if(Serial.available()){
char cmd = Serial.read();
switch (cmd){
case 'w':
playMotion(move_forward, 6);
break;
case 's':
playMotion(move_back, 6);
break;
case 'a':
playMotion(turn_left, 7);
break;
case 'd':
playMotion(turn_right, 7);
break;
case 'j':
playMotion(look_around, 5);
break;
case 'k':
playMotion(home_position, 1);
break;
case 'l':
playMotion(original, 5);
break;
}
}
delay(100);
}
下記写真のように動いたか確認してください。不安定な場合はパラメータを色々と調整してみてください。特に足を上げるときの遷移時間はある程度短く（200〜300）、足を下ろすときの遷移時間はある程度長く（600以上）にすると安定しやすくなります。また、左右の足サーボモータを大きく曲げる動作をすると、もう片方の足に引っかかる場合があります。このような場合は足を曲げる角度を小さくする等してください。