---------------------------------------------------------------------------------- -- Company: AK development GmbH -- Engineer: A:Kurka -- -- Create Date: 31.8.2024 -- modif:31.08.24 Version A20 -- Design Name: AS21 -- Module Name: ITPModul - Behavioral ------------------------------------------------------------------------ -- virtuelle Interpolator XYZ von Koordinaten XYZs to XYZe -- Invers-Transformation von XYZ auf Achsen A4..A0 -- Achse A6 noch nicht programmiert, wird kundenspezifisch programmiert ---------------------------------------------------------------------------------- library IEEE; use IEEE.STD_LOGIC_1164.ALL; use IEEE.STD_LOGIC_ARITH.ALL; --use IEEE.STD_LOGIC_UNSIGNED.ALL; use ieee.std_logic_signed.ALL; --USE ieee.math_real.all; use IEEE.numeric_std.all; entity ITPModul is port (nrst : in std_logic; clk : in std_logic; XS :in std_logic_vector(31 downto 0);-- Startpunkt Koordinate X YS :in std_logic_vector(31 downto 0);-- Startpunkt Koordinate Y ZS :in std_logic_vector(31 downto 0);-- Startpunkt Koordinate Z XT :in std_logic_vector(31 downto 0);-- Koordinate X relativ (XE-XS) YT :in std_logic_vector(31 downto 0);-- Koordinate Y relativ (YE-YS) ZT :in std_logic_vector(31 downto 0);-- Koordinate Z relativ (ZE-ZS) pfrq :in std_logic_vector(23 downto 0);--Bahn-Periode in anzahl CLK L1 :in std_logic_vector(15 downto 0);-- länge Arm1 in mm L2 :in std_logic_vector(15 downto 0);-- länge Arm2 in mm L3 :in std_logic_vector(15 downto 0);-- länge Arm3 in mm --------------------- Xlin :in std_logic_vector(15 downto 0);-- Koord LinStz Ylin :in std_logic_vector(15 downto 0);-- Max.Eingabe +/-327.67 ebene :in std_logic_vector(3 downto 0);--interpolationsebene für LinSatz Xactiv :in std_logic; Yactiv :in std_logic; celin :in std_logic;---start interpolation für linstz in einer ebene -------------------------------------- stopp :in std_logic;-- stop interpolation,(stopp+) nach Endlage RampRun :in std_logic;-- rampe aktiv =1 strimp :in std_logic;-- start interpolation ce :in std_logic;-- start interpolation von kreis oder XYZ Satz : InversTransf ----------------------------- calcok :out std_logic:= '0';-- signalisiert alle berechnungen sind fertig XAout :out std_logic_vector(31 downto 0):=X"00000000";--virtuelle aktuelle koordinate YAout :out std_logic_vector(31 downto 0):=X"00000000"; ZAout :out std_logic_vector(31 downto 0):=X"00000000"; ITPmod :out std_logic_vector(2 DOWNTO 0):="000";--ITP für Status Meldung an CPU oAXdif :out std_logic_vector(95 downto 0):= (OTHERS => '0');--Sollwertdiff(16Bit) von inv.Transfer und Lin oImpLk :out std_logic:='1';--impulse von xyz itp enditp :out std_logic:='0';-- ende von Kontur:=0 (enditp-) itprun :out std_logic:='0';-- =1:interpolator läuft,aktuelle Sollwerte in oAXinkr(6x 32bit) testdata :out std_logic_vector(15 downto 0):= X"0000" ); end ITPModul; architecture Behavioral of ITPModul is --========================================================== --Umrechnng Winkel in Rad(FP) in Inkremente gem. AchsnAuflösung -- if InkrAX = 1 achse Inkrementieren, --if IDekrAX = 1 achse Dekrementieren, COMPONENT AxConv is port ( nrst : in std_logic; clk : in std_logic; ceConv : in std_logic; AFPinp :in std_logic_vector(31 downto 0);--Input Achsen FP Resol :in std_logic_vector(31 downto 0);--2PI/AchsenAuflösung AchsInkr :out std_logic_vector(31 downto 0);-- Anzahl Inkremene AXgrd :out std_logic_vector(15 downto 0)-- Achsenwinkel In Grd.X ); END COMPONENT; ------------------------------------------- COMPONENT CosFP is port( nrst : in std_logic; clk : in std_logic; cecos: in std_logic;-- startimpuls ArcCosFP ainp :in std_logic_vector(31 downto 0);-- input X= cos(x), FP format cos :OUT std_logic_vector(31 downto 0)-- output arccos(X), FP format ); END COMPONENT; ----------------------------------------------------- COMPONENT atanFP is port( nrst : in std_logic; clk : in std_logic; ceatan : in std_logic;-- startimpuls afkt Xinp :in std_logic_vector(31 downto 0);-- input X FP FP format Yinp :in std_logic_vector(31 downto 0);-- input Y FP FP format atanFP :OUT std_logic_vector(31 downto 0):=X"00000000"-- output Winkel(rad) in FP format ); END COMPONENT; ----------------------------------------------- -- if a > b then result = "0001" COMPONENT cmpgr IS PORT ( a : IN STD_LOGIC_VECTOR(31 DOWNTO 0); b : IN STD_LOGIC_VECTOR(31 DOWNTO 0); clk : IN STD_LOGIC; ce : IN STD_LOGIC; result : OUT STD_LOGIC_VECTOR(0 DOWNTO 0) ); END COMPONENT;-- cmpgr; ------------------------------------------------------------------------------- COMPONENT ArcCosFP is port( nrst : in std_logic; clk : in std_logic; ceacos: in std_logic;-- startimpuls ArcCosFP ainp :in std_logic_vector(31 downto 0);-- input X= cos(x), FP format acos :OUT std_logic_vector(31 downto 0):=X"00000000"-- output arccos(X), FP format ); END COMPONENT; ---------------------------------------------------------------- COMPONENT mul IS port ( clk: IN std_logic; a: IN std_logic_VECTOR(25 downto 0); b: IN std_logic_VECTOR(25 downto 0); ce: IN std_logic; p: OUT std_logic_VECTOR(25 downto 0)); END COMPONENT; ----------------------------------------------------------- COMPONENT DivAtan is port( nrst : in std_logic; clk : in std_logic; Fa :in std_logic_vector(31 downto 0);--Input FP Divident Fb :in std_logic_vector(31 downto 0);--Input FP Divisor cedivatan :in std_logic; Angle :out std_logic_VECTOR(25 downto 0)); end COMPONENT; --=========================================================== -----------für initITP------------------------------------- COMPONENT IntToFP IS port ( a: IN std_logic_VECTOR(31 downto 0); clk: IN std_logic; ce: IN std_logic; result: OUT std_logic_VECTOR(31 downto 0)); END COMPONENT; ------------------------------------------------------------ COMPONENT int16ToFP IS PORT ( a : IN STD_LOGIC_VECTOR(15 DOWNTO 0); clk : IN STD_LOGIC; ce : IN STD_LOGIC; result : OUT STD_LOGIC_VECTOR(31 DOWNTO 0) ); END COMPONENT; ------------------------------------------------------------ COMPONENT divFP IS port ( a: IN std_logic_VECTOR(31 downto 0); b: IN std_logic_VECTOR(31 downto 0); clk: IN std_logic; ce: IN std_logic; result: OUT std_logic_VECTOR(31 downto 0) ); END COMPONENT; ---------------------------------------------------- COMPONENT mulFP IS port ( a: IN std_logic_VECTOR(31 downto 0); b: IN std_logic_VECTOR(31 downto 0); clk: IN std_logic; ce: IN std_logic; result: OUT std_logic_VECTOR(31 downto 0) ); END COMPONENT; --------------------------------------------- COMPONENT addFP IS port ( a: IN std_logic_VECTOR(31 downto 0); b: IN std_logic_VECTOR(31 downto 0); clk: IN std_logic; ce: IN std_logic; result: OUT std_logic_VECTOR(31 downto 0)); END COMPONENT; ----------------------------------------- COMPONENT subFP IS port ( a: IN std_logic_VECTOR(31 downto 0); b: IN std_logic_VECTOR(31 downto 0); clk: IN std_logic; ce: IN std_logic; result: OUT std_logic_VECTOR(31 downto 0)); END COMPONENT; ----------------------------------------------------- COMPONENT sqrtFP IS port ( a: IN std_logic_VECTOR(31 downto 0); clk: IN std_logic; ce: IN std_logic; result: OUT std_logic_VECTOR(31 downto 0)); END COMPONENT; ---------------------------------------------------- COMPONENT FPtoInt32 IS port ( a: IN std_logic_VECTOR(31 downto 0); clk: IN std_logic; ce: IN std_logic; result: OUT std_logic_VECTOR(31 downto 0)); END COMPONENT; --------------------------------------------------- COMPONENT FPtoInt IS port ( a: IN std_logic_VECTOR(31 downto 0); clk: IN std_logic; ce: IN std_logic; result: OUT std_logic_VECTOR(24 downto 0)); END COMPONENT; --====================================================== COMPONENT FPtoInt16 IS port ( a: IN std_logic_VECTOR(31 downto 0); clk: IN std_logic; ce: IN std_logic; result: OUT std_logic_VECTOR(15 downto 0) ); END COMPONENT; --==== testfunktionen werden nicht implementiert ==== COMPONENT tstCos IS PORT ( phase_in : IN STD_LOGIC_VECTOR(15 DOWNTO 0); y_out : OUT STD_LOGIC_VECTOR(15 DOWNTO 0); clk : IN STD_LOGIC; ce : IN STD_LOGIC ); END COMPONENT; --------------------------------------------------------- COMPONENT tstSin IS PORT ( phase_in : IN STD_LOGIC_VECTOR(15 DOWNTO 0); x_out : OUT STD_LOGIC_VECTOR(15 DOWNTO 0); clk : IN STD_LOGIC; ce : IN STD_LOGIC ); END COMPONENT; ----------------------------------------------------------- COMPONENT mul16 IS PORT ( clk : IN STD_LOGIC; a : IN STD_LOGIC_VECTOR(15 DOWNTO 0); b : IN STD_LOGIC_VECTOR(15 DOWNTO 0); ce : IN STD_LOGIC; p : OUT STD_LOGIC_VECTOR(15 DOWNTO 0) ); END COMPONENT; --========================================================== --ATTRIBUTE KEEP : STRING; --ATTRIBUTE KEEP of mul12:SIGNAL is "true"; --ATTRIBUTE DONT_TOUCH : string; --ATTRIBUTE DONT_TOUCH of mulFP: COMPONENT is "yes"; -------------------------------------------------------------------- TYPE STATE_TYPE IS (S0,S1,S2,S3,S4,S5,S6,S7,S8,S9,S10,S11,S12,S13,S14); TYPE T_Param16 IS ARRAY(5 DOWNTO 0) OF std_logic_vector(15 DOWNTO 0); TYPE T_Param18 IS ARRAY(5 DOWNTO 0) OF std_logic_vector(17 DOWNTO 0); TYPE T_AXwert IS ARRAY(3 DOWNTO 0) OF INTEGER RANGE -32768 TO 32767; TYPE T_AXIS IS ARRAY(5 DOWNTO 0) OF INTEGER RANGE -32768 TO 32767; TYPE T_AXInc IS ARRAY(5 DOWNTO 0) OF std_logic_vector(31 DOWNTO 0); TYPE T_AXDif IS ARRAY(5 DOWNTO 0) OF std_logic_vector(15 DOWNTO 0); ----- TYPE T_AxFP IS ARRAY(5 DOWNTO 0) OF std_logic_vector(31 DOWNTO 0); TYPE T_Axr IS ARRAY(5 DOWNTO 0) OF std_logic_vector(15 DOWNTO 0); ------------------------------------------------------------------------- CONSTANT clkToFP : integer range 0 to 127 := 1; CONSTANT clkTosq : integer range 0 to 127 := 1; CONSTANT clkTomul : integer range 0 to 127 := 1; CONSTANT clkToadd : integer range 0 to 127 := 1; CONSTANT clkTosub : integer range 0 to 127 := 1; CONSTANT clkTosqrt : integer range 0 to 127 := 1; CONSTANT clkTodiv : integer range 0 to 127 := 2; CONSTANT clkFPtoInt : integer range 0 to 127 := 1; CONSTANT clkToAtan : integer range 0 to 127 := 25; CONSTANT clkToAcos : integer range 0 to 127 := 25; CONSTANT clkToCos : integer range 0 to 127 := 25; CONSTANT clkAtan : integer range 0 to 127 := 30; CONSTANT clkAtan32 : integer range 0 to 127 := 11; CONSTANT clkdiv32 : integer range 0 to 127 := 32; CONSTANT clkTosin : integer range 0 to 127 := 1; CONSTANT clkToMuli : integer range 0 to 127 := 2; CONSTANT C2 :std_logic_vector(31 DOWNTO 0):=X"40000000";-- zahl 2 in FP format 32bit CONSTANT C1 :std_logic_vector(31 DOWNTO 0):=X"3F800000";-- zahl 1 in FP format 32bit CONSTANT C0 :std_logic_vector(31 DOWNTO 0):=X"00000000";-- zahl 0 in FP format 32bit CONSTANT PIdiv2 :std_logic_vector(31 DOWNTO 0):=X"3FC90FDB";-- +1.57079632679 PI/2 in FP Format CONSTANT PIndiv2 :std_logic_vector(31 DOWNTO 0):=X"BFC90FDB";-- -1.57079632679 PI/2 in FP Format CONSTANT PI :std_logic_vector(31 DOWNTO 0):=X"40490FDB";-- 3.14159265359 PI in FP Format ------- CONSTANT C132 :std_logic_vector(31 DOWNTO 0):=X"20000000";--zahl 1 in format 1Q32 bit CONSTANT C126 :std_logic_vector(25 DOWNTO 0):="01" & X"000000";--zahl 1 in format 1Q26 bit CONSTANT C026 :std_logic_vector(25 DOWNTO 0):="00" & X"000000";-- zahl 0 in FP format 26bit CONSTANT Pihalf :std_logic_vector(25 DOWNTO 0):="001" & "10010010000111111011000";--Pi Half in 2Q26 ---------- --Anzahl Inkremente / Ummdrehung FP=für 20'000 = X"469c4000" -- Resol = 500 , 2PI / 500 = 0.0125 = 3c4ccccd -- 2PI/1000 =0.00628318531 = 3bcde32e -- 2Pi/2000 =0.00314159265 = 3b4de32e -- 2PI/ 1440 =0.00436332313 = 3b8efa35 -- 2PI/ 3600 = 0.00174532925 = 3ae4c388,1 Inkrement in rad FP bei 3600 Inkr pro U -- 2PI/5000 = 0.00125663706 = 3aa4b5be -- 2PI/10000 = 0.00062831853 = 3a24b5be -- 2PI /20000 =0.00031415927= X"39a4b5be" = 1 Inkrement in rad FP bei 20000 Inkr pro U --------------------------------------------------------------------------------- -----------3ae4c388 = 0.00174532923847 = 1 Inkrement in Rad,FP=2Pi/inkr pro U vo Rob Arm --CONSTANT A0res :std_logic_vector(31 DOWNTO 0):=X"37ce3750";--=0.00002458287 rad/inkr --CONSTANT A1res :std_logic_vector(31 DOWNTO 0):=X"37d4f7a6";--=0.00002538769 --CONSTANT A2res :std_logic_vector(31 DOWNTO 0):=X"3858f501";--=0.00005172659 --CONSTANT A3res :std_logic_vector(31 DOWNTO 0):=X"38532aa6";--=0.00005034604 --CONSTANT A4res :std_logic_vector(31 DOWNTO 0):=X"3a548e6e";--=0008108382 --CONSTANT A5res :std_logic_vector(31 DOWNTO 0):=X"38b847d0";--=0.0000878718 ----------------------------------------------------------------------- --CONSTANT C_A0res :std_logic_vector(31 DOWNTO 0):=X"48799a00";--255592 inkr /U --CONSTANT C_A1res :std_logic_vector(31 DOWNTO 0):=X"4871b2c0";--247499 inkr/U --CONSTANT C_A2res :std_logic_vector(31 DOWNTO 0):=X"47ed3e80";--121469 inkr/U --CONSTANT C_A3res :std_logic_vector(31 DOWNTO 0):=X"47f3c000";--124800 inkr/U --CONSTANT C_A4res :std_logic_vector(31 DOWNTO 0):=X"45f22800";--7749 inkr/U --CONSTANT C_A5res :std_logic_vector(31 DOWNTO 0):=X"478ba800";--71504 inkr/U -------------------------------------------------------------------- CONSTANT C_A0rad :std_logic_vector(31 DOWNTO 0):=X"471ee6b3";--40678.70 inkr /rad CONSTANT C_A1rad :std_logic_vector(31 DOWNTO 0):=X"4719deae";--39390.68 inkr /rad CONSTANT C_A2rad :std_logic_vector(31 DOWNTO 0):=X"469708c8";--19332.39 inkr /rad CONSTANT C_A3rad :std_logic_vector(31 DOWNTO 0):=X"469b2d0f";--19862.53 inkr /rad CONSTANT C_A4rad :std_logic_vector(31 DOWNTO 0):=X"449a28f6";--1233.28 inkr /rad CONSTANT C_A5rad :std_logic_vector(31 DOWNTO 0):=X"4631d0d7";--11380.21 inkr /rad ---------------------Virtuelle Inkremente------------------------------------------- --CONSTANT C_A0rad :std_logic_vector(31 DOWNTO 0):=X"43fa0000";--10000 inkr /rad:461c4000 --CONSTANT C_A1rad :std_logic_vector(31 DOWNTO 0):=X"43fa0000";--5000 inkr/rad:459c4000 --CONSTANT C_A2rad :std_logic_vector(31 DOWNTO 0):=X"43fa0000";--3000 inkr/rad:453b8000 --CONSTANT C_A3rad :std_logic_vector(31 DOWNTO 0):=X"43fa0000";--1000 inkr/rad:447a0000 --CONSTANT C_A4rad :std_logic_vector(31 DOWNTO 0):=X"43fa0000";--500 inkr/rad:43fa0000 --CONSTANT C_A5rad :std_logic_vector(31 DOWNTO 0):=X"43fa0000";--200 Inkr/rad:43480000 ------------------------------------------------------------------ CONSTANT A12res :std_logic_vector(31 DOWNTO 0):=X"3ae4c388"; CONSTANT AGres :std_logic_vector(31 DOWNTO 0):= X"3ae4c388";-- ------------------Kommt später über Config Darei ------------- CONSTANT C_Xs :STD_LOGIC_VECTOR(31 DOWNTO 0):= X"000001F4";-- 500 CONSTANT C_Ys :STD_LOGIC_VECTOR(31 DOWNTO 0):= X"FFFFFF38";-- -200 CONSTANT C_Zs :STD_LOGIC_VECTOR(31 DOWNTO 0):= X"00000078";-- 120 ----------------------wird im TOP gesetzt -------------------- --CONSTANT C_pfrq :STD_LOGIC_VECTOR(23 DOWNTO 0):= X"006400";--ITP frqPeriode:25600*10ns=256uS=3.9kHz --CONSTANT C_pfrq :STD_LOGIC_VECTOR(23 DOWNTO 0):= X"000320";--ITP frqPeriode:800*10ns=8uS=125kHz --CONSTANT C_pfrq :STD_LOGIC_VECTOR(23 DOWNTO 0):= X"000190";--ITP frqPeriode:400*10nS=4uS=250KHz --CONSTANT C_pfrq :STD_LOGIC_VECTOR(23 DOWNTO 0):= X"0000C8";--ITP frqPeriode:200*10nS=2uS=500Khz --CONSTANT C_pfrq :STD_LOGIC_VECTOR(23 DOWNTO 0):= X"000064";--ITP frqPeriode:100*10nS=1uS=1Mhz ---------------------------------------------------------- --============================================================================== SIGNAL stateIni :integer range 0 to 31:= 0; SIGNAL stateITP :integer range 0 to 31:= 0; SIGNAL staterunX :integer range 0 to 7:= 0; SIGNAL staterunY :integer range 0 to 7:= 0; SIGNAL staterunZ :integer range 0 to 7:= 0; SIGNAL staterunLk :integer range 0 to 7:= 0; SIGNAL stateangle :integer range 0 to 63:= 0; SIGNAL stateLinX :integer range 0 to 7:= 0; SIGNAL stateLinY :integer range 0 to 7:= 0; SIGNAL statechg :integer range 0 to 7:= 0; SIGNAL ITPSTATE :Integer range 0 to 2 := 0; ----------------------------------------------- --========= Calc ALL Modul================================== SIGNAL stateAll :STATE_TYPE; --======================================================================== --============= LinStz Interpolator Signale ============================== SIGNAL sitprun :std_logic:='0';--itprun für interne gebrauch --======= runLinX,runLinY signale====================================== SIGNAL stateLin :STATE_TYPE; SIGNAL stateLinB :STATE_TYPE; SIGNAL linLka :INTEGER:= 0; SIGNAL LinLk :INTEGER:= 0;-- mAXAX für Linstz SIGNAL dLklin :INTEGER:= 0; SIGNAL dXlin :INTEGER:= 0; SIGNAL dYlin :INTEGER:= 0; SIGNAL XlinAbs :std_logic_vector(15 DOWNTO 0):=X"0000"; SIGNAL YLinAbs :std_logic_vector(15 DOWNTO 0):=X"0000"; SIGNAL Xlina :std_logic_vector(15 DOWNTO 0):=X"0000"; SIGNAL YLina :std_logic_vector(15 DOWNTO 0):=X"0000"; SIGNAL Xdifa :std_logic_vector(15 DOWNTO 0):=X"0000"; SIGNAL Ydifa :std_logic_vector(15 DOWNTO 0):=X"0000"; SIGNAL FLinXneg :std_logic:='0';--Flag koord ist negativ SIGNAL FLinYneg :std_logic:='0';-- SIGNAL LinITPok :std_logic:='0';-- SIGNAL LinRun :std_logic:='0';-- SIGNAL ImpLkLin :std_logic:='1';--Lk Impulse von LinITP --============================================================================= SIGNAL lincalcOK :std_logic:='0';--start für RAMP von LinSatz --======== Ende Signale LinStz Verarbeitung ============================= SIGNAL XYZcalcOK :std_logic:='0';--start für RAMP von XYZSatz --====================XYZ ITP============================================================ -----------iniITPxyz Signale---------------------------------------- SIGNAL YXTneg :std_logic:='0';--XT < YT then YXTneg :=1 ---------------------- ---SIGNAL cefrqFP :std_logic:='0';-- start Int to FP frqPER =>frqFP --SIGNAL frqPER :std_logic_vector(31 DOWNTO 0):=X"00000000";-- FrqPeriode, Steuerung ITP frq --SIGNAL frqFP :std_logic_vector(31 DOWNTO 0):=X"00000000";-- frq FP -------------------- SIGNAL FXt :std_logic_vector(31 DOWNTO 0):=(OTHERS => '0'); SIGNAL FYt :std_logic_vector(31 DOWNTO 0):=(OTHERS => '0'); SIGNAL FZt :std_logic_vector(31 DOWNTO 0):=(OTHERS => '0'); SIGNAL FXtabs :std_logic_vector(31 DOWNTO 0):=(OTHERS => '0');-- abs wert für crxdiv usw. SIGNAL FYtabs :std_logic_vector(31 DOWNTO 0):=(OTHERS => '0'); SIGNAL FZtabs :std_logic_vector(31 DOWNTO 0):=(OTHERS => '0'); ----- state3----------------------------- SIGNAL cesqXYZ :std_logic:='0';-- start sq SIGNAL sqFXt :std_logic_vector(31 DOWNTO 0):=(OTHERS => '0'); SIGNAL sqFYt :std_logic_vector(31 DOWNTO 0):=(OTHERS => '0'); SIGNAL sqFZt :std_logic_vector(31 DOWNTO 0):=(OTHERS => '0'); ---------------state 4 ----------------------- SIGNAL ceaddXY :std_logic:='0';-- start Add XY SIGNAL addXY :std_logic_vector(31 DOWNTO 0):=(OTHERS => '0'); ---------------state 5 ---------------------- SIGNAL ceaddXYZ :std_logic:='0';-- start Add XYZ SIGNAL addXYZ :std_logic_vector(31 DOWNTO 0):=(OTHERS => '0'); --------------- state 6-------------------------- SIGNAL cesqrtFLk :std_logic:='0';-- start sqrt SIGNAL FLk :std_logic_vector(31 DOWNTO 0):=(OTHERS => '0');--FLk Länge in XT,YT,ZT SIGNAL FLT :std_logic_vector(31 DOWNTO 0):=(OTHERS => '0');--FLT länge in XT,YT SIGNAL XTLTdiv :std_logic_vector(31 DOWNTO 0):=(OTHERS => '0');--XTLTdiv:=FXT/FLT --------state10------------------------ SIGNAL ceTgAT :std_logic:='0';-- Start catgAT SIGNAL AZFP :std_logic_vector(31 DOWNTO 0):=(OTHERS => '0');--AZFP= Winkel Strecke ZT -------state 11------------------------------ SIGNAL cecmpAT :std_logic:='0'; SIGNAL ATgrPI2 :STD_LOGIC_VECTOR(0 DOWNTO 0):=(others => '0'); SIGNAL grPI2 :std_logic:='0'; SIGNAL ATFP :std_logic_vector(31 DOWNTO 0):=(OTHERS => '0');--ATFP= Winkel Strecke YT XT SIGNAL initITPok :std_logic:='0';--initITP fertig SIGNAL ITPlin :STD_LOGIC_VECTOR(2 DOWNTO 0):= "000"; --================== InitITPlin fertig==================================== ----------------------------------------------------- --==========RUNITP Signale(Interpolator für virtuelle Achsen XYZ )========== SIGNAL ITPxyz :STD_LOGIC_VECTOR(2 DOWNTO 0):= "000"; SIGNAL XTabs :std_logic_vector(31 downto 0):=(OTHERS => '0');-- Targetkord(XT= abs SIGNAL YTabs :std_logic_vector(31 downto 0):=(OTHERS => '0');-- Targetkord(YT= abs SIGNAL ZTabs :std_logic_vector(31 downto 0):=(OTHERS => '0');-- Targetkord(ZT= avs SIGNAL fXneg :std_logic:='0';-- negativ Flag X für Xe - Xs SIGNAL fYneg :std_logic:='0';-- negativ Flag X für Ye - Ys SIGNAL fZneg :std_logic:='0';-- negativ Flag X für Ze - Zs SIGNAL ceXYZF :std_logic:='0';-- start für initITPlin SIGNAL runXYZ :std_logic:='0';-- Proc IMpX..IMpZ aktiv SIGNAL ceFlkInt :std_logic:='0';-- Flk to LK SIGNAL XYZok :std_logic:='0';-- XYZbresenham fertig ------------Signale für Bresenham process : brshXYZ ---------------------------- SIGNAL statebrsh :INTEGER RANGE 0 TO 7 := 0; SIGNAL dLk :integer:= 0; SIGNAL dX :integer:= 0; SIGNAL dY :integer:= 0; SIGNAL dZ :integer:= 0; SIGNAL Lk :std_logic_vector(31 downto 0):=(OTHERS => '0');--leitkoordinate SIGNAL Lka :integer;-- Aktuelle Virtuelle Koordinate Lk SIGNAL strtITP :std_logic:='0';-- start für XYZ itp --------------------------------------------------------------------------- --------------Signale für impX, generiert Xa--------------------------------- SIGNAL Xa :std_logic_vector(31 downto 0):=(OTHERS => '0');-- Aktuelle Koordinate X SIGNAL ImpX :std_logic:='1';-- inkrement Impuls --------------Signale für impY-----generiert Ya---------------------------- SIGNAL Ya :std_logic_vector(31 downto 0):=(OTHERS => '0');-- Aktuelle Koordinate Y SIGNAL ImpY :std_logic:='1';-- inkrement Impuls --------------Signale für impZ----generiert Za----------------------------- SIGNAL Za :std_logic_vector(31 downto 0):=(OTHERS => '0');-- Aktuelle Koordinate z SIGNAL ImpZ :std_logic:='1';-- inkrement Impuls -------------------------------------------------------- ------------------------generiert ImpLk---------------------------- SIGNAL ctrPLk :Integer range 0 to 33554431:=0;-- dekr. Counter für Imp. Periode SIGNAL ImpLk :std_logic:='1';-- inkrement virtuelle Lk Impuls von XYZ ITP (invers Trans) --====================================================================== -----RUNANGLE Berechnung : a0 := arctan(Ya/Xa) und aXYT:=arctan(YT/XT) Berechnung------------------------------- SIGNAL ceXYZFP :std_logic:='0'; ---------------------- SIGNAL FXa :std_logic_vector(31 DOWNTO 0):=(OTHERS => '0');--Aktuelle X Position FP SIGNAL FYa :std_logic_vector(31 DOWNTO 0):=(OTHERS => '0'); SIGNAL FZa :std_logic_vector(31 DOWNTO 0):=(OTHERS => '0'); SIGNAL AZFPabs :std_logic_vector(31 DOWNTO 0):=(OTHERS => '0');-- abs werte -----------------------state 0--------------------- SIGNAL FXYZaOK :std_logic:= '0';--signalisiert wenn FXYZa bereit SIGNAL suPIAT :std_logic_vector(31 downto 0):=(OTHERS => '0'); SIGNAL adPIAT :std_logic_vector(31 downto 0):=(OTHERS => '0'); ----------------- state 2---------------------------------- SIGNAL cePI2 :std_logic:='0';-- start für sPIAT := PI/2 - PIATabs für sin AT SIGNAL PIAT :std_logic_vector(31 downto 0):=(OTHERS => '0'); SIGNAL PIATabs :std_logic_vector(31 downto 0):=(OTHERS => '0'); SIGNAL sPIAT :std_logic_vector(31 downto 0):=(OTHERS => '0'); ---------------- state 3 ---------------------------------- SIGNAL cetgFP :std_logic:='0'; SIGNAL A0FP :std_logic_vector(31 DOWNTO 0):=(OTHERS => '0');--32 bit Resultat von ArcTanFP SIGNAL PIAZ :std_logic_vector(31 downto 0):=(OTHERS => '0');--Res Pi/2 - AZFP SIGNAL sinAZ :std_logic_vector(31 downto 0):=(OTHERS => '0');-- SIGNAL cosAZ :std_logic_vector(31 downto 0):=(OTHERS => '0');-- SIGNAL cosAT :std_logic_vector(31 downto 0):=(OTHERS => '0');-- SIGNAL sinAT :std_logic_vector(31 downto 0):=(OTHERS => '0');-- -----------------State 9 --------------------------------- SIGNAL cemul9 :std_logic:='0'; SIGNAL sAZAT :std_logic_vector(31 downto 0):=(OTHERS => '0');-- SIGNAL cAZAT :std_logic_vector(31 downto 0):=(OTHERS => '0');-- -----------------State 10 ----------------------------------- SIGNAL cemul10 :std_logic:='0'; SIGNAL sAZAT3 :std_logic_vector(31 downto 0):=(OTHERS => '0');-- SIGNAL cAZAT3 :std_logic_vector(31 downto 0):=(OTHERS => '0');-- SIGNAL cAZAZ3 :std_logic_vector(31 downto 0):=(OTHERS => '0');-- -----------------State 11--------------------------- SIGNAL cesubW :std_logic:='0'; SIGNAL aFXw :std_logic_vector(31 downto 0):=(OTHERS => '0');-- SIGNAL sFXw :std_logic_vector(31 downto 0):=(OTHERS => '0');-- SIGNAL FXw :std_logic_vector(31 downto 0):=(OTHERS => '0');-- SIGNAL FYw :std_logic_vector(31 downto 0):=(OTHERS => '0');-- SIGNAL FZw :std_logic_vector(31 downto 0):=(OTHERS => '0');-- ---------------- State 12------------------------------ SIGNAL cemul12 :std_logic:='0'; SIGNAL sqXw :std_logic_vector(31 downto 0):=(OTHERS => '0');-- SIGNAL sqYw :std_logic_vector(31 downto 0):=(OTHERS => '0');-- SIGNAL subATAW :std_logic_vector(31 downto 0):=(OTHERS => '0');-- ---------------state 13----------------------------------- SIGNAL subPIdiv :std_logic_vector(31 downto 0):=(OTHERS => '0');-- SIGNAL ceaddXYw :std_logic:='0'; SIGNAL sqXYw :std_logic_vector(31 downto 0):=(OTHERS => '0');-- SIGNAL AwFP :std_logic_vector(31 downto 0):=(OTHERS => '0');-- ---------------- State 14 --------------------------------- SIGNAL cesqrtLw :std_logic:='0'; SIGNAL cemAZA3 :std_logic:='0'; SIGNAL LwFP :std_logic_vector(31 downto 0):=(OTHERS => '0');-- SIGNAL DivPIdiv :std_logic_vector(31 downto 0):=(OTHERS => '0');-- ------------------state 15---------------------------- SIGNAL cedAZA3 :std_logic:='0'; SIGNAL sqZ2 :std_logic_vector(31 downto 0):=(OTHERS => '0');-- SIGNAL sqW2 :std_logic_vector(31 downto 0):=(OTHERS => '0');-- ------------------state 16 ----------------------------------- SIGNAL cedivPIdiv :std_logic:='0'; SIGNAL ceA4FP :std_logic:='0'; SIGNAL sqaddWZ :std_logic_vector(31 downto 0):=(OTHERS => '0');-- ----------------- state 17 ----------------------------------- SIGNAL ceAZmul :std_logic:='0'; SIGNAL FL12 :std_logic_vector(31 downto 0):=(OTHERS => '0');-- SIGNAL A12FP :std_logic_vector(31 downto 0):=(OTHERS => '0');-- SIGNAL runCalcP1 :std_logic:='0';-- start für calcP1 ----------------- state 18 -------------------------------- SIGNAL A4FP :std_logic_vector(31 downto 0):=(OTHERS => '0');-- SIGNAL absdivPIdiv :std_logic_vector(31 downto 0):= (OTHERS => '0');-- ----------------- state 19 -------------------------------- SIGNAL sub1min :std_logic_vector(31 downto 0):= (OTHERS => '0');-- SIGNAL cesub1min :std_logic:='0'; ----------------- state 20 -------------------------------- SIGNAL ceA3FP :std_logic:='0'; SIGNAL A3FP :std_logic_vector(31 downto 0):=(OTHERS => '0');-- --=============================================================== ----------------state 22-- nur für test ----------------- SIGNAL ceXYZw :std_logic:='0'; SIGNAL Xw :std_logic_vector(31 downto 0):=(OTHERS => '0');-- SIGNAL Yw :std_logic_vector(31 downto 0):=(OTHERS => '0');-- SIGNAL Zw :std_logic_vector(31 downto 0):=(OTHERS => '0');-- --======================run angle fertig============================================= SIGNAL A5FP :std_logic_vector(31 downto 0):=X"00000001";-- --------------Signale für XYZ Modul------------------------------- SIGNAL L4 :std_logic_vector(31 downto 0):=(OTHERS => '0'); SIGNAL AXYT :std_logic_vector(19 DOWNTO 0):=(OTHERS => '0');--20 bit integer output ---------------------------------- --*************************************************************************** --====================================================== ---------calcALL------------------------------ SIGNAL FL1 :std_logic_vector(31 DOWNTO 0):= (OTHERS => '0'); SIGNAL FL2 :std_logic_vector(31 DOWNTO 0):= (OTHERS => '0'); SIGNAL FL3 :std_logic_vector(31 DOWNTO 0):= (OTHERS => '0'); SIGNAL FL4 :std_logic_vector(31 DOWNTO 0):= (OTHERS => '0'); SIGNAL FW1 :std_logic_vector(31 DOWNTO 0):= (OTHERS => '0'); SIGNAL FW2 :std_logic_vector(31 DOWNTO 0):= (OTHERS => '0'); SIGNAL FZ1 :std_logic_vector(31 DOWNTO 0):= (OTHERS => '0'); SIGNAL FZ2 :std_logic_vector(31 DOWNTO 0):= (OTHERS => '0'); SIGNAL FA1 :std_logic_vector(31 DOWNTO 0):= (OTHERS => '0'); SIGNAL FA2 :std_logic_vector(31 DOWNTO 0):= (OTHERS => '0'); SIGNAL ceLFP :std_logic:='0';--FP conversion enable ------------------------------------------------------------- --======================================================================== ------------RUNANGLE vorher-Signale für calcP1------------------ SIGNAL ceP1 :std_logic:='0';--start sq für P1 SIGNAL StrtP3 :std_logic:='0';-- start für parallel ablauf P3 SIGNAL cesqL :std_logic:='0'; SIGNAL sqL1 :std_logic_vector(31 DOWNTO 0):=(OTHERS => '0'); SIGNAL sqL2 :std_logic_vector(31 DOWNTO 0):=(OTHERS => '0'); SIGNAL sqL12 :std_logic_vector(31 DOWNTO 0):=(OTHERS => '0'); SIGNAL mul12 :std_logic_vector(31 DOWNTO 0);----------:=(OTHERS => '0'); ----SIGNAL mul12i :std_logic_vector(31 DOWNTO 0):=(OTHERS => '0'); SIGNAL ceadd1 :std_logic:='0'; SIGNAL sqadd1 :std_logic_vector(31 DOWNTO 0):= (OTHERS => '0'); SIGNAL cesub1 :std_logic:='0'; SIGNAL sqsub1 :std_logic_vector(31 DOWNTO 0):= (OTHERS => '0'); SIGNAL cem32 :std_logic:='0'; SIGNAL mulres1 :std_logic_vector(31 DOWNTO 0);----------:= (OTHERS => '0'); ----SIGNAL mulres1i :std_logic_vector(31 DOWNTO 0):= (OTHERS => '0'); SIGNAL cediv1 :std_logic:='0'; SIGNAL ax :std_logic_vector(31 DOWNTO 0):= (OTHERS => '0'); --------------------------- SIGNAL ceacosFP :std_logic:='0';-- Start arccosFP SIGNAL acosAX :std_logic_vector(31 DOWNTO 0):= (OTHERS => '0'); SIGNAL asinAX :std_logic_vector(31 DOWNTO 0):= (OTHERS => '0');-- wird nicht benützt SIGNAL ceA1FP :std_logic:='0'; SIGNAL A1FP :std_logic_vector(31 DOWNTO 0):=(OTHERS => '0'); SIGNAL cesubA1 :std_logic:='0'; SIGNAL A1SFP :std_logic_vector(31 DOWNTO 0):= (OTHERS => '0'); SIGNAL cecosFP :std_logic:='0'; SIGNAL cosA1FP :std_logic_vector(31 DOWNTO 0):= (OTHERS => '0');--1te resultat cosinFP SIGNAL sinA1FP :std_logic_vector(31 DOWNTO 0):= (OTHERS => '0');--2te resultat cosinFP --------------------- SIGNAL ceZ1W1 :std_logic:='0'; ------ RUNANGLE vorher Calc P1 blatt1 fertig, Blatt2 anfang SIGNAL ceZW12 :std_logic:='0'; SIGNAL Z2Z1 :std_logic_vector(31 DOWNTO 0):= (OTHERS => '0'); SIGNAL W2W1 :std_logic_vector(31 DOWNTO 0):= (OTHERS => '0'); SIGNAL cedivZW :std_logic:='0'; SIGNAL divZW :std_logic_vector(31 DOWNTO 0):= (OTHERS => '0'); SIGNAL ceatanZW :std_logic:='0'; SIGNAL atanZW :std_logic_vector(31 DOWNTO 0):= (OTHERS => '0'); SIGNAL A2FP :std_logic_vector(31 DOWNTO 0):= (OTHERS => '0'); SIGNAL cesubAGA2 :std_logic:='0'; SIGNAL AGA2 :std_logic_vector(31 DOWNTO 0):= (OTHERS => '0'); SIGNAL cesubA2 :std_logic:='0'; -------- RUNANGLE vorherende calc P1 --------------------------------- ----------------------------------------------------------------- ----------ITPImpGen--------------------------------------------- ---TYPE T_INKDEK is ARRAy(5 DOWNTO 0) of std_logic_vector(1 DOWNO 0); -- Achsen(Inc, Dec) SIGNAL AXinc :T_AXInc;--IS ARRAY(5 DOWNTO 0) OF SLV(31 DOWNTO 0); SIGNAL AXalt :T_AXInc;--IS ARRAY(5 DOWNTO 0) OF SLV(31 DOWNTO 0); SIGNAL vAXdif :T_AXInc;--IS ARRAY(5 DOWNTO 0) OF SLV(31 DOWNTO 0); SIGNAL AXdif :T_AXdif;--IS ARRAY(5 DOWNTO 0) OF SLV(15 DOWNTO 0); ------------------ SIGNAL AXGr :T_Axr;-- Winkel der Achsen A0..A5 in GRD.X, (std Logic vector) SIGNAL ceAxx :std_logic:='0';-- start für Axconv für A1,A2 A12 ---------------------- SIGNAL A12 :std_logic_vector(19 DOWNTO 0):=X"00000";--20 bit integer output SIGNAL statITPimp :INTEGER RANGE 0 TO 31; SIGNAL AXinkr :std_logic_vector(191 downto 0):= (OTHERS => '0');--Sollwerte von inv.Transfer und Lin --====================================================================== --************************************************************************** BEGIN --=================================================================================== ---pfrq SLV(23 downto 0);--Bahn-Periode in anzahl CLK (10ns) --- frqPer :32 bit --FrqGen:PROCESS(clk) --VARIABLE cntrclk : INTEGER RANGE 0 TO 1000000; --BEGIN -- IF rising_edge(clk) THEN -- IF nrst = '0' THEN -- cntrclk := 0; -- cefrqFP <= '0'; -- ELSE -- IF cntrclk > 0 THEN -- cntrclk := cntrclk - 1; -- cefrqFP <= '0'; -- ELSE -- frqPER <= X"00" & pfrq;--ITP frqPeriode laden -- cefrqFP <= '1';-- start Int to FP für frqPer -- cntrclk := 4000; --- 20 us interval -- END IF; -- END IF; --IF nrst /ELSE -- END IF; --clk --END PROCESS;--FrqGen --============================================================================ --=======Linear Interpolation in erinzelnen Achsen X,Y gemäss ebene, Kartesisch===== -- Generiert eine linearinterpolation in den AchsenXY gemäss konzept von Optinumeric -- Programmiert als lin Interpolationsatz mit nur 2 Achsen,:label 1 --============================================================================ --=============****************************************=================================== ---====== XYZ Interpolation(invers Transformation) ablauf in den Achsen A5..A0 :ab Label 6 --========================================================================= --Entity :oAXdif:std_logic_vector(95 downto 0):differenz von alle 6 Achsen(6 *16 bits) --SIGNAL AXdif :T_AXdif;--IS ARRAY(5 DOWNTO 0) OF SLV(15 DOWNTO 0); -- Liefert in der Entity Signal oAxdif(95..0):achsen solldifferenzen. -- später werden in TopAS21 in der Procedur SollInc(z6024) die Sollwerte für Alle 6 Achsen (SOLL) -- wobei diese als Input für Ramp dienen. -------------------------------------------------- ImpSwitch:PROCESS(clk) VARIABLE Lin :std_logic := '0';--für lin=1, für XYZ = 0 VARIABLE INDX :INTEGER RANGE 0 TO 127:= 0; VARIABLE RunStop :std_logic := '0';--speichert Stopp impuls -- bis Rampenende BEGIN IF rising_edge(clk) THEN IF nrst = '0' THEN itprun <= '0'; sitprun <= '0'; enditp <= '0'; indx := 0; ITPmod <= "000";-- init ITmod lin:= '0';-- init ITPSTATE <= 0; statechg <= 0; oAXdif <= (OTHERS => '0'); CalcOK <= '0';-- RESET IMPULS fürstart RAMP ELSE CASE ITPSTATE IS WHEN 0 => IF celin = '1' THEN Lin := '1'; indx := conv_integer(ebene(1 DOWNTO 0));-- ebene ini Receiv gesetzt Byte(1) 2..0 ITPSTATE <= 1; ELSIF ce = '1' THEN Lin := '0'; ITPSTATE <= 1; ELSE ITPSTATE <= 0; END IF; WHEN 1 => IF lin = '1' THEN ITPmod <= ITPlin; ELSE ITPmod <= ITPxyz; END IF; --------------- IF (XYZok = '1') OR (LinITPok = '1') THEN ITPmod <= "000"; ITPSTATE <= 0; ELSE ITPSTATE <= 1; END IF; WHEN OTHERS => ITPSTATE <= 0; END CASE; ---------------------------- CASE statechg IS WHEN 0 => IF linrun = '1' THEN --Lin Itp pulsw itprun <= '1';--Itp Läuft sitprun <= '1'; enditp <= '1';--itp Läuft statechg <= 1; ELSIF runXYZ = '1' THEN --InvwersTransf Impulse itprun <= '1';--Itp Läuft sitprun <= '1'; enditp <= '1';--itp Läuft statechg <= 6; ELSE -- keine Interpolation, warten auf rampen ende resp neue stz statechg <= 0;-- und warten END IF; --================================================================== ------------------- LinSatz------------------------------------ WHEN 1 => -- ablauf linStz IF linrun = '1' THEN IF LinITPok = '1' THEN itprun <= '0';-- lin ITP fertig sitprun <= '0'; ELSE -- LINInterpolation läuft oImpLk <= ImpLkLin;--Impulse LK von LinITP calcOK <= lincalcOK;--start für RAMP --oAXinkr(31..0), Xlina(15..0) IF Xactiv = '1' THEN ---- WinkelAchsen Inkremente auf entity oAXdif(indx*32+15 DOWNTO indx*32) <= Xdifa; END IF; IF Yactiv = '1' THEN oAXdif(indx*32+31 DOWNTO indx*32+16) <= Ydifa; END IF; ---------------------------- IF stopp = '1' OR RunStop = '1' THEN RunStop := '1'; --Stopp speichern itpRun <= '0'; -- Unterbruch wg limit oder Endlage sitprun <= '0'; enditp <= '0';-- ende kontur ELSE ItpRun <= '1';-- ITP lin läufzt sitprun <= '1'; enditp <= '1';-- ende kontur END IF; END IF; ELSE -- linRun fertig, warten auf rampenende IF RampRun = '1' THEN IF stopp = '1' OR RunStop = '1' THEN RunStop := '1'; --Stopp speichern END IF; statechg <= 1; ELSE enditp <= '0'; RunStop := '0'; statechg <= 0;-- ende satz, warten auf neue satz END IF; END IF; --==================================================================================------------------------XYZ Satz= inv.Transf------------------------- WHEN 6 => -- impuls ausgabe bei invers transf IF runXYZ = '1' THEN --interpolation (Bresenham) läuft,itprun=1 oImpLk <= ImpLk;--virt.ImpulseLK von Inv.Transf (XYZ ITP,Bresenham) FOR i IN 0 TO 5 LOOP oAXdif(i*16+15 DOWNTO i*16) <= AXdif(i);-- WinkelAchsensolldif Inkremente auf entity END LOOP; calcOK <= XYZcalcOK;--start für RAMP IF stopp = '1' OR RunStop = '1' THEN ItpRun <= '0'; -- Unterbruch wg limit oder Endlage sitprun <= '0'; RunStop := '1'; -- speichern Stopp ELSE IF XYZok = '1' THEN --- wenn Bressenham fertig ItpRun <= '0'; enditp <= '0'; -- ende kontur sitprun <= '0'; ELSE ItpRun <= '1'; sitprun <= '1'; enditp <= '1'; -- kontur läuft END IF; END IF; statechg <= 6; ELSE -- Bresenham Interpolation fertig IF RampRun = '1' THEN IF stopp = '1' OR RunStop = '1' THEN RunStop := '1'; --Stopp speichern END IF; statechg <= 6; ELSE RunStop := '0'; statechg <= 0;-- ende satz, warten auf neue satz END IF; END IF; --nächste Impulsausgabe WHEN OTHERS => statechg <= 0; END CASE; END IF;--if nrst/else END IF;-- clk END PROCESS;-- IMPSwitch --============================================================= -- bei linstz werden nur 16 bit akzeptiert (ist nur zum optimieren der Achsen) -- d.h. Max eingabe = +/- 327.67 für X,oder Y Linsatz --==Steuert ITP für linStz ============================== -- LinLk (intg):-- Generiert aus MAXAX bei Empfang von Linstz -- Xlin,Ylin wurden bei Empfang von Linstz geladen -- generiert ITPlin (status ITP) ------------------------------------------------ runITPlin:PROCESS(clk) VARIABLE cntrclk : INTEGER RANGE 0 TO 3000000:= 0;-- für 30ms max VARIABLE iEB :INTEGER RANGE 0 TO 2 := 0;-- zwischenspeicher für ebene BEGIN IF rising_edge(clk) THEN IF nrst = '0' THEN LinRun <= '0'; -- reset start Bresenhamlin dXlin <= 0; dYlin <= 0; dLkLin <= 0; StateLin <= S0; cntrclk := 0; iEB := 0; XLinAbs <= (OTHERS =>'0'); YLinAbs <= (OTHERS =>'0'); ITPlin <= "000"; --init ITP für Status ELSE CASE stateLin IS WHEN S0 => -- start mit celin IF ceLin = '1' THEN iEB := conv_integer(Ebene);-- Ebene für linSatz StateLin <= S1; ELSE StateLin <= S0;-- warten END IF; WHEN S1 => -- init div koordinaten ITPlin(conv_integer(iEB)) <= '1';--Itp für aktive Ebene ----------------- IF Xlin(15) = '1' THEN -- negativ koord FLinXneg <= '1'; XLinAbs <= (Xlin XOR X"FFFF") + 1; ELSE FLinXneg <= '0'; XlinAbs <= Xlin; END IF; ---------------------- IF Ylin(15) = '1' THEN -- negativ koord FLinYneg <= '1'; YLinAbs <= (Ylin XOR X"FFFF") + 1; ELSE FLinYneg <= '0'; YlinAbs <= Ylin; END IF; -------------------- StateLin <= S2; WHEN S2 => dXlin <= conv_integer(XLinAbs);-- Xkoord dYlin <= conv_integer(YLinAbs); StateLin <= S3; WHEN S3 => IF dXLin >= dYlin THEN LinLk <= dXLin;--maxax setzen ELSE LinLk <= dYLin; END IF; -- für 2 status mldg 30 us cntrclk := 2000000;-- 20 mS warte zeit StateLin <= S4; WHEN S4 => IF cntrclk > 0 THEN cntrclk := cntrclk -1; StateLin <= S4; ELSE dLklin <= LinLk;--Leit koord, ITPlin(conv_integer(iEB)) <= '0';--Itp init für status cntrclk := 170;-- 2.0 us min Impulslänge StateLin <= S5;---S5;----ohne start imp END IF; ---------auf startImpuls warten------------------------- WHEN S5 => --- warten auf Start Impuls länge min 2,4us IF cntrclk > 0 THEN cntrclk := cntrclk -1; IF strimp = '1' THEN stateLin <= S5;-- warten ELSE stateLin <= S6;-- impuls ist kürzer, neu anfangen END IF; ELSE -- zeit abgelaufen stateLin <= S7;-- impuls hat richtige länge END IF; WHEN S6 => -- neu anfangen mit Impuls abwarten cntrclk := 170;-- 2.0 us min Impulslänge stateLin <= S5;-- neu anfangen -----------start impuls empfangen------------------------ WHEN S7 => LinRun <= '1'; -- Bresenhamlin starten(impuls) ITPlin(conv_integer(iEB)) <= '1';--Itp für status StateLin <= S8; WHEN S8 => StateLin <= S9; WHEN S9 => -- warten bis interpolation (ITP) fertig ist IF LinITPok = '1' THEN LinRun <= '0'; ITPlin(conv_integer(iEB)) <= '0';--Itp für status(itp fertig) StateLin <= S10;-- und weiter ELSE StateLin <= S9;-- warten bis ITP fertig ist END IF; WHEN S10 => StateLin <= S0;-- und Warten auf nächste LInStz WHEN OTHERS => dXlin <= 0; dYlin <= 0; dLkLin <= 0; StateLin <= S0; END CASE; END IF;-- if nrst/else END IF; -- if clk END PROCESS;-- runITPlin --================================================================= --=====================Bresenham Interpolation für linStz ========== --- interpoliert in XY eine gewählte Ebene in LinStz(linear Satz) -- ImpXlin,ImpYin sind Impulse in XY,, --ImpLklin sind virtuelle Impulse in entlang der Leitkoordinate -- XlinA,YlinA ist absolute Position -- Ende der Interpolation : LinItpok = 1 (2 clk impuls) --Vorschubspeed definiert mit pfrq (anz.clk(10ns) zwischen --zwei ImpLklin impulsen ------------------------------------------------------- breshamLin: PROCESS(clk) VARIABLE count :INTEGER; VARIABLE errX :INTEGER; VARIABLE errY :INTEGER; VARIABLE XaltA :std_logic_vector(15 DOWNTO 0):=X"0000"; VARIABLE YaltA :std_logic_vector(15 DOWNTO 0):=X"0000"; BEGIN IF rising_edge(clk) THEN IF nrst = '0' THEN stateLinB <= S0; errX :=0; errY :=0; XLinA <= (OTHERS =>'0'); YLinA <= (OTHERS =>'0'); LinItpok <= '0'; count := 0; ImpLkLin <= '1';--Impulse (Lk)von LinITP LinCalcOK <= '0'; ELSE CASE statelinB IS WHEN S0 => IF LinRun = '1' THEN count := conv_integer(pfrq); stateLinB <= S1; ELSE errX :=dLklin/2; errY :=dLklin/2; LinLka <= 0; LinItpok <= '0'; stateLinB <= S0; END IF; WHEN S1 => LinCalcOK <= '0';-- RESET IMPULS calcOK IF count = 0 THEN -- warten für pfrq periode IF LinLka < linLk THEN --wenn noch nicht fertig LinLka <= LinLka + 1; ------------------------ errX := errX - dXlin; IF errX < 0 THEN XlinA <= XlinA + 1; ImpLklin <= '0'; errX := errX + dLkLin; END IF; ---------------------- errY := errY - dYlin; IF errY < 0 THEN YLinA <= YLinA + 1; ImpLklin <= '0'; errY := errY + dLkLin; END IF; ------------------------- count := conv_integer(pfrq); stateLinB <= S2;-- itp noch nicht fertig ELSE -- dann itp fertig count := 0;-- pfrq Zähler stoppen stateLinB <= S4;--dann itp fertig END IF; ELSE -- warten auf count ablauf(prfg periode) count := count - 1; stateLinB <= S1; END IF; WHEN S2 => --noch nicht fertig ImpLklin <= '1'; XdifA <= XlinA - XaltA; YdifA <= YlinA - YaltA; stateLinB <= S3; WHEN S3 => XaltA := XlinA; YaltA := YLinA; LinCalcOK <= '1';--dann start für daten einlesen in RaMP IF stopp = '1' THEN --sofort ITP anhalten stateLinB <= S5;-- Stopp Exit ELSE -- kein stopp stateLinB <= S1;-- dann repeat END IF; -------------------------------- WHEN S4 => -- itp fertig stateLinB <= S5;-- normal exit -------------------------------------------- WHEN S5 => -- stop exit XLinA <= (OTHERS =>'0'); YLinA <= (OTHERS =>'0'); errX :=0; errY :=0; count := 0; ImpLkLin <= '1';--Impulse (Lk)von LinITP stoppen LinItpok <= '1'; stateLinB <= S0;--und warten auf nächste linITP Satz WHEN others => stateLinB <= S0; END CASE; END IF;--IF nrst/ELSE END IF; -- if clk END PROCESS;-- bresenham für XY LinKoordinaten --============= ende LinStz Interpolation ===================================== ------------------------------------------------------------------------------------ --======Invers transformation mit Linearinterpolation in XYZ Raum================ ----------------------------------------------------------------- --======berechnet gemäss XT,YT,ZT die Priode Dauer PX,Py,PZ für virtualle Strecke XYZ --====== sowie AtFP, AtgrPI2, AZFP, FLk =============================================== --== Flk := sqrt (xT^2 + YT^2 + ZT^2) ----vorbereitung Berechnungen für virtuelle Interpolation in XYZ-------------------- --== Formel für X Achse, gilt das gleiche für Achse Y und Z --== FxT := Int to FP, FrX := FxT/ FLk, FrY := FYT/ FLk,FrZ := FzT/ FLk, --= FPx := frqFP *FrX, Px := Fpto Int, dtto Y,Z, frqFP = Fp(Frequenzperiode) --------------------------------- --== ATFP = atanFP(FYT/FXT) --== ATgrPI2 := if AtFP > Pi/2 (ccmpAT) , cmp AT mit PI/2: if > then ATgrPI2:= 1, grPI2 <= '1'; --== AZFP:= atanFP(FlT/FZt) ----------------------------------------------------------------- iniITPxyz: PROCESS(clk) VARIABLE cntrclk : INTEGER RANGE 0 TO 511:= 0; BEGIN IF rising_edge(clk) THEN IF nrst = '0' THEN stateIni <= 0; cntrclk := 0; YXTneg <= '0'; initITPok <='0';--reset ELSE CASE stateini IS WHEN 0 => IF ceXYZF = '1' THEN --start initITP, Start FP von XT,YT,ZT cntrclk := clkFPtoInt; stateIni <= 1; ELSE stateIni <= 0; END IF; initITPok <='0';--reset WHEN 1 => stateIni <= 2; WHEN 2 =>-- Berechnen FXt..FZt IF cntrclk = 0 THEN cesqXYZ <= '1';-- start für sq(XT,YT,ZT) cntrclk := clktoSq; stateIni <= 3; ELSE cntrclk := cntrclk - 1; stateIni <= 2; END IF; WHEN 3 =>-- Berechnen sq (X,Y,Z) IF cntrclk = 0 THEN cesqXYZ <= '0';-- reset ceXYZF ceaddXY <= '1';-- start für add(X,Y) und XTLTdiv:=FXT/FLT cntrclk := clktoAdd; stateIni <= 4; ELSE cntrclk := cntrclk - 1; stateIni <= 3; END IF; WHEN 4 =>-- Berechnen addFP XY IF cntrclk = 0 THEN ceaddXY <= '0';-- reset ce addXY ceaddXYZ <= '1';-- start für add(X,Y,Z) cntrclk := clktoadd; stateIni <= 5; ELSE cntrclk := cntrclk - 1; stateIni <= 4; END IF; WHEN 5 =>-- Berechnen addFP XYZ IF cntrclk = 0 THEN ceaddXY <= '0';-- reset ce addXY cesqrtFLk <= '1';-- start für sqrtFLk FXtabs <= FXt AND X"7FFFFFFF"; FYtabs <= FYt AND X"7FFFFFFF"; FZtabs <= FZt AND X"7FFFFFFF"; cntrclk := clktosqrt; stateIni <= 6; ELSE cntrclk := cntrclk - 1; stateIni <= 5; END IF; WHEN 6 =>-- Berechnen sqrt(addXYZ)=>FLk, sqrt(addXY)=>FLT IF cntrclk = 0 THEN cesqrtFLk <= '0';-- reset ce sqrtFL cntrclk :=clkFptoInt; ceFlkInt <= '1'; --start FP to INt für Flk To Lk stateIni <= 7; ELSE cntrclk := cntrclk - 1; stateIni <= 6; END IF; WHEN 7 => ---FP to INt für Flk To Lk ceFlkInt <= '0'; -- reset ce IF cntrclk = 0 THEN stateIni <= 9; ELSE cntrclk := cntrClk - 1; stateIni <= 7; END IF; --------------------------------------------- WHEN 9 => -- Berechnen FP to Int = PX.. IF cntrclk = 0 THEN cntrclk := 36; -- 22 clk für atan,32 clk für acos cetgAT <= '1';-- Start für ctgYXT:ATFP:= acos(XT/LT) stateIni <= 10; ELSE cntrclk := cntrclk - 1; stateIni <= 9; END IF; WHEN 10 => --Berechnen ATFP:=acos(FXT/FLT) und AZFP:= atan(FZT/FLT) IF cntrclk = 0 THEN cetgAT <= '0';-- reset cecmpAT <= '1';-- startcmp AT mit PI/2 stateIni <= 11;-- und ende initITPlin: ATFP und AZFP fertig ELSE cntrclk := cntrclk - 1; stateIni <= 10; END IF; WHEN 11 => --ausführen cmp AT mit PI/2: ATgrPI2 fertig cecmpAT <= '0';-- reset stateIni <= 12; WHEN 12 => -- IF ATgrPI2 = "1" THEN -- ATgrPI ist vektor mit Bit 0 grPI2 <= '1'; ELSE grPI2 <= '0'; END IF; stateIni <= 13; WHEN 13 => -- initITPok <= '1'; -- init ITP fertig stateIni <= 0;-- und Zurück an Anfang WHEN OTHERS => stateIni <= 0; END CASE; END IF;-- nrst if/else END IF; --clk END PROCESS;--end iniITPxyz --======Start von iniITPxyz und starten Impulsausgabe========== --Virtuelle -Interpolation von XYZ Starten und steuern --starten bresenham interpolation xyz & Lk --Generieren ITPxyz für Status Meldung runITP: PROCESS(clk) VARIABLE cntrclk :INTEGER RANGE 0 TO 30000000:= 0;-- für 30ms max BEGIN IF rising_edge(clk) THEN IF nrst = '0' THEN stateITP <= 0; cntrclk :=0; ITPxyz <= "000"; ELSE CASE stateITP IS WHEN 0 => IF ce = '1' THEN ITPxyz <= "111"; stateITP <= 2; ELSE stateITP <= 0;-- Warten auf Start END IF; WHEN 2 => IF XT(31) = '1' THEN -- if XT negativ XTabs <= (XT XOR X"FFFFFFFF") + 1;-- XT negieren fXneg <= '1';--neg Flag setzen ELSE XTabs <= XT; fXneg <= '0';--neg Flag,Rücksetzen END IF; -------- IF YT(31) = '1' THEN -- if YT negativ YTabs <= (YT XOR X"FFFFFFFF") + 1;-- YT negieren fYneg <= '1';--neg Flag setzen ELSE YTabs <= YT; fYneg <= '0';--neg Flag,Rücksetzen END IF; -------- IF ZT(31) = '1' THEN -- if ZT negativ ZTabs <= (ZT XOR X"FFFFFFFF") + 1;-- ZT negieren fZneg <= '1';--neg Flag setzen ELSE ZTabs <= ZT; fZneg <= '0';--neg Flag,Rücksetzen END IF; ceXYZF <= '1';--Start für InitITPxyz, Start des iniITPlin Processes stateITP <= 3; WHEN 3 => -- wenn iniITPxyz fertig berchnet IF initITPok = '1' THEN cntrclk := 2000000;-- 20 ms warten wg rd Status stateITP <= 4;---4; --dann weiter ELSE ceXYZF <= '0';--reset start für InitITP stateITP <= 3;-- warten auf init ITP END IF; ------------------------------------ WHEN 4 =>-- warten 20 ms für status rd IF cntrclk > 0 THEN cntrclk := cntrclk -1; stateITP <= 4;-- dann warten ELSE ITPxyz <= "000"; cntrclk := 170;-- 2.0 us min Impulslänge für start Impuls stateITP <= 5; END IF; WHEN 5 => --- warten auf Start Impuls länge min 2,4us IF cntrclk > 0 THEN cntrclk := cntrclk -1; IF strimp = '1' THEN stateITP <= 5;-- warten ELSE stateITP <= 6;-- impuls ist kürzer, neu anfangen END IF; ELSE -- zeit abgelaufen stateITP <= 7;-- impuls hat richtige länge END IF; WHEN 6 => -- neu anfangen mit Impuls abwarten cntrclk := 170;-- 2 us min Impulslänge stateITP <= 5; --------------------------------------- WHEN 7 => runXYZ <= '1'; --Impuls ausgabe(bresenham) starten ITPxyz <= "111"; stateITP <= 8; WHEN 8 => -- warten bis InterpolationXYZ fertig IF stopp = '0' THEN --wenn keine Endlage dann weiter IF XYZok = '1' THEN -- ITP (bresenham) ist fertig runXYZ <= '0';-- reset stateITP <= 0;-- itp fertig ITPxyz <= "000"; ELSE -- stateITP <= 8;-- warten bis koordinaten fertig END IF; ELSE ----wenn Endlage dann sofort itp anhalten stateITP <= 9; END IF; WHEN 9 => -- sofort stopp ITPxyz <= "000"; runXYZ <= '0';-- impuls Ausgabe stoppen stateITP <= 0;--itp fertig WHEN OTHERS => stateITP <= 0; END CASE; END IF;-- nrst if/else END IF; --clk END PROCESS;--runITP --========================================================================== --=================XYZ Bresenham =Interpolation Am Roboterkopf ========== --erzeugt virtuelle Impulse für ImpX,ImpY,ImpZ sowie ImpLK(entlang leitKoordinate) am Roboter Kopf --und aktuelle virt.Position XA,YA,ZA (integer), --Ende der Interpolation mit XYZok =1 Impuls(2clk) --Vorschubspeed definiert mit pfrq (Anz.clk(10nS) zwischen zwei ImpLK -- impLK startet RunAngle d.h. die Invers transf Berchnung --------------------------------------------------------------------------------------------------- bresham: PROCESS(clk) VARIABLE count :INTEGER; VARIABLE errX :INTEGER; VARIABLE errY :INTEGER; VARIABLE errZ :INTEGER; VARIABLE startON :std_logic:= '0'; BEGIN IF rising_edge(clk) THEN IF nrst = '0' THEN statebrsh <= 0; dLk <= 0; dX <= 0; dY <= 0; dZ <= 0; errX :=0; XAout <= (OTHERS =>'0'); YAout <= (OTHERS =>'0'); ZAout <= (OTHERS =>'0'); errY :=0; errZ :=0; XA <= (OTHERS =>'0'); YA <= (OTHERS =>'0'); ZA <= (OTHERS =>'0'); ImpX <= '1'; ImpY <= '1'; ImpZ <= '1'; ImpLk <= '1'; count := 0; XYZok <= '0'; startON := '0'; ELSE CASE statebrsh IS WHEN 0 => IF runXYZ = '1' THEN dX <= conv_integer(XTabs);-- Xkoord dY <= conv_integer(YTabs); dZ <= conv_integer(ZTabs); dLk <= conv_integer(Lk);--Leit koord statebrsh <= 1; ELSE --anfang der berechnung nach strimp IF strimp = '1' AND startON = '0' THEN XA <= XS;-- Startpunkt setzen YA <= YS; ZA <= ZS; startON := '1'; END IF; errX :=dLk/2; errY :=dLk/2; errZ :=dLk/2; Lka <= 0; XYZok <= '0'; statebrsh <= 0; END IF; WHEN 1 => IF count = 0 THEN -- warten für pfrq periode IF Lka < Lk THEN Lka <= Lka + 1; ImpLK <= '0'; ------------------------ errX := errX - dX; IF errX < 0 THEN IF fXneg = '0' THEN XA <= XA + 1; ELSE XA <= XA - 1; END IF; ImpX <= '0'; errX := errX + dLk; END IF; ---------------------- errY := errY - dY; IF errY < 0 THEN IF fYneg = '0' THEN YA <= YA + 1; ELSE YA <= YA - 1; END IF; ImpY <= '0'; errY := errY + dLk; END IF; ------------------------- errZ := errZ - dZ; IF errZ < 0 THEN IF fZneg = '0' THEN ZA <= ZA + 1; ELSE ZA <= ZA - 1; END IF; ImpZ <= '0'; errZ := errZ + dLk; END IF; ------------------------- count := conv_integer(pfrq); statebrsh <= 2; ELSE statebrsh <= 3;--dann itp fertig END IF; ELSE count := count - 1; statebrsh <= 1; END IF; WHEN 2 => ImpLk <= '1'; ImpX <= '1'; ImpY <= '1'; ImpZ <= '1'; XAout <= XA; YAout <= YA; ZAout <= ZA; IF stopp = '1' THEN --sofort anhalzen, endlage statebrsh <= 5; ELSE --kein stop statebrsh <= 1; END IF; WHEN 3 => XYZok <='1'; statebrsh <= 0;-- fertig ohne stop WHEN 5 => dLk <= 0; dX <= 0; dY <= 0; dZ <= 0; errX :=0; errY :=0; errZ :=0; XYZok <='1';--fertig mit stop statebrsh <= 0;-- fertig ohne stop WHEN others => statebrsh <= 0; END CASE; END IF;--IF nrst/ELSE END IF; -- if clk END PROCESS;-- bresenham für XYZ Koordinaten --=====================Berechnung Invers Transformation==== --============RUNANGLE :Berechnung der Winkel in den Roboterachsen ========== --XT,YT,ZT sind strecken koordinaten die im XYZ Satz oder Kreissatz angegeben werden -- bezogen auf Fräsenendpunkt (roboter KopfF) --XS,YS,ZS sind startposition Koordinaten von wo XT--ZT gestartet werden -- Xa,Ya,Za sind Aktuelle Koordinaten am Fräserendpunkt (roboterkopf) --L1,L2,L3 sind roboterarm längen in mm -- Bezeichnung FP = Wert ist Floatingpoint Format, Bsp: ATFP Winkel AT in FP format ---------------------------------------------------------------------------- -- Folgende Werte werden hier Berechnet : -- LT :Streckenlänge in XY Ebene:LT :=sqrt((xT^2 + YT^2) -- LK :streckenlänge in XYZ:Lk :=sqrt((xT^2 + YT^2 + ZT^2); -- AT = arccos(XT / LT) -- AZ = arctan(ZT/LT) -- Roboter Achsen in Ebene W : P0,P1,P2. EbeneW dreht sich mit Winkel A0 -- aus Xa,Ya,Za werden FXw,FYw,FZw : positionen in roboterebene W(winkel A1,A2,A3) -- Aus Xa,Ya wird A0 Achse berechnet(A0FP in FP format, in radiant):A0Fp := atanFP( Fxa/FYa); -- aus Xw,Yw werden Lw und Aw, Distanz und winkel in Roboterebene W -- daraus die Winkelposition A3,A4 ---------------------------------------------------------- ---------- Ebene X / Y berechnungen------------------- -- LwFP := sqrt(FXw^2 + FYw^2); - länge zwischen P0-P2 in Ebene XY -- Winkel der Ebene W(P0,P2) =(AW), Winkel A0 ist Winkel zwischen P0..P3 -- A12FP := aTan(LzFP / LwFP), A12 = Winkel gerade zwischen P0 und P2 in ebene W ------------------------------------------------------------------------ -- ****** Ende der Berechnungen wird mit Impuls FXYZaOK signalisiert ******* --- siehe stateangle = 20 ---------------------------------------------------------------------------- runangle:PROCESS(clk) VARIABLE ctrclk :INTEGER RANGE 0 TO 127:= 0; VARIABLE startON :std_logic:= '0'; BEGIN IF rising_edge(clk) THEN IF nrst = '0' THEN stateangle <= 0; ctrclk := 0; FXYZaOK <= '0'; ceXYZFP <= '0'; cetgFP<='0'; cemul9<='0'; cemul10<='0'; cesubW <='0'; cePI2 <='0'; cemAZA3 <= '0';cedAZA3 <= '0';cedivPIdiv<='0'; ceAZmul<= '0'; cesub1min <= '0';ceA3FP<='0'; ceA4FP <='0'; cetgFP <= '0'; ceP1 <= '0'; cesqL <= '0'; ceadd1 <= '0'; cesub1<= '0'; cediv1 <= '0'; ceacosFP <= '0'; ceA1FP <='0'; cecosFP <= '0'; ceZ1W1 <= '0'; cesubA1 <= '0'; startON := '0'; -- Fl1 <= (OTHERS => '0'); Fl2 <= (OTHERS => '0');Fl12 <= (OTHERS => '0'); -- mul12 <= (OTHERS => '0'); ELSE CASE stateangle IS WHEN 0 => --IF (ImpLk = '0' AND strimp='0') OR (strimp = '1' AND StartON = '0') THEN -- start zurst mit ce IF (ImpLk = '0') THEN ceXYZFP <= '1';--Start für Xa,Ya,Zz=>FP, und Pi/2-AZFP,Pi/2 +/- ATFP ctrclk := clkToFP; stateangle <= 1; ELSE FXYZaOK <= '0'; --reset stateangle <= 0;-- warten auf LK Impuls END IF; WHEN 1 => -- ausführen Xa,Ya,Zz=>FP und Pi/2-AZFP,Pi/2 +/- ATFP IF ctrclk > 0 THEN ctrclk := ctrclk -1; stateangle <= 1; ELSE StartON := '1'; ceXYZFP <= '0';--reset CE stateangle <= 2; END IF; WHEN 2 => -- zwischen state notwendig**** IF grPI2 = '1' THEN -- wenn AT > PI/2 dann sub PI/2 sonst ADD PIAT <= suPIAT; ELSE PIAT <= adPIAT; END IF; stateangle <= 3; WHEN 3 => -- zwischen state notwendig **** cePI2 <= '1'; ctrclk := clkTosub; stateangle <= 4; WHEN 4 => --ausführen PIAZ :=PI/2 - PIAT IF ctrclk > 0 THEN ctrclk := ctrclk -1; stateangle <= 4; ELSE cePI2 <= '0';-- reset ctrclk := 35;--clk Atan für durchlauf ****** ceTgFP <= '1'; --arcTang und cos starten stateangle <= 5; END IF; WHEN 5 => --Ausführen A0FP:=atan(Ya/Xa),und Cos(4 bml), A=FP fertig***** IF ctrclk > 0 THEN ctrclk := ctrclk -1; stateangle <= 5; ELSE --tanYX , ATFP,AZFP fertig cemul9 <= '1';-- start sinAZ* sinAT, sinAZ* cosAT ctrclk := clkTomul; ceTgFP <= '0'; --Reset ArcTang start Impuls stateangle <= 9; END IF; WHEN 9 => -- ausführen; sinAZ* sinAT, sinAZ* cosAT IF ctrclk > 0 THEN ctrclk := ctrclk -1; stateangle <= 9; ELSE cemul9 <= '0';--reset ctrclk := clkTomul; cemul10 <= '1'; --start sAZAT* FL3, cAZAT* FL3 stateangle <= 10; END IF; WHEN 10 => --ausführen sAZAT* FL3, cAZAT* FL3 IF ctrclk > 0 THEN ctrclk := ctrclk -1; stateangle <= 10; ELSE cemul10 <= '0';-- reset ctrclk := clkTosub; ceSubw <= '1'; -- Start für sub/add FXa -/+ sAZAT3 usw stateangle <= 11; END IF; WHEN 11 => -- ausführen sub/add FXa -/+ sAZAT3.. = FYw,FZw fertig IF ctrclk > 0 THEN ctrclk := ctrclk -1; stateangle <= 11; ELSE ceSubw <= '0';-- reset stateangle <= 12; END IF; WHEN 12 =>-- für test stateangle <= 13; WHEN 13 => -- zwischen state notwendig *** IF grPI2 = '1' THEN FXw <= aFXw; ELSE FXw <= sFXw; END IF; ctrclk := clkTosq; cemul12 <= '1'; --Start für sqXw:=sq(FXw),sqYw:=sq(FYw) und ATPI:= ATFP-PI/2 stateangle <= 15; ---FXW fertig Berechnet ----------------state reserve---------------------------- WHEN 15 => --- Ausführen Fxw^2,FYw^2 IF ctrclk > 0 THEN ctrclk := ctrclk -1; stateangle <= 15; ELSE cemul12 <= '0';-- reset stateangle <= 16; END IF; WHEN 16 => ceXYZw <= '1';---für test ***** XYZ W *********************** stateangle <= 17; -----Blatt6:csqXw,csqYw, Blatt5: csubAwAt-- gestartet mit cemul12 ---------------- WHEN 17 => -- ausführen sqXw:=sq(FXw),sqYw:=sq(FYw) ctrclk := 23;-- 22 clk für atan ceXYZw <= '0'; -- reset ceaddXYw <= '1'; --start fürAwFP:=atan(FYw/FXw) und sqXYW:=sqXw+SqYw,subPIDIV0subAtAW-pIDIV0 stateangle <= 18; -----Blatt6: catgAw (22clk),caddXYw, Blatt5:csubPidiv--- gestartet mit ceaddXYw--------- WHEN 18 =>-- ausführen AwFP:=atan(FYw/FXw) und sqXYW:=sqXw+ SqYw,subPidiv=subAtAw-PIdiv2 IF ctrclk > 0 THEN ctrclk := ctrclk -1; stateangle <= 18; ELSE cesqrtLw <= '1'; --start LwFP:=sqrt(sqXYw) und subPIdiv= subATAW-pidIV2 ctrclk := clkTosq; ceaddXYw <= '0'; --reset stateangle <= 19; END IF; ------Blatt6:csqrtLW, Blatt5 :cdivPIdiv2-- gestartet mit cesqrtLW --------- WHEN 19 => --ausführen LwFP:= sqrt(sqXYw),LWFP fertig, subPidiv=subAtAw-PIdiv2 IF ctrclk > 0 THEN ctrclk := ctrclk - 1; stateangle <= 19; ELSE cesqrtLw <= '0';-- reset cemAzA3 <= '1'; --start AtFp-AwFp absDivPIdiv <= DivPIdiv;--absDivPiDiv vorläaufig setzen ctrclk := clkTosub; stateangle <= 22; END IF; ----reserve state ---------------- ----- Blatt5:csqZ2,csqW2,cmuAZFP ---- gestartet mit cemAzA3 ---------------- WHEN 22 => --- ausführen sq(FZw),sq(LWFP), mul(AZFP,DivPIdiv)------ IF ctrclk > 0 THEN ctrclk := ctrclk - 1; stateangle <= 22; ELSE cemAZA3 <= '0'; --reset cedAZA3 <= '1';--start subAtAw-PIdiv2 ctrclk := clkTosub; stateangle <= 23; END IF; ---- Blatt 5:caddWZ, csub1min--- gestartet mit cedAZA3 --------------- WHEN 23 => --- ausführen sqaddWZ=sqZ2 + sqW2,subPIdiv = IF ctrclk > 0 THEN ctrclk := ctrclk - 1; stateangle <= 23; ELSE cedAZA3 <= '0';-- reset cedivPIdiv <= '1'; ceA4FP <= '1'; -- start sqrt(sqaddWZ), atan A12, ctrclk := clkToAtan;--clk Atan stateangle <= 24; END IF; --- Blatt5:catanA12,sqrtWZ,cmulAZFP -- gestartet mit ceA4FP ------- WHEN 24 => --ausführen atan(FZW,LWFP),divFp,vsqrt(sqaddWZ), mul(sub1min,AZFP) IF ctrclk > 0 THEN ctrclk := ctrclk - 1; stateangle <= 24; ELSE absdivPIdiv <= divPIdiv;-- vorläufig ceAZmul <= '1'; ceP1 <= '1'; -- START intToFP für L1,L2 => FL1,FL2 ceA4FP <= '0'; --reset cedivPIdiv <= '0'; stateangle <= 25;-- END IF; WHEN 25 => IF absDivPidiv(31) = '1' THEN absDivPidiv(31) <= '0';-- Abs von DivPiDiv END IF; ctrclk := clkTosub; cesub1min <= '1'; stateangle <= 26; WHEN 26 =>-- sub1min := 1 - absdivPIdiv IF ctrclk > 0 THEN ctrclk := ctrclk - 1; stateangle <= 26; ELSE ceP1 <= '0';-- FL1,FL2 fertig cesub1min <= '0'; ceAZmul <= '0';-- a4FP fertig ceA3FP <= '1';-- startsub1min* AZFP => A3FP cesqL <= '1';-- start für FL11^2,Fl2^2,FL12^2 ctrclk := clkTomul; stateangle <= 27; END IF; -- Ab hier Funktionen aus calcP1--------------------------------------- WHEN 27 => -- A3FP :=sub1min * AZFP, a3FP fertig,FL11^2,Fl2^2,FL12^2 IF ctrclk > 0 THEN ctrclk := ctrclk - 1; stateangle <= 27; ELSE IF strImp = '0' THEN StartON := '0'; END IF; cesqL <= '0';--FL11^2,Fl2^2,FL12^2=> SqL1,sqL12,sqL2 fertig ceA3FP <= '0';--a3FP fertig ceadd1<= '1'; -- startsqL1+sqL12,Fl12*FL1 ctrclk := clkTomul; stateangle <= 28; END IF; WHEN 28 => -- ausführen L1+sqL12,Fl12*FL1 IF ctrclk > 0 THEN ctrclk := ctrclk - 1; stateangle <= 28; ELSE ceadd1<= '0';--L1+sqL12,Fl12*FL1 fertig cesub1 <= '1'; -- start für: sqadd1-sqL2, mul12 * 2 ctrclk := clkTomul; stateangle <= 29; END IF; WHEN 29 => -- ausführen: sqadd1-sqL2, mul12 * 2 IF ctrclk > 0 THEN ctrclk := ctrclk - 1; stateangle <= 29; ELSE cesub1 <= '0';--sqadd1-sqL2, mul12 * 2 fertig cediv1 <= '1'; -- start sqsub1/ mulres1 => ax ctrclk := clkToDiv; stateangle <= 30; END IF; WHEN 30 => IF ctrclk > 0 THEN ctrclk := ctrclk - 1; stateangle <= 30; ELSE cediv1 <= '0';--sqsub1/ mulres1 => ax fertig ceacosFP <= '1'; -- start für atang( Ax) ctrclk := clkTOacos; stateangle <= 31; END IF; WHEN 31 => IF ctrclk > 0 THEN ctrclk := ctrclk - 1; stateangle <= 31; ELSE ceacosFP <= '0';--atang( Ax) fertig ceA1FP <= '1'; -- start für acosAx + A12FP ctrclk := clkTOadd; stateangle <= 32; END IF; WHEN 32 => IF ctrclk > 0 THEN ctrclk := ctrclk - 1; stateangle <= 32; ELSE ceA1FP <= '0';--acosAx + A12FP => A+FP fertig ************* cesubA1 <= '1'; --start für PI/2 - A1FP ctrclk := clkTOsub; stateangle <= 33; END IF; WHEN 33 => IF ctrclk > 0 THEN ctrclk := ctrclk - 1; stateangle <= 33; ELSE cesubA1 <= '0';--PI/2 - A1FP => A1SFP fertig cecosFP <= '1'; --Start für cos(a1FP) & cosFP(a1SFP) ctrclk := clkTOcos; stateangle <= 34; END IF; WHEN 34 => IF ctrclk > 0 THEN ctrclk := ctrclk - 1; stateangle <= 34; ELSE cecosFP <= '0';--cos(a1FP) & cosFP(a1SFP) =>cosA1FP,sinA1FP fertig ceZ1W1 <= '1'; --start für sinA1FP*FL1, cosA1FP * FL1 ctrclk := clkTOmul; stateangle <= 35; END IF; WHEN 35 => IF ctrclk > 0 THEN ctrclk := ctrclk - 1; stateangle <= 35; ELSE ceZ1W1 <= '0';--sinA1FP*FL1=> FZ1,cosA1FP * FL1=>FW1 fertig ****** cezw12 <= '1'; -- start für FZw-Fz1 & LwFP-FW1 ctrclk := clkTOsub; stateangle <= 36; END IF; WHEN 36 => IF ctrclk > 0 THEN ctrclk := ctrclk - 1; stateangle <= 36; ELSE cezw12 <= '0';--FZw-Fz1=Z2Z1 & LwFP-FW1=W"W1 fertig ceatanZW <= '1';-- start für atan ctrclk := clkTOatan; stateangle <= 37; END IF; WHEN 37 => IF ctrclk > 0 THEN ctrclk := ctrclk - 1; stateangle <= 37; ELSE ceatanZW <= '0';-- AtanZW fertig cesubA2 <= '1'; -- start für atanZw- a1FP ctrclk := clkTOsub; stateangle <= 38; END IF; WHEN 38 => IF ctrclk > 0 THEN ctrclk := ctrclk - 1; stateangle <= 38; ELSE cesubA2 <= '0';-- A2FP fertig ******************** FXYZaOK <= '1';--RUNANGLE bereit stateangle <= 0;-- alle Achsen fertig --Berechnung fertg,wartn auf nächste Start END IF; WHEN OTHERS => stateangle <= 0; END CASE; END IF;-- nrst if/else END IF; --clk END PROCESS;-- end runangle --=======Ende der InversTransformation(IT) Berechnungen, Alle Achsen Winkel in FP Format===== -- *******Ende der ITP wird signalisiert mit Impuls fXYZaOK************************************* --========================================================================================= --========================================================================================== -- ITPImpulsGen: -- Aktuelle Winkel der Achsen A0FP..A5FP(rad) umrechnen in Anzahl -- soll-Inkremente:output AXconv:AchsInkr(31 bit),siehe modul AXconv, alle achsen Paralell -- gespeichert in --AXinc. --------------------------------------------------------------------------- --TYPE T_Axr IS ARRAY(5 DOWNTO 0) OF std_logic_vector(15 DOWNTO 0); --SIGNAL AXGrd :T_Axr; aktuelle Winkel der Achsen A0..A5 in Grad.x , -- *********zum vergleich mit Sketch Simulation ************** ---------------------------------------------------------------------------- ----SIGNAL AXinc :T_AXInc;--IS ARRAY(5 DOWNTO 0) OF SLV(31 DOWNTO 0); -- Aktuelle Winkel der Achsen A0FP..A5FP(rad) umrechnen in Anzahl -- soll-Inkremente:output AXconv:AchsInkr(31 bit),siehe modul AXconv, alle achsen Paralell -- gespeichert in --AXinc :T_AXinc; ------------------------------------------ --SIGNAL AXalt :T_AXInc;--IS ARRAY(5 DOWNTO 0) OF SLV(31 DOWNTO 0); --speichert vorherige wert von AXinc, wird benötig um Differenz zwischen -- den einzelne Sollerte aus AXinc zum --SIGNAL vAXdif :T_AXInc;--IS ARRAY(5 DOWNTO 0) OF SLV(31 DOWNTO 0); -------------------------------------------------------------------------------- --SIGNAL AXdif :T_AXdif;--IS ARRAY(5 DOWNTO 0) OF SLV(15 DOWNTO 0); -- AXdif entspricht vAxdif aber beschränkt auf 16 bit, wird benötigt in -- procedur ImpSwitch --============================================================================ ITPImpGen: PROCESS(clk) VARIABLE cntrclk :Integer range 0 to 15:= 0; VARIABLE strt :STD_LOGIC := '1'; BEGIN IF rising_edge(clk) THEN IF nrst = '0' THEN statITPimp <= 0; cntrclk := 0; XYZcalcok <= '0'; FOR i IN 0 TO 5 LOOP AXalt(i) <= (OTHERS => '0'); AXdif(i) <= (OTHERS => '0');-- vAXdif(i) <= (OTHERS => '0'); END LOOP; strt := '0'; ELSE CASE statITPimp IS WHEN 0 => -- warten auf start IF fXYZaOK = '1' THEN ---wenn Berechnung fertig dann Pulse berchnen ceAxx <= '1';--- Start Convert für Alle Achsen A0.. A4 cntrclk := 6;---anz clk für convert statITPimp <= 1; ELSE IF sitprun = '0' AND strt = '0' THEN strt := '1';--vorbereiten flag für AXalt einlesen END IF; XYZcalcok <= '0'; statITPimp <= 0; END IF; WHEN 1 => --ausführung Convert (5 clk) ceAxx <= '0'; IF cntrclk = 0 THEN statITPimp <= 2; ELSE cntrclk := cntrclk -1; statITPimp <= 1;-- warten bis Convert fertig END IF; WHEN 2 =>-- IF strt= '1' THEN --strt dient der start speicherung von 1. Wert von Axinc in Axalt FOR i IN 0 TO 5 LOOP AXalt(i)<= AXinc(i); END LOOP; END IF; statITPimp <= 3;-- warten auf nächste WHEN 3 => strt := '0';-- reset strt, erste wert von AXinc ist gespeichert in Axalt ----------------------------------------------------- FOR i IN 0 TO 5 LOOP vAXdif(i) <= AXinc(i) - AXalt(i);-- Differenz errechnen inn 32 bit AXdif(i)<= vAXdif(i)(15 DOWNTO 0);--beschränken auf 16 bit, wird im ImpSwitch END LOOP; ------------------------ statITPimp <= 4; WHEN 4 => FOR i IN 0 TO 5 LOOP -- speichern der vorherige AXinc wert AXalt(i) <= AXinc(i); END LOOP; XYZcalcok <= '1';-- umrechnung fertig statITPimp <= 0;-- warten auf nächste Lk impuls WHEN OTHERS => statITPimp <= 0; END CASE; END IF; -- if nrst/else END IF; -- clk END PROCESS;--end ITPImpGEn --======================================================= --========================= --TestOutput: PROCESS(sqadd1,mul12,mulres1,sqsub1,ax,acosAX,A12FP,cosA1FP,sinA1FP, -- Fw1,Fz1,subATAW,subPIdiv,cesqL,FL1,FL12) testOutput:PROCESS(clk) BEGIN IF rising_edge(clk) THEN IF nrst = '0' THEN testdata <= (OTHERS => '0'); ELSE -- 16 bit daten für testoutput testdata(0) <= sqadd1(0);-- A0 testdata(1) <= mul12(0);-- testdata(2) <= mulres1(0);-- testdata(3) <= sqsub1(0);-- testdata(4) <= ax(0);-- testdata(5) <= acosAX(0);-- testdata(6) <= A12FP(0);-- testdata(7) <= cosA1FP(0);-- testdata(8) <= sinA1FP(0);-- testdata(9) <= Fw1(0);-- testdata(10) <= FZ1(0);-- testdata(11) <= subATAW(0);-- testdata(12) <= subPIdiv(0);-- testdata(13) <= cesqL;-- testdata(14) <= FL1(0);-- testdata(15) <= FL12(0);-- END IF; END IF; END PROCESS; --========================================================================== --======================================================== --============================================================== ---************************************************************** --------------IntToFP für ITP FrqPERIOD, Start mit frqpos------ ---cfrqFP: IntToFP port map(a=>frqPER,clk=>clk,ce=>cefrqFp,result=>frqFP); --==================================================================== -------------------- initITP ------------------------------------------ ------XYZT => FPx..FPz, FPLk-----Initialisierung beim neuen Satz-------- cXTFP: IntToFP port map(a=>XT,clk=>clk,ce=>ceXYZF,result=>FXt); cYTFP: IntToFP port map(a=>YT,clk=>clk,ce=>ceXYZF,result=>FYt); cZTFP: IntToFP port map(a=>ZT,clk=>clk,ce=>ceXYZF,result=>FZt); -------------sqFxt := FXt^2 state 3------------------------------------------- cmulsqX: mulFP port map(a=>FXt,b=>FXt,clk=>clk,ce=>cesqXYZ,result=>sqFxt); cmulsqY: mulFP port map(a=>FYt,b=>FYt,clk=>clk,ce=>cesqXYZ,result=>sqFyt); cmulsqZ: mulFP port map(a=>FZt,b=>FZt,clk=>clk,ce=>cesqXYZ,result=>sqFzt); --------------addXY := sqFxt + sqFYt state 4------------------------------------------ caddXY: addFP port map(a=>sqFXt,b=>sqFYt,clk=>clk,ce=>ceaddXY,result=>addXY); --------------addXYZ := addXY + sqFZt ------------------------------------------ caddXYZ: addFP port map(a=>addXY,b=>sqFZt,clk=>clk,ce=>ceaddXYZ,result=>addXYZ); ----------------FLk := sqrt(addXYZ),FLT:=sqrt(addXY)----state6-------------------- csqrtFLT: sqrtFP port map(a=>addXY,clk=>clk,ce=>cesqrtFLk,result=>FLT); csqrtFLk: sqrtFP port map(a=>addXYZ,clk=>clk,ce=>cesqrtFLk,result=>FLk); ------------------------------------------------------------------------------- cFPToInt: FPtoInt32 port map(a=>FLk,clk=>clk,ce=>ceFlkInt,result=>Lk);-- Flk in Integer32 slv ------------------------------------------------------------------------------ cXTLTdiv: divFP port map(a=>FXT,b=>FLT,clk=>clk,ce=>ceFlkInt,result=>XTLTdiv); ------------------AT FP Berechnen-State 10---------------------------- cacosAT: ArcCosFP port map(nrst=>nrst,clk=>clk,ceacos=>cetgAT,aInp=>XTLTdiv,acos=>ATFP);--ATFP :=acos(XT/LT) ----------------- AZ FP Berechnen --------------------------------------- ccmpAT: cmpgr port map(a=>ATFP,b=>PIdiv2,clk=>clk,ce=>cecmpAT,result=>ATgrPI2);-- wird in RUNANGLE Gebraucht catgAZT: atanFP port map(nrst=>nrst,clk=>clk,ceatan=>cetgAT,Xinp=>FLT,Yinp=>FZT,atanFP=>AZFP); --------------------- ende initITPlin --------------------------------------------- --******************************************************************************* --==================================================================================== ---------------RUNANGLE ----------------------------------- -----------Xa,Ya,,Za => FP----------------------------------state 0----------- cXaFP : IntToFP port map(a=>Xa,clk=>clk,ce=>cexyzFP,result=>FXa); cYaFP : IntToFP port map(a=>Ya,clk=>clk,ce=>cexyzFP,result=>FYa); cZaFP : IntToFP port map(a=>Za,clk=>clk,ce=>cexyzFP,result=>FZa); cL3FP: Int16ToFP port map(a=>L3,clk=>clk,ce=>cexyzFP,result=>FL3); ------------PIAZ:=Pi/2-AZFP, PIAT:=Pi/2-ATFP---------------state 0 -------- csubPIAZ: subFP port map(a=>PIdiv2,b=>AZFP,clk=>clk,ce=>ceXYZFP,result=>PIAZ); csubPIAT: subFP port map(a=>ATFP,b=>PIdiv2,clk=>clk,ce=>ceXYZFP,result=>suPIAT); caddPIAT: addFP port map(a=>PIdiv2,b=>ATFP,clk=>clk,ce=>ceXYZFP,result=>adPIAT); ----------If ATFP > pI/2 then PIAT := suPIAT else PIAT:= adPiAT --- Starte 18, 19 ---- siehe InitITP xyz state 11----------------------------------- ---------sub: Pi/2 -PIAT ------------------------------------ State 2------- csubSAT: subFP port map(a=>PIdiv2,b=>PIAT,clk=>clk,ce=>cePI2,result=>sPIAT); ------A0FP:= atan(FYa/FXa) and cosFP: PIAZ,AZFP,PIAT,SPIAT -----state 3----- ctanYX: atanFP port map(nrst=>nrst,clk=>clk,ceatan=>cetgFP,Xinp=>FXa,Yinp=>FYa,atanFP=>A0FP); csinAZ: cosFP port map(nrst=>nrst,clk=>clk,cecos=>cetgFP,ainp=>PIAZ,cos=>sinAZ); ccosAZ: cosFP port map(nrst=>nrst,clk=>clk,cecos=>cetgFP,ainp=>AZFP,cos=>cosAZ); ccosAT: cosFP port map(nrst=>nrst,clk=>clk,cecos=>cetgFP,ainp=>PIAT,cos=>cosAT); csinAT: cosFP port map(nrst=>nrst,clk=>clk,cecos=>cetgFP,ainp=>sPIAT,cos=>sinAT); --------------mul: sinAZ * sinAT, sinAZ * cosAT----------------- state9 ------- cmul90: mulFP port map(a=>sinAZ,b=>sinAT,clk=>clk,ce=>cemul9,result=>sAZAT); cmul91: mulFP port map(a=>sinAZ,b=>cosAT,clk=>clk,ce=>cemul9,result=>cAZAT); --------------mul: sAZAT * L3, cAZAT * L3 ----------------------state10 ------ csAZAT: mulFP port map(a=>sAZAT,b=>FL3,clk=>clk,ce=>cemul10,result=>sAZAT3); ccAZAT: mulFP port map(a=>cAZAT,b=>FL3,clk=>clk,ce=>cemul10,result=>cAZAT3); ccAZAZ: mulFP port map(a=>cosAZ,b=>FL3,clk=>clk,ce=>cemul10,result=>cAZAZ3); ------------sub: FXa-sAZAT3,FYa - cAZAT3, FZa - cosAZ ----State 11 ----------- caddFXw: addFP port map(a=>FXa,b=>sAZAT3,clk=>clk,ce=>cesubW,result=>aFXw); csubFXw: subFP port map(a=>FXa,b=>sAZAT3,clk=>clk,ce=>cesubW,result=>sFXw); ------ csubFYw: subFP port map(a=>FYa,b=>cAZAT3,clk=>clk,ce=>cesubW,result=>FYw); caddFZw: addFP port map(a=>FZa,b=>cAZAZ3,clk=>clk,ce=>cesubW,result=>FZw); ------------sq : FXw, FYw ,sub ATFP-AWFP=subATAW---------------State 12 -------- csqXw: mulFP port map(a=>FXw,b=>FXw,clk=>clk,ce=>cemul12,result=>sqXw); csqYw: mulFP port map(a=>FYw,b=>FYw,clk=>clk,ce=>cemul12,result=>sqYw); ------------ ----------------------------state 13 ----------- caddXYw: addFP port map(a=>sqXw,b=>sqYw,clk=>clk,ce=>ceaddXYw,result=>sqXYw); catgAw: atanFP port map(nrst=>nrst,clk=>clk,ceatan=>ceaddXYw,Xinp=>FXw,Yinp=>FYw,atanFP=>AwFP); ----------------------------------------State 14------------------------- csqrtLw: sqrtFP port map(a=>sqXYw,clk=>clk,ce=>cesqrtLw,result=>LwFP); -----------------------------------------State15------------------- csqZ2: mulFP port map(a=>FZw,b=>FZw,clk=>clk,ce=>cemAZA3,result=>sqZ2); csqW2: mulFP port map(a=>LwFP,b=>LwFP,clk=>clk,ce=>cemAZA3,result=>sqW2); csubAWAT: subFP port map(a=>ATFP,b=>AWFP,clk=>clk,ce=>cemAZA3,result=>subATAW); ---------------------------------------- State 16 ---------------- caddWZ: addFP port map(a=>sqZ2,b=>sqW2,clk=>clk,ce=>cedAZA3,result=>sqaddWZ); csubPIdiv: subFP port map(a=>subATAW,b=>PIdiv2,clk=>clk,ce=>cedAZA3,result=>subPIdiv);--alfa ----------------------------------------- State 17---------------------- catanA12: atanFP port map(nrst=>nrst,clk=>clk,ceatan=>ceA4FP,Xinp=>LwFP,Yinp=>FZw,atanFP=>A12FP); csqrtWZ: sqrtFP port map(a=>sqaddWZ,clk=>clk,ce=>ceA4FP,result=>FL12); cdivPIdiv2: divFP port map(a=>subPIdiv,b=>PIdiv2,clk=>clk,ce=>cedivPIdiv,result=>divPIdiv); ----------------------------- state 18----------------------------------------- cmulAZFP: mulFP port map(a=>AZFP,b=>divPIdiv,clk=>clk,ce=>ceAZmul,result=>A4FP); ----------------------------- state 19----------------------------------------- csub1min: subFP port map(a=>C1,b=>absdivPIdiv,clk=>clk,ce=>cesub1min,result=>sub1min); ----------------------------- state 20----------------------------------------- cmulAZFPs: mulFP port map(a=>sub1min,b=>AZFP,clk=>clk,ce=>ceA3FP,result=>A3FP); ------------------ende run angle ---------------------------------------------------------- --==================TEST ====================================================================== ------------ FXw,FYw,FZw => Xw,Yw,Zw integer ------------state 18----** für test******** cFPwXtoIntw: FPtoInt32 port map(a=>FXw,clk=>clk,ce=>ceXYZw,result=>Xw);-- FXw in Integer32 Xw slv cFPwYToIntw: FPtoInt32 port map(a=>FYw,clk=>clk,ce=>ceXYZw,result=>Yw);-- FXw in Integer32 Xw slv cFPwZToIntw: FPtoInt32 port map(a=>FZw,clk=>clk,ce=>ceXYZw,result=>Zw);-- FXw in Integer32 Xw slv --====================================================================== cL1FP: Int16ToFP port map(a=>L1,clk=>clk,ce=>ceP1,result=>FL1); --L1:16 bit cL2FP: int16ToFP port map(a=>L2,clk=>clk,ce=>ceP1,result=>FL2); -- L2 16 biot -------- FL 12 wurde bei RUNANGLE bereits berechnet ----------------------- ------------------------------------------------------------------------ csqL1: mulFP port map(a=>FL1,b=>FL1,clk=>clk,ce=>cesqL,result=>sqL1);-- quadrat FL1 csqL2: mulFP port map(a=>FL2,b=>FL2,clk=>clk,ce=>cesqL,result=>sqL2);-- Quadrat FL2 csqL12: mulFP port map(a=>FL12,b=>FL12,clk=>clk,ce=>cesqL,result=>sqL12);-- Quadrat Fl12 cmul12: mulFP port map(a=>FL1,b=>FL12,clk=>clk,ce=>ceadd1,result=>mul12);-- FL1 * FL12 => mul12 --------------------- cadd12: addFP port map(a=>sqL1,b=>sqL12,clk=>clk,ce=>ceadd1,result=>sqadd1); csub12: subFP port map(a=>sqadd1,b=>sqL2,clk=>clk,ce=>cesub1,result=>sqsub1); --------------------------- cmulres1: mulFP port map(a=>mul12,b=>C2,clk=>clk,ce=>cesub1,result=>mulres1);-- mul12 * 2 =>mulres1 cdiv: divFP port map(a=>sqsub1,b=>mulres1,clk=>clk,ce=>cediv1,result=>ax); ----------------------ax fertig berechnet----------------------------------- ccosAX: ArcCosFP port map(nrst=>nrst,clk=>clk,ceacos=>ceacosFP,aInp=>AX,acos=>acosAX); caddA1: addFP port map(a=>acosAX,b=>A12FP,clk=>clk,ce=>ceA1FP,result=>A1FP); ----------------------- a1CFP:=Pi/2 - A1FP---------state 8-------------------- csubA1: subFP port map(a=>PIdiv2,b=>A1FP,clk=>clk,ce=>cesubA1,result=>A1SFP); ----------------------cos/sin +FP ------------------state 9---------------- ccosFp: cosFP port map(nrst=>nrst,clk=>clk,cecos=>cecosFP,ainp=>A1FP,cos=>cosA1FP); csinFp: cosFP port map(nrst=>nrst,clk=>clk,cecos=>cecosFP,ainp=>A1SFP,cos=>sinA1FP); ----------------------mul--------------------------state 10 --------------------- cmulW1: mulFP port map(a=>cosA1FP,b=>FL1,clk=>clk,ce=>ceZ1W1,result=>FW1); cmulZ1: mulFP port map(a=>sinA1FP,b=>FL1,clk=>clk,ce=>ceZ1W1,result=>FZ1); -------- Blatt 1 calcP1 fertig -----Blatt2 calpP+ : Berechnung A2----- csubZ2Z1: subFP port map(a=>FZw,b=>FZ1,clk=>clk,ce=>ceZW12,result=>Z2Z1); csubW2W1: subFP port map(a=>LwFP,b=>FW1,clk=>clk,ce=>ceZW12,result=>W2W1); ----------------------- state ------------------------------ catgZW: atanFP port map(nrst=>nrst,clk=>clk,ceatan=>ceatanZw,Xinp=>w2w1,Yinp=>z2z1,atanFP=>AtanZW); -----------------state ---------------------------------------- csubA2: subFP port map(a=>atanZW,b=>A1FP,clk=>clk,ce=>cesubA2,result=>A2FP); ------------------end RUNANGLE-------------------------------------------- --***************************************************************************** --==================ITPImpGen ------------------------------------------------- ------------------ convert Winkel DakrAx,InkrAX-------------------------- cA0conv: AxConv port map(nrst=>nrst,clk=>clk,ceConv=>ceAxx,AFPinp=>A0FP,Resol=>C_A0rad, AchsInkr=>AXInc(0),AXGrd=>AXGr(0));--A0 cA1conv: AxConv port map(nrst=>nrst,clk=>clk,ceConv=>ceAxx,AFPinp=>A1FP,Resol=>C_A1rad, AchsInkr=>AXInc(1),AXGrd=>AXGr(1));--A1 cA2conv: AxConv port map(nrst=>nrst,clk=>clk,ceConv=>ceAxx,AFPinp=>A2FP,Resol=>C_A2rad, AchsInkr=>AXInc(2),AXGrd=>AXGr(2));--A2 cA3conv: AxConv port map(nrst=>nrst,clk=>clk,ceConv=>ceAxx,AFPinp=>A3FP,Resol=>C_A3rad, AchsInkr=>AXInc(3),AXGrd=>AXGr(3));--A3 cA4conv: AxConv port map(nrst=>nrst,clk=>clk,ceConv=>ceAxx,AFPinp=>A4FP,Resol=>C_A4rad, AchsInkr=>AXInc(4),AXGrd=>AXGr(4));--A4 cA5conv: AxConv port map(nrst=>nrst,clk=>clk,ceConv=>ceAxx,AFPinp=>A5FP,Resol=>C_A5rad, AchsInkr=>AXInc(5),AXGrd=>AXGr(5));--A5 ------------------------nur für tests------------------------------ --cA12conv: AxConv port map(nrst=>nrst,clk=>clk,ceConv=>ceAxx,AFPinp=>A12FP,Resol=>A12res, --Axint=>SA12); --cATconv: AxConv port map(nrst=>nrst,clk=>clk,ceConv=>ceAxx,AFPinp=>ATFP,Resol=>A12res,Axint=>SAT); --cAZconv: AxConv port map(nrst=>nrst,clk=>clk,ceConv=>ceAxx,AFPinp=>AZFP,Resol=>A12res,Axint=>SAZ); --cAwconv: AxConv port map(nrst=>nrst,clk=>clk,ceConv=>ceAxx,AFPinp=>AwFP,Resol=>A12res,Axint=>SAw); --=============================================================================== -----********************* Div Testwerte ************************************ ---- test ArcTang-------------------- -- Testwert tgFP=500/3000=0.1666666 =X"3e2aaaab" -- Test Resultat für versuch mit 0.166666 = 0.16514867740814 =X"3e291cbc" ------------------------------------------------------------------ --- testwert Input: 60grd= 1.0471975512 =X"3F860A92"------------------------------- ----- Output:cos = 0.5=FP :X"3F000000",Testresultat :0.499998986721 ------------------------- --- +60 Grad Winkel :1.0471975512 rad = 3f860a92,-60 Grad:-1.04.. =bf860a92 ---- + 90 Grad = 3fc90fdb , -90 Grd = bfc90fdb ---------------------- ----sin :0.86602540379= FP: 3f5db3d7, Testresultat : ----- ------------------------------------------------------------- --input FP-Testwert(XinpCos) für arcsin(0.5)=X"3f000000", Resultat =0.5235987756= X"3F060a92" (FP) --input FP-Testwert(XinpCos) für arccos(0.5)=X"3f000000", Resultat =1.0471975512= X"3F" (FP) --======================================================================== end Behavioral;