1+ {P2 Stepper motor driver }
2+
3+
4+ CON
5+
6+
7+ VAR
8+ long cog ' cog flag/id
9+
10+ long CP 'current position
11+ long SPD 'Speed in steps/sec
12+ long STM 'steps to move +/- Routine clears this after values are read
13+ long Asteps 'Number of steps for acceleration phase
14+ long Offset 'degrees at ends of range to skip
15+
16+
17+ long MODEb 'mode bits
18+ 'b0 = 0 - stopped, 1 - moving
19+ 'b1 = 1 - accel
20+ 'b2 = 1 - decel
21+ 'b3 = 1 - at slew speed
22+ 'b4 = direction, 1=cw
23+ 'b5 1 = decel to stop
24+
25+ long sspn 'SSPIN - set to 0 to stop, 1 to start
26+ long cwpin 'cw pin to driver
27+ long ccwpin 'ccw pin to driver
28+ long frequency 'clock frequency
29+ long del
30+ long vel
31+ long as
32+ long ds
33+ long ss
34+ long stn
35+
36+
37+ PUB start(sp, cw, ccw): result
38+ stop()
39+ longmove(@sspn, @sp, 3) ' save pins
40+ frequency := clkfreq
41+ cog := COGINIT(COGEXEC_NEW,@_begin,@cp) +1
42+ return cog
43+
44+ PUB stop()
45+
46+ '' Stop stepper driver
47+ '' -- frees a cog if driver was running
48+
49+ if (cog) ' cog active?
50+ cogstop(cog-1) ' yes, shut it down
51+ cog := 0 ' and mark stopped
52+
53+ PUB move(sp,st)| t
54+
55+ asteps := sp/10 +2 'effectively sets max acceleration
56+ t := abs(st/2)
57+ offset := 40
58+ if asteps > t
59+ asteps := t 'limits accel/ decel steps to 1/2 the total steps
60+ ss := abs(st) - (asteps*2)
61+ if ss == 0
62+ sp := 10*(asteps -2) 'limit acceleration for short moves
63+
64+ longmove(@spd, @sp, 2) 'copy values - start the move
65+
66+ PUB getpos(): result
67+ return @cp
68+
69+ PUB getsteps(): result
70+ return @stm
71+
72+ PUB getmode(): result
73+ return @modeb
74+
75+ PUB getoff(): result
76+ return @offset
77+
78+ PUB getvel(): result
79+ return @vel
80+
81+ PUB getas(): result
82+ return @asteps
83+
84+ PUB getss(): result
85+ return @ss
86+
87+
88+ DAT ORG 0
89+
90+ _begin mov ptrb, ptra
91+ add ptrb, #24 '4 bytes * 6 longs
92+ setq #4-1
93+ rdlong sspin,ptrb[0] 'get three pin numbers and frequency
94+
95+ drvl cwpn
96+ drvl ccwpn
97+
98+ _stopped rdlong steps, ptra[2] wz 'wait for steps to be non zero
99+ if_z jmp #_stopped
100+
101+ setq #5-1 'get 5 parameters from hub
102+ rdlong cpos, ptra[0]
103+
104+ wrlong #0, ptra[2] 'set steps to zero to indicate we received them
105+
106+ ' wrlong astps, ptra[12]
107+ ' wrlong angoff, ptra[13]
108+
109+ drvl cwpn
110+ drvl ccwpn
111+ mov stepp, cwpn 'anticipate cw
112+ mov mode, #0
113+ abs steps wc 'convert neg steps to pos
114+ muxc mode, #16
115+ if_nc jmp #_dirset 'set direction pin based on c D[31]
116+ mov stepp, ccwpn 'if ccw swap the pins
117+ _dirset wrlong mode, ptra[5] 'output the mode with dir bit set
118+
119+ _setup mov halfsp, speed
120+ shr halfsp, #1 'make half speed variable
121+
122+ mov stepn, #0
123+ mov stom, steps
124+ mov newmin, angoff
125+ mov newrange, n360
126+ sub newrange, angoff
127+ sub newrange, angoff 'newrange = angoff to (360-angoff)
128+ shl newmin, #7 'partial multiply x128 11930464/128 = 93206 remaining
129+ mov xsteps, #0
130+ mov t3, astps
131+ shl t3, #1 'x2 - accel and decel
132+ cmp t3, stom wc 'more than accel + decel steps?
133+ if_nc jmp #_waitstart
134+ mov xsteps, steps
135+ sub xsteps, t3 'xsteps = excess steps above accel +decel or zero
136+ mov stom, t3
137+
138+
139+ '******************************************************************************************************
140+
141+ _waitstart testp sspin wc 'wait for start pin = 1
142+ if_nc jmp #_waitstart
143+
144+ or mode, #%011 'moving, accel
145+ wrlong mode, ptra[5]
146+ call #_dostep 'output a step
147+
148+ _mainloop testp sspin wc 'still in run mode?
149+ if_c jmp #_mlp1 'yes, keep running
150+ 'if stop pressed
151+ or mode, #%100000 'turn on decel_to_stop
152+ sub steps, xsteps
153+ mov xsteps, #0
154+
155+ _mlp1 cmp astps, stepn wz 'done accelerating?
156+ if_z jmp # _mlp2
157+ if_nz call #_waitstep
158+ jmp #_mainloop
159+
160+ _mlp2 cmp xsteps, #1 wz 'wait till the last at_speed step
161+ if_z jmp #_checknextmove
162+
163+ _mlp3 tjz xsteps, #_mlp5 'if xsteps = 0, decelerate
164+ _mlp4 andn mode, #%01110 'acceleration done
165+ or mode, #%01000 'set mode -> at speed
166+ jmp #_mlp6
167+
168+ _mlp5 andn mode, #%01110
169+ or mode, #%00100 'set mode -> decel
170+ wrlong t2, ptra[11] 'output top speed
171+
172+ _mlp6 wrlong mode, ptra[5]
173+ call #_waitstep
174+ jmp #_mainloop
175+
176+ '******************************************************************************************************
177+
178+ _checknextmove
179+ rdlong steps2, ptra[2] wz
180+ if_z jmp #_mlp3 'no new move to consider
181+
182+ mov t1, #0 'else- check if direction is same
183+ abs steps2 wc 'convert neg steps to pos
184+ muxc t1, #16 'set direction bit
185+ mov t2, mode
186+ and t2, #%10000 'clear all but direction bit
187+ cmp t1, t2 wz ' is direction the same as current
188+ if_nz jmp #_mlp3 'direction is different we need to stop before starting next move
189+
190+ rdlong speed2, ptra[1]
191+ cmp speed, speed2 wz 'is speed the same
192+ if_nz jmp #_mlp3 'not the same, can't continue
193+
194+ _same_speed add steps, steps2 'add new steps to current steps
195+ add xsteps, steps2 'new steps are all at_speed steps
196+ wrlong #0, ptra[2] 'set steps to zero to indicate we received them
197+ jmp #_mlp4
198+
199+ '*****************************************************************************************************
200+
201+ _waitstep waitct2 'wait for the delay period to end
202+ 'then start another pulse
203+ _dostep tjnz steps, #_dostep1 'zero steps left, we're done
204+ pop t1 'pop the unused return address
205+ and mode, #%0100000 'indicate stopped but leave b5 set if decelerate_to_stop
206+ wrlong cpos, ptra[0]
207+ wrlong mode, ptra[5]
208+ jmp #_stopped
209+
210+ _dostep1 drvh stepp 'step pin high
211+ getct cv
212+ addct1 cv,delayh 'start the pulse high timer
213+
214+ testb mode, #4 wc 'check the direction bit
215+ sumc cpos, #1 'add/subtract from current position
216+ wrlong cpos, ptra[0] 'update current position
217+
218+ waitct1
219+ drvl stepp 'step pulse end
220+
221+ _ws1 testb mode, #3 wc 'at speed?
222+ if_c sub xsteps, #1 'yes, dec xsteps
223+ if_c jmp #_ws2 'yes, keep old delay
224+
225+ call #_getdelay 'get new delay if accel or decel
226+
227+ _ws2 addct2 cv, delay 'start the delay timer
228+ sub steps, #1 'decrement the steps to go
229+ testb mode, #3 wc 'mode = at speed?
230+ if_nc add stepn, #1 'inc stepn if not
231+ ret
232+
233+ _getdelay qmul stepn, newrange 'scale step to range angoff - (360 - angoff)
234+ getqx t1
235+ shl t1, #7 'partial multiply *32
236+ qdiv t1, stom
237+ getqx newvalue
238+ add newvalue, newmin 'new value is converted oldvalue
239+ qmul newvalue, mult 'finish multiply * 93206
240+ getqx newvalue
241+ qrotate halfsp, newvalue 'get cosine of vector
242+ getqx t1
243+ mov t2, halfsp
244+ sub t2, t1 'get velocity
245+
246+ fges t2, #1 'avoid divide by zero
247+ qdiv freq, t2 'delay = frequency/ velocity
248+ getqx delay
249+ fles delay, maxdelay
250+ ret
251+
252+
253+ '-----------------------------------------------------------------------------------------------
254+
255+ delayh long 1000 'step output high time 1000/200_000_000 = 5�s
256+ delay long 1 'delay between steps - calculated
257+ n360 long 360 'the number 360
258+ mult long 93206 '372827 * 32 = 11930464 * 360 = FFFFFFFF = 4294967295 = 359.999999999degress
259+ maxdelay long 95000000
260+
261+ cpos res 1 'current position - steps
262+ speed res 1 'speed -steps/sec
263+ steps res 1 'steps to move
264+ astps res 1 'accel/ decel steps
265+ angoff res 1
266+
267+ mode res 1 'b0 = 0 - stopped, 1 - moving
268+ 'b1 = 1 - accel
269+ 'b2 = 1 - decel
270+ 'b3 = 1 - at slew speed
271+ 'b4 = 1 direction = cw
272+
273+ sspin res 1 'start =1 stop =0 pin (input)
274+ cwpn res 1 'output to driver
275+ ccwpn res 1 ' "
276+ freq res 1 'system clock frequency
277+
278+ stepp res 1 'depending on direction, either cwpn or ccwpn
279+
280+ t1 res 1 'temp variables
281+ t2 res 1
282+ t3 res 1
283+ cv res 1 'counter value
284+
285+ stepn res 1 'step number
286+ oldval res 1
287+ newmin res 1
288+ newvalue res 1 'converted old value
289+ newrange res 1
290+ halfsp res 1 'speed/2
291+ stom res 1 'steps to move
292+ xsteps res 1 'excess steps = total steps - (2*asteps)
293+
294+ steps2 res 1 'second move steps and speed
295+ speed2 res 1
296+
297+ fit 150
298+
299+ con { license }
300+
301+ {{
302+
303+ Terms of Use: MIT License
304+
305+ Permission is hereby granted, free of charge, to any person obtaining a copy of this
306+ software and associated documentation files (the "Software"), to deal in the Software
307+ without restriction, including without limitation the rights to use, copy, modify,
308+ merge, publish, distribute, sublicense, and/or sell copies of the Software, and to
309+ permit persons to whom the Software is furnished to do so, subject to the following
310+ conditions:
311+
312+ The above copyright notice and this permission notice shall be included in all copies
313+ or substantial portions of the Software.
314+
315+ THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED,
316+ INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A
317+ PARTICULAR PURPOSE AND NON-INFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
318+ HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF
319+ CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE
320+ OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
321+
322+ }}
0 commit comments