Created
November 23, 2017 06:20
-
-
Save d3v-null/220c49c88146932b690f218edc4af243 to your computer and use it in GitHub Desktop.
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
#!/usr/bin/env python | |
from __future__ import print_function | |
import sys | |
import time | |
import re | |
# import pyuarm | |
import yaml | |
from pprint import pformat | |
from uf.utils.log import logger_init, logging | |
from uf.wrapper.swift_api import SwiftAPI | |
from uf.wrapper.uarm_api import UarmAPI | |
SPEED_DEFAULT = 200 | |
Z_DEFAULT = 150 | |
SIMPLE_RESPONSE_REGEX = r"ok V(?P<response>\S+)" | |
TEMP_RESPONSE_REGEX = r"ok T:(?P<response>\S+)" | |
def main(): | |
config = {} | |
with open("config.yaml", 'r') as ymlfile: | |
config = yaml.load(ymlfile) | |
debug = config.get('debug') | |
if debug: | |
logger_init(logging.DEBUG) | |
arm = None | |
try: | |
arm = get_api(config) | |
except Exception as exc: | |
if debug: | |
import pudb; pudb.set_trace() | |
raise exc | |
print("Error connecting to the arm", file=sys.stderr) | |
exit(1) | |
time.sleep(2) | |
device_info = arm.get_device_info() | |
logging.info('device info:\n%s' % pformat(device_info)) | |
fw_version = tuple(int(number) for number in device_info[2].split('.')) | |
logging.info('firmware version:\n%s' % (fw_version, )) | |
mode_response = arm.send_cmd_sync("M2400 S1") | |
logging.info('mode response:\n%s' % mode_response) | |
if not high5(arm, config): | |
print("Error excuting one of the operations.", file=sys.stderr) | |
exit(2) | |
arm.set_servo_detach(wait=True) | |
def get_api(config): | |
device = config.get('device', 'swift') | |
if device == 'swift': | |
logging.info('setup swift ...') | |
api = SwiftAPI() | |
elif device == 'metal': | |
logging.info('setup metal ...') | |
api = UarmAPI() | |
return api | |
def high5(arm, config): | |
""" | |
Perform high-five actions on the uarm using the API instance `arm` | |
""" | |
wait = config.get('wait', False) | |
z = config.get('z', Z_DEFAULT) | |
speed = config.get('speed', SPEED_DEFAULT) | |
if not home_arm(arm, config): | |
# fails fast instead of failing at the end | |
return | |
movements = [ | |
# raise arm | |
{'x':150, 'y':0, 'z':z, 'speed':speed, 'wait':wait}, | |
] | |
direction = config.get('direction', 'cw') | |
if direction == 'cw': | |
movements.extend([ | |
# backswing | |
{'x':150, 'y':70, 'z':z, 'speed':speed, 'wait':wait}, | |
# in | |
{'x':150, 'y':-70, 'z':z, 'speed':speed, 'wait':wait}, | |
]) | |
else: | |
movements.extend([ | |
# backswing | |
{'x':150, 'y':-70, 'z':z, 'speed':speed, 'wait':wait}, | |
# in | |
{'x':150, 'y':70, 'z':z, 'speed':speed, 'wait':wait}, | |
]) | |
for movement in movements: | |
if not perform_movement(arm, **movement): | |
return | |
time.sleep(0.1) | |
while arm.get_is_moving(): | |
time.sleep(0.1) | |
time.sleep(0.3) | |
if not home_arm(arm, config): | |
return | |
return True | |
def home_arm(arm, config={}): | |
wait = config.get('wait', False) | |
speed = config.get('speed', SPEED_DEFAULT) | |
z = config.get('z', Z_DEFAULT) | |
if not perform_movement(arm, x=150, y=0, z=z, speed=speed, wait=wait): | |
return | |
return True | |
def perform_movement(arm, **movement): | |
logging.info('set ' + ' '.join([ | |
"%s:%s" % (key.upper(), value) for key, value in movement.items() | |
])) | |
if not arm.set_position(**movement): | |
return | |
time.sleep(0.1) | |
while arm.get_is_moving(): | |
time.sleep(0.1) | |
return True | |
def send_cmd_sync_ok(arm, command, response_regex=None): | |
""" | |
Send a command and wait for it to complete. Optionally parse the response. | |
""" | |
logging.debug("sending command \"%s\"" % command) | |
response = arm.send_cmd_sync(command) | |
logging.debug("command response \"%s\"" % response) | |
if not response.startswith("ok"): | |
raise RuntimeError("command \"%s\" failed: %s" % (command, response)) | |
if response_regex: | |
response_match = re.search(response_regex, response) | |
if response_match and 'response' in response_match.groupdict(): | |
response = response_match.groupdict()['response'] | |
else: | |
raise ValueError( | |
"response \"%s\" for command \"%s\" did not match regex \"%s\"" % | |
(response, command, response_regex) | |
) | |
return response | |
if __name__ == "__main__": | |
main() |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment