runangle.vhd


1
----------------------------------------------------------------------------------

2
-- Company: 

3
-- Engineer: 

4
-- 

5
-- Create Date:    09:36:37 08/20/2024 

6
-- Design Name: 

7
-- Module Name:    runangle - Behavioral 

8
-- Project Name: 

9
-- Target Devices: 

10
-- Tool versions: 

11
-- Description: 

12
--

13
-- Dependencies: 

14
--

15
-- Revision: 

16
-- Revision 0.01 - File Created

17
-- Additional Comments: 

18
--

19
----------------------------------------------------------------------------------

20
library IEEE;

21
use IEEE.STD_LOGIC_1164.ALL;

22
use IEEE.NUMERIC_STD.ALL;

23
24
25
entity runangle is

26
  port (nrst    : in std_logic;

27
      clk     : in std_logic;

28
--      XS      :in std_logic_vector(31 downto 0);-- Startpunkt Koordinate X

29
--      YS      :in std_logic_vector(31 downto 0);-- Startpunkt Koordinate Y

30
--      ZS      :in std_logic_vector(31 downto 0);-- Startpunkt Koordinate Z

31
--      XT      :in std_logic_vector(31 downto 0);-- Koordinate X relativ (XE-XS)

32
--      YT      :in std_logic_vector(31 downto 0);-- Koordinate Y  relativ (YE-YS)

33
--      ZT      :in std_logic_vector(31 downto 0);-- Koordinate Z  relativ (ZE-ZS)

34
--      pfrq    :in std_logic_vector(23 downto 0);--Bahn-Periode in anzahl CLK 

35
--      L1      :in std_logic_vector(15 downto 0);-- länge Arm1 in mm

36
--      L2      :in std_logic_vector(15 downto 0);-- länge Arm2 in mm

37
--      L3      :in std_logic_vector(15 downto 0);-- länge Arm3 in mm

38
      ---------------------

39
--      Xlin    :in std_logic_vector(15 downto 0);-- Koord LinStz

40
--      Ylin    :in std_logic_vector(15 downto 0);-- Max.Eingabe +/-327.67

41
--      ebene    :in std_logic_vector(3 downto 0);--interpolationsebene für LinSatz

42
--      Xactiv  :in std_logic;

43
--      Yactiv  :in std_logic;

44
--      celin    :in std_logic;---start interpolation für linstz in einer ebene

45
      --------------------------------------

46
--      stopp    :in std_logic;-- stop interpolation,(stopp+) nach  Endlage

47
--      RampRun  :in std_logic;-- rampe aktiv =1

48
      strimp  :in std_logic;-- start interpolation

49
--      ce      :in std_logic;-- start interpolation von kreis oder XYZ Satz : InversTransf

50
      -----------------------------

51
--      calcok  :out std_logic:= '0';-- signalisiert alle berechnungen sind fertig

52
--      XAout    :out std_logic_vector(31 downto 0):=X"00000000";--virtuelle aktuelle koordinate

53
--      YAout    :out std_logic_vector(31 downto 0):=X"00000000";

54
--      ZAout    :out std_logic_vector(31 downto 0):=X"00000000";

55
--      ITPmod   :out std_logic_vector(2 DOWNTO 0):="000";--ITP für Status Meldung an CPU

56
--      oAXdif  :out std_logic_vector(95 downto 0):= (OTHERS => '0');--Sollwertdiff(16Bit) von  inv.Transfer und Lin

57
--      oImpLk  :out std_logic:='1';--impulse von xyz itp

58
--      enditp   :out std_logic:='0';-- ende von Kontur:=0 (enditp-)

59
--      itprun  :out std_logic:='0';-- =1:interpolator läuft,aktuelle Sollwerte in oAXinkr(6x 32bit)

60
--      testdata :out std_logic_vector(15 downto 0):= X"0000";

61
62
63
      ImpLk  : in std_logic

64
      );

65
  

66
end runangle;

67
68
architecture Behavioral of runangle is

69
70
CONSTANT clkToFP : integer range 0 to 127 := 1;

71
CONSTANT clkTosq : integer range 0 to 127 := 1;

72
CONSTANT clkTomul : integer range 0 to 127 := 1;

73
CONSTANT clkToadd : integer range 0 to 127 := 1;

74
CONSTANT clkTosub : integer range 0 to 127 := 1;

75
CONSTANT clkTosqrt : integer range 0 to 127 := 1;

76
CONSTANT clkTodiv : integer range 0 to 127 := 2;

77
CONSTANT clkFPtoInt : integer range 0 to 127 := 1;

78
CONSTANT clkToAtan : integer range 0 to 127 := 25;

79
CONSTANT clkToAcos : integer range 0 to 127 := 25;

80
CONSTANT clkToCos : integer range 0 to 127 := 25;

81
CONSTANT clkAtan : integer range 0 to 127 := 30;

82
CONSTANT clkAtan32 : integer range 0 to 127 := 11;

83
CONSTANT clkdiv32 : integer range 0 to 127 := 32;

84
85
CONSTANT clkTosin  : integer range 0 to 127 := 1;

86
CONSTANT clkToMuli  : integer range 0 to 127 := 2;

87
CONSTANT C2     :std_logic_vector(31 DOWNTO 0):=X"40000000";-- zahl 2 in FP format 32bit

88
CONSTANT C1     :std_logic_vector(31 DOWNTO 0):=X"3F800000";-- zahl 1 in FP format 32bit

89
CONSTANT C0     :std_logic_vector(31 DOWNTO 0):=X"00000000";-- zahl 0 in FP format 32bit

90
CONSTANT PIdiv2   :std_logic_vector(31 DOWNTO 0):=X"3FC90FDB";-- +1.57079632679 PI/2 in FP Format

91
CONSTANT PIndiv2   :std_logic_vector(31 DOWNTO 0):=X"BFC90FDB";-- -1.57079632679 PI/2 in FP Format

92
CONSTANT PI      :std_logic_vector(31 DOWNTO 0):=X"40490FDB";-- 3.14159265359 PI in FP Format

93
94
-------

95
CONSTANT C132   :std_logic_vector(31 DOWNTO 0):=X"20000000";--zahl 1 in format 1Q32 bit

96
CONSTANT C126   :std_logic_vector(25 DOWNTO 0):="01" & X"000000";--zahl 1 in format 1Q26 bit

97
CONSTANT C026   :std_logic_vector(25 DOWNTO 0):="00" & X"000000";-- zahl 0 in FP format 26bit

98
CONSTANT Pihalf :std_logic_vector(25 DOWNTO 0):="001" & "10010010000111111011000";--Pi Half in 2Q26

99
100
SIGNAL stateangle :integer range 0 to 63;

101
102
SIGNAL ctrPLk :Integer range 0 to 33554431;-- dekr. Counter  für Imp. Periode

103
--SIGNAL ImpLk  :std_logic;-- inkrement virtuelle Lk Impuls von XYZ ITP (invers Trans)

104
SIGNAL grPI2  :std_logic;

105
106
-----RUNANGLE Berechnung : a0 := arctan(Ya/Xa) und aXYT:=arctan(YT/XT) Berechnung-------------------------------                            

107
SIGNAL ceXYZFP   :std_logic;

108
109
-----------------------state 0---------------------

110
SIGNAL FXYZaOK    :std_logic;--signalisiert wenn FXYZa bereit

111
SIGNAL suPIAT  :std_logic_vector(31 downto 0);

112
SIGNAL adPIAT  :std_logic_vector(31 downto 0);

113
----------------- state 2----------------------------------

114
SIGNAL cePI2   :std_logic;-- start für sPIAT := PI/2 - PIATabs für sin AT 

115
SIGNAL PIAT    :std_logic_vector(31 downto 0);

116
---------------- state 3 ----------------------------------

117
SIGNAL cetgFP     :std_logic;

118
-----------------State 9 ---------------------------------

119
SIGNAL cemul9   :std_logic;

120
-----------------State 10 -----------------------------------

121
SIGNAL cemul10   :std_logic;

122
-----------------State 11---------------------------

123
SIGNAL cesubW   :std_logic:='0';

124
SIGNAL aFXw  :std_logic_vector(31 downto 0);--

125
SIGNAL sFXw  :std_logic_vector(31 downto 0);--

126
SIGNAL FXw  :std_logic_vector(31 downto 0);--

127
---------------- State 12------------------------------

128
SIGNAL cemul12   :std_logic;

129
---------------state 13-----------------------------------

130
SIGNAL ceaddXYw   :std_logic;

131
---------------- State 14 ---------------------------------

132
SIGNAL cesqrtLw   :std_logic;

133
SIGNAL cemAZA3   :std_logic;

134
SIGNAL DivPIdiv  :std_logic_vector(31 downto 0);--

135
------------------state 15----------------------------

136
SIGNAL cedAZA3   :std_logic; 

137
------------------state 16 -----------------------------------

138
SIGNAL cedivPIdiv   :std_logic;

139
SIGNAL ceA4FP   :std_logic;   

140
----------------- state 17 -----------------------------------

141
SIGNAL ceAZmul   :std_logic;

142
----------------- state 18 --------------------------------

143
SIGNAL absdivPIdiv  :std_logic_vector(31 downto 0);--

144
----------------- state 19 --------------------------------

145
SIGNAL cesub1min   :std_logic;

146
----------------- state 20 --------------------------------

147
SIGNAL ceA3FP   :std_logic;

148
149
----------------state 22-- nur für test ----------------- 

150
SIGNAL ceXYZw :std_logic;

151
152
SIGNAL ceP1 :std_logic;--start sq für P1

153
SIGNAL cesqL :std_logic;

154
155
SIGNAL ceadd1 :std_logic;

156
SIGNAL cesub1 :std_logic;

157
158
SIGNAL cediv1  :std_logic;

159
---------------------------

160
SIGNAL ceacosFP   :std_logic;-- Start arccosFP

161
SIGNAL ceA1FP  :std_logic;

162
SIGNAL cesubA1  :std_logic;

163
SIGNAL cecosFP  :std_logic;

164
165
------ RUNANGLE vorher Calc P1 blatt1 fertig, Blatt2 anfang

166
SIGNAL ceZW12    :std_logic;

167
SIGNAL ceatanZW    :std_logic;

168
SIGNAL cesubA2    :std_logic;

169
170
---------------------

171
SIGNAL ceZ1W1  :std_logic;

172
173
begin

174
runangle:PROCESS

175
VARIABLE ctrclk :INTEGER RANGE 0 TO 127:= 0;

176
VARIABLE startON :std_logic:= '0';

177
BEGIN

178
  wait until (rising_edge(clk));

179
  

180
    IF nrst = '0' THEN

181
      stateangle <= 0;

182
      ctrclk := 0;

183
      FXYZaOK <= '0';

184
      ceXYZFP <= '0'; cetgFP<='0'; cemul9<='0'; cemul10<='0';

185
      cesubW <='0'; cePI2 <='0';

186
      cemAZA3 <= '0';cedAZA3 <= '0';cedivPIdiv<='0'; ceAZmul<= '0';

187
      cesub1min <= '0';ceA3FP<='0';

188
      ceA4FP <='0'; cetgFP <= '0';

189
      ceP1 <= '0';

190
      cesqL <= '0'; ceadd1 <= '0'; cesub1<= '0';

191
      cediv1 <= '0'; ceacosFP <= '0'; ceA1FP <='0';

192
      cecosFP <= '0'; ceZ1W1 <= '0'; cesubA1 <= '0';

193
      startON := '0';

194
    ELSE

195
      CASE stateangle IS

196
        WHEN 0 =>

197
          IF (ImpLk = '0') THEN  

198
            ceXYZFP <= '1';--Start für Xa,Ya,Zz=>FP, und Pi/2-AZFP,Pi/2 +/- ATFP

199
            ctrclk := clkToFP;

200
            stateangle <= 1;

201
          ELSE

202
            FXYZaOK <= '0'; --reset

203
            stateangle <= 0;-- warten auf LK Impuls

204
          END IF;

205
        WHEN 1 => -- ausführen Xa,Ya,Zz=>FP und Pi/2-AZFP,Pi/2 +/- ATFP

206
          IF ctrclk > 0 THEN

207
            ctrclk := ctrclk -1;

208
            ceXYZFP <= '0';--reset CE

209
            stateangle <= 1;

210
          ELSE

211
            StartON := '1';

212
            stateangle <= 2;

213
          END IF;

214
        WHEN 2 =>  -- zwischen state notwendig****

215
          IF grPI2 = '1' THEN -- wenn AT > PI/2 dann  sub PI/2 sonst ADD

216
              PIAT <= suPIAT;

217
          ELSE PIAT <= adPIAT;

218
          END IF;

219
          stateangle <= 3;

220
        WHEN 3 =>  -- zwischen state notwendig ****

221
           cePI2 <= '1';

222
           ctrclk := clkTosub;

223
           stateangle <= 4;

224
        WHEN 4 => --ausführen PIAZ :=PI/2 - PIAT

225
          IF ctrclk > 0 THEN

226
            ctrclk := ctrclk -1;

227
            cePI2 <= '0';-- reset

228
            stateangle <= 4;

229
          ELSE

230
            ctrclk  := 35;--clk Atan für durchlauf  ******  

231
            ceTgFP <= '1'; --arcTang und cos starten

232
            stateangle <= 5;

233
          END IF;

234
        WHEN 5 => --Ausführen A0FP:=atan(Ya/Xa),und Cos(4 bml),  A=FP fertig*****

235
          IF ctrclk > 0 THEN

236
            ctrclk := ctrclk -1;

237
            ceTgFP <= '0'; --Reset ArcTang start Impuls

238
            stateangle <= 5;

239
          ELSE --tanYX , ATFP,AZFP fertig

240
            cemul9 <= '1';-- start sinAZ* sinAT, sinAZ* cosAT

241
            ctrclk := clkTomul;

242
            stateangle <= 9;

243
          END IF;

244
        WHEN 9 => -- ausführen; sinAZ* sinAT, sinAZ* cosAT

245
          IF ctrclk > 0 THEN

246
            ctrclk := ctrclk -1;

247
            cemul9 <= '0';--reset

248
            stateangle <= 9;

249
          ELSE

250
            ctrclk := clkTomul;

251
            cemul10 <= '1'; --start sAZAT* FL3, cAZAT* FL3

252
            stateangle <= 10;

253
          END IF;

254
        WHEN 10 => --ausführen  sAZAT* FL3, cAZAT* FL3

255
            IF ctrclk > 0 THEN

256
              ctrclk := ctrclk -1;

257
              cemul10 <= '0';-- reset

258
              stateangle <= 10;

259
            ELSE

260
              ctrclk := clkTosub;

261
              ceSubw <= '1'; -- Start für sub/add FXa -/+ sAZAT3 usw

262
              stateangle <= 11;

263
            END IF;

264
        WHEN 11 =>  -- ausführen sub/add FXa -/+ sAZAT3.. = FYw,FZw fertig

265
          IF ctrclk > 0 THEN

266
            ctrclk := ctrclk -1;

267
            ceSubw <= '0';-- reset

268
            stateangle <= 11;

269
          ELSE

270
            stateangle <= 12;

271
          END IF;

272
        WHEN 12 =>-- für test 

273
            stateangle <= 13;

274
        WHEN 13 => -- zwischen state notwendig ***

275
            IF grPI2 = '1' THEN

276
              FXw <= aFXw;

277
            ELSE FXw <= sFXw;

278
            END IF;

279
            ctrclk := clkTosq;

280
            cemul12 <= '1'; --Start für sqXw:=sq(FXw),sqYw:=sq(FYw) und ATPI:= ATFP-PI/2

281
            stateangle <= 15; ---FXW fertig Berechnet 

282
        ----------------state reserve----------------------------

283
        WHEN 15 =>  ---  Ausführen Fxw^2,FYw^2

284
            IF ctrclk > 0 THEN

285
              ctrclk := ctrclk -1;

286
              cemul12 <= '0';-- reset

287
              stateangle <= 15;

288
            ELSE

289
              stateangle <= 16;

290
            END IF;

291
        WHEN 16 =>

292
            ceXYZw <= '1';---für test ***** XYZ W ***********************

293
            stateangle <= 17;  

294
            -----Blatt6:csqXw,csqYw, Blatt5: csubAwAt-- gestartet mit cemul12 ----------------            

295
        WHEN 17 =>  -- ausführen sqXw:=sq(FXw),sqYw:=sq(FYw)  

296
            ctrclk  := 23;-- 22 clk für atan

297
            ceXYZw <= '0'; -- reset

298
            ceaddXYw <= '1'; --start fürAwFP:=atan(FYw/FXw) und sqXYW:=sqXw+SqYw,subPIDIV0subAtAW-pIDIV0

299
            stateangle <= 18;

300
        -----Blatt6: catgAw (22clk),caddXYw, Blatt5:csubPidiv--- gestartet mit ceaddXYw---------

301
        WHEN 18 =>-- ausführen AwFP:=atan(FYw/FXw) und  sqXYW:=sqXw+ SqYw,subPidiv=subAtAw-PIdiv2

302
            IF ctrclk > 0 THEN

303
              ctrclk := ctrclk -1;

304
              ceaddXYw <= '0'; --reset 

305
              stateangle <= 18;

306
            ELSE

307
              cesqrtLw <= '1'; --start LwFP:=sqrt(sqXYw) und subPIdiv= subATAW-pidIV2

308
              ctrclk  := clkTosq;

309
              stateangle <= 19;

310
            END IF;

311
        ------Blatt6:csqrtLW, Blatt5 :cdivPIdiv2-- gestartet mit cesqrtLW ---------

312
        WHEN 19 => --ausführen LwFP:= sqrt(sqXYw),LWFP fertig, subPidiv=subAtAw-PIdiv2

313
          IF ctrclk > 0 THEN

314
            ctrclk := ctrclk - 1;

315
            cesqrtLw <= '0';-- reset

316
            stateangle <= 19;

317
          ELSE

318
            cemAzA3 <= '1'; --start AtFp-AwFp

319
            absDivPIdiv <= DivPIdiv;--absDivPiDiv vorläaufig setzen

320
            ctrclk := clkTosub;

321
            stateangle <= 22;

322
          END IF;

323
        ----reserve state ----------------

324
        ----- Blatt5:csqZ2,csqW2,cmuAZFP ---- gestartet mit cemAzA3 ----------------

325
         WHEN 22 =>  --- ausführen sq(FZw),sq(LWFP), mul(AZFP,DivPIdiv)------

326
          IF ctrclk > 0 THEN

327
            ctrclk := ctrclk - 1;

328
            cemAZA3 <=  '0'; --reset

329
            stateangle <= 22;

330
          ELSE

331
            cedAZA3 <= '1';--start subAtAw-PIdiv2

332
            ctrclk := clkTosub;

333
            stateangle <= 23;

334
          END IF;

335
        ---- Blatt 5:caddWZ, csub1min--- gestartet mit cedAZA3 ---------------

336
        WHEN 23 =>  --- ausführen sqaddWZ=sqZ2 + sqW2,subPIdiv =

337
          IF ctrclk > 0 THEN

338
            ctrclk := ctrclk - 1;

339
            cedAZA3 <= '0';-- reset

340
            stateangle <= 23;

341
          ELSE

342
            cedivPIdiv <= '1';

343
            ceA4FP <= '1'; -- start sqrt(sqaddWZ), atan A12,

344
            ctrclk  := clkToAtan;--clk Atan

345
            stateangle <= 24;

346
          END IF;

347
        --- Blatt5:catanA12,sqrtWZ,cmulAZFP -- gestartet mit ceA4FP -------

348
        WHEN 24 => --ausführen atan(FZW,LWFP),divFp,vsqrt(sqaddWZ), mul(sub1min,AZFP)

349
            IF ctrclk > 0 THEN

350
              ctrclk := ctrclk - 1;

351
              ceA4FP <= '0'; --reset

352
              cedivPIdiv <= '0';

353
              stateangle <= 24;

354
            ELSE

355
              absdivPIdiv <= divPIdiv;-- vorläufig

356
              ceAZmul <= '1';

357
              ceP1 <= '1'; -- START intToFP für  L1,L2 => FL1,FL2

358
              stateangle <= 25;--

359
            END IF;    

360
        WHEN 25 =>

361
          IF absDivPidiv(31) = '1' THEN

362
            absDivPidiv(31) <= '0';-- Abs von DivPiDiv

363
          END IF;

364
          ceP1 <= '0';-- FL1,FL2 fertig

365
          ceAZmul <= '0';-- a4FP fertig

366
          ctrclk := clkTosub;

367
          cesub1min <= '1';

368
          stateangle <= 26;

369
        WHEN 26 =>-- sub1min := 1 - absdivPIdiv

370
          IF ctrclk > 0 THEN

371
            ctrclk := ctrclk - 1;

372
            cesub1min <= '0';

373
            stateangle <= 26;

374
          ELSE

375
            ceA3FP <= '1';-- startsub1min* AZFP => A3FP 

376
            cesqL <= '1';-- start für FL11^2,Fl2^2,FL12^2

377
            ctrclk := clkTomul;

378
            stateangle <= 27;

379
          END IF;

380
         -- Ab hier Funktionen aus calcP1---------------------------------------

381
        WHEN 27 => -- A3FP :=sub1min * AZFP, a3FP fertig,FL11^2,Fl2^2,FL12^2 

382
          IF ctrclk > 0 THEN

383
            ctrclk := ctrclk - 1;

384
            cesqL <= '0';--FL11^2,Fl2^2,FL12^2=> SqL1,sqL12,sqL2  fertig

385
            ceA3FP <= '0';--a3FP fertig

386
            stateangle <= 27;

387
          ELSE  

388
            IF strImp = '0' THEN

389
              StartON := '0';

390
            END IF;

391
            ceadd1<= '1'; -- startsqL1+sqL12,Fl12*FL1

392
            ctrclk := clkTomul;

393
            stateangle <= 28;

394
          END IF;  

395
        WHEN 28 => -- ausführen  L1+sqL12,Fl12*FL1  

396
          IF ctrclk > 0 THEN

397
            ctrclk := ctrclk - 1;

398
            ceadd1<= '0';--L1+sqL12,Fl12*FL1  fertig

399
            stateangle <= 28;

400
          ELSE  

401
            cesub1 <= '1'; -- start für: sqadd1-sqL2, mul12 * 2

402
            ctrclk := clkTomul;

403
            stateangle <= 29;

404
          END IF;

405
        WHEN 29 => -- ausführen: sqadd1-sqL2, mul12 * 2

406
          IF ctrclk > 0 THEN

407
            ctrclk := ctrclk - 1;

408
            cesub1 <= '0';--sqadd1-sqL2, mul12 * 2 fertig

409
            stateangle <= 29;

410
          ELSE  

411
            cediv1 <= '1'; -- start sqsub1/ mulres1 => ax

412
            ctrclk := clkToDiv;

413
            stateangle <= 30;

414
          END IF;

415
        WHEN 30 =>

416
          IF ctrclk > 0 THEN

417
            ctrclk := ctrclk - 1;

418
            cediv1 <= '0';--sqsub1/ mulres1 => ax fertig

419
            stateangle <= 30;

420
          ELSE    

421
            ceacosFP <= '1'; -- start für atang( Ax)

422
            ctrclk := clkTOacos;

423
            stateangle <= 31;

424
          END IF;  

425
        WHEN 31 =>    

426
          IF ctrclk > 0 THEN

427
            ctrclk := ctrclk - 1;

428
            ceacosFP <= '0';--atang( Ax) fertig

429
            stateangle <= 31;

430
          ELSE      

431
            ceA1FP <= '1'; -- start für acosAx + A12FP

432
            ctrclk := clkTOadd;

433
            stateangle <= 32;

434
          END IF;  

435
        WHEN 32 =>    

436
          IF ctrclk > 0 THEN

437
            ctrclk := ctrclk - 1;

438
            ceA1FP <= '0';--acosAx + A12FP => A+FP fertig ************

439
            stateangle <= 32;

440
          ELSE        

441
            cesubA1 <= '1'; --start für PI/2 - A1FP

442
            ctrclk := clkTOsub;

443
            stateangle <= 33;

444
          END IF;  

445
        WHEN 33 =>      

446
          IF ctrclk > 0 THEN

447
            ctrclk := ctrclk - 1;

448
            cesubA1 <= '0';--PI/2 - A1FP => A1SFP fertig

449
            stateangle <= 33;

450
          ELSE    

451
            cecosFP <= '1'; --Start  für cos(a1FP) & cosFP(a1SFP)

452
            ctrclk := clkTOcos;

453
            stateangle <= 34;

454
          END IF;  

455
        WHEN 34 =>    

456
          IF ctrclk > 0 THEN

457
            ctrclk := ctrclk - 1;

458
            cecosFP <= '0';--cos(a1FP) & cosFP(a1SFP) =>cosA1FP,sinA1FP fertig

459
            stateangle <= 34;

460
          ELSE    

461
            ceZ1W1 <= '1'; --start für sinA1FP*FL1, cosA1FP * FL1

462
            ctrclk := clkTOmul;

463
            stateangle <= 35;

464
          END IF;

465
        WHEN 35 =>    

466
          IF ctrclk > 0 THEN

467
            ctrclk := ctrclk - 1;

468
            ceZ1W1 <= '0';--sinA1FP*FL1=> FZ1,cosA1FP * FL1=>FW1  fertig ******

469
            stateangle <= 35;

470
          ELSE    

471
            cezw12 <= '1'; -- start für FZw-Fz1 & LwFP-FW1

472
            ctrclk := clkTOsub;

473
            stateangle <= 36;

474
          END IF;

475
        WHEN 36 =>    

476
          IF ctrclk > 0 THEN

477
            ctrclk := ctrclk - 1;

478
            cezw12 <= '0';--FZw-Fz1=Z2Z1 & LwFP-FW1=W"W1 fertig

479
            stateangle <= 36;

480
          ELSE    

481
            ceatanZW <= '1';-- start für atan

482
            ctrclk := clkTOatan;

483
            stateangle <= 37;

484
          END IF;

485
        WHEN 37 =>

486
          IF ctrclk > 0 THEN

487
            ctrclk := ctrclk - 1;

488
            ceatanZW <= '0';-- AtanZW fertig

489
            stateangle <= 37;

490
          ELSE  

491
            cesubA2 <= '1'; -- start für atanZw- a1FP

492
            ctrclk := clkTOsub;

493
            stateangle <= 38;

494
          END IF;

495
        WHEN 38 =>

496
          IF ctrclk > 0 THEN

497
            ctrclk := ctrclk - 1;

498
            cesubA2 <= '0';-- A2FP fertig ********************

499
            stateangle <= 38;

500
          ELSE    

501
            FXYZaOK <= '1';--RUNANGLE  bereit

502
            stateangle <= 0;-- alle Achsen fertig

503
          --Berechnung fertg,wartn auf nächste Start

504
          END IF;

505
        WHEN OTHERS  =>

506
          stateangle <= 0;

507
      END CASE;

508
    END IF;-- nrst if/else

509
510
END PROCESS;-- end runangle

511
512
end Behavioral;