fl { PropForth5.5 SteppingMotor driver Using Motor SPG20-1362(360step/revolution) Unipolar type 2013/03/29 23:09:03 TD62083 SPG20-1362 ------- -------------- P19 ------|I1 O1|-----------| phai_1 | P18 ------|I2 O2|-----------| phai-2 COM |--- +5V(Motor Power) P17 ------|I3 O3|-----------| _phai-1 | P16 ------|I4 O4|-----------| _phai-2 | GND ------|GND COM|---| -------------- ------- | +5V(Motor Power) phai_1 _motor P19 phai-2 _motor+1 P18 _phai-1 _motor+2 P17 _phai-2 _motor+3 P16 rpm[rot/min] = (step-angle[degree]/360[degree]) * pulse_freq[Hz] * 60[sec] A = step-angle[degree] = 1 [SPG20-1362] f = pulse_freq[Hz] rpm = (A/6) * f --> T = 1/f = A/(6 * rpm) f(Hz or pps) = 6 * rpm step velocity is f(Hz or pps) . Create a structure, either a halfstep, or full step, specify the pins, 16 17 18 & 19 in this case \ For half stepping d16 d300 d10 step_create_halfstep moth \ Initialize moth step_init \ Step forward (CW) d4096 moth step \ Step reverse (CCW) d-4096 moth step \ Turn off drivers moth step_sleep \ Turn on drivers moth step_active \ Set the step speed (If target_velocity is bigger than max_velocity, setted to max_velocity.) d400 moth step_setspeed \ Set accel/de-accel d10 motf step_setaccel \ or \ For full stepping d16 d300 d10 step_create_fullstep motf \ Initialize motf step_init \ Step forward d2048 motf step (CW) \ Step reverse d-2048 motf step (CCW) \ Turn off drivers motf step_sleep \ Turn on drivers motf step_active \ Set the step speed (If target_velocity is bigger than max_velocity, setted to max_velocity.) d400 motf step_setspeed \ Set accel/de-accel d10 motf step_setaccel index of step structure 00 - 03 -- current step position - steps 04 - 07 -- current step velocity - steps / sec 08 - 0B -- max step velocity - steps / sec (should be a multiple of step acceleration) 0C - 0F -- taget velocity - steps / sec 10 - 13 -- step acceleration - steps/sec/sec 14 - 33 -- stepper motor output mask(32bits) 34 - 37 -- 8 longs - step values (steppermotor output-pattern) HalfStep:8Longs FullStep:4Longs 38 -- steps during accel/de-accel 3A -- step position mask Operation inside word"step" 1) 0 < L < n /\ / \ / \ / \ / \ / \ / \ <-L/2-><-L/2-> accel de-accel 2) n < L <= 2n ________________ / \ / \ / \ / \ / \ / \ / \ <-n/2-><----- L-n ----><-n/2-> accel constant-speed de-accel 3) 2n < L _____________________________ / \ / \ / \ / \ / \ / \ / \ <- n -><---------- L-2n -----------><- n -> accel constant-speed de-accel } \ Uncomment when debug-mode \ 1 wconstant step_debug \ wvariable sw wvariable full_step -2 allot 3 c, 6 c, hC c, 9 c, \ phase_data for full-step wvariable half_step -2 allot 1 c, 3 c, 2 c, 6 c, 4 c, hC c, 8 c, 9 c, \ phase_data for half-step h0 wconstant _step_position h4 wconstant _step_current_velocity h8 wconstant _step_max_velocity hC wconstant _step_target_velocity h10 wconstant _step_accel h14 wconstant _step_mask h18 wconstant _step_bits h38 wconstant _steps h3A wconstant _step_position_mask h3B wconstant _step_size \ Display index of step structure \ step_dump ( addr -- ) addr:moth or motf : step_dump ." current_position: " dup L@ . cr ." current_velocity: " dup _step_current_velocity + L@ . cr ." max_velocity: " dup _step_max_velocity + L@ . cr ." target_velocity: " dup _step_target_velocity + L@ . cr ." accel: " dup _step_accel + L@ . cr ." steps during accel/de-accel: " dup _steps + W@ . cr ." position_mask: " dup _step_position_mask + C@ . cr base W@ swap hex ." mask: " dup _step_mask + L@ .long cr ." bits: " _step_bits + h20 bounds do i L@ .long space 4 +loop base W! cr ; \ step_dump1 ( addr -- ) addr:moth or motf : step_dump1 ." current_position: " dup L@ . ." current_velocity: " dup _step_current_velocity + L@ . ." max_velocity: " dup _step_max_velocity + L@ . ." accel: " _step_accel + L@ . ; \ Clear outa(motor-pin) \ step_sleep ( addr -- ) addr:moth or motf : step_sleep _step_mask + L@ outa COG@ swap andn outa COG! ; \ Set position and Set power to motor-pin \ step_active( addr -- ) : step_active dup _step_position + L@ \ Get _step_position over _step_position_mask + C@ \ Get _step_position_mask and 4* over + _step_bits + L@ \ get steppermotor output-pattern swap _step_mask + L@ \ Get _step_mask outa COG@ swap andn or outa COG! \ Set power to motor-pin ; \ Get next vel(ticks) \ _step_time( addr -- ticks) addr:moth or motf : _step_time dup _step_current_velocity + L@ \ Get _step_current_velocity ( addr current_vel ) over _step_accel + L@ + \ Get _step_accel ( addr next_vel ) swap _step_max_velocity + L@ min \ Compare next_velocity and max_velocity ( compared_velocity ) clkfreq swap u/ \ Get ticks ; \ Update _step_position and _step_current_velocity \ ( addr +-1 -- addr ) addr:moth or motf : __step over _step_position + dup L@ rot + swap L! \ Update _step_position +-1 dup \ ( addr addr ) _step_current_velocity + L@ dup \ ( addr [current_velocity] [current_velocity] ) 2 ST@ _step_target_velocity + L@ < \ ( addr [current_velocity] 1/0 ) if over _step_accel + L@ + \ current_velocity < target_velocity else over _step_accel + L@ - \ current_velocity >= target_velocity dup 0< if drop 0 then then \ ( addr [current_velocity+accel] ) over _step_current_velocity + L! \ Update _step_current_velocity \ ( addr ) dup step_active \ Set position and Set power to motor-pin ; \ Update _step_position and _step_current_velocity \ ( +-1 addr * * -- +-1 addr * * ) addr:moth or motf : __step1 2 ST@ _step_position + dup L@ 5 ST@ + swap L! \ Update _step_position +-1 2 ST@ _step_current_velocity + L@ dup \ ( +-1 addr * * [current_velocity] [current_velocity] ) 4 ST@ _step_target_velocity + L@ \ ( +-1 addr * * [current_velocity] [current_velocity] [target_velocity] ) 2dup <> \ Skip if current_velocity=target_velocity if < if 3 ST@ _step_accel + L@ + \ Accelaration else 3 ST@ _step_accel + L@ - \ De-acceleration dup 0< if drop 0 then then \ ( +-1 addr * * [current_velocity+accel] ) 3 ST@ _step_current_velocity + L! \ Update _step_current_velocity else 3drop then \ ( +-1 addr * * ) 2 ST@ step_active \ Set position and Set power to motor-pin ; : __step_constant over _step_position + dup L@ rot + swap L! dup step_active ; \ Set motor-pin to output mode \ step_init ( addr -- ) addr:moth or motf : step_init dup _step_mask + L@ dira COG@ or dira COG! dup _step_position + 0 swap L! dup _step_current_velocity + 0 swap L! step_sleep ; \ Create step structure and clear area of step structure \ ( -- ) : _step_create lockdict variable _step_size allot lastnfa nfa>pfa 2+ alignl freedict \ Clear step-structure dup _step_size 0 fill ; \ Create a step-structure for HalfStep \ ( n1 n2 n3 -- ) n1:Top pin for motor n2:max step velocity - (steps/sec) n3:Steps for accel/de-accel \ step_create_halfstep name : step_create_halfstep over over u/ \ ( motor_top_pin max_vel steps accel ) _step_create \ ( motor_top_pin max_vel steps accel name's_addr ) tuck _step_accel + L! tuck _steps + W! tuck 2dup _step_max_velocity + L! _step_target_velocity + L! dup _step_position_mask + 7 swap C! over hF swap lshift over _step_mask + L! \ ( motor_top_pin name's_addr ) \ Create motor_bit_pattern for FullStep 8 0 do half_step i + C@ 2 ST@ lshift over i 4* _step_bits + + L! loop 2drop ; \ Set target_speed \ ( addr n -- ) addr:moth or motf n:target speed : step_setspeed over _step_max_velocity + L@ min swap _step_target_velocity + L! ; \ Set accel \ ( addr n -- ) addr:moth or motf n:accel : step_setaccel over _step_max_velocity + L@ over u/mod drop 0= \ Skip if (_step_max_velocity/accel) has remainder if over _step_max_velocity + L@ 2/ min \ max of accel is _step_max_velocity/2 dup 2 ST@ _step_accel + L! over _step_max_velocity + L@ swap u/ \ Calculate steps during accel/de-accel swap _steps + W! else ." Can't change accel." cr then ; \ Create a step-structure for FullfStep \ ( n1 n2 n3 -- ) n1:Top pin for motor n2:max step velocity - (steps/sec) n3:Steps for accel/de-accel \ step_create_fullstep name : step_create_fullstep over over u/ \ ( motor_top_pin max_vel steps accel ) _step_create \ ( motor_top_pin max_vel steps accel name's_addr ) tuck _step_accel + L! tuck _steps + W! tuck 2dup _step_max_velocity + L! _step_target_velocity + L! dup _step_position_mask + 3 swap C! over hF swap lshift over _step_mask + L! \ ( motor_top_pin name's_addr ) \ Create motor_bit_pattern for FullStep 4 0 do full_step i + C@ 2 ST@ lshift over i 4* _step_bits + + L! loop 2drop ; \ Display cnt and dT [ifdef step_debug : waitcnt over . dup . + cr ; ] d16 d800 d80 step_create_halfstep moth d16 d800 d80 step_create_fullstep motf \ Move step to CW/CCW \ step ( L addr -- ) L:step number(minus=CCW plus=CW) addr:moth or motf : step over 0< if -1 \ CCW else over 0> if 1 \ CW else 0 \ STOP then then \ ( L addr -1/0/1 ) dup 0<> \ Skip when equal 0 if swap rot abs \ ( +-1 addr L ) over dup _step_target_velocity + L@ swap _step_accel + L@ u/ \ ( +-1 addr L n ) n:Calculate steps by _step_target_velocity 2dup <= if \ case 0 < L <= n ( +-1 addr L n ) 2 ST@ _step_target_velocity + L@ >r \ Push _step_target_velocity drop dup 2/ swap \ ( +-1 addr L/2 L ) 2 ST@ _step_time cnt COG@ + \ Get cnt ( +-1 addr L/2 L cnt ) [ifdef step_debug sw W@ if moth else motf then dup _step_position + L@ .long space _step_current_velocity + L@ .long space cr ] \ Acceleration and De-acceleration swap 0 do \ ( +-1 addr L/2 cnt ) __step1 swap 1- dup 0= if 2 ST@ _step_target_velocity + 0 swap L! then swap [ifdef step_debug sw W@ if moth else motf then dup _step_position + L@ .long space _step_current_velocity + L@ .long space ] 2 ST@ _step_time \ ( +-1 addr * cnt cnt1 ) waitcnt loop \ ( +-1 addr * cnt ) else 2dup 2* > if \ case 2n < L ( +-1 addr L n ) 2 ST@ _step_target_velocity + L@ >r \ Push _step_target_velocity 2dup - nip swap \ ( +-1 addr L-n L ) 2 ST@ _step_time cnt COG@ + \ Get cnt ( +-1 addr L-n L cnt ) [ifdef step_debug sw W@ if moth else motf then dup _step_position + L@ .long space _step_current_velocity + L@ .long space cr ] \ Acceleration and Condtant and De-acceleration swap 0 do \ ( +-1 addr L-n cnt ) __step1 swap 1- dup 0= if 2 ST@ _step_target_velocity + 0 swap L! then swap [ifdef step_debug sw W@ if moth else motf then dup _step_position + L@ .long space _step_current_velocity + L@ .long space ] 2 ST@ _step_time \ ( +-1 addr * cnt cnt1 ) waitcnt loop \ ( +-1 addr * cnt ) else \ case n < L <= 2n ( +-1 addr L n ) 2 ST@ _step_target_velocity + L@ >r \ Push _step_target_velocity 2 ST@ _step_target_velocity + dup L@ 2/ swap L! drop over dup _step_target_velocity + L@ \ Re-calculate n swap _step_accel + L@ \ ( +-1 addr L target_velocity accel ) u/ \ ( +-1 addr L n/2 ) over swap - swap \ ( +-1 addr L-n/2 L ) 2 ST@ _step_time cnt COG@ + \ Get cnt ( +-1 addr L-n/2 L cnt ) [ifdef step_debug sw W@ if moth else motf then dup _step_position + L@ .long space _step_current_velocity + L@ .long space cr ] \ Acceleration and Condtant and De-acceleration swap 0 do \ ( +-1 addr L-n/2 cnt ) __step1 swap 1- dup 0= if 2 ST@ _step_target_velocity + 0 swap L! then swap [ifdef step_debug sw W@ if moth else motf then dup _step_position + L@ .long space _step_current_velocity + L@ .long space ] 2 ST@ _step_time \ ( +-1 addr * cnt cnt1 ) waitcnt loop \ ( +-1 addr * cnt ) then then r> 3 ST@ _step_target_velocity + L! \ Pop _step_target_velocity 2drop 2drop else 2drop then ; \ Test \ ( n -- ) n:total steps [ifdef step_debug : half_test 1 sw W! moth step_init moth d500 step_setspeed moth step_dump ." position velocity cnt dT" cr moth step moth step_dump ; : full_test 0 sw W! motf step_init motf d400 step_setspeed motf step_dump ." position velocity cnt dT" cr motf step motf step_dump ; ]