diff --git a/scripts/pythonAPI/common.py b/scripts/pythonAPI/common.py index 36d9772..441f1c1 100644 --- a/scripts/pythonAPI/common.py +++ b/scripts/pythonAPI/common.py @@ -9,5 +9,10 @@ class Angle(enum.Enum): J6 = '05' -class Coords(enum.Enum): - ... \ No newline at end of file +class Coord(enum.Enum): + X = '00' + Y = '01' + Z = '02' + Rx = '03' + Ry = '04' + Rz = '05' \ No newline at end of file diff --git a/scripts/pythonAPI/mycobot 3.py b/scripts/pythonAPI/mycobot 3.py new file mode 100644 index 0000000..f6f8009 --- /dev/null +++ b/scripts/pythonAPI/mycobot 3.py @@ -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 \ No newline at end of file diff --git a/scripts/pythonAPI/mycobot.py b/scripts/pythonAPI/mycobot.py index 9ee7a7a..7360dda 100644 --- a/scripts/pythonAPI/mycobot.py +++ b/scripts/pythonAPI/mycobot.py @@ -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 \ No newline at end of file