diff --git a/dobotWrapperPy/dobotapi.py b/dobotWrapperPy/dobotapi.py index c70b525..be2474d 100644 --- a/dobotWrapperPy/dobotapi.py +++ b/dobotWrapperPy/dobotapi.py @@ -1,4 +1,3 @@ -# mypy: disable-error-code="import" import struct import threading import time @@ -11,8 +10,6 @@ from .enums.HHTTrigMode import HHTTrigMode from .enums.alarm import Alarm -# Unused import: tagVersionRail - kept as per original -# from .enums.tagVersionRail import tagVersionRail from .message import Message from .paramsStructures import ( tagAutoLevelingParams, @@ -105,7 +102,6 @@ def close(self) -> None: del self.conn if self.verbose: port_name = "serial port" - # Check if self.conn and its attributes still exist before trying to access them if ( hasattr(self, "conn") and self.conn is not None @@ -124,7 +120,7 @@ def __del__(self) -> None: Destructor that ensures the connection is closed when the object is deleted. """ - if self._on: # Ensure close is only called if not already explicitly closed + if self._on: self.close() def initialize_robot(self) -> None: @@ -135,17 +131,19 @@ def initialize_robot(self) -> None: """ self.set_queued_cmd_start_exec() self.set_queued_cmd_clear() - # These parameter settings were implicitly queued in the original code by using ControlValues.Both. - # To maintain that behavior, is_queued=True is now explicitly passed. self.set_ptp_joint_params( tagPTPJointParams( - velocity=[200, 200, 200, 200], acceleration=[200, 200, 200, 200] + velocity=[200, 200, 200, 200], + acceleration=[200, 200, 200, 200], ), is_queued=True, ) self.set_ptp_coordinate_params( tagPTPCoordinateParams( - xyzVelocity=200, rVelocity=200, xyzAcceleration=200, rAcceleration=200 + xyzVelocity=200, + rVelocity=200, + xyzAcceleration=200, + rAcceleration=200, ), is_queued=True, ) @@ -153,7 +151,8 @@ def initialize_robot(self) -> None: tagPTPJumpParams(jumpHeight=10, zLimit=200), is_queued=True ) self.set_ptp_common_params( - tagPTPCommonParams(velocityRatio=100, accelerationRatio=100), is_queued=True + tagPTPCommonParams(velocityRatio=100, accelerationRatio=100), + is_queued=True, ) self.get_pose() @@ -170,7 +169,8 @@ def _send_command_with_params( Args: command_id: The ID of the command. - control_value: The base control value (e.g., ReadWrite for set, Zero for get). + control_value: The base control value (e.g., ReadWrite for set, + Zero for get). params: Optional byte string of parameters. wait: If True and command is queued, waits for execution. put_on_queue: If True, command is added to Dobot's queue. @@ -190,9 +190,9 @@ def _send_command_with_params( msg.ctrl = control_value # Immediate command if params is not None: - msg.params = bytearray(params) + msg.params = params else: - msg.params = bytearray([]) + msg.params = bytes(bytearray([])) if self.verbose: print(f"dobot: sending from {command_id.name}: {msg}") @@ -217,7 +217,7 @@ def get_queued_cmd_current_index(self) -> int: + f" Params: {response.params.hex()}" ) raise ValueError( - "Invalid response for GetQueuedCmdCurrentIndex: insufficient data" + "Invalid response for GetQueuedCmdCurrentIndex: " "insufficient data" ) idx = struct.unpack_from(" tagPose: if self.verbose: print( - "dobot: x:%03.1f y:%03.1f z:%03.1f r:%03.1f j1:%03.1f j2:%03.1f j3:%03.1f j4:%03.1f" + "dobot: x:%03.1f y:%03.1f z:%03.1f r:%03.1f " + "j1:%03.1f j2:%03.1f j3:%03.1f j4:%03.1f" % ( unpacked_response.x, unpacked_response.y, @@ -269,7 +270,8 @@ def _read_message(self) -> Optional[Message]: def _send_command(self, msg: Message, wait: bool = False) -> Message: """ - Sends a message to the Dobot and optionally waits for its execution if queued. + Sends a message to the Dobot and + optionally waits for its execution if queued. Args: msg: The message object to send. msg.ctrl indicates if queued. @@ -280,7 +282,8 @@ def _send_command(self, msg: Message, wait: bool = False) -> Message: Raises: TypeError: If no response is received. - ValueError: If waiting for a queued command and response is malformed. + ValueError: If waiting for a queued command + and response is malformed. """ self.lock.acquire() try: @@ -297,10 +300,11 @@ def _send_command(self, msg: Message, wait: bool = False) -> Message: if response is None: raise TypeError( - f"No response from Dobot. Sent msg ID {msg.id.name if msg.id else 'N/A'}, ctrl {msg.ctrl}" + "No response from Dobot. " + f"Sent msg ID {msg.id.name if msg.id else 'N/A'}," + f" ctrl {msg.ctrl}" ) - # Check if the command sent was intended to be queued (isQueued bit is set) is_command_queued_by_sender = ( msg.ctrl.value & ControlValues.isQueued.value ) != 0 @@ -308,7 +312,6 @@ def _send_command(self, msg: Message, wait: bool = False) -> Message: if not wait or not is_command_queued_by_sender: return response - # If wait=True and command was queued, response.params should contain queuedCmdIndex (uint64_t) if not response.params or len(response.params) < 8: warnings.warn( f"Command {msg.id.name} was queued (wait=True), but response.params " @@ -323,15 +326,16 @@ def _send_command(self, msg: Message, wait: bool = False) -> Message: f"dobot: waiting for command index {expected_idx} (sent {msg.id.name})" ) - wait_timeout_seconds = 10 # Max time to wait for a single command execution - start_wait_time = time.time() + # wait_timeout_seconds = 10 # Max time to wait + # start_wait_time = time.time() while True: - if time.time() - start_wait_time > wait_timeout_seconds: - warnings.warn( - f"Timeout waiting for command index {expected_idx} ({msg.id.name}) to execute." - ) - break + # if time.time() - start_wait_time > wait_timeout_seconds: + # warnings.warn( + # f"Timeout waiting for command index " + # f"{expected_idx} ({msg.id.name}) to execute." + # ) + # break try: current_idx = self.get_queued_cmd_current_index() @@ -398,14 +402,19 @@ def set_cp_cmd( return None def set_end_effector_gripper( - self, enable: bool = True, grip: bool = False, wait: bool = False, is_queued: bool = False + self, + enable: bool = True, + grip: bool = False, + wait: bool = False, + is_queued: bool = False, ) -> Optional[int]: """ Sets the status of the gripper. Protocol ID: 63. Can be queued. Args: - enable: True to enable (grip), False to disable (release). + enable: True to enable the functionality, False to disable. + grip: True to enable (grip), False to disable (release). wait: If True and command is queued, waits for execution. is_queued: If True, command is added to the queue. @@ -413,11 +422,13 @@ def set_end_effector_gripper( Queued command index if is_queued is True, else None. """ # Param: uint8_t isCtrlEnable (always 1 for set), uint8_t isGripped - params_payload = bytearray([(0x01 if enable else 0x00), (0x01 if grip else 0x00)]) + params_payload = bytearray( + [(0x01 if enable else 0x00), (0x01 if grip else 0x00)] + ) response = self._send_command_with_params( CommunicationProtocolIDs.END_EFFECTOR_GRIPPER, # ID 63 ControlValues.ReadWrite, - params_payload, + bytes(params_payload), wait, put_on_queue=is_queued, ) @@ -450,7 +461,7 @@ def set_end_effector_suction_cup( response = self._send_command_with_params( CommunicationProtocolIDs.END_EFFECTOR_SUCTION_CUP, # ID 62 ControlValues.ReadWrite, - params_payload, + bytes(params_payload), wait, put_on_queue=is_queued, ) @@ -703,7 +714,7 @@ def set_device_sn(self, device_serial_number: str) -> None: self._send_command_with_params( CommunicationProtocolIDs.DEVICE_SN, # ID 0 ControlValues.ReadWrite, # rw=1, isQueued=0 - params_payload, + bytes(params_payload), ) # Set operations usually don't return specific data beyond ACK, handled by _send_command. return None @@ -734,7 +745,7 @@ def set_device_name(self, device_name: str) -> None: self._send_command_with_params( CommunicationProtocolIDs.DEVICE_NAME, # ID 1 ControlValues.ReadWrite, # rw=1, isQueued=0 - params_payload, + bytes(params_payload), ) return None @@ -860,7 +871,7 @@ def reset_pose( self._send_command_with_params( CommunicationProtocolIDs.RESET_POSE, # ID 11 ControlValues.ReadWrite, # rw=1, isQueued=0 - params_payload, + bytes(params_payload), ) return None @@ -1068,7 +1079,7 @@ def set_hht_trig_mode(self, mode: HHTTrigMode) -> None: self._send_command_with_params( CommunicationProtocolIDs.SET_GET_HHTTRIG_MODE, # ID 40 ControlValues.ReadWrite, # rw=1, isQueued=0 - params_payload, + bytes(params_payload), ) def get_hht_trig_mode(self) -> HHTTrigMode: @@ -1101,7 +1112,7 @@ def set_hht_trig_output_enabled(self, is_enabled: bool) -> None: self._send_command_with_params( CommunicationProtocolIDs.SET_GET_HHTTRIG_OUTPUT_ENABLED, # ID 41 ControlValues.ReadWrite, # rw=1, isQueued=0 - params_payload, + bytes(params_payload), ) return None @@ -1206,7 +1217,7 @@ def set_end_effector_laser( response = self._send_command_with_params( CommunicationProtocolIDs.END_EFFECTOR_LASER, # ID 61 ControlValues.ReadWrite, - params_payload, + bytes(params_payload), wait, put_on_queue=is_queued, ) @@ -2113,9 +2124,12 @@ def get_io_di(self, address: int) -> Level: # Address param is for user expecta Returns: The Level of the DI. Caller should check response address. """ + address_format = " Tuple[float, float]: @@ -2354,7 +2368,7 @@ def set_wifi_config_mode(self, enable: bool) -> Message: return self._send_command_with_params( CommunicationProtocolIDs.WIFI_CONFIG_MODE, # ID 150 ControlValues.ReadWrite, # rw=1, isQueued=0 - params_payload, + bytes(params_payload), ) def get_wifi_config_mode(self) -> bool: @@ -2391,7 +2405,7 @@ def set_wifi_ssid(self, ssid: str) -> Message: return self._send_command_with_params( CommunicationProtocolIDs.WIFI_SSID, # ID 151 ControlValues.ReadWrite, # rw=1, isQueued=0 - params_payload, + bytes(params_payload), ) def get_wifi_ssid(self) -> str: @@ -2423,7 +2437,7 @@ def set_wifi_password(self, password: str) -> Message: return self._send_command_with_params( CommunicationProtocolIDs.WIFI_PASSWORD, # ID 152 ControlValues.ReadWrite, # rw=1, isQueued=0 - params_payload, + bytes(params_payload), ) def get_wifi_password(self) -> str: @@ -2598,7 +2612,7 @@ def set_lost_step_params(self, value: float) -> Message: return self._send_command_with_params( CommunicationProtocolIDs.SET_LOST_STEP_PARAMS, # ID 170 ControlValues.ReadWrite, # rw=1, isQueued=0 - params_payload, + bytes(params_payload), ) def set_lost_step_cmd( @@ -2648,7 +2662,8 @@ def set_queued_cmd_start_download( self, total_loop: int, line_per_loop: int ) -> Message: """ - Starts downloading commands to queue for offline execution. Immediate command. + Starts downloading commands to queue for offline execution. + Immediate command. Protocol ID: 243. Args: @@ -2658,11 +2673,13 @@ def set_queued_cmd_start_download( Returns: The response message from the Dobot. """ - params_payload = bytearray(struct.pack(" Message: diff --git a/dobotWrapperPy/dobotasync.py b/dobotWrapperPy/dobotasync.py index 860f0f3..4381e0b 100644 --- a/dobotWrapperPy/dobotasync.py +++ b/dobotWrapperPy/dobotasync.py @@ -64,16 +64,16 @@ async def connect(self) -> None: self._loop = asyncio.get_running_loop() self.dobotApiInterface.initialize_robot() - async def _on_sigint(self, signum: int, frame: Optional[Any]) -> None: + def _on_sigint(self, signum: int, frame: Optional[Any]) -> None: print("SIGINT received. Force Stopping robot...") self.force_stop() match self._endEffectorType: case EndEffectorType.CUP: - await self.suck(False) + self._run_in_loop(self.suck, False) case EndEffectorType.GRIPPER: - await self.grip(False, False) + self._run_in_loop(self.grip, False, False) case EndEffectorType.LASER: - await self.laser(False) + self._run_in_loop(self.laser, False) # Exit the program immediately — no further code runs sys.exit(130) # Standard exit code for Ctrl+C @@ -373,7 +373,7 @@ async def set_device_name(self, name: str) -> None: await self._run_in_loop(self.dobotApiInterface.set_device_name, name) async def set_device_rail_capability( - self, name: str, enable: bool, version: tagVersionRail + self, enable: bool, version: tagVersionRail ) -> None: await self._run_in_loop( self.dobotApiInterface.set_device_rail_capability, @@ -479,7 +479,7 @@ async def get_pwm(self, address: int) -> Tuple[float, float]: result = await self._run_in_loop(self.dobotApiInterface.get_io_pwm, address) return (result.frequency, result.dutyCycle) - async def set_motor(self, address: EMotorIndex, enable: bool, speed: float) -> None: + async def set_motor(self, address: EMotorIndex, enable: bool, speed: int) -> None: await self._run_in_loop( self.dobotApiInterface.set_e_motor, tagEMOTOR(address, enable, speed), diff --git a/dobotWrapperPy/paramsStructures.py b/dobotWrapperPy/paramsStructures.py index 8d4ba3f..8f5a771 100644 --- a/dobotWrapperPy/paramsStructures.py +++ b/dobotWrapperPy/paramsStructures.py @@ -2187,7 +2187,7 @@ class tagEMOTOR: # uint8 - Boolean flag for instruction enabled insEnabled: bool # double - Speed of the motor - speed: float + speed: int def pack(self) -> bytes: """ @@ -2201,7 +2201,7 @@ def pack(self) -> bytes: """ # Format: < (little-endian), B (uint8), B (uint8), d (double) # Total size = 1 + 1 + 8 = 10 bytes - format_string = "