mirror of
https://github.com/elephantrobotics/mycobot_ros.git
synced 2026-07-05 19:47:04 +00:00
333 lines
No EOL
8.8 KiB
Python
333 lines
No EOL
8.8 KiB
Python
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 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):
|
|
...
|
|
|
|
def send_coords(self, coords, speed, mode):
|
|
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):
|
|
...
|
|
|
|
def is_all_servo_enable(self):
|
|
...
|
|
|
|
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 |