{ Tachyon (Juno) S3 Robot Copntrol Language XBee and PI\ing Added } pub (TRCS3) ; DECIMAL \ Scaled S3 Parameters - 3.125 inch wheel diameter - 5.75 inch between wheel centers #9817 == wcirc \ *1000 #1015 == cts \ number counts per wheel rotation #10337 == ctin \ *100 counts per inch #1806 == rcirc \ * 100 rotation circumference #3612 == tcirc \ *100 #518 == ctdeg \ *100 - rotation #1036 == ctdeg \ *100 -turn \ S3 Motor Control Parameters #P13 == LE \ left encoder pin #P14 == RE \ right encoder pin #P16 == LD \ left direction pin #P17 == RD \ right direction pin #P18 == LW \ left motor control pin #P19 == RW \ rght motor control pin LONG lduty, rduty, lenc, renc, xl, xr, lvel, rvel, xe, dt #256 LONGS pwms LW 2 MASKS == pmio #5000 == freq 60 == ls1 \ speed settings range: 0 - 255 59 == rs1 100 == ls2 102 == rs2 150 == ls3 153 == rs3 200 == ls4 203 == rs4 \ Set Wheel Directions pub FRWD 1 LD PIN! 1 RD PIN! ; pub BKWD 0 LD PIN! 0 RD PIN! ; pub RRWD 1 LD PIN! 0 RD PIN! ; pub RLWD 0 LD PIN! 1 RD PIN! ; \ Set Wheel Speeds pub S1 ls1 lduty ! rs1 rduty ! ; pub S2 ls2 lduty ! rs2 rduty ! ; pub S3 ls3 lduty ! rs3 rduty ! ; pub S4 ls4 lduty ! rs4 rduty ! ; pub Sx \ ( n1 n2 -- ) where n1 is the left and n2 the right speed setting rduty ! lduty ! ; \ Motor Control pub M1 LW SETPWM ; pub M2 RW SETPWM ; pub GO \ ( -- ) execute to set S3 in motion lduty @ M1 rduty @ M2 ; pub S \ ( -- ) S3 stops setting duties to 0 0 lduty ! 0 rduty ! GO ; pub H \ ( -- ) halts S3 - executing GO continues current motion 0 M1 0 M2 ; pub MSTART pmio pwms freq PWM.START ; pub .duty lduty @ . SPACE rduty @ . CR ; \ Encoders pri rc_encoders 0 xl ! 0 xr ! BEGIN LE PIN@ DUP xl @ = IF DROP ELSE xl ! lenc ++ THEN RE PIN@ DUP xr @ = IF DROP ELSE xr ! renc ++ THEN 0 UNTIL ; pub ESTART ' rc_encoders TASK? RUN 50 ms ; pub .enc lenc @ . SPACE renc @ . CR ; pub .lenc lenc @ . ; pub .renc renc @ . ; pub zenc 0 lenc ! 0 renc ! ; \ Ping #P3 MASK == CPG LONG pdist pri rc_ping A 1 FRQ BEGIN CPG OUTCLR A #P3 APIN NCO #400 NEGATE PHSA COG! #1000 CTRMODE 0 PHSA COG! CPG INPUTS #50 ms PHSA COG@ #11800 / pdist ! 0 UNTIL ; pub .dist pdist @ . ; pub PSTART ' rc_ping TASK? RUN 50 ms ; \ Activating Cogs pub START MSTART ESTART PSTART ; pub R \ ( -- ) reboots system clearing current cog tasks REBOOT ; { End of TRCL Kernel } \ Indefinite Motions pub F \ ( -- ) S3 moves forward at current speed setting FRWD GO ; pub B \ ( -- ) S3 moves backwards BKWD GO ; pub RR \ ( -- ) S3 rotates to the right RRWD GO ; pub RL \ ( -- ) S3 rotates to leftS RLWD GO ; pub TR \ ( -- ) S3 turns right FRWD lduty @ LW SETPWM 0 RW SETPWM ; pub TL \ ( -- ) S3 turns left FRWD 0 LW SETPWM rduty @ RW SETPWM ; pub BTR \ ( -- ) S3 backs to right BKWD lduty @ LW SETPWM 0 RW SETPWM ; pub BTL \ ( -- ) S3 backs to left BKWD 0 LW SETPWM rduty @ RW SETPWM ; \ Defined Motions pri ckRENC xe ! BEGIN renc @ xe @ => UNTIL H ; pri ckLENC xe ! BEGIN lenc @ xe @ => UNTIL H ; pub GF ctin #1000 */ zenc F ckRENC ; pub GB ctin #1000 */ zenc B ckLENC ; pub GODT \ ( n -- ) moves for n tenth sec in specified direction at set speed GO #100 * ms H ; pub GTR cttdeg #100 */ zenc TR ckLENC ; pub GTL cttdeg #100 */ zenc TL ckRENC ; pub GRR ctrdeg #100 */ zenc RR ckRENC ; pub GRL ctrdeg #100 */ zenc RL ckLENC ; \ Special Motions pub VR \ ( --) S3 veers right 5 degrees #5 GTR F ; pub VL \ ( -- ) S3 veers ledt 5 degrees #5 GTL F ; pub RU \ ( -- ) S3 ramps up to selected speed setting at 50 ms intervals lduty @ 20 DO I lduty ! I rduty ! GO 50 ms 5 +LOOP ; pub RD \ ( -- ) S3 ramps down from current speed setting at 50 ms intervals BEGIN lduty @ 5 - lduty ! rduty @ 5 - rduty ! GO 50 ms lduty @ > 0 WHILE REPEAT ; pub GSTEP \ ( n1 n2 -- ) move n1 steps in set direction at n2 ms incrementsF dt ! FOR GO dt @ ms H dt @ ms LOOP ;