def __init__(self, yaml: dict, logger: Logger): # Contains all components (including subcomponents). # Ports are components too. self.components_by_name = {} # {'name': object, ...} self.names_by_component = {} # {'name': object, ...} def add_component(name, component): self.components_by_name[name] = component self.names_by_component[component] = name if hasattr(component, 'get_subcomponents'): for subname, subcomponent in component.get_subcomponents(): add_component(name + '.' + subname, subcomponent) for component_yaml in yaml['components']: if component_yaml['type'] == 'odrive': add_component(component_yaml['name'], ODriveComponent(component_yaml)) elif component_yaml['type'] == 'generalpurpose': add_component(component_yaml['name'], GeneralPurposeComponent(component_yaml)) elif component_yaml['type'] == 'teensy': add_component(component_yaml['name'], TeensyComponent(self, component_yaml)) elif component_yaml['type'] == 'motor': add_component(component_yaml['name'], MotorComponent(component_yaml)) elif component_yaml['type'] == 'encoder': add_component(component_yaml['name'], EncoderComponent(self, component_yaml)) elif component_yaml['type'] == 'lpf': add_component(component_yaml['name'], LowPassFilterComponent(self)) else: logger.warn('test rig has unsupported component ' + component_yaml['type']) continue # List of disjunct sets, where each set holds references of the mutually connected components self.connections = [] for connection_yaml in yaml['connections']: self.connections.append( set(self.components_by_name[name] for name in connection_yaml)) self.connections = list(disjoint_sets(self.connections)) # Dict for fast lookup of the connection sets for each port self.net_by_component = {} for s in self.connections: for port in s: self.net_by_component[port] = s
def __init__(self, name, serial_no): self.name = name self.serial_no = serial_no self.shutdown_token = Event() self.logger = Logger(verbose=False) self.device = None odrive.find_all("usb", self.serial_no, lambda device: self.did_discover_device(device), self.shutdown_token, self.shutdown_token, self.logger)
def prepare(self, logger: Logger): """ Connects to the ODrive """ if not self.handle is None: return logger.debug('waiting for {} ({})'.format(self.yaml['name'], self.yaml['serial-number'])) self.handle = odrive.find_any(path="usb", serial_number=self.yaml['serial-number'], timeout=60) #, printer=print) assert (self.handle) #for axis_idx, axis_ctx in enumerate(self.axes): # axis_ctx.handle = self.handle.__dict__['axis{}'.format(axis_idx)] for encoder_idx, encoder_ctx in enumerate(self.encoders): encoder_ctx.handle = self.handle.__dict__['axis{}'.format( encoder_idx)].encoder # TODO: distinguish between axis and motor context for axis_idx, axis_ctx in enumerate(self.axes): axis_ctx.handle = self.handle.__dict__['axis{}'.format(axis_idx)]
def prepare(self, logger: Logger): """ Connects to the ODrive """ if not self.handle is None: return logger.debug('waiting for {} ({})'.format(self.yaml['name'], self.yaml['serial-number'])) self.handle = odrive.find_any(channel_termination_token=shutdown_token, serial_number=self.yaml['serial-number'], timeout=60) #, printer=print) assert (self.handle) #for axis_idx, axis_ctx in enumerate(self.axes): # axis_ctx.handle = getattr(self.handle, f'axis{axis_idx}') for encoder_idx, encoder_ctx in enumerate(self.encoders): encoder_ctx.handle = getattr(self.handle, f'axis{encoder_idx}').encoder # TODO: distinguish between axis and motor context for axis_idx, axis_ctx in enumerate(self.axes): axis_ctx.handle = getattr(self.handle, f'axis{axis_idx}')
def prepare(self, logger: Logger): # Make sure no funny configuration is active logger.debug('Setting up clean configuration...') self.axis_ctx.parent.erase_config_and_reboot() # Set motor calibration values self.axis_ctx.handle.motor.config.phase_resistance = float( self.motor_ctx.yaml['phase-resistance']) self.axis_ctx.handle.motor.config.phase_inductance = float( self.motor_ctx.yaml['phase-inductance']) self.axis_ctx.handle.motor.config.pre_calibrated = True # Set brake resistor settings if 'brake-resistance' in self.axis_ctx.parent.yaml: logger.debug( f"brake resistor set to {self.axis_ctx.parent.yaml['brake-resistance']} Ohm" ) self.axis_ctx.parent.handle.config.brake_resistance = float( self.axis_ctx.parent.yaml['brake-resistance']) # The docs say this requires a reboot but here's a small secret: # Since the brake resistor is also started in clear_errors() this # circumvents the need for a reboot. self.axis_ctx.parent.handle.config.enable_brake_resistor = True else: logger.debug("brake resistor disabled") # TODO: set vbus voltage trip level based on yaml self.axis_ctx.parent.handle.config.dc_max_negative_current = -1 self.axis_ctx.parent.handle.config.enable_brake_resistor = False # Set calibration settings self.axis_ctx.handle.encoder.config.direction = 0 self.axis_ctx.handle.encoder.config.use_index = False self.axis_ctx.handle.encoder.config.calib_scan_omega = 12.566 # 2 electrical revolutions per second self.axis_ctx.handle.encoder.config.calib_scan_distance = 50.265 # 8 revolutions self.axis_ctx.handle.encoder.config.bandwidth = 1000 self.axis_ctx.parent.handle.clear_errors() logger.debug('Calibrating encoder offset...') request_state(self.axis_ctx, AXIS_STATE_ENCODER_OFFSET_CALIBRATION) time.sleep(9) # actual calibration takes 8 seconds test_assert_eq(self.axis_ctx.handle.current_state, AXIS_STATE_IDLE) test_assert_no_error(self.axis_ctx)
def test_watchdog(axis, feed_func, logger: Logger): """ Tests the watchdog of one axis, using the provided function to feed the watchdog. This test assumes that the testing host has no more than 300ms random delays. """ start = time.monotonic() axis.config.enable_watchdog = False axis.error = 0 axis.config.watchdog_timeout = 1.0 axis.watchdog_feed() axis.config.enable_watchdog = True test_assert_eq(axis.error, 0) for _ in range(5): # keep the watchdog alive for 3.5 seconds time.sleep(0.7) logger.debug('feeding watchdog at {}s'.format(time.monotonic() - start)) feed_func() err = axis.error logger.debug('checking error at {}s'.format(time.monotonic() - start)) test_assert_eq(err, 0) logger.debug('letting watchdog expire...') time.sleep(1.3) # let the watchdog expire test_assert_eq(axis.error, errors.axis.ERROR_WATCHDOG_TIMER_EXPIRED)
parser.add_argument("--test-rig-yaml", type=argparse.FileType('r'), required=True, help="test rig YAML file") parser.add_argument( "--setup-host", action='store_true', default=False, help="configure operating system functions such as GPIOs (requires root)") parser.set_defaults(ignore=[]) args = parser.parse_args() # Load objects test_rig_yaml = yaml.load(args.test_rig_yaml, Loader=yaml.BaseLoader) logger = Logger() testrig = TestRig(test_rig_yaml, logger) if args.setup_host: for gpio in testrig.get_components(LinuxGpioComponent): num = gpio.num logger.debug('exporting GPIO ' + str(num) + ' to user space...') if not os.path.isdir("/sys/class/gpio/gpio{}".format(num)): with open("/sys/class/gpio/export", "w") as fp: fp.write(str(num)) os.chmod("/sys/class/gpio/gpio{}/value".format(num), stat.S_IROTH | stat.S_IWOTH) os.chmod("/sys/class/gpio/gpio{}/direction".format(num), stat.S_IROTH | stat.S_IWOTH)
if args.test_rig_yaml is None: test_rig_name = os.environ.get('ODRIVE_TEST_RIG_NAME', '') if test_rig_name == '': print( "You must either provide a --test-rig-yaml argument or set the environment variable ODRIVE_TEST_RIG_NAME." ) sys.exit(1) path = os.path.join( os.path.dirname( os.path.dirname(os.path.dirname(os.path.realpath(__file__)))), test_rig_name + '.yaml') args.test_rig_yaml = open(path, 'r') # Load objects test_rig_yaml = yaml.load(args.test_rig_yaml, Loader=yaml.BaseLoader) logger = Logger() testrig = TestRig(test_rig_yaml, logger) shutdown_token = fibre.Event() if args.setup_host: for gpio in testrig.get_components(LinuxGpioComponent): num = gpio.num logger.debug('exporting GPIO ' + str(num) + ' to user space...') if not os.path.isdir("/sys/class/gpio/gpio{}".format(num)): with open("/sys/class/gpio/export", "w") as fp: fp.write(str(num)) os.chmod("/sys/class/gpio/gpio{}/value".format(num), stat.S_IRWXU | stat.S_IRWXG | stat.S_IRWXO) os.chmod("/sys/class/gpio/gpio{}/direction".format(num), stat.S_IRWXU | stat.S_IRWXG | stat.S_IRWXO)