---------------------------------------------------------------------------------- -- Company: -- Engineer: -- -- Create Date: 09:36:37 08/20/2024 -- Design Name: -- Module Name: runangle - Behavioral -- Project Name: -- Target Devices: -- Tool versions: -- Description: -- -- Dependencies: -- -- Revision: -- Revision 0.01 - File Created -- Additional Comments: -- ---------------------------------------------------------------------------------- library IEEE; use IEEE.STD_LOGIC_1164.ALL; use IEEE.NUMERIC_STD.ALL; entity runangle 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"; ImpLk : in std_logic ); end runangle; architecture Behavioral of runangle is 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 SIGNAL stateangle :integer range 0 to 63; SIGNAL ctrPLk :Integer range 0 to 33554431;-- dekr. Counter für Imp. Periode --SIGNAL ImpLk :std_logic;-- inkrement virtuelle Lk Impuls von XYZ ITP (invers Trans) SIGNAL grPI2 :std_logic; -----RUNANGLE Berechnung : a0 := arctan(Ya/Xa) und aXYT:=arctan(YT/XT) Berechnung------------------------------- SIGNAL ceXYZFP :std_logic; -----------------------state 0--------------------- SIGNAL FXYZaOK :std_logic;--signalisiert wenn FXYZa bereit SIGNAL suPIAT :std_logic_vector(31 downto 0); SIGNAL adPIAT :std_logic_vector(31 downto 0); ----------------- state 2---------------------------------- SIGNAL cePI2 :std_logic;-- start für sPIAT := PI/2 - PIATabs für sin AT SIGNAL PIAT :std_logic_vector(31 downto 0); ---------------- state 3 ---------------------------------- SIGNAL cetgFP :std_logic; -----------------State 9 --------------------------------- SIGNAL cemul9 :std_logic; -----------------State 10 ----------------------------------- SIGNAL cemul10 :std_logic; -----------------State 11--------------------------- SIGNAL cesubW :std_logic:='0'; SIGNAL aFXw :std_logic_vector(31 downto 0);-- SIGNAL sFXw :std_logic_vector(31 downto 0);-- SIGNAL FXw :std_logic_vector(31 downto 0);-- ---------------- State 12------------------------------ SIGNAL cemul12 :std_logic; ---------------state 13----------------------------------- SIGNAL ceaddXYw :std_logic; ---------------- State 14 --------------------------------- SIGNAL cesqrtLw :std_logic; SIGNAL cemAZA3 :std_logic; SIGNAL DivPIdiv :std_logic_vector(31 downto 0);-- ------------------state 15---------------------------- SIGNAL cedAZA3 :std_logic; ------------------state 16 ----------------------------------- SIGNAL cedivPIdiv :std_logic; SIGNAL ceA4FP :std_logic; ----------------- state 17 ----------------------------------- SIGNAL ceAZmul :std_logic; ----------------- state 18 -------------------------------- SIGNAL absdivPIdiv :std_logic_vector(31 downto 0);-- ----------------- state 19 -------------------------------- SIGNAL cesub1min :std_logic; ----------------- state 20 -------------------------------- SIGNAL ceA3FP :std_logic; ----------------state 22-- nur für test ----------------- SIGNAL ceXYZw :std_logic; SIGNAL ceP1 :std_logic;--start sq für P1 SIGNAL cesqL :std_logic; SIGNAL ceadd1 :std_logic; SIGNAL cesub1 :std_logic; SIGNAL cediv1 :std_logic; --------------------------- SIGNAL ceacosFP :std_logic;-- Start arccosFP SIGNAL ceA1FP :std_logic; SIGNAL cesubA1 :std_logic; SIGNAL cecosFP :std_logic; ------ RUNANGLE vorher Calc P1 blatt1 fertig, Blatt2 anfang SIGNAL ceZW12 :std_logic; SIGNAL ceatanZW :std_logic; SIGNAL cesubA2 :std_logic; --------------------- SIGNAL ceZ1W1 :std_logic; begin runangle:PROCESS VARIABLE ctrclk :INTEGER RANGE 0 TO 127:= 0; VARIABLE startON :std_logic:= '0'; BEGIN wait until (rising_edge(clk)); 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'; ELSE CASE stateangle IS WHEN 0 => 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; ceXYZFP <= '0';--reset CE stateangle <= 1; ELSE StartON := '1'; 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; cePI2 <= '0';-- reset stateangle <= 4; ELSE 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; ceTgFP <= '0'; --Reset ArcTang start Impuls stateangle <= 5; ELSE --tanYX , ATFP,AZFP fertig cemul9 <= '1';-- start sinAZ* sinAT, sinAZ* cosAT ctrclk := clkTomul; stateangle <= 9; END IF; WHEN 9 => -- ausführen; sinAZ* sinAT, sinAZ* cosAT IF ctrclk > 0 THEN ctrclk := ctrclk -1; cemul9 <= '0';--reset stateangle <= 9; ELSE 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; cemul10 <= '0';-- reset stateangle <= 10; ELSE 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; ceSubw <= '0';-- reset stateangle <= 11; ELSE 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; cemul12 <= '0';-- reset stateangle <= 15; ELSE 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; ceaddXYw <= '0'; --reset stateangle <= 18; ELSE cesqrtLw <= '1'; --start LwFP:=sqrt(sqXYw) und subPIdiv= subATAW-pidIV2 ctrclk := clkTosq; 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; cesqrtLw <= '0';-- reset stateangle <= 19; ELSE 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; cemAZA3 <= '0'; --reset stateangle <= 22; ELSE 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; cedAZA3 <= '0';-- reset stateangle <= 23; ELSE 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; ceA4FP <= '0'; --reset cedivPIdiv <= '0'; stateangle <= 24; ELSE absdivPIdiv <= divPIdiv;-- vorläufig ceAZmul <= '1'; ceP1 <= '1'; -- START intToFP für L1,L2 => FL1,FL2 stateangle <= 25;-- END IF; WHEN 25 => IF absDivPidiv(31) = '1' THEN absDivPidiv(31) <= '0';-- Abs von DivPiDiv END IF; ceP1 <= '0';-- FL1,FL2 fertig ceAZmul <= '0';-- a4FP fertig ctrclk := clkTosub; cesub1min <= '1'; stateangle <= 26; WHEN 26 =>-- sub1min := 1 - absdivPIdiv IF ctrclk > 0 THEN ctrclk := ctrclk - 1; cesub1min <= '0'; stateangle <= 26; ELSE 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; cesqL <= '0';--FL11^2,Fl2^2,FL12^2=> SqL1,sqL12,sqL2 fertig ceA3FP <= '0';--a3FP fertig stateangle <= 27; ELSE IF strImp = '0' THEN StartON := '0'; END IF; 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; ceadd1<= '0';--L1+sqL12,Fl12*FL1 fertig stateangle <= 28; ELSE 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; cesub1 <= '0';--sqadd1-sqL2, mul12 * 2 fertig stateangle <= 29; ELSE cediv1 <= '1'; -- start sqsub1/ mulres1 => ax ctrclk := clkToDiv; stateangle <= 30; END IF; WHEN 30 => IF ctrclk > 0 THEN ctrclk := ctrclk - 1; cediv1 <= '0';--sqsub1/ mulres1 => ax fertig stateangle <= 30; ELSE ceacosFP <= '1'; -- start für atang( Ax) ctrclk := clkTOacos; stateangle <= 31; END IF; WHEN 31 => IF ctrclk > 0 THEN ctrclk := ctrclk - 1; ceacosFP <= '0';--atang( Ax) fertig stateangle <= 31; ELSE ceA1FP <= '1'; -- start für acosAx + A12FP ctrclk := clkTOadd; stateangle <= 32; END IF; WHEN 32 => IF ctrclk > 0 THEN ctrclk := ctrclk - 1; ceA1FP <= '0';--acosAx + A12FP => A+FP fertig ************ stateangle <= 32; ELSE cesubA1 <= '1'; --start für PI/2 - A1FP ctrclk := clkTOsub; stateangle <= 33; END IF; WHEN 33 => IF ctrclk > 0 THEN ctrclk := ctrclk - 1; cesubA1 <= '0';--PI/2 - A1FP => A1SFP fertig stateangle <= 33; ELSE cecosFP <= '1'; --Start für cos(a1FP) & cosFP(a1SFP) ctrclk := clkTOcos; stateangle <= 34; END IF; WHEN 34 => IF ctrclk > 0 THEN ctrclk := ctrclk - 1; cecosFP <= '0';--cos(a1FP) & cosFP(a1SFP) =>cosA1FP,sinA1FP fertig stateangle <= 34; ELSE ceZ1W1 <= '1'; --start für sinA1FP*FL1, cosA1FP * FL1 ctrclk := clkTOmul; stateangle <= 35; END IF; WHEN 35 => IF ctrclk > 0 THEN ctrclk := ctrclk - 1; ceZ1W1 <= '0';--sinA1FP*FL1=> FZ1,cosA1FP * FL1=>FW1 fertig ****** stateangle <= 35; ELSE cezw12 <= '1'; -- start für FZw-Fz1 & LwFP-FW1 ctrclk := clkTOsub; stateangle <= 36; END IF; WHEN 36 => IF ctrclk > 0 THEN ctrclk := ctrclk - 1; cezw12 <= '0';--FZw-Fz1=Z2Z1 & LwFP-FW1=W"W1 fertig stateangle <= 36; ELSE ceatanZW <= '1';-- start für atan ctrclk := clkTOatan; stateangle <= 37; END IF; WHEN 37 => IF ctrclk > 0 THEN ctrclk := ctrclk - 1; ceatanZW <= '0';-- AtanZW fertig stateangle <= 37; ELSE cesubA2 <= '1'; -- start für atanZw- a1FP ctrclk := clkTOsub; stateangle <= 38; END IF; WHEN 38 => IF ctrclk > 0 THEN ctrclk := ctrclk - 1; cesubA2 <= '0';-- A2FP fertig ******************** stateangle <= 38; ELSE 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 PROCESS;-- end runangle end Behavioral;