Here, let me fix that for you:
First the code:
library IEEE;
use IEEE.STD_LOGIC_1164.ALL;
use IEEE.NUMERIC_STD.ALL;
-- PID pseudocode from Wikipedia https://en.wikipedia.org/wiki/PID_controller
-- in an arbitrary design decision, the time interval dt = 1
-- previous_error = 0
-- integral = 0
-- loop:
-- error = setpoint - measured_value
-- integral = integral + error * dt
-- derivative = (error - previous_error) / dt
-- output = Kp * error + Ki * integral + Kd * derivative
-- previous_error = error
-- wait(dt)
-- goto loop
entity PID is
generic( fraction_bits : integer := 8 );
Port ( clk : in std_logic;
clk_en : in std_logic;
sync_reset : in std_logic;
Kp, Ki, Kd : in signed; -- control coefficients
setpoint : in signed; -- input setpoint
measured : in signed; -- feedback from external plant
pid_output : out signed -- control output
);
end PID;
architecture Behavioral of PID is
function imax(arg1 : integer; arg2 : integer) return integer is
begin
if arg1 > arg2 then return arg1; end if;
return arg2;
end function;
constant error_len : integer := imax( setpoint'length, measured'length);
constant integral_len : integer := error_len + 10; -- give an extra 10 bits to integral, hope it doesn't overflow!!
constant derivative_len : integer := error_len + 1;
constant product_len : integer := Ki'length + integral_len + 1; -- This needs more computation
constant pos_limit : signed(pid_output'length-1 downto 0) := (pid_output'left =>'0', others => '1');
constant neg_limit : signed(pid_output'length-1 downto 0) := (pid_output'left =>'1', others => '0');
signal error : signed(error_len-1 downto 0) := (others => '0');
signal previous_error : signed(error_len-1 downto 0) := (others => '0');
signal integral : signed(integral_len - 1 downto 0) := (others => '0');
signal derivative : signed(derivative_len - 1 downto 0) := (others => '0');
signal product_sum : signed(product_len - 1 downto 0) := (others => '0');
begin
process(clk) is
begin
if rising_edge(clk) then
if sync_reset = '1' then
error <= (others => '0');
previous_error <= (others => '0');
integral <= (others => '0');
derivative <= (others => '0');
product_sum <= (others => '0');
pid_output <= (others => '0');
elsif clk_en = '1' then
error <= resize(setpoint, error_len) - resize(measured, error_len);
previous_error <= error;
integral <= integral + resize(error, integral_len);
derivative <= resize(error, derivative_len) - resize(previous_error, derivative_len);
product_sum <= (resize(Kp * error, product_len) + resize(Ki * integral, product_len) + resize(Kd * derivative, product_len))/2**fraction_bits;
-- now clip the output to prevent overflow/underflow
if product_sum < resize(neg_limit, product_len) then
pid_output <= neg_limit;
elsif product_sum > resize(pos_limit, product_len) then
pid_output <= pos_limit;
else
pid_output <= resize(product_sum, pid_output'length);
end if;
end if;
end if;
end process;
end Behavioral;
Then a testbench:
library IEEE;
use IEEE.STD_LOGIC_1164.ALL;
use IEEE.NUMERIC_STD.ALL;
entity tb_pid is
end tb_pid;
architecture Behavioral of tb_pid is
component PID is
generic( fraction_bits : integer := 8 );
Port ( clk : in std_logic;
clk_en : in std_logic;
sync_reset : in std_logic;
Kp, Ki, Kd : in signed; -- control coefficients
setpoint : in signed; -- input setpoint
measured : in signed; -- feedback from external plant
pid_output : out signed -- control output
);
end component;
constant fraction_bits : integer := 8;
signal clk : std_logic := '0';
signal sync_reset : std_logic := '1';
signal Kp, Ki, Kd : signed(9 downto 0) := (others => '0');
signal setpoint : signed(9 downto 0) := to_signed( integer( 2**fraction_bits * 1.0 ), Kd'length);
signal measured : signed(9 downto 0) := (others => '0');
signal pid_output : signed(9 downto 0) := (others => '0');
begin
clk <= not clk after 10 ns;
sync_reset <= '0' after 100 ns;
Kp <= to_signed( integer( 2**fraction_bits * 0.5 ), Kp'length); -- setup some coefficients
Ki <= to_signed( integer( 2**fraction_bits * 0.125 ), Ki'length);
Kd <= to_signed( integer( 2**fraction_bits * 0.25 ), Kd'length);
setpoint <= to_signed( integer( 2**fraction_bits * (-1.0) ), Kd'length) after 1000 ns; -- flip at 1000 ns
dut: PID
generic map( fraction_bits => fraction_bits)
port map(
clk => clk,
clk_en => '1',
sync_reset => sync_reset,
Kp => Kp, -- control coefficients
Ki => Ki,
Kd => Kd,
setpoint => setpoint, -- input setpoint
measured => measured, -- feedback from external plant
pid_output => pid_output -- control output
);
measured <= pid_output; -- model of external plant here
end Behavioral;And here is the result:
Obviously, the damping is not very good with these coefficients.. A further challenge is to add automatic coefficient computation to make the controller critically damped, or as close as possible to critically damped. This quickly gets deep into control theory, especially with non-linear or time varying plant models.