FPGA学习笔记（5）——步进电机梯形加减速算法

使用加减速的目的是：防止步进电机的启动频率过快而无法正常启动，避免控制脉冲频率变化过大造成电机丢步或过冲。

空载启动频率，即步进电机在空载情况下能够正常启动的脉冲频率，如果脉冲频率高于该值，电机不能正常启动，可能发生丢步或堵转。在有负载的情况下，启动频率应更低。如果要使电机达到高速转动，脉冲频率应该有加速过程，即启动频率较低，然后按一定加速度升到所希望的高频（电机转速从低速升到高速）。

步进电机的基本概念及控制模式参考如下链接：

https://www.elecfans.com/d/1294049.html

---

通过梯形加减速要达到的效果：

• 控制脉冲频率从0到期望速度有直线加减速的过程；
• 电机运动结束能够到达指定的位置，即经过加减速曲线后能够准确在预期位置处停止。

梯形加减速的算法实现

梯形加减速的位移、速度、加速度曲线如图1所示，速度曲线分为加速、匀速、减速三个部分。

图1 梯形加减速示意图

    图2所示，对直线加速段进行离散，在0-ta之间插入若干中间速度值，控制速度从小到大取值，即可实现直线加速。本次设计采用对0-ta进行n等分，每一份的时间为t_s=ta/n，也是将期望速度（speed）进行n等分，即在加速段均匀插入n-1个中间速度值，每经过时间t_s，对当前速度加 speed/n ，直到达到期望速度speed。减速过程与之相反。

本次设计只实现图2所示的加减速情况，n=100。

图2 离散化后的直线加减速速度变化曲线

---

梯形加减速模块框图如图3所示

图3

功能说明：上位机传送电机目标位置、目标速度、加速时间、减速时间，经过加减速模块后，能够在目标位置准确停下。

引脚说明

Clk	系统时钟50MHz
Rst_n	复位信号，低电平有效
Step	电机目标位置，输入脉冲个数。示例代码中为64000
Speed	电机目标速度，输入脉冲频率（hz）。示例代码中为100khz
T_accel	加速时间（ms）。示例代码中为300ms
T_decel	减速时间（ms）。示例代码中为400ms
Pul	输出脉冲信号

---

各个模块定义

1. 定时计数模块

在加速或减速控制开启状态下启动该计数器，加速计数的时间为t_s=ta/n，减速计数的时间为

t_s=(tc-tb)/n。

代码：

assign  T_clk = 26'd50_000_000;
assign	CNT_up = (t_accel*T_clk)/100000;	//acc time;计数器的计数值
assign	CNT_dn = (t_decel*T_clk)/100000;	//dec time


always@(posedge clk or negedge rst_n)
begin
	if (!rst_n)
		cnt_up <= 'b0;
	else if(cnt_up == CNT_up - 1'b1)
		cnt_up <= 'b0;
	else if(up_en)                       
 //加速使能有效时才开始计数，所以speed_current赋值条件为cnt_up == 0
		cnt_up <= cnt_up + 1'b1;
end
always@(posedge clk or negedge rst_n)
begin
	if (!rst_n)
		cnt_dn <= 'b0;
	else if(cnt_dn == CNT_dn - 1'b1)
		cnt_dn <= 'b0;
	else if(down_en)
		cnt_dn <= cnt_dn + 1'b1;
end

2. 加减速使能模块

通过比较当前脉冲数与加速阶段的总脉冲数和需要减速时的脉冲数，确定加速使能信号up_en和减速使能信号down_en。

图2所示，加速阶段的总脉冲数为红线下方的面积，即CNT_sup=1/2×Vt×(ta-ta/n)

注意单位转换：hz X s，得到的是脉冲数，减速阶段同理。

代码：

assign  CNT_con= step_target - speed_target*(t_decel-t_decel/100)/2000; 
//when dec time ，减速的时间点，总脉冲数减去加减速总共所需的脉冲数，hz x s，得出的才是脉冲个数
assign  CNT_sup= speed_target*(t_accel-t_accel/100)/2000;
//加速阶段的脉冲总数

always@(posedge clk or negedge rst_n)    //加减速的使能条件要合理布置，用速度作为判断条件时容                
                                         //易出错
begin
	if(!rst_n) begin
		up_en   <= 'b0;
		down_en <= 'b0;
	end
	else if (cnt_con <= CNT_sup)    //当输出脉冲个数小于等于加速阶段的总脉冲数时，置一
		up_en	<= 1'b1;	
	else if  (cnt_con > CNT_con && speed_current > 'b0) 
//when dec，当到达减速点时，给减速使能置一，同时条件还必须包括speed_current > 'b0，
//减速到0时，该条件会永远满足，一直减速下去
		down_en <= 1'b1;
	else begin
		up_en	<= 1'b0;
		down_en <= 1'b0;
	end
end

3. 加减速变化模块

当加速使能信号up_en为真时，对当前速度加speed/n，当减速使能信号down_en为真时，对当前速度减speed/n。

代码：

always@(posedge clk or negedge rst_n)
begin
	if(!rst_n)
		speed_current <= 'b0;
	else if(up_en && cnt_up == 0)    //必须是在up_en刚刚有效时就进行加减速
		speed_current <= speed_current + speed_target/100;
	else if(down_en && cnt_dn == 0)
		speed_current <= speed_current - speed_target/100;
	else 
		speed_current <= speed_current ;
end

4. 脉冲输出模块

计算出当前速度对应的脉冲周期，进而计算出计数器计数到周期值所对应的计数值。

CNT_per= T_clk/speed_current;

代码：

assign  CNT_per= T_clk/speed_current;	//pulse out，当前速度对应的脉冲周期的计数值


always@(posedge clk or negedge rst_n)    //输出脉冲的周期计数器
begin
	if (!rst_n)
		cnt_per <= 'b0;
	else if(cnt_per == CNT_per - 1'b1 || cnt_up == CNT_up - 1'b1
		|| cnt_dn == CNT_dn - 1'b1)     //清零条件要加上 cnt_up和cnt_dn计数到最大值减一时，
                                        //因为下一段的速度值与前一段不同，速度变化后要重新开始                        
                                        //对输出脉冲周期的计数
		cnt_per <= 'b0;
	else 	cnt_per <= cnt_per + 1'b1;
end
//pulse out
always@(posedge clk or negedge rst_n)
begin
	if(!rst_n)
		pul <= 'b0;
	else if(cnt_per == CNT_per/2-1 || cnt_per == CNT_per-1) //脉冲输出，中间结束各翻转一次
		pul <= ~pul;
	else	pul <= pul;
end

5. 脉冲计数模块

记录输出脉冲的个数，且只能计数到目标位置。

always@(posedge clk or negedge rst_n)
begin
	if (!rst_n)
		cnt_con <= 'b0;
	else if(cnt_per == CNT_per - 1'b1 && cnt_con != step_target)    
// 脉冲周期计数器计满且输出脉冲个数不等于要求个数时才加1，
// 计数器只计数一次的写法
		cnt_con <= cnt_con + 1'b1;
	else	cnt_con <= cnt_con;
end

---

仿真结果

• Speed_current的波形满足直线加减速曲线；
• 实际加速时间是297ms，减速时间是396ms；
• 在输出脉冲数为64000时，Speed_current正好减速到0，电机停止。

满足预期的功能要求。

图4 直线加减速仿真结果图

---

完整代码如下：

Rtl：

module	linear_acc_dec(
	input		clk,
	input		rst_n,
	input	[15:0]	step_target,	// pulse_count
	input	[16:0]	speed_target,	// hz
	input	[8:0]	t_accel,	// ms
	input	[8:0]	t_decel,
	output	reg	pul		
);
//0-speed_target Insert 100 intermediate values
	wire	[33:0]	CNT_up;     //counter 3ms/20ns
	wire	[34:0]	CNT_dn;     //声明位宽时，不能只看结果值，要能容下计算过程中的最大值
	reg	[17:0]	cnt_up;
	reg	[17:0]	cnt_dn;
	reg	[16:0]	speed_current;
	reg		up_en;
	reg		down_en;
	wire	[15:0]	CNT_per;
	reg	[15:0]	cnt_per;	//T-pul period
	wire	[15:0]	CNT_con;
	reg	[15:0]	cnt_con;	//pulse count
	wire	[15:0]  CNT_sup;
	wire	[25:0]	T_clk  ;

assign  T_clk = 26'd50_000_000;
assign	CNT_up = (t_accel*T_clk)/100000;	//acc time
assign	CNT_dn = (t_decel*T_clk)/100000;	//dec time
assign  CNT_per= T_clk/speed_current;	//pulse out
assign  CNT_con= step_target - speed_target*(t_decel-t_decel/100)/2000; 
//when dec time
assign  CNT_sup= speed_target*(t_accel-t_accel/100)/2000;

always@(posedge clk or negedge rst_n)
begin
	if (!rst_n)
		cnt_up <= 'b0;
	else if(cnt_up == CNT_up - 1'b1)
		cnt_up <= 'b0;
	else if(up_en)
		cnt_up <= cnt_up + 1'b1;
end
always@(posedge clk or negedge rst_n)
begin
	if (!rst_n)
		cnt_dn <= 'b0;
	else if(cnt_dn == CNT_dn - 1'b1)
		cnt_dn <= 'b0;
	else if(down_en)
		cnt_dn <= cnt_dn + 1'b1;
end
//100 intermediate values
always@(posedge clk or negedge rst_n)
begin
	if(!rst_n)
		speed_current <= 'b0;
	else if(up_en && cnt_up == 0)
		speed_current <= speed_current + speed_target/100;
	else if(down_en && cnt_dn == 0)
		speed_current <= speed_current - speed_target/100;
	else 
		speed_current <= speed_current ;
end
//up_en,down_en
always@(posedge clk or negedge rst_n)
begin
	if(!rst_n) begin
		up_en   <= 'b0;
		down_en <= 'b0;
	end
	else if (cnt_con <= CNT_sup)
		up_en	<= 1'b1;	
	else if  (cnt_con > CNT_con && speed_current > 'b0)//when dec
		down_en <= 1'b1;
	else begin
		up_en	<= 1'b0;
		down_en <= 1'b0;
	end
end
//pulse count
always@(posedge clk or negedge rst_n)
begin
	if (!rst_n)
		cnt_con <= 'b0;
	else if(cnt_per == CNT_per - 1'b1 && cnt_con != step_target)
		cnt_con <= cnt_con + 1'b1;
	else	cnt_con <= cnt_con;
end

//cnt_pulse out
always@(posedge clk or negedge rst_n)
begin
	if (!rst_n)
		cnt_per <= 'b0;
	else if(cnt_per == CNT_per - 1'b1 || cnt_up == CNT_up - 1'b1
		|| cnt_dn == CNT_dn - 1'b1)
		cnt_per <= 'b0;
	else 	cnt_per <= cnt_per + 1'b1;
end
//pulse out
always@(posedge clk or negedge rst_n)
begin
	if(!rst_n)
		pul <= 'b0;
	else if(cnt_per == CNT_per/2-1 || cnt_per == CNT_per-1)
		pul <= ~pul;
	else	pul <= pul;
end

endmodule

Tb：

`timescale	1ns/1ns
`define	clk_period 20
module tb();
reg		clk		;
reg		rst_n		;
reg	[15:0]	step_target	;	
reg	[16:0]	speed_target	;	
reg	[8:0]	t_accel		;	
reg	[8:0]	t_decel		;
wire		pul		;
initial begin
	clk <= 'b0;
	rst_n <= 'b0;
	#(`clk_period)
	rst_n <= 'b1;
	step_target <= 64000;
	speed_target<=100000;
	t_accel <= 300;
	t_decel <= 400;
	#(`clk_period*50000000)
	$stop ;
end

always #(`clk_period/2) clk<=~clk;

linear_acc_dec
u1
(
	.clk		(clk		),
	.rst_n		(rst_n		),
	.step_target	(step_target	),
	.speed_target	(speed_target	),
	.t_accel	(t_accel	),	
	.t_decel	(t_decel	),	
	.pul		(pul		)
);
endmodule