Skip to content

Commit f5dbcb8

Browse files
anla-xuanla-xu
committed
Add serial port lock and fix bugs
1 parent 17c2e81 commit f5dbcb8

15 files changed

+770
-743
lines changed

‎pymycobot/__init__.py‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -54,7 +54,7 @@
5454
from pymycobot.mybuddyemoticon import MyBuddyEmoticon
5555
__all__.append("MyBuddyEmoticon")
5656

57-
__version__ = "3.3.6"
57+
__version__ = "3.3.7b1"
5858
__author__ = "Elephantrobotics"
5959
__email__ = "weiquan.xu@elephantrobotics.com"
6060
__git_url__ = "https://github.com/elephantrobotics/pymycobot"

‎pymycobot/error.py‎

Lines changed: 3 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -126,7 +126,7 @@ def public_check(parameter_list, kwargs, robot_limit, class_name, exception_clas
126126
# TODO 280/320共用MyCobot，无法进行数据限位
127127
# elif parameter == 'coords':
128128
# check_coords(value, robot_limit, class_name, exception_class)
129-
elif parameter in ['rftype', 'move_type', 'end', 'is_linear', 'status', 'mode']:
129+
elif parameter in ['rftype', 'move_type', 'end', 'is_linear', 'status', 'mode', 'direction']:
130130
check_0_or_1(parameter, value, [0, 1], value_type, exception_class, int)
131131
elif parameter == 'acceleration':
132132
check_value_type(parameter, value_type, exception_class, int)
@@ -194,6 +194,8 @@ def public_check(parameter_list, kwargs, robot_limit, class_name, exception_clas
194194
current_max = 500
195195
if value < current_min or value > current_max:
196196
raise exception_class("The range of current is {} ~ {}, but the received is {}".format(current_min, current_max, value))
197+
elif parameter == 'end_direction':
198+
check_0_or_1(parameter, value, [1, 2, 3], value_type, exception_class, int)
197199

198200
def calibration_parameters(**kwargs):
199201
with open(os.path.dirname(os.path.abspath(__file__))+"/robot_limit.json") as f:

‎pymycobot/generate.py‎

Lines changed: 32 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -179,10 +179,17 @@ def is_power_on(self):
179179
-1 - error data
180180
"""
181181
return self._mesg(ProtocolCode.IS_POWER_ON, has_reply=True)
182-
183-
def release_all_servos(self):
184-
"""Relax all joints"""
185-
return self._mesg(ProtocolCode.RELEASE_ALL_SERVOS)
182+
183+
def release_all_servos(self, data=None):
184+
"""Relax all joints
185+
186+
Args:
187+
data: 1 - Undamping (The default is damping)
188+
"""
189+
if data is None:
190+
return self._mesg(ProtocolCode.RELEASE_ALL_SERVOS)
191+
else:
192+
return self._mesg(ProtocolCode.RELEASE_ALL_SERVOS, data)
186193

187194
def is_controller_connected(self):
188195
"""Wether connected with Atom.
@@ -405,7 +412,7 @@ def jog_increment(self, joint_id, increment, speed):
405412
return self._mesg(ProtocolCode.JOG_INCREMENT, joint_id, [self._angle2int(increment)], speed)
406413

407414
def jog_stop(self):
408-
"""Stop jog moving"""
415+
"""Stop jog moving (Atom 7.0 Delete this interface)"""
409416
return self._mesg(ProtocolCode.JOG_STOP)
410417

411418
def pause(self):
@@ -574,7 +581,7 @@ def set_servo_data(self, servo_id, data_id, value, mode=None):
574581
value: 0 - 4096
575582
mode: 0 - indicates that value is one byte(default), 1 - 1 represents a value of two bytes.
576583
"""
577-
if mode is not None:
584+
if mode is None:
578585
self.calibration_parameters(class_name = self.__class__.__name__, id=servo_id, address=data_id, value=value)
579586
return self._mesg(ProtocolCode.SET_SERVO_DATA, servo_id, data_id, value)
580587
else:
@@ -630,17 +637,23 @@ def joint_brake(self, joint_id):
630637
self.calibration_parameters(class_name = self.__class__.__name__, id=joint_id)
631638
return self._mesg(ProtocolCode.JOINT_BRAKE, joint_id)
632639

633-
def release_servo(self, servo_id):
640+
def release_servo(self, servo_id, mode=None):
634641
"""Power off designated servo
635642
636643
Args:
637644
servo_id: int
638645
for mycobot / mecharm: Joint id 1 - 6
639646
for mypalletizer: Joint id 1 - 4
640647
for myArm: joint id 1 - 7
648+
mode: Default damping, set to 1, cancel damping
641649
"""
642-
self.calibration_parameters(class_name = self.__class__.__name__, id=servo_id)
643-
return self._mesg(ProtocolCode.RELEASE_SERVO, servo_id)
650+
if mode is None:
651+
self.calibration_parameters(class_name = self.__class__.__name__, id=servo_id)
652+
return self._mesg(ProtocolCode.RELEASE_SERVO, servo_id)
653+
654+
else:
655+
self.calibration_parameters(class_name = self.__class__.__name__, id=servo_id)
656+
return self._mesg(ProtocolCode.RELEASE_SERVO, servo_id, mode)
644657

645658
def focus_servo(self, servo_id):
646659
"""Power on designated servo
@@ -1195,4 +1208,13 @@ def set_four_pieces_zero(self):
11951208
int: 0 or 1 (1 - success)
11961209
"""
11971210
return self._mesg(ProtocolCode.SET_FOUR_PIECES_ZERO, has_reply = True)
1198-
1211+
1212+
def jog_rpy(self, end_direction, direction):
1213+
"""Rotate the end around a fixed axis in the base coordinate system (320 atom 7.0 is available)
1214+
1215+
Args:
1216+
end_direction (int): Roll, Pitch, Yaw (1-3)
1217+
direction (int): 1 - forward rotation, 0 - reverse rotation
1218+
"""
1219+
self.calibration_parameters(class_name = self.__class__.__name__, end_direction=end_direction)
1220+
return self._mesg(ProtocolCode.JOG_ABSOLUTE, end_direction, direction)

‎pymycobot/mecharm.py‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -2,5 +2,5 @@
22
from pymycobot import MyCobot
33

44
class MechArm(MyCobot):
5-
def __init__(self, port, baudrate="115200", timeout=0.1, debug=False, thread_lock=False):
5+
def __init__(self, port, baudrate="115200", timeout=0.1, debug=False, thread_lock=True):
66
super(MechArm, self).__init__(port, baudrate, timeout, debug, thread_lock)

‎pymycobot/mercury.py‎

Lines changed: 86 additions & 83 deletions
Original file line numberDiff line numberDiff line change
@@ -1,6 +1,7 @@
11
# coding=utf-8
22

33
import time
4+
import threading
45

56
from pymycobot.generate import CommandGenerator
67
from pymycobot.common import ProtocolCode, write, read
@@ -28,6 +29,7 @@ def __init__(self, port, baudrate="115200", timeout=0.1, debug=False):
2829
self._serial_port.timeout = timeout
2930
self._serial_port.rts = False
3031
self._serial_port.open()
32+
self.lock = threading.Lock()
3133

3234

3335
def _mesg(self, genre, *args, **kwargs):
@@ -43,94 +45,95 @@ def _mesg(self, genre, *args, **kwargs):
4345
has_reply: Whether there is a return value to accept.
4446
"""
4547
real_command, has_reply = super(Mercury, self)._mesg(genre, *args, **kwargs)
46-
self._write(self._flatten(real_command))
48+
with self.lock:
49+
self._write(self._flatten(real_command))
4750

48-
if has_reply:
49-
data = self._read(genre, _class=self.__class__.__name__)
50-
if genre == ProtocolCode.SET_SSID_PWD:
51-
return None
52-
res = self._process_received(data, genre, 14)
53-
if res == []:
54-
return None
55-
if genre in [
56-
ProtocolCode.ROBOT_VERSION,
57-
ProtocolCode.GET_ROBOT_ID,
58-
ProtocolCode.IS_POWER_ON,
59-
ProtocolCode.IS_CONTROLLER_CONNECTED,
60-
ProtocolCode.IS_PAUSED, # TODO have bug: return b''
61-
ProtocolCode.IS_IN_POSITION,
62-
ProtocolCode.IS_MOVING,
63-
ProtocolCode.IS_SERVO_ENABLE,
64-
ProtocolCode.IS_ALL_SERVO_ENABLE,
65-
ProtocolCode.GET_SERVO_DATA,
66-
ProtocolCode.GET_DIGITAL_INPUT,
67-
ProtocolCode.GET_GRIPPER_VALUE,
68-
ProtocolCode.IS_GRIPPER_MOVING,
69-
ProtocolCode.GET_SPEED,
70-
ProtocolCode.GET_ENCODER,
71-
ProtocolCode.GET_BASIC_INPUT,
72-
ProtocolCode.GET_TOF_DISTANCE,
73-
ProtocolCode.GET_END_TYPE,
74-
ProtocolCode.GET_MOVEMENT_TYPE,
75-
ProtocolCode.GET_REFERENCE_FRAME,
76-
ProtocolCode.GET_FRESH_MODE,
77-
ProtocolCode.GET_GRIPPER_MODE,
78-
ProtocolCode.SET_SSID_PWD,
79-
ProtocolCode.COBOTX_IS_GO_ZERO,
80-
ProtocolCode.GET_ERROR_DETECT_MODE
81-
]:
82-
return self._process_single(res)
83-
elif genre in [ProtocolCode.GET_ANGLES]:
84-
return [self._int2angle(angle) for angle in res]
85-
elif genre in [
86-
ProtocolCode.GET_COORDS,
87-
ProtocolCode.MERCURY_GET_BASE_COORDS,
88-
ProtocolCode.GET_TOOL_REFERENCE,
89-
ProtocolCode.GET_WORLD_REFERENCE,
90-
]:
91-
if res:
51+
if has_reply:
52+
data = self._read(genre, _class=self.__class__.__name__)
53+
if genre == ProtocolCode.SET_SSID_PWD:
54+
return None
55+
res = self._process_received(data, genre, 14)
56+
if res == []:
57+
return None
58+
if genre in [
59+
ProtocolCode.ROBOT_VERSION,
60+
ProtocolCode.GET_ROBOT_ID,
61+
ProtocolCode.IS_POWER_ON,
62+
ProtocolCode.IS_CONTROLLER_CONNECTED,
63+
ProtocolCode.IS_PAUSED, # TODO have bug: return b''
64+
ProtocolCode.IS_IN_POSITION,
65+
ProtocolCode.IS_MOVING,
66+
ProtocolCode.IS_SERVO_ENABLE,
67+
ProtocolCode.IS_ALL_SERVO_ENABLE,
68+
ProtocolCode.GET_SERVO_DATA,
69+
ProtocolCode.GET_DIGITAL_INPUT,
70+
ProtocolCode.GET_GRIPPER_VALUE,
71+
ProtocolCode.IS_GRIPPER_MOVING,
72+
ProtocolCode.GET_SPEED,
73+
ProtocolCode.GET_ENCODER,
74+
ProtocolCode.GET_BASIC_INPUT,
75+
ProtocolCode.GET_TOF_DISTANCE,
76+
ProtocolCode.GET_END_TYPE,
77+
ProtocolCode.GET_MOVEMENT_TYPE,
78+
ProtocolCode.GET_REFERENCE_FRAME,
79+
ProtocolCode.GET_FRESH_MODE,
80+
ProtocolCode.GET_GRIPPER_MODE,
81+
ProtocolCode.SET_SSID_PWD,
82+
ProtocolCode.COBOTX_IS_GO_ZERO,
83+
ProtocolCode.GET_ERROR_DETECT_MODE
84+
]:
85+
return self._process_single(res)
86+
elif genre in [ProtocolCode.GET_ANGLES]:
87+
return [self._int2angle(angle) for angle in res]
88+
elif genre in [
89+
ProtocolCode.GET_COORDS,
90+
ProtocolCode.MERCURY_GET_BASE_COORDS,
91+
ProtocolCode.GET_TOOL_REFERENCE,
92+
ProtocolCode.GET_WORLD_REFERENCE,
93+
]:
94+
if res:
95+
r = []
96+
for idx in range(3):
97+
r.append(self._int2coord(res[idx]))
98+
for idx in range(3, 6):
99+
r.append(self._int2angle(res[idx]))
100+
return r
101+
else:
102+
return res
103+
elif genre in [ProtocolCode.GET_SERVO_VOLTAGES]:
104+
return [self._int2coord(angle) for angle in res]
105+
elif genre in [ProtocolCode.GET_BASIC_VERSION, ProtocolCode.SOFTWARE_VERSION, ProtocolCode.GET_ATOM_VERSION]:
106+
return self._int2coord(self._process_single(res))
107+
elif genre in [
108+
ProtocolCode.GET_JOINT_MAX_ANGLE,
109+
ProtocolCode.GET_JOINT_MIN_ANGLE,
110+
]:
111+
return self._int2coord(res[0])
112+
elif genre == ProtocolCode.GET_ANGLES_COORDS:
113+
r = []
114+
for index in range(len(res)):
115+
if index < 7:
116+
r.append(self._int2angle(res[index]))
117+
elif index < 10:
118+
r.append(self._int2coord(res[index]))
119+
else:
120+
r.append(self._int2angle(res[index]))
121+
return r
122+
elif genre == ProtocolCode.GO_ZERO:
92123
r = []
93-
for idx in range(3):
94-
r.append(self._int2coord(res[idx]))
95-
for idx in range(3, 6):
96-
r.append(self._int2angle(res[idx]))
124+
if res:
125+
if 1 not in res[1:]:
126+
return res[0]
127+
else:
128+
for i in range(1, len(res)):
129+
if res[i] == 1:
130+
r.append(i)
97131
return r
132+
elif genre in [ProtocolCode.COBOTX_GET_ANGLE, ProtocolCode.COBOTX_GET_SOLUTION_ANGLES]:
133+
return self._int2angle(res[0])
98134
else:
99135
return res
100-
elif genre in [ProtocolCode.GET_SERVO_VOLTAGES]:
101-
return [self._int2coord(angle) for angle in res]
102-
elif genre in [ProtocolCode.GET_BASIC_VERSION, ProtocolCode.SOFTWARE_VERSION, ProtocolCode.GET_ATOM_VERSION]:
103-
return self._int2coord(self._process_single(res))
104-
elif genre in [
105-
ProtocolCode.GET_JOINT_MAX_ANGLE,
106-
ProtocolCode.GET_JOINT_MIN_ANGLE,
107-
]:
108-
return self._int2coord(res[0])
109-
elif genre == ProtocolCode.GET_ANGLES_COORDS:
110-
r = []
111-
for index in range(len(res)):
112-
if index < 7:
113-
r.append(self._int2angle(res[index]))
114-
elif index < 10:
115-
r.append(self._int2coord(res[index]))
116-
else:
117-
r.append(self._int2angle(res[index]))
118-
return r
119-
elif genre == ProtocolCode.GO_ZERO:
120-
r = []
121-
if res:
122-
if 1 not in res[1:]:
123-
return res[0]
124-
else:
125-
for i in range(1, len(res)):
126-
if res[i] == 1:
127-
r.append(i)
128-
return r
129-
elif genre in [ProtocolCode.COBOTX_GET_ANGLE, ProtocolCode.COBOTX_GET_SOLUTION_ANGLES]:
130-
return self._int2angle(res[0])
131-
else:
132-
return res
133-
return None
136+
return None
134137

135138
def open(self):
136139
self._serial_port.open()

0 commit comments

Comments
 (0)