11# coding=utf-8
2- from pymycobot import MyArm
2+
3+ import time
4+
5+ from pymycobot .generate import CommandGenerator
36from pymycobot .common import ProtocolCode , write , read
7+ from pymycobot .error import calibration_parameters
48
59
6- class CobotX (MyArm ):
10+ class CobotX (CommandGenerator ):
11+ _write = write
12+ _read = read
713 def __init__ (self , port , baudrate = "115200" , timeout = 0.1 , debug = False ):
8- super ().__init__ (port , baudrate , timeout , debug )
14+ """
15+ Args:
16+ port : port string
17+ baudrate : baud rate string, default '115200'
18+ timeout : default 0.1
19+ debug : whether show debug info
20+ """
21+ super (CobotX , self ).__init__ (debug )
22+ self .calibration_parameters = calibration_parameters
23+ import serial
24+
25+ self ._serial_port = serial .Serial ()
26+ self ._serial_port .port = port
27+ self ._serial_port .baudrate = baudrate
28+ self ._serial_port .timeout = timeout
29+ self ._serial_port .rts = False
30+ self ._serial_port .open ()
931
32+
33+ def _mesg (self , genre , * args , ** kwargs ):
34+ """
35+
36+ Args:
37+ genre: command type (Command)
38+ *args: other data.
39+ It is converted to octal by default.
40+ If the data needs to be encapsulated into hexadecimal,
41+ the array is used to include them. (Data cannot be nested)
42+ **kwargs: support `has_reply`
43+ has_reply: Whether there is a return value to accept.
44+ """
45+ real_command , has_reply = super (CobotX , self )._mesg (genre , * args , ** kwargs )
46+ self ._write (self ._flatten (real_command ))
47+
48+ if has_reply :
49+ data = self ._read (genre )
50+ if genre == ProtocolCode .SET_SSID_PWD :
51+ return None
52+ res = self ._process_received (data , genre )
53+ if genre in [
54+ ProtocolCode .ROBOT_VERSION ,
55+ ProtocolCode .GET_ROBOT_ID ,
56+ ProtocolCode .IS_POWER_ON ,
57+ ProtocolCode .IS_CONTROLLER_CONNECTED ,
58+ ProtocolCode .IS_PAUSED , # TODO have bug: return b''
59+ ProtocolCode .IS_IN_POSITION ,
60+ ProtocolCode .IS_MOVING ,
61+ ProtocolCode .IS_SERVO_ENABLE ,
62+ ProtocolCode .IS_ALL_SERVO_ENABLE ,
63+ ProtocolCode .GET_SERVO_DATA ,
64+ ProtocolCode .GET_DIGITAL_INPUT ,
65+ ProtocolCode .GET_GRIPPER_VALUE ,
66+ ProtocolCode .IS_GRIPPER_MOVING ,
67+ ProtocolCode .GET_SPEED ,
68+ ProtocolCode .GET_ENCODER ,
69+ ProtocolCode .GET_BASIC_INPUT ,
70+ ProtocolCode .GET_TOF_DISTANCE ,
71+ ProtocolCode .GET_END_TYPE ,
72+ ProtocolCode .GET_MOVEMENT_TYPE ,
73+ ProtocolCode .GET_REFERENCE_FRAME ,
74+ ProtocolCode .GET_FRESH_MODE ,
75+ ProtocolCode .GET_GRIPPER_MODE ,
76+ ProtocolCode .SET_SSID_PWD ,
77+ ProtocolCode .COBOTX_IS_GO_ZERO ,
78+ ]:
79+ return self ._process_single (res )
80+ elif genre in [ProtocolCode .GET_ANGLES ]:
81+ return [self ._int2angle (angle ) for angle in res ]
82+ elif genre in [
83+ ProtocolCode .GET_COORDS ,
84+ ProtocolCode .GET_TOOL_REFERENCE ,
85+ ProtocolCode .GET_WORLD_REFERENCE ,
86+ ]:
87+ if res :
88+ r = []
89+ for idx in range (3 ):
90+ r .append (self ._int2coord (res [idx ]))
91+ for idx in range (3 , 6 ):
92+ r .append (self ._int2angle (res [idx ]))
93+ return r
94+ else :
95+ return res
96+ elif genre in [ProtocolCode .GET_SERVO_VOLTAGES ]:
97+ return [self ._int2coord (angle ) for angle in res ]
98+ elif genre in [
99+ ProtocolCode .GET_JOINT_MAX_ANGLE ,
100+ ProtocolCode .GET_JOINT_MIN_ANGLE ,
101+ ]:
102+ return self ._int2coord (res [0 ])
103+ elif genre == ProtocolCode .GET_ANGLES_COORDS :
104+ r = []
105+ for index in range (len (res )):
106+ if index < 7 :
107+ r .append (self ._int2angle (res [index ]))
108+ elif index < 10 :
109+ r .append (self ._int2coord (res [index ]))
110+ else :
111+ r .append (self ._int2angle (res [index ]))
112+ return r
113+ else :
114+ return res
115+ return None
116+
10117 def set_solution_angles (self , angle , speed ):
11118 """Set zero space deflection angle value
12-
119+
13120 Args:
14- angle: Angle of joint 1.
121+ angle: Angle of joint 1. The angle range is -90 ~ 90
15122 speed: 1 - 100.
16123 """
17- return self ._mesg (ProtocolCode .COBOTX_SET_SOLUTION_ANGLES , [self ._angle2int (angle )], speed )
18-
124+ self .calibration_parameters (
125+ class_name = self .__class__ .__name__ , speed = speed , solution_angle = angle
126+ )
127+ return self ._mesg (
128+ ProtocolCode .COBOTX_SET_SOLUTION_ANGLES , [self ._angle2int (angle )], speed
129+ )
130+
19131 def get_solution_angles (self ):
20132 """Get zero space deflection angle value"""
21- return self ._mesg (ProtocolCode .COBOTX_GET_SOLUTION_ANGLES , has_reply = True )
133+ return self ._mesg (ProtocolCode .COBOTX_GET_SOLUTION_ANGLES , has_reply = True )
134+
135+ def write_move_c (self , transpoint , endpoint , speed ):
136+ """_summary_
137+
138+ Args:
139+ transpoint (list): Arc passing point coordinates
140+ endpoint (list): Arc end point coordinates
141+ speed (int): 1 ~ 100
142+ """
143+ start = []
144+ end = []
145+ for index in range (6 ):
146+ if index < 3 :
147+ start .append (self ._coord2int (transpoint [index ]))
148+ end .append (self ._coord2int (endpoint [index ]))
149+ else :
150+ start .append (self ._angle2int (transpoint [index ]))
151+ end .append (self ._angle2int (endpoint [index ]))
152+ return self ._mesg (ProtocolCode .WRITE_MOVE_C , start , end , speed )
153+
154+ def focus_all_servos (self ):
155+ """Lock all joints"""
156+ return self ._mesg (ProtocolCode .FOCUS_ALL_SERVOS )
157+
158+ def go_zero (self ):
159+ """go zero"""
160+ return self ._mesg (ProtocolCode .GO_ZERO )
161+
162+ def get_angle (self , joint_id ):
163+ """Get single joint angle
164+
165+ Args:
166+ joint_id (int): 1 ~ 7 or 11 ~ 13.
167+ """
168+ self .calibration_parameters (class_name = self .__class__ .__name__ , id = joint_id )
169+ return self ._mesg (ProtocolCode .COBOTX_GET_ANGLE , joint_id , has_reply = True )
170+
171+ def is_gone_zero (self ):
172+ """Check if it has returned to zero
173+
174+ Return:
175+ 1 : Return to zero successfully.
176+ 0 : Returning to zero.
177+ """
178+ return self ._mesg (ProtocolCode .COBOTX_IS_GO_ZERO , has_reply = True )
179+
180+ def set_encoder (self , joint_id , encoder ):
181+ """Set a single joint rotation to the specified potential value.
182+
183+ Args:
184+ joint_id (int): Joint id 1 - 7.
185+ encoder: The value of the set encoder.
186+ """
187+ # TODO CobotX此接口待定
188+ # self.calibration_parameters(
189+ # class_name=self.__class__.__name__, id=joint_id, encoder=encoder
190+ # )
191+ return self ._mesg (ProtocolCode .SET_ENCODER , joint_id , [encoder ])
0 commit comments