330 lines
11 KiB
Python
330 lines
11 KiB
Python
#!/usr/bin/env python3
|
|
# -*- coding: utf-8 -*-
|
|
"""
|
|
CANopen ↔ PLC demo (python-can + canopen)
|
|
----------------------------------------
|
|
Fill in the CONFIG section to match your PLC / device.
|
|
Tested with python-can >= 4 and canopen >= 2.
|
|
|
|
Common use cases:
|
|
- Python as CANopen "master" talking to a PLC/device node (slave).
|
|
- Monitor/drive DS402-like nodes (Controlword/Statusword and Position/Velocity).
|
|
|
|
Author: ChatGPT
|
|
"""
|
|
|
|
import time
|
|
import threading
|
|
from typing import Optional, Dict, Any
|
|
|
|
import can # python-can
|
|
import canopen # python-canopen
|
|
|
|
|
|
# =====================
|
|
# CONFIG (EDIT ME)
|
|
# =====================
|
|
CONFIG: Dict[str, Any] = dict(
|
|
# Choose a backend matching your CAN interface:
|
|
# Linux SocketCAN example: backend="socketcan", channel="can0"
|
|
# PEAK PCAN example: backend="pcan", channel="PCAN_USBBUS1"
|
|
# Kvaser example: backend="kvaser", channel="0"
|
|
backend="socketcan",
|
|
channel="can0",
|
|
bitrate=500000, # 125000 / 250000 / 500000 / 1000000
|
|
|
|
# Target node (PLC or device) information
|
|
node_id=2, # PLC/device CANopen Node-ID (1..127)
|
|
eds_path="device.eds", # Path to the device EDS/DCF file (put the file next to this script)
|
|
|
|
# Heartbeat / SYNC (optional)
|
|
heartbeat_consumer_time_ms=0, # Set >0 to expect heartbeat from node
|
|
heartbeat_producer_time_ms=0, # Requires device support; 0 = don't change
|
|
use_sync=True, # Use SYNC object for synchronous PDOs
|
|
sync_period_ms=10, # SYNC period if acting as producer
|
|
|
|
# PDO mapping (DS402-like defaults) — adjust to your PLC/device map
|
|
# RxPDO: what Python sends TO the device (e.g., Controlword, Target position)
|
|
#rxpdo_map=(
|
|
# {"index": 0x6040, "subindex": 0x00, "size": 16}, # Controlword (UINT16)
|
|
# {"index": 0x607A, "subindex": 0x00, "size": 32}, # Target position (INT32)
|
|
#),
|
|
TxPDO: what Python receives FROM the device (e.g., Statusword, Actual position)
|
|
txpdo_map=(
|
|
{"index": 0x6041, "subindex": 0x00, "size": 16}, # Statusword (UINT16)
|
|
{"index": 0x6064, "subindex": 0x00, "size": 32}, # Position actual value (INT32)
|
|
),
|
|
# Transmission types:
|
|
# For SYNC: usually 1..240 (1=sub-every SYNC). For async/evented: 255.
|
|
rxpdo_trans_type=1,
|
|
txpdo_trans_type=1,
|
|
|
|
# Demo motion (for DS402 profile position mode)
|
|
do_enable_sequence=True,
|
|
target_positions=[0, 10000, -5000, 0],
|
|
dwell_ms=500, # wait time after each command
|
|
)
|
|
|
|
|
|
# =============
|
|
# Helpers
|
|
# =============
|
|
def make_bus(cfg: Dict[str, Any]) -> can.Bus:
|
|
"""Create and return a python-can Bus."""
|
|
backend = cfg["backend"].lower()
|
|
if backend == "socketcan":
|
|
bus = can.interface.Bus(bustype="socketcan", channel=cfg["channel"], bitrate=cfg["bitrate"])
|
|
elif backend == "pcan":
|
|
bus = can.interface.Bus(bustype="pcan", channel=cfg["channel"], bitrate=cfg["bitrate"])
|
|
elif backend == "kvaser":
|
|
bus = can.interface.Bus(bustype="kvaser", channel=cfg["channel"], bitrate=cfg["bitrate"])
|
|
else:
|
|
raise ValueError(f"Unsupported backend: {backend}")
|
|
return bus
|
|
|
|
|
|
def setup_network(cfg: Dict[str, Any]) -> canopen.Network:
|
|
"""Attach canopen.Network to an existing python-can Bus."""
|
|
bus = make_bus(cfg)
|
|
net = canopen.Network()
|
|
# Attach python-can bus
|
|
net.connect(bustype="python-can", channel=bus)
|
|
return net
|
|
|
|
|
|
def add_node(net: canopen.Network, cfg: Dict[str, Any]) -> canopen.RemoteNode:
|
|
"""Add a remote node using an EDS file."""
|
|
node = canopen.RemoteNode(cfg["node_id"], cfg["eds_path"])
|
|
net.add_node(node)
|
|
return node
|
|
|
|
|
|
def set_heartbeat(node: canopen.RemoteNode, cfg: Dict[str, Any]) -> None:
|
|
"""Optionally configure heartbeat via SDO (if supported by device)."""
|
|
try:
|
|
if cfg.get("heartbeat_producer_time_ms", 0) > 0:
|
|
node.sdo[0x1017].raw = int(cfg["heartbeat_producer_time_ms"])
|
|
# Heartbeat consumer is typically configured on the *master* side; for canopen lib,
|
|
# you can add a consumer to monitor a node. Here we use basic timeout checks in the loop.
|
|
except Exception as e:
|
|
print(f"[WARN] Heartbeat config skipped: {e}")
|
|
|
|
|
|
def clear_pdos(node: canopen.RemoteNode) -> None:
|
|
"""Disable and clear all existing PDO mappings before re-mapping."""
|
|
# Disable all TxPDOs and RxPDOs
|
|
for pdo in node.tpdo.values():
|
|
pdo.enabled = False
|
|
pdo.clear()
|
|
pdo.save()
|
|
for pdo in node.rpdo.values():
|
|
pdo.enabled = False
|
|
pdo.clear()
|
|
pdo.save()
|
|
|
|
|
|
def map_pdos(node: canopen.RemoteNode, cfg: Dict[str, Any]) -> None:
|
|
"""Map RxPDO/TxPDO according to CONFIG and enable them."""
|
|
# --- Map RxPDOs (what WE send TO device) ---
|
|
rpdo1 = node.rpdo[1]
|
|
rpdo1.clear()
|
|
for m in cfg["rxpdo_map"]:
|
|
rpdo1.add_variable(m["index"], m["subindex"], length=m["size"])
|
|
rpdo1.trans_type = cfg["rxpdo_trans_type"]
|
|
rpdo1.enabled = True
|
|
rpdo1.save()
|
|
|
|
# --- Map TxPDOs (what device sends TO us) ---
|
|
tpdo1 = node.tpdo[1]
|
|
tpdo1.clear()
|
|
for m in cfg["txpdo_map"]:
|
|
tpdo1.add_variable(m["index"], m["subindex"], length=m["size"])
|
|
tpdo1.trans_type = cfg["txpdo_trans_type"]
|
|
tpdo1.enabled = True
|
|
tpdo1.save()
|
|
|
|
# Apply configuration on device
|
|
node.nmt.state = 'PRE-OPERATIONAL'
|
|
node.nmt.state = 'OPERATIONAL'
|
|
|
|
|
|
def start_sync_producer(net: canopen.Network, period_ms: int) -> threading.Thread:
|
|
"""Start a SYNC producer thread (if this controller should emit SYNC)."""
|
|
stop_flag = {"stop": False}
|
|
|
|
def _loop():
|
|
sync = canopen.sync.SyncProducer(net)
|
|
interval = max(0.001, period_ms / 1000.0)
|
|
while not stop_flag["stop"]:
|
|
sync.transmit()
|
|
time.sleep(interval)
|
|
|
|
t = threading.Thread(target=_loop, daemon=True)
|
|
t.stop_flag = stop_flag # type: ignore[attr-defined]
|
|
t.start()
|
|
return t
|
|
|
|
|
|
# =============
|
|
# DS402 helpers
|
|
# =============
|
|
CW_SHUTDOWN = 0x0006
|
|
CW_SWITCH_ON = 0x0007
|
|
CW_ENABLE_OPERATION = 0x000F
|
|
CW_DISABLE_VOLTAGE = 0x0000
|
|
CW_QUICK_STOP = 0x0002
|
|
CW_FAULT_RESET = 0x0080
|
|
|
|
|
|
def drive_enable_sequence(node: canopen.RemoteNode) -> None:
|
|
"""Minimal DS402 enable sequence using SDO Controlword/Statusword."""
|
|
cw = node.sdo[0x6040]
|
|
sw = node.sdo[0x6041]
|
|
|
|
def _write(value: int):
|
|
cw.raw = value
|
|
time.sleep(0.05)
|
|
|
|
# Fault reset if needed
|
|
if sw.raw & 0x0008: # Fault bit
|
|
_write(CW_FAULT_RESET)
|
|
time.sleep(0.1)
|
|
|
|
_write(CW_SHUTDOWN)
|
|
_write(CW_SWITCH_ON)
|
|
_write(CW_ENABLE_OPERATION)
|
|
|
|
|
|
def set_mode_of_operation(node: canopen.RemoteNode, mode: int) -> None:
|
|
"""Set DS402 mode of operation (e.g., 1=Profile Position, 3=Profile Velocity, 6=Homing, 8=CSP...)."""
|
|
node.sdo[0x6060].raw = int(mode)
|
|
# Optional: read back
|
|
_ = node.sdo[0x6061].raw
|
|
|
|
|
|
# =============
|
|
# Main demo
|
|
# =============
|
|
def main() -> None:
|
|
cfg = CONFIG
|
|
print("Connecting CAN...")
|
|
net = setup_network(cfg)
|
|
print(f"Connected via {cfg['backend']}:{cfg['channel']} @ {cfg['bitrate']}")
|
|
|
|
# Add node
|
|
node = add_node(net, cfg)
|
|
|
|
# NMT reset + pre-op
|
|
print("Resetting node...")
|
|
node.nmt.reset_node()
|
|
time.sleep(0.5)
|
|
node.nmt.state = 'PRE-OPERATIONAL'
|
|
time.sleep(0.2)
|
|
|
|
# Optional: heartbeat
|
|
set_heartbeat(node, cfg)
|
|
|
|
# PDO mapping
|
|
print("Mapping PDOs...")
|
|
clear_pdos(node)
|
|
map_pdos(node, cfg)
|
|
|
|
# Optional: start SYNC producer if we are the controller
|
|
sync_thread: Optional[threading.Thread] = None
|
|
if cfg.get("use_sync", False):
|
|
print(f"Starting SYNC producer @ {cfg['sync_period_ms']} ms")
|
|
sync_thread = start_sync_producer(net, cfg["sync_period_ms"])
|
|
|
|
# Go operational
|
|
print("Switching to OPERATIONAL")
|
|
node.nmt.state = 'OPERATIONAL'
|
|
time.sleep(0.1)
|
|
|
|
# Enable sequence (DS402)
|
|
if cfg.get("do_enable_sequence", True):
|
|
try:
|
|
print("Setting mode of operation = 1 (Profile Position)")
|
|
set_mode_of_operation(node, 1)
|
|
except Exception as e:
|
|
print(f"[WARN] Skipping mode set: {e}")
|
|
print("Running enable sequence...")
|
|
try:
|
|
drive_enable_sequence(node)
|
|
except Exception as e:
|
|
print(f"[WARN] Enable sequence failed/skipped: {e}")
|
|
|
|
# Shortcut handles to mapped PDO variables
|
|
# RxPDO1 variables we control
|
|
try:
|
|
rpdo1 = node.rpdo[1]
|
|
cw_var = rpdo1['Controlword'] if 'Controlword' in rpdo1.map else rpdo1[0]
|
|
tgt_pos_var = rpdo1['Target position'] if 'Target position' in rpdo1.map else rpdo1[1]
|
|
except Exception:
|
|
rpdo1 = None
|
|
cw_var = None
|
|
tgt_pos_var = None
|
|
|
|
# TxPDO1 variables we read
|
|
try:
|
|
tpdo1 = node.tpdo[1]
|
|
sw_var = tpdo1['Statusword'] if 'Statusword' in tpdo1.map else tpdo1[0]
|
|
act_pos_var = tpdo1['Position actual value'] if 'Position actual value' in tpdo1.map else tpdo1[1]
|
|
except Exception:
|
|
tpdo1 = None
|
|
sw_var = None
|
|
act_pos_var = None
|
|
|
|
# Demo loop: write a few target positions and read back status/position
|
|
try:
|
|
for idx, target in enumerate(cfg.get("target_positions", []), 1):
|
|
print(f"\n[{idx}] Command target position = {target}")
|
|
if rpdo1 is not None and tgt_pos_var is not None:
|
|
tgt_pos_var.raw = int(target)
|
|
# Ensure CW has Enable Operation bit set when using RxPDO
|
|
if cw_var is not None:
|
|
cw_var.raw = CW_ENABLE_OPERATION
|
|
rpdo1.transmit()
|
|
else:
|
|
# Fallback via SDO
|
|
print(" (No RxPDO mapped; writing via SDO 0x607A)")
|
|
node.sdo[0x607A].raw = int(target)
|
|
node.sdo[0x6040].raw = CW_ENABLE_OPERATION
|
|
|
|
# Read back
|
|
t0 = time.time()
|
|
while time.time() - t0 < (cfg.get("dwell_ms", 500) / 1000.0):
|
|
if tpdo1 is not None:
|
|
# Request update on SYNC; if evented/cyclic, values will refresh automatically
|
|
time.sleep(0.01)
|
|
try:
|
|
sw = sw_var.raw if sw_var is not None else None
|
|
pos = act_pos_var.raw if act_pos_var is not None else None
|
|
print(f" TPDO: Status=0x{(sw or 0):04X}, Position={pos}")
|
|
except Exception:
|
|
pass
|
|
else:
|
|
# Read via SDO occasionally
|
|
try:
|
|
sw = node.sdo[0x6041].raw
|
|
pos = node.sdo[0x6064].raw
|
|
print(f" SDO: Status=0x{sw:04X}, Position={pos}")
|
|
except Exception:
|
|
pass
|
|
time.sleep(0.05)
|
|
|
|
print("\nDemo completed.")
|
|
finally:
|
|
# Stop SYNC
|
|
if sync_thread is not None:
|
|
sync_thread.stop_flag["stop"] = True # type: ignore[attr-defined]
|
|
time.sleep(0.05)
|
|
# Disconnect
|
|
try:
|
|
net.disconnect()
|
|
except Exception:
|
|
pass
|
|
|
|
|
|
if __name__ == "__main__":
|
|
main()
|