From 20e3988eee30145755975c3f928ed8a76371cc9b Mon Sep 17 00:00:00 2001 From: hm-thinkbook14p Date: Mon, 11 May 2026 22:00:49 +0800 Subject: [PATCH] =?UTF-8?q?=E6=9B=B4=E6=96=B0=EF=BC=9A=E6=95=B4=E6=9C=BA?= =?UTF-8?q?=E9=80=BB=E8=BE=91=E3=80=81ID=E6=8C=87=E7=A4=BA=E5=99=A8?= =?UTF-8?q?=E9=A9=B1=E5=8A=A8=E3=80=81=E5=8A=A8=E6=80=81Modbus=E5=9C=B0?= =?UTF-8?q?=E5=9D=80=E3=80=81=E6=8C=87=E4=BB=A4=E6=96=87=E6=A1=A3?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- CMakeLists.txt | 1 + README.md | 155 ++++++++++++++++++++++++++ app/main.c | 13 ++- app/stepper/stepper_control.c | 42 +++++++ app/stepper/stepper_control.h | 23 ++++ app/tasks/work_task.c | 202 +++++++++++++++++++++++++++++++++- bsp/device/id/id_driver.c | 34 ++++++ bsp/device/id/id_driver.h | 14 +++ bsp/modbus/modbus_slave.c | 188 +++++++++++++++++++++---------- bsp/modbus/modbus_slave.h | 2 +- drv/system/sys_data_bus.h | 4 + 11 files changed, 614 insertions(+), 64 deletions(-) create mode 100644 README.md create mode 100644 bsp/device/id/id_driver.c create mode 100644 bsp/device/id/id_driver.h diff --git a/CMakeLists.txt b/CMakeLists.txt index febf80d..5a6a50b 100644 --- a/CMakeLists.txt +++ b/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 diff --git a/README.md b/README.md new file mode 100644 index 0000000..002052d --- /dev/null +++ b/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 diff --git a/app/main.c b/app/main.c index f423f4f..3a398d9 100644 --- a/app/main.c +++ b/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(); diff --git a/app/stepper/stepper_control.c b/app/stepper/stepper_control.c index 86ca58e..9a97c6e 100644 --- a/app/stepper/stepper_control.c +++ b/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; +} diff --git a/app/stepper/stepper_control.h b/app/stepper/stepper_control.h index a09b5f8..8a6eb66 100644 --- a/app/stepper/stepper_control.h +++ b/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 diff --git a/app/tasks/work_task.c b/app/tasks/work_task.c index 521b1d7..d821bd6 100644 --- a/app/tasks/work_task.c +++ b/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) diff --git a/bsp/device/id/id_driver.c b/bsp/device/id/id_driver.c new file mode 100644 index 0000000..ad3b27d --- /dev/null +++ b/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; +} diff --git a/bsp/device/id/id_driver.h b/bsp/device/id/id_driver.h new file mode 100644 index 0000000..a0e4cf4 --- /dev/null +++ b/bsp/device/id/id_driver.h @@ -0,0 +1,14 @@ +#ifndef ID_DRIVER_H__ +#define ID_DRIVER_H__ + +#include "common_types.h" +#include + +#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 diff --git a/bsp/modbus/modbus_slave.c b/bsp/modbus/modbus_slave.c index 8465626..4ea7f5f 100644 --- a/bsp/modbus/modbus_slave.c +++ b/bsp/modbus/modbus_slave.c @@ -5,40 +5,48 @@ #include "config.h" #include -#define MODBUS_RX_TIMEOUT_MS 5 -#define MODBUS_TX_GUARD_US 2 +#define MODBUS_RX_TIMEOUT_MS 5 +#define MODBUS_TX_GUARD_US 2 -#define MODBUS_EXCEPTION_ILLEGAL_FUNCTION 0x01 -#define MODBUS_EXCEPTION_ILLEGAL_DATA_ADDR 0x02 -#define MODBUS_EXCEPTION_ILLEGAL_DATA_VAL 0x03 +#define MODBUS_EXCEPTION_ILLEGAL_FUNCTION 0x01 +#define MODBUS_EXCEPTION_ILLEGAL_DATA_ADDR 0x02 +#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_END 0x001F -#define REG_BUS_DATA_HIGH_START 0x0020 -#define REG_BUS_DATA_HIGH_END 0x003F -#define REG_FLAG_LOW 0x0040 -#define REG_FLAG_HIGH 0x0041 +#define REG_BUS_DATA_START 0x0000 +#define REG_BUS_DATA_END 0x001F +#define REG_BUS_DATA_HIGH_START 0x0020 +#define REG_BUS_DATA_HIGH_END 0x003F +#define REG_FLAG_LOW 0x0040 +#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 uint16_t rx_len; -static uint32_t rx_tick_start; -static uint8_t initialized; +static uint8_t rx_buf[MODBUS_BUF_SIZE]; +static uint16_t rx_len; +static uint32_t rx_tick_start; +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++) { - if (crc & 0x0001) { + for (j = 0; j < 8; j++) + { + 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) { - case MODBUS_FUNC_READ_HOLDING: - 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_read_holding(reg_addr, reg_count); + switch (func) + { + case MODBUS_FUNC_READ_HOLDING: + 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_read_holding(reg_addr, reg_count); + break; - case MODBUS_FUNC_WRITE_SINGLE_REG: - 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); + case MODBUS_FUNC_WRITE_SINGLE_REG: + 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; - default: - modbus_send_exception(func, MODBUS_EXCEPTION_ILLEGAL_FUNCTION); - break; + default: + modbus_send_exception(func, MODBUS_EXCEPTION_ILLEGAL_FUNCTION); + 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 (state == MODBUS_IDLE) { + if (available > 0) + { + if (state == MODBUS_IDLE) + { state = MODBUS_RX_IN_PROGRESS; rx_len = 0; } - while (available > 0 && rx_len < MODBUS_BUF_SIZE) { - if (uart_receive(UART_ID_USART1, &byte, 1, 0) == RET_OK) { + while (available > 0 && rx_len < MODBUS_BUF_SIZE) + { + 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(); } } diff --git a/bsp/modbus/modbus_slave.h b/bsp/modbus/modbus_slave.h index 6cbe7cd..3910f47 100644 --- a/bsp/modbus/modbus_slave.h +++ b/bsp/modbus/modbus_slave.h @@ -5,7 +5,7 @@ #include #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 diff --git a/drv/system/sys_data_bus.h b/drv/system/sys_data_bus.h index 6c2870a..2f297a6 100644 --- a/drv/system/sys_data_bus.h +++ b/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 {