Skip to content

Commit c1b98f7

Browse files
anla-xuanla-xu
committed
push code
1 parent bb8ba83 commit c1b98f7

File tree

8 files changed

+75
-19
lines changed

8 files changed

+75
-19
lines changed

‎pymycobot/__init__.py‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -48,7 +48,7 @@
4848
from pymycobot.mybuddyemoticon import MyBuddyEmoticon
4949
__all__.append("MyBuddyEmoticon")
5050

51-
__version__ = "3.1.9b1"
51+
__version__ = "3.1.9b3"
5252
__author__ = "Elephantrobotics"
5353
__email__ = "[email protected]"
5454
__git_url__ = "https://github.com/elephantrobotics/pymycobot"

‎pymycobot/cobotx.py‎

Lines changed: 30 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -112,6 +112,15 @@ def _mesg(self, genre, *args, **kwargs):
112112
else:
113113
r.append(self._int2angle(res[index]))
114114
return r
115+
elif genre == ProtocolCode.GO_ZERO:
116+
r = []
117+
if 1 not in res[1:]:
118+
return res[0]
119+
else:
120+
for i in range(1, len(res)):
121+
if res[i] == 1:
122+
r.append(i)
123+
return r
115124
else:
116125
return res
117126
return None
@@ -158,8 +167,13 @@ def focus_all_servos(self):
158167
return self._mesg(ProtocolCode.FOCUS_ALL_SERVOS)
159168

160169
def go_zero(self):
161-
"""go zero"""
162-
return self._mesg(ProtocolCode.GO_ZERO)
170+
"""Control the machine to return to the zero position.
171+
172+
Return:
173+
1 : All motors return to zero position.
174+
list : Motor with corresponding ID failed to return to zero.
175+
"""
176+
return self._mesg(ProtocolCode.GO_ZERO, has_reply=True)
163177

164178
def get_angle(self, joint_id):
165179
"""Get single joint angle
@@ -191,3 +205,17 @@ def set_encoder(self, joint_id, encoder):
191205
# class_name=self.__class__.__name__, id=joint_id, encoder=encoder
192206
# )
193207
return self._mesg(ProtocolCode.SET_ENCODER, joint_id, [encoder])
208+
209+
def servo_restore(self, joint_id):
210+
"""Abnormal recovery of joints
211+
212+
Args:
213+
joint_id (int): Joint ID.
214+
arm : 1 ~ 7
215+
waist : 13
216+
All joints: 254
217+
"""
218+
self.calibration_parameters(
219+
class_name=self.__class__.__name__, servo_restore=joint_id
220+
)
221+
self._mesg(ProtocolCode.SERVO_RESTORE, joint_id)

‎pymycobot/common.py‎

Lines changed: 17 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -168,6 +168,7 @@ class ProtocolCode(object):
168168
GET_SERVO_STATUS = 0xE4
169169
GET_SERVO_TEMPS = 0xE5
170170
GET_SERVO_LASTPDI = 0xE6
171+
SERVO_RESTORE = 0xE7
171172

172173
GET_BASE_COORDS = 0xF0
173174
BASE_TO_SINGLE_COORDS = 0xF1
@@ -211,6 +212,7 @@ class ProtocolCode(object):
211212
LOCK_SERVOS = "M18"
212213
GET_CURRENT_COORD_INFO = "M114"
213214
GET_BACK_ZERO_STATUS = "M119"
215+
IS_MOVING_END = "M9"
214216

215217

216218
class DataProcessor(object):
@@ -313,7 +315,7 @@ def _process_received(self, data, genre, arm=6):
313315
header_i, header_j = 0, 1
314316
while header_j < data_len - 4:
315317
if self._is_frame_header(data, header_i, header_j):
316-
if arm == 6:
318+
if arm in [6, 7]:
317319
cmd_id = data[header_i + 3]
318320
elif arm == 12:
319321
cmd_id = data[header_i + 4]
@@ -324,7 +326,7 @@ def _process_received(self, data, genre, arm=6):
324326
header_j += 1
325327
else:
326328
return []
327-
if arm == 6:
329+
if arm in [6, 7]:
328330
data_len = data[header_i + 2] - 2
329331
elif arm == 12:
330332
data_len = data[header_i + 3] - 2
@@ -337,7 +339,7 @@ def _process_received(self, data, genre, arm=6):
337339
data_pos = header_i + 5
338340
data_len -= 1
339341
else:
340-
if arm == 6:
342+
if arm in [6, 7]:
341343
data_pos = header_i + 4
342344
elif arm == 12:
343345
data_pos = header_i + 5
@@ -346,11 +348,11 @@ def _process_received(self, data, genre, arm=6):
346348

347349
# process valid data
348350
res = []
349-
if genre in [ProtocolCode.GET_SERVO_VOLTAGES, ProtocolCode.GET_SERVO_STATUS, ProtocolCode.GET_SERVO_TEMPS]:
351+
if genre in [ProtocolCode.GET_SERVO_VOLTAGES, ProtocolCode.GET_SERVO_STATUS, ProtocolCode.GET_SERVO_TEMPS, ProtocolCode.GO_ZERO]:
350352
for i in valid_data:
351353
res.append(i)
352-
return res
353-
if data_len in [6, 8, 12, 14, 24, 60]:
354+
return res
355+
if data_len in [6, 8, 12, 14, 24, 26, 60]:
354356
for header_i in range(0, len(valid_data), 2):
355357
one = valid_data[header_i : header_i + 2]
356358
res.append(self._decode_int16(one))
@@ -423,13 +425,15 @@ def write(self, command, method=None):
423425
self._serial_port.flush()
424426

425427

426-
def read(self, genre, method=None):
428+
def read(self, genre, method=None, command=None):
427429
datas = b""
428430
data_len = -1
429431
k = 0
430432
pre = 0
431433
t = time.time()
432434
wait_time = 0.1
435+
if genre == ProtocolCode.GO_ZERO:
436+
wait_time = 120
433437
if method is not None:
434438
if genre == 177:
435439
while True:
@@ -462,6 +466,12 @@ def read(self, genre, method=None):
462466
k += 1
463467
if data_len == 1 and data == b"\xfa":
464468
datas += data
469+
if [i for i in datas] == command:
470+
datas = b''
471+
data_len = -1
472+
k = 0
473+
pre = 0
474+
continue
465475
break
466476
elif len(datas) == 2:
467477
data_len = struct.unpack("b", data)[0]

‎pymycobot/error.py‎

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -252,6 +252,9 @@ def calibration_parameters(**kwargs):
252252
elif parameter == 'value':
253253
if value < 1 or value > 32000:
254254
raise CobotXDataException("The angle value is 1 ~ 32000, but received {}".format(value))
255+
elif parameter == "servo_restore":
256+
if value not in [1,2,3,4,5,6,7,13,254]:
257+
raise CobotXDataException("The joint_id should be in [1,2,3,4,5,6,7,13,254], but received {}".format(value))
255258

256259
elif class_name == "MyAgv":
257260
for parameter in parameter_list[1:]:

‎pymycobot/generate.py‎

Lines changed: 4 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -425,7 +425,7 @@ def stop(self):
425425
"""Stop moving"""
426426
return self._mesg(ProtocolCode.STOP)
427427

428-
def set_encoder(self, joint_id, encoder):
428+
def set_encoder(self, joint_id, encoder, speed):
429429
"""Set a single joint rotation to the specified potential value.
430430
431431
Args:
@@ -435,9 +435,10 @@ def set_encoder(self, joint_id, encoder):
435435
for mycobot gripper: Joint id 7
436436
for myArm: Joint id 1 - 7.
437437
encoder: The value of the set encoder.
438+
speed : 1 - 100
438439
"""
439-
self.calibration_parameters(class_name = self.__class__.__name__, encode_id = joint_id, encoder = encoder)
440-
return self._mesg(ProtocolCode.SET_ENCODER, joint_id, [encoder])
440+
self.calibration_parameters(class_name = self.__class__.__name__, encode_id = joint_id, encoder = encoder, speed=speed)
441+
return self._mesg(ProtocolCode.SET_ENCODER, joint_id, [encoder], speed)
441442

442443
def get_encoder(self, joint_id):
443444
"""Obtain the specified joint potential value.

‎pymycobot/myagv.py‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -99,7 +99,7 @@ def _mesg(self, genre, *args, **kwargs):
9999
has_reply: Whether there is a return value to accept.
100100
"""
101101
has_reply = kwargs.get("has_reply", None)
102-
real_command = self._process_data_command(genre, args)
102+
real_command = self._process_data_command(genre, self.__class__.__name__, args)
103103
command = [
104104
ProtocolCode.HEADER.value,
105105
ProtocolCode.HEADER.value,

‎pymycobot/myarm.py‎

Lines changed: 6 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -80,13 +80,12 @@ def _mesg(self, genre, *args, **kwargs):
8080
"""
8181
real_command, has_reply = super(
8282
MyArm, self)._mesg(genre, *args, **kwargs)
83-
self._write(self._flatten(real_command))
83+
command = self._flatten(real_command)
84+
self._write(command)
8485

8586
if has_reply:
86-
data = self._read(genre)
87-
if genre == ProtocolCode.SET_SSID_PWD:
88-
return None
89-
res = self._process_received(data, genre)
87+
data = self._read(genre, command=command)
88+
res = self._process_received(data, genre, arm=7)
9089
if genre in [
9190
ProtocolCode.ROBOT_VERSION,
9291
ProtocolCode.GET_ROBOT_ID,
@@ -141,6 +140,8 @@ def _mesg(self, genre, *args, **kwargs):
141140
else:
142141
r.append(self._int2angle(res[index]))
143142
return r
143+
elif genre == ProtocolCode.GET_SOLUTION_ANGLES:
144+
return self._int2angle(res[0])
144145
else:
145146
return res
146147
return None

‎pymycobot/ultraArm.py‎

Lines changed: 13 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -125,6 +125,11 @@ def _request(self, flag=""):
125125
print(e)
126126
count += 1
127127
continue
128+
elif flag == 'isStop':
129+
if "Moving end" in data:
130+
return 1
131+
else:
132+
return 0
128133
elif flag == None:
129134
return 0
130135

@@ -584,3 +589,11 @@ def close(self):
584589

585590
def open(self):
586591
self._serial_port.open()
592+
593+
def is_moving_end(self):
594+
"""Get the current state of all home switches."""
595+
command = ProtocolCode.IS_MOVING_END + ProtocolCode.END
596+
self._serial_port.write(command.encode())
597+
self._serial_port.flush()
598+
self._debug(command)
599+
return self._request("isStop")

0 commit comments

Comments
 (0)