---------------------------------------------------------------------------------- -- 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; --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 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 -- Signal in die Port map verschoben, dass man es testen kann (sonst wird es entfernt) ); 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 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; signal ctrclk : integer range 0 to 127; SIGNAL stateangle :integer range 0 to 63; signal next_stateangle : integer range 0 to 63; begin runangle:PROCESS 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 <= 63; next_stateangle <= 1; ELSE FXYZaOK <= '0'; --reset END IF; WHEN 1 => -- ausführen Xa,Ya,Zz=>FP und Pi/2-AZFP,Pi/2 +/- ATFP ceXYZFP <= '0';--reset CE -- StartON := '1'; stateangle <= 2; 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 <= 63; next_stateangle <= 4; WHEN 4 => --ausführen PIAZ :=PI/2 - PIAT cePI2 <= '0';-- reset ctrclk <= 35;--clk Atan für durchlauf ****** ceTgFP <= '1'; --arcTang und cos starten stateangle <= 63; next_stateangle <= 5; WHEN 5 => --Ausführen A0FP:=atan(Ya/Xa),und Cos(4 bml), A=FP fertig***** ceTgFP <= '0'; --Reset ArcTang start Impuls --tanYX , ATFP,AZFP fertig cemul9 <= '1';-- start sinAZ* sinAT, sinAZ* cosAT ctrclk <= clkTomul; stateangle <= 63; next_stateangle <= 9; WHEN 9 => -- ausführen; sinAZ* sinAT, sinAZ* cosAT cemul9 <= '0';--reset ctrclk <= clkTomul; cemul10 <= '1'; --start sAZAT* FL3, cAZAT* FL3 stateangle <= 63; next_stateangle <= 10; WHEN 10 => --ausführen sAZAT* FL3, cAZAT* FL3 cemul10 <= '0';-- reset ctrclk <= clkTosub; ceSubw <= '1'; -- Start für sub/add FXa -/+ sAZAT3 usw stateangle <= 63; next_stateangle <= 11; WHEN 11 => -- ausführen sub/add FXa -/+ sAZAT3.. = FYw,FZw fertig ceSubw <= '0';-- reset stateangle <= 12; 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 <= 63; ---FXW fertig Berechnet next_stateangle <= 15; ----------------state reserve---------------------------- WHEN 15 => --- Ausführen Fxw^2,FYw^2 cemul12 <= '0';-- reset stateangle <= 16; 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 <= 63; next_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 ceaddXYw <= '0'; --reset cesqrtLw <= '1'; --start LwFP:=sqrt(sqXYw) und subPIdiv= subATAW-pidIV2 ctrclk <= clkTosq; stateangle <= 63; next_stateangle <= 19; ------Blatt6:csqrtLW, Blatt5 :cdivPIdiv2-- gestartet mit cesqrtLW --------- WHEN 19 => --ausführen LwFP:= sqrt(sqXYw),LWFP fertig, subPidiv=subAtAw-PIdiv2 cesqrtLw <= '0';-- reset cemAzA3 <= '1'; --start AtFp-AwFp absDivPIdiv <= DivPIdiv;--absDivPiDiv vorläaufig setzen ctrclk <= clkTosub; stateangle <= 63; next_stateangle <= 22; ----reserve state ---------------- ----- Blatt5:csqZ2,csqW2,cmuAZFP ---- gestartet mit cemAzA3 ---------------- WHEN 22 => --- ausführen sq(FZw),sq(LWFP), mul(AZFP,DivPIdiv)------ cemAZA3 <= '0'; --reset cedAZA3 <= '1';--start subAtAw-PIdiv2 ctrclk <= clkTosub; stateangle <= 63; next_stateangle <= 23; ---- Blatt 5:caddWZ, csub1min--- gestartet mit cedAZA3 --------------- WHEN 23 => --- ausführen sqaddWZ=sqZ2 + sqW2,subPIdiv = cedAZA3 <= '0';-- reset cedivPIdiv <= '1'; ceA4FP <= '1'; -- start sqrt(sqaddWZ), atan A12, ctrclk <= clkToAtan;--clk Atan stateangle <= 63; next_stateangle <= 24; --- Blatt5:catanA12,sqrtWZ,cmulAZFP -- gestartet mit ceA4FP ------- WHEN 24 => --ausführen atan(FZW,LWFP),divFp,vsqrt(sqaddWZ), mul(sub1min,AZFP) ceA4FP <= '0'; --reset cedivPIdiv <= '0'; absdivPIdiv <= divPIdiv;-- vorläufig ceAZmul <= '1'; ceP1 <= '1'; -- START intToFP für L1,L2 => FL1,FL2 stateangle <= 25; 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 <= 63; next_stateangle <= 26; WHEN 26 =>-- sub1min := 1 - absdivPIdiv cesub1min <= '0'; ceA3FP <= '1';-- startsub1min* AZFP => A3FP cesqL <= '1';-- start für FL11^2,Fl2^2,FL12^2 ctrclk <= clkTomul; stateangle <= 63; next_stateangle <= 27; -- Ab hier Funktionen aus calcP1--------------------------------------- WHEN 27 => -- A3FP :=sub1min * AZFP, a3FP fertig,FL11^2,Fl2^2,FL12^2 cesqL <= '0';--FL11^2,Fl2^2,FL12^2=> SqL1,sqL12,sqL2 fertig ceA3FP <= '0';--a3FP fertig IF strImp = '0' THEN -- StartON := '0'; END IF; ceadd1<= '1'; -- startsqL1+sqL12,Fl12*FL1 ctrclk <= clkTomul; stateangle <= 63; next_stateangle <= 28; WHEN 28 => -- ausführen L1+sqL12,Fl12*FL1 ceadd1<= '0';--L1+sqL12,Fl12*FL1 fertig cesub1 <= '1'; -- start für: sqadd1-sqL2, mul12 * 2 ctrclk <= clkTomul; stateangle <= 63; next_stateangle <= 29; WHEN 29 => -- ausführen: sqadd1-sqL2, mul12 * 2 cesub1 <= '0';--sqadd1-sqL2, mul12 * 2 fertig cediv1 <= '1'; -- start sqsub1/ mulres1 => ax ctrclk <= clkToDiv; stateangle <= 63; next_stateangle <= 30; WHEN 30 => cediv1 <= '0';--sqsub1/ mulres1 => ax fertig ceacosFP <= '1'; -- start für atang( Ax) ctrclk <= clkTOacos; stateangle <= 63; next_stateangle <= 31; WHEN 31 => ceacosFP <= '0';--atang( Ax) fertig ceA1FP <= '1'; -- start für acosAx + A12FP ctrclk <= clkTOadd; stateangle <= 63; next_stateangle <= 32; WHEN 32 => ceA1FP <= '0';--acosAx + A12FP => A+FP fertig ************ cesubA1 <= '1'; --start für PI/2 - A1FP ctrclk <= clkTOsub; stateangle <= 63; next_stateangle <= 33; WHEN 33 => cesubA1 <= '0';--PI/2 - A1FP => A1SFP fertig cecosFP <= '1'; --Start für cos(a1FP) & cosFP(a1SFP) ctrclk <= clkTOcos; stateangle <= 63; next_stateangle <= 34; WHEN 34 => cecosFP <= '0';--cos(a1FP) & cosFP(a1SFP) =>cosA1FP,sinA1FP fertig ceZ1W1 <= '1'; --start für sinA1FP*FL1, cosA1FP * FL1 ctrclk <= clkTOmul; stateangle <= 63; next_stateangle <= 35; WHEN 35 => ceZ1W1 <= '0';--sinA1FP*FL1=> FZ1,cosA1FP * FL1=>FW1 fertig ****** cezw12 <= '1'; -- start für FZw-Fz1 & LwFP-FW1 ctrclk <= clkTOsub; stateangle <= 63; next_stateangle <= 36; WHEN 36 => cezw12 <= '0';--FZw-Fz1=Z2Z1 & LwFP-FW1=W"W1 fertig ceatanZW <= '1';-- start für atan ctrclk <= clkTOatan; stateangle <= 63; next_stateangle <= 37; WHEN 37 => ceatanZW <= '0';-- AtanZW fertig cesubA2 <= '1'; -- start für atanZw- a1FP ctrclk <= clkTOsub; stateangle <= 63; next_stateangle <= 38; WHEN 38 => cesubA2 <= '0';-- A2FP fertig ******************** FXYZaOK <= '1';--RUNANGLE bereit stateangle <= 0;-- alle Achsen fertig --Berechnung fertg,wartn auf nächste Start when 63 => if(ctrclk > 0) then ctrclk <= ctrclk - 1; else stateangle <= next_stateangle; end if; WHEN OTHERS => stateangle <= 0; END CASE; END IF;-- nrst if/else END PROCESS;-- end runangle end Behavioral;