\ just download

\ then do moves like:

\ RBASE 1000 GO X

\ Joints are RBASE, SHLDR, ELBOW, WRIST, GRIP
 
\ X is word that tracks move in foreground

\ Moves should be smooth, start slow and accelerate ramp up,
\ move constant speed, then ramp down to perfect stop.

\ acceleration is setable xxx CMDACC !
 
\ Top velocity is settable xxx CMDVEL !
 
\ and position to go to is settable 0 -1000 CMDPOS !

SCRUB

HEX
\ 0 RAM !

( : EEWORD ;

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

HALFSPEEDCPU 

 2E4 CONSTANT MINPOS EEWORD (  .59 mS
 B9A CONSTANT MAXPOS EEWORD ( 2.38 ms
 MAXPOS MINPOS + 2/ CONSTANT MIDPOS EEWORD
 MAXPOS MINPOS - CONSTANT RANGE EEWORD

VARIABLE CH EEWORD

: CHVAR
  <BUILDS HERE P, 6 ALLOT DOES> P@ CH @ + 
; EEWORD

CHVAR CMDPOS EEWORD
CHVAR CURPOS EEWORD
CHVAR CMDVEL EEWORD
CHVAR CURVEL EEWORD
CHVAR CMDACC EEWORD
CHVAR CURACC EEWORD
CHVAR DECCNT EEWORD

  C3 CONSTANT PWMCTL# EEWORD ( DIV BY 8
61A8 CONSTANT PER#  EEWORD ( 61A8 for 50 Hz, 5161 for 60 Hz
 3E8 CONSTANT SCALE# EEWORD

: CHSET
  <BUILDS P, DOES> P@ CH ! 
; EEWORD
 
0  DUP CHSET RBASE EEWORD ( 0
1+ DUP CHSET SHLDR EEWORD ( 1
1+ DUP CHSET ELBOW EEWORD ( 2
1+ DUP CHSET WRIST EEWORD ( 3
1+ DUP CHSET GRIP  EEWORD ( 4
1+     CHSET EXTRA EEWORD ( 5


: PWMALOAD E00 @ DROP PWMCTL# E00 ! ; EEWORD

: RCINIT
  HALFSPEEDCPU 

  0 CH !
  CMDPOS 6 SCALE# 2/ FILL
  CURPOS 6 SCALE# 2/ FILL
  CMDVEL 6 10 FILL
  CURVEL 6 ERASE
  CMDACC 6 1 FILL
  CURACC 6 1 FILL

  8000   E03 !
  0      E0D !
  0      E0E !
  000E   E0F ! ( CENTER ALIGN FOR DIV BY 2
  PER#   E05 ! 

  MIDPOS E06 !
  MIDPOS E07 !
  MIDPOS E08 !
  MIDPOS E09 !
  MIDPOS E0A !
  MIDPOS E0B ! ( NOT USED

  PWMALOAD
; EEWORD

: LMTPOS 0 MAX SCALE# MIN ; EEWORD

: RC ( COUNT FROM 0 TO SCALE#
  LMTPOS RANGE SCALE# */ MINPOS + E06 CH @ + ! 
  PWMALOAD
; EEWORD

: RC? ( COUNT --- SCALED VALUE
  E06 CH @ + @ MINPOS - SCALE# RANGE */
; EEWORD

: MOVEDONE?
  CMDPOS @ CURPOS @ = 
; EEWORD

: MOVEDIR?
  CMDPOS @ CURPOS @ < 
; EEWORD

: REMAINING?
  CMDPOS @ CURPOS @ - ABS
; EEWORD

: DECEL?
  0 DECCNT !
  REMAINING?
  CURVEL @
  BEGIN
    OVER 0> 
    OVER 0> AND
  WHILE
    DECCNT 1+!
    CMDACC @ - ( CALC VEL
    SWAP OVER - SWAP ( TAKE FROM REMAINING
  REPEAT
  0>
  SWAP DROP
; EEWORD

: VMX
  MOVEDONE? NOT
  IF
    CMDACC @
    DECEL? IF NEGATE ELSE CURVEL @ CMDVEL @ = IF DROP 0 THEN THEN
    CURACC ! 
    CURVEL @ CURACC @ + 0 MAX CMDVEL @ MIN CURVEL !  
    MOVEDIR? 
    CURVEL @ MOVEDIR? IF NEGATE THEN CURPOS +!
    MOVEDIR? NOT =
    IF CMDPOS @ CURPOS ! THEN
    CURPOS @ LMTPOS CURPOS !
    CURPOS @ RC
  ELSE
    0 CURVEL ! 
    0 CURACC ! 
  THEN
; EEWORD

: VELMOVE
  CH @
  RBASE VMX
  SHLDR VMX
  ELBOW VMX
  WRIST VMX
  GRIP  VMX
  CH !
; EEWORD

: GO LMTPOS CMDPOS ! ; EEWORD

: RGO CMDPOS @ + LMTPOS CMDPOS ! ; EEWORD ( RELATIVE GO

 RCINIT EVERY C350 CYCLES SCHEDULE-RUNS VELMOVE



( BEYOND THIS, ARE TOOLS FOR CHECK OUT
: LABEL
  CR ."  CURPOS CURVEL CURACC DECCNT CMDPOS CMDVEL CMDACC" ( RCSERV"
; EEWORD

: ?7 @ 7 .R ; EEWORD

: SETTINGS?
  CR CURACC CURVEL CURPOS ?7 ?7 ?7 DECCNT ?7 CMDPOS ?7 CMDVEL ?7 CMDACC ?7
(  E06 CH @ + ?7 
; EEWORD

: X
  LABEL
  BEGIN
    MOVEDONE? ?TERMINAL OR NOT
  WHILE
    SETTINGS?
  REPEAT
  SETTINGS? ( AT LEAST ONCE, OR ONE LAST TIME 
; EEWORD

: T
  LABEL
  CH @
  RBASE SETTINGS?
  SHLDR SETTINGS?
  ELBOW SETTINGS?
  WRIST SETTINGS?
  GRIP  SETTINGS?
  CH !
; EEWORD

: VEL  CMDVEL ! ; EEWORD
: VEL? CURVEL ?7 CMDVEL ?7 ; EEWORD
: ACC  CMDACC ! ; EEWORD
: ACC? CURACC ?7 CMDACC ?7 ; EEWORD

: ALLFILL ( VAL CHVARIABLE --- )
  CH @ - SWAP 6 SWAP FILL
; EEWORD
DECIMAL

\ SAVE-RAM