FPGA驱动步进电机-Sin曲线加速

基本实现原理
实际仿真的波形
程序

以下由特权同学的FPGA文档摘取

Sin 曲线控制 step 脉冲信号生成的功能框图如下所示。

基本实现原理

①判断步进电机驱动的目标频率 stepper_delay_target 与当前频率 stepper_delay_current的值是否一致，若一致，则不做任何加速、减速操作，保持当前速度运行；若目标频率高于当前频率，则执行加速；若目标频率低于当前频率，则执行减速。

②在加速或减速控制开启状态下，1ms 分频计数逻辑每个 1ms 产生一个高脉冲，用于切换当前的速度。

③在每 1ms，步进电机的速度都会加速或减速一定的频率值，这个频率值和匀加速总是"固定"不同，它是变化的。在低频时，速度变化量较大，而高速时，速度变化量较小。这个速度到底如何变化？变化多少？我们现实用 matlab 产生了一个 0‐pi 的 sin 曲线，并且将曲线所对应的 1024 个 16bit 数据存储到了 FPGA 的片内 ROM 里（实际使用中，0~0.5pi 部分的上升曲线作为步进电机加速的频率曲线，而 0.5pi~pi 的下降曲线作为步进电机减速的频率曲线）。如图所示，X 取值为 0~511 的曲线（左侧曲线，取 0~0.5pi 的 512 个正弦值放大 32768倍，并且挪到 0 点以上）作为加速曲线，X 取值为 512~1023 的曲线（右侧曲线，取 0.5pi~pi的 512 个正弦值放大 32768 倍）作为减速曲线。

那么这两段曲线如何使用？

由于加速或减速所用到的 sin 查表值都是 512 个，所以我们加速或减速所需经过的中间频率都是 512 个。对于加速操作，目标频率 stepper_delay_target，加速起始频率stepper_delay_current 的差值，需要经过 x 个中间加速频率点（x 从 0 递增到 511），我们可以算得当前加速的 delta_speed_pulse = (stepper_delay_target ‐stepper_delay_current ) * (x 为地址对应的 ROM 数据)/32768，即最终用(stepper_delay_target ‐ delta_speed_pulse/32768)作为当前频率点。换句话说，就是用 sin 曲线作为整个加速过程的频率差值，用目标频率减去这个差值，就可以得到从其实频率逐渐靠近目标频率的加速过程，并且这个加速过程和图示左侧的曲线一样越来越平滑。对于减速操作也类似，只不过它是用停止频率或减速目标频率加上当前 sin 曲线算出的频率差值作为当前中间频率点。

④实际的 FPGA coding 中，必须把"频率"换算为"周期"，便于计数。这个换算很简单，用 1s 时间除以"频率"值即可。而由于"周期"必须换算为"时钟脉冲个数"为单位，所以我们把这个除法运算中的"除数"也换算为"时钟脉冲个数"为单位，因此 1s/20ns（时钟周期为 20ns）即除数 50_000_000。

⑤算出了加速或减速过程中不断变化的步进电机驱动的脉冲周期stepper_delay_current_period，就能够产生步进电机驱动的脉冲了。

实际仿真的波形

以上匀加速/减速操作下，设定步进电机的启动频率为 500Hz，可以稳定加速到 5KHz，加速时间需要 512*1ms = 512ms

程序

c 复制代码

module stepper_motor_controller(
		input clk,		//50MHz
		input rst_n,	//复位信号，低电平有效

		output stepper_motor_reset_n,	//步进电机复位信号，低电平有效
		output reg stepper_motor_en_n,				//步进电机驱动信号输出有效信号，低电平有效
		output reg stepper_motor_clk,				//步进电机的前进脉冲，上升沿有效发起一个步进电机转动
		
		input stepper_work_en,	//步进电机使能
		input[19:0] stepper_delay_target,	//步进电机两次stpe之间的延时，取值必须大于2，目标延时值
		output reg[19:0] stepper_delay_current	//步进电机两次stpe之间的延时，取值必须大于2，当前延时值
	);

parameter START_FRE = 20'd1000;	//步进电机起步频率，单位Hz
parameter TIMER_1MS = 20'd50_000;	//1ms定时计数最大值，时钟为50MHz(20ns)
parameter CLK_NUMER_PERIORD = 32'd50_000_000;	//除数，单位20ns

//---------------------------------------------------------------------------
reg speed_up_en;		//加速功能开启
reg[1:0] speed_up_en_r; //加速寄存器打一拍
reg speed_down_en; 		//减速功能开启
reg[1:0] speed_down_en_r;//减速寄存器打一拍
reg[19:0] stepper_cnt; 
 
wire pos_speed_up_en;	//speed_up_en上升沿，表示开始启动加速	
wire pos_speed_down_en;	//speed_down_en上升沿，表示开始启动减速 

//---------------------------------------------------------------------------
//复位
assign stepper_motor_reset_n = rst_n; 
 
//---------------------------------------------------------------------------
//1s加速/减速定时
reg[19:0] xcnt;
 
always @(posedge clk or negedge rst_n)
	if(!rst_n) xcnt <= 20'd0;
	else if(!speed_up_en && !speed_down_en) xcnt <= 20'd0;
	else if(xcnt < (TIMER_1MS-1)) xcnt <= xcnt+1'b1;
	else xcnt <= 20'd0;	
 
wire motor_speed_change = (xcnt == (TIMER_1MS-1));	//ms定时信号，高电平有效，切换速度
 
//---------------------------------------------------------------------------
//例化ROM，存储sin输出1024个0-pi的结果，放大1024倍
reg[9:0] rom_addr;
wire[15:0] rom_data;

blk_mem_gen_0	uut_blk_mem_gen_0 (	//IP核
	.address ( rom_addr ),
	.clock ( clk ),
	.q ( rom_data )
	);		
	
//---------------------------------------------------------------------------
//匀加速、减速控制 

	//加速控制使能
always @(posedge clk or negedge rst_n)
	if(!rst_n) speed_up_en <= 1'b0; 
	else if(motor_speed_change && (rom_addr == 10'd511)) speed_up_en <= 1'b0;	
	else if(((stepper_delay_current < stepper_delay_target) && stepper_work_en)) speed_up_en <= 1'b1;
	else speed_up_en <= 1'b0;

always @(posedge clk)
	speed_up_en_r <= {speed_up_en_r[0],speed_up_en};	//记录电平变化

assign pos_speed_up_en = ~speed_up_en_r[1] & speed_up_en_r[0];	//speed_up_en上升沿，表示开始启动加速

	//减速控制使能
always @(posedge clk or negedge rst_n)
	if(!rst_n) speed_down_en <= 1'b0; 
	else if(motor_speed_change && (rom_addr == 10'd1023)) speed_down_en <= 1'b0;  
	else if(stepper_work_en) begin
		if(stepper_delay_current > stepper_delay_target) speed_down_en <= 1'b1; 
		else speed_down_en <= 1'b0;
	end
	else begin
		if(stepper_delay_current > START_FRE) speed_down_en <= 1'b1;
		else speed_down_en <= 1'b0;	
	end
	
always @(posedge clk)	
	speed_down_en_r <= {speed_down_en_r[0],speed_down_en};
	
assign pos_speed_down_en = ~speed_down_en_r[1] & speed_down_en_r[0];	//speed_down_en上升沿，表示开始启动减速	
wire neg_speed_down_en = speed_down_en_r[1] & ~speed_down_en_r[0];
	
//---------------------------------------------------------------------------
//ROM地址产生，对应加速/减速参数选择控制

always @(posedge clk or negedge rst_n)
	if(!rst_n) rom_addr <= 10'd0;
	else if(pos_speed_up_en) rom_addr <= 10'd0;
	else if(pos_speed_down_en) rom_addr <= 10'd512;
	else if(motor_speed_change && (speed_up_en || speed_down_en)) rom_addr <= rom_addr+1'b1;
	else ;	

//---------------------------------------------------------------------------
//乘法运算
wire[35:0] mult_result;
reg[19:0] mult_datab;
reg[19:0] speed_up_start;

	//锁存加速起始频率
always @(posedge clk or negedge rst_n)
	if(!rst_n) speed_up_start <= 20'd0;
	else if(pos_speed_up_en) speed_up_start <= stepper_delay_current;

	//计算当前运行频率和目标频率差
always @(posedge clk or negedge rst_n)
	if(!rst_n) mult_datab <= 20'd0;
	else if(pos_speed_up_en) mult_datab <= stepper_delay_target - stepper_delay_current;	//加速
	else if(pos_speed_down_en) begin
		if(!stepper_work_en) mult_datab <= stepper_delay_current - START_FRE;				//停止的减速
		else mult_datab <= stepper_delay_current - stepper_delay_target;					//运行中减速
	end

	//频率差*(加速频率/32768)
mult_gen_0	uut_mult_gen_0 (
	.clock ( clk ),
	.dataa ( rom_data ),
	.datab ( mult_datab ),
	.result ( mult_result )
	);

wire[19:0] delta_speed_down_pulse = mult_result[34:15];	//加速或减速频率差值
wire[19:0] delta_speed_up_pulse = mult_result[34:15];	//加速或减速频率差值

//---------------------------------------------------------------------------
//步进电机使能控制

always @(posedge clk or negedge rst_n)
	if(!rst_n) stepper_motor_en_n <= 1'b1;
	else if(stepper_work_en) stepper_motor_en_n <= 1'b0;
	else if(!stepper_work_en && (stepper_delay_current == START_FRE)) stepper_motor_en_n <= 1'b1;

//---------------------------------------------------------------------------
//步进电机的step产生

	//步进电机当前频率产生
always @(posedge clk or negedge rst_n)
	if(!rst_n) stepper_delay_current <= START_FRE;
	else if(neg_speed_down_en) begin
		if(stepper_work_en) stepper_delay_current <= stepper_delay_target;
		else stepper_delay_current <= START_FRE;
	end
	else if(motor_speed_change) begin
		if(stepper_work_en) begin
			if(speed_up_en) stepper_delay_current <= speed_up_start + delta_speed_up_pulse;
			else if(speed_down_en) stepper_delay_current <= stepper_delay_target + delta_speed_down_pulse;
			else stepper_delay_current <= stepper_delay_target;
		end
		else begin
			if(speed_down_en) stepper_delay_current <= START_FRE + delta_speed_down_pulse;
			else stepper_delay_current <= START_FRE;
		end
	end

wire[19:0] stepper_delay_current_period;	
wire[31:0] div_result;		

div_gen_0	uut_div_gen_0 (
	.clock ( clk ),
	.denom ( stepper_delay_current ),
	.numer ( CLK_NUMER_PERIORD ),
	.quotient ( div_result ),
	.remain (  )
	);

assign stepper_delay_current_period = div_result[19:0];	
	
	
reg[19:0] r_stepper_delay_current_period;	

	//步进电机驱动周期锁存
always @(posedge clk or negedge rst_n)
	if(!rst_n) r_stepper_delay_current_period <= 1'd0;
	else if(stepper_cnt == 20'd0) r_stepper_delay_current_period <= stepper_delay_current_period;	
	
	//步进电机时钟频率的计数
always @(posedge clk or negedge rst_n)
	if(!rst_n) stepper_cnt <= 20'd0;
	else if(stepper_cnt < r_stepper_delay_current_period[19:0]) stepper_cnt <= stepper_cnt+1'b1;
	else stepper_cnt <= 20'd0;

	//步进电机时钟频率产生	
always @(posedge clk or negedge rst_n)
	if(!rst_n) stepper_motor_clk <= 1'b0;
	else if(stepper_cnt < {1'b0,r_stepper_delay_current_period[19:1]}) stepper_motor_clk <= 1'b0;
	else stepper_motor_clk <= 1'b1;
	

endmodule