-
Notifications
You must be signed in to change notification settings - Fork 2
/
Copy pathintervalgen.py
294 lines (257 loc) · 14.1 KB
/
intervalgen.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
#!/usr/bin/python3
"""
This module has a couple of step timing classes for stepper motors.
Timings are generated on demand by a generator that uses various settings held by the class.
Some of these settings can be changed as the generator is running, as can a couple of settings
within the motor instance that contains this class; the generator adapts dynamically to
these changes.
The generators work in their own time frame so can be used both in real time and to pre-prepare
step timings, this is handled by the code calling these generators.
"""
import pootlestuff.watchables as wv
from pootlestuff.watchables import loglvls
import time
class stepgen(wv.watchablesmart):
"""
base class for tick interval generator.
Instances of this class are created for each entry in a motor's 'stepmodes' in the json confog file.
It records
"""
def __init__(self, name, motor, value, wabledefs, ticklogfile=None, **kwargs):
"""
base class for step interval generators.
name: it's the name!
motor: the motor that owns this generator
value: a dict of further settings for creating an instance. This base class uses:
['mode']: defines if this generator should use software to drive the stepping, or DMA (high precision) to drive the stepping.
This is not used within this class (it doesn't care) but is relevant when a step operation is being setup.
['usteplevel']: defines the microstep level that will be used. This is only used to scale between each step issued by this
generator and the resulting position change.
Other entries in value are defined by subclasses.
wabledefs: more watchable vars wanted by subclasses
ticklogfile: If not None then steps are written to this file as csv.
"""
self.name=name
self.motor=motor
self.mode=value['mode']
usvlist=motor.getustepnames()
wabledefs.append(('usteplevel', wv.enumWatch, usvlist[0], False, {'vlist': usvlist})), # selects the microstep level to use with these settings)
super().__init__(value=value, wabledefs=wabledefs, **kwargs)
self.ticklogf=ticklogfile
def log(self, *args, **kwargs):
self.motor.log(*args, **kwargs)
def getmicrosteps(self):
"""
returns a 2 tuple:
0: number of microsteps per step for the microsteplevel
1: position change per actual tick
"""
uslevelv = self.motor.getusteplevel(self.usteplevel.getValue())# number of microsteps per step
return uslevelv, self.motor.maxstepfactor // uslevelv
def tickgen(self):
raise NotImplementedError()
class stepgenonespeed(stepgen):
"""
A very basic step generator that defines the (only) step speed that is used.
"""
def __init__(self, **kwargs):
wables=[
('steprate', wv.floatWatch, .01, False), # The step rate in full steps per second
]
super().__init__(wabledefs=wables, **kwargs)
self.log(loglvls.DEBUG,'stepgen %s setup for usteplevel %s and tick of %4.3f' % (self.name, self.usteplevel.getValue(), self.steprate.getValue()))
def tickgen(self, command, initialpos):
"""
generator function for step times.
yields a sequence of fixed step times
command: 'run' or 'goto'.
run only terminates if the motor's stepactive variable is set False. motor.target_dir is monitored as is this instance's steprate.
goto will return no step required once target is reached, and terminate if running set False. the motor's targetpos is monitored and
direction is changed if necessary. This instance's steprate variable is monitored.
initialpos: starting position to use only used for goto to track current position relative to target position
returns:
when changing direction this returns a 2 - tuple
0: 1 character string: 'F' or 'R'
1: time to next step (this will be same interval)
otherwise
time to next step
-or-
None if in goto mode and reached target
if motor.stepactive goes False or mode is onegoto and destination reached, rasies StopIteration
"""
if self.ticklogf is None:
tlog=None
else:
tlog=open(self.ticklogf,'w')
isgoto=command=='goto' or command=='onegoto' # goto or move?
uslevelv, usteps = self.getmicrosteps()
activedirection=None # which direction motor currently running
motor=self.motor
motor.updatetickerparams=True
if isgoto:
currentposv=initialpos # get the current position - we assume we are in control of this so don't re-read it
while self.motor.stepactive:
if motor.updatetickerparams:
stepsleft=motor.targetrawpos.getValue()-currentposv
absstepsleft=abs(stepsleft)
stepsps=self.steprate.getValue()
newdir='F' if stepsleft > 0 else 'R'
usetick=1/stepsps/uslevelv
if newdir != activedirection:
activedirection=newdir
yield newdir, 0.00002 if activedirection is None else usetick
if tlog:
tlog.write('%s,%7.5f,%d\n' % (newdir, usetick, currentposv))
motor.updatetickerparams=False
if absstepsleft >= usteps: # if remaining usteps less than the current step size stop now
currentposv += usteps if activedirection =='F' else -usteps
absstepsleft -= usteps
yield usetick
if tlog:
tlog.write('%s,%7.5f,%d\n' % (' ', usetick, currentposv))
else:
if command=='onegoto':
break
yield None
if tlog:
tlog.write('X,0,%d\n' % currentposv)
currenttick=None
else:
while self.motor.stepactive:
if motor.updatetickerparams:
newdir = 'F' if self.motor.target_dir.getValue() > 0 else 'R'
stepsps=self.steprate.getValue()
usetick=1/stepsps/uslevelv
if newdir != activedirection:
usetick = 0.00002 if activedirection is None else usetick
activedirection=newdir
if tlog:
tlog.write('%s,%7.5f\n' % (newdir, usetick))
yield newdir, usetick
motor.updatetickerparams=False
yield usetick
if tlog:
tlog.write('%s,%7.5f\n' % (' ', usetick))
if tlog:
tlog.close()
class stepconstacc(stepgen):
"""
generates step timings with constant slope ramping.
"""
def __init__(self, **kwargs):
wables=[
('slowtps', wv.floatWatch, 1, False), # The initial steps per second
('fasttps', wv.floatWatch, .1, False), # The fastest steps per second
('slope', wv.floatWatch, .1, False), # change in step interval per second
]
super().__init__(wabledefs=wables, **kwargs)
self.log(loglvls.DEBUG, 'stepgen %s setup for usteplevel %s and initial tick of %4.3f' % (self.name, self.usteplevel.getValue(), self.slowtps.getValue()))
def tickgen(self, command, initialpos):
"""
generator function for step times.
yields a sequence of step times, stopping at target in goto or onegotomode. Speed is ramped up and down and various parameters are monitored to update
the generated steps - see under command below.
command: 'run' or 'goto'.
run only terminates if the motor's stepactive variable is set False. motor.target_dir is monitored as is this instance's steprate.
goto will return no step required once target is reached, and terminate if running set False. the motor's targetpos is monitored and
direction is changed if necessary. This instance's steprate variable is monitored.
initialpos: starting position to use only used for goto to track current position relative to target position
returns:
when changing direction this returns a 2 - tuple
0: 1 character string: 'F' or 'R'
1: time to next step (this will be same interval)
otherwise
time to next step
-or-
None if in goto mode and reached target
if motor.stepactive goes False or mode is onegoto and destination reached, raises stopiteration
Note that the motor will decelerate before StopIteration if stepactive goes false.
"""
isgoto=command=='goto' or command=='onegoto' # goto or move?
tickpos=initialpos
uslevelv, usteps = self.getmicrosteps() # uslevelv is number of microsteps per step for the microsteplevel
uslevelv=float(uslevelv) # so it's right for later
print('uslevelv', uslevelv, 'usteps',usteps) # usteps is the change in position for each step
motor=self.motor
motor.updatetickerparams=True
currdir=None
currtps=self.slowtps.getValue() # number of full steps per second to start at
xelapsed=time.time() # just a testing var
ramping=True # just a testing var
if self.ticklogf is None:
tlog=None
else:
tlog=open(self.ticklogf,'w')
while self.motor.stepactive:
if motor.updatetickerparams:
slowtps=self.slowtps.getValue()
fasttps=self.fasttps.getValue()
slope=self.slope.getValue()
if isgoto:
target=self.motor.targetrawpos.getValue()
offset=target-tickpos
absoffset=abs(offset)
newdir=1 if offset > 0 else -1
else:
newdir = 1 if self.motor.target_dir.getValue() > 0 else -1
currtick = 1/currtps/uslevelv
if isgoto:
deceltime=(currtps-slowtps) / slope # time it will take to get to slow speed from current speed
averagetps=(currtps+slowtps) / 2
decelfullsteps = averagetps * deceltime
decelusteps = (decelfullsteps+5) * self.motor.maxstepfactor # distance from target deceleration should start
motor.updatetickerparams=False
if newdir != currdir or isgoto and (absoffset < decelusteps): # check if slowdown needed
if currtps <= slowtps: # reached min speed
if newdir != currdir: # are we waiting to change direction?
yield 'F' if newdir > 0 else 'R', .00002 if currdir is None else currtick
currdir = newdir
elif isgoto:
if absoffset < usteps/2: # and as close as possible to target
if command=='onegoto':
break
yield None
else:
yield currtick # just a bit further.....
tickpos += usteps*currdir
if isgoto:
absoffset -= usteps
else: # slowing down
currtps -= slope*currtick # not exactly right, but simple calc
if currtps < slowtps:
currtps=slowtps
currtick=1/currtps/uslevelv
yield currtick
tickpos += usteps*currdir
if isgoto:
absoffset -= usteps
elif currtps < fasttps: # go faster
currtps += slope*currtick # over accelerates more at low speeds - but simple calculation
if currtps > fasttps:
currtps = fasttps
currtick = 1/currtps/uslevelv
yield currtick
tickpos += usteps*currdir
if isgoto:
absoffset -= usteps
deceltime=(currtps-slowtps) / slope # time it will take to get to slow speed from current speed
averagetps=(currtps+slowtps) / 2
decelfullsteps = averagetps * deceltime
decelusteps = (decelfullsteps+5) * self.motor.maxstepfactor # distance from target deceleration should start
else: # speed is max - keep going
if ramping:
ramping=False
yield currtick
tickpos += usteps*currdir
if isgoto:
absoffset -= usteps
while currtps > slowtps:
currtps -= slope * currtick
currtick=1/currtps/uslevelv
yield currtick
if tlog:
tlog.write('B: %s,%7.5f,%d\n' % (' ' , currtick/uslevelv, tickpos))
tickpos += currdir*usteps
if tlog:
tlog.write('%s,%7.5f,%d\n' % ('X' if currdir > 0 else 'R' , currtick/uslevelv, tickpos))
tlog.close()