国产FPGA安路SF1系列测评【使用SF1 FPGA 进行6自由度机械臂控制】

Grayson__

国产FPGA安路SF1系列测评【使用SF1 FPGA 进行6自由度机械臂控制】 [复制链接]

 

        本期测评内容是使用 SF1 FPGA 控制六自由度机械臂进行物体的简单抓取，使用到的软件为安路的 TangDynasty 。

        六自由度机械臂顾名思义拥有六个转动关节，本次测评使用的是六个转动角度为 0°-270°的舵机，控制周期为20ms，脉冲范围为0.5ms-2.5ms，对应的转动角度为 0°-270°，且二者呈线性关系，公式为 y=x/135+0.5 ,(x为角度，y为脉冲时间ms)。舵机转动的原理就是输出PWM脉冲信号，通过控制脉冲范围(0.5ms-2.5ms)，进而控制舵机转动角度。

        本次使用时钟是25MHz。这样20ms的控制周期对应的计数值应为 19'd500_000；将前面脉冲范围与转动角度的关系式(y=x/135+0.5)转换为计数个数的关系式，则为 TIME_CNT = Degree *185+12500，由于 FPGA 无法对小数进行计算且除法运算所占用的逻辑资源过大，我们将公式做如下转换：

       TIME_CNT = Degree<<7 + Degree<<6 + Degree - Degree<<3 + 12500

        这样 FPGA 就可以快速计算结果了，采用两级流水线即可得出角度对应的脉冲计数值，当cnt计数值小于计算所得的脉冲计数值时，输出高电平，否则输出低电平，这样就完成了6个舵机转动角度的控制了，这部分为在 pwm 模块中完成：

module pwm(
	input				clk,
	input				rst_n,
	input				en_in,
	input				change_flag,
	input		[8:0]	degree0,
	input		[8:0]	degree1,
	input		[8:0]	degree2,
	input		[8:0]	degree3,
	input		[8:0]	degree4,
	input		[8:0]	degree5,
	output	reg			pwm_out0,
	output	reg			pwm_out1,
	output	reg			pwm_out2,
	output	reg			pwm_out3,
	output	reg			pwm_out4,
	output	reg			pwm_out5
);
//parameter define
parameter       TIME_20MS   = 19'd500_000; 

//reg define
reg   [16:0]    pwm_cnt0;
reg   [16:0]    pwm_cnt1;
reg   [16:0]    pwm_cnt2;
reg   [16:0]    pwm_cnt3;
reg   [16:0]    pwm_cnt4;
reg   [16:0]    pwm_cnt5;
reg	  [19:0]	cnt;

reg	  [16:0]	pwm_cnt0_m0,pwm_cnt0_m1,pwm_cnt0_m2,pwm_cnt0_m3;
reg	  [16:0]	pwm_cnt1_m0,pwm_cnt1_m1,pwm_cnt1_m2,pwm_cnt1_m3;
reg	  [16:0]	pwm_cnt2_m0,pwm_cnt2_m1,pwm_cnt2_m2,pwm_cnt2_m3;
reg	  [16:0]	pwm_cnt3_m0,pwm_cnt3_m1,pwm_cnt3_m2,pwm_cnt3_m3;
reg	  [16:0]	pwm_cnt4_m0,pwm_cnt4_m1,pwm_cnt4_m2,pwm_cnt4_m3;
reg	  [16:0]	pwm_cnt5_m0,pwm_cnt5_m1,pwm_cnt5_m2,pwm_cnt5_m3;


//pipeline 1
always @(posedge clk or negedge rst_n) begin
	if(!rst_n) begin
		pwm_cnt0_m0 <= 17'd0;pwm_cnt0_m1 <= 17'd0;pwm_cnt0_m2 <= 17'd0;pwm_cnt0_m3 <= 17'd0;
	    pwm_cnt1_m0 <= 17'd0;pwm_cnt1_m1 <= 17'd0;pwm_cnt1_m2 <= 17'd0;pwm_cnt1_m3 <= 17'd0;
	    pwm_cnt2_m0 <= 17'd0;pwm_cnt2_m1 <= 17'd0;pwm_cnt2_m2 <= 17'd0;pwm_cnt2_m3 <= 17'd0;
	    pwm_cnt3_m0 <= 17'd0;pwm_cnt3_m1 <= 17'd0;pwm_cnt3_m2 <= 17'd0;pwm_cnt3_m3 <= 17'd0;
	    pwm_cnt4_m0 <= 17'd0;pwm_cnt4_m1 <= 17'd0;pwm_cnt4_m2 <= 17'd0;pwm_cnt4_m3 <= 17'd0;
	    pwm_cnt5_m0 <= 17'd0;pwm_cnt5_m1 <= 17'd0;pwm_cnt5_m2 <= 17'd0;pwm_cnt5_m3 <= 17'd0;
	end
	else begin
		pwm_cnt0_m0 <= degree0<<7;pwm_cnt0_m1 <= degree0<<6;pwm_cnt0_m2 <= degree0<<0;pwm_cnt0_m3 <= degree0<<3;
		pwm_cnt1_m0 <= degree1<<7;pwm_cnt1_m1 <= degree1<<6;pwm_cnt1_m2 <= degree1<<0;pwm_cnt1_m3 <= degree1<<3;
		pwm_cnt2_m0 <= degree2<<7;pwm_cnt2_m1 <= degree2<<6;pwm_cnt2_m2 <= degree2<<0;pwm_cnt2_m3 <= degree2<<3;
		pwm_cnt3_m0 <= degree3<<7;pwm_cnt3_m1 <= degree3<<6;pwm_cnt3_m2 <= degree3<<0;pwm_cnt3_m3 <= degree3<<3;
		pwm_cnt4_m0 <= degree4<<7;pwm_cnt4_m1 <= degree4<<6;pwm_cnt4_m2 <= degree4<<0;pwm_cnt4_m3 <= degree4<<3;
		pwm_cnt5_m0 <= degree5<<7;pwm_cnt5_m1 <= degree5<<6;pwm_cnt5_m2 <= degree5<<0;pwm_cnt5_m3 <= degree5<<3;
	end
end

//pipeline 2
always @(posedge clk or negedge rst_n) begin
	if(!rst_n) begin
		pwm_cnt0 <= 17'd37475;
		pwm_cnt1 <= 17'd37475;
		pwm_cnt2 <= 17'd37475;
		pwm_cnt3 <= 17'd37475;
		pwm_cnt4 <= 17'd37475;	
		pwm_cnt5 <= 17'd37475;
	end
	else if(!en_in)begin
		pwm_cnt0 <= pwm_cnt0_m0 + pwm_cnt0_m1 + pwm_cnt0_m2 - pwm_cnt0_m3 + 14'd12500;
		pwm_cnt1 <= pwm_cnt1_m0 + pwm_cnt1_m1 + pwm_cnt1_m2 - pwm_cnt1_m3 + 14'd12500;
		pwm_cnt2 <= pwm_cnt2_m0 + pwm_cnt2_m1 + pwm_cnt2_m2 - pwm_cnt2_m3 + 14'd12500;
		pwm_cnt3 <= pwm_cnt3_m0 + pwm_cnt3_m1 + pwm_cnt3_m2 - pwm_cnt3_m3 + 14'd12500;
		pwm_cnt4 <= pwm_cnt4_m0 + pwm_cnt4_m1 + pwm_cnt4_m2 - pwm_cnt4_m3 + 14'd12500;	
		pwm_cnt5 <= pwm_cnt5_m0 + pwm_cnt5_m1 + pwm_cnt5_m2 - pwm_cnt5_m3 + 14'd12500;
	end	
	else begin
		pwm_cnt0 <= 17'd37475;
		pwm_cnt1 <= 17'd37475;
		pwm_cnt2 <= 17'd37475;
		pwm_cnt3 <= 17'd37475;
		pwm_cnt4 <= 17'd37475;	
		pwm_cnt5 <= 17'd37475;	
	end
end

//20ms cnt
always @(posedge clk or negedge rst_n) begin
	if(!rst_n)
		cnt <= 20'd0;
	else if(!en_in) begin
		if(end_cnt)
			cnt <= 20'd0;
		else if(change_flag)
			cnt <= 20'd0;
		else
			cnt <= cnt + 1'b1;
	end
	else
		cnt <= 20'd0;
end

assign end_cnt = (cnt == (TIME_20MS-1'b1));

//pwm_out0
always @(posedge clk or negedge rst_n) begin
	if(!rst_n)
		pwm_out0 <= 1'b0;		
	else begin
		if(cnt<pwm_cnt0)
			pwm_out0 <= 1'b1;
		else
			pwm_out0 <= 1'b0;	
	end
end

//pwm_out1
always @(posedge clk or negedge rst_n) begin
	if(!rst_n)
		pwm_out1 <= 1'b0;		
	else begin
		if(cnt<pwm_cnt1)
			pwm_out1 <= 1'b1;
		else
			pwm_out1 <= 1'b0;	
	end
end

//pwm_out2
always @(posedge clk or negedge rst_n) begin
	if(!rst_n)
		pwm_out2 <= 1'b0;		
	else begin
		if(cnt<pwm_cnt2)
			pwm_out2 <= 1'b1;
		else
			pwm_out2 <= 1'b0;	
	end
end

//pwm_out3
always @(posedge clk or negedge rst_n) begin
	if(!rst_n)
		pwm_out3 <= 1'b0;		
	else begin
		if(cnt<pwm_cnt3)
			pwm_out3 <= 1'b1;
		else
			pwm_out3 <= 1'b0;	
	end
end

//pwm_out4
always @(posedge clk or negedge rst_n) begin
	if(!rst_n)
		pwm_out4 <= 1'b0;		
	else begin
		if(cnt<pwm_cnt4)
			pwm_out4 <= 1'b1;
		else
			pwm_out4 <= 1'b0;	
	end
end

//pwm_out5
always @(posedge clk or negedge rst_n) begin
	if(!rst_n)
		pwm_out5 <= 1'b0;		
	else begin
		if(cnt<pwm_cnt5)
			pwm_out5 <= 1'b1;
		else
			pwm_out5 <= 1'b0;	
	end
end


endmodule

        之后新建 pwm_top 顶层模块，例化  pwm 模块，同时简单控制机械臂抓取物体，机械臂抓取的动作采用三段式状态机控制，分为四个动作：初始-转动-抓取-返回。该控制总共用时20s，每5s更新一次动作，使用按键 H3 （en_in，低电平有效）控制机械臂是否执行抓取：

module pwm_top(
	input			clk,
	input			rst_n,
	input			en_in,
	output			pwm_out0,
	output			pwm_out1,
	output			pwm_out2,
	output			pwm_out3,
	output			pwm_out4,
	output			pwm_out5		
);

//parameter define
parameter	CNT_1S = 26'd25_000_000;
parameter	IDLE  = 3'd0;
parameter	STEP1 = 3'd1;
parameter	STEP2 = 3'd2;
parameter	STEP3 = 3'd3;
parameter	STEP4 = 3'd4;

//reg define
reg		[2:0]	cur_state;
reg		[2:0]	nex_state;	
reg		[25:0]	cnt;
reg		[4:0]	cnt_s;

reg		[8:0]	degree0;
reg     [8:0]   degree1;
reg     [8:0]   degree2;
reg     [8:0]   degree3;
reg     [8:0]   degree4;
reg     [8:0]   degree5;

reg				change_flag;



pwm u_pwm(
	.clk		(clk  ),
	.rst_n		(rst_n),
	.en_in		(en_in),
	.change_flag(change_flag),
	.degree0	(degree0),
	.degree1	(degree1),
	.degree2	(degree2),
	.degree3	(degree3),
	.degree4	(degree4),
	.degree5	(degree5),
	.pwm_out0	(pwm_out0),
	.pwm_out1	(pwm_out1),
	.pwm_out2	(pwm_out2),
	.pwm_out3	(pwm_out3),
	.pwm_out4	(pwm_out4),
	.pwm_out5	(pwm_out5)
);

//FSM1
always @(posedge clk or negedge rst_n) begin
	if(!rst_n)
		cur_state <= IDLE;
	else
		cur_state <= nex_state;
end

//FSM2
always @(*) begin
	if(!rst_n)
		nex_state = IDLE;
	else begin
		case(cur_state)
			IDLE:if(!en_in)
					nex_state = STEP1;
				else
					nex_state = IDLE;
			STEP1:begin
				if(!en_in)
					if((cnt_s == 5'd4) && (cnt == CNT_1S - 1'b1))
						nex_state = STEP2;
					else
						nex_state = STEP1;
				else
					nex_state = IDLE;
			end
			STEP2:begin
				if(!en_in)
					if((cnt_s == 5'd9) && (cnt == CNT_1S - 1'b1))
						nex_state = STEP3;
					else
						nex_state = STEP2;
				else
					nex_state = IDLE;
			end				
			STEP3:begin
				if(!en_in)
					if((cnt_s == 5'd14) && (cnt == CNT_1S - 1'b1))
						nex_state = STEP4;
					else
						nex_state = STEP3;
				else
					nex_state = IDLE;
			end
			STEP4:begin
				if(!en_in)
					if((cnt_s == 5'd19) && (cnt == CNT_1S - 1'b1))
						nex_state = IDLE;
					else
						nex_state = STEP4;
				else
					nex_state = IDLE;
			end
			default:nex_state = IDLE;
		endcase
	end
end

//FSM3
always @(posedge clk or negedge rst_n) begin
	if(!rst_n) begin
		degree0 <= 9'd135;
		degree1 <= 9'd135;
	    degree2 <= 9'd135;
	    degree3 <= 9'd135;
	    degree4 <= 9'd135;
	    degree5 <= 9'd135;
	end
	else begin
		case(cur_state)
			IDLE :begin
				degree0 <= 9'd135;
				degree1 <= 9'd135;
			    degree2 <= 9'd135;
			    degree3 <= 9'd135;
			    degree4 <= 9'd135;
			    degree5 <= 9'd135;
			end
			STEP1:begin
				degree0 <= 9'd225;
			    degree1 <= 9'd180;
			    degree2 <= 9'd45 ;
			    degree3 <= 9'd90 ;
			    degree4 <= 9'd135;
			    degree5 <= 9'd135;
			end
			STEP2:begin
				degree0 <= 9'd225;
			    degree1 <= 9'd180;
			    degree2 <= 9'd45 ;
			    degree3 <= 9'd90 ;
			    degree4 <= 9'd135;
			    degree5 <= 9'd0  ;
			end	
			STEP3:begin
				degree0 <= 9'd135;
			    degree1 <= 9'd135;
			    degree2 <= 9'd135;
			    degree3 <= 9'd135;
			    degree4 <= 9'd135;
			    degree5 <= 9'd0  ;
			end	
			STEP4:begin
				degree0 <= 9'd135;
			    degree1 <= 9'd135;
			    degree2 <= 9'd135;
			    degree3 <= 9'd135;
			    degree4 <= 9'd135;
			    degree5 <= 9'd135;
			end	
			default:begin
				degree0 <= 9'd135;
			    degree1 <= 9'd135;
			    degree2 <= 9'd135;
			    degree3 <= 9'd135;
			    degree4 <= 9'd135;
			    degree5 <= 9'd135;
			end
		endcase
	end	
end

//1s
always @(posedge clk or negedge rst_n) begin
	if(!rst_n)
		cnt <= 26'd0;
	else if(!en_in) begin
		if(cnt == CNT_1S - 1'b1)
			cnt <= 26'd0;
		else
			cnt <= cnt + 1'b1;
	end
	else
		cnt <= 26'd0;
end

//20s
always @(posedge clk or negedge rst_n) begin
	if(!rst_n)
		cnt_s <= 5'd0;
	else if(!en_in) begin
		if(cnt == CNT_1S - 1'b1) begin
			if(cnt_s == 5'd20 - 1'b1)
				cnt_s <= 5'd0;
			else
				cnt_s <= cnt_s + 1'b1;
		end
		else
			cnt_s <= cnt_s;
	end
	else
		cnt_s <= 5'd0;
end

always @(posedge clk or negedge rst_n) begin
	if(!rst_n)
		change_flag <= 1'b0;
	else if(((cnt_s == 5'd4) && (cnt == CNT_1S - 1'b1)) || ((cnt_s == 5'd9) && (cnt == CNT_1S - 1'b1))
				|| ((cnt_s == 5'd14) && (cnt == CNT_1S - 1'b1)) || ((cnt_s == 5'd19) && (cnt == CNT_1S - 1'b1)))
		change_flag <= 1'b1;
	else
		change_flag <= 1'b0;
end

endmodule

        进行引脚约束，pwm_out0~pwm_out5 分别对应舵机0~5 ，添加时钟约束：

create_clock -name clk -period 40 -waveform {0 20} [get_nets {clk}]

         编译无误后将 bit 文件下载至开发板，机械臂中有三个舵机能正确执行操作，另外三个舵机出现抖动和无响应的现象，经过各种控制变量法查找原因均未能解决（机械臂无问题），然而相同代码在其他fpga上跑，机械臂能够正确执行。个人能力不足，原因尚待查找。

cimdoug

好，谢谢分享

秦天qintian0303

6自由度机械臂控制重点还是算法吧      

Grayson__

秦天qintian0303 发表于 2023-4-2 08:25 6自由度机械臂控制重点还是算法吧      

是的，驱动机械臂需要进行运动的分析建模，抓取物体还需用到传感器定位物体坐标。FPGA端可以用来完成舵机控制、接口扩展、信号处理等，MCU部分可以用来完成轨迹控制，运动分析等。这篇文章中只是进行了较简单的转动角度转换计算与控制。

Grayson__

Robotic_Arm.zip (1.47 MB, 下载次数: 9)

2023-4-3 17:57 上传

点击文件名下载附件

这是工程源码