{ Tachyon (Juno) Robot Control for BOE Platform + Prop Activity Board Hardware includes standard servo+Ping, wheel encoders, high speed continuous servos, XBee (dout - P11 din - P10), and a 7.4V Li-Ion rechargeable battery pack. } pub (TBOEP) ; DECIMAL \ Robot Platform Constants - 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 #15130 == ld0 \ sets zero position of left servo #15255 == 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 #150 == ls1 #150 == rs1 #250 == ls2 #250 == rs2 #500 == ls3 #500 == rs3 #750 == ls4 #750 == rs4 \ Wheel Direction Controls 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 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 ! ; pub .duty ( -- \ displays current right & left duty counts ) lduty @ . SPACE rduty @ . CR ; 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 ( -- \ hard stop ) S0 GO ; pub H ( -- \ soft stop ) 0 lduty ! 0 rduty ! ; LONG lenc, renc, xl, xr \ encoder parameters #P14 #2 MASKS INPUTS pri rc_motors ( -- \ l/r servo pwm 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 left servo driver and encoder in a cog ) ' rc_motors TASK? RUN 50 ms ; \ Encoders Record Total Counts per Wheel Rotation 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 ; 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 ; pub R ( -- \ hardware reset - execute START to continue ) REBOOT ; pub START ( -- \ initialize and start drivers ) DECIMAL MSTART ESTART VSTART PSTART ; { End of RCL 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 ; \ In the following two functions, motion continues until the right/left \ encoder count excedes the value stored in X. LONG X \ for local storage of encoder value pri CKRENC ( -- \ check right encoder ) BEGIN renc @ X @ => UNTIL H ; pri CKLENC ( -- \ check left encoder ) BEGIN lenc @ X @ => UNTIL H ; pub GF ( n -- \ go forward n inches ) ctin * X ! zenc F CKLENC ; pub GB ( n -- \ go backward n inches ) ctin * X ! zenc B CKRENC ; pub GODT ( n -- \ go for n/10 seconds ) GO #100 * ms H ; pub GTR ( n -- \ turn right n degrees ) ctdeg * #100 / X ! zenc TR CKLENC ; pub GTL ( n -- \ turn left n degrees ) ctdeg * #100 / X ! zenc TL CKRENC ; pub GRR ( n -- \ rotate right n degrees ) ctdeg * #200 / X ! zenc RR CKRENC ; pub GRL ctdeg * 200 / X ! zenc RL CKRENC ; \ Special Motions LONG dt pub VR ( -- \ veer right 10 degrees ) #10 GTR F ; pub VL ( -- \ veer left 10 degrees ) #10 GTL F ; pub RU ( val -- \ ramp up speed in 100 ms increments ) 4 + 0 DO I M1 I M2 100 ms 4 +LOOP ; pub RD ( val -- \ ramp down speed in 100 ms increments ) 4 + 0 DO ldty @ I - M1 rdty @ I - M2 100 ms 4 +LOOP ; pub GSTEP ( n1 n2 -- \ move forward 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 pt2 ; pri avoid 45 >pos 400 ms dist 400 ms -45 >pos 400 ms dist 400 ms > IF 45 TL ELSE 45 TR THEN ; pub MAN2 BEGIN dist 12 < IF H 6 GB avoid 0 >pos F THEN ESC? UNTIL ;