更新：整机逻辑、ID指示器驱动、动态Modbus地址、指令文档

CMakeLists.txt

@@ -26,6 +26,7 @@ include_directories(
     ${CMAKE_SOURCE_DIR}/bsp/device/eeprom
     ${CMAKE_SOURCE_DIR}/bsp/device/limit_switch
     ${CMAKE_SOURCE_DIR}/bsp/device/as5600
+    ${CMAKE_SOURCE_DIR}/bsp/device/id
     ${CMAKE_SOURCE_DIR}/bsp/modbus
     ${CMAKE_SOURCE_DIR}/drv/system
     ${CMAKE_SOURCE_DIR}/drv/timer

README.md

@@ -0,0 +1,155 @@
+# StepMotorCtrl - 步进电机控制系统
+
+基于 **PY32MD530H28U7TR** 单片机的步进电机控制系统，适用于 claw machine（抓娃娃机）等场景，提供精准的电机位置控制、角度控制和标定功能。
+
+## 硬件平台
+
+| 项目 | 说明 |
+|------|------|
+| 主控芯片 | PY32MD530H28U7TR (Cortex-M0+) |
+| 电机驱动 | TIM1 高分辨率 PWM，正弦/余弦微步进控制 |
+| 角度传感器 | AS5600 (I2C1, 地址 0x36) |
+| 存储器 | AT24C02 (I2C1, 地址 0x50) |
+| 电流检测 | PGA + 内部 ADC |
+| 通讯接口 | RS485 (USART1, Modbus RTU)、UART1 (调试)、LPUART1 (备份) |
+| 到位开关 | DI_MIN (原点)、DI_MAX (最大值) |
+| ID 指示器 | 4 位 GPIO (PB5~PB8)，支持 0~15 共 16 个地址 |
+
+### 引脚分配
+
+详见 [PinConfig.md](.trae/skills/create-project/references/PinConfig.md)
+
+## 软件架构
+
+```
+StepMotorCtrl/
+├── app/                    # 应用层
+│   ├── main.c              # 主程序入口
+│   ├── stepper/            # 步进电机控制
+│   └── tasks/              # 任务模块（work_task, monitor_task）
+├── bsp/                    # 板级支持包
+│   ├── device/             # 外设驱动
+│   │   ├── as5600/         # 角度传感器
+│   │   ├── eeprom/         # 存储器
+│   │   ├── id/             # ID 指示器
+│   │   ├── key/            # 按键
+│   │   ├── led/            # LED
+│   │   └── limit_switch/   # 到位开关
+│   ├── modbus/             # Modbus RTU 从站
+│   └── hardware_init.c/h   # 硬件初始化
+├── config/                 # 配置文件
+│   └── config.h            # 引脚定义与参数配置
+├── drv/                    # 驱动层
+│   ├── adc/                # ADC + PGA
+│   ├── gpio/               # GPIO
+│   ├── i2c/                # I2C
+│   ├── log/                # 日志
+│   ├── system/             # 系统时钟 + 数据总线
+│   ├── timer/              # 定时器 PWM
+│   └── uart/               # UART
+└── libs/                   # MCU 官方库
+    ├── CMSIS/              # Cortex-M 内核头文件
+    └── device/             # PY32MD530 设备文件 + 链接脚本
+```
+
+## 构建
+
+### 环境要求
+
+- **编译器**: arm-none-eabi-gcc (含 newlib-nano)
+- **构建工具**: CMake + MinGW Make (Windows) / make (Linux)
+- **烧录工具**: 建议使用 PyOCD 或 J-Link
+
+### 构建步骤
+
+```bash
+mkdir build && cd build
+cmake .. -G "MinGW Makefiles"
+mingw32-make -j4
+```
+
+生成文件：
+- `StepMotorCtrl.elf` — ELF 可执行文件
+- `StepMotorCtrl.hex` — HEX 烧录文件
+- `StepMotorCtrl.bin` — BIN 烧录文件
+
+## Modbus 通讯协议
+
+| 参数 | 值 |
+|------|-----|
+| 物理层 | RS485 半双工 |
+| 波特率 | 115200, 8N1 |
+| 协议 | Modbus RTU |
+| 主站地址 | 0xFE |
+| 从站地址 | 0x40 + ID (ID 由 PB5~PB8 引脚读取) |
+
+### 主要寄存器
+
+| 地址 | 名称 | 说明 |
+|------|------|------|
+| 0x0001 | MOTOR_POSITION | 当前位置 (步数) |
+| 0x0002 | MOTOR_TARGET | 目标位置 (步数) |
+| 0x0005 | AS5600_ANGLE | 角度值 (0.1°, 0~3599) |
+| 0x000F | DEVICE_ID | 设备 ID (0~15) |
+| 0x0042 | CMD | 指令寄存器 |
+| 0x0043 | STATUS | 整机状态 |
+| 0x0044 | ANGLE_TARGET | 角度目标 (0.1°, 0~3600) |
+
+### 指令码
+
+| 码值 | 指令 | 说明 |
+|------|------|------|
+| 0x01 | GO_MAX | 运行到最大值 |
+| 0x02 | GO_MIN | 运行到最小值 (原点) |
+| 0x03 | STOP | 停止运行 |
+| 0x04 | GO_POSITION | 运行到指定步数 |
+| 0x05 | GO_ANGLE | 运行到指定角度 |
+| 0x06 | CALIBRATE | 开始标定 |
+| 0x07 | CALIB_END | 结束标定 |
+
+### 通讯示例
+
+```hex
+# 读当前位置
+请求:  FE 03 00 01 00 01 CRC_LO CRC_HI
+响应:  40+ID 03 02 POS_HI POS_LO CRC_LO CRC_HI
+
+# 运行到最大值
+请求:  FE 06 00 42 00 01 CRC_LO CRC_HI
+响应:  40+ID 06 00 42 00 01 CRC_LO CRC_HI
+
+# 运行到指定角度 (45.0°)
+请求:  FE 06 00 44 01 C2 CRC_LO CRC_HI         <- ANGLE_TARGET = 450
+请求:  FE 06 00 42 00 05 CRC_LO CRC_HI         <- CMD = GO_ANGLE
+```
+
+详见 [command.md](.trae/skills/create-project/references/command.md)
+
+## 功能说明
+
+### 步进电机控制
+- S 型加减速算法，支持加速、匀速、减速三个阶段
+- 正弦/余弦微步进驱动，每电周期 1024 个微步
+- 支持绝对位置移动和相对位置移动
+- 到位自动停止，支持紧急停止
+
+### 标定功能
+- 自动标定：运行到 DI_MIN 和 DI_MAX 开关位置，记录步数
+- 无 DI_MAX 时：根据原点开关和标定步数计算导轨长度
+
+### 整机逻辑
+- 上电初始化后进入待机模式，等待 Modbus 指令
+- 支持指令队列和状态上报
+- LED 指示灯反馈：待机 (500ms 闪烁)、运行 (100ms 闪烁)、标定 (80ms 闪烁)
+
+## 功能文档
+
+| 文档 | 说明 |
+|------|------|
+| [function.md](.trae/skills/create-project/references/function.md) | 功能规格与运行逻辑 |
+| [command.md](.trae/skills/create-project/references/command.md) | Modbus 指令集与通讯协议 |
+| [PinConfig.md](.trae/skills/create-project/references/PinConfig.md) | 引脚配置表 |
+
+## License
+
+Proprietary

app/main.c

@@ -20,6 +20,7 @@
 #endif
 #include "as5600_driver.h"
 #include "modbus_slave.h"
+#include "id_driver.h"
 #include "stepper_control.h"
 #include "tasks/work_task.h"
 #include "tasks/monitor_task.h"
@@ -188,7 +189,17 @@ int main(void)
 
     as5600_init();
 
-    modbus_slave_init(MODBUS_SLAVE_ADDR);
+    id_driver_init();
+    {
+        sys_data_bus_value_t id_val;
+
+        id_val.u = id_get_value();
+        sys_data_bus_write(SYS_DATA_BUS_DEVICE_ID, id_val);
+        id_val.u = id_get_slave_address();
+        sys_data_bus_write(SYS_DATA_BUS_MODBUS_SLAVE_ADDR, id_val);
+    }
+
+    modbus_slave_init(id_get_slave_address());
 
     stepper_init();
 

app/stepper/stepper_control.c

@@ -560,3 +560,45 @@ stepper_state_t stepper_get_state(void)
 {
     return state;
 }
+
+ret_code_t stepper_move_to_min(void)
+{
+    return stepper_move_to(origin_steps);
+}
+
+ret_code_t stepper_move_to_max(void)
+{
+    return stepper_move_to(max_steps);
+}
+
+ret_code_t stepper_move_to_angle(float angle_deg)
+{
+    int32_t steps;
+
+    if (angle_deg < 0.0f) angle_deg = 0.0f;
+    if (angle_deg > 360.0f) angle_deg = 360.0f;
+
+    steps = (int32_t)((calibrated_steps * angle_deg) / 360.0f);
+
+    return stepper_move_to(origin_steps + steps);
+}
+
+uint8_t stepper_is_running(void)
+{
+    return running;
+}
+
+int32_t stepper_get_max_steps(void)
+{
+    return max_steps;
+}
+
+int32_t stepper_get_origin_steps(void)
+{
+    return origin_steps;
+}
+
+uint8_t stepper_is_calibrated(void)
+{
+    return calibrated;
+}

app/stepper/stepper_control.h

@@ -13,6 +13,22 @@
 #define STEPPER_PWM_CENTER       1800
 #define STEPPER_HOLD_CURRENT     50
 
+#define STEPPER_CMD_NONE         0x0000
+#define STEPPER_CMD_GO_MAX       0x0001
+#define STEPPER_CMD_GO_MIN       0x0002
+#define STEPPER_CMD_STOP         0x0003
+#define STEPPER_CMD_GO_POSITION  0x0004
+#define STEPPER_CMD_GO_ANGLE     0x0005
+#define STEPPER_CMD_CALIBRATE    0x0006
+#define STEPPER_CMD_CALIB_END    0x0007
+#define STEPPER_CMD_READ_STATUS  0x0008
+
+#define STEPPER_STATUS_IDLE      0x0000
+#define STEPPER_STATUS_RUNNING   0x0001
+#define STEPPER_STATUS_CALIB     0x0002
+#define STEPPER_STATUS_STOPPED   0x0003
+#define STEPPER_STATUS_ERROR     0x0004
+
 typedef enum {
     STEPPER_STATE_IDLE = 0,
     STEPPER_STATE_ACCEL,
@@ -31,13 +47,20 @@ ret_code_t stepper_init(void);
 ret_code_t stepper_deinit(void);
 ret_code_t stepper_move_to(int32_t target);
 ret_code_t stepper_move_rel(int32_t steps);
+ret_code_t stepper_move_to_min(void);
+ret_code_t stepper_move_to_max(void);
+ret_code_t stepper_move_to_angle(float angle_deg);
 ret_code_t stepper_stop(void);
 ret_code_t stepper_emergency_stop(void);
 ret_code_t stepper_calibrate(void);
+uint8_t    stepper_is_running(void);
 void       stepper_run(void);
 
 int32_t    stepper_get_position(void);
 int32_t    stepper_get_target(void);
+int32_t    stepper_get_max_steps(void);
+int32_t    stepper_get_origin_steps(void);
+uint8_t    stepper_is_calibrated(void);
 stepper_state_t stepper_get_state(void);
 
 #endif

app/tasks/work_task.c

@@ -1,23 +1,223 @@
 #include "work_task.h"
 #include "system_driver.h"
+#include "sys_data_bus.h"
+#include "stepper_control.h"
+#include "limit_switch.h"
 #include "led_driver.h"
 #include "config.h"
 
+#define DI_MIN  LIMIT_SW_MIN
+#define DI_MAX  LIMIT_SW_MAX
+
+#define WORK_TASK_LED_IDLE_MS       500
+#define WORK_TASK_LED_RUN_MS        100
+#define WORK_TASK_LED_CALIB_MS      80
+#define WORK_TASK_LED_STOPPED_MS    0
+
+typedef enum {
+    MACHINE_STANDBY = 0,
+    MACHINE_MOVING,
+    MACHINE_CALIBRATING,
+    MACHINE_STOPPED,
+} machine_mode_t;
+
 static uint32_t last_run_tick;
 static uint32_t run_count;
+static machine_mode_t machine_mode;
+static uint16_t last_cmd;
+static uint32_t led_toggle_tick;
+static uint8_t led_state_on;
+
+static void work_update_data_bus(void)
+{
+    sys_data_bus_value_t val;
+
+    val.i = stepper_get_position();
+    sys_data_bus_write(SYS_DATA_BUS_MOTOR_POSITION, val);
+
+    val.i = stepper_get_target();
+    sys_data_bus_write(SYS_DATA_BUS_MOTOR_TARGET, val);
+
+    val.u = (uint32_t)stepper_get_state();
+    sys_data_bus_write(SYS_DATA_BUS_MOTOR_STATE, val);
+
+    val.u = limit_switch_get_state(DI_MIN) == LIMIT_SW_STATE_TRIGGERED ? 1 : 0;
+    sys_data_bus_write(SYS_DATA_BUS_DI_MIN_STATE, val);
+
+    val.u = limit_switch_get_state(DI_MAX) == LIMIT_SW_STATE_TRIGGERED ? 1 : 0;
+    sys_data_bus_write(SYS_DATA_BUS_DI_MAX_STATE, val);
+}
+
+static void work_update_status(void)
+{
+    sys_data_bus_value_t val;
+
+    switch (machine_mode) {
+        case MACHINE_STANDBY:
+            val.u = STEPPER_STATUS_IDLE;
+            break;
+        case MACHINE_MOVING:
+            val.u = STEPPER_STATUS_RUNNING;
+            break;
+        case MACHINE_CALIBRATING:
+            val.u = STEPPER_STATUS_CALIB;
+            break;
+        case MACHINE_STOPPED:
+            val.u = STEPPER_STATUS_STOPPED;
+            break;
+        default:
+            val.u = STEPPER_STATUS_IDLE;
+            break;
+    }
+
+    sys_data_bus_write(SYS_DATA_BUS_MOTOR_STATUS, val);
+}
+
+static uint16_t work_read_command(void)
+{
+    sys_data_bus_value_t val;
+
+    val = sys_data_bus_read(SYS_DATA_BUS_MOTOR_CMD);
+
+    return (uint16_t)(val.u & 0xFFFF);
+}
+
+static void work_clear_command(void)
+{
+    sys_data_bus_value_t val;
+
+    val.u = STEPPER_CMD_NONE;
+    sys_data_bus_write(SYS_DATA_BUS_MOTOR_CMD, val);
+}
+
+static void work_dispatch_command(uint16_t cmd)
+{
+    switch (cmd) {
+        case STEPPER_CMD_GO_MAX:
+            machine_mode = MACHINE_MOVING;
+            stepper_move_to_max();
+            break;
+
+        case STEPPER_CMD_GO_MIN:
+            machine_mode = MACHINE_MOVING;
+            stepper_move_to_min();
+            break;
+
+        case STEPPER_CMD_STOP:
+            machine_mode = MACHINE_STOPPED;
+            stepper_stop();
+            break;
+
+        case STEPPER_CMD_GO_POSITION:
+        {
+            sys_data_bus_value_t target;
+
+            target = sys_data_bus_read(SYS_DATA_BUS_MOTOR_TARGET);
+            machine_mode = MACHINE_MOVING;
+            stepper_move_to(target.i);
+            break;
+        }
+
+        case STEPPER_CMD_GO_ANGLE:
+        {
+            sys_data_bus_value_t angle_reg;
+
+            angle_reg = sys_data_bus_read(SYS_DATA_BUS_ANGLE_TARGET);
+            machine_mode = MACHINE_MOVING;
+            stepper_move_to_angle((float)angle_reg.u * 0.1f);
+            break;
+        }
+
+        case STEPPER_CMD_CALIBRATE:
+            machine_mode = MACHINE_CALIBRATING;
+            stepper_calibrate();
+            break;
+
+        case STEPPER_CMD_CALIB_END:
+            if (machine_mode == MACHINE_CALIBRATING) {
+                stepper_stop();
+            }
+            machine_mode = MACHINE_STOPPED;
+            break;
+
+        case STEPPER_CMD_READ_STATUS:
+            break;
+
+        default:
+            break;
+    }
+}
+
+static void work_led_update(void)
+{
+    uint32_t now;
+    uint32_t interval;
+
+    now = system_get_tick();
+
+    switch (machine_mode) {
+        case MACHINE_STANDBY:
+            interval = WORK_TASK_LED_IDLE_MS;
+            break;
+        case MACHINE_MOVING:
+            interval = WORK_TASK_LED_RUN_MS;
+            break;
+        case MACHINE_CALIBRATING:
+            interval = WORK_TASK_LED_CALIB_MS;
+            break;
+        case MACHINE_STOPPED:
+            led_set(LED_ON);
+            return;
+        default:
+            interval = WORK_TASK_LED_IDLE_MS;
+            break;
+    }
+
+    if ((now - led_toggle_tick) >= interval) {
+        led_toggle_tick = now;
+        led_state_on = !led_state_on;
+        led_set(led_state_on ? LED_ON : LED_OFF);
+    }
+}
 
 void work_task_init(void)
 {
     last_run_tick = 0;
     run_count = 0;
+    machine_mode = MACHINE_STANDBY;
+    last_cmd = STEPPER_CMD_NONE;
+    led_toggle_tick = 0;
+    led_state_on = 0;
+
+    led_init();
 }
 
 void work_task_run(void)
 {
+    uint16_t cmd;
+
     last_run_tick = system_get_tick();
     run_count++;
 
-    led_blink(500);
+    work_update_data_bus();
+    work_update_status();
+    work_led_update();
+
+    cmd = work_read_command();
+
+    if (cmd != STEPPER_CMD_NONE && cmd != last_cmd) {
+        last_cmd = cmd;
+        work_clear_command();
+        work_dispatch_command(cmd);
+    }
+
+    if (machine_mode == MACHINE_MOVING && !stepper_is_running()) {
+        machine_mode = MACHINE_STANDBY;
+    }
+
+    if (machine_mode == MACHINE_CALIBRATING && !stepper_is_running()) {
+        machine_mode = MACHINE_STANDBY;
+    }
 }
 
 uint32_t work_task_get_last_run_tick(void)

bsp/device/id/id_driver.c

@@ -0,0 +1,34 @@
+#include "id_driver.h"
+#include "gpio_driver.h"
+#include "config.h"
+
+static uint8_t current_id;
+
+static uint8_t id_read_pins(void)
+{
+    uint8_t val = 0;
+
+    if (gpio_get_level(ID_PORT, ID_PIN_1)) val |= 0x01;
+    if (gpio_get_level(ID_PORT, ID_PIN_2)) val |= 0x02;
+    if (gpio_get_level(ID_PORT, ID_PIN_3)) val |= 0x04;
+    if (gpio_get_level(ID_PORT, ID_PIN_4)) val |= 0x08;
+
+    return val;
+}
+
+ret_code_t id_driver_init(void)
+{
+    current_id = id_read_pins();
+
+    return RET_OK;
+}
+
+uint8_t id_get_value(void)
+{
+    return current_id;
+}
+
+uint8_t id_get_slave_address(void)
+{
+    return ID_BASE_ADDR + current_id;
+}

bsp/device/id/id_driver.h

@@ -0,0 +1,14 @@
+#ifndef ID_DRIVER_H__
+#define ID_DRIVER_H__
+
+#include "common_types.h"
+#include <stdint.h>
+
+#define ID_PIN_COUNT            4
+#define ID_BASE_ADDR            0x40
+
+ret_code_t id_driver_init(void);
+uint8_t    id_get_value(void);
+uint8_t    id_get_slave_address(void);
+
+#endif

bsp/modbus/modbus_slave.c

@@ -5,40 +5,48 @@
 #include "config.h"
 #include <string.h>
 
-#define MODBUS_RX_TIMEOUT_MS       5
+#define MODBUS_RX_TIMEOUT_MS 5
-#define MODBUS_TX_GUARD_US         2
+#define MODBUS_TX_GUARD_US 2
 
-#define MODBUS_EXCEPTION_ILLEGAL_FUNCTION    0x01
+#define MODBUS_EXCEPTION_ILLEGAL_FUNCTION 0x01
-#define MODBUS_EXCEPTION_ILLEGAL_DATA_ADDR   0x02
+#define MODBUS_EXCEPTION_ILLEGAL_DATA_ADDR 0x02
-#define MODBUS_EXCEPTION_ILLEGAL_DATA_VAL    0x03
+#define MODBUS_EXCEPTION_ILLEGAL_DATA_VAL 0x03
 
-#define MODBUS_ADDR_BROADCAST      0x00
+#define MODBUS_ADDR_BROADCAST 0x00
 
-#define REG_BUS_DATA_START         0x0000
+#define REG_BUS_DATA_START 0x0000
-#define REG_BUS_DATA_END           0x001F
+#define REG_BUS_DATA_END 0x001F
-#define REG_BUS_DATA_HIGH_START    0x0020
+#define REG_BUS_DATA_HIGH_START 0x0020
-#define REG_BUS_DATA_HIGH_END      0x003F
+#define REG_BUS_DATA_HIGH_END 0x003F
-#define REG_FLAG_LOW               0x0040
+#define REG_FLAG_LOW 0x0040
-#define REG_FLAG_HIGH              0x0041
+#define REG_FLAG_HIGH 0x0041
+#define REG_CMD 0x0042
+#define REG_STATUS 0x0043
+#define REG_ANGLE_TARGET 0x0044
 
-static uint8_t      slave_address;
+static uint8_t slave_address;
 static modbus_state_t state;
-static uint8_t      rx_buf[MODBUS_BUF_SIZE];
+static uint8_t rx_buf[MODBUS_BUF_SIZE];
-static uint16_t     rx_len;
+static uint16_t rx_len;
-static uint32_t     rx_tick_start;
+static uint32_t rx_tick_start;
-static uint8_t      initialized;
+static uint8_t initialized;
 
 static uint16_t modbus_crc16_calc(const uint8_t *data, uint16_t len)
 {
     uint16_t crc = 0xFFFF;
     uint16_t i, j;
 
-    for (i = 0; i < len; i++) {
+    for (i = 0; i < len; i++)
+    {
         crc ^= data[i];
-        for (j = 0; j < 8; j++) {
+        for (j = 0; j < 8; j++)
-            if (crc & 0x0001) {
+        {
+            if (crc & 0x0001)
+            {
                 crc = (crc >> 1) ^ 0xA001;
-            } else {
+            }
+            else
+            {
                 crc = crc >> 1;
             }
         }
@@ -81,36 +89,61 @@ static void modbus_handle_read_holding(uint16_t reg_addr, uint16_t reg_count)
     sys_data_bus_value_t bus_val;
     uint32_t flags;
 
-    if (reg_count < 1 || reg_count > 125) {
+    if (reg_count < 1 || reg_count > 125)
+    {
         modbus_send_exception(MODBUS_FUNC_READ_HOLDING, MODBUS_EXCEPTION_ILLEGAL_DATA_VAL);
         return;
     }
 
     if ((reg_addr >= REG_BUS_DATA_START && (reg_addr + reg_count - 1) <= REG_BUS_DATA_END) ||
         (reg_addr >= REG_BUS_DATA_HIGH_START && (reg_addr + reg_count - 1) <= REG_BUS_DATA_HIGH_END) ||
-        (reg_addr >= REG_FLAG_LOW && (reg_addr + reg_count - 1) <= REG_FLAG_HIGH)) {
+        (reg_addr >= REG_FLAG_LOW && (reg_addr + reg_count - 1) <= REG_ANGLE_TARGET))
+    {
 
         resp[0] = slave_address;
         resp[1] = MODBUS_FUNC_READ_HOLDING;
         resp[2] = (uint8_t)(reg_count * 2);
 
-        for (i = 0; i < reg_count; i++) {
+        for (i = 0; i < reg_count; i++)
+        {
             uint16_t addr = reg_addr + i;
             uint16_t reg_val = 0;
 
-            if (addr <= REG_BUS_DATA_END) {
+            if (addr <= REG_BUS_DATA_END)
+            {
                 bus_val = sys_data_bus_read((sys_data_bus_id_t)addr);
                 reg_val = (uint16_t)(bus_val.u & 0xFFFF);
-            } else if (addr >= REG_BUS_DATA_HIGH_START && addr <= REG_BUS_DATA_HIGH_END) {
+            }
+            else if (addr >= REG_BUS_DATA_HIGH_START && addr <= REG_BUS_DATA_HIGH_END)
+            {
                 bus_val = sys_data_bus_read((sys_data_bus_id_t)(addr - REG_BUS_DATA_HIGH_START));
                 reg_val = (uint16_t)((bus_val.u >> 16) & 0xFFFF);
-            } else if (addr == REG_FLAG_LOW) {
+            }
+            else if (addr == REG_FLAG_LOW)
+            {
                 flags = 0;
                 reg_val = (uint16_t)(flags & 0xFFFF);
-            } else if (addr == REG_FLAG_HIGH) {
+            }
+            else if (addr == REG_FLAG_HIGH)
+            {
                 flags = 0;
                 reg_val = (uint16_t)((flags >> 16) & 0xFFFF);
             }
+            else if (addr == REG_CMD)
+            {
+                bus_val = sys_data_bus_read(SYS_DATA_BUS_MOTOR_CMD);
+                reg_val = (uint16_t)(bus_val.u & 0xFFFF);
+            }
+            else if (addr == REG_STATUS)
+            {
+                bus_val = sys_data_bus_read(SYS_DATA_BUS_MOTOR_STATUS);
+                reg_val = (uint16_t)(bus_val.u & 0xFFFF);
+            }
+            else if (addr == REG_ANGLE_TARGET)
+            {
+                bus_val = sys_data_bus_read(SYS_DATA_BUS_ANGLE_TARGET);
+                reg_val = (uint16_t)(bus_val.u & 0xFFFF);
+            }
 
             resp[3 + i * 2] = (uint8_t)((reg_val >> 8) & 0xFF);
             resp[4 + i * 2] = (uint8_t)(reg_val & 0xFF);
@@ -122,7 +155,9 @@ static void modbus_handle_read_holding(uint16_t reg_addr, uint16_t reg_count)
         resp[resp_len + 1] = (uint8_t)((crc >> 8) & 0xFF);
 
         modbus_send_response(resp, resp_len + 2);
-    } else {
+    }
+    else
+    {
         modbus_send_exception(MODBUS_FUNC_READ_HOLDING, MODBUS_EXCEPTION_ILLEGAL_DATA_ADDR);
     }
 }
@@ -132,14 +167,17 @@ static void modbus_handle_write_single_reg(uint16_t reg_addr, uint16_t reg_value
     uint8_t resp[8];
     uint16_t crc;
 
-    if (reg_addr <= REG_BUS_DATA_END) {
+    if (reg_addr <= REG_BUS_DATA_END)
+    {
         sys_data_bus_value_t val;
         sys_data_bus_value_t current;
 
         current = sys_data_bus_read((sys_data_bus_id_t)reg_addr);
         val.u = (current.u & 0xFFFF0000) | reg_value;
         sys_data_bus_write((sys_data_bus_id_t)reg_addr, val);
-    } else if (reg_addr >= REG_BUS_DATA_HIGH_START && reg_addr <= REG_BUS_DATA_HIGH_END) {
+    }
+    else if (reg_addr >= REG_BUS_DATA_HIGH_START && reg_addr <= REG_BUS_DATA_HIGH_END)
+    {
         sys_data_bus_value_t val;
         sys_data_bus_value_t current;
         uint8_t idx = (uint8_t)(reg_addr - REG_BUS_DATA_HIGH_START);
@@ -147,7 +185,21 @@ static void modbus_handle_write_single_reg(uint16_t reg_addr, uint16_t reg_value
         current = sys_data_bus_read((sys_data_bus_id_t)idx);
         val.u = (current.u & 0x0000FFFF) | ((uint32_t)reg_value << 16);
         sys_data_bus_write((sys_data_bus_id_t)idx, val);
-    } else {
+    }
+    else if (reg_addr == REG_CMD)
+    {
+        sys_data_bus_value_t val;
+        val.u = reg_value;
+        sys_data_bus_write(SYS_DATA_BUS_MOTOR_CMD, val);
+    }
+    else if (reg_addr == REG_ANGLE_TARGET)
+    {
+        sys_data_bus_value_t val;
+        val.u = reg_value;
+        sys_data_bus_write(SYS_DATA_BUS_ANGLE_TARGET, val);
+    }
+    else
+    {
         modbus_send_exception(MODBUS_FUNC_WRITE_SINGLE_REG, MODBUS_EXCEPTION_ILLEGAL_DATA_ADDR);
         return;
     }
@@ -170,12 +222,14 @@ static void modbus_process_frame(void)
 {
     uint16_t crc_recv, crc_calc;
 
-    if (rx_len < 4) {
+    if (rx_len < 4)
+    {
         state = MODBUS_IDLE;
         return;
     }
 
-    if (rx_buf[0] != slave_address && rx_buf[0] != MODBUS_ADDR_BROADCAST) {
+    if (rx_buf[0] != slave_address && rx_buf[0] != MODBUS_ADDR_BROADCAST)
+    {
         state = MODBUS_IDLE;
         return;
     }
@@ -183,7 +237,8 @@ static void modbus_process_frame(void)
     crc_recv = (uint16_t)rx_buf[rx_len - 1] << 8 | rx_buf[rx_len - 2];
     crc_calc = modbus_crc16_calc(rx_buf, rx_len - 2);
 
-    if (crc_recv != crc_calc) {
+    if (crc_recv != crc_calc)
+    {
         state = MODBUS_IDLE;
         return;
     }
@@ -193,28 +248,31 @@ static void modbus_process_frame(void)
         uint16_t reg_addr;
         uint16_t reg_count;
 
-        switch (func) {
+        switch (func)
-            case MODBUS_FUNC_READ_HOLDING:
+        {
-                if (rx_len < 8) {
+        case MODBUS_FUNC_READ_HOLDING:
-                    break;
+            if (rx_len < 8)
-                }
+            {
-                reg_addr = (uint16_t)rx_buf[2] << 8 | rx_buf[3];
-                reg_count = (uint16_t)rx_buf[4] << 8 | rx_buf[5];
-                modbus_handle_read_holding(reg_addr, reg_count);
                 break;
+            }
+            reg_addr = (uint16_t)rx_buf[2] << 8 | rx_buf[3];
+            reg_count = (uint16_t)rx_buf[4] << 8 | rx_buf[5];
+            modbus_handle_read_holding(reg_addr, reg_count);
+            break;
 
-            case MODBUS_FUNC_WRITE_SINGLE_REG:
+        case MODBUS_FUNC_WRITE_SINGLE_REG:
-                if (rx_len < 8) {
+            if (rx_len < 8)
-                    break;
+            {
-                }
-                reg_addr = (uint16_t)rx_buf[2] << 8 | rx_buf[3];
-                reg_count = (uint16_t)rx_buf[4] << 8 | rx_buf[5];
-                modbus_handle_write_single_reg(reg_addr, reg_count);
                 break;
+            }
+            reg_addr = (uint16_t)rx_buf[2] << 8 | rx_buf[3];
+            reg_count = (uint16_t)rx_buf[4] << 8 | rx_buf[5];
+            modbus_handle_write_single_reg(reg_addr, reg_count);
+            break;
 
-            default:
+        default:
-                modbus_send_exception(func, MODBUS_EXCEPTION_ILLEGAL_FUNCTION);
+            modbus_send_exception(func, MODBUS_EXCEPTION_ILLEGAL_FUNCTION);
-                break;
+            break;
         }
     }
 
@@ -245,20 +303,25 @@ void modbus_slave_poll(void)
     uint8_t byte;
     uint32_t now;
 
-    if (!initialized) {
+    if (!initialized)
+    {
         return;
     }
 
     available = uart_available(UART_ID_USART1);
 
-    if (available > 0) {
+    if (available > 0)
-        if (state == MODBUS_IDLE) {
+    {
+        if (state == MODBUS_IDLE)
+        {
             state = MODBUS_RX_IN_PROGRESS;
             rx_len = 0;
         }
 
-        while (available > 0 && rx_len < MODBUS_BUF_SIZE) {
+        while (available > 0 && rx_len < MODBUS_BUF_SIZE)
-            if (uart_receive(UART_ID_USART1, &byte, 1, 0) == RET_OK) {
+        {
+            if (uart_receive(UART_ID_USART1, &byte, 1, 0) == RET_OK)
+            {
                 rx_buf[rx_len++] = byte;
             }
             available--;
@@ -267,14 +330,17 @@ void modbus_slave_poll(void)
         rx_tick_start = system_get_tick();
     }
 
-    if (state == MODBUS_RX_IN_PROGRESS && rx_len > 0) {
+    if (state == MODBUS_RX_IN_PROGRESS && rx_len > 0)
+    {
         now = system_get_tick();
-        if ((now - rx_tick_start) >= MODBUS_RX_TIMEOUT_MS) {
+        if ((now - rx_tick_start) >= MODBUS_RX_TIMEOUT_MS)
+        {
             state = MODBUS_RX_COMPLETE;
         }
     }
 
-    if (state == MODBUS_RX_COMPLETE) {
+    if (state == MODBUS_RX_COMPLETE)
+    {
         modbus_process_frame();
     }
 }

bsp/modbus/modbus_slave.h

@@ -5,7 +5,7 @@
 #include <stdint.h>
 
 #define MODBUS_SLAVE_ADDR       0x01
-#define MODBUS_MASTER_ADDR      0xF0
+#define MODBUS_MASTER_ADDR      0xFE
 #define MODBUS_BUF_SIZE         256
 
 #define MODBUS_FUNC_READ_COILS             0x01

drv/system/sys_data_bus.h

@@ -22,6 +22,10 @@ typedef enum {
     SYS_DATA_BUS_IS_CALIBRATED,
     SYS_DATA_BUS_IS_DI_MAX_INSTALLED,
     SYS_DATA_BUS_MODBUS_SLAVE_ADDR,
+    SYS_DATA_BUS_DEVICE_ID,
+    SYS_DATA_BUS_MOTOR_CMD,
+    SYS_DATA_BUS_MOTOR_STATUS,
+    SYS_DATA_BUS_ANGLE_TARGET,
 } sys_data_bus_id_t;
 
 typedef union {