倒立振子ロボット ver2を作る（２）-マイコンの再選定とモータ制御

前回、倒立振子ロボット ver2のための新しいモータを選定し、そのモータ寸法に合わせて車体の再構成を行いました。

今回はちょっと地味ですが、モータが変わったことによる変更点に対処していきます。

簡単に言えばUSB経由でモータが制御できなくなったため、今までは姿勢角の検出だけをやっていたマイコンにモータへの指令値送信、モータからのエンコーダ情報取得等をさせるようにします。

出力以外にモータのどこが変わったのか

今まで使っていたモータはシリアルサーボとかコマンドサーボとか呼ばれるもので、シリアル通信で信号を授受し、サーボを動かしたりサーボの情報を取れたりするものでした。

私が使っていた近藤科学社の他、フタバ社やDynamixelというブランド？会社？のものが有名です。DynamixelのやつはROSのドライバも用意されていて、恐らく一番メジャーなんじゃないかと思います。

このモータの何が良かったかって、USBケーブルで接続してROSが動いているホストPCから直接、制御と情報取得ができたんですね。

一方、今回の選定したモータは普通のDCギヤードモータにエンコーダが内蔵されているもので、モータドライバへの出力、エンコーダパルスの検出をGPIOを使って実施する必要があり、これはホストPCにとってはあまり得意なことではありません。GPIOのついたマイコン（と呼ぶのかな）が必要になります。

他に、回路搭載用のセンサの情報取得なんかもPCよりマイコンが得意ですので、今までIMUとのインターフェース用マイコンとしてLambda Plusを搭載していたところでした。

マイコンの再選定

今まで、姿勢角検出だけできればよかったマイコンにはArduino UNOの互換ボードであり、既にIMUが搭載されていたLambda Plusを使用していました。

IMUが既に基板に載っているというのはものすごく便利なのですが、左右のモータのエンコーダパルスを取得するために必要な、外部割り込みに対応したピンの数が不足していました。（２本しか無いが、エンコーダ左右✕２信号ずつで４本必要）

そこで、Arduino系統で言えばMEGA、またはそれ以外のマイコンを選ぶことになります。

今回は（理由は忘れましたが）チップとしてESP-WROOM-32（ESP32？）が載り、電源系が安定していると噂のESPr developer 32を使用することにしました。
www.switch-science.com
本当になんでこれを選んだのか思い出せないのですが、

• ピン配置に自由度がある
• 小型だから基板の他パーツ配置の自由度が高い（ArduinoMEGAだとオリジナルのシールドみたいにしてパーツを並べることになるが、意外と狭くてやりづらい）
• 純粋にハイスペック（クロック周波数が高い）

あたりがまあ理由になるのかなあという感じです。

スイッチサイエンスから購入し、使い慣れたArduino IDEをESP32開発ボード対応としました。

エンコーダ情報の取得

モータエンコーダ情報の取得は、以下のサイトを参考にしました。（ありがとうございます）
A相とB相のエンコーダ情報を2bitの１つの数として扱うやり方は目からウロコでした。
jumbleat.com

外部割り込みのやり方はArduinoとESP32開発ボードで異なりますので注意が必要です。それについては以下のサイトを参考にしました。（ありがとうございます）
lastminuteengineers.com

Arduinoに比べるとESP32系はやはり情報が少ない気がして、若干苦労しました。

左右モータのA相、B相で計4本のピンとなりますが、まずはモータ１つ分、GPIOピン２本分でカウントをしてみます。それぞれHIGH⇔LOWの変化があったら割り込み関数を呼び出します。

例えば左モータのA相のパルスが来て割り込み処理を行っていて、それが終了する前に右モータのB相のパルスが来てしまうと同じ関数を入れ子的に呼び出すことになり、更に同じペースで割り込みが起これば「未完了の割り込み関数という負債」がたまっていく気がするのですが、どうするんでしょう。

クロック周波数の高いマイコンを選んでおいてよかった、ということでいいのかな。もしかしたら、ものすごく速い回転には対応しないかも知れません。

実際のコード

流れとしては

1. エンコーダのA相、B相がつながったGPIOポート（17番、16番）の電圧が変化したら割り込み関数を呼び出すようにピンアサインを決定（void setup()）
2. 割り込み関数では、A相、B相それぞれのピン状態を読み、2bitの1つの値として格納（pulse_L）（上記webサイト参照）
3. pulse_Lを前回の値（last_pulse_L）と比較。（無いと思うが）全く変化が無ければチャタリングとしてオミット。0⇔3の変化ならエンコーダカウント値（count_L）をカウントアップ／ダウン、それ以外で値が飛んでいたらチャタリングと扱いオミット。順当に増／減していればカウントアップ／ダウン。
4. メイン関数ではエンコーダカウント値に変化があれば、それに基づいてカウント値、モータシャフト回転量（723.24カウント／回転）、回転速度をシリアルモニタに表示。

// use pin 16 & 17 for input of the encoder of the left moter, 34 & 35 for right motor
#define ENC_L_A 17
#define ENC_L_B 16
#define ENC_R_A 35 //not used
#define ENC_R_B 34 //not used

// configuration of servo city motor : https://www.servocity.com/317-rpm-spur-gear-motor-w-encoder
#define PULSE_PER_ROUND 723.24  //  at output shaft


//  parameters for interrupt
volatile byte pulse_L, last_pulse_L; //pulse state (integrate A/B phase as a single 2bit value)
volatile long count_L = 0; //encoder total count
//volatile bool LED = false; // for debug

//  parameters to record time
int time1, time2, dt;

//  parameters for motor output shaft state
long current_count_L, last_count_L = 0;
float total_round = 0, last_round = 0, rpm = 0;


void IRAM_ATTR ISR() {
  //  refresh each value if it is changed
  //  handle those as a single 2 bit number; 1st bit is High/Low of A, 2nd bit is High/Low of B
  pulse_L = !digitalRead(ENC_L_A) << 1;
  pulse_L += !digitalRead(ENC_L_B);

  //  pulse_L will incremented as 0, 01(1), 11(3), 10(2) or reversed order of them.
  //  exchange value of 2 & 3 since this order is a bit confusing.
  if (pulse_L == 3) {
    pulse_L = 2;
  }
  else if (pulse_L == 2) {
    pulse_L = 3;
  }

  //  exception process
  if (pulse_L == last_pulse_L) {
    //  do nothing when encoder signal has chattered.
  }

  else if (pulse_L == 0 && last_pulse_L == 3) {
    count_L--;
  }
  else if (pulse_L == 3 && last_pulse_L == 0) {
    count_L++;
  }

  //  counting encoder;main process
  else if (abs(pulse_L - last_pulse_L) > 1) {
    // do nothing when encoder signal has hopped(= chattering).
  }

  else if (pulse_L > last_pulse_L) {
    count_L--;
  }
  else {
    count_L++;
  }

  //  reserve pulse info as last one of those
  last_pulse_L = pulse_L;

}


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

  pinMode(ENC_L_A, INPUT_PULLUP);
  pinMode(ENC_L_B, INPUT_PULLUP);

  pinMode(23, OUTPUT);

  attachInterrupt(ENC_L_A, ISR, CHANGE);
  attachInterrupt(ENC_L_B, ISR, CHANGE);
}


void loop() {
  // print encoder state only when it was rotated
  if (count_L != last_count_L) {
    // print total count oh the encoder
    Serial.print(count_L);
    Serial.println(" count");

    // calculate and print total round of motor output shaft 
    total_round = float(count_L / PULSE_PER_ROUND);
    Serial.print(total_round);
    Serial.println(" round");

    // calculate passed time since the last loop
    time1 = micros();
    dt = time1 - time2;
    time2 = time1; //nearer is better to preserve last time of loop since you get dt

    //calculate and print the rotation speed[rpm] based on total count of the encoder and dt[microseconds]
    //be careful the parameters should be regarded as float in case they become extremely small numbers
    rpm = ((float(count_L - last_count_L) / dt) / PULSE_PER_ROUND) * 1000000 * 60;
    Serial.print(rpm);
    Serial.println(" rpm");

    // preserve encoder data as them of the last loop
    last_count_L = count_L;
    Serial.println();
  }
}

モータ回転の制御

倒立振子ロボット ver1では、モータの回転自体もUSBから位置、速度、トルク等を指示すればそのとおりに動かすことができました。

こちらについても今回は普通のDCモータとなりますので、モータドライバにGPIOからHIGH/LOW信号を送って正転／逆転を制御するのが一般的かと思います。

安めのモータドライバだと、入力は以下のような構成になっていて、入力1，2にHIGH/LOWで正転、LOW/HIGHで逆転みたいな感じなんではないかと思います。細部はしばらく使っていないので忘れましたが・・・

モータの回転速度を制御する場合、HIGH側の入力電圧をPWMによってMAXから目減りさせ、それでモータ回転速度をMAXから落としていたような気がします。

• モータ電源
• モータドライバ電源
• GND
• モータドライバ入力１
• モータドライバ入力2

今回使用したpololu社のVNH5019というドライバは更に追加で以下のような構成になっていて、少し高級感がありました。回転方向のみを２つのドライバ入力ピン（本品の場合はAピン/Bピン）のHIGH/LOWで決め、回転速度は別のピンでPWM入力により決めます。

• モータ回転速度（PWM）
• モータドライバ入力無効化（A/Bピンそれぞれ１本ずつ）
• 電流検出ピン
• 電流検出無効化ピン（HIGHで電流検出無効化）

（再掲）
www.pololu.com

無効化ピンをLOWに落とせば出力が止まります。（念のため両方落としたら良いと思いますが、片方だけでもモータは回らなくなった気がします。）

電流検出ピンはAnalogReadで読み取れるはずなのですが、今の所上手く使えていません。参照用に3.3Vを読むと確かに3.3Vが読めるので、AnalogRead周りのコードが間違っているということはないはずですが・・・　予備含め3個買って3個とも使えなかったので、何かコツがあるんでしょうか・・・わかる人おしえてください。電流検出無効化はもちろんオフです。。。

実際のコード

ESP32でPWM出力をする場合Arduinoとは少し勝手が異なる（setup関数内でのピン定義の部分）ので注意が必要でした。

PWM出力は以下を
wak-tech.com
pololu VNH5019については以下を、それぞれ参考にさせてもらいました（ありがとうございます）
yunit.techblog.jp

コードは相当に単純で、流れとしては

1. ドライバの入力A、Bをそれぞれピン15、14に接続したものとして、PWM入力をピンA10に接続したものとして取り扱う
2. それぞれのピンを出力／PWM出力として定義
3. あとは自由ですが、以下では30msecごとに正転出力0~255、逆転出力0〜255（最大値は4096）という1周期15秒程度でモータが正転／逆転を繰り返すコードとしました。

// pololu motor driver : VNH5019 test

// use pin15 as INA, pin14 as INB for the motor driver, A10 as PWM output(analog output)
#define INA_L 15
#define INB_L 14
#define PWM_L A10

// channels for PWM
#define CHANNEL_L 0

// pins for encoder input A/B
#define ENC_A_1 17 // not used
#define ENC_B_1 16 // not used

// (not used) encoder configuration : pulse output per round of motor shaft
#define PULSE_PER_ROUND 723.24 //encoder pulse resolution * gear ratio

void setup() {
  //  set pins as output
  pinMode(INA_L, OUTPUT);
  pinMode(INB_L, OUTPUT);

  // set PWM channel and pin
  ledcSetup(CHANNEL_L, 12800, 10); // channel:1(0〜?), pwm_frequency:12800 Hz, relosution : 10bit 4096 steps
  ledcAttachPin(PWM_L, CHANNEL_L); // set pin:PWM_L to channel 0

  // start serial communication
  Serial.begin(115200);
}

void loop() {
  // count up i from 0 to 255 by every 30[msec]
  for (int i = 0; i <= 255; i++) { //maximum of pwm output for esp32 is 4096

    // drive the motor
    vnh5019_setspeed(i);

    // delay 30[msec] to next loop
    delay(30);
  }

  // count down i from 0 to -255 by every 30[msec]
  for (int i = 0; i >= -255; i--) {//maximum of pwm output for esp32 is 4096

    // drive the motor
    vnh5019_setspeed(i);

    // delay 30[msec] to next loop
    delay(30);
  }
}

// function to drive motor
void vnh5019_setspeed(int pwm_value) {

  // when pwm_value > 0, drive motor forward
  if (pwm_value > 0) {

    // set PWM value : degree of motor output (maximum is 4096, but limited to 255)
    ledcWrite(CHANNEL_L, pwm_value);

    // set pins HIGH/LOW to drive motor forward
    digitalWrite(INA_L, HIGH);
    digitalWrite(INB_L, LOW);
    //    Serial.println(pwm_value);
  }

  else if (pwm_value < 0) {

    // set PWM value : degree of motor output (be careful PWM value itself should be positive number)
    ledcWrite(CHANNEL_L, abs(pwm_value));

    // set pins HIGH/LOW to drive motor backward
    digitalWrite(INA_L, LOW);
    digitalWrite(INB_L, HIGH);
    //    Serial.println(pwm_value);
  }

  else { // when PWM value is 0, enfree the motor
    ledcWrite(CHANNEL_L, pwm_value);
    digitalWrite(INA_L, LOW);
    digitalWrite(INB_L, LOW);
    //    Serial.println(pwm_value);
  }
}

元々Arduinoを使ってテストしていたコードなのでPWM出力はMAXでも255/4095と少なめですが、ちゃんと回ります。

次回はこれをベースに、倒立振子ロボットのマイコン部分の実装（主にホストPCとの通信部分）をしていきます。

ktd-prototype.hatenablog.com