mirror of
https://github.com/elephantrobotics/mycobot_ros.git
synced 2026-07-05 19:47:04 +00:00
refactor(pythonapi): suport python2
This commit is contained in:
张立军
parent
98c41cb51b
commit
b247ca14ee
3 changed files with 416 additions and 34 deletions
|
|
@ -9,5 +9,10 @@ class Angle(enum.Enum):
|
|||
J6 = '05'
|
||||
|
||||
|
||||
class Coords(enum.Enum):
|
||||
...
|
||||
class Coord(enum.Enum):
|
||||
X = '00'
|
||||
Y = '01'
|
||||
Z = '02'
|
||||
Rx = '03'
|
||||
Ry = '04'
|
||||
Rz = '05'
|
||||
357
scripts/pythonAPI/mycobot 3.py
Normal file
357
scripts/pythonAPI/mycobot 3.py
Normal file
|
|
@ -0,0 +1,357 @@
|
|||
import sys
|
||||
sys.path.append('.')
|
||||
import time, subprocess, 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() :
|
||||
is_moving() :
|
||||
pause() :
|
||||
resume() :
|
||||
stop() :
|
||||
is_paused() :
|
||||
get_speed() :
|
||||
set_speed() :
|
||||
'''
|
||||
|
||||
def __init__(self):
|
||||
_prot = subprocess.run(['echo -n /dev/ttyUSB*'],
|
||||
stdout=subprocess.PIPE,
|
||||
shell=True).stdout.decode('utf-8')
|
||||
_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('fe fe 02 10 fa')
|
||||
|
||||
def power_off(self):
|
||||
self._write('fe fe 02 11 fa')
|
||||
|
||||
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)
|
||||
command = 'fefe0621{}{}{}fa'.format(id, _hex, hex(speed)[2:])
|
||||
# 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 = 'fe fe 02 23 fa'
|
||||
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 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()
|
||||
# print(data)
|
||||
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
|
||||
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
|
||||
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
|
||||
|
|
@ -26,9 +26,8 @@ class MyCobot():
|
|||
'''
|
||||
|
||||
def __init__(self):
|
||||
_prot = subprocess.run(['echo -n /dev/ttyUSB*'],
|
||||
stdout=subprocess.PIPE,
|
||||
shell=True).stdout.decode('utf-8')
|
||||
_prot = subprocess.check_output(['echo -n /dev/ttyUSB*'],
|
||||
shell=True)
|
||||
_boudrate = '115200'
|
||||
_timeout = 0.1
|
||||
|
||||
|
|
@ -45,10 +44,10 @@ class MyCobot():
|
|||
exit(0)
|
||||
|
||||
def power_on(self):
|
||||
self._write('fe fe 02 10 fa')
|
||||
self._write('fefe0210fa')
|
||||
|
||||
def power_off(self):
|
||||
self._write('fe fe 02 11 fa')
|
||||
self._write('fefe0211fa')
|
||||
|
||||
def set_free_mode(self):
|
||||
self._write('fefe0213fa')
|
||||
|
|
@ -149,7 +148,7 @@ class MyCobot():
|
|||
data_list (list): [x, y, z, rx, ry, rz] (mm)
|
||||
|
||||
'''
|
||||
command = 'fe fe 02 23 fa'
|
||||
command = 'fefe0223fa'
|
||||
self._write(command)
|
||||
if self.serial_port.inWaiting() > 0:
|
||||
data = self._read()
|
||||
|
|
@ -158,31 +157,52 @@ class MyCobot():
|
|||
else:
|
||||
return []
|
||||
|
||||
def send_coord(self):
|
||||
...
|
||||
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 '
|
||||
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 += (_hex)
|
||||
|
||||
command += '{}{}fa'.format(speed, mode)
|
||||
# print(command)
|
||||
print(command)
|
||||
self._write(command)
|
||||
|
||||
def is_servo_enable(self):
|
||||
...
|
||||
pass
|
||||
|
||||
def is_all_servo_enable(self):
|
||||
...
|
||||
pass
|
||||
|
||||
def set_color(self, rgb):
|
||||
'''Set the light color
|
||||
|
|
@ -191,12 +211,12 @@ class MyCobot():
|
|||
rgs (str): example 'ff0000'
|
||||
|
||||
'''
|
||||
command = 'fe fe 05 6a {} fa'.format(rgb)
|
||||
command = 'fefe056a{}fa'.format(rgb)
|
||||
# print(command)
|
||||
self._write(command)
|
||||
|
||||
def is_moving(self):
|
||||
command = 'fe fe 02 2b fa'
|
||||
command = 'fefe022bfa'
|
||||
self._write(command)
|
||||
data = self._read(2)
|
||||
# print(data)
|
||||
|
|
@ -209,16 +229,16 @@ class MyCobot():
|
|||
return False
|
||||
|
||||
def pause(self):
|
||||
self._write('fe fe 02 26 fa')
|
||||
self._write('fefe0226fa')
|
||||
|
||||
def resume(self):
|
||||
self._write('fe fe 02 28 fa')
|
||||
self._write('fefe0228fa')
|
||||
|
||||
def stop(self):
|
||||
self._write('fe fe 02 29 fa')
|
||||
self._write('fefe0229fa')
|
||||
|
||||
def is_paused(self):
|
||||
self._write('fe fe 02 27 fa')
|
||||
self._write('fefe0227fa')
|
||||
data = self._read()
|
||||
flag = int(data.hex(), 16)
|
||||
return False if flag else True
|
||||
|
|
@ -227,11 +247,11 @@ class MyCobot():
|
|||
if len(coords) != 6:
|
||||
print('The lenght of coords is not right')
|
||||
return
|
||||
command = 'fe fe 0d 2a '
|
||||
command = 'fefe0d2a'
|
||||
for coord in coords:
|
||||
_hex = self._coord_to_hex(coord)
|
||||
|
||||
command += (_hex + ' ')
|
||||
command += (_hex)
|
||||
|
||||
command += 'fa'
|
||||
print(command)
|
||||
|
|
@ -241,7 +261,7 @@ class MyCobot():
|
|||
return False if flag else True
|
||||
|
||||
def get_speed(self):
|
||||
self._write('fe fe 02 40 fa')
|
||||
self._write('fefe0240fa')
|
||||
data = self._read()
|
||||
if data:
|
||||
return int(data.hex(), 16)
|
||||
|
|
@ -256,11 +276,11 @@ class MyCobot():
|
|||
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))
|
||||
self._write('fefe0341{}fa'.format(_hex))
|
||||
|
||||
def _parse_data(self, data, name):
|
||||
data_list = []
|
||||
data = data.hex()
|
||||
data = data.encode('hex')
|
||||
# print(data)
|
||||
if name == 'get_angles':
|
||||
data = data[-26:-2]
|
||||
|
|
@ -285,17 +305,17 @@ class MyCobot():
|
|||
|
||||
return (data_list)
|
||||
|
||||
def _hex_to_degree(self, _hex: str):
|
||||
def _hex_to_degree(self, _hex):
|
||||
_int = self._hex_to_int(_hex)
|
||||
return _int * 18 / 314
|
||||
|
||||
def _hex_to_int(self, _hex: str):
|
||||
def _hex_to_int(self, _hex):
|
||||
_int = int(_hex, 16)
|
||||
if _int > 0x8000:
|
||||
_int -= 0x10000
|
||||
return _int
|
||||
|
||||
def _angle_to_hex(self, _degree: float, is_degree=True):
|
||||
def _angle_to_hex(self, _degree, is_degree=True):
|
||||
if is_degree:
|
||||
radian = (_degree * (3140 / 180))
|
||||
else:
|
||||
|
|
@ -304,7 +324,7 @@ class MyCobot():
|
|||
if radian < 0:
|
||||
radian += 0x10000
|
||||
radian = round(radian)
|
||||
s = str(hex(radian))[2:]
|
||||
s = str(hex(int(radian)))[2:]
|
||||
s = self._complement_zero(s)
|
||||
return s
|
||||
|
||||
|
|
@ -313,7 +333,7 @@ class MyCobot():
|
|||
coord = int(coord)
|
||||
if coord < 0:
|
||||
coord += 0x10000
|
||||
s = str(hex(coord))[2:]
|
||||
s = str(hex(int(coord)))[2:]
|
||||
s = self._complement_zero(s)
|
||||
return s
|
||||
|
||||
|
|
@ -325,12 +345,12 @@ class MyCobot():
|
|||
s = ''.join(['0' for _ in range(need_len)] + [s])
|
||||
return s
|
||||
|
||||
def _write(self, data: str):
|
||||
def _write(self, data):
|
||||
# print(data)
|
||||
data = bytes.fromhex(data)
|
||||
data = data.decode('hex')
|
||||
self.serial_port.write(data)
|
||||
time.sleep(0.05)
|
||||
|
||||
def _read(self, size: int=1024):
|
||||
def _read(self, size=1024):
|
||||
data = self.serial_port.read(size)
|
||||
return data
|
||||
Loading…
Add table
Reference in a new issue