1 | library IEEE;
|
2 | use IEEE.STD_LOGIC_1164.ALL;
|
3 | use IEEE.NUMERIC_STD.ALL;
|
4 |
|
5 |
|
6 | entity PI_Cntr is
|
7 | Port ( clk : in STD_LOGIC;
|
8 | setpoint : in STD_LOGIC_VECTOR (31 downto 0);
|
9 | actualpoint : in STD_LOGIC_VECTOR (31 downto 0);
|
10 | output : out STD_LOGIC_VECTOR (31 downto 0)
|
11 | );
|
12 | end PI_Cntr;
|
13 |
|
14 |
|
15 | architecture Behavioral of PI_Cntr is
|
16 |
|
17 | ------------- Begin Cut here for COMPONENT Declaration ------ COMP_TAG
|
18 | COMPONENT float_sum
|
19 | PORT (
|
20 | a : IN STD_LOGIC_VECTOR(31 DOWNTO 0);
|
21 | b : IN STD_LOGIC_VECTOR(31 DOWNTO 0);
|
22 | operation : IN STD_LOGIC_VECTOR(5 DOWNTO 0);
|
23 | result : OUT STD_LOGIC_VECTOR(31 DOWNTO 0)
|
24 | );
|
25 | END COMPONENT;
|
26 |
|
27 | COMPONENT float_mult
|
28 | PORT (
|
29 | a : IN STD_LOGIC_VECTOR(31 DOWNTO 0);
|
30 | b : IN STD_LOGIC_VECTOR(31 DOWNTO 0);
|
31 | result : OUT STD_LOGIC_VECTOR(31 DOWNTO 0)
|
32 | );
|
33 | END COMPONENT;
|
34 |
|
35 |
|
36 | -- COMP_TAG_END ------ End COMPONENT Declaration ------------
|
37 |
|
38 | signal dt : STD_LOGIC_VECTOR(31 DOWNTO 0) := x"38d1b717"; -- 100us
|
39 | signal Kp : STD_LOGIC_VECTOR(31 DOWNTO 0) := x"3f800000"; -- 1
|
40 | signal Ki : STD_LOGIC_VECTOR(31 DOWNTO 0) := x"3c23d70a"; -- 0.01
|
41 | signal max : STD_LOGIC_VECTOR(31 DOWNTO 0) := x"42200000"; -- 40
|
42 | signal min : STD_LOGIC_VECTOR(31 DOWNTO 0) := x"c2200000"; -- - 40
|
43 |
|
44 | signal mult_a, mult_b, mult_r : STD_LOGIC_VECTOR(31 DOWNTO 0):= x"00000000";
|
45 | signal add_a, add_b, add_r : STD_LOGIC_VECTOR(31 DOWNTO 0):= x"00000000";
|
46 | signal oper : STD_LOGIC_VECTOR(5 DOWNTO 0) := "000000";
|
47 | signal integral : STD_LOGIC_VECTOR(31 DOWNTO 0):= x"00000000";
|
48 | signal error : STD_LOGIC_VECTOR(31 DOWNTO 0):= x"00000000";
|
49 | signal temp, temp2 : STD_LOGIC_VECTOR(31 DOWNTO 0):= x"00000000";
|
50 |
|
51 |
|
52 | signal cnt : integer range 0 to 15 := 0;
|
53 |
|
54 | begin
|
55 | C0 : float_sum PORT MAP ( a => add_a,
|
56 | b => add_b,
|
57 | operation => oper,
|
58 | result => add_r
|
59 | );
|
60 | C1 : float_mult PORT MAP ( a => mult_a,
|
61 | b => mult_b,
|
62 | result => mult_r
|
63 | );
|
64 |
|
65 |
|
66 | GEN: block
|
67 | begin
|
68 | process(clk)
|
69 | begin
|
70 | IF rising_edge(clk) THEN
|
71 | cnt <= cnt + 1;
|
72 | IF cnt = 0 THEN -- Berechnen Error
|
73 | add_a <= setpoint;
|
74 | add_b <= actualpoint;
|
75 | oper <= "000001";
|
76 | ELSIF cnt = 1 THEN
|
77 | error <= add_r;
|
78 | ELSIF cnt = 2 THEN
|
79 | mult_a <= dt; -- temp = error*dt
|
80 | mult_b <= error;
|
81 | ELSIF cnt = 3 THEN
|
82 | temp <= mult_r; -- Ergebnis der Multiplication wird in
|
83 | ELSIF cnt = 4 THEN -- integral = integral + temp
|
84 | add_a <= integral;
|
85 | add_b <= temp;
|
86 | oper <= "000000";
|
87 |
|
88 | mult_a <= Kp; -- temp2 = Kp * error
|
89 | mult_b <= error;
|
90 | ELSIF cnt = 5 THEN
|
91 | integral <= add_r;
|
92 | temp2 <= mult_r;
|
93 | ELSIF cnt = 6 THEN
|
94 | mult_a <= Ki; -- temp = Ki*Integral
|
95 | mult_b <= integral;
|
96 | ELSIF cnt = 7 THEN
|
97 | temp <= mult_r;
|
98 | ELSIF cnt = 8 THEN -- Pre_error = temp + temp2
|
99 | add_a <= temp;
|
100 | add_b <= temp2;
|
101 | oper <= "000000";
|
102 | ELSIF cnt = 9 THEN
|
103 | output <= add_r;
|
104 | cnt <= 0;
|
105 | END IF;
|
106 | END IF;
|
107 | END PROCESS;
|
108 | END BLOCK GEN;
|
109 |
|
110 | end Behavioral;
|