diff --git a/dobotWrapperPy/dobotapi.py b/dobotWrapperPy/dobotapi.py index 21954ad..37fde51 100644 --- a/dobotWrapperPy/dobotapi.py +++ b/dobotWrapperPy/dobotapi.py @@ -21,6 +21,7 @@ tagCPParams, tagDevice, tagEMOTOR, + tagEMOTORDistance, tagEndEffectorParams, tagIODO, tagIOMultiplexing, @@ -2206,6 +2207,37 @@ def set_e_motor( return None return None + def set_e_motor_distance( + self, params: tagEMOTORDistance, wait: bool = False, is_queued: bool = False + ) -> Optional[int]: + """ + Controls an external motor (stepper). + Protocol ID: 135. Can be queued. + + Args: + params: External motor parameters. + wait: If True and command is queued, waits for execution. + is_queued: If True, command is added to the queue. + + Returns: + Queued command index if is_queued is True, else None. + """ + response = self._send_command_with_params( + CommunicationProtocolIDs.EMOTOR, # ID 135 + ControlValues.ReadWrite, + params.pack(), # EMotor + wait, + put_on_queue=is_queued, + ) + if is_queued: + if response.params and len(response.params) >= 8: + return int(struct.unpack(" Optional[int]: diff --git a/dobotWrapperPy/dobotasync.py b/dobotWrapperPy/dobotasync.py index b77368a..f487e05 100644 --- a/dobotWrapperPy/dobotasync.py +++ b/dobotWrapperPy/dobotasync.py @@ -1,4 +1,5 @@ from .dobotapi import DobotApi +import math from .dobotConnection import DobotConnection import warnings import struct @@ -17,6 +18,7 @@ from .paramsStructures import ( tagIOMultiplexing, tagWithL, + tagEMOTORDistance, tagDevice, tagPTPCommonParams, tagPTPCoordinateParams, @@ -85,7 +87,9 @@ def __del__(self) -> None: if hasattr(self, "dobotApiInterface") and self.dobotApiInterface is not None: del self.dobotApiInterface - def _run_in_loop(self, func: Callable[..., _T], *args: typing.Any) -> typing.Awaitable[_T]: + def _run_in_loop( + self, func: Callable[..., _T], *args: typing.Any + ) -> typing.Awaitable[_T]: if self._loop is None: raise Exception("Dobot not connected") return self._loop.run_in_executor(None, func, *args) @@ -488,6 +492,32 @@ async def set_motor(self, address: EMotorIndex, enable: bool, speed: int) -> Non True, ) + async def set_motor_distance( + self, address: EMotorIndex, enable: bool, speed: int, distance: int + ) -> None: + await self._run_in_loop( + self.dobotApiInterface.set_e_motor_distance, + tagEMOTORDistance(address, enable, speed, distance), + True, + True, + ) + + async def move_conveyor_belt( + self, speed: int, distance_cm: int, address: EMotorIndex, direction: int = 1 + ) -> None: + + STEP_PER_CIRCLE = 360.0 / 1.8 * 10.0 * 16.0 + MM_PER_CIRCLE = 3.1415926535898 * 36.0 + if 0.0 <= speed <= 100.0 and (direction == 1 or direction == -1): + motor_speed = math.floor( + speed * STEP_PER_CIRCLE / MM_PER_CIRCLE * direction + ) + await self.set_motor_distance(address, True, motor_speed, distance_cm) + else: + raise Exception( + f"Wrong speed or direction. Current params: Speed: {speed}, Distance: {distance_cm} cm, Direction: {direction}, Address: {address.value}" + ) + async def set_color_sensor( self, enable: bool, port: int, version: TagVersionColorSensorAndIR ) -> None: diff --git a/dobotWrapperPy/paramsStructures.py b/dobotWrapperPy/paramsStructures.py index 8f5a771..2dff549 100644 --- a/dobotWrapperPy/paramsStructures.py +++ b/dobotWrapperPy/paramsStructures.py @@ -2251,6 +2251,86 @@ def unpack(cls, data: bytes) -> "tagEMOTOR": ) +@dataclass +class tagEMOTORDistance: + """Represents an EMOTOR distance command.""" + + # uint8 - Represents EMotorIndex + address: EMotorIndex + # uint8 - Boolean flag for instruction enabled + insEnabled: bool + # double - Speed of the motor + speed: int + # int - Distance travelled + distance: int + + def pack(self) -> bytes: + """ + Packs the tagEMOTOR object into a bytes sequence. + + Packs the index (EMotorIndex value as uint8), insEnabled (boolean as uint8), + and speed (double) into a byte string. Uses little-endian byte order. + + Returns: + A bytes object representing the packed data (1 + 1 + 8 = 10 bytes). + """ + # Format: < (little-endian), B (uint8), B (uint8), d (double) + # Total size = 1 + 1 + 8 = 10 bytes + format_string = " "tagEMOTORDistance": + """ + Unpacks a bytes sequence into a tagEMOTOR object. + + Args: + data: The bytes to unpack, expected to be 10 bytes (2 uint8s + 1 double). + + Returns: + A tagEMOTOR object. + + Raises: + struct.error: If the input bytes are not the expected size (10 bytes). + ValueError: If the unpacked byte value for index does not correspond to a valid EMotorIndex enum member. + """ + format_string = "