esp32でKONDOサーボを動かす

 

ROBO-ONEに出場するにはKONDOサーボを動かす必要があります。

esp32はSerial通信ポートを3つ持っています。UART0,UART1,UART2です。PCとマシンを接続して通信しながらKONDOサーボを動かしたい時があるかもしれないので、UART0は使わないようにします。

私の場合、11.1V系サーボ(KRS2552)と7.4Vサーボ(KRS3204)の2種類を使うので電圧が違うサーボごとにSerialを使い分けるようにしました。KONDOの公式は1つのUARTに接続できるサーボの推奨個数は9個と言っています。3kg級のマシンを作る時はサーボの数が19個を余裕で超えるため、UART0も使う必要がありそうです。四肢でSerialを分けた方が配線的には楽なので本当は4つ欲しいところ。

https://kondo-robot.com/faq/ics_board_-tutorial1

シリアルピン保護のため、ポリスイッチ↓をそれぞれ付けました。冷却されると導電性が戻って再び使用可能になります。

http://akizukidenshi.com/catalog/g/gP-11307/

11.1V系 → UART1

7.4V系  → UART2

f:id:sasapero:20200814125056p:plainf:id:sasapero:20200816021941p:plain

KONDOサーボを動かす通信規格をICS(Interactive Communication System)と言い、近藤科学独自のデータ通信規格です。PWMと違って、サーボを数珠つなぎ出来るのが特徴です。KONDO公式のライブラリはTXとRXを交互に切り替えることによってサーボに指令角度を送信するのと同時にサーボの温度や電流を受信できます。

そのTXとRXを切り替えるためのモジュール(ICS変換基板)が市販されている↓のですが、場所をとるので基板に実装しました。

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

ICS変換基板の自作についてはPONDAさんの記事↓を参考にしました。そちらの記事に記載されている回路図とほぼ同じですが、私が引いた回路図も載せておきます。使用している部品も購入先もPONDAさんと同じです。

 http://robooptions.blog.fc2.com/blog-entry-10.html

f:id:sasapero:20200816032511p:plainf:id:sasapero:20200816033529j:plain

 -------------------------------------------------------------------------

esp32のUART1は初期状態では内部フラッシュに接続されているため、UARTとしての使用はできません。UART1のピンアサインを変更することで使用可能になります。HardwareSerial.cppにあるTX1とRX1を定義しなおせば良いです。

Visual studio codeでプログラミングをする場合、以下のパスにHardwareSerial.cppがあります。

C:\Users\ユーザー名\.platformio\packages\framework-arduinoespressif32\cores\esp32\HardwareSerial.cpp

#ifndef RX1
#define RX1 14 //9
#endif

#ifndef TX1
#define TX1 12 //10
#endif

 -------------------------------------------------------------------------

KONDOサーボは通信速度が3種類から選べます(①)。esp32に書き込むプログラムで設定したBAUDRATEと合っていないとサーボは動きません。

私はプリメイドAIから拝借したサーボを使用したのですが、箱出しの状態では通信速度が115200と1250000が混合?しているのでICSマネージャーで設定し直してください。また、ストレッチやスピード、パンチ等も初期設定とは異なる状態のものが多いので、ICS Serial Managerの「②ファイル→開く→初期値(initial value)」から一括で書き込むと便利です。

//ICS関係
const long BAUDRATE = 115200; //115200
const int TIMEOUT = 10;     //通信できてないか確認用にわざと遅めに設定

f:id:sasapero:20200816035649p:plain

 

 -------------------------------------------------------------------------

プログラム

KONDOのライブラリは初期状態ではUARTが1つしか動かせないので、以下のように2つで動かせるようにします。BAUDRATEとTIMEOUTの設定は1つで良いです。ライブラリの説明書19Pにも同様の記述があります。

//ICS関係
const long BAUDRATE = 115200;
const int TIMEOUT = 10; //通信できてないか確認用にわざと遅めに設定

const byte EN_PIN1 = 15;
IcsHardSerialClass krs1(&Serial1EN_PIN1BAUDRATETIMEOUT); 

const byte EN_PIN2 = 13;
IcsHardSerialClass krs2(&Serial2EN_PIN2BAUDRATETIMEOUT); 

  -------------------------------------------------------------------------

HeartToHeart4やRobovie-MakerなどのGUIなソフトウェアは使わず、サーボIDや指令角度を手打ちしてモーション作成を行います。以下↓は「ID1のサーボを0度まで動かす」プログラムです。

krs1.setPos(1,3500); //サーボの角度を3500へ 

IDは0~31、指令角度は3500~11500が有効で、ありえないIDや角度が入力してしまい実行されると、予期しない動きをする可能性があります。

サーボを壊したくないので、範囲外の数値が入力され、実行されるとサーボが脱力するようにライブラリを変更しました。更に、この関数は指令角度を3500~11500で指定する必要があり、直観的に何度なのか分かりづらいため、degreeで指定できるように変更しました。

「IcsBaseClass.h」を以下のように変更し、

//    int setPos(byte id,unsigned int pos);    //目標値設定
      int setPos(byte id,int deg);    //目標値設定 0801 pos → deg にするために型をintに変更

 

「IcsBaseClass.cpp」にある「setPos(byte id, int deg)」を以下のように変更します。 

int IcsBaseClass::setPos(byte idint deg) //0803 pos → degに変換
{
  byte txCmd[3];
  byte rxCmd[3];
  unsigned int rePos;
  bool flg;

  //0803 deg→posに変換するために追加
  long pos = deg * 29.633;
  pos = pos + 7500;

  txCmd[0= 0x80 + id;             // CMD
  txCmd[1= ((pos >> 7& 0x007F); // POS_H
  txCmd[2= (pos & 0x007F);        // POS_L

  if (deg > MAX_DEG || deg < MIN_DEG)
  {
    txCmd[0= 0x80 + id; // CMD
    txCmd[1= 0;
    txCmd[2= 0;
    //    return ICS_FALSE;
  }

  //送受信

  flg = synchronize(txCmd, sizeof txCmdrxCmd, sizeof rxCmd);
  if (flg == false)
  {
    return ICS_FALSE;
  }

  rePos = ((rxCmd[1<< 7& 0x3F80+ (rxCmd[2& 0x007F); //フリー

  return rePos;
}

※ posからdegに変換する関数がデフォルトで入っているので、その関数をsetPos()内で呼び出すという方法でも同じことは出来ます。

 

また、いちいちサーボのIDを調べながらモーション作成するのは時間がかかるため、IDとマシンの部位を関連付けました。

  name_set(L_arm_1, -45);  name_set(R_arm_1, 45);
  name_set(L_arm_2, -130); name_set(R_arm_2, -130);

部位の名前はご自身で分かりやすく命名してください。

//      name   ID

#define BODY_1 1
#define BODY_2 2
#define L_leg_1 3
#define L_leg_2 4
#define L_leg_3 5
#define L_leg_4 6
#define R_leg_1 7
#define R_leg_2 8
#define R_leg_3 9
#define R_leg_4 10
#define R_arm_1 11
#define R_arm_2 12
#define L_arm_1 13
#define L_arm_2 14

int name_set(int nameint deg_2)
{

  if (name == BODY_1)
  {
    krs1.setPos(BODY_1deg_2);
  }
  else if (name == BODY_2)
  {
    krs2.setPos(BODY_2deg_2);
  }
  else if (name == L_leg_1)
  {
    krs1.setPos(L_leg_1deg_2);
  }
  else if (name == L_leg_2)
  {
    krs1.setPos(L_leg_2deg_2);
  }
  else if (name == L_leg_3)
  {
    krs1.setPos(L_leg_3deg_2);
  }
  else if (name == L_leg_4)
  {
    krs2.setPos(L_leg_4deg_2);
  }
  else if (name == R_leg_1)
  {
    krs1.setPos(R_leg_1deg_2);
  }
  else if (name == R_leg_2)
  {
    krs1.setPos(R_leg_2deg_2);
  }
  else if (name == R_leg_3)
  {
    krs1.setPos(R_leg_3deg_2);
  }
  else if (name == R_leg_4)
  {
    krs2.setPos(R_leg_4deg_2);
  }
  else if (name == R_arm_1)
  {
    krs2.setPos(R_arm_1deg_2);
  }
  else if (name == R_arm_2)
  {
    krs1.setPos(R_arm_2deg_2);
  }
  else if (name == L_arm_1)
  {
    krs2.setPos(L_arm_1deg_2);
  }
  else if (name == L_arm_2)
  {
    krs1.setPos(L_arm_2deg_2);
  }
}

 

 ご指摘&アドバイスあればお願いします!