fl

hex

{
PropForth4.5

SteppingMotor driver
Using Motor TS3166N913 0.9degree/step 400step/revolution
25/07/2011 15:47:41

_B-phase     _motor     P16   0x10      
 B-phase     _motor+1   P17   0x11      
_A-phase     _motor+2   P18   0x12      
 A-phase     _motor+3   P19   0x13      

T = t/(step*rpm)
T:Pulse_time(1-phase) 
t:60seconds
step:400(step/revolution)
rpm:Revolusion per minute

-- Full step ---------------------------------------
T=60/(400*rpm)= 0.15/rpm
0.15=150msec=12000000ticks=0xb71b00

1rpm    --> 1-phase wait_time is 12000000ticks/1
10rpm   --> 1-phase wait_time is 12000000ticks/10
100rpm  --> 1-phase wait_time is 12000000ticks/100
----------------------------------------------------
-- Half step ---------------------------------------
T=60/(800*rpm)= 0.075/rpm
0.075=75msec=6000000ticks=0x5b8d80

1rpm    --> 1-phase wait_time is 6000000ticks/1
10rpm   --> 1-phase wait_time is 6000000ticks/10
100rpm  --> 1-phase wait_time is 6000000ticks/100
----------------------------------------------------

}

10 wconstant _motor
190 wconstant STEPS_PER_REV             \ 400 step per revolution for stepping motor 
variable step_mask 
wvariable full_step 9 c, a c, 6 c, 5 c,                          \ phase_data for full-step
wvariable half_step 9 c, 8 c, a c, 2 c, 6 c, 4 c, 5 c, 1 c,      \ phase_data for half-step

b71b00 constant base_pulse_time
variable pulse_time
variable current_rpm


\ initialize stepping motor ( -- )
: init_motor 
_motor 4 0 do dup pinout 1+ loop drop   
f _motor lshift invert outa COG@ and step_mask L!  
;

\ rotate(full-step) motor on desired rpm ( n1 -- )  n1:rpm
\                                                      positive CW
\                                                      negative CCW
: motor_rpm_full
init_motor
dup 0< if invert 1+ 0 else 1 then swap                                     \ check CW or CCW
base_pulse_time swap u/ dup pulse_time L! cnt COG@ + swap     
begin
     if        \ CW 
          STEPS_PER_REV 0 do i 4 u/mod drop                                \ get number(0 - 3) 
                              full_step 2+ + C@ _motor lshift              \ get address for stepping-motor-data
                              step_mask L@ or outa COG!                    \ set stepping-motor-data to motor_pot
                              pulse_time L@ waitcnt                        \ wait 1-pulse-time
          loop
          1

     else      \ CCW
          0 
          STEPS_PER_REV 0 do 3 + 4 u/mod drop dup                           \ get number(3 - 0) 
                              full_step 2+ + C@ _motor lshift               \ get address for stepping-motor-data
                              step_mask L@ or outa COG! swap                \ set stepping-motor-data to motor_pot
                              pulse_time L@ waitcnt swap                    \ wait 1-pulse-time
          loop
          drop 
          0
     thens         
0 until
;

\ rotate(half-step) motor on desired rpm ( n1 -- )  n1:rpm
\                                                      positive CW
\                                                      negative CCW
: motor_rpm_half
init_motor
dup 0< if invert 1+ 0 else 1 then swap                                     \ check CW or CCW
base_pulse_time 1 rshift swap u/ dup pulse_time L! cnt COG@ + swap     
begin
     if        \ CW 
          STEPS_PER_REV 1 lshift 0 do i 8 u/mod drop                                \ get number(0 - 7) 
                              half_step 2+ + C@ _motor lshift              \ get address for stepping-motor-data
                              step_mask L@ or outa COG!                    \ set stepping-motor-data to motor_pot
                              pulse_time L@ waitcnt                        \ wait 1-pulse-time
          loop
          1

     else      \ CCW
          0 
          STEPS_PER_REV 1 lshift 0 do 7 + 8 u/mod drop dup                           \ get number(7 - 0) 
                              half_step 2+ + C@ _motor lshift               \ get address for stepping-motor-data
                              step_mask L@ or outa COG! swap                \ set stepping-motor-data to motor_pot
                              pulse_time L@ waitcnt swap                    \ wait 1-pulse-time
          loop
          drop 
          0
     thens         
0 until
;

: encoder_loop begin encoder 0 until ;

\ Demo for full-step by using rotary-encoder
: full
0 counter L!                            \ initialize encoder_counter
0 current_rpm  L!
read_encoder prev W!                    \ read initial value
c" 0 motor_rpm_full" 5 cogx             \ start motor
c" encoder_loop" 4 cogx                 \ start encoder
begin
     counter L@ current_rpm L@ <>
     if 
          5 cogreset                              \ stop stepping_motor
          a delms
          c" counter L@ motor_rpm_full" 5 cogx
          counter L@ dup current_rpm  L! . cr    
     then
     200 delms
0 until
5 cogreset
4 cogreset
; 

\ Demo for half-step by using rotary-encoder
: half
0 counter L!                            \ initialize encoder_counter
0 current_rpm  L!
read_encoder prev W!                    \ read initial value
c" 0 motor_rpm_half" 5 cogx             \ start motor
c" encoder_loop" 4 cogx                 \ start encoder
begin
     counter L@ current_rpm L@ <>
     if 
          5 cogreset                              \ stop stepping_motor
          a delms
          c" counter L@ motor_rpm_half" 5 cogx    \ start motor     value of encoder multiple 2
          counter L@ dup current_rpm  L! . cr    
     then
     200 delms
0 until
5 cogreset
4 cogreset
; 




: demo1        \ Demo for full-step(1 revolution CW/CCW) & Half-step(1 revolution CW/CCW)
_motor 4 0 do dup pinout 1+ loop drop   
f _motor lshift invert outa COG@ and step_mask L! 
\ full-step 
 STEPS_PER_REV 0 do i 4 u/mod drop full_step 2+ + C@ _motor lshift step_mask L@ or outa COG! 5 delms loop
 0 STEPS_PER_REV 0 do 3 + 4 u/mod drop dup full_step 2+ + C@ _motor lshift step_mask L@ or outa COG! 5 delms loop drop
\ half step
STEPS_PER_REV 2 u*  0 do i 8 u/mod drop half_step 2+ + C@ _motor lshift step_mask L@ or outa COG! 5 delms loop
0 STEPS_PER_REV 2 u* 0 do 7 + 8 u/mod drop dup half_step 2+ + C@ _motor lshift step_mask L@ or outa COG! 5 delms loop drop
;

: test1         \ 2 revolution to CW ( n1 -- ) for full-step   n1:rpm
_motor 4 0 do dup pinout 1+ loop drop   
f _motor lshift invert outa COG@ and step_mask L!  
base_pulse_time swap u/ dup pulse_time L! st? cnt COG@ +  
2 0 do
STEPS_PER_REV 0 do  i 4 u/mod drop full_step 2+ + C@ _motor lshift step_mask L@ or outa COG!  pulse_time L@ waitcnt loop
loop
st?
drop
;

: test2        \ 2 revolution to CCW ( n1 -- ) for full-step    n1:rpm
_motor 4 0 do dup pinout 1+ loop drop   
f _motor lshift invert outa COG@ and step_mask L!  
base_pulse_time swap u/ dup pulse_time L! st? cnt COG@ +  
2 0 do
0 STEPS_PER_REV 0 do 3 + 4 u/mod drop dup full_step 2+ + C@ _motor lshift step_mask L@ or outa COG! swap 
pulse_time L@ waitcnt swap loop drop
loop
st?
drop
;



: full_limit   \ (n -- )  check loop-time
init_motor
dup pulse_time L! cnt COG@ +      
          STEPS_PER_REV 0 do i 4 u/mod drop                                \ get number(0 - 3) 
                              full_step 2+ + C@ _motor lshift              \ get address for stepping-motor-data
                              step_mask L@ or outa COG!                    \ set stepping-motor-data to motor_pot
                              pulse_time L@ waitcnt                        \ wait 1-pulse-time
          loop
;

: half_limit   \ (n -- )  check loop-time
init_motor
dup pulse_time L! cnt COG@ +   
          STEPS_PER_REV 1 lshift 0 do i 8 u/mod drop                                \ get number(0 - 7) 
                              half_step 2+ + C@ _motor lshift              \ get address for stepping-motor-data
                              step_mask L@ or outa COG!                    \ set stepping-motor-data to motor_pot
                              pulse_time L@ waitcnt                        \ wait 1-pulse-time
          loop
;

decimal