制御系の主な機能は以下の4点です。
センサ回路から本体傾斜角θと本体角速度ωを得ます。また,DCサーボモータに付属するロータリエンコーダから車輪並進速度vを得ます。 これらの入力に基づいて倒立制御式がトルク指令u(k)を計算し,モータドライバ回路がモータへ出力を行います。 所望のモータ電流を正確かつ応答性良く得るため,モータ出力部にはPI制御による電流フィードバック系を構築しました。 無線コントローラはジョイスティック(可変抵抗)を搭載し,AD変換値をシリアルデータで送信します。 マイコン(SH/7125)はSCIでそれを受信し,目標値vdに変換して倒立制御式に与えます。
メイン回路は以下のように設計製作しました。
メイン回路は,信号処理を担うマイコンと各回路を接続する役割を持ちます。マイコンはSH/7125(ルネサステクノロジ)を使用し,C言語により開発を行いました。 開発環境は,ルネサステクノロジが提供する統合開発環境HEWを利用しました。デバックにはPCとのシリアル通信を用い,マイコンの内部情報をPCで確認しながらプログラミングを進めました。
SH/7125の詳しい使い方やC言語についてはweb上や書籍に情報が溢れているので割愛します・・・というか,そこまで説明するのたるいorz
プログラムの全体構造はシンプルで,起動SWを押すと倒立制御に関する各タイマ割り込み処理の動作を許可して永久ループに入ります。 動作の停止は停止SWを押すことでなされ,再度倒立動作をさせる場合はメイン回路のリセットスイッチを押す仕様です。
倒立振御を行う各割り込み処理は,以下のように実装しました。
離散時間のフィードバック制御を行うために,SH/7125のタイマ割り込み機能を用いて,各処理を一定時間間隔で起動します。図7.6に示した@〜Cの数字は,無線コントローラからデータが入力されてから計算結果がPWM dutyに反映されるまでの順序です。 無線コントローラからのデータ は,9600[bps]のシリアル通信によって75[ms]間隔で倒立振子本体へ送信されます。そのデータは126[ms]間隔で起動するWDT(Watch Dog Timer)による割り込み処理で監視され,速度目標値vdが設定されます。 すると,その値を使って倒立制御処理が10[ms]間隔で出力トルクu(k)を計算し,さらに短い160[μs]間隔でPI制御処理がPWM DutyであるU(n)を計算します。
TPIは小さく設定するほどモータ電流の応答性を高めることができる。しかし,PI制御処理時間よりも大きく設定しなければ,他の処理を同時に実行することができない。そこで,TPIをPI制御処理時間(実測約50[μs])の約3倍である160[μs]に設定することにした。
TAの決定方法は,適当である。製作した倒立振子は質量が5[kg]あるので,10[Hz]を超えるような振動は現れないと考えた。そこで,TAを十分小さな10[ms](サンプリング周波数:100[Hz])に決定した。
割り込み周期の長い処理が短い処理を妨げないよう,またリアルタイムに扱うべき処理を考慮し,SH/7125の機能を利用して割り込み優先度を設定した。
例えば,SCI処理は倒立制御処理よりも起動周期が長い。しかし,実行時間が約300[μs]である倒立制御処理を優先させると,シリアルデータの受信を取りこぼす可能性があるため,優先度を高く設定している。