{ TACHYON Stepper Motor Robot Control Language 28BY-J48 12 Volt Unipolar Stepper + ULLN2003 Driver Parallax Propeller Mini Board and Laser Ping XBee Wireless Communication 7.4 V LiPol Battery and StepUp StepDown Power Module } pub (TRCSM) ; DECIMAL \ Pin Assignments #P4 == LA \ Left Wheel Motor Controller #P5 == LB #P6 == LC #P7 == LD #P8 == RA \ Right Wheel Motor Controller #P9 == RB #P10 == RC #P11 == RD #P16 == PG \ Laser PING #P30 == Txd \ console #P31 == Rxd \ System Constants - 6 cm wheel diameter - 8.2 cm between wheel centers - 128 steps/rotation #188 == wcirc \ cm*10 #68 == ctcm \ counts per cm* 10 #51 == ctdg \ counts per degree*100 \ System Variables BYTE rstp \ -1 : right wheel stopped - Note: -1 eq true 0 eq false BYTE lstp \ -1 : left wheel stopped BYTE rfb \ -1 : right wheel forward BYTE lfb \ -1 : left wheel foirward LONG ldy \ left wheel pulse width in us LONG rdy \ right wheel pulse width LONG lfstep \ left forward step counts LONG lbstep \ left backward step counts LONG rfstep \ right fortward step counts LONG rbstep \ right backward step counts LONG si \ calculated step no. LONG dist \ ping distance in cm pri >ldy \ ( n -- ) store left pulse width ldy ! ; pri ldy> \ ( -- n) leave left pulse width ldy @ ; pri >rdy \ ( n -- ) store right pulse width rdy ! ; pri rdy> \ ( -- n ) leave right pulse width rdy @ ; pub .enc \ ( -- data ) display left and right wheel encoder values lfstep @ . SPACE lbstep @ . SPACE SPACE rfstep @ . SPACE rbstep @ . CR ; pub .lenc lfstep @ . ; pub .renc rfstep @ . ; pub .lenc1 lbstep @ , ; pub .renc1 rbstep @ . ; pub zenc \ ( -- ) set all encoder values to 0 lfstep ~ lbstep ~ rfstep ~ rbstep ~ ; pub FRWD \ ( -- ) set forward direction lfb C~~ rfb C~~ ; pub BKWD \ ( -- ) set backward direction lfb C~ rfb C~ ; pub RRWD \ ( -- ) set rotate right direction lfb C~~ rfb C~ ; pub RLWD \ ( -- ) set rotate left direction lfb C~ rfb C~~ ; pub S1 \ ( -- ) set speed 1 - very slow #4000 >ldy #4000 >rdy ; pub S2 \ ( -- ) set speed 2 - slow #2000 >ldy #2000 >rdy ; pub S3 \ ( -- ) set speed - moderate #1500 >ldy #1500 >rdy ; pub S4 \ ( -- ) set speed - fast #1000 >ldy #1000 >rdy ; pub Sx \ ( n1 n2 -- ) set left (n1) & right (n2) speeds (us) >ldy >rdy ; #4 #8 MASKS OUTPUTS pri linit \ ( -- ) set left motor to initial state %1000 #4 #4 PINS! ; pri rinit \ ( -- ) set right motor to initial state %1000 #8 #4 PINS! ; pri lfrwd \ ( -- ) move left wheel forward one step LC HIGH ldy> us LD LOW ldy> us LB HIGH ldy> us LC LOW ldy> us LA HIGH ldy> us LB LOW ldy> us LD HIGH ldy> us LA LOW ldy> us ; pri lbkwd \ ( -- ) move left wheel backward one step LA HIGH ldy> us LD LOW ldy> us LB HIGH ldy> us LA LOW ldy> us LC HIGH ldy> us LB LOW ldy> us LD HIGH ldy> us LC LOW ldy> us ; pri rc_lwheel \ ( -- ) left wheel task linit BEGIN lstp C@ IF linit ELSE lfb C@ IF lfrwd lfstep ++ ELSE lbkwd lbstep ++ THEN THEN 0 UNTIL ; pub LSTART ' rc_lwheel TASK? RUN 50 ms ; pri rfrwd \ ( -- ) move right wheel forward one step RA HIGH rdy> us RD LOW rdy> us RB HIGH rdy> us RA LOW rdy> us RC HIGH rdy> us RB LOW rdy> us RD HIGH rdy> us RC LOW rdy> us ; pri rbkwd \ ( -- ) move right wheel backward one step RC HIGH rdy> us RD LOW rdy> us RB HIGH rdy> us RC LOW rdy> us RA HIGH rdy> us RB LOW rdy> us RD HIGH rdy> us RA LOW rdy> us ; pri rc_rwheel \ ( -- ) right wheel task rinit BEGIN rstp C@ IF rinit ELSE rfb C@ IF rfrwd rfstep ++ ELSE rbkwd rbstep ++ THEN THEN 0 UNTIL ; pub RSTART ' rc_rwheel TASK? RUN 50 ms ; pri rc_ping \ ( -- ) laser ping task A 1 FRQ BEGIN A PG DUP LOW APIN NCO #5 NEGATE PHSA COG! #1000 CTRMODE 0 PHSA COG! PG FLOAT 20 ms PHSA COG@ #1715 #800000 */ dist ! 50 ms 0 UNTIL ; pub .dist \ -- ) display current ping distance in cm dist @ . CR ; pub PSTART ' rc_ping TASK? RUN 50 ms ; pub H \ ( -- ) execute halt command rstp C~~ lstp C~~ ; pub START \ ( -- ) assigns all tasks to cogs H PSTART LSTART RSTART ; \ Indefinite Motions pub M1 \ ( -- ) set left wheel motion lstp C~ ; pub M2 \ ( -- ) set right wheel motion rstp C~ ; pub GO \ ( -- ) set both wheels motion M1 M2 ; pub F \ ( -- ) move forward FRWD GO ; pub B \ ( -- ) move backwards BKWD GO ; pub RR \ ( -- ) rotate right RRWD GO ; pub RL \ ( -- ) rotate left RLWD GO ; pub TR \ ( -- ) turn right FRWD M1 ; pub TL \ (--) turn left FRWD M2 ; pub BTR \ ( -- ) back to right BKWD M1 ; pub BTL \ ( -- ) back to left BKWD M2 ; \ Defined Motions pri cklencf BEGIN lfstep @ si @ > UNTIL H ; pri cklencb BEGIN lbstep @ si @ > UNTIL H ; pri ckrencf BEGIN rfstep @ si @ > UNTIL H ; pub GF \ ( n -- ) go forward n centimeters ctcm #10 */ si ! zenc F cklencf ; pub GB \ ( n -- ) go backward n centimeters ctcm #10 */ si ! zenc B cklencb ; pub GRR \ ( n -- ) rotate right n degrees ctdg #100 */ si ! zenc RR cklencf ; pub GRL \ ( n -- ) rotate left n degrees ctdg #100 */ si ! zenc RL ckrencf ; pub GTR \ ( n -- ) rotate right n degrees ctdg #50 */ si ! zenc TR cklencf ; pub GTL \ ( n -- ) turn left n degrees ctdg #50 */ si ! zenc TL ckrencf ; pub GODT \ ( dt -- ) move dt ms in predefined direction & speed GO ms H ; pub VR \ ( -- ) veer right 10 degrees H #10 GTR F ; pub VL \ ( -- ) veer left 10 degrees H #10 GTL F ; \ The following commands require that tasks LSTART & RSTART are not activated. pub GSTPF \ ( n -- ) go forward n steps FOR lfrwd rfrwd NEXT ; pub GSTPB \ ( n -- ) go backward n steps FOR lbkwd rbkwd NEXT ; pub GSTPRR \ ( n -- ) rotate right n steps FOR lfrwd rbkwd NEXT ; pub GSTPRL \ ( n -- ) rotate left n steps FOR lbkwd rfrwd NEXT ; pub GSTPTR \ ( n -- ) turn right n steps FOR lfrwd NEXT ; pub GSTPTL \ ( n -- ) turn left n steps FOR rfrwd NEXT ;