( Mike an interesting tool would be to output the IK inputs
( on an unsuccessful IK solution FSTRIDE , FREACH , FHEIGHT
( and add to that possibly which joint caused the violation

( Add serial channel equivalents to PS2 controller commands

( Make "stopped" detector and stop walking.
( Change joysticks to be cummulative and push button to stop walking.

( Add 15 degree "tweak" correction per servo in FLASH Table
( Add turn in place
( Ability to set height and stride, as opposed to incrementing.
( Serial control done to allow hanging a PDA or bluetooth wireless from PC.

( LEGS
( EH3R      0
(
(      5        1
(                  
(
(      4        2
(
(           3
(
(           2
(           1
(   17      0     5
(    16          4
(     15        3
(                  
(
(      12       6
(    13          7
(   14      9     8
(           10
(           11



( EH3I   5        0
(
(        4        1
(                  
(        3        2
(
(
( 17,16,15        0, 1, 2
(
( 14,13,12        3, 4, 5
(
( 11,10, 9        6, 7, 8


SCRUB


( This software developed by
( Mike Keesling  www.bio-bot.com
( Randy M. Dumse www.newmicros.com

( CONDITIONAL COMPILE WORDS

( MAKE INLINE/ROUND' TRUE  FOR INLINE COMPILE 
( MAKE INLINE/ROUND' FALSE FOR ROUND  COMPILE
( COMMENT OUT ONE OF THE TWO CONDITIONALS ON LINES BELOW

 FALSE ( THIS ONE IS FOR ROUND
( TRUE  ( THIS ONE IS FOR INLINE

 CONSTANT INLINE/ROUND' EEWORD

: \EH3R ( ROUND CONDITIONAL COMPILE OF CODE CONTROL
  INLINE/ROUND' IF [COMPILE] \ ELSE THEN
; IMMEDIATE EEWORD

: \EH3I ( INLINE CONDITIONAL COMPILE OF CODE CONTROL
  INLINE/ROUND' IF ELSE [COMPILE] \ THEN
; IMMEDIATE EEWORD

HEX
 400 CONSTANT 807OFF EEWORD ( SERVOPOD '807 REGISTER ADDRESS BASE
( 0 CONSTANT 807OFF EEWORD ( ISOPOD '805,'803 REGISTER ADDRESS BASE

DECIMAL
( timing constants
  32767 CONSTANT TMRCNT EEWORD ( ~13.11 mS Timebase for PWM/Timers
   7500 CONSTANT MIDPOS EEWORD (  1.50 mS
     18 CONSTANT SERVO#  EEWORD  ( NUMBER OF SERVOS TO DEFINE

VARIABLE SAVE-GAIT  EEWORD  ( THE GAIT # SELECTED
VARIABLE SAVE-GAITINC. EEWORD  ( THE GAITINC. # SELECTED

VARIABLE SERVO      EEWORD  ( THE CHANNEL OF THE SERVO WE ARE WORKING ON
VARIABLE SROW       EEWORD  ( USED FOR TRACKING NUMBER IF SERVO VARS DEFINED
0 SROW !                    ( COMPILE TIME TOOL DOES NOT NEED INITIALIZATION
                            ( THIS LETS US KNOW HOW DEEP OUT TABLE IS 
			    ( DURING COMPILE TIME SERVO# * SROW = SIZE OF TABLE
  
( SERVOVAR BUILDS A WORD JUST LIKE VARIABLE, WITH SOME NOTABLE EXCEPTIONS
( THE VARIABLE CALLED SERVO ACTS LIKE AN INDEX WITHIN WHATEVER WE DEFINE 
( WITH THE WORD SERVOVAR. FOR EXAMPLE, SO WHEN WE FETCH A VALUE, IT IS 
( IN REVERENCE TO THE SERVO WE ARE WORKING ON.
( : SERVOVAR
( <BUILDS HERE P, SERVO# ALLOT DOES>  COMPILE TIME BEHAVIOR IS TO ALLOCATE 
(                                 ROOM FOR 18 SERVOS  FOR EVERY DATA 
(  P@ SERVO @ +                   ELEMENT WE DEFINE. THIS IS THE RUN TIME 
(                                 BEHAVIOR OF THE WORD. IT TAKES THE 
(                                 VARIABLE SERVO, FETCHES IT'S CONTENTS 
(                                 AND ADDS THAT INDEX TO THE BASE ADDRESS 
(                                 OF THE VARIABLE WE DEFINE.
( ; EEWORD
( LATER, WE CAN BUILD EEPROM EQUIVELENTS USING EE!
( NOW, EVERY TIME WE EXECUTE CHVAR, 
( WE ALLOT SPACE FOR 18 SERVOS FOR THAT VARIABLE
( EVENTUALLY, I WOULD LIKE TO CODE IN THE ABILITY TO PUT THIS IN DATA FLASH
( SINCE THESE VALUES DO NOT CHANGE ONCE YOU HAVE EVERYTHING WORKING PROPERLY

: SERVOVAR
  <BUILDS HERE P, SERVO# 2* ALLOT  SROW 1+! DOES> ( ADDED SROW TO TRACK ROWS
  P@ SERVO @ 2* +
; EEWORD

  SERVOVAR CHANNEL   EEWORD   ( THE "HARD" CHANNEL THAT SERVO NUMBER USES
  SERVOVAR MINTIME   EEWORD   ( THE MIN PULSE THAT SERVO CAN ACCEPT
  SERVOVAR MAXTIME   EEWORD   ( THE MAX PULSE THAT SERVO CAN ACCEPT
  SERVOVAR MINSPAN   EEWORD   ( THE MIN USER UNITS THAT SERVO REPRESENTS +/-
  SERVOVAR MAXSPAN   EEWORD   ( THE MAX USER UNITS THAT SERVO REPRESENTS +/-
  SERVOVAR COEFF     EEWORD   ( THE CONVERSION COEFFICIENT, CALCULATE ONCE
  SERVOVAR FOFFSET   EEWORD   ( THE CONVERSION OFFSET, CALCULATE ONCE
  SERVOVAR MAXMINSPAN EEWORD  ( THE MIN USER UNITS THAT SERVO REPRESENTS +/-
  SERVOVAR MINMAXSPAN EEWORD  ( THE MAX USER UNITS THAT SERVO REPRESENTS +/-
  SERVOVAR MECHADJ-D EEWORD   ( THE CORR. FACTOR FOR MISALGN TYP +/-15DEG
  SERVOVAR MECHADJ-T EEWORD   ( THE CORR. FACTOR FOR MISALGN TYP +/- TIME 
( SERVOVAR HOMEPOS   EEWORD   ( THE HOME POSITION
( SERVOVAR FEEDBK1   EEWORD   ( PROPOSED SENSOR #1
( SERVOVAR FEEDBK2   EEWORD   ( PROPOSED SENSOR #2
( SERVOVAR FEEDBK3   EEWORD   ( PROPOSED SENSOR #3
( IF WE KEEP THIS ALL TOGETHER SROW WORKS
SROW @ CONSTANT SROW# EEWORD  ( THIS KEEPS THINGS FLEXABLE, CODE WILL NOT BREAK
                              ( IF YOU DO ALL YOUR SERVOVAR'S CONTIGUOUSLY
                              ( AND BEFORE THIS POINT. YOU CAN USE TO "DIVE"
                              ( INTO THE TABLE, BY ALWAYS KNOWING HOW "DEEP" IT
                              ( IS. 


DECIMAL
: F>S 32767.0E0 FMIN -32768.0E0 FMAX F>D DROP ; EEWORD  ( MAKE FLOAT SINGLE

( THIS IS A SCALING COEFFICIENT. STORES THE COEFF IN THE TABLE
: MAKE-COEFF
  MAXTIME @ MINTIME @ - S>F
  MAXSPAN @ MINSPAN @ - S>F
  F/ COEFF F!
; EEWORD

: MAKE-OFFSET
  MAXTIME @ S>F MAXSPAN @ S>F COEFF F@ F* F- FOFFSET F!
; EEWORD

( THIS IS USED TO FILL THE SERVO VALUE TABLE
( IT EXPECTS IT'S INPUT IN THE FORM OF:
( FIRST, STORE THE # OF THE SERVO YOU WANT TO MAKE AN ENTRY FOR IN
( THE VARIABLE CALLED SERVO
( THEN THE STACK IS LIKE THIS
( PLYSICAL CHANNEL OF THE DEVICE,
( THEN THE MINIMUM AND MAXIMUM TIMING FOR THE SERVO
( THEN THE JOINT ANGLES THESE REPRESENT. THESE CAN BE PRETTY MUCH WHAT YOU
( WANT. I USE A SCALE OF 10:1 1 UNIT IS A TENTH OF A DEGREE, 900 IS 90 DEG

: MAKE-SERVO 
  MAXSPAN ! MINSPAN !

  MINSPAN @ MAXSPAN @ MIN MINMAXSPAN !
  MINSPAN @ MAXSPAN @ MAX MAXMINSPAN !

  MAXTIME ! MINTIME ! CHANNEL !
  MAKE-COEFF
  MAKE-OFFSET

  0 MECHADJ-D ! 
  0 MECHADJ-T ! 
; EEWORD

( JUST TO BE REDUNDANT...
( THE NUMBER STORED IN THE VARIABLESERVO IS THE LOGICAL CHANNEL 
( THE FIRST NUMBER IN THE WE FEED INTO MAKE-SERVO
( IS THE PHYSICAL CHANNEL ASSOCIATED AS DEFINED IN RC2
( 
( PWMA0-5 = CH0  TO 5
( PWMB0-5 = CH6  TO 11
( TA0-3   = CH12 TO 15
( TB0-4   = CH16 TO 19
( TMIN IS THE MINIMUM PULSEWIDTH SETTING FOR A GIVEN ANGLE
( MINSPAN IS THE ANGLE ASSOCIATED WITH THAT PULSEWIDTH
( TMAX IS THE MAXIMUM PULSEWIDTH SETTING FOR A GIVEN ANGLE
( MAXSPAN IS THE ANGLE ASSOCIATED WITH THAT PULSEWIDTH
( NOTE I PREFER ANGLE IN THE FORM OF 900 = 90 DEGREES. 
( THIS CAN BE OTHER UNITS AS WELL, AS LONG AS THE RELATIONSHIP
( BETWEEN IN AND OUT IS LINEAR
( CH TMIN TMAX SPNMAX SPNMIN
( NEED TO INCORPORATE THE HOME POSITION
( I DID NOT PAY A HUGE ANOUNT OF ATTENTION TO THESE VALUES. THESE ARE
( A FIRST STAB. YOU CAULD HARD MOUNT THE JOINTS AND ADJUST WITH 
( HERE.


( WORD TO MAKE ALL OF THE SERVO TABLE ENTRIES
: MAKE-SERV-TABLE
  ( SET HIP-L/R JOINTS:
  0 SERVO ! 
  0 1926 5494 -700 700 MAKE-SERVO

  3 SERVO ! 
  3 1926 5494 -700 700 MAKE-SERVO

  6 SERVO ! 
  6 1926 5494 -700 700 MAKE-SERVO

  9 SERVO ! 
  9 1926 5494 -700 700 MAKE-SERVO

  12 SERVO ! 
  12 1926 5494 -700 700 MAKE-SERVO

  15 SERVO ! 
  15 1926 5494 -700 700 MAKE-SERVO
  ( END HIP JOINTS

  ( START OF HIP-U/D RAISE/FALL
  1 SERVO ! 
  1 2594 4602 -400 400 MAKE-SERVO

  4 SERVO ! 
  4 2594 4602 -400 400 MAKE-SERVO

  7 SERVO ! 
  7 2594 4602 -400 400 MAKE-SERVO

  10 SERVO ! 
  \EH3R 10 2594 4602 -400 400 MAKE-SERVO
  \EH3I 10 4853 2845 -400 400 MAKE-SERVO

  13 SERVO ! 
  \EH3R 13 2594 4602 -400 400 MAKE-SERVO
  \EH3I 13 4853 2845 -400 400 MAKE-SERVO

  16 SERVO ! 
  \EH3R 16 2594 4602 -400 400 MAKE-SERVO
  \EH3I 16 4853 2845 -400 400 MAKE-SERVO
  ( END OF HIP-U/D RAISE/FALL

  ( START OF KNEE
  2 SERVO ! 
  2 2594 4602 -500 -1300 MAKE-SERVO

  5 SERVO ! 
  5 2594 4602 -500 -1300 MAKE-SERVO

  8 SERVO ! 
  8 2594 4602 -500 -1300 MAKE-SERVO

  11 SERVO ! 
  \EH3R 11 2594 4602 -500 -1300 MAKE-SERVO
  \EH3I 11 4853 2845 -500 -1300 MAKE-SERVO

  14 SERVO ! 
  \EH3R 14 2594 4602 -500 -1300 MAKE-SERVO
  \EH3I 14 4853 2845 -500 -1300 MAKE-SERVO

  17 SERVO ! 
  \EH3R 17 2594 4602 -500 -1300 MAKE-SERVO
  \EH3I 17 4853 2845 -500 -1300 MAKE-SERVO
  ( END OF KNEE
; EEWORD


( DUMP THE CONTENTS OF THE SERVO TABLE 
( NOTE THE USE OF SROW# THIS ALLOWS ME TO BUILD THE TABLE LARGER
( BUT DUMP-SERVO STILL WORKS
: DUMP-SERVO
  CR SROW# 0
  DO CHANNEL I 2* SERVO# * + DUP @ . 2@ D. CR LOOP 
; EEWORD

( END OF SERVO STRUCTURE WORDS


( ************************** MAJOR CODE SECTION
( SINCE IT RELIES ON SERVO INFORMATION, THIS SECTION IS HERE
( SINCE THE JOINTS RELY ON SERVOS, JOINTS WILL FOLLOW
( THIS CODE BUILDS THE WORDS TO CONTROL THE HARDWARE REGISTERS 
( IT INITIALIZES VAST SWATHS OF REGISTERS INDESCRIMINANTLY. 
( FOR THIS REASON, YOU WOULD NEED TO MODIFY THIS SECTION IF YOU WANTED TO 
( OPEN UP BANKS OF HARDWARE LIKE ONE OF THE PWM BANKS, OR THE QUADRATURE 
( DECODERS

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

HEX
( THIS WORD TAKES A SERVO ADDRESS IN THE RANGE OF 0 TO 11, AND PUTS IT 
( INTO THE APPROPRIATE HARDWARE REGISTERS FOR PWM OUTPUT
: SETPWM ( val n -- )   ( FILTERS TO APPROPRIATE SERVO NUMBER 
  DUP 6 U< 
  IF
    [ E00 807OFF + ] LITERAL
  ELSE
    6 -
    [ E20 807OFF + ] LITERAL 
  THEN
  >R R@ 6 + + ! R@ @ DROP C3 R> ! ( C3 # IS DIV BY 8 PWM CONTROL
; EEWORD

HEX
7FFF CONSTANT  PER# EEWORD
: SETTMRCMP ( val n -- )
  8* [ D00 807OFF + ] LITERAL + ( GET ADDRESS OF TIMER
  ( NEED TO MAKE TWO STORES HERE
  ( ONE THE COUNT OF THE TIME OF THE PULSE
  ( THE OTHER THE DIFFERENCE IN REMAINING TIME UNTIL NEXT PULSE
  2DUP ( DUP COUNT AND ADDRESS, 
  1+ ! ( INC ADDRESS TO MAKE FOR CMP2, STORE COUNT THERE
(  >R ( MOVE REMAINING ADDRESS COPY TO R.S.
(  PER# SWAP - ( SUB COUNT FROM TIME BASE, DIFFERENCE BETWEEN THE TWO
(  R> ( BRING BACK REMAINING ADDRESS COPY FROM R.S.
(  !  ( THEN STORE AT CMP1
  PER# ROT - SWAP ! ( SUB COUNT FROM TIME BASE, THEN STORE AT CMP1
; EEWORD

DECIMAL
( FILTER INPUT TO STAY WITHIN RANGE OF THE USER LIMITS
( THIS ALLOWS THE POLARITY OF THE USER LIMITS TO BE REVERSED
: LMTANG ( TURN ON RED LED IF BEYOND RANGE
  DUP
  MAXMINSPAN @ MIN 
  MINMAXSPAN @ MAX 

  SWAP OVER = NOT REDLED SET
; EEWORD


( THIS WORD CONSTRUCTS THE PROPER REGISTER VALUES GIVEN YOUR SERVO TABLE 
( ENTRIES AND THE COMMANDED USER INPUT
: MAKEPULSE 
  LMTANG 
  S>F COEFF F@ F* ( Compute for slope conversion
  FOFFSET F@ F+   ( Offset for intercept
  F>S             ( Output as integer
  MECHADJ-T @ +
; EEWORD


( USE THIS WORD BY STORING THE "SOFT" CHANNEL NUMBER IN SERVO, THEN 
( PUTTING THE POSITION IN USER UNITS ON THE STACK AND CALLING IT
( 0 SERVO ! 900 RC2  WOULD MOVE SERVO 0 TO 90 DEGREES
( 
: RC2 ( COUNT FROM 0 TO SCALE#, THEN 0-11D PWM #
  CHANNEL @ SWAP  
  MAKEPULSE
  ( DUP U.
  SWAP 
  0 MAX 18 MIN
  DUP 12 U<
  IF 
    ( ." PWM "
    SETPWM
  ELSE
    ( ." TIMER "
    12 - SETTMRCMP
  THEN
; EEWORD



DECIMAL
: RCINITNEW
  TMRCNT PWMA0 PWM-PERIOD
  MIDPOS PWMA0 PWM-OUT ( Servo  0 Leg 0 Joint 1
  MIDPOS PWMA1 PWM-OUT ( Servo  1 Leg 0 Joint 2
  MIDPOS PWMA2 PWM-OUT ( Servo  2 Leg 0 Joint 3
  MIDPOS PWMA3 PWM-OUT ( Servo  3 Leg 1 Joint 1
  MIDPOS PWMA4 PWM-OUT ( Servo  4 Leg 1 Joint 2
  MIDPOS PWMA5 PWM-OUT ( Servo  5 Leg 1 Joint 3

  TMRCNT PWMB0 PWM-PERIOD
  MIDPOS PWMB0 PWM-OUT ( Servo  6 Leg 2 Joint 1
  MIDPOS PWMB1 PWM-OUT ( Servo  7 Leg 2 Joint 2
  MIDPOS PWMB2 PWM-OUT ( Servo  8 Leg 2 Joint 3
  MIDPOS PWMB3 PWM-OUT ( Servo  9 Leg 3 Joint 1
  MIDPOS PWMB4 PWM-OUT ( Servo 10 Leg 3 Joint 2
  MIDPOS PWMB5 PWM-OUT ( Servo 11 Leg 3 Joint 3

  TMRCNT TA0 PWM-PERIOD
  MIDPOS TA0 PWM-OUT   ( Servo 12 Leg 4 Joint 1
  TMRCNT TA1 PWM-PERIOD
  MIDPOS TA1 PWM-OUT   ( Servo 13 Leg 4 Joint 2
  TMRCNT TA2 PWM-PERIOD
  MIDPOS TA2 PWM-OUT   ( Servo 14 Leg 4 Joint 3
  TMRCNT TA3 PWM-PERIOD
  MIDPOS TA3 PWM-OUT   ( Servo 15 Leg 5 Joint 1

  TMRCNT TB0 PWM-PERIOD
  MIDPOS TB0 PWM-OUT   ( Servo 16 Leg 5 Joint 2
  TMRCNT TB1 PWM-PERIOD
  MIDPOS TB1 PWM-OUT   ( Servo 17 Leg 5 Joint 3
  
  TMRCNT TB2 PWM-PERIOD
  MIDPOS TB2 PWM-OUT   ( Servo 18
  TMRCNT TB3 PWM-PERIOD
  MIDPOS TB3 PWM-OUT   ( Servo 19
; EEWORD

( ***************************** MAJOR PROGRAM SECTION


( HERE WE BUILD SOME FLOATING POINT TABLES
( NOTE THE LEG# 2* ALLOT. FLOATS USE 2 MEMORY LOCATIONS SO WE HAVE TO USE
( 2* THE NUMBER OF MEMORY ENTRIES.
( THESE FLOATS ARE THE BASIC CONVEYANCE OF DATA BETWEEN THE GAIT GENERATOR,
( AND THE INVERSE KINEMATICS SOLVER

6  CONSTANT LEG#    EEWORD    ( NUMBER OF LEGS FOR THE INVERSE KINEMATICS
   VARIABLE LEG     EEWORD    ( THE NUMBER OF THE LEG WE ARE OPERATING ON.
   VARIABLE LROW    EEWORD    ( USED TO TRACK NUMBER OF LEG VARIABLES DEFINED
0 LROW !                      ( COMPILE TIME TOOL DOES NOT NEED INITIALIZATION

: FLEGVAR-IK
  <BUILDS HERE P, LEG# 2* ALLOT LROW 1+! DOES> ( ADDED SROW TO TRACK ROWS MADE
  P@ LEG @ 2* +               ( THIS TABLE APPLIES TO A WHOLE LEG, SO WE USE LEG
; EEWORD

( ALSO NOTE IT IS LEG @ 2* +  SINCE FLOATS USE 2 LOCATIONS 


( INPUT VARIABLES
FLEGVAR-IK   FTIPX     EEWORD  ( FINAL TIP DESTINATION X
FLEGVAR-IK   FTIPY     EEWORD  ( FINAL TIP DESTINATION Y
FLEGVAR-IK   FTIPZ     EEWORD  ( FINAL TIP DESTINATION Z
( OUTPUT
FLEGVAR-IK   FHIP-U/D  EEWORD  ( ANGLE OF JOINT
FLEGVAR-IK   FHIP-R/L  EEWORD  ( ANGLE OF JOINT
FLEGVAR-IK   FKNEE     EEWORD  ( ANGLE OF JOINT
FLEGVAR-IK   FANKLE    EEWORD  ( ANGLE OF JOINT ( NOT IMPLEMENTED
FLEGVAR-IK   FRADIUS   EEWORD  ( POSITION OF TIP ON ROTATED PLANE
FLEGVAR-IK   FHEIGHT   EEWORD  ( HEIGHT OF TIP ON ROTATED PLANE
FLEGVAR-IK   FTHETA    EEWORD  ( THETA ON A LEG PER LEG BASIS ( NOW DEGREES
FLEGVAR-IK   FRSLTNT   EEWORD  ( THETA+FVECTOR ON A LEG PER LEG BASIS 
FLEGVAR-IK   FSTOKE    EEWORD  ( THETA+FVECTOR EFFECT ON STRIDE


LROW @ CONSTANT LROW# EEWORD  ( THIS KEEPS THINGS FLEXABLE, CODE WILL NOT BREAK
                              ( IF YOU DO ALL YOUR SERVOVAR'S CONTIGUOUSLY
                              ( AND BEFOR THIS POINT

( HERE WE BUILD A TABLE TO TRACK THE POSITION IN THE GAIT CYCLE 0F EACH LEG

   VARIABLE GROW    EEWORD    ( USED TO TRACK NUMBER OF GAIT VARIABLES DEFINED
                              ( GAIT ROW
0 GROW ! 
: GAITVAR
  <BUILDS HERE P, LEG# ALLOT  GROW 1+! DOES> ( ADDED GROW TO TRACK ROWS 
  P@ LEG @ +                  ( THIS TABLE APPLIES TO A WHOLE LEG, SO WE USE LEG
; EEWORD

GAITVAR    GAIT-INDEX  EEWORD     ( CURRENT POSITION IN GAIT CYCLE
                                  ( THIS WILL NEED TO BE MADE INTO PART
                                  ( OF A TABLE THAT STORES ALL LEG PARAMETERS
GAITVAR    GAIT-X      EEWORD     ( X POSITION WITHIN THE CYCLE
GAITVAR    GAIT-Y      EEWORD     ( Y POSITION WITHIN THE CYCLE

GROW @ CONSTANT GROW# EEWORD ( THIS KEEPS THINGS FLEXABLE, CODE WILL NOT BREAK

DECIMAL
VARIABLE GAITINC.     EEWORD
60 CONSTANT GAITSTEPS EEWORD

CREATE X-TABLE
  0 P,  16 P,  33 P,  50 P,  66 P,  83 P,  80 P,  76 P,  73 P,  70 P, 
 66 P,  63 P,  60 P,  56 P,  53 P,  50 P,  46 P,  43 P,  40 P,  36 P, 
 33 P,  30 P,  26 P,  23 P,  20 P,  16 P,  13 P,  10 P,   6 P,   3 P, 
  0 P,  -4 P,  -7 P, -10 P, -14 P, -17 P, -20 P, -24 P, -27 P, -30 P, 
-34 P, -37 P, -40 P, -44 P, -47 P, -50 P, -54 P, -57 P, -60 P, -64 P, 
-67 P, -70 P, -74 P, -77 P, -80 P, -84 P, -67 P, -50 P, -34 P, -17 P, 
 EEWORD 

CREATE Y-TABLE
 50 P,  41 P,  33 P,  25 P,  16 P,   8 P,   0 P,   0 P,    0 P,  0 P, 
  0 P,   0 P,   0 P,   0 P,   0 P,   0 P,   0 P,   0 P,    0 P,  0 P, 
  0 P,   0 P,   0 P,   0 P,   0 P,   0 P,   0 P,   0 P,    0 P,  0 P, 
  0 P,   0 P,   0 P,   0 P,   0 P,   0 P,   0 P,   0 P,    0 P,  0 P, 
  0 P,   0 P,   0 P,   0 P,   0 P,   0 P,   0 P,   0 P,    0 P,  0 P, 
  0 P,   0 P,   0 P,   0 P,   0 P,   8 P,  16 P,  25 P,   33 P, 41 P, 
 EEWORD

: 0LEG 0 LEG ! ; EEWORD
: +LEG LEG 1+! ; EEWORD
: GAIT! GAIT-INDEX ! ; EEWORD

: STAND                 ( SETS UP FOR NO GAIT, WITH 
  0LEG 30 GAIT!          ( ALTERNATE LEGS 180 DEGREES OUT 
  +LEG 30 GAIT!          ( PHASE WITH EACH OTHER
  +LEG 30 GAIT!
  +LEG 30 GAIT!
  +LEG 30 GAIT!
  +LEG 30 GAIT!
  0 GAITINC. !
; EEWORD

: TRIPOD                ( SETS UP FOR TRIPOD GAIT, WITH 
  0LEG  0 GAIT!         ( ALTERNATE LEGS 180 DEGREES OUT 
  +LEG 30 GAIT!         ( PHASE WITH EACH OTHER
  +LEG  0 GAIT!
  +LEG 30 GAIT!
  +LEG  0 GAIT!
  +LEG 30 GAIT!
; EEWORD

: 1A                    ( SETS UP FOR ROUND GATE
  0LEG  0 GAIT!
  +LEG 10 GAIT!
  +LEG 20 GAIT!
  +LEG 30 GAIT!
  +LEG 40 GAIT!
  +LEG 50 GAIT!
; EEWORD

: 1B                    ( SETS UP FOR SQUARE GAIT
  0LEG  0 GAIT!
  +LEG 20 GAIT!
  +LEG 40 GAIT!
  +LEG 10 GAIT!
  +LEG 30 GAIT!
  +LEG 50 GAIT!
; EEWORD

: 2A                    ( SETS UP FOR "X' GATE
  0LEG  0 GAIT!
  +LEG 20 GAIT!
  +LEG 40 GAIT!
  +LEG  0 GAIT!
  +LEG 20 GAIT!
  +LEG 40 GAIT!
; EEWORD


: GAIT3A3 TRIPOD 3 DUP GAITINC. ! SAVE-GAITINC. ! ; EEWORD
: GAIT1A3 1A  3 DUP GAITINC. ! SAVE-GAITINC. ! ; EEWORD
: GAIT1B3 1B  3 DUP GAITINC. ! SAVE-GAITINC. ! ; EEWORD
: GAIT2A3 2A  3 DUP GAITINC. ! SAVE-GAITINC. ! ; EEWORD
: GAIT3A2 TRIPOD 2 DUP GAITINC. ! SAVE-GAITINC. ! ; EEWORD
: GAIT1A2 1A  2 DUP GAITINC. ! SAVE-GAITINC. ! ; EEWORD
: GAIT1B2 1B  2 DUP GAITINC. ! SAVE-GAITINC. ! ; EEWORD
: GAIT2A2 2A  2 DUP GAITINC. ! SAVE-GAITINC. ! ; EEWORD

:CASE (INIT-GAIT)
  STAND
  GAIT3A3
  GAIT1A3
  GAIT1B3
  GAIT2A3
  GAIT3A2
  GAIT1A2
  GAIT1B2
  GAIT2A2
; EEWORD

: INIT-GAIT
  0 MAX 8 MIN
  DUP SAVE-GAIT !
  (INIT-GAIT)
; EEWORD

( WEEEEEEEEEEEE

( GOING TO DO A GAIT TABLE FOR THE PATTERN OF THE FEET
( KEEPING A FLAT TRAJECTORY ON THE GROUND
( THESE TABLES DESCRIBE .01 OF AN INCH, I.E. 80 * .01 - .8 INCHES
( THE GAIT IS SUPPOSED TO PRESENT EVEN DISTANCE FOR EVERY POTITION
( MOSTLY TO CREATE SMOOTH MOTION ON THE GROUND
( TOP DEAD CENTER IS THE FIRST ENTRY, 0,46. THE FIRST GROUND CONTACT IS 
( AT 80,0 AND THE LAST GROUND CONTACT IS AT -80,0
( THIS IS MEANT TO PRODUCE A GAIT THAT FITS WITHIN +/- 1 INCH GROUND TRAVEL
( BUT CAN BE SCALED UP OR DOWN FOR DIFFERENT STRIDE LENGTHS
( IN MY INITIAL STAB, I INTEND ONLY TO VARY THE LENGTH OF THE STEPS, WHILE
( HAVING ALL GAITS RUNNING AT THE SAME PERIOD
( THIS GAIT IS ON A PLANE THAT RUNS THROUGH THE LEG WHEN THE HIP IS AT 0 DEG.
( THE CO-ORDINATES WILL BE ROTATED TO TRANSCRIBE A PLANE THAT COINCIDES WITH 
( THE DESIRED MOTION 


( SOME FLOATS AND TRIGS
( THIS IS TO ROTATE AND SCALE THE GAIT PATTERN
( I AM GOING TO REFERENCE THE CENTERLINE OF LEG 0 AS THE BODY'S CENTER
( TO GIVE AN ABSOLUTE FRAME OF REFERENCE
DECIMAL 
PI     2.0E0 F* FCONSTANT 2PI EEWORD ( 360 DEGREES EXPRESSED IN RADIANS
( PI     2.0E0 F* FCONSTANT F360# EEWORD ( 360 DEGREES EXPRESSED IN RADIANS
( PI              FCONSTANT F180# EEWORD ( 180 DEGREES 
( PI     2.0E0 F/ FCONSTANT F90#  EEWORD ( 90  DEGREES
( PI     2.0E0 F/ FNEGATE FCONSTANT F-90# EEWORD ( -90 DEG
( F360#  6.0E0 F/ FCONSTANT F60#  EEWORD ( 60  DEGREES EXPRESSED IN RADIANS
( PI     4.0E0 F/ FCONSTANT F45#  EEWORD ( 45  DEGREES
 
360.0E0 2PI   F/ FCONSTANT DR  EEWORD  ( DEGREES TO RADIANS CONVERSION CON
( VARIABLE         IK-ERROR      EEWORD  ( UNDEFINED RIGHT NOW, WILL DO LATER

( FVARIABLE ARC-X   EEWORD  ( FOR TRANSLATING ARCS, THIS IS THE CENTER OF X
( FVARIABLE ARC-Y   EEWORD  ( THE Y POINT THAT IS THE CENTER OF THE ARC

FVARIABLE FT1     EEWORD  ( USED TO SET DISTANCE OF CENTER OF GAIT 

FVARIABLE FSTRIDE EEWORD  ( HOW LONG OR SHORT TO MAKE GAIT
FVARIABLE FREACH  EEWORD  ( HOW HIGH OR LOW   TO MAKE GAIT STEPS
FVARIABLE FGAIT-X EEWORD  ( THE DEPTH  OF GAIT AFTER IT IS STRETCHED 
FVARIABLE FGAIT-Y EEWORD  ( THE HEIGHT OF GAIT AFTER IT IS STRETCHED
FVARIABLE FVECTOR EEWORD  ( THE VECTOR OF THE BODY
FVARIABLE FROTATE EEWORD  ( THE VECTOR OF ROTATION FOR THE BODY


( SAME LEGS, NO TABLE NEEDED
( IF WE BUILT WITH DIFFERENT SIZED LEGS, THEN WE COULD ADD A TABLE TO CONTAIN 
( THESE AS VARIABLES

1.20E0  FCONSTANT FHIP-H-OFFSET# EEWORD ( HORIZ OFFSET OF HIP R/F FROM HIP L/R
2.75E0  FCONSTANT FTHIGH-LENGTH# EEWORD ( LENGTH OF THIGH
4.10E0  FCONSTANT FCALF-LENGTH#  EEWORD ( LENGTH OF CALF
0.29E0  FCONSTANT FTIP-RADIUS#   EEWORD ( RADIUS OF THE RIP

FVARIABLE FRIDE-HEIGHT          EEWORD   ( HEIGHT OFF THE GROUND
FVARIABLE HIP-TIP-LENGTH        EEWORD   ( THE LENGTH OF THE HIP U/D TO THE TIP
                                         ( IF LONGER THAN CALF+THIGH THEN THERE 
                                         ( IS NO SOLUTION. NOT IMPLEMENTED YET
FVARIABLE HIP-TIP-LENGTH^2      EEWORD   ( SQUARED VALUE

FVARIABLE A1                    EEWORD   ( INTERMEDIATE SOLUTION
FVARIABLE A2                    EEWORD   ( INTERMEDIATE SOLUTION


( FLOAT TOOLS TO FOLLOW
: R>D DR F* ; EEWORD                                    ( RADIANS TO DEGREES 
: D>R [ 1E0 DR F/ ] FLITERAL F* ; EEWORD                ( DEGREES TO RADIANS 

: F**2 FDUP F* ; EEWORD
: FVECSUM F**2 FSWAP F**2 F+ FSQRT ; EEWORD

( FLOAT TRIG WORDS
: ANGCHK
  FDUP PI FNEGATE F<
  FDUP PI F+ F0= OR IF 2PI F+ THEN 
  PI FOVER F<        IF 2PI F- THEN 
; EEWORD


: FATAN2NEW ( F: Y \ X ... F in radians between -pi and +pi 
  FDUP F0=  ( x=0? 
  IF  FDROP  FDUP F0= 0=  ( y=0? 
    IF
      F0< PI  
      IF
        FNEGATE
      THEN
      F2/
    THEN
    EXIT
  THEN
  FOVER F0<  ( y<0)
  FDUP  F0<  ( x<0)
  F/ FATAN
  IF ( x<0)
    PI
    IF ( y<0)
      F-
    ELSE
      F+
    THEN
  ELSE
    DROP
  THEN 
; EEWORD


: FASIN ( F:r1 -- r2 )
  1E0 FOVER F**2 ( SINr1 1 SINr1^2
  F- FSQRT ( SINr1 1-SINr1^2^.5
  FATAN2NEW
; EEWORD


: FACOS ( F:r1 -- r2 )
  1E0 FOVER F**2 ( COSr1 1 COSr1^2
  F- FSQRT ( COSr1 1-COSr1^2^.5
  FSWAP
  FATAN2NEW
; EEWORD


( ************************************************
( ************************************************
( ************************************************
( ************************************************

( LOAD THE X-Y VALUES FOR CURRENT LEG AND GAIT POSITION
: LOAD-GAIT-X-Y ( MAKE SURE YOU STORE LEG NUMBER INTO LEG
  X-TABLE GAIT-INDEX @ + P@ GAIT-X !
  Y-TABLE GAIT-INDEX @ + P@ GAIT-Y !
; EEWORD

( STEP THROUGH THE GAIT TABLE
: INCREMENT-GAIT 
(  GAIT-INDEX @ GAITINC. @ + GAITSTEPS MOD  GAIT-INDEX !
  GAITINC. @ GAIT-INDEX +!
  GAIT-INDEX @ GAITSTEPS - 0< NOT
  IF
    GAIT-INDEX @ GAITSTEPS - GAIT-INDEX !
  THEN
; EEWORD

: FIGURE-ROTATION
  FSTRIDE F@ F**2                 ( STRETCH GAIT STRIDE
  FROTATE F@ F**2                 ( STRETCH GAIT STRIDE
  F+
  FSTRIDE F@                      ( STRETCH GAIT STRIDE
  FROTATE F@ F*                   ( STRETCH GAIT STRIDE
  FTHETA  F@ 
  FVECTOR F@ F-
  FCOS F*
  F-
  FSQRT
  FSTOKE  F!                      ( STRETCH GAIT FOR ROTATION TOO
  ( NEED TO COMPUTE RESULTANT TOO

; EEWORD

( SCALE THE GAIT IN LENGTH AND HEIGHT
: STRETCH-GAIT  ( MAKE SURE YOU STORE LEG NUMBER INTO LEG
  LOAD-GAIT-X-Y                     ( PUT INDEXED TABLE ELEMENT INTO GAIT VARS
  GAIT-X @ S>F 100.0E0 F/            ( SCALE TO INCHES
  FSTRIDE F@ F*                     ( STRETCH GAIT STRIDE
(  FSTOKE F@ F*                      ( STRETCH GAIT FOR ROTATION TOO
  FGAIT-X F!                        ( STORE 
  GAIT-Y @ S>F 100.0E0 F/            ( SCALE TO INCHES
  FREACH  F@ F*                     ( STRETCH GAIT REACH
  FGAIT-Y F!                        ( STORE  TEMPORY ATTEMPT TO FIX
; EEWORD


( ADD VECTOR OF DESIRED MOTION TO VECTOR OFFSETS FOR EACH LEG.
( THIS MAKES THE LEGS ALL ACT ON THE SAME VECTOR
: MAKE-THETA                        ( MAKE THETA ACCORDING TO LEG NUMBER
  FVECTOR F@ 
  FTHETA  F@ F+  D>R                ( LEAVE ON STACK
; EEWORD  


( THIS TAKES THE FLAT GAITS, AND ROTATES THEM THEN FEEDS TO IK INPUT VARS
: MAKE-X-Y-Z
  
  MAKE-THETA FDUP                   ( ONE COPY FOR SIN, ONE FOR COS

  FSIN FGAIT-X F@ F*                ( SCALE X TO SIN OF ANGLE
  FT1 F@ F+ FTIPX  F!               ( STORE IN FLOAT TIP VARIABLE
                                    ( REMEMBER THAT THE TIP MOVES 
                                    ( IN X-Y ACCORDING TO THETA

  FCOS FGAIT-X F@ F*                ( SCALE X TO SIN OF ANGLE
  FTIPY F!                          ( STORE IN FLOAT TIP VARIABLE
                                    ( REMEMBER THAT THE TIP MOVES 
                                    ( IN X-Y ACCORDING TO THETA


  FGAIT-Y F@  FTIPZ  F!             ( TRANSLATE GAIT HEIGHT TO TIP HEIGHT
  
; EEWORD

( DO ALL GAIT WORDS 
: MAKE-ALL-GAIT                     ( PUT ALL GAIT WORDS INTO ONE WORD
(  FIGURE-ROTATION
  STRETCH-GAIT
  MAKE-X-Y-Z
; EEWORD


( ******************* BEGINNING OF CODE FOR IK SERVO STUFF



: INIT-IK-FLOATS
  0.0E0     FVECTOR      F!    ( INITIAL DIRECTION 
  4.0E0     FT1          F!    ( OFFSET OUTWARD OF LEG
  3.5E0     FRIDE-HEIGHT F!    ( HEIGHT FROM HIP U/D PIVOT TO GND 
  2.5E0     FREACH       F!    ( HEIGHT OF STEP
  1.0E0     FSTRIDE      F!    ( LENGTH OF STEP  
  1.0E0     FSTOKE       F!

( ROTATIONAL OFFSETS OF EACH JOINT. THIS ALLOWS THE FVECTOR ADDED TO THEM
( TO PRODUCE A PARALLEL VECTOR FOR EACH LEG

  \EH3R 0LEG  90.0E0 FTHETA F!
  \EH3R +LEG  30.0E0 FTHETA F!
  \EH3R +LEG -30.0E0 FTHETA F!
  \EH3R +LEG -90.0E0 FTHETA F!
  \EH3R +LEG 210.0E0 FTHETA F!
  \EH3R +LEG 150.0E0 FTHETA F!
  
  \EH3I 0LEG  0.0E0 FTHETA F!
  \EH3I +LEG  0.0E0 FTHETA F!
  \EH3I +LEG  0.0E0 FTHETA F!
  \EH3I +LEG 180.0E0 FTHETA F!
  \EH3I +LEG 180.0E0 FTHETA F!
  \EH3I +LEG 180.0E0 FTHETA F!
  
; EEWORD

( CALCULATED VARIABLES


( LETS BUILD INCREMENTALLY
( THE WAY I INTEND TO DO THIS IS TAKE THE X-Y-Z COORDINATES, CALCULATE THE
( ANGLE THE HIP SWING NEEDS TO BE AT TO INTERSECT THAT POINT, AND THEN DETERMINE
( HEIGHT AND RADIUS OF THE POINT ON THE PLANE
( 
( FROM THERE, I WILL DETERMINE THE ANGLES OF THE JOINTS TO ACHIEVE THAT
( 
(     Z   |
(      \  Y
(       \ |
(        \|
( ----X---O  LOOKING FROM ABOVE
(
( THIS IS HOW THE INPUT CO-ORDINATES ARE FORMATTED
\ ANGLE OF HIP-R/L = ATAN(Y/(HIP-H-OFFSET + X))
\ RADIUS FROM HIP SWING IS 1/COS(ANGLE) * HIP SWING
( DISTANCE FROM HIP-U/D = RADIUS - HIP-H-OFFSET
( SO USE THAT FOR IK CALC. 

: MAKE-HIP
  FTIPY F@ FTIPX F@ FATAN2NEW FHIP-R/L F!
; EEWORD  ( WORKS

: MAKE-RADIUS
  FHIP-R/L F@ FCOS 1.0E0 FSWAP F/
  FTIPX F@ F* FHIP-H-OFFSET# F-
  FRADIUS F!
; EEWORD  ( WORKS

: MAKE-HEIGHT
  FTIPZ F@ FRIDE-HEIGHT F@ F-
  FTIP-RADIUS# F+  FHEIGHT F! 
; EEWORD

( WORKS 
( COMBINED WITH POSITIVE RIDE HEIGHT, MAKE HEIGHT MAKES US REACH DOWN
( TO REACH THE FLOOR
( RIDEHEIGHT = 4  TIP HEIGHT = .5  FHEIGHT =-3.5 SINCE WE REACH DOWN

: MAKE-HIP-TIP-LENGTH
  FRADIUS F@ F**2 
  FHEIGHT F@ F**2
  F+
  FDUP HIP-TIP-LENGTH^2 F!
  FSQRT HIP-TIP-LENGTH F!
; EEWORD

: MAKE-A1 
  FHEIGHT F@ FRADIUS F@ FATAN2NEW A1 F!
; EEWORD

\ "ACOS(((THIGH*THIGH)-(CALF*CALF)+(HTL*HTL))/((2*THIGH)HTL))"

: MAKE-A2
[ FTHIGH-LENGTH# F**2
  FCALF-LENGTH#  F**2 F- ] FLITERAL
  HIP-TIP-LENGTH^2 F@ F+
[ FTHIGH-LENGTH# 2.0E0 F* ] FLITERAL  
  HIP-TIP-LENGTH     F@  F*  F/
  FACOS A2 F!
; EEWORD

: MAKE-HIP-U/D
  A1 F@ A2 F@ F+ FHIP-U/D F!
; EEWORD ( WORKS?

: MAKE-KNEE
[ FTHIGH-LENGTH# F**2
  FCALF-LENGTH#  F**2 F+ ] FLITERAL
  HIP-TIP-LENGTH^2 F@ F-
[ FTHIGH-LENGTH# 2.0E0 F* 
  FCALF-LENGTH#        F* ] FLITERAL F/
(  FACOS FKNEE PI FSWAP F- FNEGATE F!
  FACOS PI F- FKNEE F!
; EEWORD

: MAKE-ALL-IK
  MAKE-HIP
  MAKE-RADIUS
  MAKE-HEIGHT
  MAKE-HIP-TIP-LENGTH
  MAKE-A1
  MAKE-A2
  MAKE-HIP-U/D
  MAKE-KNEE
; EEWORD ( WORKS


: FEED-SERVOS ( TAKE THE IK RESULTS, PUT INTO SERVOS
  FHIP-R/L F@ R>D 10.0E0 F* F>S LEG @ 3 * 0 + SERVO ! RC2 
  FHIP-U/D F@ R>D 10.0E0 F* F>S LEG @ 3 * 1 + SERVO ! RC2 
  FKNEE    F@ R>D 10.0E0 F* F>S LEG @ 3 * 2 + SERVO ! RC2 
; EEWORD

( ******************** BASICLY THE END OF THE I.K.
( TIME TO SQUEEZE GAIT GENERATION INTO THE I.K.
( WEEEEEEEEEEEEEEEE

: WLEG  ( n ---     where n=LEG
  LEG ! 
  MAKE-ALL-GAIT
  MAKE-ALL-IK
  FEED-SERVOS
  INCREMENT-GAIT 
; EEWORD

: 0WLEG 0 WLEG ; EEWORD
: 1WLEG 1 WLEG ; EEWORD
: 2WLEG 2 WLEG ; EEWORD
: 3WLEG 3 WLEG ; EEWORD
: 4WLEG 4 WLEG ; EEWORD
: 5WLEG 5 WLEG ; EEWORD


( ********************* JUNKIE TEST STUFF

( AT THIS POINT, THE SCALING AND TRANSLATING APPEAR TO WORK, AS DO THE
( GAIT GENERATOR WORDS THEMSELVES
( AFTER YOU INITIALIZE THE GAIT FLOATS, STORE A ROTATION INTO FTHETA
( YOU CAN STORE A POSITION IN THE GAIT, INTO GAIT-INDEX, THEN CALL 
( UBERWORD, AND SEE THE FLOAT TIP VALUES
( IF YOU STORE DIFFERENT VALUES OF ROTATION, AND SEE THE TIP POSITION CHANGE

( REALLY NEED TO THINK ABOUT A DATA STRUCTURE NOW....

DECIMAL
( ****************** SERIOUS ENDING AND INIT CODES
: INITS
  RCINITNEW                      ( SET SERVOS IN MOTION
  MAKE-SERV-TABLE                ( FILL SERVO TABLE
  INIT-IK-FLOATS                 ( SET UP IK VARIABLES
; EEWORD


INITS


( PSX CONNECTIONS
( GREEN  - VIN
( +-------
( YELLOW - +5
( BLACK  - GND
( +-------
( BROWN  - PA0
( ORANGE - PA1 
( BLUE   - PA2
( BLACK  - PA3
( +-------
( VIOLET - PA4 
(
(   J1
( +-----+_
( | X O |_VIN - GREEN
( | O O |
( | O O |_
( | X X |_+5 GND - YELLOW BLACK
( | X O | PA0 - BROWN
( | X O | PA1 - ORANGE
( | X O | PA2 - BLUE
( | X O |_PA3 - BLACK
( | X O | PA4 - VIOLET
( | O O |
( | O O |
( | O O |
( +-----+

( PSX VARIABLES
VARIABLE BITMASK EEWORD
VARIABLE BITLINE EEWORD
( KEEP DATA WORDS TOGETHER CONTIGUOUS
VARIABLE DATA EEWORD
VARIABLE DATA1 EEWORD
VARIABLE DATA2 EEWORD
VARIABLE DATA3 EEWORD
VARIABLE DATA4 EEWORD
VARIABLE DATA5 EEWORD
VARIABLE DATA6 EEWORD
VARIABLE DATA7 EEWORD
( KEEP DATA WORDS TOGETHER CONTIGUOUS
VARIABLE NEWPSX EEWORD
VARIABLE NEWKEY EEWORD
VARIABLE PREVKEY EEWORD
VARIABLE HAVEKEY EEWORD

LOOPINDEX 8BITCNT 
  7 8BITCNT END 
EEWORD

LOOPINDEX ACKTMR 
 100 ACKTMR END 
EEWORD


HEX
DEFINE DATA-IN?
  TEST-MASK 01 DATA-MASK 01 AT-ADDR 0FB1 807OFF + FOR-INPUT  EEWORD
DEFINE CMD-LO
  SET-MASK 00 CLR-MASK 02 AT-ADDR 0FB1 807OFF + FOR-OUTPUT EEWORD
DEFINE CMD-HI
  SET-MASK 02 CLR-MASK 00 AT-ADDR 0FB1 807OFF + FOR-OUTPUT EEWORD
DEFINE ATT-LO 
 SET-MASK 00 CLR-MASK 04 AT-ADDR 0FB1 807OFF + FOR-OUTPUT EEWORD
DEFINE ATT-HI
  SET-MASK 04 CLR-MASK 00 AT-ADDR 0FB1 807OFF + FOR-OUTPUT EEWORD
DEFINE CLK-LO
  SET-MASK 00 CLR-MASK 08 AT-ADDR 0FB1 807OFF + FOR-OUTPUT EEWORD
DEFINE CLK-HI
  SET-MASK 08 CLR-MASK 00 AT-ADDR 0FB1 807OFF + FOR-OUTPUT EEWORD
DEFINE ACK-LO
  SET-MASK 00 CLR-MASK 10 AT-ADDR 0FB1 807OFF + FOR-OUTPUT EEWORD
DEFINE ACK-HI
  SET-MASK 10 CLR-MASK 00 AT-ADDR 0FB1 807OFF + FOR-OUTPUT EEWORD
DEFINE ACK-IN?
  TEST-MASK 10 DATA-MASK 10 AT-ADDR 0FB8 807OFF + FOR-INPUT  EEWORD

: CMD-SET IF CMD-HI ELSE CMD-LO THEN ; EEWORD

: ACK ACK-LO ACK-HI ; EEWORD
: ACK? 
  ACKTMR RESET
  BEGIN
    ACKTMR COUNT
    ACK-IN? OR
  UNTIL 
  0 [ FB8 807OFF + ] LITERAL !
  ACKTMR VALUE
  DUP GRNLED SET
; EEWORD


' DUP CFA P@ 1+ P@ CONSTANT Fdsp EEWORD   \ find stack pointer location
HEX
0FB1 807OFF + CONSTANT Port    EEWORD
0008 CONSTANT Clkbit  EEWORD
0002 CONSTANT Cmdbit  EEWORD
0001 CONSTANT Databit EEWORD

CODE-SUB PSXOUT ( cmdout reply -- cmdout/2 reply )
87D2 P,              \ move    #Port,R2
Port P,
F854 P,              \ move    X:Fdsp,R0
Fdsp P,
87C1 P,              \ move    #Cmdbit,Y0   ; output bit mask
Cmdbit P,
6C30 P,              \ clr     A

F040 P,              \ move    X:(R0+1),X0  ; get cmdout
0001 P,
7E35 P,              \ lsr     X0           ; get lsb
D040 P,              \ move    X0,X:(R0+1)  ; save shifted cmdout
0001 P,
                     \                      ; bit to be output is in carry
6D54 P,              \ tcs     Y0,A         ; if cy=1, put bit ptrn in A1

F116 P,              \ move    X:(R2),Y0    ; read port
81C1 P,              \ bfclr   #(Clkbit+Cmdbit),Y0  ; clkbit=0, and
Cmdbit Clkbit + P,
6623 P,              \ or      A1,Y0        ; cmdbit=lsb of cmdout
D116 P,              \ move    Y0,X:(R2)    ; write port

EDD8 P,              \ rts
END-CODE EEWORD

CODE-SUB PSXIN ( cmdout reply -- cmdout reply' )
87D2 P,              \ move    #Port,R2
Port P,
F854 P,              \ move    X:Fdsp,R0
Fdsp P,
87C0 P,              \ move    #$80,X0      ; MSB bit mask
0080 P,
6C30 P,              \ clr     A

F116 P,              \ move    X:(R2),Y0    ; read port
83C1 P,              \ bfset   #Clkbit,Y0   ; clkbit=1
Clkbit P,
D116 P,              \ move    Y0,X:(R2)    ; write port
                     \                      ; now Y0=port with input bit

8DC1 P,              \ bftsth  #Databit,Y0  ; set cy if bit high
Databit P,
6D44 P,              \ tcs     X0,A         ; if cy=1, put MSB in A1

F114 P,              \ move    X:(R0),Y0    ; get reply
7E37 P,              \ lsr     Y0           ; shift right
6623 P,              \ or      A1,Y0        ; add z/nz msb
D114 P,              \ move    Y0,X:(R0)    ; save updated reply

EDD8 P,              \ rts
END-CODE EEWORD

( : DLY ; EEWORD
CODE-SUB DLY  
EDD8 P,              \ rts
END-CODE EEWORD

: XFR ( cmdout -- reply )
  0
  PSXOUT DLY DLY PSXIN 
  PSXOUT DLY DLY PSXIN 
  PSXOUT DLY DLY PSXIN 
  PSXOUT DLY DLY PSXIN 
  PSXOUT DLY DLY PSXIN 
  PSXOUT DLY DLY PSXIN 
  PSXOUT DLY DLY PSXIN 
  PSXOUT DLY DLY PSXIN 
  SWAP DROP
; EEWORD

: PSX
  YELLED ON 
  FALSE NEWPSX !
  ATT-LO
  01 XFR DROP    ACK? 
  GRNLED ?OFF IF CMD-HI ATT-HI EXIT THEN
  42 XFR DATA  ! ACK? 
  00 XFR DATA1 ! ACK? 
  DATA1 @ 5A = 
  IF
    00 XFR DATA2 ! ACK? 
    00 XFR DATA3 !
    DATA @ 73 = 
    DATA @ F3 = OR 
    IF
                     ACK?
      00 XFR 80 -        DATA4 ! ACK? 
      00 XFR 80 - NEGATE DATA5 ! ACK? 
      00 XFR 80 -        DATA6 ! ACK? 
      00 XFR 80 - NEGATE DATA7 ! 
      TRUE NEWPSX !
    THEN
  THEN
  CMD-HI
  ATT-HI
  YELLED OFF 
; EEWORD

HEX
: INIT-PSX
  FF [ FB1 807OFF + ] LITERAL ! ( OUTPUT 1'S
  EE [ FB2 807OFF + ] LITERAL ! ( DDR 0=INPUT 1=OUTPUT
  00 [ FB3 807OFF + ] LITERAL ! ( PER DISABLE PERIPHERAL FUNCTIONS
  10 [ FB5 807OFF + ] LITERAL ! ( IEN ENABLE EDGE INT
(  10 [ FB6 807OFF + ] LITERAL ! ( IPOLR PA4 ACTIVE LOW
(  00 [ FB8 807OFF + ] LITERAL ! ( IESR  EDGE SENSITIVE
  FF DATA1 !
  FF DATA2 !
  FF DATA3 !
  00 DATA4 !
  00 DATA5 !
  00 DATA6 !
  00 DATA7 !
; EEWORD

: DD
  CR
  4 0 DO DATA I + @ . LOOP
  DATA @ 73 = 
  DATA @ F3 = OR 
  IF
    8 4 DO DATA I + @ . LOOP
  THEN
; EEWORD

: DDL BEGIN DD ?TERMINAL UNTIL ; EEWORD

2F CONSTANT   "+"    EEWORD
2E CONSTANT "-"      EEWORD

57 CONSTANT   "W"    EEWORD
41 CONSTANT "A"      EEWORD
53 CONSTANT     "S"  EEWORD
5A CONSTANT   "Z"    EEWORD

52 CONSTANT   "R"    EEWORD
44 CONSTANT "D"      EEWORD
46 CONSTANT     "F"  EEWORD
43 CONSTANT   "C"    EEWORD

( 59 CONSTANT   "Y"    EEWORD
 47 CONSTANT "G"      EEWORD
( 48 CONSTANT     "H"  EEWORD
( 42 CONSTANT   "B"    EEWORD

49 CONSTANT   "I"    EEWORD
4A CONSTANT "J"      EEWORD
4B CONSTANT     "K"  EEWORD
4D CONSTANT   "M"    EEWORD

: SLCT? DATA2 @ 01 AND 0= ; EEWORD
: JOYR? DATA2 @ 02 AND 0= ; EEWORD
: JOYL? DATA2 @ 04 AND 0= ; EEWORD
: STRT? DATA2 @ 08 AND 0= ; EEWORD
: UP?   DATA2 @ 10 AND 0= ; EEWORD
: RGHT? DATA2 @ 20 AND 0= ; EEWORD
: DOWN? DATA2 @ 40 AND 0= ; EEWORD
: LEFT? DATA2 @ 80 AND 0= ; EEWORD

: L2?   DATA3 @ 01 AND 0= ; EEWORD
: R2?   DATA3 @ 02 AND 0= ; EEWORD
: L1?   DATA3 @ 04 AND 0= ; EEWORD
: R1?   DATA3 @ 08 AND 0= ; EEWORD
: /\?   DATA3 @ 10 AND 0= ; EEWORD
: O?    DATA3 @ 20 AND 0= ; EEWORD
: X?    DATA3 @ 40 AND 0= ; EEWORD
: []?   DATA3 @ 80 AND 0= ; EEWORD

DECIMAL
: RJOY-LR DATA4 @ S>F 128E0 F/ ; EEWORD
: RJOY-UD DATA5 @ S>F 128E0 F/ ; EEWORD

: LJOY-LR DATA6 @ S>F 128E0 F/ ; EEWORD
: LJOY-UD DATA7 @ S>F 128E0 F/ ; EEWORD


DECIMAL

: DIRECTION
  NEWPSX @
  IF
    RJOY-LR
    RJOY-UD
    FATAN2NEW
    R>D
    FVECTOR F!
  ELSE
    NEWKEY @
    IF
      RGHT?
      HAVEKEY @ NEWKEY @ "K" = AND OR
      IF
        5E0
        FVECTOR F@
        F+
        360E0 FOVER F< IF 360E0 F- THEN
        HAVEKEY @ IF 13 EMIT ." VECTOR " FDUP F. THEN
        FVECTOR F!      
      THEN
      LEFT?
      HAVEKEY @ NEWKEY @ "J" = AND OR
      IF
        -5E0
        FVECTOR F@
        F+
        FDUP 0E0 F< IF 360E0 F+ THEN
        ANGCHK
        HAVEKEY @ IF 13 EMIT ." VECTOR " FDUP F. THEN
        FVECTOR F!      
      THEN
    ELSE
      ADC0 ANALOGIN S>F [ 360E0 4096E0 F/ ] FLITERAL F*
      FVECTOR F!      
    THEN
  THEN
; EEWORD

: MAGNITUDE
  NEWPSX @
  IF
    LJOY-LR
    FDUP .25E0 F<
    FROTATE F!

    RJOY-LR 
    RJOY-UD
    FVECSUM 
    FDUP .25E0 F<

    AND
    IF
      GAITINC. @
      IF
        GAITINC. @ SAVE-GAITINC. !
        0 GAITINC. !
      THEN
    ELSE
      GAITINC. @ 0=
      IF
        SAVE-GAITINC. @ GAITINC. !
      THEN
    THEN
    1E0 FMIN
    3E0 F* ( SCALE 0 - 3.0 
    FSTRIDE F!
  ELSE
    NEWKEY @
    IF
      UP?
      HAVEKEY @ NEWKEY @ "I" = AND OR
      IF
        .05E0
        FSTRIDE  F@
        F+  
        HAVEKEY @ IF 13 EMIT ." STRIDE " FDUP F. THEN
        FSTRIDE F!      
      THEN
      DOWN?
      HAVEKEY @ NEWKEY @ "M" = AND OR
      IF
        -.05E0
        FSTRIDE  F@
        F+  
        HAVEKEY @ IF 13 EMIT ." STRIDE " FDUP F. THEN
        FSTRIDE F!      
      THEN
    ELSE
      ADC2 ANALOGIN S>F [ 3E0 4096E0 F/ ] FLITERAL F*
      FSTRIDE F!      
    THEN
  THEN
; EEWORD

: ROTATION
(  RJOY-LR 
(  FVECTOR F!
; EEWORD

: HEIGHT-CONTROL
(  LJOY-LR
(  2.5E0 F* 2.5E0 F+
  UP?
  HAVEKEY @ NEWKEY @ "W" = AND OR
  IF
    .05E0
    FRIDE-HEIGHT F@
    F+
    5E0 FMIN
    1E0 FMAX
    HAVEKEY @ IF 13 EMIT ." HEIGHT " FDUP F. THEN
    FRIDE-HEIGHT F!
  THEN
  DOWN?
  HAVEKEY @ NEWKEY @ "Z" = AND OR
  IF
    -.05E0
    FRIDE-HEIGHT F@
    F+
    5E0 FMIN
    1E0 FMAX
    HAVEKEY @ IF 13 EMIT ." HEIGHT " FDUP F. THEN
    FRIDE-HEIGHT F!
  THEN
; EEWORD

: REACH-CONTROL
  RGHT?
  HAVEKEY @ NEWKEY @ "S" = AND OR
  IF
    .05E0
    FREACH F@
    F+
    5E0 FMIN
    1E0 FMAX
    HAVEKEY @ IF 13 EMIT ." REACH " FDUP F. THEN
    FREACH F!
  THEN
  LEFT?
  HAVEKEY @ NEWKEY @ "A" = AND OR
  IF
    -.05E0
    FREACH F@
    F+
    5E0 FMIN
    1E0 FMAX
    HAVEKEY @ IF 13 EMIT ." REACH " FDUP F. THEN
    FREACH F!
  THEN
; EEWORD

: Gx? NEWKEY @ = HAVEKEY @ AND PREVKEY @ "G" = AND ; EEWORD

: G0? 48 Gx? ; EEWORD
: G1? 49 Gx? ; EEWORD
: G2? 50 Gx? ; EEWORD
: G3? 51 Gx? ; EEWORD
: G4? 52 Gx? ; EEWORD
: G5? 53 Gx? ; EEWORD
: G6? 54 Gx? ; EEWORD
: G7? 55 Gx? ; EEWORD
: G8? 56 Gx? ; EEWORD
: GO? 79 Gx? ; EEWORD

: GAIT
  SLCT? G0? OR IF STAND CR ."  GAIT0 " THEN
  L2? G1? OR IF 1 INIT-GAIT  CR ."  GAIT1 " THEN
  R2? G2? OR IF 2 INIT-GAIT  CR ."  GAIT2 " THEN
  L1? G3? OR IF 3 INIT-GAIT  CR ."  GAIT3 " THEN
  R1? G4? OR IF 4 INIT-GAIT  CR ."  GAIT4 " THEN
  /\? G5? OR IF 5 INIT-GAIT  CR ."  GAIT5 " THEN
  O?  G6? OR IF 6 INIT-GAIT  CR ."  GAIT6 " THEN
  X?  G7? OR IF 7 INIT-GAIT  CR ."  GAIT7 " THEN
  []? G8? OR IF 8 INIT-GAIT  CR ."  GAIT8 " THEN
  STRT? GO? OR
  IF
    SAVE-GAIT @ 1 MAX INIT-GAIT  CR ." START"
  THEN
; EEWORD


: CONTROLS
  DIRECTION
  MAGNITUDE
  ROTATION
  HEIGHT-CONTROL
  REACH-CONTROL
  GAIT
  FALSE HAVEKEY !
; EEWORD


: PHASE0
  PSX       ( 0
  CONTROLS  ( 1
; EEWORD

: PHASE1
  0WLEG     ( 2
  1WLEG     ( 3
  2WLEG     ( 4
; EEWORD

: PHASE2
  3WLEG     ( 5
  4WLEG     ( 6
  5WLEG     ( 7
; EEWORD

: PHASE3
; EEWORD

:CASE TASKS
  PHASE0
  PHASE1
  PHASE2
  PHASE3
; EEWORD

LOOPINDEX PHASECNT
  0 PHASECNT START
  2 PHASECNT END
( 3 PHASECNT END
EEWORD

: ALL
  SERVO @ >R
  LEG @ >R
  PHASECNT COUNT DROP
  PHASECNT VALUE 
  TASKS
  R> LEG !
  R> SERVO !
; EEWORD


: COMMAND-MODE
  STAND
  CR ." STAND UNTIL GAIT SELECTED AND MOVEMENT COMMANDED"
  CR ." COMMAND MODE KEYBOARD UNTIL RETURN <CR>" CR
  0 NEWKEY !
  BEGIN
    ?TERMINAL
    IF
      NEWKEY @ PREVKEY ! ( MAKE COPY
      KEY NEWKEY !       ( GET NEW KEY
      TRUE HAVEKEY !     ( SET FLAG THAT KEY HAPPENED
    THEN
    NEWKEY @ 13 =
  UNTIL
; EEWORD

: STARTUP
  ." WALKER CODE EH-3" INLINE/ROUND' IF ." I" ELSE ." R" THEN
  INITS
  INIT-PSX
  EVERY 50000 CYCLES SCHEDULE-RUNS ALL ( 100cps
  COMMAND-MODE
; EEWORD


: WATCH
  BEGIN
    13 EMIT 

(   LJOY-UD F.
(   LJOY-LR F.
(   RJOY-UD F.
(   RJOY-LR F.

    FVECTOR F?
    FSTRIDE F?
(   ROTATION
    FRIDE-HEIGHT F?
    FREACH F?
    ?TERMINAL
  UNTIL 
; EEWORD

( PROBLEM WITH THIS IS NEWKEY DOESN'T UPDATE
: TWEAK
  CR ." TWEAKING LEG " LEG @ . ."  SERVO " SERVO @ . CR
  BEGIN
    ?TERMINAL IF KEY NEWKEY ! ELSE 0 NEWKEY ! THEN
    NEWKEY @ "+" =
    IF
      MECHADJ-D 1+! "+" EMIT
    THEN
    NEWKEY @ "-" =
    IF
      MECHADJ-D 1-! "-" EMIT
    THEN
    MECHADJ-D @ 15 MIN -15 MAX MECHADJ-D ! 
    MECHADJ-D @ S>F COEFF F@ F* F>S ( Output as integer
    MECHADJ-T !
    NEWKEY @ 13 =
  UNTIL
  ( FLASH?
; EEWORD


DECIMAL
 SAVE-RAM
 AUTOSTART STARTUP ( V.7 VERSION OR LATEST 
( HEX EC00 AUTOSTART STARTUP DECIMAL ( 807 V.6 VERSION
( HEX 3C00 AUTOSTART STARTUP DECIMAL ( 805 V.6 VERSION