近藤科学シリアルコマンド(ICS)サーボでｽﾀｯｸﾁｬﾝを作った話

## はじめに

ｽﾀｯｸﾁｬﾝとは、M5Stackシリーズを使ってつくられたｽｰﾊﾟｰｶﾜｲｲﾛﾎﾞｯﾄです

https://protopedia.net/prototype/2345

詳しくは（改めて述べるほどでもないので）スタックチャンアドカレ先行組の[こちら](https://qiita.com/B-SKY-Lab/items/0232e49a9acef908d019
)や[こちら](https://mzmlab.hatenablog.com/entry/stackchan23)の記事などにお譲りします。 ~~（後出しで記事を書くと楽できて助かりますね）~~


ｽﾀｯｸﾁｬﾝのアクチュエーターには小型で安価な SG90 PWMサーボモーターが採用されることが多いようですが、この度は我が家の積みサーボと化していた、近藤科学のシリアルコマンド(ICS)サーボ、KRS-3204を使用してｽﾀｯｸﾁｬﾝを作ってみました。

<img width="400" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3322077/ec62aef1-4ee6-a909-37e7-e6818692d05e.jpeg">

KRS-3204をいくつか頂いたものの持て余していたのですが…

<img width="400" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3322077/e7d121f0-16b1-9886-6fb3-7156c0601adc.jpeg">
ｽﾀｯｸﾁｬﾝになりました！


<img width="400" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3322077/b8a08646-37a8-56a7-5aad-ee25da0cb877.jpeg"><img width="400" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3322077/3f1e3e90-7c13-54c5-cb73-fddea4f2dd5b.jpeg">
（ちょっとブラケット・サーボのはみだしが大きいか）




## 前置き
#### シリアルコマンドサーボとPWMサーボの違いとは？
PWMサーボは各サーボをそれぞれマイコンの出力に繋ぎ、PWM信号を送ることで制御しますが、シリアルコマンドサーボは1系統のシリアル通信線に数珠つなぎした複数のサーボを制御できます。

https://kondo-robot.com/faq/servomotor-use-method-machine-daisy-chain

また稼働中にサーボの動作特性を変更するなど、高度な制御が可能である場合も多いです。

他社サーボ、Feetechについて述べた記事ですが、アドカレ先行組の[こちら](https://raspberrypi.mongonta.com/stack-chan-scs0009-arduino/
)の内容とも共通する点が多数あります。 ~~（後出しで記事書くと楽でry）~~

ICS対応サーボ、KRSシリーズはFeetechサーボと比べると、ソフトウェアやオプションパーツの提供、製造販売元の近藤科学のサポートや、日本における使用者数・情報量が多いといったところが良い点であると思います。
が、難点もいろいろあり、結論を先に書くと、ｽﾀｯｸﾁｬﾝのような用途にはあまりお勧めできないと思っています。理由は後述。



## ハードウェア

#### ｽﾀｯｸﾁｬﾝの本体
M5Stack Core2

https://docs.m5stack.com/en/core/core2


#### アクチュエーター
KRS-3204 ICS

https://kondo-robot.com/product/krs-3204ics

ロボット向けに設計され、小型ながら余裕のトルクと動作角度、全段金属ギア採用、Arduinoライブラリの提供あり、と一見いいことづくめなサーボ。


KRSサーボも数珠つなぎにして使用するときはIDの付直しが必要になります。[USBアダプター](https://kondo-robot.com/product/02116
)と[ICS3.5/3.6 Manager](https://kondo-robot.com/faq/ics35mag
)を用意して設定を書き換えます。

#### フレーム・外装
ブラケットは、近藤科学が公開しているKRS-3204用ジョイントパーツサンプルデータを多少編集して使用しました。

https://kondo-robot.com/faq/krs-3204joint-parts

<img width="400" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3322077/09c83aa5-c6eb-f6a1-8697-493dd796d540.jpeg">

* サイド・ボトムブラケットを組み合わせ、ピッチ軸サーボはスナップフィット、ヨー軸サーボもブラケットにはねじ1本で止められることを狙った
（できなかったので後加工に苦労した）

サンプルデータのサーボアームを利用して外装と接続する仕様に変更しています


<img width="400" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3322077/50175c67-823e-cd24-3881-49a3fb60ac20.jpeg">

* サイズ調整、ナット埋め込み穴開けなど

外装と足は本家のFutaba RS30x版caseをこちらも多少編集して使用しました

https://github.com/meganetaaan/stack-chan/tree/dev/v1.0/case/case_RS30X


<img width="400" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3322077/b6468e22-fc0a-fa22-3a2b-3b6f8a6f8708.jpeg">

* 外装のサーボホーン接続部撤去、サーボアーム接続ねじ穴追加、3DP出力時のオーバーハング軽減など


<img width="400" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3322077/70b3384e-5396-6fec-4d20-171f339e76cf.jpeg">

* 足のサイズ調整など


<br>

<img width="400" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3322077/6fd5cce7-f17a-f18b-d618-53f9fd25fe41.jpeg">

結果、このような感じの中身になりました


#### 回路・電源

ICSデバイスとマイコンのUARTを接続するためのオプションパーツ、ICS変換基盤を用意しプロトモジュールに取り付けました。

<img width="400" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3322077/a75ab76c-c96f-d7b5-3641-987ef1d28371.jpeg">


回路図は公開されており、[同等の回路基板の自作](http://robooptions.blog.fc2.com/blog-entry-10.html)も困難ではないようですが、近藤科学公式オプションパーツを使うと確実です。


https://kondo-robot.com/product/03121


下表のように端子を接続しました。

<img width="400" src="https://kondo-robot.com/w/wp-content/uploads/ICS_adapter20170418.jpg">


|M5 Core2 | ICS変換基盤 |
|:-:|:-:|
| RXD2  | Tx  |
| TXD2  | Rx  |
| GPIO(G19) | EN_IN  |
| 3.3V  | IOREF  |
| 5V  | POW  |
| GND  | GND  |

電源はICS変換基盤のPOW, GNDに外部から5[V]を供給。この端子は本来サーボの電源用ですがCore2の5V, GND にも直結させています。
ICS変換基盤とKRS-3204の電源電圧は6[V]以上を供給することとされていますが、私が試した限り5[V]でも動作しました。




## ソフトウェア
robo8080さんのM5Unified_StackChanをベースにしました。セリフを与えておくとランダムに独り言を言うｽﾀｯｸﾁｬﾝです。

https://github.com/robo8080/M5Unified_StackChan

流行りのAIｽﾀｯｸﾁｬﾝではありませんが、ArduinoIDEで開発できて比較的シンプル、改造のし甲斐がありそうですね。


[近藤科学ICSライブラリ](https://kondo-robot.com/faq/ics-library-a2
)を導入、ServoEasingライブラリの使用箇所をICSライブラリの関数に置き換えています

<details><summary>ソースコードの主な追加変更箇所（抜粋）</summary>

~~~cpp
#include <M5Unified.h>
#include "AudioOutputM5Speaker.h"
#include <AudioFileSourceSD.h>
#include <AudioGeneratorWAV.h>
#include <AudioFileSourceBuffer.h>
#include <Avatar.h> // https://github.com/meganetaaan/m5stack-avatar
//#include <ServoEasing.hpp> // https://github.com/ArminJo/ServoEasing     

#include <IcsHardSerialClass.h>//近藤科学ICSライブラリ

#define USE_SERVO
#ifdef USE_SERVO
/*#if defined(ARDUINO_M5STACK_Core2)
  // #define SERVO_PIN_X 13  //Core2 PORT C
  // #define SERVO_PIN_Y 14
 #define SERVO_PIN_X 33  //Core2 PORT A
 #define SERVO_PIN_Y 32
#elif defined( ARDUINO_M5STACK_FIRE )
  #define SERVO_PIN_X 21
  #define SERVO_PIN_Y 22
#elif defined( ARDUINO_M5Stack_Core_ESP32 )
  #define SERVO_PIN_X 21
  #define SERVO_PIN_Y 22
#endif*/
#endif

//ICSデバイスと通信する初期設定
const byte EN_PIN = 19;//Core2 GPIOピン
long BAUDRATE = 115200;
const int TIMEOUT = 100;
IcsHardSerialClass krs(&Serial2,EN_PIN,BAUDRATE,TIMEOUT); //インスタンス＋ENピンおよびUARTの指定
const byte IDMAX = 1; //接続されているICSデバイスIDの最大値、サーボには0からこの値までのidを順に振っておく

~~~

~~~cpp
void krsSetFreeAll(void)//全サーボにフリー(トルクOFF)指令
{
  for(byte id = 0 ; id <= IDMAX ; id++)
  {
    krs.setFree(id);
  }
}
void krsSetSpeedAll(unsigned int spd)//全サーボにspeed設定
{
  for(byte id = 0 ; id <= IDMAX ; id++)
  {
    krs.setSpd(id,spd);
  }
}

void Servo_setup() 
{
krs.begin();  //ICSデバイスとの通信開始

krsSetFreeAll();//電源投入時一旦全サーボのトルクoff
krsSetSpeedAll(8);//全IDのサーボの初期スピード

for(byte id = 0 ; id <= IDMAX ; id++)
{
  krs.setStrc(id,1); //全IDのサーボの初期ストレッチ
  krs.setPos(id,7500);//全IDのサーボに位置指令7500(中央)、トルクもONになる
}

delay(1000);  //全サーボが中央に動くので適当な時間待つ

for(byte id = 0 ; id <= IDMAX ; id++)
{
  krs.setStrc(id,8); //全IDのサーボの稼働時ストレッチ
}
krsSetSpeedAll(16);//全IDのサーボの稼働時スピード

/*#ifdef USE_SERVO
  if (servo_x.attach(SERVO_PIN_X, START_DEGREE_VALUE_X, DEFAULT_MICROSECONDS_FOR_0_DEGREE, DEFAULT_MICROSECONDS_FOR_180_DEGREE)) {
    Serial.print("Error attaching servo x");
  }
  if (servo_y.attach(SERVO_PIN_Y, START_DEGREE_VALUE_Y, DEFAULT_MICROSECONDS_FOR_0_DEGREE, DEFAULT_MICROSECONDS_FOR_180_DEGREE)) {
    Serial.print("Error attaching servo y");
  }
  servo_x.setEasingType(EASE_QUADRATIC_IN_OUT);
  servo_y.setEasingType(EASE_QUADRATIC_IN_OUT);
  setSpeedForAllServos(30);
#endif*/
}
~~~


~~~cpp
void servo(void *args)
{
  int pos;
  float gazeX, gazeY;
  DriveContext *ctx = (DriveContext *)args;
  Avatar *avatar = ctx->getAvatar();

  for (;;)
  {
  
      avatar->getGaze(&gazeY, &gazeX);
      //servo_x.setEaseTo(START_DEGREE_VALUE_X + (int)(20.0 * gazeX));
      pos = krs.degPos(START_DEGREE_VALUE_X + (int)(20.0 * gazeX));//avatarの動作から算出した角度[deg]？をポジションデータに変換
      krs.setPos (1,pos);//変換したデータを ID:1 のICSサーボに送ります

      if(gazeY < 0) 
      {
        int tmp = (int)(15.0 * gazeY);
        if(tmp > 15) tmp = 15;
        //servo_y.setEaseTo(START_DEGREE_VALUE_Y + tmp);
        pos = krs.degPos(START_DEGREE_VALUE_Y + tmp);
        krs.setPos (0,pos);//変換したデータを ID:0 のICSサーボに送ります

      } 
      else 
      {
        //servo_y.setEaseTo(START_DEGREE_VALUE_Y + (int)(10.0 * gazeY));
        pos = krs.degPos(START_DEGREE_VALUE_Y + (int)(10.0 * gazeY));
        krs.setPos (0,pos);
      }
      //synchronizeAllServosStartAndWaitForAllServosToStop();
      delay(3210);

  }
}
~~~

</details>

## 結論

KRS-3204はあまりお勧めできないと書きましたがその理由は...
<br>

* #### 高い

https://kondo-robot.com/robospot/html/products/detail.php?product_id=1274

定価1万円弱、記事執筆時点ではすでに型落ちとなったためか値引きされていましたが7千円弱、某廉価版でも4千円します。
<br>

* #### 動作音が大きい
トルク発生中(キィインという感じの)甲高い動作音が鳴り続けます。個体差やパラメータによる差異はあるかもしれません。
人によってはむしろかっこいいと思うかも？感じ方の個人差もあるかもしれない点ではあります。

私はちょっとうるさいと感じたので、ｽﾀｯｸﾁｬﾝが喋り出すタイミングで一時的にトルクオフし音を消すようにしました。
~~~cpp
void play(const char* fname)
{
  Serial.printf("play file fname = %s\r\n", fname);
  if (file != nullptr) { stop(); }
  file = new AudioFileSourceSD(fname);
  buff = new AudioFileSourceBuffer(file, preallocateBuffer, preallocateBufferSize);
//  wav.begin(file, &out);

  wav.begin(buff, &out);
    
    krsSetFreeAll();//しゃべるとき全サーボをフリーにする
    delay(1000);//適当な時間待つ
  
  while (wav.isRunning())
  {//以下略
~~~

<br>

* #### ICSコマンドには「ゆっくり動かす」機能がない
例えばFutabaのコマンドサーボのポジション指定は「角度xへ、t[ms]の時間をかけて回転」させる指示ができますが、ICSコマンドはPWMサーボと同様、サーボは指定された角度へ全力で回転します。

スムーズな回転を実現するにはServoEasingライブラリのような動作補完する機能を用意する、のが正道だと思いますが、今回はサーボのspeedパラメータ[^1]を下げて対応しました。

stretch（角度の保持力）もかなり低い値とし、動き出し・動き終わりが緩やかになることを狙っています。

~~~cpp
void krsSetSpeedAll(unsigned int spd)//全サーボにspeed設定
{
  for(byte id = 0 ; id <= IDMAX ; id++)
  {
    krs.setSpd(id,spd);
  }
}

void Servo_setup() 
{
krs.begin();  //ICSデバイスとの通信開始

krsSetFreeAll();//電源投入時一旦全サーボのトルクoff
krsSetSpeedAll(8);//全IDのサーボの初期スピード

for(byte id = 0 ; id <= IDMAX ; id++)
{
  krs.setStrc(id,1); //全IDのサーボの初期ストレッチ
  krs.setPos(id,7500);//全IDのサーボに位置指令7500(中央)、トルクもONになる
}

delay(1000);  //全サーボが中央に動くので適当な時間待つ

for(byte id = 0 ; id <= IDMAX ; id++)
{
  krs.setStrc(id,8); //全IDのサーボの稼働時ストレッチ
}
krsSetSpeedAll(16);//全IDのサーボの稼働時スピード
~~~

先ほども掲載したコードからさらに抜粋。サーボのspeedは8~16まで落とされています(MAX：127)
これはちょっと下げすぎかもしれません。

またpunch・response・damping [^2]もあらかじめ最低値に設定しました。効果のほどは比較検証していないので不明です
<br>
だったらPWMで動かせばいい？ KRS-3204はそうすることも可能ですが…
<br>

* #### PWMで動かすならFutaba！
（このセクションの内容は筆者は実践してはいない点ご了承ください）

Futaba TTLコマンドサーボはPWMで制御する際、電源投入後最初にサーボが指定角度へ移動するまでの時間「Warm-up time (準備時間)」がデフォで設定されており、電源投入直後の急激な動作を抑制できるようです。おまけにKRS-3204よりも安価です。サーボにお金をかけるならこちらを採用する方が良いでしょう。

そもそも[RS303](https://www.futaba.co.jp/product/industrial_servo/command_type_servos/rs303mr
)/[RS304](https://www.futaba.co.jp/product/industrial_servo/command_type_servos/rs304md
)を採用すれば RS30x版caseを無改造で利用でき、5[V]の入力にも公式に対応しています。

また超小型で3.7[V]という低電圧にも公式対応する[RS204](https://www.futaba.co.jp/product/industrial_servo/command_type_servos/rs204md
)は、M5Stackシリーズと組み合わせたプロダクトに相性がよさそうに思えます。



## おわりに
あまり使われる例を見かけないサーボで、AI化の流行りにも乗ってないｽﾀｯｸﾁｬﾝを作ってみた顛末を、この機に乗じてまとめてみました。
マネするのはあまりお勧めはしませんが何かの参考になれば幸いです。



### 付記
* M5Unified_StackChanの起動時の処理
音量アップ(robo8080さんからアドバイスいただきました！) ＋ 起動時リセットがかかってしまう場合の改善策？＋ 起動時に特定のセリフをしゃべらせる

~~~cpp
void setup() {

  /*
      中略
  */

  M5.begin(cfg);

  M5.Lcd.clear();
  M5.Lcd.setCursor(0,0);
  M5.Lcd.setTextSize(2);
  // sets the output master volume of the sound @param (0~255)
  M5.Speaker.setVolume(250);//この一文を追加して音量アップ
  // The setChannelVolume function can be set the specified virtual channel volume in the range of 0-255. (default : 255)
  M5.Speaker.setChannelVolume(m5spk_virtual_channel, 255);

  delay(200);//ここで適当な時間待つと電源投入時リセットがかかりにくい？
  Servo_setup();
  delay(100);
  file_read();
  delay(100);

  avatar.init();
  avatar.addTask(lipSync, "lipSync");
  avatar.addTask(servo, "servo");

  //起動時に特定のボイス再生
  String startupvoice= "/startupvoice/startupvoice.wav";//起動時再生するボイスをこのフォルダとファイル名で用意しておく
  krsSetFreeAll();
  delay(123);//
  play(startupvoice.c_str());//
  delay(7654);//ここに限らずdelayの入れ方、変更はてきとー
  
  xTaskCreateUniversal(speachTask, "speachTask", 4096, nullptr, 2, nullptr, APP_CPU_NUM);
}

~~~




[^1]:KRSサーボのspeedパラメータとは、実際のところモーターの最大出力トルクを指しているらしい（よって速度制御をしているわけではない？）

[^2]:各パラメータの詳細な解説はICS Managerのマニュアルや[ICSコマンドリファレンス](https://kondo-robot.com/faq/ics3-5_3-6softwaremanual_2
)を参照願います