diff --git a/_typos.toml b/_typos.toml index 20d22e4f16e..205851345d7 100644 --- a/_typos.toml +++ b/_typos.toml @@ -20,6 +20,11 @@ PN = "PN" tro = "tro" FO = "FO" UE = "UE" +commutated = "commutated" +commutating = "commutating" +DOUT = "DOUT" +inconsistence = "inconsistence" # ignored to match with preciseflex documentation typo +mis = "mis" # ignored to match with preciseflex documentation typo [files] extend-exclude = [ diff --git a/pylabrobot/arms/__init__.py b/pylabrobot/arms/__init__.py new file mode 100644 index 00000000000..e69de29bb2d diff --git a/pylabrobot/arms/arm.py b/pylabrobot/arms/arm.py new file mode 100644 index 00000000000..b420c9b8c99 --- /dev/null +++ b/pylabrobot/arms/arm.py @@ -0,0 +1,71 @@ +from typing import Union + +from pylabrobot.arms.backend import ArmBackend +from pylabrobot.arms.coords import CartesianCoords, JointCoords +from pylabrobot.machines.machine import Machine + + +class Arm(Machine): + """A robotic arm.""" + + def __init__(self, backend: ArmBackend): + super().__init__(backend=backend) + self.backend: ArmBackend = backend + + async def move_to(self, position: Union[CartesianCoords, JointCoords]): + """Move the arm to a specified position in 3D space.""" + return self.backend.move_to(position) + + async def get_joint_position(self) -> JointCoords: + """Get the current position of the arm in 3D space.""" + return await self.backend.get_joint_position() + + async def get_cartesian_position(self) -> CartesianCoords: + """Get the current position of the arm in 3D space.""" + return await self.backend.get_cartesian_position() + + async def set_speed(self, speed: float): + """Set the speed of the arm's movement.""" + return await self.backend.set_speed(speed) + + async def get_speed(self) -> float: + """Get the current speed of the arm's movement.""" + return await self.backend.get_speed() + + async def open_gripper(self): + """Open the arm's gripper.""" + return await self.backend.open_gripper() + + async def close_gripper(self): + """Close the arm's gripper.""" + return await self.backend.close_gripper() + + async def is_gripper_closed(self) -> bool: + """Check if the gripper is currently closed.""" + return await self.backend.is_gripper_closed() + + async def halt(self): + """Stop any ongoing movement of the arm.""" + return await self.backend.halt() + + async def home(self): + """Home the arm to its default position.""" + return await self.backend.home() + + async def move_to_safe(self): + """Move the arm to a predefined safe position.""" + return await self.backend.move_to_safe() + + async def approach(self, position: Union[CartesianCoords, JointCoords], approach_height: float): + """Move the arm to a position above the specified coordinates by a certain distance.""" + return await self.backend.approach(position, approach_height) + + async def pick_plate(self, position: Union[CartesianCoords, JointCoords], approach_height: float): + """Pick a plate from the specified position.""" + return await self.backend.pick_plate(position, approach_height) + + async def place_plate( + self, position: Union[CartesianCoords, JointCoords], approach_height: float + ): + """Place a plate at the specified position.""" + return await self.backend.place_plate(position, approach_height) diff --git a/pylabrobot/arms/backend.py b/pylabrobot/arms/backend.py new file mode 100644 index 00000000000..6d65a673701 --- /dev/null +++ b/pylabrobot/arms/backend.py @@ -0,0 +1,81 @@ +from abc import ABCMeta, abstractmethod +from typing import Union + +from pylabrobot.arms.coords import CartesianCoords, JointCoords +from pylabrobot.machines.backend import MachineBackend + + +class ArmBackend(MachineBackend, metaclass=ABCMeta): + """Backend for a robotic arm""" + + @abstractmethod + async def set_speed(self, speed: float): + """Set the speed percentage of the arm's movement (0-100).""" + ... + + @abstractmethod + async def get_speed(self) -> float: + """Get the current speed percentage of the arm's movement.""" + ... + + @abstractmethod + async def open_gripper(self): + """Open the arm's gripper.""" + ... + + @abstractmethod + async def close_gripper(self): + """Close the arm's gripper.""" + ... + + @abstractmethod + async def is_gripper_closed(self) -> bool: + """Check if the gripper is currently closed.""" + ... + + @abstractmethod + async def halt(self): + """Stop any ongoing movement of the arm.""" + ... + + @abstractmethod + async def home(self): + """Home the arm to its default position.""" + ... + + @abstractmethod + async def move_to_safe(self): + """Move the arm to a predefined safe position.""" + ... + + @abstractmethod + async def approach(self, position: Union[CartesianCoords, JointCoords], approach_height: float): + """Move the arm to a position above the specified coordinates by a certain distance.""" + ... + + @abstractmethod + async def pick_plate(self, position: Union[CartesianCoords, JointCoords], approach_height: float): + """Pick a plate from the specified position.""" + ... + + @abstractmethod + async def place_plate( + self, position: Union[CartesianCoords, JointCoords], approach_height: float + ): + """Place a plate at the specified position.""" + ... + + @abstractmethod + async def move_to(self, position: Union[CartesianCoords, JointCoords]): + """Move the arm to a specified position in 3D space.""" + ... + + @abstractmethod + async def get_joint_position(self) -> JointCoords: + """Get the current position of the arm in 3D space.""" + ... + + @abstractmethod + async def get_cartesian_position(self) -> CartesianCoords: + """Get the current position of the arm in 3D space.""" + ... diff --git a/pylabrobot/arms/coords.py b/pylabrobot/arms/coords.py new file mode 100644 index 00000000000..4aee8fcf396 --- /dev/null +++ b/pylabrobot/arms/coords.py @@ -0,0 +1,29 @@ +from dataclasses import dataclass +from enum import Enum +from typing import Optional + + +class ElbowOrientation(Enum): + RIGHT = "right" + LEFT = "left" + + +@dataclass +class JointCoords: + rail: float + base: float + shoulder: float + elbow: float + wrist: float + gripper: float + + +@dataclass +class CartesianCoords: + x: float + y: float + z: float + yaw: float + pitch: float + roll: float + orientation: Optional[ElbowOrientation] = None diff --git a/pylabrobot/arms/precise_flex/__init__.py b/pylabrobot/arms/precise_flex/__init__.py new file mode 100644 index 00000000000..e69de29bb2d diff --git a/pylabrobot/arms/precise_flex/error_codes.py b/pylabrobot/arms/precise_flex/error_codes.py new file mode 100644 index 00000000000..02515cdbefd --- /dev/null +++ b/pylabrobot/arms/precise_flex/error_codes.py @@ -0,0 +1,1809 @@ +ERROR_CODES = { + 0: {"text": "Success", "description": "Operation completed successfully without an error."}, + 1: { + "text": "Warning", + "description": "Operation completed without an error, but an anomaly occurred that should be brought to the attention of the system operator.", + }, + -200: { + "text": "No memory available", + "description": "There is not sufficient memory to perform the requested operation. Large amounts of memory may be used by the following: loaded GPL procedures, arrays, strings, and the Data Logger. Check if any of these items are unusually large.", + }, + -201: { + "text": "System internal consistency error", + "description": "This error indicates that a condition has been detected that should not be possible if the controller and its system software are operating properly. Most likely, this indicates that a software bug has been encountered. Please report this message along with the process necessary to generate this problem to Precise.", + }, + -202: { + "text": "Invalid argument", + "description": "The value of an argument used in a GPL instruction, built-in method, or console command is not allowed.", + }, + -203: { + "text": "FIFO overflowed", + "description": "An overflow has occurred in a system data structure. Most likely, this indicates that a software bug has been encountered. Please report this message along with the process necessary to generate this problem to Precise.", + }, + -204: { + "text": "Not implemented", + "description": "You have attempted to use a feature of GPL or the Precise Controller that is planned but not implemented.", + }, + -205: { + "text": "Missing argument", + "description": "A required argument for a GPL instruction, built-in method, or console command was not supplied.", + }, + -206: { + "text": "Invalid auto execute mode", + "description": 'The "Auto execution mode" specifies the major motion control function that the controller should perform. For example, the execution of the DIOMotion Blocks and GPL projects are examples of two "Auto execution modes". This error indicates that an invalid mode has been specified. Most likely, the value of "Automatic execution mode" (DataID 200) has been set incorrectly.', + }, + -207: { + "text": "Too many", + "description": "The current operation has attempted to create more items of an internal data structure than is allowed. Most likely, this indicates that a software bug has been encountered. Please report this message along with the process necessary to generate this problem to Precise.", + }, + -208: { + "text": "Protection error", + "description": "You have attempted to read a hidden value, access a secured data area without proper authorization, or use the Data Logger on data that cannot be logged. This error is often generated by GPL application programs that attempt to write to Parameter Database values that cannot be modified when motor power is enabled. In these cases, disable motor power and retry the operation.", + }, + -209: { + "text": "Read only", + "description": "This error message indicates that you have attempted to alter a value that can only be read. For example, this error is generated if you attempt to change the value of an analog input.", + }, + -210: { + "text": "Operating system error code", + "description": "This error message indicates that the underlying real-time operating system (the RTOS) or one of its communications drivers has signaled an error that does not correspond to a standard GPL error. The numeric code contains the RTOS error number.", + }, + -211: { + "text": "Option not enabled", + "description": """This error is generated if you attempt to execute or enable a software or hardware option that is either not available in your system or has not been enabled. Typically, this indicates that your system software does not support the desired feature or a parameter has not be properly set. This is different from missing a required license bit, which is indicated by the "License not installed" error message. Normally, this error includes one of the following codes that further defines the problem. + + 1: Split axis capability is not supported by kinematic module. + 2: External trajectory interface is not support by system software. + 3: Ethernet is not supported by the system software. + 4: TCP network protocol is not supported by system software, so Telnet and GDE may not be used. + 5: UDP network protocol is not supported by system software. + 6: Vision interface is not supported by system software. + 7: Motor linearity compensation is either not supported by the kinematic module or is not enabled." + """, + }, + -212: { + "text": "License not installed", + "description": """A required software license is not installed on your controller. The name of the missing license is shown after this error message. The license names include: + + (1) Guidance Programming Language + (2) Motion control + (3) Conveyor tracking + (4) Advanced kinematics + (5) Complex kinematics + (6) Advanced controls + (7) Enhanced encoder + (8) Encoder latching + (9) RIO motion control + (10) Software application + (11) G&M code support + (12) Motion no kinematics support + (13) External trajectory + (14) XY compliance + (15) Z height detection + (16) GuidanceMotion application + (17) EtherCAT Master + +Older GPL systems may display the license number shown above in ( ) rather than the name. The currently installed licenses can be viewed via the web interface by selecting Utilities>Controller Options or by accessing DataID 112, "Software license option bits". To obtain a required license, contact your system administrator or Brooks.""", + }, + -213: { + "text": "Invalid password", + "description": "An invalid password was entered for a protected operation or facility. Please re-enter the correct password or ask your system administrator for the correct password.", + }, + -214: { + "text": "Cancelled", + "description": "An operation was initiated that was subsequently cancelled. No further action is required.", + }, + -215: { + "text": "No system clock interrupts", + "description": "Many of the major subsystems of the controller (e.g. the servo code, the trajectory generator, the GPL projects) are serviced by different threads of software that execute independently. This thread execution is governed in part by the ticking of a master system clock. When the system restarts, it automatically tests the system clock to ensure that it is ticking at the correct rate. These errors signify that the system clock is not operating properly. Most likely, this indicates a hardware problem with the main processor board, the MIDS.", + }, + -216: { + "text": "System clock interrupts too slow", + "description": "Many of the major subsystems of the controller (e.g. the servo code, the trajectory generator, the GPL projects) are serviced by different threads of software that execute independently. This thread execution is governed in part by the ticking of a master system clock. When the system restarts, it automatically tests the system clock to ensure that it is ticking at the correct rate. These errors signify that the system clock is not operating properly. Most likely, this indicates a hardware problem with the main processor board, the MIDS.", + }, + -217: { + "text": "System clock interrupts too fast", + "description": "Many of the major subsystems of the controller (e.g. the servo code, the trajectory generator, the GPL projects) are serviced by different threads of software that execute independently. This thread execution is governed in part by the ticking of a master system clock. When the system restarts, it automatically tests the system clock to ensure that it is ticking at the correct rate. These errors signify that the system clock is not operating properly. Most likely, this indicates a hardware problem with the main processor board, the MIDS.", + }, + -218: { + "text": "Invalid task configuration", + "description": "An invalid parameter has been entered associated with one of the major tasks of the system. For example, the update period of the trajectory generator must be specified as a number that is a power of two.", + }, + -219: { + "text": "Incompatible FPGA version", + "description": 'The firmware in the FPGA is not compatible with the hardware or software configuration options in this controller. The FPGA ("Field Programmable Gate Array") controls the robot power sequencing, encoder interfaces, motor current loop and a host of other critical functions. There are multiple versions of FPGA firmware that support various motors, encoders, and other options. Please obtain a compatible version of the FPGA firmware data or modify your configuration parameters. FPGA firmware may be loaded by following the instructions in the FAQ section of the PreciseFlex™ PreciseFlex Library.', + }, + -220: { + "text": "Not configured", + "description": "You have attempted to access some aspect of the system that has not been configured. For example, you have attempted to set a parameter for an axis that was not defined in the configuration database.", + }, + -221: { + "text": "Invalid robot type", + "description": 'A kinematic robot module does not exist for the specified robot type. This typically indicates that a value specified in the "Robot types" (DataID 116) is incorrect. Please review this list of values and look at the information on the available robot kinematic modules.', + }, + -222: { + "text": "Remote software incompatible", + "description": "In a Servo Network system, the indicated slave controller contains software that is not compatible with the master controller. Update the slave controller's software to a compatible version. Normally the master and all slaves should run the identical GPL software version.", + }, + -223: {"text": "Incompatible system", "description": ""}, + -224: {"text": "FPGA load failed", "description": ""}, + -300: { + "text": "Invalid device", + "description": "The device specified for a file or I/O operation is of the wrong type for that operation. Check the spelling of the device name and verify what types of devices are appropriate for the operation you are attempting.", + }, + -301: { + "text": "Undefined device", + "description": "The device specified for a file or I/O operation is not recognized. Check the spelling of the device name. Verify that the device (e.g. remote I/O board) is installed in your system.", + }, + -302: { + "text": "Invalid device unit", + "description": "The unit for a device specified for a file or I/O operation is not valid for that operation. Verify what device's unit is appropriate for the operation you are attempting.", + }, + -303: { + "text": "Undefined device unit", + "description": "The unit for a device specified for a file or I/O operation is not recognized. Verify that the unit (e.g. serial port) is present in your system.", + }, + -304: { + "text": "Device already in use", + "description": "An attempt to open or attach a device has failed because the device is already in use. For example, an attempt has been made to open the /dev/com2 serial port from a GPL program while the hardware MCP that uses the same port is active. Check to make sure that the device you are accessing is available for use.", + }, + -305: {"text": "Logical device already in use", "description": ""}, + -306: {"text": "Duplicate logical device", "description": ""}, + -307: {"text": "Duplicate physical device", "description": ""}, + -308: { + "text": "Too many devices", + "description": "This error indicates that an internal software configuration bug has been encountered. Please report this message along with the process that generated this problem to Precise.", + }, + -309: { + "text": "No physical device mapped", + "description": "This error indicates that an internal software configuration bug has been encountered. Please report this message along with the process that generated this problem to Precise.", + }, + -310: { + "text": "Timeout waiting for device", + "description": "A device has failed to respond within the expected time period. This message may indicate a real error or the timeout may be expected if the system is testing for the presence of a device. Verify that the device is present. Increase the timeout value for the I/O operation.", + }, + -311: { + "text": "Date/time not set", + "description": "An attempt to read the system date or time has failed because the internal clock has not been initialized. Verify that your system contains a real-time clock. If so, this error may indicate a hardware failure such as a dead clock battery.", + }, + -312: { + "text": "Invalid date/time specification", + "description": "An attempt to set the system date or time has failed because the specification does not represent a valid date or time. Enter a valid specification.", + }, + -313: {"text": "CANOpen driver initialization failure", "description": ""}, + -314: { + "text": "Device not found", + "description": "An I/O device is not responding. This message may indicate a real error or the error may be expected if the system is testing for the presence of a device.", + }, + -315: { + "text": "NVRAM not responding", + "description": "A device connected to the I2C bus is not responding. This device may be the system NVRAM or it may be a different device. This message may indicate a real error or it may be expected if the system is testing for the presence of a device.", + }, + -316: { + "text": "NVRAM invalid response", + "description": "A device connected to the I2C bus is not communicating properly. This message indicates a hardware problem. Please report this message along with the process that generated this problem to Precise.", + }, + -317: { + "text": "Configuration area invalid", + "description": "The data in the system configuration area is not valid. The configuration data may have been corrupted. This error should not be seen on a control board that has been initialized. Please report this message along with the process that generated this problem to technical support.", + }, + -318: { + "text": "Real-time-clock disabled", + "description": "The real-time clock is disabled internally. This error should never be seen and indicates a hardware problem. Please report this message to Precise.", + }, + -319: { + "text": "Real-time-clock battery failed", + "description": "The real-time clock battery has failed. This error indicates a hardware problem. Please report this message to Precise.", + }, + -320: { + "text": "Device not ready", + "description": "An attempt was made to access a device that is busy. Depending on the device, it may be attached to a different thread, or it may have not been properly initialized. Try the operation again. Make sure the procedure used to access the device is correct.", + }, + -321: { + "text": "Invalid device command", + "description": "A device has rejected a command because it was not recognized. Check to make sure you are issuing the proper command for the device.", + }, + -322: { + "text": "i2c device failure", + "description": 'Robot power has been turned off because an unrecoverable error has occurred for an I2C device. I2C is used for a number of purposes including to communicate with the remote Z-axis digital I/O for the PrecisePlace 1300/1400 robots. This failure may be due to excessive electrical noise in your system. Check that your motor wiring and cable routing follows Precise guidelines. If your system is not a PP1300/1400, disable this error by setting parameter "Disable i2c errors" (DataID 920) to 1.', + }, + -323: { + "text": "Device full", + "description": "You have attempted to write data to a device that is full. No more data can be written until some of the existing data or files are deleted.", + }, + -324: { + "text": "MCP not recognized", + "description": "The hardware Manual Control Pendant connected to the Front Panel connector is not recognized as a Precise MCP. Consequently, the system cannot communicate with the device and the MCP driver is terminated. Please obtain an authorized MCP or contact Precise to repair your existing MCP.", + }, + -325: { + "text": "SIO device failure", + "description": 'Robot power has been turned off or has been inhibited from turning on because an SIO (RS-485) device has failed to respond to polling requests as expected. Verify that all required SIO devices are connected properly. Verify that only existing SIO devices are enabled in the "SIO mode flags" (DataID 571).', + }, + -326: { + "text": "Device not enabled", + "description": "An I/O operation has failed for a device because it is not enabled. The method for enabling a device depends on the particular device. Verify that any enable flags for the device are set in the configuration database.", + }, + -327: { + "text": "Invalid device configuration", + "description": "Some device configuration parameters are invalid. Check the values of the DataID reporting this error and verify that they are correct.", + }, + -328: { + "text": "PMIC not responding", + "description": "The power management controller is not responding as expected during system startup. This error should never be seen. Please report this message along with the process that generated this problem to technical support.", + }, + -700: { + "text": "Thread execution aborted", + "description": "A GPL user thread has been stopped by request (for example from a Stop command) or a robot error occurred while the robot was attached.", + }, + -702: { + "text": "Undefined thread", + "description": "The thread name specified in a Thread Class method or console command is not known to GPL. Verify that the thread name is correct. Verify that the thread has not been stopped and the name removed from the list of threads.", + }, + -704: { + "text": "Missing quote mark", + "description": 'A double quote character (") has not been found where expected at the end of a string specification or another syntax error in the statement prevented the compiler from finding the quote mark. Fix the statement syntax and add a quote mark if needed.', + }, + -705: { + "text": "Value too small", + "description": "A parameter database value, property value, or method parameter value is smaller than allowed. Check the relevant documentation and change the value to be within the proper range.", + }, + -706: { + "text": "Value too large", + "description": "A parameter database value, property value, or method parameter value is larger than allowed. Check the relevant documentation and change the value to be within the proper range.", + }, + -707: { + "text": "Value out-of-range", + "description": "A parameter database value, property value, or method parameter value is outside the range of allowed values. Check the relevant documentation and change the value to be within the proper range.", + }, + -708: { + "text": "Value infinite or NAN", + "description": 'A numeric value has been encountered that is too large for its new data type, or a floating point value has been encountered that is marked as "Not A Number" (NAN). This error can occur when converting from a larger numeric data type to a smaller one, for example Integer to Byte, or when performing a computation that results in a very large or infinite result, for example dividing by zero, or when computing the square root of a negative number. Check your procedure to eliminate these situations, or add checks to detect and handle them.', + }, + -709: { + "text": "Division by 0", + "description": "Indicates a division by zero was attempted. In matrix (Location) operations, this error can occur if the Z-axis orientation vector of a Cartesian Location has a zero length and the Location is being re-normalized. This can be caused by severe round-off error and can be corrected by normalizing the Location value more often.", + }, + -710: { + "text": "Arithmetic overflow", + "description": "This error indicates that a mathematical operation resulted in a number that is too large to represent. Since the majority of the internal computational operations are performed in double precision arithmetic, this typically indicates a result larger than 10^308 and normally indicates a programming error.", + }, + -711: { + "text": "Singular matrix", + "description": "A matrix operation is being performed and the determinate of the matrix is zero, i.e. the matrix is singular. This is typically detected when the system attempts to invert a matrix. For example, the conversion from joint angles to motor encoder values is represented as a matrix if the axes are mechanical coupled. When the system attempts to automatically compute the inverse conversion factors to go from motor encoder values to joint angles, a matrix inversion must be performed. This error indicates that a meaningful inverse relationship does not exist.", + }, + -712: { + "text": "Invalid syntax", + "description": "If encountered during program compilation, this message indicates that the GPL parser does not understand the current instruction. Either the instruction itself has an error, or it is being used in an unexpected situation. If encountered during execution, the arguments to a command, instruction, or method do not follow the expected format.", + }, + -713: { + "text": "Symbol too long", + "description": "An object or variable name exceeds the system's limit of 128 characters.", + }, + -714: { + "text": "Unknown command", + "description": "The command keyword for a console command is not recognized. Check to make sure you have entered the command correctly. Check that the command is supported by your version of GPL.", + }, + -715: { + "text": "Invalid procedure step", + "description": "An attempt has been made to execute a procedure step that is not executable. For example, during debugging, you have attempted to specify a comment line as the next step to execute.", + }, + -716: { + "text": "Ambiguous abbreviation", + "description": "A command keyword or parameter keyword has been entered that is an abbreviation for more than one command or parameter. Reenter the command specifying a longer abbreviation that matches only one keyword.", + }, + -717: { + "text": "Invalid number format", + "description": 'A numeric constant has been entered that does not match the expected format. For example, a floating point value has been entered with an empty exponent: "2.0E".', + }, + -718: { + "text": "Missing parentheses", + "description": 'After a left parenthesis "(" was encountered, the matching right parenthesis ")" was not found where expected. This error is sometimes reported when a syntax error occurs in the argument list for a procedure call, even if the right parenthesis is present. Add the missing parenthesis or fix any syntax errors.', + }, + -719: { + "text": "Illegal use of keyword", + "description": "A GPL keyword has been encountered in a place where it is invalid. For example, a keyword occurs in a declaration where it is not allowed, or an attempt was made to create a variable with the same name as a GPL keyword. Remove the keyword or rename the variable.", + }, + -720: { + "text": "Unexpected character in expression", + "description": "An unexpected character was encountered while evaluating a numeric or string expression. For example an operator was found in an unexpected place. Correct the expression syntax.", + }, + -721: {"text": "Conflict with reserved keyword", "description": ""}, + -722: { + "text": "Unexpected text at end of line", + "description": "Unexpected characters were found at the end of a statement. There may have been a previous syntax error that confused the compiler, or a missing comment character. Correct the line.", + }, + -723: { + "text": "Invalid statement label", + "description": "A statement label has been placed where it is not allowed, for example outside a procedure, or a Goto statement refers to a statement label that was not defined. Move or define the label.", + }, + -724: {"text": "Invalid End keyword", "description": ""}, + -725: { + "text": "Unknown data type", + "description": "During XML processing, a node with an unknown data type has been encountered. Correct the XML document.", + }, + -726: { + "text": "Data type required", + "description": 'A variable or procedure declaration is missing the "As" clause that specifies the data type. Add the appropriate clause and data type.', + }, + -727: { + "text": "Cannot redefine symbol", + "description": "An attempt has been made to define a symbol that already exists in the same context. This symbol may be the name of a project, module, class, procedure or variable. Change the name or scope of the symbol.", + }, + -728: {"text": "Procedure too long", "description": ""}, + -729: { + "text": "Undefined symbol", + "description": "A symbol has been encountered that is not declared within the current context. This symbol may be the name of a project, module, class, procedure or variable. Declare the symbol.", + }, + -730: { + "text": "Invalid symbol type", + "description": "A known symbol has been encountered in a statement but its type is not valid where it is being used. For example a Sub procedure name is used in an expression as if it were a function. Correct the statement.", + }, + -731: { + "text": "Unmatched parentheses", + "description": 'A right parenthesis ")" was encountered without being preceded by a left parenthesis "(". Add a "(" where appropriate, or remove the extra ")".', + }, + -732: { + "text": "Invalid procedure end", + "description": "When compiling a procedure, a top-level statement has been found other than the expected matching procedure end statement. Verify that the correct matching end statement is present.", + }, + -733: { + "text": "Not a top-level statement", + "description": "A statement has been encountered outside of a Module or Class definition block. Move the statement inside the block.", + }, + -734: { + "text": "Object not bound to class", + "description": 'A variable had been declared to be of type "Object" which is not supported by GPL. Correct the declaration.', + }, + -735: { + "text": "Too many nested blocks", + "description": "This typically indicates that a individual GPL procedure contains too many control structures, e.g. If..Then..Else or For loops, that are embedded within each other. The system currently has a limit of 100 nested control structures. To correct this problem, the procedure must be rewritten to reduce the nesting depth.", + }, + -736: { + "text": "Duplicate statement label", + "description": "An identical duplicate statement label has been encountered when compiling either a GPL program or a command script. Labels must be unique.", + }, + -737: { + "text": "Too many errors, compile cancelled", + "description": "The compiler has encountered more than 8 errors and is stopping the compile operation. Fix the errors already listed and compile again.", + }, + -738: { + "text": "Invalid data type", + "description": "A data type keyword has been encountered where it is not allowed. For example, a variable has been declared as type Function.", + }, + -739: { + "text": "Cannot change built-in symbol", + "description": "An attempt has been made to add to a built-in class or module. These built-in classes cannot be changed.", + }, + -740: {"text": "Empty procedure", "description": ""}, + -741: { + "text": "Argument mismatch", + "description": "The arguments in a statement, console command or procedure call do not match the parameters for that statement, console command or procedure call. Change the arguments so that they match the required parameters.", + }, + -742: { + "text": "Compilation errors", + "description": "A compilation has failed because of detected errors. The specific errors are listed during the compilation operation. Or, an attempt has been made to start a project that contains compilation errors. Fix the errors and recompile.", + }, + -743: { + "text": "Invalid project file", + "description": "The Project.gpr file in your project folder is not valid. This file is normally automatically generated and managed by GDE. If you edited this file by hand, double-check that the format is valid. Otherwise use GDE to rebuild the project or restore your project from a backup.", + }, + -744: { + "text": "Invalid start procedure", + "description": 'The "start" procedure specified for your project or by a Thread.Start method could not be found or is of the wrong type. Start procedures must be Public and of type Sub or Function. Correct the start procedure specification.', + }, + -745: { + "text": "Project already exists", + "description": "An attempt was made to load a project with the same name as a project currently loaded. Change the name of the second project, or unload the first project.", + }, + -746: { + "text": "Interlocked for read", + "description": "An attempt was made to change a system resource that is currently in use. Wait a short time and try again in case the lock was temporary. Otherwise, determine the thread that is using the resource and stop it. Then, try accessing the resource again.", + }, + -747: { + "text": "Interlocked for write", + "description": "An attempt was made to change a system resource that is actively being changed. For example two threads might be attempting to delete the same project simultaneously. Wait a short time and try again. Write locks are normally temporary", + }, + -748: { + "text": "No matching control structure", + "description": "A GPL procedure is missing statements that are necessary to properly terminate one or more control structures. For example, a For statement might be missing its matching Next statement or an If instruction might not be properly terminated by an End If statement or a Case statement may not immediately follow a Select statement.", + }, + -749: { + "text": "Thread already exists", + "description": "An attempt was made to create a thread with the same name as one that already exists. Unload the first thread, or rename the second thread.", + }, + -750: { + "text": "Invalid when thread active", + "description": "An attempt was made to perform an operation that cannot be executed while a thread is active. For example, you may have attempted to start a thread that is already active or you may have attempted to delete a project with an active thread.", + }, + -751: { + "text": "Timeout starting thread", + "description": "A thread has not started or restarted within 1 second of being requested to execute. This error often occurs when restarting a thread that is still winding down because it is taking longer than expected to perform its cleanup procedures. Stop or pause the thread and repeat the operation.", + }, + -752: { + "text": "Timeout stopping thread", + "description": "A thread has not stopped within 3 seconds of when a request to stop it occurred. The thread may be waiting for some operation to complete before it can stop. For example, it may be waiting for a robot motion or I/O operation to complete. This is not a critical error and the thread will stop when it completes whatever it is doing. This error is commonly seen if a thread stop request occurs after a thread has started a long robot motion. To stop a thread quickly in this case, issue a soft E-Stop just prior to requesting the thread to stop.", + }, + -753: { + "text": "Project not compiled", + "description": "An attempt was made to access a project that is not compiled. The project may not exist, or it may be loaded but not compiled. Load the project if not loaded and then compile it.", + }, + -754: { + "text": "Thread execution complete", + "description": "An attempt was made to continue execution of a thread that has run to completion. You must restart the thread with a Start command or Thread Start method.", + }, + -755: { + "text": "Thread stack too small", + "description": "An attempt was made to allocate more data on a thread stack than the stack is able to accommodate. You may have more nested procedure calls than expected or you may have allocated too many procedure-local variables. Verify that you do not have a program recursion bug. Check the stack usage with the Show Stack command. If required, specify a larger thread stack size with the Start command or the Thread Class constructor.", + }, + -756: { + "text": "Member not shared", + "description": "You have attempted to associate a Delegate object with a non-shared class procedure, but you have not provided an object reference in the Delegate New clause. Change your New clause to provide an object reference, or change your Delegate to refer to a shared procedure.", + }, + -757: { + "text": "Object not instantiated", + "description": 'An object is being assigned to on the left-hand side of an equal sign or is being referenced in an expression and the object value block has not been allocated. To correct this problem, use the "New" qualifier in the DIM statement that declared the object to allocate its value block.', + }, + -758: { + "text": "No Get Property defined", + "description": "An attempt has been made to read the value of a write-only property (that does not support Get). This can occur by attempting to assign a value to a write-only property with an assignment operator such as +=. Eliminate the read of the property.", + }, + -759: { + "text": "Undefined value", + "description": "An attempt was made to read a variable or procedure argument that does not have any value assigned. Be sure to assign a value to a variable before referring to it.", + }, + -760: { + "text": "Invalid assignment", + "description": "An attempt was made to assign a value to a variable with an incompatible data type. For example you cannot assign an object of one class to a variable for another class.", + }, + -761: { + "text": "Cannot have list of variables", + "description": "A declaration with an initializer may define only one variable. A list of variables is not allowed in this situation. Break your declaration into multiple statements.", + }, + -762: { + "text": "Location not a Cartesian type", + "description": 'A property or a method of a Location object requires a Cartesian type value, but the Location is an Angles type instead. For example, it is invalid to reference the "X" property of a Location defined as an Angles type.', + }, + -763: { + "text": "Location not an angles type", + "description": 'A property or a method of a Location object requires an Angles type value, but the Location is a Cartesian type instead. For example, it is invalid to reference the "Angle" property of a Location defined as a Cartesian type.', + }, + -764: { + "text": "Invalid procedure overload", + "description": "An attempt was made to declare a procedure with the same name as an existing procedure, and with arguments that match too closely so that it cannot be considered an overload. Change the arguments so that the two procedures can be distinguished by the compiler.", + }, + -765: { + "text": "Array index required", + "description": "An array reference was found that does not have the correct number of index arguments specified for the number of array dimensions. Specify the correct number.", + }, + -766: { + "text": "Array index mismatch", + "description": "An array argument does not have the same number of dimensions as an corresponding array parameter. Change the arrays so that the dimensions match.", + }, + -767: { + "text": "Invalid array index", + "description": "An array index value is negative or greater than the maximum permitted by its declaration. Or, an array argument was specified that does not contain sufficient elements for the matching array parameter.", + }, + -768: { + "text": "Unsupported array access", + "description": "An attempt was made to access an array in a way that is not supported. This error should never occur in GPL 3.1 or later.", + }, + -769: { + "text": "Reference frame wrong type", + "description": "A property or a method of a RefFrame object requires a particular type of reference frame value and the type is incorrectly set. For example, the PalletIndex property is only valid when the RefFrame Type is set to pallet.", + }, + -770: { + "text": "Reference frame undefined data", + "description": "When the position of a reference frame is evaluated either directly or as part of the absolute value of a Location that is relative to the reference frame or when a property of a reference frame is being accessed, this error is generated if the Loc of the reference frame is not defined. To correct this problem, assign a defined Cartesian Location value to the refframe.Loc property.", + }, + -771: { + "text": "Stack frame does not exist", + "description": 'A command that accepts a stack frame number has specified a stack frame that does not exist. Use the "show stack" command with no frame argument to display all frames and determine the maximum valid frame number.', + }, + -772: { + "text": "Ambiguous Public reference", + "description": "References to top-level public variables do not need to be qualified by their containing module or class provided that they are unique. However if the same public variable appears in more than one module or class, you must precede its name with the name of its module or class.", + }, + -773: { + "text": "Missing module end", + "description": 'An "End Module" or "End Class" statement was not found where it was expected. Add the appropriate statement.', + }, + -774: { + "text": "Too many breakpoints", + "description": 'More than 32 breakpoints have been set in GPL procedures. Remove some of the existing breakpoints or issue "Clear All Breakpoints" and set fewer breakpoints.', + }, + -775: { + "text": "Duplicate breakpoint", + "description": 'A breakpoint was set on a line that already contains a breakpoint. If GDE does not show a breakpoint on this line, GDE could be out-of-sync with GPL. Try disconnecting and reconnecting GDE. Try issuing "Clear All Breakpoints" and set your breakpoint again.', + }, + -776: { + "text": "No instruction at this line", + "description": 'A breakpoint was set in a procedure that does not exist or on a line that contains an instruction that does not allow breakpoints. In the second case, GPL tries to set a breakpoint on the next valid instruction, but this error will be generated if there is no valid instruction before the end of the procedure.\n\nThe error can occur if you edit your program and re-compile after adding/removing lines while having breakpoints set. It could also occur if GDE is out-of-sync with GPL. Try disconnecting and reconnecting GDE. Try issuing "Clear All Breakpoints" and set your breakpoints again.', + }, + -778: { + "text": "Objects not allowed for class", + "description": "An attempt was made to use the New clause to allocate an object for a built-in class that does not allow objects.", + }, + -779: { + "text": "Thread paused in eval", + "description": 'A console command such as "Show Variable" has invoked a procedure that has paused due to an error or breakpoint. The command cannot continue. Normally this error is not seen.', + }, + -780: { + "text": "Unsupported procedure reference", + "description": "A console command or a Const statement has attempted to call a user-defined function or property. This type of access is not supported.", + }, + -781: { + "text": "Missing string", + "description": 'The system is expecting a string value, such as following a concatenation operator ("&") but a string value was not found.', + }, + -782: { + "text": "Object value is Nothing", + "description": 'An object is being referenced in an expression of some type, and its value is not allocated and therefore undefined. To correct this problem, use the "New" qualifier in the DIM statement to allocate the value, and then define the required properties.', + }, + -783: { + "text": "Short string", + "description": "The number of characters stored in a string variable has been found to be less than is indicated by the string length. This is an abnormal condition and might occur if two threads are writing to the same string variable at the same time.", + }, + -784: { + "text": "Invalid property", + "description": "An object property is being accessed that is not valid given the settings of the other properties of the object. For example, an Exception object can be marked as a general error or a robot error. Depending upon this setting, certain properties are not accessible.", + }, + -785: { + "text": "Branch not permitted", + "description": "A GoTo instruction specifies a branch to a instruction that is not permitted. This typically means that a GoTo is attempting to branch into or out of a Try...Catch...Finally...End Try block that is not permitted. See the section on Exception handling for more information.", + }, + -786: { + "text": "Project generated error", + "description": "This error is never generated automatically and is provided solely as a convenience for GPL application programs. GPL projects can use the Throw instruction to emit this error code to indicate special application errors. The exception Qualifier can be used to provide additional information.", + }, + -787: { + "text": "Invalid in shared procedure", + "description": "An attempt has been made to access a non-shared and non-constant class variable from within a shared class procedure. Shared class procedures are not associated with an object instance, so they cannot access object variables. Either the procedure should not be declared Shared, or the class variable of interest should be declared Shared or Const.", + }, + -788: { + "text": "Inconsistent MOVE.TRIGGER mode", + "description": "Most likely, this indicates that a MOVE.TRIGGER instruction was executed that specified that the signal should be triggered a distance from the start or the end of the next motion, but the motion was not a straight-line or arc motion. To correct this problem, change the trigger mode or the motion type.", + }, + -789: { + "text": "Procedure exception", + "description": "A GPL procedure instruction has detected an exception that interrupts normal program execution. This error is normally handled internally by GPL and is not seen by the user.", + }, + -790: { + "text": "Invalid static initializer", + "description": "An attempt has been made to call a user-defined method in the initializer of a statically allocated variable or Const value. These variables include Module level variables, Shared class variables, and Static procedure variables. User-defined methods include constructors (New method) for user-defined classes.", + }, + -791: { + "text": "Conveyor must be base RefFrame", + "description": "Conveyor type RefFrame objects cannot themselves have a defined RefFrame value. That is, a Conveyor reference frame must always be the last (base) reference frame in a series of relative reference frames. However, other types of RefFrame objects can be relative to (above) Conveyor reference frames.", + }, + -792: { + "text": "undefined", + "description": "Before a Conveyor RefFrame can be used, its ConveyorRobot property must be set to specify the number of the Robot whose first axis provides the encoder signal for the conveyor. Without this information, the system has no way to determine the position of a conveyor.", + }, + -793: { + "text": "Arc cannot transition conveyors", + "description": "Circular interpolated motions can be performed relative to a conveyor belt. However, this type of motion cannot be used to accelerate on to a conveyor or off of a conveyor. That is, all three points that define a circular interpolated motion must all be relative to the same conveyor belt. To transition on to or off of a conveyor, use a straight-line Cartesian motion.", + }, + -794: { + "text": "ZClearance property not set", + "description": "A Move.Approach instruction was executed that referenced a Location whose ZClearance property has not been set to a realistic value. This instruction attempts to move the robot's tool to \"ZClearance\" mm above the Location's position. When a Location is first created, its ZClearance property is set to a very large number that cannot be reached. To correct this problem, set the Location's ZClearance property to the desired value in mm before executing the Approach instruction.", + }, + -795: { + "text": "XML documents do not match", + "description": "An attempt was made to define a parameter that is already defined. Only occurs in CommandProgram class methods.", + }, + -796: { + "text": "No delegate defined", + "description": "A reference has been made to an undefined delegate. Verify that any referenced delegate has been properly defined.", + }, + -797: { + "text": "Object not up-to-date", + "description": "An object that depends on another object or data structure has been referenced after the underlying data structure has been changed. Only occurs in CommandProgram class methods.", + }, + -798: { + "text": "No module defined", + "description": "An attempt has been made to declare a variable when no containing module has been defined. Only occurs in CommandProgram class methods.", + }, + -799: { + "text": "XML error", + "description": "An XML library error has occurred while parsing, accessing, or storing an XML document. This error is accompanied by an error code which further qualifies the error. See the XmlDoc.Message method for a string value that shows the error details.", + }, + -800: { + "text": "No XML document", + "description": "An attempt has been made to access an XmlDoc class object that is not associated with any document. Use the XmlDoc LoadString, LoadFile, or New methods to create a document.", + }, + -801: { + "text": "No XML node", + "description": "An attempt has been made to access an XmlNode class object that is not associated with any document node. This dissociation can occur if the referenced node is removed while the XmlNode object is still pointing to it. Check your program flow to see if nodes are being removed when you do not expect it. Remember that removing a parent may remove its children also.", + }, + -802: { + "text": "Undefined XML name", + "description": "A search for a named node using an XmlNode method has failed to find the named node. If you are not sure whether or not the node exists, use the HasElement or HasAttribute methods to check or enclose your search within a Try / Catch block.", + }, + -803: { + "text": "Invalid XML node type", + "description": 'An attempt has been made to use an XML node of a particular type where it is not allowed. For example, the parents of "attribute" nodes must be "element" nodes, and the parents of "element" nodes must be nodes of type "element", "entity", "document" or "htmldocument".', + }, + -804: { + "text": "XML documents do not match", + "description": "An attempt was made to add a XML node that is a member of one document to a second, different document. This is not permitted. Use the XmlNode.Clone method, with the second parameter specified, to create a clone of the node on an alternate document. You can add the clone to the second document.", + }, + -805: { + "text": "Invalid circular reference", + "description": "A Const symbol declaration refers to other Const symbols in a circular manner such that the original symbol references itself. This is not permitted.", + }, + -806: { + "text": "Invalid Const reference", + "description": "A Const symbol declaration refers to non-constant values, such as user variables or functions. This is not permitted. Const symbols may only be defined in terms of constants, other Const symbols, and built-in system functions.", + }, + -807: { + "text": "Invalid exception", + "description": "The exception object parameter to a Throw statement is invalid. Probably the ErrorCode property of the exception is not set to a negative value. Verify that you are setting the ErrorCode property to a negative value after creating the exception object.", + }, + -808: { + "text": "Branch out of Finally block not permitted", + "description": "An attempt has been made to exit from the Finally block of a Try … End control structure by using a statement such as a Goto, End, Exit, or Return. Finally blocks must always continue to the End Try statement.", + }, + -809: { + "text": "Expression too complex", + "description": "An attempt had been made to evaluate an expression that contains more the 32 objects, or that contains object references in a recursive loop. For example, a reference frame that refers to itself. Break the expression into multiple statements or remove any recursive references.", + }, + -810: { + "text": "Unexpected end of line", + "description": "The compiler has encountered the end of a line at an unexpected place, for example in the middle of an incomplete expression. Correct the syntax of the indicated line.", + }, + -811: { + "text": "No Set Property defined", + "description": "A Property definition has been encountered that is not declared ReadOnly and does not contain a Set statement. Either add a Set … End Set block or add the ReadOnly keyword to your Property definition.", + }, + -812: { + "text": "Not allowed in factory test system", + "description": "A special limited functionality version of GPL is loaded into the controller and one of the eliminated functions has been invoked. Replace the GPL system with the latest version available on the Precise Support website.", + }, + -1009: { + "text": "No robot attached", + "description": "A function or method was executed that required that a robot be ATTACHED. This error is often generated if you attempt to execute one of the methods in the Move Class without first attaching the robot. Correct your GPL program by inserting a Robot.Attached method prior to executing the instruction that contains the Move Class method.", + }, + -1000: { + "text": "Invalid robot number", + "description": "A robot number has been specified that is less than 1 or more than the number of configured robots.", + }, + -1001: { + "text": "Undefined robot", + "description": 'A robot number must be specified (1 to N), but no number was defined. For example, this error can occur if you are referencing a Parameter Database value that requires that a robot be specified as the "unit" (second) parameter but this value is left blank.', + }, + -1002: { + "text": "Invalid axis number", + "description": "An axis number has been specified that is less than 1 or more than the number of axes configured in the referenced robot. For example, this error will be generated if you are accessing a location's angle value (location.angle(n)) but the axis number is undefined or set to 0.", + }, + -1003: { + "text": "Undefined axis", + "description": "An axis has been specified that is not configured for the referenced robot. This can occur if an axis bit mask has been specified that references an axis that is not currently configured.", + }, + -1004: { + "text": "Invalid motor number", + "description": "A motor number has been specified that is less than 1 or more than the number of motors configured in the referenced robot.", + }, + -1005: { + "text": "Undefined motor", + "description": "A motor has been specified that is not configured for the referenced robot. This can occur if a motor bit mask has been specified that references a motor that is not currently configured.", + }, + -1006: { + "text": "Robot already attached", + "description": "An Auto Execution task or a GPL project has attempted to gain control of a robot by executing a Robot.Attach method or similar function, but the specified robot is already in use by another task. Alternately, this error is generated if you attempt to Attach or Select a robot, but the task is already attached to a different robot.", + }, + -1007: { + "text": "Robot not ready to be attached", + "description": 'An Auto Execution task or a GPL project has attempted to gain control of a robot by executing a Robot.Attach method or similar function, but the specified robot is not in a state where it can be attached. If this was generated by a GPL project, it might indicate: That the system is configured to execute DIOMotion blocks instead of a GPL motion program (DataID 200) or "Auto start auto execute mode" (DataID 202) is not set to TRUE. If so, please check the Setup>Startup Configuration web page to verify the current setup.', + }, + -1008: { + "text": "Can't detached a moving robot", + "description": "An operation is attempting to Detach a robot that is currently moving. Normally, it is not possible to generate this error condition because the Robot.Attached method automatically waits for the robot to stop before attempting the detach operation.", + }, + -1010: { + "text": "No robot selected", + "description": 'A function or method was executed that required that the robot be SELECTED. By default, the first robot or the Attached robot is set as the selected robot. A number of "readonly" operations require that a robot be selected, e.g. location.Here. Correct your GPL program by inserting a Robot.Selected method prior to executing the instruction that generated the exception.', + }, + -1011: { + "text": "Illegal during special Cartesian mode", + "description": "The robot is performing a special Cartesian trajectory mode such as conveyor tracking, outputting a DAC signal based upon the Cartesian tool tip speed, performing real-time trajectory modification, etc. When the trajectory generator is in this mode, the requested operation that produced this error is not permitted to execute. For example, jogging or moving along a joint interpolated trajectory or changing the tool length cannot be performed while tracking a conveyor belt. You must either terminate the current Cartesian trajectory mode with a Robot.RapidDecel or other means before initiating the new robot control mode or you must modify your program to use a method consistent with the current trajectory mode. For example, if you attempted to initiate a joint interpolated motion, use a Cartesian straight-line motion instead.", + }, + -1012: { + "text": "Joint out-of-range", + "description": "This indicates that the specified robot axes are either beyond or were attempted to be moved beyond their software limits, i.e. outside of their permitted ranges of travel. This error is also generated if you attempt to set the minimum and maximum soft and hard joint limits to inconsistent values. If you are narrowing the limits, you should set the new soft limits first and then change the hard limits. If you attempt to make both changes at the same time with the web interface, the system will process the new hard limits first, determine that they violate the old soft limits, and generate this error message.", + }, + -1013: { + "text": "Motor out-of-range", + "description": "This indicates that the specified robot motors are either beyond or were attempted to be moved beyond their software limits. This error is also generated if you attempt to set the minimum and maximum soft and hard motor limits to inconsistent values. For most robots, this error code will never be generated because the joint limits will be used exclusively. However, some robot's have coupled motors such that it may be possible to not violate a joint limit and still encounter the extreme travel limit of a motor. In these cases, this error message may be generated even when it appears that the robot's joint's are within their permitted ranges of travel.", + }, + -1014: { + "text": "Time out during nulling", + "description": 'At the end of a program generated motion, if the axes of the robot take too long to achieve the "InRange" constraint limits, this error message will be generated and program execution will be terminated. This may indicate that the InRange limit has been set too tightly or that the robot may not be able to get to the specified final position due to an obstruction.', + }, + -1015: { + "text": "Invalid roll over spec", + "description": "The Continuous Turn (encoder roll-over compensation) angle (DataID 2302) was set non-zero for an axis and the axis is not designed to support continuous turning. Please review the information for your kinematic module in the Kinematic Library documentation and ensure that the axis has been designed to support this feature.", + }, + -1016: { + "text": "Torque control mode incorrect", + "description": "Either the system is in torque control mode and should not be for the currently execution instruction or the system should be in torque control mode but is not. This can be generated in the following situations: Torque control mode is active and an instruction is executed to start External Trajectory mode, Jog Control mode, or torque control mode. An instruction is issued to set the torque values, but no motors are in torque control mode.", + }, + -1017: { + "text": "Not in position control mode", + "description": "A motion control instruction was initiated that requires that the robot be in the standard position controlled mode and the robot is in a special control mode, e.g. velocity control or jogging mode. For example, to initiate any of the following, the robot must be in the standard position control mode: torque control mode, velocity control mode, jog mode or any of the Move Class position controlled methods (e.g. Move.Loc).", + }, + -1018: { + "text": "Not in velocity control mode", + "description": "This error is generated if an instruction attempts to set the velocity mode speeds of an axis, but the system is not in velocity control mode.", + }, + -1019: { + "text": "Timeout sending servo setpoint", + "description": "The GPL trajectory generator sends a setpoint to the servos at the time interval determined by DataID 600, (Trajectory Generator update period in sec). This error occurs if a new setpoint is ready but the previous setpoint has not been sent. Verify that the DataID 603 (Servo update period in sec) value is one-half or less than the value of DataID 600 (Trajectory Generator update period in sec). In a servo network system, this error may indicate a network failure or unexpected network congestion. Provided that DataID 600 and 603 are set properly, this error should never be seen in a non-servo-network system. If the problem persists, contact Precise.", + }, + -1020: { + "text": "Timeout reading servo status", + "description": 'The servos send status information to GPL at the time interval determined by DataID 600, (Trajectory Generator update period in sec). This error occurs if no status information has been received by GPL for the past 32 milliseconds. If this error occurs when you are configuring a new controller system, it might indicate that you have changed some system parameters that are marked as "Restart required". However, you have attempted to operate the system without rebooting the controller. This can be corrected by restarting the controller. In a servo network system, this error may indicate a network failure or unexpected network congestion. This error should never be seen in a properly configured non-servo-network system. If the problem persists, contact Precise.', + }, + -1021: { + "text": "Robot not homed", + "description": 'An operation was invoked that requires that the robot\'s motors be homed. If a robot is equipped with incremental encoders, when the controller is restarted, the system does not have any knowledge of where each axes is located in the workspace. Homing establishes a repeatable "zero" position for each axis. The robot can be homed by pressing a button on the web Operator Control Panel, the web Virtual Manual Control Panel or via a program instruction.', + }, + -1022: { + "text": "Invalid homing parameter", + "description": "While executing the homing sequence an invalid parameter was encountered. This indicates that one of the Parameter Database values that controls homing (DataID 28xx) has been incorrect set. For example, an illegal homing method may be specified (DataID 2803) or the homing speed may be zero (DataID 2804).", + }, + -1023: { + "text": "Missed signal during homing", + "description": "During the homing operation with the robot moving, an error was detected prior to finding the signal that was expected. The detected error is most likely caused by an unexpected hardstop encountered or a over-travel limit switch tripping.", + }, + -1024: { + "text": "Encoder index disabled", + "description": 'This is normally generated by the homing routines. If a selected homing method tests for an encoder zero index signal and the encoder index is not enabled, this error is generated. This typically occurs if the "Encoder counts used for resolution calc" (DataID 10203) or the "Encoder revs used for resolution" (DataID 10204) are not properly setup.', + }, + -1025: { + "text": "Timeout enabling power", + "description": 'A request to enable robot power has failed to complete within the timeout period. Either a hardware failure has prevented power from coming on, or the timeout period is too short. Check the System Messages on the web interface Operator Control Panel for additional errors that may indicate a hardware failure. Verify that the controller is properly cabled. Try increasing the value of parameter "Timeout waiting for power to come on in sec" (DataID 262). This error may also occur as a GPL exception if power does not come on when the Controller.PowerEnabled method is used with a non-zero timeout parameter.', + }, + -1026: { + "text": "Timeout enabling amp", + "description": 'Robot power has been turned off during the robot power-on sequence because one or more power amplifiers have not become ready within the timeout period. Please try the following procedures: Check the System Messages on the web interface Operator Control Panel for additional errors that may indicate a hardware failure. Verify that the controller is properly cabled. Try increasing the value of parameter "Timeout waiting for amps to come on in sec" (DataID 264). If this occurs when the controller is first booted, try repeating the enable power operation after delaying for 1 minute. This error message can be generated if the robot includes absolute encoders that take a minute or so to complete their initialize before becoming ready to operate.', + }, + -1027: { + "text": "Timeout starting commutation", + "description": 'Robot power has been turned off during the robot power-on sequence because one or more motors have not completed their commutation sequence within the timeout period. Check the System Messages on the web interface Operator Control Panel for additional errors that may indicate a hardware failure. Try increasing the value of parameter "Timeout waiting for commutation in sec" (DataID 266). Verify that the controller is properly cabled and that the encoders and motors are wired correctly. See documentation section First Time Mechanism Integration and verify that the controller commutation parameters are set correctly for your motor and encoder combination. Verify that the FPGA firmware on the controller supports your motor and encoder combination.', + }, + -1028: { + "text": "Hard E-Stop", + "description": 'A hard E-Stop condition has been detected. Any robot motion in progress is stopped rapidly and robot power is turned off. One the following has occurred: A front panel E-Stop loop ("ESTOP_L 1" or "ESTOP_L 2") has been broken. The digital input signal specified by "Hard E-Stop DIN" (DataID 244) has been asserted. The parameter database item "Hard E-Stop" (DataID 243) has been set to TRUE. A "fatal" or "severe" error is detected and GPL automatically internally asserts an E-stop as a safety precaution to ensure that motor power is disabled. When a hard e-stop is asserted for any reason, if the "E-stop delay" (DataID 267) is set too short, it is possible that a "Amplifier under-voltage" error (-3109) will also be generated due to the DC motor bus voltage dropping before the amplifiers are disabled.', + }, + -1029: { + "text": "Asynchronous error", + "description": "An error signal from the servos or trajectory generator has been received by GPL, but no specific error code has been received. The error log entry immediately following this one normally indicates the actual error. Error signaling within the motion subsystem is a two-step process. When an error is first detected, a signal is sent to GPL immediately so that a controlled deceleration sequence can begin. This first signal generates a -1029 error code. Several milliseconds later, a more specific error code is sent to identify the source of the error. This second error code overwrites the -1029 error. Error -1029 is only seen if the error log is sampled after the initial error signal is received and before the specific error code is received.", + }, + -1030: { + "text": "Fatal asynchronous error", + "description": 'A severe error signal from the servos or trajectory generator has been received by GPL, but no specific error code has been received. The error log entry immediately following this one normally indicates the actual error. Error signaling within the motion subsystem is a two-step process. When a severe error is first detected, a signal is sent to GPL immediately so that a controlled deceleration sequence can begin. This first signal generates a -1030 error code. Several milliseconds later, a more specific error code is sent to identify the source of the error. This second error code overwrites the -1030 error. Error -1030 is only seen if the error log is sampled after the initial error signal is received and before the specific error code is received. Unlike standard errors, a severe error prevents robot power from being enabled until the controller is rebooted or "Reset fatal error" (DataID 247) is set to 1.', + }, + -1031: { + "text": "Analog input value too small", + "description": 'When reading an analog input signal, if after the scale and offset is applied, the value of the signal is lower than the limit set by "Gen AIO In min scaled value" (DataID 526), the analog value is set to the minimum value and this error is generated.', + }, + -1032: { + "text": "Analog input value too big", + "description": 'When reading an analog input signal, if after the scale and offset is applied, the value of the signal is higher than the limit set by "Gen AIO In max scaled value" (DataID 527), the analog value is set to the maximum value and this error is generated.', + }, + -1033: { + "text": "Invalid Cartesian value", + "description": "For robots with less than 6 independent degrees-of-freedom, certain combinations of tool orientations and positions are not possible. This does not indicate an axis limit stop error such as when a program attempts to move a linear axis beyond its end of travel. This error refers to positions and orientations that are not possible even if each axis of the robot has an unlimited range of motion. For example, for a 4-axis Cartesian robot with a theta axis, the tool cannot be rotated about the world X or Y axis. So if an instruction has a destination that requires that the tool be moved from pointing down to pointing up, this error will be generated.", + }, + -1034: { + "text": "Negative overtravel", + "description": "This is generated when the optional negative travel limit hardware switch has been tripped. This error indicates that a specified axis it outside of its permitted range-of-motion.", + }, + -1035: { + "text": "Positive overtravel", + "description": "This is generated when the optional positive travel limit hardware switch has been tripped. This error indicates that a specified axis it outside of its permitted range-of-motion.", + }, + -1036: { + "text": "Kinematics not installed", + "description": 'An operation was invoked that requires that the system be able to convert between joint and Cartesian (XYZ) coordinates. However, the system software has not been configured with a robot kinematics (geometry) module. The kinematic modules are selected using the "Robot types" (DataID 116) and are described in the Kinematics Library section of the PreciseFlex Library. Select an appropriate module and restart the system. If there is not an appropriate kinematics module, contact Precise.', + }, + -1037: { + "text": "Motors not commutated", + "description": "The operation that was attempted may not require that the robot's motors be homed, however, they must have their commutation reference established. Setting the commutation reference provides the controller with the knowledge of how to energize the individual motor phases as the motor rotates. For example, motors can be placed into torque control mode after they have been commutated and without homing the motors.", + }, + -1038: { + "text": "Project generated robot error", + "description": "This error is never generated automatically and is provided solely as a convenience for GPL application programs. GPL projects can use the Throw instruction to generate this special error to indicate special application errors that are robot specific.", + }, + -1039: { + "text": "Position too close", + "description": "This indicates that an XYZ destination is too close to the center of the robot and cannot be reached. For example, your robot may have an inner and an outer rotary link that dictates the radial distance of the gripper. If the outer link is shorter than the inner link, there will be a circular region at the center of the robot that cannot be accessed. In other cases, if the inner and outer link are the same lengths, the robot may be able to reach the center position, but such a position might be a mathematical singularity where two or more joints degenerate into the same motion. These types of conditions are signaled by this error code.", + }, + -1040: { + "text": "Position too far", + "description": "This indicates that an XYZ destination is beyond the robot's reach. This is typically generated by robot's with rotary links where the position is beyond the range of the fully outstretched links. To avoid excessive joint rotation speeds, this error may be signaled a few degrees before the fully extended position in manual jog mode or when the robot is moving along a straight-line path.", + }, + -1041: { + "text": "Invalid Base transform", + "description": "The Base transformation for a robot is required to have a zero pitch value. That is, a Base transform can translate the robot in all three directions and can rotate the robot about the world Z-axis, but it can not rotate the robot about the world X-axis or Y-axis.", + }, + -1042: { + "text": "Can't change robot config", + "description": "A motion was initiated that attempted to change the robot configuration (e.g. Righty vs. Lefty) when such a change is not permitted. For example, you cannot change the robot configuration during a Cartesian straight-line motion. Normally, if you specify a motion destination as a Cartesian Location, any differences in configuration are ignored. However, if you specify a Angles Location as the destination for a Cartesian motion and the Angles correspond to a different configuration, this error will be signaled.", + }, + -1043: { + "text": "Asynchronous soft error", + "description": "A soft error signal from the servos or trajectory generator has been received by GPL, but no specific error code has been received. The error log entry immediately following this one normally indicates the actual error. Error signaling within the motion subsystem is a two-step process. When an error is first detected, a signal is sent to GPL immediately so that a controlled deceleration sequence can begin. This first signal generates a -1043 error code. Several milliseconds later, a more specific error code is sent to identify the source of the error. This second error code overwrites the -1043 error. Error -1043 is only seen if the error log is sampled after the initial error signal is received and before the specific error code is received. Unlike standard errors, a soft error does not disable robot power.", + }, + -1044: { + "text": "Auto mode disabled", + "description": 'This indicates that: (1) the Auto/Manual hardware input signal has been switched to Manual and motor power has been disabled or (2) an automatic program controlled motion was attempted, but the Auto/Manual hardware input signal is set to Manual. When the Auto/Manual signal is set to Manual, only Jog ("Manual control") mode is permitted.', + }, + -1045: { + "text": "Soft E-STOP", + "description": 'A soft E-Stop condition has been detected. Any robot motion in progress is stopped rapidly but robot power is left on. One of the following has occurred: The GPL property Controller.SoftEStop has been set to TRUE. The GPL console command SoftEStop has been executed. The digital input signal specified by "Soft E-Stop DIN" (DataID 246) has been asserted. The parameter database item "Soft E-Stop" (DataID 245) has been set to TRUE.', + }, + -1046: { + "text": "Power not enabled", + "description": "An operation was attempted, such as trying to Attach to a robot, and power to the robot is required but has not yet been enabled. Enable high power and repeat the operation.", + }, + -1047: { + "text": "Virtual MCP in Jog mode", + "description": "An operation was attempted, such as trying to Attach to a robot, but the robot is already attached and is being controlled in Jog control mode via the web based Virtual MCP. Go to the Web Virtual MCP, place the robot into computer control mode, and retry the operation.", + }, + -1048: { + "text": "Hardware MCP in Jog mode", + "description": "An operation was attempted, such as trying to Attach to a robot, but the robot is already attached and is being controlled in Jog control mode via the hardware MCP. Go to the MCP, place the robot into computer control mode, and retry the operation.", + }, + -1049: { + "text": "Timeout on homing DIN", + "description": 'During the homing operation, a digital input signal that is specified for a motor via the "Wait to home axis DIN" (DataID 2812) failed to turn on before the timeout period defined by the "Timeout on home axis, sec" (DataID 2813) expired.', + }, + -1050: { + "text": "Illegal during joint motion", + "description": "An operation or program instruction has been executed that requires that the robot either be stopped or moving in a Cartesian control mode. However, the robot is executing a program controlled joint interpolated motion. Wait for the joint interpolated motion to terminate before executing this instruction.", + }, + -1051: { + "text": "Incorrect Cartesian trajectory mode", + "description": "An operation or program instruction has been executed that requires that the Trajectory Generator be in a specific Cartesian mode, but the mode was incorrect. Some possible problems could be: An instruction attempted to start the Real-time Trajectory Modification mode, but this mode is already active. An instruction attempted to set Real-time Trajectory Modification parameters, but this mode is not active.", + }, + -1052: { + "text": "Beyond conveyor limits", + "description": "Indicates that the position for a motion being planned is beyond the upstream or downstream limits of the referenced conveyor belt. This error is generated in the following circumstances: A motion is being planned that is relative to a conveyor belt and the final position is projected to be out of the conveyors downstream limit before the robot can reach this position. If a position is upstream of the upstream limit, the system pauses thread execution until the position comes within the limits and no error is generated.", + }, + -1053: { + "text": "Beyond conveyor limits while tracking", + "description": "Indicates that a position is beyond the upstream or downstream limits of a conveyor belt while the robot is tracking the belt. This error is generated in the following circumstances: The robot is moving relative to a conveyor belt and the final destination for the motion or the instantaneous position is beyond either limit of the conveyor belt.", + }, + -1054: { + "text": "Can't attach Encoder Only robot", + "description": "An Auto Execution task (such as that for the Manual Control Pendant) or a GPL project has attempted to gain control of a robot by executing a Robot.Attach method or similar function, but the specified robot is an Encoder Only module. This type of kinematic module can be accessed to read the position of an encoder, but cannot be used to drive the encoder. Therefore, it is not legal to Attach this type of robot. Use the Robot.Selected method instead, if you only need read-only access to the robot.", + }, + -1055: { + "text": "Cartesian motion not configured", + "description": 'This error is generated if you attempt to perform a Cartesian motion either in manual control or program control mode, and some key Cartesian motion parameters have not been initialized. Typically, this is caused by: The "100% Cartesian speeds" (DataID 2701) being set to 0 instead of their proper positive non-zero values. The "100% Cartesian accels" (DataID 2703) being set to 0 instead of their proper positive non-zero values.', + }, + -1056: { + "text": "Incompatible robot position", + "description": "The current robot position is not compatible with the requirements of the operation that has been initiated. Situations where this error can be generated include: For the RPRR kinematic module, the two yaw axes have been defined to move at a fixed offset relative to each other. However, at the start of a straight line motion, the yaw axes are not in proper alignment.", + }, + -1057: { + "text": "Timeout waiting for front panel button", + "description": 'For Category 3 (CAT-3) systems, the front panel "High Power Enable" button or the digital input specified by DataID 268 must transition from low to high within 30 seconds of a power-on request for high power to actually be enabled. This error is generated if the low to high transition is not seen within this time period.', + }, + -1058: { + "text": "Commutation disabled by servos", + "description": "This error indicates that the servos have invalidated the motor commutation in response to an error condition. For example, a severe encoder error may have occurred. Check the Error Log for additional error messages that indicate why commutation was disabled. In most cases, re-enabling robot power causes the motor commutation to be re-established and this error to be cleared.", + }, + -1059: { + "text": "Homed state disabled by servos", + "description": "This error indicates that the servos have marked an axis as not homed in response to an error condition. For example, a severe encoder error may have occurred. Check the Error Log for additional error messages that indicate why homing was invalidated. You cannot operate the axis under program control until you re-execute the axis homing procedure. You can use the MCP in joint control mode to move an axis that is not homed.", + }, + -1060: { + "text": "Remote E-STOP (node n)", + "description": 'Indicates that servo network node n is asserting an E-Stop condition. Whether E-Stop is connected to the main controller or to a remote node depends on your robot model and configuration. See the explanation for error -1028 "Hard E-Stop" for possible reasons for this error.', + }, + -1061: { + "text": "Illegal when robot moving", + "description": "This error is generated when a robot is moving under computer controller and an operation is performed that requires that the robot be stopped. For example, this occurs if the Set Payload console command is issued while the robot is moving.", + }, + -1062: { + "text": "Certified Safety Zones not supported", + "description": "This error indicates that the robot's PAC files include the specification of a Safety Certified Safety Zone, but the robot does not support this feature. These safety zones ensure that the robot does not exceed certain speed limits within specified physical areas. Typically, this error occurs if the wrong type of safety zone is specified or if the robot's safety certification functions are not operational but should be.", + }, + -1063: { + "text": "Certified Safety Zones cannot be rotated", + "description": "This error indicates that the robot's PAC files include the specification of a Safety Certified Safety Zone, but the orientation of the zone is rotated. For computational reasons, Certified Safety Zones must always be non-rotated rectangular volumes. Ensure that the Yaw, Pitch, and Roll parameters for the safety zone are zero. Alternately, use an Uncertified Safety Zone if you do not require a safety certified operation and would like to use a rotated safety zone.", + }, + -1064: { + "text": "Tool tip violates Safety Zone", + "description": 'This error indicates that the robot\'s tool tip (TCP) violates one or more defined Uncertified Safety Zones. A violation will occur if the TCP enters a "keep out" or exits a "keep in" safety zone. This error can occur if the end position of a program-controlled motion is in error, anytime during a Cartesian or joint programmed controlled motion, or anytime during a World, Tool, or Joint manual control motion. Once a violation has occurred, the robot must be backed out when high power is disabled or by using manual control Free mode or by using manual control Joint, Tool, or World mode (so long as the manual jog motion reduces the violation of the safety zone).', + }, + -1065: { + "text": "Tool tip speed too high in Safety Zone", + "description": "This error indicates that the robot's tool tip (TCP) is either moving down too fast in Z or moving too fast in the XY plane in a defined speed-limited non-rotated rectangular Safety Certified Safety Zone. Robots like the PreciseFlex 300/400/3400 can operate safely at their highest speed throughout their working volume, except when the tool tip is moving down and might pin an operator's hand to a hard surface. Additionally, robots like the PreciseFlex DD need to limit their horizontal speed when they are on near-vertical surfaces. If this error occurs, reduce the speed of the robot before it enters these safety zones and retry the motion. Please consult the operating manuals for these robots to determine when certified speed-limited safety zones must be used to ensure the safe operation of these robots.", + }, + -1066: { + "text": "Invalid Data ID code", + "description": "The Data ID code and the specified index values do not form a valid data reference. Check that the appropriate index values are specified with this Data ID.", + }, + -1501: { + "text": "Unknown Data ID code", + "description": "The Data ID code specified is not known by GPL. Check that the Data ID is valid. Some Data ID codes only become known if certain software options are enabled or configurations are selected. For example, you cannot specify absolute encoder parameters if you do not have any absolute encoders configured. Check that you are not specifying codes for a component that is not configured.", + }, + -1502: { + "text": "Inconsistent duplicate Data ID code", + "description": "For networked servo systems, the definitions for a Data ID must be the same for the master and all slave nodes. This error indicates some differences were detected. Verify that compatible software is loaded on the master node and all slave nodes.", + }, + -1505: { + "text": "Uninitialized parameter database", + "description": "The parameter database was not initialized properly at system startup. This error indicates an internal system error and should never been seen.", + }, + -1507: { + "text": "Must be master node", + "description": "For networked servo systems, some data and operations are only allowed on the master node. An attempt has been made to access such data or operations directly from a slave node.", + }, + -1509: { + "text": "Invalid data index", + "description": "The data index value specified in a Data ID reference is invalid. Probably an array index has been specified that is greater than the actual array size.", + }, + -1510: { + "text": "Database internal consistency error", + "description": "An internal error has occurred during a database reference. The parameter specified along with the Data ID may not be valid. This error should never be seen.", + }, + -1511: { + "text": "Invalid pointer value", + "description": "An internal error has occurred during a database reference. The parameter specified along with the Data ID may not be valid. This error should never be seen.", + }, + -1512: { + "text": "Undefined callback routine", + "description": "An internal error has occurred during a database reference. This error should never be seen.", + }, + -1513: {"text": "Invalid data from callback routine", "description": ""}, + -1514: { + "text": "Invalid parameter array index", + "description": "A parameter index value specified in a Data ID reference is invalid. Probably the robot number or other index value is too large.", + }, + -1515: { + "text": "Invalid data file format", + "description": 'Some or all of the data in a configuration ".pac" file could not be read because it did not have the expected format. Either the file has become corrupted or an attempt to manually edit the file has introduced some errors.', + }, + -1516: { + "text": "Invalid number of axes or motors", + "description": "The total number of axes or motors specified for a robot violates the number permitted. For example, this can occur if you specify 5 axes for the XYZTheta robot module. More subtly, if you configure the maximum number of motors supported by the system and then attempt to turn on split-axis control for one or axes, this requires that more motors be added.", + }, + -1517: { + "text": "Invalid signal number", + "description": "A specified analog or digital signal number is not within the allowed range of values for the signal type expected. Change the signal number to a valid value.", + }, + -1518: { + "text": "Undefined signal number", + "description": "A specified analog or digital signal is not currently installed in the controller. Remote signals may dynamically change between installed and uninstalled status depending on the state of the remote device. Verify that all remote devices are connected and active.", + }, + -1519: { + "text": "Output signal required", + "description": "An input signal has been specified when an output signal is required. Change the signal number to an appropriate value.", + }, + -1520: { + "text": "Can't open parameter DB file", + "description": "During initialization, one or more of the parameter database files (*.pac files) could not be found on the controller's flash drive in the folder /flash/config. Restore the files and reboot your controller.", + }, + -1521: { + "text": "Invalid parameter DB file not loaded", + "description": "A parameter database file (*.pac file) was found in the controller's flash drive folder /flash/config but the file format is not valid. Restore the file and reboot your controller.", + }, + -1522: { + "text": "Cannot write value when power on", + "description": "An attempt has been made to alter a Parameter Database value when motor power is enabled. However, the parameter can only be modified if motor power is disabled. This is a safety precaution to ensure that the robot does not move erratically when the value is modified. To avoid this error condition, disable motor power and retry modifying the Parameter Database value.", + }, + -1523: { + "text": "Coupled axis must be on same node", + "description": "Two coupled robot axes, for example two axes that are coupled in Split-axis control, have been defined to be driven by amplifiers that are contained on two different physical controllers that are connected on a Servo Network. This is not permitted. The two axes must be driven by amplifiers on the same physical controller.", + }, + -1524: { + "text": "Saving PDB values to flash not allowed", + "description": "A special command has been issued that inhibits any modified Parameter Database values that are in memory from being written to the flash disk. This command is typically issued by system programs that temporarily modify PDB values, which if written to the flash, would corrupt the valid data that is already contained on the flash. If you wish to modify PDB values and permanently save the changes, reboot the controller to clear this special mode.", + }, + -1525: {"text": "Servo network not allowed with EtherCAT", "description": ""}, + -1526: {"text": "EtherCAT configuration mismatch", "description": ""}, + -1527: {"text": "EtherCAT object not supported", "description": ""}, + -1528: {"text": "Too many EtherCAT slaves", "description": ""}, + -1550: { + "text": "Data ID cannot be logged", + "description": "A data log specification refers to a Data ID that cannot be logged. Some values require too much computation to permit real-time logging.", + }, + -1551: { + "text": "Invalid when datalogger enabled", + "description": "An attempt has been made to perform an operation that is not valid while the datalogger is active. Disable the datalogger and try again.", + }, + -1552: { + "text": "Datalogger not initialized", + "description": 'An attempt has been made to start the datalogger before it has been initialized. Normally this error is not seen if the web interface is used for logging. It may be seen if database parameter "Data logger enable" (DataID 753) is set before parameter "Data logger load command" (DataID 752) is set.', + }, + -1553: { + "text": "No data items defined", + "description": "An attempt has been made to start the datalogger before defining any items to log.", + }, + -1554: { + "text": "Trigger Data ID must be logged", + "description": "A datalogger trigger specification refers to a Data ID that is not specified as a logged item. Triggers can only specify Data ID values that are also being logged.", + }, + -1555: { + "text": "Trigger Data IDs on different nodes", + "description": "In a networked servo system, a datalogger trigger specification attempted to compare two Data IDs whose values exist on different network nodes. A trigger can only compare items that are on the same node.", + }, + -1556: { + "text": "Trigger not allowed for Data ID", + "description": "Some Data ID values cannot be used as trigger values. For example Cartesian positions or velocities are computed from logged data after logging is complete, so these values cannot be tested at logging time.", + }, + -1558: { + "text": "Too many remote data items", + "description": "In a servo network or GSB-based system, items collected by the datalogger from slave nodes are streamed from the slave to the master in real time. The amount of data that can be streamed depends on the trajectory period, the datalogger sampling interval, and the size and number of data items being logged from the slave. This error indicates that you have requested more data to be logged than can be streamed. Reduce the number of data items requested from the slave or increase the datalogger sampling interval.", + }, + -1560: { + "text": "Invalid when CPU Monitor enabled", + "description": "An attempt was made to start the CPU Monitor utility or read data from it when it was active. Wait until the current monitor interval is complete, or cancel the current monitor and try again.", + }, + -1561: { + "text": "No CPU Monitor data available", + "description": "An attempt was made to read the CPU Monitor data before the CPU Monitor utility was run. Execute the CPU Monitor utility and try again.", + }, + -1600: { + "text": "Power off requested", + "description": 'Robot power had been turned off by request. One of the following occurred: The GPL property Controller.PowerEnabled has been set to FALSE. The parameter database item "Power enable" (DataID 241) has been set to FALSE.', + }, + -1601: { + "text": "Software Reset: using default settings", + "description": 'When the system was restarted, the "Software Reset" switch on the MCIM Selector Switch was set to the ON state. This forced the system to read in the default configuration files (*.PAC) instead of the standard files. This feature should be used if a configuration files becomes corrupted or a setting inadvertently make the system unusable.', + }, + -1602: { + "text": "External E-STOP", + "description": 'A front panel E-Stop loop ("ESTOP_L 1" or "ESTOP_L 2") has been broken and the status signal "External ESTOP_L" is asserted, indicating that external equipment is the source of the E-Stop.', + }, + -1603: { + "text": "Watchdog timer expired", + "description": "The hardware watchdog timer on the CPU board has expired and robot power is disabled. This error may indicate a hardware failure or software bug and should not normally be seen. If this error persists, please contact customer service to report this problem.", + }, + -1604: { + "text": "Power light failure", + "description": "The robot power-on light interfaced via the front panel connector has burned out. Robot power is turned off and may not be turned back on. Please contact customer service.", + }, + -1605: { + "text": "Unknown power off request", + "description": "Robot power is off or was turned off, but no request was made to turn it off. This error may indicate a hardware failure or software bug and should not normally be seen.", + }, + -1606: { + "text": "E-STOP stuck off", + "description": "When the controller is restarted, a diagnostic program has detected an E-Stop circuit that is stuck in the off state. Robot power cannot be turned on until the stuck E-Stop circuit is repaired and the controller is restarted.", + }, + -1607: { + "text": "Trajectory task overrun", + "description": 'The periodic trajectory generation system task was unable to complete its executing during its allotted period. The parameter "Trajectory Generator update period in sec" (DataID 600) is set too small for the type of robot you are using.', + }, + -1609: { + "text": "E-STOP timer failed", + "description": "When the controller is restarted, a diagnostic program has detected that the hardware E-Stop timer is not triggering as expected. Robot power cannot be turned on and you must restart the controller. If this error persists, please contact customer service to report this problem.", + }, + -1610: { + "text": "Controller overheating", + "description": 'In GPL 3.0 H1 and later, this error is reported in addition to -1617 CPU overheating, -3144 Amplifier overheating or -3145 Motor overheating. These other errors provide detailed information on the specific component that is generating the overheating error condition. Prior to GPL 3.0 H1, this is the only error message that is generated when the CPU or one of the power amplifiers has exceeded its permitted operating temperature. For the CPU, the limit is 90 degrees Celsius. For the amplifiers of the G3xxx and G1xxxA/B controllers (except for the G3x3xA 30A version), the limit is 80C. For the amplifiers of the G3x3xA 30A version, the limit is 100C. For the G2xxx controllers, the amplifier chips are equipped with internal temperature monitoring to protect the power modules, but they do not have temperature sensors that can be read. If the CPU is overheating, it will switch off in 5 minutes and then you will need to reboot the controller. For all versions of software, robot power is automatically turned off and cannot be turned on until the overheated device cools. In a servo network, the overheated device may reside in the master controller or one of the slaves. This error is issued each time a command to enable robot power is received until all temperatures return to acceptable levels. DataIDs 126, 12605 and 12110 contain the "CPU temperature" and "Amp temperature", and "Motor temperatures" respectively, and can be displayed to determine which device is overheating.', + }, + -1611: { + "text": "Auto/Manual switch set to Manual", + "description": 'A attempt has been made to enable robot power, using one of the standard means, when the Auto/Manual switch is set to Manual. The robot power has not been enabled. The standard means of enabling power are by using: the PowerEnabled property of the Controller class in GPL, the "Enable Power" parameter (DataID 241), the "Automatic power on" parameter (DataID 240), the power enable digital input (defined by DataID 242), or CANopen.', + }, + -1612: { + "text": "Power supply relay stuck", + "description": 'An attempt to turn on power has failed because an internal diagnostic test indicates that the motor power supply relay is stuck in the "on" position and may be unsafe. Reboot your controller. If this error persists, contact customer service.', + }, + -1613: { + "text": "Power supply shorted", + "description": "Power has been disabled because the motor power supply has detected that it is shorted. Check the wiring and the amplifiers to make sure no short is present.", + }, + -1614: { + "text": "Power supply overloaded", + "description": 'Power has been disabled because the motor power supply has detected an overload condition. Verify that you are not attempting to draw more than the rated current from the power supply. Verify that the parameter "Delay after turning on power in sec" (DataID 263) is set long enough to allow power to stabilize before the amplifiers are enabled.', + }, + -1615: { + "text": "No 3-phase power", + "description": "A power supply has been configured to use 3-phase power, but the 3rd phase is not connected or has failed. It is still possible to run the power supply in this mode at low power, but attempting to draw high power may overheat the power supply.", + }, + -1616: { + "text": "Shutdown due to overheating", + "description": 'The CPU exceeded its operating temperature for too long a period of time and the controller automatically turned off. When the CPU first exceeds its maximum permitted limit of 90 degrees Celsius, a "Controller overheating" error (-1610) is generated and motor power is disabled. If the temperature does not decline within 5 minutes, error code -1616 is logged to the flash in a special system file ("/flash/system/errlog.txt")and the controller is shutdown. The next time the controller is restarted, the -1616 error code will be displayed in the error log. This error code will be displayed every time the controller is restarted until the error log is cleared.', + }, + -1617: { + "text": "CPU overheating", + "description": 'Either the master or a slave CPU board temperature has exceeded its maximum temperature of 90 degrees Celsius. See CPU temperature (DataID 126) for the current temperature of all CPUs. If the temperature does not fall below 90 degrees, the controller will switch off in 5 minutes and the controller must be restarted. Normally this error is followed by the generic error -1610 "Controller overheating". Ensure there is good air circulation around the controller and check that the fans are not blocked. The Installation section of the controller hardware manuals provide guidelines for ventilating and cooling controllers.', + }, + -1618: { + "text": "Power supply not communicating", + "description": 'Some PreciseFlex™ power supplies communicate with the PreciseFlex™ controller for identification, error detection, and safety interlocks. This communications has failed. Be sure that all cables between the controller and power supply are installed properly. Verify that DataID 128 "Power supply type" is set properly for the type of power supply you are using.', + }, + -1619: { + "text": "Power disabled by GIO timeout", + "description": 'This errors occurs in two possible cases: If the error message indicates node 8 and the robot has a "safety board" (e.g. a PreciseFlex™ 3400), this error indicates that the safety board has stopped responding for 32 trajectory periods (approximately 0.128 seconds). This error disables the robot motor power and this error cannot be disabled. Otherwise, the error is related to the GIO board indicated by the node number in the error message. The GIO has stopped responding for 32 trajectory periods and the "GIO mode" (DataID 574) has bit mask 2 set, so that robot motor is disabled. If bit mask 2 is clear, GIO board timeouts do not disable robot motor power. This error might be due to: a wiring problem; improperly installed RS485 bus termination jumpers on the main CPU, Safety, GIO or GSB boards; or excessive noise in another cable, motor, or amplifier that is close to the RS485 cable. Issuing a "GSB Show" command from the System Web Console may provide additional information about the source of the error.', + }, + -1620: { + "text": "Safety diagnostics failed", + "description": 'In enhanced CAT3 mode, the safety diagnostic checks have failed. Robot power cannot be enabled. Previous error messages may indicate the cause of the failure. Use the "Show Safety" web console command for more details. Once the problem has been resolved, try to enable power again.', + }, + -1621: { + "text": "Safety software configuration mismatch", + "description": 'The safety configuration bits in DataID 2031 "Enhanced safety mode" do not match the settings of DataID 117 "Safety mode" or require hardware not present in your controller. For example, you set bit &H001 of DataID 2031 but DataID 117 is not set to 4 or 5 or you set bit &H200 in DataID 2031 but do not have a 3-phase power supply. Verify that DataID 117 is correct. For hardware-related errors, contact Brooks support. If a numeric value is displayed with this error, the value indicates what aspect of the safety configuration did not match. See DataID 2031 "Enhanced safety mode" for a description of the bits that form this value.', + }, + -1622: { + "text": "Not allowed in safety mode", + "description": 'You have attempted to perform some operation that is not allowed in your current safety mode. For example, you have attempted to enable power from a user program while DataID 117 "Safety mode" is set to 5 (Enhanced CAT-3 full).', + }, + -1623: { + "text": "ESTOP1 stuck on", + "description": "The safety diagnostics in enhanced CAT3 mode have determined that ESTOP channel 1 is asserted. Probably your channel 1 ESTOP loop is open. Close the loop and retry the operation.", + }, + -1624: { + "text": "ESTOP2 stuck on", + "description": "The safety diagnostics in enhanced CAT3 mode have determined that ESTOP channel 2 is asserted. Probably your channel 2 ESTOP loop is open. Close the loop and retry the operation.", + }, + -1625: { + "text": "ESTOP1 stuck off", + "description": "The safety diagnostics in enhanced CAT3 mode have determined that ESTOP channel 1 cannot be asserted by the internal force-ESTOP circuit. Your channel 1 ESTOP may be wired incorrectly.", + }, + -1626: { + "text": "ESTOP2 stuck off", + "description": "The safety diagnostics in enhanced CAT3 mode have determined that ESTOP channel 2 cannot be asserted by the internal force-ESTOP circuit. Your channel 2 ESTOP may be wired incorrectly.", + }, + -1627: { + "text": "Motor power stuck on", + "description": 'The safety diagnostics in enhanced CAT3 mode have determined the DC voltage indicated by DataID 12684 ("Nominal DC bus voltage, volt") is too high when the motor power has been turned off. This may indicate a failed internal safety circuit or an unplugged cable.', + }, + -1628: { + "text": "Motor power stuck off", + "description": 'The safety diagnostics in enhanced CAT3 mode have determined the DC voltage indicated by DataID 12684 ("Nominal DC bus voltage, volt") is too low when the motor power has been turned on. This may indicate a failed internal safety circuit or an unplugged cable.', + }, + -1629: { + "text": "FFC ENA_PWR' signal stuck on", + "description": 'In a controller that includes a FFC safety board, the safety diagnostics in enhanced CAT3 mode have determined the DC voltage indicated by DataID 12684 ("Nominal DC bus voltage, volt") is too high when the motor power has been turned off. Probably the FFC_ENA_PWR\' signal on the FFC board is stuck on or this signal from the controller is shorted high. When the safety board is present, the output from the motor power supply should be off when the controller commands that motor power should be disabled. If the output of motor power supply is on, there is a problem with the motor power disable signal and its associated circuits.', + }, + -1630: { + "text": "FFC ENA_PWR' signal stuck off", + "description": 'In a controller that includes a FFC safety board, the safety diagnostics in enhanced CAT3 mode have determined the DC voltage indicated by DataID 12684 ("Nominal DC bus voltage, volt") is too low when the motor power has been turned on. Probably the FFC_ENA_PWR\' signal on the FFC board is stuck off or this signal from the controller is shorted low.', + }, + -1631: { + "text": "Power dump circuit failed", + "description": 'The safety diagnostics in enhanced CAT3 mode have determined the DC voltage indicated by DataID 12684 ("Nominal DC bus voltage, volt") is not falling quickly enough when power is turned off. This indicates that the power dump circuit has failed or is missing. Verify that you are using the correct robot *.pac files with this robot.', + }, + -1632: { + "text": "At least one motor must be enabled", + "description": 'The safety diagnostics in enhanced CAT3 mode have determined that there are no motors enabled for the robot. At least one motor must be present to perform the safety tests. Check the values for DataID 2105 ("Simulate servo interface") and DataID 2026 ("Motor disable mask") to make sure at least one motor is enabled. If you really want all motors disabled, you must also disable safety mode using DataID 117.', + }, + -1633: { + "text": "Hardware watchdog timer failed to disable power", + "description": 'The safety diagnostics in enhanced CAT3 mode have determined the DC voltage indicated by DataID 12684 ("Nominal DC bus voltage, volt") is too high after the hardware watchdog timer has expired. There may be a hardware problem with the watchdog timer, or you have attempted to enable safety mode on an older controller that does not contain a hardware watchdog timer.', + }, + -1634: { + "text": "FPGA watchdog timer failed to disable power", + "description": 'The safety diagnostics in enhanced CAT3 mode have determined the DC voltage indicated by DataID 12684 ("Nominal DC bus voltage, volt") is too high after the watchdog timer in the FPGA has expired. There may be a hardware problem with the watchdog timer.', + }, + -1635: { + "text": "FPGA watchdog trigger stuck on", + "description": "The FPGA indicates that the watchdog timer is triggered even though it is being polled normally. There may be a hardware problem with the watchdog timer or the FPGA.", + }, + -1636: { + "text": "FPGA watchdog trigger stuck off", + "description": "The FPGA indicates that the watchdog timer is not triggered even though it is not being polled. There may be a hardware problem with the watchdog timer or the FPGA.", + }, + -1700: { + "text": "Cannot get local host name", + "description": "The Ethernet network is not configured properly. Check the parameter values for DataIDs 420, 421 and 422.", + }, + -1701: { + "text": "Cannot get local host address", + "description": "The Ethernet network is not configured properly. Check the parameter values for DataIDs 420, 421 and 422.", + }, + -1702: { + "text": "Connection refused", + "description": "An attempt to connect to a TCP server was refused. Be sure your IP address and port numbers are correct. Be sure the server is ready to accept connections.", + }, + -1703: { + "text": "No connection", + "description": "An attempt was made to send or receive on socket that was not connected to a TCP or UDP port.", + }, + -1704: { + "text": "Invalid network address", + "description": "The IP address specified cannot be accessed.", + }, + -1705: { + "text": "Network timeout", + "description": "A network operation did not complete within the allowed time period. Depending on the operation, the connection may or may not be closed.", + }, + -1706: { + "text": "Already connected", + "description": "An attempt was made to connect a socket to an IP Address and port when there is already a connection.", + }, + -1707: { + "text": "Socket not open", + "description": "An attempt was made to use a network socket that is not open.", + }, + -1708: { + "text": "Connection closed", + "description": "An attempted was made to use a socket whose connection has been closed either locally or by the remote endpoint.", + }, + -1709: { + "text": "Invalid protocol", + "description": "A message was received that does not follow a known communications protocol.", + }, + -1710: { + "text": "Invalid multicast address", + "description": "The IP address is not a valid multicast address. The recommended range for multicast IP addresses is from 239.192.0.0 through 239.195.255.255.", + }, + -1720: { + "text": "Web interface not enabled", + "description": 'An external system attempted to access one of the controller\'s web pages, but access via the web server has been disabled. To enable the web interface, modify the value of the "Web password security" (DataId 450) database parameter.', + }, + -1730: { + "text": "Modbus/RIO exception: n", + "description": "A MODBUS or RIO board request failed with exception code n. The standard MODBUS exception codes are: 1 = Illegal function, 2 = Illegal data address, 3 = Illegal data value, 4 = Device has failed.", + }, + -1731: { + "text": "Modbus/RIO device timeout", + "description": "A MODBUS device or RIO board is not responding or is not responding within the specified timeout period.", + }, + -1732: { + "text": "Modbus/RIO disable requested", + "description": 'A MODBUS device or RIO board connection has closed because of a user request. For example, you have set the parameter "Remote IO module enable" (DataID 554) to zero.', + }, + -1740: { + "text": "Servo latency too large", + "description": "The master controller cannot connect with a slave controller because the measured communications latency is too large or is negative. Typically there should be no more than 80 microseconds of latency between the nodes. Verify that both controllers are on the same LAN and external traffic is low. Verify that your Ethernet switch is operating properly. Verify that the servo network protocols for all nodes are compatible.", + }, + -1750: {"text": "EtherCAT error", "description": ""}, + -1751: {"text": "EtherCAT slave not responding", "description": ""}, + -1752: {"text": "EtherCAT synchronous message timeout", "description": ""}, + -1753: {"text": "EtherCAT slave not ready", "description": ""}, + -1754: {"text": "EtherCAT disabled", "description": ""}, + -2101: { + "text": "Vertical search limit violated", + "description": "This error is generated during the vertical motion to search for the height of the nest. This indicates that the motion search distance limit was encountered before the specified force level was achieved.", + }, + -2102: { + "text": "Horizontal search limit violated", + "description": "This error is generated during a horizontal motion to search for the edges of the nest. This indicates that the motion search distance limit was encountered before the specified force level was achieved.", + }, + -2103: { + "text": "Insufficient number of Tool X samples", + "description": "The nest height detection routines did not collect the minimum required number of force samples either when the plate was in free air or when the plate was pressing against the nest.", + }, + -2104: { + "text": "Insufficient number of Tool Tx torque samples", + "description": "The nest orientation detection did not collect the minimum required number of torque samples either when the plate was rotated between the edges of the nest.", + }, + -2105: { + "text": "No free air during Yaw detection", + "description": "When using Tx to determine the nest Yaw angle, the line fit to the first samples overlapped with the line fit to the last of the samples. This indicates that no free air region was detected.", + }, + -2106: { + "text": "Motor exceeded peak torque", + "description": "One or more motors was generating the peak permitted torque. The torque will be saturated and the Cartesian gripper forces and torques will not be accurate. The offending motor numbers are listed.", + }, + -2107: { + "text": "GPL 3.2 or later required", + "description": "This package relies on features contained in GPL 3.2 or later.", + }, + -2850: { + "text": "Invalid Gripper Type", + "description": "This command requires a servoed gripper and none is detected.", + }, + -2851: { + "text": "Invalid Station ID", + "description": "A station ID less than 1 or greater than the maximum number of stations has been specified.", + }, + -2852: { + "text": "Invalid robot state to execute command", + "description": "The command cannot be executed while the robot is in its current state. For example, you cannot issue a TeachPlate command while the robot is moving.", + }, + -2853: { + "text": "Rail not at correct station", + "description": "The optional rail is not currently at or moving toward the destination station for this command. Issue a MoveRail command to move the rail to the desired station.", + }, + -2854: { + "text": "Invalid Station type", + "description": "The robot cannot move to a station of the requested type. For example, the PP100 robot cannot move to a horizontal station.", + }, + -2855: { + "text": "No gripper close sensor", + "description": "The command requires a gripper-close sensor but no such sensor exists for the current robot type.", + }, + -3000: {"text": "NULL pointer detected", "description": ""}, + -3001: {"text": "Too many arguments", "description": ""}, + -3002: {"text": "Too few arguments", "description": ""}, + -3003: {"text": "Illegal value", "description": ""}, + -3004: {"text": "Servo not initialized", "description": ""}, + -3005: {"text": "Servo mode transition failed", "description": ""}, + -3006: {"text": "Servo mode locked", "description": ""}, + -3007: {"text": "Servo hash table not found", "description": ""}, + -3008: {"text": "Servo hash entry collision", "description": ""}, + -3009: {"text": "No hash entry found", "description": ""}, + -3010: {"text": "Servo hash table full", "description": ""}, + -3011: {"text": "Illegal parameter access", "description": ""}, + -3012: { + "text": "One or more servo tasks stopped", + "description": "A software watchdog timer has not been updated in the required time. This normally indicates a controller hardware failure or a system software bug.", + }, + -3013: {"text": "Servo task submission failed", "description": ""}, + -3014: { + "text": "Cal parameters not set correctly", + "description": """This error is generated if a homing operation is initiated and one or more parameters that affect homing are not set properly. For example, this error is generated in the following circumstances: + + The "Hardstop envelope limit, mcnt" (DataID 10122) is less thanor equal to zero or it is greater than either the "Soft envelope error limit, mcnt" (DataID 10302) or the "Hard envelope error limit, mcnt" (DataID 10303). If you are not using DataID 10122 for your homing method, set it to a small non-zero value to avoid this error.""", + }, + -3015: { + "text": "Cal position not ready", + "description": """If your robot is equipped with the Precise Absolute encoders, e.g. you have a PrecisePlace 2300/2400 robot, this indicates that the robot does not have its factory encoder calibration data defined. Most likely, one of the "Index code for calibration" (DataID 16241) values is zero. To correct the problem, run the factory encoder calibration program. + + For 3rd party absolute encoders, this error indicates that the full precision absolute encoder position could not be read from the encoder during the homing operation. In some cases, this indicates an error in reading the multiple turn counter for the encoder. If this problem persists, it probably indicates an encoder or controller hardware failure. + + For incremental encoders, this error indicates that a signal required for the selected homing method was not found (e.g. a missing homing or limit or index signal) or a signal was corrupted (e.g. incorrect index due to excessive skew).""", + }, + -3016: { + "text": "Illegal cal seek command", + "description": "This error should never be generated. It indicates that the homing operation sent an illegal command to the servo code. Please report this message along with the process that generated this problem to Precise.", + }, + -3017: { + "text": "No axis selected", + "description": "A debug control panel operation has been requested for an axis that does not exist on a particular servo network node. Reselect the axis and try again. Verify that the node mapped to the axis is active on the network.", + }, + -3100: { + "text": "Hard envelope error", + "description": """This error is generated if the value of the "Position tracking error, mcnt" (DataID 12320) for an axis exceeds its "Hard envelope error limit, mcnt" (DataID 10303) for a sufficient period of time. This error indicates that there was a significant difference between an axis' commanded position and its actual position. This can occur if: + + The axis is being driven too fast for the load that it is carrying. + The axis hits an obstacle and cannot advance. + The axis is oscillating due to a servo tuning instability. + There is a hardware failure of some type. + Normally, this error can be avoided by reducing the speed and/or acceleration of a motion.""", + }, + -3101: { + "text": "PID output saturated too long", + "description": """This error indicates that the sum of the servo feedback terms ("Compensator output torque" (DataID 12304) minus the sum of the "Dynamic feedforward torque" (DataID 12337) and the "Filtered feedforward torque" (DataID 12331)) has either saturated the maximum specified torque for an axis or the "Max positive/negative torque limit for PID feedback" (10351,10352) for more than the time specified the "PID output saturation duration limit" (DataID 10369), which is set to 200 msec by default. + + This error is generated if the limits are set too low or the axis has been over-driven or the axis has collided with an obstacle or some other unexpected error has occurred. + + This check reduces the time that an axis is over-driven or that it drives into an obstacle.""", + }, + -3102: { + "text": "Illegal zero index", + "description": """An encoder zero index pulse was detected when none is expected. The axis is marked as "not calibrated" and the robot must be re-homed. Verify that parameters "Encoder counts for resolution calc, ecnt" (DataID 10203) and "Encoder revs for resolution calc, rev" (DataID 10204) are correct. If the unexpected pulse is due to noise, the parameter "Index noise spikes limit" (DataID 10222) may be adjusted. If the unexpected pulse is due to encoder slippage, parameter "Index skew count limit, mcnt" (DataID 10221) may be adjusted.""", + }, + -3103: { + "text": "Missing zero index", + "description": """No encoder zero index pulse was detected when one is expected. The axis is marked as "not calibrated" and the robot must be re-homed. Verify that parameters "Encoder counts for resolution calc, ecnt" (DataID 10203) and "Encoder revs for resolution calc, rev" (DataID 10204) are correct. If the missing pulse is due to encoder slippage, parameter "Index skew count limit, mcnt" (DataID 10221) may be adjusted.""", + }, + -3104: { + "text": "Motor duty cycle exceeded", + "description": """Duty cycle testing is intended to prevent a motor from being damaged due to overheating. The overheating estimate is computed based upon the average power that is supplied to a motor by an amplifier over a period of time. + + The duty cycle criteria is defined by the "RMS rated motor current, A(rms)" (DataID 10611), "Duty cycle limit in terms of rated torque" (DataID 10623), "Duty cycle exceeded duration" (DataID 10622) and "Duty cycle SPR filter pole" (DataID 10621). + + If the dynamically computed "Duty cycle value, tcnt^2" (DataID 12606) exceeds the "Duty cycle limit, tcnt^2" (DataID 10624), which is defined from the criteria above, the "Motor duty cycle exceeded" error is generated and motor power is disabled. + + If this error is generated, but the motor is still very cool, try changing the "Duty cycle SPR filter pole" (DataID 10621) to average the power over a longer period of time. This will reduce the effect of short periods of high power utilization.""", + }, + -3105: { + "text": "Motor stalled", + "description": """This error indicates that the torque/current for a motor has been saturated at the peak value as defined by the "RMS rated motor current, A(rms)" (DataID 10611) * "AUTO mode motor PEAK(non-RMS)/(RMS rated) current, %" (DataID 10613) for "Motor stalled check duration" (DataID 10617) seconds. + + This is different than the conventional definition of having a motor not moving for a period of time with the torque/current continuously above a specified level.""", + }, + -3106: { + "text": "Axis over-speed", + "description": """This error is generated when power is enabled or during normal running if the system detects that an axis has violated a speed limit. + + If this occurs when power is enabled, it indicates that the axis has violated the speed limit defined by "Special power-up speed limit" (DataID 10210). The possible causes for this error are as follows: + + The motor torque sign is negated and the axis is attempting to run away at high speed. + For 3rd party amplifiers, there is an excessive DAC offset. + For gravity loaded axes, the axis might be dropping very quickly after the brake is released. + A somewhat large offset between the amplifier/motor phase currents might be causing the axis to move excessively when the system attempts to automatically compensate for the phase offset. The "Disable auto phase offset adjustment" (DataID 10695) can be used to turn off this adjustment if desired. + The "Special power-up speed limit" (DataID 10210) may be set too low. + If this error occurs when the system is running, it indicates that either the "Run-time speed limit" (DataID 10208) or the "Manual mode speed limit" (DataID 10209) has been violated. When this runtime error occurs, do the following: + + Reduce the speed of your motions to avoid damaging the motor or its gear train or violating manual control safety regulations. + Review the values of 10208 and 10209 and ensure that they are set properly. + If possible, reduce the gear ratio of the motor to reduce the maximum motor rotational speed. + If the error is triggered by intermittent noise in the velocity signal, reduce the "Motor velocity SPR filter pole" (10207). This value will not affect the PID loop tuning, but it does affect other functions of the system. Review the documentation for 10207 before you change its value.""", + }, + -3107: { + "text": "Amplifier over-current", + "description": "This error is generated if the FPGA firmware detects that the output motor current has exceeded the specified current limits for too long a time.", + }, + -3108: { + "text": "Amplifier over-voltage", + "description": """This error is generated by the FPGA firmware when it detects that the DC bus voltage is too high. The limit on the bus voltage is a function of the controller model being utilized. For the G1xxxA/B controllers, the maximum voltage is approximately 59.5 VDC. For the G3xxx and G2xxxB/C series controllers, the maximum voltage was approximately 445 VDC, but in May 2014 was adjusted down to 436 VDC. Whenever a motor decelerates, it typically pumps power back into the motor power supply and the voltage will rise above its nominal value. For example, if the nominal bus voltage is 330V, it is not unusual to see the voltage rise into the high 300's when a large motor is decelerating. To monitor the DC bus voltage, see "Raw DC bus voltage, volt" (DataID 12684). If this problem persists, it may indicate that the motor power supply must be changed or augmented to include more capacity to dump or absorb more power when the robot is decelerating.""", + }, + -3109: { + "text": "Amplifier under-voltage", + "description": """This indicates that the DC motor bus has dropped too low. This can occur if: + + The bus voltage does not rise above 10V the first time that motor power is enabled. + The bus voltage falls too far (typically 30%) below its nominal value at any time after power has been enabled. + The bus voltage falls below 10V while motor power and the motor amplifiers are enabled. + If this error is generated at the same time as a "Hard E-STOP" error (-1028), it is possible that the "E-stop delay" (DataID 267) is set too short and the DC motor bus voltage is dropping before the amplifiers are disabled. + + If this error persists, it could be due to a fuse on the Motor Power Supply being blown. To monitor the DC bus voltage, see "Raw DC bus voltage, volt" (DataID 12684). To see the current setting of the nominal voltage look at "Nominal DC bus voltage, volt" (DataID 12683).""", + }, + -3110: { + "text": "Amplifier fault", + "description": """This is a generic message indicating that the amplifier hardware has detected a significant problem and has shut down. For example, the output motor current has exceeded the rated limits of the hardware, or the input power to the amplifier has failed while the amplifier was enabled. Frequently, a separate amplifier-related error message is also displayed that provides details about the specific problem. + + Verify that the amplifiers are configured properly. + Verify the motors are wired correctly. + Verify the current loop tuning is correct. Unstable current loop tuning can trigger an amplifier fault. + Verify the motor and motor harness are not shorted. This can be done by disconnecting the motor and measuring the resistance between the UVW phases. The resistance should be the same for each pair and low, but not zero. + If this error occurs when an E-STOP is asserted, verify that the "Delay after setting brakes" (DataID 260) is longer than or equal to the "E-Stop delay" (DataID 267). Normally, DataID 260 should be at least 0.1 seconds shorter than DataID 267.""", + }, + -3111: { + "text": "Brake fault", + "description": "This error indicates that the FPGA has detected a fault condition in the hardware motor brake driver circuit. This test is not currently enabled, so this error message should never be generated.", + }, + -3112: { + "text": "Excessive dual encoder slippage", + "description": """If an axis has been configured for dual encoder loop control (i.e. two encoders are used to control a single motor), this error is generated if there is an excessive position or speed differential between the readings of the two encoders. The slippage limits are defined by "Dual loop position slippage limit" (DataID 10212) and "Dual loop speed slippage limit" (DataID 10216). + + If the axis is driven by a traction drive, some amount of slippage occurs every time that the axis is accelerated or decelerated. This normal slippage is automatically corrected for by the system software. If excessive speed slippage occurs, it could indicate that one of the two encoders has failed and the system was shutdown to prevent a run-away condition.""", + }, + -3113: { + "text": "Motor commutation setup failed", + "description": """The procedure for determining the commutation reference angle for the motor failed and the reference angle was not established. This error normally occurs the first time that motor power is enabled after the controller is restarted or during the homing process. + + The followings are the common causes for the failure: + + Motor power was disabled. The motor power was manually disabled or was automatically disabled due to another error occurring before the end of the search process. + Lose motor cable. Some commutation reference search processes move the motor a short distance. During this small motion, the motor cable became disconnected. Encoder feedback lost. + TEhnec ofdeeedback device (encoder) is not working properly or the "Encoder type" (DataID 10027) was incorrectly set. + Incorrect configuration parameters. Any of the following parameters may have been incorrect set. The Precise Configuration Utility (PCU) contains tools that can be used to verify the correct settings. + DataID 10108, Dedicated DIN's selection + DataID 10650, Commutation sign + DataID 10651, # of pole pairs per motor revolution + DataID 10652, Commutation counts per electrical cycle + Poor current loop tuning. The motor current loop may not be properly tuned. + Improper configuration parameters for selected commutation search method. While the default parameter values will work for a wide variety of axis configurations, there are cases which require parameter adjustment in order to properly perform commutation reference finding. Refer to the selected commutation method parameter description for details. + If the problem persists after checking the above items, please contact Precise support for further assistance.""", + }, + -3114: { + "text": "Servo tasks overrun", + "description": 'The servo tasks are not able to complete their servo computations within the specified servo period. Too many axes are being servoed by a single board, the parameter "Servo update period in sec" (DataID 603) is too small, or a CPU failure has occurred. This error is fatal and prevents robot power from being turned on until the controller is rebooted.', + }, + -3115: { + "text": "Encoder quadrature error", + "description": """For incremental encoders, the encoder count and direction of change is derived from two square waves (channels A & B) that are 90 degrees out of phase. If at any time, the FPGA detects that the phase angle between the channels is too small, a quadrature error is generated. Normally, this error is caused by noise in the encoder lines. To correct this problem, please see the recommendations on wiring encoders and motors in the Installation Section of the Controller Hardware Manual. The motor wiring is as important as the encoder wiring since the motors are often generating the noise that is cause the erroneous reading on the encoder channels. + + When this error occurs, the motor must be commutated again and should be re-homed since this error indicates that the position of the motor/encoder is not longer exactly known. + + For robots with gravity loaded axes (e.g. Z-axes), the "Severe error power off mode" (DataID 142) should be set to mode 1. If the robot's incremental encoders suffer a quadrature error, the robot's brakes will be set immediately and minimize dropping due to gravity loading.""", + }, + -3116: {"text": "Precise encoder index error", "description": ""}, + -3117: { + "text": "Amplifier RMS current exceeded", + "description": """This error indicates that the rated RMS current for an integrated amplifier has been exceeded and the software has disabled motor power to protect the amplifier from being damaged. This is an internal test that is automatically performed and cannot be disabled. See the specifications for your controller for the RMS rating of the amplifiers. If this error occurs, consider doing the following: + + Reduce the accelerations, decelerations and speeds for your motions. + Verify that the rated RMS current for the motor is equal to or below that of the amplifier. If the motor needs to operate at a higher average current level (due to gravity loading, constantly working against friction, etc.), consider purchasing a controller with amplifiers that have a higher rated RMS current. + This error is different than a "Motor duty cycle exceeded" (-3104) error. The duty cycle testing is intended to protect a motor from being damaged due to over-heating. There are a number of parameters for configuring the duty cycle testing to match a motor's operating specifications.""", + }, + -3118: { + "text": "Dedicated DINs not config'ed for Hall", + "description": """If the "Commutation reference setup config" (DataID 10700) specifies that the "Hall-effect" method is to be used for commutating a motor, then the "Dedicated DIN's selection" (DataID 10108) must be set to configure the single-ended inputs in the corresponding encoder connector for use as hall sensor inputs. If DataID 10108 is not set properly, this error is generated. Normally, DataID 10108 is automatically configured if DataID 10700 specifies the "Hall-effect" method.""", + }, + -3119: { + "text": "Illegal 6-step number", + "description": """If the "Commutation reference setup config" (DataID 10700) specifies that the "Hall-effect" method is to be used for commutating a motor, the single-ended inputs in the corresponding encoder connector are read to determine the hall sensor 6-step value. The only permitted hall readings are values from 1 to 6. If the single-ended digital inputs are set to some other value, this error is generated.""", + }, + -3120: { + "text": "Illegal commutation angle", + "description": """This is a general error message that indicates a problem has been detected with the commutation reference angle. Some possible problems that would generate this error message include: + + The "Commutation counts per electrical cycle" (DataID 10652) may be set to zero or some other invalid number. + For a motor with an absolute encoder, the "Commutation offset" (DataID 10775) may be un-initialized so the commutation reference angle cannot be set based upon the encoders single turn data reading. + For a motor with a serial incremental encoder that outputs hall sensor readings during startup, the hall sensor reading may have been unavailable or invalid. + For a motor with hall sensors, the commutation angle specified for a hall sensor reading (DataIDs 10744-10756) may be un-initialized. + For a motor with an analog encoder, the "Analog hall commutation phase angle" (DataID 10756) may not yield a valid commutation reference angle.""", + }, + -3121: { + "text": "Encoder fault", + "description": """This code is generated when an error occurs in communication with a serial encoder such as a Panasonic or Yaskawa serial encoder. This error indicates one of several possible problems. + + For the Yaskawa Sigma II/III serial absolute encoder, this error is generated when the encoder signals a "Runtime Error" due to an error in the encoder's memory. When this occurs, bit 1 in the "Encoder alarm" field (DataID 12251) is set. Check the encoder cable and cycle the power to see if the error goes away. See the Yaskawa encoder alarm documentation for more details. + For the Panasonic serial incremental encoder (type 41), this error is generated when the encoder signals a Preload error after the encoder has been initialized. When this occurs, bit 7 is set in the "Encoder alarm" field (DataID 12251). See the Panasonic encoder alarm documentation for more details. + + This is a severe error and requires the encoder to be re-initialization and/or power cycled.""", + }, + -3122: { + "text": "Soft envelope error", + "description": """The position error of an axis has exceeded the value set in the "Soft envelope error limit" (DataID 10302). This is a safety precaution to ensure that each axis does not deviate too far from its intended position. This error may indicate that the following has occurred: + + An axis has been commanded to accelerate too quickly or move too fast and does not have the power to perform the operation. If necessary, this can be confirmed by datalogging the "Compensator output torque" (DataID 12304) and the "Position tracking error" (DataID 12320) for the axis in question. If this is the source of the envelope error, the position error will increase significantly when the motor torque saturates at its maximum value for several tens of milliseconds. + The axis has gone unstable due to a hardware failure or some other error. This can be confirmed by listening for an audible noise or by datalogging the "Velocity tracking error" (DataID 12321) for the axis in question and looking for high frequency oscillations. + An axis may have crashed into an obstacle and is unable to move to the specified position. + The "Software envelope error limit" (DataID 10302) may be set to too small of a value.""", + }, + -3123: { + "text": "Cannot switch serial encoder mode", + "description": "When an absolute or incremental serial encoder is employed, it can operate in either sync or async mode. In order to switch between the two modes the axis motor power must be turned OFF. If it's not then this error is issued.", + }, + -3124: { + "text": "Serial encoder busy", + "description": """During communication with a serial absolute or incremental encoder, if the encoder takes too long to process a command, this error is issued. When the encoder is in its normal synchronous mode, this error can be generated when the encoder is periodically returning position data. During asynchronous mode , this error can be generated when the host controller issues a specific encoder command to perform a diagnostic function. + + If this error persists, please check the encoder connections and power cycle the encoder.""", + }, + -3125: { + "text": "Illegal encoder command", + "description": "The serial command issued from the controller is not supported by the encoder. Normally, this error should never occur. However, if it is generated, it indicates that there is an internal implementation error. Please inform Precise.", + }, + -3126: { + "text": "Encoder operation error", + "description": """This indicates that a serial encoder is rotating at too high a speed before motor power is turned on or an error has been detected in the encoder position data. This error is generated when the encoder signals the following alarms conditions: + + For Panasonic and Tamagawa serial encoders, this error is generated when "Over-speed" (bit 0) and/or "Counter Error" (bit 2) is set in the "Encoder alarm" field (DataID 12251). + For Yaskawa serial encoders, this error is generated when "Absolute Error" (bit 3), "Over-speed" (bit 4) and/or "Reset Complete" (bit 6) is set in the "Encoder alarm" field (DataID 12251). + This is a standard error and requires the encoder to be re-initialization and/or power cycled. + + See the Panasonic, Tamagawa and Yaskawa encoder alarm documentation for more details.""", + }, + -3127: { + "text": "Encoder battery low", + "description": "Many serial absolute encoders require a connection to an external battery. If the battery voltage is too low, this error is issued. When this error is generated, the encoder is still operational and its internal data, e.g. multi-turn value, will still be valid. However, the battery should be replaced as soon as possible before the internal data is lost.", + }, + -3128: { + "text": "Encoder battery down", + "description": """Many serial absolute encoders require a connection to an external battery. If the battery is dead or this backup power is disconnected (even momentarily), this error is issued and will be latched until cleared. When this error occurs, the encoder's internal multi-turn data is no longer valid and the encoder position must be re-calibrated. The battery must be replaced or reconnected and the factory setup for the encoder must be executed again. + + If this error occurs, verify that the battery voltage is adequate (typically 3.6V or above) and is connected to the encoder via the controller. Once the battery power has been restored, execute the factory calibration program to clear this latched error and reset the encoder's multiple turn counter.""", + }, + -3129: { + "text": "Invalid encoder multi-turn data", + "description": """During run-time, this error is triggered when an encoder's multi-turn counter either over-flows or under-flows or the encoder itself detects a read error of the multi-turn data. + + The over-flow or under-flow error should not occur so long as the encoder is not continuously turned in a single direction and the homing setup is done properly. + + If the error is generated when the controller is restarted or during homing, the error is most likely due to a read error and the encoder should be power cycled to clear this condition. + + For Panasonic and Tamagawa serial absolute encoders, this error is generated when the "Multi-turn Counter Overflow" bit (bit 3) and/or "Multi-turn read error" bit (bit 5) is set in the "Encoder alarm" field (DataID 12251). See the Panasonic/Tamagawa encoder alarm documentation for more details. + + This is a standard error and requires the encoder to be re-initialization and/or power cycled""", + }, + -3130: { + "text": "Illegal encoder operation mode", + "description": """During normal run-time operation, serial absolute and serial incremental encoders should be in synchronous communication mode. If an encoder is not in this mode when motor power is enabled, this error is issued. Normally, this error should not be generated. + + If this error persists, use the Absolute Encoder Diagnostics page in the web interface to re-initialize the serial encoder or power cycle the encoder.""", + }, + -3131: { + "text": "Encoder not supported or mis-matched", + "description": """The parameter "Encoder type" (DataID 10027) is set to a value not supported by the servos or that is inconsistent with the ID code returned by a serial encoder. This can occur if: + + The Encode type is not set to the proper value + When the controller first communicates with the encoder, the encoder communication is corrupted and the wrong encoder ID is received + For bus line absolute encoders, one or more of the encoders did not properly boot and the encoder message received by the controller did not include the response from all of the expected encoders + + This error is fatal and prevents robot power from being turned on until the controller is rebooted.""", + }, + -3132: { + "text": "Trajectory extrapolation limit exceeded", + "description": """This error occurs in systems with slave controllers or GSB boards that are networked to a master controller via Ethernet or RS485. If a set point transmission is lost, the servo code on the slave controller or GSB extrapolates from the previous trajectory set points. If the number of sequential missed set points exceeds the "Number of consecutive extrapolations allowed" (DataID 10424), the servo generates this severe level error and disables motor power. + + If this error occurs, verify that the value of DataID 10424 is not set too low. Typically, this value should be set to 8. + + If this error is generated by an Ethernet slave controller, there is probably noise in the Ethernet network. Ensure that shielded twisted pair Ethernet cables are used to interconnect the master and slave controllers and in any other part of the network that could inject noise. + + If this error is generated by a GSB slave, there is probably noise on the RS485 bus. Verify that the termination jumpers are installed correctly and that the RS485 connectors are firmly seated on all boards, and ensure that shielded twisted pair wire is used for all of the RS485 signals. View the GSB error statistics to identify what GSB board is experiencing noise. + + To minimize noise generation in a custom high voltage robot mechanism, verify that the recommended ferrite beads are installed on all motor power wires and that the communication cables are not routed next to the high voltage signal lines. + + If this error is generated by servos on the master controller, or in systems that are not part of a servo network, it indicates that the controller's CPU is overloaded. If the problem persists, please contact Precise technical support.""", + }, + -3133: { + "text": "Amplifier fault, DC bus stuck", + "description": "This error is generated if an amplifier is in a fault state and the user tries to re-enable motor power while the DC bus voltage is still not below 18VDC. If an amplifier fault has occurred, the high power relay to the motor power supply must be disengaged before motor power is re-enabled.", + }, + -3134: { + "text": "Encoder data or accel/decel limit error", + "description": """This error indicates that either invalid position data has been received from a serial encoder or a serial encoder's position reading changed by too large of a value in a very short period of time. Most often, this means that six or more consecutive "Absolute encoder bad readings" (DataID 12269) or "Absolute encoder communication errors" (DataID 12259) have occurred. If the controller detects a single error of these types, it will usually automatically correct the error and continue normal operation. + + When this error code is generated, the robot must be homed again to re-sample the encoder's full absolute position information. Also, the encoder may be disabled. To restart the encoder's operation, use the "Reset" function in the "Absolute Encoder Diagnostics" page of the web interface. + + If the mechanism is expected to operate at very high accelerations and decelerations, increase the limit used to detect bad encoder readings by reducing the value defined by the "Min. accel time to 5000 RPM, msec" DataID 10252. + + If DataID 10252 is properly set for the expected operation of the axis and the axis is not moving exceeding fast and this problem persists, it typically indicates that there is a hardware problem. The possible hardware defects (starting with the most likely cause) are: a poor connection in an encoder cable (please ensure all contacts are high compression with gold plating); a damaged encoder cable; electronic noise in the encoder cable; a defective encoder; a defective controller.""", + }, + -3135: { + "text": "Phase offset too large", + "description": "The detected amplifier phase offset is too large to be corrected automatically. To perform phase offset correction manually, disable the automatic phase offset adjustment using DataID 10695.", + }, + -3136: { + "text": "Excessive movement during phase offset adjustment", + "description": "Automatic amplifier phase offset correction (DataID 10695) can only be performed on Precise integrated amplifiers and when the motor is not in motion. If this error continues to persist even when the motor is stopped and you have Precise amplifiers, disable this adjustment by setting DataID 10695 to 1 and contact Precise support.", + }, + -3137: { + "text": "Amplifier hardware failure or invalid configuration", + "description": """This error will be reported in the following situations: + + Amplifiers are being enabled and the controller does not have any integrated amplifiers but a Precise amplifier has been specified in the configuration database. + The motor DC bus voltage is too high or low for the configured Precise amplifiers. Most likely, this indicates a software configuration error.""", + }, + -3138: { + "text": "Encoder position not ready", + "description": """The accuracy of the absolute encoder position data or the single turn position data of a serial encoder is reduced due to excessive rotational speed when the controller is turned on. + + For Panasonic and Tamagawa serial encoders, this error is generated when the "Reduced encoder resolution" (bit 1) is set in the "Encoder alarm" field (DataID 12251). See the Panasonic/Tamagawa encoder alarm documentation for more details. + + This alarm bit will be reset automatically by the encoder. However it is sometimes necessary to perform a re-initialization to clear the alarm condition.""", + }, + -3139: { + "text": "Encoder not ready", + "description": """A serial encoder is not ready to operate. This error is generated in the following situations: + + The configuration parameter specifies a serial encoder but the encoder is not physically connected to the controller. + The servo code failed to initialize the serial encoder. + The servo detects the serial encoder model (Panasonic and Tamagawa only), but the model is different from the specified "Encoder type" (DataID 10027). + An attempt was made to enable motor power after a serial encoder communication error (-3140) occurred without re-initialize the encoder. + If a serial encoder is successfully initialized and is operated normally, the "Serial encoder ready" (bit 2) of "Encoder software status word" (DataID 12200) will be set to 1.""", + }, + -3140: { + "text": "Encoder communication error", + "description": """The controller hardware has failed to establish communication with a serial encoder or communication was lost after the encoder was initialized. This typically indicates that the FPGA firmware has timed out while waiting for a communication packet from the encoder. The servo code may also issue an "Encoder not ready" error (-3139) depending upon the failure situation. + + In addition to issuing this error, the "Serial encoder communication error" bit (bit 26) of the "Encoder software status word" (DataID 12200) will be set to 1. If the communication error is detected during the normal synchronized operation, the "Serial encoder ready" bit (bit 2) of the status word will be set to 0. + + This is a severe error and requires the encoder to be re-initialization and/or power cycled.""", + }, + -3141: { + "text": "Encoder overheated", + "description": """A serial encoder has overheated. This error is generated in the following situations. + + For a Yaskawa serial encoder, this error is generated when the "Overheat" bit (bit 5) is set in the "Encoder alarm" field (DataID 12251). See the Yaskawa encoder alarm documentation for more details. + + This is a severe error and requires the encoder to be re-initialization and/or power cycled.""", + }, + -3142: { + "text": "Encoder hall sensor error", + "description": """A serial incremental encoder has detected an error in its hall effect data. + + For a Panasonic serial incremental encoder (type 41), this error is generated when the "Count error between phase" bit (bit 4) and/or the "Illegal Hall Data bit (bit 6) is set in the "Encoder alarm" field (DataID 12251). + This is a severe error and requires the encoder to be re-initialization and/or power cycled.""", + }, + -3143: { + "text": "General serial bus encoder error", + "description": """This error is generated by some serial encoders that pass back a limited amount of status information while the robot is running. When this error occurs, it means that an encoder has signaled an error, but no specific information about the nature of the error is known. + + It may be possible to obtain more information on the error if you cycle the AC power on the controller. For example, if this error was due to a low battery voltage condition, cycling AC power and re-homing the robot will generate an "Encoder battery low" (-3127) error. + + To reset this error, go to the Absolute Encoder maintenance panel in the Settings section of the controller's web interface. If the encoder error persists after the encoder is reset, the Operator Control Panel will display the detailed error information. + + + Currently, this error only occurs with the daisy-chained serial bus Panasonic encoders that are utilized in the Denso line of robots.""", + }, + -3144: { + "text": "Amplifier overheating", + "description": 'The indicated power amplifier has exceeded its permitted operating temperature. Normally this error is followed by the generic error -1610 "Controller overheating". For amplifiers of the G3xxx or G1xxxA/B controllers (except for the G3x3xA 30A version), the limit is 80C. For the amplifiers of the G3x3xA 30A version, the limit is 100C. See Amplifier temperature (DataID 12605) for the actual amplifier temperatures. The G2xxx controller power amplifier chips are equipped with internal temperature monitoring to protect the power modules, but they do not have temperature sensors that can be read.', + }, + -3145: { + "text": "Motor overheating", + "description": 'The indicated motor has exceeded its permitted operating temperature. Normally this error is followed by the generic error -1610 "Controller overheating". The parameter "Max motor temperature" (DataID 10110) determines the maximum allowed temperature. See "Motor temperature" (DataID 12110) for the actual motor temperature. Motor temperature monitoring is configurable and requires special temperature sensors in the motors and special signal conditioning electronics.', + }, + -3146: { + "text": "Earlier encoder error inhibiting power", + "description": "An attempt to enable motor power failed because a severe encoder error previously occurred. Consequently, the encoder is not operational and permitting motor power to be enabled could result in the motor being unstable. Typically, the encoder in question is a serial absolute or incremental type and is either not communicating properly or must be reset. Please go to the following web page to view the serial encoder status and to clear any error conditions: Setup > Hardware Tuning and Diagnostics > Absolute Encoder.", + }, + -3147: { + "text": "Abnormal envelope error", + "description": """This error indicates a more severe instance of the condition signaled by the "Hard envelope error" (-3100). Like the "Hard" error, this error indicates that there was a significant difference betortween an axis' commanded position and its actual position. And, like the "Hard" error, this error is generated when the value of the "Position tracking error, mcnt" (DataID 12320) for an axis exceeds its "Hard envelope error limit, mcnt" (DataID 10303) for a sufficient period of time. + + + However, the "Abnormal" error is generated in place of the "Hard" error when the command velocity is lower than 33% of the Manual mode speed limit (DataID 10209). So, the "Abnormal" error indicates that a significant position error occurred when an axis was not supposed to be moving fast. This error normally indicates a collision occurred when the robot was moving at a slow speed or one of the following motor configuration parameters are incorrect. + + Encoder sign (DataID 10202) + Torque sign (DataID 10609) + Commutation sign (DataID 10650) + Hard envelope error (DataID 10303) + Commutation offset (DataID 10775) + Commutation position at zero index (16653)""", + }, + -3148: { + "text": "Encoder hardware related warning", + "description": "This indicates that an internal encoder warning bit is ON. Currently this warning is only reported by BiSS encoders. Please consult the documentation for the specific encoder model to determine the nature of the error.\n\nThis is only a warning message. This error will not stop program execution or turn Off robot/motor power.", + }, + -3149: { + "text": "Velocity restrict limit exceeded", + "description": """(CAT-3, GPL 4.2 or later) The low-level control that advances the commutation angle for a BLDC motor has detected that the motor is attempting to turn faster than the Run-time or Manual mode speed limits (DataIDs 10208/10209) permit. The motor is immediately shut down and an error is signaled. + + Since a BLDC motor cannot generate torque if the commutation angle is not properly set and cannot rotate unless the commutation angle is properly advanced, this check provides a low level independent test to ensure that a motor is not rotating faster than permitted. This is sometimes referred to as an independent "Velocity Restrict" test. + + This error will only be reported in controllers with FPGA firmware that supports CAT-3 safety capability, and the CAT-3 safety feature is enabled in software. + + This error may indicate that: + + DataIDs 10208/10209 are set too low + An application program is trying to drive the motor faster than allowed + An encoder read error has occurred due to noise on the encoder lines or bad data has been sent by an encoder. For non-CAT-3 systems, these types of errors typically generate "Encoder data or accel/decel limit errors" (-3134) or "Encoder operation errors" (-3126) or "Encoder communication errors" (-3140). + A system hardware failure or a system software error has occurred that attempted to drive the motor faster than allowed.""", + }, + -3150: {"text": "Position compare not enabled", "description": ""}, + -3151: {"text": "Position compare memory allocation failed", "description": ""}, + -3152: {"text": "Position compare buffer empty", "description": ""}, + -3153: {"text": "Failed to start position compare task", "description": ""}, + -3154: {"text": "Position compare buffer full", "description": ""}, + -3155: {"text": "Invalid DOUT for position compare", "description": ""}, + -3156: {"text": "Motor moving away from compare position", "description": ""}, + -3157: {"text": "Position compare internal inconsistence error", "description": ""}, + -3160: { + "text": "Dump circuit duty cycle exceeded", + "description": "The motor power dump circuit has been turn on too long. The dump circuit has been switched off to avoid overheating the dump resistor.\n\nNOTE: If you load GPL versions 4.1J1 and later or 4.2E and later into a PreciseFlex™400 Rev B or earlier robot, this error may be erroneously generated. If this occurs, loading GPL 4.2H or later will properly detect the dump board in the robot and eliminate this error.", + }, + -3161: {"text": "No position updated in Fpga", "description": ""}, + -4000: { + "text": "Cannot connect to vision server", + "description": 'GPL cannot establish an Ethernet TCP connection with the Precise Vision software on the vision host PC. If the web interface is not working, check the basic Ethernet connectivity. In addition, verify that the "Vision Server IP address" (DataID 424) is set properly for your vision host PC. Make sure that Precise Vision is active on that PC.', + }, + -4001: { + "text": "Invalid vision protocol", + "description": "GPL is unable to decode a message received from PreciseVision. Verify that the versions of GPL and PreciseVision are compatible. If so, please report this error to customer service.", + }, + -4002: { + "text": "Vision interlocked", + "description": "The communications link to PreciseVision is being used by a different GPL thread. Only one thread may access vision at a time. Stop any other threads that may be accessing PreciseVision.", + }, + -4003: {"text": "Vision interlocked", "description": ""}, + -4010: { + "text": "Vision invalid protocol", + "description": "PreciseVision is unable to decode a message received from GPL. Verify that the versions of GPL and PreciseVision are compatible. If so, please report this error to customer service.", + }, + -4011: { + "text": "Vision internal error", + "description": "An unexpected error has occurred within PreciseVision. Please report this error to customer service", + }, + -4012: { + "text": "Vision unknown process", + "description": "A GPL vision method has specified a vision process that is not defined in the current PreciseVision project. Verify that the correct GPL project and PreciseVision project are loaded.", + }, + -4013: { + "text": "Vision unknown tool", + "description": "A GPL vision method has specified a vision tool that is not defined in the current PreciseVision project. Verify that the correct GPL project and PreciseVision project are loaded.", + }, + -4014: { + "text": "Vision invalid index", + "description": "A GPL vision Result method has specified an index for a result that does not exist. Verify that the correct GPL project and PreciseVision project are loaded. Verify the number of results returned by the current vision process.", + }, + -4016: { + "text": "Vision invalid arguments", + "description": "The argument values specified in a vision ToolProperty method are not valid for that property.", + }, + -4017: { + "text": "Vision property not found", + "description": "The property name specified in a vision ToolProperty method does not exist. Verify that the correct GPL project and PreciseVision project are loaded.", + }, + -4018: { + "text": "Vision property protected", + "description": "An attempt has been made to change a vision tool property than cannot be modified.", + }, + -4019: { + "text": "Vision process failed", + "description": "Execution of a selected process within PreciseVision has failed. This is normally due to the acquisition operation failing.", + }, + -4020: { + "text": "Vision invalid calibration type", + "description": "A remote request to execute a vision calibration procedure failed because an invalid calibration type was specified. Set the calibration type to a valid type and try again.", + }, + -4021: { + "text": "Vision invalid project name", + "description": "A remote request to load a vision project failed. Probably the project name was not valid. Verify that the name specified is correct and that the file exists in the correct location.", + }, + -4022: { + "text": "Vision calibration file not found", + "description": "A remote request to load a vision calibration file failed because the calibration file could not be found. Verify that the file name specified is correct and that the file exists in the correct location.", + }, + -4023: { + "text": "Vision project not saved", + "description": "A remote request to load a new vision project has failed because the current project has not been saved. Save the current project before attempting to load a new one.", + }, +} diff --git a/pylabrobot/arms/precise_flex/pf_3400.py b/pylabrobot/arms/precise_flex/pf_3400.py new file mode 100644 index 00000000000..0bbc782e4c2 --- /dev/null +++ b/pylabrobot/arms/precise_flex/pf_3400.py @@ -0,0 +1,8 @@ +from pylabrobot.arms.precise_flex.precise_flex_backend import PreciseFlexBackend + + +class PreciseFlex400Backend(PreciseFlexBackend): + """Backend for the PreciseFlex 400 robotic arm.""" + + def __init__(self, host: str, port: int = 10100, timeout=20) -> None: + super().__init__("pf3400", host, port, timeout) diff --git a/pylabrobot/arms/precise_flex/pf_400.py b/pylabrobot/arms/precise_flex/pf_400.py new file mode 100644 index 00000000000..adfd4445825 --- /dev/null +++ b/pylabrobot/arms/precise_flex/pf_400.py @@ -0,0 +1,8 @@ +from pylabrobot.arms.precise_flex.precise_flex_backend import PreciseFlexBackend + + +class PreciseFlex400Backend(PreciseFlexBackend): + """Backend for the PreciseFlex 400 robotic arm.""" + + def __init__(self, host: str, port: int = 10100, timeout=20) -> None: + super().__init__("pf400", host, port, timeout) diff --git a/pylabrobot/arms/precise_flex/precise_flex_api.py b/pylabrobot/arms/precise_flex/precise_flex_api.py new file mode 100644 index 00000000000..094e41ccd02 --- /dev/null +++ b/pylabrobot/arms/precise_flex/precise_flex_api.py @@ -0,0 +1,2055 @@ +import asyncio +from typing import Optional + +from pylabrobot.arms.precise_flex.error_codes import ERROR_CODES +from pylabrobot.io.tcp import TCP + + +class PreciseFlexError(Exception): + def __init__(self, replycode: int, message: str): + self.replycode = replycode + self.message = message + + # Map error codes to text and descriptions + error_info = ERROR_CODES + if replycode in error_info: + text = error_info[replycode]["text"] + description = error_info[replycode]["description"] + super().__init__(f"PreciseFlexError {replycode}: {text}. {description} - {message}") + else: + super().__init__(f"PreciseFlexError {replycode}: {message}") + + +class PreciseFlexBackendApi: + """ + API for interacting with the PreciseFlex robot. + Documentation and error codes available at https://www2.brooksautomation.com/#Root/Welcome.htm + """ + + def __init__(self, host: str = "192.168.0.1", port: int = 10100, timeout=20) -> None: + super().__init__() + self.host = host + self.port = port + self.timeout = timeout + self.io = TCP(host=self.host, port=self.port) + + async def setup(self): + """Connect to the PreciseFlex backend.""" + await self.io.setup() + + async def stop(self): + """Disconnect from the PreciseFlex backend.""" + await self.io.stop() + + # region GENERAL COMMANDS + async def attach(self, attach_state: Optional[int] = None) -> int: + """Attach or release the robot, or get attachment state. + + Parameters: + attach_state (int, optional): If omitted, returns the attachment state. + 0 = release the robot + 1 = attach the robot + + Returns: + int: If attach_state is omitted, returns 0 if robot is not attached, -1 if attached. + Otherwise returns 0 on success. + + Note: + The robot must be attached to allow motion commands. + """ + if attach_state is None: + response = await self.send_command("attach") + return int(response) + else: + await self.send_command(f"attach {attach_state}") + return 0 + + async def get_base(self) -> tuple[float, float, float, float]: + """Get the robot base offset. + + Returns: + tuple: A tuple containing (x_offset, y_offset, z_offset, z_rotation) + """ + data = await self.send_command("base") + parts = data.split() + if len(parts) != 4: + raise PreciseFlexError(-1, "Unexpected response format from base command.") + + x_offset = float(parts[0]) + y_offset = float(parts[1]) + z_offset = float(parts[2]) + z_rotation = float(parts[3]) + + return (x_offset, y_offset, z_offset, z_rotation) + + async def set_base( + self, x_offset: float, y_offset: float, z_offset: float, z_rotation: float + ) -> None: + """Set the robot base offset. + + Parameters: + x_offset (float): Base X offset + y_offset (float): Base Y offset + z_offset (float): Base Z offset + z_rotation (float): Base Z rotation + + Note: + The robot must be attached to set the base. + Setting the base pauses any robot motion in progress. + """ + await self.send_command(f"base {x_offset} {y_offset} {z_offset} {z_rotation}") + + async def exit(self) -> None: + """Close the communications link immediately. + + Note: + Does not affect any robots that may be active. + """ + await self.io.write("exit".encode("utf-8") + b"\n") + + async def home(self) -> None: + """Home the robot associated with this thread. + + Note: + Requires power to be enabled. + Requires robot to be attached. + Waits until the homing is complete. + """ + await self.send_command("home") + + async def home_all(self) -> None: + """Home all robots. + + Note: + Requires power to be enabled. + Requires that robots not be attached. + """ + await self.send_command("homeAll") + + async def get_power_state(self) -> int: + """Get the current robot power state. + + Returns: + int: Current power state (0 = disabled, 1 = enabled) + """ + response = await self.send_command("hp") + return int(response) + + async def set_power(self, enable: bool, timeout: int = 0) -> None: + """Enable or disable robot high power. + + Parameters: + enable (bool): True to enable power, False to disable + timeout (int, optional): Wait timeout for power to come on. + 0 or omitted = do not wait for power to come on + > 0 = wait this many seconds for power to come on + -1 = wait indefinitely for power to come on + + Raises: + PreciseFlexError: If power does not come on within the specified timeout. + """ + power_state = 1 if enable else 0 + if timeout == 0: + await self.send_command(f"hp {power_state}") + else: + await self.send_command(f"hp {power_state} {timeout}") + + async def get_mode(self) -> int: + """Get the current response mode. + + Returns: + int: Current mode (0 = PC mode, 1 = verbose mode) + """ + response = await self.send_command("mode") + return int(response) + + async def set_mode(self, mode: int) -> None: + """Set the response mode. + + Parameters: + mode (int): Response mode to set. + 0 = Select PC mode + 1 = Select verbose mode + + Note: + When using serial communications, the mode change does not take effect + until one additional command has been processed. + """ + if mode not in [0, 1]: + raise ValueError("Mode must be 0 (PC mode) or 1 (verbose mode)") + await self.send_command(f"mode {mode}") + + async def get_monitor_speed(self) -> int: + """Get the global system (monitor) speed. + + Returns: + int: Current monitor speed as a percentage (1-100) + """ + response = await self.send_command("mspeed") + return int(response) + + async def set_monitor_speed(self, speed_percent: int) -> None: + """Set the global system (monitor) speed. + + Parameters: + speed_percent (int): Speed percentage between 1 and 100, where 100 means full speed. + + Raises: + ValueError: If speed_percent is not between 1 and 100. + """ + if not (1 <= speed_percent <= 100): + raise ValueError("Speed percent must be between 1 and 100") + await self.send_command(f"mspeed {speed_percent}") + + async def nop(self) -> None: + """No operation command. + + Does nothing except return the standard reply. Can be used to see if the link + is active or to check for exceptions. + """ + await self.send_command("nop") + + async def get_payload(self) -> int: + """Get the payload percent value for the current robot. + + Returns: + int: Current payload as a percentage of maximum (0-100) + """ + response = await self.send_command("payload") + return int(response) + + async def set_payload(self, payload_percent: int) -> None: + """Set the payload percent of maximum for the currently selected or attached robot. + + Parameters: + payload_percent (int): Payload percentage from 0 to 100 indicating the percent + of the maximum payload the robot is carrying. + + Raises: + ValueError: If payload_percent is not between 0 and 100. + + Note: + If the robot is moving, waits for the robot to stop before setting a value. + """ + if not (0 <= payload_percent <= 100): + raise ValueError("Payload percent must be between 0 and 100") + await self.send_command(f"payload {payload_percent}") + + async def set_parameter( + self, + data_id: int, + value, + unit_number: Optional[int] = None, + sub_unit: Optional[int] = None, + array_index: Optional[int] = None, + ) -> None: + """Change a value in the controller's parameter database. + + Parameters: + data_id (int): DataID of parameter. + value: New parameter value. If string, will be quoted automatically. + unit_number (int, optional): Unit number, usually the robot number (1 - N_ROB). + sub_unit (int, optional): Sub-unit, usually 0. + array_index (int, optional): Array index. + + Note: + Updated values are not saved in flash unless a save-to-flash operation + is performed (see DataID 901). + """ + if unit_number is not None and sub_unit is not None and array_index is not None: + # 5 argument format + if isinstance(value, str): + await self.send_command(f'pc {data_id} {unit_number} {sub_unit} {array_index} "{value}"') + else: + await self.send_command(f"pc {data_id} {unit_number} {sub_unit} {array_index} {value}") + else: + # 2 argument format + if isinstance(value, str): + await self.send_command(f'pc {data_id} "{value}"') + else: + await self.send_command(f"pc {data_id} {value}") + + async def get_parameter( + self, + data_id: int, + unit_number: Optional[int] = None, + sub_unit: Optional[int] = None, + array_index: Optional[int] = None, + ) -> str: + """Get the value of a numeric parameter database item. + + Parameters: + data_id (int): DataID of parameter. + unit_number (int, optional): Unit number, usually the robot number (1-NROB). + sub_unit (int, optional): Sub-unit, usually 0. + array_index (int, optional): Array index. + + Returns: + str: The numeric value of the specified database parameter. + """ + if unit_number is not None: + if sub_unit is not None: + if array_index is not None: + response = await self.send_command(f"pd {data_id} {unit_number} {sub_unit} {array_index}") + else: + response = await self.send_command(f"pd {data_id} {unit_number} {sub_unit}") + else: + response = await self.send_command(f"pd {data_id} {unit_number}") + else: + response = await self.send_command(f"pd {data_id}") + return response + + async def reset(self, robot_number: int) -> None: + """Reset the threads associated with the specified robot. + + Stops and restarts the threads for the specified robot. Any TCP/IP connections + made by these threads are broken. This command can only be sent to the status thread. + + Parameters: + robot_number (int): The number of the robot thread to reset, from 1 to N_ROB. Must not be zero. + + Raises: + ValueError: If robot_number is zero or negative. + """ + if robot_number <= 0: + raise ValueError("Robot number must be greater than zero") + await self.send_command(f"reset {robot_number}") + + async def get_selected_robot(self) -> int: + """Get the number of the currently selected robot. + + Returns: + int: The number of the currently selected robot. + """ + response = await self.send_command("selectRobot") + return int(response) + + async def select_robot(self, robot_number: int) -> None: + """Change the robot associated with this communications link. + + Does not affect the operation or attachment state of the robot. The status thread + may select any robot or 0. Except for the status thread, a robot may only be + selected by one thread at a time. + + Parameters: + robot_number (int): The new robot to be connected to this thread (1 to N_ROB) or 0 for none. + """ + await self.send_command(f"selectRobot {robot_number}") + + async def get_signal(self, signal_number: int) -> int: + """Get the value of the specified digital input or output signal. + + Parameters: + signal_number (int): The number of the digital signal to get. + + Returns: + int: The current signal value. + """ + response = await self.send_command(f"sig {signal_number}") + sig_id, sig_val = response.split() + return int(sig_val) + + async def set_signal(self, signal_number: int, value: int) -> None: + """Set the specified digital input or output signal. + + Parameters: + signal_number (int): The number of the digital signal to set. + value (int): The signal value to set. 0 = off, non-zero = on. + """ + await self.send_command(f"sig {signal_number} {value}") + + async def get_system_state(self) -> int: + """Get the global system state code. + + Returns: + int: The global system state code. Please see documentation for DataID 234. + """ + response = await self.send_command("sysState") + return int(response) + + async def get_tool(self) -> tuple[float, float, float, float, float, float]: + """Get the current tool transformation values. + + Returns: + tuple: A tuple containing (X, Y, Z, yaw, pitch, roll) for the tool transformation. + """ + data = await self.send_command("tool") + # Remove "tool:" prefix if present + if data.startswith("tool: "): + data = data[6:] + + parts = data.split() + if len(parts) != 6: + raise PreciseFlexError(-1, "Unexpected response format from tool command.") + + x, y, z, yaw, pitch, roll = self._parse_xyz_response(parts) + + return (x, y, z, yaw, pitch, roll) + + async def set_tool( + self, x: float, y: float, z: float, yaw: float, pitch: float, roll: float + ) -> None: + """Set the robot tool transformation. + + The robot must be attached to set the tool. Setting the tool pauses any robot motion in progress. + + Parameters: + x (float): Tool X coordinate. + y (float): Tool Y coordinate. + z (float): Tool Z coordinate. + yaw (float): Tool yaw rotation. + pitch (float): Tool pitch rotation. + roll (float): Tool roll rotation. + """ + await self.send_command(f"tool {x} {y} {z} {yaw} {pitch} {roll}") + + async def get_version(self) -> str: + """Get the current version of TCS and any installed plug-ins. + + Returns: + str: The current version information. + """ + return await self.send_command("version") + + # region LOCATION COMMANDS + async def get_location( + self, location_index: int + ) -> tuple[int, int, float, float, float, float, float, float]: + """Get the location values for the specified station index. + + Parameters: + location_index (int): The station index, from 1 to N_LOC. + + Returns: + tuple: A tuple containing (type_code, station_index, val1, val2, val3, val4, val5, val6) + - For Cartesian type (type_code=0): (0, station_index, X, Y, Z, yaw, pitch, roll, unused = 0.0) + - For angles type (type_code=1): (1, station_index, angle1, angle2, angle3, angle4, angle5, angle6) - any unused angles are set to 0.0 + """ + data = await self.send_command(f"loc {location_index}") + parts = data.split(" ") + + type_code = int(parts[0]) + station_index = int(parts[1]) + + # location stored as cartesian + if type_code == 0: + x, y, z, yaw, pitch, roll = self._parse_xyz_response(parts[2:8]) + return (type_code, station_index, x, y, z, yaw, pitch, roll) + + # location stored as angles + if type_code == 1: + angle1, angle2, angle3, angle4, angle5, angle6 = self._parse_angles_response(parts[2:]) + return (type_code, station_index, angle1, angle2, angle3, angle4, angle5, angle6) + + raise PreciseFlexError(-1, "Unexpected response format from loc command.") + + async def get_location_angles( + self, location_index: int + ) -> tuple[int, int, float, float, float, float, float, float]: + """Get the angle values for the specified station index. + + Parameters: + location_index (int): The station index, from 1 to N_LOC. + + Returns: + tuple: A tuple containing (type_code, station_index, angle1, angle2, angle3, angle4, angle5, angle6) + + Raises: + PreciseFlexError: If attempting to get angles from a Cartesian location. + """ + data = await self.send_command(f"locAngles {location_index}") + parts = data.split(" ") + + type_code = int(parts[0]) + if type_code != 1: + raise PreciseFlexError(-1, "Location is not of angles type.") + + station_index = int(parts[1]) + angle1, angle2, angle3, angle4, angle5, angle6 = self._parse_angles_response(parts[2:]) + + return (type_code, station_index, angle1, angle2, angle3, angle4, angle5, angle6) + + async def set_location_angles( + self, + location_index: int, + angle1: float, + angle2: float = 0.0, + angle3: float = 0.0, + angle4: float = 0.0, + angle5: float = 0.0, + angle6: float = 0.0, + ) -> None: + """Set the angle values for the specified station index. + + Parameters: + location_index (int): The station index, from 1 to N_LOC. + angle1 (float): Axis 1 angle. + angle2 (float): Axis 2 angle. Defaults to 0.0. + angle3 (float): Axis 3 angle. Defaults to 0.0. + angle4 (float): Axis 4 angle. Defaults to 0.0. + angle5 (float): Axis 5 angle. Defaults to 0.0. + angle6 (float): Axis 6 angle. Defaults to 0.0. + + Note: + If more arguments than the number of axes for this robot, extra values are ignored. + If less arguments than the number of axes for this robot, missing values are assumed to be zero. + """ + await self.send_command( + f"locAngles {location_index} {angle1} {angle2} {angle3} {angle4} {angle5} {angle6}" + ) + + async def get_location_xyz( + self, location_index: int + ) -> tuple[int, int, float, float, float, float, float, float]: + """Get the Cartesian position values for the specified station index. + + Parameters: + location_index (int): The station index, from 1 to N_LOC. + + Returns: + tuple: A tuple containing (type_code, station_index, X, Y, Z, yaw, pitch, roll) + + Raises: + PreciseFlexError: If attempting to get Cartesian position from an angles type location. + """ + data = await self.send_command(f"locXyz {location_index}") + parts = data.split(" ") + + type_code = int(parts[0]) + if type_code != 0: + raise PreciseFlexError(-1, "Location is not of Cartesian type.") + + if len(parts) != 8: + raise PreciseFlexError(-1, "Unexpected response format from locXyz command.") + + station_index = int(parts[1]) + x, y, z, yaw, pitch, roll = self._parse_xyz_response(parts[2:8]) + + return (type_code, station_index, x, y, z, yaw, pitch, roll) + + async def set_location_xyz( + self, + location_index: int, + x: float, + y: float, + z: float, + yaw: float = 0.0, + pitch: float = 0.0, + roll: float = 0.0, + ) -> None: + """Set the Cartesian position values for the specified station index. + + Parameters: + location_index (int): The station index, from 1 to N_LOC. + x (float): X coordinate. + y (float): Y coordinate. + z (float): Z coordinate. + yaw (float): Yaw rotation. Defaults to 0.0. + pitch (float): Pitch rotation. Defaults to 0.0. + roll (float): Roll rotation. Defaults to 0.0. + """ + await self.send_command(f"locXyz {location_index} {x} {y} {z} {yaw} {pitch} {roll}") + + async def get_location_z_clearance(self, location_index: int) -> tuple[int, float, bool]: + """Get the ZClearance and ZWorld properties for the specified location. + + Parameters: + location_index (int): The station index, from 1 to N_LOC. + + Returns: + tuple: A tuple containing (station_index, z_clearance, z_world) + """ + data = await self.send_command(f"locZClearance {location_index}") + parts = data.split(" ") + + if len(parts) != 3: + raise PreciseFlexError(-1, "Unexpected response format from locZClearance command.") + + station_index = int(parts[0]) + z_clearance = float(parts[1]) + z_world = True if float(parts[2]) != 0 else False + + return (station_index, z_clearance, z_world) + + async def set_location_z_clearance( + self, location_index: int, z_clearance: float, z_world: Optional[bool] = None + ) -> None: + """Set the ZClearance and ZWorld properties for the specified location. + + Parameters: + location_index (int): The station index, from 1 to N_LOC. + z_clearance (float): The new ZClearance property value. + z_world (float, optional): The new ZWorld property value. If omitted, only ZClearance is set. + """ + if z_world is None: + await self.send_command(f"locZClearance {location_index} {z_clearance}") + else: + z_world_int = 1 if z_world else 0 + await self.send_command(f"locZClearance {location_index} {z_clearance} {z_world_int}") + + async def get_location_config(self, location_index: int) -> tuple[int, int]: + """Get the Config property for the specified location. + + Parameters: + location_index (int): The station index, from 1 to N_LOC. + + Returns: + tuple: A tuple containing (station_index, config_value) + config_value is a bit mask where: + - 0 = None (no configuration specified) + - 0x01 = GPL_Righty (right shouldered configuration) + - 0x02 = GPL_Lefty (left shouldered configuration) + - 0x04 = GPL_Above (elbow above the wrist) + - 0x08 = GPL_Below (elbow below the wrist) + - 0x10 = GPL_Flip (wrist pitched up) + - 0x20 = GPL_NoFlip (wrist pitched down) + - 0x1000 = GPL_Single (restrict wrist axis to +/- 180 degrees) + Values can be combined using bitwise OR. + """ + data = await self.send_command(f"locConfig {location_index}") + parts = data.split(" ") + + if len(parts) != 2: + raise PreciseFlexError(-1, "Unexpected response format from locConfig command.") + + station_index = int(parts[0]) + config_value = int(parts[1]) + + return (station_index, config_value) + + async def set_location_config(self, location_index: int, config_value: int) -> None: + """Set the Config property for the specified location. + + Parameters: + location_index (int): The station index, from 1 to N_LOC. + config_value (int): The new Config property value as a bit mask where: + - 0 = None (no configuration specified) + - 0x01 = GPL_Righty (right shouldered configuration) + - 0x02 = GPL_Lefty (left shouldered configuration) + - 0x04 = GPL_Above (elbow above the wrist) + - 0x08 = GPL_Below (elbow below the wrist) + - 0x10 = GPL_Flip (wrist pitched up) + - 0x20 = GPL_NoFlip (wrist pitched down) + - 0x1000 = GPL_Single (restrict wrist axis to +/- 180 degrees) + Values can be combined using bitwise OR. + + Raises: + ValueError: If config_value contains invalid bits or conflicting configurations. + """ + # Define valid bit masks + GPL_RIGHTY = 0x01 + GPL_LEFTY = 0x02 + GPL_ABOVE = 0x04 + GPL_BELOW = 0x08 + GPL_FLIP = 0x10 + GPL_NOFLIP = 0x20 + GPL_SINGLE = 0x1000 + + # All valid bits + ALL_VALID_BITS = ( + GPL_RIGHTY | GPL_LEFTY | GPL_ABOVE | GPL_BELOW | GPL_FLIP | GPL_NOFLIP | GPL_SINGLE + ) + + # Check for invalid bits + if config_value & ~ALL_VALID_BITS: + raise ValueError(f"Invalid config bits specified: 0x{config_value:X}") + + # Check for conflicting configurations + if (config_value & GPL_RIGHTY) and (config_value & GPL_LEFTY): + raise ValueError("Cannot specify both GPL_Righty and GPL_Lefty") + + if (config_value & GPL_ABOVE) and (config_value & GPL_BELOW): + raise ValueError("Cannot specify both GPL_Above and GPL_Below") + + if (config_value & GPL_FLIP) and (config_value & GPL_NOFLIP): + raise ValueError("Cannot specify both GPL_Flip and GPL_NoFlip") + + await self.send_command(f"locConfig {location_index} {config_value}") + + async def dest_c(self, arg1: int = 0) -> tuple[float, float, float, float, float, float, int]: + """Get the destination or current Cartesian location of the robot. + + Parameters: + arg1 (int, optional): Selects return value. Defaults to 0. + 0 = Return current Cartesian location if robot is not moving + 1 = Return target Cartesian location of the previous or current move + + Returns: + tuple: A tuple containing (X, Y, Z, yaw, pitch, roll, config) + If arg1 = 1 or robot is moving, returns the target location. + If arg1 = 0 and robot is not moving, returns the current location. + """ + if arg1 == 0: + data = await self.send_command("destC") + else: + data = await self.send_command(f"destC {arg1}") + + parts = data.split() + if len(parts) != 7: + raise PreciseFlexError(-1, "Unexpected response format from destC command.") + + x, y, z, yaw, pitch, roll = self._parse_xyz_response(parts[:6]) + config = int(parts[6]) + + return (x, y, z, yaw, pitch, roll, config) + + async def dest_j(self, arg1: int = 0) -> tuple[float, float, float, float, float, float]: + """Get the destination or current joint location of the robot. + + Parameters: + arg1 (int, optional): Selects return value. Defaults to 0. + 0 = Return current joint location if robot is not moving + 1 = Return target joint location of the previous or current move + + Returns: + list[float]: A list containing [axis1, axis2, ..., axisn] + If arg1 = 1 or robot is moving, returns the target joint positions. + If arg1 = 0 and robot is not moving, returns the current joint positions. + """ + if arg1 == 0: + data = await self.send_command("destJ") + else: + data = await self.send_command(f"destJ {arg1}") + + parts = data.split() + if not parts: + raise PreciseFlexError(-1, "Unexpected response format from destJ command.") + + # Ensure we have exactly 6 elements, padding with 0.0 if necessary + angle1, angle2, angle3, angle4, angle5, angle6 = self._parse_angles_response(parts) + + return (angle1, angle2, angle3, angle4, angle5, angle6) + + async def here_j(self, location_index: int) -> None: + """Record the current position of the selected robot into the specified Location as angles. + + The Location is automatically set to type "angles". + + Parameters: + location_index (int): The station index, from 1 to N_LOC. + """ + await self.send_command(f"hereJ {location_index}") + + async def here_c(self, location_index: int) -> None: + """Record the current position of the selected robot into the specified Location as Cartesian. + + The Location object is automatically set to type "Cartesian". + Can be used to change the pallet origin (index 1,1,1) value. + + Parameters: + location_index (int): The station index, from 1 to N_LOC. + """ + await self.send_command(f"hereC {location_index}") + + async def where(self) -> tuple[float, float, float, float, float, float, tuple[float, ...]]: + """Return the current position of the selected robot in both Cartesian and joints format. + + Returns: + tuple: A tuple containing (X, Y, Z, yaw, pitch, roll, (axis1, axis2, axis3, axis4, axis5, axis6)) + """ + data = await self.send_command("where") + parts = data.split() + + if len(parts) < 6: + # In case of incomplete response, wait for EOM and try to read again + await self.wait_for_eom() + data = await self.send_command("where") + parts = data.split() + if len(parts) < 6: + raise PreciseFlexError(-1, "Unexpected response format from where command.") + + x, y, z, yaw, pitch, roll = self._parse_xyz_response(parts[0:6]) + axes = self._parse_angles_response(parts[6:]) + + return (x, y, z, yaw, pitch, roll, axes) + + async def where_c(self) -> tuple[float, float, float, float, float, float, int]: + """Return the current Cartesian position and configuration of the selected robot. + + Returns: + tuple: A tuple containing (X, Y, Z, yaw, pitch, roll, config) + """ + data = await self.send_command("wherec") + parts = data.split() + + if len(parts) != 7: + # In case of incomplete response, wait for EOM and try to read again + await self.wait_for_eom() + data = await self.send_command("wherec") + parts = data.split() + if len(parts) != 7: + raise PreciseFlexError(-1, "Unexpected response format from wherec command.") + + x, y, z, yaw, pitch, roll = self._parse_xyz_response(parts[0:6]) + config = int(parts[6]) + + return (x, y, z, yaw, pitch, roll, config) + + async def where_j(self) -> tuple[float, float, float, float, float, float]: + """Return the current joint position for the selected robot. + + Returns: + tuple: A tuple containing (axis1, axis2, axis3, axis4, axis5, axis6) + """ + data = await self.send_command("wherej") + parts = data.split() + + if not parts: + # In case of incomplete response, wait for EOM and try to read again + await self.wait_for_eom() + data = await self.send_command("wherej") + parts = data.split() + if not parts: + raise PreciseFlexError(-1, "Unexpected response format from wherej command.") + + axes = self._parse_angles_response(parts) + return axes + + # region PROFILE COMMANDS + + async def get_profile_speed(self, profile_index: int) -> float: + """Get the speed property of the specified profile. + + Parameters: + profile_index (int): The profile index to query. + + Returns: + float: The current speed as a percentage. 100 = full speed. + """ + response = await self.send_command(f"Speed {profile_index}") + profile, speed = response.split() + return float(speed) + + async def set_profile_speed(self, profile_index: int, speed_percent: float) -> None: + """Set the speed property of the specified profile. + + Parameters: + profile_index (int): The profile index to modify. + speed_percent (float): The new speed as a percentage. 100 = full speed. + Values > 100 may be accepted depending on system configuration. + """ + await self.send_command(f"Speed {profile_index} {speed_percent}") + + async def get_profile_speed2(self, profile_index: int) -> float: + """Get the speed2 property of the specified profile. + + Parameters: + profile_index (int): The profile index to query. + + Returns: + float: The current speed2 as a percentage. Used for Cartesian moves. + """ + response = await self.send_command(f"Speed2 {profile_index}") + profile, speed2 = response.split() + return float(speed2) + + async def set_profile_speed2(self, profile_index: int, speed2_percent: float) -> None: + """Set the speed2 property of the specified profile. + + Parameters: + profile_index (int): The profile index to modify. + speed2_percent (float): The new speed2 as a percentage. 100 = full speed. + Used for Cartesian moves. Normally set to 0. + """ + await self.send_command(f"Speed2 {profile_index} {speed2_percent}") + + async def get_profile_accel(self, profile_index: int) -> float: + """Get the acceleration property of the specified profile. + + Parameters: + profile_index (int): The profile index to query. + + Returns: + float: The current acceleration as a percentage. 100 = maximum acceleration. + """ + response = await self.send_command(f"Accel {profile_index}") + profile, accel = response.split() + return float(accel) + + async def set_profile_accel(self, profile_index: int, accel_percent: float) -> None: + """Set the acceleration property of the specified profile. + + Parameters: + profile_index (int): The profile index to modify. + accel_percent (float): The new acceleration as a percentage. 100 = maximum acceleration. + Maximum value depends on system configuration. + """ + await self.send_command(f"Accel {profile_index} {accel_percent}") + + async def get_profile_accel_ramp(self, profile_index: int) -> float: + """Get the acceleration ramp property of the specified profile. + + Parameters: + profile_index (int): The profile index to query. + + Returns: + float: The current acceleration ramp time in seconds. + """ + response = await self.send_command(f"AccRamp {profile_index}") + profile, accel_ramp = response.split() + return float(accel_ramp) + + async def set_profile_accel_ramp(self, profile_index: int, accel_ramp_seconds: float) -> None: + """Set the acceleration ramp property of the specified profile. + + Parameters: + profile_index (int): The profile index to modify. + accel_ramp_seconds (float): The new acceleration ramp time in seconds. + """ + await self.send_command(f"AccRamp {profile_index} {accel_ramp_seconds}") + + async def get_profile_decel(self, profile_index: int) -> float: + """Get the deceleration property of the specified profile. + + Parameters: + profile_index (int): The profile index to query. + + Returns: + float: The current deceleration as a percentage. 100 = maximum deceleration. + """ + response = await self.send_command(f"Decel {profile_index}") + profile, decel = response.split() + return float(decel) + + async def set_profile_decel(self, profile_index: int, decel_percent: float) -> None: + """Set the deceleration property of the specified profile. + + Parameters: + profile_index (int): The profile index to modify. + decel_percent (float): The new deceleration as a percentage. 100 = maximum deceleration. + Maximum value depends on system configuration. + """ + await self.send_command(f"Decel {profile_index} {decel_percent}") + + async def get_profile_decel_ramp(self, profile_index: int) -> float: + """Get the deceleration ramp property of the specified profile. + + Parameters: + profile_index (int): The profile index to query. + + Returns: + float: The current deceleration ramp time in seconds. + """ + response = await self.send_command(f"DecRamp {profile_index}") + profile, decel_ramp = response.split() + return float(decel_ramp) + + async def set_profile_decel_ramp(self, profile_index: int, decel_ramp_seconds: float) -> None: + """Set the deceleration ramp property of the specified profile. + + Parameters: + profile_index (int): The profile index to modify. + decel_ramp_seconds (float): The new deceleration ramp time in seconds. + """ + await self.send_command(f"DecRamp {profile_index} {decel_ramp_seconds}") + + async def get_profile_in_range(self, profile_index: int) -> float: + """Get the InRange property of the specified profile. + + Parameters: + profile_index (int): The profile index to query. + + Returns: + float: The current InRange value (-1 to 100). + -1 = do not stop at end of motion if blending is possible + 0 = always stop but do not check end point error + > 0 = wait until close to end point (larger numbers mean less position error allowed) + """ + response = await self.send_command(f"InRange {profile_index}") + profile, in_range = response.split() + return float(in_range) + + async def set_profile_in_range(self, profile_index: int, in_range_value: float) -> None: + """Set the InRange property of the specified profile. + + Parameters: + profile_index (int): The profile index to modify. + in_range_value (float): The new InRange value from -1 to 100. + -1 = do not stop at end of motion if blending is possible + 0 = always stop but do not check end point error + > 0 = wait until close to end point (larger numbers mean less position error allowed) + + Raises: + ValueError: If in_range_value is not between -1 and 100. + """ + if not (-1 <= in_range_value <= 100): + raise ValueError("InRange value must be between -1 and 100") + await self.send_command(f"InRange {profile_index} {in_range_value}") + + async def get_profile_straight(self, profile_index: int) -> bool: + """Get the Straight property of the specified profile. + + Parameters: + profile_index (int): The profile index to query. + + Returns: + int: The current Straight property value. + True = follow a straight-line path + False = follow a joint-based path (coordinated axes movement) + """ + response = await self.send_command(f"Straight {profile_index}") + profile, straight = response.split() + + return True if straight == "True" else False + + async def set_profile_straight(self, profile_index: int, straight_mode: bool) -> None: + """Set the Straight property of the specified profile. + + Parameters: + profile_index (int): The profile index to modify. + straight_mode (bool): The path type to use. + True = follow a straight-line path + False = follow a joint-based path (robot axes move in coordinated manner) + + Raises: + ValueError: If straight_mode is not True or False. + """ + straight_int = 1 if straight_mode else 0 + await self.send_command(f"Straight {profile_index} {straight_int}") + + async def set_motion_profile_values( + self, + profile: int, + speed: float, + speed2: float, + acceleration: float, + deceleration: float, + acceleration_ramp: float, + deceleration_ramp: float, + in_range: float, + straight: bool, + ): + """ + Set motion profile values for the specified profile index on the PreciseFlex robot. + + Parameters: + profile (int): Profile index to set values for. + speed (float): Percentage of maximum speed. 100 = full speed. Values >100 may be accepted depending on system config. + speed2 (float): Secondary speed setting, typically for Cartesian moves. Normally 0. Interpreted as a percentage. + acceleration (float): Percentage of maximum acceleration. 100 = full accel. + deceleration (float): Percentage of maximum deceleration. 100 = full decel. + acceleration_ramp (float): Acceleration ramp time in seconds. + deceleration_ramp (float): Deceleration ramp time in seconds. + in_range (float): InRange value, from -1 to 100. -1 = allow blending, 0 = stop without checking, >0 = enforce position accuracy. + straight (bool): If True, follow a straight-line path (-1). If False, follow a joint-based path (0). + """ + if not (0 <= speed): + raise ValueError("Speed must be >= 0 (percent).") + + if not (0 <= speed2): + raise ValueError("Speed2 must be >= 0 (percent).") + + if not (0 <= acceleration <= 100): + raise ValueError("Acceleration must be between 0 and 100 (percent).") + + if not (0 <= deceleration <= 100): + raise ValueError("Deceleration must be between 0 and 100 (percent).") + + if acceleration_ramp < 0: + raise ValueError("Acceleration ramp must be >= 0 (seconds).") + + if deceleration_ramp < 0: + raise ValueError("Deceleration ramp must be >= 0 (seconds).") + + if not (-1 <= in_range <= 100): + raise ValueError("InRange must be between -1 and 100.") + + straight_int = -1 if straight else 0 + await self.send_command( + f"Profile {profile} {speed} {speed2} {acceleration} {deceleration} {acceleration_ramp} {deceleration_ramp} {in_range} {straight_int}" + ) + + async def get_motion_profile_values( + self, profile: int + ) -> tuple[int, float, float, float, float, float, float, float, bool]: + """ + Get the current motion profile values for the specified profile index on the PreciseFlex robot. + + Parameters: + profile (int): Profile index to get values for. + + Returns: + tuple: A tuple containing (profile, speed, speed2, acceleration, deceleration, acceleration_ramp, deceleration_ramp, in_range, straight) + - profile (int): Profile index + - speed (float): Percentage of maximum speed + - speed2 (float): Secondary speed setting + - acceleration (float): Percentage of maximum acceleration + - deceleration (float): Percentage of maximum deceleration + - acceleration_ramp (float): Acceleration ramp time in seconds + - deceleration_ramp (float): Deceleration ramp time in seconds + - in_range (float): InRange value (-1 to 100) + - straight (bool): True if straight-line path, False if joint-based path + """ + data = await self.send_command(f"Profile {profile}") + parts = data.split(" ") + if len(parts) != 9: + raise PreciseFlexError(-1, "Unexpected response format from device.") + + return ( + int(parts[0]), + float(parts[1]), + float(parts[2]), + float(parts[3]), + float(parts[4]), + float(parts[5]), + float(parts[6]), + float(parts[7]), + False if int(parts[8]) == 0 else True, + ) + + # region MOTION COMMANDS + async def halt(self): + """Stops the current robot immediately but leaves power on.""" + await self.send_command("halt") + + async def move(self, location_index: int, profile_index: int) -> None: + """Move to the location specified by the station index using the specified profile. + + Parameters: + location_index (int): The index of the location to which the robot moves. + profile_index (int): The profile index for this move. + + Note: + Requires that the robot be attached. + """ + await self.send_command(f"move {location_index} {profile_index}") + + async def move_appro(self, location_index: int, profile_index: int) -> None: + """Approach the location specified by the station index using the specified profile. + + This is similar to "move" except that the Z clearance value is included. + + Parameters: + location_index (int): The index of the location to which the robot moves. + profile_index (int): The profile index for this move. + + Note: + Requires that the robot be attached. + """ + await self.send_command(f"moveAppro {location_index} {profile_index}") + + async def move_extra_axis( + self, axis1_position: float, axis2_position: Optional[float] = None + ) -> None: + """Post a move for one or two extra axes during the next Cartesian motion. + + Does not cause the robot to move at this time. Only some kinematic modules support extra axes. + + Parameters: + axis1_position (float): The destination position for the 1st extra axis. + axis2_position (float, optional): The destination position for the 2nd extra axis, if any. + + Note: + Requires that the robot be attached. + """ + if axis2_position is None: + await self.send_command(f"moveExtraAxis {axis1_position}") + else: + await self.send_command(f"moveExtraAxis {axis1_position} {axis2_position}") + + async def move_one_axis( + self, axis_number: int, destination_position: float, profile_index: int + ) -> None: + """Move a single axis to the specified position using the specified profile. + + Parameters: + axis_number (int): The number of the axis to move. + destination_position (float): The destination position for this axis. + profile_index (int): The index of the profile to use during this motion. + + Note: + Requires that the robot be attached. + """ + await self.send_command(f"moveOneAxis {axis_number} {destination_position} {profile_index}") + + async def move_c( + self, + profile_index: int, + x: float, + y: float, + z: float, + yaw: float, + pitch: float, + roll: float, + config: Optional[int] = None, + ) -> None: + """Move the robot to the Cartesian location specified by the arguments. + + Parameters: + profile_index (int): The profile index to use for this motion. + x (float): X coordinate. + y (float): Y coordinate. + z (float): Z coordinate. + yaw (float): Yaw rotation. + pitch (float): Pitch rotation. + roll (float): Roll rotation. + config (int, optional): If specified, sets the Config property for the location. + + Note: + Requires that the robot be attached. + """ + if config is None: + await self.send_command(f"moveC {profile_index} {x} {y} {z} {yaw} {pitch} {roll}") + else: + await self.send_command(f"moveC {profile_index} {x} {y} {z} {yaw} {pitch} {roll} {config}") + + async def move_j(self, profile_index: int, *joint_angles: float) -> None: + """Move the robot to the angles location specified by the arguments. + + Parameters: + profile_index (int): The profile index to use for this motion. + *joint_angles (float): The joint angles specifying where the robot should move. + The number of arguments must match the number of axes in the robot. + + Note: + Requires that the robot be attached. + """ + angles_str = " ".join(str(angle) for angle in joint_angles) + await self.send_command(f"moveJ {profile_index} {angles_str}") + + async def release_brake(self, axis: int) -> None: + """Release the axis brake. + + Overrides the normal operation of the brake. It is important that the brake not be set + while a motion is being performed. This feature is used to lock an axis to prevent + motion or jitter. + + Parameters: + axis (int): The number of the axis whose brake should be released. + """ + await self.send_command(f"releaseBrake {axis}") + + async def set_brake(self, axis: int) -> None: + """Set the axis brake. + + Overrides the normal operation of the brake. It is important not to set a brake on an + axis that is moving as it may damage the brake or damage the motor. + + Parameters: + axis (int): The number of the axis whose brake should be set. + """ + await self.send_command(f"setBrake {axis}") + + async def state(self) -> str: + """Return state of motion. + + This value indicates the state of the currently executing or last completed robot motion. + For additional information, please see 'Robot.TrajState' in the GPL reference manual. + + Returns: + str: The current motion state. + """ + return await self.send_command("state") + + async def wait_for_eom(self) -> None: + """Wait for the robot to reach the end of the current motion. + + Waits for the robot to reach the end of the current motion or until it is stopped by + some other means. Does not reply until the robot has stopped. + """ + await self.send_command("waitForEom") + await asyncio.sleep(0.2) # Small delay to ensure command is fully processed + + async def zero_torque(self, enable: bool, axis_mask: int = 1) -> None: + """Sets or clears zero torque mode for the selected robot. + + Individual axes may be placed into zero torque mode while the remaining axes are servoing. + + Parameters: + enable (bool): If True, enable torque mode for axes specified by axis_mask. + If False, disable torque mode for the entire robot. + axis_mask (int): The bit mask specifying the axes to be placed in torque mode when enable is True. + The mask is computed by OR'ing the axis bits: + 1 = axis 1, 2 = axis 2, 4 = axis 3, 8 = axis 4, etc. + Ignored when enable is False. + """ + + if enable: + assert axis_mask > 0, "axis_mask must be greater than 0" + await self.send_command(f"zeroTorque 1 {axis_mask}") + else: + await self.send_command("zeroTorque 0") + + # region PAROBOT COMMANDS + + async def change_config(self, grip_mode: int = 0) -> None: + """Change Robot configuration from Righty to Lefty or vice versa using customizable locations. + + Uses customizable locations to avoid hitting robot during change. + Does not include checks for collision inside work volume of the robot. + Can be customized by user for their work cell configuration. + + Parameters: + grip_mode (int): Gripper control mode. + 0 = do not change gripper (default) + 1 = open gripper + 2 = close gripper + """ + await self.send_command(f"ChangeConfig {grip_mode}") + + async def change_config2(self, grip_mode: int = 0) -> None: + """Change Robot configuration from Righty to Lefty or vice versa using algorithm. + + Uses an algorithm to avoid hitting robot during change. + Does not include checks for collision inside work volume of the robot. + Can be customized by user for their work cell configuration. + + Parameters: + grip_mode (int): Gripper control mode. + 0 = do not change gripper (default) + 1 = open gripper + 2 = close gripper + """ + await self.send_command(f"ChangeConfig2 {grip_mode}") + + async def get_grasp_data(self) -> tuple[float, float, float]: + """Get the data to be used for the next force-controlled PickPlate command grip operation. + + Returns: + tuple: A tuple containing (plate_width_mm, finger_speed_percent, grasp_force_newtons) + """ + data = await self.send_command("GraspData") + parts = data.split() + + if len(parts) != 3: + raise PreciseFlexError(-1, "Unexpected response format from GraspData command.") + + plate_width = float(parts[0]) + finger_speed = float(parts[1]) + grasp_force = float(parts[2]) + + return (plate_width, finger_speed, grasp_force) + + async def set_grasp_data( + self, plate_width_mm: float, finger_speed_percent: float, grasp_force_newtons: float + ) -> None: + """Set the data to be used for the next force-controlled PickPlate command grip operation. + + This data remains in effect until the next GraspData command or the system is restarted. + + Parameters: + plate_width_mm (float): The plate width in mm. + finger_speed_percent (float): The finger speed during grasp where 100 means 100%. + grasp_force_newtons (float): The gripper squeezing force, in Newtons. + A positive value indicates the fingers must close to grasp. + A negative value indicates the fingers must open to grasp. + """ + await self.send_command( + f"GraspData {plate_width_mm} {finger_speed_percent} {grasp_force_newtons}" + ) + + async def get_grip_close_pos(self) -> float: + """Get the gripper close position for the servoed gripper. + + Returns: + float: The current gripper close position. + """ + data = await self.send_command("GripClosePos") + return float(data) + + async def set_grip_close_pos(self, close_position: float) -> None: + """Set the gripper close position for the servoed gripper. + + The close position may be changed by a force-controlled grip operation. + + Parameters: + close_position (float): The new gripper close position. + """ + await self.send_command(f"GripClosePos {close_position}") + + async def get_grip_open_pos(self) -> float: + """Get the gripper open position for the servoed gripper. + + Returns: + float: The current gripper open position. + """ + data = await self.send_command("GripOpenPos") + return float(data) + + async def set_grip_open_pos(self, open_position: float) -> None: + """Set the gripper open position for the servoed gripper. + + Parameters: + open_position (float): The new gripper open position. + """ + await self.send_command(f"GripOpenPos {open_position}") + + async def gripper(self, grip_mode: int) -> None: + """Opens or closes the servoed or digital-output-controlled gripper. + + Parameters: + grip_mode (int): Grip mode. + 1 = Open gripper + 2 = Close gripper + """ + if grip_mode not in [1, 2]: + raise ValueError("Grip mode must be 1 (open) or 2 (close)") + await self.send_command(f"Gripper {grip_mode}") + + async def move_rail( + self, station_id: Optional[int] = None, mode: int = 0, rail_destination: Optional[float] = None + ) -> None: + """Moves the optional linear rail. + + The rail may be moved immediately or simultaneously with the next pick or place motion. + The location may be associated with the station or specified explicitly. + + Parameters: + station_id (int, optional): The destination station ID. Only used if rail_destination is omitted. + mode (int): Mode of operation. + 0 or omitted = cancel any pending MoveRail + 1 = Move rail immediately + 2 = Move rail during next pick or place + rail_destination (float, optional): If specified, use this value as the rail destination + rather than the station location. + """ + if rail_destination is not None: + await self.send_command(f"MoveRail {station_id or ''} {mode} {rail_destination}") + elif station_id is not None: + await self.send_command(f"MoveRail {station_id} {mode}") + else: + await self.send_command(f"MoveRail {mode}") + + async def move_to_safe(self) -> None: + """Moves the robot to Safe Position. + + Does not include checks for collision with 3rd party obstacles inside the work volume of the robot. + """ + await self.send_command("movetosafe") + + async def get_pallet_index(self, station_id: int) -> tuple[int, int, int, int]: + """Get the current pallet index values for the specified station. + + Parameters: + station_id (int): Station ID, from 1 to N_LOC. + + Returns: + tuple: A tuple containing (station_id, pallet_index_x, pallet_index_y, pallet_index_z) + """ + data = await self.send_command(f"PalletIndex {station_id}") + parts = data.split() + + if len(parts) != 4: + raise PreciseFlexError(-1, "Unexpected response format from PalletIndex command.") + + station_id = int(parts[0]) + pallet_index_x = int(parts[1]) + pallet_index_y = int(parts[2]) + pallet_index_z = int(parts[3]) + + return (station_id, pallet_index_x, pallet_index_y, pallet_index_z) + + async def set_pallet_index( + self, station_id: int, pallet_index_x: int = 0, pallet_index_y: int = 0, pallet_index_z: int = 0 + ) -> None: + """Set the pallet index value from 1 to n of the station used by subsequent pick or place. + + If an index argument is 0 or omitted, the corresponding index is not changed. + Negative values generate an error. + + Parameters: + station_id (int): Station ID, from 1 to N_LOC. + pallet_index_x (int, optional): Pallet index X. If 0 or omitted, X index is not changed. + pallet_index_y (int, optional): Pallet index Y. If 0 or omitted, Y index is not changed. + pallet_index_z (int, optional): Pallet index Z. If 0 or omitted, Z index is not changed. + + Raises: + ValueError: If any index value is negative. + """ + if pallet_index_x < 0: + raise ValueError("Pallet index X cannot be negative") + if pallet_index_y < 0: + raise ValueError("Pallet index Y cannot be negative") + if pallet_index_z < 0: + raise ValueError("Pallet index Z cannot be negative") + + await self.send_command( + f"PalletIndex {station_id} {pallet_index_x} {pallet_index_y} {pallet_index_z}" + ) + + async def get_pallet_origin( + self, station_id: int + ) -> tuple[int, float, float, float, float, float, float, int]: + """Get the current pallet origin data for the specified station. + + Parameters: + station_id (int): Station ID, from 1 to N_LOC. + + Returns: + tuple: A tuple containing (station_id, x, y, z, yaw, pitch, roll, config) + """ + data = await self.send_command(f"PalletOrigin {station_id}") + parts = data.split() + + if len(parts) != 8: + raise PreciseFlexError(-1, "Unexpected response format from PalletOrigin command.") + + station_id = int(parts[0]) + x = float(parts[1]) + y = float(parts[2]) + z = float(parts[3]) + yaw = float(parts[4]) + pitch = float(parts[5]) + roll = float(parts[6]) + config = int(parts[7]) + + return (station_id, x, y, z, yaw, pitch, roll, config) + + async def set_pallet_origin( + self, + station_id: int, + x: float, + y: float, + z: float, + yaw: float, + pitch: float, + roll: float, + config: Optional[int] = None, + ) -> None: + """Define the origin of a pallet reference frame. + + Specifies the world location and orientation of the (1,1,1) pallet position. + Must be followed by a PalletX command. + + The orientation and configuration specified here determines the world orientation + of the robot during all pick or place operations using this pallet. + + Parameters: + station_id (int): Station ID, from 1 to N_LOC. + x (float): World location X coordinate. + y (float): World location Y coordinate. + z (float): World location Z coordinate. + yaw (float): Yaw rotation. + pitch (float): Pitch rotation. + roll (float): Roll rotation. + config (int, optional): The configuration flags for this location. + """ + if config is None: + await self.send_command(f"PalletOrigin {station_id} {x} {y} {z} {yaw} {pitch} {roll}") + else: + await self.send_command( + f"PalletOrigin {station_id} {x} {y} {z} {yaw} {pitch} {roll} {config}" + ) + + async def get_pallet_x(self, station_id: int) -> tuple[int, int, float, float, float]: + """Get the current pallet X data for the specified station. + + Parameters: + station_id (int): Station ID, from 1 to N_LOC. + + Returns: + tuple: A tuple containing (station_id, x_position_count, world_x, world_y, world_z) + """ + data = await self.send_command(f"PalletX {station_id}") + parts = data.split() + + if len(parts) != 5: + raise PreciseFlexError(-1, "Unexpected response format from PalletX command.") + + station_id = int(parts[0]) + x_position_count = int(parts[1]) + world_x = float(parts[2]) + world_y = float(parts[3]) + world_z = float(parts[4]) + + return (station_id, x_position_count, world_x, world_y, world_z) + + async def set_pallet_x( + self, station_id: int, x_position_count: int, world_x: float, world_y: float, world_z: float + ) -> None: + """Define the last point on the pallet X axis. + + Specifies the world location of the (n,1,1) pallet position, where n is the x_position_count value. + Must follow a PalletOrigin command. + + Parameters: + station_id (int): Station ID, from 1 to N_LOC. + x_position_count (int): X position count. + world_x (float): World location X coordinate. + world_y (float): World location Y coordinate. + world_z (float): World location Z coordinate. + """ + await self.send_command( + f"PalletX {station_id} {x_position_count} {world_x} {world_y} {world_z}" + ) + + async def get_pallet_y(self, station_id: int) -> tuple[int, int, float, float, float]: + """Get the current pallet Y data for the specified station. + + Parameters: + station_id (int): Station ID, from 1 to N_LOC. + + Returns: + tuple: A tuple containing (station_id, y_position_count, world_x, world_y, world_z) + """ + data = await self.send_command(f"PalletY {station_id}") + parts = data.split() + + if len(parts) != 5: + raise PreciseFlexError(-1, "Unexpected response format from PalletY command.") + + station_id = int(parts[0]) + y_position_count = int(parts[1]) + world_x = float(parts[2]) + world_y = float(parts[3]) + world_z = float(parts[4]) + + return (station_id, y_position_count, world_x, world_y, world_z) + + async def set_pallet_y( + self, station_id: int, y_position_count: int, world_x: float, world_y: float, world_z: float + ) -> None: + """Define the last point on the pallet Y axis. + + Specifies the world location of the (1,n,1) pallet position, where n is the y_position_count value. + If this command is executed, a 2 or 3-dimensional pallet is assumed. + Must follow a PalletX command. + + Parameters: + station_id (int): Station ID, from 1 to N_LOC. + y_position_count (int): Y position count. + world_x (float): World location X coordinate. + world_y (float): World location Y coordinate. + world_z (float): World location Z coordinate. + """ + await self.send_command( + f"PalletY {station_id} {y_position_count} {world_x} {world_y} {world_z}" + ) + + async def get_pallet_z(self, station_id: int) -> tuple[int, int, float, float, float]: + """Get the current pallet Z data for the specified station. + + Parameters: + station_id (int): Station ID, from 1 to N_LOC. + + Returns: + tuple: A tuple containing (station_id, z_position_count, world_x, world_y, world_z) + """ + data = await self.send_command(f"PalletZ {station_id}") + parts = data.split() + + if len(parts) != 5: + raise PreciseFlexError(-1, "Unexpected response format from PalletZ command.") + + station_id = int(parts[0]) + z_position_count = int(parts[1]) + world_x = float(parts[2]) + world_y = float(parts[3]) + world_z = float(parts[4]) + + return (station_id, z_position_count, world_x, world_y, world_z) + + async def set_pallet_z( + self, station_id: int, z_position_count: int, world_x: float, world_y: float, world_z: float + ) -> None: + """Define the last point on the pallet Z axis. + + Specifies the world location of the (1,1,n) pallet position, where n is the z_position_count value. + If this command is executed, a 3-dimensional pallet is assumed. + Must follow a PalletX and PalletY command. + + Parameters: + station_id (int): Station ID, from 1 to N_LOC. + z_position_count (int): Z position count. + world_x (float): World location X coordinate. + world_y (float): World location Y coordinate. + world_z (float): World location Z coordinate. + """ + await self.send_command( + f"PalletZ {station_id} {z_position_count} {world_x} {world_y} {world_z}" + ) + + async def pick_plate_station( + self, + station_id: int, + horizontal_compliance: bool = False, + horizontal_compliance_torque: int = 0, + ) -> bool: + """Moves to a predefined position or pallet location and picks up plate. + + If the arm must change configuration, it automatically goes through the Park position. + At the conclusion of this routine, the arm is left gripping the plate and stopped at the nest approach position. + Use Teach function to teach station pick point. + + Parameters: + station_id (int): Station ID, from 1 to Max. + horizontal_compliance (bool): If True, enable horizontal compliance while closing the gripper to allow centering around the plate. + horizontal_compliance_torque (int): The % of the original horizontal holding torque to be retained during compliance. If omitted, 0 is used. + + Returns: + bool: True if the plate was successfully grasped or force control was not used. + False if the force-controlled gripper detected no plate present. + """ + horizontal_compliance_int = 1 if horizontal_compliance else 0 + ret_code = await self.send_command( + f"PickPlate {station_id} {horizontal_compliance_int} {horizontal_compliance_torque}" + ) + return ret_code != "0" + + async def place_plate_station( + self, + station_id: int, + horizontal_compliance: bool = False, + horizontal_compliance_torque: int = 0, + ) -> None: + """Moves to a predefined position or pallet location and places a plate. + + If the arm must change configuration, it automatically goes through the Park position. + At the conclusion of this routine, the arm is left gripping the plate and stopped at the nest approach position. + Use Teach function to teach station place point. + + Parameters: + station_id (int): Station ID, from 1 to Max. + horizontal_compliance (bool): If True, enable horizontal compliance during the move to place the plate, to allow centering in the fixture. + horizontal_compliance_torque (int): The % of the original horizontal holding torque to be retained during compliance. If omitted, 0 is used. + """ + horizontal_compliance_int = 1 if horizontal_compliance else 0 + await self.send_command( + f"PlacePlate {station_id} {horizontal_compliance_int} {horizontal_compliance_torque}" + ) + + async def get_rail_position(self, station_id: int) -> float: + """Get the position of the optional rail axis that is associated with a station. + + Parameters: + station_id (int): Station ID, from 1 to Max. + + Returns: + float: The current rail position for the specified station. + """ + data = await self.send_command(f"Rail {station_id}") + return float(data) + + async def set_rail_position(self, station_id: int, rail_position: float) -> None: + """Set the position of the optional rail axis that is associated with a station. + + The station rail data is loaded and saved by the LoadFile and StoreFile commands. + + Parameters: + station_id (int): Station ID, from 1 to Max. + rail_position (float): The new rail position. + """ + await self.send_command(f"Rail {station_id} {rail_position}") + + async def teach_plate_station(self, station_id: int, z_clearance: float = 50.0) -> None: + """Sets the plate location to the current robot position and configuration. + + The location is saved as Cartesian coordinates. Z clearance must be high enough to withdraw the gripper. + If this station is a pallet, the pallet indices must be set to 1, 1, 1. The pallet frame is not changed, + only the location relative to the pallet. + + Parameters: + station_id (int): Station ID, from 1 to Max. + z_clearance (float): The Z Clearance value. If omitted, a value of 50 is used. If specified and non-zero, this value is used. + """ + await self.send_command(f"TeachPlate {station_id} {z_clearance}") + + async def get_station_type(self, station_id: int) -> tuple[int, int, int, float, float, float]: + """Get the station configuration for the specified station ID. + + Parameters: + station_id (int): Station ID, from 1 to Max. + + Returns: + tuple: A tuple containing (station_id, access_type, location_type, z_clearance, z_above, z_grasp_offset) + - station_id (int): The station ID + - access_type (int): 0 = horizontal, 1 = vertical + - location_type (int): 0 = normal single, 1 = pallet (1D, 2D, 3D) + - z_clearance (float): ZClearance value in mm + - z_above (float): ZAbove value in mm + - z_grasp_offset (float): ZGrasp offset + """ + data = await self.send_command(f"StationType {station_id}") + parts = data.split() + + if len(parts) != 6: + raise PreciseFlexError(-1, "Unexpected response format from StationType command.") + + station_id = int(parts[0]) + access_type = int(parts[1]) + location_type = int(parts[2]) + z_clearance = float(parts[3]) + z_above = float(parts[4]) + z_grasp_offset = float(parts[5]) + + return (station_id, access_type, location_type, z_clearance, z_above, z_grasp_offset) + + async def set_station_type( + self, + station_id: int, + access_type: int, + location_type: int, + z_clearance: float, + z_above: float, + z_grasp_offset: float, + ) -> None: + """Set the station configuration for the specified station ID. + + Parameters: + station_id (int): Station ID, from 1 to Max. + access_type (int): The station access type. + 0 = horizontal (for "hotel" carriers accessed by horizontal move) + 1 = vertical (for stacks or tube racks accessed with vertical motion) + location_type (int): The location type. + 0 = normal single location + 1 = pallet (1D, 2D, or 3D regular arrays requiring column, row, and layer index) + z_clearance (float): ZClearance value in mm. The horizontal or vertical distance + from the final location used when approaching or departing from a station. + z_above (float): ZAbove value in mm. The vertical offset used with horizontal + access when approaching or departing from the location. + z_grasp_offset (float): ZGrasp offset. Added to ZClearance when an object is + being held to compensate for the part in the gripper. + + Raises: + ValueError: If access_type or location_type are not valid values. + """ + if access_type not in [0, 1]: + raise ValueError("Access type must be 0 (horizontal) or 1 (vertical)") + + if location_type not in [0, 1]: + raise ValueError("Location type must be 0 (normal single) or 1 (pallet)") + + await self.send_command( + f"StationType {station_id} {access_type} {location_type} {z_clearance} {z_above} {z_grasp_offset}" + ) + + # region SSGRIP COMMANDS + + async def home_all_if_no_plate(self) -> int: + """Tests if the gripper is holding a plate. If not, enable robot power and home all robots. + + Returns: + int: -1 if no plate detected and the command succeeded, 0 if a plate was detected. + """ + response = await self.send_command("HomeAll_IfNoPlate") + return int(response) + + async def grasp_plate( + self, plate_width_mm: float, finger_speed_percent: int, grasp_force_newtons: float + ) -> int: + """Grasps a plate with limited force. + + A plate can be grasped by opening or closing the gripper. The actual commanded gripper + width generated by this function is a few mm smaller (or larger) than plate_width_mm + to permit the servos PID loop to generate the gripping force. + + Parameters: + plate_width_mm (float): Plate width in mm. Should be accurate to within about 1 mm. + finger_speed_percent (int): Percent speed to close fingers. 1 to 100. + grasp_force_newtons (float): Maximum gripper squeeze force in Newtons. + A positive value indicates the fingers must close to grasp. + A negative value indicates the fingers must open to grasp. + + Returns: + int: -1 if the plate has been grasped, 0 if the final gripping force indicates no plate. + + Raises: + ValueError: If finger_speed_percent is not between 1 and 100. + """ + if not (1 <= finger_speed_percent <= 100): + raise ValueError("Finger speed percent must be between 1 and 100") + + response = await self.send_command( + f"GraspPlate {plate_width_mm} {finger_speed_percent} {grasp_force_newtons}" + ) + return int(response) + + async def release_plate( + self, open_width_mm: float, finger_speed_percent: int, in_range: float = 0.0 + ) -> None: + """Releases the plate after a GraspPlate command. + + Opens (or closes) the gripper to the specified width and cancels the force limit + once the plate is released to avoid applying an excessive force to the plate. + + Parameters: + open_width_mm (float): Open width in mm. + finger_speed_percent (int): Percent speed to open fingers. 1 to 100. + in_range (float): Optional. The standard InRange profile property for the gripper open move. + If omitted, a zero value is assumed. + + Raises: + ValueError: If finger_speed_percent is not between 1 and 100. + """ + if not (1 <= finger_speed_percent <= 100): + raise ValueError("Finger speed percent must be between 1 and 100") + + await self.send_command(f"ReleasePlate {open_width_mm} {finger_speed_percent} {in_range}") + + async def is_fully_closed(self) -> int: + """Tests if the gripper is fully closed by checking the end-of-travel sensor. + + Returns: + int: For standard gripper: -1 if the gripper is within 2mm of fully closed, otherwise 0. + For dual gripper: A bitmask of the closed state of each gripper where gripper 1 is bit 0 + and gripper 2 is bit 1. A bit being set to 1 represents the corresponding gripper being closed. + """ + response = await self.send_command("IsFullyClosed") + return int(response) + + async def set_active_gripper( + self, gripper_id: int, spin_mode: int = 0, profile_index: Optional[int] = None + ) -> None: + """(Dual Gripper Only) Sets the currently active gripper and modifies the tool reference frame. + + Parameters: + gripper_id (int): Gripper ID, either 1 or 2. Determines which gripper is set to active. + spin_mode (int): Optional spin mode. + 0 or omitted = do not rotate the gripper 180deg immediately. + 1 = Rotate gripper 180deg immediately. + profile_index (int, optional): Profile Index to use for spin motion. + + Raises: + ValueError: If gripper_id is not 1 or 2, or if spin_mode is not 0 or 1. + """ + if gripper_id not in [1, 2]: + raise ValueError("Gripper ID must be 1 or 2") + + if spin_mode not in [0, 1]: + raise ValueError("Spin mode must be 0 or 1") + + if profile_index is not None: + await self.send_command(f"SetActiveGripper {gripper_id} {spin_mode} {profile_index}") + else: + await self.send_command(f"SetActiveGripper {gripper_id} {spin_mode}") + + async def get_active_gripper(self) -> int: + """(Dual Gripper Only) Returns the currently active gripper. + + Returns: + int: 1 if Gripper A is active, 2 if Gripper B is active. + """ + response = await self.send_command("GetActiveGripper") + return int(response) + + async def free_mode(self, on: bool, axis: int = 0): + """ + Activates or deactivates free mode. The robot must be attached to enter free mode. + + Parameters: + on (bool): If True, enable free mode. If False, disable free mode for all axes. + axis (int): Axis to apply free mode to. 0 = all axes or > 0 = Free just this axis. Ignored if 'on' parameter is False. + """ + if not on: + axis = -1 # means turn off free mode for all axes + await self.send_command(f"freemode {axis}") + + async def open_gripper(self): + """Open the gripper.""" + await self.send_command("gripper 1") + + async def close_gripper(self): + """Close the gripper.""" + await self.send_command("gripper 2") + + async def pick_plate( + self, + position_id: int, + horizontal_compliance: bool = False, + horizontal_compliance_torque: int = 0, + ): + """Pick an item at the specified position ID. + + Parameters: + position_id (int): The ID of the position where the plate should be picked. + horizontal_compliance (bool): enable horizontal compliance while closing the gripper to allow centering around the plate. + horizontal_compliance_torque (int): The % of the original horizontal holding torque to be retained during compliance. If omitted, 0 is used. + """ + horizontal_compliance_int = 1 if horizontal_compliance else 0 + ret_code = await self.send_command( + f"pickplate {position_id} {horizontal_compliance_int} {horizontal_compliance_torque}" + ) + if ret_code == "0": + raise PreciseFlexError(-1, "the force-controlled gripper detected no plate present.") + + async def place_plate( + self, + position_id: int, + horizontal_compliance: bool = False, + horizontal_compliance_torque: int = 0, + ): + """Place an item at the specified position ID. + + Parameters: + position_id (int): The ID of the position where the plate should be placed. + horizontal_compliance (bool): enable horizontal compliance during the move to place the plate, to allow centering in the fixture. + horizontal_compliance_torque (int): The % of the original horizontal holding torque to be retained during compliance. If omitted, 0 is used. + """ + horizontal_compliance_int = 1 if horizontal_compliance else 0 + await self.send_command( + f"placeplate {position_id} {horizontal_compliance_int} {horizontal_compliance_torque}" + ) + + async def teach_position(self, position_id: int, z_clearance: float = 50.0): + """Sets the plate location to the current robot position and configuration. The location is saved as Cartesian coordinates. + + Parameters: + position_id (int): The ID of the position to be taught. + z_clearance (float): Optional. The Z Clearance value. If omitted, a value of 50 is used. Z clearance must be high enough to withdraw the gripper. + """ + await self.send_command(f"teachplate {position_id} {z_clearance}") + + async def send_command(self, command: str) -> str: + await self.io.write(command.encode("utf-8") + b"\n") + await asyncio.sleep(0.2) # wait a bit for the robot to process the command + reply = await self.io.readline() + + print(f"Sent command: {command}, Received reply: {reply!r}") + + return self._parse_reply_ensure_successful(reply) + + def _parse_xyz_response( + self, parts: list[str] + ) -> tuple[float, float, float, float, float, float]: + if len(parts) != 6: + raise PreciseFlexError(-1, "Unexpected response format for Cartesian coordinates.") + + x = float(parts[0]) + y = float(parts[1]) + z = float(parts[2]) + yaw = float(parts[3]) + pitch = float(parts[4]) + roll = float(parts[5]) + + return (x, y, z, yaw, pitch, roll) + + def _parse_angles_response( + self, parts: list[str] + ) -> tuple[float, float, float, float, float, float]: + if len(parts) < 3: + raise PreciseFlexError(-1, "Unexpected response format for angles.") + + # Ensure we have exactly 6 elements, padding with 0.0 if necessary + angle1 = float(parts[0]) + angle2 = float(parts[1]) + angle3 = float(parts[2]) + angle4 = float(parts[3]) if len(parts) > 3 else 0.0 + angle5 = float(parts[4]) if len(parts) > 4 else 0.0 + angle6 = float(parts[5]) if len(parts) > 5 else 0.0 + + return (angle1, angle2, angle3, angle4, angle5, angle6) + + def _parse_reply_ensure_successful(self, reply: bytes) -> str: + """Parse reply from Precise Flex. + + Expected format: b'replycode data message\r\n' + - replycode is an integer at the beginning + - data is rest of the line (excluding CRLF) + """ + print("REPLY: ", reply) + text = reply.decode().strip() # removes \r\n + if not text: + raise PreciseFlexError(-1, "Empty reply from device.") + + parts = text.split(" ", 1) + if len(parts) == 1: + replycode = int(parts[0]) + data = "" + else: + replycode, data = int(parts[0]), parts[1] + + if replycode != 0: + # if error is reported, the data part generally contains the error message + raise PreciseFlexError(replycode, data) + + return data diff --git a/pylabrobot/arms/precise_flex/precise_flex_api_tests.py b/pylabrobot/arms/precise_flex/precise_flex_api_tests.py new file mode 100644 index 00000000000..0d40b3ab87a --- /dev/null +++ b/pylabrobot/arms/precise_flex/precise_flex_api_tests.py @@ -0,0 +1,1944 @@ +import asyncio +import unittest +from contextlib import asynccontextmanager + +import pytest + +from pylabrobot.arms.precise_flex.precise_flex_api import PreciseFlexBackendApi + + +@pytest.mark.hardware # include/exclude via "pytest -m hardware" +class PreciseFlexApiHardwareTests(unittest.IsolatedAsyncioTestCase): + """Integration tests for PreciseFlex robot - RUNS ON ACTUAL HARDWARE""" + + # Connection config + ROBOT_HOST = "192.168.0.1" + ROBOT_PORT = 10100 + + # Test constants + TEST_PROFILE_ID = 20 + TEST_LOCATION_ID = 20 + TEST_PARAMETER_ID = 17018 + TEST_SIGNAL_ID = 20064 + + SAFE_LOCATION_C = (175, 0.003, 169.995, -0.001, 90, 180, 1) + SAFE_LOCATION_J = (170.003, 0, 180, -180, 75.486) + TEST_LOCATION_C_RIGHT = (271.692, 115.89, 169.969, 2.473, 90, 180, 1) + TEST_LOCATION_J_RIGHT = (169.975, -16.736, 115.991, -110.057, 75.527) + TEST_LOCATION_C_LEFT = (246.024, -147.689, 169.955, 6.879, 90, 180, 2) + TEST_LOCATION_J_LEFT = (169.888, 10.079, 231.358, -240.229, 75.533) + + lock = asyncio.Lock() + + async def asyncSetUp(self): + """Ensure connected, baseline the robot, capture station state.""" + await self.__class__.lock.acquire() + self.robot = PreciseFlexBackendApi(self.ROBOT_HOST, self.ROBOT_PORT) + await self.robot.setup() + await self.robot.set_power(True, timeout=20) + await self.robot.attach(1) + print("Robot connected successfully") + + # Move to a safe joint pose if needed + current_position = await self.robot.where_j() + if any(abs(a - b) > 1 for a, b in zip(current_position, self.SAFE_LOCATION_J)): + await self.robot.move_to_safe() + await self.robot.wait_for_eom() + + # Snapshot station state that tests might alter + self._original_station_type = await self.robot.get_station_type(self.TEST_LOCATION_ID) + self._original_station_location = await self.robot.get_location(self.TEST_LOCATION_ID) + + if self._original_station_location[2] == 1: + self._original_pallet_index = await self.robot.get_pallet_index(self.TEST_LOCATION_ID) + self._original_pallet_origin = await self.robot.get_pallet_origin(self.TEST_LOCATION_ID) + self._original_pallet_final_x_pos = await self.robot.get_pallet_x(self.TEST_LOCATION_ID) + self._original_pallet_final_y_pos = await self.robot.get_pallet_y(self.TEST_LOCATION_ID) + self._original_pallet_final_z_pos = await self.robot.get_pallet_z(self.TEST_LOCATION_ID) + + self._original_profile = await self.robot.get_motion_profile_values(self.TEST_PROFILE_ID) + + async def asyncTearDown(self): + """Restore station edits and leave the link up for the next test.""" + + # Best-effort restore, even if a test failed mid-operation + try: + # Restore station type + if hasattr(self, "_original_station_type"): + await self.robot.set_station_type(*self._original_station_type) + + # Restore pallet config if applicable + if hasattr(self, "_original_station_location") and self._original_station_location[2] == 1: + if hasattr(self, "_original_pallet_origin"): + await self.robot.set_pallet_origin(*self._original_pallet_origin) + if hasattr(self, "_original_pallet_final_x_pos"): + await self.robot.set_pallet_x(*self._original_pallet_final_x_pos) + if hasattr(self, "_original_pallet_final_y_pos"): + await self.robot.set_pallet_y(*self._original_pallet_final_y_pos) + if hasattr(self, "_original_pallet_final_z_pos"): + await self.robot.set_pallet_z(*self._original_pallet_final_z_pos) + if hasattr(self, "_original_pallet_index"): + await self.robot.set_pallet_index(*self._original_pallet_index) + + # Restore station location + if hasattr(self, "_original_station_location"): + type_code = self._original_station_location[0] + if type_code == 0: + await self.robot.set_location_xyz(*self._original_station_location[1:8]) + else: + await self.robot.set_location_angles(*self._original_station_location[1:8]) + + # Set profile back to default values + await self.robot.set_motion_profile_values(self.TEST_PROFILE_ID, *self._original_profile[1:]) + + except Exception as e: + # Do not hide teardown failures. Print and continue so later tests can try to recover. + print(f"asyncTearDown encountered an error: {e}") + + finally: + await self.robot.stop() + self.__class__.lock.release() + + # ------------------------ + # Helpers + # ------------------------ + + @asynccontextmanager + async def _preserve_setting(self, getter_method: str, setter_method: str): + """Context manager to preserve and restore robot settings""" + # Get original value + original_value = await getattr(self.robot, getter_method)() + + try: + yield original_value + finally: + # Restore original value + try: + if isinstance(original_value, tuple): + await getattr(self.robot, setter_method)(*original_value) + else: + await getattr(self.robot, setter_method)(original_value) + print(f"Setting restored to: {original_value}") + except Exception as e: + print(f"Error restoring setting: {e}") + + # region GENERAL COMMANDS + # async def test_robot_connection_and_version(self) -> None: + # """Test basic connection and version info""" + # version = await self.robot.get_version() + # self.assertIsInstance(version, str) + # print(f"Robot version: {version}") + + async def test_get_base(self) -> None: + base = await self.robot.get_base() + self.assertIsInstance(base, tuple) + print(f"Robot base: {base}") + + async def test_set_base(self) -> None: + """Test set_base()""" + async with self._preserve_setting("get_base", "set_base"): + # Test setting to a different base if possible + test_base = (0, 0, 0, 0) + print(f"Setting test base to: {test_base}") + await self.robot.set_base(*test_base) + new_base = await self.robot.get_base() + print(f"Base set to: {new_base}") + self.assertEqual(new_base, test_base) + + async def test_home(self) -> None: + """Test home() command""" + await self.robot.home() + print("Robot homed successfully") + + async def test_home_all(self) -> None: + """Test home_all() command""" + # Note: This requires robots not to be attached, so we'll detach first + await self.robot.attach(0) + await self.robot.home_all() + await self.robot.attach() # Re-attach for other tests + print("All robots homed successfully") + + async def test_get_power_state(self) -> None: + """Test get_power_state()""" + power_state = await self.robot.get_power_state() + self.assertIsInstance(power_state, int) + self.assertIn(power_state, [0, 1]) + print(f"Power state: {power_state}") + + async def test_get_mode(self) -> None: + """Test get_mode()""" + mode = await self.robot.get_mode() + self.assertIsInstance(mode, int) + self.assertIn(mode, [0, 1]) + print(f"Current mode: {mode}") + + async def test_get_monitor_speed(self) -> None: + """Test get_monitor_speed()""" + speed = await self.robot.get_monitor_speed() + self.assertIsInstance(speed, int) + self.assertGreaterEqual(speed, 1) + self.assertLessEqual(speed, 100) + print(f"Monitor speed: {speed}%") + + async def test_set_monitor_speed(self) -> None: + """Test set_monitor_speed()""" + async with self._preserve_setting("get_monitor_speed", "set_monitor_speed"): + # Test setting different speeds + test_speed = 50 + await self.robot.set_monitor_speed(test_speed) + speed = await self.robot.get_monitor_speed() + self.assertEqual(speed, test_speed) + print(f"Monitor speed set to: {speed}%") + + async def test_nop(self) -> None: + """Test nop() command""" + await self.robot.nop() + print("NOP command executed successfully") + + async def test_get_payload(self) -> None: + """Test get_payload()""" + payload = await self.robot.get_payload() + self.assertIsInstance(payload, int) + self.assertGreaterEqual(payload, 0) + self.assertLessEqual(payload, 100) + print(f"Payload: {payload}%") + + async def test_set_payload(self) -> None: + """Test set_payload()""" + async with self._preserve_setting("get_payload", "set_payload"): + # Test setting different payload values + test_payload = 25 + await self.robot.set_payload(test_payload) + payload = await self.robot.get_payload() + self.assertEqual(payload, test_payload) + print(f"Payload set to: {payload}%") + + async def test_parameter_operations(self) -> None: + """Test get_parameter() and set_parameter()""" + # Get original value + original_value = await self.robot.get_parameter(self.TEST_PARAMETER_ID) + print(f"Original parameter value: {original_value}") + + # Test setting and getting back + test_value = "test_value" + await self.robot.set_parameter(self.TEST_PARAMETER_ID, test_value) + + # Get the value back + retrieved_value = await self.robot.get_parameter(self.TEST_PARAMETER_ID) + print(f"Retrieved parameter value: {retrieved_value}") + + # Restore original value + await self.robot.set_parameter(self.TEST_PARAMETER_ID, original_value) + + async def test_get_selected_robot(self) -> None: + """Test get_selected_robot()""" + selected_robot = await self.robot.get_selected_robot() + self.assertIsInstance(selected_robot, int) + self.assertGreaterEqual(selected_robot, 0) + print(f"Selected robot: {selected_robot}") + + async def test_select_robot(self) -> None: + """Test select_robot()""" + async with self._preserve_setting("get_selected_robot", "select_robot"): + # Test selecting robot 1 + await self.robot.select_robot(1) + selected = await self.robot.get_selected_robot() + self.assertEqual(selected, 1) + print(f"Selected robot set to: {selected}") + + async def test_signal_operations(self) -> None: + """Test get_signal() and set_signal()""" + + # Get original signal value + original_value = await self.robot.get_signal(self.TEST_SIGNAL_ID) + print(f"Original signal {self.TEST_SIGNAL_ID} value: {original_value}") + + try: + # Test setting signal + test_value = 1 if original_value == 0 else 0 + await self.robot.set_signal(self.TEST_SIGNAL_ID, test_value) + + # Verify the change + new_value = await self.robot.get_signal(self.TEST_SIGNAL_ID) + if test_value == 0: + self.assertEqual(new_value, 0) + else: + self.assertNotEqual(new_value, 0) + print(f"Signal {self.TEST_SIGNAL_ID} set to: {new_value}") + + finally: + # Restore original value + await self.robot.set_signal(self.TEST_SIGNAL_ID, original_value) + + async def test_get_system_state(self) -> None: + """Test get_system_state()""" + system_state = await self.robot.get_system_state() + self.assertIsInstance(system_state, int) + print(f"System state: {system_state}") + + async def test_get_tool(self) -> None: + """Test get_tool()""" + tool = await self.robot.get_tool() + self.assertIsInstance(tool, tuple) + self.assertEqual(len(tool), 6) + x, y, z, yaw, pitch, roll = tool + self.assertTrue(all(isinstance(val, (int, float)) for val in tool)) + print(f"Tool transformation: X={x}, Y={y}, Z={z}, Yaw={yaw}, Pitch={pitch}, Roll={roll}") + + async def test_set_tool(self) -> None: + """Test set_tool()""" + async with self._preserve_setting("get_tool", "set_tool"): + # Test setting tool transformation + test_tool = (10.0, 20.0, 30.0, 0.0, 0.0, 0.0) + await self.robot.set_tool(*test_tool) + + current_tool = await self.robot.get_tool() + # Allow for small floating point differences + for i, (expected, actual) in enumerate(zip(test_tool, current_tool)): + self.assertLess( + abs(expected - actual), + 0.001, + f"Tool value {i} mismatch: expected {expected}, got {actual}", + ) + + print(f"Tool transformation set to: {current_tool}") + + # region LOCATION COMMANDS + async def test_get_location(self) -> None: + """Test get_location()""" + location_data = await self.robot.get_location(self.TEST_LOCATION_ID) + self.assertIsInstance(location_data, tuple) + self.assertEqual(len(location_data), 8) + type_code, station_index, val1, val2, val3, val4, val5, val6 = location_data + self.assertIsInstance(type_code, int) + self.assertIn(type_code, [0, 1]) # 0 = Cartesian, 1 = angles + self.assertEqual(station_index, self.TEST_LOCATION_ID) + print( + f"Location {self.TEST_LOCATION_ID}: type={type_code}, values=({val1}, {val2}, {val3}, {val4}, {val5}, {val6})" + ) + + async def test_get_location_angles(self) -> None: + """Test get_location_angles()""" + # This test assumes location is already angles type or will fail appropriately + try: + location_data = await self.robot.get_location_angles(self.TEST_LOCATION_ID) + self.assertIsInstance(location_data, tuple) + self.assertEqual(len(location_data), 8) + type_code, station_index, angle1, angle2, angle3, angle4, angle5, angle6 = location_data + self.assertEqual(type_code, 1) # Should be angles type + self.assertEqual(station_index, self.TEST_LOCATION_ID) + print( + f"Location angles {self.TEST_LOCATION_ID}: ({angle1}, {angle2}, {angle3}, {angle4}, {angle5}, {angle6})" + ) + except Exception as e: + print(f"Location {self.TEST_LOCATION_ID} is not angles type or error occurred: {e}") + + async def test_set_location_angles(self) -> None: + """Test set_location_angles()""" + # Get original location data + original_location = await self.robot.get_location(self.TEST_LOCATION_ID) + + try: + # Test setting angles + test_angles = ( + 15.0, + 25.0, + 35.0, + 45.0, + 55.0, + 0.0, + ) # last is set to 0.0 as angle7 is typically unused on PF400, some other robots may use fewer angles + await self.robot.set_location_angles(self.TEST_LOCATION_ID, *test_angles) + + # Verify the angles were set + location_data = await self.robot.get_location_angles(self.TEST_LOCATION_ID) + _, _, angle1, angle2, angle3, angle4, angle5, angle6 = location_data + + # Check first 6 angles (angle7 is typically 0) + retrieved_angles = (angle1, angle2, angle3, angle4, angle5, angle6) + for i, (expected, actual) in enumerate(zip(test_angles, retrieved_angles)): + self.assertLess( + abs(expected - actual), 0.001, f"Angle {i+1} mismatch: expected {expected}, got {actual}" + ) + + print(f"Location angles set successfully: {retrieved_angles}") + + finally: + # Restore original location + type_code = original_location[0] + if type_code == 1: # Was angles type + await self.robot.set_location_angles(self.TEST_LOCATION_ID, *original_location[2:8]) + else: # Was Cartesian type + await self.robot.set_location_xyz(self.TEST_LOCATION_ID, *original_location[2:8]) + + async def test_get_location_xyz(self) -> None: + """Test get_location_xyz()""" + # This test assumes location is already Cartesian type or will fail appropriately + try: + location_data = await self.robot.get_location_xyz(self.TEST_LOCATION_ID) + self.assertIsInstance(location_data, tuple) + self.assertEqual(len(location_data), 8) + type_code, station_index, x, y, z, yaw, pitch, roll = location_data + self.assertEqual(type_code, 0) # Should be Cartesian type + self.assertEqual(station_index, self.TEST_LOCATION_ID) + print( + f"Location XYZ {self.TEST_LOCATION_ID}: X={x}, Y={y}, Z={z}, Yaw={yaw}, Pitch={pitch}, Roll={roll}" + ) + except Exception as e: + print(f"Location {self.TEST_LOCATION_ID} is not Cartesian type or error occurred: {e}") + + async def test_set_location_xyz(self) -> None: + """Test set_location_xyz()""" + # Get original location data + original_location = await self.robot.get_location(self.TEST_LOCATION_ID) + + try: + # Test setting Cartesian coordinates + test_coords = (150.0, 250.0, 350.0, 10.0, 20.0, 30.0) + await self.robot.set_location_xyz(self.TEST_LOCATION_ID, *test_coords) + + # Verify the coordinates were set + location_data = await self.robot.get_location_xyz(self.TEST_LOCATION_ID) + _, _, x, y, z, yaw, pitch, roll = location_data + + retrieved_coords = (x, y, z, yaw, pitch, roll) + for i, (expected, actual) in enumerate(zip(test_coords, retrieved_coords)): + self.assertLess( + abs(expected - actual), + 0.001, + f"Coordinate {i} mismatch: expected {expected}, got {actual}", + ) + + print(f"Location XYZ set successfully: {retrieved_coords}") + + finally: + # Restore original location + type_code = original_location[0] + if type_code == 0: # Was Cartesian type + await self.robot.set_location_xyz(self.TEST_LOCATION_ID, *original_location[2:8]) + else: # Was angles type + await self.robot.set_location_angles(self.TEST_LOCATION_ID, *original_location[2:8]) + + async def test_get_location_z_clearance(self) -> None: + """Test get_location_z_clearance()""" + clearance_data = await self.robot.get_location_z_clearance(self.TEST_LOCATION_ID) + self.assertIsInstance(clearance_data, tuple) + self.assertEqual(len(clearance_data), 3) + station_index, z_clearance, z_world = clearance_data + self.assertEqual(station_index, self.TEST_LOCATION_ID) + self.assertIsInstance(z_clearance, float) + self.assertIsInstance(z_world, bool) + print(f"Location {self.TEST_LOCATION_ID} Z clearance: {z_clearance}, Z world: {z_world}") + + async def test_set_location_z_clearance(self) -> None: + """Test set_location_z_clearance()""" + original_clearance = await self.robot.get_location_z_clearance(self.TEST_LOCATION_ID) + _, orig_z_clearance, orig_z_world = original_clearance + + try: + # Test setting only z_clearance + test_z_clearance = 50.0 + await self.robot.set_location_z_clearance(self.TEST_LOCATION_ID, test_z_clearance) + + clearance_data = await self.robot.get_location_z_clearance(self.TEST_LOCATION_ID) + _, z_clearance, z_world = clearance_data + self.assertLess(abs(z_clearance - test_z_clearance), 0.001) + print(f"Z clearance set to: {z_clearance}") + + # Test setting both z_clearance and z_world + test_z_world = True + await self.robot.set_location_z_clearance( + self.TEST_LOCATION_ID, test_z_clearance, test_z_world + ) + + clearance_data = await self.robot.get_location_z_clearance(self.TEST_LOCATION_ID) + _, z_clearance, z_world = clearance_data + self.assertLess(abs(z_clearance - test_z_clearance), 0.001) + self.assertEqual(z_world, test_z_world) + print(f"Z clearance and world set to: {z_clearance}, {z_world}") + + finally: + # Restore original values + await self.robot.set_location_z_clearance( + self.TEST_LOCATION_ID, orig_z_clearance, orig_z_world + ) + + async def test_get_location_config(self) -> None: + """Test get_location_config()""" + config_data = await self.robot.get_location_config(self.TEST_LOCATION_ID) + self.assertIsInstance(config_data, tuple) + self.assertEqual(len(config_data), 2) + station_index, config_value = config_data + self.assertEqual(station_index, self.TEST_LOCATION_ID) + self.assertIsInstance(config_value, int) + self.assertGreaterEqual(config_value, 0) + + # Decode config bits for display + config_bits = [] + if config_value == 0: + config_bits.append("None") + else: + if config_value & 0x01: + config_bits.append("Righty") + if config_value & 0x02: + config_bits.append("Lefty") + if config_value & 0x04: + config_bits.append("Above") + if config_value & 0x08: + config_bits.append("Below") + if config_value & 0x10: + config_bits.append("Flip") + if config_value & 0x20: + config_bits.append("NoFlip") + if config_value & 0x1000: + config_bits.append("Single") + + config_str = " | ".join(config_bits) + print(f"Location {self.TEST_LOCATION_ID} config: 0x{config_value:X} ({config_str})") + + async def test_set_location_config(self) -> None: + """Test set_location_config()""" + original_config = await self.robot.get_location_config(self.TEST_LOCATION_ID) + _, orig_config_value = original_config + + try: + # Test setting basic config (Righty) + test_config = 0x01 # GPL_Righty + await self.robot.set_location_config(self.TEST_LOCATION_ID, test_config) + + config_data = await self.robot.get_location_config(self.TEST_LOCATION_ID) + _, config_value = config_data + self.assertEqual(config_value, test_config) + print(f"Location config set to: 0x{config_value:X} (Righty)") + + # Test setting combined config (Lefty + Above + NoFlip) + test_config = 0x02 | 0x04 | 0x20 # GPL_Lefty | GPL_Above | GPL_NoFlip + await self.robot.set_location_config(self.TEST_LOCATION_ID, test_config) + + config_data = await self.robot.get_location_config(self.TEST_LOCATION_ID) + _, config_value = config_data + self.assertEqual(config_value, test_config) + print(f"Location config set to: 0x{config_value:X} (Lefty | Above | NoFlip)") + + # Test setting None config + test_config = 0x00 + await self.robot.set_location_config(self.TEST_LOCATION_ID, test_config) + + config_data = await self.robot.get_location_config(self.TEST_LOCATION_ID) + _, config_value = config_data + self.assertEqual(config_value, test_config) + print(f"Location config set to: 0x{config_value:X} (None)") + + # Test invalid config bits + with self.assertRaises(ValueError): + await self.robot.set_location_config(self.TEST_LOCATION_ID, 0x80) # Invalid bit + + # Test conflicting configurations + with self.assertRaises(ValueError): + await self.robot.set_location_config(self.TEST_LOCATION_ID, 0x01 | 0x02) # Righty + Lefty + + with self.assertRaises(ValueError): + await self.robot.set_location_config(self.TEST_LOCATION_ID, 0x04 | 0x08) # Above + Below + + with self.assertRaises(ValueError): + await self.robot.set_location_config(self.TEST_LOCATION_ID, 0x10 | 0x20) # Flip + NoFlip + + finally: + # Restore original config + await self.robot.set_location_config(self.TEST_LOCATION_ID, orig_config_value) + + async def test_dest_c(self) -> None: + """Test dest_c()""" + # Test with default argument (current location) + dest_data = await self.robot.dest_c() + self.assertIsInstance(dest_data, tuple) + self.assertEqual(len(dest_data), 7) + x, y, z, yaw, pitch, roll, config = dest_data + self.assertTrue(all(isinstance(val, (int, float)) for val in dest_data)) + print( + f"Current Cartesian destination: X={x}, Y={y}, Z={z}, Yaw={yaw}, Pitch={pitch}, Roll={roll}, Config={config}" + ) + + # Test with arg1=1 (target location) + dest_data_target = await self.robot.dest_c(1) + self.assertIsInstance(dest_data_target, tuple) + self.assertEqual(len(dest_data_target), 7) + print(f"Target Cartesian destination: {dest_data_target}") + + async def test_dest_j(self) -> None: + """Test dest_j()""" + # Test with default argument (current joint positions) + dest_data = await self.robot.dest_j() + self.assertIsInstance(dest_data, tuple) + self.assertEqual(len(dest_data), 6) + self.assertTrue(all(isinstance(val, (int, float)) for val in dest_data)) + print(f"Current joint destination: {dest_data}") + + # Test with arg1=1 (target joint positions) + dest_data_target = await self.robot.dest_j(1) + self.assertIsInstance(dest_data_target, tuple) + self.assertEqual(len(dest_data_target), 6) + print(f"Target joint destination: {dest_data_target}") + + async def test_here_j(self) -> None: + """Test here_j()""" + original_location = await self.robot.get_location(self.TEST_LOCATION_ID) + + try: + # Record current position as angles + await self.robot.here_j(self.TEST_LOCATION_ID) + + # Verify the location was recorded as angles type + location_data = await self.robot.get_location(self.TEST_LOCATION_ID) + type_code = location_data[0] + self.assertEqual(type_code, 1) # Should be angles type + print(f"Current position recorded as angles at location {self.TEST_LOCATION_ID}") + + finally: + # Restore original location + type_code = original_location[0] + if type_code == 0: # Was Cartesian type + await self.robot.set_location_xyz(self.TEST_LOCATION_ID, *original_location[2:8]) + else: # Was angles type + await self.robot.set_location_angles(self.TEST_LOCATION_ID, *original_location[2:8]) + + async def test_here_c(self) -> None: + """Test here_c()""" + original_location = await self.robot.get_location(self.TEST_LOCATION_ID) + + try: + # Record current position as Cartesian + await self.robot.here_c(self.TEST_LOCATION_ID) + + # Verify the location was recorded as Cartesian type + location_data = await self.robot.get_location(self.TEST_LOCATION_ID) + type_code = location_data[0] + self.assertEqual(type_code, 0) # Should be Cartesian type + print(f"Current position recorded as Cartesian at location {self.TEST_LOCATION_ID}") + + finally: + # Restore original location + type_code = original_location[0] + if type_code == 0: # Was Cartesian type + await self.robot.set_location_xyz(self.TEST_LOCATION_ID, *original_location[2:8]) + else: # Was angles type + await self.robot.set_location_angles(self.TEST_LOCATION_ID, *original_location[2:8]) + + async def test_where(self) -> None: + """Test where()""" + position_data = await self.robot.where() + self.assertIsInstance(position_data, tuple) + self.assertEqual(len(position_data), 7) + x, y, z, yaw, pitch, roll, axes = position_data + self.assertTrue(all(isinstance(val, (int, float)) for val in [x, y, z, yaw, pitch, roll])) + self.assertIsInstance(axes, tuple) + self.assertEqual(len(axes), 6) + self.assertTrue(all(isinstance(val, (int, float)) for val in axes)) + print( + f"Current position - Cartesian: X={x}, Y={y}, Z={z}, Yaw={yaw}, Pitch={pitch}, Roll={roll}" + ) + print(f"Current position - Joints: {axes}") + + async def test_where_c(self) -> None: + """Test where_c()""" + position_data = await self.robot.where_c() + self.assertIsInstance(position_data, tuple) + self.assertEqual(len(position_data), 7) + x, y, z, yaw, pitch, roll, config = position_data + self.assertTrue(all(isinstance(val, (int, float)) for val in position_data)) + self.assertIn(config, [1, 2]) # 1 = Righty, 2 = Lefty + print( + f"Current Cartesian position: X={x}, Y={y}, Z={z}, Yaw={yaw}, Pitch={pitch}, Roll={roll}, Config={config}" + ) + + async def test_where_j(self) -> None: + """Test where_j()""" + joint_data = await self.robot.where_j() + self.assertIsInstance(joint_data, tuple) + self.assertEqual(len(joint_data), 6) + self.assertTrue(all(isinstance(val, (int, float)) for val in joint_data)) + print(f"Current joint positions: {joint_data}") + + # region PROFILE COMMANDS + async def test_get_profile_speed(self) -> None: + """Test get_profile_speed()""" + speed = await self.robot.get_profile_speed(self.TEST_PROFILE_ID) + self.assertIsInstance(speed, float) + self.assertGreaterEqual(speed, 0) + print(f"Profile {self.TEST_PROFILE_ID} speed: {speed}%") + + async def test_set_profile_speed(self) -> None: + """Test set_profile_speed()""" + original_speed = await self.robot.get_profile_speed(self.TEST_PROFILE_ID) + + try: + # Test setting different speed + test_speed = 50.0 + await self.robot.set_profile_speed(self.TEST_PROFILE_ID, test_speed) + + speed = await self.robot.get_profile_speed(self.TEST_PROFILE_ID) + self.assertLess(abs(speed - test_speed), 0.001) + print(f"Profile speed set to: {speed}%") + + finally: + # Restore original speed + await self.robot.set_profile_speed(self.TEST_PROFILE_ID, original_speed) + + async def test_get_profile_speed2(self) -> None: + """Test get_profile_speed2()""" + speed2 = await self.robot.get_profile_speed2(self.TEST_PROFILE_ID) + self.assertIsInstance(speed2, float) + self.assertGreaterEqual(speed2, 0) + print(f"Profile {self.TEST_PROFILE_ID} speed2: {speed2}%") + + async def test_set_profile_speed2(self) -> None: + """Test set_profile_speed2()""" + original_speed2 = await self.robot.get_profile_speed2(self.TEST_PROFILE_ID) + + try: + # Test setting different speed2 + test_speed2 = 25.0 + await self.robot.set_profile_speed2(self.TEST_PROFILE_ID, test_speed2) + + speed2 = await self.robot.get_profile_speed2(self.TEST_PROFILE_ID) + self.assertLess(abs(speed2 - test_speed2), 0.001) + print(f"Profile speed2 set to: {speed2}%") + + finally: + # Restore original speed2 + await self.robot.set_profile_speed2(self.TEST_PROFILE_ID, original_speed2) + + async def test_get_profile_accel(self) -> None: + """Test get_profile_accel()""" + accel = await self.robot.get_profile_accel(self.TEST_PROFILE_ID) + self.assertIsInstance(accel, float) + self.assertGreaterEqual(accel, 0) + print(f"Profile {self.TEST_PROFILE_ID} acceleration: {accel}%") + + async def test_set_profile_accel(self) -> None: + """Test set_profile_accel()""" + original_accel = await self.robot.get_profile_accel(self.TEST_PROFILE_ID) + + try: + # Test setting different acceleration + test_accel = 75.0 + await self.robot.set_profile_accel(self.TEST_PROFILE_ID, test_accel) + + accel = await self.robot.get_profile_accel(self.TEST_PROFILE_ID) + self.assertLess(abs(accel - test_accel), 0.001) + print(f"Profile acceleration set to: {accel}%") + + finally: + # Restore original acceleration + await self.robot.set_profile_accel(self.TEST_PROFILE_ID, original_accel) + + async def test_get_profile_accel_ramp(self) -> None: + """Test get_profile_accel_ramp()""" + accel_ramp = await self.robot.get_profile_accel_ramp(self.TEST_PROFILE_ID) + self.assertIsInstance(accel_ramp, float) + self.assertGreaterEqual(accel_ramp, 0) + print(f"Profile {self.TEST_PROFILE_ID} acceleration ramp: {accel_ramp} seconds") + + async def test_set_profile_accel_ramp(self) -> None: + """Test set_profile_accel_ramp()""" + original_accel_ramp = await self.robot.get_profile_accel_ramp(self.TEST_PROFILE_ID) + + try: + # Test setting different acceleration ramp + test_accel_ramp = 0.5 + await self.robot.set_profile_accel_ramp(self.TEST_PROFILE_ID, test_accel_ramp) + + accel_ramp = await self.robot.get_profile_accel_ramp(self.TEST_PROFILE_ID) + self.assertLess(abs(accel_ramp - test_accel_ramp), 0.001) + print(f"Profile acceleration ramp set to: {accel_ramp} seconds") + + finally: + # Restore original acceleration ramp + await self.robot.set_profile_accel_ramp(self.TEST_PROFILE_ID, original_accel_ramp) + + async def test_get_profile_decel(self) -> None: + """Test get_profile_decel()""" + decel = await self.robot.get_profile_decel(self.TEST_PROFILE_ID) + self.assertIsInstance(decel, float) + self.assertGreaterEqual(decel, 0) + print(f"Profile {self.TEST_PROFILE_ID} deceleration: {decel}%") + + async def test_set_profile_decel(self) -> None: + """Test set_profile_decel()""" + original_decel = await self.robot.get_profile_decel(self.TEST_PROFILE_ID) + + try: + # Test setting different deceleration + test_decel = 80.0 + await self.robot.set_profile_decel(self.TEST_PROFILE_ID, test_decel) + + decel = await self.robot.get_profile_decel(self.TEST_PROFILE_ID) + self.assertLess(abs(decel - test_decel), 0.001) + print(f"Profile deceleration set to: {decel}%") + + finally: + # Restore original deceleration + await self.robot.set_profile_decel(self.TEST_PROFILE_ID, original_decel) + + async def test_get_profile_decel_ramp(self) -> None: + """Test get_profile_decel_ramp()""" + decel_ramp = await self.robot.get_profile_decel_ramp(self.TEST_PROFILE_ID) + self.assertIsInstance(decel_ramp, float) + self.assertGreaterEqual(decel_ramp, 0) + print(f"Profile {self.TEST_PROFILE_ID} deceleration ramp: {decel_ramp} seconds") + + async def test_set_profile_decel_ramp(self) -> None: + """Test set_profile_decel_ramp()""" + original_decel_ramp = await self.robot.get_profile_decel_ramp(self.TEST_PROFILE_ID) + + try: + # Test setting different deceleration ramp + test_decel_ramp = 0.3 + await self.robot.set_profile_decel_ramp(self.TEST_PROFILE_ID, test_decel_ramp) + + decel_ramp = await self.robot.get_profile_decel_ramp(self.TEST_PROFILE_ID) + self.assertLess(abs(decel_ramp - test_decel_ramp), 0.001) + print(f"Profile deceleration ramp set to: {decel_ramp} seconds") + + finally: + # Restore original deceleration ramp + await self.robot.set_profile_decel_ramp(self.TEST_PROFILE_ID, original_decel_ramp) + + async def test_get_profile_in_range(self) -> None: + """Test get_profile_in_range()""" + in_range = await self.robot.get_profile_in_range(self.TEST_PROFILE_ID) + self.assertIsInstance(in_range, float) + self.assertGreaterEqual(in_range, -1) + self.assertLessEqual(in_range, 100) + print(f"Profile {self.TEST_PROFILE_ID} InRange: {in_range}") + + async def test_set_profile_in_range(self) -> None: + """Test set_profile_in_range()""" + original_in_range = await self.robot.get_profile_in_range(self.TEST_PROFILE_ID) + + try: + # Test setting different InRange values + test_in_range = 50.0 + await self.robot.set_profile_in_range(self.TEST_PROFILE_ID, test_in_range) + + in_range = await self.robot.get_profile_in_range(self.TEST_PROFILE_ID) + self.assertLess(abs(in_range - test_in_range), 0.001) + print(f"Profile InRange set to: {in_range}") + + # Test boundary values + await self.robot.set_profile_in_range(self.TEST_PROFILE_ID, -1.0) + in_range = await self.robot.get_profile_in_range(self.TEST_PROFILE_ID) + self.assertLess(abs(in_range - (-1.0)), 0.001) + + await self.robot.set_profile_in_range(self.TEST_PROFILE_ID, 100.0) + in_range = await self.robot.get_profile_in_range(self.TEST_PROFILE_ID) + self.assertLess(abs(in_range - 100.0), 0.001) + + finally: + # Restore original InRange + await self.robot.set_profile_in_range(self.TEST_PROFILE_ID, original_in_range) + + async def test_get_profile_straight(self) -> None: + """Test get_profile_straight()""" + straight = await self.robot.get_profile_straight(self.TEST_PROFILE_ID) + self.assertIsInstance(straight, bool) + print( + f"Profile {self.TEST_PROFILE_ID} Straight: {straight} ({'straight-line' if straight else 'joint-based'} path)" + ) + + async def test_set_profile_straight(self) -> None: + """Test set_profile_straight()""" + original_straight = await self.robot.get_profile_straight(self.TEST_PROFILE_ID) + + try: + # Test setting different straight mode + test_straight = not original_straight + await self.robot.set_profile_straight(self.TEST_PROFILE_ID, test_straight) + + straight = await self.robot.get_profile_straight(self.TEST_PROFILE_ID) + self.assertEqual(straight, test_straight) + print( + f"Profile Straight set to: {straight} ({'straight-line' if straight else 'joint-based'} path)" + ) + + finally: + # Restore original straight mode + await self.robot.set_profile_straight(self.TEST_PROFILE_ID, original_straight) + + async def test_get_motion_profile_values(self) -> None: + """Test get_motion_profile_values()""" + profile_data = await self.robot.get_motion_profile_values(self.TEST_PROFILE_ID) + self.assertIsInstance(profile_data, tuple) + self.assertEqual(len(profile_data), 9) + + ( + profile_id, + speed, + speed2, + accel, + decel, + accel_ramp, + decel_ramp, + in_range, + straight, + ) = profile_data + + self.assertEqual(profile_id, self.TEST_PROFILE_ID) + self.assertIsInstance(speed, float) + self.assertGreaterEqual(speed, 0) + self.assertIsInstance(speed2, float) + self.assertGreaterEqual(speed2, 0) + self.assertIsInstance(accel, float) + self.assertGreaterEqual(accel, 0) + self.assertIsInstance(decel, float) + self.assertGreaterEqual(decel, 0) + self.assertIsInstance(accel_ramp, float) + self.assertGreaterEqual(accel_ramp, 0) + self.assertIsInstance(decel_ramp, float) + self.assertGreaterEqual(decel_ramp, 0) + self.assertIsInstance(in_range, float) + self.assertGreaterEqual(in_range, -1) + self.assertLessEqual(in_range, 100) + self.assertIsInstance(straight, bool) + + print( + f"Motion profile {self.TEST_PROFILE_ID}: speed={speed}%, speed2={speed2}%, accel={accel}%, decel={decel}%" + ) + print( + f" accel_ramp={accel_ramp}s, decel_ramp={decel_ramp}s, in_range={in_range}, straight={straight}" + ) + + async def test_set_motion_profile_values(self) -> None: + """Test set_motion_profile_values()""" + # Get original profile values + original_profile = await self.robot.get_motion_profile_values(self.TEST_PROFILE_ID) + + try: + # Test setting complete motion profile + test_values = { + "speed": 60.0, + "speed2": 15.0, + "acceleration": 70.0, + "deceleration": 75.0, + "acceleration_ramp": 0.4, + "deceleration_ramp": 0.2, + "in_range": 25.0, + "straight": True, + } + + await self.robot.set_motion_profile_values( + self.TEST_PROFILE_ID, + test_values["speed"], + test_values["speed2"], + test_values["acceleration"], + test_values["deceleration"], + test_values["acceleration_ramp"], + test_values["deceleration_ramp"], + test_values["in_range"], + bool(test_values["straight"]), + ) + + # Verify the values were set + profile_data = await self.robot.get_motion_profile_values(self.TEST_PROFILE_ID) + ( + profile_id, + speed, + speed2, + accel, + decel, + accel_ramp, + decel_ramp, + in_range, + straight, + ) = profile_data + + self.assertLess(abs(speed - test_values["speed"]), 0.001) + self.assertLess(abs(speed2 - test_values["speed2"]), 0.001) + self.assertLess(abs(accel - test_values["acceleration"]), 0.001) + self.assertLess(abs(decel - test_values["deceleration"]), 0.001) + self.assertLess(abs(accel_ramp - test_values["acceleration_ramp"]), 0.001) + self.assertLess(abs(decel_ramp - test_values["deceleration_ramp"]), 0.001) + self.assertLess(abs(in_range - test_values["in_range"]), 0.001) + self.assertEqual(straight, test_values["straight"]) + + print(f"Motion profile values set successfully: {profile_data}") + + finally: + # Restore original profile values + ( + _, + orig_speed, + orig_speed2, + orig_accel, + orig_decel, + orig_accel_ramp, + orig_decel_ramp, + orig_in_range, + orig_straight, + ) = original_profile + await self.robot.set_motion_profile_values( + self.TEST_PROFILE_ID, + orig_speed, + orig_speed2, + orig_accel, + orig_decel, + orig_accel_ramp, + orig_decel_ramp, + orig_in_range, + orig_straight, + ) + + # region MOTION COMMANDS + + async def test_move(self) -> None: + """Test move() command""" + # Save test location + await self.robot.set_location_xyz(self.TEST_LOCATION_ID, *self.TEST_LOCATION_C_LEFT[:-1]) + await self.robot.set_location_config( + self.TEST_LOCATION_ID, self.TEST_LOCATION_C_LEFT[-1] + ) # GPL_Lefty + + # Move to test location + await self.robot.move(self.TEST_LOCATION_ID, self.TEST_PROFILE_ID) + await self.robot.wait_for_eom() + + # Verify we moved to the test location + new_position = await self.robot.where_c() + self.assertLess(abs(new_position[0] - self.TEST_LOCATION_C_LEFT[0]), 2.0) + self.assertLess(abs(new_position[1] - self.TEST_LOCATION_C_LEFT[1]), 2.0) + self.assertLess(abs(new_position[2] - self.TEST_LOCATION_C_LEFT[2]), 2.0) + print(f"Move to location {self.TEST_LOCATION_ID} completed successfully") + + async def test_halt(self) -> None: + """Test halt() command""" + # Start a movement + await self.robot.move_j(self.TEST_PROFILE_ID, *self.TEST_LOCATION_J_RIGHT) + + # Immediately halt the movement + await self.robot.halt() + print("Halt command executed successfully") + + async def test_move_appro(self) -> None: + """Test move_appro() command""" + + test_z_clearance = 20 + + # Save test location & z clearance + await self.robot.set_location_xyz(self.TEST_LOCATION_ID, *self.TEST_LOCATION_C_LEFT[:-1]) + await self.robot.set_location_z_clearance(self.TEST_LOCATION_ID, test_z_clearance) + + # Move to test location with approach + await self.robot.move_appro(self.TEST_LOCATION_ID, self.TEST_PROFILE_ID) + await self.robot.wait_for_eom() + + print( + f"Move approach to location {self.TEST_LOCATION_ID} with z-clearance {test_z_clearance} completed successfully" + ) + + async def test_move_extra_axis(self) -> None: + """Test move_extra_axis() command""" + # Test with single axis - very small movement (5mm) + await self.robot.move_extra_axis(5.0) + print("Move extra axis (single) command executed successfully") + + # Test with two axes - very small movements (5mm each) + await self.robot.move_extra_axis(5.0, 5.0) + print("Move extra axis (dual) command executed successfully") + + async def test_move_one_axis(self) -> None: + """Test move_one_axis() command""" + # Get current joint positions for restoration + original_joints = await self.robot.where_j() + + try: + # Test moving axis 1 by a very small amount (2 degrees) + test_axis = 1 + current_position = original_joints[test_axis - 1] # Convert to 0-based index + new_position = current_position + 2.0 # Move only 2 degrees + + await self.robot.move_one_axis(test_axis, new_position, self.TEST_PROFILE_ID) + await self.robot.wait_for_eom() + + # Verify the axis moved + new_joints = await self.robot.where_j() + self.assertLess(abs(new_joints[test_axis - 1] - new_position), 1.0) + print(f"Move one axis {test_axis} to {new_position} completed successfully") + + finally: + # Restore original position + await self.robot.move_j(self.TEST_PROFILE_ID, *original_joints) + await self.robot.wait_for_eom() + + # async def test_where_c_loop(self) -> None: + # """Test where_c() command loop""" + + # for i in range(20): + # new_position = await self.robot.where_c() + # print(f"POS_{i+1}: {new_position}") + + # async def test_move_c_loop(self) -> None: + # """Test move_c() command loop""" + + # for i in range(20): + # await self.robot.move_c(self.TEST_PROFILE_ID, *self.TEST_LOCATION_C_RIGHT) + # await self.robot.move_c(self.TEST_PROFILE_ID, *self.TEST_LOCATION_C_LEFT) + # await self.robot.wait_for_eom() + # try: + # new_position = await self.robot.where_c() + # except Exception as e: + # print(f"Failed to get Cartesian position after move_c: {e}") + # await self.robot.wait_for_eom() + # new_position = await self.robot.where_c() + # print(f"POS_{i+1}: {new_position}") + + async def test_move_c(self) -> None: + """Test move_c() command""" + + await self.robot.move_c(self.TEST_PROFILE_ID, *self.TEST_LOCATION_C_LEFT) + await self.robot.wait_for_eom() + + # Verify position + new_position = await self.robot.where_c() + + self.assertLess(abs(new_position[0] - self.TEST_LOCATION_C_LEFT[0]), 2.0) + self.assertLess(abs(new_position[1] - self.TEST_LOCATION_C_LEFT[1]), 2.0) + self.assertLess(abs(new_position[2] - self.TEST_LOCATION_C_LEFT[2]), 2.0) + print("Move Cartesian completed successfully") + + async def test_move_j(self) -> None: + """Test move_j() command""" + # Record current joint positions for restoration + original_joints = await self.robot.where_j() + + try: + # Create test joint positions (very small movements - 1 degree each) + test_joints = tuple(joint + 1.0 for joint in original_joints) + + await self.robot.move_j(self.TEST_PROFILE_ID, *test_joints) + await self.robot.wait_for_eom() + + # Verify joint positions + new_joints = await self.robot.where_j() + + for i, (expected, actual) in enumerate(zip(test_joints, new_joints)): + if i > 4 and original_joints[i] == 0.0: # not all robots have 6 axes + if abs(expected - actual) >= 1.0: + print(f"Warning: Joint {i+1} position mismatch: expected {expected}, got {actual}") + else: + self.assertLess(abs(expected - actual), 1.0, f"Joint {i+1} position mismatch") + + print("Move joints completed successfully") + + finally: + # Return to original position + await self.robot.move_j(self.TEST_PROFILE_ID, *original_joints) + await self.robot.wait_for_eom() + + async def test_release_brake(self) -> None: + """Test release_brake() command""" + test_axis = 1 + + # Release brake on test axis + await self.robot.release_brake(test_axis) + print(f"Brake released on axis {test_axis} successfully") + + # Note: In a real test environment, you might want to check brake status + # but that would require additional API methods + + async def test_set_brake(self) -> None: + """Test set_brake() command""" + test_axis = 1 + + # Set brake on test axis + await self.robot.set_brake(test_axis) + print(f"Brake set on axis {test_axis} successfully") + + # Release the brake again for safety + await self.robot.release_brake(test_axis) + + async def test_state(self) -> None: + """Test state() command""" + motion_state = await self.robot.state() + self.assertIsInstance(motion_state, str) + self.assertGreater(len(motion_state), 0) + print(f"Motion state: {motion_state}") + + async def test_wait_for_eom(self) -> None: + """Test wait_for_eom() command""" + # Get current position and make a very small movement (1mm) + current_pos = await self.robot.where_c() + x, y, z, yaw, pitch, roll, config = current_pos + + # Start a very small movement + await self.robot.move_c(self.TEST_PROFILE_ID, x + 1, y, z, yaw, pitch, roll) + + # Wait for end of motion + await self.robot.wait_for_eom() + print("Wait for end of motion completed successfully") + + # Return to original position + await self.robot.move_c(self.TEST_PROFILE_ID, x, y, z, yaw, pitch, roll) + await self.robot.wait_for_eom() + + async def test_zero_torque(self) -> None: + """Test zero_torque() command""" + test_axis_mask = 1 # Enable zero torque for axis 1 + + try: + # Enable zero torque mode for axis 1 + await self.robot.zero_torque(True, test_axis_mask) + print(f"Zero torque enabled for axis mask {test_axis_mask}") + + # Brief pause to allow the mode to take effect + await asyncio.sleep(0.5) + + finally: + # Disable zero torque mode for safety + await self.robot.zero_torque(False) + print("Zero torque mode disabled") + + # region PAROBOT COMMANDS + async def test_change_config(self) -> None: + """Test change_config() command""" + # Record current config for restoration + original_config = await self.robot.get_location_config(self.TEST_LOCATION_ID) + _, orig_config_value = original_config + + try: + # Test with default grip mode (no gripper change) + await self.robot.change_config() + print("Change config (default) executed successfully") + + # Test with gripper open + await self.robot.change_config(1) + print("Change config with gripper open executed successfully") + + # Test with gripper close + await self.robot.change_config(2) + print("Change config with gripper close executed successfully") + + finally: + # Allow time for robot to settle and restore original config if needed + await asyncio.sleep(1.0) + + async def test_change_config2(self) -> None: + """Test change_config2() command""" + try: + # Test with default grip mode (no gripper change) + await self.robot.change_config2() + print("Change config2 (default) executed successfully") + + # Test with gripper open + await self.robot.change_config2(1) + print("Change config2 with gripper open executed successfully") + + # Test with gripper close + await self.robot.change_config2(2) + print("Change config2 with gripper close executed successfully") + + finally: + # Allow time for robot to settle + await asyncio.sleep(1.0) + + async def test_get_grasp_data(self) -> None: + """Test get_grasp_data()""" + grasp_data = await self.robot.get_grasp_data() + self.assertIsInstance(grasp_data, tuple) + self.assertEqual(len(grasp_data), 3) + plate_width, finger_speed, grasp_force = grasp_data + self.assertIsInstance(plate_width, float) + self.assertIsInstance(finger_speed, float) + self.assertIsInstance(grasp_force, float) + print( + f"Grasp data: plate_width={plate_width}mm, finger_speed={finger_speed}%, grasp_force={grasp_force}N" + ) + + async def test_set_grasp_data(self) -> None: + """Test set_grasp_data()""" + # Get original grasp data for restoration + original_grasp = await self.robot.get_grasp_data() + + try: + # Test setting grasp data + test_plate_width = 10.5 + test_finger_speed = 75.0 + test_grasp_force = 20.0 + + await self.robot.set_grasp_data(test_plate_width, test_finger_speed, test_grasp_force) + + # Verify the data was set + new_grasp = await self.robot.get_grasp_data() + plate_width, finger_speed, grasp_force = new_grasp + + self.assertLess(abs(plate_width - test_plate_width), 0.001) + self.assertLess(abs(finger_speed - test_finger_speed), 0.001) + self.assertLess(abs(grasp_force - test_grasp_force), 0.001) + print(f"Grasp data set successfully: {new_grasp}") + + finally: + # Restore original grasp data + await self.robot.set_grasp_data(*original_grasp) + + async def test_get_grip_close_pos(self) -> None: + """Test get_grip_close_pos()""" + close_pos = await self.robot.get_grip_close_pos() + self.assertIsInstance(close_pos, float) + print(f"Gripper close position: {close_pos}") + + async def test_set_grip_close_pos(self) -> None: + """Test set_grip_close_pos()""" + # Get original close position for restoration + original_close_pos = await self.robot.get_grip_close_pos() + + try: + # Test setting close position + test_close_pos = original_close_pos + 5.0 + await self.robot.set_grip_close_pos(test_close_pos) + + # Verify the position was set + new_close_pos = await self.robot.get_grip_close_pos() + self.assertLess(abs(new_close_pos - test_close_pos), 0.001) + print(f"Gripper close position set to: {new_close_pos}") + + finally: + # Restore original close position + await self.robot.set_grip_close_pos(original_close_pos) + + async def test_get_grip_open_pos(self) -> None: + """Test get_grip_open_pos()""" + open_pos = await self.robot.get_grip_open_pos() + self.assertIsInstance(open_pos, float) + print(f"Gripper open position: {open_pos}") + + async def test_set_grip_open_pos(self) -> None: + """Test set_grip_open_pos()""" + # Get original open position for restoration + original_open_pos = await self.robot.get_grip_open_pos() + + try: + # Test setting open position + test_open_pos = original_open_pos + 5.0 + await self.robot.set_grip_open_pos(test_open_pos) + + # Verify the position was set + new_open_pos = await self.robot.get_grip_open_pos() + self.assertLess(abs(new_open_pos - test_open_pos), 0.001) + print(f"Gripper open position set to: {new_open_pos}") + + finally: + # Restore original open position + await self.robot.set_grip_open_pos(original_open_pos) + + async def test_gripper(self) -> None: + """Test gripper() command""" + # Test opening gripper + await self.robot.gripper(1) + print("Gripper opened successfully") + + # Brief delay to allow gripper to move + await asyncio.sleep(0.5) + + # Test closing gripper + await self.robot.gripper(2) + print("Gripper closed successfully") + + # Test invalid grip mode + with self.assertRaises(ValueError): + await self.robot.gripper(3) + + async def test_move_to_safe(self) -> None: + """Test move_to_safe() command""" + # Record current position for comparison + original_position = await self.robot.where_c() + + # Move to safe position + await self.robot.move_to_safe() + await self.robot.wait_for_eom() + + # Verify we moved to a different position + safe_position = await self.robot.where_c() + position_changed = any( + abs(orig - safe) > 1.0 for orig, safe in zip(original_position[:6], safe_position[:6]) + ) + + print("Move to safe position completed successfully") + print(f"Position changed: {position_changed}") + + async def test_set_pallet_index(self) -> None: + """Test set_pallet_index() and get_pallet_index()""" + # Get original station type to check if it's a pallet + original_station = await self.robot.get_station_type(self.TEST_LOCATION_ID) + ( + _, + orig_access_type, + orig_location_type, + orig_z_clearance, + orig_z_above, + orig_z_grasp_offset, + ) = original_station + + was_pallet = orig_location_type == 1 + original_pallet = None + + try: + # If it's already a pallet, get the original pallet index + if was_pallet: + original_pallet = await self.robot.get_pallet_index(self.TEST_LOCATION_ID) + _, orig_x, orig_y, orig_z = original_pallet + print( + f"Station {self.TEST_LOCATION_ID} is already pallet type with index X={orig_x}, Y={orig_y}, Z={orig_z}" + ) + else: + # Convert to pallet type first + await self.robot.set_station_type( + self.TEST_LOCATION_ID, + orig_access_type, + 1, + orig_z_clearance, + orig_z_above, + orig_z_grasp_offset, + ) + print(f"Station {self.TEST_LOCATION_ID} converted to pallet type for testing") + + # Test get_pallet_index() + pallet_data = await self.robot.get_pallet_index(self.TEST_LOCATION_ID) + self.assertIsInstance(pallet_data, tuple) + self.assertEqual(len(pallet_data), 4) + station_id, pallet_x, pallet_y, pallet_z = pallet_data + self.assertEqual(station_id, self.TEST_LOCATION_ID) + self.assertIsInstance(pallet_x, int) + self.assertIsInstance(pallet_y, int) + self.assertIsInstance(pallet_z, int) + print( + f"Current pallet index for station {station_id}: X={pallet_x}, Y={pallet_y}, Z={pallet_z}" + ) + + # Test setting all indices + test_x, test_y, test_z = 1, 1, 1 + await self.robot.set_pallet_index(self.TEST_LOCATION_ID, test_x, test_y, test_z) + + # Verify the indices were set + new_pallet = await self.robot.get_pallet_index(self.TEST_LOCATION_ID) + _, pallet_x, pallet_y, pallet_z = new_pallet + self.assertEqual(pallet_x, test_x) + self.assertEqual(pallet_y, test_y) + self.assertEqual(pallet_z, test_z) + print(f"Pallet index set successfully: X={pallet_x}, Y={pallet_y}, Z={pallet_z}") + + # Test setting only X index + await self.robot.set_pallet_index(self.TEST_LOCATION_ID, pallet_index_x=1) + new_pallet = await self.robot.get_pallet_index(self.TEST_LOCATION_ID) + _, pallet_x, _, _ = new_pallet + self.assertEqual(pallet_x, 1) + print(f"Pallet X index set to: {pallet_x}") + + finally: + # Restore everything to original state + if was_pallet and original_pallet: + # Restore original pallet indices + _, orig_x, orig_y, orig_z = original_pallet + await self.robot.set_pallet_index(self.TEST_LOCATION_ID, orig_x, orig_y, orig_z) + print(f"Restored original pallet indices: X={orig_x}, Y={orig_y}, Z={orig_z}") + else: + # Convert back to original station type (not pallet) + await self.robot.set_station_type( + self.TEST_LOCATION_ID, + orig_access_type, + orig_location_type, + orig_z_clearance, + orig_z_above, + orig_z_grasp_offset, + ) + print(f"Station {self.TEST_LOCATION_ID} restored to original non-pallet type") + + async def test_get_and_set_pallet_origin_and_setup(self) -> None: + """Test set_pallet_origin() and comprehensive pallet configuration""" + + # Test setting complete pallet configuration + test_origin = (100.0, 200.0, 300.0, 10.0, 20.0, 30.0, 1) # Include config + test_x_count, test_x_offset = ( + 3, + (test_origin[0] + 10.0, test_origin[1], test_origin[2]), + ) # X direction offset + test_y_count, test_y_offset = ( + 4, + (test_origin[0], test_origin[1] + 15.0, test_origin[2]), + ) # Y direction offset + test_z_count, test_z_offset = ( + 2, + (test_origin[0], test_origin[1], test_origin[2] + 8.0), + ) # Z direction offset + + # Set config as a pallet + await self.robot.set_station_type(self.TEST_LOCATION_ID, 0, 1, 20, 1, 0) + + # Set pallet origin first + await self.robot.set_pallet_origin(self.TEST_LOCATION_ID, *test_origin) + print(f"Pallet origin set to: {test_origin}") + + # Set pallet X configuration + await self.robot.set_pallet_x(self.TEST_LOCATION_ID, test_x_count, *test_x_offset) + print(f"Pallet X set: count={test_x_count}, offset={test_x_offset}") + + # Set pallet Y configuration + await self.robot.set_pallet_y(self.TEST_LOCATION_ID, test_y_count, *test_y_offset) + print(f"Pallet Y set: count={test_y_count}, offset={test_y_offset}") + + # Set pallet Z configuration + await self.robot.set_pallet_z(self.TEST_LOCATION_ID, test_z_count, *test_z_offset) + print(f"Pallet Z set: count={test_z_count}, offset={test_z_offset}") + + # Verify all configurations were set correctly + new_origin = await self.robot.get_pallet_origin(self.TEST_LOCATION_ID) + new_pallet_x = await self.robot.get_pallet_x(self.TEST_LOCATION_ID) + new_pallet_y = await self.robot.get_pallet_y(self.TEST_LOCATION_ID) + new_pallet_z = await self.robot.get_pallet_z(self.TEST_LOCATION_ID) + + # Verify origin + _, x, y, z, yaw, pitch, roll, config = new_origin + for i, (expected, actual) in enumerate(zip(test_origin[:-1], (x, y, z, yaw, pitch, roll))): + self.assertLess(abs(expected - actual), 0.001, f"Origin coordinate {i} mismatch") + self.assertEqual(config, test_origin[-1]) + + # Verify X configuration + _, x_count, world_x, world_y, world_z = new_pallet_x + self.assertEqual(x_count, test_x_count) + for i, (expected, actual) in enumerate(zip(test_x_offset, (world_x, world_y, world_z))): + self.assertLess(abs(expected - actual), 0.001, f"Pallet X offset {i} mismatch") + + # Verify Y configuration + _, y_count, world_x, world_y, world_z = new_pallet_y + self.assertEqual(y_count, test_y_count) + for i, (expected, actual) in enumerate(zip(test_y_offset, (world_x, world_y, world_z))): + self.assertLess(abs(expected - actual), 0.001, f"Pallet Y offset {i} mismatch") + + # Verify Z configuration + _, z_count, world_x, world_y, world_z = new_pallet_z + self.assertEqual(z_count, test_z_count) + for i, (expected, actual) in enumerate(zip(test_z_offset, (world_x, world_y, world_z))): + self.assertLess(abs(expected - actual), 0.001, f"Pallet Z offset {i} mismatch") + + # Test that pallet indexing works correctly by setting different indices + # and verifying the calculated positions change appropriately + test_indices = [(1, 1, 1), (2, 1, 1), (1, 2, 1), (1, 1, 2)] + expected_offsets = [ + (0, 0, 0), # Base position (index 1,1,1) + (test_x_offset[0], test_x_offset[1], test_x_offset[2]), # X+1 + (test_y_offset[0], test_y_offset[1], test_y_offset[2]), # Y+1 + (test_z_offset[0], test_z_offset[1], test_z_offset[2]), # Z+1 + ] + + for (x_idx, y_idx, z_idx), (exp_x_off, exp_y_off, exp_z_off) in zip( + test_indices, expected_offsets + ): + # Set the pallet index + await self.robot.set_pallet_index(self.TEST_LOCATION_ID, x_idx, y_idx, z_idx) + + # Verify the index was set + pallet_index = await self.robot.get_pallet_index(self.TEST_LOCATION_ID) + _, curr_x_idx, curr_y_idx, curr_z_idx = pallet_index + self.assertEqual((curr_x_idx, curr_y_idx, curr_z_idx), (x_idx, y_idx, z_idx)) + + # Calculate expected position based on origin + index offsets + expected_x = ( + test_origin[0] + + (x_idx - 1) * test_x_offset[0] + + (y_idx - 1) * test_y_offset[0] + + (z_idx - 1) * test_z_offset[0] + ) + expected_y = ( + test_origin[1] + + (x_idx - 1) * test_x_offset[1] + + (y_idx - 1) * test_y_offset[1] + + (z_idx - 1) * test_z_offset[1] + ) + expected_z = ( + test_origin[2] + + (x_idx - 1) * test_x_offset[2] + + (y_idx - 1) * test_y_offset[2] + + (z_idx - 1) * test_z_offset[2] + ) + + print( + f"Index ({x_idx},{y_idx},{z_idx}) -> Expected position: ({expected_x:.1f}, {expected_y:.1f}, {expected_z:.1f})" + ) + + print("Complete pallet configuration test passed successfully") + + async def test_pick_plate_station(self) -> None: + """Test pick_plate_station() command""" + + # Save test location - force it to Cartesian type for this test + await self.robot.set_location_xyz(self.TEST_LOCATION_ID, *self.TEST_LOCATION_C_LEFT[:-1]) + await self.robot.set_location_z_clearance(self.TEST_LOCATION_ID, 20.0) + + # Test basic pick without compliance + result = await self.robot.pick_plate_station(self.TEST_LOCATION_ID) + self.assertIsInstance(result, bool) + print(f"Pick plate station (basic) result: {result}") + + # Test pick with horizontal compliance + result = await self.robot.pick_plate_station( + self.TEST_LOCATION_ID, horizontal_compliance=True, horizontal_compliance_torque=50 + ) + self.assertIsInstance(result, bool) + print(f"Pick plate station (with compliance) result: {result}") + + async def test_place_plate_station(self) -> None: + """Test place_plate_station() command""" + + # Test basic place without compliance + await self.robot.set_location_xyz(self.TEST_LOCATION_ID, *self.TEST_LOCATION_C_LEFT[:-1]) + await self.robot.set_location_z_clearance(self.TEST_LOCATION_ID, 20.0) + + await self.robot.place_plate_station(self.TEST_LOCATION_ID) + print("Place plate station (basic) executed successfully") + + # Test place with horizontal compliance + await self.robot.place_plate_station( + self.TEST_LOCATION_ID, horizontal_compliance=True, horizontal_compliance_torque=25 + ) + print("Place plate station (with compliance) executed successfully") + + async def test_teach_plate_station(self) -> None: + """Test teach_plate_station() command""" + + # Test teaching with default Z clearance + await self.robot.teach_plate_station(self.TEST_LOCATION_ID) + print(f"Plate station {self.TEST_LOCATION_ID} taught with default clearance") + + # Test teaching with custom Z clearance + test_clearance = 75.0 + await self.robot.teach_plate_station(self.TEST_LOCATION_ID, test_clearance) + + # Verify the clearance was set + new_clearance = await self.robot.get_location_z_clearance(self.TEST_LOCATION_ID) + _, z_clearance, _ = new_clearance + self.assertLess(abs(z_clearance - test_clearance), 0.001) + print(f"Plate station taught with custom clearance: {z_clearance}") + + async def test_get_station_type(self) -> None: + """Test get_station_type()""" + station_data = await self.robot.get_station_type(self.TEST_LOCATION_ID) + self.assertIsInstance(station_data, tuple) + self.assertEqual(len(station_data), 6) + station_id, access_type, location_type, z_clearance, z_above, z_grasp_offset = station_data + + self.assertEqual(station_id, self.TEST_LOCATION_ID) + self.assertIn(access_type, [0, 1]) # 0 = horizontal, 1 = vertical + self.assertIn(location_type, [0, 1]) # 0 = normal, 1 = pallet + self.assertIsInstance(z_clearance, float) + self.assertIsInstance(z_above, float) + self.assertIsInstance(z_grasp_offset, float) + + access_str = "horizontal" if access_type == 0 else "vertical" + location_str = "normal single" if location_type == 0 else "pallet" + print( + f"Station {station_id}: access={access_str}, type={location_str}, clearance={z_clearance}, above={z_above}, grasp_offset={z_grasp_offset}" + ) + + async def test_set_station_type(self) -> None: + """Test set_station_type()""" + + # Test setting station type + test_values = ( + 0, # access_type: horizontal + 1, # location_type: pallet + 60.0, # z_clearance + 15.0, # z_above + 5.0, # z_grasp_offset + ) + + await self.robot.set_station_type(self.TEST_LOCATION_ID, *test_values) + + # Verify the station type was set + new_station = await self.robot.get_station_type(self.TEST_LOCATION_ID) + _, access_type, location_type, z_clearance, z_above, z_grasp_offset = new_station + + self.assertEqual(access_type, test_values[0]) + self.assertEqual(location_type, test_values[1]) + self.assertLess(abs(z_clearance - test_values[2]), 0.001) + self.assertLess(abs(z_above - test_values[3]), 0.001) + self.assertLess(abs(z_grasp_offset - test_values[4]), 0.001) + + print(f"Station type set successfully: {test_values}") + + # Test invalid access type + with self.assertRaises(ValueError): + await self.robot.set_station_type(self.TEST_LOCATION_ID, 3, 0, 50.0, 10.0, 0.0) + + # Test invalid location type + with self.assertRaises(ValueError): + await self.robot.set_station_type(self.TEST_LOCATION_ID, 0, 3, 50.0, 10.0, 0.0) + + # region SSGRIP COMMANDS + # async def test_home_all_if_no_plate(self) -> None: # commented out because it messes up other tests + # """Test home_all_if_no_plate()""" + # result = await self.robot.home_all_if_no_plate() + # self.assertIsInstance(result, int) + # self.assertIn(result, [-1, 0]) + + # result_str = "no plate detected and command succeeded" if result == -1 else "plate detected" + # print(f"Home all if no plate result: {result} ({result_str})") + + async def test_grasp_plate(self) -> None: + """Test grasp_plate()""" + # Test with valid parameters for closing gripper + result = await self.robot.grasp_plate(15.0, 50, 10.0) + self.assertIsInstance(result, int) + self.assertIn(result, [-1, 0]) + + result_str = "plate grasped" if result == -1 else "no plate detected" + print(f"Grasp plate (close) result: {result} ({result_str})") + + # Test with negative force for opening gripper + result = await self.robot.grasp_plate(25.0, 75, -5.0) + self.assertIsInstance(result, int) + self.assertIn(result, [-1, 0]) + print(f"Grasp plate (open) result: {result}") + + # Test invalid finger speed + with self.assertRaises(ValueError): + await self.robot.grasp_plate(15.0, 0, 10.0) # Speed too low + + with self.assertRaises(ValueError): + await self.robot.grasp_plate(15.0, 101, 10.0) # Speed too high + + async def test_release_plate(self) -> None: + """Test release_plate()""" + # Test basic release + await self.robot.release_plate(100.0, 50) + print("Release plate (basic) executed successfully") + + # Test release with InRange + await self.robot.release_plate(120.0, 75, 5.0) + print("Release plate (with InRange) executed successfully") + + # Test invalid finger speed + with self.assertRaises(ValueError): + await self.robot.release_plate(100.0, 0) # Speed too low + + with self.assertRaises(ValueError): + await self.robot.release_plate(100.0, 101) # Speed too high + + async def test_is_fully_closed(self) -> None: + """Test is_fully_closed()""" + closed_state = await self.robot.is_fully_closed() + self.assertIsInstance(closed_state, int) + print(f"Gripper fully closed state: {closed_state}") + + # For standard gripper: -1 means within 2mm of fully closed, 0 means not + # For dual gripper: bitmask where bit 0 = gripper 1, bit 1 = gripper 2 + if closed_state in [-1, 0]: + state_str = "within 2mm of fully closed" if closed_state == -1 else "not fully closed" + print(f"Standard gripper state: {state_str}") + else: + gripper1_closed = bool(closed_state & 1) + gripper2_closed = bool(closed_state & 2) + print( + f"Dual gripper state: Gripper 1 {'closed' if gripper1_closed else 'open'}, Gripper 2 {'closed' if gripper2_closed else 'open'}" + ) + + async def test_get_active_gripper(self) -> None: + """Test get_active_gripper() (Dual Gripper Only)""" + try: + active_gripper = await self.robot.get_active_gripper() + self.assertIsInstance(active_gripper, int) + self.assertIn(active_gripper, [1, 2]) + + gripper_name = "A" if active_gripper == 1 else "B" + print(f"Active gripper: {active_gripper} (Gripper {gripper_name})") + except Exception as e: + print(f"Dual gripper not available, skipping get_active_gripper test: {e}") + + async def test_free_mode(self) -> None: + """Test free_mode()""" + try: + # Test enabling free mode for all axes + await self.robot.free_mode(True, 0) + print("Free mode enabled for all axes") + + # Brief pause to allow mode to take effect + await asyncio.sleep(0.5) + + # Test enabling free mode for specific axis + await self.robot.free_mode(True, 1) + print("Free mode enabled for axis 1") + + # Brief pause + await asyncio.sleep(0.5) + + finally: + # Always disable free mode for safety + await self.robot.free_mode(False) + print("Free mode disabled for all axes") + + async def test_open_gripper(self) -> None: + """Test open_gripper()""" + await self.robot.open_gripper() + print("Gripper opened successfully") + + # Brief delay to allow gripper to move + await asyncio.sleep(0.5) + + async def test_close_gripper(self) -> None: + """Test close_gripper()""" + await self.robot.close_gripper() + print("Gripper closed successfully") + + # Brief delay to allow gripper to move + await asyncio.sleep(0.5) + + async def test_pick_plate(self) -> None: + """Test pick_plate()""" + + # set test location + await self.robot.set_location_xyz(self.TEST_LOCATION_ID, *self.TEST_LOCATION_C_LEFT[:-1]) + await self.robot.set_location_config(self.TEST_LOCATION_ID, self.TEST_LOCATION_C_LEFT[-1]) + await self.robot.set_location_z_clearance(self.TEST_LOCATION_ID, 20.0) + + # Test basic pick without compliance + try: + await self.robot.pick_plate(self.TEST_LOCATION_ID) + except Exception as e: + if "no plate present" in str(e).lower(): + print("No plate present - this is fine for testing") + else: + raise + print(f"Pick plate (basic) at location {self.TEST_LOCATION_ID} executed successfully") + + # Test pick with horizontal compliance + try: + await self.robot.pick_plate( + self.TEST_LOCATION_ID, horizontal_compliance=True, horizontal_compliance_torque=50 + ) + except Exception as e: + if "no plate present" in str(e).lower(): + print("No plate present - this is fine for testing") + else: + raise + print(f"Pick plate (with compliance) at location {self.TEST_LOCATION_ID} executed successfully") + + async def test_place_plate(self) -> None: + """Test place_plate()""" + # set test location + await self.robot.set_location_xyz(self.TEST_LOCATION_ID, *self.TEST_LOCATION_C_LEFT[:-1]) + await self.robot.set_location_config(self.TEST_LOCATION_ID, self.TEST_LOCATION_C_LEFT[-1]) + await self.robot.set_location_z_clearance(self.TEST_LOCATION_ID, 20.0) + + # Test basic place without compliance + await self.robot.place_plate(self.TEST_LOCATION_ID) + print(f"Place plate (basic) at location {self.TEST_LOCATION_ID} executed successfully") + + # Test place with horizontal compliance + await self.robot.place_plate( + self.TEST_LOCATION_ID, horizontal_compliance=True, horizontal_compliance_torque=25 + ) + print( + f"Place plate (with compliance) at location {self.TEST_LOCATION_ID} executed successfully" + ) + + async def test_teach_position(self) -> None: + """Test teach_position()""" + + # Test teaching with default Z clearance + await self.robot.teach_position(self.TEST_LOCATION_ID) + print(f"Position {self.TEST_LOCATION_ID} taught with default clearance (50.0)") + + # Verify the location was recorded as Cartesian type + location_data = await self.robot.get_location(self.TEST_LOCATION_ID) + type_code = location_data[0] + self.assertEqual(type_code, 0) # Should be Cartesian type + + # Test teaching with custom Z clearance + test_clearance = 75.0 + await self.robot.teach_position(self.TEST_LOCATION_ID, test_clearance) + + # Verify the clearance was set + new_clearance = await self.robot.get_location_z_clearance(self.TEST_LOCATION_ID) + _, z_clearance, _ = new_clearance + self.assertLess(abs(z_clearance - test_clearance), 0.001) + print(f"Position taught with custom clearance: {z_clearance}") + + # region ROBOT SPECIFIC TESTS + + #### + #### THESE MAY FAIL IF YOUR ROBOT DOES NOT HAVE THE FEATURE #### + #### + + async def test_move_rail(self) -> None: + """Test move_rail() command""" + # Test canceling pending move rail + await self.robot.move_rail(mode=0) + print("Move rail canceled successfully") + + # Test moving rail immediately with explicit destination + await self.robot.move_rail(station_id=1, mode=1, rail_destination=100.0) + print("Move rail immediately with destination executed successfully") + + # Test moving rail with station ID only + await self.robot.move_rail(station_id=self.TEST_LOCATION_ID, mode=1) + print("Move rail immediately with station executed successfully") + + # Test setting rail to move during next pick/place + await self.robot.move_rail(station_id=self.TEST_LOCATION_ID, mode=2) + print("Move rail during next pick/place set successfully") + + async def test_get_rail_position(self) -> None: + """Test get_rail_position()""" + rail_pos = await self.robot.get_rail_position(self.TEST_LOCATION_ID) + self.assertIsInstance(rail_pos, float) + print(f"Rail position for station {self.TEST_LOCATION_ID}: {rail_pos}") + + async def test_set_rail_position(self) -> None: + """Test set_rail_position()""" + # Test setting rail position + test_rail_pos = 100.0 + await self.robot.set_rail_position(self.TEST_LOCATION_ID, test_rail_pos) + + # Verify the position was set + new_rail_pos = await self.robot.get_rail_position(self.TEST_LOCATION_ID) + self.assertLess(abs(new_rail_pos - test_rail_pos), 0.001) + print(f"Rail position set to: {new_rail_pos}") + + async def test_set_active_gripper(self) -> None: + """Test set_active_gripper() (Dual Gripper Only)""" + # Note: This test assumes a dual gripper system + # Get original active gripper for restoration + try: + original_gripper = await self.robot.get_active_gripper() + except Exception as e: + # Skip test if dual gripper not available + print(f"Dual gripper not available, skipping set_active_gripper test: {e}") + return + + try: + # Test setting gripper 1 without spin + await self.robot.set_active_gripper(1, 0) + active_gripper = await self.robot.get_active_gripper() + self.assertEqual(active_gripper, 1) + print("Active gripper set to 1 (no spin)") + + # Test setting gripper 2 without spin + await self.robot.set_active_gripper(2, 0) + active_gripper = await self.robot.get_active_gripper() + self.assertEqual(active_gripper, 2) + print("Active gripper set to 2 (no spin)") + + # Test setting gripper with spin and profile + await self.robot.set_active_gripper(1, 1, self.TEST_PROFILE_ID) + active_gripper = await self.robot.get_active_gripper() + self.assertEqual(active_gripper, 1) + print("Active gripper set to 1 (with spin and profile)") + + # Test invalid gripper ID + with self.assertRaises(ValueError): + await self.robot.set_active_gripper(3, 0) + + # Test invalid spin mode + with self.assertRaises(ValueError): + await self.robot.set_active_gripper(1, 2) + + finally: + # Restore original active gripper + await self.robot.set_active_gripper(original_gripper, 0) diff --git a/pylabrobot/arms/precise_flex/precise_flex_backend.py b/pylabrobot/arms/precise_flex/precise_flex_backend.py new file mode 100644 index 00000000000..db5e357146d --- /dev/null +++ b/pylabrobot/arms/precise_flex/precise_flex_backend.py @@ -0,0 +1,433 @@ +from abc import ABC, abstractmethod +from typing import Optional, Union + +from pylabrobot.arms.backend import ArmBackend +from pylabrobot.arms.coords import CartesianCoords, ElbowOrientation, JointCoords +from pylabrobot.arms.precise_flex.precise_flex_api import PreciseFlexBackendApi + + +class CoordsConverter(ABC): + @abstractmethod + def convert_to_joint_space( + self, position: tuple[float, float, float, float, float, float] + ) -> JointCoords: + """Convert a tuple of joint angles to a JointSpace object.""" + ... + + @abstractmethod + def convert_to_cartesian_space( + self, position: tuple[float, float, float, float, float, float, Optional[ElbowOrientation]] + ) -> CartesianCoords: + ... + + @abstractmethod + def convert_to_joints_array( + self, position: JointCoords + ) -> tuple[float, float, float, float, float, float]: + ... + + @abstractmethod + def convert_to_cartesian_array( + self, position: CartesianCoords + ) -> tuple[float, float, float, float, float, float, int]: + """Convert a CartesianSpace object to a list of cartesian coordinates.""" + ... + + @abstractmethod + def _convert_orientation_enum_to_int(self, orientation: Optional[ElbowOrientation]) -> int: + """Convert an ElbowOrientation enum to an integer.""" + ... + + +class PreciseFlex400SpaceConverter(CoordsConverter): + def convert_to_joint_space( + self, position: tuple[float, float, float, float, float, float] + ) -> JointCoords: + """Convert a tuple of joint angles to a JointCoords object.""" + if len(position) != 6: + raise ValueError("Position must be a tuple of 6 joint angles.") + return JointCoords(0, position[0], position[1], position[2], position[3], 0) + + def convert_to_cartesian_space( + self, position: tuple[float, float, float, float, float, float, Optional[ElbowOrientation]] + ) -> CartesianCoords: + """Convert a tuple of cartesian coordinates to a CartesianCoords object.""" + if len(position) != 7: + raise ValueError( + "Position must be a tuple of 7 values (x, y, z, yaw, pitch, roll, orientation)." + ) + orientation = ElbowOrientation(position[6]) + return CartesianCoords( + position[0], position[1], position[2], position[3], position[4], position[5], orientation + ) + + def convert_to_joints_array( + self, position: JointCoords + ) -> tuple[float, float, float, float, float, float]: + """Convert a JointSpace object to a list of joint angles.""" + joints = ( + position.base, + position.shoulder, + position.elbow, + position.wrist, + 0, + 0, + ) # PF400 has 4 joints, last two are fixed + return joints + + def convert_to_cartesian_array( + self, position: CartesianCoords + ) -> tuple[float, float, float, float, float, float, int]: + """Convert a CartesianSpace object to a list of cartesian coordinates.""" + orientation_int = self._convert_orientation_enum_to_int(position.orientation) + arr = ( + position.x, + position.y, + position.z, + position.yaw, + position.pitch, + position.roll, + orientation_int, + ) + return arr + + def _convert_orientation_enum_to_int(self, orientation: Optional[ElbowOrientation]) -> int: + """Convert an ElbowOrientation enum to an integer.""" + if orientation is None: + return 0 + elif orientation == ElbowOrientation.LEFT: + return 1 + elif orientation == ElbowOrientation.RIGHT: + return 2 + else: + raise ValueError("Invalid ElbowOrientation value.") + + +class PreciseFlex3400SpaceConverter(CoordsConverter): + def convert_to_joint_space( + self, position: tuple[float, float, float, float, float, float] + ) -> JointCoords: + """Convert a tuple of joint angles to a JointCoords object.""" + if len(position) != 6: + raise ValueError("Position must be a tuple of 6 joint angles.") + return JointCoords(0, position[0], position[1], position[2], position[3], position[4]) + + def convert_to_cartesian_space( + self, position: tuple[float, float, float, float, float, float, Optional[ElbowOrientation]] + ) -> CartesianCoords: + """Convert a tuple of cartesian coordinates to a CartesianCoords object.""" + if len(position) != 7: + raise ValueError( + "Position must be a tuple of 7 values (x, y, z, yaw, pitch, roll, orientation)." + ) + orientation = ElbowOrientation(position[6]) + return CartesianCoords( + position[0], position[1], position[2], position[3], position[4], position[5], orientation + ) + + def convert_to_joints_array( + self, position: JointCoords + ) -> tuple[float, float, float, float, float, float]: + """Convert a JointSpace object to a list of joint angles.""" + joints = ( + position.base, + position.shoulder, + position.elbow, + position.wrist, + position.gripper, + 0, + ) # PF400 has 5 joints, last is fixed + return joints + + def convert_to_cartesian_array( + self, position: CartesianCoords + ) -> tuple[float, float, float, float, float, float, int]: + """Convert a CartesianSpace object to a list of cartesian coordinates.""" + orientation_int = self._convert_orientation_enum_to_int(position.orientation) + arr = ( + position.x, + position.y, + position.z, + position.yaw, + position.pitch, + position.roll, + orientation_int, + ) + return arr + + def _convert_orientation_enum_to_int(self, orientation: Optional[ElbowOrientation]) -> int: + """Convert an ElbowOrientation enum to an integer.""" + if orientation is None: + return 0 + elif orientation == ElbowOrientation.RIGHT: + return 1 + elif orientation == ElbowOrientation.LEFT: + return 2 + else: + raise ValueError("Invalid ElbowOrientation value.") + + +class CoordsConverterFactory: + @staticmethod + def create_coords_converter(model: str) -> CoordsConverter: + """Factory method to create a CoordsConverter based on the robot model.""" + if model == "pf400": + return PreciseFlex400SpaceConverter() + elif model == "pf3400": + return PreciseFlex3400SpaceConverter() + else: + raise ValueError(f"Unsupported robot model: {model}") + + +class PreciseFlexBackend(ArmBackend, ABC): + """Backend for the PreciseFlex robotic arm - Default to using Cartesian coordinates, some methods in Brook's TCS don't work with Joint coordinates.""" + + def __init__(self, model: str, host: str, port: int = 10100, timeout=20) -> None: + super().__init__() + self.api = PreciseFlexBackendApi(host=host, port=port, timeout=timeout) + self.space_converter = CoordsConverterFactory.create_coords_converter(model) + self.profile_index: int = 1 + self.location_index: int = 1 + self.horizontal_compliance: bool = False + self.horizontal_compliance_torque: int = 0 + + async def setup(self): + """Initialize the PreciseFlex backend.""" + await self.api.setup() + await self.set_pc_mode() + await self.power_on_robot() + await self.attach() + + async def stop(self): + """Stop the PreciseFlex backend.""" + await self.detach() + await self.power_off_robot() + await self.api.exit() + await self.api.stop() + + async def set_speed(self, speed_percent: float): + """Set the speed percentage of the arm's movement (0-100).""" + await self.api.set_profile_speed(self.profile_index, speed_percent) + + async def get_speed(self) -> float: + """Get the current speed percentage of the arm's movement.""" + return await self.api.get_profile_speed(self.profile_index) + + async def open_gripper(self): + """Open the gripper.""" + await self.api.open_gripper() + + async def close_gripper(self): + """Close the gripper.""" + await self.api.close_gripper() + + async def is_gripper_closed(self) -> bool: + """Check if the gripper is currently closed.""" + ret_int = await self.api.is_fully_closed() + if ret_int == -1: + return True + else: + return False + + async def halt(self): + """Stop any ongoing movement of the arm.""" + await self.api.halt() + + async def home(self): + """Homes robot.""" + await self.api.home() + + async def move_to_safe(self): + """Move the arm to a predefined safe position.""" + await self.api.move_to_safe() + + def _convert_orientation_int_to_enum(self, orientation_int: int) -> Optional[ElbowOrientation]: + if orientation_int == 1: + return ElbowOrientation.LEFT + elif orientation_int == 2: + return ElbowOrientation.RIGHT + else: + return None + + def _convert_orientation_enum_to_int(self, orientation: Optional[ElbowOrientation]) -> int: + if orientation == ElbowOrientation.LEFT: + return 1 + elif orientation == ElbowOrientation.RIGHT: + return 2 + else: + return 0 + + async def home_all(self): + """Homes all robots.""" + await self.api.home_all() + + async def attach(self): + """Attach the robot.""" + await self.api.attach(1) + + async def detach(self): + """Detach the robot.""" + await self.api.attach(0) + + async def power_on_robot(self): + """Power on the robot.""" + await self.api.set_power(True, self.api.timeout) + + async def power_off_robot(self): + """Power off the robot.""" + await self.api.set_power(False) + + async def set_pc_mode(self): + """Set the controller to PC mode.""" + await self.api.set_mode(0) + + async def select_robot(self, robot_id: int) -> None: + """Select the specified robot.""" + await self.api.select_robot(robot_id) + + async def version(self) -> str: + """Get the robot's version.""" + return await self.api.get_version() + + async def approach(self, position: Union[CartesianCoords, JointCoords], approach_height: float): + """Move the arm to a position above the specified coordinates by a certain distance.""" + if type(position) == JointCoords: + joints = self.space_converter.convert_to_joints_array(position) + await self._approach_j(joints, approach_height) + elif type(position) == CartesianCoords: + xyz = self.space_converter.convert_to_cartesian_array(position) + await self._approach_c(xyz[:-1], approach_height, xyz[-1]) + else: + raise ValueError("Position must be of type JointSpace or CartesianSpace.") + + async def pick_plate(self, position: Union[CartesianCoords, JointCoords], approach_height: float): + """Pick a plate from the specified position.""" + if type(position) == JointCoords: + raise ValueError("pick_plate only supports CartesianCoords for PreciseFlex.") + elif type(position) == CartesianCoords: + xyz = self.space_converter.convert_to_cartesian_array(position) + await self._pick_plate_c(xyz[:-1], approach_height, xyz[-1]) + else: + raise ValueError("Position must be of type JointSpace or CartesianSpace.") + + async def place_plate( + self, position: Union[CartesianCoords, JointCoords], approach_height: float + ): + """Place a plate at the specified position.""" + if type(position) == JointCoords: + raise ValueError("place_plate only supports CartesianCoords for PreciseFlex.") + elif type(position) == CartesianCoords: + xyz = self.space_converter.convert_to_cartesian_array(position) + await self._place_plate_c(xyz[:-1], approach_height, xyz[-1]) + else: + raise ValueError("Position must be of type JointSpace or CartesianSpace.") + + async def move_to(self, position: Union[CartesianCoords, JointCoords]): + """Move the arm to a specified position in 3D space.""" + if type(position) == JointCoords: + joints = self.space_converter.convert_to_joints_array(position) + await self._move_to_j(joints) + elif type(position) == CartesianCoords: + xyz = self.space_converter.convert_to_cartesian_array(position) + await self._move_to_c(xyz[:-1], xyz[-1]) + else: + raise ValueError("Position must be of type JointSpace or CartesianSpace.") + + async def get_joint_position(self) -> JointCoords: + """Get the current position of the arm in 3D space.""" + position_j = await self._get_position_j() + return self.space_converter.convert_to_joint_space(position_j) + + async def get_cartesian_position(self) -> CartesianCoords: + """Get the current position of the arm in 3D space.""" + position_c = await self._get_position_c() + return self.space_converter.convert_to_cartesian_space(position_c) + + async def _approach_j( + self, joint_position: tuple[float, float, float, float, float, float], approach_height: float + ): + """Move the arm to a position above the specified coordinates by a certain distance.""" + await self.api.set_location_angles(self.location_index, *joint_position) + await self.api.set_location_z_clearance(self.location_index, approach_height) + await self.api.move_appro(self.location_index, self.profile_index) + + async def _pick_plate_j( + self, joint_position: tuple[float, float, float, float, float, float], approach_height: float + ): + """Pick a plate from the specified position.""" + await self.api.set_location_angles(self.location_index, *joint_position) + await self.api.set_location_z_clearance(self.location_index, approach_height) + await self.api.pick_plate( + self.location_index, self.horizontal_compliance, self.horizontal_compliance_torque + ) + + async def _place_plate_j( + self, joint_position: tuple[float, float, float, float, float, float], approach_height: float + ): + """Place a plate at the specified position.""" + await self.api.set_location_angles(self.location_index, *joint_position) + await self.api.set_location_z_clearance(self.location_index, approach_height) + await self.api.place_plate( + self.location_index, self.horizontal_compliance, self.horizontal_compliance_torque + ) + + async def _move_to_j(self, joint_position: tuple[float, float, float, float, float, float]): + """Move the arm to a specified position in 3D space.""" + await self.api.move_j(self.location_index, *joint_position) + + async def _get_position_j(self) -> tuple[float, float, float, float, float, float]: + """Get the current position of the arm in 3D space.""" + return await self.api.where_j() + + async def _approach_c( + self, + cartesian_position: tuple[float, float, float, float, float, float], + approach_height: float, + orientation: int = 0, + ): + """Move the arm to a position above the specified coordinates by a certain distance.""" + await self.api.set_location_xyz(self.location_index, *cartesian_position) + await self.api.set_location_z_clearance(self.location_index, approach_height) + await self.api.set_location_config(self.location_index, orientation) + await self.api.move_appro(self.location_index, self.profile_index) + + async def _pick_plate_c( + self, + cartesian_position: tuple[float, float, float, float, float, float], + approach_height: float, + orientation: int = 0, + ): + """Pick a plate from the specified position.""" + await self.api.set_location_xyz(self.location_index, *cartesian_position) + await self.api.set_location_z_clearance(self.location_index, approach_height) + await self.api.set_location_config(self.location_index, orientation) + await self.api.pick_plate( + self.location_index, self.horizontal_compliance, self.horizontal_compliance_torque + ) + + async def _place_plate_c( + self, + cartesian_position: tuple[float, float, float, float, float, float], + approach_height: float, + orientation: int = 0, + ): + """Place a plate at the specified position.""" + await self.api.set_location_xyz(self.location_index, *cartesian_position) + await self.api.set_location_z_clearance(self.location_index, approach_height) + await self.api.set_location_config(self.location_index, orientation) + await self.api.place_plate( + self.location_index, self.horizontal_compliance, self.horizontal_compliance_torque + ) + + async def _move_to_c( + self, cartesian_position: tuple[float, float, float, float, float, float], orientation: int = 0 + ): + """Move the arm to a specified position in 3D space.""" + await self.api.move_c(self.profile_index, *cartesian_position, config=orientation) + + async def _get_position_c( + self, + ) -> tuple[float, float, float, float, float, float, Optional[ElbowOrientation]]: + """Get the current position of the arm in 3D space.""" + position = await self.api.where_c() + return (*position[:6], self._convert_orientation_int_to_enum(position[6])) diff --git a/pylabrobot/arms/precise_flex/precise_flex_backend_tests.py b/pylabrobot/arms/precise_flex/precise_flex_backend_tests.py new file mode 100644 index 00000000000..a3932850869 --- /dev/null +++ b/pylabrobot/arms/precise_flex/precise_flex_backend_tests.py @@ -0,0 +1,99 @@ +import unittest + +import pytest + +from pylabrobot.arms.coords import CartesianCoords, ElbowOrientation, JointCoords +from pylabrobot.arms.precise_flex.precise_flex_backend import PreciseFlexBackend + + +@pytest.mark.hardware # include/exclude via "pytest -m hardware" +class PreciseFlexBackendHardwareTests(unittest.IsolatedAsyncioTestCase): + """Integration tests for PreciseFlex robot - RUNS ON ACTUAL HARDWARE""" + + # Connection config + MODEL = "pf3400" + ROBOT_HOST = "192.168.0.1" + ROBOT_PORT = 10100 + + # Test constants + TEST_PROFILE_ID = 20 + TEST_LOCATION_ID = 20 + TEST_PARAMETER_ID = 17018 + TEST_SIGNAL_ID = 20064 + + SAFE_LOCATION_C = CartesianCoords(175, 0, 169.994, -0.001, 90, 180, ElbowOrientation.RIGHT) + SAFE_LOCATION_J = JointCoords(0, 170.003, 0, 180, -180, 75.486) + + TEST_LOCATION_J_LEFT = JointCoords(0, 169.932, 16.883, 230.942, -224.288, 75.662) + TEST_LOCATION_C_LEFT = CartesianCoords( + 328.426, -115.219, 169.932, 23.537, 90, 180, ElbowOrientation.LEFT + ) + + TEST_LOCATION_J_RIGHT = JointCoords(0, 169.968, -4.238, 117.915, -100.062, 75.668) + TEST_LOCATION_C_RIGHT = CartesianCoords( + 342.562, 280.484, 169.969, 13.612, 90, 180, ElbowOrientation.RIGHT + ) + + async def asyncSetUp(self): + """Connect to actual PreciseFlex robot""" + self.robot = PreciseFlexBackend(self.MODEL, self.ROBOT_HOST, self.ROBOT_PORT) + await self.robot.setup() + + async def asyncTearDown(self): + """Cleanup robot connection""" + if hasattr(self, "robot"): + await self.robot.stop() + + async def test_set_speed(self): + await self.robot.set_speed(50) + speed = await self.robot.get_speed() + self.assertEqual(speed, 50) + + async def test_open_close_gripper(self): + await self.robot.close_gripper() + closed = await self.robot.is_gripper_closed() + self.assertTrue(closed) + + await self.robot.open_gripper() + closed = await self.robot.is_gripper_closed() + self.assertFalse(closed) + + async def test_home(self): + await self.robot.home() + + async def test_move_to_safe(self): + await self.robot.move_to_safe() + + async def test_approach_c(self): + await self.robot.approach(self.TEST_LOCATION_C_LEFT, 20) + + async def test_approach_j(self): + await self.robot.approach(self.TEST_LOCATION_J_LEFT, 20) + + async def test_pick_plate(self): + try: + await self.robot.pick_plate(self.TEST_LOCATION_C_RIGHT, 10) + except Exception as e: + if "no plate present" in str(e).lower(): + pass + else: + raise + + async def test_place_plate(self): + await self.robot.place_plate(self.TEST_LOCATION_C_LEFT, 15) + + async def test_move_to_j(self): + await self.robot.move_to(self.TEST_LOCATION_J_RIGHT) + + async def test_move_to_c(self): + await self.robot.move_to(self.TEST_LOCATION_C_RIGHT) + + async def test_get_position_j(self): + """Test getting joint position""" + position_j = await self.robot.get_joint_position() + self.assertIsInstance(position_j, JointCoords) + + async def test_get_position_c(self): + """Test getting cartesian position""" + position_c = await self.robot.get_cartesian_position() + self.assertIsInstance(position_c, CartesianCoords) diff --git a/pylabrobot/io/tcp.py b/pylabrobot/io/tcp.py new file mode 100644 index 00000000000..b0b32fd35c5 --- /dev/null +++ b/pylabrobot/io/tcp.py @@ -0,0 +1,135 @@ +import asyncio +import logging +from dataclasses import dataclass +from typing import Optional + +from pylabrobot.io.capture import CaptureReader, Command, capturer, get_capture_or_validation_active +from pylabrobot.io.errors import ValidationError +from pylabrobot.io.io import IOBase +from pylabrobot.io.validation_utils import LOG_LEVEL_IO, align_sequences + +logger = logging.getLogger(__name__) + + +@dataclass +class TCPCommand(Command): + data: str + + def __init__(self, device_id: str, action: str, data: str, module: str = "tcp"): + super().__init__(module=module, device_id=device_id, action=action) + self.data = data + + +class TCP(IOBase): + def __init__(self, host: str, port: int = 5000): + self._host = host + self._port = port + self._reader: Optional[asyncio.StreamReader] = None + self._writer: Optional[asyncio.StreamWriter] = None + + if get_capture_or_validation_active(): + raise RuntimeError("Cannot create a new TCP object while capture or validation is active") + + async def setup(self): + self._reader, self._writer = await asyncio.open_connection(self._host, self._port) + + async def stop(self): + if self._writer is not None: + self._writer.close() + await self._writer.wait_closed() + self._reader = None + self._writer = None + + async def write(self, data: bytes): + assert self._writer is not None, "forgot to call setup?" + self._writer.write(data + b"\n") + await self._writer.drain() + logger.log(LOG_LEVEL_IO, "[%s:%d] write %s", self._host, self._port, data) + capturer.record( + TCPCommand( + device_id=f"{self._host}:{self._port}", action="write", data=data.decode("unicode_escape") + ) + ) + + async def read(self, num_bytes: int = -1) -> bytes: + assert self._reader is not None, "forgot to call setup?" + data = await self._reader.read(num_bytes) + logger.log(LOG_LEVEL_IO, "[%s:%d] read %s", self._host, self._port, data) + capturer.record( + TCPCommand( + device_id=f"{self._host}:{self._port}", action="read", data=data.decode("unicode_escape") + ) + ) + return data + + async def readline(self) -> bytes: + assert self._reader is not None, "forgot to call setup?" + + data = await self._reader.read(128) + last_line = data.split(b"\r\n")[0] # fix for errors with multiplate lines returned + last_line += b"\r\n" + + logger.log(LOG_LEVEL_IO, "[%s:%d] readline %s", self._host, self._port, last_line) + capturer.record( + TCPCommand( + device_id=f"{self._host}:{self._port}", + action="readline", + data=last_line.decode("unicode_escape"), + ) + ) + return last_line + + def serialize(self): + return { + "host": self._host, + "port": self._port, + } + + @classmethod + def deserialize(cls, data: dict) -> "TCP": + return cls( + host=data["host"], + port=data["port"], + ) + + +class TCPValidator(TCP): + def __init__(self, cr: CaptureReader, host: str, port: int = 5000): + super().__init__(host, port) + self.cr = cr + + async def setup(self): + pass + + async def write(self, data: bytes): + next_command = TCPCommand(**self.cr.next_command()) + if not ( + next_command.module == "tcp" + and next_command.device_id == f"{self._host}:{self._port}" + and next_command.action == "write" + ): + raise ValidationError(f"Next line is {next_command}, expected TCP write") + if next_command.data != data.decode("unicode_escape"): + align_sequences(expected=next_command.data, actual=data.decode("unicode_escape")) + raise ValidationError("Data mismatch: difference was written to stdout.") + + async def read(self, num_bytes: int = 128) -> bytes: + next_command = TCPCommand(**self.cr.next_command()) + if not ( + next_command.module == "tcp" + and next_command.device_id == f"{self._host}:{self._port}" + and next_command.action == "read" + and len(next_command.data) == num_bytes + ): + raise ValidationError(f"Next line is {next_command}, expected TCP read {num_bytes}") + return next_command.data.encode() + + async def readline(self) -> bytes: + next_command = TCPCommand(**self.cr.next_command()) + if not ( + next_command.module == "tcp" + and next_command.device_id == f"{self._host}:{self._port}" + and next_command.action == "readline" + ): + raise ValidationError(f"Next line is {next_command}, expected TCP readline") + return next_command.data.encode() diff --git a/pytest.ini b/pytest.ini index db60836e22f..2750f4aa22c 100644 --- a/pytest.ini +++ b/pytest.ini @@ -1,3 +1,6 @@ [pytest] python_files = *_tests.py +markers = hardware: tests requiring connected devices + +addopts = -m "not hardware" \ No newline at end of file