fl { SteppingMotor driver Using Motor SPG20-1362(360step/revolution) Unipolar type PropForth5.5 2013/04/08 20:37:15 Front-Left and Rear-Left Front_right and Rear-Right TD62083 SPG20-1362 TD62083 SPG20-1362 ------- -------------- ------- -------------- P20 ------|I1 O1|-----------| phai_1 | P19 ------|I1 O1|-----------| phai_1 | P21 ------|I2 O2|-----------| phai-2 COM |-- +10V(Motor Power) P18 ------|I2 O2|-----------| phai-2 COM |-- +10V(Motor Power) P22 ------|I3 O3|-----------| _phai-1 | P17 ------|I3 O3|-----------| _phai-1 | P23 ------|I4 O4|-----------| _phai-2 | P16 ------|I4 O4|-----------| _phai-2 | | | -------------- | | -------------- | | SPG20-1362 | | SPG20-1362 | | -------------- | | -------------- P20 ------|I5 O5|-----------| phai_1 | P19 ------|I5 O5|-----------| phai_1 | P21 ------|I6 O6|-----------| phai-2 COM |-- +10V(Motor Power) P18 ------|I6 O6|-----------| phai-2 COM |-- +10V(Motor Power) P22 ------|I7 O7|-----------| _phai-1 | P17 ------|I7 O7|-----------| _phai-1 | P23 ------|I8 O8|-----------| _phai-2 | P16 ------|I8 O8|-----------| _phai-2 | GND ------|GND COM|---| -------------- GND ------|GND COM|---| -------------- ------- | ------- | +10V(Motor Power) +10V(Motor Power) 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) . Proceeding-distance per 1step SPG20-1362's step_angle is 1[degree] Step-distance[mm] = (tire_diameter[mm] X Pi)/(360[degree)/step_angle) 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 motor_status 00 - 03 -- Front(Rear)-Left step_velocity (Cog0) 04 - 07 -- Front(Rear)-Left steps (Cog0) 08 - 0B -- Front(Rear)-Right step_velocity (Cog1 ) 0C - 0F -- Front(Rear)-Right steps (Cog1 ) struct_addr 00 - 03 -- step structure addr for Front(Rear)-Left 04 - 07 -- step structure addr for Front(Rear)-Right } \ Uncomment when debug-mode \ 1 wconstant step_debug \ **************************************************************************** \ *********** User setting ************************************************** \ Step-distance[mm/step] = (tire_diameter[mm] X Pi)/(360[degree)/step_angle) variable D d3140 wconstant Pi d37 wconstant diameter 1 wconstant step_angle diameter Pi u* d360 step_angle u/ u/ D L! \ Wheel width d100 wconstant wheel_w \ Wheel distance d120 wconstant wheel_d \ **************************************************************************** \ **************************************************************************** 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 variable motor_status d12 allot variable struct_addr \ 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 ; \ 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 \ ( +-1 addr * -- +-1 addr * ) addr:moth* or motf* : __step1 over _step_position + dup L@ 4 ST@ + swap L! \ Update _step_position +-1 over _step_current_velocity + L@ dup \ ( +-1 addr * [current_velocity] [current_velocity] ) 3 ST@ _step_target_velocity + L@ \ ( +-1 addr * [current_velocity] [current_velocity] [target_velocity] ) 2dup <> \ Skip if current_velocity=target_velocity if < if 2 ST@ _step_accel + L@ + \ Accelaration else 2 ST@ _step_accel + L@ - \ De-acceleration dup 0< if drop 0 then then \ ( +-1 addr * [current_velocity+accel] ) 2 ST@ _step_current_velocity + L! \ Update _step_current_velocity else 3drop then \ ( +-1 addr * ) over step_active \ Set position and Set power to motor-pin ; \ 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) is odd 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 ; ] \ Front(Rear) Left -- cog0 ( top_pin max_velocity accel_steps ) d16 d800 d80 step_create_halfstep moth0 \ Front(Rear) Right -- cog1 ( top_pin max_velocity accel_steps ) d20 d800 d80 step_create_halfstep moth1 \ Move step to CW/CCW \ step1 ( L addr -- ) L:step number(minus=CCW plus=CW) addr:moth or motf : step1 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 _step_time cnt COG@ + \ Get cnt ( +-1 addr L cnt ) [ifdef step_debug cogid 2* struct_addr + W@ dup _step_position + L@ .long space _step_current_velocity + L@ .long space cr ] \ Acceleration and Constant and De-acceleration \ ." loop start:" st? swap 0 do \ ( +-1 addr cnt ) \ ." step1-A" st? __step1 \ ." i" i . \ step1-B" st? \ Exit do-loop if current_velocity is 0 over _step_current_velocity + L@ 0= if leave then [ifdef step_debug cogid 2* struct_addr + W@ dup _step_position + L@ .long space _step_current_velocity + L@ .long space ] over _step_time \ ( +-1 addr cnt cnt1 ) waitcnt loop \ ( +-1 addr cnt ) \ ." loop end:" st? then 3drop ; \ Operation for stepmotor on each Cog : motor \ Get moth* address cogid 0= if moth0 else moth1 then \ Save moth* addr dup cogid 2* struct_addr + W! \ Initialize motor step_init begin begin cogid \ Get id 8 u* motor_status 2dup + L@ \ Get velocity >r + 4+ L@ \ Get steps r> \ ( steps velocity ) 2dup + 0= if 2drop 0 else 1 then until \ Clear velocity and steps cogid 8 u* motor_status + dup 0 swap L! 4+ 0 swap L! \ motor_status 16 dump \ Get moth* address cogid 2* struct_addr + W@ \ ( steps speed moth* ) dup rot swap _step_target_velocity + L! \ Update _step_target_velocity ( steps moth* ) step1 0 until ; : main \ Clear motor_status area motor_status d16 0 fill 500 motor_status L! -2000 motor_status 4+ L! \ Right 500 motor_status 8 + L! 2000 motor_status d12 + L! \ left c" motor" 0 cogx \ Front(Rear)-Left c" motor" 1 cogx \ Front(Rear)-Right begin motor_status dup L@ swap 4+ L@ + motor_status 8 + dup L@ swap 4+ L@ + + 0= if 0 motor_status L! -200 motor_status 4+ L! 0 motor_status 8 + L! 200 motor_status d12 + L! ." update done" 1 else 0 then st? until ; : main1 \ Clear motor_status area motor_status d16 0 fill 500 motor_status L! 670 motor_status 4+ L! \ Right 500 motor_status 8 + L! 670 motor_status d12 + L! \ left \ c" motor" 0 cogx \ Front(Rear)-Left \ c" motor" 1 cogx \ Front(Rear)-Right begin motor_status dup L@ swap 4+ L@ + motor_status 8 + dup L@ swap 4+ L@ + + 0= if 0 motor_status L! 50 motor_status 4+ L! 0 motor_status 8 + L! 50 motor_status d12 + L! ." update done" 1 else 0 then st? until ; \ Move step to CW/CCW \ step2 ( L addr -- ) L:step number(minus=CCW plus=CW) addr:moth or motf : step2 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 _step_time cnt COG@ + \ Get cnt ( +-1 addr L cnt ) [ifdef step_debug moth0 dup _step_position + L@ .long space _step_current_velocity + L@ .long space cr ] \ Acceleration and Constant and De-acceleration \ ." loop start:" st? swap 0 do \ ( +-1 addr cnt ) \ ." step1-A" st? __step1 \ ." i" i . \ step1-B" st? \ Exit do-loop if current_velocity is 0 over _step_current_velocity + L@ 0= if leave then [ifdef step_debug moth0 dup _step_position + L@ .long space _step_current_velocity + L@ .long space ] over _step_time \ ( +-1 addr cnt cnt1 ) waitcnt loop \ ( +-1 addr cnt ) \ ." loop end:" st? then 3drop ; : test ." position velocity cnt dT" cr moth0 step_init \ target-speed=d500 steps=d670 d500 moth0 _step_target_velocity + L! d670 moth0 step2 0 moth0 _step_target_velocity + L! d50 moth0 step2 ;