Smoother (X,Y,Z) movement for 3D printers and CNC.
davidsaunders
Posts: 1,559
Firstly I must thank kwinn for the concept, for he solidified it in my mind.
This is a simple speed ratio based system for creating a linear movement in 3 dimentions, so long as the motors on all axises are equally capable under there respective loads. At present it does NOT have any acceleration, so it is still a bit limited.
I also need to write more of the Public methods for setting control parameters.
Though this is a quick response to the idea presented by kwin, and it takes a single cog. There is a slight delay between axis movements, though this is a lot better than using a modified form of Bresenham's Line Algorithm for the purpose, and this is also a lot smaller.
The best part is that this is mostly in PASM
.
If you notice any errors or anything that could be done better please let me know.
This is a simple speed ratio based system for creating a linear movement in 3 dimentions, so long as the motors on all axises are equally capable under there respective loads. At present it does NOT have any acceleration, so it is still a bit limited.
I also need to write more of the Public methods for setting control parameters.
Though this is a quick response to the idea presented by kwin, and it takes a single cog. There is a slight delay between axis movements, though this is a lot better than using a modified form of Bresenham's Line Algorithm for the purpose, and this is also a lot smaller.
The best part is that this is mostly in PASM
.'This is a simple stepper motor controler for a three axis device like a 3D printer.
'We are only using full stepping for this one.
CON
xBase = 0
yBase = 4
zBase = 8
xPinM = $000F 'Pin Mask for X axis stepper.
yPinM = $00F0 'Pin Mask for Y axis stepper.
zPinM = $0F00 'Pin Mask for Z axis stepper.
zrPin = 12 'Zeroing switch input.
xMax = $7FFFFFFF 'Furthest from zero allowed for X axis.
yMax = $7FFFFFFF 'Furthest from zero allowed for Y axis.
zMax = $7FFFFFFF 'Furthest from zero allowed for Z axis.
xDely = 156250
yDely = 156250
zDely = 156250
'The parameter offsets in the array that the cog reads:
pCX = 0
pCY = 1
pCZ = 2
pNX = 3
pNY = 4
pNZ = 5
pDX = 6
pDY = 7
pDZ = 8
pGo = 9
VAR
LONG DY, DX, DZ
LONG PBLCK[12]
LONG xDelay, yDelay, zDelay
PUB MoveTo(ax, ay, az)
DX := UABS(ax - PBLCK[pCX])
DY := UABS(ay - PBLCK[pCY])
DZ := UABS(az - PBLCK[pCZ])
IF DX > DY AND DX > DZ
PBLCK[pDX] := xDelay
PBLCK[pDy] := ((DX << 4) * ((DX / DY) << 4)) >> 4
PBLCK[pDz] := ((DX << 4) * ((DX / DZ) << 4)) >> 4
pBLCK[pNX] := ax
pBLCK[pNY] := aY
pBLCK[pNZ] := aZ
PBLCK[pGo] := 1
PRI UABS(v)
IF v < 0
v := -v
RETURN v
PUB Init
COGNEW(Main, @PBLCK)
xDelay := xDely
yDelay := yDely
zDelay := zDely
DAT
ORG
Main MOV DIRA, Msk
:Lp JMPRET GetP_R,GetP
OR CG, CG WZ
IF_NZ JMPRET DoLine_R, DoLine
IF_Z JMP #:Lp
'┌──────────────────────────────────────────────────────────────────┐
'│PROC: GetP │
'│ │
'│*DESCRIPTION: │
'│ Reads the parameter block from HUB RAM. Takes 432 to 446 clock │
'│ cycles. │
'│ │
'└──────────────────────────────────────────────────────────────────┘
GetP MOV Ptr, PAR 'PAR points at the Parameter block.
MOV Lptr, #CurX 'COG Address of first parameter.
MOV C, #12
:Lp MOVS :upd, ptr
ADD LPtr, #4
:upd RDLONG Lptr, Ptr
DJNZ C, #:Lp
GetP_R RET
'┌──────────────────────────────────────────────────────────────────┐
'│PROC: PutP │
'└──────────────────────────────────────────────────────────────────┘
PutP MOV Ptr, PAR 'PAR points at the Parameter block.
MOV Lptr, #CurX 'COG Address of first parameter.
MOV C, #12
:Lp MOVS :upd, ptr
ADD LPtr, #4
:upd WRLONG Lptr, Ptr
DJNZ C, #:Lp
PutP_R RET
'┌──────────────────────────────────────────────────────────────────┐
'│PROC: DoLine: │
'│ │
'│*DESCRIPTION: │
'│Moves X, Y, and Z based on the delays until the move is complete. │
'│ │
'│ │
'└──────────────────────────────────────────────────────────────────┘
DoLine MOV XT, CNT
MOV YT, CNT
MOV ZT, CNT
ADD XT, XDly
ADD YT, YDly
ADD YT, YDly
:Lp
CMP XT, CNT WZ,WC
IF_BE ADD XT, CNT
IF_BE JMPRET MvX_R, MvX
CMP YT, CNT WZ,WC
IF_BE ADD YT, CNT
IF_BE JMPRET MvY_R, MvY
CMP YT, CNT WZ,WC
IF_BE ADD YT, CNT
IF_BE JMPRET MvY_R, MvY
CMP CurX, X WZ,WC
IF_BE CMP CurY, Y WZ,WC
IF_BE CMP CurZ, Z
IF_BE JMPRET PutP_R,PutP
IF_BE RET
JMP #:LP
DoLine_R RET
'┌──────────────────────────────────────────────────────────────────┐
'│PROC: MvX: │
'│ │
'└──────────────────────────────────────────────────────────────────┘
MvX SUB X, CurX NR,WC,WZ
MOV T, Stps
IF_Z JMP MvX_R
IF_C SUB CurX, #1
IF_NC ADD CurX, #1
MOV T0, CurX
AND T0, #$3
SHL T, XBase
SHR T, T0
MOV T0, OUTA
ANDN T0, XMsk
JMP #Mv_T
'┌──────────────────────────────────────────────────────────────────┐
'│PROC: MvY: │
'│ │
'└──────────────────────────────────────────────────────────────────┘
MvY SUB Y, CurY NR,WC,WZ
MOV T, Stps
IF_Z JMP MvX_R
IF_C SUB CurY, #1
IF_NC ADD CurY, #1
MOV T0, CurY
AND T0, #$3
SHL T, YBase
SHR T, T0
MOV T0, OUTA
ANDN T0, YMsk
JMP #Mv_T
'┌──────────────────────────────────────────────────────────────────┐
'│PROC: MvZ: │
'│ │
'└──────────────────────────────────────────────────────────────────┘
MvZ SUB Z, CurZ NR,WC,WZ
MOV T, Stps
IF_Z JMP MvX_R
IF_C SUB CurZ, #1
IF_NC ADD CurZ, #1
MOV T0, CurZ
AND T0, #$3
SHL T, ZBase
SHR T, T0
MOV T0, OUTA
ANDN T0, ZMsk
Mv_T OR T0, T
MOV OUTA, T0
MvX_R
MvY_R
MvZ_R RET
CurX LONG 0 'The current X Position in steps.
CurY LONG 0 'The current Y Position in steps.
CurZ LONG 0 'The current Z Position in steps.
X LONG 0 'The requested X position in steps.
Y LONG 0 'The requested Y position in steps.
Z LONG 0 'The requested Z position in steps.
XDly LONG xDely 'The delay between X steps, in clock cycles.
YDLY LONG yDely 'The delay between Y steps, in clock cycles.
ZDLY LONG zDely 'The delay between Z steps, in clock cycles.
CG LONG 0 'A flag to tell us when to go.
C LONG 0 'Used in loops as an index.
T LONG 0 'Used to store in process values.
T0 LONG 0
XT LONG 0
YT LONG 0
ZT LONG 0
Stps LONG %0011_0110_1100_1001 'The step output values.
Msk LONG (xPinM | yPinM | zPinM)
XMsk LONG xPinM
YMsk LONG yPinM
ZMsk LONG zPinM
Lptr LONG 0
Ptr LONG 0
'The states of a the four pins to control a steper in forward order.
'cStep BYTE %0011, %0110, %1100, %1001
{{
┌──────────────────────────────────────────────────────────────────────────────────────────────────────────────────────────────┐
│ TERMS OF USE: MIT License │
├──────────────────────────────────────────────────────────────────────────────────────────────────────────────────────────────┤
│Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation │
│files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, │
│modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software│
│is furnished to do so, subject to the following conditions: │
│ │
│The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.│
│ │
│THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE │
│WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR │
│COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, │
│ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. │
└──────────────────────────────────────────────────────────────────────────────────────────────────────────────────────────────┘
}}
I caught a couple of errors in my code. They are corrected.If you notice any errors or anything that could be done better please let me know.
Comments
IT IS NOW FIXED.
I again ask if anyone notices any errors, or has any suggestions on how to do it better, please let me know.
'This is a simple stepper motor controler for a three axis device like a 3D printer. 'We are only using full stepping for this one. CON xBase = 0 yBase = 4 zBase = 8 xPinM = $000F 'Pin Mask for X axis stepper. yPinM = $00F0 'Pin Mask for Y axis stepper. zPinM = $0F00 'Pin Mask for Z axis stepper. zrPin = 12 'Zeroing switch input. xMax = $7FFFFFFF 'Furthest from zero allowed for X axis. yMax = $7FFFFFFF 'Furthest from zero allowed for Y axis. zMax = $7FFFFFFF 'Furthest from zero allowed for Z axis. xDely = 156250 yDely = 156250 zDely = 156250 'The parameter offsets in the array that the cog reads: pCX = 0 'Current C position in steps. pCY = 1 'Current Y position in steps. pCZ = 2 'Current Z position in steps. pNX = 3 'Requested X position in steps, absolute. pNY = 4 'Requested Y position in steps. pNZ = 5 'Requested Z position in steps. pDX = 6 'Calculated delay between steps for X. pDY = 7 'Calculated delay between steps for Y. pDZ = 8 'Calculated delay between steps for Z pGo = 9 'Flag used to determine when it is time to go. VAR LONG DY, DX, DZ LONG PBLCK[12] LONG xDelay, yDelay, zDelay PUB MoveTo(ax, ay, az) DX := UABS(ax - PBLCK[pCX]) DY := UABS(ay - PBLCK[pCY]) DZ := UABS(az - PBLCK[pCZ]) IF DX > DY AND DX > DZ PBLCK[pDX] := xDelay PBLCK[pDy] := ((DX << 4) * ((DX / DY) << 4)) >> 4 PBLCK[pDz] := ((DX << 4) * ((DX / DZ) << 4)) >> 4 pBLCK[pNX] := ax pBLCK[pNY] := aY pBLCK[pNZ] := aZ PBLCK[pGo] := 1 PRI UABS(v) IF v < 0 v := -v RETURN v PUB Init COGNEW(Main, @PBLCK) xDelay := xDely yDelay := yDely zDelay := zDely DAT ORG Main MOV DIRA, Msk :Lp JMPRET GetP_R,GetP OR CG, CG WZ IF_NZ JMPRET DoLine_R, DoLine IF_Z JMP #:Lp '┌──────────────────────────────────────────────────────────────────┐ '│PROC: GetP │ '│ │ '│*DESCRIPTION: │ '│ Reads the parameter block from HUB RAM. Takes 432 to 446 clock │ '│ cycles. │ '│ │ '└──────────────────────────────────────────────────────────────────┘ GetP MOV Ptr, PAR 'PAR points at the Parameter block. MOV Lptr, #CurX 'COG Address of first parameter. MOV C, #12 Lp0 MOVS upd0, ptr ADD LPtr, #4 upd0 RDLONG Lptr, Ptr DJNZ C, #Lp0 JMP GetP_R PutP MOVI upd0, :upw JMP GetP '142, 18 :upw WRLONG Lptr, Ptr PutP_R GetP_R RET '┌──────────────────────────────────────────────────────────────────┐ '│PROC: PutP │ '└──────────────────────────────────────────────────────────────────┘ 'PutP MOV Ptr, PAR 'PAR points at the Parameter block. ' MOV Lptr, #CurX 'COG Address of first parameter. ' MOV C, #12 ':Lp MOVS :upd, ptr ' ADD LPtr, #4 ':upd WRLONG Lptr, Ptr ' DJNZ C, #:Lp 'PutP_R RET '┌──────────────────────────────────────────────────────────────────┐ '│PROC: DoLine: │ '│ │ '│*DESCRIPTION: │ '│Moves X, Y, and Z based on the delays until the move is complete. │ '│ │ '│ │ '└──────────────────────────────────────────────────────────────────┘ DoLine MOV XT, CNT MOV YT, CNT MOV ZT, CNT ADD XT, XDly ADD YT, YDly ADD YT, YDly :Lp CMP XT, CNT WZ,WC IF_BE ADD XT, CNT IF_BE JMPRET MvX_R, MvX CMP YT, CNT WZ,WC IF_BE ADD YT, CNT IF_BE JMPRET MvY_R, MvY CMP YT, CNT WZ,WC IF_BE ADD YT, CNT IF_BE JMPRET MvY_R, MvY CMP CurX, X WZ,WC IF_BE CMP CurY, Y WZ,WC IF_BE CMP CurZ, Z IF_BE JMPRET PutP_R,PutP IF_BE XOR CG,CG IF_BE RET JMP #:LP DoLine_R RET '┌──────────────────────────────────────────────────────────────────┐ '│PROC: MvX: │ '│ │ '└──────────────────────────────────────────────────────────────────┘ MvX SUB X, CurX NR,WC,WZ MOV T, Stps IF_Z JMP MvX_R IF_C SUB CurX, #1 IF_NC ADD CurX, #1 MOV T0, CurX AND T0, #$3 SHL T, XBase SHR T, T0 MOV T0, OUTA ANDN T0, XMsk JMP #Mv_T '┌──────────────────────────────────────────────────────────────────┐ '│PROC: MvY: │ '│ │ '└──────────────────────────────────────────────────────────────────┘ MvY SUB Y, CurY NR,WC,WZ MOV T, Stps IF_Z JMP MvX_R IF_C SUB CurY, #1 IF_NC ADD CurY, #1 MOV T0, CurY AND T0, #$3 SHL T, YBase SHR T, T0 MOV T0, OUTA ANDN T0, YMsk JMP #Mv_T '┌──────────────────────────────────────────────────────────────────┐ '│PROC: MvZ: │ '│ │ '└──────────────────────────────────────────────────────────────────┘ MvZ SUB Z, CurZ NR,WC,WZ MOV T, Stps IF_Z JMP MvX_R IF_C SUB CurZ, #1 IF_NC ADD CurZ, #1 MOV T0, CurZ AND T0, #$3 SHL T, ZBase SHR T, T0 MOV T0, OUTA ANDN T0, ZMsk Mv_T OR T0, T MOV OUTA, T0 MvX_R MvY_R MvZ_R RET CurX LONG 0 'The current X Position in steps. CurY LONG 0 'The current Y Position in steps. CurZ LONG 0 'The current Z Position in steps. X LONG 0 'The requested X position in steps. Y LONG 0 'The requested Y position in steps. Z LONG 0 'The requested Z position in steps. XDly LONG xDely 'The delay between X steps, in clock cycles. YDLY LONG yDely 'The delay between Y steps, in clock cycles. ZDLY LONG zDely 'The delay between Z steps, in clock cycles. CG LONG 0 'A flag to tell us when to go. C LONG 0 'Used in loops as an index. T LONG 0 'Used to store in process values. T0 LONG 0 XT LONG 0 YT LONG 0 ZT LONG 0 Stps LONG %0011_0110_1100_1001 'The step output values. Msk LONG (xPinM | yPinM | zPinM) XMsk LONG xPinM YMsk LONG yPinM ZMsk LONG zPinM Lptr LONG 0 Ptr LONG 0 'The states of a the four pins to control a steper in forward order. 'cStep BYTE %0011, %0110, %1100, %1001 {{ ┌──────────────────────────────────────────────────────────────────────────────────────────────────────────────────────────────┐ │ TERMS OF USE: MIT License │ ├──────────────────────────────────────────────────────────────────────────────────────────────────────────────────────────────┤ │Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation │ │files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, │ │modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software│ │is furnished to do so, subject to the following conditions: │ │ │ │The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.│ │ │ │THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE │ │WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR │ │COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, │ │ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. │ └──────────────────────────────────────────────────────────────────────────────────────────────────────────────────────────────┘ }}As always I know that I make mistakes, so if you notice any please point it out.
If anyone can see a way to make this a bit smaller it would be appreciated, I would like to get this spin module under 128 longs (0.5KB).
I still need to add code for detecting the zero switch, though that is not a huge issue (manual zeroing works well).
I did not know that SPIN had an absolute value of operator, thank you for that.
I will be adding provisions to change the delay values, so that it can support user specified speeds provided in GCode (I forget the command at the moment). I will also be adding some code to make sure that the speeds are with in the limits of all axises, for devices that have differing limits for each axis.
I have also made some simple changes to the code to improve it.
In order to maintain values in the tenths with integers I pass all values in from G-Code as value << 10
'This is a simple stepper motor controller for a three axis device like a 3D printer. 'We are only using full stepping for this one. CON xBase = 0 yBase = 4 zBase = 8 xPinM = $000F 'Pin Mask for X axis stepper. yPinM = $00F0 'Pin Mask for Y axis stepper. zPinM = $0F00 'Pin Mask for Z axis stepper. zrPin = 12 'Zeroing switch input. xMax = $7FFFFFFF 'Furthest from zero allowed for X axis. yMax = $7FFFFFFF 'Furthest from zero allowed for Y axis. zMax = $7FFFFFFF 'Furthest from zero allowed for Z axis. xDely = 156250 yDely = 156250 zDely = 156250 'The parameter offsets in the array that the cog reads: pCX = 0 'Current C position in steps. pCY = 1 'Current Y position in steps. pCZ = 2 'Current Z position in steps. pNX = 3 'Requested X position in steps, absolute. pNY = 4 'Requested Y position in steps. pNZ = 5 'Requested Z position in steps. pDX = 6 'Calculated delay between steps for X. pDY = 7 'Calculated delay between steps for Y. pDZ = 8 'Calculated delay between steps for Z pGo = 9 'Flag used to determine when it is time to go. VAR LONG DY, DX, DZ LONG PBLCK[12] LONG xDelay, yDelay, zDelay LONG UnitX, UnitY, UnitZ PUB MoveTo(ax, ay, az) 'Parameters are 1024 times Units to move, Units are usualy mm :) . DX := ||(((ax * UnitX) - (PBLCK[pCX] << 10)) DY := ||(((ay * UnitY) - (PBLCK[pCY] << 10)) DZ := ||(((az * UnitZ) - (PBLCK[pCZ] << 10)) >> 10 IF DX > DY AND DX > DZ PBLCK[pDX] := xDelay >> 10 PBLCK[pDy] := ((DX << 4) * ((DX / DY) << 4)) >> 4 PBLCK[pDz] := ((DX << 4) * ((DX / DZ) << 4)) >> 4 pBLCK[pNX] := (ax * UnitX) >> 10 pBLCK[pNY] := (aY * UnitY) >> 10 pBLCK[pNZ] := (aZ * UnitZ) >> 10 PBLCK[pGo] := 1 PUB SetPos(ax,ay,az) 'Parameters are 1024 times number of steps. PBLCK[pCX] := (ax * UnitX) >> 10 PBLCK[pCY] := (ay * UnitY) >> 10 PBLCK[pCZ] := (az * UnitZ) >> 10 RETURN PUB SetUnit(ax,ay,az,af) 'Parameters are in 1024 times steps per unit. UnitX := ax >> 10 UnitY := ay >> 10 UnitZ := az >> 10 RETURN PUB Stop PUB SetSpeed(ax,ay,az) 'Parameters are in 1024 times steps per second. xDelay := CLKFREQ / (ax >> 10) yDelay := CLKFREQ / (ay >> 10) zDelay := CLKFREQ / (az >> 10) RETURN PUB Init COGNEW(@Main, @PBLCK) xDelay := xDely yDelay := yDely zDelay := zDely DAT ORG Main MOV DIRA, Msk :Lp JMPRET GetP_R,GetP OR CG, CG WZ IF_NZ JMPRET DoLine_R, DoLine IF_Z JMP #:Lp '┌──────────────────────────────────────────────────────────────────┐ '│PROC: GetP │ '│ │ '│*DESCRIPTION: │ '│ Reads the parameter block from HUB RAM. Takes 432 to 446 clock │ '│ cycles. │ '│ │ '└──────────────────────────────────────────────────────────────────┘ GetP MOV Ptr, PAR 'PAR points at the Parameter block. MOV Lptr, #CurX 'COG Address of first parameter. MOV C, #12 Lp0 MOVS upd0, ptr ADD LPtr, #4 upd0 RDLONG Lptr, Ptr DJNZ C, #Lp0 JMP GetP_R PutP MOVI upd0, :upw JMP GetP '142, 18 :upw WRLONG Lptr, Ptr PutP_R GetP_R RET '┌──────────────────────────────────────────────────────────────────┐ '│PROC: DoLine: │ '│ │ '│*DESCRIPTION: │ '│Moves X, Y, and Z based on the delays until the move is complete. │ '│ │ '│ │ '└──────────────────────────────────────────────────────────────────┘ DoLine MOV XT, CNT MOV YT, CNT MOV ZT, CNT ADD XT, XDly ADD YT, YDly ADD YT, YDly :Lp CMP XT, CNT WZ,WC IF_BE ADD XT, CNT IF_BE JMPRET MvX_R, MvX CMP YT, CNT WZ,WC IF_BE ADD YT, CNT IF_BE JMPRET MvY_R, MvY CMP YT, CNT WZ,WC IF_BE ADD YT, CNT IF_BE JMPRET MvY_R, MvY CMP CurX, X WZ,WC IF_BE CMP CurY, Y WZ,WC IF_BE CMP CurZ, Z IF_BE JMPRET PutP_R,PutP IF_BE XOR CG,CG IF_BE RET JMP #:LP DoLine_R RET '┌──────────────────────────────────────────────────────────────────┐ '│PROC: MvX: │ '│ │ '└──────────────────────────────────────────────────────────────────┘ MvX SUB X, CurX NR,WC,WZ MOV T, Stps IF_Z JMP MvX_R IF_C SUB CurX, #1 IF_NC ADD CurX, #1 MOV T0, CurX AND T0, #$3 SHL T, XBase SHR T, T0 MOV T0, OUTA ANDN T0, XMsk JMP #Mv_T '┌──────────────────────────────────────────────────────────────────┐ '│PROC: MvY: │ '│ │ '└──────────────────────────────────────────────────────────────────┘ MvY SUB Y, CurY NR,WC,WZ MOV T, Stps IF_Z JMP MvX_R IF_C SUB CurY, #1 IF_NC ADD CurY, #1 MOV T0, CurY AND T0, #$3 SHL T, YBase SHR T, T0 MOV T0, OUTA ANDN T0, YMsk JMP #Mv_T '┌──────────────────────────────────────────────────────────────────┐ '│PROC: MvZ: │ '│ │ '└──────────────────────────────────────────────────────────────────┘ MvZ SUB Z, CurZ NR,WC,WZ MOV T, Stps IF_Z JMP MvX_R IF_C SUB CurZ, #1 IF_NC ADD CurZ, #1 MOV T0, CurZ AND T0, #$3 SHL T, ZBase SHR T, T0 MOV T0, OUTA ANDN T0, ZMsk Mv_T OR T0, T MOV OUTA, T0 MvX_R MvY_R MvZ_R RET CurX LONG 0 'The current X Position in steps. CurY LONG 0 'The current Y Position in steps. CurZ LONG 0 'The current Z Position in steps. X LONG 0 'The requested X position in steps. Y LONG 0 'The requested Y position in steps. Z LONG 0 'The requested Z position in steps. XDly LONG xDely 'The delay between X steps, in clock cycles. YDLY LONG yDely 'The delay between Y steps, in clock cycles. ZDLY LONG zDely 'The delay between Z steps, in clock cycles. CG LONG 0 'A flag to tell us when to go. C LONG 0 'Used in loops as an index. T LONG 0 'Used to store in process values. T0 LONG 0 XT LONG 0 YT LONG 0 ZT LONG 0 Stps LONG %0011_0110_1100_1001 'The step output values. Msk LONG (xPinM | yPinM | zPinM) XMsk LONG xPinM YMsk LONG yPinM ZMsk LONG zPinM Lptr LONG 0 Ptr LONG 0 'The states of a the four pins to control a steper in forward order. 'cStep BYTE %0011, %0110, %1100, %1001 {{ ┌──────────────────────────────────────────────────────────────────────────────────────────────────────────────────────────────┐ │ TERMS OF USE: MIT License │ ├──────────────────────────────────────────────────────────────────────────────────────────────────────────────────────────────┤ │Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation │ │files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, │ │modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software│ │is furnished to do so, subject to the following conditions: │ │ │ │The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.│ │ │ │THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE │ │WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR │ │COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, │ │ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. │ └──────────────────────────────────────────────────────────────────────────────────────────────────────────────────────────────┘ }}Please point out any errors you may see, as well as any improvements you can see possible. Thank you all.
Looks good. I'm really happy to see you doing something with this idea. I've often thought a lot of the current programs for controlling 3D motion were more complicated than they needed to be but never had the time or hardware to test things on. Great work so far, and hope it proves to be a useful idea and driver code.
I will let yo know how it goes once I have tested it on real Hardware. Thank you for the suggestion.
I agree that much of the driver code seems way to complex for what does. I have always subscribed to the KISS school of thought on such things.
This may apply: Simple speed profiles in real time, fixed point math only
For my ramping I was thinking something like taking the inner loop:
'┌──────────────────────────────────────────────────────────────────┐ '│PROC: DoLine: │ '│ │ '│*DESCRIPTION: │ '│Moves X, Y, and Z based on the delays until the move is complete. │ '│ │ '│ │ '└──────────────────────────────────────────────────────────────────┘ DoLine MOV XT, CNT MOV YT, CNT MOV ZT, CNT ADD XT, XDly ADD YT, YDly ADD YT, YDly :Lp CMP XT, CNT WZ,WC IF_BE ADD XT, CNT IF_BE JMPRET MvX_R, MvX CMP YT, CNT WZ,WC IF_BE ADD YT, CNT IF_BE JMPRET MvY_R, MvY CMP YT, CNT WZ,WC IF_BE ADD YT, CNT IF_BE JMPRET MvY_R, MvY CMP CurX, X WZ,WC IF_BE CMP CurY, Y WZ,WC IF_BE CMP CurZ, Z IF_BE JMPRET PutP_R,PutP IF_BE XOR CG,CG IF_BE RET JMP #:LP DoLine_R RETAnd modifying it so as to take a small ramping delta, for a number of steps, and just adjust the delay accordingly. The delta would be calculated before passing the parameters to the assembly code, as would the number of steps for each axis to keep everything in step.So the inner loop would end up looking something like:
'┌──────────────────────────────────────────────────────────────────┐ '│PROC: DoLine: │ '│ │ '│*DESCRIPTION: │ '│Moves X, Y, and Z based on the delays until the move is complete. │ '│ │ '│ │ '└──────────────────────────────────────────────────────────────────┘ DoLine MOV XT, CNT MOV YT, CNT MOV ZT, CNT ADD XT, XDly ADD YT, YDly ADD YT, YDly :Lp CMP XT, CNT WZ,WC IF_BE ADD XT, CNT IF_BE JMPRET MvX_R, MvX CMP YT, CNT WZ,WC IF_BE ADD YT, CNT IF_BE JMPRET MvY_R, MvY CMP YT, CNT WZ,WC IF_BE ADD YT, CNT IF_BE JMPRET MvY_R, MvY CMP CurX, X WZ,WC IF_BE CMP CurY, Y WZ,WC IF_BE CMP CurZ, Z IF_BE JMPRET PutP_R,PutP IF_BE XOR CG,CG IF_BE RET JMP #:LP DoLine_R RETWith additional provisions for ramping down of course, though that should do the ramp up part.New delta time = old delta time - (old delta time ramping function * ramp factor)
The ramping function would subtract a fraction of the delta time from the current delta time to generate the new delta time. For example the current delta time could be shifted right 10 bits and multiplied by the ramp factor (lets say 7) to decrease the new delta time by 7/1024ths of the current delta time. This would increase the speed of the stepper by approximately 0.7% every step. The same calculation could be used to ramp up speed by subtracting from the delta time, and ramp down by adding to the delta time.
I try to compile the code from post #7 (http://forums.parallax.com/showthread.php/160860-Smoother-%28X-Y-Z%29-movement-for-3D-printers-and-CNC.?p=1327408&viewfull=1#post1327408) in PropellerTool and I get errors in lines 42, 43 & 44, about missing parenthesis at the end of each line.
This would be the correct code? The ">> 10" at the end of DZ is correct?
Antonio
Thanks for the clarification. Now it seems more logical.
Is this a typo error too?
DAT ORG Main MOV DIRA, Msk :Lp JMPRET GetP_R,#GetP '<== added # in JMPRET GetP_R,#GetP to work properly OR CG, CG WZ IF_NZ JMPRET DoLine_R, DoLine IF_Z JMP #:LpDo you have a demo or explanation how to use this? I have tried the following code, but running in GEAR Emulator It can't worked yet:
{Demo Smoother XYZ 3D for steppers} CON _clkmode = xtal1 + pll16x 'Standard clock mode * crystal frequency = 80 MHz _xinfreq = 5_000_000 OBJ steppers : "steppers.spin" PUB DemoMain steppers.Init steppers.SetUnit(10,20,30,40) 'what is "af" parameter for? steppers.SetSpeed(1,10,100) waitcnt(10_000 + cnt) 'wait for emulation steppers.SetPos(5000,5000,5000) steppers.MoveTo(6000,5010,5001) repeat while trueI did have the filament feed stepper control there as well, and moved that out to a seperate file, so it used to be correct.
Easy to correct, the af parameter used to be to set the feedrate for the filamant.