{ Tachyon (Juno) BOE Platform Robot Control Language Prop Activity Board + Standard Servo+Ping Wheel Encoders, high spreed continous serevos XBee (dout - P11 din - P10), and a 7.4V Li-Ion Rechargeable Battery } pub (TBOEP) ; DECIMAL \ Robot Platform Parameters - 2.625 inch wheel diameter 4.25 inch between wheel centers #8 == wcirc \ wheel circumference (inches) #64 == cnts \ number of counts per wheel rotation #8 == ctin \ counts per inch #134 == rcirc \ rotation circumference (inches)*10 #268 == tcirc \ turn circumfirence (inches)*10 #60 == ctdeg \ counts per degree*100 - based on turn circum. \ Motor Drivers and Utilities #15155 == ld0 \ sets zero position of left servo #15275 == rd0 \ sets zero position of right servo #8 == adj \ scale factor: set for duty range 0-1000 LONG lduty, rduty, ldty, rdty, ldir, rdir \ wheel servo paramters \ Set Wheel Directions pub FRWD ( -- \ both wheels forward ) 1 ldir ! -1 rdir ! ; pub BKWD ( -- \ both wheels backwards ) -1 ldir ! 1 rdir ! ; pub RRWD ( -- \ left forward, right backward ) 1 ldir ! 1 rdir ! ; pub RLWD ( -- \ left backward, rght forward ) -1 ldir ! -1 rdir ! ; \ Set Wheel Speeds #150 == ls1 \ speed settings #150 == rs1 #250 == ls2 #250 == rs2 #500 == ls3 #500 == rs3 #750 == ls4 #750 == rs4 pub S0 0 ldty ! 0 rdty ! ; pub S1 ( -- \ low speed ) ls1 ldty ! rs1 rdty ! ; pub S2 ( -- \ moderate speed ) ls2 ldty ! rs2 rdty ! ; pub S3 ( -- \ medium speed ) ls3 ldty ! rs3 rdty ! ; pub S4 ( -- \ high speed ) ls4 ldty ! rs4 rdty ! ; pub Sx ( n1 n2 -- \ where n1 is the left & n2 is the right speed setting ) rdty ! ldty ! ; \ Motor Controls pub M1 ( duty -- \ sets left servo duty ) ldir @ * ld0 + adj * lduty ! ; pub M2 ( duty -- \ sets right servo duty ) rdir @ * rd0 + adj * rduty ! ; pub GO ldty @ M1 rdty @ M2 ; pub S ( -- \ stop motion ) S0 GO ; pub H ( -- \ pause motion ) 0 lduty ! 0 rduty ! ; pub .duty ( -- \ displays current right & left duty counts ) lduty @ . SPACE rduty @ . CR ; pri rc_motors ( -- \ l/r servo drivers ) A NCO #P12 DUP LOW APIN 1 FRQ 0 PHSA COG! B NCO #P13 DUP LOW APIN 1 FRQ 0 PHSB COG! BEGIN lduty @ NEGATE PHSA COG! rduty @ NEGATE PHSB COG! 20 ms 0 UNTIL ; pub MSTART ( -- \ executes l/r servo drivers in a cog ) ' rc_motors TASK? RUN 50 ms ; \ Encoders Record Total Counts per Wheel Rotation LONG lenc, renc, xl, xr \ encoder parameters #P14 #2 MASKS INPUTS pri rc_enc ( -- \ l/r encoders ) TRUE xl ! TRUE xr ! BEGIN #P14 PIN@ DUP xl @ = IF DROP ELSE xl ! lenc ++ THEN #P15 PIN@ DUP xr @ = IF DROP ELSE xr ! renc ++ THEN 0 UNTIL ; pub ESTART ' rc_enc TASK? RUN 50 ms ; pub .enc ( -- \ displays current encoder counter values ) lenc @ . SPACE SPACE renc @ . CR ; pub .lenc lenc @ . ; pub .renc renc @ . ; pub zenc ( -- \ set left and right encoder counts to 0 ) 0 lenc ! 0 renc ! ; \ Ping Ultrasonic Distance Measurement (inches). #1400 == p0 \ servo zero position LONG (dist), pos, ang \ ping distance and servo position pub >pos ( angle -- \ where angle range is 90 to -90 degrees: left to right ) DUP ang ! 10 * p0 + 80 * pos ! ; pub dist ( -- value \ places current ping distance on stack ) (dist) @ ; pub .dist ( -- \ displays current ping distance ) dist . ; pri rc_ping ( -- \ ping driver ) A 1 FRQ BEGIN #P16 MASK OUTPUTS A #P16 DUP LOW APIN NCO #400 NEGATE PHSA COG! #1000 CTRMODE 0 PHSA COG! #P16 MASK INPUTS 50 ms PHSA COG@ #11800 / (dist) ! 0 UNTIL ; pub PSTART ( -- \ start ping cog ) ' rc_ping TASK? RUN 50 ms ; \ Ping Servo pri rc_servo A #P17 DUP LOW APIN NCO 1 FRQ BEGIN pos @ NEGATE PHSA COG! 20 ms 0 UNTIL ; pub VSTART ' rc_servo TASK? RUN 50 ms ; 200 \ ZActivating Cogs pub R ( -- \ hardware reset - execute START to continue ) REBOOT ; pub START ( -- \ initialize and start drivers ) DECIMAL MSTART ESTART VSTART PSTART ; { End of TRCL Kernel ******************************************************************************** } \ Indefinite Motions (Drone Mode) pub F ( -- \ go forward ) FRWD GO ; pub B ( -- \ go backward ) BKWD GO ; pub RL ( -- \ rotate in place counter clockwise ) RLWD GO ; pub RR ( -- \ rotate in place clockwise ) RRWD GO ; pub TR ( -- \ turn right ) FRWD ldty @ M1 0 M2 ; pub TL ( -- \ turn left ) FRWD 0 M1 rdty @ M2 ; pub BTR ( -- \ back to right ) BKWD ldty @ M1 0 M2 ; pub BTL ( -- \ back to left ) BKWD 0 M1 rdty @ M2 ; \ Defined Motions \ In the following two functions, motion continues until the right/left \ encoder count excedes the value stored in X. LONG xe \ for local storage of encoder value pri CKRENC ( -- \ check right encoder ) BEGIN renc @ xe @ => UNTIL H ; pri CKLENC ( -- \ check left encoder ) BEGIN lenc @ xe @ => UNTIL H ; pub GF ( n -- \ go forward n inches ) ctin * xe ! zenc F CKLENC ; pub GB ( n -- \ go backward n inches ) ctin * xe ! zenc B CKRENC ; pub GODT ( n -- \ go for n/10 seconds at current speed and direction ) GO #100 * ms H ; pub GTR ( n -- \ turn right n degrees ) ctdeg * #100 / xe ! zenc TR CKLENC ; pub GTL ( n -- \ turn left n degrees ) ctdeg * #100 / xe ! zenc TL CKRENC ; pub GRR ( n -- \ rotate n degrees right ) ctdeg * #200 / xe ! zenc RR CKRENC ; pub GRL { n -- \ rotate n degress left ctdeg * 200 / xe ! zenc RL CKRENC ; \ Special Motions LONG DT 50 == dt 10 == dn pub VR ( -- \ veer right 10 degrees ) #10 GTR F ; pub VL ( -- \ veer left 10 degrees ) #10 GTL F ; pub RU ( -- \ ramp up speed in dt ms increments ) ldty @ dn + 0 DO I M1 I M2 dt ms dn +LOOP ; pub RD ( -- \ ramp down speed in dt ms increments ) ldty @ dn + 0 DO ldty @ I - M1 rdty @ I - M2 dt ms dn +LOOP S ; pub GSTEP ( n1 n2 -- \ move io preset direction & speed n1 steps at n2 ms increments ) DT ! 0 DO GO DT @ ms H DT @ ms LOOP ; { This Completes the RCL Core ******************************************************************************* } \ Maneuvers pri pt2 2000 ms ; pub MAN1 \ ( -- ) Exercise Basic Motions S2 F pt2 B pt2 RR pt2 RL pt2 TR pt2 TL pt2 BTR pt2 BTL pt2 H ; pri avoid \ execute basic avoidance maneuver 45 >pos 400 ms dist 400 ms -45 >pos 400 ms dist 400 ms > IF 45 GTL ELSE 45 GTR THEN ; BYTE Q# pub MAN2 ( -- \ simple avoidance ) 4 Q# C! S2 F BEGIN WHILE Q# C@ 0 > dist 10 < IF H 6 GB avoid 0 >pos Q# C-- F THEN REPEAT H ;