ITPModul.vhd

 ----------------------------------------------------------------------------------
-- Company: AK development GmbH
-- Engineer:  A:Kurka
-- Create Date:   31.8.2021 
-- modif:20.08.24 Version A21
-- 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 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 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
--======================================================================
--**************************************************************************
--===================================================================================
---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
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
  --IF rising_edge(clk) THEN
  wait until (rising_edge(clk));
    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 := to_integer(unsigned(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
                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;
              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
                IF XYZok = '1' THEN --- wenn Bressenham fertig
                  ItpRun <= '0';
                  enditp <= '0'; -- ende kontur
                  sitprun <= '0';
                  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;
                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
VARIABLE cntrclk : INTEGER RANGE 0 TO 3000000:= 0;-- für 30ms max
VARIABLE iEB :INTEGER RANGE  0 TO 2 := 0;-- zwischenspeicher für ebene    
  --IF rising_edge(clk) THEN
  wait until (rising_edge(clk));
    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 := to_integer(unsigned(Ebene));-- Ebene für linSatz
            StateLin <= S1;
          ELSE
            StateLin <= S0;-- warten
          END IF;
        WHEN S1 => -- init div koordinaten
          ITPlin(iEB) <= '1';--Itp für aktive Ebene
          -----------------
          IF Xlin(15) = '1' THEN -- negativ koord
            FLinXneg <= '1';
            XLinAbs <= std_logic_vector((signed(Xlin XOR X"FFFF")) + 1);
            --XLinAbs <= (Xlin XOR X"FFFF") + 1;
          ELSE
            FLinXneg <= '0';
            XlinAbs <= Xlin;
          END IF;
          ----------------------
          IF Ylin(15) = '1' THEN -- negativ koord
            FLinYneg <= '1';
            YLinAbs <= std_logic_vector((signed(Ylin XOR X"FFFF")) + 1);
            --YLinAbs <= (Ylin XOR X"FFFF") + 1;
          ELSE
            FLinYneg <= '0';
            YlinAbs <= Ylin;
          END IF;
          --------------------
          StateLin <= S2;
        WHEN S2 =>
          dXlin <= to_integer(unsigned(XLinAbs));-- Xkoord
          dYlin <= to_integer(unsigned(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(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
              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(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(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
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";
  ---IF rising_edge(clk) THEN
  wait until (rising_edge(clk));
    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 := to_integer(unsigned(pfrq));
              stateLinB <= S1;
              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 <= std_logic_vector(signed(XlinA) + 1);
                ImpLklin <= '0';
                errX := errX + dLkLin;
              END IF;
              ----------------------
              errY := errY - dYlin;  
              IF errY < 0 THEN
                YLinA <= std_logic_vector(signed(YLinA) + 1);
                ImpLklin <= '0';
                errY := errY + dLkLin;
              END IF;
              -------------------------
              count := to_integer(unsigned(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 <= std_logic_vector(signed(XlinA) - signed(XaltA));
          YdifA <= std_logic_vector(signed(YlinA) - signed(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
VARIABLE cntrclk : INTEGER RANGE 0 TO 511:= 0;                                                                                      
  --IF rising_edge(clk) THEN
  wait until (rising_edge(clk));
    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;
            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;
            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
            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
VARIABLE cntrclk :INTEGER RANGE 0 TO 30000000:= 0;-- für 30ms max
  ---IF rising_edge(clk) THEN
  wait until (rising_edge(clk));
    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
              XTAbs <= std_logic_vector((signed(XT XOR X"FFFFFFFF")) + 1);
              fXneg <= '1';--neg Flag setzen
              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
              YTAbs <= std_logic_vector((signed(YT XOR X"FFFFFFFF")) + 1);
              fYneg <= '1';--neg Flag setzen
              YTabs <= YT; 
              fYneg <= '0';--neg Flag,Rücksetzen
            END IF;
            --------
            IF ZT(31)  = '1' THEN -- if ZT negativ
              ZTabs <= std_logic_vector((signed(ZT XOR X"FFFFFFFF")) + 1);-- ZT negieren
              fZneg <= '1';--neg Flag setzen
              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
              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
                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
VARIABLE count :INTEGER;
VARIABLE errX   :INTEGER;
VARIABLE errY   :INTEGER;
VARIABLE errZ   :INTEGER;
--VARIABLE startON :std_logic:= '0';
  ----IF rising_edge(clk) THEN
  wait until (rising_edge(clk));
    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 <= to_integer(unsigned(XTabs));-- Xkoord
              dY <= to_integer(unsigned(YTabs));
              dZ <= to_integer(unsigned(ZTabs));
              dLk <= to_integer(unsigned(Lk));--Leit koord
              statebrsh <= 1;
            ELSE --anfang der berechnung nach strimp
              --IF strimp = '1' AND startON = '0' THEN
              IF strimp = '1' 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
          ---------- ursprünglich-----------
            --IF Lka < Lk  THEN..Lka:integer,LkSLV(31..0)
              ---Lka <= Lka + 1;
          -----------------------------------------------
            IF (to_unsigned(Lka,32) < to_integer(unsigned(Lk)))  THEN
              Lka <= to_integer(to_unsigned(Lka,32) + 1);
              ImpLK <= '0';
              ------------------------
              errX := errX - dX;  
              IF errX < 0 THEN
                IF fXneg = '0' THEN
                  XA <= std_logic_vector(signed(XA) + 1);
                  XA <= std_logic_vector(signed(XA) - 1);
                END IF;
                ImpX <= '0';
                errX := errX + dLk;
              END IF;
              ----------------------
              errY := errY - dY;  
              IF errY < 0 THEN
                IF fYneg = '0' THEN
                  YA <= std_logic_vector(signed(YA) + 1);
                  YA <= std_logic_vector(signed(YA) - 1);
                END IF;
                ImpY <= '0';
                errY := errY + dLk;
              END IF;
              -------------------------
              errZ := errZ - dZ;  
              IF errZ < 0 THEN
                IF fZneg = '0' THEN
                  ZA <= std_logic_vector(signed(ZA) + 1);
                  ZA <= std_logic_vector(signed(ZA) - 1);
                END IF;
                ImpZ <= '0';
                errZ := errZ + dLk;
              END IF;
              -------------------------
              count := to_integer(unsigned(pfrq));
              statebrsh <= 2;
              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)
runangle:PROCESS
VARIABLE ctrclk :INTEGER RANGE 0 TO 127:= 0;
--VARIABLE startON :std_logic:= '0';
  --IF rising_edge(clk) THEN  
  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';
--      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;
            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;
              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;
              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;
              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;
              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;
            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;
            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;
            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;
            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;
            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;
            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;
            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;
            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;
            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;
            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;
            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;
            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
VARIABLE cntrclk :Integer range 0 to 15:= 0;
VARIABLE strt :STD_LOGIC := '1';
  ----IF rising_edge(clk) THEN
  wait until (rising_edge(clk));
    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) <= std_logic_vector(signed(AXinc(i)) - signed(AXalt(i)));-- Differenz errechnen inn 32 bit
          AXdif(i)<= std_logic_vector(signed(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
  ---IF rising_edge(clk) THEN
  wait until (rising_edge(clk));
    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;

Kontakt/Impressum – Datenschutzerklärung – Nutzungsbedingungen – Werbung auf Mikrocontroller.net