Code:
library IEEE;
use IEEE.STD_LOGIC_1164.ALL;
Use IEEE.STD_LOGIC_ARITH .ALL;
Use IEEE.STD_LOGIC_UNSIGNED.ALL;
entity FOPID_Controller is
port (
clk : in std_logic;
Reset : in std_logic;
speed_ref : in std_logic_vector(31 downto 0);
speed : in std_logic_vector(31 downto 0);
output : out std_logic_vector(31 downto 0)
);
end FOPID_Controller;
architecture behavioral of FOPID_Controller is
constant Kp : real := 2.050; -- Proportional gain
constant Ki : real := 3.756; -- Integral gain
constant Kd : real := -0.078; -- Derivative gain
constant lambda : real := 0.98; -- Fractional-order integrator exponent
signal error : signed(31 downto 0);
signal integrator : signed(31 downto 0); -- Increased width for overflow prevention
signal differentiator : signed(31 downto 0);
signal fractional_integrator : signed(31 downto 0);
begin
-- Error calculation
process (clk, reset)
begin
if reset then
error <= (others => '0');
else
error <= speed_ref - speed;
end if;
end process;
-- Integral term (with overflow prevention)
process (clk, reset)
begin
if reset then
integrator <= (others => '0');
else
integrator <= integrator + (Ki * error);
if integrator > (2**63 - 1) then
integrator <= (2**63 - 1);
elsif integrator < 0 then
integrator <= 0;
end if;
end if;
end process;
-- Fractional-order integrator using a power series approximation
process (clk, reset)
begin
if reset then
fractional_integrator <= (others => '0');
else
fractional_integrator <= fractional_integrator + ((Ki * error * clk'event and clk = '1') * ((error)**lambda));
if fractional_integrator > (2**63 - 1) then
fractional_integrator <= (2**63 - 1);
elsif fractional_integrator < 0 then
fractional_integrator <= 0;
end if;
end if;
end process;
-- Derivative term
process (clk, reset)
begin
if reset then
differentiator <= (others => '0');
else
differentiator <= Kd * (error - (error >> 1)); -- Simple approximation
end if;
end process;
-- Controller output (PWM duty cycle)
pwm_duty <= to_unsigned((Kp * error) + (to_signed(fractional_integrator >> 32)) + integrator + differentiator, 8);
end architecture rtl;
Last edited by a moderator: