Source code for wni.actuator

"""Control the actuator from Python!  Wow."""

from __future__ import print_function, absolute_import, division

import struct
import sys

import nanomsg

import wni.config as config
from wni.util import setter


class Actuator(object):
    """Control Firgelli USB from Microzed

    Call Actuator.position to get the position reported by the actuator
    directly, or Actuator.cached_position to get the cached position.  It takes
    about 100 ms to talk to the hardware, so the caching is necessary.
    """
    ID_VENDOR = 1240
    ID_PRODUCT = 64607

    ACCURACY = 20
    EXTEND_LIMIT = 200
    RETRACT_LIMIT = 150
    SPEED = 100

    def __init__(self, idVendor=ID_VENDOR, idProduct=ID_PRODUCT):
        import usb.core
        import usb.util
        self.dev = usb.core.find(idVendor=idVendor, idProduct=idProduct)
        if self.dev is None:
            msg = ('Could not find USB device with vendor id {}, product id {}'
                   .format(idVendor, idProduct))
            raise ValueError(msg)
        cfg = self.dev.get_active_configuration()
        intf = cfg[0,0]
        matcher = lambda e: usb.util.endpoint_direction(e.bEndpointAddress) == usb.util.ENDPOINT_IN
        self.endpoint_in = usb.util.find_descriptor(
            intf,
            custom_match=matcher
        )
        self.set_defaults()
        self.cached_position = self.position

    def write(self, addr, value):
        addr = struct.pack('B', addr)
        if value is not None:
            value = struct.pack('H', value)
        else:
            value = b''
        msg =  addr + value
        self.dev.write(1, msg)

    @property
    def position(self):
        while True:
            self.write(0x10, None)
            ret = self._read_current_value()
            if ret[0] == 0x10:
                self.cached_position = struct.unpack('H', struct.pack('BB', ret[1], ret[2]))[0]
                return self.cached_position

    @position.setter
    def position(self, pos):
        """Set position from 0-1023"""
        self.write(0x20, pos)
        # this is a little fishy... we'll assume that the actuator gets
        # _exactly to the position we asked for.  It almost definitely doesn't
        self.cached_position = pos

    @property
    def accuracy(self, acc):
        return self._accuracy

    @accuracy.setter
    def accuracy(self, acc):
        """Sets accuracy between 0-1023; smaller is more accurate."""
        self.write(0x01, acc)
        self._accuracy = acc

    @setter
    def retract_limit(self, limit):
        """Sets retraction limit between 0-1023; larger is more extended"""
        self.write(0x02, limit)

    @setter
    def extend_limit(self, limit):
        """Sets extension limit between 0-1023; larger is more extended."""
        self.write(0x03, limit)

    @property
    def speed(self):
        return self._speed

    @speed.setter
    def speed(self, value):
        """Sets the max speed the actuator can make outside of stall conditions"""
        self.write(0x21, value)
        self._speed = value

    def _read_current_value(self):
        return self.endpoint_in.read(3)

    def get_feedbacks(self, n):
        return [self.get_feedback() for _ in range(n)]

    def set_defaults(self):
        self.accuracy = self.ACCURACY
        self.extend_limit = self.EXTEND_LIMIT
        self.retract_limit = self.RETRACT_LIMIT
        self.speed = self.SPEED
        # self.position = self.position

class FakeActuator(object):

    ACCURACY = Actuator.ACCURACY
    EXTEND_LIMIT = Actuator.EXTEND_LIMIT
    RETRACT_LIMIT = Actuator.RETRACT_LIMIT
    SPEED = Actuator.SPEED
    def __init__(self, *args, **kwargs):
        self.position = 200
        self.cached_position = 200


if __name__ == '__main__':
    act = Actuator()
    print('an actuator has been instantiated as "act".  Go crazy.')