#Zynqでロボット制御します
Zynqの習作が必要だったのでロボットに使いました。
チャレンジした際の技術的な話をしていこうかと思ってます。
特に、HDLの記事は少ないようなのでその辺を中心に語っていこうかなと。
図１ お手上げの図
ZynqのボードはZynqBerryを使用しています。
http://www.trenz.jp/products/te0726/
RaspberryPiのZynq版で、GPIOがピンヘッダで出てるので都合がよいのです。
それをユニバーサル基板でRCサーボに電源と信号を与えられるように工作しました。
#役割分担
ZynqにはARMプロセッサ（ZynqではPSと呼ぶ）とFPGA（同じくPL）が入ってます。
PSとPLの役割分担ですが、PSではロボットのモーションをUARTからコンフィギュレーション用のフラッシュのコンフィギュレーション領域（PSのファームウェアとPLのビットストリーム）より後ろに格納、取り出しを行います。
PLではRCサーボの制御とRC受信器の信号取り込みをやってます。
つまり、RC系のタイミング制御をハード処理とし、ソフトは姿勢の制御に専念できるようにしました。
PSとPLの接続はAXI-GPIOです。RC系のコントロール部は元々Latticeのデバイス用に作ったものを流用したのでAXI化されてないです。もっともAXI化するメリットもあまり無いですし。
図２ ブロック図
#RCサーボコントローラ部（ServoCTRL）
まず、RCサーボの信号について下図のようなタイミング波形です。
図３ RCサーボの制御信号の図
Highレベルのパルス幅でサーボホーンの角度が決まります。
1.5msでだいたいセンターです。
信号レベルは3.3Vでよいので、Zynqから直接駆動できます。（バンク電圧が3.3Vならね！）
ちなみに受信器から出てくる信号も同様です。というか、受信器から出てくる信号がこの形っていう表現のほうが正しいかもしれませんが。
コードはVHDLで書いてます。
コード一覧は以下URLに置きました。興味があれば見てください。
https://github.com/x7700jp/x7700jp_codes
| ファイル名 | 説明 |
|---|---|
| TE0726_TOP.vhd | トップデザイン |
| ├TKSERVO.vhd | サーボ制御 |
| │├TKSERVO_ANGL.vhd | 角度パルス生成 |
| │└TKSERVO_TMG.vhd | タイミング制御 |
| └TKRCV.vhd | RC受信器信号の数値化 |
| bench_TKSERVO.vhd | TKSERVOのテストベンチ |
| bench_TKRCV.vhd | TKRCVのテストベンチ |
トップデザイン、テストベンチについては説明の必要に需要があればいずれ書こうと思います。
TKSERVOのポートの説明
entity TKSERVO is
generic (
MASTER : boolean := TRUE; -- TRUE:MASTER / FALSE:SLAVE
CW : integer := 7 ; -- PERI_GEN cnt Width
CNT : integer := 109
);
port (
iCLK : in std_logic;
iRST : in std_logic;
iANGLE : in std_logic_vector (7 downto 0);
bTMG : inout std_logic; -- MASTER TRUE : output / FALSE : input
bPERI : inout std_logic; -- MASTER TRUE : output / FALSE : input
oSERVO : out std_logic
);
入出力の説明の前にTKSERVOの概要について説明します。
TKSERVOでは与えられたアングル信号に比例したRC信号を出力します。
その際、RC信号のタイミング制御を各モジュールに持たせると非効率なのでタイミング生成は１つのモジュール（マスター）にて行い、スレーブはもらったタイミングで動作します。
■generic値について
Master：Trueでマスターになります。マスターはbTMG/bPERIが出力になります。
CW：下記CNT値の表現に必要なビット数（下記CNT=109なので7ビット）
CNT：bTMGのタイミングカウンタ値
決め方は
$CNT=0.02/(3656*PERI)$ ただし、PERI:iCLKの周期
例：iCLK=20MHz(PERI=50ns)時 $CNT=0.02/(36565010^{-9})=109.5$
※なんだってこんなややこし計算にしたかというと、前述した流用元では都合が良かったのです。たしか。
■ポートについて
iCLK ： クロック入力（立ち上がりエッジ）
iRST ： リセット入力（1でリセット）
iANGLE ： サーボホーンの回転角度（-127～+127）
bTMG ： タイミング信号の入出力（マスター時出力）
bPERI ： タイミング信号の入出力（マスター時出力）
oSERVO ： RC信号出力
■シミュレーション結果
図４ シミュレーション結果
それらしく動いてます。
#次回予告と雑記
###次回予告
RC受信器信号の数値化についてを予定してます。
これはマイコンでやるよりFPGAのほうが良い例となります。
とくに複数チャネルではすごくすっきりします。
###雑記
RC信号で動くRCサーボにはアナログ方式とデジタル方式の2種類があります。
信号そのものは変わらないのですが、その取り扱いが変わります。
特にアナログ方式はパルス間隔（図３でいうところの20msのところ）が開くと保持トルクが変わりました。デジタル方式はどうやら値をラッチするようです。
従って、マルチプレクサで制御しているようなコントローラの場合は制御数が多いとアナログ方式のRCサーボは特性低下する場合があるかもしれません。気をつけましょう。
その点、Zynqであれば、制御タイミングは緻密にできますし、パルス間隔を任意に変動させて保持トルクもコントロールできます。それが有用かどうかはともかくね。
では次回。