HEX

: HALFSPEEDCPU F413 FA1 ! 82 F00 ! ; ( DIV BY 2

HALFSPEEDCPU 

C3 CONSTANT PWMCTL# ( DIV BY 8
( VARIABLE LOSTOP 4E2 LOSTOP !
VARIABLE LOSTOP 2E4 LOSTOP !

( VARIABLE HISTOP 9C4 HISTOP !
VARIABLE HISTOP B9A HISTOP !

VARIABLE PER#   61A8 PER# ! ( 61A8 for 50 Hz, 5161 for 60 Hz

VARIABLE SCALE# 3E8 SCALE# !

: MIDWAY HISTOP @ LOSTOP @ + 2/ ;

: 8* 2* 2* 2* ; ( 8 * WITHOUT THE SPEED PENALTY

: SETPWM ( val n -- )
  DUP 6 U< 
  IF
    E00 
  ELSE
    6 -
    E20 
  THEN
  >R R@ 6 + + ! R@ @ DROP PWMCTL# R> !
;

: SETTMRCMP ( val n -- )  8* D00 + 2DUP 1+ ! PER# @ ROT - SWAP ! ;

: RCINIT
  8000   E03 !
  0      E0D !
  0      E0E !
  000E   E0F ! ( CENTER ALIGN FOR DIV BY 2
  PER# @ E05 ! 
  8000   E23 !
  0      E2D !
  0      E2E !
  000E   E2F ! ( CENTER ALIGN FOR DIV BY 2
  PER# @ E25 ! 

  C 0
  DO
    MIDWAY I SETPWM
  LOOP

  C 0 
  DO
    3824  I 8* D06 + ! ( DIVIDE BY 16 SAME AS PWM ASSUME HALFSPEED CPU
    MIDWAY I SETTMRCMP
    0001  I 8* D07 + !
  LOOP
;


( INVERT PATTERN 
( 0111 0000 1111 1100 0000
( RULE: > 6 AND < C OR > 16 AND < 19
( DECIMAL COUNT ON SERVOS
( RULE:: DUP 6 U< NOT OVER C U< AND
( RULE:: DUP 16 U< NOT OVER 19 U< AND
( RULE:: OR

: INVRC 
  DUP 6 U< NOT OVER C U< AND
  OVER DUP 10 U< NOT SWAP 13 U< AND OR NOT
  IF SWAP NEGATE SCALE# @ + SWAP THEN
;

: RC ( COUNT FROM 0 TO SCALE#, THEN 0-11D PWM #
  INVRC
  SWAP 0 MAX SCALE# @ MIN HISTOP @ LOSTOP @ - SCALE# @ */ LOSTOP @ + SWAP 
  0 MAX 1C MIN
  DUP C U<
  IF 
    SETPWM
  ELSE
    C - SETTMRCMP
  THEN
;

( : RC? ( COUNT --- SCALED VALUE
(   0 MAX 1C MIN 
(   DUP C U<
(   IF 
(     DUP 6 U< IF E06 ELSE E20 THEN +
(   ELSE
(     C - 8* D01 +
(   THEN
(   @ LOSTOP @ - SCALE# @ HISTOP @ LOSTOP @ - */
( ;

( : %PWM 64 MIN 0 MAX E05 @ 64 */ E07 ! E00 @ DROP C3 E00 ! ;
( : %PWMX
(   6 MIN 0 MAX >R ( limit channel and put away on return stack
(   64 MIN 0 MAX ( limit percent
(   E05 @ 64 */ ( scale to PER#
(   R> E06 + ! ( add channel # and install time
(   E00 @ DROP C3 E00 ! ( force load
( ;

: -LOOPVAR 
  <BUILDS HERE P, 1- DUP , , DOES>
  P@ DUP @ 0= IF DUP 1 + @ SWAP ! TRUE ELSE 1-! FALSE THEN ;

DECIMAL

( 13 TILT DN TO UP
( 12 PAN RIGHT TO LEFT

(    2,1,0  9,10,11
(    5,4,3  6,7,8
( 17,18,19  14,15,16  

( 900 CONSTANT 900
( 100 CONSTANT 100
600 CONSTANT UP
400 CONSTANT DN

: SL ( SHOULDERS LEFT TRIPOD
  DUP 9 RC
  DUP 3 RC
     16 RC
;

: SR ( SHOULDERS RIGHT TRIPOD
  DUP 6 RC
  DUP 19 RC
      0 RC
;

: KL ( KNEES LEFT TRIPOD
  DUP 10 RC
  DUP 15 RC
      4 RC
;

: KR ( KNEES RIGHT TRIPOD
  DUP 7 RC
  DUP 18 RC
      1 RC
;

: LL ( LEGTIPS LEFT TRIPOD
  DUP 11 RC
  DUP 14 RC
      5 RC
;

: LR ( LEGTIPS RIGHT TRIPOD
  DUP 2 RC
  DUP 8 RC
      17 RC
;

DECIMAL
40 -LOOPVAR DL

MACHINE W
  ON-MACHINE W
    APPEND-STATE W0
    APPEND-STATE W1
    APPEND-STATE W2
    APPEND-STATE W3
    APPEND-STATE W4
    APPEND-STATE W5
    APPEND-STATE W6

IN-STATE W0
CONDITION TRUE
CAUSES RCINIT 
THEN-STATE W1
TO-HAPPEN

IN-STATE W1
CONDITION DL
CAUSES UP SL DN SR UP 12 RC
THEN-STATE W2
TO-HAPPEN

IN-STATE W2
CONDITION DL
CAUSES DN KL
THEN-STATE W3
TO-HAPPEN

IN-STATE W3
CONDITION DL
CAUSES UP KR
THEN-STATE W4
TO-HAPPEN

IN-STATE W4
CONDITION DL
CAUSES DN SL UP SR  DN 12 RC ( S4
THEN-STATE W5
TO-HAPPEN

IN-STATE W5
CONDITION DL
CAUSES DN KR ( S1
THEN-STATE W6
TO-HAPPEN

IN-STATE W6
CONDITION DL
CAUSES UP KL ( S1
THEN-STATE W1
TO-HAPPEN

W0 SET-STATE ( INSTALL
DECIMAL

EVERY 10000 CYCLES SCHEDULE-RUNS W