fl { Stepping Motor drive by using L6470 PropForth5.0/5.2 23/08/2012 08:54:45 } hex \ ( n1 n2 n3 n4 n5 -- n6 ) n1:sending data(8bit) n2:_SDI n3:_SDO n4:_CLK n5:_CS n6:receive data lockdict create a_RxTx forthentry $C_a_lxasm w, h130 h113 1- tuck - h9 lshift or here W@ alignl h10 lshift or l, z2WyPW1 l, zfiPZB l, z1SyLI[ l, z2WyPb1 l, zfiPeB l, z1SyLI[ l, z2WyPj1 l, zfiPmB l, z1SyLI[ l, z2WyPr1 l, zfiPuB l, z1SyLI[ l, zfyPOO l, z1[ixZC l, z2WyQ88 l, z1[ixZD l, zfy\r1 l, z2WiQ3E l, z1YiQ7l l, z20o\r1 l, zgyPO1 l, z1bfxZF l, z1[ZxZF l, z1bixZD l, z3[yQCY l, z1bixZC l, z2WiPSi l, z1SV01X l, 0 l, freedict 0 wconstant _CS1 \ motor1 1 wconstant _CS2 \ motor2 2 wconstant _CS3 \ motor3 3 wconstant _CLK 4 wconstant _SDI 5 wconstant _STCK 6 wconstant _RST 7 wconstant _SDO 8 wconstant _FLAG 9 wconstant _BUSY wvariable CS wvariable dir wvariable regbyte -2 allot 0 c, 3 c, 2 c, 3 c, 3 c, 2 c, 2 c, 2 c, 2 c, 1 c, 1 c, 1 c, 1 c, 2 c, 1 c, 1 c, 1 c, 1 c, 1 c, 1 c, 1 c, 2 c, 1 c, 1 c, 2 c, 2 c, \ decimal wvariable user_motor -2 allot 400 w, \ 400step/rotation 447 w, \ SPEED value at 1rpm 100 w, \ Acceleration 100 w, \ Deceleration 152 w, \ Maximum speed 0 w, \ Minimum 20 w, \ Holding current 32 w, \ Run current 64 w, \ Acceleration starting current 32 w, \ Deceleration starting current 1032 w, \ Intersect speed 128 w, \ Start slope 19 w, \ Acceleration final slope 64 w, \ Deceleration final slope 0 w, \ Thermal compensation 0 w, \ ADC (read only) 15 w, \ Over-current threshold 127 w, \ Stall Threshold (set to max) 300 w, \ Full step speed (changes from microstep) hex : CS_out_l CS W@ pinlo ; : CS_out_h CS W@ pinhi ; \ : _cs_out_l _CS pinlo ; \ : _cs_out_h _CS pinhi ; : _clk_out_l _CLK pinlo ; : _clk_out_h _CLK pinhi ; : _sdi_out_l _SDI pinlo ; : _sdi_out_h _SDI pinhi ; : _stck_out_l _STCK pinlo ; : _stck_out_h _STCK pinhi ; : _rst_out_l _RST pinlo ; : _rst_out_h _RST pinhi ; : FWD 1 dir W! ; : REV 0 dir W! ; : +DIR dir W@ or ; \ setup to use L6470 : setup _CS1 7 0 do dup pinout dup pinhi 1+ loop drop \ set port(from _CS1 to _RST) to output and high _rst_out_l 1 delms _rst_out_h \ reset ; \ ( n1 n2 -- n3 ) n1:value n2:register number n3:separate byte : separate regbyte + C@ \ parameter is 1byte 1 over = if drop FF and else \ parameter is 2byte 2 over = if drop dup FF and swap 8 rshift \ parameter is 3byte else drop dup FF and swap dup 8 rshift swap 10 rshift thens ; \ ( n1 -- n2 n3 n4 ) n1:value n1,n2,n3:separate n1 to 3byte : 3_separate dup FF and swap dup 8 rshift swap 10 rshift ; \ combine separate response \ ( value n1 -- n2) value:separate response n1:separate number n2:result : combine 1 over = if drop FF and else 2 over = if drop FF and swap 8 lshift or else drop FF and swap 8 lshift or swap 10 lshift or thens ; \ ( n1 -- n2) n1:command code n2:response : RxTx \ _cs_out_l CS_out_l 80 0 8 0 do _clk_out_l \ response data \ 1 lshift 1 _SDO lshift ina COG@ and if 1+ ." 1" else ." 0" then rot2 1 lshift 1 _SDO lshift ina COG@ and if 1+ then rot2 \ transmit data 2dup and if _sdi_out_h \ ." 1" else _sdi_out_l \ ." 0" then _clk_out_h 1 rshift rot loop \ _cs_out_h CS_out_h rot2 2drop \ cr ; \ ( n1 -- ) n1:command code : command RxTx drop ; \ : command _SDI _SDO _CLK CS W@ a_RxTx drop ; \ Using Assembler "a_RxTx" \ ( n1 n2 -- ) n1:patameter from 1byte to 3byte n2:parameter's number : param_out 0 do command loop ; \ ( n1 -- n2 ) n1:parameter's number n2:response from 1byte to 3byte : param_in 0 do 0 RxTx loop ; \ : param_in 0 do 0 _SDI _SDO _CLK CS W@ a_RxTx loop ; \ Using Assembler "a_RxTx" \ TYPE A only command : StepClock 58 +DIR command ; \ ( n -- ) n:act : ReleaseSW 3 lshift 92 or +DIR command ; : GoHome 70 command ; : GoMark 78 command ; : ResetPos D8 command ; : ResetDevice C0 command ; : SoftStop B0 command ; : HardStop B8 command ; : SoftHiZ A0 command ; : HardHiZ A8 command ; \ TYPE B command + 3_parameter(output) \ ( n -- ) n:speed : Run 3_separate 50 +DIR command 3 param_out ; \ ( n -- ) n:step : Move 3_separate 40 +DIR command 3 param_out ; \ ( n -- ) n:abs position : GoTo 3_separate 60 command 3 param_out ; \ ( n -- ) n: abs position : GoTo_DIR 3 separate 68 +DIR command 3 param_out ; \ (n1 n2 -- ) n1:speed n2:act : GoUntil >r 3_separate r> 3 lshift 82 or command 3 param_out ; \ TYPE C command + 2_parameter(input) \ ( -- n ) n:status(16bit) : GetStatus D0 command 2 param_in swap 8 lshift or ; \ TYPE D command(register) + output_parameter(1byte/2byte/3byte) \ ( n1 n2 -- n3 ) n1:value n2:register address n3:response : SetParam dup command dup >r separate r> regbyte + C@ param_out ; \ TYPE E command(register) + input_parameter(1byte/2byte/3byte) \ ( n1 -- n2) n1:register number n2:response : GetParam dup 20 or command regbyte + C@ dup >r param_in r> combine ; \ ( -- ) 1:ready 0:not ready Wait until controller is ready : ready? begin GetStatus 2 and until ; : TAB 9 emit ; : showRegister ready? hex ." ABS_POS" 1 GetParam TAB TAB . cr ." EL_POS" 2 GetParam TAB TAB . cr ." MARK" 3 GetParam TAB TAB . cr ." SPEED" 4 GetParam TAB TAB . cr ." ACC" 5 GetParam TAB TAB . cr ." DEC" 6 GetParam TAB TAB . cr ." MAX_SPEED" 7 GetParam TAB . cr ." MIN_SPEED" 8 GetParam TAB . cr ." KVAL_HOLD" 9 GetParam TAB . cr ." KVAL_RUN" A GetParam TAB . cr ." KVAL_ACC" B GetParam TAB . cr ." KVAL_DEC" C GetParam TAB . cr ." INT_SPD" D GetParam TAB TAB . cr ." ST_SLP" E GetParam TAB TAB . cr ." FN_SLP_ACC" F GetParam TAB . cr ." FN_SLP_DEC" 10 GetParam TAB . cr ." K_THERM" 11 GetParam TAB TAB . cr ." ADC_OUT" 12 GetParam TAB TAB . cr ." OCD_TH" 13 GetParam TAB TAB . cr ." STALL_TH" 14 GetParam TAB . cr ." FS_SPD" 15 GetParam TAB TAB . cr ." STEP_MODE" 16 GetParam TAB . cr ." ALARM_EN" 17 GetParam TAB . cr ." CONFIG" 18 GetParam TAB TAB . cr ." STATUS" 19 GetParam TAB TAB . cr cr decimal ; \ change step-mode \ ( n -- ) n:step mode 0=fullstep 1=halfstep 2=1/4microstep 3=1/8microstep 4=1/16microstep 5=1/32microstep 6=1/64microstep 7=1/128microstep : change_step_mode \ If current mode is same, do nothing ready? 16 GetParam 2dup 7 and <> if \ If not HiZ, do SoftHiZ GetStatus 1 and 0= if SoftHiZ then F8 and + 16 SetParam \ set ABS_POS to zero 0 1 SetParam else 2drop then \ Print step-mode ." step-mode=" 16 GetParam 7 and . cr ; \ ( n -- ) n:turns rotate motor for n times : turns user_motor W@ 16 GetParam 7 and lshift u* Move ; \ ( n -- ) n:Rotate Per Minute : rpm user_motor 2+ W@ \ Get user-motor's 1rpm constant u* Run ; \ -------------------- test code ---------------------------- \ Default 1/128-Microstep : demo setup _CS1 CS W! \ select motor1 FWD 5000 Move showRegister ; : Fulltest setup _CS1 CS W! \ select motor1 0 change_step_mode FWD 400 Run 1 delms 32 0 do 1 GetParam . cr \ print ABS_POS 64 delms loop SoftHiZ ; : microtest setup _CS1 CS W! \ select motor1 7 change_step_mode FWD 400 Run 1 delms 50 0 do 1 GetParam . cr \ print ABS_POS 64 delms loop SoftHiZ ; : test1 setup _CS1 CS W! \ select motor1 FWD 3E8 50 0 do dup Run 64 delms 4 GetParam . cr \ print speed(step/tick) 64 + loop SoftHiZ ; : turn_test setup _CS1 CS W! \ select motor1 FWD 7 change_step_mode ." 1 Turn by 1/128 Microstep" cr 1 turns 400 delms 6 change_step_mode ." 1 Turn by 1/64 Microstep" cr 1 turns 400 delms 5 change_step_mode ." 1 Turn by 1/32 Microstep" cr 1 turns 400 delms 4 change_step_mode ." 1 Turn by 1/16 Microstep" cr 1 turns 400 delms 3 change_step_mode ." 1 Turn by 1/8 Microstep" cr 1 turns 400 delms 2 change_step_mode ." 1 Turn by 1/4 Microstep" cr 1 turns 400 delms 1 change_step_mode ." 1 Turn by Half step" cr 1 turns 400 delms 0 change_step_mode ." 1 Turn by Full step" cr 1 turns ; : rpm_test setup _CS1 CS W! \ select motor1 FWD 7 change_step_mode ." 1 rpm by 1/128 Microstep" cr 1 rpm 3D 0 do 3E8 delms loop SoftStop 400 delms 3 change_step_mode ." 1 rpm by 1/8 Microstep" cr 1 rpm 3D 0 do 3E8 delms loop SoftStop 400 delms 0 change_step_mode ." 1 rpm by Full step" cr 1 rpm 3D 0 do 3E8 delms loop SoftStop ; : change_setting setup _CS1 CS W! \ select motor1 showRegister user_motor 4 + 16 5 do dup W@ i SetParam 2+ loop showRegister ; decimal { ( n1 n2 n3 n4 n5 -- n6 ) n1:sending data(8bit) n2:_SDI n3:_SDO n4:_CLK n5:_CS n6:receive data fl build_BootOpt :rasm \ get cs_mask mov $C_treg1 , # 1 shl $C_treg1 , $C_stTOS spop \ get clk_mask mov $C_treg2 , # 1 shl $C_treg2 , $C_stTOS spop \ get sdo_mask mov $C_treg3 , # 1 shl $C_treg3 , $C_stTOS spop \ get sdi_mask mov $C_treg4 , # 1 shl $C_treg4 , $C_stTOS spop shl $C_stTOS , # d24 \ set _CS to Low andn outa , $C_treg1 mov $C_treg6 , # 8 __1 \ set _CLK to Low andn outa , $C_treg2 \ input bit from _SDO shl __result , # 1 mov $C_treg5 , $C_treg3 and $C_treg5 , ina wz if_nz add __result , # 1 \ output _SDI shl $C_stTOS , # 1 wc if_c or outa , $C_treg4 if_nc andn outa , $C_treg4 \ set _CLK to High or outa , $C_treg2 djnz $C_treg6 , # __1 \ set _CS to High or outa , $C_treg1 mov $C_stTOS , __result \ spop jexit __result 0 ;asm a_RxTx }