diff --git a/.gitignore b/.gitignore index 67970dc..da7d841 100644 --- a/.gitignore +++ b/.gitignore @@ -1,3 +1,5 @@ urdf.zip -.DS_Store \ No newline at end of file +.DS_Store +.vscode/settings.json +.vscode/c_cpp_properties.json \ No newline at end of file diff --git a/scripts/pythonAPI/mycobot.py b/scripts/pythonAPI/mycobot.py index 06fba8d..377d0b7 100755 --- a/scripts/pythonAPI/mycobot.py +++ b/scripts/pythonAPI/mycobot.py @@ -1,6 +1,6 @@ import sys sys.path.append('.') -import time, subprocess, serial, struct +import time, serial, struct class MyCobot(): '''MyCobot Python API @@ -16,6 +16,9 @@ class MyCobot(): set_color() : get_coords() : send_coords() : + jog_angle() : + jog_coord() : + jog_stop() : is_moving() : pause() : resume() : @@ -25,9 +28,10 @@ class MyCobot(): set_speed() : ''' - def __init__(self): - _prot = subprocess.check_output(['echo -n /dev/ttyUSB*'], - shell=True) + def __init__(self, port): + # _prot = subprocess.check_output(['echo -n /dev/ttyUSB*'], + # shell=True) + _prot = port _boudrate = '115200' _timeout = 0.1 @@ -196,9 +200,42 @@ class MyCobot(): command += (_hex) command += '{}{}fa'.format(speed, mode) - print(command) + # print(command) self._write(command) + def jog_angle(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(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 @@ -282,7 +319,10 @@ class MyCobot(): def _parse_data(self, data, name): data_list = [] data = data.encode('hex') - # print(data) + 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): @@ -294,14 +334,14 @@ class MyCobot(): data = data[-26:-2] for i in range(6): _hex = data[i * 4: (i * 4) + 4] - _coord = self._hex_to_int(_hex) / 10 + _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 + _radian = self._hex_to_int(_hex) / 1000.0 data_list.append(_radian) return (data_list) diff --git a/scripts/pythonAPI/mycobot3.py b/scripts/pythonAPI/mycobot3.py index 263386d..f9c1d63 100755 --- a/scripts/pythonAPI/mycobot3.py +++ b/scripts/pythonAPI/mycobot3.py @@ -1,6 +1,6 @@ import sys sys.path.append('.') -import time, subprocess, serial, struct +import time, serial, struct class MyCobot(): '''MyCobot Python API @@ -16,6 +16,9 @@ class MyCobot(): set_color() : get_coords() : send_coords() : + jog_angle() : + jog_coord() : + jog_stop() : is_moving() : pause() : resume() : @@ -25,10 +28,11 @@ class MyCobot(): set_speed() : ''' - def __init__(self): - _prot = subprocess.run(['echo -n /dev/ttyUSB*'], - stdout=subprocess.PIPE, - shell=True).stdout.decode('utf-8') + 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 @@ -45,10 +49,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') @@ -150,7 +154,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() @@ -199,6 +203,39 @@ class MyCobot(): command += '{}{}fa'.format(speed, mode) # print(command) self._write(command) + + def jog_angle(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(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 @@ -283,7 +320,9 @@ class MyCobot(): def _parse_data(self, data, name): data_list = [] data = data.hex() - # print(data) + 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): @@ -295,14 +334,14 @@ class MyCobot(): data = data[-26:-2] for i in range(6): _hex = data[i * 4: (i * 4) + 4] - _coord = self._hex_to_int(_hex) / 10 + _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 + _radian = self._hex_to_int(_hex) / 1000.0 data_list.append(_radian) return (data_list)