mirror of
https://github.com/elephantrobotics/mycobot_ros.git
synced 2026-07-05 19:47:04 +00:00
update
This commit is contained in:
zhangLijun
parent
2db25b9293
commit
99813539d3
10 changed files with 5 additions and 1321 deletions
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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):
|
||||
|
|
|
|||
|
|
@ -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():
|
||||
|
|
|
|||
138
scripts/pythonAPI/.gitignore
vendored
138
scripts/pythonAPI/.gitignore
vendored
|
|
@ -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/
|
||||
|
|
@ -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.
|
||||
|
|
@ -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'
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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*'],
|
||||
|
|
|
|||
Loading…
Add table
Reference in a new issue