プリメイドAIの近藤科学製サーボをESP32でギュインギュイン動かす

# 概要
- 近藤科学製サーボモーターをESP32を使用して制御する
- 今回は2019年ごろに流行った二足歩行ロボット『プリメイドAI』を使用して動作確認を行う。

# 用意するもの
- ESP32を搭載したマイコンボード
- 近藤科学製サーボを搭載したハードウェア
- それなりに動くPC(本解説はwindowsを想定しています。Mac,linuxでもたぶん行けるとは思いますが未検証です)

今回はESP32を搭載したマイコンボード『GPN-4ICS』[^1]と、近藤科学製サーボを搭載されている市販の二足歩行ロボット『プリメイドAI』[^2]を使用して動作確認を行います。

![3396d553-f75f-4a7c-96cc-ebc60cce65e0_base_resized.jpg](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/600123/8c736095-6df9-bb7e-79c3-924d2d8e77d0.jpeg)


# とりあえず動かしてみる
## 1)環境構築
### ArduinoIDEのインストール

　windowsユーザーはexe版を使用することをお勧めします
　https://www.arduino.cc/en/main/software

### ArduinoIDEにESP32の環境構築を行う
　ESP32の環境構築のやり方
1. Arduino IDEを起動
2. ファイル→環境設定をクリック
3. 『追加のボードマネージャー』に https://dl.espressif.com/dl/package_esp32_index.json を追加する
4. Arduino IDEを再起動させる
5. ツール→ボード→ボードマネージャーをクリック
6. ESP32 と検索し出てきたものをインストール
7. Arduino IDE再起動
8. Enjoy!

### 近藤科学様の公式ライブラリを入れる

近藤科学製のサーボを動かす方法として

- 公式ライブラリを導入する
- 仕様書を参照してライブラリを自作する  

という手段がありますが、今回はとりあえず動かすことを目標とするので公式ライブラリを使用します。


1.  ICS_Library_for_Arduino_V2
https://kondo-robot.com/faq/ics-library-a2
導入方法はダウンロードしたライブラリに付属している取扱説明書を参照

2. 導入したライブラリの内部を一部書き換え
近藤科学様の公式ライブラリはとても使いやすいものですが、サーボへ動作指示を出すとサーボから戻り値が返ってくるまで待機するという仕様となっています。
そのままでも一応動きますが、せっかくこんなサーボが沢山載ったハードを動かすなら複数のサーボを同時に制御して歩行とかをさせたいので、戻り値が返ってくるのを待たずに次の処理に行けるようにします。
以下の部分をコメントアウト

```C++:IcsHardSerialClass.cppの後半部分
　rxSize = icsHardSerial->readBytes(rxBuf, rxLen);

	if (rxSize != rxLen) //受信数確認
	{
		return false;
	}
```
### ESP32のシリアルの宣言ピンの指定(任意)
使用しているESP32ボードの仕様によってはSerial1を使用することになるかと思いますが、ESP32はSerial1をそのまま使用しようとすると不具合が発生します(なぜ発生するかの説明は省きます。結構有名な話なので調べるとすぐ出てきます)
そのため、**もし**Serial1を使用する際は、『C:\Users\ユーザー名\AppData\Local\Arduino15\packages\esp32\hardware\esp32\1.0.3\cores\esp32』にあるHardwareSerial.cppの前半部分を少し弄ってください。

```書き換えたHardwareSerial.cpp(該当箇所のみ）
#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <inttypes.h>

#include "pins_arduino.h"
#include "HardwareSerial.h"

#ifndef RX1
#define RX1 32//9から32に変更
#endif

#ifndef TX1
#define TX1 33//10から33に変更
#endif

#ifndef RX2
#define RX2 16
#endif

#ifndef TX2
#define TX2 17
#endif
```

## 2)動かしてみる
ライブラリの使い方は近藤科学さんがかなり丁寧に解説してくださっているのでこちらを参照してください。
https://kondo-robot.com/faq/ics_board_-tutorial2
注意点として、今回は近藤サーボにコマンドを送信後、返ってくるデータを完全に無視して動作させているので、**動かすタイミングによっては送信データと受信データがぶつかり脱力します。** ~~だれか対策考えてください~~ぶつからないようにタイミングを調整する等工夫をしてください。
<details><summary>サンプルソースコード</summary><div>

```
#include <IcsHardSerialClass.h>

const byte EN_PIN = 4;
const byte EN_PIN2 = 13;
const long BAUDRATE = 1250000;
const int TIMEOUT = 1000;		//通信できてないか確認用にわざと遅めに設定


IcsHardSerialClass krs2(&Serial2,EN_PIN2,BAUDRATE,TIMEOUT);  //インスタンス＋ENピン(2番ピン)およびUARTの指定

IcsHardSerialClass krs(&Serial1,EN_PIN,BAUDRATE,TIMEOUT); 

void standup();
void servomove();


void setup() 
{
  // put your setup code here, to run once:
  //サーボモータの通信初期設定
  krs.begin();//Body
  krs2.begin();//head_and_arm
  //standup();
}

void loop() {

    
    servomove();

    //standup();

}


void servomove()
{
   
   krs2.setPos(1,7500); 
   krs2.setPos(2,7500); 

   krs.setPos(1,7500); 
   krs.setPos(2,7500); 
   delay(1000);
// krs2.begin();
   krs2.setPos(1,8000); 
   krs2.setPos(2,7000); 
   //krs.begin();
   krs.setPos(1,8000); 
   krs.setPos(2,7000); 
   delay(1000);

}



void standup()
{
  delay(1000);
  krs.setPos(1,7500); 
  delay(1000); 
  krs2.setPos(1,7500);
  delay(1000);  
  krs.setPos(2,7500);
  delay(1000);  
  krs2.setPos(2,7500);
  delay(1000);  
  krs.setPos(3,7500);
  delay(1000);  
  krs2.setPos(3,7500);
  delay(1000);  
  krs.setPos(4,7500);
  delay(1000);  
  krs2.setPos(4,9375);
  delay(1000);  
  krs.setPos(5,7500);
  delay(1000);  
  krs2.setPos(5,5625);
  delay(1000);  
  krs.setPos(6,7500);
  delay(1000);  
  krs2.setPos(6,7500);
  delay(1000);  
  krs.setPos(7,7500);
  delay(1000);  
  krs2.setPos(7,7500);
  delay(1000);  
  krs.setPos(8,7500);
  delay(1000);  
  krs2.setPos(8,7500);
  delay(1000);  
  krs.setPos(9,7500);
  delay(1000);  
  krs2.setPos(9,7500);
  delay(1000);  
  krs.setPos(10,7500);
  delay(1000);  
  krs2.setPos(10,7500);
  delay(1000);  
  krs.setPos(11,7500);
  delay(1000); 
  krs2.setPos(11,7500);
  delay(1000);  
  krs.setPos(12,7500); 
   delay(1000);
  krs.setPos(13,7500);
   delay(1000); 
  krs.setPos(14,7500);
  delay(1000);
}
```
</div></details>

動作確認時の筆者ツイート(動画付き)
<blockquote class="twitter-tweet"><p lang="ja" dir="ltr">一応動きましたよ。<a href="https://twitter.com/hashtag/%E3%83%97%E3%83%AA%E3%83%A1%E3%82%A4%E3%83%89AI%E3%83%8F%E3%83%83%E3%82%AF?src=hash&amp;ref_src=twsrc%5Etfw">#プリメイドAIハック</a><a href="https://twitter.com/hashtag/%E3%83%97%E3%83%AA%E3%83%A1%E3%82%A4%E3%83%89AI%E8%84%B3%E6%AD%BB%E3%83%8F%E3%83%83%E3%82%AF?src=hash&amp;ref_src=twsrc%5Etfw">#プリメイドAI脳死ハック</a> <a href="https://t.co/rDacdR7eQI">pic.twitter.com/rDacdR7eQI</a></p>&mdash; 樋ログッピー@技術書典 応援祭参加中 (@Guppy_T_Higuchi) <a href="https://twitter.com/Guppy_T_Higuchi/status/1172185749103362048?ref_src=twsrc%5Etfw">September 12, 2019</a></blockquote> <script async src="https://platform.twitter.com/widgets.js" charset="utf-8"></script> 


>追記
近藤科学さんのサーボには『スレーブモード』という機能が存在しており、そのモードを有効化することでデータ衝突を防げるという案を かえでさん[@kedtn](https://twitter.com/kedtn/status/1318535294447095808 "@kedtn") からいただきました。
各サーボに設定を反映する為にロボットを分解しサーボを一つ一つ設定変更する必要があるので面倒ではありますが、一度設定したらかなり楽になるので、できる人は行っておくといいかもしれませんね。




# おまけ
筆者が技術系イベント参加時に稼働させていたプリメイドAIのプログラムを今更ですが公開します。技術書典の数日前に作っていたので全くリファクタリングがされていないです。なので人様に見せられる状態ではないですがそれでもいいならどうぞ。
（シリアルの宣言周りは使用するボードの仕様に合わせて変更してください）
<details><summary>イベント用ソースコード</summary><div>

```onyudou-tomas.ino
#include <IcsHardSerialClass.h>

const byte EN_PIN = 4;
const byte EN_PIN2 = 13;
const long BAUDRATE = 1250000;
const int TIMEOUT = 1000;		//通信できてないか確認用にわざと遅めに設定


IcsHardSerialClass krs2(&Serial2,EN_PIN2,BAUDRATE,TIMEOUT);  //インスタンス＋ENピン(2番ピン)およびUARTの指定

IcsHardSerialClass krs(&Serial1,EN_PIN,BAUDRATE,TIMEOUT); 

void standup();
void servomove();

void kubihuri();//0~3
void hey();//3~6
void kubiyoko();//6~10
void kubiyoko2();
void march_with_keyboard();//10~14
void guda();//14~15

void setup() 
{
  // put your setup code here, to run once:
  //サーボモータの通信初期設定
  krs.begin();//Body
  krs2.begin();//head_and_arm
  standup();
}

void loop() {
  // put your main code here, to run repeatedly:
    krs2.setPos(5,4725);
    delay(500);
    krs2.setPos(7,8000);
    delay(500);
    krs2.setPos(9,4500);
    delay(500);
    krs.setPos(2,6500);
    delay(500);
    krs2.setPos(4,10000);
    krs.setPos(1,10000);
    delay(1500);
    
    kubihuri();//0~3
    hey();//3~6
    kubiyoko();
    kubiyoko2();//6~10
    march_with_keyboard();//10~14
    guda();//14~15
    krs.setPos(1,7500);
    
    kubiyoko2();//15~17

    //standup();

    while(true)
    {
    }
}

void kubihuri()
{
  for(int i=0;i<6;i++)
  {
    krs2.setPos(1,7500);
    delay(250);
    krs2.setPos(1,8000);
    delay(250); 
  }
}

void hey()
{
  krs.setPos(1,5000);//test 10000 or 4000
  krs2.setPos(9,7700);
  delay(100);
  krs2.setPos(5,7380);
  krs2.setPos(2,6500);
  delay(100);
  krs2.setPos(7,10000);
  
  
  kubihuri();
}

void kubiyoko()
{
  krs.setPos(1,10000);
  krs2.setPos(5,4725);
  krs2.setPos(7,8000);
  delay(100);
  krs2.setPos(9,4500);
  krs.setPos(2,6500);
  krs2.setPos(4,10000);
  krs2.setPos(2,7500);

  delay(100);
  krs2.setPos(3,7000);
  delay(500);
  krs2.setPos(3,8000);
  delay(500);
  krs2.setPos(3,7000);
  delay(500);
  krs2.setPos(3,8000);
  delay(500);
}

void kubiyoko2()
{
  krs2.setPos(3,7000);
  delay(500);
  krs2.setPos(3,8000);
  delay(500);
  krs2.setPos(3,7000);
  delay(500);
  krs2.setPos(3,8000);
  delay(500);
  krs2.setPos(3,7500);
}

void march_with_keyboard()
{
  krs.setPos(1,5000);
  krs2.setPos(4,9375);
  krs2.setPos(8,8500);
  delay(100);
  krs2.setPos(5,5625);
  krs2.setPos(7,10000);
  krs.setPos(6,7900);
  krs.setPos(14,7100);
  //delay(100);
  krs.setPos(5,7900);
  krs.setPos(13,7100);
  delay(500);
  krs.setPos(2,4000);
  krs.setPos(8,9000);
  krs.setPos(10,9000);
  delay(250);
  krs.setPos(8,7500);
  krs.setPos(10,7500);
  delay(250);
  krs.setPos(2,5000);
  krs.setPos(6,7500);
  krs.setPos(14,7500);
  krs.setPos(5,7500);
  krs.setPos(13,7500);
  delay(1000);
  krs.setPos(5,7200);
  krs.setPos(13,7800);
  krs.setPos(6,7200);
  krs.setPos(14,7800);
  delay(500);
  krs.setPos(2,6000);
  krs.setPos(7,6000);
  krs.setPos(9,6000);
  delay(250);
  krs.setPos(7,7500);
  krs.setPos(9,7500);
  delay(250);
  krs.setPos(2,5000);
  krs.setPos(6,7500);
  krs.setPos(14,7500);
  krs.setPos(5,7500);
  krs.setPos(13,7500);
  delay(1000);
}

void guda()
{
  krs.setPos(1,10000);
  krs.setPos(2,5000);
  krs2.setPos(7,7500);
  krs2.setPos(9,7500);
  delay(100);
  krs.setPos(5,7200);
  krs.setPos(13,7800);
  krs.setPos(6,7200);
  krs.setPos(14,7800);
  delay(900);
  krs.setPos(6,7500);
  krs.setPos(14,7500);
  krs.setPos(5,7500);
  krs.setPos(13,7500);
  delay(100);
  
}



void servomove()
{
   
   krs2.setPos(1,7500); 
   krs2.setPos(2,7500); 

   krs.setPos(1,7500); 
   krs.setPos(2,7500); 
   delay(1000);

// krs2.begin();
   krs2.setPos(1,8000); 
   krs2.setPos(2,7000); 
   //krs.begin();
   krs.setPos(1,8000); 
   krs.setPos(2,7000); 
   delay(1000);

}


void standup()
{
  delay(1000);
  krs.setPos(1,7500); 
  delay(1000); 
  krs2.setPos(1,7500);
  delay(1000);  
  krs.setPos(2,7500);
  delay(1000);  
  krs2.setPos(2,7500);
  delay(1000);  
  krs.setPos(3,7500);
  delay(1000);  
  krs2.setPos(3,7500);
  delay(1000);  
  krs.setPos(4,7500);
  delay(1000);  
  krs2.setPos(4,9375);
  delay(1000);  
  krs.setPos(5,7500);
  delay(1000);  
  krs2.setPos(5,5625);
  delay(1000);  
  krs.setPos(6,7500);
  delay(1000);  
  krs2.setPos(6,7500);
  delay(1000);  
  krs.setPos(7,7500);
  delay(1000);  
  krs2.setPos(7,7500);
  delay(1000);  
  krs.setPos(8,7500);
  delay(1000);  
  krs2.setPos(8,7500);
  delay(1000);  
  krs.setPos(9,7500);
  delay(1000);  
  krs2.setPos(9,7500);
  delay(1000);  
  krs.setPos(10,7500);
  delay(1000);  
  krs2.setPos(10,7500);
  delay(1000);  
  krs.setPos(11,7500);
  delay(1000); 
  krs2.setPos(11,7500);
  delay(1000);  
  krs.setPos(12,7500); 
   delay(1000);
  krs.setPos(13,7500);
   delay(1000); 
  krs.setPos(14,7500);
  delay(1000);
}
```
</div></details>

[^1]: GPN-4ICS　https://www.shortlovers.net/gpn-4ics
[^2]: プリメイドAI公式サイト　http://robots.dmm.com/robot/premaidai