在本次调试FPGA和双舵机的抓取机构时,我们遇到了这样一个问题:即如何使双舵机的机械结构平稳地运行既定的轨迹,该双舵机的抓取机构类似于如下图所示的二连杆:
两个舵机分别安装在肩部和肘部,腕部无舵机。
我们需要这个机构完成的功能为:在收到相关开始信号后,机构能自行地执行一系列动作,按照既定轨迹抓取垃圾桶并复位,再传回已完成信号,最终我们做出来的结果(也即我们的预期结果)大致如下图所示:
舵机的转动方向是根据信号线所给信号的占空比来确定的,双舵机仅需要两个占空比值就可以确定这时候抓取装置的形态,所以简而言之,我们的FPGA需要做的任务就是从某时刻开始,按照一定的时间间隔,输出不同的状态(两个占空比值)
很自然的,我们可以提取出 抓取装置运行的轨迹 上的几个关键点,抓取装置逐个沿直线到达这些关键点,就可以完成这项功能,在本次实验中,取了8个关键点,也即8种状态。
怎样逐个地到达这8种状态呢?
最简单的方法当然是直接计时,用一个变量counter记录clk地下降沿,到达一定的时间,进入一定的状态,比如:2s时,设置两个占空比的值使抓取装置进入第一个状态,4s时,修改两个占空比的值进入第二个状态……
但是这种方法在实际调试时存在非常大的困难,主要问题在于,我们每种状态的舵机参数需要实际调试(即修改占空比的值,观察装置的状态)才能得出,而且分离的舵机测得的角度-占空比对应关系在装置组合在一起后,由于装置安装的误差等原因是无法直接使用的。
所以,运用这种方法,将导致调试每个状态的参数时都将按照时序运行一遍八个状态,这有极大可能破坏该装置,别问我为什么知道。
为了方便调试,我采取了这样的方法,首先分离时序电路,计时的模块单独计时八个状态的时间,只是将输出改为输出三位二进制编码:
module green_code_flow(clk_1us,flag,run,zhua,shou,result);
input clk_1us,flag;
output run,zhua,shou,result;
reg run,zhua,shou,result;
reg [40:0] counter; //
reg [40:0] step;
initial
begin
counter <= 0;
result = 0;
end
always@ (posedge clk_1us)
begin
step=1000000;
if(flag == 1)
begin
counter = counter+1;
if (counter == step *1) //1s
begin
run = 0;
zhua = 0;
shou = 0;
end
if (counter == step *2)
begin
run = 0;
zhua = 0;
shou = 1;
end
if (counter == step *3)
begin
run = 0;
zhua = 1;
shou = 1;
end
if (counter == step *4)
begin
run = 0;
zhua = 1;
shou = 0;
end
if (counter == step *5)
begin
run = 1;
zhua = 1;
shou = 0;
end
if (counter == step *6)
begin
run = 1;
zhua = 1;
shou = 1;
end
if (counter == step *7)
begin
run = 1;
zhua = 0;
shou = 1;
end
if (counter == step *8)
begin
run = 1;
zhua = 0;
shou = 0;
end
if (counter > step *9)
begin
run = 0;
zhua = 0;
shou = 0;
counter = counter -1;
result=1;
end
end
else
begin
counter =0;
result =0;
end
end
endmodule
上述是格雷码的输出模块,主要功能就是按时序输出三位格雷码。
module CatchControl(clk1,run,zhua,shou,DJ_shang_percent,DJ_xia_percent);
input clk1;
input run;
input zhua;
input shou;
output DJ_xia_percent;
output DJ_shang_percent;
reg [7:0]DJ_xia_percent;
reg [7:0]DJ_shang_percent;
always@ (posedge clk1)
begin
if(run == 0 && zhua == 0 && shou == 1)
begin
DJ_shang_percent <= 28;
DJ_xia_percent <= 21;
end
if(run == 0 && zhua == 1 && shou == 1)
begin
DJ_shang_percent <= 36;
DJ_xia_percent <= 21;
end
if(run == 0 && zhua == 1 && shou == 0)
begin
DJ_shang_percent <= 40;
DJ_xia_percent <= 21;
end
if(run == 1 && zhua == 1 && shou == 0)
begin
DJ_shang_percent <= 31;
DJ_xia_percent <= 27;
end
if(run == 1 && zhua == 1 && shou == 1)
begin
DJ_shang_percent <= 22;
DJ_xia_percent <= 36;
end
if(run == 1 && zhua == 0 && shou == 1)
begin
DJ_shang_percent <= 8;
DJ_xia_percent <= 46;
end
if(run == 1 && zhua == 0 && shou == 0)
begin
DJ_shang_percent <= 8;
DJ_xia_percent <= 36;
end
if(run == 0 && zhua == 0 && shou == 0)
begin
DJ_shang_percent <= 12;
DJ_xia_percent <= 36;
end
end
endmodule
上述是舵机根据格雷码而调整各个参数的模块,主要功能是根据输入的格雷码,而输出不同状态下的舵机占空比。
为什么要采用格雷码呢?
这样会使调试更加方便,我们将时序电路和执行电路分离开来以后,可以对它们分别进行调试;
对时序电路,可以将三个输出分别接入FPGA上的小灯,根据灯亮来判断一个操作的时间是否合适,应该确立多大的时间间隔;
对执行电路,我们可以将三个输入分别接上FPGA的三个拨码开关,从第一个状态开始,按顺序改变一个拨码开关的值即可得到下一个状态,并可以随时拨回回到上一个状态,这样,我们在调试中就可以单独调试两个状态变换的中间过程是否会出问题(例如撞到小车上其他装置等),并修改参数。
附:FPGA的steer模块,即利用PWM_percent给出舵机驱动信号的模块:
module steer(clk,PWM_out,PWM_percent);
input clk;//clk=50M=50 000 000,
input [7:0]PWM_percent;
output PWM_out; //led
reg [18:0] pwm,counter; //PMW脉宽计数,周期计数
reg PWM_out;
always@ (posedge clk)
begin
counter <= counter+1;
if ( counter <= PWM_percent*2500)
PWM_out <= 1;
else
PWM_out <= 0;
if (counter >=250000) //250000 200hz
counter <= 0;
end
endmodule