Capstone — Automated Cell

"""gripper_sim.py — XEG electric gripper = Modbus SERVER(Slave) simulator

Manual 5.4.5.1 (p.106-111). The robot becomes a Modbus CLIENT (master) and controls the gripper.
In this situation, the PC takes over the ‘gripper’ role. Try controlling it with modbus_master.py instead of the robot.

Register map (XEG-64 standard, decimal address, zero_mode):
  Holding Register (Write)
    1536 gripper models (2624 = XEG-64)
    1552 HOME Reset (Write 1 to reset)
    1600 directions (0=close / 1=open)
    1601 travel stroke [0.01mm]
    1602 Movement speed [0.01mm/s]
    1603 Grip stroke [0.01mm]
    1604 grip speed [0.01mm/s]
    1605 Grip force [%]
    1606 Confirm/execute settings (write 1 to start operation → automatically return to 0 after completion)
  Input Register (Read)
    769 (0x301) Gripper status (0 Idle, 1 Busy, 2 Pos reached, 3 Hold, 4~7 Alarm)
    770 Current position [0.01mm]
    771 (0x303) Firmware version major
    772 (0x304) Firmware version minor

Run: python gripper_sim.py # 0.0.0.0:502 (separate port when running simultaneously with robot server)
       python gripper_sim.py --port 1503
Exit: Ctrl+C"""
import argparse
import sys
import threading
import time

from pymodbus.datastore import (
    ModbusSequentialDataBlock,
    ModbusServerContext,
    ModbusSlaveContext,
)
from pymodbus.server import StartTcpServer

CO, DI, HR, IR = 1, 2, 3, 4

ST_IDLE, ST_BUSY, ST_POS, ST_HOLD = 0, 1, 2, 3
REG_STATUS = 769
REG_CUR_POS = 770


class GripperLogic(threading.Thread):
    def __init__(self, context, period=0.05):
        super().__init__(daemon=True)
        self.ctx = context[0]
        self.period = period
        self.busy = False
        self.ticks = 0
        self.move_ticks = 8
        #Initial values: model, firmware, status Idle
        self.ctx.setValues(HR, 1536, [2624])
        self.ctx.setValues(IR, REG_STATUS, [ST_IDLE])
        self.ctx.setValues(IR, REG_CUR_POS, [0])
        self.ctx.setValues(IR, 771, [1, 0])     # FW 1.0

    def run(self):
        print("Starting gripper logic thread.")
        while True:
            confirm = self.ctx.getValues(HR, 1606, count=1)[0]
            if not self.busy and confirm == 1:
                direction = self.ctx.getValues(HR, 1600, count=1)[0]
                stroke = self.ctx.getValues(HR, 1601, count=1)[0]
                self.busy = True
                self.ticks = 0
                self.target = stroke if direction == 1 else 0
                self.ctx.setValues(IR, REG_STATUS, [ST_BUSY])
                print(f"▶ Gripper {'open' if direction == 1 else 'close'}"
                      f"(target {self.target/100:.2f} mm)")
            elif self.busy:
                self.ticks += 1
                if self.ticks >= self.move_ticks:
                    self.ctx.setValues(IR, REG_CUR_POS, [self.target])
                    self.ctx.setValues(IR, REG_STATUS, [ST_POS])
                    self.ctx.setValues(HR, 1606, [0])   #Confirmation flag automatic reset
                    self.busy = False
                    print(f"✔ Action completed: state=2(Pos), position={self.target/100:.2f} mm")
            time.sleep(self.period)


def build_context():
    store = ModbusSlaveContext(
        di=ModbusSequentialDataBlock(0, [0] * 100),
        co=ModbusSequentialDataBlock(0, [0] * 100),
        hr=ModbusSequentialDataBlock(0, [0] * 1700),
        ir=ModbusSequentialDataBlock(0, [0] * 800),
        zero_mode=True,
    )
    return ModbusServerContext(slaves=store, single=True)


def main():
    ap = argparse.ArgumentParser()
    ap.add_argument("--host", default="0.0.0.0")
    ap.add_argument("--port", type=int, default=502)
    args = ap.parse_args()

    #cp949 Prevent logic thread from dying when outputting ▶/✔ in console.
    try:
        sys.stdout.reconfigure(encoding="utf-8", errors="replace")
    except Exception:
        pass

    context = build_context()
    GripperLogic(context).start()
    print("=" * 60)
    print(f"XEG Gripper Modbus SERVER Simulator {args.host}:{args.port}")
    print("Try controlling it using 1600/1601/1606 with modbus_master.py instead of the robot (Client).")
    print("Exit: Ctrl+C")
    print("=" * 60)
    try:
        StartTcpServer(context=context, address=(args.host, args.port))
    except KeyboardInterrupt:
        print("\nServer shutdown.")


if __name__ == "__main__":
    main()

"""modbus_master.py — Modbus Master (Client) practice tool

It is used for two purposes.
  (1) Command line CLI: Learn 4 data types by directly reading/writing registers of the simulator (slave)
  (2) import module: Import and use the RobotMaster class from another practice script

Based on pymodbus 3.6.x. Install: python -m pip install pymodbus==3.6.9

CLI example:
    # Assume the robot server simulator (robot_server_sim.py) is running on port 502
    python modbus_master.py read-hr 100 1 # Read speed settings
    python modbus_master.py write-hr 100 50 # Write at 50% speed
    python modbus_master.py read-ir 524 1 # Read operation status
    python modbus_master.py read-di 0 8 # SO1~SO8 status
    python modbus_master.py write-coil 0 1 # SI1(Start) trigger
    python modbus_master.py read-coil 0 8"""
import argparse
import sys

from pymodbus.client import ModbusTcpClient


class RobotMaster:
    """Modbus TCP master wrapper to control robots/slaves."""

    def __init__(self, host="127.0.0.1", port=502, unit=1):
        self.client = ModbusTcpClient(host, port=port)
        self.unit = unit

    def __enter__(self):
        if not self.client.connect():
            raise ConnectionError(f"Failed to connect to slave: {self.client}")
        return self

    def __exit__(self, *exc):
        self.client.close()

    #--- Read ---------------------------------------------------------------
    def read_holding(self, addr, count=1):
        rr = self.client.read_holding_registers(addr, count=count, slave=self.unit)
        self._check(rr)
        return rr.registers

    def read_input(self, addr, count=1):
        rr = self.client.read_input_registers(addr, count=count, slave=self.unit)
        self._check(rr)
        return rr.registers

    def read_coils(self, addr, count=1):
        rr = self.client.read_coils(addr, count=count, slave=self.unit)
        self._check(rr)
        return rr.bits[:count]

    def read_discrete(self, addr, count=1):
        rr = self.client.read_discrete_inputs(addr, count=count, slave=self.unit)
        self._check(rr)
        return rr.bits[:count]

    #---Writing-------------------------------------------------------------
    def write_holding(self, addr, values):
        if isinstance(values, int):
            values = [values]
        #Modbus registers are transmitted as 16bit unsigned. returned by split_word()
        #Negative words (e.g. High=-5) are also accepted and encoded as 0~65535 (& 0xFFFF).
        #The slave interprets it again as a signed value.
        values = [v & 0xFFFF for v in values]
        rr = self.client.write_registers(addr, values, slave=self.unit)
        self._check(rr)

    def write_coil(self, addr, value):
        rr = self.client.write_coil(addr, bool(value), slave=self.unit)
        self._check(rr)

    def write_coils(self, addr, values):
        rr = self.client.write_coils(addr, [bool(v) for v in values], slave=self.unit)
        self._check(rr)

    @staticmethod
    def _check(rr):
        if rr.isError():
            raise IOError(f"Modbus error response: {rr}")


def main():
    ap = argparse.ArgumentParser(description="Modbus TCP Master CLI")
    ap.add_argument("--host", default="127.0.0.1")
    ap.add_argument("--port", type=int, default=502)
    ap.add_argument("--unit", type=int, default=1, help="Slave ID")
    ap.add_argument("op", choices=[
        "read-hr", "write-hr", "read-ir",
        "read-coil", "write-coil", "read-di",
    ])
    ap.add_argument("addr", type=int)
    ap.add_argument("value", type=int, nargs="?", default=1,
                    help="The read series is the count, and the write series is the value to write.")
    args = ap.parse_args()

    try:
        with RobotMaster(args.host, args.port, args.unit) as m:
            if args.op == "read-hr":
                print(m.read_holding(args.addr, args.value))
            elif args.op == "write-hr":
                m.write_holding(args.addr, args.value)
                print(f"HR[{args.addr}] <- {args.value} done")
            elif args.op == "read-ir":
                print(m.read_input(args.addr, args.value))
            elif args.op == "read-coil":
                print([int(b) for b in m.read_coils(args.addr, args.value)])
            elif args.op == "write-coil":
                m.write_coil(args.addr, args.value)
                print(f"Coil[{args.addr}] <- {bool(args.value)} done")
            elif args.op == "read-di":
                print([int(b) for b in m.read_discrete(args.addr, args.value)])
    except Exception as e:
        print(f"[Error] {e}", file=sys.stderr)
        sys.exit(1)


if __name__ == "__main__":
    main()

# Shared library (`_shared/`)

This is a Python tool/simulator shared by all parties that reproduces HIWIN robot communication using only a PC.
Instead of duplicating large simulators from piece to piece, we manage them here in one place. (Based on pymodbus 3.6.9 / pyserial 3.5, all execution verification completed)

## Installation

```powershell
cd lecture/_shared
python -m pip install -r requirements.txt
#If Hangul is broken in the Windows console:
set PYTHONUTF8=1     # PowerShell: $env:PYTHONUTF8=1
```

## How to use it in each part

- Run the **standalone simulator** (Robot/Gripper/Vision) directly in a separate terminal, and connect the convenience `main.py` to With-TCP/Socket (no import required).
  ```powershell
  python lecture/_shared/robot_server_sim.py --port 1502 --host 127.0.0.1
  ```
- **Helper module** (`word_tools`, `modbus_master`) is imported by convenience `main.py`. Path bootstrap:
  ```python
  import os, sys
  sys.path.insert(0, os.path.join(os.path.dirname(__file__), "..", "..", "..", "..", "_shared"))
  from word_tools import split_word
  from modbus_master import RobotMaster
  ```
(Because the edited file location is `GROUP/LEC/src/PROJECT/main.py`, it is 4 levels higher up to `_shared`.)

## File list

| file | Role | How to use |
|------|------|---------|
| `word_tools.py` | 32bit Low/High word segmentation/restoration, SWAP_WORD, IEEE754 conversion | 05, 07, Appendix B |
| `modbus_master.py` | Modbus Master CLI + `RobotMaster` Class | 05, 06, 07 |
| `robot_server_sim.py` | Robot = Modbus slave (implementation of command cycle) | 06, 09 |
| `gripper_sim.py` | XEG electric gripper = Modbus slave | 07, 09 |
| `tcp_echo_server.py` | TCP/IP high-level system (vision) simulator, `{data}` format | 03, 09 |
| `serial_echo.py` | RS-232 serial echo server (virtual COM) | 04 |
| `serial_client.py` | RS-232 serial client (virtual COM) | 04 |

> I/O handshake (Part 02) and integrated controller (Part 09) are included in each part of `src/<project>/main.py`.

## Check quick operation

```powershell
#1) Conversion tool (no equipment required)
python word_tools.py

#2) Launch the robot server (one terminal)
python robot_server_sim.py --port 1502 --host 127.0.0.1

#3) Control as master from another terminal
python modbus_master.py --port 1502 read-di 0 8     # SO1~SO8 (SO4=Ready)
python modbus_master.py --port 1502 read-ir 524 1   #Operation status (1=Idle)
```

> **Port 502 Permissions**: On Windows, port 502 may require administrator privileges. In practice, like `--port 1502`
> If you use port 1024 or higher, the process will proceed without permission issues. It actually uses Robots/equipment 502.

> **Running two simulators simultaneously**: Disconnect the ports. Example) Robot `--port 1502`, Gripper `--port 1503`.

# This lecture practice code dependency
# Install: python -m pip install -r requirements.txt
#
# The API of pymodbus varies depending on the version. This lecture is based on 3.6.x Written/Verified.
pymodbus==3.6.9
pyserial==3.5

"""robot_server_sim.py — Robot = Modbus SERVER(Slave) simulator

Manual Ch.5.3 (p.41~80). The situation where the PLC/higher master controls the robot is reproduced on the PC.
Leave this program open and open modbus_master.py or QModMaster (GUI) in another window.
Practice the “instruction cycle” by reading/writing registers.

  Command execution procedure (Manual 5.3.3.5):
    1) HR[201] <- Command number (e.g. 0=PTP, 1=LIN, 3=JOG, 4=GO HOME)
    2) HR[202..] <- parameter
    3) HR[200] <- 1 (execution trigger)
    4) Robot operates → IR[524] (operation status) is 2(Running) → 1(Idle), IR[200]=1(Success)
    5) HR[200] <- 0 (Master resets, prepares for next command)

  ※ The parameter arrangement of this simulator has been simplified for learning purposes (refer to the PTP annotation below).
    For the full parameter table of the actual robot, refer to Table 16 in the manual.

Register summary (zero_mode, address is 0-base):
  Discrete Input (read only): 0~ SO1..SOn (SO1=Run, SO2=Held, SO3=Fault, SO4=Ready ...)
  Coil (read/write): 0~ SI1..SIn (SI1=Start, SI2=Hold, SI3=Stop, SI4=Enable ...),
                              300~DO1..DOOn
  Input Register (read only): 100 speed%, 200 command status, 201 current command, 300~ joint A1..A6(L/H),
                              400~ Orthogonal
  Holding Reg (read/write): 100 speed setting, 200 execution trigger, 201 command number, 202 ~ parameter

Run: python robot_server_sim.py # 0.0.0.0:502, Slave ID 1
       python robot_server_sim.py --port 1502 --unit 1
Exit: Ctrl+C"""
import argparse
import os
import sys
import threading
import time

sys.path.insert(0, os.path.dirname(os.path.abspath(__file__)))
from word_tools import split_word, combine_word  # noqa: E402

from pymodbus.datastore import (  # noqa: E402
    ModbusSequentialDataBlock,
    ModbusServerContext,
    ModbusSlaveContext,
)
from pymodbus.server import StartTcpServer  # noqa: E402

#Use Modbus function code as register type selector (pymodbus convention)
CO, DI, HR, IR = 1, 2, 3, 4

CMD_NAMES = {0: "PTP", 1: "LIN", 2: "CIRC", 3: "JOG", 4: "GO HOME", 5: "T_STOP", 6: "STOP"}


class RobotLogic(threading.Thread):
    """Monitors command triggers in the background and operates the virtual robot."""

    def __init__(self, context, unit=1, period=0.05):
        super().__init__(daemon=True)
        self.ctx = context[unit if not context.single else 0]
        self.period = period
        self.busy = False
        self.ack_wait = False     #Wait for the master to reset to HR[200]=0
        self.ticks = 0
        self.move_ticks = 10      #Approximately 0.5 second virtual travel time
        self._init_registers()

    def _init_registers(self):
        #SO default state: SO4=Ready ON
        self.ctx.setValues(DI, 0, [0, 0, 0, 1, 0, 0, 0, 0])
        #IR State Reset
        self.ctx.setValues(IR, 100, [100])        #speed 100%
        self.ctx.setValues(IR, 200, [0, 0])       #Command status=0, current command=0
        self.ctx.setValues(IR, 524, [1])          #Operation status = Idle
        self.ctx.setValues(IR, 522, [1])          #Motor excitation ON
        #Current position (joint/orthogonal) to 0
        self.ctx.setValues(IR, 300, [0] * 12)
        self.ctx.setValues(IR, 400, [0] * 12)

    #--- Helper -------------------------------------------------------------
    def _get(self, fc, addr, count=1):
        return self.ctx.getValues(fc, addr, count=count)

    def _set(self, fc, addr, values):
        self.ctx.setValues(fc, addr, values)

    def _set_so(self, idx, on):
        self._set(DI, idx, [1 if on else 0])

    #--- Execute command --------------------------------------------------------
    def _start_command(self):
        cmd = self._get(HR, 201)[0]
        self.current_cmd = cmd
        self.busy = True
        self.ticks = 0
        self._set(IR, 524, [2])         # Running
        self._set(IR, 201, [cmd])       #current command
        self._set(IR, 200, [0])         #status in progress
        self._set_so(0, 1)              # SO1=Run ON
        print(f"▶ Start command: {cmd} ({CMD_NAMES.get(cmd, '?')})")

    def _finish_command(self):
        cmd = self.current_cmd
        params = self._get(HR, 200, count=20)   # HR[200..219]
        #params[0]=trigger, [1]=command number, [2]=type, [3..14]=6 axis L/H, [15]=speed...
        if cmd in (0, 1):                        # PTP / LIN
            mtype = params[2]                    # 0=Joint, 1=Cartesian
            axis_words = params[3:15]            #12 words = 6 axes (L, H)
            target = self.ctx_in_eng(axis_words)
            if mtype == 0:
                self._set(IR, 300, axis_words)   #Current joint position = target
                print(f"Joint movement completed A1~A6 = {target}")
            else:
                self._set(IR, 400, axis_words)   #Current Cartesian Position = Target
                print(f"Orthogonal movement completed X..C = {target}")
            spd = params[15] if params[15] else 100
            self._set(IR, 100, [spd])
        elif cmd == 4:                           # GO HOME
            self._set(IR, 300, [0] * 12)
            self._set(IR, 400, [0] * 12)
            print("Return to origin completed")
        #common finish
        self._set(IR, 524, [1])         # Idle
        self._set(IR, 200, [1])         # Success
        self._set_so(0, 0)              # SO1=Run OFF
        self.busy = False
        self.ack_wait = True
        print(f"✔ Command completion: IR[200]=1(Success). Reset the master to HR[200]<-0.")

    @staticmethod
    def ctx_in_eng(words):
        """[L,H,L,H,...] 12 words -> List of 6 engineering values (0.001 scale)."""
        out = []
        for i in range(0, 12, 2):
            low = words[i] if words[i] < 32768 else words[i] - 65536
            high = words[i + 1] if words[i + 1] < 32768 else words[i + 1] - 65536
            out.append(round(combine_word(low, high) * 0.001, 3))
        return out

    #--- Main Loop --------------------------------------------------------
    def run(self):
        print("Starting the robot logic thread.")
        while True:
            trig = self._get(HR, 200)[0]
            if self.ack_wait:
                if trig == 0:
                    self.ack_wait = False     #Confirm master reset → Next commands possible
            elif self.busy:
                self.ticks += 1
                if self.ticks >= self.move_ticks:
                    self._finish_command()
            elif trig == 1:
                self._start_command()
            time.sleep(self.period)


def build_context():
    store = ModbusSlaveContext(
        di=ModbusSequentialDataBlock(0, [0] * 600),
        co=ModbusSequentialDataBlock(0, [0] * 600),
        hr=ModbusSequentialDataBlock(0, [0] * 2100),
        ir=ModbusSequentialDataBlock(0, [0] * 600),
        zero_mode=True,
    )
    return ModbusServerContext(slaves=store, single=True)


def main():
    ap = argparse.ArgumentParser()
    ap.add_argument("--host", default="0.0.0.0")
    ap.add_argument("--port", type=int, default=502)
    ap.add_argument("--unit", type=int, default=1)
    args = ap.parse_args()

    #When outputting ▶/✔ from the Windows default console (cp949), UnicodeEncodeError occurs.
    #Prevent logic threads from dying (compared to environments where PYTHONUTF8 is not set)
    try:
        sys.stdout.reconfigure(encoding="utf-8", errors="replace")
    except Exception:
        pass

    context = build_context()
    logic = RobotLogic(context, unit=args.unit)
    logic.start()

    print("=" * 60)
    print(f"Robot Modbus SERVER Simulator {args.host}:{args.port} (Slave ID {args.unit})")
    print("Connect to the master (modbus_master.py / QModMaster) and practice the command cycle.")
    print("Exit: Ctrl+C")
    print("=" * 60)
    try:
        StartTcpServer(context=context, address=(args.host, args.port))
    except KeyboardInterrupt:
        print("\nServer shutdown.")


if __name__ == "__main__":
    main()

"""serial_client.py — RS-232 robot client (PC simulation, chapter 03-rs232)

Open the other side (COM5) of the com0com virtual COM port pair to transmit packets and
Receives and outputs the response from the echo server (serial_echo.py, COM6).
In an actual robot, this flow is written as COPEN(SER,HANDLE) -> CWRITE -> CREAD.

Run (serial_echo.py should be floating in another terminal):
    python serial_client.py --port COM5 --baud 9600 --value 123"""

import argparse
import serial  # pyserial


def main():
    ap = argparse.ArgumentParser(description="RS-232 client (send packet and receive response)")
    ap.add_argument("--port", default="COM5", help="Open virtual COM port (default COM5)")
    ap.add_argument("--baud", type=int, default=9600, help="baud rate (must be the same on both sides)")
    ap.add_argument("--value", default="123", help="Value to send (default 123)")
    args = ap.parse_args()

    ser = serial.Serial(args.port, args.baud, timeout=1)

    packet = ("{" + args.value + "}").encode("ascii")   #Example: b'{123}'
    ser.write(packet)
    print(f"[client] {args.port} @ {args.baud}bps -> send {packet!r}")

    resp = ser.read(64)                                  #Receive echo server response
    print(f"[client] receive {resp!r} (decode: {resp.decode('ascii', errors='replace')})")

    ser.close()


if __name__ == "__main__":
    main()

"""serial_echo.py — RS-232 echo server (PC simulation, chapter 03-rs232)

Open one side (COM6) of the com0com virtual COM port pair (e.g. COM5 <-> COM6),
The received data is returned by wrapping it in curly braces { } as in the HIWIN packet format.
The PC imitates the RS-232 response flow (role of the upper system) of a real robot.

Run:
    python serial_echo.py --port COM6 --baud 9600
Stop: Ctrl+C"""

import argparse
import serial  # pyserial


def main():
    ap = argparse.ArgumentParser(description="RS-232 echo server (serial reception -> {data} response)")
    ap.add_argument("--port", default="COM6", help="Open virtual COM port (default COM6)")
    ap.add_argument("--baud", type=int, default=9600, help="baud rate (must be the same on both sides)")
    args = ap.parse_args()

    #timeout=1: prevents infinite waiting by making read() wait for data only for 1 second.
    ser = serial.Serial(args.port, args.baud, timeout=1)
    print(f"[echo] {args.port} @ {args.baud}bps opened. Waiting to receive... (Ctrl+C Exit)")
    try:
        while True:
            data = ser.read(64)          #Receive up to 64 bytes (if none, b'' after 1 second)
            if not data:
                continue
            text = data.decode("ascii", errors="replace")
            reply = "{" + text + "}"      #Wrap it in { } like HIWIN packet format
            ser.write(reply.encode("ascii"))
            print(f"[echo] Receive {data!r} -> Reply {reply!r}")
    except KeyboardInterrupt:
        print("\n[echo] End")
    finally:
        ser.close()


if __name__ == "__main__":
    main()

"""tcp_echo_server.py — High-level system (vision) simulator for Ethernet TCP/IP communication practice

Manual Ch.3 (p.30~33). The robot operates as a client, and the vision/higher system operates as a server.
In accordance with the HRSS communication protocol, packets are surrounded by curly braces { } and separated by commas.

  Sent by robot: {TRIG} (request to shoot)
  Server responds: {123.45,67.89,30.0} (X, Y, rotation angle)

Run:
    python tcp_echo_server.py # vision mode, 0.0.0.0:6000
    python tcp_echo_server.py --mode echo # Returns the received value as is.
    python tcp_echo_server.py --port 6000 --host 0.0.0.0

Exit: Ctrl+C"""
import argparse
import socket


def make_vision_reply(request: str) -> str:
    """Simple fake vision results. In reality, the coordinates detected by the camera are included."""
    #Different coordinates may be given depending on the request text (e.g. TRIG, COUNT, etc.).
    samples = {
        "TRIG":  "123.45,67.89,30.0",
        "TRIG2": "-45.10,210.00,-15.0",
    }
    return samples.get(request.strip(), "0.00,0.00,0.0")


def handle(conn: socket.socket, addr, mode: str):
    print(f"[connection] {addr} connection")
    with conn:
        buf = ""
        while True:
            data = conn.recv(1024)
            if not data:
                break
            buf += data.decode("ascii", errors="replace")
            #Processed as packet terminator '}'
            while "}" in buf:
                pkt, buf = buf.split("}", 1)
                payload = pkt.replace("{", "").strip()
                print(f"Receive: {{{payload}}}")
                if mode == "echo":
                    reply = payload
                else:
                    reply = make_vision_reply(payload)
                msg = "{" + reply + "}"
                conn.sendall(msg.encode("ascii"))
                print(f"Sent from: {msg}")
    print(f"[End] {addr}")


def main():
    ap = argparse.ArgumentParser()
    ap.add_argument("--host", default="0.0.0.0")
    ap.add_argument("--port", type=int, default=6000)
    ap.add_argument("--mode", choices=["vision", "echo"], default="vision")
    args = ap.parse_args()

    with socket.socket(socket.AF_INET, socket.SOCK_STREAM) as srv:
        srv.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
        srv.bind((args.host, args.port))
        srv.listen(1)
        print(f"Waiting for TCP server {args.host}:{args.port} (mode={args.mode})")
        print("Wait for the robot (client) to connect to COPEN (ETH, ...).  Exit: Ctrl+C")
        try:
            while True:
                conn, addr = srv.accept()
                handle(conn, addr, args.mode)
        except KeyboardInterrupt:
            print("\nShutting down the server.")


if __name__ == "__main__":
    main()

"""word_tools.py — HIWIN robot Modbus data conversion practice tool

Refer to manual Appendix II (32bit split storage) and 5.4.4 (SWAP_WORD / IEEE754).
This is a learning module that you can check directly with Python. No need for pymodbus.

Run:
    python word_tools.py # Demo (verification of manual example)
    python word_tools.py 90.83 # Check split/restore with random real values"""
import struct
import sys


# ---------------------------------------------------------------------------
#1) 32bit value ↔ Low/High word division (Manual Appendix II, p.131~133)
#The robot stores values exceeding 16 bits (coordinates, speed, etc.) divided into two registers.
#Each word is signed 16 bits (-32768~32767).
# ---------------------------------------------------------------------------
def split_word(value: int):
    """signed integer -> (low_word, high_word).  Both signed 16bit.

    >>> split_word(90830) # A1 = 90.830° (= 90.83 * 1000)
    (25294, 1)
    >>> split_word(300000) #
    (-27680, 4)
    >>> split_word(-300000) #
    (27680, -5)"""
    high = value // 65536            #Quotient (round down if negative → consistent with manual rule)
    rem = value % 65536              #Rest: always 0 to 65535
    low = rem - 65536 if rem > 32767 else rem   #Expressed as signed 16bit
    return low, high


def combine_word(low: int, high: int) -> int:
    """(low_word, high_word) -> Original signed integer.

    >>> combine_word(25294, 1)
    90830
    >>> combine_word(-6742, -7) # X = -399.958 mm (Manual example)
    -399958"""
    return high * 65536 + (low & 0xFFFF)   #High maintains sign, low is treated as unsigned


def to_engineering(low: int, high: int, scale: float = 0.001) -> float:
    """Low/High words to actual engineering values. (Coordinates/angle scale=0.001)"""
    return combine_word(low, high) * scale


# ---------------------------------------------------------------------------
#2) SWAP_WORD (Manual 5.4.4.1, p.102)
#Exchange the upper/lower word positions of the 32-bit value. (Correct byte order difference by equipment)
# ---------------------------------------------------------------------------
def swap_word(v: int) -> int:
    """High word ↔ Low word exchange.

    >>> swap_word(10) # 0x0000000A -> 0x000A0000
    655360"""
    v &= 0xFFFFFFFF
    return ((v & 0xFFFF) << 16) | ((v >> 16) & 0xFFFF)


# ---------------------------------------------------------------------------
#3) IEEE754 single precision conversion (Manual 5.4.4.2 / 5.4.4.3, p.104~105)
#Since Modbus cannot transmit real numbers directly, they are encoded and received as 32-bit integers.
# ---------------------------------------------------------------------------
def ieee754_encode(f: float) -> int:
    """Real number -> 32bit integer (IEEE754 single).

    >>> ieee754_encode(10.5)
    1093140480"""
    return struct.unpack(">I", struct.pack(">f", f))[0]


def ieee754_decode(i: int) -> float:
    """32bit integer (IEEE754 single) -> real number.

    >>> ieee754_decode(1093140480)
    10.5"""
    return struct.unpack(">f", struct.pack(">I", i & 0xFFFFFFFF))[0]


def reg_to_int32(reg_low: int, reg_high: int) -> int:
    """2 registers (unsigned 16bit) -> unsigned 32bit. For IEEE754 decoding preprocessing."""
    return ((reg_high & 0xFFFF) << 16) | (reg_low & 0xFFFF)


# ---------------------------------------------------------------------------
#demo
# ---------------------------------------------------------------------------
def _demo():
    print("=== 32bit partition/restore (Appendix II) ===")
    for eng in (90.830, 300.000, -399.958):
        raw = round(eng * 1000)
        low, high = split_word(raw)
        back = to_engineering(low, high)
        print(f"  {eng:>10.3f}  ->  raw={raw:>8}  Low={low:>7}  High={high:>3}"
              f"-> Restore={back:>10.3f} {'OK' if abs(back-eng) < 1e-6 else 'FAIL'}")

    print("\n=== SWAP_WORD (5.4.4.1) ===")
    print(f"swap_word(10) = {swap_word(10)} (expecting 655360)")

    print("\n=== IEEE754 (5.4.4.2/3) ===")
    print(f"ieee754_encode(10.5) = {ieee754_encode(10.5)} (expecting 1093140480)")
    print(f"ieee754_decode(...) = {ieee754_decode(1093140480)} (expecting 10.5)")


if __name__ == "__main__":
    if len(sys.argv) > 1:
        val = float(sys.argv[1])
        raw = round(val * 1000)
        low, high = split_word(raw)
        print(f"input {val} (×1000 = {raw})")
        print(f"  Low word  = {low}   (0x{low & 0xFFFF:04X})")
        print(f"  High word = {high}  (0x{high & 0xFFFF:04X})")
        print(f"restore = {to_engineering(low, high)}")
        print(f"  IEEE754   = {ieee754_encode(val)}")
    else:
        _demo()

"""09. Comprehensive practice — vision, robot, gripper automation cell (cell_controller)

When the higher-level vision system reports the part location (TCP/IP), the robot moves to those coordinates (Modbus).
Pick it up with a gripper (Modbus), place it at a designated location, and return to the origin. That is, Flight 03 (TCP/IP) +
Part 06 (Robot = Server, Command Cycle) + Part 07 (Gripper = Client Control) + Part 08 (Monitoring)
It is an integrated controller bundled into one script.

This controller acts as a “cell controller (higher level controller/PLC)”.
  - Connect to Vision (higher system) as a TCP Client, send {TRIG} and receive coordinates.
  - For the robot (Modbus Server/Slave), the command cycle is run through the Modbus Master.
  - Send open/close commands to the gripper (Modbus Server/Slave) via Modbus Master.

Run the three simulators on different ports.
  Terminal 1: python ../../_shared/tcp_echo_server.py --mode vision --port 6000
  Terminal 2: python ../../_shared/robot_server_sim.py --port 1502 --host 127.0.0.1
  Terminal 3: python ../../_shared/gripper_sim.py --port 1503 --host 127.0.0.1
  Terminal 4: python src/cell_controller/main.py # execute one cycle
            python src/cell_controller/main.py --trig TRIG2 # Negative coordinates (Task 1)
            python src/cell_controller/main.py --cycles 2 # TRIG→TRIG2 repetition (Task 2)

Shutdown: The controller automatically shuts down after one cycle (or specified number of times). The simulator is Ctrl+C."""
import os
import sys

#--- Add _shared shared library path (Search for _shared from the top regardless of editing position or depth) ---
_d = os.path.dirname(os.path.abspath(__file__))
while _d != os.path.dirname(_d) and not os.path.isdir(os.path.join(_d, "_shared")):
    _d = os.path.dirname(_d)
sys.path.insert(0, os.path.join(_d, "_shared"))

#Prevent Hangul/symbols from breaking or dying in the cp949 console.
try:
    sys.stdout.reconfigure(encoding="utf-8", errors="replace")
except Exception:
    pass

import argparse  # noqa: E402
import socket    # noqa: E402
import time      # noqa: E402

from modbus_master import RobotMaster   #noqa: Modbus master controlling the E402 robot/gripper
from word_tools import split_word       #noqa: E402 coordinates 32bit → Low/High word split


#── Cell operation parameters (adjust to field values if necessary) ─────────────────────────────
APPROACH_Z = 50.0     #Access height [mm]: Safe height above the part
PICK_Z = 5.0          #Pick height [mm]: Actual picking height
PLACE_X = 200.0       #Place position
PLACE_Y = 0.0         #Place position Y [mm]
PLACE_Z = APPROACH_Z  #Place at approach height (simplified)
GRIP_STROKE = 4000    #Grip stroke [0.01mm] = 40.00mm (width of gripping the part)
OPEN_STROKE = 6000    #Opening stroke [0.01mm] = 60.00mm
SPEED = 50            #speed %
ACCEL = 100           #acceleration %
TOOL = 1              #Tool number
BASE = 0              #Base number
POLL = 0.05           #Status polling cycle [s]
TIMEOUT = 10.0        #Step-by-step completion wait limit [s]


def log(step, msg):
    print(f"  [{step}] {msg}")


#── 1) Vision: Receiving coordinates via TCP/IP (Part 03) ───────────────────────────────────
def get_vision(host, port, trig="TRIG"):
    """Send {TRIG} to the vision system, receive {X,Y,R} and parse it as (x, y, r) float."""
    print(f"[1] Request part location to vision system ({host}:{port})...")
    try:
        with socket.create_connection((host, port), timeout=5) as s:
            req = "{" + trig + "}"
            s.sendall(req.encode("ascii"))
            log("VISION", f"send {req}")
            raw = s.recv(1024).decode("ascii", errors="replace")
    except OSError as e:
        raise ConnectionError(
            f"Vision Server connection failure ({host}:{port})."
            f"Is 'python ../../_shared/tcp_echo_server.py --mode vision --port {port}' displayed? ({e})"
        )
    log("VISION", f"receive {raw.strip()}")
    payload = raw.replace("{", "").replace("}", "").strip()
    parts = [p.strip() for p in payload.split(",")]
    if len(parts) < 3:
        raise ValueError(f"Vision response format error: {raw!r} (requires 3 X,Y,R)")
    x, y, r = float(parts[0]), float(parts[1]), float(parts[2])
    print(f"→ Part coordinates X={x} mm Y={y} mm Rotation angle R={r} deg")
    return x, y, r


#── 2) Robot: Cartesian movement with Modbus command cycle (Part 06) ───────────────────────────
def robot_move(m, x, y, z, a=0.0, b=0.0, c=0.0, cmd=1, mtype=1, label=""):
    """One cycle of Cartesian LIN movement.

    After integerizing each of the 6 coordinate axes (X, Y, Z, A, B, C) to ×1000, split them into Low/High using split_word().
    Fill in HR203~214. Because write_holding() automatically encodes negative word 0xFFFF
    Negative coordinates (e.g. X=-45.10 in TRIG2) can also be passed as is.

    Instruction Cycle (Manual 5.3.3.5):
      ① HR201=Command number, HR202=Motion type, HR203~214=6 axis, HR215~218=Speed/Acceleration/Tool/Base
      ② HR200=1 (execution trigger)
      ③ After IR524 rises to 2 (Running), poll until 1 (Idle) & IR200 == 1 (Success).
      ④ HR200=0 (Reset — If not, the next command will be locked)"""
    axis = []
    for v in (x, y, z, a, b, c):
        low, high = split_word(round(v * 1000))   #×1000 Integerization and 32bit division
        axis += [low, high]                        #12 words in order [L, H]
    params = [cmd, mtype] + axis + [SPEED, ACCEL, TOOL, BASE]   # HR201~218
    label = label or f"({x}, {y}, {z})"
    print(f"[Robot] LIN movement → {label}")
    m.write_holding(201, params)        #① Write HR201~218 at once
    m.write_holding(200, 1)             #② Execution trigger
    _wait_robot(m)                      #③ Complete polling (step 2)
    _reset_robot_trigger(m)             #④ Trigger reset + wait for the robot to recognize the reset
    log("ROBOT", "Move completed (IR200=1 Success, IR524=1 Idle)")


def _wait_robot(m):
    """Wait for the robot to complete step 2.

    ⚠️ Pitfall ①: If you look at IR524 as 'Idle(1)' at once, the previous command is already Idle.
       It remains (state afterimage/latch) and passes before a new command even starts.
       So (1) First, check that IR524==2(Running) is posted and say 'This command is
       Hold ‘Started’, (2) and then complete with IR524==1(Idle) & IR200==1(Success)
       Judge. (Same type of afterimage avoidance as the gripper's HR1606==0 accompanying confirmation)"""
    t0 = time.time()
    #(1) Wait for Running(2) to start — Avoid the previous Idle latch
    while m.read_input(524, 1)[0] != 2:
        if time.time() - t0 > TIMEOUT:
            raise TimeoutError("Robot motion start signal (IR524=2 Running) timeout")
        time.sleep(POLL)
    #(2) Wait for Idle(1) & Success(1) to complete
    while not (m.read_input(524, 1)[0] == 1 and m.read_input(200, 1)[0] == 1):
        if time.time() - t0 > TIMEOUT:
            raise TimeoutError("Robot operation completion signal (IR524=1 & IR200=1) timeout")
        time.sleep(POLL)


def _reset_robot_trigger(m):
    """Reset the trigger to HR200=0 and leave it for a moment for the robot to 'recognize' the reset.

    ⚠️ Trap ②: The robot waits for the master to return HR200 to 0 after completion.
       Enters 'ack waiting' state (Manual 5.3.3.5 Procedure ⑤). Immediately after HR200=0
       If you write HR200=1 in the following command, the robot logic does not see '0' even once, so ack is not issued.
       It is not released and the next command is completely ignored (locked). Just one or two polling cycles
       Leave it blank to ensure that the robot observes the reset."""
    m.write_holding(200, 0)
    time.sleep(POLL * 2)


def go_home(m):
    """Complete the cycle by returning to the origin (GO HOME = command number 4)."""
    print("[Robot] GO HOME (return to origin) …")
    m.write_holding(201, 4)             #① Command number 4 = GO HOME
    m.write_holding(200, 1)             #② Execution trigger
    _wait_robot(m)                      #③ Complete polling (step 2)
    _reset_robot_trigger(m)             #④ Trigger Reset + Wait for Acknowledgment
    log("ROBOT", "Return to origin completed (IR400~411 = 0)")


#── 3) Gripper: Picking/Putting with Modbus (Part 07) ─────────────────────────────────
def gripper_act(m, direction, stroke):
    """One action of the gripper. direction 0=close (pinch) / 1=open (drop).

    Procedure (Manual 5.4.5.1):
      ① HR1600=Direction, ② HR1601=Execution, ③ HR1606=1 (Execution)
      ④ Completion polling — IR769==2 (Pos reached) **and** HR1606==0 (automatic return) checked simultaneously.

    ⚠️ If you look at IR769, an afterimage (Pos=2) of the previous action remains and the loop is completed before the new action ends.
      A race to escape arises. The gripper resets itself to zero upon completion.
      Check HR1606 together to clarify that “this operation is complete.”"""
    name = "Open (Place)" if direction == 1 else "Close (Pinch)"
    print(f"[Gripper] {name} → Stroke {stroke / 100:.2f} mm")
    m.write_holding(1600, direction)    #① Direction
    m.write_holding(1601, stroke)       #② Movement administration
    m.write_holding(1606, 1)            #③ Execution trigger
    t0 = time.time()
    while not (m.read_input(769, 1)[0] == 2 and m.read_holding(1606, 1)[0] == 0):  # ④
        if time.time() - t0 > TIMEOUT:
            raise TimeoutError("Gripper completion signal (IR769=2 & HR1606=0) timeout")
        time.sleep(POLL)
    pos = m.read_input(770, 1)[0]
    flag = m.read_holding(1606, 1)[0]
    log("GRIPPER", f"Completed (IR769=2 Pos, IR770={pos}={pos / 100:.2f}mm, HR1606={flag} automatic return)")


#── Full Pick and Place Cycle ─────────────────────────────────────────────────
def run_cycle(args, trig, robot, gripper):
    """Vision → Robot → Gripper in one cycle. robot/gripper opens RobotMaster."""
    t0 = time.time()
    x, y, r = get_vision(args.host, args.vision_port, trig)

    print("[2] Pick operation (approach → descent → pick up) …")
    robot_move(robot, x, y, APPROACH_Z, c=r, label=f"Peek Access ({x},{y},Z={APPROACH_Z})")
    robot_move(robot, x, y, PICK_Z, c=r, label=f"Pick drop ({x},{y},Z={PICK_Z})")
    gripper_act(gripper, 0, GRIP_STROKE)                      #close = pinch
    robot_move(robot, x, y, APPROACH_Z, c=r, label=f"Pick up ({x},{y},Z={APPROACH_Z})")

    print("[3] Place operation (move → place)...")
    robot_move(robot, PLACE_X, PLACE_Y, PLACE_Z, label=f"Move Place ({PLACE_X},{PLACE_Y},Z={PLACE_Z})")
    gripper_act(gripper, 1, OPEN_STROKE)                      #Open = Place

    print("[4] Conclusion…")
    go_home(robot)
    dt = time.time() - t0
    print(f"=== Cycle completion (cycle time {dt:.2f}s) ===\n")
    return dt


def main():
    ap = argparse.ArgumentParser(description="Vision, robot, gripper automation cell controller")
    ap.add_argument("--host", default="127.0.0.1",
                    help="Three devices common host (default 127.0.0.1)")
    ap.add_argument("--vision-port", type=int, default=6000, help="Vision (TCP) port")
    ap.add_argument("--robot-port", type=int, default=1502, help="Robot (Modbus) port")
    ap.add_argument("--gripper-port", type=int, default=1503, help="Gripper (Modbus) port")
    ap.add_argument("--trig", default="TRIG", help="Vision Trigger Token (TRIG/TRIG2)")
    ap.add_argument("--cycles", type=int, default=1,
                    help="Number of repetitions. If it is 2 or more, TRIG→TRIG2→TRIG… Take turns processing (task 2)")
    args = ap.parse_args()

    print("=" * 60)
    print("Start of vision/robot/gripper automation cell controller")
    print(f"vision {args.host}:{args.vision_port} (TCP)")
    print(f"Robot {args.host}:{args.robot_port} (Modbus)")
    print(f"Gripper {args.host}:{args.gripper_port} (Modbus)")
    print("=" * 60)

    trig_seq = ["TRIG", "TRIG2"]
    times = []
    try:
        #The robot/gripper connection is opened once and reused for multiple cycles.
        with RobotMaster(args.host, args.robot_port) as robot, \
             RobotMaster(args.host, args.gripper_port) as gripper:
            for i in range(args.cycles):
                trig = args.trig if args.cycles == 1 else trig_seq[i % len(trig_seq)]
                print(f"\n----- Cycle {i + 1}/{args.cycles} (trigger {trig}) -----")
                times.append(run_cycle(args, trig, robot, gripper))
    except ConnectionError as e:
        print(f"\n[Connection error] {e}", file=sys.stderr)
        sys.exit(1)
    except (TimeoutError, ValueError, IOError) as e:
        print(f"\n[Operation error] {e}", file=sys.stderr)
        sys.exit(2)

    if times:
        print("=" * 60)
        print(f"Complete {len(times)} cycle."
              f"Average cycle time {sum(times) / len(times):.2f}s"
              f"(min {min(times):.2f}s / max {max(times):.2f}s)")
        print("=" * 60)


if __name__ == "__main__":
    main()