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.
@@ -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] = _wrap_neg_pi_to_pi(values[wrap(j)] - values[wrap(start)])
163
- result[i] = values[wrap(start)] + (sum(errs) / len(errs))
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 = [_wrap_neg_pi_to_pi(a - b) for a, b in zip(avg_interp, expected)]
327
+ err = [a - b for a, b in zip(avg_interp, expected)]
319
328
 
320
- # Make the error seem reasonable, so unwrap if we happen to span
321
- # the pi boundary.
322
- if (max(err) - min(err)) > 1.5 * math.pi:
323
- err = [x if x > 0 else x + 2 * math.pi for x in err]
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, 1024))
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 = 0x0108
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
- for line in fp.readlines():
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
- try:
1066
- # We might need to have some sense of pole count first.
1067
- if await self.read_config_double("motor.poles") == 0:
1068
- # Pick something arbitrary for now.
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
- pass
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
@@ -12,4 +12,4 @@
12
12
  # See the License for the specific language governing permissions and
13
13
  # limitations under the License.
14
14
 
15
- VERSION="0.3.71"
15
+ VERSION="0.3.73"
@@ -1,21 +1,25 @@
1
1
  Metadata-Version: 2.1
2
2
  Name: moteus
3
- Version: 0.3.71
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: pyserial >=3.5
16
- Requires-Dist: python-can >=3.3
17
- Requires-Dist: pyelftools >=0.26
18
- Requires-Dist: importlib-metadata >=3.6
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=R3pRgGqrDCcmKQqFE7Fr4p8TSie179iqNfBwEJS3pL0,12416
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=gARNQ2_irh_1zp5YBTXmZK5U7SziBna8R140e_Wyfxk,48998
9
- moteus/moteus_tool.py,sha256=KBs5jhjbiJzPhDtLtG5v5e-JVd8Na6R1wXwshTORAQc,71806
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=fRb1RjssxPcJGNsseW3PhLRKjIGLBI8Kcy8nFzG1RJA,627
17
+ moteus/version.py,sha256=5_fWVR7dSRZGbeKSGDI9a1MBAxaHKTOCWtgVlCY30BE,627
18
18
  moteus/win32_aioserial.py,sha256=culdl-vYxBKD5n2s5LkIMGyUaHyCcEc8BL5-DWEaxX8,2025
19
- moteus-0.3.71.dist-info/METADATA,sha256=Aig2WFPGut825pNwrj1AbhlBHELoajjrMzoBMZSNrys,3388
20
- moteus-0.3.71.dist-info/WHEEL,sha256=yQN5g4mg4AybRjkgi-9yy4iQEFibGQmlz78Pik5Or-A,92
21
- moteus-0.3.71.dist-info/entry_points.txt,sha256=accRcwir_K8wCf7i3qHb5R6CPh5SiSgd5a1A92ibb9E,56
22
- moteus-0.3.71.dist-info/top_level.txt,sha256=aZzmI_yecTaDrdSp29pTJuowaSQ9dlIZheQpshGg4YQ,7
23
- moteus-0.3.71.dist-info/RECORD,,
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,,
@@ -1,5 +1,5 @@
1
1
  Wheel-Version: 1.0
2
- Generator: bdist_wheel (0.41.2)
2
+ Generator: bdist_wheel (0.37.1)
3
3
  Root-Is-Purelib: true
4
4
  Tag: py3-none-any
5
5
 
@@ -1,2 +1,3 @@
1
1
  [console_scripts]
2
2
  moteus_tool = moteus.moteus_tool:main
3
+