1.今回やること

前回に引きつづきLegoでつくるFPVラジコンのお話です。
今回は写真右側のLegoで作るFPVラジコンの車輪やカメラTilt操作用のサーボの制御についてまとめます。

2.出来たもの

2-1.いきなり完成品動画

今のところDualshock3でBluetooth接続なので、wifi化したい。 pic.twitter.com/ocUUnpMHh4
— AIRPOCKET＠rastaman vibration (@AirpocketRobot) February 26, 2020

完成しました！
カメラで撮影した画像をPCにストリーミングしてFPV操縦も出来ています。

2-2.いきなり完成品プログラム

制御にはESP32 devkit C、開発環境にはArduino IDEを使用しています。
ESP32の開発環境づくりはこちらのサイトなどで紹介されています。
また、サーボドライバーPCA9685とPS3コントローラーをESP32で使うためにそれぞれの外部ライブラリをインストールしてください。
Adafruit PWM Servo Driver Livrary
Control your ESP32 projects with a PS3 controller!

// ESP32 devkit C使用
// PCA9685使用
// dualshock3使用

# include <Wire.h>
# include <Adafruit_PWMServoDriver.h>
# include <Ps3Controller.h>

int player = 0;
int battery = 0;

Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();

# define SERVOMIN 80 // This is the 'minimum' pulse length count (out of 4096)
# define SERVOMAX 500 // This is the 'maximum' pulse length count (out of 4096)
# define SERVO_FREQ 50 // Analog servos run at ~50 Hz updates
# define CAMMAX 440
# define CAMCENTER 300
# define CAMMIN 200
# define CAMUNDER 160
# define AXISMAX 128

int vx = 0;     //横移動速度        left -100～100 right　
int vy = 0;     //前後移動速度      back -100～100 front
int vr = 0;     //回転速度          left -100～100 right
int CAMpitch = CAMCENTER; //カメラ上下位置   200～440

void set_speed(){
    //vx; 横移動速度   前進  + 後進  -    -AXISMAX~AXSISMAX
    //vy; 前後移動速度 右移動+ 左移動-    -AXISMAX~AXSISMAX
    //vr; 回転速度     右回転+ 左回転-    -AXISMAX~AXSISMAX

    int v[4]; //v[0]右前輪出力,v[1]左前輪出力,v[2]左後輪出力,v[3]右前輪出力
    double k = 1.0;  //直進速度に対する回転速度の係数
    
    //車輪の回転方向の変更を行う場合は正負反転
    double c[4] = { -1.0 , 1.0 , 1.0 , -1.0 }; //出力補正係数　0.8~1.0

    //各モーターの出力が100%を超えないためには |vr| + |vy| + |vr| <= 100である必要が有るため処理
    double vadd = abs(vx) + abs(vy) + abs(vr);

    if (vadd >= AXISMAX){
        vx = vx*(AXISMAX/vadd);
        vy = vy*(AXISMAX/vadd);
        vr = vr*(AXISMAX/vadd);
    }

    v[0] = ( -vx + vy + (k * vr) ) * c[0];
    v[1] = (  vx + vy - (k * vr) ) * c[1];
    v[2] = ( -vx + vy - (k * vr) ) * c[2];
    v[3] = (  vx + vy + (k * vr) ) * c[3];

    for(int i = 0 ; i<=3 ;i++){
        pwm.setPWM(i,0,map(v[i], -AXISMAX, AXISMAX, SERVOMIN, SERVOMAX));
        Serial.print("wheel ");
        Serial.print(i);
        Serial.print(" output = ");
        Serial.print(v[i]);
        Serial.print("% : ");
        Serial.print(map(v[i],-AXISMAX,AXISMAX,SERVOMIN,SERVOMAX));
        Serial.println("/4096");
    }

    pwm.setPWM(4,0,CAMpitch);
}
void notify()
{
    //---------------- Analog stick value events ---------------
   if( abs(Ps3.event.analog_changed.stick.lx) + abs(Ps3.event.analog_changed.stick.ly) > 2 ){
//       Serial.print("Moved the left stick:");
//       Serial.print(" x="); Serial.print(Ps3.data.analog.stick.lx, DEC);
//       Serial.print(" y="); Serial.print(Ps3.data.analog.stick.ly, DEC);
//       Serial.println();
       vy = -Ps3.data.analog.stick.ly;
       set_speed();
    }

   if( abs(Ps3.event.analog_changed.stick.rx) + abs(Ps3.event.analog_changed.stick.ry) > 2 ){
//       Serial.print("Moved the right stick:");
//       Serial.print(" x="); Serial.print(Ps3.data.analog.stick.rx, DEC);
//       Serial.print(" y="); Serial.print(Ps3.data.analog.stick.ry, DEC);
//       Serial.println();

       vr = -Ps3.data.analog.stick.rx;
       set_speed();
   }

   //--------------- Analog D-pad button events ----------------
   if( abs(Ps3.event.analog_changed.button.up) ){
//       Serial.print("Pressing the up button: ");
//       Serial.println(Ps3.data.analog.button.up, DEC);
       CAMpitch += Ps3.data.analog.button.up / 20;
       if(CAMpitch > CAMMAX){
          CAMpitch = CAMMAX;
       }
       set_speed();
   }

   if( abs(Ps3.event.analog_changed.button.down) ){
//       Serial.print("Pressing the down button: ");
//       Serial.println(Ps3.data.analog.button.down, DEC);
       CAMpitch -= Ps3.data.analog.button.down / 20;
       if(CAMpitch < CAMMIN){
          CAMpitch = CAMMIN;
       }
       set_speed();
   }

   if( abs(Ps3.event.analog_changed.button.right) ){
//       Serial.print("Pressing the right button: ");
//       Serial.println(Ps3.data.analog.button.right, DEC);
       CAMpitch = CAMCENTER;
       set_speed();
   }


   if( abs(Ps3.event.analog_changed.button.left) ){
//       Serial.print("Pressing the left button: ");
//       Serial.println(Ps3.data.analog.button.left, DEC);
       CAMpitch = CAMCENTER;
       set_speed();
   }

   //---------- Analog shoulder/trigger button events ----------
   if( abs(Ps3.event.analog_changed.button.l1)){
//       Serial.print("Pressing the left shoulder button: ");
//       Serial.println(Ps3.data.analog.button.l1, DEC);
       vx = -Ps3.data.analog.button.l1;
       set_speed();
   }

   if( abs(Ps3.event.analog_changed.button.r1) ){
//       Serial.print("Pressing the right shoulder button: ");
//       Serial.println(Ps3.data.analog.button.r1, DEC);
       vx = Ps3.data.analog.button.r1;
       set_speed();       
   }
}

void onConnect(){
    Serial.println("Connected.");
}

void setup() {
    Serial.begin(115200);

    Ps3.attach(notify);
    Ps3.attachOnConnect(onConnect);
    Ps3.begin("00:11:22:33:44:55");  //接続するESP32のmacアドレスを入力
    pwm.begin();
    pwm.setOscillatorFrequency(27000000);  // The int.osc. is closer to 27MHz  
    pwm.setPWMFreq(SERVO_FREQ);  // Analog servos run at ~50 Hz updates
    pwm.setPWM(0,1,0);
    pwm.setPWM(1,1,0);
    pwm.setPWM(2,1,0);
    pwm.setPWM(3,1,0);
    pwm.setPWM(4,0,300);
    //delay(1000);

    Serial.println("start");
}

void loop() {

    delay(10000);

}

3.ESP32とPCA9685でサーボモーター制御

サーボモーターといっても産業用まで含めるととてもたくさんの種類がありますが、今回使用するサーボはPWM制御方式のサーボモーターです。デジタル信号で制御量をコントロールする為デジタルサーボと呼ばれる事もあります。
電子工作をしているとフィードバック情報をもとにさらに高度な制御が出来るコマンドサーボ（シリアルサーボ）もありますが、今回は使用しません。

3-1.今回使用するサーボモーター

今回使用するモーターは5個、車輪用の360°タイプのサーボが4個と、カメラ用の270°タイプのサーボが1個です。
いずれもPWM方式のサーボモーターですが、制御内容が若干異なります。

3-1-1.360°回転タイプ（ローテーションタイプ）サーボモーター

このタイプのサーボモーターは、PWM制御によりサーボの回転方向と回転速度を指定します。一方向に回り続ける事が出来るため、車輪の駆動に使用しています。

3-1-2.270°タイプサーボモーター

このタイプのサーボモーターは、PWM制御によりサーボの目的角度を指定します。稼働範囲が決まっていて、今回のモーターであれば0~270°の範囲内でしか動けません。カメラの上下首振りに使用しています。

3-2.PWM制御について

3-2-1.モーターの仕様など

PWM制御については様々なサイトで詳細な説明が有ります。
[こちらのサイト]
(https://monoist.atmarkit.co.jp/mn/articles/0706/06/news132.html)なども判りやすく説明されていますので参考にしてみてください。
今回のサーボモーターは電源入力が3.3~6Vに対応している様です。電圧が高い程高速で動作します。
またStall Current（停動電流）が700mAです。4輪全てを全力出力させ、かつ外部負荷で停止すると2.8Aの電力が流れます。結構な電流ですので要注意です。

PWM制御に関しては、このギークサーボのデータシートが入手できていません。実際いPWM信号を流して動作範囲を確認したところ、50Hzでの制御範囲はduty比で80/4096~500/4096程度でした。
ローテーションサーボの場合、80/4096で反転全速、500/4096で巡回転全速、290/4096で静止状態です。
270°サーボの場合、80/4096で0°、500/4096で270°相当となります。

実際にはサーボの個体差もあるため、回転数や静止角度に（結構大きな）誤差が生じます。

3-2-2.PCA9685サーボモータードライバー

PCA9685は16台のサーボを同時に制御できる超便利ボードです。
Aliexpressだと一個163円！！とかで買えちゃいます。何個か持っとくと何かと便利。

ただし！
すべてんのピンの合計電流の許容値が400mAらしく、サーボ5個を動かすには容量不足感が否めません。私が使っている範囲では特に不具合は有りませんが、最悪の場合、加熱による出火などもあり得るのでご使用は自己責任でお願いします。

PCA9685はESP32とI2C通信を行い、サーボの制御指令を受けとり実際にサーボをコントロールします。
ロジック電圧は3.3VですのでESP32から直接供給を受けられます。モーター駆動電圧は別途受ける必要が有ります。
サーボはNo.0~15の16個を制御、一台目のPCA9685のI2C通信アドレスは0x40。17台目以降のサーボも制御したい場合は追加する事も可能。2台目以降のアドレスは基板上のジャンパをはんだ付けすることで変更可能です。

4.DUALSHOCK 3による制御

ESP32 devkit CとDUALSHOCK 3をBlurtooth接続してローバーをコントロールしています。
このサイトを参考にすると簡単に接続出来ます。

接続手順は以下の通りです。

①接続先のESP32に次のコードを入力し、シリアルモニタでmacアドレスを確認する。

BTmacadrs.ino
//*****************************************************
void setup(void) {
  Serial.begin(115200);
  Serial.println("-----------------------------");
  uint8_t btmac[6];
  esp_read_mac(btmac, ESP_MAC_BT);
  Serial.printf("[Bluetooth] Mac Address = %02X:%02X:%02X:%02X:%02X:%02X\r\n", btmac[0], btmac[1], btmac[2], btmac[3], btmac[4], btmac[5]);
}

void loop() {
}
//*****************************************************

②PCにDUALSHOCK3を接続し、SixaxisPairToolで接続先のESP32のmacアドレスを書きこむ。
SixaxisPairToolダウンロード

③Aruduino IDEにドライバライブラリをインストールする。
ドライバライブラリ

あとは、コードを書けばPS3とペアリング出来ます。

5.ESP32-CAMによる動画ストリーミング

ESO32-CAMはOV2640というカメラを搭載したESP32開発ボードです。
ESP32の開発ボードにもメモリの搭載量などにより多くの種類があるのですが、動画のストリーミングを円滑に行うためにメモリ搭載量を増やしつつ、ボード上にカメラとの接続端子を設けているモデルです。
デモ用のコードを入れればすぐにWi-Fiによる動画ストリーミングが出来る便利ボードです。

唯一の弱点は技適を取っていない点です。基本的に技適を取っていない無線機器は日本国内では使用出来ませんが、技適未取得機器を用いた実験等の特例制度を活用すると短期間の実験等のみを目的として使用が許可されます。

今回は、ESP32-CAMを用いたFPVラジコンの実験を目的として届け出をしています。
使用届出が面倒な場合は、技適取得済みの同等のデバイスとしてM5Cameraが有ります。
こちらは筐体付きのユニットになっていますのでESP32-CAMよりも使いやすいかもしれません。

ESP32-CAMについては技適未取得と言うこともあり、日本語情報が少なかったのですが、こちらのサイトで詳しく紹介されていましたので参考にしてください。

接続すると、ESP32-CAMがサーバーとなり動画を配信します。視聴する際にはESP32CAMのIPアドレスを指定しますが、アドレスを自動取得にしておくとアドレスが変わって意しまった際に見つけられません。
次の行を追加して固定IPにしておきましょう。

ライブラリincludeの後あたりに・・・

IPAddress ip(192, 168, 1, 200);
IPAddress gateway(192, 168, 1, 1);
IPAddress subnet(255, 255, 0, 0);

アドレスは自分の環境に合わせて適宜変更してください。

setup()関数中のWiFi.beginの前に・・・

  WiFi.config(ip,gateway,subnet);

Roverに取り付ける際は、デモコードをインストールした上で3.3Vの電源とGNDを接続するだけです。今回は、Rover制御用のESP32の3V3、GNDと接続しています。

6.配線図

配線は上図の通りです。

6-1.ボード間接続のIO対応

ボードのIO対応表は以下の通り。

ESP32 devkit C	ESP32
3V3	VCC
GND	GND

ESP32 devkit C	PCA9685
3V3	VCC
GND	GND
GPIO 21	SDA
GPIO 22	SCL

6-2.電源供給

今回は電源としてモバイルバッテリーを使いました。
私が使ったのは昔に買ってあまり使っていなかったcheero power plusです。2.1A出力と1.0A出力がそれぞれ1ラインついているので、ESP32へ1.0A、PCA9685へ2.1Aを接続しています。

PCA9685への電力供給はバッテリーと直結したいため、micro USBのブレイクアウトボードを通じて電源とGNDを取っています。不要なUSBケーブルがあったら、ケーブルを剥いて直結してもいいと思います。

ESP32 devkit Cへはmicro USBでつないでいます。ESP32-CAMへはESP32 devkit Cの3V3から電源を取っています。容量が足り無さそうでちょっと不安でしたが今のところ安定してい動いています。

6-3.サーボ接続

PCA9685とサーボの接続は似た色同士をつなげばOKです。

Servo配線色	PCA9685端子色
茶	黒
赤	赤
黄	黄

各チャンネルの割り当ては次の通りです。

PCA9685 ch	Servo配置
0	右前輪
1	左前輪
2	左後輪
3	右後輪
4	カメラTilt

7.まとめ

Lego tecnichは図面いらずのフィーリングで形を作れるので、私の様な工作初心者にもとても優しいですね。
ESP32-CAMのストリーミングはブラウザがあれば見えるので、もっぱらOculus Questで見ながら操縦してます。少々酔いますが、PCのディスプレイで見るより没入感があるのでお勧めです。

とは言いつつ今回のRoverをつくりながら、ほぼ同じものがM5Stickを使えばもっとラクに作れるんじゃないか？と思いついてしまいました。
実際に作ると簡単に出来てしまったので、早速次の記事で紹介したいと思います。