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