---
# ----------------------------------------------------------------------------
# Service Commissioning File
# ----------------------------------------------------------------------------
# Copyright: Cybus GmbH
# Contact: support@cybus.io
# ----------------------------------------------------------------------------
# Connecting a Universal Robots Cobot (Example)
# ----------------------------------------------------------------------------

description: >
  Service commissioning file that reads robot mode, safety status, joint
  angles, and TCP position from a Universal Robots cobot over the built-in
  Modbus/TCP server and maps them into an ISA-95 style MQTT topic hierarchy
  (Example)

metadata:
  name: Universal Robots Cobot Example
  provider: cybus
  homepage: https://www.cybus.io
  version: 1.0.0

parameters:
  cobotHost:
    description: Hostname or IP address of the Universal Robots control box
    type: string
    default: 192.168.1.100

  cobotPort:
    description: Modbus/TCP port of the Universal Robots control box
    type: integer
    default: 502

definitions:
  # ISA-95 style topic prefix: <enterprise>/<site>/<area>/<line>/cobot
  MQTT_TOPIC_PREFIX: enterprise/hamburg/assembly/line-1/cobot

resources:
  # Connection to the Universal Robots control box (built-in Modbus/TCP
  # server on port 502)
  cobotConnection:
    type: Cybus::Connection
    properties:
      protocol: Modbus
      targetState: connected
      connection:
        host: !ref cobotHost
        port: !ref cobotPort
        connectionStrategy:
          initialDelay: 1000
          maxDelay: 30000
          incrementFactor: 2

  # Robot state registers. Register addresses from the register table of the
  # Universal Robots support article "Modbus server". Each value occupies
  # one 16-bit register.
  # Robot mode is decoded as int16BE because CB2 controllers report -1 for
  # "No controller".
  robotMode:
    type: Cybus::Endpoint
    properties:
      protocol: Modbus
      connection: !ref cobotConnection
      subscribe:
        fc: 3
        address: 258
        length: 1
        dataType: int16BE
        interval: 1000

  powerOn:
    type: Cybus::Endpoint
    properties:
      protocol: Modbus
      connection: !ref cobotConnection
      subscribe:
        fc: 3
        address: 260
        length: 1
        dataType: uint16BE
        interval: 1000

  protectiveStop:
    type: Cybus::Endpoint
    properties:
      protocol: Modbus
      connection: !ref cobotConnection
      subscribe:
        fc: 3
        address: 261
        length: 1
        dataType: uint16BE
        interval: 1000

  emergencyStop:
    type: Cybus::Endpoint
    properties:
      protocol: Modbus
      connection: !ref cobotConnection
      subscribe:
        fc: 3
        address: 262
        length: 1
        dataType: uint16BE
        interval: 1000

  # Joint angles: registers 270-275 report milli-radians as signed 16-bit
  # values. A transform rule converts them to degrees before publishing
  # (1 mrad = 180 / (1000 * pi) degrees = 0.0572957795 degrees).
  jointAngleBase:
    type: Cybus::Endpoint
    properties:
      protocol: Modbus
      connection: !ref cobotConnection
      subscribe:
        fc: 3
        address: 270
        length: 1
        dataType: int16BE
        interval: 500
      rules:
        - transform:
            expression: |
              {
                "timestamp": timestamp,
                "value": $round(value * 0.0572957795, 2)
              }

  jointAngleShoulder:
    type: Cybus::Endpoint
    properties:
      protocol: Modbus
      connection: !ref cobotConnection
      subscribe:
        fc: 3
        address: 271
        length: 1
        dataType: int16BE
        interval: 500
      rules:
        - transform:
            expression: |
              {
                "timestamp": timestamp,
                "value": $round(value * 0.0572957795, 2)
              }

  jointAngleElbow:
    type: Cybus::Endpoint
    properties:
      protocol: Modbus
      connection: !ref cobotConnection
      subscribe:
        fc: 3
        address: 272
        length: 1
        dataType: int16BE
        interval: 500
      rules:
        - transform:
            expression: |
              {
                "timestamp": timestamp,
                "value": $round(value * 0.0572957795, 2)
              }

  jointAngleWrist1:
    type: Cybus::Endpoint
    properties:
      protocol: Modbus
      connection: !ref cobotConnection
      subscribe:
        fc: 3
        address: 273
        length: 1
        dataType: int16BE
        interval: 500
      rules:
        - transform:
            expression: |
              {
                "timestamp": timestamp,
                "value": $round(value * 0.0572957795, 2)
              }

  jointAngleWrist2:
    type: Cybus::Endpoint
    properties:
      protocol: Modbus
      connection: !ref cobotConnection
      subscribe:
        fc: 3
        address: 274
        length: 1
        dataType: int16BE
        interval: 500
      rules:
        - transform:
            expression: |
              {
                "timestamp": timestamp,
                "value": $round(value * 0.0572957795, 2)
              }

  jointAngleWrist3:
    type: Cybus::Endpoint
    properties:
      protocol: Modbus
      connection: !ref cobotConnection
      subscribe:
        fc: 3
        address: 275
        length: 1
        dataType: int16BE
        interval: 500
      rules:
        - transform:
            expression: |
              {
                "timestamp": timestamp,
                "value": $round(value * 0.0572957795, 2)
              }

  # TCP position: registers 400-402 report tenths of millimeters in the base
  # frame as signed 16-bit values. A transform rule converts them to
  # millimeters before publishing.
  tcpPositionX:
    type: Cybus::Endpoint
    properties:
      protocol: Modbus
      connection: !ref cobotConnection
      subscribe:
        fc: 3
        address: 400
        length: 1
        dataType: int16BE
        interval: 500
      rules:
        - transform:
            expression: |
              {
                "timestamp": timestamp,
                "value": value / 10
              }

  tcpPositionY:
    type: Cybus::Endpoint
    properties:
      protocol: Modbus
      connection: !ref cobotConnection
      subscribe:
        fc: 3
        address: 401
        length: 1
        dataType: int16BE
        interval: 500
      rules:
        - transform:
            expression: |
              {
                "timestamp": timestamp,
                "value": value / 10
              }

  tcpPositionZ:
    type: Cybus::Endpoint
    properties:
      protocol: Modbus
      connection: !ref cobotConnection
      subscribe:
        fc: 3
        address: 402
        length: 1
        dataType: int16BE
        interval: 500
      rules:
        - transform:
            expression: |
              {
                "timestamp": timestamp,
                "value": value / 10
              }

  # General purpose register 128: written by the robot program with
  # write_port_register(128, <value>), for example as a cycle counter.
  cycleCounter:
    type: Cybus::Endpoint
    properties:
      protocol: Modbus
      connection: !ref cobotConnection
      subscribe:
        fc: 3
        address: 128
        length: 1
        dataType: uint16BE
        interval: 1000

  mapping:
    type: Cybus::Mapping
    properties:
      mappings:
        - subscribe:
            endpoint: !ref robotMode
          publish:
            topic: !sub '${MQTT_TOPIC_PREFIX}/state/robot-mode'
        - subscribe:
            endpoint: !ref powerOn
          publish:
            topic: !sub '${MQTT_TOPIC_PREFIX}/state/power-on'
        - subscribe:
            endpoint: !ref protectiveStop
          publish:
            topic: !sub '${MQTT_TOPIC_PREFIX}/state/protective-stop'
        - subscribe:
            endpoint: !ref emergencyStop
          publish:
            topic: !sub '${MQTT_TOPIC_PREFIX}/state/emergency-stop'
        - subscribe:
            endpoint: !ref jointAngleBase
          publish:
            topic: !sub '${MQTT_TOPIC_PREFIX}/joint/base/angle'
        - subscribe:
            endpoint: !ref jointAngleShoulder
          publish:
            topic: !sub '${MQTT_TOPIC_PREFIX}/joint/shoulder/angle'
        - subscribe:
            endpoint: !ref jointAngleElbow
          publish:
            topic: !sub '${MQTT_TOPIC_PREFIX}/joint/elbow/angle'
        - subscribe:
            endpoint: !ref jointAngleWrist1
          publish:
            topic: !sub '${MQTT_TOPIC_PREFIX}/joint/wrist-1/angle'
        - subscribe:
            endpoint: !ref jointAngleWrist2
          publish:
            topic: !sub '${MQTT_TOPIC_PREFIX}/joint/wrist-2/angle'
        - subscribe:
            endpoint: !ref jointAngleWrist3
          publish:
            topic: !sub '${MQTT_TOPIC_PREFIX}/joint/wrist-3/angle'
        - subscribe:
            endpoint: !ref tcpPositionX
          publish:
            topic: !sub '${MQTT_TOPIC_PREFIX}/tcp/position/x'
        - subscribe:
            endpoint: !ref tcpPositionY
          publish:
            topic: !sub '${MQTT_TOPIC_PREFIX}/tcp/position/y'
        - subscribe:
            endpoint: !ref tcpPositionZ
          publish:
            topic: !sub '${MQTT_TOPIC_PREFIX}/tcp/position/z'
        - subscribe:
            endpoint: !ref cycleCounter
          publish:
            topic: !sub '${MQTT_TOPIC_PREFIX}/program/cycle-counter'
