DCモータの速度を制御するコードをマイクロコントローラに配備する
はじめに
DCモータは制御しやすい．このプロジェクトで使用するDCモータは入力電圧で制御できる．電圧が高くなると速くなる．
しかし，モータの負荷が変化するような場合には状況が複雑になる．希望の速度を保つためには電圧をその場で調整する必要がある．そのため，実際の速度を測定し，希望の速度と比較し，モータに適切な電圧を入力する制御器が必要となる．
このプロジェクトでは，Arduinoマイクロコントローラに速度制御器を配備する．モータの実際の速度はエンコーダを使って測定する．希望の速度はポテンショメータを使って設定する．モータはモータシールドで駆動される．基準速度と実際の速度はシリアルRS-232バスで送信され，デバイスフレームワークを使って読み取る．
このプロジェクトでは，まずDCモータのモデルを得て，制御器を設計する方法を示す．次にデジタルのエンコーダ信号とアナログのポテンショメータの信号を読み取り，希望の速度を保つためにシリアルデータとアナログPWM（パルス幅変調）モータ信号を出力する方法を示す．
制御器を使うことで，系を扱う方法も変わる．制御器がない場合は直接電圧を使って系とやりとりする．制御器がある場合は，制御器に希望の速度を指定し，あとは制御器に任せる．
部品
Pololu 9.7:1メタルギアモータ25Dx48L mm LP 6V，48 CPRエンコーダ付き[リンク]
Pololu 25D mmメタルギアモータブラケット[リンク]
Pololuアルミ製ユニバーサルマウントハブ，4mm軸用 ネジ穴#4-40[リンク]
Pololuホイール60×8mm [リンク]および2 #4-40 1/2"機械ねじ2個[リンク]
木製立方体ブロック2個[リンク]，#4-40 1/4"機械ねじ4〜6個[リンク]，長方形粘着マウントテープ[リンク]
Arduino Uno [リンク]
Arduinoモータシールド[リンク]
ProtoShield [リンク]
ポテンショメータ[リンク]
ワイヤ各種[リンク]
6ピンオスヘッダ[リンク]
USBケーブル[リンク]
電源アダプタ[リンク]
ProtoShield，ワイヤ，ヘッダの代りに，はんだ付け不要ブレッドボード[リンク]とオス-オスジャンパー線[リンク]でもよい．
組立て
2つの立方体ブロックを接着し，振動を防ぐためにブロックの底の四隅に長方形粘着マウントテープを4片貼り，1/4”ねじを使ってブロック上にブラケットを取り付ける：
ブラケットに付属のM3ねじでGearmotorブラケットにモータを取り付ける：
マウントハブに付属のねじと六角レンチを使って，モータの軸にマウントハブを取り付ける：
マウントハブにホイールを1/2”ねじで取り付ける：
モータシールド，モータヘッダ，ポテンショメータをワイヤで接続する（これはProtoShieldを使ってもブレッドボードを使ってもできる）：
（モータヘッダからのワイヤの色は図の中の色と合致している．）
モータシールドとProtoShieldをArduinoボードの上に積み重ねる：
積み重ねたボードをヘッダを使ってモータに接続する：
DCモータのモデル
入力が電圧，出力が速度であるDCモータの入出力モデルは一次系である．一次系の2つのパラメータはゲインと時定数である．データを送信してデータをモータから受け取り，これらのパラメータを求める．
データを取得するだけなので，サンプリング周期はそれほど重要ではない．
sp = 1;モータはギア比が9.68:1であり，エンコーダは1回転で12回カウントする．これを出力軸の回転毎分(rpm)に変換する．
encoderS = (60 /12 9.68sp)デューティ比を入力とし，それをモータに送るモデルを作成する．これはエンコーダの信号も読み取って送信する．
入力のデューティ比はシリアル上で送信し，速度はエンコーダの立ち上がりエッジを使って取得する．
ics = {0 -> "Serial", 2 -> "RisingEdgeCount"};モデルの出力はモータのデューティ比と速度で，シリアル上で送信される．
ocs = {11 -> <|"Type" -> "Analog", "MaxValue" -> 100|>, 1 -> "Serial"};モデルはシリアルのデューティ比信号をPWM信号として送信する．2つ目の入力はエンコーダの立ち上がりエッジのカウントで，これはrpmに変換され，シリアル上で送り返される．
pwmToSpeed = StateSpaceModel[{{}, {}, {}, {{1, 0}, {0, encoderS}}}, SamplingPeriod -> sp]モデルを埋め込んだら，Arduinoはシリアルデューティ比の値を読み取り，結果のモータ速度を送り返す．
ℳ = MicrocontrollerEmbedCode[pwmToSpeed, <|"Target" -> "ArduinoUno", "Inputs" -> ics, "Outputs" -> ocs|>, "/dev/tty.usbmodem14121"]マイクロコントローラへの接続を開く．
dev = DeviceOpen["Serial", "/dev/tty.usbmodem14121"]受信した文字を式に変換する関数．
fromASCII[ls_] := ToExpression[FromCharacterCode[Rest[ls]]]速度を受信してデューティ比を送信する，スケジュールされたタスクを開始する．
rpm = {0};
task = SessionSubmit@ScheduledTask[
If[DeviceExecute[dev, "SerialReadyQ"],
AppendTo[rpm, DeviceReadBuffer[dev, "ReadTerminator" -> ℳ["Serial"]["EndByte"]]];DeviceExecute[dev, "ReadFlush"];
rpmL = fromASCII[Last[rpm]];
If[Length[rpm] > 200, rpm = Drop[rpm, 150] ];DeviceWrite[dev, ℳ["Serial"]["StartByte"]];DeviceWrite[dev, ToString@dutyCycle];DeviceWrite[dev, ℳ["Serial"]["EndByte"]]
],
sp];デューティ比を設定し，モータ速度を読み取る2つのゲージを作成する．
Grid[{{AngularGauge[Dynamic[dutyCycle], {0, 100}, GaugeLabels -> Automatic], AngularGauge[Dynamic[rpmL], {0, 650}, GaugeLabels -> Automatic]}}]デューティ比を設定し，速度が安定するまで待ってから記録する．また，各デューティ比において，モータにかけられた実際の電圧を測る．これはモータシールドのターミナルBの電圧である．
motorData = IconizedObject[«motor data»];Grid[Join[{{"デューティ比(%)", "速度(rpm)", "電圧 (v)"}}, motorData], Frame -> True, Background -> {None, {{LightOrange, White}}}]Out[13]=
| | | |
| ----------- | --------- | ------- |
| "デューティ比(%)" | "速度(rpm)" | "電圧(v)" |
| 0 | 0 | 0 |
| 10 | 89.876 | 1.07 |
| 20 | 318.182 | 3.39 |
| 25 | 387.087 | 4.07 |
| 30 | 442.562 | 4.63 |
| 35 | 491.322 | 5.06 |
| 40 | 527.169 | 5.4 |
| 45 | 553.926 | 5.62 |
| 50 | 572.004 | 5.81 |
| 55 | 592.562 | 5.99 |
| 60 | 607.438 | 6.12 |
| 65 | 619.008 | 6.23 |
| 70 | 623.45 | 6.32 |
| 75 | 634.298 | 6.4 |
| 80 | 743.802 | 6.46 |
| 90 | 634.091 | 6.48 |
| 95 | 640.496 | 6.56 |
| 100 | 651.86 | 6.66 |タスクを停止し，マイクロコントローラへの接続を閉じる．
TaskRemove[task];
DeviceClose[dev]測定した電圧に対する速度をプロットする．
ListLinePlot[motorData[[All, {3, 2}]], FrameLabel -> {"電圧(v)", "速度(rpm)"}, Frame -> True, GridLines -> Automatic, ImageSize -> Medium]異常データ点が1点ある以外，プロットは線形であることが分かる．
ListLinePlot[Drop[motorData[[All, {3, 2}]], {-4}], FrameLabel -> {"電圧(v)", "速度(rpm)"}, Frame -> True, GridLines -> Automatic, ImageSize -> Medium]オフセットを無視して，モータのゲインをプロットの傾きで設定する．
LinearModelFit[Drop[motorData[[All, {3, 2}]], {-4}], v, v]
gain = D[%[v], v]モータの時定数を求めるために，ステップ入力に対するモータの応答を見る．Arduinoへの接続を開き，スケジュールされたタスクを再開する．急激にデューティ比を増やし，速度が安定するまで待つ．接続を閉じ，スケジュールされたタスクを終了する．
モータの速度応答をプロットし，初期速度で始まって最終速度で安定するまでの部分を拡大する．
ListLinePlot[fromASCII /@ Rest[rpm], GridLines -> Automatic, Frame -> True, FrameLabel -> {"時間(秒)", "速度(rpm)"}, PlotRange -> {{5, 11}, All}]モータが定常状態の値に達するのに5秒かかっていることが分かる．整定時間は時定数のおおよそ4倍なので，時定数を推定することができる．
τ = 4 / 5.0;入力が電圧で出力が速度のモータのモデル．
tfm = TransferFunctionModel[(gain/1 + τ s), s]モータシールドのモデル
モータとモデルへの実際の入力は電圧であるが，Arduinoは電圧信号を送信しない．Arduinoはデューティ比を送信する．実際の電圧はモータシールドによってかけられる．そのため，電圧からデューティ比への変換を知る必要がある．このデューティ比がモータシールドによって電圧として送信されるのである．
電圧に対するデューティ比のプロット．
voltVsDC = ListLinePlot[motorData[[All, {3, 1}]], FrameLabel -> {"電圧(v)", "デューティ比(%)"}, Frame -> True, GridLines -> Automatic, ImageSize -> Medium]区分関数を使ってこれを近似する．
Flatten[{1, Position[motorData, {10 | 20 | 40 | 55 | 75, _, _}], -1}];
Short[splitMotorData = Take[motorData, #] & /@ Partition[%, 2, 1], 2]データをフィットするための関数．
nonlinearFit[data_] := Module[{data31 = data[[All, {3, 1}]], c0, c1, c2},
{data31[[1, 1]] <= v < data31[[-1, 1]], NonlinearModelFit[data31, c0 + c1 v + c2 v^2, {c0, c1, c2 }, v][v]//Chop}]フィットする．
pElecFit = Which@@Flatten[Join[nonlinearFit /@ splitMotorData, {{motorData[[-1, -1]] <= v, 100}, {True, 0}}]]測定されたデータとフィットしたデータ．
Show[voltVsDC, Plot[pElecFit, {v, 0, 7}, PlotStyle -> Orange]]シールド上の逆電圧変換モデル．制御器が使用するサンプリング周期は後で求める．
sp=.
ishield = NonlinearStateSpaceModel[{{}, pElecFit}, {}, v, SamplingPeriod -> sp]制御器の設計と配備
サンプリング周期が0.25秒のPID制御器を設計する．
sp = 0.25;PIDTune[tfm, Automatic, "Feedback"];
pid = ToDiscreteTimeModel[%, sp]エンコーダの値をrpmで読み取るモデル．
encoderScaler = StateSpaceModel[{{}, {}, {}, {{(60 /12 9.68sp)}}}, SamplingPeriod -> sp]基準速度はポテンショメータで設定され，これは0Vから5Vまでの範囲である．これを0rpmから500rpmにスケールする．
potScaler = StateSpaceModel[{{}, {}, {}, {{(500/5)}}}, SamplingPeriod -> sp, SystemsModelLabels -> None];基準速度とエンコーダの実際の速度を比較する比較器．
comparator = StateSpaceModel[{{}, {}, {}, {{1, -1}}}, SamplingPeriod -> sp, SystemsModelLabels -> None];制御器は2入力3出力である．
1つ目の入力は基準速度で，ポテンショメータの値で設定する．2つ目の入力はエンコーダが測定する実際の速度である．
ics = {"A5" -> "Analog", 2 -> "RisingEdgeCount"};1つ目の出力はモータを駆動するPWM信号のデューティ比で，他の2つの出力はシリアルRS-232接続上で送信される基準速度と実際の速度である．
ocs = {11 -> <|"Type" -> "Analog", "MaxValue" -> 100|>, 1 -> "Serial", 1 -> "Serial"};制御器全体．
controller = SystemsConnectionsModel[{potScaler, comparator, pid, ishield, encoderScaler}, {{1, 1}{2, 1}, {2, 1}{3, 1}, {3, 1}{4, 1}, {5, 1}{2, 2}}, {{1, 1}, {5, 1}}, {{4, 1}, {1, 1}, {5, 1}}]パッケージをロードする．
Needs["MicrocontrollerKit`"]コードを配備する．
ℳ = MicrocontrollerEmbedCode[controller, <|"Target" -> "ArduinoUno", "Inputs" -> ics, "Outputs" -> ocs|>, "/dev/tty.usbmodem14121"]検証
ターゲットへのシリアル接続を開く．
dv = DeviceOpen["Serial", "/dev/tty.usbmodem14121"]シリアル接続から受信する実際の速度と基準速度の値を解釈する関数を作成する．
parseData[{ℳ["Serial"]["StartByte"], refs__, ℳ["Serial"]["DelimiterByte"], acts__}] := ToExpression@FromCharacterCode@#& /@ {{refs}, {acts}}
parseData[___] := {0, 0}値を0.25秒ごとに読み取るスケジュールされたタスクを設定する．
y = {};
task = SessionSubmit@ScheduledTask[
If[DeviceExecute[dv, "SerialReadyQ"],
AppendTo[y, parseData@DeviceReadBuffer[dv, "ReadTerminator" -> ℳ["Serial"]["EndByte"]]];DeviceExecute[dv, "ReadFlush"];
],
0.25
];基準値と外乱を設定するために，応答を示す結果をプロットする．
ListLinePlot[y, PlotRange -> {All, {0, 550}}, PlotLegends -> {"基準速度", "実際の速度"}]スケジュールされたタスクとデバイス接続を終了する．
TaskRemove[task];
DeviceClose[dv];制御器の性能は妥当であることが分かる．
発展
- 開ループ制御器を実装し，違いを比較する．
- 他の閉ループ設計を試す．
- 速度を時計回りと反時計回りの両方で制御する．
- サンプリング周期を増減して，性能に影響が出るかどうかを見る．
- シリアル通信によってコードのサイズがどれだけ小さくなっているのかを確認する．