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
程序
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
