聞きかじりめも

主にC++やメディア処理技術などに関して気付いたことを書いていきます．ここが俺のメモ帳だ！

倒立振子制作日記（そのに）

ソースコード Arduino

PWMによる電流制御

前回の記事でも取り上げましたが，この記事によるとDCモータで出力制御系を構成するときは電圧入力ではなく電流入力にすると理論的扱いがしやすいとのことですので，私もこの記事と同様に電流入力系を構成しようと思います．（注：以下，超基本的な所から説明します）

ちょっと復習

DCモータにはT-I特性（トルクvs電流）とT-N特性（トルクvs回転数）というものがあり，トルクと電流は比例し，負荷トルクが増すと直線的に回転数が減少する，という特性があります（ちなみに電圧を変えるとT-N特性が平行移動します）．つまり電流入力さえできればその時の負荷トルクさえ分かれば回転数（速度）が制御できることになります．

で，どーすんの？

さて一方で，例えDAコンバータを積んでいたとしてもマイコンにできるのはGPIOの電圧出力を変調することのみです．更に言うとArduinoUNOにはDACもないのでPWM出力をアナログ出力とみなすしかありません．モータの自己インダクタンスとか慣性とか非線形摩擦とかを考慮して，Hブリッジへの入力電圧だけでモータ入力電流値を予測するのは困難です．
そこで出てくるのが電流フィードバックです．これはモータへの入力電流を何らかの方法で計測し，その測定値をPWM出力にフィードバックして目標電流値を実現する，というものです．今回使用したモータドライバL298Nにはセンサ用PIN（と称するHブリッジの下流）がありますので，抵抗を挟んでGNDに接続し，抵抗間の電圧をアナログ入力に読ませることにしました．以前の記事では電流計測ICを用意していましたが，使い方が悪いのかセンサ自体の性能かわかりませんがノイズでかすぎて使い物にならなかったので抵抗にしました．

64種類 各20個（合計1280個）金属膜抵抗キット 1/4W（0.25W） 許容差±1%

64種類 各20個（合計1280個）金属膜抵抗キット 1/4W（0.25W） 許容差±1%

この抵抗セットがめちゃくちゃ便利でした．そんなに大量に使う予定がないなら，アキバでバラ100本入り100円とかで買うより良いかも．

コーディングする前に

電流センサ抵抗の選定

電流センサ抵抗の値はどうすればいいのか．これは中学生でも知っているオームの法則を利用します．下に今回の電流計測回路の簡単な回路図を示します．

   Vss
    |
   [M] Motor (実際はHブリッジで接続)
    |
    |---- V (Analog Input)
    |
I   >
↓   > R
    >
    |
    |
   ---
   ///

マイコンのアナログ入力はハイインピーダンスであるため，Analog Inputへは殆ど電流が流れません．そのためモータ入力電流は抵抗電流 \(I\) で近似でき，オームの法則 \(V=RI\) を利用してモータ入力電流をアナログ入力から得ることができます．従って，Analog Inputの許容最大電圧とMotor入力として想定する最大入力電流が分かれば \(R\) の値を見積もれます．

ArduinoUno(互換機)のアナログ入力の許容最大電圧はマイコン電源電圧と同じ \(V_{cc}=5[V]\) であり，モータドライバL298Nの定格最大電流は \(I_{max} = 2[A]\) であるので，

\(R = V_{cc} / I_{max} = 5[V]/2[A] = 2.5[\Omega]\)

と求められます．しかし現実には電池の供給能力の関係上2[A]も出せませんし，消費電力を考えると抵抗は小さい方がモータ入力に割く電流が大きくとれ，制御に有利そうです．そこで抵抗値をもっと下げて \(R=1[\Omega]\) にしてみるとどうでしょう．すると，定格最大電流は変わらないので想定最大電圧は

\(V_{max} = RI_{max} = 1[\Omega] \times 2[A] = 2[V]\)

と，先程の5[V]よりだいぶ低くなってしまいました．マイコンのアナログ入力の最大値は変わらず5[V]ですので，これはセンサ入力の分解能が低下することを意味します．計算は省略しますが，逆に抵抗値を増大させてセンサの分解能を上げようとする場合，そもそもの制御量であるモータ入力電流の最大値が減少してしまいます（或いは，最悪の場合マイコンのアナログ入力回路を破壊する程の電圧を許容することになります）．ということでここはバランスよく決めなくてはなりません．今回は手元に2.2[Ω]の抵抗があったので，最初の計算に近いということもありこいつを使います．

アナログ入力から電流値への変換

電子工作やPCに疎い人は分からないかもしれないので一応説明しておくと，実はマイコンのアナログ入力から直接返ってくる値は物理的な電圧値（実数）ではなく，それをADコンバータ(ADC)が読み取ってADCの精度(bit数)に応じて予め決められた最大値との比を整数値で返す，という動作をします．例えば，5[V]駆動の8bitADCに2[V]を入力すると，

\(256 \times {2[V] \over 5[V]} = 102.4 \simeq 102\)

という値が返ってきます．従ってこの逆算式

\(V = V_{max} \times {A \over A_{max}}\)

を通して物理値である電圧が得られますね（AはADCの出力）．更にセンサ値（今回は電流値）に変換するには電圧とセンシングしたい値との関係式をデータシートから読み取って実装すればいい訳です．今回はその関係式とはオームの法則に他ならないので， \(I=V/R\) として先程選定した抵抗値をぶち込めば晴れて電流値を読み取ることができます．

フィードバック制御

今回は速度型PI制御器を構成しました．同じPID制御でも位置型と速度型があるようで，位置型は制御量uそのものをPIDにより求めるもの，速度型は制御量uの増分をPIDにより求めるものです．今回の場合は電圧で直接電流を制御するので，以前の電流入力を保持する構造が何もありません．従って位置型にすると目標値に達した瞬間に電圧が0になり電流が急低下するといった現象が起こると考えられるので，速度型を採用する方が良さそうです．電流もいわば速度だし．

というわけで，制御量の数式は以下のようになります．

\( u_t = u_{t-1} + \Delta u, \)
\(\Delta u = K_pe_t + K_i\sum_t{e_t},\)
\(\space e_t = I_\theta -I_t \)

フィードバックゲインは限界感度法っぽい感じで適当に決めました．

制御量のPWM出力への変換

制御量をどうやってPWM出力に変換するのかも悩みどころです．制御量uは電流から生成したので，duty比が最大になった時に想定した最大電流が流れると仮定して，次の変換式に掛けました．

\(duty \space cycle = 255 \times {u_t/I_{max}}\)

255というのはArduinoのPWM出力が8bitだからです．単位がごちゃごちゃだし，あとあと考えてみると変な仮定な気がしますが，電流を電圧にしたところでゲインが変わるだけだし，これで動いたのでとりあえず良しとしましょう．

Arduinoスケッチ

CurrentFeedback.h

#ifndef CURRENTFEEDBACK_H_
#define CURRENTFEEDBACK_H_
#include "Arduino.h"

#define LOOP_MAX   20     //  アナログ入力平均化ループの周期
#define ANALOG_MAX 1024   //  アナログ入力は10bit
#define DUTY_MAX   255    //  デューティ比は8bit

//  定数
//  current_max = analog_voltage / current_resist = 2.27A
const float analog_voltage = 5.0;    //  アナログ最大電圧[V](analog = 1024)
const float current_resist = 2.2;    //  モータ電流抵抗[ohm]
const float current_max = 2.27;      //  モータ印加電流最大値[A]

class CurrentFeedback{
  protected:  
  //  Pin
  int analog_pin;     //  電流センサ
  int input1_pin;     //  正回転
  int input2_pin;     //  逆回転
  int enable_pin;     //  PWM出力
  //  内部変数
  float current_target;       //  モータ印加電流の目標値[A]
  float Kp_current;           //  電流比例フィードバックゲイン
  float Ki_current;           //  電流積分フィードバックゲイン
  float error_integral;       //  誤差積分値
  float pwm_duty;             //  PWM出力のデューティ比（-255.0 ~ 255.0）

  public:
  //  コンストラクタ
  CurrentFeedback(){

  }
  CurrentFeedback(int analog, int input1, int input2, int enable, float feedback_gain_p = 1.0, float feedback_gain_i = 1.0){
    begin(analog, input1, input2, enable, feedback_gain_p, feedback_gain_i);
  }
  //  デストラクタ
  ~CurrentFeedback(){

  }
  //  初期化関数(PIN, フィードバックゲインの登録)
  void begin(int analog, int input1, int input2, int enable, float feedback_gain_p = 1.0, float feedback_gain_i = 1.0){
    //  引数の保存
    analog_pin = analog; input1_pin = input1; input2_pin = input2; enable_pin = enable;
    Kp_current = feedback_gain_p; Ki_current = feedback_gain_i;
    //  値の初期化
    current_target = 0;
    pwm_duty = 0;
    error_integral = 0;
  }
  //  目標値の登録
  void setTargetCurrent(float target_current){
    current_target = target_current;
  }
  //  現在の測定値を読みだす
  float getCurrent(){
    //  ADCの平均出力値を算出
    float analog_mean = 0;
    for(int i=0;i<LOOP_MAX;i++){
      int a = analogRead(analog_pin);
      analog_mean += (float)a/LOOP_MAX;
    }
    //  ADC出力値から電流値への変換
    float current_sense = analog_mean / ANALOG_MAX * analog_voltage / current_resist;
    current_sense = (digitalRead(input1_pin) == HIGH)? current_sense : -current_sense;   //  印加電流の符号を決定
    return current_sense;
  }
  //  登録した目標値に向けてフィードバック制御
  void output(){
    //  sensor
    float current_sense = getCurrent();   //  モータ印加電流の実測値[A] 符号は回転方向

    //  controller
    //  フィードバックコントローラ 速度型PI制御
    float current_error = current_target - current_sense;  //  誤差
    error_integral += current_error;
    float control_input = Kp_current * current_error + Ki_current * error_integral;   //  PI制御
    pwm_duty += control_input / current_max * DUTY_MAX;  //  current -> duty cycle(-256 ~ 255)
    if(pwm_duty > DUTY_MAX) pwm_duty = DUTY_MAX;
    else if(pwm_duty < -DUTY_MAX) pwm_duty = -DUTY_MAX;
    int pwm_dst = (int)abs(pwm_duty);

    //  actuator
    //  モーター回転方向
    if(pwm_duty > 0) {
      digitalWrite(input1_pin, HIGH);
      digitalWrite(input2_pin, LOW);
    }
    else if(pwm_duty < 0) {
      digitalWrite(input1_pin, LOW);
      digitalWrite(input2_pin, HIGH);
    }
    else{
      digitalWrite(input1_pin, LOW);
      digitalWrite(input2_pin, LOW);  
    }
    analogWrite(enable_pin, pwm_dst);
  }
  //  緊急停止
  void quickStop(){
    current_target = 0;
    pwm_duty = 0;
    digitalWrite(input1_pin, HIGH);
    digitalWrite(input2_pin, HIGH);  
    analogWrite(enable_pin, 255);
  }
};

#endif  // CURRENTFEEDBACK_H_

こんな感じでクラス化してみました．Arduino IDEでクラスを書くのは初めてだし，ArduinoでC++記法がどこまで許されてるのか分からなかったのでちょっと大変でしたが，なんとかうまくいきました．やっぱC言語よりクラスが書けるC++の方が慣れると便利ですね．

動作テスト用スケッチ

#include "CurrentFeedback.h"

#define MOTOR1_INPUT1_PIN 4  // Input 1
#define MOTOR1_INPUT2_PIN 5  // Input 2
#define MOTOR1_PWM_PIN 6  // Enable 1
#define MOTOR1_CURRENT_PIN 1  // Analog 1

CurrentFeedback current1;
float current_target = 0;

void setup() {
  // put your setup code here, to run once:
  Serial.begin(19200);
  Serial.println("hello");
  current1.begin(MOTOR1_CURRENT_PIN, MOTOR1_INPUT1_PIN, MOTOR1_INPUT2_PIN, MOTOR1_PWM_PIN, 1.2, 0.4);
}

void loop() {
  // put your main code here, to run repeatedly:

  //  シリアル入力（目標値の決定）
  char c = Serial.read();
  if(c == '1'){
    if(current_target > -current_max){
      current_target -= 0.05;
    }
    else{
      current_target = -current_max;
    }
    Serial.println(current_target, DEC);
  }
  if(c == '2'){
    if(current_target < current_max){
      current_target += 0.05;
    }
    else {
      current_target = current_max;
    }
  Serial.println(current_target, DEC);
  }
  if(c == '3'){
    current_target = 0;
    Serial.println(current_target, DEC);
  }
  if(c == '4'){
    current_target = 0;
    current1.quickStop();
    Serial.println("------------STOP------------");
  }

  current1.setTargetCurrent(current_target);
  current1.output();
  Serial.print("         ");
  Serial.println(current1.getCurrent(), DEC);
}

シリアルモニタからモータの入力電流を指定してそのときの測定値を出力させるプログラムです．実行した結果は，ハンチングが激しいですが電流値は一応制御できてるっぽいです．やったぜ．

（2016/09/19修正）ADCとDACを書き間違えていたので修正