MODBUS Server (Robot = Slave)

"""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()

"""06. MODBUS SERVER (robot = slave) — PTP command cycle (server_motion)

As a master (higher PLC), it sends one cycle of PTP commands to the robot slave.
Goal: Move PTP (Joint) of A1 joint to 90.000°.

Follow the manual 5.3.3.5 command execution procedure as follows:
    ① HR[201] <- Command number (0=PTP)
    ② HR[202~218] <- Parameter (Motion type/6-axis L-H/Speed/Acceleration/Tool/Base)
    ③ HR[200] <- 1 (execution trigger)
    ④ Polling until IR[524] 2(Run)->1(Idle) & IR[200]=1(Success)
    ⑤ HR[200] <- 0 (master reset -> prepare for next command)

Why 90° is (24464, 1): 90.000° = 90×1000 = 90000.
Since it does not fit into one 16-bit word (-32768 to 32767), split it with split_word().
    Low = 90000 % 65536 = 24464
    High = 90000 // 65536 = 1
    Restore = 1×65536 + 24464 = 90000 -> ×0.001 = 90.000°

Precedence: Launch the robot slave in another terminal.
    python ../../_shared/robot_server_sim.py --port 1502 --host 127.0.0.1
Run:
    python src/server_motion/main.py --port 1502"""
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 time  # noqa: E402

from modbus_master import RobotMaster  # noqa: E402
from word_tools import split_word  # noqa: E402


def main():
    ap = argparse.ArgumentParser(description="PTP command cycle (robot=slave)")
    ap.add_argument("--host", default="127.0.0.1")
    ap.add_argument("--port", type=int, default=1502)
    args = ap.parse_args()

    A1_low, A1_high = split_word(90000)        # (24464, 1) = 90.000°

    print("=== PTP command cycle (A1=90° angle movement) ===")
    try:
        with RobotMaster(args.host, args.port) as m:
            #Write ① HR201=command number, ② HR202~218=parameters at once.
            #HR201 Command number 0 = PTP
            #HR202 Motion Type 0 = Joint
            #HR203~214 6 axis L/H (A1=24464,1 / A2~A6=0)
            #HR215~218 Speed 50% / Acceleration 100% / Tool1 / Base2
            params = [0,                                  #201 Command number PTP
                      0,                                  #202 Motion Type Joint
                      A1_low, A1_high,                    # 203/204 A1
                      0, 0, 0, 0, 0, 0, 0, 0, 0, 0,       # 205~214 A2~A6
                      50, 100, 1, 2]                      # 215~218
            m.write_holding(201, params)
            print(f"[①] HR[201]=0(PTP), HR[202]=0(Joint), "
                  f"HR[203:205]={[A1_low, A1_high]}  (split_word(90000))")
            print(f"[②] HR[215:219]=[50, 100, 1, 2] Speed 50% / Acceleration 100% / Tool1 / Base2")

            m.write_holding(200, 1)                       #③ Execution trigger
            print("[③] HR[200]<-1 execution trigger")

            #④ Wait for operation completion.
            #Success (IR200=1) and Idle (IR524=1) of the previous command may remain.
            #First, wait for this command to start (Running=2) and then wait for completion.
            while m.read_input(524, 1)[0] != 2:
                time.sleep(0.02)
            while not (m.read_input(524, 1)[0] == 1 and m.read_input(200, 1)[0] == 1):
                time.sleep(0.05)
            joint = m.read_input(300, 2)                  #Current joint A1 L/H
            angle = (joint[1] * 65536 + joint[0]) * 0.001
            print(f"[④] Wait for operation completion... IR[524]={m.read_input(524, 1)[0]}(Idle),"
                  f"IR[200]={m.read_input(200, 1)[0]}(Success)")
            print(f"Current joint IR[300:302] = {joint} -> {angle:.3f}°")

            m.write_holding(200, 0)                       #⑤ Trigger reset
            print("[⑤] HR[200]<-0 trigger reset (prepare for next command)")

        print("PTP cycle complete ✅")
        return 0
    except Exception as e:
        print(f"[Error] {e}", file=sys.stderr)
        print("Check if the robot slave is floating:"
              "python ../../_shared/robot_server_sim.py --port "
              f"{args.port} --host {args.host}", file=sys.stderr)
        return 1


if __name__ == "__main__":
    sys.exit(main())