プログラミング

【Arduino】複数のサーボモーターでモーション作りをする方法

複数のサーボモーターをあらかじめ作成したモーションの通りに動かすArduinoの自作コードを紹介します。現在のモーターの角度と取らせたいポーズのモーターの角度をスムーズに切り替えることで滑らかなモーションの実現を目指します。

👆実際にこの記事のコードを使用してサーボモーターを制御している様子。（ネコパンチのモーションです）

使用するライブラリについて

今回はServo.hライブラリを使用します。Servo.hは、Arduino IDEに標準でインストールされているため別途インストールする必要はありません。
また、サーボモーターを動かす関数には角度を指定するServo.write()とパルス幅を指定するServo.writeMicroseconds()がありますが、今回はより細かい角度で動かしたいのでServo.writeMicroseconds()を使用します。

コード全体の紹介

まずは、コード全体の大まかな説明をします。詳細な解説は「コードのゆる解説」をご確認ください。

  1. #include <Servo.h>
  3. Servo myservoU; // 3.先create servo object to control a servo
  4. Servo myservoM; // 6.中create servo object to control a servo
  5. Servo myservoD; // 5.根本create servo object to control a servo
  6. // サーボ制御用の変数
  7. #define sv_U_default 1500 // 1000〜2000で1500が中央
  8. #define sv_M_default 1500 - 80
  9. #define sv_D_default 1500 - 20
  10. float servoUMD_now[3] = {{sv_U_default}, {sv_M_default}, {sv_D_default + 500}};
  11. float pose_setUMD[8][3] = {
  12. {sv_U_default, sv_M_default, sv_D_default + 500}, // pose 0 : ホームポジション
  13. {sv_U_default + 400, sv_M_default - 650, sv_D_default + 250}, // pose 1 : ネコパンチ構え
  14. {sv_U_default - 200, sv_M_default - 400, sv_D_default}, // pose 2 : ネコパンチ振りかざし
  15. {sv_U_default + 200, sv_M_default + 100, sv_D_default + 30}, // pose 3 : ネコパンチ振り下ろし
  16. {sv_U_default + 200, sv_M_default - 200, sv_D_default}, // pose 4 : ネコお手振りかざし
  17. {sv_U_default - 200, sv_M_default + 200, sv_D_default}, // pose 5 : ネコお手振り下ろし
  18. {sv_U_default, sv_M_default + 200, sv_D_default}, // pose 6 : ネコお手振り下ろし2
  19. {sv_U_default, sv_M_default - 650, sv_D_default + 500} // pose 7 : ネコの手引っ込め
  20. };
  22. void setup()
  23. {
  24.   // シリアル通信の開始
  25.   Serial.begin(115200);
  26.   // サーボのアタッチ
  27.   myservoU.attach(3);
  28.   myservoM.attach(6);
  29.   myservoD.attach(5);
  30.   Serial.println("Start!!");
  32.   delayAndMoveServoUMD(4000, pose_setUMD[0]); // ホームポジションで4秒待機
  33. }
  35. void loop()
  36. {
  37.   delayAndMoveServoUMD(3000, pose_setUMD[1]); // ネコパンチ構え
  38.   delay(200);
  39.   delayAndMoveServoUMD(1000, pose_setUMD[2]); // ネコパンチ振り上げ
  40.   delayAndMoveServoUMD(1000, pose_setUMD[3]); // ネコパンチ振りおろし
  41.   delay(200);
  42.   delayAndMoveServoUMD(2000, pose_setUMD[7]); // お客さんが手を引っ込めるように手をたたむ
  43.   delayAndMoveServoUMD(2000, pose_setUMD[0]); // ホームポジションへ
  44.   delay(4000);
  45. }
  47. void delayAndMoveServoUMD (int delay_time, float targetUMD[3]) {
  48.   int delay_in_roop = 5;
  49.   float level = 0; // levelはroopないで0からPIになる
  50.   const float d_level = PI / (delay_time / delay_in_roop);
  51.   float servo_calc[3];
  53.   for (int i = 0; i < delay_time; i += delay_in_roop){
  54.     for (int i = 0; i < 3; i++) {
  55.       servo_calc[i] = servoUMD_now[i] * (1 - (-cos(level) + 1) / 2) + targetUMD[i] * ((-cos(level) + 1) / 2);
  56.     }
  57.     myservoU.writeMicroseconds((int)servo_calc[0]);
  58.     myservoM.writeMicroseconds((int)servo_calc[1]);
  59.     myservoD.writeMicroseconds((int)servo_calc[2]);
  60.     Serial.printf("servo U : %f,M : %f, D : %f\n", servo_calc[0], servo_calc[1], servo_calc[2]);
  61.     // Serial.printf("A : %f, B : %f, A + B = %f\n", (1 - (-cos(level) + 1) / 2), (-cos(level) + 1) / 2, (1 - (-cos(level) + 1) / 2) + (-cos(level) + 1) / 2);
  62.     level += d_level;
  63.     delay(delay_in_roop);
  64.   }
  65.   for (int i = 0; i < 3; i++){
  66.     servoUMD_now[i] = targetUMD[i];
  67.   }
  68. }

このコードでは現在の角度をfloat型の配列 servoUMD_now[] に格納し、用意しておいたポーズセット pose_setUMD[][] から呼び出したポーズに徐々に切り替えるようにして複数のサーボモーターを同時に制御しています。また、ポーズの切り替え、待機、シリアル信号での監視はdelayAndMoveServoUMD()関数で行っています。

ポーズの切り替えの際は、現在のポーズ servoUMD_now[] に 1 - (cosθ + 1) / 2 を、次に取らせたいポーズに (cosθ + 1) / 2 を掛け、足し合わせることで滑らかにポーズを切り替えています。

実際の画像と合わせるとこのようになります。また、今回はコサインを計算に使用しましたが、他の関数でもポーズの切り替えに使用できます。作りたいもののモチーフに合うように作成すると雰囲気が出るのでお勧めです。

コードのゆる解説

ここからはゆるくコード全体の解説をしていきます。実装する際どこをどう書き換えれば良いのかわからない場合や実装につまづいた際にご活用ください。

電圧の高いサーボモーターや壊れやすい部品を扱う場合は、注意してサーボモーターを動かす必要があります。初めはサーボモーターを1つずつ動かす、シリアルモニターで角度を確認しながら調節するなど、慎重に動かすことを強くお勧めします。

Servo.hライブラリを読み込みます。Servo.hライブラリはArduino IDEに標準でインストールされているため別途インストールする必要はありません

  1. #include <Servo.h>

　

サーボモーターのオブジェクトを宣言します。今回のこのコードでは先端, 中間, 根本の3つのサーボがあるため、それぞれmyservoU, myservoM, myservoDという名前で宣言しています。
宣言するオブジェクトの数、オブジェクト名は適宜変更してください。

  1. Servo myservoU; // 6.先create servo object to control a servo
  2. Servo myservoM; // 5.中create servo object to control a servo
  3. Servo myservoD; // 3.根本create servo object to control a servo

　

ここでは、サーボモーターを制御するための変数を宣言します。まずはdefineでそれぞれのモーターが90度になる信号を定義し、それをもとにサーボモーターの初期位置、各種ポーズを宣言します。今回はservo.writeMicroseconds()関数を使用して制御するため1500のとき90度になるようにハード側、ソフト側で調節する必要があります。下に調節の流れ、ポーズ作成の流れを図解するのでそちらもご活用ください。

  1. // サーボ制御用の変数
  2. #define sv_U_default 1500 // 1000〜2000で1500が中央
  3. #define sv_M_default 1500 - 80
  4. #define sv_D_default 1500 - 20
  5. float servoUMD_now[3] = {{sv_U_default}, {sv_M_default}, {sv_D_default + 500}};
  6. float pose_setUMD[8][3] = {
  7. {sv_U_default, sv_M_default, sv_D_default + 500}, // pose 0 : ホームポジション
  8. {sv_U_default + 400, sv_M_default - 650, sv_D_default + 250}, // pose 1 : ネコパンチ構え
  9. {sv_U_default - 200, sv_M_default - 400, sv_D_default}, // pose 2 : ネコパンチ振りかざし
  10. {sv_U_default + 200, sv_M_default + 100, sv_D_default + 30}, // pose 3 : ネコパンチ振り下ろし
  11. {sv_U_default + 200, sv_M_default - 200, sv_D_default}, // pose 4 : ネコお手振りかざし
  12. {sv_U_default - 200, sv_M_default + 200, sv_D_default}, // pose 5 : ネコお手振り下ろし
  13. {sv_U_default, sv_M_default + 200, sv_D_default}, // pose 6 : ネコお手振り下ろし2
  14. {sv_U_default, sv_M_default - 650, sv_D_default + 500} // pose 7 : ネコの手引っ込め
  15. };

ここから、UMDと書かれた変数名と関数名がたびたび登場しますがservoU, servoM, servoDをに関わる変数や関数という意味なので関数名や変数名はそれぞれわかりやすいように書き換えてください。
servoUMD_now[]はfloat型で長さが3の配列ですが使うサーボの数が2つならservoUM_now[2]、4つならservoUMDR_now[4]のように配列の長さを変更する必要があります。pose_setUMD[][]はfloat型の2次元配列ですが、こちらもpose_setUMD[用意したポーズの数][サーボモーターの数]となるように変更する必要があります。

servo.writeMicroseconds()関数について
サーボモーターはPWM信号と呼ばれるパルス信号によって命令をできるのですが、servo.writeMicroseconds()関数では直接そのパルス幅を指定します。だいたいのモーターが1500のときが中間、1000のとき手前90度に、2000のとき奥側90度になるそうですが、モーターによって個体差があるそうです。今回使用したモーターの場合は＋500, ー520で＋90度, ー90度になりました。

　

setup()関数内ではシリアル通信の開始、サーボオブジェクトへデバイスのピンの割り当てなどを行っています。サーボオブジェクトの数やオブジェクト名が異なる場合は書き換えが必要です。SerialはdelayAndMoveServoUMD()関数内で角度の計算が正常に行われているか確認するために使用します。モーションが完成したあとはここのSerial.begin()とdelayAndMoveServoUMD()関数内のSerial.printf()関数は消しても構いません。

  1. void setup()
  2. {
  3.   // シリアル通信の開始
  4.   Serial.begin(115200);
  5.   // サーボのアタッチ
  6.   myservoU.attach(3);
  7.   myservoM.attach(6);
  8.   myservoD.attach(5);
  9.   Serial.println("Start!!");
  11.   delayAndMoveServoUMD(4000, pose_setUMD[0]); // ホームポジションで4秒待機
  12. }

　

loop()関数内ではdelayAndMoveServoUMD()関数を複数回呼び出し、次々にポーズを切り替えています。このコードのようにポーズの内容や意図を細かに書いておくとモーションを作り変えたり、修正する際に役立つのでお勧めです。

  1. void loop()
  2. {
  3.   delayAndMoveServoUMD(3000, pose_setUMD[1]); // ネコパンチ構え
  4.   delay(200);
  5.   delayAndMoveServoUMD(1000, pose_setUMD[2]); // ネコパンチ振り上げ
  6.   delayAndMoveServoUMD(1000, pose_setUMD[3]); // ネコパンチ振りおろし
  7.   delay(200);
  8.   delayAndMoveServoUMD(2000, pose_setUMD[7]); // お客さんが手を引っ込めるように手をたたむ
  9.   delayAndMoveServoUMD(2000, pose_setUMD[0]); // ホームポジションへ
  10.   delay(4000);
  11. }

　

delayAndMoveServoUMD()関数は第1引数にint型delay_timeとしてこの関数を実行する時間、第2引数にfloat型配列targetUMD[3]として各モーターをどの角度に動かすかを指定できます。書き換える必要のある箇所が多いので赤字で示しています。どうか書き換えれば良いか下に箇条書きで示しておきます。

  1. void delayAndMoveServoUMD (int delay_time, float targetUMD[3]) {
  2.   int delay_in_roop = 5;
  3.   float level = 0; // levelはroopないで0からPIになる
  4.   const float d_level = PI / (delay_time / delay_in_roop);
  5.   float servo_calc[3];
  7.   for (int i = 0; i < delay_time; i += delay_in_roop){
  8.     for (int i = 0; i < 3; i++) {
  9.       servo_calc[i] = servoUMD_now[i] * (1 - (-cos(level) + 1) / 2) + targetUMD[i] * ((-cos(level) + 1) / 2);
  10.     }
  11.     myservoU.writeMicroseconds((int)servo_calc[0]);
  12.     myservoM.writeMicroseconds((int)servo_calc[1]);
  13.     myservoD.writeMicroseconds((int)servo_calc[2]);
  14.     Serial.printf("servo U : %f,M : %f, D : %f\n", servo_calc[0], servo_calc[1], servo_calc[2]);
  15.     // Serial.printf("A : %f, B : %f, A + B = %f\n", (1 - (-cos(level) + 1) / 2), (-cos(level) + 1) / 2, (1 - (-cos(level) + 1) / 2) + (-cos(level) + 1) / 2);
  16.     level += d_level;
  17.     delay(delay_in_roop);
  18.   }
  19.   for (int i = 0; i < 3; i++){
  20.     servoUMD_now[i] = targetUMD[i];
  21.   }
  22. }
  • 47行目, 51行目, 54行目, 65行目の赤字で示した3は使用するモーターの数に書き換えてください。
  • 57 ~ 59行目では計算した結果をもとにサーボモーターオブジェクトを動かすように指示を出していますがこれもオブジェクト名や数が変わっている場合は書き換える必要があります。
  • 60行目ではシリアル通信に現在の角度をモニタリングできるように書き直す必要があります。このforループ内での現在の角度はservoUMD_nowではないので気をつけてください。

コードのゆる解説は以上になります。今回の記事が、サーボモーターを使ったロボット作りに役立てば幸いです。

まとめ

今回は複数のサーボモーターを滑らかに制御する方法、モーションの作り方を説明しました。for文などで直接サーボモーターを制御するよりも、ポーズと時間を指定するだけでモーションを作れるため、自然なモーションをたくさん作れるようになります。

謝辞

このコードはTechSeeker Hackathon 2024の成果物の一部です。このような学習の機会を用意してくださった方々、同じチームとして支えてくださったデジもく会のみなさんに感謝します。本当にありがとうございました。

-プログラミング
-, ,