New Micros NMIY-0031, Manual

Page: 180 / 188

2 / 2 6 / 2 0 0 2 2 : 0 6 P M 2 / 2 6 / 2 0 0 2 1 8 0
N M I L - 7 0 5 6 S E R V O M O T O R D R I V E R
S E C O N D F O R T H S A M P L E C O D E
: DW@ W@ W@ SWAP ;
: END? STATUS@ 04 AND ;
: WAIT-FOR-MOVE-END BEGIN END? UNTIL ;
( Command set )
: LFIL 1E CMD! ; ( Load filter parameters )
: LTRJ 1F CMD! ; ( Load trajectory )
: STT 01 CMD! ; ( Start motion )
: DFH 02 CMD! ; ( Define home )
: RDRP 0A CMD! DW@ REAL-POS 2! ; ( Read real position )
: UDF 04 CMD! ; ( Update filter )
: MSKI 1C CMD! ; ( Mask interrupts )
: RSTI 1D CMD! ; ( Reset interrupts)
: RDRV 0B CMD! W@ REAL-VEL ! ; ( Read real velocity )
: MOTOR-OFF
LTRJ
0100 W! ( Bit 8 = motor off, output zero drive voltage )
STT
;
: MOTOR-STOP
LTRJ
0200 W! ( Bit 9 = stop abruptly with maximum deceleration )
STT
WAIT-FOR-MOVE-END
;
: MOTOR-STOP-SMOOTHLY
LTRJ
0400 W! ( Bit 10 = sto smoothly, programmed deceleration )
STT
WAIT-FOR-MOVE-END
;
( KP, KI, KD, IL, and DS define the compensating )
( PID filter in the LM629 )
: RESET
0 COM! ( Software reset )
1000 0 DO LOOP ( 1.5 ms wait loop )
RSTI ( Reset interrupts )
0000 W!
;
: FILTER
LFIL
DS @ 100 * 0F OR W!
KP @ W!
KI @ W!
KD @ W!
IL @ W!
UDF
;