runangle.vhd

----------------------------------------------------------------------------------

-- 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;