Skip to content

Commit cdd8049

Browse files
Zachary ZhangZachary Zhang
Zachary Zhang
authored and
Zachary Zhang
committed
feat(pythonAPI): add new method.
1 parent 997ea2d commit cdd8049

File tree

4 files changed

+112
-34
lines changed

4 files changed

+112
-34
lines changed

‎API/Python/pymycobot/mycobot.py‎

100644100755
Lines changed: 54 additions & 15 deletions
Original file line numberDiff line numberDiff line change
@@ -1,12 +1,12 @@
11
import sys
22
sys.path.append('.')
3-
import time, subprocess, serial, struct
3+
import time, serial, struct
44

55
class MyCobot():
66
'''MyCobot Python API
77
88
Possessed function:
9-
power_on() :
9+
power_on() :
1010
power_off() :
1111
get_angles() :
1212
get_angles_of_radian() :
@@ -16,6 +16,9 @@ class MyCobot():
1616
set_color() :
1717
get_coords() :
1818
send_coords() :
19+
jog_angle() :
20+
jog_coord() :
21+
jog_stop() :
1922
is_moving() :
2023
pause() :
2124
resume() :
@@ -26,9 +29,9 @@ class MyCobot():
2629
'''
2730

2831
def __init__(self, port):
29-
_prot = port
3032
# _prot = subprocess.check_output(['echo -n /dev/ttyUSB*'],
31-
# shell=True)
33+
# shell=True)
34+
_prot = port
3235
_boudrate = '115200'
3336
_timeout = 0.1
3437

@@ -197,9 +200,42 @@ def send_coords(self, coords, speed, mode):
197200
command += (_hex)
198201

199202
command += '{}{}fa'.format(speed, mode)
200-
print(command)
203+
# print(command)
204+
self._write(command)
205+
206+
def jog_angle(joint_id, direction, speed):
207+
'''Joint control
208+
209+
joint_id: string
210+
direction: int [0, 1]
211+
speed: int (0 - 100)
212+
'''
213+
command = 'fefe0530'
214+
direction = hex(direction)[2:]
215+
direction = self._complement_zero(direction, digit=2)
216+
speed = hex(speed)[2:]
217+
speed = self._complement_zero(speed, digit=2)
218+
command += '{}{}{}fa'.format(joint_id, direction, speed)
219+
self._write(command)
220+
221+
def jog_coord(coord, direction, speed):
222+
'''Coord control
223+
224+
coord: string
225+
direction: int [0, 1]
226+
speed: int (0 - 100)
227+
'''
228+
command = 'fefe0532'
229+
direction = hex(direction)[2:]
230+
direction = self._complement_zero(direction, digit=2)
231+
speed = hex(speed)[2:]
232+
speed = self._complement_zero(speed, digit=2)
233+
command += '{}{}{}fa'.format(coord, direction, speed)
201234
self._write(command)
202235

236+
def jog_stop(self):
237+
self._write('fefe0234fa')
238+
203239
def is_servo_enable(self):
204240
pass
205241

@@ -232,7 +268,7 @@ def is_moving(self):
232268

233269
def pause(self):
234270
self._write('fefe0226fa')
235-
271+
236272
def resume(self):
237273
self._write('fefe0228fa')
238274

@@ -244,7 +280,7 @@ def is_paused(self):
244280
data = self._read()
245281
flag = int(data.hex(), 16)
246282
return False if flag else True
247-
283+
248284
def is_in_position(self, coords):
249285
if len(coords) != 6:
250286
print('The lenght of coords is not right')
@@ -283,7 +319,10 @@ def set_speed(self, speed):
283319
def _parse_data(self, data, name):
284320
data_list = []
285321
data = data.encode('hex')
286-
# print(data)
322+
data = data[-28:]
323+
print(data)
324+
if not (data.startswith('20') and data.endswith('fa')):
325+
return []
287326
if name == 'get_angles':
288327
data = data[-26:-2]
289328
for i in range(6):
@@ -295,22 +334,22 @@ def _parse_data(self, data, name):
295334
data = data[-26:-2]
296335
for i in range(6):
297336
_hex = data[i * 4: (i * 4) + 4]
298-
_coord = self._hex_to_int(_hex) / 10
337+
_coord = self._hex_to_int(_hex) / 10.0
299338
data_list.append(_coord)
300339

301340
elif name == 'get_angles_of_radian':
302341
data = data[-26:-2]
303342
for i in range(6):
304343
_hex = data[i * 4: (i * 4) + 4]
305-
_radian = self._hex_to_int(_hex) / 1000
344+
_radian = self._hex_to_int(_hex) / 1000.0
306345
data_list.append(_radian)
307346

308347
return (data_list)
309348

310349
def _hex_to_degree(self, _hex):
311350
_int = self._hex_to_int(_hex)
312-
return _int * 18 / 314
313-
351+
return _int * 18 / 314
352+
314353
def _hex_to_int(self, _hex):
315354
_int = int(_hex, 16)
316355
if _int > 0x8000:
@@ -328,7 +367,7 @@ def _angle_to_hex(self, _degree, is_degree=True):
328367
radian = round(radian)
329368
s = str(hex(int(radian)))[2:]
330369
s = self._complement_zero(s)
331-
return s
370+
return s
332371

333372
def _coord_to_hex(self, coord):
334373
coord *= 10
@@ -338,14 +377,14 @@ def _coord_to_hex(self, coord):
338377
s = str(hex(int(coord)))[2:]
339378
s = self._complement_zero(s)
340379
return s
341-
380+
342381
def _complement_zero(self, s, digit=4):
343382
s_len = len(s)
344383
if s_len == digit:
345384
return s
346385
need_len = digit - s_len
347386
s = ''.join(['0' for _ in range(need_len)] + [s])
348-
return s
387+
return s
349388

350389
def _write(self, data):
351390
# print(data)

‎API/Python/pymycobot/mycobot3.py‎

100644100755
Lines changed: 57 additions & 18 deletions
Original file line numberDiff line numberDiff line change
@@ -1,12 +1,12 @@
11
import sys
22
sys.path.append('.')
3-
import time, subprocess, serial, struct
3+
import time, serial, struct
44

55
class MyCobot():
66
'''MyCobot Python API
77
88
Possessed function:
9-
power_on() :
9+
power_on() :
1010
power_off() :
1111
get_angles() :
1212
get_angles_of_radian() :
@@ -16,6 +16,9 @@ class MyCobot():
1616
set_color() :
1717
get_coords() :
1818
send_coords() :
19+
jog_angle() :
20+
jog_coord() :
21+
jog_stop() :
1922
is_moving() :
2023
pause() :
2124
resume() :
@@ -26,10 +29,10 @@ class MyCobot():
2629
'''
2730

2831
def __init__(self, port):
29-
_prot = port
3032
# _prot = subprocess.run(['echo -n /dev/ttyUSB*'],
31-
# stdout=subprocess.PIPE,
32-
# shell=True).stdout.decode('utf-8')
33+
# stdout=subprocess.PIPE,
34+
# shell=True).stdout.decode('utf-8')
35+
_prot = port
3336
_boudrate = '115200'
3437
_timeout = 0.1
3538

@@ -46,10 +49,10 @@ def __init__(self, port):
4649
exit(0)
4750

4851
def power_on(self):
49-
self._write('fe fe 02 10 fa')
52+
self._write('fefe0210fa')
5053

5154
def power_off(self):
52-
self._write('fe fe 02 11 fa')
55+
self._write('fefe0211fa')
5356

5457
def set_free_mode(self):
5558
self._write('fefe0213fa')
@@ -98,6 +101,7 @@ def send_angle(self, id, degree, speed):
98101
_hex = self._angle_to_hex(degree)
99102
speed = self._complement_zero(hex(speed)[2:], digit=2)
100103
command = 'fefe0621{}{}{}fa'.format(id, _hex, speed)
104+
# print(command)
101105
self._write(command)
102106

103107
def send_angles(self, degrees, speed):
@@ -150,7 +154,7 @@ def get_coords(self):
150154
data_list (list): [x, y, z, rx, ry, rz] (mm)
151155
152156
'''
153-
command = 'fe fe 02 23 fa'
157+
command = 'fefe0223fa'
154158
self._write(command)
155159
if self.serial_port.inWaiting() > 0:
156160
data = self._read()
@@ -199,6 +203,39 @@ def send_coords(self, coords, speed, mode):
199203
command += '{}{}fa'.format(speed, mode)
200204
# print(command)
201205
self._write(command)
206+
207+
def jog_angle(joint_id, direction, speed):
208+
'''Joint control
209+
210+
joint_id: string
211+
direction: int [0, 1]
212+
speed: int (0 - 100)
213+
'''
214+
command = 'fefe0530'
215+
direction = hex(direction)[2:]
216+
direction = self._complement_zero(direction, digit=2)
217+
speed = hex(speed)[2:]
218+
speed = self._complement_zero(speed, digit=2)
219+
command += '{}{}{}fa'.format(joint_id, direction, speed)
220+
self._write(command)
221+
222+
def jog_coord(coord, direction, speed):
223+
'''Coord control
224+
225+
coord: string
226+
direction: int [0, 1]
227+
speed: int (0 - 100)
228+
'''
229+
command = 'fefe0532'
230+
direction = hex(direction)[2:]
231+
direction = self._complement_zero(direction, digit=2)
232+
speed = hex(speed)[2:]
233+
speed = self._complement_zero(speed, digit=2)
234+
command += '{}{}{}fa'.format(coord, direction, speed)
235+
self._write(command)
236+
237+
def jog_stop(self):
238+
self._write('fefe0234fa')
202239

203240
def is_servo_enable(self):
204241
pass
@@ -232,7 +269,7 @@ def is_moving(self):
232269

233270
def pause(self):
234271
self._write('fe fe 02 26 fa')
235-
272+
236273
def resume(self):
237274
self._write('fe fe 02 28 fa')
238275

@@ -244,7 +281,7 @@ def is_paused(self):
244281
data = self._read()
245282
flag = int(data.hex(), 16)
246283
return False if flag else True
247-
284+
248285
def is_in_position(self, coords):
249286
if len(coords) != 6:
250287
print('The lenght of coords is not right')
@@ -283,7 +320,9 @@ def set_speed(self, speed):
283320
def _parse_data(self, data, name):
284321
data_list = []
285322
data = data.hex()
286-
# print(data)
323+
data = data[-28:]
324+
if not (data.startswith('20') and data.endswith('fa')):
325+
return []
287326
if name == 'get_angles':
288327
data = data[-26:-2]
289328
for i in range(6):
@@ -295,22 +334,22 @@ def _parse_data(self, data, name):
295334
data = data[-26:-2]
296335
for i in range(6):
297336
_hex = data[i * 4: (i * 4) + 4]
298-
_coord = self._hex_to_int(_hex) / 10
337+
_coord = self._hex_to_int(_hex) / 10.0
299338
data_list.append(_coord)
300339

301340
elif name == 'get_angles_of_radian':
302341
data = data[-26:-2]
303342
for i in range(6):
304343
_hex = data[i * 4: (i * 4) + 4]
305-
_radian = self._hex_to_int(_hex) / 1000
344+
_radian = self._hex_to_int(_hex) / 1000.0
306345
data_list.append(_radian)
307346

308347
return (data_list)
309348

310349
def _hex_to_degree(self, _hex: str):
311350
_int = self._hex_to_int(_hex)
312-
return _int * 18 / 314
313-
351+
return _int * 18 / 314
352+
314353
def _hex_to_int(self, _hex: str):
315354
_int = int(_hex, 16)
316355
if _int > 0x8000:
@@ -328,7 +367,7 @@ def _angle_to_hex(self, _degree: float, is_degree=True):
328367
radian = round(radian)
329368
s = str(hex(radian))[2:]
330369
s = self._complement_zero(s)
331-
return s
370+
return s
332371

333372
def _coord_to_hex(self, coord):
334373
coord *= 10
@@ -338,14 +377,14 @@ def _coord_to_hex(self, coord):
338377
s = str(hex(coord))[2:]
339378
s = self._complement_zero(s)
340379
return s
341-
380+
342381
def _complement_zero(self, s, digit=4):
343382
s_len = len(s)
344383
if s_len == digit:
345384
return s
346385
need_len = digit - s_len
347386
s = ''.join(['0' for _ in range(need_len)] + [s])
348-
return s
387+
return s
349388

350389
def _write(self, data: str):
351390
# print(data)

‎API/Python/setup.py‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -26,5 +26,5 @@
2626
"Operating System :: OS Independent",
2727
],
2828
install_requires=['pyserial'],
29-
python_requires='>=3.5',
29+
python_requires='2.7, >=3.5',
3030
)

API/Python/example/test.py renamed to API/Python/test/test.py

File renamed without changes.

0 commit comments

Comments
 (0)