ZYBOを使ったモータ制御 (1)

##はじめに
ZYBOを使ってモーションコントローラの設計を目指しています。
これまでにZYBOでDA変換器や割り込みを使用した時間管理を試しました。
https://qiita.com/tvrcw/items/61c0cce9a9c8e843def4
https://qiita.com/tvrcw/items/67074cb41668a0c482da
これらに加えてモータの情報を取り込むことができれば制御系を組むことができます。今回は安川電機のサーボドライバシグマVを使用してACモータを回すことを目的として，パルスカウンタの設計を行います。また，実際に簡単な制御を行ってみます。

###パルスカウンタの設計
安川サーボドライバはユーザ側にABZ相パルスの差動出力を送ります。ここにレシーバとカウンタを設置することでモータの回転角情報を取得することができます。今回はラインドライバAM26LS32ACDRを載せた基板を用意し，FPGAでパルスをカウントします。AXIバスIPを用意して次のようなコードをユーザ回路部分に追記しました。

``` PulseCounter.vhd
	input wire epc_a,epc_b;

	reg signed [31:0] epc_p;
	reg signed [31:0] epc_ap,epc_an,epc_bp,epc_bn;
	reg signed [31:0] epc_pn;
	reg signed [31:0] epc_psum;
	reg [3:0] epc_cnt;	initial epc_cnt=4'b0;
	localparam [3:0] epc_cnt_max=4'h9;

	always@(posedge clk) begin
		if(ena_epc!=1'b1) begin
			epc_pn<=31'h0;
			epc_psum<=31'h0;
			epc_p<=31'h0;
			epc_cnt<=4'h0;
		end
		else begin
			epc_pn<=epc_ap+epc_an+epc_bp+epc_bn;
			if(epc_cnt==4'h0) epc_psum<=epc_pn; else epc_psum<=epc_psum+epc_pn;
			if(epc_cnt==4'h8) epc_p<=(epc_psum[31]==1'b0)? {4'b0000,epc_psum[30:3]}:{4'b1111,epc_psum[30:3]};
			epc_cnt<=(epc_cnt==epc_cnt_max)? 4'h0:epc_cnt+4'h1;
		end 
	end

	always@(posedge epc_a) begin if(ena_epc!=1'b1) epc_ap<=31'b0; else epc_ap<=(epc_b==1'b0)? epc_ap+32'h1:epc_ap-32'h1; end
	always@(negedge epc_a) begin if(ena_epc!=1'b1) epc_an<=31'b0; else epc_an<=(epc_b==1'b1)? epc_an+32'h1:epc_an-32'h1; end
	always@(posedge epc_b) begin if(ena_epc!=1'b1) epc_bp<=31'b0; else epc_bp<=(epc_a==1'b1)? epc_bp+32'h1:epc_bp-32'h1; end
	always@(negedge epc_b) begin if(ena_epc!=1'b1) epc_bn<=31'b0; else epc_bn<=(epc_a==1'b0)? epc_bn+32'h1:epc_bn-32'h1; end

	wire signed [31:0] si;
	reg [31:0] sf;		initial sf=32'b0;
	reg 	   si_sign;	initial si_sign=1'b0;
	reg [30:0] si_norm;	initial si_norm=32'b0;

	assign si=epc_p;	
	always@(posedge clk)
	begin
		si_sign<=(si[31]==1'b1)? 1'b1:1'b0;
		si_norm<=(si[31]==1'b1)? (~si[30:0])+31'b1:si[30:0];
		
		if		(si_norm[30]==1'b1)	sf<={si_sign,8'h9d,si_norm[29:7]};
		else if	(si_norm[29]==1'b1)	sf<={si_sign,8'h9c,si_norm[28:6]};
		else if	(si_norm[28]==1'b1)	sf<={si_sign,8'h9b,si_norm[27:5]};
		else if	(si_norm[27]==1'b1)	sf<={si_sign,8'h9a,si_norm[26:4]};
		else if	(si_norm[26]==1'b1)	sf<={si_sign,8'h99,si_norm[25:3]};
		else if	(si_norm[25]==1'b1)	sf<={si_sign,8'h98,si_norm[24:2]};
		else if	(si_norm[24]==1'b1)	sf<={si_sign,8'h97,si_norm[23:1]};
		else if	(si_norm[23]==1'b1)	sf<={si_sign,8'h96,si_norm[22:0]};
		else if	(si_norm[22]==1'b1)	sf<={si_sign,8'h95,si_norm[21:0],1'b0};
		else if	(si_norm[21]==1'b1)	sf<={si_sign,8'h94,si_norm[20:0],2'b0};
		else if	(si_norm[20]==1'b1)	sf<={si_sign,8'h93,si_norm[19:0],3'b0};
		else if	(si_norm[19]==1'b1)	sf<={si_sign,8'h92,si_norm[18:0],4'b0};
		else if	(si_norm[18]==1'b1)	sf<={si_sign,8'h91,si_norm[17:0],5'b0};
		else if	(si_norm[17]==1'b1)	sf<={si_sign,8'h90,si_norm[16:0],6'b0};
		else if	(si_norm[16]==1'b1)	sf<={si_sign,8'h8f,si_norm[15:0],7'b0};
		else if	(si_norm[15]==1'b1)	sf<={si_sign,8'h8e,si_norm[14:0],8'b0};
		else if	(si_norm[14]==1'b1)	sf<={si_sign,8'h8d,si_norm[13:0],9'b0};
		else if	(si_norm[13]==1'b1)	sf<={si_sign,8'h8c,si_norm[12:0],10'b0};
		else if	(si_norm[12]==1'b1)	sf<={si_sign,8'h8b,si_norm[11:0],11'b0};
		else if	(si_norm[11]==1'b1)	sf<={si_sign,8'h8a,si_norm[10:0],12'b0};
		else if	(si_norm[10]==1'b1)	sf<={si_sign,8'h89,si_norm[9:0],13'b0};
		else if	(si_norm[9]==1'b1)	sf<={si_sign,8'h88,si_norm[8:0],14'b0};
		else if	(si_norm[8]==1'b1)	sf<={si_sign,8'h87,si_norm[7:0],15'b0};
		else if	(si_norm[7]==1'b1)	sf<={si_sign,8'h86,si_norm[6:0],16'b0};
		else if	(si_norm[6]==1'b1)	sf<={si_sign,8'h85,si_norm[5:0],17'b0};
		else if	(si_norm[5]==1'b1)	sf<={si_sign,8'h84,si_norm[4:0],18'b0};
		else if	(si_norm[4]==1'b1)	sf<={si_sign,8'h83,si_norm[3:0],19'b0};
		else if	(si_norm[3]==1'b1)	sf<={si_sign,8'h82,si_norm[2:0],20'b0};
		else if	(si_norm[2]==1'b1)	sf<={si_sign,8'h81,si_norm[1:0],21'b0};
		else if	(si_norm[1]==1'b1)	sf<={si_sign,8'h80,si_norm[0],22'b0};
		else if	(si_norm[0]==1'b1)	sf<={si_sign,8'h7f,23'b0};
		else						sf<=32'b0;
	end
```
(Qiitaではverilog拡張子.vだとコードに色がつかないためVHDL拡張子.vhdとしています。)

パルスカウンタはFIRの移動平均付きです。パルスカウンタは高速に回るのですが，制御周期中に暇そうなので移動平均を取って制御演算器に渡すことにしています。

パルスカウンタの値を読みとるついでに，PS-PL間で浮動小数点の受け渡しをテストしようと思い，符号付き整数から単精度浮動小数点への変換回路を作成しました(AXIバスの幅的に単精度が楽なので)。この符号付き整数と単精度浮動小数点のカウント値をLinuxに渡して読んでみようと思います。

**この回路の制約ファイルを書くときの注意があります。always@(posedge epc_a)とalways@(posedge epc_b)を使える様にするためには，.xdcに以下を追記する必要があります。

```constraints.xdc
set_property CLOCK_DEDICATED_ROUTE FALSE [get_nets epc_a_0]
set_property CLOCK_DEDICATED_ROUTE FALSE [get_nets epc_b_0]
```

###Linux側のコード
制御演算は制御入力のような大きい値からエンコーダ分解能のような小さな値まで幅広いダイナミックレンジが要求されるため，固定小数点演算では丸め込みが大きくなったりするようです。ダイナミックレンジを大きく取って多ビットの固定小数点を用意すると，却って浮動小数点の方がコードが小さくなったりします。そこで，浮動小数点をPL側で使用したいと思っています。そこで，PS側のプログラムからPLに浮動小数点を渡したいのですが，どのように浮動小数点形式の32ビット信号をPLに渡すかが問題になりました。これは共用体を使って解決します。

```Controller.cc
typedef union { float f; int i; } ufloat;
```
C++はアドレスに書き込むときint型が一番素直に動作すると思います。単にfloat型のデータをアドレスに渡すのではなく，XX(変数).fに値を代入した後にXX.iを書き込むと上手くいきます。エンコーダの値を読むときには，次のようにint型で受け取ると，PLから渡されるビットを読み取れます。

```Controller.cc
	if((uio_fd=open("/dev/uio0",O_RDWR))==-1){
		printf("Can not open /dev/uio0\n"); exit(1);
	}
	map_addr=(unsigned int*)mmap(NULL,0x1000,PROT_READ|PROT_WRITE,MAP_SHARED,uio_fd,0);

	static ufloat fpga_Xres1 = {.f=0.0};
	static ufloat fpga_Xres2 = {.f=0.0};

	fpga_Xres1.i	= map_addr[2];	//自分で設定したアドレス　符号付き整数
	fpga_Xres2.i	= map_addr[3];	//自分で設定したアドレス　単精度浮動小数点
```

この段階で，fpga_Xres1には符号付き整数の32ビット，fpga_Xres2には単精度浮動小数点の32ビットが入っています。そこで，次のようにパルスカウント数にエンコーダ分解能をかけて回転角に換算します。

```Controller.cc
	const double EPC_RESOLUTION = 2.0*M_PI/pow(2,20)	//20bitエンコーダの分解能

	fpga_Xres1.f  = fpga_Xres1.i*EPC_RESOLUTION;	
	fpga_Xres2.f  = fpga_Xres2.f*EPC_RESOLUTION;	
```
fpga_Xres1ではint型のパルス数が入っているためint型とdouble型の掛け算，fpga_Xres2ではfloat型のパルス数が入っているためfloat型とdouble型の掛け算をしています。モータを一周させてみた時の二つの応答を確認すると，同じ結果になります。どちらも6.28 radくらい動いています。

![res_mawashi.png](https://qiita-image-store.s3.amazonaws.com/0/43995/ad2f81c2-e65a-1360-e6e7-84a95ded4b8f.png)

エンコーダが読んでいるのがわかったので，実際にモータの角度を制御しました。以前に作ったDA変換器を使ってサーボドライバにトルク指令を送ります。発生トルクの式は一行です。

```Controller.cc
	static float Xref;
	static ufloat fpga_Xref;

	Xref=20.0*(0.15-fpga_Xres1.f);	//発生トルクの式
	fpga_Xref.i=(unsigned int) (Xref/(20.0/pow(2,16))+pow(2,15));
	map_addr[1]=fpga_Xref.i;	//DA変換器の指令値用に設定したアドレス
```

P制御と呼ばれるものです。ダンピング項の付加は面倒なのでモータの摩擦に任せます。

![res.png](https://qiita-image-store.s3.amazonaws.com/0/43995/13a035dc-b0ff-cf93-c3d5-75cb1e4b8427.png)

P制御なので振動しますし摩擦によるオフセットが乗りましたが，とりあえず角度の制御ができていました。今回は簡単の確認のため制御演算をPSで行いましたが，次はPLに制御演算回路を設置します。