サーボモータの制御 Servo Motor Control

FPGAでプロポーショナルシステム用（通称：プロポ）サーボモータを制御してみます。

日本製だとフタバ製が有名ですが、様々なサイズや回転角のものが各社から販売されています。

例えば

http://www.hobbyking.com/hobbyking/store/__84__189__Servos_Parts-All_Servos.html

http://www.gws.com.tw/english/product/servo/servo.htm

形状は多種多様ですが、これらのサーボモータの制御はPWMで行います

厳密な統一規格があるわけではないのですが、それなりというか程々の互換性はあるようです。

50kHzのクロック生成

周期：約20msec = 50Hz

パルス幅分解能：50kHz=20usecで作成してみます。

50kHzのクロック生成verilog:

module clk50khz(clkin50mhz,clkout);

input clkinmhz;

output reg clkout;

reg [9:0] counter;

always @(negedge clkin50mhz)

begin

if (counter > 500) begin

counter <= 0;

clkout <= ~clkout;

end

else begin

counter <= counter + 1;

end

end

endmodule

pwm出力

手近にあった、サーボモータでテストしたところ、

cntinの値が0x2C=44だと、垂直の位置になり、０x50=80で、水平の位置になりました。

これより、80-44=36, 90/36=2.5なので、50usec毎にパルス幅を変更できれば、1度づつ位置を調整できることになります。

pwm出力verilog:

module servo_control(clkin, rstn, cntin, pwmout);

input clkin;

input rstn;

input [11:0]cntin;

output reg pwmout;

reg [11:0]counter;

always@(negedge clkin or negedge rstn)

begin

if (rstn == 0) begin

counter = 0;

pwmout = 1;

end

else begin

if (counter > 999) begin

pwmout = 1;

counter = 0;

end

else if (counter > cntin) begin

pwmout = 0;

end

counter = counter + 1;

end

end

endmodule