( Before download this program, Besure to remove the
( comment "\" on the 3rd or 4th line from the beginning 
( of the program below for IsoPod or ServoPod selection

 

( The code is used on the "H3-Walker {hexapod} Robot code" 
( and in the code is the ordering of the servos.

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

( So the left front shoulder, forward and aft, is PWM0, 
( the left front shoulder, up and down, is PWM1, 
( the left knee, up and down, is PWM2, 

( The right front shoulder, forward and aft, is PWM9, 
( the right front shoulder, up and down, is PWM10, 
( the right knee, up and down, is PWM11, etc...

( The PWM run from 0 to 11. After that the timers are numbered 
( TMRA0 is 12 
( TMRA1 is 13 
( TMRA2 is 14 
( TMRA3 is 15 

( TMRB0 is 16 
( TMRB1 is 17 
( TMRB2 is 18 
( TMRB3 is 19

( Not use, but the further timers would be

( TMRC0 is 20 
( TMRC1 is 21 

( TMRD0 is 22 
( TMRD1 is 23 
( TMRD2 is 24 
( TMRD3 is 25

( Note: In the code left front shoulder, forward and aft, is called 
( the shoulder, the left front shoulder, up and down, is called the knee, 
( and the left knee, up and down, is called the leg tip. 

( Remove a comment "\" XXXx IS BASE-ADDR line below 
( for the program to run on your 'Pod(TM)

( -------------------- Program start --------------------------------- )

SCRUB ( clear the flash, wait for few seconds ... 

HEX

\ C00 IS BASE-ADDR EEWORD ( IsoPod & IsoPodX)

\ 1000 IS BASE-ADDR EEWORD ( ServoPod )

 
: HALFSPEEDCPU 
    F413 BASE-ADDR 3A1 + ( FA1 ) ! 
    82 BASE-ADDR 300 + ( F00 ) !  ( DIV BY 2 
;
EEWORD 

HALFSPEEDCPU 

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

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

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

VARIABLE SCALE# 3E8 SCALE# ! EEWORD

: MIDWAY HISTOP @ LOSTOP @ + 2/ ; EEWORD 

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

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

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

: RCINIT
  8000   BASE-ADDR 203 + ( E03 ) !
  0      BASE-ADDR 20D + ( E0D ) !
  0      BASE-ADDR 20E + ( E0E ) !
  000E   BASE-ADDR 20F + ( E0F ) ! ( CENTER ALIGN FOR DIV BY 2
  PER# @ BASE-ADDR 205 + ( E05 ) ! 
  8000   BASE-ADDR 223 + ( E23 ) !
  0      BASE-ADDR 22D + ( E2D ) !
  0      BASE-ADDR 22E + ( E2E ) !
  000E   BASE-ADDR 22F + ( E2F ) ! ( CENTER ALIGN FOR DIV BY 2
  PER# @ BASE-ADDR 225 + ( E25 ) ! 

  C 0
  DO
    MIDWAY I SETPWM
  LOOP

  C 0 
  DO
    ( DIVIDE BY 16 SAME AS PWM ASSUME HALFSPEED CPU
    3824  I 8* BASE-ADDR 106 + ( D06 ) + ! 
    MIDWAY I SETTMRCMP
    0001  I 8* BASE-ADDR 107 + ( D07 ) + !
  LOOP
;
EEWORD


( 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
;
EEWORD

: 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
;
EEWORD

( : 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 
;
EEWORD

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 EEWORD
400 CONSTANT DN EEWORD 

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

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

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

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

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

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

DECIMAL
40 -LOOPVAR DL EEWORD 

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

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

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

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

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

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

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

IN-STATE W6
CONDITION DL
CAUSES UP KL ( S1
THEN-STATE W1
TO-HAPPEN
IN-EE
	
( W0 SET-STATE 
( DECIMAL
( EVERY 10000 CYCLES SCHEDULE-RUNS W

DECIMAL
: STARTUP 
  HALFSPEEDCPU
  W0 SET-STATE
  EVERY 10000 CYCLES SCHEDULE-RUNS W
;
EEWORD 

HEX 3C00 AUTOSTART STARTUP
SAVE-RAM