ROSで使えるモータを作りたい!

 ROS開発などを行っていると、定速制御ができてフィードバックが取れるモーターが欲しくなってしまいます。普通のDCブラシモーターでは回転の特性が線形になってくれないのでエンコーダつけたりLAP制御しないときれいな制御ができません。また、ROS対応の製品(ダイナミクセルとか)ではサイズが小さすぎてAGVやAMRのような大きな製品には適していません。かと言って大きいモーターになると一個10万円クラスになってしまい、とても高価です。筆者はとにかくハイパワーなロボットを安くで作りたいということで色々思考錯誤している訳ですがようやくいい感じの組み合わせを見つけたので、日本語記事も全然無いしせっかくなので紹介したいと思います。

350W インホイールモータ

VESCとは

 スウェーデン出身の方が開発したオープンソースのBLDCドライバです。VESC自体はオープンソースなのでいろんな会社が同じESCを違う名前で出したりしています。その中でもFLIPSKYが結構人気な気がします。主に電動スケートボード用に開発されており、大電流を流すことができます。通信プロトコルもたくさんあり、ESCを2台用意することでCAN通信を行いトラクションコントロールなんてこともできちゃいます。値段は1個あたり1万円しないくらい。使ってるモーターも1万円しないくらいなので合計2万円くらい(実際には1万5千円くらい)で高性能なモータを作ることができます。

FLIPSKY FSESC

VESCにたどり着くまで

 会社で作ってるロボットでホールセンサつきインホイールモータを動かしたいと思って、色々調査しているときにたまたま発見した代物で筆者のほしい機能が全て入っていました。
 まず一般的なESCはPPM入力でモーターを回転させることしかできません。センサ付きESCだと低回転から回すことはできるものの、フィードバックが取れないので、ホールセンサのピンに割り込んであげるなどの工夫が必要でした。そこでSeeedStudioが販売しているブラシレスシールドhttps://akizukidenshi.com/catalog/g/gM-14473/を買ってみて使ってみました。この機種はパルスでフィードバックを返してくれるため一応回転数等は読めるというものでした。しかしながら欠点として、回転の方向のフィードバックが取れない、大電流が流せないという問題がありました。また回転を無理やり逆回転にすると一発で壊れてしましました。
 そこで諦めてESCを自分で作ろうとしていた時に色々調べていたら、たどり着いたのがVESCです。こいつの何がすごいって、まず高電圧大電流が流せます。 (入力60Vまで、電流50Aピーク130A)筆者が使用を予定しているインホイールモータは24V350Wなので、ちょうど良いスペックでした。さらに豊富な機能及び通信プロトコルとフィードバック機能内部パラメータが専用ソフトウェアツールで書き換えられるので、ROS等に使うのにはかなり良い気がします。

具体的な機能

主要な機能を書いていきます。

・定電流制御 定電流でモーターが回せます。
・定速制御 PID制御で一定の速度で回転させることができます。ゲインは専用ツールから書き換えられます。
・デューティ制御 PWM的な制御ができます
・トルク制御 いまいちよくわかっていませんがそれっぽい機能が使えます(もしかしたらDuty制御と一緒かも)
・位置制御 ステッパのように位置制御ができます。精度はステッパの方がいいとは思いますが。

フィードバック
・rpm rpmがUARTから取れます。しかも回転方向の識別も可能。
・オドメトリ UARTから回転量が取得できます。マイナスの場合はちゃんと減算してくれます。
・電圧 UARTから電圧を取得することができます。
・電流 UARTから電流が取得できます。

他にも機能はたくさんあるのですが、書くのがめんどくさいのでこれくらいで。

通信プロトコル

ESCというとPPMが主流なのですがPPM以外にもいろいろ使えます。

・PPM おなじみのやつ、ラジコンのプロポ受信機等と接続して使う
・UART 速度制御及び、フィードバック情報が取得できます。
・ADC DACや可変抵抗などを使って速度制御ができます。
・CAN CANでも使えるようです。(あんまり詳しくないですが
・BlueTooth 電動スケートボードに乗ってるときに使う速度制御リモコンとつながるみたい。

VESC TOOL

micro USBで接続することでファームウェア及び設定を書き換えることができます。
BLDCのキャリブレーションなどもこのソフトで行います。
ここでしっかりパラメータ調整を行わないと、PPMやUARTの入力でモータがきれいに動いてくれません。
VESCtool細かい機能などは暇があれば載せます。(日本語記事が殆どないので辛いですよね)

Arduino Mega2560サンプルソース

https://flipsky.net/blogs/vesc-tool/how-to-control-fsescbase-on-vesc-with-arduino このサイトを参考にして作りました。かなり簡単です。
シリアルモニタから回転数を入力することで使えます。ライブラリは上の記事のを参照してください。

#include <Servo.h>
#include <VescUart.h>

Servo esc;
VescUart UART;

String command;
int rpm = 0;
const uint8_t VescOutputPin = 5;

void setup() {

  /*UART*/
  Serial.begin(9600);
  Serial1.begin(115200);
  while (!Serial1) {;}
  UART.setSerialPort(&Serial1);

  /*PPM*/
  esc.attach(VescOutputPin);
  esc.writeMicroseconds(1500);

}

 void loop() {

  /*set speed*/
  if (Serial.available()) {
    command = Serial.readStringUntil('\n');
    rpm = command.toInt();
    Serial.println(rpm);
    Serial.println(map(rpm, -400 , 400, 1000, 2000));
  }
  
  /*if use PPM*/
  esc.writeMicroseconds(map(rpm, -400 , 400, 1000, 2000));
  /*if use UART*/
  UART.setRPM((float)rpm);
  
  /*get status*/
  if ( UART.getVescValues() ) {

    Serial.println(UART.data.rpm/15);//このモータのポール数が15なので
    Serial.println(UART.data.inpVoltage);
    Serial.println(UART.data.ampHours);
    Serial.println(UART.data.tachometer);
  }
}

実際に回してみた動画

使ってるモーターで700rpm以上出たので、単純計算で25km/h以上でます。ロボットに積むときはリミッターを書けるか衝突防止センサをつけることになると思います。

応用

Arduinoから制御ができるようになったので、ROS対応できるようにします!
ArduPilot Roverなんかでも使えるようにしようと思っています。