( From LM629 document, Commands
( Define Home
( SIP    Set Index Position
( LPEI   Interrupt on Error
( LPES   Stop on Error
( LFIL   Load Filter Parameters
( UDF    Update Filter
( LTRJ   Load Trajectory
( STT    Start Motiong
( RDSTAT Read Status
( RDSIGS Read Signals Register
( RDIP   Read Index Position
( RDDP   Read Desired Position
( RDRP   Read Real Position
( RDDV   Read Desired Velocity
( RDRV   Read Real Velocity
( RDSUM  Read Intergation Sum







HEX

( PWM H BRIDGE DRIVE 20 KHz

    800 CONSTANT PER#
      1 CONSTANT LOW#
PER# 2/ CONSTANT HALF#

FVARIABLE Kp
FVARIABLE Ki
FVARIABLE Kd
FVARIABLE il
FVARIABLE Al
FVARIABLE ACCUM 

FVARIABLE Yp
FVARIABLE Yi
FVARIABLE Yd
FVARIABLE Y

VARIABLE CMDV

: PWMA0LOAD E06 ! E00 C@ DROP 03 E00 ! ;

: INIT
  PER#    E05 ! ( SET MODULO FOR PWM COUNT
  0       E0C ! ( GET RID OF DEADBAND
  0       E0D ! 
  0       E0E !
  100C    E0F ! ( EDGE ALIGN PWA0/1 COMP
  HALF#   PWMA0LOAD
  8000    E03 ! ( TURNS ON OUTPUT LAST THING
  A   S>F Kp F!
  1   S>F Ki F!
  4   S>F Kd F!
  3E8 S>F il F!
  3E8 S>F Al F!
  0   S>F ACCUM F!
  0       CMDV !
;

: FWD PER# PWMA0LOAD ;
: REV LOW# PWMA0LOAD ;
: STOP HALF# PWMA0LOAD ;

: POSPID
  E48 @ E49 @ D>F CMDV @ S>F F+ Kp F@ F* Yp F!
  E49 2@ D>F ACCUM F@ F+ ACCUM F!
    ACCUM F@ Al F@         FMIN ACCUM F!
    ACCUM F@ Al F@ FNEGATE FMAX ACCUM F!
  ACCUM F@ Ki F@ F* Yi F!
    Yi F@ il F@         FMIN Yi F! 
    Yi F@ il F@ FNEGATE FMAX Yi F! 
  E44 @ S>F Kd F@ F* Yd F!
  Yp F@ Yi F@ F+ Yd F@ F+ Y F!
  Y F@ F>D DROP HALF# MIN HALF# NEGATE MAX 
  HALF# + LOW# MAX PER# MIN PWMA0LOAD 
;

 INIT EVERY C350 CYCLES SCHEDULE-RUNS POSPID

: X 
  DECIMAL
  BEGIN
(    CR ( use one of these two lines
    D EMIT ( or the other, CR for scroll, D EMIT for over write one line
    E47 2@ A D.R SPACE ( ENCODER POSITION
    Yp F@ F. ( POS ERROR SIGNAL
    Yi F@ F. ( INT ERROR SIGNAL
    Yd F@ F. ( DIR ERROR SIGNAL
    Y  F@ F. ( TOT ERROR SIGNAL
    ACCUM F@ F. ( ACCUM 
    A SPACES
    ?TERMINAL
  UNTIL
;

( INIT EVERY 2000 CYCLES SCHEDULE-RUNS POSPID ( for very fast but may crash