def find_any(path="usb", serial_number=None,
search_cancellation_token=None, channel_termination_token=None,
timeout=None, logger=Logger(verbose=False), find_multiple=False):
"""
Blocks until the first matching Fibre object is connected and then returns that object
"""
result = []
done_signal = Event(search_cancellation_token)
def did_discover_object(obj):
result.append(obj)
if find_multiple:
if len(result) >= int(find_multiple):
done_signal.set()
else:
done_signal.set()
find_all(path, serial_number, did_discover_object, done_signal, channel_termination_token, logger)
try:
done_signal.wait(timeout=timeout)
except TimeoutError:
if not find_multiple:
return None
finally:
done_signal.set() # terminate find_all
if find_multiple:
return result
else:
return result[0] if len(result) > 0 else None
def get_odrives(self):
self.odrvs = [None, None, None]
# Get ODrives
done_signal = Event(None)
def discovered_odrv(obj):
print("Found odrive with sn: {}".format(obj.serial_number))
if obj.serial_number in self.SERIAL_NUMS:
self.odrvs[self.SERIAL_NUMS.index(obj.serial_number)] = obj
print("ODrive is # {}".format(
self.SERIAL_NUMS.index(obj.serial_number)))
else:
print("ODrive sn not found in list. New ODrive?")
if not None in self.odrvs:
done_signal.set()
odrive.find_all("usb", None, discovered_odrv, done_signal, None,
Logger(verbose=False))
# Wait for ODrives
try:
done_signal.wait(timeout=120)
finally:
done_signal.set()
rospy.loginfo("Found ODrives")
def find_all_odrives(self,
path="usb",
serial_number=None,
search_cancellation_token=None,
channel_termination_token=None,
timeout=30,
logger=Logger(verbose=True)):
"""
Blocks until timeout
"""
result = []
done_signal = Event(search_cancellation_token)
self.drive_count = 0
def did_discover_object(obj):
result.append(obj)
self.drive_count += 1
odrive.find_all(path, serial_number, did_discover_object, done_signal,
channel_termination_token, logger)
try:
done_signal.wait(timeout=timeout)
except TimeoutError:
print("Timeouted")
finally:
done_signal.set() # terminate find_all
return result
def find_any(path="usb", serial_number=None,
search_cancellation_token=None, channel_termination_token=None,
timeout=None, logger=Logger(verbose=False)):
"""
Blocks until the first matching Fibre node is connected and then returns that node
"""
result = [ None ]
done_signal = Event(search_cancellation_token)
def did_discover_object(obj):
result[0] = obj
done_signal.set()
find_all(path, serial_number, did_discover_object, done_signal, channel_termination_token, logger)
try:
done_signal.wait(timeout=timeout)
finally:
done_signal.set() # terminate find_all
return result[0]
fibre_src = os.path.join(
os.path.dirname(os.path.dirname(os.path.realpath(__file__))),
"Firmware", "fibre", "python", "fibre")
fibre_link = os.path.join(os.path.dirname(os.path.realpath(__file__)),
"fibre")
if not os.path.exists(fibre_link):
os.symlink(fibre_src, fibre_link, True)
# TODO: find a better place for this
if not creating_package:
import platform
if platform.system() == 'Linux':
import odrive.utils
from fibre.utils import Logger
try:
odrive.utils.setup_udev_rules(Logger())
except PermissionError:
print(
"Warning: could not set up udev rules. Run `sudo odrivetool udev-setup` to try again."
)
setup(
name='odrive',
packages=['odrive', 'odrive.dfuse', 'fibre'],
scripts=['odrivetool', 'odrivetool.bat', 'odrive_demo.py'],
version=version,
description=
'Control utilities for the ODrive high performance motor controller',
author='Oskar Weigl',
author_email='*****@*****.**',
license='MIT',
# Get ODrives
done_signal = Event(None)
def discovered_odrv(obj):
print("Found odrive with sn: {}".format(obj.serial_number))
if obj.serial_number in SERIAL_NUMS:
odrvs[SERIAL_NUMS.index(obj.serial_number)] = obj
print("ODrive is # {}".format(SERIAL_NUMS.index(
obj.serial_number)))
else:
print("ODrive sn not found in list. New ODrive?")
if not None in odrvs:
done_signal.set()
odrive.find_all("usb", None, discovered_odrv, done_signal, None,
Logger(verbose=False))
# Wait for ODrives
try:
done_signal.wait(timeout=120)
finally:
done_signal.set()
# Which odrives
if args.which_odrive == None:
to_calib = odrvs
else:
to_calib = [odrvs[args.which_odrive]]
for odrv in to_calib:
odrv.config.brake_resistance = 0.5
print("Calibrating ODrive # {}".format(to_calib.index(odrv)))
odrvs = [None]
# Get ODrives
done_signal = Event(None)
def discovered_odrv(obj):
print("Found odrive with sn: {}".format(obj.serial_number))
if obj.serial_number in SERIAL_NUMS:
odrvs[SERIAL_NUMS.index(obj.serial_number)] = obj
print("ODrive is # {}".format(SERIAL_NUMS.index(obj.serial_number)))
else:
print("ODrive sn not found in list. New ODrive?")
if not None in odrvs:
done_signal.set()
odrive.find_all("usb", None, discovered_odrv, done_signal, None, Logger(verbose=False))
# Wait for ODrives
try:
done_signal.wait(timeout=120)
finally:
done_signal.set()
# Which odrives
if args.which_odrive == None:
to_calib = odrvs
else:
to_calib = [odrvs[args.which_odrive]]
for odrv in to_calib:
odrv.config.brake_resistance = 0.5
print("Calibrating ODrive # {}".format(to_calib.index(odrv)))
import json
import logging
import threading
import fibre
from fibre.utils import Logger
logger = logging.getLogger(__name__)
logger.setLevel(logging.DEBUG)
fibre_logger = Logger(verbose=logger.getEffectiveLevel() == logging.DEBUG)
class OdriveManager:
_CONFIG_KEY = 'config'
_CRC_KEY = 'crc16'
def __init__(self, path, serial_number, cache_file=None):
self.channel = None
self.path = path
self.serial_number = serial_number
self.cache_file = cache_file or 'odrive-cache-{serial_number}.json' \
.format(serial_number=serial_number)
def setup_odrive(self, config, crc):
"""
Set up fibre remote object using given configuration.
"""
json_data = {"name": "fibre_node", "members": config}
self.channel._interface_definition_crc = crc
# Just some black magic
"Firmware", "fibre", "python", "fibre")
fibre_link = os.path.join(os.path.dirname(os.path.realpath(__file__)),
"fibre")
if not os.path.exists(fibre_link):
if sys.version_info > (3, 3):
os.symlink(fibre_src, fibre_link, target_is_directory=True)
else:
os.symlink(fibre_src, fibre_link)
# TODO: find a better place for this
if not creating_package:
import platform
if platform.system() == 'Linux':
from fibre.utils import Logger
try:
odrive.version.setup_udev_rules(Logger())
except Exception:
print(
"Warning: could not set up udev rules. Run `sudo odrivetool udev-setup` to try again."
)
try:
setup(
name='odrive',
packages=['odrive', 'odrive.dfuse', 'fibre'],
scripts=['odrivetool', 'odrivetool.bat', 'odrive_demo.py'],
version=version,
description=
'Control utilities for the ODrive high performance motor controller',
author='Oskar Weigl',
author_email='*****@*****.**',
if test_rig_yaml['type'] == 'parallel':
#all_tests.append(TestHighVelocity())
all_tests.append(TestHighVelocityInViscousFluid(load_current=35, driver_current=45))
# all_tests.append(TestVelCtrlVsPosCtrl())
# TODO: test step/dir
# TODO: test sensorless
# TODO: test ASCII protocol
# TODO: test protocol over UART
elif test_rig_yaml['type'] == 'loopback':
all_tests.append(TestSelfLoadedPosVelDistribution(
rpm_range=3000, load_current_range=60, driver_current_lim=70))
print(str(args.ignore))
logger = Logger()
os.chdir(script_path + '/../Firmware')
# Build a dictionary of odrive test contexts by name
odrives_by_name = {}
for odrv_idx, odrv_yaml in enumerate(test_rig_yaml['odrives']):
name = odrv_yaml['name'] if 'name' in odrv_yaml else 'odrive{}'.format(odrv_idx)
if not name in args.ignore:
odrives_by_name[name] = ODriveTestContext(name, odrv_yaml)
# Build a dictionary of axis test contexts by name (e.g. odrive0.axis0)
axes_by_name = {}
for odrv_ctx in odrives_by_name.values():
for axis_idx, axis_ctx in enumerate(odrv_ctx.axes):
if not axis_ctx.name in args.ignore:
def __init__(self, timeout):
# specify left, middle, and right ODrives
rospy.loginfo("Looking for ODrives...")
self.SERIAL_NUMS = [
35593293288011, # Left, 0
35623406809166, # Middle, 1
35563278839886] # Right, 2
self.odrvs = [
None,
None,
None]
# Get ODrives
done_signal = Event(None)
def discovered_odrv(obj):
print("Found odrive with sn: {}".format(obj.serial_number))
if obj.serial_number in self.SERIAL_NUMS:
self.odrvs[self.SERIAL_NUMS.index(obj.serial_number)] = obj
print("ODrive is # {}".format(self.SERIAL_NUMS.index(obj.serial_number)))
else:
print("ODrive sn not found in list. New ODrive?")
if not None in self.odrvs:
done_signal.set()
odrive.find_all("usb", None, discovered_odrv, done_signal, None, Logger(verbose=False))
# Wait for ODrives
try:
done_signal.wait(timeout=120)
finally:
done_signal.set()
# self.odrv0 = odrive.find_any()
# # odrv1 = odrive.find_any()
# # odrv2 = odrive.find_any()
rospy.loginfo("Found ODrives")
# Set left and right axis
self.leftAxes = [self.odrvs[0].axis0, self.odrvs[0].axis1, self.odrvs[1].axis1]
self.rightAxes = [self.odrvs[1].axis0, self.odrvs[2].axis0, self.odrvs[2].axis1]
self.axes = self.leftAxes + self.rightAxes
# Set axis state
rospy.logdebug("Enabling Watchdog")
for ax in (self.leftAxes + self.rightAxes):
ax.watchdog_feed()
ax.config.watchdog_timeout = 2
ax.encoder.config.ignore_illegal_hall_state = True
# Clear errors
for odrv in self.odrvs:
dump_errors(odrv, True)
for ax in (self.leftAxes + self.rightAxes):
ax.controller.vel_ramp_enable = True
ax.requested_state = AXIS_STATE_CLOSED_LOOP_CONTROL
ax.controller.config.control_mode = CTRL_MODE_VELOCITY_CONTROL
# Sub to topic
rospy.Subscriber('joy', Joy, self.vel_callback)
# Set first watchdog
self.timeout = timeout # log error if this many seconds occur between received messages
self.timer = rospy.Timer(rospy.Duration(self.timeout), self.watchdog_callback, oneshot=True)
self.watchdog_fired = False
# Init other variables
self.last_msg_time = 0
self.last_recv_time = 0
self.next_wd_feed_time = 0
rospy.loginfo("Ready for topic")
rospy.spin()