moteus 0.3.71__py3-none-any.whl → 0.3.73__py3-none-any.whl
This diff represents the content of publicly available package versions that have been released to one of the supported registries. The information contained in this diff is provided for informational purposes only and reflects changes between package versions as they appear in their respective public registries.
- moteus/calibrate_encoder.py +92 -9
- moteus/moteus.py +104 -1
- moteus/moteus_tool.py +202 -13
- moteus/version.py +1 -1
- {moteus-0.3.71.dist-info → moteus-0.3.73.dist-info}/METADATA +11 -5
- {moteus-0.3.71.dist-info → moteus-0.3.73.dist-info}/RECORD +9 -9
- {moteus-0.3.71.dist-info → moteus-0.3.73.dist-info}/WHEEL +1 -1
- {moteus-0.3.71.dist-info → moteus-0.3.73.dist-info}/entry_points.txt +1 -0
- {moteus-0.3.71.dist-info → moteus-0.3.73.dist-info}/top_level.txt +0 -0
moteus/calibrate_encoder.py
CHANGED
@@ -14,6 +14,9 @@
|
|
14
14
|
|
15
15
|
import json
|
16
16
|
import math
|
17
|
+
import scipy.optimize
|
18
|
+
|
19
|
+
BIN_COUNT = 64
|
17
20
|
|
18
21
|
class Entry:
|
19
22
|
direction = 0
|
@@ -159,8 +162,8 @@ def _window_average(values, window_size):
|
|
159
162
|
end = i + window_size // 2
|
160
163
|
errs = [0] * (end - start)
|
161
164
|
for j in range(start, end):
|
162
|
-
errs[j - start] =
|
163
|
-
result[i] =
|
165
|
+
errs[j - start] = values[wrap(j)]
|
166
|
+
result[i] = sum(errs) / len(errs)
|
164
167
|
|
165
168
|
return result
|
166
169
|
|
@@ -176,6 +179,8 @@ class CalibrationResult:
|
|
176
179
|
self.total_delta = None
|
177
180
|
self.ratio = None
|
178
181
|
|
182
|
+
self.fit_metric = None
|
183
|
+
|
179
184
|
self.errors = []
|
180
185
|
|
181
186
|
def __repr__(self):
|
@@ -184,6 +189,7 @@ class CalibrationResult:
|
|
184
189
|
"phase_invert": self.phase_invert,
|
185
190
|
"poles": self.poles,
|
186
191
|
"offset": self.offset,
|
192
|
+
"fit_metric": self.fit_metric,
|
187
193
|
"errors": self.errors,
|
188
194
|
})
|
189
195
|
|
@@ -193,13 +199,16 @@ class CalibrationResult:
|
|
193
199
|
'phase_invert': self.phase_invert,
|
194
200
|
'poles': self.poles,
|
195
201
|
'offset': self.offset,
|
202
|
+
'fit_metric': self.fit_metric,
|
196
203
|
}
|
197
204
|
|
198
205
|
|
199
206
|
def calibrate(parsed,
|
200
207
|
desired_direction=1,
|
201
208
|
max_remainder_error=0.1,
|
202
|
-
allow_phase_invert=True
|
209
|
+
allow_phase_invert=True,
|
210
|
+
allow_optimize=True,
|
211
|
+
force_optimize=False):
|
203
212
|
'''Calibrate the motor.
|
204
213
|
|
205
214
|
:param desired_direction: For positive unwrapped_position, should
|
@@ -315,19 +324,90 @@ def calibrate(parsed,
|
|
315
324
|
|
316
325
|
expected = [(2.0 * math.pi / 65536.0) * (result.poles / 2) * x for x in xpos]
|
317
326
|
|
318
|
-
err = [
|
327
|
+
err = [a - b for a, b in zip(avg_interp, expected)]
|
319
328
|
|
320
|
-
# Make the error
|
321
|
-
|
322
|
-
|
323
|
-
|
329
|
+
# Make the error balanced about 0.
|
330
|
+
mean_err = sum(err) / len(err)
|
331
|
+
wrapped_mean_err = _wrap_neg_pi_to_pi(mean_err)
|
332
|
+
delta_mean_err = mean_err - wrapped_mean_err
|
333
|
+
err = [x - delta_mean_err for x in err]
|
324
334
|
|
325
335
|
avg_window = int(len(err) / result.poles)
|
326
336
|
avg_err = _window_average(err, avg_window)
|
327
337
|
|
328
|
-
offset_x = list(range(0, 65536,
|
338
|
+
offset_x = list(range(0, 65536, 65536 // BIN_COUNT))
|
329
339
|
offset = _interpolate(offset_x, xpos, avg_err)
|
330
340
|
|
341
|
+
MAX_ERROR = 0.8
|
342
|
+
|
343
|
+
# The firmware will complain if individual steps are more
|
344
|
+
# than this apart.
|
345
|
+
MAX_STEP_CHANGE = 3.5
|
346
|
+
|
347
|
+
# Penalize solutions that have error greater than this.
|
348
|
+
PENALTY_RATIO = 0.80
|
349
|
+
|
350
|
+
def full_metric(x, *args):
|
351
|
+
errors = 0
|
352
|
+
result = 0
|
353
|
+
resampled = _interpolate(xpos, offset_x + [65536],
|
354
|
+
list(x) + [x[0]])
|
355
|
+
for a, b in zip(err, resampled):
|
356
|
+
this_err = abs(_wrap_neg_pi_to_pi(a - b))
|
357
|
+
# Heavily penalize if we would have an error.
|
358
|
+
if this_err > PENALTY_RATIO * MAX_ERROR:
|
359
|
+
this_err = (PENALTY_RATIO * MAX_ERROR +
|
360
|
+
(this_err - PENALTY_RATIO * MAX_ERROR) * 10)
|
361
|
+
if this_err > MAX_ERROR:
|
362
|
+
errors += 1
|
363
|
+
result += this_err ** 2
|
364
|
+
|
365
|
+
for i in range(0, len(list(x))):
|
366
|
+
nexti = (i + 1) % len(list(x))
|
367
|
+
delta = abs(x[nexti] - x[i])
|
368
|
+
if delta > (PENALTY_RATIO * MAX_STEP_CHANGE):
|
369
|
+
result += (
|
370
|
+
10 * (delta - PENALTY_RATIO * MAX_STEP_CHANGE) ** 2)
|
371
|
+
return result, errors
|
372
|
+
|
373
|
+
def metric(x, *args):
|
374
|
+
result = full_metric(x, *args)[0]
|
375
|
+
print(f"optimizing - metric={result:.5f} ", end='\r', flush=True)
|
376
|
+
return result
|
377
|
+
|
378
|
+
starting_metric, starting_errors = full_metric(offset)
|
379
|
+
|
380
|
+
if (force_optimize or (
|
381
|
+
allow_optimize and
|
382
|
+
(starting_metric > 30 or
|
383
|
+
starting_errors > 0))):
|
384
|
+
print()
|
385
|
+
print(f"Initial metric: {starting_metric}")
|
386
|
+
# Optimize these initial offsets.
|
387
|
+
optimres = scipy.optimize.minimize(metric, offset, tol=1e1)
|
388
|
+
|
389
|
+
if not optimres.success:
|
390
|
+
result.errors.append(
|
391
|
+
f"optimization failed {result.message}")
|
392
|
+
|
393
|
+
print()
|
394
|
+
offset = list(optimres.x)
|
395
|
+
|
396
|
+
result.fit_metric, _ = full_metric(offset)
|
397
|
+
|
398
|
+
# Now double check our results.
|
399
|
+
resampled_offset = _interpolate(xpos, offset_x + [65536],
|
400
|
+
offset + [offset[0]])
|
401
|
+
any_sample_error = False
|
402
|
+
sample_errors = []
|
403
|
+
for a, b in zip(err, resampled_offset):
|
404
|
+
sample_error = _wrap_neg_pi_to_pi(a - b)
|
405
|
+
sample_errors.append(sample_error)
|
406
|
+
if not any_sample_error and abs(sample_error) > MAX_ERROR:
|
407
|
+
result.errors.append(
|
408
|
+
f"excessive error in curve fit |{sample_error}| > {MAX_ERROR}")
|
409
|
+
any_sample_error = True
|
410
|
+
|
331
411
|
result.offset = offset
|
332
412
|
|
333
413
|
result.debug = {
|
@@ -340,6 +420,9 @@ def calibrate(parsed,
|
|
340
420
|
'avg_interp' : avg_interp,
|
341
421
|
'err' : err,
|
342
422
|
'avg_err' : avg_err,
|
423
|
+
'offset_x' : offset_x,
|
424
|
+
'offset' : offset,
|
425
|
+
'sample_errors' : sample_errors,
|
343
426
|
}
|
344
427
|
|
345
428
|
return result
|
moteus/moteus.py
CHANGED
@@ -252,6 +252,17 @@ class Register(enum.IntEnum):
|
|
252
252
|
MILLISECOND_COUNTER = 0x070
|
253
253
|
CLOCK_TRIM = 0x071
|
254
254
|
|
255
|
+
AUX1_PWM1 = 0x076,
|
256
|
+
AUX1_PWM2 = 0x077,
|
257
|
+
AUX1_PWM3 = 0x078,
|
258
|
+
AUX1_PWM4 = 0x079,
|
259
|
+
AUX1_PWM5 = 0x07a,
|
260
|
+
AUX2_PWM1 = 0x07b,
|
261
|
+
AUX2_PWM2 = 0x07c,
|
262
|
+
AUX2_PWM3 = 0x07d,
|
263
|
+
AUX2_PWM4 = 0x07e,
|
264
|
+
AUX2_PWM5 = 0x07f,
|
265
|
+
|
255
266
|
REGISTER_MAP_VERSION = 0x102
|
256
267
|
SERIAL_NUMBER = 0x120
|
257
268
|
SERIAL_NUMBER1 = 0x120
|
@@ -267,6 +278,16 @@ class Register(enum.IntEnum):
|
|
267
278
|
DRIVER_FAULT1 = 0x140
|
268
279
|
DRIVER_FAULT2 = 0x141
|
269
280
|
|
281
|
+
UUID1 = 0x150
|
282
|
+
UUID2 = 0x151
|
283
|
+
UUID3 = 0x152
|
284
|
+
UUID4 = 0x153
|
285
|
+
|
286
|
+
UUID_MASK1 = 0x0154
|
287
|
+
UUID_MASK2 = 0x0155
|
288
|
+
UUID_MASK3 = 0x0156
|
289
|
+
UUID_MASK4 = 0x0157
|
290
|
+
|
270
291
|
|
271
292
|
class Mode(enum.IntEnum):
|
272
293
|
"""Valid values for the Register.MODE register"""
|
@@ -347,6 +368,19 @@ class CurrentResolution:
|
|
347
368
|
q_A = mp.F32
|
348
369
|
|
349
370
|
|
371
|
+
class PwmResolution:
|
372
|
+
aux1_pwm1 = mp.INT16
|
373
|
+
aux1_pwm2 = mp.INT16
|
374
|
+
aux1_pwm3 = mp.INT16
|
375
|
+
aux1_pwm4 = mp.INT16
|
376
|
+
aux1_pwm5 = mp.INT16
|
377
|
+
aux2_pwm1 = mp.INT16
|
378
|
+
aux2_pwm2 = mp.INT16
|
379
|
+
aux2_pwm3 = mp.INT16
|
380
|
+
aux2_pwm4 = mp.INT16
|
381
|
+
aux2_pwm5 = mp.INT16
|
382
|
+
|
383
|
+
|
350
384
|
class Parser(mp.RegisterParser):
|
351
385
|
def read_position(self, resolution):
|
352
386
|
return self.read_mapped(resolution, 0.01, 0.0001, 0.00001)
|
@@ -507,6 +541,9 @@ def parse_register(parser, register, resolution):
|
|
507
541
|
return parser.read_int(resolution)
|
508
542
|
elif register == Register.CLOCK_TRIM:
|
509
543
|
return parser.read_int(resolution)
|
544
|
+
elif (register >= Register.AUX1_PWM1 and
|
545
|
+
register <= Register.AUX2_PWM5):
|
546
|
+
return parser.read_pwm(resolution)
|
510
547
|
else:
|
511
548
|
# We don't know what kind of value this is, so we don't know
|
512
549
|
# the units.
|
@@ -530,7 +567,7 @@ class Result:
|
|
530
567
|
id = None
|
531
568
|
arbitration_id = None
|
532
569
|
bus = None
|
533
|
-
values =
|
570
|
+
values = {}
|
534
571
|
|
535
572
|
def __repr__(self):
|
536
573
|
value_str = ', '.join(['{}(0x{:03x}): {}'.format(Register(key).name, key, value)
|
@@ -609,6 +646,7 @@ class Controller:
|
|
609
646
|
position_resolution=PositionResolution(),
|
610
647
|
vfoc_resolution=VFOCResolution(),
|
611
648
|
current_resolution=CurrentResolution(),
|
649
|
+
pwm_resolution=PwmResolution(),
|
612
650
|
transport=None,
|
613
651
|
can_prefix=0x0000):
|
614
652
|
self.id = id
|
@@ -616,6 +654,7 @@ class Controller:
|
|
616
654
|
self.position_resolution = position_resolution
|
617
655
|
self.vfoc_resolution = vfoc_resolution
|
618
656
|
self.current_resolution = current_resolution
|
657
|
+
self.pwm_resolution = pwm_resolution
|
619
658
|
self.transport = transport
|
620
659
|
self._parser = make_parser(id)
|
621
660
|
self._can_prefix = can_prefix
|
@@ -1315,6 +1354,70 @@ class Controller:
|
|
1315
1354
|
async def set_trim(self, *args, **kwargs):
|
1316
1355
|
return await self.execute(self.make_set_trim(*args, **kwargs))
|
1317
1356
|
|
1357
|
+
def make_aux_pwm(self, *,
|
1358
|
+
aux1_pwm1=None,
|
1359
|
+
aux1_pwm2=None,
|
1360
|
+
aux1_pwm3=None,
|
1361
|
+
aux1_pwm4=None,
|
1362
|
+
aux1_pwm5=None,
|
1363
|
+
aux2_pwm1=None,
|
1364
|
+
aux2_pwm2=None,
|
1365
|
+
aux2_pwm3=None,
|
1366
|
+
aux2_pwm4=None,
|
1367
|
+
aux2_pwm5=None,
|
1368
|
+
query=False,
|
1369
|
+
query_override=None):
|
1370
|
+
result = self._make_command(query=query, query_override=query_override)
|
1371
|
+
|
1372
|
+
pr = self.pwm_resolution
|
1373
|
+
resolutions = [
|
1374
|
+
pr.aux1_pwm1 if aux1_pwm1 is not None else mp.IGNORE,
|
1375
|
+
pr.aux1_pwm2 if aux1_pwm2 is not None else mp.IGNORE,
|
1376
|
+
pr.aux1_pwm3 if aux1_pwm3 is not None else mp.IGNORE,
|
1377
|
+
pr.aux1_pwm4 if aux1_pwm4 is not None else mp.IGNORE,
|
1378
|
+
pr.aux1_pwm5 if aux1_pwm5 is not None else mp.IGNORE,
|
1379
|
+
pr.aux2_pwm1 if aux2_pwm1 is not None else mp.IGNORE,
|
1380
|
+
pr.aux2_pwm2 if aux2_pwm2 is not None else mp.IGNORE,
|
1381
|
+
pr.aux2_pwm3 if aux2_pwm3 is not None else mp.IGNORE,
|
1382
|
+
pr.aux2_pwm4 if aux2_pwm4 is not None else mp.IGNORE,
|
1383
|
+
pr.aux2_pwm5 if aux2_pwm5 is not None else mp.IGNORE,
|
1384
|
+
]
|
1385
|
+
|
1386
|
+
data_buf = io.BytesIO()
|
1387
|
+
writer = Writer(data_buf)
|
1388
|
+
combiner = mp.WriteCombiner(
|
1389
|
+
writer, 0x00, int(Register.AUX1_PWM1), resolutions)
|
1390
|
+
|
1391
|
+
if combiner.maybe_write():
|
1392
|
+
writer.write_pwm(aux1_pwm1, pr.aux1_pwm1)
|
1393
|
+
if combiner.maybe_write():
|
1394
|
+
writer.write_pwm(aux1_pwm2, pr.aux1_pwm2)
|
1395
|
+
if combiner.maybe_write():
|
1396
|
+
writer.write_pwm(aux1_pwm3, pr.aux1_pwm3)
|
1397
|
+
if combiner.maybe_write():
|
1398
|
+
writer.write_pwm(aux1_pwm4, pr.aux1_pwm4)
|
1399
|
+
if combiner.maybe_write():
|
1400
|
+
writer.write_pwm(aux1_pwm5, pr.aux1_pwm5)
|
1401
|
+
if combiner.maybe_write():
|
1402
|
+
writer.write_pwm(aux2_pwm1, pr.aux2_pwm1)
|
1403
|
+
if combiner.maybe_write():
|
1404
|
+
writer.write_pwm(aux2_pwm2, pr.aux2_pwm2)
|
1405
|
+
if combiner.maybe_write():
|
1406
|
+
writer.write_pwm(aux2_pwm3, pr.aux2_pwm3)
|
1407
|
+
if combiner.maybe_write():
|
1408
|
+
writer.write_pwm(aux2_pwm4, pr.aux2_pwm4)
|
1409
|
+
if combiner.maybe_write():
|
1410
|
+
writer.write_pwm(aux2_pwm5, pr.aux2_pwm5)
|
1411
|
+
|
1412
|
+
self._format_query(query, query_override, data_buf, result)
|
1413
|
+
|
1414
|
+
result.data = data_buf.getvalue()
|
1415
|
+
|
1416
|
+
return result
|
1417
|
+
|
1418
|
+
async def set_aux_pwm(self, *args, **kwargs):
|
1419
|
+
return await self.execute(self.make_aux_pwm(*args, **kwargs))
|
1420
|
+
|
1318
1421
|
def _extract(self, value):
|
1319
1422
|
if len(value):
|
1320
1423
|
return value[0]
|
moteus/moteus_tool.py
CHANGED
@@ -44,6 +44,26 @@ MAX_FLASH_BLOCK_SIZE = 32
|
|
44
44
|
FIND_TARGET_TIMEOUT = 0.01 if sys.platform != 'win32' else 0.05
|
45
45
|
|
46
46
|
|
47
|
+
def _wrap_neg_pi_to_pi(value):
|
48
|
+
while value > math.pi:
|
49
|
+
value -= 2.0 * math.pi
|
50
|
+
while value < -math.pi:
|
51
|
+
value += 2.0 * math.pi
|
52
|
+
return value
|
53
|
+
|
54
|
+
|
55
|
+
def lerp(array, ratio):
|
56
|
+
array_size = len(array)
|
57
|
+
|
58
|
+
left_index = int(math.floor(ratio * array_size))
|
59
|
+
right_index = (left_index + 1) % array_size
|
60
|
+
fraction = (ratio - left_index / array_size) * array_size
|
61
|
+
left_comp = array[left_index]
|
62
|
+
right_comp = array[right_index]
|
63
|
+
|
64
|
+
return (left_comp * (1.0 - fraction)) + (right_comp * fraction)
|
65
|
+
|
66
|
+
|
47
67
|
class FirmwareUpgrade:
|
48
68
|
'''This encodes "magic" rules about upgrading firmware, largely about
|
49
69
|
how to munge configuration options so as to not cause behavior
|
@@ -54,7 +74,7 @@ class FirmwareUpgrade:
|
|
54
74
|
self.old = old
|
55
75
|
self.new = new
|
56
76
|
|
57
|
-
SUPPORTED_ABI_VERSION =
|
77
|
+
SUPPORTED_ABI_VERSION = 0x0109
|
58
78
|
|
59
79
|
if new > SUPPORTED_ABI_VERSION:
|
60
80
|
raise RuntimeError(f"\nmoteus_tool needs to be upgraded to support this firmware\n\n (likely 'python -m pip install --upgrade moteus')\n\nThe provided firmare is ABI version 0x{new:04x} but this moteus_tool only supports up to 0x{SUPPORTED_ABI_VERSION:04x}")
|
@@ -66,6 +86,42 @@ class FirmwareUpgrade:
|
|
66
86
|
lines = old_config.split(b'\n')
|
67
87
|
items = dict([line.split(b' ') for line in lines if b' ' in line])
|
68
88
|
|
89
|
+
if self.new <= 0x0108 and self.old >= 0x0109:
|
90
|
+
# When downgrading, we should warn if a motor thermistor
|
91
|
+
# value other than 47k is configured and enabled.
|
92
|
+
if (int(items.get(b'servo.enable_motor_temperature', '0')) != 0 and
|
93
|
+
int(float(items.get(b'servo.motor_thermistor_ohm', b'0.0'))) != 47000):
|
94
|
+
print("Motor thermistor values other than 47000 ohm not supported in firmware <= 0x0108, disabling")
|
95
|
+
items[b'servo.enable_motor_temperature'] = b'0'
|
96
|
+
|
97
|
+
items.pop(b'servo.motor_thermistor_ohm')
|
98
|
+
|
99
|
+
# Aux PWM out is not supported on <= 0x0108.
|
100
|
+
items.pop(b'aux1.pwm_period_us')
|
101
|
+
items.pop(b'aux2.pwm_period_us')
|
102
|
+
|
103
|
+
def make_key(mpsource, index):
|
104
|
+
return f'motor_position.sources.{mpsource}.compensation_table.{index}'.encode('utf8')
|
105
|
+
|
106
|
+
if make_key(0, 0) in items:
|
107
|
+
# Downsample encoder compensation bins.
|
108
|
+
print("Downsampling encoder compensation tables from version >= 0x0109")
|
109
|
+
for mpsource in range(0, 3):
|
110
|
+
bins = []
|
111
|
+
|
112
|
+
scale = float(items.pop(f'motor_position.sources.{mpsource}.compensation_scale'.encode('utf8'), 0.0)) / 127.0
|
113
|
+
new_size = 256
|
114
|
+
old_size = 32
|
115
|
+
ratio = new_size // old_size
|
116
|
+
|
117
|
+
for i in range(0, new_size):
|
118
|
+
key = make_key(mpsource, i)
|
119
|
+
bins.append(float(items.get(key)) * scale)
|
120
|
+
del items[key]
|
121
|
+
|
122
|
+
for i in range(0, old_size):
|
123
|
+
items[make_key(mpsource, i)] = str(bins[i*ratio]).encode('utf8')
|
124
|
+
|
69
125
|
if self.new <= 0x0107 and self.old >= 0x0108:
|
70
126
|
if float(items.get(b'servo.bemf_feedforward', '0')) == 0.0:
|
71
127
|
print("Reverting servo.bemf_feedforward to 1.0")
|
@@ -75,7 +131,7 @@ class FirmwareUpgrade:
|
|
75
131
|
# motor_position.output.sign was broken in older versions.
|
76
132
|
if int(items[b'motor_position.output.sign']) != 1:
|
77
133
|
print("WARNING: motor_position.output.sign==-1 is broken in order versions, disabling")
|
78
|
-
items[b'motor_position.output.sign'] = '1'
|
134
|
+
items[b'motor_position.output.sign'] = b'1'
|
79
135
|
pass
|
80
136
|
|
81
137
|
if self.new <= 0x0105 and self.old >= 0x0106:
|
@@ -307,6 +363,67 @@ class FirmwareUpgrade:
|
|
307
363
|
print("Upgrading servo.bemf_feedforward to 0.0")
|
308
364
|
items[b'servo.bemf_feedforward'] = b'0.0'
|
309
365
|
|
366
|
+
if self.new >= 0x0109 and self.old <= 0x0108:
|
367
|
+
# Try to fix up the motor commutation offset tables.
|
368
|
+
old_offsets = []
|
369
|
+
i = 0
|
370
|
+
while True:
|
371
|
+
key = f'motor.offset.{i}'.encode('utf8')
|
372
|
+
if key not in items:
|
373
|
+
break
|
374
|
+
old_offsets.append(float(items.get(key)))
|
375
|
+
i += 1
|
376
|
+
|
377
|
+
offsets = old_offsets[:]
|
378
|
+
|
379
|
+
# Unwrap this, then re-center the whole thing around 0.
|
380
|
+
for i in range(1, len(offsets)):
|
381
|
+
offsets[i] = (offsets[i - 1] +
|
382
|
+
_wrap_neg_pi_to_pi(offsets[i] -
|
383
|
+
offsets[i - 1]))
|
384
|
+
mean_offset = sum(offsets) / len(offsets)
|
385
|
+
delta = mean_offset - _wrap_neg_pi_to_pi(mean_offset)
|
386
|
+
offsets = [x - delta for x in offsets]
|
387
|
+
|
388
|
+
if any([abs(a - b) > 0.01
|
389
|
+
for a, b in zip(offsets, old_offsets)]):
|
390
|
+
print("Re-wrapping motor commutation offsets")
|
391
|
+
for i in range(len(offsets)):
|
392
|
+
key = f'motor.offset.{i}'.encode('utf8')
|
393
|
+
items[key] = f'{offsets[i]}'.encode('utf8')
|
394
|
+
|
395
|
+
if self.new >= 0x0109 and self.old <= 0x0108:
|
396
|
+
# If we had a motor thermistor enabled in previous
|
397
|
+
# versions, it was with a value of 47000.
|
398
|
+
if int(items.get(b'servo.enable_motor_temperature', b'0')) != 0:
|
399
|
+
print("Thermistor from <= 0x0109 assumed to be 47000")
|
400
|
+
items[b'servo.motor_thermistor_ohm'] = b'47000'
|
401
|
+
|
402
|
+
def make_key(mpsource, index):
|
403
|
+
return f'motor_position.sources.{mpsource}.compensation_table.{index}'.encode('utf8')
|
404
|
+
|
405
|
+
new_size = 256
|
406
|
+
old_size = 32
|
407
|
+
ratio = new_size // old_size
|
408
|
+
|
409
|
+
if make_key(0, 0) in items:
|
410
|
+
print("Upsampling encoder compensation tables for version >= 0x0109")
|
411
|
+
for mpsource in range(0, 3):
|
412
|
+
old_bins = [float(items.get(make_key(mpsource, i)).decode('latin1')) for i in range(0, 32)]
|
413
|
+
scale = max([abs(x) for x in old_bins])
|
414
|
+
bins = []
|
415
|
+
for i in range(new_size):
|
416
|
+
if scale != 0.0:
|
417
|
+
old_i = i // ratio
|
418
|
+
value = lerp(old_bins, i / new_size)
|
419
|
+
int_value = int(127 * value / scale)
|
420
|
+
else:
|
421
|
+
int_value = 0
|
422
|
+
items[make_key(mpsource, i)] = str(int_value).encode('utf8')
|
423
|
+
|
424
|
+
items[f'motor_position.sources.{mpsource}.compensation_scale'.encode('utf8')] = str(scale).encode('utf8')
|
425
|
+
|
426
|
+
|
310
427
|
lines = [key + b' ' + value for key, value in items.items()]
|
311
428
|
return b'\n'.join(lines)
|
312
429
|
|
@@ -651,7 +768,13 @@ class Stream:
|
|
651
768
|
async def write_config_stream(self, fp):
|
652
769
|
errors = []
|
653
770
|
|
654
|
-
|
771
|
+
config_lines = fp.readlines()
|
772
|
+
|
773
|
+
for i, line in enumerate(config_lines):
|
774
|
+
if i % 20 == 0:
|
775
|
+
print(f"Writing config {100*i/len(config_lines):.0f}% ",
|
776
|
+
end='\r', flush=True)
|
777
|
+
|
655
778
|
line = line.rstrip()
|
656
779
|
if len(line) == 0:
|
657
780
|
continue
|
@@ -661,6 +784,8 @@ class Stream:
|
|
661
784
|
except moteus.CommandError as ce:
|
662
785
|
errors.append(line.decode('latin1'))
|
663
786
|
|
787
|
+
print()
|
788
|
+
|
664
789
|
if len(errors):
|
665
790
|
print("\nSome config could not be set:")
|
666
791
|
for line in errors:
|
@@ -824,6 +949,19 @@ class Stream:
|
|
824
949
|
|
825
950
|
return cal_voltage
|
826
951
|
|
952
|
+
async def clear_motor_offsets(self):
|
953
|
+
i = 0
|
954
|
+
while True:
|
955
|
+
try:
|
956
|
+
await self.command(f"conf set motor.offset.{i} 0")
|
957
|
+
except moteus.CommandError as ce:
|
958
|
+
if 'error setting' in ce.message:
|
959
|
+
# This means we hit the end of the offsets.
|
960
|
+
break
|
961
|
+
else:
|
962
|
+
raise
|
963
|
+
i += 1
|
964
|
+
|
827
965
|
async def do_calibrate(self):
|
828
966
|
self.firmware = await self.read_data("firmware")
|
829
967
|
|
@@ -833,6 +971,11 @@ class Stream:
|
|
833
971
|
print("This will move the motor, ensure it can spin freely!")
|
834
972
|
await asyncio.sleep(2.0)
|
835
973
|
|
974
|
+
# Force all existing offsets to 0, that way if we had a
|
975
|
+
# discontinuous offset error, sending the stop will be able to
|
976
|
+
# clear it (and we are about to overwrite them anyway).
|
977
|
+
await self.clear_motor_offsets()
|
978
|
+
|
836
979
|
# Clear any faults that may be there.
|
837
980
|
await self.command("d stop")
|
838
981
|
|
@@ -1002,6 +1145,10 @@ class Stream:
|
|
1002
1145
|
raise RuntimeError(
|
1003
1146
|
'hall effect calibration requires specifying --cal-motor-poles')
|
1004
1147
|
|
1148
|
+
if (self.args.cal_motor_poles % 2) == 1:
|
1149
|
+
raise RuntimeError(
|
1150
|
+
'only motors with even numbers of poles are supported')
|
1151
|
+
|
1005
1152
|
commutation_source = await self.read_config_int(
|
1006
1153
|
"motor_position.commutation_source")
|
1007
1154
|
aux_number = await self.read_config_int(
|
@@ -1062,21 +1209,26 @@ class Stream:
|
|
1062
1209
|
await self.command("d stop")
|
1063
1210
|
|
1064
1211
|
async def ensure_valid_theta(self, encoder_cal_voltage):
|
1065
|
-
|
1066
|
-
|
1067
|
-
|
1068
|
-
|
1069
|
-
await self.command(f"conf set motor.poles 2")
|
1212
|
+
# We might need to have some sense of pole count first.
|
1213
|
+
if await self.read_config_double("motor.poles") == 0:
|
1214
|
+
# Pick something arbitrary for now.
|
1215
|
+
await self.command(f"conf set motor.poles 2")
|
1070
1216
|
|
1217
|
+
try:
|
1071
1218
|
motor_position = await self.read_data("motor_position")
|
1072
|
-
if motor_position.homed == 0 and not motor_position.theta_valid:
|
1073
|
-
# We need to find an index.
|
1074
|
-
await self.find_index(encoder_cal_voltage)
|
1075
1219
|
except RuntimeError:
|
1076
1220
|
# Odds are we don't support motor_position, in which case
|
1077
1221
|
# theta is always valid for the older versions that don't
|
1078
1222
|
# support it.
|
1079
|
-
|
1223
|
+
return
|
1224
|
+
|
1225
|
+
if motor_position.error != 0:
|
1226
|
+
raise RuntimeError(
|
1227
|
+
f"encoder error: {repr(motor_position.error)}")
|
1228
|
+
|
1229
|
+
if motor_position.homed == 0 and not motor_position.theta_valid:
|
1230
|
+
# We need to find an index.
|
1231
|
+
await self.find_index(encoder_cal_voltage)
|
1080
1232
|
|
1081
1233
|
async def calibrate_encoder_mapping_absolute(self, encoder_cal_voltage):
|
1082
1234
|
await self.ensure_valid_theta(encoder_cal_voltage)
|
@@ -1119,7 +1271,10 @@ class Stream:
|
|
1119
1271
|
cal_file,
|
1120
1272
|
desired_direction=1 if not self.args.cal_invert else -1,
|
1121
1273
|
max_remainder_error=self.args.cal_max_remainder,
|
1122
|
-
allow_phase_invert=allow_phase_invert
|
1274
|
+
allow_phase_invert=allow_phase_invert,
|
1275
|
+
allow_optimize=not self.args.cal_disable_optimize,
|
1276
|
+
force_optimize=self.args.cal_force_optimize,
|
1277
|
+
)
|
1123
1278
|
|
1124
1279
|
if cal_result.errors:
|
1125
1280
|
raise RuntimeError(f"Error(s) calibrating: {cal_result.errors}")
|
@@ -1360,9 +1515,19 @@ class Stream:
|
|
1360
1515
|
return 1 if x >= 0 else -1
|
1361
1516
|
|
1362
1517
|
velocity_samples = []
|
1518
|
+
power_samples = []
|
1363
1519
|
|
1364
1520
|
while True:
|
1365
1521
|
data = await self.read_servo_stats()
|
1522
|
+
|
1523
|
+
total_current_A = math.hypot(data.d_A, data.q_A)
|
1524
|
+
total_power_W = voltage * total_current_A
|
1525
|
+
|
1526
|
+
power_samples.append(total_power_W)
|
1527
|
+
|
1528
|
+
if len(power_samples) > AVERAGE_COUNT:
|
1529
|
+
del power_samples[0]
|
1530
|
+
|
1366
1531
|
velocity_samples.append(data.velocity)
|
1367
1532
|
|
1368
1533
|
if len(velocity_samples) > (3 * AVERAGE_COUNT):
|
@@ -1374,6 +1539,24 @@ class Stream:
|
|
1374
1539
|
if (time.time() - start_time) > 2.0:
|
1375
1540
|
return recent_average
|
1376
1541
|
|
1542
|
+
if len(power_samples) >= AVERAGE_COUNT:
|
1543
|
+
average_power_W = sum(power_samples) / len(power_samples)
|
1544
|
+
max_power_W = (self.args.cal_max_kv_power_factor *
|
1545
|
+
self.args.cal_motor_power)
|
1546
|
+
|
1547
|
+
# This is a safety. During speed measurement, current
|
1548
|
+
# should always be near 0. However, if the encoder
|
1549
|
+
# commutation calibration failed, we can sometimes
|
1550
|
+
# trigger large currents during the Kv detection phase
|
1551
|
+
# while not actually moving.
|
1552
|
+
if (abs(recent_average) < 0.2 and
|
1553
|
+
average_power_W > max_power_W):
|
1554
|
+
await self.command("d stop")
|
1555
|
+
|
1556
|
+
raise RuntimeError(
|
1557
|
+
f"Motor failed to spin, {average_power_W} > " +
|
1558
|
+
f"{max_power_W}")
|
1559
|
+
|
1377
1560
|
if (len(velocity_samples) >= AVERAGE_COUNT and
|
1378
1561
|
abs(recent_average) < 0.2):
|
1379
1562
|
return recent_average
|
@@ -1765,11 +1948,17 @@ async def async_main():
|
|
1765
1948
|
parser.add_argument('--cal-force-kv', metavar='Kv', type=float,
|
1766
1949
|
default=None,
|
1767
1950
|
help='do not calibrate Kv, but use the specified value')
|
1951
|
+
parser.add_argument('--cal-force-optimize', action='store_true',
|
1952
|
+
help='require nonlinear commutation optimization')
|
1953
|
+
parser.add_argument('--cal-disable-optimize', action='store_true',
|
1954
|
+
help='prevent nonlinear commutation optimization')
|
1768
1955
|
|
1769
1956
|
|
1770
1957
|
parser.add_argument('--cal-max-remainder', metavar='F',
|
1771
1958
|
type=float, default=0.1,
|
1772
1959
|
help='maximum allowed error in calibration')
|
1960
|
+
parser.add_argument('--cal-max-kv-power-factor', type=float,
|
1961
|
+
default=1.25)
|
1773
1962
|
parser.add_argument('--cal-raw', metavar='FILE', type=str,
|
1774
1963
|
help='write raw calibration data')
|
1775
1964
|
|
moteus/version.py
CHANGED
@@ -1,21 +1,25 @@
|
|
1
1
|
Metadata-Version: 2.1
|
2
2
|
Name: moteus
|
3
|
-
Version: 0.3.
|
3
|
+
Version: 0.3.73
|
4
4
|
Summary: moteus brushless controller library and tools
|
5
5
|
Home-page: https://github.com/mjbots/moteus
|
6
6
|
Author: mjbots Robotic Systems
|
7
7
|
Author-email: info@mjbots.com
|
8
|
+
License: UNKNOWN
|
8
9
|
Keywords: moteus
|
10
|
+
Platform: UNKNOWN
|
9
11
|
Classifier: Development Status :: 3 - Alpha
|
10
12
|
Classifier: Intended Audience :: Developers
|
11
13
|
Classifier: License :: OSI Approved :: Apache Software License
|
12
14
|
Classifier: Programming Language :: Python :: 3
|
13
15
|
Requires-Python: >=3.7, <4
|
14
16
|
Description-Content-Type: text/markdown
|
15
|
-
Requires-Dist:
|
16
|
-
Requires-Dist:
|
17
|
-
Requires-Dist: pyelftools >=0.26
|
18
|
-
Requires-Dist:
|
17
|
+
Requires-Dist: importlib-metadata (>=3.6)
|
18
|
+
Requires-Dist: numpy (<2)
|
19
|
+
Requires-Dist: pyelftools (>=0.26)
|
20
|
+
Requires-Dist: pyserial (>=3.5)
|
21
|
+
Requires-Dist: python-can (>=3.3)
|
22
|
+
Requires-Dist: scipy (>=1.8.0)
|
19
23
|
Requires-Dist: pywin32 ; platform_system == "Windows"
|
20
24
|
|
21
25
|
# Python bindings for moteus brushless controller #
|
@@ -117,3 +121,5 @@ qr.torque = moteus.F32
|
|
117
121
|
|
118
122
|
c = moteus.Controller(position_resolution=pr, query_resolution=qr)
|
119
123
|
```
|
124
|
+
|
125
|
+
|
@@ -1,12 +1,12 @@
|
|
1
1
|
moteus/__init__.py,sha256=DxasQre5HmK3BS3I35K8GEUvFZF3_OE_hjVzQy-seIE,739
|
2
2
|
moteus/aioserial.py,sha256=GeWuvsZKCRrfBN33JZFjtBXPr-0sKpQv9shRn2ulcDA,1079
|
3
3
|
moteus/aiostream.py,sha256=YAkVF6QWsA49vqO-GgXEohDghqm_-nnajJzhO_Q9qNQ,3696
|
4
|
-
moteus/calibrate_encoder.py,sha256=
|
4
|
+
moteus/calibrate_encoder.py,sha256=R3gWoKY-ikYlXViiCrhX_0BH7Ahx3T7zvDjX47go-Wc,15167
|
5
5
|
moteus/command.py,sha256=UkOsbtkso6Oyex8CfbpAKpBNriik519ymxL86EZGkRs,1169
|
6
6
|
moteus/export.py,sha256=vRIfldaqz1eHtWo3993SvatATtu73TZbejL0hzQe8YE,1646
|
7
7
|
moteus/fdcanusb.py,sha256=7PrQiCTROY96gdT2zSZYU1bOCriw-I7H6NspaZpiEx4,7431
|
8
|
-
moteus/moteus.py,sha256=
|
9
|
-
moteus/moteus_tool.py,sha256=
|
8
|
+
moteus/moteus.py,sha256=vImSRBn6VEmoijD6hvrSRVxuv1_xVaEJU3F_3Wi6GiE,52498
|
9
|
+
moteus/moteus_tool.py,sha256=ALwXiDWVwiAx9yJu0o-2eKd47wyKH5mRPq264mZrn5E,79662
|
10
10
|
moteus/multiplex.py,sha256=LF6MuelzYHqqsCJuCB9YeEyUA03eBaTYRwAVotX3qm8,10120
|
11
11
|
moteus/posix_aioserial.py,sha256=2oDrw8TBEwuEQjY41g9rHeuFeffcPHqMwNS3nf5NVq8,3137
|
12
12
|
moteus/pythoncan.py,sha256=ofotOrDuaFhTLvaokaO3EJK6quVc75Bq-ue70lDMtXI,4071
|
@@ -14,10 +14,10 @@ moteus/reader.py,sha256=9i1-h4aGd4syfqtWJcpg70Bl-bmunkGU4FmXmOLyRt8,12121
|
|
14
14
|
moteus/regression.py,sha256=M5gjDBYJQ64iBXIrvBhMkD8TYhtlnQ85x8U4py0niGA,1196
|
15
15
|
moteus/router.py,sha256=501W5GZ12rFoc1lmcH3S7IYsoc-Q_-FJ4B3i37RzE3Q,2061
|
16
16
|
moteus/transport.py,sha256=WhkW2G9i25lkOlO55eI5_oXmU0PhDmxTeJ75Sg_7nTI,1021
|
17
|
-
moteus/version.py,sha256=
|
17
|
+
moteus/version.py,sha256=5_fWVR7dSRZGbeKSGDI9a1MBAxaHKTOCWtgVlCY30BE,627
|
18
18
|
moteus/win32_aioserial.py,sha256=culdl-vYxBKD5n2s5LkIMGyUaHyCcEc8BL5-DWEaxX8,2025
|
19
|
-
moteus-0.3.
|
20
|
-
moteus-0.3.
|
21
|
-
moteus-0.3.
|
22
|
-
moteus-0.3.
|
23
|
-
moteus-0.3.
|
19
|
+
moteus-0.3.73.dist-info/METADATA,sha256=HIVrIqvoZY517yT3s_Euh10uzoZjYn4KmSHj2r_7K04,3490
|
20
|
+
moteus-0.3.73.dist-info/WHEEL,sha256=G16H4A3IeoQmnOrYV4ueZGKSjhipXx8zc8nu9FGlvMA,92
|
21
|
+
moteus-0.3.73.dist-info/entry_points.txt,sha256=indCsEML1fmtWJU1WiV-d7UmmTaAMhyBLEc1iiKnexQ,57
|
22
|
+
moteus-0.3.73.dist-info/top_level.txt,sha256=aZzmI_yecTaDrdSp29pTJuowaSQ9dlIZheQpshGg4YQ,7
|
23
|
+
moteus-0.3.73.dist-info/RECORD,,
|
File without changes
|