Created
April 22, 2014 19:09
-
-
Save tianic/11190746 to your computer and use it in GitHub Desktop.
Robot test code
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
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 |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment