Skip to content

Commit 8375182

Browse files
committed
push code
1 parent 65b7716 commit 8375182

File tree

9 files changed

+184
-66
lines changed

9 files changed

+184
-66
lines changed

CHANGELOG.md

Lines changed: 5 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,10 @@
11
# ChangeLog for pymycobot
22

3+
## v3.2.4 (2023-11-1)
4+
5+
- release v3.2.4
6+
- ultraArm add sync function
7+
38
## v3.2.3 (2023-10-20)
49

510
- release v3.2.3

pymycobot/__init__.py

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -20,7 +20,7 @@
2020
from pymycobot.myarm import MyArm
2121
from pymycobot.myarmsocket import MyArmSocket
2222
from pymycobot.elephantrobot import ElephantRobot
23-
from pymycobot.cobotx import CobotX
23+
from pymycobot.mercury import Mercury
2424
from pymycobot.myagv import MyAgv
2525
from pymycobot.mecharmsocket import MechArmSocket
2626

@@ -41,7 +41,7 @@
4141
"MechArm",
4242
"MyArm",
4343
"ElephantRobot",
44-
"CobotX",
44+
"Mercury",
4545
"MyAgv",
4646
"MechArmSocket",
4747
"MyArmSocket"
@@ -52,7 +52,7 @@
5252
from pymycobot.mybuddyemoticon import MyBuddyEmoticon
5353
__all__.append("MyBuddyEmoticon")
5454

55-
__version__ = "3.2.3"
55+
__version__ = "3.2.4"
5656
__author__ = "Elephantrobotics"
5757
__email__ = "weiquan.xu@elephantrobotics.com"
5858
__git_url__ = "https://github.com/elephantrobotics/pymycobot"

pymycobot/common.py

Lines changed: 34 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -174,6 +174,10 @@ class ProtocolCode(object):
174174
SET_ERROR_DETECT_MODE = 0xE8
175175
GET_ERROR_DETECT_MODE = 0xE9
176176

177+
MERCURY_GET_BASE_COORDS = 0xF0
178+
MERCURY_SET_BASE_COORD = 0xF1
179+
MERCURY_SET_BASE_COORDS = 0xF2
180+
MERCURY_JOG_BASE_COORD = 0xF3
177181

178182
GET_BASE_COORDS = 0xF0
179183
BASE_TO_SINGLE_COORDS = 0xF1
@@ -229,7 +233,7 @@ def _encode_int16(self, data):
229233
if isinstance(data, int):
230234
return [
231235
ord(i) if isinstance(i, str) else i
232-
for i in list(struct.pack(">h", data))
236+
for i in list(struct.pack(">H", data))
233237
]
234238
else:
235239
res = []
@@ -255,7 +259,19 @@ def _int2angle(self, _int):
255259

256260
def _int2coord(self, _int):
257261
return round(_int / 10.0, 2)
258-
262+
263+
@classmethod
264+
def crc_check(cls, command):
265+
crc = 0xffff
266+
for index in range(len(command)):
267+
crc ^= command[index]
268+
for _ in range(8):
269+
if crc & 1 == 1:
270+
crc >>= 1
271+
crc ^= 0xA001
272+
else:
273+
crc >>= 1
274+
return cls._encode_int16(_, crc)
259275
# def encode_int16(self, data):
260276
# encoded_data = []
261277

@@ -283,7 +299,7 @@ def _process_data_command(self, genre, _class, args):
283299
processed_args = []
284300
for index in range(len(args)):
285301
if isinstance(args[index], list):
286-
if genre == ProtocolCode.SET_ENCODERS_DRAG and index == 0 and _class == "CobotX":
302+
if genre == ProtocolCode.SET_ENCODERS_DRAG and index == 0 and _class == "Mercury":
287303
for data in args[index]:
288304
byte_value = data.to_bytes(4, byteorder='big', signed=True)
289305
res = []
@@ -296,7 +312,7 @@ def _process_data_command(self, genre, _class, args):
296312
if isinstance(args[index], str):
297313
processed_args.append(ord(args[index]))
298314
else:
299-
if genre == ProtocolCode.SET_SERVO_DATA and _class == "CobotX" and index == 2:
315+
if genre == ProtocolCode.SET_SERVO_DATA and _class == "Mercury" and index == 2:
300316
byte_value = args[index].to_bytes(2, byteorder='big', signed=True)
301317
res = []
302318
for i in range(len(byte_value)):
@@ -324,7 +340,7 @@ def _process_received(self, data, genre, arm=6):
324340
header_i, header_j = 0, 1
325341
while header_j < data_len - 4:
326342
if self._is_frame_header(data, header_i, header_j):
327-
if arm in [6, 7]:
343+
if arm in [6, 7, 14]:
328344
cmd_id = data[header_i + 3]
329345
elif arm == 12:
330346
cmd_id = data[header_i + 4]
@@ -339,6 +355,9 @@ def _process_received(self, data, genre, arm=6):
339355
data_len = data[header_i + 2] - 2
340356
elif arm == 12:
341357
data_len = data[header_i + 3] - 2
358+
elif arm == 14:
359+
data_len = data[header_i + 2] - 3
360+
342361
unique_data = [ProtocolCode.GET_BASIC_INPUT, ProtocolCode.GET_DIGITAL_INPUT]
343362

344363
if cmd_id in unique_data:
@@ -348,7 +367,7 @@ def _process_received(self, data, genre, arm=6):
348367
data_pos = header_i + 5
349368
data_len -= 1
350369
else:
351-
if arm in [6, 7]:
370+
if arm in [6, 7, 14]:
352371
data_pos = header_i + 4
353372
elif arm == 12:
354373
data_pos = header_i + 5
@@ -360,7 +379,7 @@ def _process_received(self, data, genre, arm=6):
360379
if genre in [ProtocolCode.GET_SERVO_VOLTAGES, ProtocolCode.GET_SERVO_STATUS, ProtocolCode.GET_SERVO_TEMPS, ProtocolCode.GO_ZERO]:
361380
for i in valid_data:
362381
res.append(i)
363-
return res
382+
return res
364383
if data_len in [6, 8, 12, 14, 16, 24, 26, 60]:
365384
for header_i in range(0, len(valid_data), 2):
366385
one = valid_data[header_i : header_i + 2]
@@ -405,7 +424,6 @@ def _process_received(self, data, genre, arm=6):
405424
data1 = self._decode_int8(valid_data[i : i + 1])
406425
res.append(0xFF & data1 if data1 < 0 else data1)
407426
return res
408-
print(valid_data)
409427
res.append(self._decode_int8(valid_data))
410428
if genre == ProtocolCode.GET_ACCEI_DATA:
411429
for i in range(len(res)):
@@ -442,7 +460,7 @@ def write(self, command, method=None):
442460
self._serial_port.flush()
443461

444462

445-
def read(self, genre, method=None, command=None):
463+
def read(self, genre, method=None, command=None, _class=None):
446464
datas = b""
447465
data_len = -1
448466
k = 0
@@ -488,6 +506,13 @@ def read(self, genre, method=None, command=None):
488506
while True and time.time() - t < wait_time:
489507
data = self._serial_port.read()
490508
k += 1
509+
if _class == "Mercury":
510+
if data_len == 3:
511+
datas += data
512+
crc = self._serial_port.read(2)
513+
if DataProcessor.crc_check(datas) == [v for v in crc]:
514+
datas+=crc
515+
break
491516
if data_len == 1 and data == b"\xfa":
492517
datas += data
493518
if [i for i in datas] == command:

pymycobot/error.py

Lines changed: 16 additions & 16 deletions
Original file line numberDiff line numberDiff line change
@@ -16,7 +16,7 @@
1616
class MyCobotDataException(Exception):
1717
pass
1818

19-
class CobotXDataException(Exception):
19+
class MercuryDataException(Exception):
2020
pass
2121

2222
class MyAgvDataException(Exception):
@@ -196,29 +196,29 @@ def calibration_parameters(**kwargs):
196196
robot_limit = json.load(f)
197197
parameter_list = list(kwargs.keys())
198198
class_name = kwargs.get("class_name", None)
199-
if class_name == "CobotX":
199+
if class_name == "Mercury":
200200
for parameter in parameter_list[1:]:
201201
value = kwargs.get(parameter, None)
202202
if parameter == 'id' and value not in robot_limit[class_name][parameter]:
203-
check_id(value, robot_limit[class_name][parameter], CobotXDataException)
203+
check_id(value, robot_limit[class_name][parameter], MercuryDataException)
204204
elif parameter == 'angle':
205205
id = kwargs.get('id', None)
206206
if id in [11,12,13]:
207207
index = robot_limit[class_name]['id'][id-4] - 4
208208
else:
209209
index = robot_limit[class_name]['id'][id-1] - 1
210210
if value < robot_limit[class_name]["angles_min"][index] or value > robot_limit[class_name]["angles_max"][index]:
211-
raise CobotXDataException(
211+
raise MercuryDataException(
212212
"angle value not right, should be {0} ~ {1}, but received {2}".format(
213213
robot_limit[class_name]["angles_min"][index], robot_limit[class_name]["angles_max"][index], value
214214
)
215215
)
216216
# elif parameter == 'angles':
217217
# if len(value) not in [7, 10]:
218-
# raise CobotXDataException("The length of `angles` must be 7 or 10.")
218+
# raise MercuryDataException("The length of `angles` must be 7 or 10.")
219219
# for idx, angle in enumerate(value):
220220
# if not MIN_ANGLE <= angle <= MAX_ANGLE:
221-
# raise CobotXDataException(
221+
# raise MercuryDataException(
222222
# "Has invalid angle value, error on index {0}. angle should be {1} ~ {2}.".format(
223223
# idx, MIN_ANGLE, MAX_ANGLE
224224
# )
@@ -228,43 +228,43 @@ def calibration_parameters(**kwargs):
228228

229229
index = kwargs.get('id', None) - 1
230230
if value < robot_limit[class_name]["coords_min"][index] or value > robot_limit[class_name]["coords_max"][index]:
231-
raise CobotXDataException(
231+
raise MercuryDataException(
232232
"coord value not right, should be {0} ~ {1}, but received {2}".format(
233233
robot_limit[class_name]["coords_min"][index], robot_limit[class_name]["coords_max"][index], value
234234
)
235235
)
236236
elif parameter == 'coords':
237-
check_coords(value, robot_limit, class_name, CobotXDataException)
237+
check_coords(value, robot_limit, class_name, MercuryDataException)
238238

239239
elif parameter == 'speed' and not 1 <= value <= 100:
240-
raise CobotXDataException(
240+
raise MercuryDataException(
241241
"speed value not right, should be 1 ~ 100, the error speed is %s"
242242
% value
243243
)
244244

245245
elif parameter == 'rgb':
246-
check_rgb_value(value, CobotXDataException, class_name)
246+
check_rgb_value(value, MercuryDataException, class_name)
247247
# if direction is not None:
248248
elif parameter in ['direction', 'flag', 'mode']:
249249
if value not in [0, 1]:
250-
raise CobotXDataException("{} only supports 0 or 1, but received {}".format(parameter, value))
250+
raise MercuryDataException("{} only supports 0 or 1, but received {}".format(parameter, value))
251251

252252
elif parameter == 'coord_id':
253253
if value < 1 or value > 6:
254-
raise CobotXDataException("coord_id only supports 1 ~ 6, but received {}".format(value))
254+
raise MercuryDataException("coord_id only supports 1 ~ 6, but received {}".format(value))
255255

256256
elif parameter == 'solution_angle':
257257
if value > 90 or value < -90:
258-
raise CobotXDataException("The angle range is -90° ~ 90°, but received {}".format(value))
258+
raise MercuryDataException("The angle range is -90° ~ 90°, but received {}".format(value))
259259
elif parameter == 'address':
260260
if value < 32 or value > 34:
261-
raise CobotXDataException("The angle address is 32 ~ 34, but received {}".format(value))
261+
raise MercuryDataException("The angle address is 32 ~ 34, but received {}".format(value))
262262
elif parameter == 'value':
263263
if value < 1 or value > 32000:
264-
raise CobotXDataException("The angle value is 1 ~ 32000, but received {}".format(value))
264+
raise MercuryDataException("The angle value is 1 ~ 32000, but received {}".format(value))
265265
elif parameter == "servo_restore":
266266
if value not in [1,2,3,4,5,6,7,13,254]:
267-
raise CobotXDataException("The joint_id should be in [1,2,3,4,5,6,7,13,254], but received {}".format(value))
267+
raise MercuryDataException("The joint_id should be in [1,2,3,4,5,6,7,13,254], but received {}".format(value))
268268

269269
elif class_name == "MyAgv":
270270
for parameter in parameter_list[1:]:

pymycobot/generate.py

Lines changed: 15 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -114,16 +114,22 @@ def _mesg(self, genre, *args, **kwargs):
114114
command_data = [command_data[0]] + self._encode_int16(command_data[1]*10)
115115
elif genre == 115:
116116
command_data = [command_data[1],command_data[3]]
117-
118117
LEN = len(command_data) + 2
118+
119119
command = [
120-
ProtocolCode.HEADER,
121-
ProtocolCode.HEADER,
122-
LEN,
123-
genre,
124-
command_data,
125-
ProtocolCode.FOOTER,
126-
]
120+
ProtocolCode.HEADER,
121+
ProtocolCode.HEADER,
122+
LEN,
123+
genre,
124+
]
125+
if command_data:
126+
command.extend(command_data)
127+
if self.__class__.__name__ == "Mercury":
128+
command[2] += 1
129+
command.extend(self.crc_check(command))
130+
131+
else:
132+
command.append(ProtocolCode.FOOTER)
127133

128134
real_command = self._flatten(command)
129135
has_reply = kwargs.get("has_reply", False)
@@ -761,7 +767,7 @@ def set_basic_output(self, pin_no, pin_signal):
761767
pin_signal: 0 / 1
762768
"""
763769
self.calibration_parameters(class_name = self.__class__.__name__, pin_signal=pin_signal)
764-
return self._mesg(ProtocolCode.SET_BASIC_OUTPUT, pin_no, pin_signal, has_reply=True)
770+
return self._mesg(ProtocolCode.SET_BASIC_OUTPUT, pin_no, pin_signal)
765771

766772
def get_basic_input(self, pin_no):
767773
"""Get basic input for M5 version.

0 commit comments

Comments
 (0)