Isomax FORTH servo control test, proof of concept for the ServoPod board. Multitasking works great.

FORTH Isomax servo test

: TSET ( n -- ) \ set timer pins output period
\ max period of 65535 clock ticks
DUP TA0 PWM-PERIOD DUP TA1 PWM-PERIOD DUP TA2 PWM-PERIOD
DUP TA3 PWM-PERIOD DUP TB0 PWM-PERIOD DUP TB1 PWM-PERIOD
DUP TB2 PWM-PERIOD DUP TB3 PWM-PERIOD DUP TC0 PWM-PERIOD
DUP TC1 PWM-PERIOD DUP TD0 PWM-PERIOD DUP TD1 PWM-PERIOD
TD2 PWM-PERIOD ;

: PSET ( n -- ) \ set pwm pins output period
\ max period of 32767 clock ticks
DUP PWMA0 PWM-PERIOD PWMB0 PWM-PERIOD ;

: HZ>PERIOD ( n1 -- n2 ) \ converts frequency to period
\ period represented by ticks of 2.5 MHz clock
25000 SWAP / 100 * ;

: FSET ( n -- ) \ set pwm frequency
\ min around 38 hz on timer pins (for PWM output)
\ min around 76 hz on pwm pins
DUP 76 >
IF 
  HZ>PERIOD DUP TSET PSET
ELSE
  DUP 38 >
  IF
    32767 PSET HZ>PERIOD TSET
  ELSE
    32767 PSET 65535 TSET DROP
  THEN
THEN ;


50 FSET


: SRV01 ( n -- ) \ input duty cycle
\ 0 to 65536 represents 0% to 100% "on"
TA0 PWM-OUT ;
: SRV02 TA1 PWM-OUT ; : SRV03 TA2 PWM-OUT ; 
: SRV04 TA3 PWM-OUT ; : SRV05 TB0 PWM-OUT ;
: SRV06 TB1 PWM-OUT ; : SRV07 TB2 PWM-OUT ;
: SRV08 TB3 PWM-OUT ; : SRV09 TC0 PWM-OUT ;
: SRV10 TC1 PWM-OUT ; : SRV11 TD0 PWM-OUT ;
: SRV12 TD1 PWM-OUT ; : SRV13 TD2 PWM-OUT ;

VARIABLE XX 0 XX !
VARIABLE YY 0 YY !

MACHINE SET_SRV01
MACHINE SET_SRV02



ON-MACHINE SET_SRV01
APPEND-STATE SRV01_LEFT
APPEND-STATE SRV01_RIGHT

IN-STATE SRV01_LEFT
CONDITION XX @ 0 =
CAUSES 7000 SRV01
THEN-STATE SRV01_RIGHT
TO-HAPPEN

IN-STATE SRV01_RIGHT
CONDITION XX @ -1 =
CAUSES 3000 SRV01
THEN-STATE SRV01_LEFT
TO-HAPPEN



ON-MACHINE SET_SRV02
APPEND-STATE SRV02_LEFT
APPEND-STATE SRV02_RIGHT

IN-STATE SRV02_LEFT
CONDITION YY @ 0 =
CAUSES 7000 SRV02
THEN-STATE SRV02_RIGHT
TO-HAPPEN

IN-STATE SRV02_RIGHT
CONDITION YY @ -1 =
CAUSES 3000 SRV02
THEN-STATE SRV02_LEFT
TO-HAPPEN


SRV01_LEFT SET-STATE INSTALL SET_SRV01
SRV02_LEFT SET-STATE INSTALL SET_SRV02