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