init

sdk/changingtek_p_rtu_Servo.py

@@ -0,0 +1,284 @@
+"""
+CTAG2F90D（Servo）控制器SDK (基于Modbus RTU的RS-485通信)
+---------------------------------------------
+一个轻量级的Python SDK，用于通过minimalmodbus库控制电机，
+具有线程安全、清晰的寄存器访问方式和实时数据监控功能。
+
+依赖安装:
+    pip install minimalmodbus pyserial
+
+作者: 知行机器人
+"""
+
+import time
+import threading
+import minimalmodbus
+import serial
+
+# -----------------------------
+# 寄存器映射 (保持寄存器，功能码0x03/0x06)
+# -----------------------------
+# 写入寄存器 (目标值/控制)
+REG_TARGET_POS_HIGH       = 0x0102  # 目标位置（高16位）
+REG_TARGET_POS_LOW        = 0x0103  # 目标位置（低16位）
+REG_TARGET_SPEED          = 0x0104  # 目标速度
+REG_TARGET_FORCE          = 0x0105  # 目标力/力矩
+REG_TARGET_ACCELERATION   = 0x0106  # 目标加速度
+REG_TARGET_DECELERATION   = 0x0107  # 目标减速度
+REG_MOTION_TRIGGER        = 0x0108  # 运动触发（0: 空闲, 1: 触发）
+
+# 读取寄存器 (反馈/状态)
+REG_REAL_POS_HIGH         = 0x0418  # 实时位置（高16位）
+REG_REAL_POS_LOW          = 0x0419  # 实时位置（低16位）
+REG_REAL_SPEED            = 0x041A  # 实时速度反馈
+REG_REAL_CURRENT          = 0x041B  # 实时电流反馈
+
+
+class MotorController:
+    def __init__(self, port: str, slave_id: int = 1, baudrate: int = 115200, timeout: float = 1.0):
+        """初始化与电机控制器的Modbus RTU通信
+
+        参数:
+            port: 串口端口（例如，Windows上为'COM3'，Linux上为'/dev/ttyUSB0'）
+            slave_id: Modbus从机地址（默认: 1）
+            baudrate: 串口波特率（默认: 115200）
+            timeout: 读写超时时间（秒，默认: 1.0）
+        """
+        self.instrument = minimalmodbus.Instrument(port, slave_id)
+        self.instrument.serial.baudrate = baudrate
+        self.instrument.serial.bytesize = 8
+        self.instrument.serial.parity = serial.PARITY_NONE
+        self.instrument.serial.stopbits = 1
+        self.instrument.serial.timeout = timeout
+        self.instrument.mode = minimalmodbus.MODE_RTU
+        self._lock = threading.Lock()  # 线程锁，确保串口操作线程安全
+        self._monitor_running = False  # 监控线程运行标志
+        self._monitor_thread = None    # 监控线程对象
+
+    # -----------------------------
+    # 低级寄存器操作工具方法
+    # -----------------------------
+    def _write_register(self, addr: int, value: int) -> None:
+        """写入单个16位保持寄存器（Modbus功能码0x06）
+
+        参数:
+            addr: 寄存器地址
+            value: 要写入的值
+        """
+        with self._lock:
+            self.instrument.write_register(addr, value, functioncode=6)
+
+    def _read_register(self, addr: int) -> int:
+        """读取单个16位保持寄存器（Modbus功能码0x03）
+
+        参数:
+            addr: 寄存器地址
+
+        返回:
+            寄存器的值
+        """
+        with self._lock:
+            return self.instrument.read_register(addr, functioncode=3)
+
+    def _write_registers(self, addr: int, values: list) -> None:
+        """写入多个16位保持寄存器（Modbus功能码0x10）
+
+        参数:
+            addr: 起始寄存器地址
+            values: 要写入的值列表
+        """
+        with self._lock:
+            self.instrument.write_registers(addr, values)
+
+    def _read_registers(self, addr: int, count: int) -> list:
+        """读取多个16位保持寄存器（Modbus功能码0x03）
+
+        参数:
+            addr: 起始寄存器地址
+            count: 要读取的寄存器数量
+
+        返回:
+            寄存器值的列表
+        """
+        with self._lock:
+            return self.instrument.read_registers(addr, count, functioncode=3)
+
+    # -----------------------------
+    # 目标值/控制写入操作
+    # -----------------------------
+    def set_target_position(self, position: int) -> None:
+        """设置目标位置（组合高低16位寄存器）
+
+        参数:
+            position: 目标位置值
+        """
+        hi = (position >> 16) & 0xFFFF  # 提取高16位
+        lo = position & 0xFFFF          # 提取低16位
+        self._write_registers(REG_TARGET_POS_HIGH, [hi, lo])
+
+    def set_target_speed(self, speed: int) -> None:
+        """设置目标速度
+
+        参数:
+            speed: 目标速度值
+        """
+        self._write_register(REG_TARGET_SPEED, speed)
+
+    def set_target_force(self, force: int) -> None:
+        """设置目标力/力矩
+
+        参数:
+            force: 目标力/力矩值
+        """
+        self._write_register(REG_TARGET_FORCE, force)
+
+    def set_target_acceleration(self, acceleration: int) -> None:
+        """设置目标加速度
+
+        参数:
+            acceleration: 目标加速度值
+        """
+        self._write_register(REG_TARGET_ACCELERATION, acceleration)
+
+    def set_target_deceleration(self, deceleration: int) -> None:
+        """设置目标减速度
+
+        参数:
+            deceleration: 目标减速度值
+        """
+        self._write_register(REG_TARGET_DECELERATION, deceleration)
+
+    def trigger_motion(self) -> None:
+        """使用已配置的目标参数触发运动"""
+        self._write_register(REG_MOTION_TRIGGER, 1)
+
+    def temp_move(self, position_mm: int, speed_pct: int, force_pct: int,
+                  accel: int, decel: int, trigger: bool = True) -> None:
+        """临时运动控制方法，一次性设置所有运动参数并可选触发运动
+
+        参数:
+            position_mm: 目标位置(毫米)
+            speed_pct: 速度百分比
+            force_pct: 力/力矩百分比
+            accel: 加速度值
+            decel: 减速度值
+            trigger: 是否立即触发运动，默认为True
+        """
+        self.set_target_position(position_mm)
+        self.set_target_speed(speed_pct)
+        self.set_target_force(force_pct)
+        self.set_target_acceleration(accel)
+        self.set_target_deceleration(decel)
+
+        if trigger:
+            self.trigger_motion()
+            print(f"已触发运动到位置 {position_mm}，速度 {speed_pct}%，力 {force_pct}%")
+
+    # -----------------------------
+    # 反馈/状态读取操作
+    # -----------------------------
+    def read_real_position(self) -> int:
+        """读取实时位置（组合高低16位寄存器）
+
+        返回:
+            组合后的32位实时位置（处理有符号值）
+        """
+        regs = self._read_registers(REG_REAL_POS_HIGH, 2)
+        hi, lo = regs[0], regs[1]
+        combined = (hi << 16) | lo
+        # 处理32位有符号整数
+        if combined & 0x80000000:  # 最高位为1表示负数
+            combined = combined - 0x100000000
+        return combined
+
+    def read_real_speed(self) -> int:
+        """读取实时速度反馈
+
+        返回:
+            实时速度值
+        """
+        return self._read_register(REG_REAL_SPEED)
+
+    def read_real_current(self) -> int:
+        """读取实时电流反馈
+
+        返回:
+            实时电流值
+        """
+        return self._read_register(REG_REAL_CURRENT)
+
+    # -----------------------------
+    # 实时监控线程
+    # -----------------------------
+    def _monitor_loop(self, interval: float = 0.5) -> None:
+        """实时数据监控的后台循环"""
+        while self._monitor_running:
+            try:
+                pos = self.read_real_position()
+                speed = self.read_real_speed()
+                current = self.read_real_current()
+                print(f"[实时数据] 位置: {pos:6d}, 速度: {speed:3d}, 电流: {current:3d}")
+            except Exception as e:
+                print(f"监控错误: {e}")
+            time.sleep(interval)
+
+    def start_monitoring(self, interval: float = 0.01) -> None:
+        """启动实时数据监控线程
+
+        参数:
+            interval: 监控间隔（秒，默认: 0.01）
+        """
+        if not self._monitor_running:
+            self._monitor_running = True
+            self._monitor_thread = threading.Thread(
+                target=self._monitor_loop,
+                args=(interval,)
+            )
+            self._monitor_thread.daemon = True  # 守护线程，主程序退出时自动结束
+            self._monitor_thread.start()
+            print("实时监控已启动。")
+
+    def stop_monitoring(self) -> None:
+        """停止实时数据监控线程"""
+        self._monitor_running = False
+        if self._monitor_thread:
+            self._monitor_thread.join(timeout=1.0)  # 等待线程结束
+        print("实时监控已停止。")
+
+
+if __name__ == "__main__":
+    # 示例用法（根据实际设备调整端口/从机ID）
+    PORT = "COM5"
+    SLAVE_ID = 1
+    sdk = MotorController(PORT, SLAVE_ID, baudrate=115200, timeout=1.0)
+
+    try:
+        # 启动实时监控
+        sdk.start_monitoring(interval=0.5)
+        
+        # 循环控制：5次往复运动
+        # 假设 9000 为闭合位置，0 为张开位置
+        # 最后一次循环结束后，确保处于张开位置 (0)
+        for i in range(5):
+            print(f"\n--- 第 {i+1}/5 次循环 ---")
+            
+            # 阶段1: 运动到位置 9000 (闭合?)
+            sdk.temp_move(position_mm=9000, speed_pct=5, force_pct=15, accel=60, decel=60, trigger=True)
+            time.sleep(5)  # 等待运动完成
+            
+            # 阶段2: 运动到位置 0 (张开?)
+            sdk.temp_move(position_mm=0, speed_pct=5, force_pct=15, accel=60, decel=60, trigger=True)
+            time.sleep(5)  # 等待运动完成
+            
+        print("\n循环结束，确认最终状态为张开 (位置 0)...")
+        # 再次发送指令确保最终状态为 0 (虽然循环最后一步已经是 0，但这保证了"最后要张开"的要求)
+        sdk.temp_move(position_mm=0, speed_pct=5, force_pct=15, accel=60, decel=60, trigger=True)
+        time.sleep(1)
+
+    except KeyboardInterrupt:
+        print("\n程序被用户中断。")
+    except Exception as e:
+        print(f"执行过程中出错: {e}")
+    finally:
+        sdk.stop_monitoring()
+        print("程序已完成。")

sdk/changingtek_p_rtu_Servo_cpp_version/Changingtek_p_rtu_Servo.cpp

@@ -0,0 +1,517 @@
+#include "Changingtek_p_rtu_Servo.h"
+#include <iostream>
+#include <chrono>
+#include <thread>
+#include <sstream>
+#include <iomanip>
+#include <cstring>
+
+#ifndef _WIN32
+#include <sys/select.h>
+#endif
+
+// 辅助函数: 字节交换 (Modbus 是大端序)
+static uint16_t swap_bytes(uint16_t val) {
+    return (val << 8) | (val >> 8);
+}
+
+Changingtek_p_rtu_Servo::Changingtek_p_rtu_Servo(const std::string& port, int slave_id, int baudrate, double timeout)
+    : port_name(port), slave_id(slave_id), baudrate(baudrate), timeout(timeout), hSerial(INVALID_SERIAL_HANDLE), _monitor_running(false) {
+}
+
+Changingtek_p_rtu_Servo::~Changingtek_p_rtu_Servo() {
+    stop_monitoring();
+    disconnect();
+}
+
+bool Changingtek_p_rtu_Servo::connect() {
+    std::lock_guard<std::mutex> lock(_mutex);
+#ifdef _WIN32
+    // 打开串口
+    // 使用 \\.\COMxx 格式以支持 COM10 以上的端口
+    std::string full_port_name = port_name;
+    if (full_port_name.find("\\\\.\\") == std::string::npos) {
+        full_port_name = "\\\\.\\" + port_name;
+    }
+    
+    hSerial = CreateFileA(full_port_name.c_str(),
+                          GENERIC_READ | GENERIC_WRITE,
+                          0,
+                          NULL,
+                          OPEN_EXISTING,
+                          0,
+                          NULL);
+
+    if (hSerial == INVALID_HANDLE_VALUE) {
+        std::cerr << "Error opening serial port: " << port_name << " (Error: " << GetLastError() << ")" << std::endl;
+        return false;
+    }
+
+    // 配置串口参数
+    DCB dcbSerialParams = {0};
+    dcbSerialParams.DCBlength = sizeof(dcbSerialParams);
+
+    if (!GetCommState(hSerial, &dcbSerialParams)) {
+        std::cerr << "Error getting serial state" << std::endl;
+        CloseHandle(hSerial);
+        hSerial = INVALID_HANDLE_VALUE;
+        return false;
+    }
+
+    dcbSerialParams.BaudRate = baudrate;
+    dcbSerialParams.ByteSize = 8;
+    dcbSerialParams.StopBits = ONESTOPBIT;
+    dcbSerialParams.Parity = NOPARITY;
+
+    if (!SetCommState(hSerial, &dcbSerialParams)) {
+        std::cerr << "Error setting serial state" << std::endl;
+        CloseHandle(hSerial);
+        hSerial = INVALID_HANDLE_VALUE;
+        return false;
+    }
+
+    // 设置超时
+    COMMTIMEOUTS timeouts = {0};
+    DWORD timeout_ms = static_cast<DWORD>(timeout * 1000);
+    
+    // ReadIntervalTimeout: 字节间最大间隔时间
+    timeouts.ReadIntervalTimeout = 50; 
+    // 总读取超时 = Multiplier * 字节数 + Constant
+    timeouts.ReadTotalTimeoutConstant = timeout_ms;
+    timeouts.ReadTotalTimeoutMultiplier = 10;
+    timeouts.WriteTotalTimeoutConstant = timeout_ms;
+    timeouts.WriteTotalTimeoutMultiplier = 10;
+
+    if (!SetCommTimeouts(hSerial, &timeouts)) {
+        std::cerr << "Error setting serial timeouts" << std::endl;
+        CloseHandle(hSerial);
+        hSerial = INVALID_HANDLE_VALUE;
+        return false;
+    }
+
+    return true;
+#else
+    // Linux implementation
+    hSerial = open(port_name.c_str(), O_RDWR | O_NOCTTY | O_SYNC);
+    if (hSerial < 0) {
+        std::cerr << "Error opening serial port: " << port_name << " (Error: " << strerror(errno) << ")" << std::endl;
+        return false;
+    }
+
+    struct termios tty;
+    if (tcgetattr(hSerial, &tty) != 0) {
+        std::cerr << "Error from tcgetattr: " << strerror(errno) << std::endl;
+        close(hSerial);
+        hSerial = INVALID_SERIAL_HANDLE;
+        return false;
+    }
+
+    cfsetospeed(&tty, B115200); // 默认先设为 115200, 后面根据参数调整
+    cfsetispeed(&tty, B115200);
+
+    // 设置自定义波特率
+    speed_t speed;
+    switch (baudrate) {
+        case 9600:   speed = B9600; break;
+        case 19200:  speed = B19200; break;
+        case 38400:  speed = B38400; break;
+        case 57600:  speed = B57600; break;
+        case 115200: speed = B115200; break;
+        case 230400: speed = B230400; break;
+        default:     speed = B115200; break;
+    }
+    cfsetospeed(&tty, speed);
+    cfsetispeed(&tty, speed);
+
+    tty.c_cflag = (tty.c_cflag & ~CSIZE) | CS8;     // 8-bit chars
+    tty.c_iflag &= ~IGNBRK;                         // disable break processing
+    tty.c_lflag = 0;                                // no signaling chars, no echo,
+                                                    // no canonical processing
+    tty.c_oflag = 0;                                // no remapping, no delays
+    tty.c_cc[VMIN]  = 0;                            // read doesn't block
+    tty.c_cc[VTIME] = 5;                            // 0.5 seconds read timeout
+
+    tty.c_iflag &= ~(IXON | IXOFF | IXANY);         // shut off xon/xoff ctrl
+
+    tty.c_cflag |= (CLOCAL | CREAD);                // ignore modem controls,
+                                                    // enable reading
+    tty.c_cflag &= ~(PARENB | PARODD);              // shut off parity
+    tty.c_cflag &= ~CSTOPB;                         // 1 stop bit
+
+    if (tcsetattr(hSerial, TCSANOW, &tty) != 0) {
+        std::cerr << "Error from tcsetattr: " << strerror(errno) << std::endl;
+        close(hSerial);
+        hSerial = INVALID_SERIAL_HANDLE;
+        return false;
+    }
+    return true;
+#endif
+}
+
+void Changingtek_p_rtu_Servo::disconnect() {
+    std::lock_guard<std::mutex> lock(_mutex);
+    if (hSerial != INVALID_SERIAL_HANDLE) {
+#ifdef _WIN32
+        CloseHandle(hSerial);
+#else
+        close(hSerial);
+#endif
+        hSerial = INVALID_SERIAL_HANDLE;
+    }
+}
+
+// -----------------------------
+// Modbus 核心实现
+// -----------------------------
+
+uint16_t Changingtek_p_rtu_Servo::calculate_crc(const std::vector<uint8_t>& data) {
+    uint16_t crc = 0xFFFF;
+    for (uint8_t byte : data) {
+        crc ^= byte;
+        for (int i = 0; i < 8; ++i) {
+            if (crc & 0x0001) {
+                crc >>= 1;
+                crc ^= 0xA001;
+            } else {
+                crc >>= 1;
+            }
+        }
+    }
+    return crc;
+}
+
+void Changingtek_p_rtu_Servo::send_frame(const std::vector<uint8_t>& frame) {
+    if (hSerial == INVALID_SERIAL_HANDLE) {
+        // 尝试重新连接，但不在此处加锁，因为调用方已经加锁了
+        // 这里需要小心死锁，假设 send_frame 总是由公有方法调用，而公有方法已经加锁
+        // 如果 connect 内部也加锁，需要使用 std::recursive_mutex 或者把锁逻辑移到外层
+        // 为了简单起见，我们假设 connect 是线程安全的或者被外层锁保护
+        // 但 connect 目前使用了 lock_guard，这会导致死锁如果在这里调用。
+        // 修改策略：底层函数不负责连接，只负责发送。连接由上层保证。
+        // 或者：connect 使用 try_lock 或者 recursive_mutex。
+        // 鉴于目前架构，我们在 connect 中使用了 lock_guard。
+        // 我们应该在 send_frame 之前确保连接。
+        throw_error("Port not open");
+    }
+
+#ifdef _WIN32
+    PurgeComm(hSerial, PURGE_RXCLEAR | PURGE_TXCLEAR);
+    DWORD bytes_written;
+    if (!WriteFile(hSerial, frame.data(), static_cast<DWORD>(frame.size()), &bytes_written, NULL)) {
+        throw_error("Write failed");
+    }
+    if (bytes_written != frame.size()) {
+        throw_error("Write incomplete");
+    }
+#else
+    tcflush(hSerial, TCIOFLUSH);
+    ssize_t bytes_written = write(hSerial, frame.data(), frame.size());
+    if (bytes_written < 0) {
+        throw_error("Write failed: " + std::string(strerror(errno)));
+    }
+    if (static_cast<size_t>(bytes_written) != frame.size()) {
+        throw_error("Write incomplete");
+    }
+#endif
+}
+
+std::vector<uint8_t> Changingtek_p_rtu_Servo::receive_response(int expected_min_bytes) {
+    std::vector<uint8_t> buffer;
+    buffer.reserve(256);
+    uint8_t tmp_buf[256];
+    
+#ifdef _WIN32
+    DWORD bytes_read;
+    if (!ReadFile(hSerial, tmp_buf, 256, &bytes_read, NULL)) {
+        throw_error("Read failed");
+    }
+    if (bytes_read == 0) {
+        throw_error("Read timeout (no data)");
+    }
+    for (DWORD i = 0; i < bytes_read; i++) {
+        buffer.push_back(tmp_buf[i]);
+    }
+#else
+    fd_set set;
+    struct timeval timeout_tv;
+    
+    timeout_tv.tv_sec = static_cast<long>(timeout);
+    timeout_tv.tv_usec = static_cast<long>((timeout - static_cast<long>(timeout)) * 1000000);
+
+    FD_ZERO(&set);
+    FD_SET(hSerial, &set);
+
+    int rv = select(hSerial + 1, &set, NULL, NULL, &timeout_tv);
+    if (rv == -1) {
+        throw_error("Select failed: " + std::string(strerror(errno)));
+    } else if (rv == 0) {
+        throw_error("Read timeout (no data)");
+    } else {
+        ssize_t n = read(hSerial, tmp_buf, sizeof(tmp_buf));
+        if (n < 0) {
+             throw_error("Read failed: " + std::string(strerror(errno)));
+        }
+        if (n == 0) {
+             throw_error("Read returned 0 (EOF?)");
+        }
+        for (ssize_t i = 0; i < n; i++) {
+            buffer.push_back(tmp_buf[i]);
+        }
+    }
+#endif
+
+    if (buffer.size() < 2) throw_error("Response too short");
+    
+    uint16_t received_crc = buffer[buffer.size() - 2] | (buffer[buffer.size() - 1] << 8);
+    std::vector<uint8_t> data_for_crc(buffer.begin(), buffer.end() - 2);
+    uint16_t calc_crc = calculate_crc(data_for_crc);
+
+    if (received_crc != calc_crc) {
+        throw_error("CRC Error");
+    }
+
+    return buffer;
+}
+
+void Changingtek_p_rtu_Servo::throw_error(const std::string& msg) {
+    throw std::runtime_error("Changingtek_p_rtu_Servo Error: " + msg);
+}
+
+// -----------------------------
+// Modbus 辅助方法
+// -----------------------------
+
+void Changingtek_p_rtu_Servo::_write_register(uint16_t addr, uint16_t value) {
+    std::lock_guard<std::mutex> lock(_mutex);
+    if (hSerial == INVALID_SERIAL_HANDLE) throw_error("Not connected");
+
+    std::vector<uint8_t> frame;
+    frame.push_back(static_cast<uint8_t>(slave_id));
+    frame.push_back(0x06); // Write Single Register
+    frame.push_back(addr >> 8);
+    frame.push_back(addr & 0xFF);
+    frame.push_back(value >> 8);
+    frame.push_back(value & 0xFF);
+
+    uint16_t crc = calculate_crc(frame);
+    frame.push_back(crc & 0xFF);
+    frame.push_back(crc >> 8);
+
+    send_frame(frame);
+    
+    // 接收响应 (回显)
+    receive_response();
+}
+
+void Changingtek_p_rtu_Servo::_write_registers(uint16_t addr, const std::vector<uint16_t>& values) {
+    std::lock_guard<std::mutex> lock(_mutex);
+    if (hSerial == INVALID_SERIAL_HANDLE) throw_error("Not connected");
+
+    std::vector<uint8_t> frame;
+    frame.push_back(static_cast<uint8_t>(slave_id));
+    frame.push_back(0x10); // Write Multiple Registers
+    frame.push_back(addr >> 8);
+    frame.push_back(addr & 0xFF);
+    uint16_t count = static_cast<uint16_t>(values.size());
+    frame.push_back(count >> 8);
+    frame.push_back(count & 0xFF);
+    frame.push_back(count * 2); // Byte count
+
+    for (uint16_t val : values) {
+        frame.push_back(val >> 8);
+        frame.push_back(val & 0xFF);
+    }
+
+    uint16_t crc = calculate_crc(frame);
+    frame.push_back(crc & 0xFF);
+    frame.push_back(crc >> 8);
+
+    send_frame(frame);
+    
+    // 接收响应
+    receive_response();
+}
+
+uint16_t Changingtek_p_rtu_Servo::_read_register(uint16_t addr) {
+    auto res = _read_registers(addr, 1);
+    if (res.empty()) throw_error("Empty response");
+    return res[0];
+}
+
+std::vector<uint16_t> Changingtek_p_rtu_Servo::_read_registers(uint16_t addr, int count) {
+    std::lock_guard<std::mutex> lock(_mutex);
+    if (hSerial == INVALID_SERIAL_HANDLE) throw_error("Not connected");
+
+    std::vector<uint8_t> frame;
+    frame.push_back(static_cast<uint8_t>(slave_id));
+    frame.push_back(0x03); // Read Holding Registers
+    frame.push_back(addr >> 8);
+    frame.push_back(addr & 0xFF);
+    frame.push_back(count >> 8);
+    frame.push_back(count & 0xFF);
+
+    uint16_t crc = calculate_crc(frame);
+    frame.push_back(crc & 0xFF);
+    frame.push_back(crc >> 8);
+
+    send_frame(frame);
+    
+    std::vector<uint8_t> response = receive_response();
+    
+    // 解析响应
+    // [SlaveID, FuncCode, ByteCount, Data..., CRC_L, CRC_H]
+    if (response.size() < static_cast<size_t>(3 + count * 2 + 2)) {
+        throw_error("Response data length mismatch");
+    }
+
+    if (response[1] != 0x03) {
+        throw_error("Response function code mismatch");
+    }
+
+    std::vector<uint16_t> values;
+    for (int i = 0; i < count; ++i) {
+        uint16_t val = (response[3 + i * 2] << 8) | response[3 + i * 2 + 1];
+        values.push_back(val);
+    }
+    return values;
+}
+
+// -----------------------------
+// 目标值/控制写入操作
+// -----------------------------
+
+void Changingtek_p_rtu_Servo::set_target_position(int position) {
+    uint16_t hi = (position >> 16) & 0xFFFF;
+    uint16_t lo = position & 0xFFFF;
+    std::vector<uint16_t> values = {hi, lo};
+    _write_registers(REG_TARGET_POS_HIGH, values);
+}
+
+void Changingtek_p_rtu_Servo::set_target_speed(int speed) {
+    _write_register(REG_TARGET_SPEED, static_cast<uint16_t>(speed));
+}
+
+void Changingtek_p_rtu_Servo::set_target_force(int force) {
+    _write_register(REG_TARGET_FORCE, static_cast<uint16_t>(force));
+}
+
+void Changingtek_p_rtu_Servo::set_target_acceleration(int acceleration) {
+    _write_register(REG_TARGET_ACCELERATION, static_cast<uint16_t>(acceleration));
+}
+
+void Changingtek_p_rtu_Servo::set_target_deceleration(int deceleration) {
+    _write_register(REG_TARGET_DECELERATION, static_cast<uint16_t>(deceleration));
+}
+
+void Changingtek_p_rtu_Servo::trigger_motion() {
+    _write_register(REG_MOTION_TRIGGER, 1);
+}
+
+void Changingtek_p_rtu_Servo::temp_move(int position_mm, int speed_pct, int force_pct, int accel, int decel, bool trigger) {
+    // 优化：一次性写入所有参数 (REG_TARGET_POS_HIGH ~ REG_MOTION_TRIGGER)
+    // 地址连续: 
+    // 0x0102: POS_H
+    // 0x0103: POS_L
+    // 0x0104: SPEED
+    // 0x0105: FORCE
+    // 0x0106: ACCEL
+    // 0x0107: DECEL
+    // 0x0108: TRIGGER
+    
+    std::vector<uint16_t> values;
+    values.push_back((position_mm >> 16) & 0xFFFF);
+    values.push_back(position_mm & 0xFFFF);
+    values.push_back(static_cast<uint16_t>(speed_pct));
+    values.push_back(static_cast<uint16_t>(force_pct));
+    values.push_back(static_cast<uint16_t>(accel));
+    values.push_back(static_cast<uint16_t>(decel));
+    
+    if (trigger) {
+        values.push_back(1);
+    } else {
+        // 如果不触发，是否需要写入0或者不写该寄存器？
+        // 为了保持连续写入，写入0 (空闲)
+        values.push_back(0);
+    }
+    
+    _write_registers(REG_TARGET_POS_HIGH, values);
+
+    if (trigger) {
+        std::cout << "已触发运动到位置 " << position_mm << "，速度 " << speed_pct << "%，力 " << force_pct << "%" << std::endl;
+    }
+}
+
+// -----------------------------
+// 反馈/状态读取操作
+// -----------------------------
+
+int Changingtek_p_rtu_Servo::read_real_position() {
+    // 连续读取高低位
+    std::vector<uint16_t> regs = _read_registers(REG_REAL_POS_HIGH, 2);
+    uint32_t combined = (regs[0] << 16) | regs[1];
+    
+    // 处理32位有符号整数
+    int32_t final_val;
+    if (combined & 0x80000000) {
+        final_val = static_cast<int32_t>(combined); // C++ 中 uint32 转 int32，如果是补码表示则直接转换即可
+        // 或者更安全的做法:
+        // final_val = -static_cast<int32_t>((~combined + 1)); 
+        // 但通常 combined 就是补码形式，直接 cast 即可。
+    } else {
+        final_val = static_cast<int32_t>(combined);
+    }
+    return final_val;
+}
+
+int Changingtek_p_rtu_Servo::read_real_speed() {
+    return static_cast<int16_t>(_read_register(REG_REAL_SPEED));
+}
+
+int Changingtek_p_rtu_Servo::read_real_current() {
+    return static_cast<int16_t>(_read_register(REG_REAL_CURRENT));
+}
+
+// -----------------------------
+// 实时监控
+// -----------------------------
+
+void Changingtek_p_rtu_Servo::_monitor_loop(double interval) {
+    while (_monitor_running) {
+        try {
+            int pos = read_real_position();
+            int speed = read_real_speed();
+            int current = read_real_current();
+            
+            // 使用 printf 格式化输出，或者 iomanip
+            // 为了防止输出混乱，可以加个简单的锁或者直接输出
+            // std::cout 是线程安全的(字符级)，但多个线程输出可能会交错
+            std::cout << "[实时数据] 位置: " << std::setw(6) << pos 
+                      << ", 速度: " << std::setw(3) << speed 
+                      << ", 电流: " << std::setw(3) << current << std::endl;
+        } catch (const std::exception& e) {
+            std::cout << "监控错误: " << e.what() << std::endl;
+        }
+        
+        // sleep
+        std::this_thread::sleep_for(std::chrono::milliseconds(static_cast<int>(interval * 1000)));
+    }
+}
+
+void Changingtek_p_rtu_Servo::start_monitoring(double interval) {
+    if (!_monitor_running) {
+        _monitor_running = true;
+        _monitor_thread = std::thread(&Changingtek_p_rtu_Servo::_monitor_loop, this, interval);
+        _monitor_thread.detach(); // 分离线程，类似 Python daemon=True
+        std::cout << "实时监控已启动。" << std::endl;
+    }
+}
+
+void Changingtek_p_rtu_Servo::stop_monitoring() {
+    if (_monitor_running) {
+        _monitor_running = false;
+        // 由于 detach 了，不能 join。
+        // 但我们可以等待一小段时间让循环退出
+        std::this_thread::sleep_for(std::chrono::milliseconds(200));
+        std::cout << "实时监控已停止。" << std::endl;
+    }
+}

sdk/changingtek_p_rtu_Servo_cpp_version/Changingtek_p_rtu_Servo.h

@@ -0,0 +1,189 @@
+#ifndef CHANGINGTEK_P_RTU_SERVO_H
+#define CHANGINGTEK_P_RTU_SERVO_H
+
+#include <string>
+#include <vector>
+#include <cstdint>
+#include <thread>
+#include <atomic>
+#include <mutex>
+
+#ifndef WIN32_LEAN_AND_MEAN
+#define WIN32_LEAN_AND_MEAN
+#endif
+
+#ifdef _WIN32
+#include <windows.h>
+typedef HANDLE SerialHandle;
+#define INVALID_SERIAL_HANDLE INVALID_HANDLE_VALUE
+#else
+#include <unistd.h>
+#include <fcntl.h>
+#include <termios.h>
+#include <errno.h>
+#include <sys/ioctl.h>
+#include <cstring>
+typedef int SerialHandle;
+#define INVALID_SERIAL_HANDLE -1
+#endif
+#include <iostream>
+
+// -----------------------------
+// 寄存器地址映射 (保持寄存器, 功能码 0x03/0x06/0x10)
+// -----------------------------
+
+// 写入寄存器 (目标值/控制)
+const uint16_t REG_TARGET_POS_HIGH       = 0x0102;  // 目标位置（高16位）
+const uint16_t REG_TARGET_POS_LOW        = 0x0103;  // 目标位置（低16位）
+const uint16_t REG_TARGET_SPEED          = 0x0104;  // 目标速度
+const uint16_t REG_TARGET_FORCE          = 0x0105;  // 目标力/力矩
+const uint16_t REG_TARGET_ACCELERATION   = 0x0106;  // 目标加速度
+const uint16_t REG_TARGET_DECELERATION   = 0x0107;  // 目标减速度
+const uint16_t REG_MOTION_TRIGGER        = 0x0108;  // 运动触发（0: 空闲, 1: 触发）
+
+// 读取寄存器 (反馈/状态)
+const uint16_t REG_REAL_POS_HIGH         = 0x0418;  // 实时位置（高16位）
+const uint16_t REG_REAL_POS_LOW          = 0x0419;  // 实时位置（低16位）
+const uint16_t REG_REAL_SPEED            = 0x041A;  // 实时速度反馈
+const uint16_t REG_REAL_CURRENT          = 0x041B;  // 实时电流反馈
+
+/**
+ * Changingtek_p_rtu_Servo 类
+ * 
+ * 封装了基于 Modbus RTU (RS-485) 的伺服步进控制器协议。
+ * 提供了连接、运动控制以及状态读取功能。
+ */
+class Changingtek_p_rtu_Servo {
+public:
+    /**
+     * 构造函数
+     * @param port 串口名称，例如 "COM3" 或 "/dev/ttyUSB0"
+     * @param slave_id Modbus 从站地址 (默认 1)
+     * @param baudrate 波特率 (默认 115200)
+     * @param timeout 读写超时时间 (秒, 默认 1.0)
+     */
+    Changingtek_p_rtu_Servo(const std::string& port, int slave_id = 1, int baudrate = 115200, double timeout = 1.0);
+    ~Changingtek_p_rtu_Servo();
+
+    /**
+     * 连接串口
+     * @return 成功返回 true，失败返回 false
+     */
+    bool connect();
+
+    /**
+     * 断开串口连接
+     */
+    void disconnect();
+
+    // ------------- 目标值/控制写入操作 -------------
+
+    /**
+     * 设置目标位置
+     * @param position 目标位置值
+     */
+    void set_target_position(int position);
+
+    /**
+     * 设置目标速度
+     * @param speed 目标速度值
+     */
+    void set_target_speed(int speed);
+
+    /**
+     * 设置目标力/力矩
+     * @param force 目标力/力矩值
+     */
+    void set_target_force(int force);
+
+    /**
+     * 设置目标加速度
+     * @param acceleration 目标加速度值
+     */
+    void set_target_acceleration(int acceleration);
+
+    /**
+     * 设置目标减速度
+     * @param deceleration 目标减速度值
+     */
+    void set_target_deceleration(int deceleration);
+
+    /**
+     * 触发运动
+     */
+    void trigger_motion();
+
+    /**
+     * 临时运动控制方法，一次性设置所有运动参数并可选触发运动
+     * 
+     * @param position_mm 目标位置(毫米/脉冲)
+     * @param speed_pct 速度百分比
+     * @param force_pct 力/力矩百分比
+     * @param accel 加速度值
+     * @param decel 减速度值
+     * @param trigger 是否立即触发运动，默认为 true
+     */
+    void temp_move(int position_mm, int speed_pct, int force_pct, int accel, int decel, bool trigger = true);
+
+    // ------------- 反馈/状态读取操作 -------------
+
+    /**
+     * 读取实时位置
+     * @return 组合后的32位实时位置
+     */
+    int read_real_position();
+
+    /**
+     * 读取实时速度反馈
+     * @return 实时速度值
+     */
+    int read_real_speed();
+
+    /**
+     * 读取实时电流反馈
+     * @return 实时电流值
+     */
+    int read_real_current();
+
+    // ------------- 实时监控 -------------
+
+    /**
+     * 启动实时数据监控线程
+     * @param interval 监控间隔（秒，默认: 0.5）
+     */
+    void start_monitoring(double interval = 0.5);
+
+    /**
+     * 停止实时数据监控线程
+     */
+    void stop_monitoring();
+
+private:
+    std::string port_name;
+    int slave_id;
+    int baudrate;
+    double timeout;
+    SerialHandle hSerial;
+
+    // 线程安全锁
+    std::mutex _mutex;
+
+    // 监控线程相关
+    std::atomic<bool> _monitor_running;
+    std::thread _monitor_thread;
+    void _monitor_loop(double interval);
+
+    // 内部 Modbus 辅助函数
+    void _write_register(uint16_t addr, uint16_t value);
+    void _write_registers(uint16_t addr, const std::vector<uint16_t>& values);
+    uint16_t _read_register(uint16_t addr);
+    std::vector<uint16_t> _read_registers(uint16_t addr, int count);
+    
+    // 底层通信函数
+    void send_frame(const std::vector<uint8_t>& frame);
+    std::vector<uint8_t> receive_response(int expected_min_bytes = 0);
+    uint16_t calculate_crc(const std::vector<uint8_t>& data);
+    void throw_error(const std::string& msg);
+};
+
+#endif // CHANGINGTEK_P_RTU_SERVO_H

sdk/changingtek_p_rtu_Servo_cpp_version/Makefile

@@ -0,0 +1,18 @@
+CXX = g++
+CXXFLAGS = -std=c++11 -Wall -pthread
+TARGET = example_servo
+SRCS = main.cpp Changingtek_p_rtu_Servo.cpp
+OBJS = $(SRCS:.cpp=.o)
+
+all: $(TARGET)
+
+$(TARGET): $(OBJS)
+	$(CXX) $(CXXFLAGS) -o $@ $^
+
+%.o: %.cpp
+	$(CXX) $(CXXFLAGS) -c $< -o $@
+
+clean:
+	rm -f $(OBJS) $(TARGET)
+
+.PHONY: all clean

sdk/changingtek_p_rtu_Servo_cpp_version/README.md

@@ -0,0 +1,71 @@
+# Changingtek 伺服步进控制器 C++ SDK
+
+基于 Modbus RTU 协议的伺服步进电机控制 SDK (C++ 版本)。
+本 SDK 采用跨平台设计，一套代码可同时支持 **Windows** 和 **Linux (x86/ARM)** 系统。
+
+## 目录结构
+
+- `Changingtek_p_rtu_Servo.h`: SDK 头文件 (核心接口)
+- `Changingtek_p_rtu_Servo.cpp`: SDK 实现文件 (Modbus RTU 通信逻辑)
+- `main.cpp`: 主程序 (包含 5 次循环控制演示)
+- `compile_and_run.bat`: Windows 编译运行脚本 (MSVC)
+- `Makefile`: Linux 编译脚本
+
+## 编译与运行指南
+
+### 1. Windows 环境
+
+**前提条件**: 安装 Visual Studio (支持 C++ 开发)。
+
+**操作步骤**:
+1. 确保 USB 转串口模块已插入，并确认端口号 (例如 `COM3`)。
+2. 如需修改端口，编辑 `main.cpp` 中的 `port` 变量。
+3. 双击运行 `compile_and_run.bat`，或在命令行中执行：
+   ```cmd
+   compile_and_run.bat
+   ```
+
+---
+
+### 2. Linux 环境 (Ubuntu / ARM 架构)
+
+**前提条件**: 系统已安装 `g++` 和 `make` 工具。
+
+**操作步骤**:
+
+1. **编译**:
+   打开终端进入项目目录，执行 `make` 命令：
+   ```bash
+   cd /path/to/changingtek_p_rtu_Servo_cpp_version
+   make
+   ```
+   编译成功后会生成可执行文件 `example_servo`。
+
+2. **配置权限与端口**:
+   *   确认设备端口：通常为 `/dev/ttyUSB0` (USB转串口) 或 `/dev/ttyTHS1` (嵌入式板载串口)。
+   *   如果端口不是默认的 `/dev/ttyUSB0`，请修改 `main.cpp` 第 20 行：
+       ```cpp
+       port = "/dev/ttyTHS1"; // 示例：修改为实际端口
+       ```
+       修改后需要重新运行 `make`。
+   *   授予串口读写权限 (如遇到 Permission denied 错误)：
+       ```bash
+       sudo chmod 666 /dev/ttyUSB0
+       ```
+
+3. **运行**:
+   ```bash
+   ./example_servo
+   ```
+
+4. **清理**:
+   删除编译生成的文件：
+   ```bash
+   make clean
+   ```
+
+## 功能特性
+
+- **跨平台兼容**: 自动识别操作系统 (`_WIN32` 宏)，底层分别调用 Windows API 和 Linux 系统调用 (termios)，无需修改代码即可移植。
+- **通信优化**: 针对伺服控制优化，使用 Modbus 0x10 功能码实现多寄存器同步写入 (位置/速度/力矩/加减速)，显著降低通信延迟。
+- **线程安全**: 内部集成互斥锁 (`std::mutex`)，支持在主线程控制运动的同时，后台线程实时读取电机状态。

sdk/changingtek_p_rtu_Servo_cpp_version/compile_and_run.bat

@@ -0,0 +1,6 @@
+@echo off
+call "C:\Program Files\Microsoft Visual Studio\18\Community\VC\Auxiliary\Build\vcvars64.bat"
+cl /utf-8 /EHsc main.cpp Changingtek_p_rtu_Servo.cpp /Fe:example_servo.exe
+if %errorlevel% neq 0 exit /b %errorlevel%
+del *.obj
+example_servo.exe

sdk/changingtek_p_rtu_Servo_cpp_version/main.cpp

@@ -0,0 +1,71 @@
+#include "Changingtek_p_rtu_Servo.h"
+#include <iostream>
+#include <thread>
+#include <chrono>
+
+#ifdef _WIN32
+#include <windows.h>
+#endif
+
+int main() {
+#ifdef _WIN32
+    // 设置控制台代码页为 UTF-8，解决中文乱码问题
+    SetConsoleOutputCP(65001);
+#endif
+
+    std::string port;
+#ifdef _WIN32
+    port = "COM3"; // Windows 默认端口
+#else
+    port = "/dev/ttyUSB0"; // Linux 默认端口
+#endif
+    
+    int slave_id = 1;
+
+    std::cout << "正在初始化伺服步进控制器 SDK (端口: " << port << ", ID: " << slave_id << ")..." << std::endl;
+
+    try {
+        Changingtek_p_rtu_Servo sdk(port, slave_id);
+
+        if (!sdk.connect()) {
+            std::cerr << "连接失败！" << std::endl;
+            return 1;
+        }
+        std::cout << "连接成功！" << std::endl;
+
+        // 启动实时监控
+        sdk.start_monitoring(0.5);
+
+        // 循环控制：5次往复运动
+        // 假设 9000 为闭合位置，0 为张开位置
+        // 最后一次循环结束后，确保处于张开位置 (0)
+        for (int i = 0; i < 5; ++i) {
+            std::cout << "\n--- 第 " << (i + 1) << "/5 次循环 ---" << std::endl;
+
+            // 阶段1: 运动到位置 9000 (闭合?)
+            std::cout << "执行阶段 1: 移动到位置 9000..." << std::endl;
+            sdk.temp_move(9000, 50, 25, 60, 60, true);
+            std::this_thread::sleep_for(std::chrono::seconds(3)); // 等待运动完成
+
+            // 阶段2: 运动到位置 0 (张开?)
+            std::cout << "执行阶段 2: 移动到位置 0..." << std::endl;
+            sdk.temp_move(0, 50, 25, 60, 60, true);
+            std::this_thread::sleep_for(std::chrono::seconds(3)); // 等待运动完成
+        }
+
+        std::cout << "\n循环结束，确认最终状态为张开 (位置 0)..." << std::endl;
+        // 再次发送指令确保最终状态为 0 (虽然循环最后一步已经是 0，但这保证了"最后要张开"的要求)
+        sdk.temp_move(0, 50, 25, 60, 60, true);
+        std::this_thread::sleep_for(std::chrono::seconds(1));
+
+        // 停止监控
+        sdk.stop_monitoring();
+        std::cout << "程序已完成。" << std::endl;
+
+    } catch (const std::exception& e) {
+        std::cerr << "发生异常: " << e.what() << std::endl;
+        return 1;
+    }
+
+    return 0;
+}

sdk/test_gripper_comm.py

@@ -0,0 +1,11 @@
+from changingtek_p_rtu_Servo import MotorController
+
+
+PORT = "COM3"
+SLAVE_ID = 1
+
+
+if __name__ == "__main__":
+    sdk = MotorController(PORT, SLAVE_ID, baudrate=115200, timeout=1.0)
+    print("Reading gripper position...")
+    print("position:", sdk.read_real_position())