Skip to content

Commit 73637f3

Browse files
author
vinman
committed
add check verify and fix some bugs
1 parent ac9587c commit 73637f3

File tree

11 files changed

+116
-27
lines changed

11 files changed

+116
-27
lines changed

‎example/wrapper/common/5006-set_cgpio_dialog_analog.py‎

Lines changed: 7 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -37,6 +37,13 @@
3737
for i in range(8):
3838
code = arm.set_cgpio_digital(i, value)
3939
print('set_cgpio_digital({}, {}), code={}'.format(i, value, code))
40+
time.sleep(0.5)
41+
42+
value = 0
43+
for i in range(8):
44+
code = arm.set_cgpio_digital(i, value)
45+
print('set_cgpio_digital({}, {}), code={}'.format(i, value, code))
46+
time.sleep(0.5)
4047

4148
value = 2.6
4249
code = arm.set_cgpio_analog(0, value)

‎example/wrapper/common/5007-set_cgpio_input_output_function.py‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -38,6 +38,7 @@
3838
for i in range(4):
3939
code = arm.set_cgpio_digital_input_function(i, value)
4040
print('set_cgpio_digital_input_function({}, {}), code={}'.format(i, value, code))
41+
# Reset: 255
4142
for i in range(4):
4243
code = arm.set_cgpio_digital_input_function(i, 255)
4344
print('set_cgpio_digital_input_function({}, {}), code={}'.format(i, value, code))
@@ -47,6 +48,7 @@
4748
for i in range(4, 8):
4849
code = arm.set_cgpio_digital_output_function(i, value)
4950
print('set_cgpio_digital_output_function({}, {}), code={}'.format(i, value, code))
51+
# Reset: 255
5052
for i in range(4, 8):
5153
code = arm.set_cgpio_digital_output_function(i, 255)
5254
print('set_cgpio_digital_output_function({}, {}), code={}'.format(i, value, code))

‎example/wrapper/common/5008-get_cgpio_state.py‎

Lines changed: 10 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -36,11 +36,16 @@
3636

3737
code, states = arm.get_cgpio_state()
3838
print('get_cgpio_state, code={}'.format(code))
39-
print('state: {} {}'.format(states[0], states[1]))
40-
print('digit_io: {:x} {:x} {:x} {:x}'.format(*states[2:6]))
41-
print('analog: {:f} {:f} {:f} {:f}'.format(*states[6:10]))
42-
print('input_conf: {}'.format(states[10]))
43-
print('output_conf: {}'.format(states[11]))
39+
print('GPIO state: {}'.format(states[0]))
40+
print('GPIO error code: {}'.format(states[1]))
41+
print('Digital->Input->FunctionalIO: {}'.format([states[2] >> i & 0x0001 for i in range(8)]))
42+
print('Digital->Input->ConfiguringIO: {}'.format([states[3] >> i & 0x0001 for i in range(8)]))
43+
print('Digital->Output->FunctionalIO: {}'.format([states[4] >> i & 0x0001 for i in range(8)]))
44+
print('Digital->Output->ConfiguringIO: {}'.format([states[5] >> i & 0x0001 for i in range(8)]))
45+
print('Analog->Input: {}'.format(states[6:8]))
46+
print('Analog->Output: {}'.format(states[8:10]))
47+
print('Digital->Input->Conf: {}'.format(states[10]))
48+
print('Digital->Output->Conf: {}'.format(states[11]))
4449

4550

4651

‎example/wrapper/robot.conf‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,2 +1,2 @@
11
[xArm]
2-
ip = 192.168.1.190
2+
ip = 192.168.1.200

‎xarm/core/config/x_config.py‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -147,6 +147,7 @@ class SocketConf:
147147
class UxbusReg:
148148
GET_VERSION = 1
149149
GET_ROBOT_SN = 2
150+
CHECK_VERIFY = 3
150151
SHUTDOWN_SYSTEM = 10
151152
MOTION_EN = 11
152153
SET_STATE = 12

‎xarm/core/wrapper/uxbus_cmd.py‎

Lines changed: 11 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -47,6 +47,13 @@ def set_nu8(self, funcode, datas, num):
4747
return [XCONF.UxbusState.ERR_NOTTCP]
4848
return self.send_pend(funcode, 0, XCONF.UxbusConf.SET_TIMEOUT)
4949

50+
@lock_require
51+
def set_get_nu8(self, funcode, datas, num_send, num_get):
52+
ret = self.send_xbus(funcode, datas, num_send)
53+
if ret != 0:
54+
return [XCONF.UxbusState.ERR_NOTTCP]
55+
return self.send_pend(funcode, num_get, XCONF.UxbusConf.SET_TIMEOUT)
56+
5057
@lock_require
5158
def get_nu8(self, funcode, num):
5259
ret = self.send_xbus(funcode, 0, 0)
@@ -119,6 +126,10 @@ def get_version(self):
119126
def get_robot_sn(self):
120127
return self.get_nu8(XCONF.UxbusReg.GET_ROBOT_SN, 40)
121128

129+
def check_verification(self):
130+
# txdata = signature, 175: signature length if use 14-character SN for plain text, do not miss '\n's
131+
return self.get_nu8(XCONF.UxbusReg.CHECK_VERIFY, 1)
132+
122133
def shutdown_system(self, value):
123134
txdata = [value]
124135
return self.set_nu8(XCONF.UxbusReg.SHUTDOWN_SYSTEM, txdata, 1)

‎xarm/core/wrapper/uxbus_cmd_tcp.py‎

Lines changed: 13 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -76,6 +76,10 @@ def send_pend(self, funcode, num, timeout):
7676
times -= 1
7777
rx_data = self.arm_port.read()
7878
if rx_data != -1 and len(rx_data) > 7:
79+
# print('recv:', end=' ')
80+
# for i in range(len(rx_data)):
81+
# print(hex(rx_data[i]), end=',')
82+
# print()
7983
ret[0] = self.check_xbus_prot(rx_data, funcode)
8084
if ret[0] in [0, XCONF.UxbusState.ERR_CODE, XCONF.UxbusState.WAR_CODE]:
8185
if num == -1:
@@ -96,10 +100,16 @@ def send_xbus(self, funcode, datas, num):
96100
send_data += convert.u16_to_bytes(self.prot_flag)
97101
send_data += convert.u16_to_bytes(num + 1)
98102
send_data += bytes([funcode])
99-
for i in range(num):
100-
send_data += bytes([datas[i]])
101-
103+
if type(datas) == str:
104+
send_data += datas.encode()
105+
else:
106+
for i in range(num):
107+
send_data += bytes([datas[i]])
102108
self.arm_port.flush()
109+
# print('send:', end=' ')
110+
# for i in range(len(send_data)):
111+
# print(hex(send_data[i]), end=',')
112+
# print()
103113
ret = self.arm_port.write(send_data)
104114
if ret != 0:
105115
return -1

‎xarm/version.py‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1 +1 @@
1-
__version__ = '1.0.0'
1+
__version__ = '1.0.1'

‎xarm/wrapper/xarm_api.py‎

Lines changed: 11 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -749,6 +749,17 @@ def get_version(self):
749749
"""
750750
return self._arm.get_version()
751751

752+
def check_verification(self):
753+
"""
754+
check verification
755+
:return: tuple((code, status)), only when code is 0, the returned result is correct.
756+
code: See the API code documentation for details.
757+
status:
758+
0: verified
759+
other: not verified
760+
"""
761+
return self._arm.check_verification()
762+
752763
def shutdown_system(self, value=1):
753764
"""
754765
Shutdown the xArm controller system

‎xarm/x3/__init__.py‎

Lines changed: 51 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -109,6 +109,8 @@ def __init__(self, port=None, is_radian=False, do_not_open=False, **kwargs):
109109
self._minor_version_number = 0 # 固件次版本号
110110
self._revision_version_number = 0 # 固件修正版本号
111111

112+
self._is_set_move = False
113+
112114
Events.__init__(self)
113115
if not do_not_open:
114116
self.connect()
@@ -358,7 +360,7 @@ def _check_version(self):
358360
self.get_version()
359361
time.sleep(0.1)
360362
count -= 1
361-
pattern = re.compile(r'.*[vV](\d+)\.(\d+)\.(\d+)$')
363+
pattern = re.compile(r'.*[vV](\d+)\.(\d+)\.(\d+)')
362364
m = re.match(pattern, self._version)
363365
if m:
364366
self._major_version_number, self._minor_version_number, self._revision_version_number = map(int,
@@ -969,12 +971,35 @@ def disconnect(self):
969971
pass
970972
self._report_connect_changed_callback(False, False)
971973

974+
def _sync_tcp(self, index=None):
975+
if not self._stream_report or not self._stream_report.connected:
976+
self.get_position()
977+
self.get_servo_angle()
978+
self._last_angles = self._angles
979+
if index is None:
980+
self._last_position = self._position
981+
elif isinstance(index, int) and 0 <= index < 6:
982+
self._last_position[index] = self._position[index]
983+
print('=============sync_tcp: index={}'.format(index))
984+
985+
def _sync_joints(self, index=None):
986+
if not self._stream_report or not self._stream_report.connected:
987+
self.get_position()
988+
self.get_servo_angle()
989+
self._last_position = self._position
990+
if index is None:
991+
self._last_angles = self._angles
992+
elif isinstance(index, int) and 0 <= index < 7:
993+
self._last_angles[index] = self._angles[index]
994+
print('**********sync_joint: index={}'.format(index))
995+
972996
def _sync(self):
973997
if not self._stream_report or not self._stream_report.connected:
974998
self.get_position()
975999
self.get_servo_angle()
9761000
self._last_position = self._position
9771001
self._last_angles = self._angles
1002+
print('>>>>>>>>sync_all')
9781003

9791004
class _WaitMove:
9801005
def __init__(self, owner, timeout):
@@ -1148,10 +1173,10 @@ def set_position(self, x=None, y=None, z=None, roll=None, pitch=None, yaw=None,
11481173
ret = self.arm_cmd.move_lineb(self._last_position, self._last_tcp_speed, self._last_tcp_acc, self._mvtime, radius)
11491174
else:
11501175
ret = self.arm_cmd.move_line(self._last_position, self._last_tcp_speed, self._last_tcp_acc, self._mvtime)
1151-
11521176
logger.info('API -> set_position -> ret={}, pos={}, radius={}, velo={}, acc={}'.format(
11531177
ret[0], self._last_position, radius, self._last_tcp_speed, self._last_tcp_acc
11541178
))
1179+
self._is_set_move = True
11551180
if wait and ret[0] in [0, XCONF.UxbusState.WAR_CODE, XCONF.UxbusState.ERR_CODE]:
11561181
if not self._enable_report:
11571182
warnings.warn('if you want to wait, please enable report')
@@ -1315,6 +1340,7 @@ def set_servo_angle(self, servo_id=None, angle=None, speed=None, mvacc=None, mvt
13151340
logger.info('API -> set_servo_angle -> ret={}, angles={}, velo={}, acc={}'.format(
13161341
ret[0], self._last_angles, self._last_joint_speed, self._last_joint_acc
13171342
))
1343+
self._is_set_move = True
13181344
if wait and ret[0] in [0, XCONF.UxbusState.WAR_CODE, XCONF.UxbusState.ERR_CODE]:
13191345
if not self._enable_report:
13201346
warnings.warn('if you want to wait, please enable report')
@@ -1342,6 +1368,7 @@ def set_servo_angle_j(self, angles, speed=None, mvacc=None, mvtime=None, is_radi
13421368
logger.debug('API -> set_servo_angle_j -> ret={}, angles={}, velo={}, acc={}'.format(
13431369
ret[0], _angles, self._last_joint_speed, self._last_joint_acc
13441370
))
1371+
self._is_set_move = True
13451372
return ret[0]
13461373

13471374
@xarm_is_ready(_type='set')
@@ -1392,6 +1419,7 @@ def move_circle(self, pose1, pose2, percent, speed=None, mvacc=None, mvtime=None
13921419
logger.info('API -> move_circle -> ret={}, pos1={}, pos2={}, percent={}%, velo={}, acc={}'.format(
13931420
ret[0], pose_1, pose_2, percent, self._last_tcp_speed, self._last_tcp_acc
13941421
))
1422+
self._is_set_move = True
13951423

13961424
if wait and ret[0] in [0, XCONF.UxbusState.WAR_CODE, XCONF.UxbusState.ERR_CODE]:
13971425
if not self._enable_report:
@@ -1425,9 +1453,11 @@ def move_gohome(self, speed=None, mvacc=None, mvtime=None, is_radian=None, wait=
14251453
logger.info('API -> move_gohome -> ret={}, velo={}, acc={}'.format(
14261454
ret[0], speed, mvacc
14271455
))
1456+
self._is_set_move = True
14281457
if ret[0] in [0, XCONF.UxbusState.WAR_CODE, XCONF.UxbusState.ERR_CODE]:
1429-
self._last_position = [201.5, 0, 140.5, -3.1415926, 0, 0]
1430-
self._last_angles = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
1458+
pass
1459+
# self._last_position = [201.5, 0, 140.5, -3.1415926, 0, 0]
1460+
# self._last_angles = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
14311461
if wait and ret[0] in [0, XCONF.UxbusState.WAR_CODE, XCONF.UxbusState.ERR_CODE]:
14321462
if not self._enable_report:
14331463
warnings.warn('if you want to wait, please enable report')
@@ -1536,6 +1566,7 @@ def set_servo_attach(self, servo_id=None):
15361566
logger.info('set_servo_attach--begin')
15371567
ret = self.motion_enable(servo_id=servo_id, enable=True)
15381568
self.set_state(0)
1569+
self._sync()
15391570
logger.info('set_servo_attach--end')
15401571
return ret
15411572

@@ -1548,6 +1579,7 @@ def set_servo_detach(self, servo_id=None):
15481579
assert isinstance(servo_id, int) and 1 <= servo_id <= 8, 'The value of parameter servo_id can only be 1-8.'
15491580
ret = self.arm_cmd.set_brake(servo_id, 1)
15501581
logger.info('API -> set_servo_detach -> ret={}'.format(ret[0]))
1582+
self._sync()
15511583
return ret[0]
15521584

15531585
@xarm_is_connected(_type='get')
@@ -1571,6 +1603,13 @@ def get_robot_sn(self):
15711603
pass
15721604
return ret[0], self._robot_sn
15731605

1606+
@xarm_is_connected(_type='get')
1607+
def check_verification(self):
1608+
ret = self.arm_cmd.check_verification()
1609+
if ret[0] in [0, XCONF.UxbusState.ERR_CODE, XCONF.UxbusState.WAR_CODE]:
1610+
ret[0] = 0
1611+
return ret[0], ret[1]
1612+
15741613
@xarm_is_connected(_type='set')
15751614
def shutdown_system(self, value=1):
15761615
ret = self.arm_cmd.shutdown_system(value)
@@ -1596,6 +1635,7 @@ def set_state(self, state=0):
15961635
# self._last_position[:6] = self.position
15971636
# self._last_angles = self.angles
15981637
self._sleep_finish_time = 0
1638+
# self._is_sync = False
15991639
self.get_state()
16001640
if self._state in [3, 4]:
16011641
if self._is_ready:
@@ -2018,12 +2058,14 @@ def emergency_stop(self):
20182058
while self.state != 4 and time.time() - start_time < 3:
20192059
self.set_state(4)
20202060
time.sleep(0.1)
2061+
self.set_state(0)
20212062
self._is_stop = True
2022-
start_time = time.time()
2023-
self.motion_enable(enable=True)
2024-
while self.state in [0, 3, 4] and time.time() - start_time < 3:
2025-
self.set_state(0)
2026-
time.sleep(0.1)
2063+
if not self._is_ready:
2064+
start_time = time.time()
2065+
self.motion_enable(enable=True)
2066+
while self.state in [0, 3, 4] and time.time() - start_time < 3:
2067+
self.set_state(0)
2068+
time.sleep(0.1)
20272069
self._sleep_finish_time = 0
20282070
logger.info('emergency_stop--end')
20292071

0 commit comments

Comments
 (0)