From f1b46923fe3aeb3015042cdcb3267f82eb03846e Mon Sep 17 00:00:00 2001 From: Mrkun5018 <2220138602@qq.com> Date: Wed, 12 Feb 2025 18:22:37 +0800 Subject: [PATCH 01/19] add MyCobot 280 X5PI API --- pymycobot/__init__.py | 4 +- pymycobot/common.py | 5 +- pymycobot/mycobot280x5pi.py | 837 ++++++++++++++++++++++++++++++++++++ 3 files changed, 844 insertions(+), 2 deletions(-) create mode 100644 pymycobot/mycobot280x5pi.py diff --git a/pymycobot/__init__.py b/pymycobot/__init__.py index ad9afe50..6369c45c 100644 --- a/pymycobot/__init__.py +++ b/pymycobot/__init__.py @@ -4,6 +4,7 @@ import datetime import sys from pymycobot.mycobot280 import MyCobot280 +from pymycobot.mycobot280x5pi import MyCobot280X5PI from pymycobot.mypalletizer260 import MyPalletizer260 from pymycobot.mecharm270 import MechArm270 from pymycobot.mycobot280socket import MyCobot280Socket @@ -77,7 +78,8 @@ "MyCobot320Socket", "MyArmMControl", "ChassisControl", - "ConveyorAPI" + "ConveyorAPI", + "MyCobot280X5PI" ] if sys.platform == "linux": diff --git a/pymycobot/common.py b/pymycobot/common.py index 3ae52e17..d32b9b7e 100644 --- a/pymycobot/common.py +++ b/pymycobot/common.py @@ -110,7 +110,8 @@ class ProtocolCode(object): GET_ROBOT_ID = 0x03 OVER_LIMIT_RETURN_ZERO = 0x04 SET_ROBOT_ID = 0x04 - + CLEAR_COMMAND_QUEUE = 0x05 + CHECK_ASYNC_OR_SYNC = 0x06 GET_ERROR_INFO = 0x07 CLEAR_ERROR_INFO = 0x08 GET_ATOM_VERSION = 0x09 @@ -147,6 +148,7 @@ class ProtocolCode(object): SET_FREE_MODE = 0x1A IS_FREE_MODE = 0x1B COBOTX_GET_ANGLE = 0x1C + IS_CONTROLLER_BUTTON_PRESSED = 0x1C POWER_ON_ONLY = 0x1D SET_VISION_MODE = 0x1D SET_CONTROL_MODE = 0x1E @@ -172,6 +174,7 @@ class ProtocolCode(object): GET_COORD = 0x2D SEND_ANGLES_AUTO = 0x2E GET_SOLUTION_ANGLES = 0x2E + WRITE_ANGLE_TIME = 0x2E SET_SOLUTION_ANGLES = 0x2F # JOG MODE AND OPERATION diff --git a/pymycobot/mycobot280x5pi.py b/pymycobot/mycobot280x5pi.py new file mode 100644 index 00000000..bb4a331e --- /dev/null +++ b/pymycobot/mycobot280x5pi.py @@ -0,0 +1,837 @@ +#!/usr/bin/env python +# -*- coding: UTF-8 -*- +from __future__ import division + +import functools +import time +import math +import threading +import serial + +from pymycobot.generate import CommandGenerator +from pymycobot.common import ProtocolCode, write, read +from pymycobot.error import calibration_parameters + + +def setup_serial_port(port, baudrate, timeout): + serial_api = serial.Serial(port=port, baudrate=baudrate, timeout=timeout) + serial_api.rts = False + if not serial_api.is_open: + serial_api.open() + return serial_api + + +class MyCobot280X5Api(CommandGenerator): + + def __init__(self, port, baudrate=100_0000, timeout=0.1, debug=False, thread_lock=True): + super(MyCobot280X5Api, self).__init__(debug) + self._serial_port = setup_serial_port(port, baudrate, timeout) + self.thread_lock = thread_lock + self.lock = threading.Lock() + + self.calibration_parameters = functools.partial(calibration_parameters, class_name="MyCobot280") + self._write = functools.partial(write, self) + self._read = functools.partial(read, self) + + def _mesg(self, genre, *args, **kwargs): + """ + + Args: + genre: command type (Command) + *args: other data. + It is converted to octal by default. + If the data needs to be encapsulated into hexadecimal, + the array is used to include them. (Data cannot be nested) + **kwargs: support `has_reply` + has_reply: Whether there is a return value to accept. + """ + real_command, has_reply, _ = super(MyCobot280X5Api, self)._mesg(genre, *args, **kwargs) + if self.thread_lock: + with self.lock: + return self._res(real_command, has_reply, genre) + else: + return self._res(real_command, has_reply, genre) + + def _res(self, real_command, has_reply, genre): + if genre == ProtocolCode.SET_SSID_PWD or genre == ProtocolCode.GET_SSID_PWD: + self._write(self._flatten(real_command)) + data = self._read(genre) + else: + try_count = 0 + while try_count < 3: + self._write(self._flatten(real_command)) + data = self._read(genre) + if data is not None and data != b'': + break + try_count += 1 + else: + return -1 + if genre == ProtocolCode.SET_SSID_PWD: + return 1 + + if genre == ProtocolCode.GET_QUICK_INFO: + res = [] + valid_data = data[4:-1] + for header_i in range(0, len(valid_data), 2): + if header_i < 26: + one = valid_data[header_i: header_i + 2] + res.append(self._decode_int16(one)) + res.extend(valid_data[25:]) + else: + res = self._process_received(data, genre) + + if res is None: + return None + if genre in [ProtocolCode.SET_BASIC_OUTPUT]: + return 1 + if res is not None and isinstance(res, list) and len(res) == 1 and genre not in [ + ProtocolCode.GET_BASIC_VERSION, ProtocolCode.GET_JOINT_MIN_ANGLE, ProtocolCode.GET_JOINT_MAX_ANGLE, + ProtocolCode.SOFTWARE_VERSION, ProtocolCode.GET_ATOM_VERSION + ]: + return res[0] + + if genre in [ + ProtocolCode.IS_POWER_ON, + ProtocolCode.IS_CONTROLLER_CONNECTED, + ProtocolCode.IS_PAUSED, # TODO have bug: return b'' + ProtocolCode.IS_IN_POSITION, + ProtocolCode.IS_MOVING, + ProtocolCode.IS_SERVO_ENABLE, + ProtocolCode.IS_ALL_SERVO_ENABLE, + ProtocolCode.GET_SERVO_DATA, + ProtocolCode.GET_DIGITAL_INPUT, + ProtocolCode.GET_GRIPPER_VALUE, + ProtocolCode.IS_GRIPPER_MOVING, + ProtocolCode.GET_SPEED, + ProtocolCode.GET_ENCODER, + ProtocolCode.GET_BASIC_INPUT, + ProtocolCode.GET_TOF_DISTANCE, + ProtocolCode.GET_END_TYPE, + ProtocolCode.GET_MOVEMENT_TYPE, + ProtocolCode.GET_REFERENCE_FRAME, + ProtocolCode.GET_FRESH_MODE, + ProtocolCode.GET_GRIPPER_MODE, + ProtocolCode.GET_ERROR_INFO, + ProtocolCode.GET_COMMUNICATE_MODE, + ProtocolCode.SET_COMMUNICATE_MODE, + ProtocolCode.SetHTSGripperTorque, + ProtocolCode.GetHTSGripperTorque, + ProtocolCode.GetGripperProtectCurrent, + ProtocolCode.InitGripper, + ProtocolCode.SET_FOUR_PIECES_ZERO + ]: + return self._process_single(res) + elif genre in [ProtocolCode.GET_ANGLES, ProtocolCode.SOLVE_INV_KINEMATICS]: + return [self._int2angle(angle) for angle in res] + elif genre in [ProtocolCode.GET_COORDS, ProtocolCode.GET_TOOL_REFERENCE, ProtocolCode.GET_WORLD_REFERENCE]: + if res: + r = [] + for idx in range(3): + r.append(self._int2coord(res[idx])) + for idx in range(3, 6): + r.append(self._int2angle(res[idx])) + return r + else: + return res + elif genre in [ProtocolCode.GET_SERVO_VOLTAGES]: + return [self._int2coord(angle) for angle in res] + elif genre in [ProtocolCode.GET_JOINT_MAX_ANGLE, ProtocolCode.GET_JOINT_MIN_ANGLE]: + return self._int2coord(res[0]) + elif genre in [ + ProtocolCode.GET_BASIC_VERSION, ProtocolCode.SOFTWARE_VERSION, ProtocolCode.GET_ATOM_VERSION + ]: + return self._int2coord(self._process_single(res)) + elif genre in [ProtocolCode.GET_REBOOT_COUNT]: + return self._process_high_low_bytes(res) + elif genre in (ProtocolCode.GET_ANGLES_COORDS, ProtocolCode.GET_QUICK_INFO): + r = [] + for index, el in enumerate(res): + if index < 6: + r.append(self._int2angle(el)) + elif index < 9: + r.append(self._int2coord(el)) + elif index < 12: + r.append(self._int2angle(el)) + else: + r.append(el) + return r + else: + return res + + def wait(self, t): + time.sleep(t) + return self + + def close(self): + self._serial_port.close() + + def open(self): + self._serial_port.open() + + +class MyCobot280X5PI(MyCobot280X5Api): + + def __init__(self, port, baudrate=100_0000, timeout=0.1, debug=False, thread_lock=True): + """ + Args: + port : port string + baudrate : baud rate int, default 100_0000 + timeout : default 0.1 + debug : whether show debug info + """ + super().__init__(port, baudrate, timeout, debug, thread_lock) + + # System Status + def get_modify_version(self): + """get modify version""" + return self._mesg(ProtocolCode.ROBOT_VERSION) + + def get_system_version(self): + """get system version""" + return self._mesg(ProtocolCode.SOFTWARE_VERSION) + + def clear_queue(self): + """Clear the command queue""" + return self._mesg(ProtocolCode.CLEAR_COMMAND_QUEUE) + + def async_or_sync(self): + """Set the command execution mode + Return: + 0: Asynchronous mode + 1: Synchronous mode + """ + return self._mesg(ProtocolCode.CHECK_ASYNC_OR_SYNC) + + def get_error_information(self): + """Obtaining robot error information + + Return: + 0: No error message. + 1 ~ 6: The corresponding joint exceeds the limit position. + 16 ~ 19: Collision protection. + 32: Kinematics inverse solution has no solution. + 33 ~ 34: Linear motion has no adjacent solution. + """ + return self._mesg(ProtocolCode.GET_ERROR_INFO, has_reply=True) + + def clear_error_information(self): + """Clear robot error message""" + return self._mesg(ProtocolCode.CLEAR_ERROR_INFO, has_reply=True) + + def power_on(self): + """Open communication with Atom.""" + return self._mesg(ProtocolCode.POWER_ON) + + def power_off(self): + """Close communication with Atom.""" + return self._mesg(ProtocolCode.POWER_OFF) + + def is_power_on(self): + """Adjust robot arm status + + Return: + 1 - power on + 0 - power off + -1 - error data + """ + return self._mesg(ProtocolCode.IS_POWER_ON, has_reply=True) + + def release_all_servos(self, data=None): + """Relax all joints + + Args: + data: 1 - Undamping (The default is damping) + """ + if data is None: + return self._mesg(ProtocolCode.RELEASE_ALL_SERVOS) + else: + return self._mesg(ProtocolCode.RELEASE_ALL_SERVOS, data) + + def read_next_error(self): + """Robot Error Detection + + Return: + list len 6. + 0 : No abnormality + 1 : Communication disconnected + 2 : Unstable communication + 3 : Servo abnormality + """ + return self._mesg(ProtocolCode.READ_NEXT_ERROR, has_reply=True) + + def set_fresh_mode(self, mode): + """Set command refresh mode + + Args: + mode: int. + 1 - Always execute the latest command first. + 0 - Execute instructions sequentially in the form of a queue. + """ + self.calibration_parameters(mode=mode) + return self._mesg(ProtocolCode.SET_FRESH_MODE, mode) + + def get_fresh_mode(self): + """Query sports mode""" + return self._mesg(ProtocolCode.GET_FRESH_MODE, has_reply=True) + + def set_vision_mode(self, flag): + """Set the visual tracking mode to limit the posture flipping of send_coords in refresh mode. + (Only for visual tracking function) + + Args: + flag: 0/1; 0 - close; 1 - open + """ + self.calibration_parameters(flag=flag) + return self._mesg(ProtocolCode.SET_VISION_MODE, flag) + + # Overall Status + def set_free_mode(self, flag): + """set to free mode + + Args: + flag: 0/1 + """ + self.calibration_parameters(flag=flag) + return self._mesg(ProtocolCode.SET_FREE_MODE, flag) + + def is_free_mode(self): + """Check if it is free mode + + Return: + 0/1 + """ + return self._mesg(ProtocolCode.IS_FREE_MODE, has_reply=True) + + # MDI mode and operation + def get_radians(self): + """Get the radians of all joints + + Return: + list: A list of float radians [radian1, ...] + """ + angles = self._mesg(ProtocolCode.GET_ANGLES, has_reply=True) + return [round(angle * (math.pi / 180), 3) for angle in angles] + + def send_radians(self, radians, speed): + """Send the radians of all joints to robot arm + + Args: + radians: a list of radian values( List[float]), length 6 + speed: (int )0 ~ 100 + """ + calibration_parameters(len6=radians, speed=speed) + degrees = [self._angle2int(radian * (180 / math.pi)) + for radian in radians] + return self._mesg(ProtocolCode.SEND_ANGLES, degrees, speed) + + def sync_send_angles(self, degrees, speed, timeout=15): + """Send the angle in synchronous state and return when the target point is reached + + Args: + degrees: a list of degree values(List[float]), length 6. + speed: (int) 0 ~ 100 + timeout: default 15s. + """ + t = time.time() + self.send_angles(degrees, speed) + while time.time() - t < timeout: + f = self.is_in_position(degrees, 0) + if f == 1: + break + time.sleep(0.1) + return 1 + + def sync_send_coords(self, coords, speed, mode=0, timeout=15): + """Send the coord in synchronous state and return when the target point is reached + + Args: + coords: a list of coord values(List[float]) + speed: (int) 0 ~ 100 + mode: (int): 0 - angular（default）, 1 - linear + timeout: default 15s. + """ + t = time.time() + self.send_coords(coords, speed, mode) + while time.time() - t < timeout: + if self.is_in_position(coords, 1) == 1: + break + time.sleep(0.1) + return 1 + + def get_angles_coords(self): + """Get joint angles and coordinates""" + return self._mesg(ProtocolCode.GET_ANGLES_COORDS, has_reply=True) + + def get_quick_move_message(self): + """Get the quick move message""" + return self._mesg(ProtocolCode.GET_QUICK_INFO, has_reply=True) + + # JOG mode and operation + def write_angles_time_control(self, angles, step_time): + """Write the angle of a joint in time control mode + Args: + angles: Angle value + step_time: Time unit: 30ms, range(1 ~ 255) + """ + if step_time not in range(1, 256): + raise ValueError("step_time must be in range(1 ~ 255)") + angles = [self._angle2int(angle) for angle in angles] + return self._mesg(ProtocolCode.WRITE_ANGLE_TIME, angles, step_time) + + def jog_rpy(self, end_direction, direction, speed): + """Rotate the end around a fixed axis in the base coordinate system + + Args: + end_direction (int): Roll, Pitch, Yaw (1-3) + direction (int): 1 - forward rotation, 0 - reverse rotation + speed (int): 1 ~ 100 + """ + self.calibration_parameters(end_direction=end_direction, speed=speed) + return self._mesg(ProtocolCode.JOG_ABSOLUTE, end_direction, direction, speed) + + def jog_increment_angle(self, joint_id, increment, speed): + """ angle step mode + + Args: + joint_id: int 1-6. + increment: Angle increment value + speed: int (0 - 100) + """ + self.calibration_parameters(id=joint_id, speed=speed) + return self._mesg(ProtocolCode.JOG_INCREMENT, joint_id, [self._angle2int(increment)], speed) + + def jog_increment_coord(self, axis, increment, speed): + """coord step mode + + Args: + axis: axis id 1 - 6. + increment: Coord increment value + speed: int (1 - 100) + """ + self.calibration_parameters(id=axis, speed=speed) + value = self._coord2int(increment) if axis <= 3 else self._angle2int(increment) + return self._mesg(ProtocolCode.JOG_INCREMENT_COORD, axis, [value], speed) + + def set_HTS_gripper_torque(self, torque): + """Set new adaptive gripper torque + + Args: + torque (int): 150 ~ 980 + + Return: + 0: Set failed + 1: Set successful + """ + self.calibration_parameters(torque=torque) + return self._mesg(ProtocolCode.SetHTSGripperTorque, [torque], has_reply=True) + + def get_HTS_gripper_torque(self): + """Get gripper torque + + Returns: + int: 150 ~ 980 + """ + return self._mesg(ProtocolCode.GetHTSGripperTorque, has_reply=True) + + def get_gripper_protect_current(self): + """Get the gripper protection current + + Returns: + int: 1 ~ 500 + """ + return self._mesg(ProtocolCode.GetGripperProtectCurrent, has_reply=True) + + def init_gripper(self): + """Initialize gripper + + Returns: + int: 0 or 1 (1 - success) + """ + return self._mesg(ProtocolCode.InitGripper, has_reply=True) + + def set_gripper_protect_current(self, current): + """Set the gripper protection current + + Args: + current (int): 1 ~ 500 + """ + self.calibration_parameters(current=current) + + return self._mesg(ProtocolCode.SetGripperProtectCurrent, [current]) + + # Atom IO + def set_pin_mode(self, pin_no, pin_mode): + """Set the state mode of the specified pin in atom. + + Args: + pin_no (int): pin. + pin_mode (int): 0 - input, 1 - output, 2 - input pull up + """ + self.calibration_parameters(pin_mode=pin_mode) + return self._mesg(ProtocolCode.SET_PIN_MODE, pin_no, pin_mode) + + def get_gripper_value(self, gripper_type=None): + """Get the value of gripper. + + Args: + gripper_type (int): default 1 + 1: Adaptive gripper + 3: Parallel gripper + 4: Flexible gripper + + Return: + gripper value (int) + """ + if gripper_type is None: + return self._mesg(ProtocolCode.GET_GRIPPER_VALUE, has_reply=True) + else: + self.calibration_parameters(gripper_type=gripper_type) + return self._mesg(ProtocolCode.GET_GRIPPER_VALUE, gripper_type, has_reply=True) + + def is_gripper_moving(self): + """Judge whether the gripper is moving or not + + Returns: + 0 - not moving + 1 - is moving + -1- error data + """ + return self._mesg(ProtocolCode.IS_GRIPPER_MOVING, has_reply=True) + + def get_tool_system_version(self): + """ + Read the terminal primary and minor version numbers + """ + return self._mesg(ProtocolCode.GET_ATOM_VERSION, has_reply=True) + + def get_tool_modified_version(self): + """ + Read the terminal modified version number + """ + return self._mesg(ProtocolCode.OVER_LIMIT_RETURN_ZERO, has_reply=True) + + def is_tool_connected(self): + """Check the end connection status""" + return self._mesg(ProtocolCode.IS_CONTROLLER_CONNECTED, has_reply=True) + + def is_tool_button_click(self): + """Check whether the button on the end is pressed""" + return self._mesg(ProtocolCode.IS_CONTROLLER_BUTTON_PRESSED, has_reply=True) + + def set_pwm_output(self, channel, frequency, pin_val): + """ PWM control + + Args: + channel (int): IO number. + frequency (int): clock frequency + pin_val (int): Duty cycle 0 ~ 256; 128 means 50% + """ + return self._mesg(ProtocolCode.SET_PWM_OUTPUT, channel, [frequency], pin_val) + + # communication mode + def set_transponder_mode(self, mode): + """Set basic communication mode + + Args: + mode: 0 - Turn off transparent transmission，1 - Open transparent transmission + """ + self.calibration_parameters(mode=mode) + return self._mesg(ProtocolCode.SET_COMMUNICATE_MODE, mode, has_reply=True) + + def get_transponder_mode(self): + return self._mesg(ProtocolCode.GET_COMMUNICATE_MODE, has_reply=True) + + # g9 servo + def move_round(self): + """Drive the 9g steering gear clockwise for one revolution + """ + return self._mesg(ProtocolCode.move_round) + + def set_four_pieces_zero(self): + """Set the zero position of the four-piece motor + + Returns: + int: 0 or 1 (1 - success) + """ + return self._mesg(ProtocolCode.SET_FOUR_PIECES_ZERO, has_reply=True) + + # Running Status and Settings + def set_joint_max(self, id, angle): + """Set the joint maximum angle + + Args: + id: int. + Joint id 1 - 6 + for gripper: Joint id 7 + angle: 0 ~ 180 + """ + self.calibration_parameters(id=id, angle=angle) + return self._mesg(ProtocolCode.SET_JOINT_MAX, id, angle) + + def set_joint_min(self, id, angle): + """Set the joint minimum angle + + Args: + id: int. + Joint id 1 - 6 + for gripper: Joint id 7 + angle: 0 ~ 180 + """ + self.calibration_parameters(id=id, angle=angle) + return self._mesg(ProtocolCode.SET_JOINT_MIN, id, angle) + + # servo state value + def get_servo_speeds(self): + """Get joint speed + + Return: + A list unit step/s + """ + return self._mesg(ProtocolCode.GET_SERVO_SPEED, has_reply=True) + + def get_servo_voltages(self): + """Get joint voltages + + Return: + A list volts < 24 V + """ + return self._mesg(ProtocolCode.GET_SERVO_VOLTAGES, has_reply=True) + + def get_servo_status(self): + """Get joint status + + Return: + [voltage, sensor, temperature, current, angle, overload], a value of 0 means no error, a value of 1 indicates an error + """ + return self._mesg(ProtocolCode.GET_SERVO_STATUS, has_reply=True) + + def get_servo_temps(self): + """Get joint temperature + + Return: + A list unit ℃ + """ + return self._mesg(ProtocolCode.GET_SERVO_TEMPS, has_reply=True) + + # Coordinate transformation + def set_tool_reference(self, coords): + """Set tool coordinate system + + Args: + coords: a list of coords value(List[float]) + [x(mm), y, z, rx(angle), ry, rz] + """ + self.calibration_parameters(coords=coords) + coord_list = [] + for idx in range(3): + coord_list.append(self._coord2int(coords[idx])) + for angle in coords[3:]: + coord_list.append(self._angle2int(angle)) + return self._mesg(ProtocolCode.SET_TOOL_REFERENCE, coord_list) + + def get_tool_reference(self): + """Get tool coordinate system """ + return self._mesg(ProtocolCode.GET_TOOL_REFERENCE, has_reply=True) + + def set_world_reference(self, coords): + """Set the world coordinate system + + Args: + coords: a list of coords value(List[float]) + [x(mm), y, z, rx(angle), ry, rz]\n + """ + self.calibration_parameters(coords=coords) + coord_list = [] + for idx in range(3): + coord_list.append(self._coord2int(coords[idx])) + for angle in coords[3:]: + coord_list.append(self._angle2int(angle)) + return self._mesg(ProtocolCode.SET_WORLD_REFERENCE, coord_list) + + def get_world_reference(self): + """Get the world coordinate system""" + return self._mesg(ProtocolCode.GET_WORLD_REFERENCE, has_reply=True) + + def set_reference_frame(self, rftype): + """Set the base coordinate system + + Args: + rftype: 0 - base 1 - tool. + """ + self.calibration_parameters(rftype=rftype) + return self._mesg(ProtocolCode.SET_REFERENCE_FRAME, rftype) + + def get_reference_frame(self): + """Get the base coordinate system + + Return: + 0 - base 1 - tool. + """ + return self._mesg(ProtocolCode.GET_REFERENCE_FRAME, has_reply=True) + + def set_movement_type(self, move_type): + """Set movement type + + Args: + move_type: 1 - movel, 0 - moveJ + """ + self.calibration_parameters(move_type=move_type) + return self._mesg(ProtocolCode.SET_MOVEMENT_TYPE, move_type) + + def get_movement_type(self): + """Get movement type + + Return: + 1 - movel, 0 - moveJ + """ + return self._mesg(ProtocolCode.GET_MOVEMENT_TYPE, has_reply=True) + + def set_end_type(self, end): + """Set end coordinate system + + Args: + end: int + 0 - flange, 1 - tool + """ + self.calibration_parameters(end=end) + return self._mesg(ProtocolCode.SET_END_TYPE, end) + + def get_end_type(self): + """Get end coordinate system + + Return: + 0 - flange, 1 - tool + """ + return self._mesg(ProtocolCode.GET_END_TYPE, has_reply=True) + + def angles_to_coords(self, angles): + """ Convert angles to coordinates + + Args: + angles : A float list of all angle. + + Return: + list: A float list of all coordinates. + """ + angles = [self._angle2int(angle) for angle in angles] + return self._mesg(ProtocolCode.GET_COORDS, angles, has_reply=True) + + def solve_inv_kinematics(self, target_coords, current_angles): + """ Convert target coordinates to angles + + Args: + target_coords: A float list of all coordinates. + current_angles : A float list of all angle. + + Return: + list: A float list of all angle. + """ + angles = [self._angle2int(angle) for angle in current_angles] + coord_list = [] + for idx in range(3): + coord_list.append(self._coord2int(target_coords[idx])) + for angle in target_coords[3:]: + coord_list.append(self._angle2int(angle)) + return self._mesg(ProtocolCode.SOLVE_INV_KINEMATICS, coord_list, angles, has_reply=True) + + def is_torque_gripper(self): + """Whether it is a force-controlled gripper + + Return: + 40 - Force control + 9 - Non-force control + """ + return self.get_servo_data(7, 1) + + def set_gripper_state(self, flag, speed, _type_1=None, is_torque=None): + """Set gripper switch state + + Args: + flag (int): 0 - open, 1 - close, 254 - release + speed (int): 1 ~ 100 + _type_1 (int): default 1 + 1 : Adaptive gripper. default to adaptive gripper + 2 : 5 finger dexterous hand + 3 : Parallel gripper, this parameter can be omitted + 4 : Flexible gripper + is_torque (int): When there is no type parameter, this parameter can be omitted. + 1: Force control + 0: Non-force control + """ + self.calibration_parameters(flag=flag, speed=speed, _type_1=_type_1, is_torque=is_torque) + args = [flag, speed] + if _type_1 is not None: + args.append(_type_1) + if is_torque is not None: + args.append(is_torque) + return self._mesg(ProtocolCode.SET_GRIPPER_STATE, *args) + + def set_gripper_value(self, gripper_value, speed, gripper_type=None, is_torque=None): + """Set gripper value + + Args: + gripper_value (int): 0 ~ 100 + speed (int): 1 ~ 100 + gripper_type (int): default 1 + 1: Adaptive gripper + 3: Parallel gripper, this parameter can be omitted + 4: Flexible gripper + is_torque (int): When there is no type parameter, this parameter can be omitted. + 1: Force control + 0: Non-force control + """ + self.calibration_parameters(gripper_value=gripper_value, speed=speed, + gripper_type=gripper_type, is_torque=is_torque) + args = [gripper_value, speed] + if gripper_type is not None: + args.append(gripper_type) + if is_torque is not None: + args.append(is_torque) + return self._mesg(ProtocolCode.SET_GRIPPER_VALUE, *args, has_reply=True) + + def drag_start_record(self): + """Start track recording + + Return: + Recording queue length + """ + + return self._mesg(ProtocolCode.DRAG_START_RECORD, has_reply=True) + + def drag_end_record(self): + """End track recording + + Return: + Recording queue length + """ + + return self._mesg(ProtocolCode.DRAG_END_RECORD, has_reply=True) + + def drag_get_record_data(self): + """Get the recorded track + + Return: + List of potential values (encoder values) and operating speeds of each joint + eg: [J1_encoder, J1_run_speed,J2_encoder, J2_run_speed,J3_encoder, J3_run_speed,J4_encoder, J4_run_speed,J5_ + encoder, J5_run_speed,J6_encoder, J6_run_speed] + """ + + return self._mesg(ProtocolCode.DRAG_GET_RECORD_DATA, has_reply=True) + + def drag_get_record_len(self): + """Get the total number of recorded points + + Return: + Recording queue length + """ + + return self._mesg(ProtocolCode.DRAG_GET_RECORD_LEN, has_reply=True) + + def drag_clear_record_data(self): + """Clear recording track + + Return: + Recording queue length 0 + """ + + return self._mesg(ProtocolCode.DRAG_CLEAR_RECORD_DATA, has_reply=True) \ No newline at end of file From 1346cd81c4d38249a9a2bd55d6456cd170891915 Mon Sep 17 00:00:00 2001 From: Mrkun5018 <2220138602@qq.com> Date: Thu, 13 Feb 2025 17:46:10 +0800 Subject: [PATCH 02/19] add MyCobot 280 X5 PI doc --- docs/MyCobot_280_X5PI_en.md | 492 ++++++++++++++++++++++++++++++++++++ docs/MyCobot_280_X5PI_zh.md | 490 +++++++++++++++++++++++++++++++++++ pymycobot/mycobot280x5pi.py | 51 +--- 3 files changed, 991 insertions(+), 42 deletions(-) create mode 100644 docs/MyCobot_280_X5PI_en.md create mode 100644 docs/MyCobot_280_X5PI_zh.md diff --git a/docs/MyCobot_280_X5PI_en.md b/docs/MyCobot_280_X5PI_en.md new file mode 100644 index 00000000..0ebf4910 --- /dev/null +++ b/docs/MyCobot_280_X5PI_en.md @@ -0,0 +1,492 @@ +# MyCobot 280 X5 PI + +[toc] + +## Python API usage instructions + +API (Application Programming Interface), also known as Application Programming Interface functions, are predefined functions. When using the following function interfaces, please import our API library at the beginning by entering the following code, otherwise it will not run successfully: + +```python +# Example +from pymycobot import MyCobot280X5PI + +mc = MyCobot280X5PI('COM3') + +# Gets the current angle of all joints +angles = mc.get_angles() +print(angles) + +# Set 1 joint to move to 40 and speed to 20 +mc.send_angle(1, 40, 20) +``` + +### 1. System Status + +#### `get_modify_version()` +- **function:** get modify version +#### `get_system_version()` +- **function:** get system version + +### 2. Overall Status + +#### `clear_queue()` +- **function:** Clear the command queue +#### `async_or_sync()` +- **function:** Set the command execution mode +- **Return value:** + - **`0`: Asynchronous mode** + - **`1`: Synchronous mode** +#### `get_error_information()` +- **function:** Obtaining robot error information +- **Return value:** + - **`0`: No error message.** + - **`1 ~ 6`: The corresponding joint exceeds the limit position.** + - **`16 ~ 19`: Collision protection.** + - **`32`: Kinematics inverse solution has no solution.** + - **`33 ~ 34`: Linear motion has no adjacent solution.** +#### `clear_error_information()` +- **function:** Clear robot error message +#### `power_on()` +- **function:** Open communication with Atom. +#### `power_off()` +- **function:** Close communication with Atom. +#### `is_power_on()` +- **function:** Adjust robot arm status +- **Return value:** + - **`1` - power on** + - **`0` - power off** + - **`-1` - error data** +#### `release_all_servos(data)` +- **function:** Relax all joints +- **Parameters:** + - **data: `1` - Undamping (The default is damping)** +#### `read_next_error()` +- **function:** Robot Error Detection +- **Return value:** + - **list len 6.** + - **`0` : No abnormality** + - **`1` : Communication disconnected** + - **`2` : Unstable communication** + - **`3` : Servo abnormality** +#### `set_fresh_mode(mode)` +- **function:** Set command refresh mode +- **Parameters:** + - **mode: int.** + - **`1` - Always execute the latest command first.** + - **`0` - Execute instructions sequentially in the form of a queue.** +#### `get_fresh_mode()` +- **function:** Query sports mode +#### `set_vision_mode(flag)` +- **function:** Set the visual tracking mode to limit the posture flipping of send_coords in refresh mode. + - **(Only for visual tracking function)** +- **Parameters:** + - **flag: 0/1; `0` - close; `1` - open** + +### 3. Motion Control Interface + +#### `get_angles()` +- **function:** Get the angle of all joints. +- **Return value:** + - **list: A float list of all angle.** +#### `send_angle(id, degree, speed)` +- **function:** Send one angle of joint to robot arm. +- **Parameters:** + - **id : Joint id(genre.Angle) int 1-6.** + - **angle : angle value(float).** + - **speed : (int) 1 ~ 100** +#### `send_angles(angles, speed)` +- **function:** Send the angles of all joints to robot arm. +- **Parameters:** + - **angles: a list of angle values(List[float]). len 6.** + - **speed : (int) 1 ~ 100** +#### `get_coords()` +- **function:** Get the coords from robot arm, coordinate system based on base. +- **Return value:** + - **list : A float list of coord .[x, y, z, rx, ry, rz]** +#### `send_coord(id, coord, speed)` +- **function:** Send one coord to robot arm. +- **Parameters:** + - **id(int) : coord id(genre.Coord) int 1-6.** + - **coord(float) : coord value, mm** + - **speed(int) : 1 ~ 100** +#### `send_coords(coords, speed, mode)` +- **function:** Send all coords to robot arm. +- **Parameters:** + - **coords: a list of coords value(List[float]).[x(mm), y, z, rx(angle), ry, rz]** + - **speed : (int) 1 ~ 100** + - **mode : (int) 0 - angluar, 1 - linear** +#### `pause()` +- **function:** Pause movement +#### `is_paused()` +- **function:** Judge whether the manipulator pauses or not. +- **Return value:** + - **`1` - paused** + - **`0` - not paused** + - **`-1` - error** +#### `resume()` +- **function:** Recovery movement +#### `stop()` +- **function:** Stop moving +#### `is_in_position(data, id = 0)` +- **function:** Judge whether in the position. +- **Parameters:** + - **data: A data list, angles or coords.len 6.** + - **id: 1 - coords, 0 - angles** +- **Return value:** + - **`1` - True** + - **`0` - False** + - **`-1` - Error** +#### `is_moving()` +- **function:** Detect if the robot is moving +- **Return value:** + - **`0` - not moving** + - **`1` - is moving** + - **`-1` - error data** +#### `write_angles_time_control(angles, step_time)` +- **function:** Write the angle of a joint in time control mode +- **Parameters:** + - **angles: Angle value** + - **step_time: Time unit: 30ms, range(1 ~ 255)** + +### 4. JOG Mode And Operation + +#### `jog_angle(joint_id, direction, speed)` +- **function:** Jog control angle. +- **Parameters:** + - **joint_id: int 1-6.** + - **direction: `0` - decrease, `1` - increase** + - **speed: int (0 - 100)** +#### `jog_rpy(end_direction, direction, speed)` +- **function:** Rotate the end around a fixed axis in the base coordinate system +- **Parameters:** + - **end_direction (int): Roll, Pitch, Yaw (1-3)** + - **direction (int): `1` - forward rotation, `0` - reverse rotation** + - **speed (int): 1 ~ 100** +#### `jog_coord(coord_id, direction, speed)` +- **function:** Jog control coord. +- **Parameters:** + - **coord_id: int 1-6** + - **direction: `0` - decrease, `1` - increase** + - **speed: int (1 - 100)** +#### `jog_increment_angle(joint_id, increment, speed)` +- **function:** angle step mode +- **Parameters:** + - **joint_id: int 1-6.** + - **increment: Angle increment value** + - **speed: int (0 - 100)** +#### `jog_increment_coord(axis, increment, speed)` +- **function:** coord step mode +- **Parameters:** + - **axis: axis id 1 - 6.** + - **increment: Coord increment value** + - **speed: int (1 - 100)** +#### `set_HTS_gripper_torque(torque)` +- **function:** Set new adaptive gripper torque +- **Parameters:** + - **torque (int): 150 ~ 980** +- **Return value:** + - **0: Set failed** + - **1: Set successful** +#### `get_HTS_gripper_torque()` +- **function:** Get gripper torque +- **Return value:** + - **int: 150 ~ 980** +#### `get_gripper_protect_current()` +- **function:** Get the gripper protection current +- **Return value:** + - **int: 1 ~ 500** +#### `init_gripper()` +- **function:** Initialize gripper +- **Return value:** + - **int: 0 or 1 (1 - success)** +#### `set_gripper_protect_current(current)` +- **function:** Set the gripper protection current +- **Parameters:** + - **current (int): 1 ~ 500** +#### `set_encoder(joint_id, encoder, speed)` +- **function:** Set a single joint rotation to the specified potential value. +- **Parameters:** + - **joint_id: int 1 - 6** + - **for gripper: Joint id 7** + - **encoder: The value of the set encoder.** + - **speed : 1 - 100** +#### `get_encoder(joint_id)` +- **function:** Obtain the specified joint potential value. +- **Parameters:** + - **joint_id: (int) 1 - 6** + - **for gripper: Joint id 7** +#### `set_encoders(encoders, sp)` +- **function:** Set the six joints of the manipulator to execute synchronously to the specified position. +- **Parameters:** + - **encoders: A encoder list. len 6.** + - **sp: speed 1 ~ 100** +#### `get_encoders()` +- **function:** Get the six joints of the manipulator +- **Return value:** + - **The list of encoders** +#### `set_encoders_drag(encoders, speeds)` +- **function:** Send all encoders and speeds +- **Parameters:** + - **encoders: encoders list.** + - **speeds: Obtained by the get_servo_speeds() method** +#### `get_joint_min_angle(joint_id)` +- **function:** Gets the minimum movement angle of the specified joint +- **Parameters:** + - **joint_id: 1 - 6** +- **Return value:** + - **angle value(float)** +#### `get_joint_max_angle(joint_id)` +- **function:** Gets the maximum movement angle of the specified joint +- **Parameters:** + - **joint_id: 1 - 6** +- **Return value:** + - **angle value(float)** +#### `set_joint_min(id, angle)` +- **function:** Set the joint minimum angle +- **Parameters:** + - **id: int.** + - **Joint id 1 - 6** + - **for gripper: Joint id 7** + - **angle: 0 ~ 180** +#### `set_joint_max(id, angle)` +- **function:** Set the joint maximum angle +- **Parameters:** + - **id: int.** + - **Joint id 1 - 6** + - **for gripper: Joint id 7** + - **angle: 0 ~ 180** + +### 5. Servo Control + +#### `is_servo_enable(servo_id)` +- **function:** To detect the connection state of a single joint +- **Parameters:** + - **servo_id: 1 - 6** +- **Return value:** + - **`0` - disable** + - **`1` - enable** + - **`-1` - error** +#### `is_all_servo_enable()` +- **function:** Detect the connection status of all joints +- **Return value:** + - **`0` - disable** + - **`1` - enable** + - **`-1` - error** +#### `set_servo_data(servo_id, data_id, value, mode)` +- **function:** Set the data parameters of the specified address of the steering gear +- **Parameters:** + - **servo_id: Serial number of articulated steering gear. 1 - 7** + - **data_id: Data address.** + - **value: 0 - 4096** + - **mode: 0 - indicates that value is one byte(default), 1 - 1 represents a value of two bytes.** +#### `get_servo_data(servo_id, data_id, mode)` +- **function:** Read the data parameter of the specified address of the steering gear. +- **Parameters:** + - **servo_id: Serial number of articulated steering gear.1 - 7** + - **data_id: Data address.** + - **mode: 0 - indicates that value is one byte(default), 1 - 1 represents a value of two bytes.** +- **Return value:** + - **values 0 - 4096** +#### `set_servo_calibration(servo_id)` +- **function:** The current position of the calibration joint actuator is the angle zero point, + - **and the corresponding potential value is 2048.** +- **Parameters:** + - **servo_id: Serial number of articulated steering gear. 1 - 6** +#### `joint_brake(joint_id)` +- **function:** Make it stop when the joint is in motion, and the buffer distance is positively related to the existing speed +- **Parameters:** + - **joint_id: 1 - 6** +#### `release_servo(servo_id, mode)` +- **function:** Power off designated servo +- **Parameters:** + - **servo_id: int 1 - 6** + - **mode: Default damping, set to 1, cancel damping** +#### `focus_servo(servo_id)` +- **function:** Power on designated servo +- **Parameters:** + - **servo_id: int 1 - 6** + +### 6. Gripper Control + +#### `get_gripper_value(gripper_type)` +- **function:** Get the value of gripper. +- **Parameters:** + - **gripper_type (int): default 1** + - **`1`: Adaptive gripper** + - **`3`: Parallel gripper** + - **`4`: Flexible gripper** +- **Return value:** + - **gripper value (int)** +#### `set_gripper_state(flag, speed, _type_1, is_torque)` +- **function:** Set gripper switch state +- **Parameters:** + - **flag (int): 0 - open, 1 - close, 254 - release** + - **speed (int): 1 ~ 100** + - **_type_1 (int): default 1** + - **`1` : Adaptive gripper. default to adaptive gripper** + - **`2` : 5 finger dexterous hand** + - **`3` : Parallel gripper, this parameter can be omitted** + - **`4` : Flexible gripper** + - **is_torque (int): When there is no type parameter, this parameter can be omitted.** + - **`1`: Force control** + - **`0`: Non-force control** +#### `set_gripper_value(gripper_value, speed, gripper_type, is_torque)` +- **function:** Set gripper value +- **Parameters:** + - **gripper_value (int): 0 ~ 100** + - **speed (int): 1 ~ 100** + - **gripper_type (int): default 1** + - **`1`: Adaptive gripper** + - **`3`: Parallel gripper, this parameter can be omitted** + - **`4`: Flexible gripper** + - **is_torque (int): When there is no type parameter, this parameter can be omitted.** + - **`1`: Force control** + - **`0`: Non-force control** +#### `set_gripper_calibration()` +- **function:** Set the current position to zero, set current position value is `2048`. +#### `is_gripper_moving()` +- **function:** Judge whether the gripper is moving or not +- **Return value:** + - **`0` - not moving** + - **`1` - is moving** + - **`-1` - error data** + +### 7. End ATOM Function + +#### `get_tool_system_version()` +- **function:** Read the terminal primary and minor version numbers +#### `get_tool_modify_version()` +- **function:** Read the terminal modified version number +#### `is_tool_connected()` +- **function:** Check the end connection status +#### `set_color(r = 0, g = 0, b = 0)` +- **function:** Set the light color on the top of the robot arm. +- **Parameters:** + - **r (int): 0 ~ 255** + - **g (int): 0 ~ 255** + - **b (int): 0 ~ 255** +#### `is_tool_button_click()` +- **function:** Check whether the button on the end is pressed +#### `set_digital_output(pin_no, pin_signal)` +- **function:** Set the terminal atom io status +- **Parameters:** + - **pin_no (int):** + - **pin_signal (int): 0 / 1** +#### `get_digital_input(pin_no)` +- **function:** singal value + +### 8. Kinematic Algorithm Interface + +#### `solve_inv_kinematics(target_coords, current_angles)` +- **function:** Convert target coordinates to angles +- **Parameters:** + - **target_coords: A float list of all coordinates.** + - **current_angles : A float list of all angle.** +- **Return value:** + - **list: A float list of all angle.** + +### 9. Coordinate System Interface + +#### `set_tool_reference(coords)` +- **function:** Set tool coordinate system +- **Parameters:** + - **coords: a list of coords value(List[float])** + - **[x(mm), y, z, rx(angle), ry, rz]** +#### `get_tool_reference()` +- **function:** Get tool coordinate system +#### `set_world_reference(coords)` +- **function:** Set the world coordinate system +- **Parameters:** + - **coords: a list of coords value(List[float])** + - **[x(mm), y, z, rx(angle), ry, rz]** +#### `get_world_reference()` +- **function:** Get the world coordinate system +#### `set_reference_frame(rftype)` +- **function:** Set the base coordinate system +- **Parameters:** + - **rftype: 0 - base 1 - tool.** +#### `get_reference_frame()` +- **function:** Get the base coordinate system +- **Return value:** + - **`0` - base `1` - tool.** +#### `set_movement_type(move_type)` +- **function:** Set movement type +- **Parameters:** + - **move_type: `1` - movel, `0` - moveJ** +#### `get_movement_type()` +- **function:** Get movement type +- **Return value:** + - **`1` - movel, `0` - moveJ** +#### `set_end_type(end)` +- **function:** Set end coordinate system +- **Parameters:** + - **end: int** + - **`0` - flange, `1` - tool** +#### `get_end_type()` +- **function:** Get end coordinate system +- **Return value:** + - **`0` - flange, `1` - tool** + +### 10. 9G Servo machine backgammon + +#### `move_round()` +- **function:** Drive the 9g steering gear clockwise for one revolution +#### `set_four_pieces_zero()` +- **function:** Set the zero position of the four-piece motor +- **Return value:** + - **int: `0` or `1` (1 - success)** + +### 11. Stdio Interface + +#### `get_angles_coords()` +- **function:** Get joint angles and coordinates +#### `get_quick_move_message()` +- **function:** Get the quick move message +#### `get_servo_speeds()` +- **function:** Get joint speed +- **Return value:** + - **A list unit step/s** + +### 12. Servo State Value Interface + +#### `get_servo_currents()` +- **function:** Get all joint current +- **Return value:** + - **A list unit mA** +#### `get_servo_voltages()` +- **function:** Get joint voltages +- **Return value:** + - **A list volts < 24 V** +#### `get_servo_status()` +- **function:** Get joint status +- **Return value:** + - **[voltage, sensor, temperature, current, angle, overload], a value of 0 means no error, a value of 1 indicates an error** +#### `get_servo_temps()` +- **function:** Get joint temperature +- **Return value:** + - **A list unit ** + +### 13. Drag Track Interface + +#### `drag_start_record()` +- **function:** Start track recording +- **Return value:** + - **Recording queue length** +#### `drag_end_record()` +- **function:** End track recording +- **Return value:** + - **Recording queue length** +#### `drag_get_record_data()` +- **function:** Get the recorded track +- **Return value:** + - **List of potential values (encoder values) and operating speeds of each joint** + - **eg: [J1_encoder, J1_run_speed,J2_encoder, J2_run_speed,J3_encoder, J3_run_speed,J4_encoder, J4_run_speed,J5_** + - **encoder, J5_run_speed,J6_encoder, J6_run_speed]** +#### `drag_get_record_len()` +- **function:** Get the total number of recorded points +- **Return value:** + - **Recording queue length** +#### `drag_clear_record_data()` +- **function:** Clear recording track +- **Return value:** + - **Recording queue length 0** \ No newline at end of file diff --git a/docs/MyCobot_280_X5PI_zh.md b/docs/MyCobot_280_X5PI_zh.md new file mode 100644 index 00000000..5269951d --- /dev/null +++ b/docs/MyCobot_280_X5PI_zh.md @@ -0,0 +1,490 @@ +# MyCobot 280 X5 PI + +[toc] + +## Python API ʹ˵ + +APIApplication Programming InterfaceӦó̽ӿڣԤȶĺʹºӿʱڿͷǵAPI⣬޷ɹУ + +```python +# ʾ +from pymycobot import MyCobot280X5PI + +mc = MyCobot280X5PI('COM3') + +# ȡйؽڵǰǶ +angles = mc.get_angles() +print(angles) + +# 1Źؽƶ40ȣٶ20 +mc.send_angle(1, 40, 20) +``` + +### 1. ϵͳ״̬ + +#### `get_modify_version()` +- **:** ȡ޸İ汾 +#### `get_system_version()` +- **:** ȡϵͳ汾 + +### 2. ״̬ + +#### `clear_queue()` +- **:** +#### `async_or_sync()` +- **:** ִģʽ +- **ֵ:** + - **`0`: 첽ģʽ** + - **`1`: ͬģʽ** +#### `get_error_information()` +- **:** ȡ˴Ϣ +- **ֵ:** + - **`0`: ޴** + - **`1~6`: Ӧؽڳ** + - **`16~19`: ײ** + - **`32`: ˶ѧ޽** + - **`33~34`: ֱ˶ڽ** +#### `clear_error_information()` +- **:** ˴Ϣ +#### `power_on()` +- **:** Atomͨ +#### `power_off()` +- **:** رAtomͨ +#### `is_power_on()` +- **:** е۵Դ״̬ +- **ֵ:** + - **`1` - ϵ** + - **`0` - Ѷϵ** + - **`-1` - ݴ** +#### `release_all_servos(data)` +- **:** ͷйؽ +- **:** + - **data: `1` - ȡ(Ĭ)** +#### `read_next_error()` +- **:** ˴ +- **ֵ:** + - **6б** + - **`0`: ** + - **`1`: ͨŶϿ** + - **`2`: ͨŲȶ** + - **`3`: 쳣** +#### `set_fresh_mode(mode)` +- **:** ˢģʽ +- **:** + - **mode: int** + - **`1` - ִ** + - **`0` - ˳ִ** +#### `get_fresh_mode()` +- **:** ѯ˶ģʽ +#### `set_vision_mode(flag)` +- **:** Ӿģʽ(ˢģʽsend_coordsλ˷ת) + - **(Ӿٹʹ)** +- **:** + - **flag: 0/1; `0` - ر; `1` - ** + +### 3. ˶ƽӿ + +#### `get_angles()` +- **:** ȡйؽڽǶ +- **ֵ:** + - **list: Ƕȸб** +#### `send_angle(id, degree, speed)` +- **:** ͵ؽڽǶ +- **:** + - **id : ؽID(1-6)** + - **angle : Ƕֵ(float)** + - **speed : (int) 1~100** +#### `send_angles(angles, speed)` +- **:** йؽڽǶ +- **:** + - **angles: Ƕֵб(List[float])6** + - **speed : (int) 1~100** +#### `get_coords()` +- **:** ȡϵ +- **ֵ:** + - **list: 긡б[x, y, z, rx, ry, rz]** +#### `send_coord(id, coord, speed)` +- **:** ͵ֵ +- **:** + - **id(int) : ID(1-6)** + - **coord(float) : ֵ(mm)** + - **speed(int) : 1~100** +#### `send_coords(coords, speed, mode)` +- **:** ֵ +- **:** + - **coords: ֵб(List[float])[x(mm), y, z, rx(Ƕ), ry, rz]** + - **speed : (int) 1~100** + - **mode : (int) 0 - Ƕģʽ, 1 - ֱģʽ** +#### `pause()` +- **:** ͣ˶ +#### `is_paused()` +- **:** жϻеǷͣ +- **ֵ:** + - **`1` - ͣ** + - **`0` - ** + - **`-1` - ** +#### `resume()` +- **:** ָ˶ +#### `stop()` +- **:** ֹͣ˶ +#### `is_in_position(data, id = 0)` +- **:** жǷ񵽴Ŀλ +- **:** + - **data: λб(ǶȻ)6** + - **id: 1 - ģʽ, 0 - Ƕģʽ** +- **ֵ:** + - **`1` - ** + - **`0` - ** + - **`-1` - ** +#### `is_moving()` +- **:** еǷ˶ +- **ֵ:** + - **`0` - ֹ** + - **`1` - ˶** + - **`-1` - ݴ** +#### `write_angles_time_control(angles, step_time)` +- **:** ʱģʽдؽڽǶ +- **:** + - **angles: Ƕֵ** + - **step_time: ʱ䵥λ30msΧ1~255** + +### 4. JOGģʽ + +#### `jog_angle(joint_id, direction, speed)` +- **:** ؽģʽ㶯 +- **:** + - **joint_id: 1-6** + - **direction: `0` - , `1` - ** + - **speed: (0-100)** +#### `jog_rpy(end_direction, direction, speed)` +- **:** ƻϵ̶תĩ +- **:** + - **end_direction (int): ƫ(1-3)** + - **direction (int): `1` - ת, `0` - ת** + - **speed (int): 1~100** +#### `jog_coord(coord_id, direction, speed)` +- **:** ģʽ㶯 +- **:** + - **coord_id: 1-6** + - **direction: `0` - , `1` - ** + - **speed: (1-100)** +#### `jog_increment_angle(joint_id, increment, speed)` +- **:** ǶȲģʽ +- **:** + - **joint_id: 1-6** + - **increment: Ƕֵ** + - **speed: (0-100)** +#### `jog_increment_coord(axis, increment, speed)` +- **:** 경ģʽ +- **:** + - **axis: ID 1-6** + - **increment: ֵ** + - **speed: (1-100)** +#### `set_HTS_gripper_torque(torque)` +- **:** ӦצŤ +- **:** + - **torque (int): 150~980** +- **ֵ:** + - **0: ʧ** + - **1: óɹ** +#### `get_HTS_gripper_torque()` +- **:** ȡצŤ +- **ֵ:** + - **int: 150~980** +#### `get_gripper_protect_current()` +- **:** ȡצ +- **ֵ:** + - **int: 1~500** +#### `init_gripper()` +- **:** ʼצ +- **ֵ:** + - **int: 01(1-ɹ)** +#### `set_gripper_protect_current(current)` +- **:** üצ +- **:** + - **current (int): 1~500** +#### `set_encoder(joint_id, encoder, speed)` +- **:** õؽڵλֵ +- **:** + - **joint_id: 1-6** + - **צӦؽIDΪ7** + - **encoder: Ŀλֵ** + - **speed : 1-100** +#### `get_encoder(joint_id)` +- **:** ȡָؽڵλֵ +- **:** + - **joint_id: (int) 1-6** + - **צӦؽIDΪ7** +#### `set_encoders(encoders, sp)` +- **:** ͬйؽڵλֵ +- **:** + - **encoders: λֵб6** + - **sp: ٶ1~100** +#### `get_encoders()` +- **:** ȡйؽڱֵ +- **ֵ:** + - **ֵб** +#### `set_encoders_drag(encoders, speeds)` +- **:** бֵٶ +- **:** + - **encoders: б** + - **speeds: ͨget_servo_speeds()ȡٶֵ** +#### `get_joint_min_angle(joint_id)` +- **:** ȡָؽСǶ +- **:** + - **joint_id: 1-6** +- **ֵ:** + - **Ƕֵ(float)** +#### `get_joint_max_angle(joint_id)` +- **:** ȡָؽǶ +- **:** + - **joint_id: 1-6** +- **ֵ:** + - **Ƕֵ(float)** +#### `set_joint_min(id, angle)` +- **:** ùؽСǶ +- **:** + - **id: int** + - **ؽID 1-6** + - **צӦؽIDΪ7** + - **angle: 0~180** +#### `set_joint_max(id, angle)` +- **:** ùؽǶ +- **:** + - **id: int** + - **ؽID 1-6** + - **צӦؽIDΪ7** + - **angle: 0~180** + +### 5. + +#### `is_servo_enable(servo_id)` +- **:** ⵥؽ״̬ +- **:** + - **servo_id: 1-6** +- **ֵ:** + - **`0` - δʹ** + - **`1` - ʹ** + - **`-1` - ** +#### `is_all_servo_enable()` +- **:** йؽ״̬ +- **ֵ:** + - **`0` - δʹ** + - **`1` - ʹ** + - **`-1` - ** +#### `set_servo_data(servo_id, data_id, value, mode)` +- **:** öַָ +- **:** + - **servo_id: ؽڶ1-7** + - **data_id: ݵַ** + - **value: 0-4096** + - **mode: 0 - ֵֽ(Ĭ), 1 - ˫ֵֽ** +#### `get_servo_data(servo_id, data_id, mode)` +- **:** ȡַָ +- **:** + - **servo_id: ؽڶ1-7** + - **data_id: ݵַ** + - **mode: 0 - ֵֽ(Ĭ), 1 - ˫ֵֽ** +- **ֵ:** + - **0-4096Χֵ** +#### `set_servo_calibration(servo_id)` +- **:** У׼ؽڵǰλΪ(Ӧλֵ2048) +- **:** + - **servo_id: ؽڶ1-6** +#### `joint_brake(joint_id)` +- **:** ؽ˶мͣ(뵱ǰٶ) +- **:** + - **joint_id: 1-6** +#### `release_servo(servo_id, mode)` +- **:** µָ +- **:** + - **servo_id: 1-6** + - **mode: ĬᣬΪ1ȡ** +#### `focus_servo(servo_id)` +- **:** ϵָ +- **:** + - **servo_id: 1-6** + +### 6. צ + +#### `get_gripper_value(gripper_type)` +- **:** ȡצ״ֵ̬ +- **:** + - **gripper_type (int): Ĭ1** + - **`1`: Ӧצ** + - **`3`: ƽмצ** + - **`4`: Լצ** +- **ֵ:** + - **צ״ֵ̬(int)** +#### `set_gripper_state(flag, speed, _type_1, is_torque)` +- **:** üצ״̬ +- **:** + - **flag (int): 0 - , 1 - ر, 254 - ͷ** + - **speed (int): 1~100** + - **_type_1 (int): Ĭ1** + - **`1` : Ӧצ** + - **`2` : ָ** + - **`3` : ƽмצ(ʡ)** + - **`4` : Լצ** + - **is_torque (int): Ͳʱʡ** + - **`1`: ** + - **`0`: ** +#### `set_gripper_value(gripper_value, speed, gripper_type, is_torque)` +- **:** üצ϶ +- **:** + - **gripper_value (int): 0~100** + - **speed (int): 1~100** + - **gripper_type (int): Ĭ1** + - **`1`: Ӧצ** + - **`3`: ƽмצ(ʡ)** + - **`4`: Լצ** + - **is_torque (int): Ͳʱʡ** + - **`1`: ** + - **`0`: ** +#### `set_gripper_calibration()` +- **:** õǰλΪ(Ӧλֵ2048) +#### `is_gripper_moving()` +- **:** жϼצǷ˶ +- **ֵ:** + - **`0` - ֹ** + - **`1` - ˶** + - **`-1` - ݴ** + +### 7. ĩATOM + +#### `get_tool_system_version()` +- **:** ȡĩΰ汾 +#### `get_tool_modify_version()` +- **:** ȡĩ޸İ汾 +#### `is_tool_connected()` +- **:** ĩ״̬ +#### `set_color(r = 0, g = 0, b = 0)` +- **:** ûе۶ƹɫ +- **:** + - **r (int): 0~255** + - **g (int): 0~255** + - **b (int): 0~255** +#### `is_tool_button_click()` +- **:** ĩ˰ťǷ +#### `set_digital_output(pin_no, pin_signal)` +- **:** ĩAtom IO״̬ +- **:** + - **pin_no (int): ź** + - **pin_signal (int): 0/1** +#### `get_digital_input(pin_no)` +- **:** ȡźֵ + +### 8. ˶ѧ㷨ӿ + +#### `solve_inv_kinematics(target_coords, current_angles)` +- **:** ĿתΪؽڽǶ +- **:** + - **target_coords: Ŀ긡б** + - **current_angles : ǰǶȸб** +- **ֵ:** + - **list: ؽڽǶȸб** + +### 9. ϵӿ + +#### `set_tool_reference(coords)` +- **:** ùϵ +- **:** + - **coords: ֵб(List[float])** + - **[x(mm), y, z, rx(Ƕ), ry, rz]** +#### `get_tool_reference()` +- **:** ȡϵ +#### `set_world_reference(coords)` +- **:** ϵ +- **:** + - **coords: ֵб(List[float])** + - **[x(mm), y, z, rx(Ƕ), ry, rz]** +#### `get_world_reference()` +- **:** ȡϵ +#### `set_reference_frame(rftype)` +- **:** ûϵ +- **:** + - **rftype: 0 - ϵ, 1 - ϵ** +#### `get_reference_frame()` +- **:** ȡϵ +- **ֵ:** + - **`0` - ϵ, `1` - ϵ** +#### `set_movement_type(move_type)` +- **:** ˶ +- **:** + - **move_type: `1` - ֱ˶, `0` - ؽ˶** +#### `get_movement_type()` +- **:** ȡ˶ +- **ֵ:** + - **`1` - ֱ˶, `0` - ؽ˶** +#### `set_end_type(end)` +- **:** ĩϵ +- **:** + - **end: int** + - **`0` - ϵ, `1` - ϵ** +#### `get_end_type()` +- **:** ȡĩϵ +- **ֵ:** + - **`0` - ϵ, `1` - ϵ** + +### 10. 9G + +#### `move_round()` +- **:** 9g˳ʱתһ +#### `set_four_pieces_zero()` +- **:** ļ׵λ +- **ֵ:** + - **int: `0``1`(1-ɹ)** + +### 11. ׼ӿ + +#### `get_angles_coords()` +- **:** ȡؽڽǶȺ +#### `get_quick_move_message()` +- **:** ȡƶϢ +#### `get_servo_speeds()` +- **:** ȡؽٶ +- **ֵ:** + - **ٶб(λ:/)** + +### 12. ״ֵ̬ӿ + +#### `get_servo_currents()` +- **:** ȡйؽڵ +- **ֵ:** + - **б(λmA)** +#### `get_servo_voltages()` +- **:** ȡؽڵѹ +- **ֵ:** + - **ѹб(С24V)** +#### `get_servo_status()` +- **:** ȡؽ״̬ +- **ֵ:** + - **[ѹ, , ¶, , Ƕ, ], 0ʾ1ʾ쳣** +#### `get_servo_temps()` +- **:** ȡؽ¶ +- **ֵ:** + - **¶б(λ)** + +### 13. ϶켣ӿ + +#### `drag_start_record()` +- **:** ʼ켣¼ +- **ֵ:** + - **¼г** +#### `drag_end_record()` +- **:** 켣¼ +- **ֵ:** + - **¼г** +#### `drag_get_record_data()` +- **:** ȡѼ¼켣 +- **ֵ:** + - **ؽڵλֵ(ֵ)ٶȵб** + - **ʾ: [J1ֵ,J1ٶ,J2ֵ,J2ٶ,J3ֵ,J3ٶ,J4ֵ,J4ٶ,J5ֵ,J5ٶ,J6ֵ,J6ٶ]** +#### `drag_get_record_len()` +- **:** ȡѼ¼ +- **ֵ:** + - **¼г** +#### `drag_clear_record_data()` +- **:** Ѽ¼켣 +- **ֵ:** + - **¼гȹ** diff --git a/pymycobot/mycobot280x5pi.py b/pymycobot/mycobot280x5pi.py index bb4a331e..59942327 100644 --- a/pymycobot/mycobot280x5pi.py +++ b/pymycobot/mycobot280x5pi.py @@ -4,7 +4,6 @@ import functools import time -import math import threading import serial @@ -284,46 +283,6 @@ def set_vision_mode(self, flag): self.calibration_parameters(flag=flag) return self._mesg(ProtocolCode.SET_VISION_MODE, flag) - # Overall Status - def set_free_mode(self, flag): - """set to free mode - - Args: - flag: 0/1 - """ - self.calibration_parameters(flag=flag) - return self._mesg(ProtocolCode.SET_FREE_MODE, flag) - - def is_free_mode(self): - """Check if it is free mode - - Return: - 0/1 - """ - return self._mesg(ProtocolCode.IS_FREE_MODE, has_reply=True) - - # MDI mode and operation - def get_radians(self): - """Get the radians of all joints - - Return: - list: A list of float radians [radian1, ...] - """ - angles = self._mesg(ProtocolCode.GET_ANGLES, has_reply=True) - return [round(angle * (math.pi / 180), 3) for angle in angles] - - def send_radians(self, radians, speed): - """Send the radians of all joints to robot arm - - Args: - radians: a list of radian values( List[float]), length 6 - speed: (int )0 ~ 100 - """ - calibration_parameters(len6=radians, speed=speed) - degrees = [self._angle2int(radian * (180 / math.pi)) - for radian in radians] - return self._mesg(ProtocolCode.SEND_ANGLES, degrees, speed) - def sync_send_angles(self, degrees, speed, timeout=15): """Send the angle in synchronous state and return when the target point is reached @@ -504,7 +463,7 @@ def get_tool_system_version(self): """ return self._mesg(ProtocolCode.GET_ATOM_VERSION, has_reply=True) - def get_tool_modified_version(self): + def get_tool_modify_version(self): """ Read the terminal modified version number """ @@ -589,6 +548,14 @@ def get_servo_speeds(self): """ return self._mesg(ProtocolCode.GET_SERVO_SPEED, has_reply=True) + def get_servo_currents(self): + """Get all joint current + + Return: + A list unit mA + """ + return self._mesg(ProtocolCode.GET_SERVO_CURRENTS, has_reply=True) + def get_servo_voltages(self): """Get joint voltages From 6156a5047205b0643115d94ec9cba5d3944be2cc Mon Sep 17 00:00:00 2001 From: Mrkun5018 <2220138602@qq.com> Date: Thu, 13 Feb 2025 18:20:33 +0800 Subject: [PATCH 03/19] Fix MyCobot 280 X5 Pi doc errors --- docs/MyCobot_280_X5PI_en.md | 2 +- docs/MyCobot_280_X5PI_zh.md | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/docs/MyCobot_280_X5PI_en.md b/docs/MyCobot_280_X5PI_en.md index 0ebf4910..cf8bf1df 100644 --- a/docs/MyCobot_280_X5PI_en.md +++ b/docs/MyCobot_280_X5PI_en.md @@ -10,7 +10,7 @@ API (Application Programming Interface), also known as Application Programming I # Example from pymycobot import MyCobot280X5PI -mc = MyCobot280X5PI('COM3') +mc = MyCobot280X5PI('/dev/ttyS1') # Gets the current angle of all joints angles = mc.get_angles() diff --git a/docs/MyCobot_280_X5PI_zh.md b/docs/MyCobot_280_X5PI_zh.md index 5269951d..6578ce47 100644 --- a/docs/MyCobot_280_X5PI_zh.md +++ b/docs/MyCobot_280_X5PI_zh.md @@ -10,7 +10,7 @@ API # ʾ from pymycobot import MyCobot280X5PI -mc = MyCobot280X5PI('COM3') +mc = MyCobot280X5PI('/dev/ttyS1') # ȡйؽڵǰǶ angles = mc.get_angles() From 3021fa25949a0e246486683f0fec9f50d6d1f31e Mon Sep 17 00:00:00 2001 From: Mrkun5018 <2220138602@qq.com> Date: Fri, 14 Feb 2025 15:57:27 +0800 Subject: [PATCH 04/19] add MyCobot 280 X5 PI sokcet sever&client --- demo/Server_280_X5PI.py | 203 ++++++++++++++++++++++++++++++++++++ pymycobot/__init__.py | 5 +- pymycobot/mycobot280x5pi.py | 144 ++++++++++++++++++++----- 3 files changed, 325 insertions(+), 27 deletions(-) create mode 100644 demo/Server_280_X5PI.py diff --git a/demo/Server_280_X5PI.py b/demo/Server_280_X5PI.py new file mode 100644 index 00000000..03cc6ec1 --- /dev/null +++ b/demo/Server_280_X5PI.py @@ -0,0 +1,203 @@ +#!/usr/bin/env python +# -*- coding: UTF-8 -*- +import fcntl +import logging +import traceback +import typing as T +import socket +import struct +import serial +import Hobot.GPIO as GPIO + +GPIO.setwarnings(False) + + +class GPIOProtocolCode: + SETUP_GPIO_MODE = 0xAA + SETUP_GPIO_STATE = 0xAB + SET_GPIO_OUTPUT = 0xAC + GET_GPIO_INPUT = 0xAD + + +def to_string(data: bytes) -> str: + return ' '.join(map(lambda x: f'{x:02x}', data)) + + +def get_local_host(name: str) -> T.Optional[str]: + host = None + dgram_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) + try: + pack_res = struct.pack('256s', bytes(name, encoding="utf8")) + host = socket.inet_ntoa(fcntl.ioctl(dgram_socket.fileno(), 0x8915, pack_res)[20:24]) + except Exception as e: + print(e) + finally: + dgram_socket.close() + return host + + +localhost = get_local_host("wlan0") + + +class SocketTransport(object): + def __init__(self, host: str = "0.0.0.0", port: int = 30002): + self.port = port + self.host = host + self.running = True + self.log = logging.getLogger("socket") + self.socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM) + self.socket.bind((host, port)) + self.socket.listen(5) + if host == "0.0.0.0": + host = localhost + self.log.info(f"start listening on {host}:{port}") + + def accept(self): + while self.running is True: + yield self.socket.accept() + + def context(self, conn: socket.socket, buffer_size: int = 1024): + while self.running is True: + try: + data_buffer = conn.recv(buffer_size) + if len(data_buffer) == 0: + break + yield data_buffer + except Exception as e: + self.log.error(f"error while reading socket: {e}") + traceback.print_exc() + break + + def close(self): + self.log.info(f"close socket on {self.host}:{self.port}") + self.running = False + self.socket.close() + + +class SerialTransport(object): + def __init__(self, comport: str = "/dev/ttyS1", baudrate: int = 100_0000, timeout: float = None): + self.serial = serial.Serial(port=comport, baudrate=baudrate, timeout=timeout) + self.log = logging.getLogger("serial") + self.baudrate = baudrate + self.comport = comport + self.open() + self.log.info(f"start serial on [{self.comport}] with baudrate [{self.baudrate}]") + + def send(self, data: bytes): + self.serial.write(data) + + def recv(self, size: int = 1024) -> bytes: + return self.serial.read(size) + + @property + def is_open(self): + return self.serial.is_open + + def close(self): + self.serial.close() + + def open(self): + if not self.serial.is_open: + self.serial.open() + + +class Server280X5PI(object): + """ + Server for 280 X5 Pi + + 1. System GPIO operating protocol adheres to protocol MyCobot 280 x5 PI. + 2. This server only does the work of forwarding protocol and does not participate in the analysis of instructions. + 3. The server is only responsible for forwarding the data received from the socket to the serial port and vice versa. + 4. Instruction parsing is done entirely by the client + 5. The server is responsible for setting the GPIO mode + """ + def __init__(self, socket_transport: SocketTransport, serial_transport: SerialTransport, debug=True): + self.debug = debug + self.socket_transport = socket_transport + self.serial_transport = serial_transport + self.log = logging.getLogger("server") + + def mainloop(self): + try: + self.log.info("tcp server started.") + for conn, addr in self.socket_transport.accept(): + self.log.info(f"{addr} accepted!") + for data in self.socket_transport.context(conn, buffer_size=1024): + self.serial_transport.log.info(f"{addr} recv << {to_string(data)}") + if data[3] == GPIOProtocolCode.SETUP_GPIO_MODE: + try: + mode = GPIO.BCM if data[4] == 0 else GPIO.BOARD + GPIO.setmode(mode) + self.log.debug(f"{addr} setup gpio mode => {mode}") + serial_data = bytes([0xfe, 0xfe, 0x03, data[3], 0x01, 0xfa]) + except Exception as e: + self.log.error(f"{addr} setup gpio mode error: {e}") + serial_data = bytes([0xfe, 0xfe, 0x03, data[3], 0xff, 0xfa]) + + elif data[3] == GPIOProtocolCode.SETUP_GPIO_STATE: + try: + mode = GPIO.OUT if data[5] == 1 else GPIO.IN + level = GPIO.HIGH if data[6] == 1 else GPIO.LOW + self.log.debug(f"{addr} setup gpio state, mode => {mode}, level => {level}") + GPIO.setup(data[4], mode, initial=level) + serial_data = bytes([0xfe, 0xfe, 0x03, data[3], 0x01, 0xfa]) + except Exception as e: + self.log.error(f"{addr} setup gpio state error: {e}") + serial_data = bytes([0xfe, 0xfe, 0x03, data[3], 0xff, 0xfa]) + + elif data[3] == GPIOProtocolCode.SET_GPIO_OUTPUT: + try: + level = GPIO.HIGH if data[5] == 1 else GPIO.LOW + self.log.debug(f"{addr} set gpio output, level => {level}") + GPIO.output(data[4], level) + serial_data = bytes([0xfe, 0xfe, 0x03, data[3], 0x01, 0xfa]) + except Exception as e: + self.log.error(f"{addr} set gpio output error: {e}") + serial_data = bytes([0xfe, 0xfe, 0x03, data[3], 0xff, 0xfa]) + + elif data[3] == GPIOProtocolCode.GET_GPIO_INPUT: + try: + self.log.debug(f"{addr} get gpio input, channel => {data[4]}") + level = GPIO.input(data[4]) + self.log.debug(f"{addr} get gpio input, level => {level}") + serial_data = bytes([0xfe, 0xfe, 0x03, data[3], level, 0xfa]) + except Exception as e: + self.log.error(f"{addr} get gpio input error: {e}") + serial_data = bytes([0xfe, 0xfe, 0x03, data[3], 0xff, 0xfa]) + else: + self.serial_transport.send(data) + serial_data = self.serial_transport.recv() + + self.serial_transport.log.info(f"{addr} send >> {to_string(serial_data)}") + conn.send(serial_data) + else: + self.log.info(f"{addr} closed!") + except Exception as e: + self.log.error(f"server error: {e}") + self.log.exception(traceback.format_exc()) + finally: + self.socket_transport.close() + self.serial_transport.close() + self.log.info("server closed") + + +def main(debug: bool = False): + logging.basicConfig( + level=logging.DEBUG if debug else logging.INFO, + format="%(asctime)s - [%(name)s] %(levelname)7s - %(message)s", + handlers=[ + logging.StreamHandler(), + logging.FileHandler("server.log") + ] + ) + + socket_transport = SocketTransport(host="0.0.0.0", port=30002) + + serial_transport = SerialTransport(comport="/dev/ttyS1", baudrate=100_0000, timeout=0.1) + + Server280X5PI(socket_transport, serial_transport).mainloop() + GPIO.cleanup() + + +if __name__ == '__main__': + main() diff --git a/pymycobot/__init__.py b/pymycobot/__init__.py index 157cc6f8..e3e13144 100644 --- a/pymycobot/__init__.py +++ b/pymycobot/__init__.py @@ -4,7 +4,7 @@ import datetime import sys from pymycobot.mycobot280 import MyCobot280 -from pymycobot.mycobot280x5pi import MyCobot280X5PI +from pymycobot.mycobot280x5pi import MyCobot280X5PI, MyCobot280X5PISocket from pymycobot.mypalletizer260 import MyPalletizer260 from pymycobot.mecharm270 import MechArm270 from pymycobot.mycobot280socket import MyCobot280Socket @@ -79,7 +79,8 @@ "MyArmMControl", "ChassisControl", "ConveyorAPI", - "MyCobot280X5PI" + "MyCobot280X5PI", + "MyCobot280X5PISocket" ] if sys.platform == "linux": diff --git a/pymycobot/mycobot280x5pi.py b/pymycobot/mycobot280x5pi.py index 59942327..d3200788 100644 --- a/pymycobot/mycobot280x5pi.py +++ b/pymycobot/mycobot280x5pi.py @@ -3,6 +3,7 @@ from __future__ import division import functools +import socket import time import threading import serial @@ -12,7 +13,7 @@ from pymycobot.error import calibration_parameters -def setup_serial_port(port, baudrate, timeout): +def setup_serial_connect(port, baudrate, timeout): serial_api = serial.Serial(port=port, baudrate=baudrate, timeout=timeout) serial_api.rts = False if not serial_api.is_open: @@ -20,18 +21,28 @@ def setup_serial_port(port, baudrate, timeout): return serial_api +def setup_socket_connect(host, port, timeout): + socket_api = socket.socket(socket.AF_INET, socket.SOCK_STREAM) + socket_api.settimeout(timeout) + socket_api.connect((host, port)) + return socket_api + + +class GPIOProtocolCode: + SETUP_GPIO_MODE = 0xAA + SETUP_GPIO_STATE = 0xAB + SET_GPIO_OUTPUT = 0xAC + GET_GPIO_INPUT = 0xAD + + class MyCobot280X5Api(CommandGenerator): - def __init__(self, port, baudrate=100_0000, timeout=0.1, debug=False, thread_lock=True): + def __init__(self, debug=False, thread_lock=True): super(MyCobot280X5Api, self).__init__(debug) - self._serial_port = setup_serial_port(port, baudrate, timeout) + self.calibration_parameters = functools.partial(calibration_parameters, class_name="MyCobot280") self.thread_lock = thread_lock self.lock = threading.Lock() - self.calibration_parameters = functools.partial(calibration_parameters, class_name="MyCobot280") - self._write = functools.partial(write, self) - self._read = functools.partial(read, self) - def _mesg(self, genre, *args, **kwargs): """ @@ -161,24 +172,8 @@ def wait(self, t): time.sleep(t) return self - def close(self): - self._serial_port.close() - def open(self): - self._serial_port.open() - - -class MyCobot280X5PI(MyCobot280X5Api): - - def __init__(self, port, baudrate=100_0000, timeout=0.1, debug=False, thread_lock=True): - """ - Args: - port : port string - baudrate : baud rate int, default 100_0000 - timeout : default 0.1 - debug : whether show debug info - """ - super().__init__(port, baudrate, timeout, debug, thread_lock) +class MyCobot280X5PICommandGenerator(MyCobot280X5Api): # System Status def get_modify_version(self): @@ -801,4 +796,103 @@ def drag_clear_record_data(self): Recording queue length 0 """ - return self._mesg(ProtocolCode.DRAG_CLEAR_RECORD_DATA, has_reply=True) \ No newline at end of file + return self._mesg(ProtocolCode.DRAG_CLEAR_RECORD_DATA, has_reply=True) + + +class MyCobot280X5PI(MyCobot280X5PICommandGenerator): + + def __init__(self, port, baudrate=100_0000, timeout=0.1, debug=False, thread_lock=True): + """ + Args: + port : port string + baudrate : baud rate int, default 100_0000 + timeout : default 0.1 + debug : whether show debug info + """ + super().__init__(debug, thread_lock) + self._serial_port = setup_serial_connect(port=port, baudrate=baudrate, timeout=timeout) + self._write = functools.partial(write, self) + self._read = functools.partial(read, self) + + def close(self): + self._serial_port.close() + + def open(self): + self._serial_port.open() + + +class MyCobot280X5PISocket(MyCobot280X5PICommandGenerator): + """MyCobot 280 X5 PI Socket Control Class + + server file: https://github.com/elephantrobotics/pymycobot/demo/Server_280_X5PI.py + """ + def __init__(self, ip, port=30002, timeout=0.1, debug=False, thread_lock=True): + super().__init__(debug, thread_lock) + self.sock = setup_socket_connect(ip, port, timeout) + self._write = functools.partial(write, self, method="socket") + self._read = functools.partial(read, self, method="socket") + + def set_gpio_mode(self, mode): + """Set pin coding method + Args: + mode: (int) 0 - BCM, 1 - BOARD + + returns: + (int) 1 - success, 255 - error + """ + if mode not in (0, 1): + raise ValueError("mode must be 0 or 1") + return self._mesg(GPIOProtocolCode.SETUP_GPIO_MODE, mode) + + def setup_gpio_state(self, pin_no, mode, initial=1): + """Set the pin as input or output + Args: + pin_no: (int) pin id + mode: (int) 0 - input, 1 - output + initial: (int) 0 - low, 1 - high + returns: + (int) 1 - success, 255 - error + """ + if mode not in (0, 1): + raise ValueError("mode must be 0 or 1") + + if initial not in (0, 1): + raise ValueError("initial must be 0 or 1") + + return self._mesg(GPIOProtocolCode.SETUP_GPIO_STATE, pin_no, mode, initial) + + def set_gpio_output(self, pin_no, state): + """Set the pin to high or low level + Args: + pin_no: (int) pin id. + state: (int) 0 - low, 1 - high + returns: + (int) 1 - success, 255 - error + """ + return self._mesg(GPIOProtocolCode.SET_GPIO_OUTPUT, pin_no, state) + + def get_gpio_input(self, pin_no): + """Get pin level status. + Args: + pin_no: (int) pin id. + Returns: + (int) 0 - low, 1 - high, 255 - error + """ + return self._mesg(GPIOProtocolCode.GET_GPIO_INPUT, pin_no, has_reply=True) + + def close(self): + self.sock.close() + + +def main(): + mc_sock = MyCobot280X5PISocket('192.168.1.246', port=30002, debug=True) + # print(mc_sock.send_angle(1, 100, 50)) + # print(mc_sock.get_quick_move_message()) + print(mc_sock.set_gpio_mode(0)) + print(mc_sock.setup_gpio_state(5, 1, initial=1)) + print(mc_sock.set_gpio_output(5, 0)) + # print(mc_sock.get_gpio_input(5)) + + +if __name__ == '__main__': + main() From d6893f61af826ce11680104609b595a33980746f Mon Sep 17 00:00:00 2001 From: Mrkun5018 <2220138602@qq.com> Date: Fri, 14 Feb 2025 16:12:07 +0800 Subject: [PATCH 05/19] MyCobot 280 X5 PI server compatible with python2 --- demo/Server_280_X5PI.py | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/demo/Server_280_X5PI.py b/demo/Server_280_X5PI.py index 03cc6ec1..7f38b2d9 100644 --- a/demo/Server_280_X5PI.py +++ b/demo/Server_280_X5PI.py @@ -19,11 +19,11 @@ class GPIOProtocolCode: GET_GPIO_INPUT = 0xAD -def to_string(data: bytes) -> str: +def to_string(data: bytes): return ' '.join(map(lambda x: f'{x:02x}', data)) -def get_local_host(name: str) -> T.Optional[str]: +def get_local_host(name: str): host = None dgram_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) try: @@ -40,7 +40,7 @@ def get_local_host(name: str) -> T.Optional[str]: class SocketTransport(object): - def __init__(self, host: str = "0.0.0.0", port: int = 30002): + def __init__(self, host="0.0.0.0", port=30002): self.port = port self.host = host self.running = True @@ -56,7 +56,7 @@ def accept(self): while self.running is True: yield self.socket.accept() - def context(self, conn: socket.socket, buffer_size: int = 1024): + def context(self, conn, buffer_size=1024): while self.running is True: try: data_buffer = conn.recv(buffer_size) @@ -75,7 +75,7 @@ def close(self): class SerialTransport(object): - def __init__(self, comport: str = "/dev/ttyS1", baudrate: int = 100_0000, timeout: float = None): + def __init__(self, comport="/dev/ttyS1", baudrate=100_0000, timeout=None): self.serial = serial.Serial(port=comport, baudrate=baudrate, timeout=timeout) self.log = logging.getLogger("serial") self.baudrate = baudrate @@ -83,10 +83,10 @@ def __init__(self, comport: str = "/dev/ttyS1", baudrate: int = 100_0000, timeou self.open() self.log.info(f"start serial on [{self.comport}] with baudrate [{self.baudrate}]") - def send(self, data: bytes): + def send(self, data): self.serial.write(data) - def recv(self, size: int = 1024) -> bytes: + def recv(self, size=1024): return self.serial.read(size) @property @@ -111,7 +111,7 @@ class Server280X5PI(object): 4. Instruction parsing is done entirely by the client 5. The server is responsible for setting the GPIO mode """ - def __init__(self, socket_transport: SocketTransport, serial_transport: SerialTransport, debug=True): + def __init__(self, socket_transport, serial_transport, debug=True): self.debug = debug self.socket_transport = socket_transport self.serial_transport = serial_transport @@ -181,7 +181,7 @@ def mainloop(self): self.log.info("server closed") -def main(debug: bool = False): +def main(debug=False): logging.basicConfig( level=logging.DEBUG if debug else logging.INFO, format="%(asctime)s - [%(name)s] %(levelname)7s - %(message)s", From 1bd4e1fefd219be37cfe965c8228e76f81ae22ab Mon Sep 17 00:00:00 2001 From: Mrkun5018 <2220138602@qq.com> Date: Mon, 17 Feb 2025 17:51:39 +0800 Subject: [PATCH 06/19] rename MyCobot280x5pi to MyCobot280RDKX5 --- ...{Server_280_X5PI.py => Server_280_RDKX5.py} | 9 ++++----- ..._280_X5PI_en.md => MyCobot_280_RDKX5_en.md} | 6 +++--- ..._280_X5PI_zh.md => MyCobot_280_RDKX5_zh.md} | 4 ++-- pymycobot/__init__.py | 6 +++--- .../{mycobot280x5pi.py => mycobot280rdkx5.py} | 18 +++++++++--------- 5 files changed, 21 insertions(+), 22 deletions(-) rename demo/{Server_280_X5PI.py => Server_280_RDKX5.py} (98%) rename docs/{MyCobot_280_X5PI_en.md => MyCobot_280_RDKX5_en.md} (99%) rename docs/{MyCobot_280_X5PI_zh.md => MyCobot_280_RDKX5_zh.md} (99%) rename pymycobot/{mycobot280x5pi.py => mycobot280rdkx5.py} (98%) diff --git a/demo/Server_280_X5PI.py b/demo/Server_280_RDKX5.py similarity index 98% rename from demo/Server_280_X5PI.py rename to demo/Server_280_RDKX5.py index 7f38b2d9..b6e9ff41 100644 --- a/demo/Server_280_X5PI.py +++ b/demo/Server_280_RDKX5.py @@ -3,7 +3,6 @@ import fcntl import logging import traceback -import typing as T import socket import struct import serial @@ -101,11 +100,11 @@ def open(self): self.serial.open() -class Server280X5PI(object): +class MyCobot280RDKX5Server(object): """ - Server for 280 X5 Pi + Server for 280 RDK-X5 - 1. System GPIO operating protocol adheres to protocol MyCobot 280 x5 PI. + 1. System GPIO operating protocol adheres to protocol MyCobot 280 RDK X5. 2. This server only does the work of forwarding protocol and does not participate in the analysis of instructions. 3. The server is only responsible for forwarding the data received from the socket to the serial port and vice versa. 4. Instruction parsing is done entirely by the client @@ -195,7 +194,7 @@ def main(debug=False): serial_transport = SerialTransport(comport="/dev/ttyS1", baudrate=100_0000, timeout=0.1) - Server280X5PI(socket_transport, serial_transport).mainloop() + MyCobot280RDKX5Server(socket_transport, serial_transport).mainloop() GPIO.cleanup() diff --git a/docs/MyCobot_280_X5PI_en.md b/docs/MyCobot_280_RDKX5_en.md similarity index 99% rename from docs/MyCobot_280_X5PI_en.md rename to docs/MyCobot_280_RDKX5_en.md index cf8bf1df..94d4e777 100644 --- a/docs/MyCobot_280_X5PI_en.md +++ b/docs/MyCobot_280_RDKX5_en.md @@ -8,9 +8,9 @@ API (Application Programming Interface), also known as Application Programming I ```python # Example -from pymycobot import MyCobot280X5PI +from pymycobot import MyCobot280RDKX5 -mc = MyCobot280X5PI('/dev/ttyS1') +mc = MyCobot280RDKX5('/dev/ttyS1') # Gets the current angle of all joints angles = mc.get_angles() @@ -464,7 +464,7 @@ mc.send_angle(1, 40, 20) #### `get_servo_temps()` - **function:** Get joint temperature - **Return value:** - - **A list unit ** + - **A list unit ��** ### 13. Drag Track Interface diff --git a/docs/MyCobot_280_X5PI_zh.md b/docs/MyCobot_280_RDKX5_zh.md similarity index 99% rename from docs/MyCobot_280_X5PI_zh.md rename to docs/MyCobot_280_RDKX5_zh.md index 6578ce47..85822b93 100644 --- a/docs/MyCobot_280_X5PI_zh.md +++ b/docs/MyCobot_280_RDKX5_zh.md @@ -8,9 +8,9 @@ API ```python # ʾ -from pymycobot import MyCobot280X5PI +from pymycobot import MyCobot280RDKX5 -mc = MyCobot280X5PI('/dev/ttyS1') +mc = MyCobot280RDKX5('/dev/ttyS1') # ȡйؽڵǰǶ angles = mc.get_angles() diff --git a/pymycobot/__init__.py b/pymycobot/__init__.py index e3e13144..1c1718fa 100644 --- a/pymycobot/__init__.py +++ b/pymycobot/__init__.py @@ -4,7 +4,7 @@ import datetime import sys from pymycobot.mycobot280 import MyCobot280 -from pymycobot.mycobot280x5pi import MyCobot280X5PI, MyCobot280X5PISocket +from pymycobot.mycobot280rdkx5 import MyCobot280RDKX5, MyCobot280RDKX5Socket from pymycobot.mypalletizer260 import MyPalletizer260 from pymycobot.mecharm270 import MechArm270 from pymycobot.mycobot280socket import MyCobot280Socket @@ -79,8 +79,8 @@ "MyArmMControl", "ChassisControl", "ConveyorAPI", - "MyCobot280X5PI", - "MyCobot280X5PISocket" + "MyCobot280RDKX5", + "MyCobot280RDKX5Socket" ] if sys.platform == "linux": diff --git a/pymycobot/mycobot280x5pi.py b/pymycobot/mycobot280rdkx5.py similarity index 98% rename from pymycobot/mycobot280x5pi.py rename to pymycobot/mycobot280rdkx5.py index d3200788..e73b34da 100644 --- a/pymycobot/mycobot280x5pi.py +++ b/pymycobot/mycobot280rdkx5.py @@ -35,10 +35,10 @@ class GPIOProtocolCode: GET_GPIO_INPUT = 0xAD -class MyCobot280X5Api(CommandGenerator): +class MyCobot280RDKX5Api(CommandGenerator): def __init__(self, debug=False, thread_lock=True): - super(MyCobot280X5Api, self).__init__(debug) + super(MyCobot280RDKX5Api, self).__init__(debug) self.calibration_parameters = functools.partial(calibration_parameters, class_name="MyCobot280") self.thread_lock = thread_lock self.lock = threading.Lock() @@ -55,7 +55,7 @@ def _mesg(self, genre, *args, **kwargs): **kwargs: support `has_reply` has_reply: Whether there is a return value to accept. """ - real_command, has_reply, _ = super(MyCobot280X5Api, self)._mesg(genre, *args, **kwargs) + real_command, has_reply, _ = super(MyCobot280RDKX5Api, self)._mesg(genre, *args, **kwargs) if self.thread_lock: with self.lock: return self._res(real_command, has_reply, genre) @@ -173,7 +173,7 @@ def wait(self, t): return self -class MyCobot280X5PICommandGenerator(MyCobot280X5Api): +class MyCobot280RDKX5CommandGenerator(MyCobot280RDKX5Api): # System Status def get_modify_version(self): @@ -799,7 +799,7 @@ def drag_clear_record_data(self): return self._mesg(ProtocolCode.DRAG_CLEAR_RECORD_DATA, has_reply=True) -class MyCobot280X5PI(MyCobot280X5PICommandGenerator): +class MyCobot280RDKX5(MyCobot280RDKX5CommandGenerator): def __init__(self, port, baudrate=100_0000, timeout=0.1, debug=False, thread_lock=True): """ @@ -821,10 +821,10 @@ def open(self): self._serial_port.open() -class MyCobot280X5PISocket(MyCobot280X5PICommandGenerator): - """MyCobot 280 X5 PI Socket Control Class +class MyCobot280RDKX5Socket(MyCobot280RDKX5CommandGenerator): + """MyCobot 280 RDK X5 Socket Control Class - server file: https://github.com/elephantrobotics/pymycobot/demo/Server_280_X5PI.py + server file: https://github.com/elephantrobotics/pymycobot/demo/Server_280_RDK_X5.py """ def __init__(self, ip, port=30002, timeout=0.1, debug=False, thread_lock=True): super().__init__(debug, thread_lock) @@ -885,7 +885,7 @@ def close(self): def main(): - mc_sock = MyCobot280X5PISocket('192.168.1.246', port=30002, debug=True) + mc_sock = MyCobot280RDKX5Socket('192.168.1.246', port=30002, debug=True) # print(mc_sock.send_angle(1, 100, 50)) # print(mc_sock.get_quick_move_message()) print(mc_sock.set_gpio_mode(0)) From 2dfef78cfc6147389596a14fb671a8324d861f9d Mon Sep 17 00:00:00 2001 From: Mrkun5018 <2220138602@qq.com> Date: Mon, 17 Feb 2025 18:08:05 +0800 Subject: [PATCH 07/19] fix MyArmMControl bugs --- pymycobot/error.py | 27 ++-- pymycobot/myarmm_control.py | 287 ++++++++++++++++++------------------ pymycobot/robot_info.py | 4 +- 3 files changed, 166 insertions(+), 152 deletions(-) diff --git a/pymycobot/error.py b/pymycobot/error.py index 605c1240..d02dcba3 100644 --- a/pymycobot/error.py +++ b/pymycobot/error.py @@ -1277,6 +1277,9 @@ def calibration_parameters(**kwargs): if value < min_angle or value > max_angle: raise ValueError(f"angle value not right, should be {min_angle} ~ {max_angle}, but received {value}") elif parameter == 'angles': + if not value: + raise ValueError("angles value can't be empty") + for i, v in enumerate(value): min_angle = limit_info["angles_min"][i] max_angle = limit_info["angles_max"][i] @@ -1285,14 +1288,14 @@ def calibration_parameters(**kwargs): f"angle value not right, should be {min_angle} ~ {max_angle}, but received {v}") elif parameter == 'coord': coord_index = kwargs['coord_id'] - 1 - min_coord = limit_info["coords_min"][coord_index] - max_coord = limit_info["coords_max"][coord_index] + min_coord = limit_info["coord_min"][coord_index] + max_coord = limit_info["coord_max"][coord_index] if not min_coord <= value <= max_coord: raise ValueError(f"coord value not right, should be {min_coord} ~ {max_coord}, but received {value}") elif parameter == 'coords': for i, v in enumerate(value): - min_coord = limit_info["coords_min"][i] - max_coord = limit_info["coords_max"][i] + min_coord = limit_info["coord_min"][i] + max_coord = limit_info["coord_max"][i] if not min_coord <= v <= max_coord: raise ValueError(f"coord value not right, should be {min_coord} ~ {max_coord}, but received {v}") elif parameter == 'encoder': @@ -1319,13 +1322,16 @@ def calibration_parameters(**kwargs): if not 1 <= s <= 100: raise ValueError(f"speed value not right, should be 1 ~ 100, the received speed is {value}") elif parameter == "servo_addr": + if not isinstance(value, int): + raise TypeError(f"The {parameter} must be an integer.") + if value in (0, 1, 2, 3, 4): if class_name == "MyArmMControl": raise ValueError("modification is not allowed between 0~4, current data id: {}".format(value)) raise ValueError("addr 0-4 cannot be modified") elif parameter == "account": - if not (8 <= len(value) <= 63): - raise ValueError("The length of password must be between 8 and 63.") + pass + elif parameter == "password": if not re.match(r'^[A-Za-z0-9]{8,63}$', value): raise ValueError("The password must be 8-63 characters long and contain only letters and numbers.") @@ -1342,13 +1348,16 @@ def calibration_parameters(**kwargs): raise ValueError(f"end_direction not right, should be 1 ~ 3, the received end_direction is {value}") elif parameter == "gripper_flag": if value not in (0, 1, 254): - raise ValueError(f"gripper_flag not right, should be 0 ~ 254, the received gripper_flag is {value}") + raise ValueError(f"gripper_flag not right, should be in (0, 1, 254), the received gripper_flag is {value}") elif parameter == "gripper_value": if not 0 <= value <= 100: raise ValueError(f"gripper_value not right, should be 0 ~ 100, the received gripper_value is {value}") elif parameter == "basic_pin_number": - if not 1 <= value <= 2: - raise ValueError("The basic pin number must be between 1 and 2.") + if not isinstance(value, int): + raise TypeError(f"The {parameter} must be an integer.") + + if not 1 <= value <= 6: + raise ValueError("The basic pin number must be between 1 ~ 6.") elif parameter == "rgb": for v in value: if not 0 <= v <= 255: diff --git a/pymycobot/myarmm_control.py b/pymycobot/myarmm_control.py index 2ba401de..0f402f23 100644 --- a/pymycobot/myarmm_control.py +++ b/pymycobot/myarmm_control.py @@ -1,5 +1,5 @@ # coding=utf-8 - +import functools import sys import logging import time @@ -35,56 +35,73 @@ def setup_logging(debug: bool = False): return logger +def setup_serial_port(port, baudrate, timeout=0.1): + serial_api = serial.Serial(port, baudrate, timeout=timeout) + serial_api.rts = False + if not serial_api.is_open: + serial_api.open() + return serial_api + + class MyArmMProcessor(DataProcessor): def __init__(self, port, baudrate, timeout=0.1, debug=False): - - self._serial_port = serial.Serial() - self._serial_port.port = port - self._serial_port.baudrate = baudrate - self._serial_port.timeout = timeout - self._serial_port.rts = False - self._serial_port.open() + super().__init__(debug) self.lock = threading.Lock() - self._version = sys.version_info[:2][0] - self.log = setup_logging(debug) - self.calibration_parameters = calibration_parameters - - def _write(self, command): - write(self, command, method=None) + self._serial_port = setup_serial_port(port, baudrate, timeout) + self.calibration_parameters = functools.partial(calibration_parameters, class_name=self.__class__.__name__) + self._write = functools.partial(write, self, method=None) + self._read = functools.partial(read, self, _class=self.__class__.__name__) - def _read(self, genre, command=None, _class=None, timeout=None): - return read(self, genre, command, timeout, _class) + # def _write(self, command): + # write(self, command, method=None) + # + # def _read(self, genre, command=None, _class=None, timeout=None): + # return read(self, genre, command, timeout, _class) + @classmethod + def __is_return(cls, genre, command): + """ + Check if the command is a return command. + """ + return len(command) == 6 and command[3] == genre + def _mesg(self, genre, *args, **kwargs): - real_command, has_reply = super(MyArmMProcessor, self)._mesg(genre, *args, **kwargs) + kwargs["has_reply"] = True + real_command, has_reply, _ = super(MyArmMProcessor, self)._mesg(genre, *args, **kwargs) real_command = self._flatten(real_command) self._write(real_command) - if genre == ProtocolCode.STOP: - has_reply = True - - if has_reply is False: - return None - with self.lock: return self._read_genre_result(genre) def _read_genre_result(self, genre): - data = self._read(genre, _class=self.__class__.__name__) + data = self._read(genre, timeout=None) if genre == ProtocolCode.SET_SSID_PWD: - return None - - res = self._process_received(data, genre) + if len(data) == 5 and data[3] == genre: + return 1 + + if genre == ProtocolCode.SET_BASIC_OUTPUT: + if self.__is_return(genre, data): + return data[-2] + + if genre == ProtocolCode.GET_ROBOT_STATUS: + res = [] + valid_data = data[4:-1] + for header_i in range(0, len(valid_data), 2): + one = valid_data[header_i: header_i + 2] + res.append(self._decode_int16(one)) + else: + res = self._process_received(data, genre) if not res: - return None + return -1 if genre in [ ProtocolCode.ROBOT_VERSION, ProtocolCode.GET_ROBOT_ID, ProtocolCode.IS_POWER_ON, ProtocolCode.IS_CONTROLLER_CONNECTED, - ProtocolCode.IS_PAUSED, # TODO have bug: return b'' + ProtocolCode.IS_PAUSED, ProtocolCode.IS_IN_POSITION, ProtocolCode.IS_MOVING, ProtocolCode.IS_SERVO_ENABLE, @@ -141,18 +158,9 @@ def _read_genre_result(self, genre): else: r.append(self._int2angle(res[index])) return r - elif genre == ProtocolCode.GET_ROBOT_STATUS: - for i in range(len(res)): - if res[i] == 0: - continue - data = bin(res[i])[2:] - res[i] = [] - while len(data) != 16: - data = "0" + data - for j in range(16): - if data[j] != "0": - res[i].append(15 - j) - return res + elif self.__is_return(genre, data): + print("11111111111111111111111") + return self._process_single(res) else: return res @@ -165,19 +173,19 @@ def __init__(self, port, baudrate=115200, timeout=0.1, debug=False): # System status def get_robot_modify_version(self): """Get the bot correction version number""" - return self._mesg(RobotProtocolCode.GET_ROBOT_MODIFY_VERSION, has_reply=True) + return self._mesg(RobotProtocolCode.GET_ROBOT_MODIFY_VERSION) def get_robot_system_version(self): """Obtaining the Robot Firmware Version (Major and Minor Versions)""" - return self._mesg(RobotProtocolCode.GET_ROBOT_SYSTEM_VERSION, has_reply=True) + return self._mesg(RobotProtocolCode.GET_ROBOT_SYSTEM_VERSION) def get_robot_tool_modify_version(self): """Get the remediation version of the bot tool""" - return self._mesg(ProtocolCode.GET_ROBOT_TOOL_MODIFY_VERSION, has_reply=True) + return self._mesg(ProtocolCode.GET_ROBOT_TOOL_MODIFY_VERSION) def get_robot_tool_system_version(self): """Get the Robot Tool Firmware Version (End Atom)""" - return self._mesg(RobotProtocolCode.GET_ROBOT_TOOL_SYSTEM_VERSION, has_reply=True) + return self._mesg(RobotProtocolCode.GET_ROBOT_TOOL_SYSTEM_VERSION) def power_on(self): """The robotic arm turns on the power""" @@ -195,7 +203,7 @@ def is_powered_on(self): 0 - power off -1 - error data """ - return self._mesg(RobotProtocolCode.IS_POWERED_ON, has_reply=True) + return self._mesg(RobotProtocolCode.IS_POWERED_ON) def release_all_servos(self, data=None): """The robot turns off the torque output @@ -216,16 +224,16 @@ def set_fresh_mode(self, mode): # TODO 22-5-19 need test 1 - Always execute the latest command first. 0 - Execute instructions sequentially in the form of a queue. """ - self.calibration_parameters(class_name=self.__class__.__name__, mode=mode) + self.calibration_parameters(mode=mode) return self._mesg(ProtocolCode.SET_FRESH_MODE, mode) def get_fresh_mode(self): """Query sports mode""" - return self._mesg(ProtocolCode.GET_FRESH_MODE, has_reply=True) + return self._mesg(ProtocolCode.GET_FRESH_MODE) def get_robot_status(self): """Get robot status""" - return self._mesg(ProtocolCode.GET_ROBOT_STATUS, has_reply=True) + return self._mesg(ProtocolCode.GET_ROBOT_STATUS) def get_angles(self): """ Get the angle of all joints. @@ -233,7 +241,7 @@ def get_angles(self): Return: list: A float list of all angle. """ - return self._mesg(ProtocolCode.GET_ANGLES, has_reply=True) + return self._mesg(ProtocolCode.GET_ANGLES) def write_angle(self, joint_id, degree, speed): """Send the angle of a joint to robot arm. @@ -243,7 +251,7 @@ def write_angle(self, joint_id, degree, speed): degree: (float) -150 ~ 150 speed : (int) 1 ~ 100 """ - self.calibration_parameters(class_name=self.__class__.__name__, id=joint_id, angle=degree, speed=speed) + self.calibration_parameters(joint_id=joint_id, angle=degree, speed=speed) return self._mesg(ProtocolCode.SEND_ANGLE, joint_id, [self._angle2int(degree)], speed) def write_angles(self, angles, speed): @@ -253,7 +261,7 @@ def write_angles(self, angles, speed): angles: (list) A float list of all angle. speed : (int) 1 ~ 100 """ - self.calibration_parameters(class_name=self.__class__.__name__, angles=angles, speed=speed) + self.calibration_parameters(angles=angles, speed=speed) angles = [self._angle2int(angle) for angle in angles] return self._mesg(ProtocolCode.SEND_ANGLES, angles, speed) @@ -263,7 +271,7 @@ def get_coords(self): Return: list: A float list of all coordinates. """ - return self._mesg(ProtocolCode.GET_COORDS, has_reply=True) + return self._mesg(ProtocolCode.GET_COORDS) def write_coord(self, coord_id, coord, speed): """Send the coordinates of a joint to robot arm. @@ -273,9 +281,9 @@ def write_coord(self, coord_id, coord, speed): coord: (float) -150 ~ 150 speed : (int) 1 ~ 100 """ - self.calibration_parameters(class_name=self.__class__.__name__, coord_id=coord_id, coord=coord, speed=speed) + self.calibration_parameters(coord_id=coord_id, coord=coord, speed=speed) value = self._coord2int(coord) if coord_id <= 3 else self._angle2int(coord) - return self._mesg(ProtocolCode.SEND_COORD, id, [value], speed) + return self._mesg(ProtocolCode.SEND_COORD, coord_id, [value], speed) def write_coords(self, coords, speed, mode=None): """Send the coordinates of all joints to robot arm. @@ -285,7 +293,7 @@ def write_coords(self, coords, speed, mode=None): speed : (int) 1 ~ 100 mode: (int) 0 - normal, 1 - low, 2 - high """ - self.calibration_parameters(class_name=self.__class__.__name__, coords=coords, speed=speed) + self.calibration_parameters(coords=coords, speed=speed) coord_list = [] for idx in range(3): coord_list.append(self._coord2int(coords[idx])) @@ -309,20 +317,20 @@ def is_in_position(self, data, mode=0): 0 - False\n -1 - Error """ - if id == 1: - self.calibration_parameters(class_name=self.__class__.__name__, coords=data) + if mode == 1: + self.calibration_parameters(coords=data) data_list = [] for idx in range(3): data_list.append(self._coord2int(data[idx])) for idx in range(3, 6): data_list.append(self._angle2int(data[idx])) - elif id == 0: - self.calibration_parameters(class_name=self.__class__.__name__, angles=data) + elif mode == 0: + self.calibration_parameters(angles=data) data_list = [self._angle2int(i) for i in data] else: raise Exception("id is not right, please input 0 or 1") - return self._mesg(ProtocolCode.IS_IN_POSITION, data_list, mode, has_reply=True) + return self._mesg(ProtocolCode.IS_IN_POSITION, data_list, mode) def is_moving(self): """Detect if the robot is moving @@ -332,7 +340,7 @@ def is_moving(self): 1 - is moving -1 - error data """ - return self._mesg(ProtocolCode.IS_MOVING, has_reply=True) + return self._mesg(ProtocolCode.IS_MOVING) def jog_rpy(self, end_direction, direction, speed): """Rotate the end around a fixed axis in the base coordinate system @@ -342,8 +350,7 @@ def jog_rpy(self, end_direction, direction, speed): direction (int): 1 - forward rotation, 0 - reverse rotation speed (int): 1 ~ 100 """ - self.calibration_parameters( - class_name=self.__class__.__name__, direction=direction, speed=speed, end_direction=end_direction + self.calibration_parameters(direction=direction, speed=speed, end_direction=end_direction ) return self._mesg(ProtocolCode.JOG_ABSOLUTE, end_direction, direction, speed) @@ -357,9 +364,7 @@ def jog_angle(self, joint_id, direction, speed): direction: 0 - decrease, 1 - increase speed: int (0 - 100) """ - self.calibration_parameters( - class_name=self.__class__.__name__, joint_id=joint_id, direction=direction, speed=speed - ) + self.calibration_parameters(joint_id=joint_id, direction=direction, speed=speed) return self._mesg(ProtocolCode.JOG_ANGLE, joint_id, direction, speed) def jog_coord(self, coord_id, direction, speed): @@ -370,9 +375,7 @@ def jog_coord(self, coord_id, direction, speed): direction: 0 - decrease, 1 - increase speed: int (1 - 100) """ - self.calibration_parameters( - class_name=self.__class__.__name__, coord_id=coord_id, direction=direction, speed=speed - ) + self.calibration_parameters(coord_id=coord_id, direction=direction, speed=speed) return self._mesg(ProtocolCode.JOG_COORD, coord_id, direction, speed) def jog_increment(self, joint_id, increment, speed): @@ -380,11 +383,14 @@ def jog_increment(self, joint_id, increment, speed): Args: joint_id(int): - for myArm: Joint id 1 - 7. + for myArm: Joint id 1 - 6. increment(int): incremental speed(int): int (0 - 100) """ - self.calibration_parameters(class_name=self.__class__.__name__, id=joint_id, speed=speed) + if not (isinstance(increment, int) or isinstance(increment, float)): + raise ValueError("increment must be int or float") + + self.calibration_parameters(joint_id=joint_id, speed=speed) return self._mesg(ProtocolCode.JOG_INCREMENT, joint_id, [self._angle2int(increment)], speed) def pause(self): @@ -399,7 +405,7 @@ def is_paused(self): 0 - not paused -1 - error """ - return self._mesg(ProtocolCode.IS_PAUSED, has_reply=True) + return self._mesg(ProtocolCode.IS_PAUSED) def resume(self): """Recovery movement""" @@ -416,8 +422,8 @@ def get_encoder(self, joint_id): joint_id: (int) for myArm: Joint id 1 - 7. """ - self.calibration_parameters(class_name=self.__class__.__name__, encode_id=joint_id) - return self._mesg(ProtocolCode.GET_ENCODER, joint_id, has_reply=True) + self.calibration_parameters(joint_id=joint_id) + return self._mesg(ProtocolCode.GET_ENCODER, joint_id) def get_encoders(self): """Get the six joints of the manipulator @@ -425,7 +431,7 @@ def get_encoders(self): Returns: The list of encoders """ - return self._mesg(ProtocolCode.GET_ENCODERS, has_reply=True) + return self._mesg(ProtocolCode.GET_ENCODERS) # Running status and Settings def get_speed(self): @@ -434,7 +440,7 @@ def get_speed(self): Returns: int """ - return self._mesg(ProtocolCode.GET_SPEED, has_reply=True) + return self._mesg(ProtocolCode.GET_SPEED) def set_speed(self, speed): """Set speed value @@ -442,7 +448,7 @@ def set_speed(self, speed): Args: speed (int): 1 ~ 100 """ - self.calibration_parameters(class_name=self.__class__.__name__, speed=speed) + self.calibration_parameters(speed=speed) return self._mesg(ProtocolCode.SET_SPEED, speed) def get_joint_min(self, joint_id): @@ -455,8 +461,8 @@ def get_joint_min(self, joint_id): Returns: angle value(float) """ - self.calibration_parameters(class_name=self.__class__.__name__, joint_id=joint_id) - return self._mesg(ProtocolCode.GET_JOINT_MIN_ANGLE, joint_id, has_reply=True) + self.calibration_parameters(joint_id=joint_id) + return self._mesg(ProtocolCode.GET_JOINT_MIN_ANGLE, joint_id) def get_joint_max(self, joint_id): """Gets the maximum movement angle of the specified joint @@ -468,8 +474,8 @@ def get_joint_max(self, joint_id): Return: angle value(float) """ - self.calibration_parameters(class_name=self.__class__.__name__, joint_id=joint_id) - return self._mesg(ProtocolCode.GET_JOINT_MAX_ANGLE, joint_id, has_reply=True) + self.calibration_parameters(joint_id=joint_id) + return self._mesg(ProtocolCode.GET_JOINT_MAX_ANGLE, joint_id) def set_joint_max(self, joint_id, angle): """Set the joint maximum angle @@ -479,8 +485,8 @@ def set_joint_max(self, joint_id, angle): for myArm: Joint id 1 - 7. angle: 0 ~ 180 """ - self.calibration_parameters(class_name=self.__class__.__name__, joint_id=joint_id, angle=angle) - return self._mesg(ProtocolCode.SET_JOINT_MAX, id, angle) + self.calibration_parameters(joint_id=joint_id, angle=angle) + return self._mesg(ProtocolCode.SET_JOINT_MAX, joint_id, angle) def set_joint_min(self, joint_id, angle): """Set the joint minimum angle @@ -490,8 +496,8 @@ def set_joint_min(self, joint_id, angle): for myArm: Joint id 1 - 7. angle: 0 ~ 180 """ - self.calibration_parameters(class_name=self.__class__.__name__, joint_id=joint_id, angle=angle) - return self._mesg(ProtocolCode.SET_JOINT_MIN, id, angle) + self.calibration_parameters(joint_id=joint_id, angle=angle) + return self._mesg(ProtocolCode.SET_JOINT_MIN, joint_id, angle) # Servo control def is_servo_enable(self, servo_id): @@ -506,8 +512,8 @@ def is_servo_enable(self, servo_id): 1 - enable -1 - error """ - self.calibration_parameters(class_name=self.__class__.__name__, servo_id=servo_id) - return self._mesg(ProtocolCode.IS_SERVO_ENABLE, servo_id, has_reply=True) + self.calibration_parameters(servo_id=servo_id) + return self._mesg(ProtocolCode.IS_SERVO_ENABLE, servo_id) def is_all_servo_enable(self): """Detect the connection status of all joints @@ -517,7 +523,7 @@ def is_all_servo_enable(self): 1 - enable -1 - error """ - return self._mesg(ProtocolCode.IS_ALL_SERVO_ENABLE, has_reply=True) + return self._mesg(ProtocolCode.IS_ALL_SERVO_ENABLE) def set_servo_data(self, servo_id, data_id, value, mode=None): """Set the data parameters of the specified address of the steering gear @@ -529,16 +535,14 @@ def set_servo_data(self, servo_id, data_id, value, mode=None): value: 0 - 4096 mode: 0 - indicates that value is one byte(default), 1 - 1 represents a value of two bytes. """ + if not isinstance(value, int): + raise TypeError("value must be int") if mode is None: - self.calibration_parameters( - class_name=self.__class__.__name__, servo_id=servo_id, servo_addr=data_id, value=value - ) + self.calibration_parameters(servo_id=servo_id, servo_addr=data_id, value=value) return self._mesg(ProtocolCode.SET_SERVO_DATA, servo_id, data_id, value) else: - self.calibration_parameters( - class_name=self.__class__.__name__, servo_id=servo_id, servo_addr=data_id, value=value, mode=mode - ) + self.calibration_parameters(servo_id=servo_id, servo_addr=data_id, value=value, mode=mode) return self._mesg(ProtocolCode.SET_SERVO_DATA, servo_id, data_id, [value], mode) def get_servo_data(self, servo_id, data_id, mode=None): @@ -553,13 +557,11 @@ def get_servo_data(self, servo_id, data_id, mode=None): values 0 - 4096 """ if mode is not None: - self.calibration_parameters( - class_name=self.__class__.__name__, servo_id=servo_id, address=data_id, mode=mode - ) - return self._mesg(ProtocolCode.GET_SERVO_DATA, servo_id, data_id, mode, has_reply=True) + self.calibration_parameters(servo_id=servo_id, servo_addr=data_id, mode=mode) + return self._mesg(ProtocolCode.GET_SERVO_DATA, servo_id, data_id, mode) - self.calibration_parameters(class_name=self.__class__.__name__, servo_id=servo_id, address=data_id) - return self._mesg(ProtocolCode.GET_SERVO_DATA, servo_id, data_id, has_reply=True) + self.calibration_parameters(servo_id=servo_id, servo_addr=data_id) + return self._mesg(ProtocolCode.GET_SERVO_DATA, servo_id, data_id) def set_servo_calibration(self, servo_id): """The current position of the calibration joint actuator is the angle zero point, @@ -568,7 +570,7 @@ def set_servo_calibration(self, servo_id): Args: servo_id: 1 ~ 8 """ - self.calibration_parameters(class_name=self.__class__.__name__, servo_id=servo_id) + self.calibration_parameters(servo_id=servo_id) return self._mesg(ProtocolCode.SET_SERVO_CALIBRATION, servo_id) def release_servo(self, servo_id, mode=None): @@ -579,11 +581,11 @@ def release_servo(self, servo_id, mode=None): mode: Default damping, set to 1, cancel damping """ if mode is None: - self.calibration_parameters(class_name=self.__class__.__name__, servo_id=servo_id) + self.calibration_parameters(servo_id=servo_id) return self._mesg(ProtocolCode.RELEASE_SERVO, servo_id) else: - self.calibration_parameters(class_name=self.__class__.__name__, servo_id=servo_id, mode=mode) + self.calibration_parameters(servo_id=servo_id, mode=mode) return self._mesg(ProtocolCode.RELEASE_SERVO, servo_id, mode) def focus_servo(self, servo_id): @@ -592,7 +594,7 @@ def focus_servo(self, servo_id): Args: servo_id: int 1 ~ 7 """ - self.calibration_parameters(class_name=self.__class__.__name__, servo_id=servo_id) + self.calibration_parameters(servo_id=servo_id) return self._mesg(ProtocolCode.FOCUS_SERVO, servo_id) def set_digital_output(self, pin_no, pin_signal): @@ -602,15 +604,15 @@ def set_digital_output(self, pin_no, pin_signal): pin_no (int): pin_signal (int): 0 / 1 """ - self.calibration_parameters(class_name=self.__class__.__name__, pin_number=pin_no, pin_signal=pin_signal) + self.calibration_parameters(pin_number=pin_no, pin_signal=pin_signal) return self._mesg(ProtocolCode.SET_DIGITAL_OUTPUT, pin_no, pin_signal) def get_digital_input(self, pin_no): """Get the terminal atom io status Returns: int 0/1 """ - self.calibration_parameters(class_name=self.__class__.__name__, pin_number=pin_no) - return self._mesg(ProtocolCode.GET_DIGITAL_INPUT, pin_no, has_reply=True) + self.calibration_parameters(pin_number=pin_no) + return self._mesg(ProtocolCode.GET_DIGITAL_INPUT, pin_no) def set_gripper_enabled(self): """Enable gripper""" @@ -622,7 +624,7 @@ def get_gripper_value(self): Return: gripper value (int) """ - return self._mesg(ProtocolCode.GET_GRIPPER_VALUE, has_reply=True) + return self._mesg(ProtocolCode.GET_GRIPPER_VALUE) def set_gripper_state(self, flag, speed): """Set gripper switch state @@ -631,7 +633,7 @@ def set_gripper_state(self, flag, speed): flag (int): 0 - open, 1 - close, 254 - release speed (int): 1 ~ 100 """ - self.calibration_parameters(class_name=self.__class__.__name__, gripper_flag=flag, speed=speed) + self.calibration_parameters(gripper_flag=flag, speed=speed) return self._mesg(ProtocolCode.SET_GRIPPER_STATE, flag, speed) def set_gripper_value(self, gripper_value, speed): @@ -642,7 +644,7 @@ def set_gripper_value(self, gripper_value, speed): speed (int): 1 ~ 100 """ - self.calibration_parameters(class_name=self.__class__.__name__, gripper_value=gripper_value, speed=speed) + self.calibration_parameters(gripper_value=gripper_value, speed=speed) return self._mesg(ProtocolCode.SET_GRIPPER_VALUE, gripper_value, speed) def set_gripper_calibration(self): @@ -657,7 +659,7 @@ def is_gripper_moving(self): 1 - is moving -1- error data """ - return self._mesg(ProtocolCode.IS_GRIPPER_MOVING, has_reply=True) + return self._mesg(ProtocolCode.IS_GRIPPER_MOVING) # Atom IO def set_led_color(self, r=0, g=0, b=0): @@ -669,7 +671,7 @@ def set_led_color(self, r=0, g=0, b=0): b (int): 0 ~ 255 """ - self.calibration_parameters(class_name=self.__class__.__name__, rgb=(r, g, b)) + self.calibration_parameters(rgb=(r, g, b)) return self._mesg(ProtocolCode.SET_COLOR, r, g, b) def is_tool_btn_clicked(self): @@ -680,7 +682,7 @@ def is_tool_btn_clicked(self): 1 - is clicked -1- error data """ - return self._mesg(ProtocolCode.IS_TOOL_BTN_CLICKED, has_reply=True) + return self._mesg(ProtocolCode.IS_TOOL_BTN_CLICKED) # Basic def set_basic_output(self, pin_no, pin_signal): @@ -690,7 +692,7 @@ def set_basic_output(self, pin_no, pin_signal): pin_no: pin port number. pin_signal: 0 / 1 """ - self.calibration_parameters(class_name=self.__class__.__name__, pin_signal=pin_signal, basic_pin_number=pin_no) + self.calibration_parameters(pin_signal=pin_signal, basic_pin_number=pin_no) return self._mesg(ProtocolCode.SET_BASIC_OUTPUT, pin_no, pin_signal) def get_basic_input(self, pin_no): @@ -699,8 +701,8 @@ def get_basic_input(self, pin_no): Args: pin_no: (int) pin port number. """ - self.calibration_parameters(class_name=self.__class__.__name__, basic_pin_number=pin_no) - return self._mesg(ProtocolCode.GET_BASIC_INPUT, pin_no, has_reply=True) + self.calibration_parameters(basic_pin_number=pin_no) + return self._mesg(ProtocolCode.GET_BASIC_INPUT, pin_no) def set_ssid_pwd(self, account: str, password: str): """Change connected wi-fi. @@ -711,8 +713,8 @@ def set_ssid_pwd(self, account: str, password: str): """ self._mesg(ProtocolCode.SET_SSID_PWD) time.sleep(0.02) - self.calibration_parameters(class_name=self.__class__.__name__, account=account, password=password) - return self._mesg(ProtocolCode.SET_SSID_PWD, account, password, has_reply=True) + self.calibration_parameters(account=account, password=password) + return self._mesg(ProtocolCode.SET_SSID_PWD, account, password) def get_ssid_pwd(self): """Get connected wi-fi account and password. @@ -720,7 +722,7 @@ def get_ssid_pwd(self): Return: (account, password) """ - return self._mesg(ProtocolCode.GET_SSID_PWD, has_reply=True) + return self._mesg(ProtocolCode.GET_SSID_PWD) def set_server_port(self, port): """Change the connection port of the server. @@ -728,6 +730,8 @@ def set_server_port(self, port): Args: port: (int) The new connection port of the server. """ + if not isinstance(port, int): + raise TypeError("server port must be int") return self._mesg(ProtocolCode.SET_SERVER_PORT, port) def get_tof_distance(self): @@ -736,7 +740,7 @@ def get_tof_distance(self): Return: (int) The unit is mm. """ - return self._mesg(ProtocolCode.GET_TOF_DISTANCE, has_reply=True) + return self._mesg(ProtocolCode.GET_TOF_DISTANCE) def set_tool_reference(self, coords): """Set tool coordinate system @@ -744,7 +748,7 @@ def set_tool_reference(self, coords): Args: coords: a list of coords value(List[float]), [x(mm), y, z, rx(angle), ry, rz] """ - self.calibration_parameters(class_name=self.__class__.__name__, coords=coords) + self.calibration_parameters(coords=coords) coord_list = [] for idx in range(3): coord_list.append(self._coord2int(coords[idx])) @@ -754,7 +758,7 @@ def set_tool_reference(self, coords): def get_tool_reference(self): """Get tool coordinate system """ - return self._mesg(ProtocolCode.GET_TOOL_REFERENCE, has_reply=True) + return self._mesg(ProtocolCode.GET_TOOL_REFERENCE) def set_world_reference(self, coords): """Set the world coordinate system @@ -762,7 +766,7 @@ def set_world_reference(self, coords): Args: coords: a list of coords value(List[float]), [x(mm), y, z, rx(angle), ry, rz]\n """ - self.calibration_parameters(class_name=self.__class__.__name__, coords=coords) + self.calibration_parameters(coords=coords) coord_list = [] for idx in range(3): coord_list.append(self._coord2int(coords[idx])) @@ -772,7 +776,7 @@ def set_world_reference(self, coords): def get_world_reference(self): """Get the world coordinate system""" - return self._mesg(ProtocolCode.GET_WORLD_REFERENCE, has_reply=True) + return self._mesg(ProtocolCode.GET_WORLD_REFERENCE) def set_reference_frame(self, rftype): """Set the base coordinate system @@ -780,7 +784,7 @@ def set_reference_frame(self, rftype): Args: rftype: 0 - base 1 - tool. """ - self.calibration_parameters(class_name=self.__class__.__name__, rftype=rftype) + self.calibration_parameters(rftype=rftype) return self._mesg(ProtocolCode.SET_REFERENCE_FRAME, rftype) def get_reference_frame(self): @@ -789,7 +793,7 @@ def get_reference_frame(self): Returns: 0 - base 1 - tool. """ - return self._mesg(ProtocolCode.GET_REFERENCE_FRAME, has_reply=True) + return self._mesg(ProtocolCode.GET_REFERENCE_FRAME) def set_movement_type(self, move_type): """Set movement type @@ -797,7 +801,7 @@ def set_movement_type(self, move_type): Args: move_type: 1 - movel, 0 - moveJ """ - self.calibration_parameters(class_name=self.__class__.__name__, move_type=move_type) + self.calibration_parameters(move_type=move_type) return self._mesg(ProtocolCode.SET_MOVEMENT_TYPE, move_type) def get_movement_type(self): @@ -806,7 +810,7 @@ def get_movement_type(self): Returns: 1 - movel, 0 - moveJ """ - return self._mesg(ProtocolCode.GET_MOVEMENT_TYPE, has_reply=True) + return self._mesg(ProtocolCode.GET_MOVEMENT_TYPE) def set_end_type(self, mode): """Set end coordinate system @@ -814,7 +818,7 @@ def set_end_type(self, mode): Args: mode: int, 0 - flange, 1 - tool """ - self.calibration_parameters(class_name=self.__class__.__name__, mode=mode) + self.calibration_parameters(mode=mode) return self._mesg(ProtocolCode.SET_END_TYPE, mode) def get_end_type(self): @@ -823,7 +827,7 @@ def get_end_type(self): Returns: 0 - flange, 1 - tool """ - return self._mesg(ProtocolCode.GET_END_TYPE, has_reply=True) + return self._mesg(ProtocolCode.GET_END_TYPE) def get_plan_speed(self): """Get planning speed @@ -831,7 +835,7 @@ def get_plan_speed(self): Returns: [movel planning speed, movej planning speed]. """ - return self._mesg(ProtocolCode.GET_PLAN_SPEED, has_reply=True) + return self._mesg(ProtocolCode.GET_PLAN_SPEED) def get_plan_acceleration(self): """Get planning acceleration @@ -839,7 +843,7 @@ def get_plan_acceleration(self): Returns: [movel planning acceleration, movej planning acceleration]. """ - return self._mesg(ProtocolCode.GET_PLAN_ACCELERATION, has_reply=True) + return self._mesg(ProtocolCode.GET_PLAN_ACCELERATION) def set_plan_speed(self, speed, is_linear): """Set planning speed @@ -848,7 +852,7 @@ def set_plan_speed(self, speed, is_linear): speed (int): (1 ~ 100). is_linear: 0 -> joint 1 -> straight line """ - self.calibration_parameters(class_name=self.__class__.__name__, speed=speed, is_linear=is_linear) + self.calibration_parameters(speed=speed, is_linear=is_linear) return self._mesg(ProtocolCode.SET_PLAN_SPEED, speed, is_linear) def set_plan_acceleration(self, acceleration, is_linear): @@ -858,7 +862,7 @@ def set_plan_acceleration(self, acceleration, is_linear): acceleration (int): (1 ~ 100). is_linear(int): 0 -> joint 1 -> straight line """ - self.calibration_parameters(class_name=self.__class__.__name__, speed=acceleration, is_linear=is_linear) + self.calibration_parameters(speed=acceleration, is_linear=is_linear) return self._mesg(ProtocolCode.SET_PLAN_ACCELERATION, acceleration, is_linear) def get_servo_speeds(self): @@ -866,14 +870,14 @@ def get_servo_speeds(self): Returns: speeds: list[float * 8] +- 3000 step/s """ - return self._mesg(ProtocolCode.GET_SERVO_SPEED, has_reply=True) + return self._mesg(ProtocolCode.GET_SERVO_SPEED) def get_servo_currents(self): """Get the joint current Returns: currents: list[float * 8] 0 ~ 3250 """ - return self._mesg(ProtocolCode.GET_SERVO_CURRENTS, has_reply=True) + return self._mesg(ProtocolCode.GET_SERVO_CURRENTS) def get_servo_voltages(self): """Get the joint voltages @@ -881,7 +885,7 @@ def get_servo_voltages(self): Returns: voltages: list[float] voltage 0 ~ 240 """ - return self._mesg(ProtocolCode.GET_SERVO_VOLTAGES, has_reply=True) + return self._mesg(ProtocolCode.GET_SERVO_VOLTAGES) def get_servo_status(self): """ @@ -891,7 +895,7 @@ def get_servo_status(self): 0 - normal, other - error """ - return self._mesg(ProtocolCode.GET_SERVO_STATUS, has_reply=True) + return self._mesg(ProtocolCode.GET_SERVO_STATUS) def get_servo_temps(self): """ @@ -899,7 +903,7 @@ def get_servo_temps(self): Returns: temperatures: list[float] 0 ~ 255 """ - return self._mesg(ProtocolCode.GET_SERVO_TEMPS, has_reply=True) + return self._mesg(ProtocolCode.GET_SERVO_TEMPS) def set_void_compensate(self, mode): """Set the virtual position compensation mode ( @@ -909,5 +913,6 @@ def set_void_compensate(self, mode): Args: mode (int): 0 - close, 1 - open """ - self.calibration_parameters(class_name=self.__class__.__name__, mode=mode) + self.calibration_parameters(mode=mode) return self._mesg(ProtocolCode.SET_VOID_COMPENSATE, mode) + diff --git a/pymycobot/robot_info.py b/pymycobot/robot_info.py index c6ead56e..c16e0849 100644 --- a/pymycobot/robot_info.py +++ b/pymycobot/robot_info.py @@ -319,11 +319,11 @@ class RobotLimit: "encoders_max": [4004, 2945, 3079, 3026, 3724, 2994, 3704, 2048] }, "MyArmMControl": { - "joint_id": [1, 2, 3, 4, 5, 6, 7], + "joint_id": [1, 2, 3, 4, 5, 6], "servo_id": [1, 2, 3, 4, 5, 6, 7, 8], "coord_id": [1, 2, 3, 4, 5, 6], "coord_min": [-833.325, -833.325, -351.11, -180, -180, -180], - "coord_max": [-833.325, -833.325, 1007.225, -180, -180, -180], + "coord_max": [833.325, 833.325, 1007.225, 180, 180, 180], "angles_min": [-170, -83, -90, -155, -91, -153, -118], "angles_max": [170, 83, 84, 153, 88, 153, 2], "encoders_min": [137, 1163, 1035, 1013, 248, 979, 220, 706], From b743d96922d4aaf1fbc0911e17275f58da224537 Mon Sep 17 00:00:00 2001 From: Mrkun5018 <2220138602@qq.com> Date: Tue, 18 Feb 2025 15:05:55 +0800 Subject: [PATCH 08/19] fix MyArmC bugs --- pymycobot/error.py | 8 +- pymycobot/myarm_api.py | 189 ++++++++++++++++++++++++++++------------ pymycobot/myarmc.py | 9 +- pymycobot/robot_info.py | 6 ++ 4 files changed, 152 insertions(+), 60 deletions(-) diff --git a/pymycobot/error.py b/pymycobot/error.py index d02dcba3..66925e8c 100644 --- a/pymycobot/error.py +++ b/pymycobot/error.py @@ -1336,8 +1336,12 @@ def calibration_parameters(**kwargs): if not re.match(r'^[A-Za-z0-9]{8,63}$', value): raise ValueError("The password must be 8-63 characters long and contain only letters and numbers.") elif parameter == "pin_number": - if not 1 <= value <= 6: - raise ValueError("The pin number must be between 1 and 6.") + pin = 6 + if class_name == "MyArmC": + pin = 2 + if not 1 <= value <= pin: + raise ValueError(f"The pin number must be between 1 and {pin}.") + elif parameter in ("direction", "mode", "pin_signal", "is_linear", "move_type", "rftype"): if not isinstance(value, int): raise TypeError(f"The {parameter} must be an integer.") diff --git a/pymycobot/myarm_api.py b/pymycobot/myarm_api.py index 1394bf94..38da7e62 100644 --- a/pymycobot/myarm_api.py +++ b/pymycobot/myarm_api.py @@ -1,24 +1,24 @@ # coding=utf-8 from __future__ import division - -import sys -import logging +import functools import threading import time - -from pymycobot.common import ProtocolCode, write, read, DataProcessor +from pymycobot.common import ProtocolCode, DataProcessor from pymycobot.error import calibration_parameters -from pymycobot.log import setup_logging import serial -class MyArmAPI(DataProcessor): - """ +def setup_serial_connect(port, baudrate, timeout=None): + serial_api = serial.Serial(port=port, baudrate=baudrate, timeout=timeout) + serial_api.rts = False + if not serial_api.is_open: + serial_api.open() + return serial_api - """ - def __init__(self, port, baudrate="115200", timeout=0.1, debug=False): +class MyArmMCProcessor(DataProcessor): + def __init__(self, port, baudrate=115200, timeout=0.1, debug=False): """ Args: port : port string @@ -26,22 +26,65 @@ def __init__(self, port, baudrate="115200", timeout=0.1, debug=False): timeout : default 0.1 debug : whether show debug info """ - super(MyArmAPI, self).__init__() - self.calibration_parameters = calibration_parameters - self._serial_port = serial.Serial() - self._serial_port.port = port - self._serial_port.baudrate = baudrate - self._serial_port.timeout = timeout - self._serial_port.rts = False - self._serial_port.open() + super(MyArmMCProcessor, self).__init__(debug=debug) + self.calibration_parameters = functools.partial(calibration_parameters, class_name=self.__class__.__name__) + self._serial_port = setup_serial_connect(port, baudrate, timeout) self.lock = threading.Lock() - self._version = sys.version_info[:2][0] - self.debug = debug - setup_logging(self.debug) - self.log = logging.getLogger(__name__) - _write = write - _read = read + @classmethod + def __check_command_integrity(cls, command): + """ + Check the integrity of the command. + Instruction format: header + header + length + genre + *data + footer + + :param command: the command to check + :return: True if the command is valid, False otherwise + """ + return len(command) >= 5 and command[-1] == ProtocolCode.FOOTER and command[2] == len(command) - 3 + + def _read_command_buffer(self, timeout=0.1): + commands = b"" + is_record = False + previous_frame = b"" + stime = time.perf_counter() + while time.perf_counter() - stime < timeout: + current_frame = self._serial_port.read(1) + + if current_frame == b"\xfe" and previous_frame == b"\xfe" and is_record is False: + is_record = True + commands = b"\xfe\xfe" + continue + + previous_frame = current_frame + if is_record is False: + continue + + commands += current_frame + if self.__check_command_integrity(commands): + break + return commands + + def _read(self, genre): + if genre == ProtocolCode.GET_ROBOT_STATUS: + timeout = 90 + elif genre == ProtocolCode.SET_SSID_PWD or genre == ProtocolCode.GET_SSID_PWD: + timeout = 0.05 + elif genre in [ProtocolCode.INIT_ELECTRIC_GRIPPER, ProtocolCode.SET_GRIPPER_MODE]: + timeout = 0.3 + elif genre in [ProtocolCode.SET_GRIPPER_CALIBRATION]: + timeout = 0.4 + else: + timeout = 0.1 + + command_buffer = self._read_command_buffer(timeout=timeout) + self.log.debug(f"_read : {' '.join(map(lambda x: f'{x:02x}', command_buffer))}") + return command_buffer + + def _write(self, command): + self.log.debug(f"_write: {' '.join(map(lambda x: f'{x:02x}', command))}") + if not self._serial_port.is_open: + self._serial_port.open() + self._serial_port.write(command) def _mesg(self, genre, *args, **kwargs): """ @@ -54,19 +97,16 @@ def _mesg(self, genre, *args, **kwargs): **kwargs: support `has_reply` has_reply: Whether there is a return value to accept. """ - time.sleep(0.01) - real_command, has_reply, _async = super(MyArmAPI, self)._mesg(genre, *args, **kwargs) - command = self._flatten(real_command) + real_command, has_reply, _async = super(MyArmMCProcessor, self)._mesg(genre, *args, **kwargs) with self.lock: - self._write(command) + time.sleep(0.1) + self._write(self._flatten(real_command)) if not has_reply: return None - timeout = kwargs.get('timeout', None) - data = self._read(genre, command=command, timeout=timeout, _class=self.__class__.__name__) + data = self._read(genre) res = self._process_received(data, genre, arm=8) - if not res: return None @@ -97,15 +137,20 @@ def _mesg(self, genre, *args, **kwargs): if genre in (ProtocolCode.COBOTX_GET_ANGLE,): result = self._int2angle(result) return result - if genre in (ProtocolCode.GET_ATOM_PRESS_STATUS, ): + if genre in (ProtocolCode.GET_ATOM_PRESS_STATUS,): if self.__class__.__name__ == 'MyArmM': return res[0] return res - elif genre in [ - ProtocolCode.GET_ANGLES, ProtocolCode.GET_JOINT_MAX_ANGLE, ProtocolCode.GET_JOINT_MIN_ANGLE, - ProtocolCode.GET_JOINTS_COORD - ]: + elif genre in [ProtocolCode.GET_ANGLES, ProtocolCode.GET_JOINT_MAX_ANGLE, ProtocolCode.GET_JOINT_MIN_ANGLE]: return [self._int2angle(angle) for angle in res] + elif genre == ProtocolCode.GET_JOINTS_COORD: + r = [] + for idx, val in enumerate(res): + if idx < 3: + r.append(self._int2coord(val)) + else: + r.append(self._int2angle(val)) + return r elif genre in [ProtocolCode.GET_SERVO_VOLTAGES]: return [self._int2coord(angle) for angle in res] elif genre == ProtocolCode.GET_ROBOT_ERROR_STATUS: @@ -124,6 +169,9 @@ def _mesg(self, genre, *args, **kwargs): else: return res + +class MyArmAPI(MyArmMCProcessor): + def get_robot_modified_version(self): """Get the bot correction version number @@ -154,6 +202,8 @@ def set_robot_err_check_state(self, status): Args: status (int): 1 open; o close """ + if status not in [0, 1]: + raise ValueError("status must be 0 or 1") self._mesg(ProtocolCode.SET_ROBOT_ERROR_CHECK_STATE, status) def get_robot_err_check_state(self): @@ -182,16 +232,21 @@ def get_recv_queue_len(self): """The current length of the read receive queue""" return self._mesg(ProtocolCode.GET_RECV_QUEUE_LENGTH, has_reply=True) + def clear_recv_queue(self): + """Clear the read receive queue""" + return self._mesg(ProtocolCode.CLEAR_RECV_QUEUE) + def get_joint_angle(self, joint_id): """ - Obtain the current angle of the specified joint, when the obtained angle is 200��, + Obtain the current angle of the specified joint, when the obtained angle is 200, it means that the joint has no communication Args: joint_id (int): 0 - 254 Returns: - + angle (int) """ + self.calibration_parameters(joint_id=joint_id) return self._mesg(ProtocolCode.COBOTX_GET_ANGLE, joint_id, has_reply=True) def get_joints_angle(self): @@ -209,7 +264,7 @@ def set_servo_calibrate(self, servo_id): Args: servo_id (int): 0 - 254 """ - + self.calibration_parameters(servo_id=servo_id) self._mesg(ProtocolCode.SET_SERVO_CALIBRATION, servo_id) def get_servo_encoder(self, servo_id): @@ -221,6 +276,7 @@ def get_servo_encoder(self, servo_id): Returns: encoder (int): 0-4095 """ + self.calibration_parameters(servo_id=servo_id) return self._mesg(ProtocolCode.GET_ENCODER, servo_id, has_reply=True) def get_servos_encoder_drag(self): @@ -277,7 +333,7 @@ def set_servo_p(self, servo_id, data): """ assert 0 <= data <= 254, "data must be between 0 and 254" - self.calibration_parameters(class_name=self.__class__.__name__, servo_id=servo_id) + self.calibration_parameters(servo_id=servo_id) self._mesg(ProtocolCode.SET_SERVO_P, servo_id, data) def get_servo_p(self, servo_id): @@ -286,6 +342,7 @@ def get_servo_p(self, servo_id): Args: servo_id (int): 0-254 """ + self.calibration_parameters(servo_id=servo_id) return self._mesg(ProtocolCode.GET_SERVO_P, servo_id, has_reply=True) def set_servo_d(self, servo_id, data): @@ -296,7 +353,7 @@ def set_servo_d(self, servo_id, data): data (int): 0-254 """ assert 0 <= data <= 254, "data must be between 0 and 254" - self.calibration_parameters(class_name=self.__class__.__name__, servo_id=servo_id) + self.calibration_parameters(servo_id=servo_id) self._mesg(ProtocolCode.MERCURY_DRAG_TECH_EXECUTE, servo_id, data) def get_servo_d(self, servo_id): @@ -305,7 +362,8 @@ def get_servo_d(self, servo_id): Args: servo_id (int): 0-254 """ - return self._mesg(ProtocolCode.GET_SERVO_D, servo_id, has_reply=True) + self.calibration_parameters(servo_id=servo_id) + return self._mesg(ProtocolCode.GET_SERVO_D, servo_id, has_reply=True, timeout=0.1) def set_servo_i(self, servo_id, data): """Set the proportional factor of the position ring I of the specified servo motor @@ -315,11 +373,12 @@ def set_servo_i(self, servo_id, data): data (int): 0 - 254 """ assert 0 <= data <= 254, "data must be between 0 and 254" - self.calibration_parameters(class_name=self.__class__.__name__, servo_id=servo_id) + self.calibration_parameters(servo_id=servo_id) self._mesg(ProtocolCode.MERCURY_DRAG_TECH_PAUSE, servo_id, data) def get_servo_i(self, servo_id): """Reads the position loop I scale factor of the specified servo motor""" + self.calibration_parameters(servo_id=servo_id) return self._mesg(ProtocolCode.GET_ERROR_DETECT_MODE, servo_id, has_reply=True) def set_servo_cw(self, servo_id, data): @@ -330,7 +389,7 @@ def set_servo_cw(self, servo_id, data): data (int): 0 - 32 """ assert 0 <= data <= 32, "data must be between 0 and 32" - self.calibration_parameters(class_name=self.__class__.__name__, servo_id=servo_id) + self.calibration_parameters(servo_id=servo_id) self._mesg(ProtocolCode.SET_SERVO_MOTOR_CLOCKWISE, servo_id, data) def get_servo_cw(self, servo_id): @@ -339,6 +398,7 @@ def get_servo_cw(self, servo_id): Args: servo_id (int): 0 - 254 """ + self.calibration_parameters(servo_id=servo_id) return self._mesg(ProtocolCode.GET_SERVO_MOTOR_CLOCKWISE, servo_id, has_reply=True) def set_servo_cww(self, servo_id, data): @@ -349,7 +409,7 @@ def set_servo_cww(self, servo_id, data): data (int): 0 - 32 """ assert 0 <= data <= 32, "data must be between 0 and 32" - self.calibration_parameters(class_name=self.__class__.__name__, servo_id=servo_id) + self.calibration_parameters(servo_id=servo_id) return self._mesg(ProtocolCode.SET_SERVO_MOTOR_COUNTER_CLOCKWISE, servo_id, data) def get_servo_cww(self, servo_id): @@ -358,6 +418,7 @@ def get_servo_cww(self, servo_id): Args: servo_id (int): 0 - 254 """ + self.calibration_parameters(servo_id=servo_id) return self._mesg(ProtocolCode.GET_SERVO_MOTOR_COUNTER_CLOCKWISE, servo_id, has_reply=True) def set_servo_system_data(self, servo_id, addr, data, mode): @@ -369,7 +430,13 @@ def set_servo_system_data(self, servo_id, addr, data, mode): data (int): 0 - 4096 mode (int): 1 - data 1byte. 2 - data 2byte """ - self.calibration_parameters(class_name=self.__class__.__name__, servo_id=servo_id, servo_addr=addr) + if mode not in (1, 2): + raise ValueError('mode must be 1 or 2') + + if data not in range(0, 4097): + raise ValueError('data must be between 0 and 4096') + + self.calibration_parameters(servo_id=servo_id, servo_addr=addr) self._mesg(ProtocolCode.SET_SERVO_MOTOR_CONFIG, servo_id, addr, [data], mode) def get_servo_system_data(self, servo_id, addr, mode): @@ -380,28 +447,35 @@ def get_servo_system_data(self, servo_id, addr, mode): addr (int): mode (int): 1 - data 1byte. 2 - data 2byte """ + if mode not in (1, 2): + raise ValueError('mode must be 1 or 2') + self.calibration_parameters(servo_id=servo_id, servo_addr=addr) return self._mesg(ProtocolCode.GET_SERVO_MOTOR_CONFIG, servo_id, addr, mode, has_reply=True) - def set_master_out_io_state(self, io_number, status=1): + def set_master_out_io_state(self, pin_number, status=1): """Set the host I/O pin status Args: - io_number: 1 - 2 + pin_number: M750(1 -6), C650(1-2) status: 0/1; 0: low; 1: high. default: 1 """ - self._mesg(ProtocolCode.SET_MASTER_PIN_STATUS, io_number, status) + if status not in (0, 1): + raise ValueError('status must be 0 or 1') + self.calibration_parameters(pin_number=pin_number) + self._mesg(ProtocolCode.SET_MASTER_PIN_STATUS, pin_number, status) - def get_master_in_io_state(self, io_number): + def get_master_in_io_state(self, pin_number): """get the host I/O pin status Args: - io_number (int): 1 - 2 + pin_number (int): M750(1 -6), C650(1-2) Returns: 0 or 1. 1: high 0: low """ - return self._mesg(ProtocolCode.GET_MASTER_PIN_STATUS, io_number, has_reply=True) + self.calibration_parameters(pin_number=pin_number) + return self._mesg(ProtocolCode.GET_MASTER_PIN_STATUS, pin_number, has_reply=True) def set_tool_out_io_state(self, io_number, status=1): """Set the Atom pin status @@ -409,21 +483,24 @@ def set_tool_out_io_state(self, io_number, status=1): Args: io_number (int): 1 - 2 status: 0 or 1; 0: low; 1: high. default: 1 - """ + self.calibration_parameters(master_pin=io_number, status=status) self._mesg(ProtocolCode.SET_ATOM_PIN_STATUS, io_number, status) - def get_tool_in_io_state(self, io_number): + def get_tool_in_io_state(self, pin): """Get the Atom pin status Args: - io_number (int): pin number + pin (int): pin Returns: 0 or 1. 1: high 0: low """ - return self._mesg(ProtocolCode.GET_ATOM_PIN_STATUS, io_number, has_reply=True) + if pin not in (0, 1): + raise ValueError("pin must be 0 or 1") + + return self._mesg(ProtocolCode.GET_ATOM_PIN_STATUS, pin, has_reply=True) def set_tool_led_color(self, r, g, b): """Set the Atom LED color diff --git a/pymycobot/myarmc.py b/pymycobot/myarmc.py index 1c47f69e..f6d37e07 100644 --- a/pymycobot/myarmc.py +++ b/pymycobot/myarmc.py @@ -1,14 +1,13 @@ # coding=utf-8 from __future__ import division - from pymycobot.common import ProtocolCode from pymycobot.myarm_api import MyArmAPI class MyArmC(MyArmAPI): - def __init__(self, port, baudrate="1000000", timeout=0.1, debug=False): + def __init__(self, port, baudrate=1000000, timeout=0.1, debug=False): super(MyArmC, self).__init__(port, baudrate, timeout, debug) def is_tool_btn_clicked(self, mode=1): @@ -21,6 +20,12 @@ def is_tool_btn_clicked(self, mode=1): Returns: list[int]: 0/1, 1: press, 0: no press """ + if not isinstance(mode, int): + raise TypeError('mode must be int') + + if mode not in [1, 2, 3, 254]: + raise ValueError('mode must be 1, 2, 3 or 254') + return self._mesg(ProtocolCode.GET_ATOM_PRESS_STATUS, mode, has_reply=True) def get_joints_coord(self): diff --git a/pymycobot/robot_info.py b/pymycobot/robot_info.py index c16e0849..6b528cef 100644 --- a/pymycobot/robot_info.py +++ b/pymycobot/robot_info.py @@ -310,6 +310,12 @@ class RobotLimit: "coords_min": [-310, -310, -140, -180, -180, -180], "coords_max": [310, 310, 480, 180, 180, 180] }, + "MyArmC": { + "joint_id": [1, 2, 3, 4, 5, 6], + "servo_id": [1, 2, 3, 4, 5, 6, 7, 8], + "angles_min": [-165, -80, -100, -160, -90, -180], + "angles_max": [165, 100, 80, 160, 90, 180] + }, "MyArmM": { "joint_id": [1, 2, 3, 4, 5, 6, 7], "servo_id": [1, 2, 3, 4, 5, 6, 7, 8], From 455b90571147ae6436b4d564286218350694313d Mon Sep 17 00:00:00 2001 From: K-Shuffler <2220138602@qq.com> Date: Fri, 21 Mar 2025 17:07:11 +0800 Subject: [PATCH 09/19] fix myArmM&C demo bug --- demo/myArm_M&C_demo/myarm_c.py | 25 +++++++++++++++++-------- demo/myArm_M&C_demo/myarm_m.py | 15 ++++++++++----- 2 files changed, 27 insertions(+), 13 deletions(-) diff --git a/demo/myArm_M&C_demo/myarm_c.py b/demo/myArm_M&C_demo/myarm_c.py index 2d89f8b3..4ba68c60 100644 --- a/demo/myArm_M&C_demo/myarm_c.py +++ b/demo/myArm_M&C_demo/myarm_c.py @@ -1,10 +1,10 @@ # 读取myarm_c的角度并发送 import socket -from pymycobot import MyArmC, MyArmM +from pymycobot import MyArmC import serial.tools.list_ports -import time -def get_port(): # 获取所有串口号 + +def get_port(): # 获取所有串口号 port_list = serial.tools.list_ports.comports() i = 1 res = {} @@ -14,6 +14,7 @@ def get_port(): # 获取所有串口号 i += 1 return res + def main(): port_dict = get_port() port_c = input("input myArm C port: ") @@ -25,10 +26,18 @@ def main(): # 请求连接 client.connect((HOST, PORT)) while True: - angle = c.get_joints_angle() - if angle is not None: - data = '\n' + str(angle) - client.send(data.encode('utf-8')) + try: + angle = c.get_joints_angle() + if angle is not None: + data = '\n' + str(angle) + client.send(data.encode('utf-8')) + except KeyboardInterrupt: + break + + except ConnectionAbortedError: + print("socket closed.") + break + if __name__ == "__main__": - main() \ No newline at end of file + main() diff --git a/demo/myArm_M&C_demo/myarm_m.py b/demo/myArm_M&C_demo/myarm_m.py index 70adfe55..f57b54e5 100644 --- a/demo/myArm_M&C_demo/myarm_m.py +++ b/demo/myArm_M&C_demo/myarm_m.py @@ -5,7 +5,8 @@ import time import socket -def get_port(): # 获取所有串口号 + +def get_port(): # 获取所有串口号 port_list = serial.tools.list_ports.comports() i = 1 res = {} @@ -15,15 +16,19 @@ def get_port(): # 获取所有串口号 i += 1 return res + def processing_data(data): data = data.split('\n')[-1] angle = list(data[1:-1].split(',')) angle = [float(i) for i in angle] angle[2] *= -1 gripper_angle = angle.pop(-1) - angle.append((gripper_angle - 0.08) / (-95.27 - 0.08) * (-123.13 + 1.23) - 1.23) + gripper_angle = (gripper_angle - 0.08) / (-95.27 - 0.08) * (-123.13 + 1.23) - 1.23 + gripper_angle = max(min(gripper_angle, -118), 2) + angle.append(gripper_angle) return angle + def main(): HOST = '127.0.0.1' PORT = 8001 @@ -47,11 +52,11 @@ def main(): while True: try: data = conn.recv(1024).decode('utf-8') - angle = processing_data(data) - m.set_joints_angle(angle, speed) + angles = processing_data(data) + m.set_joints_angle(angles, speed) except MyArmDataException: pass - + if __name__ == "__main__": main() From 3fd7ce5a6181b3a5710a54ac0b697df57affc2cc Mon Sep 17 00:00:00 2001 From: K-Shuffler <2220138602@qq.com> Date: Mon, 24 Mar 2025 09:48:16 +0800 Subject: [PATCH 10/19] update M&C demo --- demo/myArm_M&C_demo/myarm_c.py | 150 ++++++++++++++++++++++----- demo/myArm_M&C_demo/myarm_m.py | 183 ++++++++++++++++++++++++--------- demo/myArm_M&C_demo/utils.py | 56 ++++++++++ 3 files changed, 314 insertions(+), 75 deletions(-) create mode 100644 demo/myArm_M&C_demo/utils.py diff --git a/demo/myArm_M&C_demo/myarm_c.py b/demo/myArm_M&C_demo/myarm_c.py index 4ba68c60..75bf508c 100644 --- a/demo/myArm_M&C_demo/myarm_c.py +++ b/demo/myArm_M&C_demo/myarm_c.py @@ -1,43 +1,141 @@ -# 读取myarm_c的角度并发送 +#!/usr/bin/env python +# -*- coding: UTF-8 -*- +import threading +import traceback import socket +import typing as T +from serial import SerialException from pymycobot import MyArmC -import serial.tools.list_ports +import utils -def get_port(): # 获取所有串口号 - port_list = serial.tools.list_ports.comports() - i = 1 - res = {} - for port in port_list: - print("{} - {}".format(i, port.device)) - res[str(i)] = port.device - i += 1 - return res +class Config: + host = "0.0.0.0" # The host address to listen on + port = 8001 # The port to listen on + maximum_connections = 5 # The maximum number of connections allowed + + +def setup_robotic_connect(comport: str) -> T.Optional[MyArmC]: + robotic_api = None + try: + robotic_api = MyArmC(comport) + except Exception as e: + print(f" # (Error) Error while connecting robotic: {e}") + return robotic_api + + +class SocketTransportServer(threading.Thread): + def __init__(self, host="0.0.0.0", port=8001, listen_size=5): + super().__init__(daemon=True) + self.port = port + self.host = host + self.running = True + self.listen_size = listen_size + self.socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM) + self.socket.bind((host, port)) + self.socket.listen(listen_size) + self.socket_client_table = {} + if host == "0.0.0.0": + host = utils.get_local_host() + print(f" # (Info) Start listening on {host}:{port}") + + def accept(self): + while self.running is True: + try: + yield self.socket.accept() + except OSError: + print(f" # (Error) Error while accepting socket") + continue + + def context(self, address, connection): + while self.running is True: + try: + data_buffer = connection.recv() + if len(data_buffer) == 0: + print(f" # (Warn) Close connection from {address}") + del self.socket_client_table[address] + connection.close() + break + yield data_buffer + except Exception as e: + print(f" # (Error) Error while reading socket: {e}") + traceback.print_exc() + break + + def run(self): + for conn, addr in self.accept(): + print(f" # (Info) Accept connection from {addr}") + self.socket_client_table[addr] = conn + + def sendall(self, data): + exit_addr_list = [] + for address, connection in self.socket_client_table.items(): + print(f" # (Info) {address} => {data}") + try: + connection.send(data) + except WindowsError: + print(f" # (Warn) Close connection from {address}") + exit_addr_list.append(address) + + for exit_addr in exit_addr_list: + del self.socket_client_table[exit_addr] + + def close(self): + print(f" # (Info) close socket on {self.host}:{self.port}") + self.running = False + self.socket.close() def main(): - port_dict = get_port() - port_c = input("input myArm C port: ") - c_port = port_dict[port_c] - c = MyArmC(c_port, debug=False) - HOST = '127.0.0.1' - PORT = 8001 - client = socket.socket(socket.AF_INET, socket.SOCK_STREAM) - # 请求连接 - client.connect((HOST, PORT)) + serial_ports = utils.get_local_serial_port() + if len(serial_ports) == 0: + print(" # (Warn) No serial ports found. exit") + return + + serial_comport = utils.select("# (Info) Please select a serial port to connect robotic arm:", serial_ports, 1) + + print(f" # (Info) Selected {serial_comport} to connect robotic arm") + robotic_api = setup_robotic_connect(serial_comport) + if robotic_api is None: + print(" # (Error) Failed to connect robotic arm. exit") + return + + print(" # (Info) Start listening for changes in the angle of the robotic arm") + print(" # (Info) Press 【Ctrl+C】 to exit") + transport_server = SocketTransportServer( + host=Config.host, + port=Config.port, + listen_size=Config.maximum_connections + ) + transport_server.start() while True: try: - angle = c.get_joints_angle() - if angle is not None: - data = '\n' + str(angle) - client.send(data.encode('utf-8')) + angles = robotic_api.get_joints_angle() + if angles is None: + continue + + print(f" # (Info) Current angle: {angles}") + if max(angles) > 200 or min(angles) < -200: + print(" # (Warn) There is no communication between the joints, please check the connection") + continue + + transport_server.sendall(f"\n{angles}".encode('utf-8')) except KeyboardInterrupt: + print(" # (Info) Exit") break - except ConnectionAbortedError: - print("socket closed.") + except SerialException as e: + print(f" # (Error) Serial port error: {e}") break + except Exception as e: + print(f" # (Error) {e}") + + transport_server.close() + robotic_api._serial_port.close() + print(" # (Info) Close socket and serial port") + print(" # (Info) Exit") + if __name__ == "__main__": main() diff --git a/demo/myArm_M&C_demo/myarm_m.py b/demo/myArm_M&C_demo/myarm_m.py index f57b54e5..23ff5d71 100644 --- a/demo/myArm_M&C_demo/myarm_m.py +++ b/demo/myArm_M&C_demo/myarm_m.py @@ -1,61 +1,146 @@ -# myarm_m接收角度并执行 -from pymycobot.error import MyArmDataException -from pymycobot import MyArmC, MyArmM -import serial.tools.list_ports -import time +#!/usr/bin/env python +# -*- coding: UTF-8 -*- import socket +import typing as T +import utils +from serial import SerialException +from pymycobot import MyArmM -def get_port(): # 获取所有串口号 - port_list = serial.tools.list_ports.comports() - i = 1 - res = {} - for port in port_list: - print("{} - {}".format(i, port.device)) - res[str(i)] = port.device - i += 1 - return res +class Config: + host = "127.0.0.1" # C650 server host. + port = 8001 # C650 server port. + angle_conversion = False # Whether to roll back the angles of the robotic arm. + execution_speed: int = 50 # The speed of the robotic arm. + + +gripper_angular_transformation_equations = lambda x: round((x - 0.08) / (-95.27 - 0.08) * (-123.13 + 1.23) - 1.23) + + +M750_limit_info = [ + (-170, 170), + (-83, 83), + (-90, 84), + (-155, 153), + (-91, 88), + (-153, 153), + (-118, 2) +] + + +def flexible_parameters(angles: list, rollback: bool = True) -> list: + if rollback is True: + angles[-1] = gripper_angular_transformation_equations(angles[-1]) + angles[1] *= -1 + angles[2] *= -1 + + positions = [] + for i, angle in enumerate(angles): + min_angle, max_angle = M750_limit_info[i] + if angle < min_angle: + positions.append(min_angle) + elif angle > max_angle: + positions.append(max_angle) + else: + positions.append(angle) + + return positions + + +def setup_socket_connect(host, port): + try: + socket_api = socket.socket(socket.AF_INET, socket.SOCK_STREAM) + socket_api.connect((host, port)) + except ConnectionRefusedError: + return None + except OSError as e: + print(f" # (Error) Error while connecting socket: {e}") + exit(1) + return socket_api + + +def setup_robotic_connect(comport: str) -> T.Optional[MyArmM]: + robotic_api = None + try: + robotic_api = MyArmM(comport) + except SerialException as serial_error: + for error in serial_error.args: + if error.startswith("could not open port"): + print(f" # (Error) Serial port 【{comport}】 is not available.") + else: + print(f" # (Error) Error while connecting robotic: {serial_error}") + return robotic_api def processing_data(data): data = data.split('\n')[-1] - angle = list(data[1:-1].split(',')) - angle = [float(i) for i in angle] - angle[2] *= -1 - gripper_angle = angle.pop(-1) - gripper_angle = (gripper_angle - 0.08) / (-95.27 - 0.08) * (-123.13 + 1.23) - 1.23 - gripper_angle = max(min(gripper_angle, -118), 2) - angle.append(gripper_angle) - return angle - - -def main(): - HOST = '127.0.0.1' - PORT = 8001 - server = socket.socket(socket.AF_INET, socket.SOCK_STREAM) - server.bind((HOST, PORT)) - server.listen(5) - port_dict = get_port() - port_m = input("input myArm M port: ") - m_port = port_dict[port_m] - m = MyArmM(m_port, 1000000, debug=False) - speed = 100 - - print('Server start at: %s:%s' % (HOST, PORT)) - print('wait for connection...') - # 接收客户端请求 + angles = list(map(float, data[1:-1].split(','))) + return angles + +def main(host: str = Config.host, port: int = Config.port): + serial_ports = utils.get_local_serial_port() + if len(serial_ports) == 0: + print(" # (Warn) No serial ports found. exit") + return + + serial_comport = utils.select("# (Info) Please select a serial port to connect robotic arm:", serial_ports, 1) + + print(f" # (Info) Selected {serial_comport} to connect robotic arm") + + robotic_api = setup_robotic_connect(serial_comport) + if robotic_api is None: + print(" # (Error) Failed to connect robotic arm. exit") + return + + print(f" # (Info) Connected to robotic arm at {serial_comport}") + print(f" # (Info) Connecting to {host}:{port}...") + + while True: + socket_api = setup_socket_connect(host, port) + if socket_api is not None: + break + print(f" # (Error) Can't connect to {host}:{port}, connection retrying...") + + print(f" # (Info) Connected to {host}:{port}") + print(f" # (Info) Start listening for changes in the angle of the robotic arm") + print(f" # (Info) Press 【Ctrl+C】 to exit") while True: - conn, addr = server.accept() - # 客户端IP - print('Connected by ', addr) - while True: - try: - data = conn.recv(1024).decode('utf-8') - angles = processing_data(data) - m.set_joints_angle(angles, speed) - except MyArmDataException: - pass + try: + data = socket_api.recv(1024) + angles = processing_data(data.decode('utf-8')) + angles = flexible_parameters(angles, rollback=Config.angle_conversion) + + arm_angles: list = robotic_api.get_joints_angle() or [] + for joint_id, angle in enumerate(arm_angles, start=1): + if angle > 200 or angle < -200: + print( + f" # (Error) The angle of joint {joint_id} exceeds the limit {angle}, " + f"the M750 cannot follow the movement" + ) + break + else: + print(f" # (Info) Recv angles: {angles}") + robotic_api.set_joints_angle(angles=angles, speed=Config.execution_speed) + + except KeyboardInterrupt: + robotic_api.clear_recv_queue() + print(" # (Info) Received an exit instruction") + break + + except SerialException as serial_error: + print(f" # (Error) Serial port error: {serial_error}") + break + + except Exception as e: + print(f" # (Error) {e}") + break + + print(f" # (Info) Disconnecting from robotic arm at {serial_comport}...") + robotic_api._serial_port.close() + print(f" # (Info) Disconnecting from {host}:{port}...") + socket_api.close() + print(" # (Info) Exit") if __name__ == "__main__": diff --git a/demo/myArm_M&C_demo/utils.py b/demo/myArm_M&C_demo/utils.py new file mode 100644 index 00000000..25412c43 --- /dev/null +++ b/demo/myArm_M&C_demo/utils.py @@ -0,0 +1,56 @@ +#!/usr/bin/env python +# -*- coding: UTF-8 -*- +import socket +import typing as T +from serial.tools import list_ports +_T = T.TypeVar("_T") + + +def select( + message: str, + options: T.List[_T], + default: int = None, + level: int = 1, + echo: T.Callable = lambda msg: msg, + start: int = 1 +) -> T.Optional[_T]: + print(f"{message}\r\n") + p = " " * level + for ordinal, option in enumerate(options, start=start): + print(f"{p}{ordinal}. {echo(option)}") + else: + print() + + tips = f" # (Info) Select ({echo(options[default - 1])}) >" if default is not None else f" # (Info) Select: " + + while True: + try: + user_input = input(f"{tips}").lower().strip() + if len(user_input) == 0 and (default is not None or len(options) == 1): + return options[default - 1] + + if user_input.isdigit(): + return options[int(user_input) - 1] + + if user_input in options: + return user_input + + if user_input in ("q", "quit"): + return None + + raise ValueError + except ValueError: + print(f" * Invalid input, please enter a number.") + + except IndexError: + print(f" * Invalid input, please enter a valid number.") + + +def get_local_serial_port(): # 获取所有串口号 + return [comport.device for comport in list_ports.comports()] + + +def get_local_host(): + hostname = socket.gethostname() + ip_address = socket.gethostbyname(hostname) + return ip_address From 87eed94b0a75a13c6e8fa6844924788060d47755 Mon Sep 17 00:00:00 2001 From: K-Shuffler <2220138602@qq.com> Date: Mon, 24 Mar 2025 10:54:14 +0800 Subject: [PATCH 11/19] fix MyCobot280 rdkx5 bug --- pymycobot/common.py | 4 +- pymycobot/mycobot280rdkx5.py | 79 +++++++++++++++++++++--------------- 2 files changed, 50 insertions(+), 33 deletions(-) diff --git a/pymycobot/common.py b/pymycobot/common.py index 31f6c222..4d995f49 100644 --- a/pymycobot/common.py +++ b/pymycobot/common.py @@ -117,7 +117,9 @@ class ProtocolCode(object): GET_ATOM_VERSION = 0x09 CLEAR_ZERO_POS = 0x0A + SET_MONITOR_STATE = 0x0A SET_SERVO_CW = 0x0B + GET_MONITOR_STATE = 0x0B GET_SERVO_CW = 0x0C CLEAR_WAIST_QUEUE = 0x0D SET_LIMIT_SWITCH = 0x0E @@ -125,6 +127,7 @@ class ProtocolCode(object): SET_POS_SWITCH = 0x0B GET_POS_SWITCH = 0x0C + GET_COMMAND_QUEUE = 0x0C SetHTSGripperTorque = 0x35 GetHTSGripperTorque = 0x36 @@ -360,7 +363,6 @@ class ProtocolCode(object): GET_TOQUE_GRIPPER = 0xE9 SET_ERROR_DETECT_MODE = 0xE8 GET_ERROR_DETECT_MODE = 0xE9 - JOG_BASE_RPY = 0xEB MERCURY_GET_BASE_COORDS = 0xF0 MERCURY_SET_BASE_COORD = 0xF1 diff --git a/pymycobot/mycobot280rdkx5.py b/pymycobot/mycobot280rdkx5.py index e73b34da..c11cae18 100644 --- a/pymycobot/mycobot280rdkx5.py +++ b/pymycobot/mycobot280rdkx5.py @@ -13,11 +13,13 @@ from pymycobot.error import calibration_parameters -def setup_serial_connect(port, baudrate, timeout): - serial_api = serial.Serial(port=port, baudrate=baudrate, timeout=timeout) +def setup_serial_port(port, baudrate, timeout=None): + serial_api = serial.Serial() + serial_api.port = port + serial_api.baudrate = baudrate + serial_api.timeout = timeout serial_api.rts = False - if not serial_api.is_open: - serial_api.open() + serial_api.open() return serial_api @@ -79,16 +81,7 @@ def _res(self, real_command, has_reply, genre): if genre == ProtocolCode.SET_SSID_PWD: return 1 - if genre == ProtocolCode.GET_QUICK_INFO: - res = [] - valid_data = data[4:-1] - for header_i in range(0, len(valid_data), 2): - if header_i < 26: - one = valid_data[header_i: header_i + 2] - res.append(self._decode_int16(one)) - res.extend(valid_data[25:]) - else: - res = self._process_received(data, genre) + res = self._process_received(data, genre) if res is None: return None @@ -153,7 +146,7 @@ def _res(self, real_command, has_reply, genre): return self._int2coord(self._process_single(res)) elif genre in [ProtocolCode.GET_REBOOT_COUNT]: return self._process_high_low_bytes(res) - elif genre in (ProtocolCode.GET_ANGLES_COORDS, ProtocolCode.GET_QUICK_INFO): + elif genre in (ProtocolCode.GET_ANGLES_COORDS, ): r = [] for index, el in enumerate(res): if index < 6: @@ -188,6 +181,9 @@ def clear_queue(self): """Clear the command queue""" return self._mesg(ProtocolCode.CLEAR_COMMAND_QUEUE) + def get_queue_length(self): + return self._mesg(ProtocolCode.GET_COMMAND_QUEUE) + def async_or_sync(self): """Set the command execution mode Return: @@ -212,6 +208,25 @@ def clear_error_information(self): """Clear robot error message""" return self._mesg(ProtocolCode.CLEAR_ERROR_INFO, has_reply=True) + def set_monitor_state(self, state): + """ + Set the monitoring state + Args: + state: 0 - Disable monitoring + 1 - Enable monitoring + """ + return self._mesg(ProtocolCode.SET_MONITOR_STATE, state) + + def get_monitor_state(self): + """ + Get the monitoring state + """ + return self._mesg(ProtocolCode.GET_MONITOR_STATE, has_reply=True) + + def get_reboot_count(self): + """Get the number of times the robot has been restarted""" + return self._mesg(ProtocolCode.GET_REBOOT_COUNT, has_reply=True) + def power_on(self): """Open communication with Atom.""" return self._mesg(ProtocolCode.POWER_ON) @@ -231,15 +246,12 @@ def is_power_on(self): return self._mesg(ProtocolCode.IS_POWER_ON, has_reply=True) def release_all_servos(self, data=None): - """Relax all joints + """Relax all joints""" + return self._mesg(ProtocolCode.RELEASE_ALL_SERVOS) - Args: - data: 1 - Undamping (The default is damping) - """ - if data is None: - return self._mesg(ProtocolCode.RELEASE_ALL_SERVOS) - else: - return self._mesg(ProtocolCode.RELEASE_ALL_SERVOS, data) + def focus_all_servos(self): + """Damping all joints""" + return self._mesg(ProtocolCode.FOCUS_ALL_SERVOS) def read_next_error(self): """Robot Error Detection @@ -316,10 +328,6 @@ def get_angles_coords(self): """Get joint angles and coordinates""" return self._mesg(ProtocolCode.GET_ANGLES_COORDS, has_reply=True) - def get_quick_move_message(self): - """Get the quick move message""" - return self._mesg(ProtocolCode.GET_QUICK_INFO, has_reply=True) - # JOG mode and operation def write_angles_time_control(self, angles, step_time): """Write the angle of a joint in time control mode @@ -509,6 +517,11 @@ def set_four_pieces_zero(self): """ return self._mesg(ProtocolCode.SET_FOUR_PIECES_ZERO, has_reply=True) + def set_servo_data(self, servo_id, data_id, value, mode=None): + if data_id in (0, 1, 2, 3, 4): + raise ValueError("Invalid data_id, addr 0 ~ 4 cannot be modified") + return super().set_servo_data(servo_id, data_id, value, mode) + # Running Status and Settings def set_joint_max(self, id, angle): """Set the joint maximum angle @@ -810,7 +823,7 @@ def __init__(self, port, baudrate=100_0000, timeout=0.1, debug=False, thread_loc debug : whether show debug info """ super().__init__(debug, thread_lock) - self._serial_port = setup_serial_connect(port=port, baudrate=baudrate, timeout=timeout) + self._serial_port = setup_serial_port(port=port, baudrate=baudrate, timeout=timeout) self._write = functools.partial(write, self) self._read = functools.partial(read, self) @@ -885,12 +898,14 @@ def close(self): def main(): - mc_sock = MyCobot280RDKX5Socket('192.168.1.246', port=30002, debug=True) + rdx_x5 = MyCobot280RDKX5("Com30", debug=True) + print(rdx_x5.get_reboot_count()) + # mc_sock = MyCobot280RDKX5Socket('192.168.1.246', port=30002, debug=True) # print(mc_sock.send_angle(1, 100, 50)) # print(mc_sock.get_quick_move_message()) - print(mc_sock.set_gpio_mode(0)) - print(mc_sock.setup_gpio_state(5, 1, initial=1)) - print(mc_sock.set_gpio_output(5, 0)) + # print(mc_sock.set_gpio_mode(0)) + # print(mc_sock.setup_gpio_state(5, 1, initial=1)) + # print(mc_sock.set_gpio_output(5, 0)) # print(mc_sock.get_gpio_input(5)) From 49872068136099b7b70104df1cb586e30a49e2ce Mon Sep 17 00:00:00 2001 From: K-Shuffler <2220138602@qq.com> Date: Mon, 24 Mar 2025 15:57:51 +0800 Subject: [PATCH 12/19] fix MyArmM&C bugs --- pymycobot/error.py | 13 +++++++++++ pymycobot/myarm_api.py | 45 ++++++++++++++++++++++++------------- pymycobot/myarmm.py | 23 +++++++++---------- pymycobot/myarmm_control.py | 13 ++++++----- pymycobot/robot_info.py | 8 +++---- 5 files changed, 65 insertions(+), 37 deletions(-) diff --git a/pymycobot/error.py b/pymycobot/error.py index 768ea45d..2f9b3182 100644 --- a/pymycobot/error.py +++ b/pymycobot/error.py @@ -1325,6 +1325,9 @@ def calibration_parameters(**kwargs): if not value: raise ValueError("angles value can't be empty") + if len(value) != 7: + raise ValueError("The length of `angles` must be 7.") + for i, v in enumerate(value): min_angle = limit_info["angles_min"][i] max_angle = limit_info["angles_max"][i] @@ -1338,6 +1341,9 @@ def calibration_parameters(**kwargs): if not min_coord <= value <= max_coord: raise ValueError(f"coord value not right, should be {min_coord} ~ {max_coord}, but received {value}") elif parameter == 'coords': + if len(value) != 6: + raise ValueError("The length of `coords` must be 6.") + for i, v in enumerate(value): min_coord = limit_info["coord_min"][i] max_coord = limit_info["coord_max"][i] @@ -1351,12 +1357,19 @@ def calibration_parameters(**kwargs): raise ValueError( f"angle value not right, should be {min_encoder} ~ {max_encoder}, but received {value}") elif parameter == 'encoders': + if len(value) != 8: + raise ValueError("The length of `encoders` must be 8.") + for i, v in enumerate(value): max_encoder = limit_info["encoders_max"][i] min_encoder = limit_info["encoders_min"][i] if v < min_encoder or v > max_encoder: raise ValueError( f"encoder value not right, should be {min_encoder} ~ {max_encoder}, but received {v}") + + if (2048 - value[1]) + (2048 - value[2]) != 0: + raise ValueError("The 2 and 3 servos must be inverse") + elif parameter == "speed": check_value_type(parameter, value_type, TypeError, int) if not 1 <= value <= 100: diff --git a/pymycobot/myarm_api.py b/pymycobot/myarm_api.py index 38da7e62..a5e4b5da 100644 --- a/pymycobot/myarm_api.py +++ b/pymycobot/myarm_api.py @@ -10,10 +10,12 @@ def setup_serial_connect(port, baudrate, timeout=None): - serial_api = serial.Serial(port=port, baudrate=baudrate, timeout=timeout) + serial_api = serial.Serial() + serial_api.port = port + serial_api.baudrate = baudrate + serial_api.timeout = timeout serial_api.rts = False - if not serial_api.is_open: - serial_api.open() + serial_api.open() return serial_api @@ -65,7 +67,7 @@ def _read_command_buffer(self, timeout=0.1): return commands def _read(self, genre): - if genre == ProtocolCode.GET_ROBOT_STATUS: + if genre == ProtocolCode.GET_ROBOT_ERROR_STATUS: timeout = 90 elif genre == ProtocolCode.SET_SSID_PWD or genre == ProtocolCode.GET_SSID_PWD: timeout = 0.05 @@ -99,7 +101,6 @@ def _mesg(self, genre, *args, **kwargs): """ real_command, has_reply, _async = super(MyArmMCProcessor, self)._mesg(genre, *args, **kwargs) with self.lock: - time.sleep(0.1) self._write(self._flatten(real_command)) if not has_reply: @@ -388,8 +389,11 @@ def set_servo_cw(self, servo_id, data): servo_id (int): 0 - 254 data (int): 0 - 32 """ - assert 0 <= data <= 32, "data must be between 0 and 32" self.calibration_parameters(servo_id=servo_id) + if servo_id == 8: + assert 0 <= data <= 16, "data must be between 0 and 16" + else: + assert 0 <= data <= 32, "data must be between 0 and 32" self._mesg(ProtocolCode.SET_SERVO_MOTOR_CLOCKWISE, servo_id, data) def get_servo_cw(self, servo_id): @@ -408,8 +412,11 @@ def set_servo_cww(self, servo_id, data): servo_id (int): 0 - 254 data (int): 0 - 32 """ - assert 0 <= data <= 32, "data must be between 0 and 32" self.calibration_parameters(servo_id=servo_id) + if servo_id == 8: + assert 0 <= data <= 16, "data must be between 0 and 16" + else: + assert 0 <= data <= 32, "data must be between 0 and 32" return self._mesg(ProtocolCode.SET_SERVO_MOTOR_COUNTER_CLOCKWISE, servo_id, data) def get_servo_cww(self, servo_id): @@ -449,7 +456,7 @@ def get_servo_system_data(self, servo_id, addr, mode): """ if mode not in (1, 2): raise ValueError('mode must be 1 or 2') - self.calibration_parameters(servo_id=servo_id, servo_addr=addr) + self.calibration_parameters(servo_id=servo_id) return self._mesg(ProtocolCode.GET_SERVO_MOTOR_CONFIG, servo_id, addr, mode, has_reply=True) def set_master_out_io_state(self, pin_number, status=1): @@ -484,7 +491,10 @@ def set_tool_out_io_state(self, io_number, status=1): io_number (int): 1 - 2 status: 0 or 1; 0: low; 1: high. default: 1 """ - self.calibration_parameters(master_pin=io_number, status=status) + if io_number not in (1, 2): + raise ValueError("io_number must be 1 or 2") + if status not in (0, 1): + raise ValueError("status must be 0 or 1") self._mesg(ProtocolCode.SET_ATOM_PIN_STATUS, io_number, status) def get_tool_in_io_state(self, pin): @@ -497,8 +507,8 @@ def get_tool_in_io_state(self, pin): 0 or 1. 1: high 0: low """ - if pin not in (0, 1): - raise ValueError("pin must be 0 or 1") + if pin not in (1, 2): + raise ValueError("pin must be 1 or 2") return self._mesg(ProtocolCode.GET_ATOM_PIN_STATUS, pin, has_reply=True) @@ -511,8 +521,13 @@ def set_tool_led_color(self, r, g, b): b: 0-255 """ - self._mesg(ProtocolCode.GET_ATOM_LED_COLOR, r, g, b) + return self._mesg(ProtocolCode.GET_ATOM_LED_COLOR, r, g, b) - def restore_servo_system_param(self): - """Restore servo motor system parameters""" - self._mesg(ProtocolCode.RESTORE_SERVO_SYSTEM_PARAM) + def restore_servo_system_param(self, servo_id): + """ + Restore servo motor system parameters + Args: + servo_id: 0 - 255 + """ + self.calibration_parameters(servo_id=servo_id) + return self._mesg(ProtocolCode.RESTORE_SERVO_SYSTEM_PARAM, servo_id) diff --git a/pymycobot/myarmm.py b/pymycobot/myarmm.py index 132d1036..b961f0fa 100644 --- a/pymycobot/myarmm.py +++ b/pymycobot/myarmm.py @@ -1,5 +1,8 @@ # coding=utf-8 from __future__ import division + +import time + from pymycobot.common import ProtocolCode from pymycobot.myarm_api import MyArmAPI @@ -17,7 +20,7 @@ def set_joint_angle(self, joint_id, angle, speed): angle (int) : 0 - 254 speed (int) : 1 - 100 """ - self.calibration_parameters(class_name=self.__class__.__name__, joint_id=joint_id, angle=angle, speed=speed) + self.calibration_parameters(joint_id=joint_id, angle=angle, speed=speed) self._mesg(ProtocolCode.SEND_ANGLE, joint_id, [self._angle2int(angle)], speed) def set_joints_angle(self, angles, speed): @@ -27,7 +30,7 @@ def set_joints_angle(self, angles, speed): angles (list[int]): 0 - 254 speed (int): 0 - 100 """ - self.calibration_parameters(class_name=self.__class__.__name__, angles=angles, speed=speed) + self.calibration_parameters(angles=angles, speed=speed) angles = list(map(self._angle2int, angles)) return self._mesg(ProtocolCode.SEND_ANGLES, angles, speed) @@ -44,13 +47,6 @@ def stop_robot(self): """The robot stops moving""" self._mesg(ProtocolCode.STOP) - # def is_in_position(self): - # """Whether the robot has reached the specified point - # Returns: - # - # """ - # return self._mesg(ProtocolCode.IS_IN_POSITION, reply=True) - def set_servo_encoder(self, servo_id, encoder, speed): """Sets the individual motor motion to the target encoder potential value @@ -60,7 +56,7 @@ def set_servo_encoder(self, servo_id, encoder, speed): speed: (int) 1 - 100 """ - self.calibration_parameters(class_name=self.__class__.__name__, servo_id=servo_id, encoder=encoder, speed=speed) + self.calibration_parameters(servo_id=servo_id, encoder=encoder, speed=speed) self._mesg(ProtocolCode.SET_ENCODER, servo_id, [encoder], speed) def set_servos_encoder(self, positions, speed): @@ -70,12 +66,12 @@ def set_servos_encoder(self, positions, speed): positions (list[int * 8]): 0 - 4095: speed (int): 1 - 100: """ - self.calibration_parameters(class_name=self.__class__.__name__, encoders=positions, speed=speed) + self.calibration_parameters(encoders=positions, speed=speed) self._mesg(ProtocolCode.SET_ENCODERS, positions, speed) def set_servos_encoder_drag(self, encoders, speeds): """Set multiple servo motors with a specified speed to the target encoder potential value""" - self.calibration_parameters(class_name=self.__class__.__name__, encoders=encoders, speeds=speeds) + self.calibration_parameters(encoders=encoders, speeds=speeds) self._mesg(ProtocolCode.SET_ENCODERS_DRAG, encoders, speeds) def set_assist_out_io_state(self, io_number, status=1): @@ -127,6 +123,9 @@ def set_servo_enabled(self, joint_id, state): 1-focus 0-release """ + if state not in (0, 1): + raise ValueError("state must be 0 or 1") + self.calibration_parameters(jonit_id=joint_id, state=state) self._mesg(ProtocolCode.RELEASE_ALL_SERVOS, joint_id, state) def get_joints_max(self): diff --git a/pymycobot/myarmm_control.py b/pymycobot/myarmm_control.py index 0f402f23..49eaac83 100644 --- a/pymycobot/myarmm_control.py +++ b/pymycobot/myarmm_control.py @@ -36,10 +36,12 @@ def setup_logging(debug: bool = False): def setup_serial_port(port, baudrate, timeout=0.1): - serial_api = serial.Serial(port, baudrate, timeout=timeout) + serial_api = serial.Serial() + serial_api.port = port + serial_api.baudrate = baudrate + serial_api.timeout = timeout serial_api.rts = False - if not serial_api.is_open: - serial_api.open() + serial_api.open() return serial_api @@ -159,7 +161,6 @@ def _read_genre_result(self, genre): r.append(self._int2angle(res[index])) return r elif self.__is_return(genre, data): - print("11111111111111111111111") return self._process_single(res) else: return res @@ -387,10 +388,10 @@ def jog_increment(self, joint_id, increment, speed): increment(int): incremental speed(int): int (0 - 100) """ - if not (isinstance(increment, int) or isinstance(increment, float)): + if isinstance(increment, (int, float)): raise ValueError("increment must be int or float") - self.calibration_parameters(joint_id=joint_id, speed=speed) + self.calibration_parameters(joint_id=joint_id, speed=speed, angle=increment) return self._mesg(ProtocolCode.JOG_INCREMENT, joint_id, [self._angle2int(increment)], speed) def pause(self): diff --git a/pymycobot/robot_info.py b/pymycobot/robot_info.py index 615603df..2c77d1df 100644 --- a/pymycobot/robot_info.py +++ b/pymycobot/robot_info.py @@ -327,8 +327,8 @@ class RobotLimit: "MyArmM": { "joint_id": [1, 2, 3, 4, 5, 6, 7], "servo_id": [1, 2, 3, 4, 5, 6, 7, 8], - "angles_min": [-170, -83, -90, -155, -91, -153, -118], - "angles_max": [170, 83, 84, 153, 88, 153, 2], + "angles_min": [-165, -80, -100, -160, -90, -180, -118], + "angles_max": [165, 100, 80, 160, 120, 180, 2], "encoders_min": [137, 1163, 1035, 1013, 248, 979, 220, 706], "encoders_max": [4004, 2945, 3079, 3026, 3724, 2994, 3704, 2048] }, @@ -338,8 +338,8 @@ class RobotLimit: "coord_id": [1, 2, 3, 4, 5, 6], "coord_min": [-833.325, -833.325, -351.11, -180, -180, -180], "coord_max": [833.325, 833.325, 1007.225, 180, 180, 180], - "angles_min": [-170, -83, -90, -155, -91, -153, -118], - "angles_max": [170, 83, 84, 153, 88, 153, 2], + "angles_min": [-165, -80, -100, -160, -90, -180, -118], + "angles_max": [165, 100, 80, 160, 120, 180, 2], "encoders_min": [137, 1163, 1035, 1013, 248, 979, 220, 706], "encoders_max": [4004, 2945, 3079, 3026, 3724, 2994, 3704, 2048] }, From 53241c7d0114d14696f3bbcf437628841a0f05b7 Mon Sep 17 00:00:00 2001 From: K-Shuffler <2220138602@qq.com> Date: Thu, 27 Mar 2025 19:30:37 +0800 Subject: [PATCH 13/19] fix bugs --- pymycobot/myarmm.py | 11 +++++++---- pymycobot/myarmm_control.py | 18 ++++++++++++------ pymycobot/robot_info.py | 2 +- 3 files changed, 20 insertions(+), 11 deletions(-) diff --git a/pymycobot/myarmm.py b/pymycobot/myarmm.py index b961f0fa..8e0ba8a1 100644 --- a/pymycobot/myarmm.py +++ b/pymycobot/myarmm.py @@ -115,18 +115,21 @@ def clear_robot_err(self): """Clear the robot abnormality Ignore the error joint and continue to move""" self._mesg(ProtocolCode.CLEAR_ROBOT_ERROR) - def set_servo_enabled(self, joint_id, state): + def set_servo_enabled(self, servo_id, state): """Set the servo motor torque switch Args: - joint_id (int): 0-254 254-all + servo_id (int): 0-254 254-all state: 0/1 1-focus 0-release """ if state not in (0, 1): raise ValueError("state must be 0 or 1") - self.calibration_parameters(jonit_id=joint_id, state=state) - self._mesg(ProtocolCode.RELEASE_ALL_SERVOS, joint_id, state) + + if servo_id not in range(0, 255): + raise ValueError("servo_id must be between 0 and 254") + + self._mesg(ProtocolCode.RELEASE_ALL_SERVOS, servo_id, state) def get_joints_max(self): """Read the maximum angle of all joints""" diff --git a/pymycobot/myarmm_control.py b/pymycobot/myarmm_control.py index 49eaac83..3b86edb1 100644 --- a/pymycobot/myarmm_control.py +++ b/pymycobot/myarmm_control.py @@ -91,8 +91,14 @@ def _read_genre_result(self, genre): res = [] valid_data = data[4:-1] for header_i in range(0, len(valid_data), 2): - one = valid_data[header_i: header_i + 2] - res.append(self._decode_int16(one)) + d = self._decode_int16(valid_data[header_i: header_i + 2]) + if d == 0: + res.append(d) + continue + reverse_bins = reversed(bin(d)[2:]) + rank = [i for i, e in enumerate(reverse_bins) if e != '0'] + res.append(rank) + else: res = self._process_received(data, genre) if not res: @@ -388,10 +394,10 @@ def jog_increment(self, joint_id, increment, speed): increment(int): incremental speed(int): int (0 - 100) """ - if isinstance(increment, (int, float)): + if not isinstance(increment, (int, float)): raise ValueError("increment must be int or float") - self.calibration_parameters(joint_id=joint_id, speed=speed, angle=increment) + self.calibration_parameters(joint_id=joint_id, angle=increment, speed=speed) return self._mesg(ProtocolCode.JOG_INCREMENT, joint_id, [self._angle2int(increment)], speed) def pause(self): @@ -558,10 +564,10 @@ def get_servo_data(self, servo_id, data_id, mode=None): values 0 - 4096 """ if mode is not None: - self.calibration_parameters(servo_id=servo_id, servo_addr=data_id, mode=mode) + self.calibration_parameters(servo_id=servo_id, mode=mode) return self._mesg(ProtocolCode.GET_SERVO_DATA, servo_id, data_id, mode) - self.calibration_parameters(servo_id=servo_id, servo_addr=data_id) + self.calibration_parameters(servo_id=servo_id) return self._mesg(ProtocolCode.GET_SERVO_DATA, servo_id, data_id) def set_servo_calibration(self, servo_id): diff --git a/pymycobot/robot_info.py b/pymycobot/robot_info.py index 2c77d1df..55586a04 100644 --- a/pymycobot/robot_info.py +++ b/pymycobot/robot_info.py @@ -319,7 +319,7 @@ class RobotLimit: "coords_max": [310, 310, 480, 180, 180, 180] }, "MyArmC": { - "joint_id": [1, 2, 3, 4, 5, 6], + "joint_id": [1, 2, 3, 4, 5, 6, 7], "servo_id": [1, 2, 3, 4, 5, 6, 7, 8], "angles_min": [-165, -80, -100, -160, -90, -180], "angles_max": [165, 100, 80, 160, 90, 180] From 3d60a0dc28592f38ae93eb67fd12e0b97f657f9b Mon Sep 17 00:00:00 2001 From: K-Shuffler <2220138602@qq.com> Date: Fri, 28 Mar 2025 11:06:08 +0800 Subject: [PATCH 14/19] fix bug --- pymycobot/error.py | 5 +++-- pymycobot/myarmm_control.py | 2 +- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/pymycobot/error.py b/pymycobot/error.py index 2f9b3182..ab90e81b 100644 --- a/pymycobot/error.py +++ b/pymycobot/error.py @@ -1325,8 +1325,9 @@ def calibration_parameters(**kwargs): if not value: raise ValueError("angles value can't be empty") - if len(value) != 7: - raise ValueError("The length of `angles` must be 7.") + joint_length = len(limit_info["joint_id"]) + if len(value) != joint_length: + raise ValueError(f"The length of `angles` must be {joint_length}.") for i, v in enumerate(value): min_angle = limit_info["angles_min"][i] diff --git a/pymycobot/myarmm_control.py b/pymycobot/myarmm_control.py index 3b86edb1..61f9a86a 100644 --- a/pymycobot/myarmm_control.py +++ b/pymycobot/myarmm_control.py @@ -254,7 +254,7 @@ def write_angle(self, joint_id, degree, speed): """Send the angle of a joint to robot arm. Args: - joint_id: (int) 1 ~ 7 + joint_id: (int) 1 ~ 6 degree: (float) -150 ~ 150 speed : (int) 1 ~ 100 """ From 4ff4ec45daaa6609cf6082b2483783a8faf64f41 Mon Sep 17 00:00:00 2001 From: K-Shuffler <2220138602@qq.com> Date: Fri, 28 Mar 2025 17:31:45 +0800 Subject: [PATCH 15/19] fix ThreeHand api bug --- pymycobot/end_control.py | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/pymycobot/end_control.py b/pymycobot/end_control.py index 9c8d48ae..2eb67c9e 100644 --- a/pymycobot/end_control.py +++ b/pymycobot/end_control.py @@ -418,13 +418,19 @@ def set_hand_gripper_pinch_action_speed_consort(self, pinch_pose, rank_mode, gri Idle flag. By default, there is no such byte. When this byte is 1, the idle finger can be freely manipulated. """ - self.calibration_parameters(class_name=self.__class__.__name__, pinch_pose=pinch_pose, rank_mode=rank_mode, idle_flag=idle_flag) + if idle_flag is None: + self.calibration_parameters( + class_name=self.__class__.__name__, pinch_pose=pinch_pose, rank_mode=rank_mode + ) return self.__set_tool_fittings_value( FingerGripper.SET_HAND_GRIPPER_PINCH_ACTION_SPEED_CONSORT, pinch_pose, rank_mode, gripper_id=gripper_id ) else: + self.calibration_parameters( + class_name=self.__class__.__name__, pinch_pose=pinch_pose, rank_mode=rank_mode, idle_flag=idle_flag + ) return self.__set_tool_fittings_value( FingerGripper.SET_HAND_GRIPPER_PINCH_ACTION_SPEED_CONSORT, pinch_pose, rank_mode, idle_flag, gripper_id=gripper_id - ) \ No newline at end of file + ) From 0c2fb1059147caa57873374df5c95291ecfe450e Mon Sep 17 00:00:00 2001 From: K-Shuffler <2220138602@qq.com> Date: Tue, 1 Apr 2025 18:53:05 +0800 Subject: [PATCH 16/19] Fixed the issue that Pro630 could not read data --- pymycobot/pro630.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/pymycobot/pro630.py b/pymycobot/pro630.py index ab95ec57..d4a9cebd 100644 --- a/pymycobot/pro630.py +++ b/pymycobot/pro630.py @@ -9,7 +9,7 @@ class Pro630(CloseLoop): - def __init__(self, port, baudrate="115200", timeout=0.1, debug=False): + def __init__(self, port, baudrate="115200", timeout=0.1, debug=False, save_serial_log=False): """ Args: port : port string @@ -27,6 +27,7 @@ def __init__(self, port, baudrate="115200", timeout=0.1, debug=False): GPIO.setwarnings(False) GPIO.setup(self.power_control_1, GPIO.IN) GPIO.setup(self.power_control_2, GPIO.OUT) + self.save_serial_log = save_serial_log self._serial_port = serial.Serial() self._serial_port.port = port self._serial_port.baudrate = baudrate From a4b939639e29dffe0b9c058ac44ecb242d618889 Mon Sep 17 00:00:00 2001 From: K-Shuffler <2220138602@qq.com> Date: Thu, 3 Apr 2025 15:10:03 +0800 Subject: [PATCH 17/19] fix MyArmM bug --- pymycobot/myarm_api.py | 2 ++ pymycobot/myarmm.py | 16 ++++++++++++++-- 2 files changed, 16 insertions(+), 2 deletions(-) diff --git a/pymycobot/myarm_api.py b/pymycobot/myarm_api.py index a5e4b5da..66cd3785 100644 --- a/pymycobot/myarm_api.py +++ b/pymycobot/myarm_api.py @@ -75,6 +75,8 @@ def _read(self, genre): timeout = 0.3 elif genre in [ProtocolCode.SET_GRIPPER_CALIBRATION]: timeout = 0.4 + elif genre == ProtocolCode.POWER_ON: + timeout = 2 else: timeout = 0.1 diff --git a/pymycobot/myarmm.py b/pymycobot/myarmm.py index 8e0ba8a1..ce51d151 100644 --- a/pymycobot/myarmm.py +++ b/pymycobot/myarmm.py @@ -70,8 +70,20 @@ def set_servos_encoder(self, positions, speed): self._mesg(ProtocolCode.SET_ENCODERS, positions, speed) def set_servos_encoder_drag(self, encoders, speeds): - """Set multiple servo motors with a specified speed to the target encoder potential value""" - self.calibration_parameters(encoders=encoders, speeds=speeds) + """ + Set multiple servo motors with a specified speed to the target encoder potential value + Args: + encoders (list[int * 8]): 0 - 4095: + speeds (list[int * 8]): -10000 - 10000: + """ + self.calibration_parameters(encoders=encoders) + if len(encoders) != len(speeds): + raise ValueError("encoders and speeds must have the same length") + + for sid, speed in enumerate(speeds): + if not -10000 < speed < 1000: + raise ValueError(f"servo {sid} speed must be between -10000 and 1000") + self._mesg(ProtocolCode.SET_ENCODERS_DRAG, encoders, speeds) def set_assist_out_io_state(self, io_number, status=1): From 75d06d699d37bb44f8390515c650dca2b0753c2a Mon Sep 17 00:00:00 2001 From: K-Shuffler <2220138602@qq.com> Date: Mon, 7 Apr 2025 15:09:24 +0800 Subject: [PATCH 18/19] MyArmM&C adds get_joints_coord interface --- pymycobot/myarm_api.py | 7 +++++++ pymycobot/myarmc.py | 6 ------ 2 files changed, 7 insertions(+), 6 deletions(-) diff --git a/pymycobot/myarm_api.py b/pymycobot/myarm_api.py index 66cd3785..76fa3f5a 100644 --- a/pymycobot/myarm_api.py +++ b/pymycobot/myarm_api.py @@ -261,6 +261,13 @@ def get_joints_angle(self): """ return self._mesg(ProtocolCode.GET_ANGLES, has_reply=True) + def get_joints_coord(self): + """Get the coordinates + Returns: + list[float] * 6: joints angle + """ + return self._mesg(ProtocolCode.GET_JOINTS_COORD, has_reply=True) + def set_servo_calibrate(self, servo_id): """Sets the zero position of the specified servo motor diff --git a/pymycobot/myarmc.py b/pymycobot/myarmc.py index f6d37e07..102cd72f 100644 --- a/pymycobot/myarmc.py +++ b/pymycobot/myarmc.py @@ -28,9 +28,3 @@ def is_tool_btn_clicked(self, mode=1): return self._mesg(ProtocolCode.GET_ATOM_PRESS_STATUS, mode, has_reply=True) - def get_joints_coord(self): - """Get the coordinates - Returns: - list[float] * 6: joints angle - """ - return self._mesg(ProtocolCode.GET_JOINTS_COORD, has_reply=True) From 06f187cc977cb7de24264504e63e4670cc47d76d Mon Sep 17 00:00:00 2001 From: K-Shuffler <2220138602@qq.com> Date: Mon, 7 Apr 2025 17:16:02 +0800 Subject: [PATCH 19/19] fix Pro630 bug --- pymycobot/pro630.py | 17 ++++++++++++++--- pymycobot/pro630client.py | 17 ++++++++++++++--- 2 files changed, 28 insertions(+), 6 deletions(-) diff --git a/pymycobot/pro630.py b/pymycobot/pro630.py index d4a9cebd..b811ce7c 100644 --- a/pymycobot/pro630.py +++ b/pymycobot/pro630.py @@ -44,8 +44,10 @@ def _mesg(self, genre, *args, **kwargs): read_data = super(Pro630, self)._mesg(genre, *args, **kwargs) if read_data is None: return None - elif read_data == 1: - return 1 + + if isinstance(read_data, int): + return read_data + valid_data, data_len = read_data res = [] if data_len in [8, 12, 14, 16, 26, 60]: @@ -60,6 +62,14 @@ def _mesg(self, genre, *args, **kwargs): elif data_len == 6 and genre in [ProtocolCode.GET_SERVO_STATUS, ProtocolCode.GET_SERVO_VOLTAGES, ProtocolCode.GET_SERVO_CURRENTS]: for i in range(data_len): res.append(valid_data[i]) + elif genre == ProtocolCode.MERCURY_ROBOT_STATUS: + res.extend(valid_data[:8]) + data = valid_data[8:20] + for header_i in range(0, len(data), 2): + one = data[header_i: header_i + 2] + res.append(self._decode_int16(one)) + res.extend(valid_data[20:26]) + return res else: for header_i in range(0, len(valid_data), 2): one = valid_data[header_i : header_i + 2] @@ -140,7 +150,8 @@ def _mesg(self, genre, *args, **kwargs): data1 = self._decode_int8(valid_data[i : i + 1]) res.append(0xFF & data1 if data1 < 0 else data1) res.append(self._decode_int8(valid_data)) - if res == []: + + if len(res) == 0: return None if genre in [ diff --git a/pymycobot/pro630client.py b/pymycobot/pro630client.py index 095ac5e1..602b22fe 100644 --- a/pymycobot/pro630client.py +++ b/pymycobot/pro630client.py @@ -39,8 +39,10 @@ def _mesg(self, genre, *args, **kwargs): read_data = super(Pro630Client, self)._mesg(genre, *args, **kwargs) if read_data is None: return None - elif read_data == 1: - return 1 + + if isinstance(read_data, int): + return read_data + valid_data, data_len = read_data res = [] if data_len in [8, 12, 14, 16, 26, 60]: @@ -55,6 +57,14 @@ def _mesg(self, genre, *args, **kwargs): elif data_len == 6 and genre in [ProtocolCode.GET_SERVO_STATUS, ProtocolCode.GET_SERVO_VOLTAGES, ProtocolCode.GET_SERVO_CURRENTS]: for i in range(data_len): res.append(valid_data[i]) + elif genre == ProtocolCode.MERCURY_ROBOT_STATUS: + res.extend(valid_data[:8]) + data = valid_data[8:20] + for header_i in range(0, len(data), 2): + one = data[header_i: header_i + 2] + res.append(self._decode_int16(one)) + res.extend(valid_data[20:26]) + return res else: for header_i in range(0, len(valid_data), 2): one = valid_data[header_i : header_i + 2] @@ -135,7 +145,8 @@ def _mesg(self, genre, *args, **kwargs): data1 = self._decode_int8(valid_data[i : i + 1]) res.append(0xFF & data1 if data1 < 0 else data1) res.append(self._decode_int8(valid_data)) - if res == []: + + if len(res) == 0: return None if genre in [