diff --git a/uf/android/comm/usbserialandroid_ascii.py b/uf/android/comm/usbserialandroid_ascii.py new file mode 100755 index 0000000..a5284b4 --- /dev/null +++ b/uf/android/comm/usbserialandroid_ascii.py @@ -0,0 +1,69 @@ +#!/usr/bin/env python3 +# Software License Agreement (BSD License) +# +# Author: Adrian Clark +# Modifed from file written by: Duke Fong + + +import _thread, threading + +# Import USB 4 Android and USB Serial 4 Android libraries +from usb4a import usb +from usbserial4a import serial4a + +# We need to use the select_port function defined in these scripts as the port selection is slightly different +from ..utils.usbserialandroid_select_serial_port import select_port +from ...utils.log import * + +class USBSerialAndroidAscii(threading.Thread): + def __init__(self, ufc, node, iomap, dev_port = None, baud = 115200, filters = None): + + self.ports = { + 'in': {'dir': 'in', 'type': 'topic', 'callback': self.in_cb}, + 'out': {'dir': 'out', 'type': 'topic'}, + 'service': {'dir': 'in', 'type': 'service', 'callback': self.service_cb} + } + + self.node = node + self.logger = logging.getLogger('uf.' + node.replace('/', '.')) + ufc.node_init(node, self.ports, iomap) + + dev_port = select_port(logger = self.logger, dev_port = dev_port, filters = filters) + + if not dev_port: + quit(1) + + # Use the USB Serial 4 Android Serial Port Function to open + self.com = serial4a.get_serial_port(dev_port.name, baud, 8, 'N', 1) + + if not self.com.isOpen(): + raise Exception('serial open failed') + threading.Thread.__init__(self) + self.daemon = True + self.alive = True + self.start() + + def run(self): + while self.alive: + # Read_Until is the equivalent function to ReadLines + line = self.com.read_until() + if not line: + continue + line = ''.join(map(chr, line)).rstrip() + self.logger.log(logging.VERBOSE, '-> ' + line) + if self.ports['out']['handle']: + self.ports['out']['handle'].publish(line) + self.com.close() + + def stop(self): + self.alive = False + self.join() + + def in_cb(self, message): + self.logger.log(logging.VERBOSE, '<- ' + message) + self.com.write(bytes(map(ord, message + '\n'))) + + def service_cb(self, message): + pass + + diff --git a/uf/android/serial/tools/usbserialandroid_list_ports.py b/uf/android/serial/tools/usbserialandroid_list_ports.py new file mode 100755 index 0000000..469a75b --- /dev/null +++ b/uf/android/serial/tools/usbserialandroid_list_ports.py @@ -0,0 +1,128 @@ +#!/usr/bin/env python +# +# This is a module that gathers a list of serial ports including details on +# GNU/Linux systems. +# +# Author: Adrian Clark +# +# This file is was modified based off of part of pySerial. +# https://github.com/pyserial/pyserial +# (C) 2011-2015 Chris Liechti +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import absolute_import + +import glob +import os + +class USBSerial4A(): + """Wrapper for USBSerial 4 Android library""" + + def __init__(self, device): + # Store the USBDevice reference in Device + self.device = device + self.name = device.getDeviceName() + + # Haven't translated across the relevant functions yet + # But they aren't necessary for functionality + self.description = 'n/a' + self.hwid = 'n/a' + + # USB specific data pulled from USBDevice + self.vid = device.getVendorId() + self.pid = device.getProductId() + self.serial_number = device.getSerialNumber() + self.manufacturer = device.getManufacturerName() + self.product = device.getProductName() + + # Haven't translated across the relevant functions yet + # But they aren't necessary for functionality + self.location = None + self.interface = None + + self.description = self.usb_description() + self.hwid = self.usb_info() + + # Original Code from pySerial - should double check against + # What is being done with USBSerial 4 Android to ensure consistency + # + # self.usb_device_path = None + # if os.path.exists('/sys/class/tty/{}/device'.format(self.name)): + # self.device_path = os.path.realpath('/sys/class/tty/{}/device'.format(self.name)) + # self.subsystem = os.path.basename(os.path.realpath(os.path.join(self.device_path, 'subsystem'))) + # else: + # self.device_path = None + # self.subsystem = None + # check device type + # if self.subsystem == 'usb-serial': + # self.usb_interface_path = os.path.dirname(self.device_path) + #elif self.subsystem == 'usb': + # self.usb_interface_path = self.device_path + #else: + # self.usb_interface_path = None + # fill-in info for USB devices + #if self.usb_interface_path is not None: + # self.usb_device_path = os.path.dirname(self.usb_interface_path) + + # try: + # num_if = int(self.read_line(self.usb_device_path, 'bNumInterfaces')) + # except ValueError: + # num_if = 1 + + # self.vid = int(self.read_line(self.usb_device_path, 'idVendor'), 16) + # self.pid = int(self.read_line(self.usb_device_path, 'idProduct'), 16) + # self.serial_number = self.read_line(self.usb_device_path, 'serial') + # if num_if > 1: # multi interface devices like FT4232 + # self.location = os.path.basename(self.usb_interface_path) + # else: + # self.location = os.path.basename(self.usb_device_path) + + #self.manufacturer = self.read_line(self.usb_device_path, 'manufacturer') + #self.product = self.read_line(self.usb_device_path, 'product') + #self.interface = self.read_line(self.usb_interface_path, 'interface') + + #if self.subsystem in ('usb', 'usb-serial'): + # self.apply_usb_info() + #~ elif self.subsystem in ('pnp', 'amba'): # PCI based devices, raspi + #elif self.subsystem == 'pnp': # PCI based devices + # self.description = self.name + # self.hwid = self.read_line(self.device_path, 'id') + #elif self.subsystem == 'amba': # raspi + # self.description = self.name + # self.hwid = os.path.basename(self.device_path) + + #if is_link: + #self.hwid += ' LINK={}'.format(device) + def usb_info(self): + """return a string with USB related information about device""" + return 'USB VID:PID={:04X}:{:04X}{}{}'.format( + self.vid or 0, + self.pid or 0, + ' SER={}'.format(self.serial_number) if self.serial_number is not None else '', + ' LOCATION={}'.format(self.location) if self.location is not None else '') + + def usb_description(self): + """return a short string to name the port based on USB info""" + if self.interface is not None: + return '{} - {}'.format(self.product, self.interface) + elif self.product is not None: + return self.product + else: + return self.name + +from usb4a import usb +def comports(include_links=False): + # Get all the USB Devices + usb_device_list = usb.get_usb_device_list() + + # Parse them through USBSerial4A class and return as a list + return [info + for info in [USBSerial4A(d) for d in usb_device_list] + ] # hide non-present internal serial ports + +# - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - +# test +if __name__ == '__main__': + for info in sorted(comports()): + print("{0}: {0.subsystem}".format(info)) diff --git a/uf/android/usbserialandroid_swift/__init__.py b/uf/android/usbserialandroid_swift/__init__.py new file mode 100755 index 0000000..21043cc --- /dev/null +++ b/uf/android/usbserialandroid_swift/__init__.py @@ -0,0 +1,109 @@ +#!/usr/bin/env python3 +# Software License Agreement (BSD License) +# +# Author: Adrian Clark +# Modifed from file written by: Duke Fong + +from ...utils.module_group import ModuleGroup +from ..comm.usbserialandroid_ascii import USBSerialAndroidAscii +from ...comm.protocol_ascii import ProtocolAscii + +# I had to duplicate these files otherwise it pulls in original the +# original Swift files, and then crashes requiring PySerial, which +# we don't actually need +from .swift_body import SwiftBody +from .gripper import Gripper +from .pump import Pump +from .keys import Keys + +# A new class was used so we can use the USBSerialAndroidAscii Module +# Rather than the one based on PySerial +class UsbSerialAndroid_Swift(ModuleGroup): + '''\ + The top module of swift and swift_pro + default kwargs: dev_port = None, baud = 115200, filters = {'hwid': 'USB VID:PID=2341:0042'} + ''' + + def __init__(self, ufc, node, iomap, **kwargs): + + self.sub_nodes = [ + { + # Module here is changed + 'module': USBSerialAndroidAscii, + 'node': 'serial_ascii', + 'args': ['dev_port', 'baud', 'filters'], + 'iomap': { + 'out': 'inner: pkt_ser2ptc', + 'in': 'inner: pkt_ptc2ser' + } + }, + { + 'module': ProtocolAscii, + 'node': 'ptc_ascii', + 'args': ['cmd_pend_size'], + 'iomap': { + 'cmd_async': 'outer: ptc_async', + 'cmd_sync': 'outer: ptc_sync', + 'report': 'outer: ptc_report', + 'service': 'outer: ptc', + + 'packet_in': 'inner: pkt_ser2ptc', + 'packet_out': 'inner: pkt_ptc2ser' + } + }, + { + 'module': SwiftBody, + 'node': 'swift_body', + 'iomap': { + 'pos_in': 'outer: pos_in', + 'pos_out': 'outer: pos_out', + 'buzzer': 'outer: buzzer', + 'service': 'outer: service', + + 'cmd_async': 'outer: ptc_async', + 'cmd_sync': 'outer: ptc_sync', + 'report': 'outer: ptc_report' + } + }, + { + 'module': Gripper, + 'node': 'gripper', + 'iomap': { + 'service': 'outer: gripper', + 'cmd_sync': 'outer: ptc_sync' + } + }, + { + 'module': Pump, + 'node': 'pump', + 'iomap': { + 'service': 'outer: pump', + 'limit_switch': 'outer: limit_switch', + 'cmd_sync': 'outer: ptc_sync', + 'report': 'outer: ptc_report' + } + }, + { + 'module': Keys, + 'node': 'keys', + 'iomap': { + 'service': 'outer: keys', + 'key0': 'outer: key0', + 'key1': 'outer: key1', + 'cmd_sync': 'outer: ptc_sync', + 'report': 'outer: ptc_report' + } + } + ] + + if 'dev_port' not in kwargs: + kwargs['dev_port'] = None + if 'baud' not in kwargs: + kwargs['baud'] = 115200 + if 'filters' not in kwargs: + kwargs['filters'] = {'hwid': 'USB VID:PID=2341:0042'} + if 'cmd_pend_size' not in kwargs: + kwargs['cmd_pend_size'] = 2 + super().__init__(ufc, node, iomap, **kwargs) + + diff --git a/uf/android/usbserialandroid_swift/gripper.py b/uf/android/usbserialandroid_swift/gripper.py new file mode 100755 index 0000000..cfba3a3 --- /dev/null +++ b/uf/android/usbserialandroid_swift/gripper.py @@ -0,0 +1,64 @@ +#!/usr/bin/env python3 +# Software License Agreement (BSD License) +# +# Copyright (c) 2017, UFactory, Inc. +# All rights reserved. +# +# Author: Duke Fong + +from time import sleep +from ...utils.log import * + + +class Gripper(): + def __init__(self, ufc, node, iomap): + + self.ports = { + 'service': {'dir': 'in', 'type': 'service', 'callback': self.service_cb}, + + 'cmd_sync': {'dir': 'out', 'type': 'service'}, + } + + self.logger = logging.getLogger('uf.' + node.replace('/', '.')) + ufc.node_init(node, self.ports, iomap) + + def set_gripper(self, val): + if val == 'on': + self.ports['cmd_sync']['handle'].call('M2232 V1') + else: + self.ports['cmd_sync']['handle'].call('M2232 V0') + + # TODO: modify the default timeout time with a service command + for _ in range(20): + ret = self.ports['cmd_sync']['handle'].call('P2232') + if val == 'on': + if ret == 'ok V2': # grabbed + return 'ok' + else: + if ret == 'ok V0': # stop + return 'ok' + sleep(0.5) + return 'err, timeout for {}, last ret: {}'.format(val, ret) + + def service_cb(self, msg): + msg = msg.split(' ', 2) + + if msg[1] == 'value': + if msg[0] == 'get': + ret = self.ports['cmd_sync']['handle'].call('P2232') + self.logger.debug('get value ret: %s' % ret) + + if ret == 'ok V0': + return 'ok, stoped' + elif ret == 'ok V1': + return 'ok, working' + elif ret == 'ok V2': + return 'ok, grabbed' + else: + return 'err, unkown ret: %s' % ret + + if msg[0] == 'set': + self.logger.debug('set value: %s' % msg[2]) + return self.set_gripper(msg[2]) + + diff --git a/uf/android/usbserialandroid_swift/keys.py b/uf/android/usbserialandroid_swift/keys.py new file mode 100755 index 0000000..3d71bfb --- /dev/null +++ b/uf/android/usbserialandroid_swift/keys.py @@ -0,0 +1,53 @@ +#!/usr/bin/env python3 +# Software License Agreement (BSD License) +# +# Copyright (c) 2017, UFactory, Inc. +# All rights reserved. +# +# Author: Duke Fong + +from ...utils.log import * + + +class Keys(): + def __init__(self, ufc, node, iomap): + + self.ports = { + 'service': {'dir': 'in', 'type': 'service', 'callback': self.service_cb}, + 'key0': {'dir': 'out', 'type': 'topic'}, + 'key1': {'dir': 'out', 'type': 'topic'}, + + 'cmd_sync': {'dir': 'out', 'type': 'service'}, + 'report': {'dir': 'in', 'type': 'topic', 'callback': self.report_cb} + } + + self.logger = logging.getLogger('uf.' + node.replace('/', '.')) + ufc.node_init(node, self.ports, iomap) + + def report_cb(self, msg): + if self.ports['key0']['handle']: + if msg == '4 B0 V1': + self.ports['key0']['handle'].publish('short press') + elif msg == '4 B0 V2': + self.ports['key0']['handle'].publish('long press') + if self.ports['key1']['handle']: + if msg == '4 B1 V1': + self.ports['key1']['handle'].publish('short press') + elif msg == '4 B1 V2': + self.ports['key1']['handle'].publish('long press') + + def service_cb(self, msg): + msg = msg.split(' ', 2) + + if msg[1] == 'key_owner': + if msg[0] == 'get': + return 'err, get not support' + + if msg[0] == 'set': + self.logger.debug('set value: %s' % msg[2]) + if msg[2] == 'default': + return self.ports['cmd_sync']['handle'].call('P2213 V1') + elif msg[2] == 'user': + return self.ports['cmd_sync']['handle'].call('P2213 V0') + return 'err, value not support' + diff --git a/uf/android/usbserialandroid_swift/pump.py b/uf/android/usbserialandroid_swift/pump.py new file mode 100755 index 0000000..cb9ff77 --- /dev/null +++ b/uf/android/usbserialandroid_swift/pump.py @@ -0,0 +1,85 @@ +#!/usr/bin/env python3 +# Software License Agreement (BSD License) +# +# Copyright (c) 2017, UFactory, Inc. +# All rights reserved. +# +# Author: Duke Fong + +from time import sleep +from ...utils.log import * + + +class Pump(): + def __init__(self, ufc, node, iomap): + + self.ports = { + 'service': {'dir': 'in', 'type': 'service', 'callback': self.service_cb}, + 'limit_switch': {'dir': 'out', 'type': 'topic'}, + + 'cmd_sync': {'dir': 'out', 'type': 'service'}, + 'report': {'dir': 'in', 'type': 'topic', 'callback': self.report_cb} + } + + self.logger = logging.getLogger('uf.' + node.replace('/', '.')) + ufc.node_init(node, self.ports, iomap) + + def set_pump(self, val): + if val == 'on': + self.ports['cmd_sync']['handle'].call('M2231 V1') + else: + self.ports['cmd_sync']['handle'].call('M2231 V0') + + # TODO: modify the default timeout time with a service command + for _ in range(20): + ret = self.ports['cmd_sync']['handle'].call('P2231') + if val == 'on': + if ret == 'ok V2': # grabbed + return 'ok' + else: + if ret == 'ok V0': # stop + return 'ok' + sleep(0.5) + return 'err, timeout for {}, last ret: {}'.format(val, ret) + + def report_cb(self, msg): + if not self.ports['limit_switch']['handle']: + return + if msg == '6 N0 V0': + self.ports['limit_switch']['handle'].publish('off') + elif msg == '6 N0 V1': + self.ports['limit_switch']['handle'].publish('on') + + def service_cb(self, msg): + msg = msg.split(' ', 2) + + if msg[1] == 'value': + if msg[0] == 'get': + ret = self.ports['cmd_sync']['handle'].call('P2231') + self.logger.debug('get value ret: %s' % ret) + + if ret == 'ok V0': + return 'ok, stoped' + elif ret == 'ok V1': + return 'ok, working' + elif ret == 'ok V2': + return 'ok, grabbed' + else: + return 'err, unkown ret: %s' % ret + + if msg[0] == 'set': + self.logger.debug('set value: %s' % msg[2]) + return self.set_pump(msg[2]) + + elif msg[1] == 'limit_switch': + if msg[0] == 'get': + ret = self.ports['cmd_sync']['handle'].call('P2233') + self.logger.debug('get limit_switch ret: %s' % ret) + + if ret == 'ok V0': + return 'ok, off' + elif ret == 'ok V1': + return 'ok, on' + else: + return 'err, unkown ret: %s' % ret + diff --git a/uf/android/usbserialandroid_swift/swift_body.py b/uf/android/usbserialandroid_swift/swift_body.py new file mode 100755 index 0000000..a039b99 --- /dev/null +++ b/uf/android/usbserialandroid_swift/swift_body.py @@ -0,0 +1,106 @@ +#!/usr/bin/env python3 +# Software License Agreement (BSD License) +# +# Copyright (c) 2017, UFactory, Inc. +# All rights reserved. +# +# Author: Duke Fong + +from ...utils.log import * + +csys_gstr = { + 'polar': 'G2201 ', + 'cartesian': 'G0 ' +} + +class SwiftBody(): + def __init__(self, ufc, node, iomap): + + self.ports = { + 'pos_in': {'dir': 'in', 'type': 'topic', 'callback': self.pos_in_cb}, + 'pos_out': {'dir': 'out', 'type': 'topic'}, # report current position + + 'buzzer': {'dir': 'in', 'type': 'topic', 'callback': self.buzzer_cb}, + + #'status': {'dir': 'out', 'type': 'topic'}, # report unconnect, etc... + 'service': {'dir': 'in', 'type': 'service', 'callback': self.service_cb}, + + 'cmd_async': {'dir': 'out', 'type': 'topic'}, + 'cmd_sync': {'dir': 'out', 'type': 'service'}, + 'report': {'dir': 'in', 'type': 'topic', 'callback': self.report_cb} + } + + self.logger = logging.getLogger('uf.' + node.replace('/', '.')) + self.mode = 'play' + self.coordinate_system = 'cartesian' + + ufc.node_init(node, self.ports, iomap) + + def buzzer_cb(self, msg): + '''msg format: "F1000 T200", F: frequency, T: time period''' + self.ports['cmd_async']['handle'].publish('M2210 ' + msg) + + # TODO: create a thread to maintain device status and read dev_info + def read_dev_info(self): + info = [] + for c in range(2201, 2206): + ret = '' + while not ret.startswith('ok'): + ret = self.ports['cmd_sync']['handle'].call('P%d' % c) + info.append(ret.split(' ', 1)[1]) + return ' '.join(info) + + def report_cb(self, msg): + if msg == '5 V1': # power on + pass + if msg[:2] == '3 ' and self.ports['pos_out']['handle']: + self.ports['pos_out']['handle'].publish(msg[2:]) + + def pos_in_cb(self, msg): + if self.ports['cmd_async']['handle']: + if not msg.startswith('_T'): + cmd = '_T10 ' # timeout 10s + else: + tmp = msg.split(' ', 1) + cmd = tmp[0] + ' ' + msg = tmp[1] + cmd += csys_gstr[self.coordinate_system] + msg + self.ports['cmd_async']['handle'].publish(cmd) + + def service_cb(self, msg): + msg = msg.split(' ', 2) + + if msg[1] == 'mode': + if msg[0] == 'get': + return 'ok, ' + self.mode + + if msg[1] == 'dev_info': + if msg[0] == 'get': + return 'ok, ' + self.read_dev_info() + + if msg[1] == 'coordinate_system': + if msg[0] == 'get': + return 'ok, ' + self.coordinate_system + if msg[0] == 'set': + self.logger.debug('coordinate_system: %s -> %s' % (self.coordinate_system, msg[2])) + self.coordinate_system = msg[2] + return 'ok' + + if msg[1] == 'report_pos': + if msg[0] == 'set': + if msg[2] == 'off': + return self.ports['cmd_sync']['handle'].call('M2120 V0') + elif msg[2].startswith('on '): + # format e.g.: set report_pos on 0.2 + return self.ports['cmd_sync']['handle'].call('M2120 V' + msg[2][3:]) + + if msg[1] == 'cmd_sync': + if msg[0] == 'set': + return self.ports['cmd_sync']['handle'].call(msg[2]) + + if msg[1] == 'cmd_async': + if msg[0] == 'set': + self.ports['cmd_async']['handle'].publish(msg[2]) + return 'ok' + + diff --git a/uf/android/utils/usbserialandroid_select_serial_port.py b/uf/android/utils/usbserialandroid_select_serial_port.py new file mode 100755 index 0000000..efaa8bf --- /dev/null +++ b/uf/android/utils/usbserialandroid_select_serial_port.py @@ -0,0 +1,67 @@ +#!/usr/bin/env python3 +# Software License Agreement (BSD License) +# +# Author: Adrian Clark +# Modifed from file written by: Duke Fong + +# Use the usbserialandroid_list_ports function which gets +# USB attached serial ports +from ..serial.tools import usbserialandroid_list_ports + +def _dump_port(logger, d): + logger.info('{}:'.format(d.device)) + logger.info(' hwid : "{}"'.format(d.hwid)) + logger.info(' manufacturer: "{}"'.format(d.manufacturer)) + logger.info(' product : "{}"'.format(d.product)) + logger.info(' description : "{}"'.format(d.description)) + +def _dump_ports(logger): + # Dump out any USB Serial Ports + print("usb serial ports") + for d in usbserialandroid_list_ports.comports(): + _dump_port(logger, d) + +def select_port(logger = None, dev_port = None, filters = None, must_unique = False): + + if filters != None and dev_port == None: + not_unique = False + # Dump out any USB Serial Ports + for d in usbserialandroid_list_ports.comports(): + is_match = True + for k, v in filters.items(): + if not hasattr(d, k): + continue + a = getattr(d, k) + if not a: + a = '' + if a.find(v) == -1: + is_match = False + if is_match: + if dev_port == None: + dev_port = d + if logger: + # Dev_Port is an object, use .name to get the right info + logger.info('choose device: ' + dev_port.name) + _dump_port(logger, d) + else: + if logger: + logger.warning('find more than one port') + not_unique = True + if not_unique: + if logger: + logger.info('current filter: {}, all ports:'.format(filters)) + _dump_ports(logger) + if must_unique: + raise Exception('port is not unique') + + if not dev_port: + if logger: + if filters: + logger.error('port not found, current filter: {}, all ports:'.format(filters)) + else: + logger.error('please specify dev_port or filters, all ports:') + _dump_ports(logger) + return None + + return dev_port + diff --git a/uf/android/wrapper/swift_api.py b/uf/android/wrapper/swift_api.py new file mode 100755 index 0000000..563b699 --- /dev/null +++ b/uf/android/wrapper/swift_api.py @@ -0,0 +1,679 @@ +#!/usr/bin/env python3 +# Software License Agreement (BSD License) +# +# Author: Adrian Clark +# Modifed from file written by: Duke Fong + +from time import sleep +from ...ufc import ufc_init +# Use the USBSerialAndroid_Swift class +from ..usbserialandroid_swift import UsbSerialAndroid_Swift +from ...utils.log import * + + +# SERVO NUMBER INDEX +SERVO_BOTTOM = 0 +SERVO_LEFT = 1 +SERVO_RIGHT = 2 +SERVO_HAND = 3 + +## EEPROM DATA TYPE INDEX +EEPROM_DATA_TYPE_BYTE = 1 +EEPROM_DATA_TYPE_INTEGER = 2 +EEPROM_DATA_TYPE_FLOAT = 4 + + +class Usbserialandroid_SwiftAPI(): + ''' + The API wrapper of swift and swift_pro + default kwargs: dev_port = None, baud = 115200, filters = {'hwid': 'USB VID:PID=2341:0042'} + ''' + def __init__(self, **kwargs): + ''' + ''' + + self._ufc = ufc_init() + + # init swift node: + + swift_iomap = { + 'pos_in': 'pos_to_dev', + 'pos_out': 'pos_from_dev', + 'buzzer': 'buzzer', + 'service': 'service', + 'gripper': 'gripper', + 'pump': 'pump', + 'limit_switch': 'limit_switch', + 'keys': 'keys', + 'key0': 'key0', + 'key1': 'key1', + 'ptc_sync': 'ptc_sync', + 'ptc_report': 'ptc_report', + 'ptc': 'ptc' + } + + self._nodes = {} + # Use the USBSerialAndroid_Swift class + self._nodes['swift'] = UsbSerialAndroid_Swift(self._ufc, 'swift', swift_iomap, **kwargs) + + + # init swift_api node: + + self._ports = { + 'pos_to_dev': {'dir': 'out', 'type': 'topic'}, + 'pos_from_dev': {'dir': 'in', 'type': 'topic', 'callback': self._pos_from_dev_cb}, + 'buzzer': {'dir': 'out', 'type': 'topic'}, + 'service': {'dir': 'out', 'type': 'service'}, + 'gripper': {'dir': 'out', 'type': 'service'}, + 'pump': {'dir': 'out', 'type': 'service'}, + 'limit_switch': {'dir': 'in', 'type': 'topic', 'callback': self._limit_switch_cb}, + 'keys': {'dir': 'out', 'type': 'service'}, + 'key0': {'dir': 'in', 'type': 'topic', 'callback': self._key0_cb}, + 'key1': {'dir': 'in', 'type': 'topic', 'callback': self._key1_cb}, + 'ptc': {'dir': 'out', 'type': 'service'} + } + + self._iomap = { + 'pos_to_dev': 'pos_to_dev', + 'pos_from_dev': 'pos_from_dev', + 'buzzer': 'buzzer', + 'service': 'service', + 'gripper': 'gripper', + 'pump': 'pump', + 'limit_switch': 'limit_switch', + 'keys': 'keys', + 'key0': 'key0', + 'key1': 'key1', + 'ptc': 'ptc' + } + + self.pos_from_dev_cb = None + self.limit_switch_cb = None + self.key0_cb = None + self.key1_cb = None + self._dev_info = None + + self._node = 'swift_api' + self._logger = logging.getLogger(self._node) + self._ufc.node_init(self._node, self._ports, self._iomap) + + + def _pos_from_dev_cb(self, msg): + if self.pos_from_dev_cb != None: + values = list(map(lambda i: float(i[1:]), msg.split(' '))) + self.pos_from_dev_cb(values) + + def _limit_switch_cb(self, msg): + if self.limit_switch_cb != None: + self.limit_switch_cb(msg == 'on') + + def _key0_cb(self, msg): + if self.key0_cb != None: + self.key0_cb(msg) + + def _key1_cb(self, msg): + if self.key1_cb != None: + self.key1_cb(msg) + + def reset(self): + ''' + Reset include below action: + - Attach all servos + - Move to default position (150, 0, 150) with speed 200mm/min + - Turn off pump/gripper + - Set wrist servo to angle 90 + + Returns: + None + ''' + self.set_servo_attach() + sleep(0.1) + self.set_position(150, 0, 150, speed = 200, wait = True) + self.set_pump(False) + self.set_gripper(False) + self.set_wrist(90) + + def send_cmd_sync(self, msg): + ''' + This function will block until receive the response message. + + Args: + msg: string, serial command + + Returns: + string response + ''' + return self._ports['service']['handle'].call('set cmd_sync ' + msg) + + def send_cmd_async(self, msg): + ''' + This function will send out the message and return immediately. + + Args: + msg: string, serial command + + Returns: + None + ''' + self._ports['service']['handle'].call('set cmd_async ' + msg) + + def get_device_info(self): + ''' + Get the device info. + + Returns: + string list: [device model, hardware version, firmware version, api version, device UID] + ''' + ret = self._ports['service']['handle'].call('get dev_info') + if ret.startswith('ok'): + return list(map(lambda i: i[1:], ret.split(' ')[1:])) + self._logger.error('get_dev_info ret: %s' % ret) + return None + + def get_is_moving(self): + ''' + Get the arm current moving status. + + Returns: + boolean True or False + ''' + ret = self._ports['service']['handle'].call('set cmd_sync M2200') + if ret == 'ok V0': + return False + if ret == 'ok V1': + return True + self._logger.error('get_is_moving ret: %s' % ret) + return None + + def flush_cmd(self): + ''' + Wait until all async command return + + Returns: + boolean True or False + ''' + ret = self._ports['ptc']['handle'].call('set flush') + if ret == 'ok': + return True + return False + + def set_position(self, x = None, y = None, z = None, + speed = None, relative = False, + wait = False, e = None, timeout = 10, ): + ''' + Move arm to the position (x,y,z) unit is mm, speed unit is mm/sec + + Args: + x + y + z + speed + relative + wait: if True, will block the thread, until get response or timeout + + e amount of steps in the extruder + Returns: + True if successed + ''' + if wait: + cmd = 'set cmd_sync' + else: + cmd = 'set cmd_async' + + cmd += ' _T%d' % timeout + + if relative: + cmd += ' G2204' + else: + cmd += ' G0' + + if x != None: + cmd += ' X{}'.format(x) + if y != None: + cmd += ' Y{}'.format(y) + if z != None: + cmd += ' Z{}'.format(z) + if e != None: + cmd += ' E{}'.format(e) + if speed != None: + cmd += ' F{}'.format(speed) + + ret = self._ports['service']['handle'].call(cmd) + return ret.startswith('ok') # device return 'ok' even out of range + + def get_position(self): + ''' + Get current arm position (x,y,z) + + Returns: + float array of the format [x, y, z] of the robots current location + ''' + ret = self._ports['service']['handle'].call('set cmd_sync P2220') + + if ret.startswith('ok '): + values = list(map(lambda i: float(i[1:]), ret.split(' ')[1:])) + return values + self._logger.error('get_position ret: %s' % ret) + return None + + def set_polar(self, s = None, r = None, h = None, + speed = None, relative = False, + wait = False, timeout = 10): + ''' + Polar coordinate, rotation, stretch, height. + + Args: + stretch(mm) + rotation(degree) + height(mm) + speed: speed(mm/min) + relative + wait: if True, will block the thread, until get response or timeout + + Returns: + True if successed + ''' + if wait: + cmd = 'set cmd_sync' + else: + cmd = 'set cmd_async' + + cmd += ' _T%d' % timeout + + if relative: + cmd += ' G2205' + else: + cmd += ' G2201' + + if s != None: + cmd += ' S{}'.format(s) + if r != None: + cmd += ' R{}'.format(r) + if h != None: + cmd += ' H{}'.format(h) + if speed != None: + cmd += ' F{}'.format(speed) + + ret = self._ports['service']['handle'].call(cmd) + return ret.startswith('ok') + + def get_polar(self): + ''' + Get polar coordinate + + Returns: + float array of the format [rotation, stretch, height] + ''' + ret = self._ports['service']['handle'].call('set cmd_sync P2221') + + if ret.startswith('ok '): + values = list(map(lambda i: float(i[1:]), ret.split(' ')[1:])) + return values + self._logger.error('get_polar ret: %s' % ret) + return None + + def set_servo_angle(self, servo_id, angle, wait = False, timeout = 10): + ''' + Set servo angle, 0 - 180 degrees, this Function will include the manual servo offset. + + Args: + servo_id: SERVO_BOTTOM, SERVO_LEFT, SERVO_RIGHT, SERVO_HAND + angle: 0 - 180 degrees + wait: if True, will block the thread, until get response or timeout + + Returns: + succeed True or failed False + ''' + cmd = 'set cmd_sync' if wait else 'set cmd_async' + cmd += ' _T%d' % timeout + cmd += ' G2202 N{} V{}'.format(servo_id, angle) + ret = self._ports['service']['handle'].call(cmd) + return ret.startswith('ok') + + def set_wrist(self, angle, wait = False, timeout = 10): + ''' + Set swift hand wrist angle. include servo offset. + + Args: + angle: 0 - 180 degrees + wait: if True, will block the thread, until get response or timeout + + Returns: + succeed True or failed False + ''' + return self.set_servo_angle(SERVO_HAND, angle, wait = wait, timeout = timeout) + + def get_servo_angle(self, servo_id = None): + ''' + Get servo angle + + Args: + servo_id: return an array if servo_id not provided, + else specify: SERVO_BOTTOM, SERVO_LEFT, SERVO_RIGHT, SERVO_HAND + + Returns: + array of float or single float + ''' + values = [None] * 3 + if servo_id != SERVO_HAND: + ret = self._ports['service']['handle'].call('set cmd_sync P2200') + if ret.startswith('ok '): + values = list(map(lambda i: float(i[1:]), ret.split(' ')[1:])) + else: + self._logger.error('get_servo_angle N0~2 ret: %s' % ret) + if servo_id == SERVO_HAND or servo_id == None: + ret = self._ports['service']['handle'].call('set cmd_sync P2206 N3') + if ret.startswith('ok '): + values.append(float(ret[4:])) + else: + self._logger.error('get_servo_angle N3 ret: %s' % ret) + + if servo_id == None: + return values + else: + return values[servo_id] + + def set_servo_attach(self, servo_id = None, wait = False): + ''' + Set servo status attach, servo attach will lock the servo, you can't move swift with your hands. + + Args: + servo_id: if None, will attach all servos, else specify: SERVO_BOTTOM, SERVO_LEFT, SERVO_RIGHT, SERVO_HAND + wait: if True, will block the thread, until get response or timeout + + Returns: + succeed True or Failed False + ''' + cmd = 'set cmd_sync' if wait else 'set cmd_async' + if servo_id == None: + cmd += ' M17' + else: + cmd += ' M2201 N{}'.format(servo_id) + ret = self._ports['service']['handle'].call(cmd) + return ret.startswith('ok') + + def set_servo_detach(self, servo_id = None, wait = False): + ''' + Set Servo status detach, Servo Detach will unlock the servo, You can move swift with your hands. + But move function won't be effect until you attach. + + Args: + servo_id: if None, will detach all servos, else specify: SERVO_BOTTOM, SERVO_LEFT, SERVO_RIGHT, SERVO_HAND + wait: if True, will block the thread, until get response or timeout + + Returns: + succeed True or Failed False + ''' + cmd = 'set cmd_sync' if wait else 'set cmd_async' + if servo_id == None: + cmd += ' M2019' + else: + cmd += ' M2202 N{}'.format(servo_id) + ret = self._ports['service']['handle'].call(cmd) + return ret.startswith('ok') + + def get_servo_attach(self, servo_id = None): + ''' + Check servo attach status + + Args: + servo_id: SERVO_BOTTOM, SERVO_LEFT, SERVO_RIGHT, SERVO_HAND + wait: if True, will block the thread, until get response or timeout + + Returns: + succeed True or Failed False + ''' + cmd = 'set cmd_sync M2203 N{:d}'.format(servo_id) + ret = self._ports['service']['handle'].call(cmd) + if ret == 'ok V0': + return False + if ret == 'ok V1': + return True + self._logger.error('get_servo_attach ret: %s' % ret) + return None + + def set_report_position(self, interval): + ''' + Report currentpPosition in (interval) seconds. + + Args: + interval: seconds, if 0 disable report + + Returns: + None + ''' + cmd = 'set report_pos on {}'.format(round(interval, 2)) + ret = self._ports['service']['handle'].call(cmd) + if ret.startswith('ok'): + return + self._logger.error('set_report_position ret: %s' % ret) + + def register_report_position_callback(self, callback = None): + ''' + Set function to receiving current position [x, y, z, r], r is wrist angle. + + Args: + callback: set the callback function, undo by setting to None + + Returns: + None + ''' + self.pos_from_dev_cb = callback + + def register_limit_switch_callback(self, callback = None): + ''' + Set function to receiving limit switch state change event. + + Args: + callback: set the callback function, undo by setting to None + + Returns: + None + + Notes: + callback with one argument: + True: switch state change to close + False: switch state change to open + ''' + self.limit_switch_cb = callback + + def set_report_keys(self, is_on = True): + ''' + Change default function of base buttons + + Args: + is_on: + True: enable report + False: disable report, for offline teach by default + + Returns: + True if success + ''' + cmd = 'set cmd_sync M2213 V{}'.format('0' if is_on else '1') + ret = self._ports['service']['handle'].call(cmd) + if ret == 'ok': + return True + self._logger.error('set_report_keys ret: %s' % ret) + return False + + def register_key0_callback(self, callback = None): + ''' + Set function to receiving key0 state change event. + + Args: + callback: set the callback function, undo by setting to None + + Returns: + None + + Notes: + callback with one string argument: + 'short press' + 'long press' + ''' + self.key0_cb = callback + + def register_key1_callback(self, callback = None): + ''' + Set function to receiving key1 state change event. + + Args: + callback: set the callback function, undo by setting to None + + Returns: + None + + Notes: + callback with one string argument: + 'short press' + 'long press' + ''' + self.key1_cb = callback + + def set_buzzer(self, freq = 1000, time = 200): + ''' + Control buzzer. + + Args: + freq: frequency + time: time period + + Returns: + None + ''' + self._ports['buzzer']['handle'].publish('F{} T{}'.format(freq, time)) + + def set_pump(self, on, timeout = None): + ''' + Control pump on or off + + Args: + on: True on, False off + timeout: unsupported currently + + Returns: + succeed True or failed False + ''' + cmd = 'set value on' if on else 'set value off' + ret = self._ports['pump']['handle'].call(cmd) + if ret.startswith('ok'): + return True + self._logger.warning('set_pump ret: %s' % ret) + return False + + def set_gripper(self, catch, timeout = None): + ''' + Turn on/off gripper + + Args: + catch: True on / False off + wait: if True, will block the thread, until get response or timeout + + Returns: + succeed True or failed False + ''' + cmd = 'set value on' if catch else 'set value off' + ret = self._ports['gripper']['handle'].call(cmd) + if ret.startswith('ok'): + return True + self._logger.warning('set_gripper ret: %s' % ret) + return False + + def get_limit_switch(self): + ''' + Get the limit switch status. + + Returns: + boolean True or False + ''' + ret = self._ports['service']['handle'].call('set cmd_sync P2233') + if ret == 'ok V0': + return False + if ret == 'ok V1': + return True + self._logger.error('get_limit_switch ret: %s' % ret) + return None + + def get_analog(self, pin): + ''' + Get analog value from specific pin + + Args: + pin: pin number + + Returns: + integral value + ''' + ret = self._ports['service']['handle'].call('set cmd_sync P2241 N{}'.format(pin)) + if ret.startswith('ok '): + return int(ret[4:]) + self._logger.error('get_analog ret: %s' % ret) + return None + + def get_digital(self, pin): + ''' + Get digital value from specific pin. + + Args: + pin: pin number + + Returns: + high True or low False + ''' + ret = self._ports['service']['handle'].call('set cmd_sync P2240 N{}'.format(pin)) + if ret == 'ok V1': + return True + elif ret == 'ok V0': + return False + self._logger.error('get_digital ret: %s' % ret) + return None + + def set_rom_data(self, address, data, data_type = EEPROM_DATA_TYPE_BYTE): + ''' + Set data to eeprom + + Args: + address: 0 - 64K byte + data_type: EEPROM_DATA_TYPE_FLOAT, EEPROM_DATA_TYPE_INTEGER, EEPROM_DATA_TYPE_BYTE + + Returns: + True on success + ''' + ret = self._ports['service']['handle'].call('set cmd_sync M2212 N1 A{} T{} V{}'.format(address, data_type, data)) + if ret.startswith('ok'): + return True + self._logger.error('get_rom_data ret: %s' % ret) + return None + + def get_rom_data(self, address, data_type = EEPROM_DATA_TYPE_BYTE): + ''' + Get data from eeprom + + Args: + address: 0 - 64K byte + data_type: EEPROM_DATA_TYPE_FLOAT, EEPROM_DATA_TYPE_INTEGER, EEPROM_DATA_TYPE_BYTE + + Returns: + int or float value + + Notes: + EEPROM default data format, each item is one offline record data (no header at beginning): + [p0, p1, p2, p3, p4, p5 ... p_end] + + each record data is 10 bytes, and each item inside is 2 bytes: + [a0, a1, a2, a3, accessories_state] + + a0~3: unsigned fixed point of servos' angle (multiply by 100) + + accessories_state: + bit0: pump on/off + bit4: griper on/off + + p_end indicate the end of records, filled by 0xffff + ''' + ret = self._ports['service']['handle'].call('set cmd_sync M2211 N1 A{} T{}'.format(address, data_type)) + if ret.startswith('ok '): + return int(ret[4:]) if data_type != EEPROM_DATA_TYPE_FLOAT else float(ret[4:]) + self._logger.error('get_rom_data ret: %s' % ret) + return None +