FPGA实现电机转速PID控制

通过纯RTL实现电机转速PID控制，包括电机编码器值读取，电机速度、正反转控制，PID算法，卡尔曼滤波，最终实现对电机速度进行控制，使其能够渐近设定的编码器目标值。

一、设计思路

前面通过SOPC之NIOS Ⅱ实现电机转速PID控制(调用中断函数)对电机实现了PID控制，然后就可以按照其设计方式将上层的C语言实现的PID控制部分等全部转换成Verilog代码，最终实现纯RTL进行PID控制。

在前文中，电机PWM控制，电机方向和编码器值的获取，卡尔曼滤波是通过Verilog语言编写，而电机速度控制、PID控制是通过Nios Ⅱ系统中的软件部分实现的，因此需要编写电机速度模块，实现对电机PWM控制模块传入速度信息；编写PID控制模块，实现对电机速度模块速度的校正。

Nios Ⅱ中采用Avalon总线对各个底层Verilog代码进行读取和写入数据，因此也要对电机控制，电机方向和编码器值获取相关代码进行修改。

按照思路画出大概框图如下：

二、PWM控制模块

在前文PWM模块中由于需要Avalon总线的控制，因此有片选信号、片选地址、读写地址等变量，而转为纯RTL后只需要输入方向以及PWM值就可以，因此需要对前文代码进行修改

reg motor_movement; // 电机运动，1为开始、0为停止

reg motor_direction; // 电机转向，1为向前、0为向后

reg motor_fast_decay; // 电机减速，1为快制动、0为慢制动

always @(posedge clock or negedge reset_n)

begin

if (~reset_n)

begin

// PWM

high_dur <= 0;

total_dur <= 0;

// MOTOR

motor_movement <= 1'b0;

motor_direction <= 1'b1;

motor_fast_decay <= 1'b1;

end

else if (en)

begin

total_dur <= 32'd2500;

high_dur <= speeddata;

{motor_fast_decay, motor_direction, motor_movement} <= {1'b1,direction,1'b1};

end

// 方向控制

always @(*)

begin

if (motor_fast_decay)

begin

if (motor_movement)

begin

if (motor_direction)

{DC_Motor_IN2, DC_Motor_IN1, PWM} <= {1'b1, 1'b0, PWM_OUT};

else

{DC_Motor_IN2, DC_Motor_IN1, PWM} <= {1'b0, 1'b1, PWM_OUT};

end

else

{DC_Motor_IN2, DC_Motor_IN1, PWM} <= {1'b1, 1'b1, 1'b0};

end

// PWM 转速控制

reg PWM_OUT;

reg [31:0] total_dur; // 总持续时间

reg [31:0] high_dur; // 高位时间，决定电机转速，控制 PWM 占空比，值越高，占空比越大，转速越快

reg [31:0] tick; // 计数器

always @(posedge clock or negedge reset_n)

begin

if (~reset_n)

begin

tick <= 1;

end

else if (tick >= total_dur)

begin

tick <= 1;

end

else

tick <= tick + 1;

end

always @(posedge clock)

begin

PWM_OUT <= (tick <= high_dur) ? 1'b1 : 1'b0;

end

三、速度控制模块

速度控制模块的主要任务就是将PID模块传入的速度信息，转换为PWM值传入PWM控制模块，并根据速度的正负值计算电机的正转反转

//速度控制逻辑

always @(posedge clk or negedge reset_n) begin

if (~reset_n) begin

SpeedParam <= 32'd0;

direction_reg <= 0;

end else begin

//计算PWM参数 CYCLE_WIDTH_MINI = 32'd50;CYCLE_WIDTH_MAX = 32'd2500;

if (Speed_in > 32'd0) begin

SpeedParam <= CYCLE_WIDTH_MINI + (Speed_in * (CYCLE_WIDTH_MAX – CYCLE_WIDTH_MINI) / 32'd100);

end else if(Speed_in < 32'd0) begin

SpeedParam <= CYCLE_WIDTH_MINI + ((-Speed_in) * (CYCLE_WIDTH_MAX – CYCLE_WIDTH_MINI) / 32'd100);

end else begin

SpeedParam <= 32'd0;

end

//设置电机的转向

direction_reg <= Speed_in[31] ? 0 : 1;

end

四、PID控制模块

首先要将PID参数中的小数进行缩放转为定点数；

然后因为用了50MHz的时钟，时钟周期是10ns，可能导致KP、KI、KD算不完，因此将其进行分频为25MHz；

然后就是对 KP、KI、KD进行计算，输出PID结果，即电机速度；

最后为了防止电机速度和累积误差过大，对其进行限幅。

// 将Kp, Ki, Kd转化为定点数表示

parameter KP = 32'd60; // 0.06

parameter KI = 32'd1; // 0.001

parameter KD = 32'd3400; // 3.4

parameter SCALE_FACTOR = 1000; //缩放因子

reg signed [31:0] error;

reg signed [31:0] prev_error;

reg signed [31:0] integral;

reg signed [31:0] speed;

reg signed [31:0] controlOutput;

reg signed [31:0] p;

reg signed [31:0] i;

reg signed [31:0] d;

wire signed [31:0] integral_next = integral + error;

reg clk_25m;

always @(posedge clk or negedge reset_n) begin

if (~reset_n) begin

clk_25m <= 0;

end else begin

clk_25m <= ~clk_25m;

end

always @(posedge clk_25m or negedge reset_n) begin

if (~reset_n) begin

error <= 0;

integral <= 0;

speed <= 0;

end else begin

error <= targetDistance – currentDistance;

// Calculate control output

p <= error * KP;

i <= integral * KI;

d <= (error – prev_error) * KD;

controlOutput <= (p + i + d) / SCALE_FACTOR;

// 将控制输出限制在电机速度范围内

//speed <= initialSpeed + controlOutput;

if(controlOutput > $signed(32'd100)) begin

speed <= $signed(32'd100);

end else if(controlOutput < $signed(-32'd100)) begin

speed <= $signed(-32'd100);

end else begin

speed <= controlOutput;

end

//integral <= integrallimit + error;

if(integral_next >= $signed(32'd800)) begin

integral <= $signed(32'd800);

end else if(integral_next <= $signed(-32'd800)) begin

integral <= $signed(-32'd800);

end else begin

integral <= integral_next;

end

// 更新下次迭代的前一次误差和积分

always @(posedge clk_25m or negedge reset_n) begin

if(~reset_n) begin

prev_error <= 0;

end else if(error!= prev_error) begin

prev_error <= error;

end else begin

prev_error <= prev_error;

end

assign speedout = speed;

五、电机方向和编码器值的获取

电机编码器值要根据电机方向进行自增和自减，因此要先通过AB方波确认电机方向

reg DO_PULSE; //用于存储输出的电机脉冲信号

wire PULSE_XOR; //用于存储PHASE_A和PHASE_B进行异或结果

reg PULSE_XOR_PREVIOUS; //上一次的PULSE_XOR值

reg DIRECTION; //用于存储电机方向信号

reg DIRECT_PATCH; //用于存储DIRECT异或PHASE_A后取反的结果

//解码方向信号

always @(posedge DI_PHASE_A) DIRECTION <= DI_PHASE_B; //当有DI_PHASE_A的上升沿，将DI_PHASE_B的值赋给DIRECTION

always @(posedge DI_PHASE_B) DIRECT_PATCH <= ~(DIRECTION ^ DI_PHASE_A); //当有DI_PHASE_B的上升沿，将DIRECT和DI_PHASE_A进行异或后取反赋值给DIRECT_PATCH

assign DO_DIRECT = DIRECTION | DIRECT_PATCH; //将DIRECTION和DIRECT_PATCH进行与运算

//解码脉冲信号

assign PULSE_XOR = DI_PHASE_A ^ DI_PHASE_B;

always @(posedge DI_SYSCLK)

begin

if(PULSE_XOR != PULSE_XOR_PREVIOUS)

begin

DO_PULSE <= 1'b1;

PULSE_XOR_PREVIOUS <= PULSE_XOR;

end

else begin

DO_PULSE <= 1'b0;

end

获取电机方向后，对其进行计数，得到电机编码器的值并将其输出

always @(posedge clock or negedge reset_n)

begin

if(~reset_n) begin

counter_enable <= 0;

end

else if (counter_enable) begin

read_data <= pulse_counter;

end

always @(posedge clock)

begin

if(DO_PULSE ) begin

if(motor_direction) begin //如果电机正转

if(pulse_counter < $signed(32'h8000))

pulse_counter <= pulse_counter + 1; //counter随电机传回的脉冲数累加

end

else if(!motor_direction) begin //如果电机反转

if(pulse_counter > $signed(-32'h8000))

pulse_counter <= pulse_counter – 1; //counter随着电机传回的脉冲数递减

end

else

pulse_counter <= 0;

end

七、实验效果

整体的系统框图如下所示，左右两个电机的方波经过卡尔曼滤波后输入到motor_measure中，先获取其电机方向，然后对电机编码器进行计数并将左右两电机的编码器数值取平均值输入到PID控制模块中，与ISSP输入的目标编码器值进行计算出速度信息，将速度信息输入到set_speed中计算方向和PWM参数，输入到PWM控制模块进行电机控制

通过signal tap和ISSP联合抓波形，得出来的效果还是不错的，会有一点点超调量

————————————————

原文链接：https://blog.csdn.net/STATEABC/article/details/132665803

归档

分类

FPGA实现电机转速PID控制

相关文章

归档

分类