1 原理讲解
距离上一次说PID算法的事情过去蛮久了,今天又重新看了看PID的代码,其实还是存在一些不合理的地方。
整理归纳了一下原理,位置式和增量式的变化。
2 工程实现
c
`timescale 1ns / 1ps
module pid_controller(
input clk,
input rst_n,
input [15:0] setpoint,
input [15:0] feedback,
input [15:0] Kp,
input [15:0] Ki,
input [15:0] Kd,
input [15:0] clk_prescaler,
output reg [15:0] control_signal
);
// Internal signals
reg [15:0] prev_error = 16'h0000;
reg [15:0] integral = 32'h00000000;
reg [15:0] derivative = 16'h0000;
// Clock divider for sampling rate
reg [15:0] clk_divider = 0;
reg sampling_flag = 0;
always @(posedge clk or negedge rst_n) begin
//$display("Clock trigered");
if (~rst_n)
clk_divider <= 16'h0000;
else if (clk_divider == clk_prescaler) begin // clk_prescaler determines the sampling rate, thus sampling rate would be clk freq/clk_prescaler
clk_divider <= 16'h0000;
sampling_flag <= 1;
end else begin
clk_divider <= clk_divider + 1;
sampling_flag <= 0;
end
end
always @(posedge clk or negedge rst_n) begin
if (~rst_n) begin
// Reset logic generally specific to application
end
else if (sampling_flag) begin
// PID Calculation
integral <= integral + (Ki * (setpoint - feedback));
$display("Integral is %d",integral);
derivative <= Kd * ((setpoint - feedback) - prev_error);
// Calculate control signal
control_signal = (Kp * (setpoint - feedback)) + integral + derivative;
prev_error <= (setpoint - feedback);// Update previous error term to feed it for derrivative term.
end
end
endmodule
testbench
c
`timescale 1ns / 1ps
module pid_tb( );
reg clk = 0;
reg rst_n = 0;
reg [15:0] setpoint = 0;
reg [15:0] feedback = 0;
reg [15:0] Kp = 0;
reg [15:0] Ki = 0;
reg [15:0] Kd = 0;
reg [15:0] clk_prescaler = 0;
wire [15:0] control_signal;
pid_controller DUT(.clk(clk),.rst_n(rst_n),.setpoint(setpoint),.feedback(feedback),.Kp(Kp),.Ki(Ki),.Kd(Kd),.clk_prescaler(clk_prescaler),.control_signal(control_signal));
initial begin
rst_n <= 0; // Assert reset
clk_prescaler <= 5;
setpoint <= 20;
Kp <= 5;
Ki <= 2;
Kd <= 1;
#20 rst_n <= 1; // Deassert reset
end
always #1 clk = ~clk;
always begin
$monitor("Control signal is %d",control_signal);
#20 feedback <= 1;
#15 feedback <= 5;
#15 feedback <= 8;
#15 feedback <= 10;
#15 feedback <= 13;
#15 feedback <= 15;
#15 feedback <= 16;
#15 feedback <=25;
#25 $finish;
end
endmodule