05-26-2018 09:57 AM
Hi everyone. I want to design pid implementation for dual dc motor control. I am using the Spartan 3- starter kit board. I have ultrasonic range sensor and i can show the object's range on seven segment display as centimeter. And also, i can control dual dc motor by using pwm technique on fpga board. But, i can not put everything together.
I can get distance value as centimeter and i can control dc motor by using pwm technique but how can i put pid on it? or how should i write pid code? Could you please help me to get rid of this chronic issue?
Thaks.
05-28-2018 10:57 AM - edited 05-28-2018 11:10 AM
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.