\ nuova versione con getione dei pin \ versione semplificata test nuovo \ +++++++++++++++++++++++++++++++ \ Start of user definable values \ +++++++++++++++++++++++++++++++ \ pin iniziali dello stepper #22 CONSTANT Lpin \ starting left pin #12 CONSTANT Rpin \ starting right pin #53 CONSTANT half_wheeldist \ half distance between the two wheels (mm) \ #22 ( 355 ) CONSTANT Pi_num \ the ratio between the two approximates Pi ( 9 bit ) \ #7 ( 113 ) CONSTANT Pi_den \ 22/7 ha 400ppm of error, 355/113 0.1ppm #206336 CONSTANT stepratio_num \ stepper motor ratio numerator ( 19 bit ) #405 CONSTANT stepratio_den \ stepper motor ratio denominator ( 9 bit) #66 CONSTANT wheeldia \ wheel diameter (mm) \ +++++++++++++++++++++++++++++++ \ end of user definable values \ +++++++++++++++++++++++++++++++ { given circle radius _r_ : nos (number of steps) = 2 Pi _r_ stepratio / ( wheeldia Pi) = 2 * _r_ * steprationum / (wheeldia * stepratioden) -> no risk of overflow with our ratio (up to radius=2600) for the inner wheel replace _r_ with _r_ - wheeldist } \ Step pin series 1100b CONSTANT step0 0110b CONSTANT step1 0011b CONSTANT step2 1001b CONSTANT step3 \ sets all stepper pins as output/input pub steppermask ( -- mask ) 1111b DUP Lpin SHL SWAP Rpin SHL OR ; pub setoutstepperpins steppermask OUTPUTS ; pub setinstepperpins steppermask ( mask ) 0 SHROUT \ all to low ( mask 0 ) DROP INPUTS \ all to input ( ) ; \ pause between each step pub pausastepper #2 ms ; pub getouta ( -- outa_values ) OUTA COG@ ; \ gets OUTA values pub setouta ( outavalues -- ) OUTA COG! ; \ sets OUTA values pub makestep ( step pin -- ) \ makes a step DUP \ creates a spare pin (step pin pin) 1111b SWAP SHL \ creates a mask (step pin mask) getouta SWAP INVERT AND \ clears the values in OUTA corresponding to the mask (step pin outa) ROT \ (pin outa step) ROT \ (outa step pin) SHL \ (outa stepmask) OR \ (outa) setouta ; \ sinistro pub lbackward step3 Lpin makestep pausastepper step2 Lpin makestep pausastepper step1 Lpin makestep pausastepper step0 Lpin makestep pausastepper ; pub lforward step0 Lpin makestep pausastepper step1 Lpin makestep pausastepper step2 Lpin makestep pausastepper step3 Lpin makestep pausastepper ; \ destro pub rbackward step0 Rpin makestep pausastepper step1 Rpin makestep pausastepper step2 Rpin makestep pausastepper step3 Rpin makestep pausastepper ; pub rforward step3 Rpin makestep pausastepper step2 Rpin makestep pausastepper step1 Rpin makestep pausastepper step0 Rpin makestep pausastepper ; \ double motors in sync pub dforward step0 Lpin makestep step3 Rpin makestep pausastepper step1 Lpin makestep step2 Rpin makestep pausastepper step2 Lpin makestep step1 Rpin makestep pausastepper step3 Lpin makestep step0 Rpin makestep pausastepper ; pub dbackward step3 Lpin makestep step0 Rpin makestep pausastepper step2 Lpin makestep step1 Rpin makestep pausastepper step1 Lpin makestep step2 Rpin makestep pausastepper step0 Lpin makestep step3 Rpin makestep pausastepper ; \ double motors opposite directions pub drforward step0 Lpin makestep step0 Rpin makestep pausastepper step1 Lpin makestep step1 Rpin makestep pausastepper step2 Lpin makestep step2 Rpin makestep pausastepper step3 Lpin makestep step3 Rpin makestep pausastepper ; pub drbackward step3 Lpin makestep step3 Rpin makestep pausastepper step2 Lpin makestep step2 Rpin makestep pausastepper step1 Lpin makestep step1 Rpin makestep pausastepper step0 Lpin makestep step0 Rpin makestep pausastepper ; pub calculate_steps ( radius - steps ) { version with rounding 2 * stepratio_num * #10 * \ numerator is 10 times bigger stepratio_den / wheeldia / \ we have number of steps * 10 5 + #10 / \ rounding done } 2 * stepratio_num * \ numerator stepratio_den / wheeldia / \ we have number of steps DUP . ." - " ; \ docircle routine pub docircle ( radius dirflag - ) setoutstepperpins SWAP DUP ( dirflag radius radius - ) \ radius if ar half the robot half_wheeldist - calculate_steps ( dirflag radius internalsteps ) SWAP ( dirflag internalsteps radius ) half_wheeldist + calculate_steps ( dirflag internalsteps externalsteps ) SWAP OVER ( dirflag externalsteps internalsteps externalsteps ) \ now we simply repeat the operation for the number of external steps 0 SWAP ( dirflag externalsteps internalsteps accumulator externalsteps ) FOR ( dirflag externalsteps internalsteps accumulator ) \ repeat loop externalsteps times OVER + ( dirflag externalsteps internalsteps accumulator ) \ accumulator is with internalsteps added ROT ( dirflag internalsteps accumulator externalsteps ) 2DUP ( dirflag internalsteps accumulator externalsteps accumulator externalsteps ) \ to compare if we accumulated enough to step both < ( dirflag internalsteps accumulator externalsteps flag ) IF ( dirflag internalsteps accumulator externalsteps) \ not yet.. 4TH \ get the direction flag IF ( dirflag internalsteps accumulator externalsteps) lforward \ ." right" ELSE rforward \ ." left" THEN ROT ROT ( dirflag externalsteps internalsteps accumulator ) \ ..cleaning ELSE ( dirflag internalsteps accumulator externalsteps) \ both motors dforward \ ." both" SWAP OVER - ( dirflag internalsteps externalsteps accumulator) \ new accumulated value SWAP ROT ROT ( dirflag externalsteps internalsteps accumulator ) \ ..cleaning THEN \ end of the conditional statement, ready for a new iteration NEXT ( dirflag externalsteps internalsteps accumulator ) \ end of the loop 2DROP 2DROP \ cleaning the stack setinstepperpins ; \ left and right circle call the same function, with 0 or 1 in the stack to indicate the direction pub leftcircle ( radius - ) 0 docircle ( radius dirflag - ) ; pub rightcircle ( radius - ) 1 docircle ( radius dirflag - ) ; DECIMAL \ 120 leftcircle pub lavanti 100 FOR lforward NEXT ; pub ravanti 100 FOR rforward NEXT ; pub davanti 100 FOR dforward NEXT ; pub eight ( 2ndradius_right 1stradius_left ) leftcircle rightcircle ;