Robot test code

robotcode4

VARIABLE CLOCK
VARIABLE MAIN
VARIABLE MAIN_OFFSET
VARIABLE LEGSV
VARIABLE SPEED 100 SPEED !

: 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 -- ) \ 38 hz timer 76 hz pwm (minimum)
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 ;

: CHECK CLOCK @ SPEED @ MOD ;
: CHECK_OFFSET CLOCK @ SPEED @ 5 / - SPEED @ MOD ;

: LEGTIMER CHECK
0= IF MAIN @ NOT MAIN ! THEN ;
: LEGTIMER2 CHECK_OFFSET
0= IF MAIN_OFFSET @ NOT MAIN_OFFSET ! THEN ;

: GO -1 LEGSV ! ;
: STOP 0 LEGSV ! ;

50 FSET

: SRV01 ( n -- ) \ \ 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 ;

: LEG1 MAIN @ 0= IF 3300 SRV01 ELSE 5700 SRV01 THEN ;
: LEG2 MAIN_OFFSET @ 0= IF 4500 SRV02 ELSE 5500 SRV02 THEN ;
: LEG3 MAIN_OFFSET @ 0= IF 5500 SRV03 ELSE 4500 SRV03 THEN ;
: LEG4 MAIN_OFFSET @ 0= IF 4500 SRV04 ELSE 5500 SRV04 THEN ;
: LEG5 MAIN_OFFSET @ 0= IF 5500 SRV05 ELSE 4500 SRV05 THEN ;
: LEG6 MAIN_OFFSET @ 0= IF 4500 SRV06 ELSE 5500 SRV06 THEN ;
: LEG7 MAIN_OFFSET @ 0= IF 4500 SRV07 ELSE 5500 SRV07 THEN ;

MACHINE LEGS
ON-MACHINE LEGS
APPEND-STATE SETLEGS
IN-STATE SETLEGS
CONDITION LEGSV @ -1 =
CAUSES
 1 CLOCK +! LEGTIMER LEGTIMER2
 LEG1 LEG2 LEG3 LEG4 LEG5 LEG6 LEG7
THEN-STATE SETLEGS
TO-HAPPEN

SETLEGS SET-STATE INSTALL LEGS