1 | ----------------------------------------------------------------------------------
|
2 | -- Company: AK development GmbH
|
3 | -- Engineer: A:Kurka
|
4 | --
|
5 | -- Create Date: 31.8.2024
|
6 | -- modif:31.08.24 Version A20
|
7 | -- Design Name: AS21
|
8 | -- Module Name: ITPModul - Behavioral
|
9 | ------------------------------------------------------------------------
|
10 | -- virtuelle Interpolator XYZ von Koordinaten XYZs to XYZe
|
11 | -- Invers-Transformation von XYZ auf Achsen A4..A0
|
12 | -- Achse A6 noch nicht programmiert, wird kundenspezifisch programmiert
|
13 | ----------------------------------------------------------------------------------
|
14 | library IEEE;
|
15 | use IEEE.STD_LOGIC_1164.ALL;
|
16 | --use IEEE.STD_LOGIC_ARITH.ALL;
|
17 | --use IEEE.STD_LOGIC_UNSIGNED.ALL;
|
18 | --use ieee.std_logic_signed.ALL;
|
19 | --USE ieee.math_real.all;
|
20 | ---------------------------
|
21 | use IEEE.numeric_std.all;
|
22 | LIBRARY XilinxCoreLib;
|
23 |
|
24 |
|
25 |
|
26 | entity ITPModul is
|
27 | port (nrst : in std_logic;
|
28 | clk : in std_logic;
|
29 | XS :in std_logic_vector(31 downto 0);-- Startpunkt Koordinate X
|
30 | YS :in std_logic_vector(31 downto 0);-- Startpunkt Koordinate Y
|
31 | ZS :in std_logic_vector(31 downto 0);-- Startpunkt Koordinate Z
|
32 | XT :in std_logic_vector(31 downto 0);-- Koordinate X relativ (XE-XS)
|
33 | YT :in std_logic_vector(31 downto 0);-- Koordinate Y relativ (YE-YS)
|
34 | ZT :in std_logic_vector(31 downto 0);-- Koordinate Z relativ (ZE-ZS)
|
35 | pfrq :in std_logic_vector(23 downto 0);--Bahn-Periode in anzahl CLK
|
36 | L1 :in std_logic_vector(15 downto 0);-- länge Arm1 in mm
|
37 | L2 :IN std_logic_vector(15 downto 0);-- länge Arm2 in mm
|
38 | L3 :in std_logic_vector(15 downto 0);-- länge Arm3 in mm
|
39 | ---------------------
|
40 | Xlin :in std_logic_vector(15 downto 0);-- Koord LinStz
|
41 | Ylin :in std_logic_vector(15 downto 0);-- Max.Eingabe +/-327.67
|
42 | ebene :in std_logic_vector(3 downto 0);--interpolationsebene für LinSatz
|
43 | Xactiv :in std_logic;
|
44 | Yactiv :in std_logic;
|
45 | celin :in std_logic;---start interpolation für linstz in einer ebene
|
46 | --------------------------------------
|
47 | stopp :in std_logic;-- stop interpolation,(stopp+) nach Endlage
|
48 | RampRun :in std_logic;-- rampe aktiv =1
|
49 | strimp :in std_logic;-- start interpolation
|
50 | ce :in std_logic;-- start interpolation von kreis oder XYZ Satz : InversTransf
|
51 | -----------------------------
|
52 | calcok :out std_logic:= '0';-- signalisiert alle berechnungen sind fertig
|
53 | XAout :out std_logic_vector(31 downto 0):=X"00000000";--virtuelle aktuelle koordinate
|
54 | YAout :out std_logic_vector(31 downto 0):=X"00000000";
|
55 | ZAout :out std_logic_vector(31 downto 0):=X"00000000";
|
56 | ITPmod :out std_logic_vector(2 DOWNTO 0):="000";--ITP für Status Meldung an CPU
|
57 | oAXdif :out std_logic_vector(95 downto 0):= (OTHERS => '0');--Sollwertdiff(16Bit) von inv.Transfer und Lin
|
58 | oImpLk :out std_logic:='1';--impulse von xyz itp
|
59 | enditp :out std_logic:='0';-- ende von Kontur:=0 (enditp-)
|
60 | itprun :out std_logic:='0';-- =1:interpolator läuft,aktuelle Sollwerte in oAXinkr(6x 32bit)
|
61 | testdata :out std_logic_vector(15 downto 0)
|
62 | );
|
63 | end ITPModul;
|
64 |
|
65 | architecture Behavioral of ITPModul is
|
66 | --==========================================================
|
67 | --Umrechnng Winkel in Rad(FP) in Inkremente gem. AchsnAuflösung
|
68 | -- if InkrAX = 1 achse Inkrementieren,
|
69 | --if IDekrAX = 1 achse Dekrementieren,
|
70 | COMPONENT AxConv is
|
71 | port (
|
72 | nrst : in std_logic;
|
73 | clk : in std_logic;
|
74 | ceConv : in std_logic;
|
75 | AFPinp :in std_logic_vector(31 downto 0);--Input Achsen FP
|
76 | Resol :in std_logic_vector(31 downto 0);--2PI/AchsenAuflösung
|
77 | AchsInkr :out std_logic_vector(31 downto 0);-- Anzahl Inkremene
|
78 | AXgrd :out std_logic_vector(15 downto 0)-- Achsenwinkel In Grd.X
|
79 | );
|
80 | END COMPONENT;
|
81 |
|
82 | -------------------------------------------
|
83 | COMPONENT CosFP is
|
84 | port( nrst : in std_logic;
|
85 | clk : in std_logic;
|
86 | cecos: in std_logic;-- startimpuls ArcCosFP
|
87 | ainp :in std_logic_vector(31 downto 0);-- input X= cos(x), FP format
|
88 | cos :OUT std_logic_vector(31 downto 0)-- output arccos(X), FP format
|
89 | );
|
90 | END COMPONENT;
|
91 |
|
92 | -----------------------------------------------------
|
93 | COMPONENT atanFP is
|
94 | port( nrst : in std_logic;
|
95 | clk : in std_logic;
|
96 | ceatan : in std_logic;-- startimpuls afkt
|
97 | Xinp :in std_logic_vector(31 downto 0);-- input X FP FP format
|
98 | Yinp :in std_logic_vector(31 downto 0);-- input Y FP FP format
|
99 | atanFP :OUT std_logic_vector(31 downto 0):=X"00000000"-- output Winkel(rad) in FP format
|
100 | );
|
101 | END COMPONENT;
|
102 | -----------------------------------------------
|
103 | -- if a > b then result = "0001"
|
104 | COMPONENT cmpgr IS
|
105 | PORT (
|
106 | a : IN STD_LOGIC_VECTOR(31 DOWNTO 0);
|
107 | b : IN STD_LOGIC_VECTOR(31 DOWNTO 0);
|
108 | clk : IN STD_LOGIC;
|
109 | ce : IN STD_LOGIC;
|
110 | result : OUT STD_LOGIC_VECTOR(0 DOWNTO 0)
|
111 | );
|
112 | END COMPONENT;-- cmpgr;
|
113 | -------------------------------------------------------------------------------
|
114 | COMPONENT ArcCosFP is
|
115 | port( nrst : in std_logic;
|
116 | clk : in std_logic;
|
117 | ceacos: in std_logic;-- startimpuls ArcCosFP
|
118 | ainp :in std_logic_vector(31 downto 0);-- input X= cos(x), FP format
|
119 | acos :OUT std_logic_vector(31 downto 0):=X"00000000"-- output arccos(X), FP format
|
120 | );
|
121 | END COMPONENT;
|
122 |
|
123 | ----------------------------------------------------------------
|
124 | COMPONENT mul IS
|
125 | port (
|
126 | clk: IN std_logic;
|
127 | a: IN std_logic_VECTOR(25 downto 0);
|
128 | b: IN std_logic_VECTOR(25 downto 0);
|
129 | ce: IN std_logic;
|
130 | p: OUT std_logic_VECTOR(25 downto 0));
|
131 | END COMPONENT;
|
132 | -----------------------------------------------------------
|
133 | COMPONENT DivAtan is
|
134 | port( nrst : in std_logic;
|
135 | clk : in std_logic;
|
136 | Fa :in std_logic_vector(31 downto 0);--Input FP Divident
|
137 | Fb :in std_logic_vector(31 downto 0);--Input FP Divisor
|
138 | cedivatan :in std_logic;
|
139 | Angle :out std_logic_VECTOR(25 downto 0));
|
140 | end COMPONENT;
|
141 |
|
142 | --===========================================================
|
143 | -----------für initITP-------------------------------------
|
144 | COMPONENT IntToFP IS
|
145 | port (
|
146 | a: IN std_logic_VECTOR(31 downto 0);
|
147 | clk: IN std_logic;
|
148 | ce: IN std_logic;
|
149 | result: OUT std_logic_VECTOR(31 downto 0));
|
150 | END COMPONENT;
|
151 | ------------------------------------------------------------
|
152 | COMPONENT int16ToFP IS
|
153 | PORT (
|
154 | a : IN STD_LOGIC_VECTOR(15 DOWNTO 0);
|
155 | clk : IN STD_LOGIC;
|
156 | ce : IN STD_LOGIC;
|
157 | result : OUT STD_LOGIC_VECTOR(31 DOWNTO 0)
|
158 | );
|
159 | END COMPONENT;
|
160 |
|
161 | ------------------------------------------------------------
|
162 | COMPONENT divFP IS
|
163 | port (
|
164 | a: IN std_logic_VECTOR(31 downto 0);
|
165 | b: IN std_logic_VECTOR(31 downto 0);
|
166 | clk: IN std_logic;
|
167 | ce: IN std_logic;
|
168 | result: OUT std_logic_VECTOR(31 downto 0)
|
169 | );
|
170 | END COMPONENT;
|
171 | ----------------------------------------------------
|
172 | COMPONENT mulFP IS
|
173 | port (
|
174 | a: IN std_logic_VECTOR(31 downto 0);
|
175 | b: IN std_logic_VECTOR(31 downto 0);
|
176 | clk: IN std_logic;
|
177 | ce: IN std_logic;
|
178 | result: OUT std_logic_VECTOR(31 downto 0)
|
179 | );
|
180 | END COMPONENT;
|
181 | ---------------------------------------------
|
182 | COMPONENT addFP IS
|
183 | port (
|
184 | a: IN std_logic_VECTOR(31 downto 0);
|
185 | b: IN std_logic_VECTOR(31 downto 0);
|
186 | clk: IN std_logic;
|
187 | ce: IN std_logic;
|
188 | result: OUT std_logic_VECTOR(31 downto 0));
|
189 | END COMPONENT;
|
190 | -----------------------------------------
|
191 | COMPONENT subFP IS
|
192 | port (
|
193 | a: IN std_logic_VECTOR(31 downto 0);
|
194 | b: IN std_logic_VECTOR(31 downto 0);
|
195 | clk: IN std_logic;
|
196 | ce: IN std_logic;
|
197 | result: OUT std_logic_VECTOR(31 downto 0));
|
198 | END COMPONENT;
|
199 | -----------------------------------------------------
|
200 | COMPONENT sqrtFP IS
|
201 | port (
|
202 | a: IN std_logic_VECTOR(31 downto 0);
|
203 | clk: IN std_logic;
|
204 | ce: IN std_logic;
|
205 | result: OUT std_logic_VECTOR(31 downto 0));
|
206 | END COMPONENT;
|
207 | ----------------------------------------------------
|
208 | COMPONENT FPtoInt32 IS
|
209 | port (
|
210 | a: IN std_logic_VECTOR(31 downto 0);
|
211 | clk: IN std_logic;
|
212 | ce: IN std_logic;
|
213 | result: OUT std_logic_VECTOR(31 downto 0));
|
214 | END COMPONENT;
|
215 | ---------------------------------------------------
|
216 | COMPONENT FPtoInt IS
|
217 | port (
|
218 | a: IN std_logic_VECTOR(31 downto 0);
|
219 | clk: IN std_logic;
|
220 | ce: IN std_logic;
|
221 | result: OUT std_logic_VECTOR(24 downto 0));
|
222 | END COMPONENT;
|
223 | --======================================================
|
224 | COMPONENT FPtoInt16 IS
|
225 | port (
|
226 | a: IN std_logic_VECTOR(31 downto 0);
|
227 | clk: IN std_logic;
|
228 | ce: IN std_logic;
|
229 | result: OUT std_logic_VECTOR(15 downto 0)
|
230 | );
|
231 | END COMPONENT;
|
232 | --==== testfunktionen werden nicht implementiert ====
|
233 | COMPONENT tstCos IS
|
234 | PORT (
|
235 | phase_in : IN STD_LOGIC_VECTOR(15 DOWNTO 0);
|
236 | y_out : OUT STD_LOGIC_VECTOR(15 DOWNTO 0);
|
237 | clk : IN STD_LOGIC;
|
238 | ce : IN STD_LOGIC
|
239 | );
|
240 | END COMPONENT;
|
241 | ---------------------------------------------------------
|
242 | COMPONENT tstSin IS
|
243 | PORT (
|
244 | phase_in : IN STD_LOGIC_VECTOR(15 DOWNTO 0);
|
245 | x_out : OUT STD_LOGIC_VECTOR(15 DOWNTO 0);
|
246 | clk : IN STD_LOGIC;
|
247 | ce : IN STD_LOGIC
|
248 | );
|
249 | END COMPONENT;
|
250 | -----------------------------------------------------------
|
251 | COMPONENT mul16 IS
|
252 | PORT (
|
253 | clk : IN STD_LOGIC;
|
254 | a : IN STD_LOGIC_VECTOR(15 DOWNTO 0);
|
255 | b : IN STD_LOGIC_VECTOR(15 DOWNTO 0);
|
256 | ce : IN STD_LOGIC;
|
257 | p : OUT STD_LOGIC_VECTOR(15 DOWNTO 0)
|
258 | );
|
259 | END COMPONENT;
|
260 | --==========================================================
|
261 | --ATTRIBUTE KEEP : STRING;
|
262 | --ATTRIBUTE KEEP of mul12:SIGNAL is "true";
|
263 | --ATTRIBUTE DONT_TOUCH : string;
|
264 | --ATTRIBUTE DONT_TOUCH of mulFP: COMPONENT is "yes";
|
265 | --------------------------------------------------------------------
|
266 | TYPE STATE_TYPE IS (S0,S1,S2,S3,S4,S5,S6,S7,S8,S9,S10,S11,S12,S13,S14);
|
267 | TYPE T_Param16 IS ARRAY(5 DOWNTO 0) OF std_logic_vector(15 DOWNTO 0);
|
268 | TYPE T_Param18 IS ARRAY(5 DOWNTO 0) OF std_logic_vector(17 DOWNTO 0);
|
269 | TYPE T_AXwert IS ARRAY(3 DOWNTO 0) OF INTEGER RANGE -32768 TO 32767;
|
270 | TYPE T_AXIS IS ARRAY(5 DOWNTO 0) OF INTEGER RANGE -32768 TO 32767;
|
271 | TYPE T_AXInc IS ARRAY(5 DOWNTO 0) OF std_logic_vector(31 DOWNTO 0);
|
272 | TYPE T_AXDif IS ARRAY(5 DOWNTO 0) OF std_logic_vector(15 DOWNTO 0);
|
273 | -----
|
274 | TYPE T_AxFP IS ARRAY(5 DOWNTO 0) OF std_logic_vector(31 DOWNTO 0);
|
275 | TYPE T_Axr IS ARRAY(5 DOWNTO 0) OF std_logic_vector(15 DOWNTO 0);
|
276 | -------------------------------------------------------------------------
|
277 | CONSTANT clkToFP : integer range 0 to 127 := 1;
|
278 | CONSTANT clkTosq : integer range 0 to 127 := 1;
|
279 | CONSTANT clkTomul : integer range 0 to 127 := 1;
|
280 | CONSTANT clkToadd : integer range 0 to 127 := 1;
|
281 | CONSTANT clkTosub : integer range 0 to 127 := 1;
|
282 | CONSTANT clkTosqrt : integer range 0 to 127 := 1;
|
283 | CONSTANT clkTodiv : integer range 0 to 127 := 2;
|
284 | CONSTANT clkFPtoInt : integer range 0 to 127 := 1;
|
285 | CONSTANT clkToAtan : integer range 0 to 127 := 25;
|
286 | CONSTANT clkToAcos : integer range 0 to 127 := 25;
|
287 | CONSTANT clkToCos : integer range 0 to 127 := 25;
|
288 | CONSTANT clkAtan : integer range 0 to 127 := 30;
|
289 | CONSTANT clkAtan32 : integer range 0 to 127 := 11;
|
290 | CONSTANT clkdiv32 : integer range 0 to 127 := 32;
|
291 |
|
292 | CONSTANT clkTosin : integer range 0 to 127 := 1;
|
293 | CONSTANT clkToMuli : integer range 0 to 127 := 2;
|
294 | CONSTANT C2 :std_logic_vector(31 DOWNTO 0):=X"40000000";-- zahl 2 in FP format 32bit
|
295 | CONSTANT C1 :std_logic_vector(31 DOWNTO 0):=X"3F800000";-- zahl 1 in FP format 32bit
|
296 | CONSTANT C0 :std_logic_vector(31 DOWNTO 0):=X"00000000";-- zahl 0 in FP format 32bit
|
297 | CONSTANT PIdiv2 :std_logic_vector(31 DOWNTO 0):=X"3FC90FDB";-- +1.57079632679 PI/2 in FP Format
|
298 | CONSTANT PIndiv2 :std_logic_vector(31 DOWNTO 0):=X"BFC90FDB";-- -1.57079632679 PI/2 in FP Format
|
299 | CONSTANT PI :std_logic_vector(31 DOWNTO 0):=X"40490FDB";-- 3.14159265359 PI in FP Format
|
300 |
|
301 | -------
|
302 | CONSTANT C132 :std_logic_vector(31 DOWNTO 0):=X"20000000";--zahl 1 in format 1Q32 bit
|
303 | CONSTANT C126 :std_logic_vector(25 DOWNTO 0):="01" & X"000000";--zahl 1 in format 1Q26 bit
|
304 | CONSTANT C026 :std_logic_vector(25 DOWNTO 0):="00" & X"000000";-- zahl 0 in FP format 26bit
|
305 | CONSTANT Pihalf :std_logic_vector(25 DOWNTO 0):="001" & "10010010000111111011000";--Pi Half in 2Q26
|
306 | ----------
|
307 | --Anzahl Inkremente / Ummdrehung FP=für 20'000 = X"469c4000"
|
308 | -- Resol = 500 , 2PI / 500 = 0.0125 = 3c4ccccd
|
309 | -- 2PI/1000 =0.00628318531 = 3bcde32e
|
310 | -- 2Pi/2000 =0.00314159265 = 3b4de32e
|
311 | -- 2PI/ 1440 =0.00436332313 = 3b8efa35
|
312 | -- 2PI/ 3600 = 0.00174532925 = 3ae4c388,1 Inkrement in rad FP bei 3600 Inkr pro U
|
313 | -- 2PI/5000 = 0.00125663706 = 3aa4b5be
|
314 | -- 2PI/10000 = 0.00062831853 = 3a24b5be
|
315 | -- 2PI /20000 =0.00031415927= X"39a4b5be" = 1 Inkrement in rad FP bei 20000 Inkr pro U
|
316 | ---------------------------------------------------------------------------------
|
317 | -----------3ae4c388 = 0.00174532923847 = 1 Inkrement in Rad,FP=2Pi/inkr pro U vo Rob Arm
|
318 | --CONSTANT A0res :std_logic_vector(31 DOWNTO 0):=X"37ce3750";--=0.00002458287 rad/inkr
|
319 | --CONSTANT A1res :std_logic_vector(31 DOWNTO 0):=X"37d4f7a6";--=0.00002538769
|
320 | --CONSTANT A2res :std_logic_vector(31 DOWNTO 0):=X"3858f501";--=0.00005172659
|
321 | --CONSTANT A3res :std_logic_vector(31 DOWNTO 0):=X"38532aa6";--=0.00005034604
|
322 | --CONSTANT A4res :std_logic_vector(31 DOWNTO 0):=X"3a548e6e";--=0008108382
|
323 | --CONSTANT A5res :std_logic_vector(31 DOWNTO 0):=X"38b847d0";--=0.0000878718
|
324 | -----------------------------------------------------------------------
|
325 | --CONSTANT C_A0res :std_logic_vector(31 DOWNTO 0):=X"48799a00";--255592 inkr /U
|
326 | --CONSTANT C_A1res :std_logic_vector(31 DOWNTO 0):=X"4871b2c0";--247499 inkr/U
|
327 | --CONSTANT C_A2res :std_logic_vector(31 DOWNTO 0):=X"47ed3e80";--121469 inkr/U
|
328 | --CONSTANT C_A3res :std_logic_vector(31 DOWNTO 0):=X"47f3c000";--124800 inkr/U
|
329 | --CONSTANT C_A4res :std_logic_vector(31 DOWNTO 0):=X"45f22800";--7749 inkr/U
|
330 | --CONSTANT C_A5res :std_logic_vector(31 DOWNTO 0):=X"478ba800";--71504 inkr/U
|
331 | --------------------------------------------------------------------
|
332 | CONSTANT C_A0rad :std_logic_vector(31 DOWNTO 0):=X"471ee6b3";--40678.70 inkr /rad
|
333 | CONSTANT C_A1rad :std_logic_vector(31 DOWNTO 0):=X"4719deae";--39390.68 inkr /rad
|
334 | CONSTANT C_A2rad :std_logic_vector(31 DOWNTO 0):=X"469708c8";--19332.39 inkr /rad
|
335 | CONSTANT C_A3rad :std_logic_vector(31 DOWNTO 0):=X"469b2d0f";--19862.53 inkr /rad
|
336 | CONSTANT C_A4rad :std_logic_vector(31 DOWNTO 0):=X"449a28f6";--1233.28 inkr /rad
|
337 | CONSTANT C_A5rad :std_logic_vector(31 DOWNTO 0):=X"4631d0d7";--11380.21 inkr /rad
|
338 | ---------------------Virtuelle Inkremente-------------------------------------------
|
339 | --CONSTANT C_A0rad :std_logic_vector(31 DOWNTO 0):=X"43fa0000";--10000 inkr /rad:461c4000
|
340 | --CONSTANT C_A1rad :std_logic_vector(31 DOWNTO 0):=X"43fa0000";--5000 inkr/rad:459c4000
|
341 | --CONSTANT C_A2rad :std_logic_vector(31 DOWNTO 0):=X"43fa0000";--3000 inkr/rad:453b8000
|
342 | --CONSTANT C_A3rad :std_logic_vector(31 DOWNTO 0):=X"43fa0000";--1000 inkr/rad:447a0000
|
343 | --CONSTANT C_A4rad :std_logic_vector(31 DOWNTO 0):=X"43fa0000";--500 inkr/rad:43fa0000
|
344 | --CONSTANT C_A5rad :std_logic_vector(31 DOWNTO 0):=X"43fa0000";--200 Inkr/rad:43480000
|
345 | ------------------------------------------------------------------
|
346 | CONSTANT A12res :std_logic_vector(31 DOWNTO 0):=X"3ae4c388";
|
347 | CONSTANT AGres :std_logic_vector(31 DOWNTO 0):= X"3ae4c388";--
|
348 | ------------------Kommt später über Config Darei -------------
|
349 | CONSTANT C_Xs :STD_LOGIC_VECTOR(31 DOWNTO 0):= X"000001F4";-- 500
|
350 | CONSTANT C_Ys :STD_LOGIC_VECTOR(31 DOWNTO 0):= X"FFFFFF38";-- -200
|
351 | CONSTANT C_Zs :STD_LOGIC_VECTOR(31 DOWNTO 0):= X"00000078";-- 120
|
352 | ----------------------wird im TOP gesetzt --------------------
|
353 | --CONSTANT C_pfrq :STD_LOGIC_VECTOR(23 DOWNTO 0):= X"006400";--ITP frqPeriode:25600*10ns=256uS=3.9kHz
|
354 | --CONSTANT C_pfrq :STD_LOGIC_VECTOR(23 DOWNTO 0):= X"000320";--ITP frqPeriode:800*10ns=8uS=125kHz
|
355 | --CONSTANT C_pfrq :STD_LOGIC_VECTOR(23 DOWNTO 0):= X"000190";--ITP frqPeriode:400*10nS=4uS=250KHz
|
356 | --CONSTANT C_pfrq :STD_LOGIC_VECTOR(23 DOWNTO 0):= X"0000C8";--ITP frqPeriode:200*10nS=2uS=500Khz
|
357 | --CONSTANT C_pfrq :STD_LOGIC_VECTOR(23 DOWNTO 0):= X"000064";--ITP frqPeriode:100*10nS=1uS=1Mhz
|
358 |
|
359 | ----------------------------------------------------------
|
360 | --==============================================================================
|
361 | SIGNAL stateIni :integer range 0 to 31:= 0;
|
362 | SIGNAL stateITP :integer range 0 to 31:= 0;
|
363 | SIGNAL staterunX :integer range 0 to 7:= 0;
|
364 | SIGNAL staterunY :integer range 0 to 7:= 0;
|
365 | SIGNAL staterunZ :integer range 0 to 7:= 0;
|
366 | SIGNAL staterunLk :integer range 0 to 7:= 0;
|
367 | SIGNAL stateangle :integer range 0 to 63:= 0;
|
368 | SIGNAL stateLinX :integer range 0 to 7:= 0;
|
369 | SIGNAL stateLinY :integer range 0 to 7:= 0;
|
370 | SIGNAL statechg :integer range 0 to 7:= 0;
|
371 | SIGNAL ITPSTATE :Integer range 0 to 2 := 0;
|
372 | -----------------------------------------------
|
373 | --========= Calc ALL Modul==================================
|
374 | SIGNAL stateAll :STATE_TYPE;
|
375 | --========================================================================
|
376 | --============= LinStz Interpolator Signale ==============================
|
377 | SIGNAL sitprun :std_logic:='0';--itprun für interne gebrauch
|
378 | --======= runLinX,runLinY signale======================================
|
379 | SIGNAL stateLin :STATE_TYPE;
|
380 | SIGNAL stateLinB :STATE_TYPE;
|
381 | SIGNAL linLka :INTEGER:= 0;
|
382 | SIGNAL LinLk :INTEGER:= 0;-- mAXAX für Linstz
|
383 | SIGNAL dLklin :INTEGER:= 0;
|
384 | SIGNAL dXlin :INTEGER:= 0;
|
385 | SIGNAL dYlin :INTEGER:= 0;
|
386 | SIGNAL XlinAbs :std_logic_vector(15 DOWNTO 0):=X"0000";
|
387 | SIGNAL YLinAbs :std_logic_vector(15 DOWNTO 0):=X"0000";
|
388 | SIGNAL Xlina :std_logic_vector(15 DOWNTO 0):=X"0000";
|
389 | SIGNAL YLina :std_logic_vector(15 DOWNTO 0):=X"0000";
|
390 | SIGNAL Xdifa :std_logic_vector(15 DOWNTO 0):=X"0000";
|
391 | SIGNAL Ydifa :std_logic_vector(15 DOWNTO 0):=X"0000";
|
392 | SIGNAL FLinXneg :std_logic:='0';--Flag koord ist negativ
|
393 | SIGNAL FLinYneg :std_logic:='0';--
|
394 | SIGNAL LinITPok :std_logic:='0';--
|
395 | SIGNAL LinRun :std_logic:='0';--
|
396 | SIGNAL ImpLkLin :std_logic:='1';--Lk Impulse von LinITP
|
397 | --=============================================================================
|
398 | SIGNAL lincalcOK :std_logic:='0';--start für RAMP von LinSatz
|
399 | --======== Ende Signale LinStz Verarbeitung =============================
|
400 | SIGNAL XYZcalcOK :std_logic:='0';--start für RAMP von XYZSatz
|
401 | --====================XYZ ITP============================================================
|
402 | -----------iniITPxyz Signale----------------------------------------
|
403 | SIGNAL YXTneg :std_logic:='0';--XT < YT then YXTneg :=1
|
404 | ----------------------
|
405 | ---SIGNAL cefrqFP :std_logic:='0';-- start Int to FP frqPER =>frqFP
|
406 | --SIGNAL frqPER :std_logic_vector(31 DOWNTO 0):=X"00000000";-- FrqPeriode, Steuerung ITP frq
|
407 | --SIGNAL frqFP :std_logic_vector(31 DOWNTO 0):=X"00000000";-- frq FP
|
408 | --------------------
|
409 | SIGNAL FXt :unsigned(31 DOWNTO 0):=(OTHERS => '0');
|
410 | SIGNAL FYt :unsigned(31 DOWNTO 0):=(OTHERS => '0');
|
411 | SIGNAL FZt :unsigned(31 DOWNTO 0):=(OTHERS => '0');
|
412 | SIGNAL FXtabs :unsigned(31 DOWNTO 0):=(OTHERS => '0');-- abs wert für crxdiv usw.
|
413 | SIGNAL FYtabs :unsigned(31 DOWNTO 0):=(OTHERS => '0');
|
414 | SIGNAL FZtabs :unsigned(31 DOWNTO 0):=(OTHERS => '0');
|
415 | ----- state3-----------------------------
|
416 | SIGNAL cesqXYZ :std_logic:='0';-- start sq
|
417 | SIGNAL sqFXt :unsigned(31 DOWNTO 0):=(OTHERS => '0');
|
418 | SIGNAL sqFYt :unsigned(31 DOWNTO 0):=(OTHERS => '0');
|
419 | SIGNAL sqFZt :unsigned(31 DOWNTO 0):=(OTHERS => '0');
|
420 | ---------------state 4 -----------------------
|
421 | SIGNAL ceaddXY :std_logic:='0';-- start Add XY
|
422 | SIGNAL addXY :std_logic_vector(31 DOWNTO 0):=(OTHERS => '0');
|
423 | ---------------state 5 ----------------------
|
424 | SIGNAL ceaddXYZ :std_logic:='0';-- start Add XYZ
|
425 | SIGNAL addXYZ :std_logic_vector(31 DOWNTO 0):=(OTHERS => '0');
|
426 | --------------- state 6--------------------------
|
427 | SIGNAL cesqrtFLk :std_logic:='0';-- start sqrt
|
428 | SIGNAL FLk :std_logic_vector(31 DOWNTO 0):=(OTHERS => '0');--FLk Länge in XT,YT,ZT
|
429 | SIGNAL FLT :std_logic_vector(31 DOWNTO 0):=(OTHERS => '0');--FLT länge in XT,YT
|
430 | SIGNAL XTLTdiv :std_logic_vector(31 DOWNTO 0):=(OTHERS => '0');--XTLTdiv:=FXT/FLT
|
431 | --------state10------------------------
|
432 | SIGNAL ceTgAT :std_logic:='0';-- Start catgAT
|
433 | SIGNAL AZFP :std_logic_vector(31 DOWNTO 0):=(OTHERS => '0');--AZFP= Winkel Strecke ZT
|
434 | -------state 11------------------------------
|
435 | SIGNAL cecmpAT :std_logic:='0';
|
436 | SIGNAL ATgrPI2 :STD_LOGIC_VECTOR(0 DOWNTO 0):=(others => '0');
|
437 | SIGNAL grPI2 :std_logic:='0';
|
438 | SIGNAL ATFP :std_logic_vector(31 DOWNTO 0):=(OTHERS => '0');--ATFP= Winkel Strecke YT XT
|
439 | SIGNAL initITPok :std_logic:='0';--initITP fertig
|
440 | SIGNAL ITPlin :STD_LOGIC_VECTOR(2 DOWNTO 0):= "000";
|
441 | --================== InitITPlin fertig====================================
|
442 | -----------------------------------------------------
|
443 | --==========RUNITP Signale(Interpolator für virtuelle Achsen XYZ )==========
|
444 | SIGNAL ITPxyz :STD_LOGIC_VECTOR(2 DOWNTO 0):= "000";
|
445 | SIGNAL XTabs :std_logic_vector(31 downto 0):=(OTHERS => '0');-- Targetkord(XT= abs
|
446 | SIGNAL YTabs :std_logic_vector(31 downto 0):=(OTHERS => '0');-- Targetkord(YT= abs
|
447 | SIGNAL ZTabs :std_logic_vector(31 downto 0):=(OTHERS => '0');-- Targetkord(ZT= avs
|
448 | SIGNAL fXneg :std_logic:='0';-- negativ Flag X für Xe - Xs
|
449 | SIGNAL fYneg :std_logic:='0';-- negativ Flag X für Ye - Ys
|
450 | SIGNAL fZneg :std_logic:='0';-- negativ Flag X für Ze - Zs
|
451 | SIGNAL ceXYZF :std_logic:='0';-- start für initITPlin
|
452 | SIGNAL runXYZ :std_logic:='0';-- Proc IMpX..IMpZ aktiv
|
453 | SIGNAL ceFlkInt :std_logic:='0';-- Flk to LK
|
454 | SIGNAL XYZok :std_logic:='0';-- XYZbresenham fertig
|
455 | ------------Signale für Bresenham process : brshXYZ ----------------------------
|
456 | SIGNAL statebrsh :INTEGER RANGE 0 TO 7 := 0;
|
457 | SIGNAL dLk :integer:= 0;
|
458 | SIGNAL dX :integer:= 0;
|
459 | SIGNAL dY :integer:= 0;
|
460 | SIGNAL dZ :integer:= 0;
|
461 | SIGNAL Lk :std_logic_vector(31 downto 0):=(OTHERS => '0');--leitkoordinate
|
462 | SIGNAL Lka :integer;-- Aktuelle Virtuelle Koordinate Lk
|
463 | SIGNAL strtITP :std_logic:='0';-- start für XYZ itp
|
464 | ---------------------------------------------------------------------------
|
465 | --------------Signale für impX, generiert Xa---------------------------------
|
466 | SIGNAL Xa :std_logic_vector(31 downto 0):=(OTHERS => '0');-- Aktuelle Koordinate X
|
467 | SIGNAL ImpX :std_logic:='1';-- inkrement Impuls
|
468 | --------------Signale für impY-----generiert Ya----------------------------
|
469 | SIGNAL Ya :std_logic_vector(31 downto 0):=(OTHERS => '0');-- Aktuelle Koordinate Y
|
470 | SIGNAL ImpY :std_logic:='1';-- inkrement Impuls
|
471 | --------------Signale für impZ----generiert Za-----------------------------
|
472 | SIGNAL Za :std_logic_vector(31 downto 0):=(OTHERS => '0');-- Aktuelle Koordinate z
|
473 | SIGNAL ImpZ :std_logic:='1';-- inkrement Impuls
|
474 | --------------------------------------------------------
|
475 | ------------------------generiert ImpLk----------------------------
|
476 | SIGNAL ctrPLk :Integer range 0 to 33554431:=0;-- dekr. Counter für Imp. Periode
|
477 | SIGNAL ImpLk :std_logic:='1';-- inkrement virtuelle Lk Impuls von XYZ ITP (invers Trans)
|
478 | --======================================================================
|
479 | -----RUNANGLE Berechnung : a0 := arctan(Ya/Xa) und aXYT:=arctan(YT/XT) Berechnung-------------------------------
|
480 | SIGNAL ceXYZFP :std_logic:='0';
|
481 | ----------------------
|
482 | SIGNAL FXa :std_logic_vector(31 DOWNTO 0):=(OTHERS => '0');--Aktuelle X Position FP
|
483 | SIGNAL FYa :std_logic_vector(31 DOWNTO 0):=(OTHERS => '0');
|
484 | SIGNAL FZa :std_logic_vector(31 DOWNTO 0):=(OTHERS => '0');
|
485 | SIGNAL AZFPabs :std_logic_vector(31 DOWNTO 0):=(OTHERS => '0');-- abs werte
|
486 | -----------------------state 0---------------------
|
487 | SIGNAL FXYZaOK :std_logic:= '0';--signalisiert wenn FXYZa bereit
|
488 | SIGNAL suPIAT :std_logic_vector(31 downto 0):=(OTHERS => '0');
|
489 | SIGNAL adPIAT :std_logic_vector(31 downto 0):=(OTHERS => '0');
|
490 | ----------------- state 2----------------------------------
|
491 | SIGNAL cePI2 :std_logic:='0';-- start für sPIAT := PI/2 - PIATabs für sin AT
|
492 | SIGNAL PIAT :std_logic_vector(31 downto 0):=(OTHERS => '0');
|
493 | SIGNAL PIATabs :std_logic_vector(31 downto 0):=(OTHERS => '0');
|
494 | SIGNAL sPIAT :std_logic_vector(31 downto 0):=(OTHERS => '0');
|
495 | ---------------- state 3 ----------------------------------
|
496 | SIGNAL cetgFP :std_logic:='0';
|
497 | SIGNAL A0FP :std_logic_vector(31 DOWNTO 0):=(OTHERS => '0');--32 bit Resultat von ArcTanFP
|
498 | SIGNAL PIAZ :std_logic_vector(31 downto 0):=(OTHERS => '0');--Res Pi/2 - AZFP
|
499 | SIGNAL sinAZ :std_logic_vector(31 downto 0):=(OTHERS => '0');--
|
500 | SIGNAL cosAZ :std_logic_vector(31 downto 0):=(OTHERS => '0');--
|
501 | SIGNAL cosAT :std_logic_vector(31 downto 0):=(OTHERS => '0');--
|
502 | SIGNAL sinAT :std_logic_vector(31 downto 0):=(OTHERS => '0');--
|
503 | -----------------State 9 ---------------------------------
|
504 | SIGNAL cemul9 :std_logic:='0';
|
505 | SIGNAL sAZAT :std_logic_vector(31 downto 0):=(OTHERS => '0');--
|
506 | SIGNAL cAZAT :std_logic_vector(31 downto 0):=(OTHERS => '0');--
|
507 | -----------------State 10 -----------------------------------
|
508 | SIGNAL cemul10 :std_logic:='0';
|
509 | SIGNAL sAZAT3 :std_logic_vector(31 downto 0):=(OTHERS => '0');--
|
510 | SIGNAL cAZAT3 :std_logic_vector(31 downto 0):=(OTHERS => '0');--
|
511 | SIGNAL cAZAZ3 :std_logic_vector(31 downto 0):=(OTHERS => '0');--
|
512 | -----------------State 11---------------------------
|
513 | SIGNAL cesubW :std_logic:='0';
|
514 | SIGNAL aFXw :std_logic_vector(31 downto 0):=(OTHERS => '0');--
|
515 | SIGNAL sFXw :std_logic_vector(31 downto 0):=(OTHERS => '0');--
|
516 | SIGNAL FXw :std_logic_vector(31 downto 0):=(OTHERS => '0');--
|
517 | SIGNAL FYw :std_logic_vector(31 downto 0):=(OTHERS => '0');--
|
518 | SIGNAL FZw :std_logic_vector(31 downto 0):=(OTHERS => '0');--
|
519 | ---------------- State 12------------------------------
|
520 | SIGNAL cemul12 :std_logic:='0';
|
521 | SIGNAL sqXw :std_logic_vector(31 downto 0):=(OTHERS => '0');--
|
522 | SIGNAL sqYw :std_logic_vector(31 downto 0):=(OTHERS => '0');--
|
523 | SIGNAL subATAW :std_logic_vector(31 downto 0):=(OTHERS => '0');--
|
524 |
|
525 | ---------------state 13-----------------------------------
|
526 | SIGNAL subPIdiv :std_logic_vector(31 downto 0):=(OTHERS => '0');--
|
527 | SIGNAL ceaddXYw :std_logic:='0';
|
528 | SIGNAL sqXYw :std_logic_vector(31 downto 0):=(OTHERS => '0');--
|
529 | SIGNAL AwFP :std_logic_vector(31 downto 0):=(OTHERS => '0');--
|
530 | ---------------- State 14 ---------------------------------
|
531 | SIGNAL cesqrtLw :std_logic:='0';
|
532 | SIGNAL cemAZA3 :std_logic:='0';
|
533 | SIGNAL LwFP :std_logic_vector(31 downto 0):=(OTHERS => '0');--
|
534 | SIGNAL DivPIdiv :std_logic_vector(31 downto 0):=(OTHERS => '0');--
|
535 | ------------------state 15----------------------------
|
536 | SIGNAL cedAZA3 :std_logic:='0';
|
537 | SIGNAL sqZ2 :std_logic_vector(31 downto 0):=(OTHERS => '0');--
|
538 | SIGNAL sqW2 :std_logic_vector(31 downto 0):=(OTHERS => '0');--
|
539 | ------------------state 16 -----------------------------------
|
540 | SIGNAL cedivPIdiv :std_logic:='0';
|
541 | SIGNAL ceA4FP :std_logic:='0';
|
542 | SIGNAL sqaddWZ :std_logic_vector(31 downto 0):=(OTHERS => '0');--
|
543 | ----------------- state 17 -----------------------------------
|
544 | SIGNAL ceAZmul :std_logic:='0';
|
545 | SIGNAL FL12 :std_logic_vector(31 downto 0);--------:=(OTHERS => '0');--
|
546 | SIGNAL A12FP :std_logic_vector(31 downto 0);---------:=(OTHERS => '0');--
|
547 | SIGNAL runCalcP1 :std_logic:='0';-- start für calcP1
|
548 | ----------------- state 18 --------------------------------
|
549 | SIGNAL A4FP :std_logic_vector(31 downto 0):=(OTHERS => '0');--
|
550 | SIGNAL absdivPIdiv :std_logic_vector(31 downto 0):= (OTHERS => '0');--
|
551 | ----------------- state 19 --------------------------------
|
552 | SIGNAL sub1min :std_logic_vector(31 downto 0):= (OTHERS => '0');--
|
553 | SIGNAL cesub1min :std_logic:='0';
|
554 | ----------------- state 20 --------------------------------
|
555 | SIGNAL ceA3FP :std_logic:='0';
|
556 | SIGNAL A3FP :std_logic_vector(31 downto 0):=(OTHERS => '0');--
|
557 | --===============================================================
|
558 | ----------------state 22-- nur für test -----------------
|
559 | SIGNAL ceXYZw :std_logic:='0';
|
560 | SIGNAL Xw :std_logic_vector(31 downto 0):=(OTHERS => '0');--
|
561 | SIGNAL Yw :std_logic_vector(31 downto 0):=(OTHERS => '0');--
|
562 | SIGNAL Zw :std_logic_vector(31 downto 0):=(OTHERS => '0');--
|
563 | --======================run angle fertig=============================================
|
564 | SIGNAL A5FP :std_logic_vector(31 downto 0):=X"00000001";--
|
565 | --------------Signale für XYZ Modul-------------------------------
|
566 | SIGNAL L4 :std_logic_vector(31 downto 0):=(OTHERS => '0');
|
567 | SIGNAL AXYT :std_logic_vector(19 DOWNTO 0):=(OTHERS => '0');--20 bit integer output
|
568 | ----------------------------------
|
569 | --***************************************************************************
|
570 | --======================================================
|
571 | ---------calcALL------------------------------
|
572 | SIGNAL FL1 :std_logic_vector(31 DOWNTO 0);-----:= (OTHERS => '0');
|
573 |
|
574 | SIGNAL FL2 :std_logic_vector(31 DOWNTO 0);-------:= (OTHERS => '0');
|
575 |
|
576 | SIGNAL FL3 :std_logic_vector(31 DOWNTO 0):= (OTHERS => '0');
|
577 | SIGNAL FL4 :std_logic_vector(31 DOWNTO 0):= (OTHERS => '0');
|
578 | SIGNAL FW1 :std_logic_vector(31 DOWNTO 0):= (OTHERS => '0');
|
579 |
|
580 | SIGNAL FW2 :std_logic_vector(31 DOWNTO 0):= (OTHERS => '0');
|
581 | SIGNAL FZ1 :std_logic_vector(31 DOWNTO 0):= (OTHERS => '0');
|
582 |
|
583 | SIGNAL FZ2 :std_logic_vector(31 DOWNTO 0):= (OTHERS => '0');
|
584 | SIGNAL FA1 :std_logic_vector(31 DOWNTO 0):= (OTHERS => '0');
|
585 | SIGNAL FA2 :std_logic_vector(31 DOWNTO 0):= (OTHERS => '0');
|
586 | SIGNAL ceLFP :std_logic:='0';--FP conversion enable
|
587 | -------------------------------------------------------------
|
588 | --========================================================================
|
589 | ------------RUNANGLE vorher-Signale für calcP1------------------
|
590 | SIGNAL ceP1 :std_logic:='0';--start sq für P1
|
591 | SIGNAL StrtP3 :std_logic:='0';-- start für parallel ablauf P3
|
592 | SIGNAL cesqL :std_logic:='0';
|
593 | SIGNAL sqL1 :std_logic_vector(31 DOWNTO 0):=(OTHERS => '0');
|
594 | SIGNAL sqL2 :std_logic_vector(31 DOWNTO 0):=(OTHERS => '0');
|
595 | SIGNAL sqL12 :std_logic_vector(31 DOWNTO 0):=(OTHERS => '0');
|
596 | SIGNAL mul12 :std_logic_vector(31 DOWNTO 0):=(OTHERS => '0');
|
597 |
|
598 | SIGNAL ceadd1 :std_logic:='0';
|
599 | SIGNAL sqadd1 :std_logic_vector(31 DOWNTO 0):= (OTHERS => '0');
|
600 | SIGNAL cesub1 :std_logic:='0';
|
601 | SIGNAL sqsub1 :std_logic_vector(31 DOWNTO 0):= (OTHERS => '0');
|
602 | SIGNAL cem32 :std_logic:='0';
|
603 | SIGNAL mulres1 :std_logic_vector(31 DOWNTO 0):= (OTHERS => '0');
|
604 |
|
605 |
|
606 | SIGNAL cediv1 :std_logic:='0';
|
607 | SIGNAL ax :std_logic_vector(31 DOWNTO 0):= (OTHERS => '0');
|
608 |
|
609 | ---------------------------
|
610 | SIGNAL ceacosFP :std_logic:='0';-- Start arccosFP
|
611 | SIGNAL acosAX :std_logic_vector(31 DOWNTO 0):= (OTHERS => '0');
|
612 | SIGNAL asinAX :std_logic_vector(31 DOWNTO 0):= (OTHERS => '0');-- wird nicht benützt
|
613 | SIGNAL ceA1FP :std_logic:='0';
|
614 | SIGNAL A1FP :std_logic_vector(31 DOWNTO 0):=(OTHERS => '0');
|
615 | SIGNAL cesubA1 :std_logic:='0';
|
616 | SIGNAL A1SFP :std_logic_vector(31 DOWNTO 0):= (OTHERS => '0');
|
617 | SIGNAL cecosFP :std_logic:='0';
|
618 | SIGNAL cosA1FP :std_logic_vector(31 DOWNTO 0):= (OTHERS => '0');--1te resultat cosinFP
|
619 |
|
620 | SIGNAL sinA1FP :std_logic_vector(31 DOWNTO 0):= (OTHERS => '0');--2te resultat cosinFP
|
621 |
|
622 | ---------------------
|
623 | SIGNAL ceZ1W1 :std_logic:='0';
|
624 |
|
625 | ------ RUNANGLE vorher Calc P1 blatt1 fertig, Blatt2 anfang
|
626 | SIGNAL ceZW12 :std_logic:='0';
|
627 | SIGNAL Z2Z1 :std_logic_vector(31 DOWNTO 0):= (OTHERS => '0');
|
628 | SIGNAL W2W1 :std_logic_vector(31 DOWNTO 0):= (OTHERS => '0');
|
629 | SIGNAL cedivZW :std_logic:='0';
|
630 | SIGNAL divZW :std_logic_vector(31 DOWNTO 0):= (OTHERS => '0');
|
631 | SIGNAL ceatanZW :std_logic:='0';
|
632 | SIGNAL atanZW :std_logic_vector(31 DOWNTO 0):= (OTHERS => '0');
|
633 | SIGNAL A2FP :std_logic_vector(31 DOWNTO 0):= (OTHERS => '0');
|
634 | SIGNAL cesubAGA2 :std_logic:='0';
|
635 | SIGNAL AGA2 :std_logic_vector(31 DOWNTO 0):= (OTHERS => '0');
|
636 | SIGNAL cesubA2 :std_logic:='0';
|
637 | -------- RUNANGLE vorherende calc P1 ---------------------------------
|
638 | -----------------------------------------------------------------
|
639 | ----------ITPImpGen---------------------------------------------
|
640 | ---TYPE T_INKDEK is ARRAy(5 DOWNTO 0) of std_logic_vector(1 DOWNO 0); -- Achsen(Inc, Dec)
|
641 | SIGNAL AXinc :T_AXInc;--IS ARRAY(5 DOWNTO 0) OF SLV(31 DOWNTO 0);
|
642 | SIGNAL AXalt :T_AXInc;--IS ARRAY(5 DOWNTO 0) OF SLV(31 DOWNTO 0);
|
643 | SIGNAL vAXdif :T_AXInc;--IS ARRAY(5 DOWNTO 0) OF SLV(31 DOWNTO 0);
|
644 | SIGNAL AXdif :T_AXdif;--IS ARRAY(5 DOWNTO 0) OF SLV(15 DOWNTO 0);
|
645 | ------------------
|
646 | SIGNAL AXGr :T_Axr;-- Winkel der Achsen A0..A5 in GRD.X, (std Logic vector)
|
647 | SIGNAL ceAxx :std_logic:='0';-- start für Axconv für A1,A2 A12
|
648 |
|
649 | ----------------------
|
650 | SIGNAL A12 :std_logic_vector(19 DOWNTO 0):=X"00000";--20 bit integer output
|
651 | SIGNAL statITPimp :INTEGER RANGE 0 TO 31;
|
652 | SIGNAL AXinkr :std_logic_vector(191 downto 0):= (OTHERS => '0');--Sollwerte von inv.Transfer und Lin
|
653 | --======================================================================
|
654 | --**************************************************************************
|
655 |
|
656 | BEGIN
|
657 | --===================================================================================
|
658 | ---pfrq SLV(23 downto 0);--Bahn-Periode in anzahl CLK (10ns)
|
659 | --- frqPer :32 bit
|
660 | --FrqGen:PROCESS(clk)
|
661 | --VARIABLE cntrclk : INTEGER RANGE 0 TO 1000000;
|
662 | --BEGIN
|
663 | -- IF rising_edge(clk) THEN
|
664 | -- IF nrst = '0' THEN
|
665 | -- cntrclk := 0;
|
666 | -- cefrqFP <= '0';
|
667 | -- ELSE
|
668 | -- IF cntrclk > 0 THEN
|
669 | -- cntrclk := cntrclk - 1;
|
670 | -- cefrqFP <= '0';
|
671 | -- ELSE
|
672 | -- frqPER <= X"00" & pfrq;--ITP frqPeriode laden
|
673 | -- cefrqFP <= '1';-- start Int to FP für frqPer
|
674 | -- cntrclk := 4000; --- 20 us interval
|
675 | -- END IF;
|
676 | -- END IF; --IF nrst /ELSE
|
677 | -- END IF; --clk
|
678 | --END PROCESS;--FrqGen
|
679 | --============================================================================
|
680 | --=======Linear Interpolation in erinzelnen Achsen X,Y gemäss ebene, Kartesisch=====
|
681 | -- Generiert eine linearinterpolation in den AchsenXY gemäss konzept von Optinumeric
|
682 | -- Programmiert als lin Interpolationsatz mit nur 2 Achsen,:label 1
|
683 | --============================================================================
|
684 | --=============****************************************===================================
|
685 | ---====== XYZ Interpolation(invers Transformation) ablauf in den Achsen A5..A0 :ab Label 6
|
686 | --=========================================================================
|
687 | --Entity :oAXdif:std_logic_vector(95 downto 0):differenz von alle 6 Achsen(6 *16 bits)
|
688 | --SIGNAL AXdif :T_AXdif;--IS ARRAY(5 DOWNTO 0) OF SLV(15 DOWNTO 0);
|
689 | -- Liefert in der Entity Signal oAxdif(95..0):achsen solldifferenzen.
|
690 | -- später werden in TopAS21 in der Procedur SollInc(z6024) die Sollwerte für Alle 6 Achsen (SOLL)
|
691 | -- wobei diese als Input für Ramp dienen.
|
692 | --------------------------------------------------
|
693 | ImpSwitch:PROCESS(clk)
|
694 | VARIABLE Lin :std_logic := '0';--für lin=1, für XYZ = 0
|
695 | VARIABLE INDX :INTEGER RANGE 0 TO 127:= 0;
|
696 | VARIABLE RunStop :std_logic := '0';--speichert Stopp impuls
|
697 | -- bis Rampenende
|
698 | BEGIN
|
699 | IF rising_edge(clk) THEN
|
700 | IF nrst = '0' THEN
|
701 | itprun <= '0';
|
702 | sitprun <= '0';
|
703 | enditp <= '0';
|
704 | indx := 0;
|
705 | ITPmod <= "000";-- init ITmod
|
706 | lin:= '0';-- init
|
707 | ITPSTATE <= 0;
|
708 | statechg <= 0;
|
709 | oAXdif <= (OTHERS => '0');
|
710 | CalcOK <= '0';-- RESET IMPULS fürstart RAMP
|
711 | ELSE
|
712 | CASE ITPSTATE IS
|
713 | WHEN 0 =>
|
714 | IF celin = '1' THEN
|
715 | Lin := '1';
|
716 | indx := to_integer(unsigned(ebene(1 DOWNTO 0)));-- ebene ini Receiv gesetzt Byte(1) 2..0
|
717 | ITPSTATE <= 1;
|
718 | ELSIF ce = '1' THEN
|
719 | Lin := '0';
|
720 | ITPSTATE <= 1;
|
721 | ELSE ITPSTATE <= 0;
|
722 | END IF;
|
723 | WHEN 1 =>
|
724 | IF lin = '1' THEN
|
725 | ITPmod <= ITPlin;
|
726 | ELSE
|
727 | ITPmod <= ITPxyz;
|
728 | END IF;
|
729 | ---------------
|
730 | IF (XYZok = '1') OR (LinITPok = '1') THEN
|
731 | ITPmod <= "000";
|
732 | ITPSTATE <= 0;
|
733 | ELSE ITPSTATE <= 1;
|
734 | END IF;
|
735 | WHEN OTHERS => ITPSTATE <= 0;
|
736 | END CASE;
|
737 | ----------------------------
|
738 | CASE statechg IS
|
739 | WHEN 0 =>
|
740 | IF linrun = '1' THEN --Lin Itp pulsw
|
741 | itprun <= '1';--Itp Läuft
|
742 | sitprun <= '1';
|
743 | enditp <= '1';--itp Läuft
|
744 | statechg <= 1;
|
745 | ELSIF runXYZ = '1' THEN --InvwersTransf Impulse
|
746 | itprun <= '1';--Itp Läuft
|
747 | sitprun <= '1';
|
748 | enditp <= '1';--itp Läuft
|
749 | statechg <= 6;
|
750 | ELSE -- keine Interpolation, warten auf rampen ende resp neue stz
|
751 | statechg <= 0;-- und warten
|
752 | END IF;
|
753 | --==================================================================
|
754 | ------------------- LinSatz------------------------------------
|
755 | WHEN 1 => -- ablauf linStz
|
756 | IF linrun = '1' THEN
|
757 | IF LinITPok = '1' THEN
|
758 | itprun <= '0';-- lin ITP fertig
|
759 | sitprun <= '0';
|
760 | ELSE -- LINInterpolation läuft
|
761 | oImpLk <= ImpLkLin;--Impulse LK von LinITP
|
762 | calcOK <= lincalcOK;--start für RAMP
|
763 | --oAXinkr(31..0), Xlina(15..0)
|
764 | IF Xactiv = '1' THEN ---- WinkelAchsen Inkremente auf entity
|
765 | oAXdif(indx*32+15 DOWNTO indx*32) <= Xdifa;
|
766 | END IF;
|
767 | IF Yactiv = '1' THEN
|
768 | oAXdif(indx*32+31 DOWNTO indx*32+16) <= Ydifa;
|
769 | END IF;
|
770 | ----------------------------
|
771 | IF stopp = '1' OR RunStop = '1' THEN
|
772 | RunStop := '1'; --Stopp speichern
|
773 | itpRun <= '0'; -- Unterbruch wg limit oder Endlage
|
774 | sitprun <= '0';
|
775 | enditp <= '0';-- ende kontur
|
776 | ELSE
|
777 | ItpRun <= '1';-- ITP lin läufzt
|
778 | sitprun <= '1';
|
779 | enditp <= '1';-- ende kontur
|
780 | END IF;
|
781 | END IF;
|
782 | ELSE -- linRun fertig, warten auf rampenende
|
783 | IF RampRun = '1' THEN
|
784 | IF stopp = '1' OR RunStop = '1' THEN
|
785 | RunStop := '1'; --Stopp speichern
|
786 | END IF;
|
787 | statechg <= 1;
|
788 | ELSE
|
789 | enditp <= '0';
|
790 | RunStop := '0';
|
791 | statechg <= 0;-- ende satz, warten auf neue satz
|
792 | END IF;
|
793 | END IF;
|
794 | --==================================================================================------------------------XYZ Satz= inv.Transf-------------------------
|
795 | WHEN 6 => -- impuls ausgabe bei invers transf
|
796 | IF runXYZ = '1' THEN --interpolation (Bresenham) läuft,itprun=1
|
797 | oImpLk <= ImpLk;--virt.ImpulseLK von Inv.Transf (XYZ ITP,Bresenham)
|
798 | FOR i IN 0 TO 5 LOOP
|
799 | oAXdif(i*16+15 DOWNTO i*16) <= AXdif(i);-- WinkelAchsensolldif Inkremente auf entity
|
800 | END LOOP;
|
801 | calcOK <= XYZcalcOK;--start für RAMP
|
802 | IF stopp = '1' OR RunStop = '1' THEN
|
803 | ItpRun <= '0'; -- Unterbruch wg limit oder Endlage
|
804 | sitprun <= '0';
|
805 | RunStop := '1'; -- speichern Stopp
|
806 | ELSE
|
807 | IF XYZok = '1' THEN --- wenn Bressenham fertig
|
808 | ItpRun <= '0';
|
809 | enditp <= '0'; -- ende kontur
|
810 | sitprun <= '0';
|
811 | ELSE
|
812 | ItpRun <= '1';
|
813 | sitprun <= '1';
|
814 | enditp <= '1'; -- kontur läuft
|
815 | END IF;
|
816 | END IF;
|
817 | statechg <= 6;
|
818 | ELSE -- Bresenham Interpolation fertig
|
819 | IF RampRun = '1' THEN
|
820 | IF stopp = '1' OR RunStop = '1' THEN
|
821 | RunStop := '1'; --Stopp speichern
|
822 | END IF;
|
823 | statechg <= 6;
|
824 | ELSE
|
825 | RunStop := '0';
|
826 | statechg <= 0;-- ende satz, warten auf neue satz
|
827 | END IF;
|
828 | END IF; --nächste Impulsausgabe
|
829 | WHEN OTHERS => statechg <= 0;
|
830 | END CASE;
|
831 | END IF;--if nrst/else
|
832 | END IF;-- clk
|
833 | END PROCESS;-- IMPSwitch
|
834 | --=============================================================
|
835 | -- bei linstz werden nur 16 bit akzeptiert (ist nur zum optimieren der Achsen)
|
836 | -- d.h. Max eingabe = +/- 327.67 für X,oder Y Linsatz
|
837 | --==Steuert ITP für linStz ==============================
|
838 | -- LinLk (intg):-- Generiert aus MAXAX bei Empfang von Linstz
|
839 | -- Xlin,Ylin wurden bei Empfang von Linstz geladen
|
840 | -- generiert ITPlin (status ITP)
|
841 | ------------------------------------------------
|
842 | runITPlin:PROCESS(clk)
|
843 | VARIABLE cntrclk : INTEGER RANGE 0 TO 3000000:= 0;-- für 30ms max
|
844 | VARIABLE iEB :INTEGER RANGE 0 TO 2 := 0;-- zwischenspeicher für ebene
|
845 | BEGIN
|
846 | IF rising_edge(clk) THEN
|
847 | IF nrst = '0' THEN
|
848 | LinRun <= '0'; -- reset start Bresenhamlin
|
849 | dXlin <= 0;
|
850 | dYlin <= 0;
|
851 | dLkLin <= 0;
|
852 | StateLin <= S0;
|
853 | cntrclk := 0;
|
854 | iEB := 0;
|
855 | XLinAbs <= (OTHERS =>'0');
|
856 | YLinAbs <= (OTHERS =>'0');
|
857 | ITPlin <= "000"; --init ITP für Status
|
858 | ELSE
|
859 | CASE stateLin IS
|
860 | WHEN S0 => -- start mit celin
|
861 | IF ceLin = '1' THEN
|
862 | iEB := to_integer(unsigned(Ebene));-- Ebene für linSatz
|
863 | StateLin <= S1;
|
864 | ELSE
|
865 | StateLin <= S0;-- warten
|
866 | END IF;
|
867 | WHEN S1 => -- init div koordinaten
|
868 | ITPlin(iEB) <= '1';--Itp für aktive Ebene
|
869 | --ITPlin(conv_integer(iEB)) <= '1';--Itp für aktive Ebene,alte original
|
870 |
|
871 | -----------------
|
872 | IF Xlin(15) = '1' THEN -- negativ koord
|
873 | FLinXneg <= '1';
|
874 | XLin <= Xlin XOR X"FFFF";
|
875 | XLinAbs <= std_logic_vector(unsigned(Xlin) + 1);
|
876 | ELSE
|
877 | FLinXneg <= '0';
|
878 | XlinAbs <= Xlin;
|
879 | END IF;
|
880 | ----------------------
|
881 | IF Ylin(15) = '1' THEN -- negativ koord
|
882 | FLinYneg <= '1';
|
883 | YLinAbs <= (Ylin XOR X"FFFF") + 1;
|
884 | ELSE
|
885 | FLinYneg <= '0';
|
886 | YlinAbs <= Ylin;
|
887 | END IF;
|
888 | --------------------
|
889 | StateLin <= S2;
|
890 | WHEN S2 =>
|
891 | dXlin <= to_integer(XLinAbs);-- Xkoord
|
892 | dYlin <= to_integer(YLinAbs);
|
893 | StateLin <= S3;
|
894 | WHEN S3 =>
|
895 | IF dXLin >= dYlin THEN
|
896 | LinLk <= dXLin;--maxax setzen
|
897 | ELSE
|
898 | LinLk <= dYLin;
|
899 | END IF; -- für 2 status mldg 30 us
|
900 | cntrclk := 2000000;-- 20 mS warte zeit
|
901 | StateLin <= S4;
|
902 | WHEN S4 =>
|
903 | IF cntrclk > 0 THEN
|
904 | cntrclk := cntrclk -1;
|
905 | StateLin <= S4;
|
906 | ELSE
|
907 | dLklin <= LinLk;--Leit koord,
|
908 | ITPlin(conv_integer(iEB)) <= '0';--Itp init für status
|
909 | cntrclk := 170;-- 2.0 us min Impulslänge
|
910 | StateLin <= S5;---S5;----ohne start imp
|
911 | END IF;
|
912 | ---------auf startImpuls warten-------------------------
|
913 | WHEN S5 => --- warten auf Start Impuls länge min 2,4us
|
914 | IF cntrclk > 0 THEN
|
915 | cntrclk := cntrclk -1;
|
916 | IF strimp = '1' THEN
|
917 | stateLin <= S5;-- warten
|
918 | ELSE
|
919 | stateLin <= S6;-- impuls ist kürzer, neu anfangen
|
920 | END IF;
|
921 | ELSE -- zeit abgelaufen
|
922 | stateLin <= S7;-- impuls hat richtige länge
|
923 | END IF;
|
924 | WHEN S6 => -- neu anfangen mit Impuls abwarten
|
925 | cntrclk := 170;-- 2.0 us min Impulslänge
|
926 | stateLin <= S5;-- neu anfangen
|
927 | -----------start impuls empfangen------------------------
|
928 | WHEN S7 =>
|
929 | LinRun <= '1'; -- Bresenhamlin starten(impuls)
|
930 | ITPlin(to_integer(iEB)) <= '1';--Itp für status
|
931 | StateLin <= S8;
|
932 | WHEN S8 =>
|
933 | StateLin <= S9;
|
934 | WHEN S9 => -- warten bis interpolation (ITP) fertig ist
|
935 | IF LinITPok = '1' THEN
|
936 | LinRun <= '0';
|
937 | ITPlin(to_integer(iEB)) <= '0';--Itp für status(itp fertig)
|
938 | StateLin <= S10;-- und weiter
|
939 | ELSE
|
940 | StateLin <= S9;-- warten bis ITP fertig ist
|
941 | END IF;
|
942 | WHEN S10 =>
|
943 | StateLin <= S0;-- und Warten auf nächste LInStz
|
944 | WHEN OTHERS =>
|
945 | dXlin <= 0;
|
946 | dYlin <= 0;
|
947 | dLkLin <= 0;
|
948 | StateLin <= S0;
|
949 | END CASE;
|
950 | END IF;-- if nrst/else
|
951 | END IF; -- if clk
|
952 | END PROCESS;-- runITPlin
|
953 | --=================================================================
|
954 | --=====================Bresenham Interpolation für linStz ==========
|
955 | --- interpoliert in XY eine gewählte Ebene in LinStz(linear Satz)
|
956 | -- ImpXlin,ImpYin sind Impulse in XY,,
|
957 | --ImpLklin sind virtuelle Impulse in entlang der Leitkoordinate
|
958 | -- XlinA,YlinA ist absolute Position
|
959 | -- Ende der Interpolation : LinItpok = 1 (2 clk impuls)
|
960 | --Vorschubspeed definiert mit pfrq (anz.clk(10ns) zwischen
|
961 | --zwei ImpLklin impulsen
|
962 | -------------------------------------------------------
|
963 | breshamLin: PROCESS(clk)
|
964 | VARIABLE count :INTEGER;
|
965 | VARIABLE errX :INTEGER;
|
966 | VARIABLE errY :INTEGER;
|
967 | VARIABLE XaltA :unsigned(15 DOWNTO 0):=X"0000";
|
968 | VARIABLE YaltA :unsigned(15 DOWNTO 0):=X"0000";
|
969 | BEGIN
|
970 | IF rising_edge(clk) THEN
|
971 | IF nrst = '0' THEN
|
972 | stateLinB <= S0;
|
973 | errX :=0;
|
974 | errY :=0;
|
975 | XLinA <= (OTHERS =>'0');
|
976 | YLinA <= (OTHERS =>'0');
|
977 | LinItpok <= '0';
|
978 | count := 0;
|
979 | ImpLkLin <= '1';--Impulse (Lk)von LinITP
|
980 | LinCalcOK <= '0';
|
981 | ELSE
|
982 | CASE statelinB IS
|
983 | WHEN S0 =>
|
984 | IF LinRun = '1' THEN
|
985 | count := to_integer(pfrq);
|
986 | stateLinB <= S1;
|
987 | ELSE
|
988 | errX :=dLklin/2;
|
989 | errY :=dLklin/2;
|
990 | LinLka <= 0;
|
991 | LinItpok <= '0';
|
992 | stateLinB <= S0;
|
993 | END IF;
|
994 | WHEN S1 =>
|
995 | LinCalcOK <= '0';-- RESET IMPULS calcOK
|
996 | IF count = 0 THEN -- warten für pfrq periode
|
997 | IF LinLka < linLk THEN --wenn noch nicht fertig
|
998 | LinLka <= LinLka + 1;
|
999 | ------------------------
|
1000 | errX := errX - dXlin;
|
1001 | IF errX < 0 THEN
|
1002 | XlinA <= XlinA + 1;
|
1003 | ImpLklin <= '0';
|
1004 | errX := errX + dLkLin;
|
1005 | END IF;
|
1006 | ----------------------
|
1007 | errY := errY - dYlin;
|
1008 | IF errY < 0 THEN
|
1009 | YLinA <= YLinA + 1;
|
1010 | ImpLklin <= '0';
|
1011 | errY := errY + dLkLin;
|
1012 | END IF;
|
1013 | -------------------------
|
1014 | count := to_integer(pfrq);
|
1015 | stateLinB <= S2;-- itp noch nicht fertig
|
1016 | ELSE -- dann itp fertig
|
1017 | count := 0;-- pfrq Zähler stoppen
|
1018 | stateLinB <= S4;--dann itp fertig
|
1019 | END IF;
|
1020 | ELSE -- warten auf count ablauf(prfg periode)
|
1021 | count := count - 1;
|
1022 | stateLinB <= S1;
|
1023 | END IF;
|
1024 | WHEN S2 => --noch nicht fertig
|
1025 | ImpLklin <= '1';
|
1026 | XdifA <= XlinA - XaltA;
|
1027 | YdifA <= YlinA - YaltA;
|
1028 | stateLinB <= S3;
|
1029 | WHEN S3 =>
|
1030 | XaltA := XlinA;
|
1031 | YaltA := YLinA;
|
1032 | LinCalcOK <= '1';--dann start für daten einlesen in RaMP
|
1033 | IF stopp = '1' THEN --sofort ITP anhalten
|
1034 | stateLinB <= S5;-- Stopp Exit
|
1035 | ELSE -- kein stopp
|
1036 | stateLinB <= S1;-- dann repeat
|
1037 | END IF;
|
1038 | --------------------------------
|
1039 | WHEN S4 => -- itp fertig
|
1040 | stateLinB <= S5;-- normal exit
|
1041 | --------------------------------------------
|
1042 | WHEN S5 => -- stop exit
|
1043 | XLinA <= (OTHERS =>'0');
|
1044 | YLinA <= (OTHERS =>'0');
|
1045 | errX :=0;
|
1046 | errY :=0;
|
1047 | count := 0;
|
1048 | ImpLkLin <= '1';--Impulse (Lk)von LinITP stoppen
|
1049 | LinItpok <= '1';
|
1050 | stateLinB <= S0;--und warten auf nächste linITP Satz
|
1051 | WHEN others => stateLinB <= S0;
|
1052 | END CASE;
|
1053 | END IF;--IF nrst/ELSE
|
1054 | END IF; -- if clk
|
1055 | END PROCESS;-- bresenham für XY LinKoordinaten
|
1056 |
|
1057 | --============= ende LinStz Interpolation =====================================
|
1058 | ------------------------------------------------------------------------------------
|
1059 | --======Invers transformation mit Linearinterpolation in XYZ Raum================
|
1060 | -----------------------------------------------------------------
|
1061 | --======berechnet gemäss XT,YT,ZT die Priode Dauer PX,Py,PZ für virtualle Strecke XYZ
|
1062 | --====== sowie AtFP, AtgrPI2, AZFP, FLk ===============================================
|
1063 | --== Flk := sqrt (xT^2 + YT^2 + ZT^2)
|
1064 | ----vorbereitung Berechnungen für virtuelle Interpolation in XYZ--------------------
|
1065 | --== Formel für X Achse, gilt das gleiche für Achse Y und Z
|
1066 | --== FxT := Int to FP, FrX := FxT/ FLk, FrY := FYT/ FLk,FrZ := FzT/ FLk,
|
1067 | --= FPx := frqFP *FrX, Px := Fpto Int, dtto Y,Z, frqFP = Fp(Frequenzperiode)
|
1068 | ---------------------------------
|
1069 | --== ATFP = atanFP(FYT/FXT)
|
1070 | --== ATgrPI2 := if AtFP > Pi/2 (ccmpAT) , cmp AT mit PI/2: if > then ATgrPI2:= 1, grPI2 <= '1';
|
1071 | --== AZFP:= atanFP(FlT/FZt)
|
1072 | -----------------------------------------------------------------
|
1073 | iniITPxyz: PROCESS(clk)
|
1074 | VARIABLE cntrclk : INTEGER RANGE 0 TO 511:= 0;
|
1075 | BEGIN
|
1076 | IF rising_edge(clk) THEN
|
1077 | IF nrst = '0' THEN
|
1078 | stateIni <= 0;
|
1079 | cntrclk := 0;
|
1080 | YXTneg <= '0';
|
1081 | initITPok <='0';--reset
|
1082 | ELSE
|
1083 | CASE stateini IS
|
1084 | WHEN 0 =>
|
1085 | IF ceXYZF = '1' THEN --start initITP, Start FP von XT,YT,ZT
|
1086 | cntrclk := clkFPtoInt;
|
1087 | stateIni <= 1;
|
1088 | ELSE
|
1089 | stateIni <= 0;
|
1090 | END IF;
|
1091 | initITPok <='0';--reset
|
1092 | WHEN 1 =>
|
1093 | stateIni <= 2;
|
1094 | WHEN 2 =>-- Berechnen FXt..FZt
|
1095 | IF cntrclk = 0 THEN
|
1096 | cesqXYZ <= '1';-- start für sq(XT,YT,ZT)
|
1097 | cntrclk := clktoSq;
|
1098 | stateIni <= 3;
|
1099 | ELSE
|
1100 | cntrclk := cntrclk - 1;
|
1101 | stateIni <= 2;
|
1102 | END IF;
|
1103 | WHEN 3 =>-- Berechnen sq (X,Y,Z)
|
1104 | IF cntrclk = 0 THEN
|
1105 | cesqXYZ <= '0';-- reset ceXYZF
|
1106 | ceaddXY <= '1';-- start für add(X,Y) und XTLTdiv:=FXT/FLT
|
1107 | cntrclk := clktoAdd;
|
1108 | stateIni <= 4;
|
1109 | ELSE
|
1110 | cntrclk := cntrclk - 1;
|
1111 | stateIni <= 3;
|
1112 | END IF;
|
1113 | WHEN 4 =>-- Berechnen addFP XY
|
1114 | IF cntrclk = 0 THEN
|
1115 | ceaddXY <= '0';-- reset ce addXY
|
1116 | ceaddXYZ <= '1';-- start für add(X,Y,Z)
|
1117 | cntrclk := clktoadd;
|
1118 | stateIni <= 5;
|
1119 | ELSE
|
1120 | cntrclk := cntrclk - 1;
|
1121 | stateIni <= 4;
|
1122 | END IF;
|
1123 | WHEN 5 =>-- Berechnen addFP XYZ
|
1124 | IF cntrclk = 0 THEN
|
1125 | ceaddXY <= '0';-- reset ce addXY
|
1126 | cesqrtFLk <= '1';-- start für sqrtFLk
|
1127 | FXtabs <= FXt AND X"7FFFFFFF";
|
1128 | FYtabs <= FYt AND X"7FFFFFFF";
|
1129 | FZtabs <= FZt AND X"7FFFFFFF";
|
1130 | cntrclk := clktosqrt;
|
1131 | stateIni <= 6;
|
1132 | ELSE
|
1133 | cntrclk := cntrclk - 1;
|
1134 | stateIni <= 5;
|
1135 | END IF;
|
1136 | WHEN 6 =>-- Berechnen sqrt(addXYZ)=>FLk, sqrt(addXY)=>FLT
|
1137 | IF cntrclk = 0 THEN
|
1138 | cesqrtFLk <= '0';-- reset ce sqrtFL
|
1139 | cntrclk :=clkFptoInt;
|
1140 | ceFlkInt <= '1'; --start FP to INt für Flk To Lk
|
1141 | stateIni <= 7;
|
1142 | ELSE
|
1143 | cntrclk := cntrclk - 1;
|
1144 | stateIni <= 6;
|
1145 | END IF;
|
1146 | WHEN 7 => ---FP to INt für Flk To Lk
|
1147 | ceFlkInt <= '0'; -- reset ce
|
1148 | IF cntrclk = 0 THEN
|
1149 | stateIni <= 9;
|
1150 | ELSE
|
1151 | cntrclk := cntrClk - 1;
|
1152 | stateIni <= 7;
|
1153 | END IF;
|
1154 | ---------------------------------------------
|
1155 | WHEN 9 => -- Berechnen FP to Int = PX..
|
1156 | IF cntrclk = 0 THEN
|
1157 | cntrclk := 36; -- 22 clk für atan,32 clk für acos
|
1158 | cetgAT <= '1';-- Start für ctgYXT:ATFP:= acos(XT/LT)
|
1159 | stateIni <= 10;
|
1160 | ELSE
|
1161 | cntrclk := cntrclk - 1;
|
1162 | stateIni <= 9;
|
1163 | END IF;
|
1164 | WHEN 10 => --Berechnen ATFP:=acos(FXT/FLT) und AZFP:= atan(FZT/FLT)
|
1165 | IF cntrclk = 0 THEN
|
1166 | cetgAT <= '0';-- reset
|
1167 | cecmpAT <= '1';-- startcmp AT mit PI/2
|
1168 | stateIni <= 11;-- und ende initITPlin: ATFP und AZFP fertig
|
1169 | ELSE
|
1170 | cntrclk := cntrclk - 1;
|
1171 | stateIni <= 10;
|
1172 | END IF;
|
1173 | WHEN 11 => --ausführen cmp AT mit PI/2: ATgrPI2 fertig
|
1174 | cecmpAT <= '0';-- reset
|
1175 | stateIni <= 12;
|
1176 | WHEN 12 => --
|
1177 | IF ATgrPI2 = "1" THEN -- ATgrPI ist vektor mit Bit 0
|
1178 | grPI2 <= '1';
|
1179 | ELSE grPI2 <= '0';
|
1180 | END IF;
|
1181 | stateIni <= 13;
|
1182 | WHEN 13 => --
|
1183 | initITPok <= '1'; -- init ITP fertig
|
1184 | stateIni <= 0;-- und Zurück an Anfang
|
1185 | WHEN OTHERS =>
|
1186 | stateIni <= 0;
|
1187 | END CASE;
|
1188 | END IF;-- nrst if/else
|
1189 | END IF; --clk
|
1190 | END PROCESS;--end iniITPxyz
|
1191 | --======Start von iniITPxyz und starten Impulsausgabe==========
|
1192 | --Virtuelle -Interpolation von XYZ Starten und steuern
|
1193 | --starten bresenham interpolation xyz & Lk
|
1194 | --Generieren ITPxyz für Status Meldung
|
1195 | runITP: PROCESS(clk)
|
1196 | VARIABLE cntrclk :INTEGER RANGE 0 TO 30000000:= 0;-- für 30ms max
|
1197 | BEGIN
|
1198 | IF rising_edge(clk) THEN
|
1199 | IF nrst = '0' THEN
|
1200 | stateITP <= 0;
|
1201 | cntrclk :=0;
|
1202 | ITPxyz <= "000";
|
1203 | ELSE
|
1204 | CASE stateITP IS
|
1205 | WHEN 0 =>
|
1206 | IF ce = '1' THEN
|
1207 | ITPxyz <= "111";
|
1208 | stateITP <= 2;
|
1209 | ELSE
|
1210 | stateITP <= 0;-- Warten auf Start
|
1211 | END IF;
|
1212 | WHEN 2 =>
|
1213 | IF XT(31) = '1' THEN -- if XT negativ
|
1214 | XTabs <= (XT XOR X"FFFFFFFF") + 1;-- XT negieren
|
1215 | fXneg <= '1';--neg Flag setzen
|
1216 | ELSE
|
1217 | XTabs <= XT;
|
1218 | fXneg <= '0';--neg Flag,Rücksetzen
|
1219 | END IF;
|
1220 | --------
|
1221 | IF YT(31) = '1' THEN -- if YT negativ
|
1222 | YTabs <= (YT XOR X"FFFFFFFF") + 1;-- YT negieren
|
1223 | fYneg <= '1';--neg Flag setzen
|
1224 | ELSE
|
1225 | YTabs <= YT;
|
1226 | fYneg <= '0';--neg Flag,Rücksetzen
|
1227 | END IF;
|
1228 | --------
|
1229 | IF ZT(31) = '1' THEN -- if ZT negativ
|
1230 | ZTabs <= (ZT XOR X"FFFFFFFF") + 1;-- ZT negieren
|
1231 | fZneg <= '1';--neg Flag setzen
|
1232 | ELSE
|
1233 | ZTabs <= ZT;
|
1234 | fZneg <= '0';--neg Flag,Rücksetzen
|
1235 | END IF;
|
1236 | ceXYZF <= '1';--Start für InitITPxyz, Start des iniITPlin Processes
|
1237 | stateITP <= 3;
|
1238 | WHEN 3 => -- wenn iniITPxyz fertig berchnet
|
1239 | IF initITPok = '1' THEN
|
1240 | cntrclk := 2000000;-- 20 ms warten wg rd Status
|
1241 | stateITP <= 4;---4; --dann weiter
|
1242 | ELSE
|
1243 | ceXYZF <= '0';--reset start für InitITP
|
1244 | stateITP <= 3;-- warten auf init ITP
|
1245 | END IF;
|
1246 | ------------------------------------
|
1247 | WHEN 4 =>-- warten 20 ms für status rd
|
1248 | IF cntrclk > 0 THEN
|
1249 | cntrclk := cntrclk -1;
|
1250 | stateITP <= 4;-- dann warten
|
1251 | ELSE
|
1252 | ITPxyz <= "000";
|
1253 | cntrclk := 170;-- 2.0 us min Impulslänge für start Impuls
|
1254 | stateITP <= 5;
|
1255 | END IF;
|
1256 | WHEN 5 => --- warten auf Start Impuls länge min 2,4us
|
1257 | IF cntrclk > 0 THEN
|
1258 | cntrclk := cntrclk -1;
|
1259 | IF strimp = '1' THEN
|
1260 | stateITP <= 5;-- warten
|
1261 | ELSE
|
1262 | stateITP <= 6;-- impuls ist kürzer, neu anfangen
|
1263 | END IF;
|
1264 | ELSE -- zeit abgelaufen
|
1265 | stateITP <= 7;-- impuls hat richtige länge
|
1266 | END IF;
|
1267 | WHEN 6 => -- neu anfangen mit Impuls abwarten
|
1268 | cntrclk := 170;-- 2 us min Impulslänge
|
1269 | stateITP <= 5;
|
1270 | ---------------------------------------
|
1271 | WHEN 7 =>
|
1272 | runXYZ <= '1'; --Impuls ausgabe(bresenham) starten
|
1273 | ITPxyz <= "111";
|
1274 | stateITP <= 8;
|
1275 | WHEN 8 => -- warten bis InterpolationXYZ fertig
|
1276 | IF stopp = '0' THEN --wenn keine Endlage dann weiter
|
1277 | IF XYZok = '1' THEN -- ITP (bresenham) ist fertig
|
1278 | runXYZ <= '0';-- reset
|
1279 | stateITP <= 0;-- itp fertig
|
1280 | ITPxyz <= "000";
|
1281 | ELSE --
|
1282 | stateITP <= 8;-- warten bis koordinaten fertig
|
1283 | END IF;
|
1284 | ELSE ----wenn Endlage dann sofort itp anhalten
|
1285 | stateITP <= 9;
|
1286 | END IF;
|
1287 | WHEN 9 => -- sofort stopp
|
1288 | ITPxyz <= "000";
|
1289 | runXYZ <= '0';-- impuls Ausgabe stoppen
|
1290 | stateITP <= 0;--itp fertig
|
1291 | WHEN OTHERS =>
|
1292 | stateITP <= 0;
|
1293 | END CASE;
|
1294 | END IF;-- nrst if/else
|
1295 | END IF; --clk
|
1296 | END PROCESS;--runITP
|
1297 | --==========================================================================
|
1298 | --=================XYZ Bresenham =Interpolation Am Roboterkopf ==========
|
1299 | --erzeugt virtuelle Impulse für ImpX,ImpY,ImpZ sowie ImpLK(entlang leitKoordinate) am Roboter Kopf
|
1300 | --und aktuelle virt.Position XA,YA,ZA (integer),
|
1301 | --Ende der Interpolation mit XYZok =1 Impuls(2clk)
|
1302 | --Vorschubspeed definiert mit pfrq (Anz.clk(10nS) zwischen zwei ImpLK
|
1303 | -- impLK startet RunAngle d.h. die Invers transf Berchnung
|
1304 | ---------------------------------------------------------------------------------------------------
|
1305 | bresham: PROCESS(clk)
|
1306 | VARIABLE count :INTEGER;
|
1307 | VARIABLE errX :INTEGER;
|
1308 | VARIABLE errY :INTEGER;
|
1309 | VARIABLE errZ :INTEGER;
|
1310 | VARIABLE startON :std_logic:= '0';
|
1311 | BEGIN
|
1312 | IF rising_edge(clk) THEN
|
1313 | IF nrst = '0' THEN
|
1314 | statebrsh <= 0;
|
1315 | dLk <= 0;
|
1316 | dX <= 0;
|
1317 | dY <= 0;
|
1318 | dZ <= 0;
|
1319 | errX :=0;
|
1320 | XAout <= (OTHERS =>'0');
|
1321 | YAout <= (OTHERS =>'0');
|
1322 | ZAout <= (OTHERS =>'0');
|
1323 | errY :=0;
|
1324 | errZ :=0;
|
1325 | XA <= (OTHERS =>'0');
|
1326 | YA <= (OTHERS =>'0');
|
1327 | ZA <= (OTHERS =>'0');
|
1328 | ImpX <= '1';
|
1329 | ImpY <= '1';
|
1330 | ImpZ <= '1';
|
1331 | ImpLk <= '1';
|
1332 | count := 0;
|
1333 | XYZok <= '0';
|
1334 | startON := '0';
|
1335 | ELSE
|
1336 | CASE statebrsh IS
|
1337 | WHEN 0 =>
|
1338 | IF runXYZ = '1' THEN
|
1339 | dX <= conv_integer(XTabs);-- Xkoord
|
1340 | dY <= conv_integer(YTabs);
|
1341 | dZ <= conv_integer(ZTabs);
|
1342 | dLk <= conv_integer(Lk);--Leit koord
|
1343 | statebrsh <= 1;
|
1344 | ELSE --anfang der berechnung nach strimp
|
1345 | IF strimp = '1' AND startON = '0' THEN
|
1346 | XA <= XS;-- Startpunkt setzen
|
1347 | YA <= YS;
|
1348 | ZA <= ZS;
|
1349 | startON := '1';
|
1350 | END IF;
|
1351 | errX :=dLk/2;
|
1352 | errY :=dLk/2;
|
1353 | errZ :=dLk/2;
|
1354 | Lka <= 0;
|
1355 | XYZok <= '0';
|
1356 | statebrsh <= 0;
|
1357 | END IF;
|
1358 | WHEN 1 =>
|
1359 | IF count = 0 THEN -- warten für pfrq periode
|
1360 | IF Lka < Lk THEN
|
1361 | Lka <= Lka + 1;
|
1362 | ImpLK <= '0';
|
1363 | ------------------------
|
1364 | errX := errX - dX;
|
1365 | IF errX < 0 THEN
|
1366 | IF fXneg = '0' THEN
|
1367 | XA <= XA + 1;
|
1368 | ELSE
|
1369 | XA <= XA - 1;
|
1370 | END IF;
|
1371 | ImpX <= '0';
|
1372 | errX := errX + dLk;
|
1373 | END IF;
|
1374 | ----------------------
|
1375 | errY := errY - dY;
|
1376 | IF errY < 0 THEN
|
1377 | IF fYneg = '0' THEN
|
1378 | YA <= YA + 1;
|
1379 | ELSE
|
1380 | YA <= YA - 1;
|
1381 | END IF;
|
1382 | ImpY <= '0';
|
1383 | errY := errY + dLk;
|
1384 | END IF;
|
1385 | -------------------------
|
1386 | errZ := errZ - dZ;
|
1387 | IF errZ < 0 THEN
|
1388 | IF fZneg = '0' THEN
|
1389 | ZA <= ZA + 1;
|
1390 | ELSE
|
1391 | ZA <= ZA - 1;
|
1392 | END IF;
|
1393 | ImpZ <= '0';
|
1394 | errZ := errZ + dLk;
|
1395 | END IF;
|
1396 | -------------------------
|
1397 | count := conv_integer(pfrq);
|
1398 | statebrsh <= 2;
|
1399 | ELSE
|
1400 | statebrsh <= 3;--dann itp fertig
|
1401 | END IF;
|
1402 | ELSE
|
1403 | count := count - 1;
|
1404 | statebrsh <= 1;
|
1405 | END IF;
|
1406 | WHEN 2 =>
|
1407 | ImpLk <= '1';
|
1408 | ImpX <= '1';
|
1409 | ImpY <= '1';
|
1410 | ImpZ <= '1';
|
1411 | XAout <= XA;
|
1412 | YAout <= YA;
|
1413 | ZAout <= ZA;
|
1414 | IF stopp = '1' THEN --sofort anhalzen, endlage
|
1415 | statebrsh <= 5;
|
1416 | ELSE --kein stop
|
1417 | statebrsh <= 1;
|
1418 | END IF;
|
1419 | WHEN 3 =>
|
1420 | XYZok <='1';
|
1421 | statebrsh <= 0;-- fertig ohne stop
|
1422 | WHEN 5 =>
|
1423 | dLk <= 0;
|
1424 | dX <= 0;
|
1425 | dY <= 0;
|
1426 | dZ <= 0;
|
1427 | errX :=0;
|
1428 | errY :=0;
|
1429 | errZ :=0;
|
1430 | XYZok <='1';--fertig mit stop
|
1431 | statebrsh <= 0;-- fertig ohne stop
|
1432 | WHEN others => statebrsh <= 0;
|
1433 | END CASE;
|
1434 | END IF;--IF nrst/ELSE
|
1435 | END IF; -- if clk
|
1436 | END PROCESS;-- bresenham für XYZ Koordinaten
|
1437 |
|
1438 | --=====================Berechnung Invers Transformation====
|
1439 | --============RUNANGLE :Berechnung der Winkel in den Roboterachsen ==========
|
1440 | --XT,YT,ZT sind strecken koordinaten die im XYZ Satz oder Kreissatz angegeben werden
|
1441 | -- bezogen auf Fräsenendpunkt (roboter KopfF)
|
1442 | --XS,YS,ZS sind startposition Koordinaten von wo XT--ZT gestartet werden
|
1443 | -- Xa,Ya,Za sind Aktuelle Koordinaten am Fräserendpunkt (roboterkopf)
|
1444 | --L1,L2,L3 sind roboterarm längen in mm
|
1445 | -- Bezeichnung FP = Wert ist Floatingpoint Format, Bsp: ATFP Winkel AT in FP format
|
1446 | ----------------------------------------------------------------------------
|
1447 | -- Folgende Werte werden hier Berechnet :
|
1448 | -- LT :Streckenlänge in XY Ebene:LT :=sqrt((xT^2 + YT^2)
|
1449 | -- LK :streckenlänge in XYZ:Lk :=sqrt((xT^2 + YT^2 + ZT^2);
|
1450 | -- AT = arccos(XT / LT)
|
1451 | -- AZ = arctan(ZT/LT)
|
1452 | -- Roboter Achsen in Ebene W : P0,P1,P2. EbeneW dreht sich mit Winkel A0
|
1453 | -- aus Xa,Ya,Za werden FXw,FYw,FZw : positionen in roboterebene W(winkel A1,A2,A3)
|
1454 | -- Aus Xa,Ya wird A0 Achse berechnet(A0FP in FP format, in radiant):A0Fp := atanFP( Fxa/FYa);
|
1455 | -- aus Xw,Yw werden Lw und Aw, Distanz und winkel in Roboterebene W
|
1456 | -- daraus die Winkelposition A3,A4
|
1457 | ----------------------------------------------------------
|
1458 | ---------- Ebene X / Y berechnungen-------------------
|
1459 | -- LwFP := sqrt(FXw^2 + FYw^2); - länge zwischen P0-P2 in Ebene XY
|
1460 | -- Winkel der Ebene W(P0,P2) =(AW), Winkel A0 ist Winkel zwischen P0..P3
|
1461 | -- A12FP := aTan(LzFP / LwFP), A12 = Winkel gerade zwischen P0 und P2 in ebene W
|
1462 | ------------------------------------------------------------------------
|
1463 | -- ****** Ende der Berechnungen wird mit Impuls FXYZaOK signalisiert *******
|
1464 | --- siehe stateangle = 20
|
1465 | ----------------------------------------------------------------------------
|
1466 | runangle:PROCESS(clk)
|
1467 | VARIABLE ctrclk :INTEGER RANGE 0 TO 127:= 0;
|
1468 | VARIABLE startON :std_logic:= '0';
|
1469 | BEGIN
|
1470 | IF rising_edge(clk) THEN
|
1471 | IF nrst = '0' THEN
|
1472 | stateangle <= 0;
|
1473 | ctrclk := 0;
|
1474 | FXYZaOK <= '0';
|
1475 | ceXYZFP <= '0'; cetgFP<='0'; cemul9<='0'; cemul10<='0';
|
1476 | cesubW <='0'; cePI2 <='0';
|
1477 | cemAZA3 <= '0';cedAZA3 <= '0';cedivPIdiv<='0'; ceAZmul<= '0';
|
1478 | cesub1min <= '0';ceA3FP<='0';
|
1479 | ceA4FP <='0'; cetgFP <= '0';
|
1480 | ceP1 <= '0';
|
1481 | cesqL <= '0'; ceadd1 <= '0'; cesub1<= '0';
|
1482 | cediv1 <= '0'; ceacosFP <= '0'; ceA1FP <='0';
|
1483 | cecosFP <= '0'; ceZ1W1 <= '0'; cesubA1 <= '0';
|
1484 | startON := '0';
|
1485 | -- Fl1 <= (OTHERS => '0'); Fl2 <= (OTHERS => '0');Fl12 <= (OTHERS => '0');
|
1486 | -- mul12 <= (OTHERS => '0');
|
1487 | ELSE
|
1488 | CASE stateangle IS
|
1489 | WHEN 0 =>
|
1490 | --IF (ImpLk = '0' AND strimp='0') OR (strimp = '1' AND StartON = '0') THEN -- start zurst mit ce
|
1491 | IF (ImpLk = '0') THEN
|
1492 | ceXYZFP <= '1';--Start für Xa,Ya,Zz=>FP, und Pi/2-AZFP,Pi/2 +/- ATFP
|
1493 | ctrclk := clkToFP;
|
1494 | stateangle <= 1;
|
1495 | ELSE
|
1496 | FXYZaOK <= '0'; --reset
|
1497 | stateangle <= 0;-- warten auf LK Impuls
|
1498 | END IF;
|
1499 | WHEN 1 => -- ausführen Xa,Ya,Zz=>FP und Pi/2-AZFP,Pi/2 +/- ATFP
|
1500 | IF ctrclk > 0 THEN
|
1501 | ctrclk := ctrclk -1;
|
1502 | stateangle <= 1;
|
1503 | ELSE
|
1504 | StartON := '1';
|
1505 | ceXYZFP <= '0';--reset CE
|
1506 | stateangle <= 2;
|
1507 | END IF;
|
1508 | WHEN 2 => -- zwischen state notwendig****
|
1509 | IF grPI2 = '1' THEN -- wenn AT > PI/2 dann sub PI/2 sonst ADD
|
1510 | PIAT <= suPIAT;
|
1511 | ELSE PIAT <= adPIAT;
|
1512 | END IF;
|
1513 | stateangle <= 3;
|
1514 | WHEN 3 => -- zwischen state notwendig ****
|
1515 | cePI2 <= '1';
|
1516 | ctrclk := clkTosub;
|
1517 | stateangle <= 4;
|
1518 | WHEN 4 => --ausführen PIAZ :=PI/2 - PIAT
|
1519 | IF ctrclk > 0 THEN
|
1520 | ctrclk := ctrclk -1;
|
1521 | stateangle <= 4;
|
1522 | ELSE
|
1523 | cePI2 <= '0';-- reset
|
1524 | ctrclk := 35;--clk Atan für durchlauf ******
|
1525 | ceTgFP <= '1'; --arcTang und cos starten
|
1526 | stateangle <= 5;
|
1527 | END IF;
|
1528 | WHEN 5 => --Ausführen A0FP:=atan(Ya/Xa),und Cos(4 bml), A=FP fertig*****
|
1529 | IF ctrclk > 0 THEN
|
1530 | ctrclk := ctrclk -1;
|
1531 | stateangle <= 5;
|
1532 | ELSE --tanYX , ATFP,AZFP fertig
|
1533 | cemul9 <= '1';-- start sinAZ* sinAT, sinAZ* cosAT
|
1534 | ctrclk := clkTomul;
|
1535 | ceTgFP <= '0'; --Reset ArcTang start Impuls
|
1536 | stateangle <= 9;
|
1537 | END IF;
|
1538 | WHEN 9 => -- ausführen; sinAZ* sinAT, sinAZ* cosAT
|
1539 | IF ctrclk > 0 THEN
|
1540 | ctrclk := ctrclk -1;
|
1541 | stateangle <= 9;
|
1542 | ELSE
|
1543 | cemul9 <= '0';--reset
|
1544 | ctrclk := clkTomul;
|
1545 | cemul10 <= '1'; --start sAZAT* FL3, cAZAT* FL3
|
1546 | stateangle <= 10;
|
1547 | END IF;
|
1548 | WHEN 10 => --ausführen sAZAT* FL3, cAZAT* FL3
|
1549 | IF ctrclk > 0 THEN
|
1550 | ctrclk := ctrclk -1;
|
1551 | stateangle <= 10;
|
1552 | ELSE
|
1553 | cemul10 <= '0';-- reset
|
1554 | ctrclk := clkTosub;
|
1555 | ceSubw <= '1'; -- Start für sub/add FXa -/+ sAZAT3 usw
|
1556 | stateangle <= 11;
|
1557 | END IF;
|
1558 | WHEN 11 => -- ausführen sub/add FXa -/+ sAZAT3.. = FYw,FZw fertig
|
1559 | IF ctrclk > 0 THEN
|
1560 | ctrclk := ctrclk -1;
|
1561 | stateangle <= 11;
|
1562 | ELSE
|
1563 | ceSubw <= '0';-- reset
|
1564 | stateangle <= 12;
|
1565 | END IF;
|
1566 | WHEN 12 =>-- für test
|
1567 | stateangle <= 13;
|
1568 | WHEN 13 => -- zwischen state notwendig ***
|
1569 | IF grPI2 = '1' THEN
|
1570 | FXw <= aFXw;
|
1571 | ELSE FXw <= sFXw;
|
1572 | END IF;
|
1573 | ctrclk := clkTosq;
|
1574 | cemul12 <= '1'; --Start für sqXw:=sq(FXw),sqYw:=sq(FYw) und ATPI:= ATFP-PI/2
|
1575 | stateangle <= 15; ---FXW fertig Berechnet
|
1576 | ----------------state reserve----------------------------
|
1577 | WHEN 15 => --- Ausführen Fxw^2,FYw^2
|
1578 | IF ctrclk > 0 THEN
|
1579 | ctrclk := ctrclk -1;
|
1580 | stateangle <= 15;
|
1581 | ELSE
|
1582 | cemul12 <= '0';-- reset
|
1583 | stateangle <= 16;
|
1584 | END IF;
|
1585 | WHEN 16 =>
|
1586 | ceXYZw <= '1';---für test ***** XYZ W ***********************
|
1587 | stateangle <= 17;
|
1588 | -----Blatt6:csqXw,csqYw, Blatt5: csubAwAt-- gestartet mit cemul12 ----------------
|
1589 | WHEN 17 => -- ausführen sqXw:=sq(FXw),sqYw:=sq(FYw)
|
1590 | ctrclk := 23;-- 22 clk für atan
|
1591 | ceXYZw <= '0'; -- reset
|
1592 | ceaddXYw <= '1'; --start fürAwFP:=atan(FYw/FXw) und sqXYW:=sqXw+SqYw,subPIDIV0subAtAW-pIDIV0
|
1593 | stateangle <= 18;
|
1594 | -----Blatt6: catgAw (22clk),caddXYw, Blatt5:csubPidiv--- gestartet mit ceaddXYw---------
|
1595 | WHEN 18 =>-- ausführen AwFP:=atan(FYw/FXw) und sqXYW:=sqXw+ SqYw,subPidiv=subAtAw-PIdiv2
|
1596 | IF ctrclk > 0 THEN
|
1597 | ctrclk := ctrclk -1;
|
1598 | stateangle <= 18;
|
1599 | ELSE
|
1600 | cesqrtLw <= '1'; --start LwFP:=sqrt(sqXYw) und subPIdiv= subATAW-pidIV2
|
1601 | ctrclk := clkTosq;
|
1602 | ceaddXYw <= '0'; --reset
|
1603 | stateangle <= 19;
|
1604 | END IF;
|
1605 | ------Blatt6:csqrtLW, Blatt5 :cdivPIdiv2-- gestartet mit cesqrtLW ---------
|
1606 | WHEN 19 => --ausführen LwFP:= sqrt(sqXYw),LWFP fertig, subPidiv=subAtAw-PIdiv2
|
1607 | IF ctrclk > 0 THEN
|
1608 | ctrclk := ctrclk - 1;
|
1609 | stateangle <= 19;
|
1610 | ELSE
|
1611 | cesqrtLw <= '0';-- reset
|
1612 | cemAzA3 <= '1'; --start AtFp-AwFp
|
1613 | absDivPIdiv <= DivPIdiv;--absDivPiDiv vorläaufig setzen
|
1614 | ctrclk := clkTosub;
|
1615 | stateangle <= 22;
|
1616 | END IF;
|
1617 | ----reserve state ----------------
|
1618 | ----- Blatt5:csqZ2,csqW2,cmuAZFP ---- gestartet mit cemAzA3 ----------------
|
1619 | WHEN 22 => --- ausführen sq(FZw),sq(LWFP), mul(AZFP,DivPIdiv)------
|
1620 | IF ctrclk > 0 THEN
|
1621 | ctrclk := ctrclk - 1;
|
1622 | stateangle <= 22;
|
1623 | ELSE
|
1624 | cemAZA3 <= '0'; --reset
|
1625 | cedAZA3 <= '1';--start subAtAw-PIdiv2
|
1626 | ctrclk := clkTosub;
|
1627 | stateangle <= 23;
|
1628 | END IF;
|
1629 | ---- Blatt 5:caddWZ, csub1min--- gestartet mit cedAZA3 ---------------
|
1630 | WHEN 23 => --- ausführen sqaddWZ=sqZ2 + sqW2,subPIdiv =
|
1631 | IF ctrclk > 0 THEN
|
1632 | ctrclk := ctrclk - 1;
|
1633 | stateangle <= 23;
|
1634 | ELSE
|
1635 | cedAZA3 <= '0';-- reset
|
1636 | cedivPIdiv <= '1';
|
1637 | ceA4FP <= '1'; -- start sqrt(sqaddWZ), atan A12,
|
1638 | ctrclk := clkToAtan;--clk Atan
|
1639 | stateangle <= 24;
|
1640 | END IF;
|
1641 | --- Blatt5:catanA12,sqrtWZ,cmulAZFP -- gestartet mit ceA4FP -------
|
1642 | WHEN 24 => --ausführen atan(FZW,LWFP),divFp,vsqrt(sqaddWZ), mul(sub1min,AZFP)
|
1643 | IF ctrclk > 0 THEN
|
1644 | ctrclk := ctrclk - 1;
|
1645 | stateangle <= 24;
|
1646 | ELSE
|
1647 | absdivPIdiv <= divPIdiv;-- vorläufig
|
1648 | ceAZmul <= '1';
|
1649 | ceP1 <= '1'; -- START intToFP für L1,L2 => FL1,FL2
|
1650 | ceA4FP <= '0'; --reset
|
1651 | cedivPIdiv <= '0';
|
1652 | stateangle <= 25;--
|
1653 | END IF;
|
1654 | WHEN 25 =>
|
1655 | IF absDivPidiv(31) = '1' THEN
|
1656 | absDivPidiv(31) <= '0';-- Abs von DivPiDiv
|
1657 | END IF;
|
1658 | ctrclk := clkTosub;
|
1659 | cesub1min <= '1';
|
1660 | stateangle <= 26;
|
1661 | WHEN 26 =>-- sub1min := 1 - absdivPIdiv
|
1662 | IF ctrclk > 0 THEN
|
1663 | ctrclk := ctrclk - 1;
|
1664 | stateangle <= 26;
|
1665 | ELSE
|
1666 | ceP1 <= '0';-- FL1,FL2 fertig
|
1667 | cesub1min <= '0';
|
1668 | ceAZmul <= '0';-- a4FP fertig
|
1669 | ceA3FP <= '1';-- startsub1min* AZFP => A3FP
|
1670 | cesqL <= '1';-- start für FL11^2,Fl2^2,FL12^2
|
1671 | ctrclk := clkTomul;
|
1672 | stateangle <= 27;
|
1673 | END IF;
|
1674 | -- Ab hier Funktionen aus calcP1---------------------------------------
|
1675 | WHEN 27 => -- A3FP :=sub1min * AZFP, a3FP fertig,FL11^2,Fl2^2,FL12^2
|
1676 | IF ctrclk > 0 THEN
|
1677 | ctrclk := ctrclk - 1;
|
1678 | stateangle <= 27;
|
1679 | ELSE
|
1680 | IF strImp = '0' THEN
|
1681 | StartON := '0';
|
1682 | END IF;
|
1683 | cesqL <= '0';--FL11^2,Fl2^2,FL12^2=> SqL1,sqL12,sqL2 fertig
|
1684 | ceA3FP <= '0';--a3FP fertig
|
1685 | ceadd1<= '1'; -- startsqL1+sqL12,Fl12*FL1
|
1686 | ctrclk := clkTomul;
|
1687 | stateangle <= 28;
|
1688 | END IF;
|
1689 | WHEN 28 => -- ausführen L1+sqL12,Fl12*FL1
|
1690 | IF ctrclk > 0 THEN
|
1691 | ctrclk := ctrclk - 1;
|
1692 | stateangle <= 28;
|
1693 | ELSE
|
1694 | ceadd1<= '0';--L1+sqL12,Fl12*FL1 fertig
|
1695 | cesub1 <= '1'; -- start für: sqadd1-sqL2, mul12 * 2
|
1696 | ctrclk := clkTomul;
|
1697 | stateangle <= 29;
|
1698 | END IF;
|
1699 | WHEN 29 => -- ausführen: sqadd1-sqL2, mul12 * 2
|
1700 | IF ctrclk > 0 THEN
|
1701 | ctrclk := ctrclk - 1;
|
1702 | stateangle <= 29;
|
1703 | ELSE
|
1704 | cesub1 <= '0';--sqadd1-sqL2, mul12 * 2 fertig
|
1705 | cediv1 <= '1'; -- start sqsub1/ mulres1 => ax
|
1706 | ctrclk := clkToDiv;
|
1707 | stateangle <= 30;
|
1708 | END IF;
|
1709 | WHEN 30 =>
|
1710 | IF ctrclk > 0 THEN
|
1711 | ctrclk := ctrclk - 1;
|
1712 | stateangle <= 30;
|
1713 | ELSE
|
1714 | cediv1 <= '0';--sqsub1/ mulres1 => ax fertig
|
1715 | ceacosFP <= '1'; -- start für atang( Ax)
|
1716 | ctrclk := clkTOacos;
|
1717 | stateangle <= 31;
|
1718 | END IF;
|
1719 | WHEN 31 =>
|
1720 | IF ctrclk > 0 THEN
|
1721 | ctrclk := ctrclk - 1;
|
1722 | stateangle <= 31;
|
1723 | ELSE
|
1724 | ceacosFP <= '0';--atang( Ax) fertig
|
1725 | ceA1FP <= '1'; -- start für acosAx + A12FP
|
1726 | ctrclk := clkTOadd;
|
1727 | stateangle <= 32;
|
1728 | END IF;
|
1729 | WHEN 32 =>
|
1730 | IF ctrclk > 0 THEN
|
1731 | ctrclk := ctrclk - 1;
|
1732 | stateangle <= 32;
|
1733 | ELSE
|
1734 | ceA1FP <= '0';--acosAx + A12FP => A+FP fertig *************
|
1735 | cesubA1 <= '1'; --start für PI/2 - A1FP
|
1736 | ctrclk := clkTOsub;
|
1737 | stateangle <= 33;
|
1738 | END IF;
|
1739 | WHEN 33 =>
|
1740 | IF ctrclk > 0 THEN
|
1741 | ctrclk := ctrclk - 1;
|
1742 | stateangle <= 33;
|
1743 | ELSE
|
1744 | cesubA1 <= '0';--PI/2 - A1FP => A1SFP fertig
|
1745 | cecosFP <= '1'; --Start für cos(a1FP) & cosFP(a1SFP)
|
1746 | ctrclk := clkTOcos;
|
1747 | stateangle <= 34;
|
1748 | END IF;
|
1749 | WHEN 34 =>
|
1750 | IF ctrclk > 0 THEN
|
1751 | ctrclk := ctrclk - 1;
|
1752 | stateangle <= 34;
|
1753 | ELSE
|
1754 | cecosFP <= '0';--cos(a1FP) & cosFP(a1SFP) =>cosA1FP,sinA1FP fertig
|
1755 | ceZ1W1 <= '1'; --start für sinA1FP*FL1, cosA1FP * FL1
|
1756 | ctrclk := clkTOmul;
|
1757 | stateangle <= 35;
|
1758 | END IF;
|
1759 | WHEN 35 =>
|
1760 | IF ctrclk > 0 THEN
|
1761 | ctrclk := ctrclk - 1;
|
1762 | stateangle <= 35;
|
1763 | ELSE
|
1764 | ceZ1W1 <= '0';--sinA1FP*FL1=> FZ1,cosA1FP * FL1=>FW1 fertig ******
|
1765 | cezw12 <= '1'; -- start für FZw-Fz1 & LwFP-FW1
|
1766 | ctrclk := clkTOsub;
|
1767 | stateangle <= 36;
|
1768 | END IF;
|
1769 | WHEN 36 =>
|
1770 | IF ctrclk > 0 THEN
|
1771 | ctrclk := ctrclk - 1;
|
1772 | stateangle <= 36;
|
1773 | ELSE
|
1774 | cezw12 <= '0';--FZw-Fz1=Z2Z1 & LwFP-FW1=W"W1 fertig
|
1775 | ceatanZW <= '1';-- start für atan
|
1776 | ctrclk := clkTOatan;
|
1777 | stateangle <= 37;
|
1778 | END IF;
|
1779 | WHEN 37 =>
|
1780 | IF ctrclk > 0 THEN
|
1781 | ctrclk := ctrclk - 1;
|
1782 | stateangle <= 37;
|
1783 | ELSE
|
1784 | ceatanZW <= '0';-- AtanZW fertig
|
1785 | cesubA2 <= '1'; -- start für atanZw- a1FP
|
1786 | ctrclk := clkTOsub;
|
1787 | stateangle <= 38;
|
1788 | END IF;
|
1789 | WHEN 38 =>
|
1790 | IF ctrclk > 0 THEN
|
1791 | ctrclk := ctrclk - 1;
|
1792 | stateangle <= 38;
|
1793 | ELSE
|
1794 | cesubA2 <= '0';-- A2FP fertig ********************
|
1795 | FXYZaOK <= '1';--RUNANGLE bereit
|
1796 | stateangle <= 0;-- alle Achsen fertig
|
1797 | --Berechnung fertg,wartn auf nächste Start
|
1798 | END IF;
|
1799 | WHEN OTHERS =>
|
1800 | stateangle <= 0;
|
1801 | END CASE;
|
1802 | END IF;-- nrst if/else
|
1803 | END IF; --clk
|
1804 | END PROCESS;-- end runangle
|
1805 |
|
1806 | --=======Ende der InversTransformation(IT) Berechnungen, Alle Achsen Winkel in FP Format=====
|
1807 | -- *******Ende der ITP wird signalisiert mit Impuls fXYZaOK*************************************
|
1808 | --=========================================================================================
|
1809 | --==========================================================================================
|
1810 | -- ITPImpulsGen:
|
1811 | -- Aktuelle Winkel der Achsen A0FP..A5FP(rad) umrechnen in Anzahl
|
1812 | -- soll-Inkremente:output AXconv:AchsInkr(31 bit),siehe modul AXconv, alle achsen Paralell
|
1813 | -- gespeichert in --AXinc.
|
1814 | ---------------------------------------------------------------------------
|
1815 | --TYPE T_Axr IS ARRAY(5 DOWNTO 0) OF std_logic_vector(15 DOWNTO 0);
|
1816 | --SIGNAL AXGrd :T_Axr; aktuelle Winkel der Achsen A0..A5 in Grad.x ,
|
1817 | -- *********zum vergleich mit Sketch Simulation **************
|
1818 | ----------------------------------------------------------------------------
|
1819 | ----SIGNAL AXinc :T_AXInc;--IS ARRAY(5 DOWNTO 0) OF SLV(31 DOWNTO 0);
|
1820 | -- Aktuelle Winkel der Achsen A0FP..A5FP(rad) umrechnen in Anzahl
|
1821 | -- soll-Inkremente:output AXconv:AchsInkr(31 bit),siehe modul AXconv, alle achsen Paralell
|
1822 | -- gespeichert in --AXinc :T_AXinc;
|
1823 | ------------------------------------------
|
1824 | --SIGNAL AXalt :T_AXInc;--IS ARRAY(5 DOWNTO 0) OF SLV(31 DOWNTO 0);
|
1825 | --speichert vorherige wert von AXinc, wird benötig um Differenz zwischen
|
1826 | -- den einzelne Sollerte aus AXinc zum
|
1827 | --SIGNAL vAXdif :T_AXInc;--IS ARRAY(5 DOWNTO 0) OF SLV(31 DOWNTO 0);
|
1828 | --------------------------------------------------------------------------------
|
1829 | --SIGNAL AXdif :T_AXdif;--IS ARRAY(5 DOWNTO 0) OF SLV(15 DOWNTO 0);
|
1830 | -- AXdif entspricht vAxdif aber beschränkt auf 16 bit, wird benötigt in
|
1831 | -- procedur ImpSwitch
|
1832 |
|
1833 | --============================================================================
|
1834 | ITPImpGen: PROCESS(clk)
|
1835 | VARIABLE cntrclk :Integer range 0 to 15:= 0;
|
1836 | VARIABLE strt :STD_LOGIC := '1';
|
1837 | BEGIN
|
1838 | IF rising_edge(clk) THEN
|
1839 | IF nrst = '0' THEN
|
1840 | statITPimp <= 0;
|
1841 | cntrclk := 0;
|
1842 | XYZcalcok <= '0';
|
1843 | FOR i IN 0 TO 5 LOOP
|
1844 | AXalt(i) <= (OTHERS => '0');
|
1845 | AXdif(i) <= (OTHERS => '0');--
|
1846 | vAXdif(i) <= (OTHERS => '0');
|
1847 | END LOOP;
|
1848 | strt := '0';
|
1849 | ELSE
|
1850 | CASE statITPimp IS
|
1851 | WHEN 0 => -- warten auf start
|
1852 | IF fXYZaOK = '1' THEN ---wenn Berechnung fertig dann Pulse berchnen
|
1853 | ceAxx <= '1';--- Start Convert für Alle Achsen A0.. A4
|
1854 | cntrclk := 6;---anz clk für convert
|
1855 | statITPimp <= 1;
|
1856 | ELSE
|
1857 | IF sitprun = '0' AND strt = '0' THEN
|
1858 | strt := '1';--vorbereiten flag für AXalt einlesen
|
1859 | END IF;
|
1860 | XYZcalcok <= '0';
|
1861 | statITPimp <= 0;
|
1862 | END IF;
|
1863 | WHEN 1 => --ausführung Convert (5 clk)
|
1864 | ceAxx <= '0';
|
1865 | IF cntrclk = 0 THEN
|
1866 | statITPimp <= 2;
|
1867 | ELSE
|
1868 | cntrclk := cntrclk -1;
|
1869 | statITPimp <= 1;-- warten bis Convert fertig
|
1870 | END IF;
|
1871 | WHEN 2 =>--
|
1872 | IF strt= '1' THEN --strt dient der start speicherung von 1. Wert von Axinc in Axalt
|
1873 | FOR i IN 0 TO 5 LOOP
|
1874 | AXalt(i)<= AXinc(i);
|
1875 | END LOOP;
|
1876 | END IF;
|
1877 | statITPimp <= 3;-- warten auf nächste
|
1878 | WHEN 3 =>
|
1879 | strt := '0';-- reset strt, erste wert von AXinc ist gespeichert in Axalt
|
1880 | -----------------------------------------------------
|
1881 | FOR i IN 0 TO 5 LOOP
|
1882 | vAXdif(i) <= AXinc(i) - AXalt(i);-- Differenz errechnen inn 32 bit
|
1883 | AXdif(i)<= vAXdif(i)(15 DOWNTO 0);--beschränken auf 16 bit, wird im ImpSwitch
|
1884 | END LOOP;
|
1885 | ------------------------
|
1886 | statITPimp <= 4;
|
1887 | WHEN 4 =>
|
1888 | FOR i IN 0 TO 5 LOOP -- speichern der vorherige AXinc wert
|
1889 | AXalt(i) <= AXinc(i);
|
1890 | END LOOP;
|
1891 | XYZcalcok <= '1';-- umrechnung fertig
|
1892 | statITPimp <= 0;-- warten auf nächste Lk impuls
|
1893 | WHEN OTHERS =>
|
1894 | statITPimp <= 0;
|
1895 | END CASE;
|
1896 | END IF; -- if nrst/else
|
1897 | END IF; -- clk
|
1898 | END PROCESS;--end ITPImpGEn
|
1899 | --=======================================================
|
1900 | --=========================
|
1901 | --TestOutput: PROCESS(sqadd1,mul12,mulres1,sqsub1,ax,acosAX,A12FP,cosA1FP,sinA1FP,
|
1902 | -- Fw1,Fz1,subATAW,subPIdiv,cesqL,FL1,FL12)
|
1903 | testOutput:PROCESS(clk)
|
1904 | BEGIN
|
1905 | IF rising_edge(clk) THEN
|
1906 | IF nrst = '0' THEN
|
1907 | testdata <= (OTHERS => '0');
|
1908 | ELSE
|
1909 | -- 16 bit daten für testoutput
|
1910 | testdata(0) <= sqadd1(0);-- A0
|
1911 | testdata(1) <= mul12(0);--
|
1912 | testdata(2) <= mulres1(0);--
|
1913 | testdata(3) <= sqsub1(0);--
|
1914 | testdata(4) <= ax(0);--
|
1915 | testdata(5) <= acosAX(0);--
|
1916 | testdata(6) <= A12FP(0);--
|
1917 | testdata(7) <= cosA1FP(0);--
|
1918 | testdata(8) <= sinA1FP(0);--
|
1919 | testdata(9) <= Fw1(0);--
|
1920 | testdata(10) <= FZ1(0);--
|
1921 | testdata(11) <= subATAW(0);--
|
1922 | testdata(12) <= subPIdiv(0);--
|
1923 | testdata(13) <= cesqL;--
|
1924 | testdata(14) <= FL1(0);--
|
1925 | testdata(15) <= FL12(0);--
|
1926 | END IF;
|
1927 | END IF;
|
1928 | END PROCESS;
|
1929 | --==========================================================================
|
1930 | --========================================================
|
1931 |
|
1932 | --==============================================================
|
1933 | ---**************************************************************
|
1934 | --------------IntToFP für ITP FrqPERIOD, Start mit frqpos------
|
1935 | ---cfrqFP: IntToFP port map(a=>frqPER,clk=>clk,ce=>cefrqFp,result=>frqFP);
|
1936 | --====================================================================
|
1937 | -------------------- initITP ------------------------------------------
|
1938 | ------XYZT => FPx..FPz, FPLk-----Initialisierung beim neuen Satz--------
|
1939 | cXTFP: IntToFP port map(a=>XT,clk=>clk,ce=>ceXYZF,result=>FXt);
|
1940 | cYTFP: IntToFP port map(a=>YT,clk=>clk,ce=>ceXYZF,result=>FYt);
|
1941 | cZTFP: IntToFP port map(a=>ZT,clk=>clk,ce=>ceXYZF,result=>FZt);
|
1942 | -------------sqFxt := FXt^2 state 3-------------------------------------------
|
1943 | cmulsqX: mulFP port map(a=>FXt,b=>FXt,clk=>clk,ce=>cesqXYZ,result=>sqFxt);
|
1944 | cmulsqY: mulFP port map(a=>FYt,b=>FYt,clk=>clk,ce=>cesqXYZ,result=>sqFyt);
|
1945 | cmulsqZ: mulFP port map(a=>FZt,b=>FZt,clk=>clk,ce=>cesqXYZ,result=>sqFzt);
|
1946 | --------------addXY := sqFxt + sqFYt state 4------------------------------------------
|
1947 | caddXY: addFP port map(a=>sqFXt,b=>sqFYt,clk=>clk,ce=>ceaddXY,result=>addXY);
|
1948 | --------------addXYZ := addXY + sqFZt ------------------------------------------
|
1949 | caddXYZ: addFP port map(a=>addXY,b=>sqFZt,clk=>clk,ce=>ceaddXYZ,result=>addXYZ);
|
1950 | ----------------FLk := sqrt(addXYZ),FLT:=sqrt(addXY)----state6--------------------
|
1951 | csqrtFLT: sqrtFP port map(a=>addXY,clk=>clk,ce=>cesqrtFLk,result=>FLT);
|
1952 | csqrtFLk: sqrtFP port map(a=>addXYZ,clk=>clk,ce=>cesqrtFLk,result=>FLk);
|
1953 | -------------------------------------------------------------------------------
|
1954 | cFPToInt: FPtoInt32 port map(a=>FLk,clk=>clk,ce=>ceFlkInt,result=>Lk);-- Flk in Integer32 slv
|
1955 | ------------------------------------------------------------------------------
|
1956 | cXTLTdiv: divFP port map(a=>FXT,b=>FLT,clk=>clk,ce=>ceFlkInt,result=>XTLTdiv);
|
1957 | ------------------AT FP Berechnen-State 10----------------------------
|
1958 | cacosAT: ArcCosFP port map(nrst=>nrst,clk=>clk,ceacos=>cetgAT,aInp=>XTLTdiv,acos=>ATFP);--ATFP :=acos(XT/LT)
|
1959 | ----------------- AZ FP Berechnen ---------------------------------------
|
1960 | ccmpAT: cmpgr port map(a=>ATFP,b=>PIdiv2,clk=>clk,ce=>cecmpAT,result=>ATgrPI2);-- wird in RUNANGLE Gebraucht
|
1961 | catgAZT: atanFP port map(nrst=>nrst,clk=>clk,ceatan=>cetgAT,Xinp=>FLT,Yinp=>FZT,atanFP=>AZFP);
|
1962 | --------------------- ende initITPlin ---------------------------------------------
|
1963 |
|
1964 | --*******************************************************************************
|
1965 | --====================================================================================
|
1966 | ---------------RUNANGLE -----------------------------------
|
1967 | -----------Xa,Ya,,Za => FP----------------------------------state 0-----------
|
1968 | cXaFP : IntToFP port map(a=>Xa,clk=>clk,ce=>cexyzFP,result=>FXa);
|
1969 | cYaFP : IntToFP port map(a=>Ya,clk=>clk,ce=>cexyzFP,result=>FYa);
|
1970 | cZaFP : IntToFP port map(a=>Za,clk=>clk,ce=>cexyzFP,result=>FZa);
|
1971 | cL3FP: Int16ToFP port map(a=>L3,clk=>clk,ce=>cexyzFP,result=>FL3);
|
1972 | ------------PIAZ:=Pi/2-AZFP, PIAT:=Pi/2-ATFP---------------state 0 --------
|
1973 | csubPIAZ: subFP port map(a=>PIdiv2,b=>AZFP,clk=>clk,ce=>ceXYZFP,result=>PIAZ);
|
1974 | csubPIAT: subFP port map(a=>ATFP,b=>PIdiv2,clk=>clk,ce=>ceXYZFP,result=>suPIAT);
|
1975 | caddPIAT: addFP port map(a=>PIdiv2,b=>ATFP,clk=>clk,ce=>ceXYZFP,result=>adPIAT);
|
1976 | ----------If ATFP > pI/2 then PIAT := suPIAT else PIAT:= adPiAT --- Starte 18, 19
|
1977 | ---- siehe InitITP xyz state 11-----------------------------------
|
1978 | ---------sub: Pi/2 -PIAT ------------------------------------ State 2-------
|
1979 | csubSAT: subFP port map(a=>PIdiv2,b=>PIAT,clk=>clk,ce=>cePI2,result=>sPIAT);
|
1980 | ------A0FP:= atan(FYa/FXa) and cosFP: PIAZ,AZFP,PIAT,SPIAT -----state 3-----
|
1981 | ctanYX: atanFP port map(nrst=>nrst,clk=>clk,ceatan=>cetgFP,Xinp=>FXa,Yinp=>FYa,atanFP=>A0FP);
|
1982 | csinAZ: cosFP port map(nrst=>nrst,clk=>clk,cecos=>cetgFP,ainp=>PIAZ,cos=>sinAZ);
|
1983 | ccosAZ: cosFP port map(nrst=>nrst,clk=>clk,cecos=>cetgFP,ainp=>AZFP,cos=>cosAZ);
|
1984 | ccosAT: cosFP port map(nrst=>nrst,clk=>clk,cecos=>cetgFP,ainp=>PIAT,cos=>cosAT);
|
1985 | csinAT: cosFP port map(nrst=>nrst,clk=>clk,cecos=>cetgFP,ainp=>sPIAT,cos=>sinAT);
|
1986 | --------------mul: sinAZ * sinAT, sinAZ * cosAT----------------- state9 -------
|
1987 | cmul90: mulFP port map(a=>sinAZ,b=>sinAT,clk=>clk,ce=>cemul9,result=>sAZAT);
|
1988 | cmul91: mulFP port map(a=>sinAZ,b=>cosAT,clk=>clk,ce=>cemul9,result=>cAZAT);
|
1989 | --------------mul: sAZAT * L3, cAZAT * L3 ----------------------state10 ------
|
1990 | csAZAT: mulFP port map(a=>sAZAT,b=>FL3,clk=>clk,ce=>cemul10,result=>sAZAT3);
|
1991 | ccAZAT: mulFP port map(a=>cAZAT,b=>FL3,clk=>clk,ce=>cemul10,result=>cAZAT3);
|
1992 | ccAZAZ: mulFP port map(a=>cosAZ,b=>FL3,clk=>clk,ce=>cemul10,result=>cAZAZ3);
|
1993 | ------------sub: FXa-sAZAT3,FYa - cAZAT3, FZa - cosAZ ----State 11 -----------
|
1994 | caddFXw: addFP port map(a=>FXa,b=>sAZAT3,clk=>clk,ce=>cesubW,result=>aFXw);
|
1995 | csubFXw: subFP port map(a=>FXa,b=>sAZAT3,clk=>clk,ce=>cesubW,result=>sFXw);
|
1996 | ------
|
1997 | csubFYw: subFP port map(a=>FYa,b=>cAZAT3,clk=>clk,ce=>cesubW,result=>FYw);
|
1998 | caddFZw: addFP port map(a=>FZa,b=>cAZAZ3,clk=>clk,ce=>cesubW,result=>FZw);
|
1999 |
|
2000 | ------------sq : FXw, FYw ,sub ATFP-AWFP=subATAW---------------State 12 --------
|
2001 | csqXw: mulFP port map(a=>FXw,b=>FXw,clk=>clk,ce=>cemul12,result=>sqXw);
|
2002 | csqYw: mulFP port map(a=>FYw,b=>FYw,clk=>clk,ce=>cemul12,result=>sqYw);
|
2003 |
|
2004 | ------------ ----------------------------state 13 -----------
|
2005 | caddXYw: addFP port map(a=>sqXw,b=>sqYw,clk=>clk,ce=>ceaddXYw,result=>sqXYw);
|
2006 | catgAw: atanFP port map(nrst=>nrst,clk=>clk,ceatan=>ceaddXYw,Xinp=>FXw,Yinp=>FYw,atanFP=>AwFP);
|
2007 | ----------------------------------------State 14-------------------------
|
2008 | csqrtLw: sqrtFP port map(a=>sqXYw,clk=>clk,ce=>cesqrtLw,result=>LwFP);
|
2009 |
|
2010 | -----------------------------------------State15-------------------
|
2011 | csqZ2: mulFP port map(a=>FZw,b=>FZw,clk=>clk,ce=>cemAZA3,result=>sqZ2);
|
2012 | csqW2: mulFP port map(a=>LwFP,b=>LwFP,clk=>clk,ce=>cemAZA3,result=>sqW2);
|
2013 | csubAWAT: subFP port map(a=>ATFP,b=>AWFP,clk=>clk,ce=>cemAZA3,result=>subATAW);
|
2014 |
|
2015 | ---------------------------------------- State 16 ----------------
|
2016 | caddWZ: addFP port map(a=>sqZ2,b=>sqW2,clk=>clk,ce=>cedAZA3,result=>sqaddWZ);
|
2017 | csubPIdiv: subFP port map(a=>subATAW,b=>PIdiv2,clk=>clk,ce=>cedAZA3,result=>subPIdiv);--alfa
|
2018 | ----------------------------------------- State 17----------------------
|
2019 | catanA12: atanFP port map(nrst=>nrst,clk=>clk,ceatan=>ceA4FP,Xinp=>LwFP,Yinp=>FZw,atanFP=>A12FP);
|
2020 | csqrtWZ: sqrtFP port map(a=>sqaddWZ,clk=>clk,ce=>ceA4FP,result=>FL12);
|
2021 |
|
2022 | cdivPIdiv2: divFP port map(a=>subPIdiv,b=>PIdiv2,clk=>clk,ce=>cedivPIdiv,result=>divPIdiv);
|
2023 |
|
2024 | ----------------------------- state 18-----------------------------------------
|
2025 | cmulAZFP: mulFP port map(a=>AZFP,b=>divPIdiv,clk=>clk,ce=>ceAZmul,result=>A4FP);
|
2026 |
|
2027 | ----------------------------- state 19-----------------------------------------
|
2028 | csub1min: subFP port map(a=>C1,b=>absdivPIdiv,clk=>clk,ce=>cesub1min,result=>sub1min);
|
2029 | ----------------------------- state 20-----------------------------------------
|
2030 | cmulAZFPs: mulFP port map(a=>sub1min,b=>AZFP,clk=>clk,ce=>ceA3FP,result=>A3FP);
|
2031 | ------------------ende run angle ----------------------------------------------------------
|
2032 | --==================TEST ======================================================================
|
2033 | ------------ FXw,FYw,FZw => Xw,Yw,Zw integer ------------state 18----** für test********
|
2034 | cFPwXtoIntw: FPtoInt32 port map(a=>FXw,clk=>clk,ce=>ceXYZw,result=>Xw);-- FXw in Integer32 Xw slv
|
2035 | cFPwYToIntw: FPtoInt32 port map(a=>FYw,clk=>clk,ce=>ceXYZw,result=>Yw);-- FXw in Integer32 Xw slv
|
2036 | cFPwZToIntw: FPtoInt32 port map(a=>FZw,clk=>clk,ce=>ceXYZw,result=>Zw);-- FXw in Integer32 Xw slv
|
2037 | --======================================================================
|
2038 | cL1FP: Int16ToFP port map(a=>L1,clk=>clk,ce=>ceP1,result=>FL1); --L1:16 bit
|
2039 | cL2FP: int16ToFP port map(a=>L2,clk=>clk,ce=>ceP1,result=>FL2); -- L2 16 biot
|
2040 | -------- FL 12 wurde bei RUNANGLE bereits berechnet -----------------------
|
2041 | ------------------------------------------------------------------------
|
2042 | csqL1: mulFP port map(a=>FL1,b=>FL1,clk=>clk,ce=>cesqL,result=>sqL1);-- quadrat FL1
|
2043 | csqL2: mulFP port map(a=>FL2,b=>FL2,clk=>clk,ce=>cesqL,result=>sqL2);-- Quadrat FL2
|
2044 | csqL12: mulFP port map(a=>FL12,b=>FL12,clk=>clk,ce=>cesqL,result=>sqL12);-- Quadrat Fl12
|
2045 | cmul12: mulFP port map(a=>FL1,b=>FL12,clk=>clk,ce=>ceadd1,result=>mul12);-- FL1 * FL12 => mul12
|
2046 | ---------------------
|
2047 | cadd12: addFP port map(a=>sqL1,b=>sqL12,clk=>clk,ce=>ceadd1,result=>sqadd1);
|
2048 | csub12: subFP port map(a=>sqadd1,b=>sqL2,clk=>clk,ce=>cesub1,result=>sqsub1);
|
2049 | ---------------------------
|
2050 | cmulres1: mulFP port map(a=>mul12,b=>C2,clk=>clk,ce=>cesub1,result=>mulres1);-- mul12 * 2 =>mulres1
|
2051 | cdiv: divFP port map(a=>sqsub1,b=>mulres1,clk=>clk,ce=>cediv1,result=>ax);
|
2052 | ----------------------ax fertig berechnet-----------------------------------
|
2053 | ccosAX: ArcCosFP port map(nrst=>nrst,clk=>clk,ceacos=>ceacosFP,aInp=>AX,acos=>acosAX);
|
2054 | caddA1: addFP port map(a=>acosAX,b=>A12FP,clk=>clk,ce=>ceA1FP,result=>A1FP);
|
2055 | ----------------------- a1CFP:=Pi/2 - A1FP---------state 8--------------------
|
2056 | csubA1: subFP port map(a=>PIdiv2,b=>A1FP,clk=>clk,ce=>cesubA1,result=>A1SFP);
|
2057 | ----------------------cos/sin +FP ------------------state 9----------------
|
2058 | ccosFp: cosFP port map(nrst=>nrst,clk=>clk,cecos=>cecosFP,ainp=>A1FP,cos=>cosA1FP);
|
2059 | csinFp: cosFP port map(nrst=>nrst,clk=>clk,cecos=>cecosFP,ainp=>A1SFP,cos=>sinA1FP);
|
2060 | ----------------------mul--------------------------state 10 ---------------------
|
2061 | cmulW1: mulFP port map(a=>cosA1FP,b=>FL1,clk=>clk,ce=>ceZ1W1,result=>FW1);
|
2062 | cmulZ1: mulFP port map(a=>sinA1FP,b=>FL1,clk=>clk,ce=>ceZ1W1,result=>FZ1);
|
2063 | -------- Blatt 1 calcP1 fertig -----Blatt2 calpP+ : Berechnung A2-----
|
2064 | csubZ2Z1: subFP port map(a=>FZw,b=>FZ1,clk=>clk,ce=>ceZW12,result=>Z2Z1);
|
2065 | csubW2W1: subFP port map(a=>LwFP,b=>FW1,clk=>clk,ce=>ceZW12,result=>W2W1);
|
2066 | ----------------------- state ------------------------------
|
2067 | catgZW: atanFP port map(nrst=>nrst,clk=>clk,ceatan=>ceatanZw,Xinp=>w2w1,Yinp=>z2z1,atanFP=>AtanZW);
|
2068 | -----------------state ----------------------------------------
|
2069 | csubA2: subFP port map(a=>atanZW,b=>A1FP,clk=>clk,ce=>cesubA2,result=>A2FP);
|
2070 | ------------------end RUNANGLE--------------------------------------------
|
2071 | --*****************************************************************************
|
2072 | --==================ITPImpGen -------------------------------------------------
|
2073 |
|
2074 | ------------------ convert Winkel DakrAx,InkrAX--------------------------
|
2075 | cA0conv: AxConv port map(nrst=>nrst,clk=>clk,ceConv=>ceAxx,AFPinp=>A0FP,Resol=>C_A0rad,
|
2076 | AchsInkr=>AXInc(0),AXGrd=>AXGr(0));--A0
|
2077 | cA1conv: AxConv port map(nrst=>nrst,clk=>clk,ceConv=>ceAxx,AFPinp=>A1FP,Resol=>C_A1rad,
|
2078 | AchsInkr=>AXInc(1),AXGrd=>AXGr(1));--A1
|
2079 | cA2conv: AxConv port map(nrst=>nrst,clk=>clk,ceConv=>ceAxx,AFPinp=>A2FP,Resol=>C_A2rad,
|
2080 | AchsInkr=>AXInc(2),AXGrd=>AXGr(2));--A2
|
2081 | cA3conv: AxConv port map(nrst=>nrst,clk=>clk,ceConv=>ceAxx,AFPinp=>A3FP,Resol=>C_A3rad,
|
2082 | AchsInkr=>AXInc(3),AXGrd=>AXGr(3));--A3
|
2083 | cA4conv: AxConv port map(nrst=>nrst,clk=>clk,ceConv=>ceAxx,AFPinp=>A4FP,Resol=>C_A4rad,
|
2084 | AchsInkr=>AXInc(4),AXGrd=>AXGr(4));--A4
|
2085 | cA5conv: AxConv port map(nrst=>nrst,clk=>clk,ceConv=>ceAxx,AFPinp=>A5FP,Resol=>C_A5rad,
|
2086 | AchsInkr=>AXInc(5),AXGrd=>AXGr(5));--A5
|
2087 | ------------------------nur für tests------------------------------
|
2088 | --cA12conv: AxConv port map(nrst=>nrst,clk=>clk,ceConv=>ceAxx,AFPinp=>A12FP,Resol=>A12res,
|
2089 | --Axint=>SA12);
|
2090 | --cATconv: AxConv port map(nrst=>nrst,clk=>clk,ceConv=>ceAxx,AFPinp=>ATFP,Resol=>A12res,Axint=>SAT);
|
2091 | --cAZconv: AxConv port map(nrst=>nrst,clk=>clk,ceConv=>ceAxx,AFPinp=>AZFP,Resol=>A12res,Axint=>SAZ);
|
2092 | --cAwconv: AxConv port map(nrst=>nrst,clk=>clk,ceConv=>ceAxx,AFPinp=>AwFP,Resol=>A12res,Axint=>SAw);
|
2093 | --===============================================================================
|
2094 | -----********************* Div Testwerte ************************************
|
2095 | ---- test ArcTang--------------------
|
2096 | -- Testwert tgFP=500/3000=0.1666666 =X"3e2aaaab"
|
2097 | -- Test Resultat für versuch mit 0.166666 = 0.16514867740814 =X"3e291cbc"
|
2098 | ------------------------------------------------------------------
|
2099 | --- testwert Input: 60grd= 1.0471975512 =X"3F860A92"-------------------------------
|
2100 | ----- Output:cos = 0.5=FP :X"3F000000",Testresultat :0.499998986721
|
2101 | -------------------------
|
2102 | --- +60 Grad Winkel :1.0471975512 rad = 3f860a92,-60 Grad:-1.04.. =bf860a92
|
2103 | ---- + 90 Grad = 3fc90fdb , -90 Grd = bfc90fdb
|
2104 | ----------------------
|
2105 | ----sin :0.86602540379= FP: 3f5db3d7, Testresultat : -----
|
2106 | -------------------------------------------------------------
|
2107 | --input FP-Testwert(XinpCos) für arcsin(0.5)=X"3f000000", Resultat =0.5235987756= X"3F060a92" (FP)
|
2108 | --input FP-Testwert(XinpCos) für arccos(0.5)=X"3f000000", Resultat =1.0471975512= X"3F" (FP)
|
2109 | --========================================================================
|
2110 |
|
2111 | end Behavioral;
|