ICSサーボの断線チェッカー

これがなくて大会直前に何度も泣いた

やること

数珠繋ぎにした近藤科学製のサーボモーター（ICS通信）の断線箇所をチェックするための装置を作ります。

準備

ハードウェアの準備もあるので少し手間がかかります。
・断線箇所を調べたい数珠繋ぎのKRSサーボ
・Arduino系の基板（例ではTeensy4.0で1.25M接続）
・ICS変換基板もしくは半二重回路(例では2個)
・ICS Library for Arduino ver.2 ライブラリの導入

配線例(Teensy4.0)

Teensy4.0	デバイス
GND	GND
Vin	5V入力
3.3v	IOREF
05	ICS変換基盤2_EN
06	ICS変換基盤1_EN
07 RX2	ICS変換基盤1_TX
08 TX2	ICS変換基盤1_RX
15 RX3	ICS変換基盤2_TX
14 TX3	ICS変換基盤2_RX

Arduino系(Teensy4.0)用スケッチ

Arduino

# include <IcsHardSerialClass.h> //ICSサーボのライブラリ導入

//ICSサーボ設定用変数
const byte EN_L_PIN = 6;
const byte EN_R_PIN = 5;
const long BAUDRATE = 1250000;
const int TIMEOUT = 2; //返信待ちのタイムアウト時間。例では1だと失敗。
IcsHardSerialClass krs_L(&Serial2, EN_L_PIN, BAUDRATE, TIMEOUT); //インスタンス＋ENピン(23番ピン)およびUARTの指定
IcsHardSerialClass krs_R(&Serial3, EN_R_PIN, BAUDRATE, TIMEOUT); //インスタンス＋ENピン(2番ピン)およびUARTの指定

char krs_L_array[] = {0,1,2,3,4,6,7,8,9,10};//左系統に接続しているサーボのID番号を必要な個数だけ入れる
char krs_R_array[] = {1,2,3,4,6,7,8,9,10};//右系統に接続しているサーボのID番号を必要な個数だけ入れる

//各サーボの計算用変数
int k;
int once = 100;//初めて無返信を検知したサーボIDをキープする変数
String lr = "x";//上記IDが右系統か左系統かをキープする変数

void setup() {
  //PCでのシリアルモニタ表示
  Serial.begin(12500000);//シリアルモニター表示
  krs_L.begin(); //サーボモータの通信初期設定。Serial2
  krs_R.begin(); //サーボモータの通信初期設定。Serial3
}

void loop() {

  Serial.print("LEFT :");
  for (int i = 0; i < sizeof(krs_L_array); i ++) {
    k = krs_L.setFree(krs_L_array[i]);//1以外ならとりあえずサーボを脱力し位置を取得
    delayMicroseconds(2);
    Serial.print(k);
    Serial.print(",");
    if ((k == -1) and (once == 100))//初めて無返信を検知したかどうか
    {
      Serial.println();
      Serial.print("*FOUND DISCONNECT : LEFT - ");//エラーサーボの表示
      Serial.println(int(krs_L_array[i]));
      lr = "LEFT -";
      once = int(krs_L_array[i]);
      delay(1000);
    }
  }
  Serial.println();

  Serial.print("RIGHT:");
  for (int i = 0; i < sizeof(krs_R_array); i ++) {
    k = krs_R.setFree(krs_R_array[i]);//1以外ならとりあえずサーボを脱力し位置を取得
    delayMicroseconds(2);//※ICSの高速通信用に必須
    Serial.print(k);
    Serial.print(",");
    if ((k == -1) and (once == 100))
    {
      Serial.println();
      Serial.print("*FOUND DISCONNECT : RIGHT -");
      Serial.println(int(krs_R_array[i]));
      lr = "RIGHT -";
      once = int(krs_R_array[i]);
      delay(1000);
    }
  }
  Serial.println();

  if (once != 100) {//一度キープした返信のないサーボIDを表示し続ける
    Serial.print("** CHECK ");
    Serial.print(lr);
    Serial.print(once);
    Serial.println();
  }
  delayMicroseconds(2);
}

使い方

接続と設定
正しく配線を行い起動し、Arduino系の基板とPCとUSBで接続します。
krs_L_array[]とkrs_R_array[]の配列に、使っているサーボの番号を記入しておきます。
Arduino系の基板にスクリプトを書き込みます。

ポイント

Teensy4.0 - 1.25Mbpsのような高速シリアル通信の場合、公式ライブラリのkrs.コマンドそのままでは送受信の信号同士が重なってしまう不具合があるようです。
delayMicroseconds(2)をコマンド間に挟むとうまく動作します。

実行

ArduinoIDEのシリアルモニタを表示させると、サーボからの返信されたポジション情報の数値がズラズラと表示されます。
この時、すべてのサーボはフリー状態（脱力状態）となっています。
（※実行時にサーボへの電源供給をお忘れなく。）

（図の例は実行中で4番のコネクタを抜いた場合です。）

• 断線が検知可能な時間(2/1000秒)以上発生していれば、その瞬間に信号がこなかったサーボの番号を表示します。
• 仮に次の瞬間に接続が物理的に復活しても、一度信号がこなかったサーボ番号を記憶し、画面に表示し続けます。解除するにはArduino系基板をリセットします。
• 静止状態で断線が確認できない場合は、フリー状態のサーボを手でいろんな角度に動かし、配線の角度をさまざまに変更してみることで、断線を再現できるかもしれません。
• 不具合が見つかった場合には、該当のサーボにつながる配線やサーボ自体を調べます。
• この装置で検知できる不具合は最初に検知できた１箇所のみです。
• 該当以外にも不具合がないか調べる場合には、該当の不具合を修理後、改めてチェックするか、接続と設定を変更してテストを実施してください。
• 断線以外の場合でも、末端のコネクタに反応があった場合には、エラーを検出することがあります。（検出中にサーボを追加接続するなど）

おわりに

ヒューマノイドロボットは配線が多くなるので断線やサーボの不具合に悩まされますよね。
特にICSは双方向通信としてシリアル接続した場合、返信がないサーボがあるとそこで全体の通信が止まってしまい困ったことになりがちです。（通信のタイムアウトを無くしたり全部受信専用にするなどの回避方法はありますが）

Teensyなどを使ってサーボロボットの制御システムを作る時、このプログラムを一緒にいれておくと少し幸せになれるかもしれません。ダメージのあるサーボ箇所をプラレス３四郎のようにPC画面でパッと表示してくれるようにもできたりします。