diff --git a/scripts/control_marker.py b/scripts/control_marker.py index 25dba05..1aede84 100755 --- a/scripts/control_marker.py +++ b/scripts/control_marker.py @@ -12,7 +12,7 @@ from sensor_msgs.msg import JointState from std_msgs.msg import Header from visualization_msgs.msg import Marker -from pythonAPI.mycobot import MyCobot +from pymycobot.mycobot import MyCobot server = None diff --git a/scripts/control_slider.py b/scripts/control_slider.py index 8ea0b31..d9b4bfa 100755 --- a/scripts/control_slider.py +++ b/scripts/control_slider.py @@ -5,7 +5,7 @@ import time, subprocess import rospy from sensor_msgs.msg import JointState -from pythonAPI.mycobot import MyCobot +from pymycobot.mycobot import MyCobot def callback(data): diff --git a/scripts/display.py b/scripts/display.py index 2c750de..634f4d4 100755 --- a/scripts/display.py +++ b/scripts/display.py @@ -7,7 +7,7 @@ from sensor_msgs.msg import JointState from std_msgs.msg import Header from visualization_msgs.msg import Marker -from pythonAPI.mycobot import MyCobot +from pymycobot.mycobot import MyCobot def talker(): diff --git a/scripts/pythonAPI/.gitignore b/scripts/pythonAPI/.gitignore deleted file mode 100644 index a81c8ee..0000000 --- a/scripts/pythonAPI/.gitignore +++ /dev/null @@ -1,138 +0,0 @@ -# Byte-compiled / optimized / DLL files -__pycache__/ -*.py[cod] -*$py.class - -# C extensions -*.so - -# Distribution / packaging -.Python -build/ -develop-eggs/ -dist/ -downloads/ -eggs/ -.eggs/ -lib/ -lib64/ -parts/ -sdist/ -var/ -wheels/ -share/python-wheels/ -*.egg-info/ -.installed.cfg -*.egg -MANIFEST - -# PyInstaller -# Usually these files are written by a python script from a template -# before PyInstaller builds the exe, so as to inject date/other infos into it. -*.manifest -*.spec - -# Installer logs -pip-log.txt -pip-delete-this-directory.txt - -# Unit test / coverage reports -htmlcov/ -.tox/ -.nox/ -.coverage -.coverage.* -.cache -nosetests.xml -coverage.xml -*.cover -*.py,cover -.hypothesis/ -.pytest_cache/ -cover/ - -# Translations -*.mo -*.pot - -# Django stuff: -*.log -local_settings.py -db.sqlite3 -db.sqlite3-journal - -# Flask stuff: -instance/ -.webassets-cache - -# Scrapy stuff: -.scrapy - -# Sphinx documentation -docs/_build/ - -# PyBuilder -.pybuilder/ -target/ - -# Jupyter Notebook -.ipynb_checkpoints - -# IPython -profile_default/ -ipython_config.py - -# pyenv -# For a library or package, you might want to ignore these files since the code is -# intended to run in multiple environments; otherwise, check them in: -# .python-version - -# pipenv -# According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control. -# However, in case of collaboration, if having platform-specific dependencies or dependencies -# having no cross-platform support, pipenv may install dependencies that don't work, or not -# install all needed dependencies. -#Pipfile.lock - -# PEP 582; used by e.g. github.com/David-OConnor/pyflow -__pypackages__/ - -# Celery stuff -celerybeat-schedule -celerybeat.pid - -# SageMath parsed files -*.sage.py - -# Environments -.env -.venv -env/ -venv/ -ENV/ -env.bak/ -venv.bak/ - -# Spyder project settings -.spyderproject -.spyproject - -# Rope project settings -.ropeproject - -# mkdocs documentation -/site - -# mypy -.mypy_cache/ -.dmypy.json -dmypy.json - -# Pyre type checker -.pyre/ - -# pytype static type analyzer -.pytype/ - -# Cython debug symbols -cython_debug/ diff --git a/scripts/pythonAPI/README.md b/scripts/pythonAPI/README.md deleted file mode 100644 index d0e31c0..0000000 --- a/scripts/pythonAPI/README.md +++ /dev/null @@ -1,366 +0,0 @@ -# ros-python-api - -**This is python API for mycobot.** - -We support Python2, Python3.5 or later. If you want to use the api, make sure `pyserial` is installed. - -```bash -pip2 install pyserial -# or -pip3 install pyserial -``` - -**Class**: -- [MyCobot](##MyCobot) -- [Angle](##Angle) -- [Coord](##Coord) - - -## MyCobot - -### MyCobot.power_on() - -- **Description** - - Robot arm power up. - -- **Parameters** - - None - -- **Returns** - - None - -### MyCobot.power_off() - -- **Description** - - Robot arm power down. - -- **Parameters** - - None - -- **Returns** - - None - -### MyCobot.set_free_mode() - -- **Description** - - Robot arm into free moving mode. - -- **Parameters** - - None - -- **Returns** - - None - -### MyCobot.get_angles() - -- **Description** - - Get the degree of all joints. - -- **Parameters** - - None - -- **Returns** - - A float list of degree. - -### MyCobot.get_angles_by_radian() - -- **Description** - - Get the radians of all joints. - -- **Parameters** - - None - -- **Returns** - - A float list of radian. - -### MyCobot.send_angle() - -- **Description** - - Send one degree of joint to robot arm. - -- **Parameters** - - id: Joint id(common.Angle) - - degree: degree value(float) - - speed: (int) - -- **Returns** - - None - -- **Example** - - ```python - from pythonAPI.mycobot import MyCobot - from pythonAPI.common import Angle - - - mycobot = MyCobot() - mycobot.send_angle(Angle.J2.value, 10, 50) - ``` - -### MyCobot.send_angles() - -- **Description** - - Send the degrees of all joints to robot arm. - -- **Parameters** - - degrees: a list of degree value(List[float]) - - speed: (int) - -- **Returns** - - None - -- **Example** - - ```python - from pythonAPI.mycobot import MyCobot - from pythonAPI.common import Angle - - - mycobot = MyCobot() - mycobot.send_angles([0,0,0,0,0,0], 80) - ``` - -### MyCobot.send_angles_by_radian() - -- **Description** - - Send the radians of all joint to robot arm. - -- **Parameters** - - degrees: a list of radian value(List[float]) - - speed: (int) - -- **Returns** - - None - -- **Example** - - ```python - from pythonAPI.mycobot import MyCobot - from pythonAPI.common import Angle - - - mycobot = MyCobot() - mycobot.send_angles_by_radian([1,1,1,1,1,1], 70) - ``` - -### MyCobot.get_coords() - -- **Description** - - Get the Coords from robot arm. - -- **Parameters** - - None - -- **Returns** - - A float list of coord. - -### MyCobot.send_coord() - -- **Description** - - Send one coord to robot arm. - -- **Parameters** - - id: coord name(common.Coord) - - coord: coord value(float) - - speed: (int) - -- **Returns** - - None - -- **Example** - - ```python - from pythonAPI.mycobot import MyCobot - from pythonAPI.common import Coord - - - mycobot = MyCobot() - mycobot.send_coord(Coord.X.value, -40, 70) - ``` - -### MyCobot.send_coords() - -- **Description** - - Send all coords to robot arm. - -- **Parameters** - - coords: a list of coords value(List[float]) - - speed: (int) - -- **Returns** - - None - -- **Example** - - ```python - from pythonAPI.mycobot import MyCobot - from pythonAPI.common import Coord - - - mycobot = MyCobot() - mycobot.send_coords([160, 160, 160, 0, 0, 0], 70, 0) - ``` -### MyCobot.set_color() - -- **Description** - - Set the color of the light on the top of the robot arm. - - -- **Parameters** - - rgb: (string) like: "FF0000" - -- **Returns** - - None -### MyCobot.is_moving() - -- **Description** - - Judge whether the manipulator is moving or not. - -- **Parameters** - - None - -- **Returns** - - bool: `True` - moving, `False` - not moving. - -### MyCobot.pause() - -- **Description** - - Pause movement. - -- **Parameters** - - None - -- **Returns** - - None - -### MyCobot.resume() - -- **Description** - - Recovery movement. - -- **Parameters** - - None - -- **Returns** - - None - -### MyCobot.stop() - -- **Description** - - Stop moving. - -- **Parameters** - - None - -- **Returns** - - None - -### MyCobot.is_pause() - -- **Description** - - Judge whether the manipulator pauses or not. - -- **Parameters** - - None - -- **Returns** - - bool: `True` - pause, `False` - not pause. - -### MyCobot.get_speed() - -- **Description** - - Get speed. - -- **Parameters** - - None - -- **Returns** - - speed: (int) - -### MyCobot.set_speed() - -- **Description** - - Set speed. - -- **Parameters** - - speed: (int) - -- **Returns** - - None - -## Angle - -**Description** - -Instance class of joint. It's recommended to use this class to select joint. - - -## Coord - -**Description** - -Instance class of coord. It's recommended to use this class to select coord. \ No newline at end of file diff --git a/scripts/pythonAPI/__init__.py b/scripts/pythonAPI/__init__.py deleted file mode 100755 index e69de29..0000000 diff --git a/scripts/pythonAPI/common.py b/scripts/pythonAPI/common.py deleted file mode 100755 index f9e7329..0000000 --- a/scripts/pythonAPI/common.py +++ /dev/null @@ -1,18 +0,0 @@ -import enum - -class Angle(enum.Enum): - J1 = '01' - J2 = '02' - J3 = '03' - J4 = '04' - J5 = '05' - J6 = '06' - - -class Coord(enum.Enum): - X = '01' - Y = '02' - Z = '03' - Rx = '04' - Ry = '05' - Rz = '06' diff --git a/scripts/pythonAPI/mycobot.py b/scripts/pythonAPI/mycobot.py deleted file mode 100755 index 3897f90..0000000 --- a/scripts/pythonAPI/mycobot.py +++ /dev/null @@ -1,397 +0,0 @@ -import sys -sys.path.append('.') -import time, serial, struct - -class MyCobot(): - '''MyCobot Python API - - Possessed function: - power_on() : - power_off() : - get_angles() : - get_angles_of_radian() : - send_angle() : - send_angles() : - send_angles_by_radian() : - set_color() : - get_coords() : - send_coords() : - jog_angle() : - jog_coord() : - jog_stop() : - is_moving() : - pause() : - resume() : - stop() : - is_paused() : - get_speed() : - set_speed() : - ''' - - def __init__(self, port): - # _prot = subprocess.check_output(['echo -n /dev/ttyUSB*'], - # shell=True) - _prot = port - _boudrate = '115200' - _timeout = 0.1 - - for _ in range(5): - try: - self.serial_port = serial.Serial(_prot, _boudrate, timeout=_timeout) - break - except Exception as e: - print(e) - time.sleep(5) - continue - else: - print('Connect prot failed, eixt.') - exit(0) - - def power_on(self): - self._write('fefe0210fa') - - def power_off(self): - self._write('fefe0211fa') - - def set_free_mode(self): - self._write('fefe0213fa') - - def get_angles(self): - '''Get all angle return a list - - Return: - data_list (list): - - ''' - command = 'fefe0220fa' - self._write(command) - if self.serial_port.inWaiting() > 0: - data = self._read() - data_list = self._parse_data(data, 'get_angles') - return data_list - else: - return [] - - def get_angles_of_radian(self): - '''Get all angle return a list - - Return: - data_list (list): - - ''' - command = 'fefe0220fa' - self._write(command) - if self.serial_port.inWaiting() > 0: - data = self._read() - data_list = self._parse_data(data, 'get_angles_of_radian') - return data_list - else: - return [] - - def send_angle(self, id, degree, speed): - '''Send one angle - - Args: - id (common.Angle): - degree (int): - speed (int): 0 ~100 - - ''' - _hex = self._angle_to_hex(degree) - speed = self._complement_zero(hex(speed)[2:], digit=2) - command = 'fefe0621{}{}{}fa'.format(id, _hex, speed) - # print(command) - self._write(command) - - def send_angles(self, degrees, speed): - '''Send all angles - - Args: - degrees (list): example [0, 0, 0, 0, 0, 0] - speed (int): 0 ~ 100 - - ''' - if len(degrees) != 6: - print('The lenght of degrees is not right') - return - command = 'fefe0f22' - speed = self._complement_zero(hex(speed)[2:], digit=2) - for degree in degrees: - _hex = self._angle_to_hex(degree) - # print(_hex) - command += _hex - command += '{}fa'.format(speed) - # print(command) - self._write(command) - - def send_angles_by_radian(self, radians, speed): - '''Send all angles - - Args: - degrees (list): example [0, 0, 0, 0, 0, 0] - speed (int): 0 ~ 100 - - ''' - if len(radians) != 6: - print('The lenght of degrees is not right') - return - command = 'fefe0f22' - speed = self._complement_zero(hex(speed)[2:], digit=2) - for radian in radians: - # print(radian) - _hex = self._angle_to_hex(radian, is_degree=False) - # print(_hex) - command += _hex - command += '{}fa'.format(speed) - # print(command) - self._write(command) - - def get_coords(self): - '''Get all coords. - - Return: - data_list (list): [x, y, z, rx, ry, rz] (mm) - - ''' - command = 'fefe0223fa' - self._write(command) - if self.serial_port.inWaiting() > 0: - data = self._read() - data_list = self._parse_data(data, 'get_coords') - return data_list - else: - return [] - - def send_coord(self, id, coord, speed): - '''Send one coord - - Args: - id(common.Coord): - coord(fload): - speed(int): - - ''' - command = 'fefe0624' - command += id - command += self._coord_to_hex(coord) - command += self._complement_zero(hex(int(speed))[2:], digit=2) - # print(command) - self._write(command) - - def send_coords(self, coords, speed, mode): - '''Send all coords - - Args: - coords: [x, y, z, rx, ry, rz] - speed(int); - mode(int): 0 - angluar, 1 - linear - - ''' - if len(coords) != 6: - print('The lenght of coords is not right') - return - command = 'fefe1025' - speed = hex(speed)[2:] - speed = self._complement_zero(speed, digit=2) - mode = self._complement_zero(hex(mode)[2:], digit=2) - for coord in coords: - _hex = self._coord_to_hex(coord) - - command += (_hex) - - command += '{}{}fa'.format(speed, mode) - # print(command) - self._write(command) - - def jog_angle(self, joint_id, direction, speed): - '''Joint control - - joint_id: string - direction: int [0, 1] - speed: int (0 - 100) - ''' - command = 'fefe0530' - direction = hex(direction)[2:] - direction = self._complement_zero(direction, digit=2) - speed = hex(speed)[2:] - speed = self._complement_zero(speed, digit=2) - command += '{}{}{}fa'.format(joint_id, direction, speed) - self._write(command) - - def jog_coord(self, coord, direction, speed): - '''Coord control - - coord: string - direction: int [0, 1] - speed: int (0 - 100) - ''' - command = 'fefe0532' - direction = hex(direction)[2:] - direction = self._complement_zero(direction, digit=2) - speed = hex(speed)[2:] - speed = self._complement_zero(speed, digit=2) - command += '{}{}{}fa'.format(coord, direction, speed) - self._write(command) - - def jog_stop(self): - self._write('fefe0234fa') - - def is_servo_enable(self): - pass - - def is_all_servo_enable(self): - pass - - def set_color(self, rgb): - '''Set the light color - - Args: - rgs (str): example 'ff0000' - - ''' - command = 'fefe056a{}fa'.format(rgb) - # print(command) - self._write(command) - - def is_moving(self): - command = 'fefe022bfa' - self._write(command) - data = self._read(2) - # print(data) - if not data: - return True - flag = int(data.encode('hex'), 16) - if flag: - return True - else: - return False - - def pause(self): - self._write('fefe0226fa') - - def resume(self): - self._write('fefe0228fa') - - def stop(self): - self._write('fefe0229fa') - - def is_paused(self): - self._write('fefe0227fa') - data = self._read() - flag = int(data.encode('hex'), 16) - return False if flag else True - - def is_in_position(self, coords): - if len(coords) != 6: - print('The lenght of coords is not right') - return - command = 'fefe0d2a' - for coord in coords: - _hex = self._coord_to_hex(coord) - - command += (_hex) - - command += 'fa' - # print(command) - self._write(command) - data = self._read() - flag = int(data.encode('hex'), 16) - return False if flag else True - - def get_speed(self): - self._write('fefe0240fa') - data = self._read() - if data: - return int(data.encode('hex'), 16) - - def set_speed(self, speed): - '''Set speed value - - Args: - speed (int): 0 - 100 - ''' - speed = int(speed) - if not 0 <= speed <= 100: - raise Exception('speed value out of range (0 ~ 100)') - _hex = str(hex(speed))[2:] - self._write('fefe0341{}fa'.format(_hex)) - - def _parse_data(self, data, name): - data_list = [] - data = data.encode('hex') - data = data[-28:] - # print(data) - if not (data.startswith('20') and data.endswith('fa')): - return [] - if name == 'get_angles': - data = data[-26:-2] - for i in range(6): - _hex = data[i * 4: (i * 4) + 4] - degree = self._hex_to_degree(_hex) - data_list.append(degree) - - elif name == 'get_coords': - data = data[-26:-2] - for i in range(6): - _hex = data[i * 4: (i * 4) + 4] - _coord = self._hex_to_int(_hex) / 10.0 - data_list.append(_coord) - - elif name == 'get_angles_of_radian': - data = data[-26:-2] - for i in range(6): - _hex = data[i * 4: (i * 4) + 4] - _radian = self._hex_to_int(_hex) / 1000.0 - data_list.append(_radian) - - return (data_list) - - def _hex_to_degree(self, _hex): - _int = self._hex_to_int(_hex) - return _int * 18 / 314 - - def _hex_to_int(self, _hex): - _int = int(_hex, 16) - if _int > 0x8000: - _int -= 0x10000 - return _int - - def _angle_to_hex(self, _degree, is_degree=True): - if is_degree: - radian = (_degree * (3140 / 180)) - else: - radian = _degree * 1000 - radian = int(radian) - if radian < 0: - radian += 0x10000 - radian = round(radian) - s = str(hex(int(radian)))[2:] - s = self._complement_zero(s) - return s - - def _coord_to_hex(self, coord): - coord *= 10 - coord = int(coord) - if coord < 0: - coord += 0x10000 - s = str(hex(int(coord)))[2:] - s = self._complement_zero(s) - return s - - def _complement_zero(self, s, digit=4): - s_len = len(s) - if s_len == digit: - return s - need_len = digit - s_len - s = ''.join(['0' for _ in range(need_len)] + [s]) - return s - - def _write(self, data): - # print(data) - data = data.decode('hex') - self.serial_port.write(data) - time.sleep(0.05) - - def _read(self, size=1024): - data = self.serial_port.read(size) - return data diff --git a/scripts/pythonAPI/mycobot3.py b/scripts/pythonAPI/mycobot3.py deleted file mode 100755 index 67a1477..0000000 --- a/scripts/pythonAPI/mycobot3.py +++ /dev/null @@ -1,397 +0,0 @@ -import sys -sys.path.append('.') -import time, serial, struct - -class MyCobot(): - '''MyCobot Python API - - Possessed function: - power_on() : - power_off() : - get_angles() : - get_angles_of_radian() : - send_angle() : - send_angles() : - send_angles_by_radian() : - set_color() : - get_coords() : - send_coords() : - jog_angle() : - jog_coord() : - jog_stop() : - is_moving() : - pause() : - resume() : - stop() : - is_paused() : - get_speed() : - set_speed() : - ''' - - def __init__(self, port): - # _prot = subprocess.run(['echo -n /dev/ttyUSB*'], - # stdout=subprocess.PIPE, - # shell=True).stdout.decode('utf-8') - _prot = port - _boudrate = '115200' - _timeout = 0.1 - - for _ in range(5): - try: - self.serial_port = serial.Serial(_prot, _boudrate, timeout=_timeout) - break - except Exception as e: - print(e) - time.sleep(5) - continue - else: - print('Connect prot failed, eixt.') - exit(0) - - def power_on(self): - self._write('fefe0210fa') - - def power_off(self): - self._write('fefe0211fa') - - def set_free_mode(self): - self._write('fefe0213fa') - - def get_angles(self): - '''Get all angle return a list - - Return: - data_list (list): - - ''' - command = 'fefe0220fa' - self._write(command) - if self.serial_port.inWaiting() > 0: - data = self._read() - data_list = self._parse_data(data, 'get_angles') - return data_list - else: - return [] - - def get_angles_of_radian(self): - '''Get all angle return a list - - Return: - data_list (list): - - ''' - command = 'fefe0220fa' - self._write(command) - if self.serial_port.inWaiting() > 0: - data = self._read() - data_list = self._parse_data(data, 'get_angles_of_radian') - return data_list - else: - return [] - - def send_angle(self, id, degree, speed): - '''Send one angle - - Args: - id (common.Angle): - degree (int): - speed (int): 0 ~100 - - ''' - _hex = self._angle_to_hex(degree) - speed = self._complement_zero(hex(speed)[2:], digit=2) - command = 'fefe0621{}{}{}fa'.format(id, _hex, speed) - # print(command) - self._write(command) - - def send_angles(self, degrees, speed): - '''Send all angles - - Args: - degrees (list): example [0, 0, 0, 0, 0, 0] - speed (int): 0 ~ 100 - - ''' - if len(degrees) != 6: - print('The lenght of degrees is not right') - return - command = 'fefe0f22' - speed = self._complement_zero(hex(speed)[2:], digit=2) - for degree in degrees: - _hex = self._angle_to_hex(degree) - # print(_hex) - command += _hex - command += '{}fa'.format(speed) - # print(command) - self._write(command) - - def send_angles_by_radian(self, radians, speed): - '''Send all angles - - Args: - degrees (list): example [0, 0, 0, 0, 0, 0] - speed (int): 0 ~ 100 - - ''' - if len(radians) != 6: - print('The lenght of degrees is not right') - return - command = 'fefe0f22' - speed = self._complement_zero(hex(speed)[2:], digit=2) - for radian in radians: - # print(radian) - _hex = self._angle_to_hex(radian, is_degree=False) - # print(_hex) - command += _hex - command += '{}fa'.format(speed) - # print(command) - self._write(command) - - def get_coords(self): - '''Get all coords. - - Return: - data_list (list): [x, y, z, rx, ry, rz] (mm) - - ''' - command = 'fefe0223fa' - self._write(command) - if self.serial_port.inWaiting() > 0: - data = self._read() - data_list = self._parse_data(data, 'get_coords') - return data_list - else: - return [] - - def send_coord(self, id, coord, speed): - '''Send one coord - - Args: - id(common.Coord): - coord(fload): - speed(int): - - ''' - command = 'fefe0624' - command += id - command += self._coord_to_hex(coord) - command += self._complement_zero(hex(int(speed))[2:], digit=2) - # print(command) - self._write(command) - - def send_coords(self, coords, speed, mode): - '''Send all coords - - Args: - coords: [x, y, z, rx, ry, rz] - speed(int); - mode(int): 0 - angluar, 1 - linear - - ''' - if len(coords) != 6: - print('The lenght of coords is not right') - return - command = 'fefe1025 ' - speed = hex(speed)[2:] - speed = self._complement_zero(speed, digit=2) - mode = self._complement_zero(hex(mode)[2:], digit=2) - for coord in coords: - _hex = self._coord_to_hex(coord) - - command += (_hex + ' ') - - command += '{}{}fa'.format(speed, mode) - # print(command) - self._write(command) - - def jog_angle(self, joint_id, direction, speed): - '''Joint control - - joint_id: string - direction: int [0, 1] - speed: int (0 - 100) - ''' - command = 'fefe0530' - direction = hex(direction)[2:] - direction = self._complement_zero(direction, digit=2) - speed = hex(speed)[2:] - speed = self._complement_zero(speed, digit=2) - command += '{}{}{}fa'.format(joint_id, direction, speed) - self._write(command) - - def jog_coord(self, coord, direction, speed): - '''Coord control - - coord: string - direction: int [0, 1] - speed: int (0 - 100) - ''' - command = 'fefe0532' - direction = hex(direction)[2:] - direction = self._complement_zero(direction, digit=2) - speed = hex(speed)[2:] - speed = self._complement_zero(speed, digit=2) - command += '{}{}{}fa'.format(coord, direction, speed) - self._write(command) - - def jog_stop(self): - self._write('fefe0234fa') - - def is_servo_enable(self): - pass - - def is_all_servo_enable(self): - pass - - def set_color(self, rgb): - '''Set the light color - - Args: - rgs (str): example 'ff0000' - - ''' - command = 'fe fe 05 6a {} fa'.format(rgb) - # print(command) - self._write(command) - - def is_moving(self): - command = 'fe fe 02 2b fa' - self._write(command) - data = self._read(2) - # print(data) - if not data: - return True - flag = int(data.hex(), 16) - if flag: - return True - else: - return False - - def pause(self): - self._write('fe fe 02 26 fa') - - def resume(self): - self._write('fe fe 02 28 fa') - - def stop(self): - self._write('fe fe 02 29 fa') - - def is_paused(self): - self._write('fe fe 02 27 fa') - data = self._read() - flag = int(data.hex(), 16) - return False if flag else True - - def is_in_position(self, coords): - if len(coords) != 6: - print('The lenght of coords is not right') - return - command = 'fe fe 0d 2a ' - for coord in coords: - _hex = self._coord_to_hex(coord) - - command += (_hex + ' ') - - command += 'fa' - # print(command) - self._write(command) - data = self._read() - flag = int(data.hex(), 16) - return False if flag else True - - def get_speed(self): - self._write('fe fe 02 40 fa') - data = self._read() - if data: - return int(data.hex(), 16) - - def set_speed(self, speed): - '''Set speed value - - Args: - speed (int): 0 - 100 - ''' - speed = int(speed) - if not 0 <= speed <= 100: - raise Exception('speed value out of range (0 ~ 100)') - _hex = str(hex(speed))[2:] - self._write('fe fe 03 41 {} fa'.format(_hex)) - - def _parse_data(self, data, name): - data_list = [] - data = data.hex() - data = data[-28:] - if not (data.startswith('20') and data.endswith('fa')): - return [] - if name == 'get_angles': - data = data[-26:-2] - for i in range(6): - _hex = data[i * 4: (i * 4) + 4] - degree = self._hex_to_degree(_hex) - data_list.append(degree) - - elif name == 'get_coords': - data = data[-26:-2] - for i in range(6): - _hex = data[i * 4: (i * 4) + 4] - _coord = self._hex_to_int(_hex) / 10.0 - data_list.append(_coord) - - elif name == 'get_angles_of_radian': - data = data[-26:-2] - for i in range(6): - _hex = data[i * 4: (i * 4) + 4] - _radian = self._hex_to_int(_hex) / 1000.0 - data_list.append(_radian) - - return (data_list) - - def _hex_to_degree(self, _hex: str): - _int = self._hex_to_int(_hex) - return _int * 18 / 314 - - def _hex_to_int(self, _hex: str): - _int = int(_hex, 16) - if _int > 0x8000: - _int -= 0x10000 - return _int - - def _angle_to_hex(self, _degree: float, is_degree=True): - if is_degree: - radian = (_degree * (3140 / 180)) - else: - radian = _degree * 1000 - radian = int(radian) - if radian < 0: - radian += 0x10000 - radian = round(radian) - s = str(hex(radian))[2:] - s = self._complement_zero(s) - return s - - def _coord_to_hex(self, coord): - coord *= 10 - coord = int(coord) - if coord < 0: - coord += 0x10000 - s = str(hex(coord))[2:] - s = self._complement_zero(s) - return s - - def _complement_zero(self, s, digit=4): - s_len = len(s) - if s_len == digit: - return s - need_len = digit - s_len - s = ''.join(['0' for _ in range(need_len)] + [s]) - return s - - def _write(self, data: str): - # print(data) - data = bytes.fromhex(data) - self.serial_port.write(data) - time.sleep(0.05) - - def _read(self, size: int=1024): - data = self.serial_port.read(size) - return data diff --git a/scripts/test.py b/scripts/test.py index cfa59cf..fe83ae5 100644 --- a/scripts/test.py +++ b/scripts/test.py @@ -1,7 +1,7 @@ import time, random, subprocess -from pythonAPI.mycobot import MyCobot +from pymycobot.mycobot import MyCobot # from pythonAPI.mycobot3 import MyCobot as MyCobot3 -from pythonAPI.common import Angle, Coord +from pymycobot.common import Angle, Coord if __name__ == '__main__': port = subprocess.check_output(['echo -n /dev/ttyUSB*'],