449 lines
19 KiB
C
449 lines
19 KiB
C
#ifndef PROTOCOL_H
|
||
#define PROTOCOL_H
|
||
|
||
#include "main.h"
|
||
#include "usart.h"
|
||
#include <stdint.h>
|
||
#ifdef USE_ELOG
|
||
#define LOG_TAG "motor"
|
||
#include <elog.h>
|
||
#else
|
||
// 重定向log_e等函数到printf，并自动添加换行符
|
||
#define log_e(format, ...) printf(format "\r\n", ##__VA_ARGS__)
|
||
#define log_i(format, ...) printf(format "\r\n", ##__VA_ARGS__)
|
||
#define log_d(format, ...) printf(format "\r\n", ##__VA_ARGS__)
|
||
#define log_w(format, ...) printf(format "\r\n", ##__VA_ARGS__)
|
||
#endif
|
||
|
||
// 帧头帧尾定义
|
||
#define FRAME_HEADER 0xA55A5AA5
|
||
#define FRAME_TAIL 0x5AA5A55A
|
||
#define READ_ACK_TIMEOUT 50
|
||
#define ACK_OK 0x0000
|
||
#define ACK_FAILED 0x0001
|
||
#define ACK_OTHER 0x0002
|
||
|
||
// 功能码定义
|
||
#define HOST_CMD_STATUS_QUERY 0x0001 // 状态查询
|
||
#define HOST_CMD_VALVE_CTRL 0x0002 // 三通阀控制
|
||
#define HOST_CMD_PUMP_RUN_TIME 0x0003 // 泵运行时长控制
|
||
#define HOST_CMD_PUMP_RUN_SPEED 0x0004 // 泵运行速度设置
|
||
#define HOST_CMD_SOFT_STOP 0x0005 // 软急停功能
|
||
#define HOST_CMD_PUMP_RUN_STEP 0x0006 // 泵步进设置
|
||
#define HOST_CMD_SYSTEM_INIT 0x0007 // 系统初始化
|
||
#define CMD_BR_SET 0x0008 // 波特率设置
|
||
#define CMD_PR_SET 0x0009 // 通信协议设置
|
||
|
||
#define PUMP_SPEED_RPS 240//速度单位为1/240 rps
|
||
#define PUMP_ACCEL_RPS 6//加速度单位为1/6 rps/s
|
||
#define PUMP_DECEL_RPS 6//减速度单位为1/6 rps/s
|
||
#define PUMP_SPEED_RPS_MAX 50//
|
||
|
||
// 命令帧错误码定义
|
||
typedef enum {
|
||
CMD_FRAME_OK = 0,
|
||
CMD_FRAME_HEADER_ERROR = -1,
|
||
CMD_FRAME_TAIL_ERROR = -2,
|
||
CMD_FRAME_CHECK_ERROR = -3,
|
||
CMD_FRAME_CMD_ERROR = -4,
|
||
} CmdFrameError_t;
|
||
|
||
|
||
|
||
// MOONS’驱动器支持的Modbus功能码如下：
|
||
// 0x03：读取保持寄存器
|
||
// 0x06：写单个寄存器
|
||
// 0x10：写多个寄存器
|
||
#define RTU_FUNC_READ_HOLD_REG 0x03 // 读取保持寄存器
|
||
#define RTU_FUNC_READ_INPUT_REG 0x04 // 读取输入寄存器
|
||
#define RTU_FUNC_WRITE_REG 0x06 // 写单个寄存器
|
||
#define RTU_FUNC_WRITE_MULTI_REG 0x10 // 写多个寄存器
|
||
|
||
/*寄存器4001告警代码
|
||
报警代码用来表示驱动器当前的报警信息，用户可以通过查询报警代码寄存器了解具体的报警信息。报
|
||
警代码寄存器的每一位代表不同的报警信息，当某一位被置1时表示驱动器正处于该位定义的报警状态，
|
||
具体每一位的定义可参考下表：
|
||
0 位置误差超限
|
||
1 CCW方向禁止限位
|
||
2 CW方向禁止限位
|
||
3 驱动器过温
|
||
4 驱动器内部电压错误
|
||
5 驱动器过压
|
||
6 驱动器欠压
|
||
7 驱动器过流
|
||
8 电机绕组开关
|
||
9 电机编码器信号错误
|
||
10 通讯异常
|
||
11 参数保存失败
|
||
12 在电机未使能时命令其运转
|
||
13 电机重载状态
|
||
14 调用的Q程序段为空
|
||
15 存储器错误
|
||
*/
|
||
// 使用联合体表示
|
||
|
||
// 告警寄存器联合体定义
|
||
typedef union {
|
||
struct {
|
||
uint16_t position_error:1; // 位0: 位置误差超限
|
||
uint16_t ccw_limit:1; // 位1: CCW方向禁止限位
|
||
uint16_t cw_limit:1; // 位2: CW方向禁止限位
|
||
uint16_t over_temp:1; // 位3: 驱动器过温
|
||
uint16_t voltage_error:1; // 位4: 驱动器内部电压错误
|
||
uint16_t over_voltage:1; // 位5: 驱动器过压
|
||
uint16_t under_voltage:1; // 位6: 驱动器欠压
|
||
uint16_t over_current:1; // 位7: 驱动器过流
|
||
uint16_t winding_switch:1; // 位8: 电机绕组开关
|
||
uint16_t encoder_error:1; // 位9: 电机编码器信号错误
|
||
uint16_t comm_error:1; // 位10: 通讯异常
|
||
uint16_t param_save_failed:1; // 位11: 参数保存失败
|
||
uint16_t motor_disabled:1; // 位12: 在电机未使能时命令其运转
|
||
uint16_t motor_overload:1; // 位13: 电机重载状态
|
||
uint16_t empty_q_program:1; // 位14: 调用的Q程序段为空
|
||
uint16_t memory_error:1; // 位15: 存储器错误
|
||
} bits;
|
||
uint16_t all; // 访问完整的16位寄存器
|
||
}AlarmCode_t;
|
||
|
||
// 用于输出具体的告警信息字符串
|
||
static const uint8_t alarmInfo[16][50]={
|
||
"位置误差超限",
|
||
"CCW方向禁止限位",
|
||
"CW方向禁止限位",
|
||
"驱动器过温",
|
||
"驱动器内部电压错误",
|
||
"驱动器过压",
|
||
"驱动器欠压",
|
||
"驱动器过流",
|
||
"电机绕组开<EFBFBD><EFBFBD>",
|
||
"电机编码器信号错误",
|
||
"通讯异常",
|
||
"参数保存失败",
|
||
"在电机未使能时命令其运转",
|
||
"电机重载状态",
|
||
"调用的Q程序段为空",
|
||
"存储器错误",
|
||
};
|
||
|
||
/*
|
||
寄存器4002状态代码
|
||
状态代码用来表示驱动器当前的工作状态，用户可以通过查询状态寄存器了解具体的状态信息。状态代
|
||
码寄存器的每一位代表不同的状态信息，当某一位被置1时表示驱动器正处于该位定义的状态，具体每一
|
||
位的定义可参考下表。
|
||
|
||
位 说明
|
||
0 使能
|
||
1 采样中（软件示波器功能开启）
|
||
2 驱动器报故障
|
||
3 运动到位
|
||
4 运动中
|
||
5 点动运行中
|
||
6 减速中
|
||
7 等待输入信号（例如执行WI指令）
|
||
8 参数保存中
|
||
9 驱动器报警告
|
||
10 回原点中
|
||
11 等待时间（例如执行WT、WD指令）
|
||
12 内部使用
|
||
13 编码器检测中
|
||
14 Q程序运行中
|
||
15 初始化（步进系），伺服准备好（伺服系）
|
||
*/
|
||
|
||
typedef union {
|
||
struct {
|
||
uint16_t enable:1; // 位0: 使能
|
||
uint16_t sample:1; // 位1: 采样中（软件示波器功能开启）
|
||
uint16_t fault:1; // 位2: 驱动器报故障
|
||
uint16_t position_reached:1; // 位3: 运动到位
|
||
uint16_t moving:1; // 位4: 运动中
|
||
uint16_t point_move:1; // 位5: 点动运行中
|
||
uint16_t decelerating:1; // 位6: 减速中
|
||
uint16_t wait_input:1; // 位7: 等待输入信号（例如执行WI指令）
|
||
uint16_t parameter_save:1; // 位8: 参数保存中
|
||
uint16_t alarm:1; // 位9: 驱动器报警告
|
||
uint16_t return_home:1; // 位10: 回原点中
|
||
uint16_t wait_time:1; // 位11: 等待时间（例如执行WT、WD指令）
|
||
uint16_t encoder_check:1; // 位12: 编码器检测中
|
||
uint16_t q_program_run:1; // 位13: Q程序运行中
|
||
uint16_t init:1; // 位14: 初始化（步进系），伺服准备好（伺服系）
|
||
} bits;
|
||
uint16_t all;
|
||
} StatusCode_t;
|
||
static const uint8_t statusInfo[16][60]={
|
||
"使能",
|
||
"采样中（软件示波器功能开启）",
|
||
"驱动器报故障",
|
||
"运动到位",
|
||
"运动中",
|
||
"点动运行中",
|
||
"减速中",
|
||
"等待输入信号（例如执行WI指令）",
|
||
"参数保存中",
|
||
"驱动器报警告",
|
||
"回原点中",
|
||
"等待时间（例如执行WT、WD指令）",
|
||
"内部使用",
|
||
"编码器检测中",
|
||
"Q程序运行中",
|
||
"初始化（步进系），伺服准备好（伺服系）",
|
||
};
|
||
|
||
// 位置控制，点对点模式
|
||
/* 设置加速度、减速度、速度和目标位置，对应MOONS’ SCL指令如下：
|
||
+----------+----------+----------+--------------+--------------+--------------+--------------------------------------------------+
|
||
| SCL指令 | 设定数值 | 单位 | 寄存器地址 | 十六进制格式 | 写入寄存器值 | 说明 |
|
||
| | | | | 寄存器地址 | | |
|
||
+----------+----------+----------+--------------+--------------+--------------+--------------------------------------------------+
|
||
| AC | 100 | Rps/sec | 40028 | 001B | 600 | 预设置加速度为100，需要对寄存器40028 |
|
||
| | | | | | | 写入600(0x0258) |
|
||
+----------+----------+----------+--------------+--------------+--------------+--------------------------------------------------+
|
||
| DE | 100 | Rps/sec | 40029 | 001C | 600 | 预设置减速度为100，需要对寄存器40029 |
|
||
| | | | | | | 写入600(0x0258) |
|
||
+----------+----------+----------+--------------+--------------+--------------+--------------------------------------------------+
|
||
| VE | 1 | Rps | 40030 | 001D | 240 | 预设置速度为1，需要对寄存器40030写入 |
|
||
| | | | | | | 240(0x00F0) |
|
||
+----------+----------+----------+--------------+--------------+--------------+--------------------------------------------------+
|
||
| DI | 200000 | Counts | 40031,40032 | 001E,001F | 200000 | 预设定目标位置为200000，需要对40031和40032寄存器 |
|
||
| | | | | | | 写入200000(0x00030D40) |
|
||
+----------+----------+----------+--------------+--------------+--------------+--------------------------------------------------+
|
||
*/
|
||
|
||
#define RTU_PUMP_CMD_AC 0x001B // 加速度
|
||
#define RTU_PUMP_CMD_DE 0x001C // 减速度
|
||
#define RTU_PUMP_CMD_VE 0x001D // 速度
|
||
#define RTU_PUMP_CMD_DI 0x001E // 目标位置
|
||
#define RTU_PUMP_CMD_HW 0x006C // 硬件版本40109
|
||
#define RTU_PUMP_CMD_BR 0x0094 //波特率BR，40149
|
||
#define RTU_PUMP_CMD_PR 0x0095 //通信协议PR，40150
|
||
#define RTU_PUMP_CMD_SPEED 0x000A //瞬时实际速度,40011
|
||
#define RTU_PUMP_CMD_POS 0x0008 //绝对位置,40009-40010
|
||
|
||
// 速度控制，慢跑Jogging模式
|
||
/*
|
||
+----------+----------+----------+------------+--------------+--------------+--------------------------------------+----------+
|
||
| SCL指令 | 设定数值 | 单位 | 寄存器地址 | 十六进制格式 | 写入寄存器值 | 说明 | |
|
||
| | | | | 寄存器地址 | | | |
|
||
+----------+----------+----------+------------+--------------+--------------+--------------------------------------+----------+
|
||
| JA | 100 | Rps/sec | 40047 | 002E | 600 | 预设置加速度为100， | |
|
||
| | | | | | | 需要对寄存器40047写600入600(0x0258) | |
|
||
+----------+----------+----------+------------+--------------+--------------+--------------------------------------+----------+
|
||
| JL | 100 | Rps/sec | 40048 | 002F | 600 | 预设置减速度为100， | |
|
||
| | | | | | | 需要对寄存器40048写入600(0x0258) | |
|
||
+----------+----------+----------+------------+--------------+--------------+--------------------------------------+----------+
|
||
| JS | 10 | Rps | 40049 | 0030 | 2400 | 预设置速度为10， | (0x0960) |
|
||
| | | | | | | 需要对寄存器40049写入2400 | |
|
||
+----------+----------+----------+------------+--------------+--------------+--------------------------------------+----------+
|
||
*/
|
||
#define RTU_PUMP_CMD_JA 0x002E // Jog加速度
|
||
#define RTU_PUMP_CMD_JL 0x002F // Jog减速度
|
||
#define RTU_PUMP_CMD_JS 0x0030 // Jog速度
|
||
|
||
|
||
|
||
/* 备注：
|
||
1. Modbus报文读写时，注意寄存器地址间的转换关系，如寄存器40125转换后为0x007C， 即
|
||
40125-40000-1=124(0x007C)
|
||
2. PR=5模式和PR=133模式的区别:
|
||
为了将目标位置DI=200000写入目标位置寄存器(40032 ,40031)，即向32位寄存器地址写入
|
||
200000(0x030D40)
|
||
• 在PR=5，即Big Endian模式下，表示写入32位数据为高16位数据排放在内存的低地址端，低
|
||
16位数据排放在内存的高地址端
|
||
• 在PR=133，即Little Endian模式下，表示写入32位数据为低16位数据排放在内存的低地址端，
|
||
高16位数据排放在内存的高地址端
|
||
3. 速度、加/减速度寄存器参数设定值单位
|
||
• 速度类寄存器参数设定值单位为 1
|
||
240 rps
|
||
• 加/减速度类寄存器参数设定值单位为 1
|
||
6 rps/s
|
||
*/
|
||
#define RTU_PUMP_CMD_CO 0x007C // 操作码，Command Opcode
|
||
|
||
#define RTU_PUMP_CMD_SC 0x0002 // 状态寄存器
|
||
#define RTU_PUMP_CMD_AL 0x0001 // 告警寄存器
|
||
|
||
|
||
|
||
|
||
|
||
// valve
|
||
/*
|
||
+------------------------------------+------------+
|
||
| 指令名称 | 寄存器地址 |
|
||
+------------------------------------+------------+
|
||
| 控制模式设置为CIA402模式 | 00B1h |
|
||
+------------------------------------+------------+
|
||
| 运行模式设置为原点回归模式 | 03C2h |
|
||
+------------------------------------+------------+
|
||
| 设置原点回归方式为17（负限位开关） | 0416h |
|
||
| 或者18（正限位开关）两个选一个 | |
|
||
+------------------------------------+------------+
|
||
| 设置寻找原点开关为10000（高速） | 0417h |
|
||
+------------------------------------+------------+
|
||
| 设置寻找原点信号速度为1000（低速） | 0419h |
|
||
+------------------------------------+------------+
|
||
| 设置回零加速度为200000 | 041Bh |
|
||
+------------------------------------+------------+
|
||
| 使电机准备 | 0380h |
|
||
+------------------------------------+------------+
|
||
| 使电机<E794B5><E69CBA><EFBFBD>能 | 0380h |
|
||
+------------------------------------+------------+
|
||
| 使电机使能 | 0380h |
|
||
+------------------------------------+------------+
|
||
| 使电机运行原点回归 | 0380h |
|
||
+------------------------------------+------------+
|
||
*/
|
||
#define VALVE_PULSE_PER_ROUND 10000 //阀门一圈的脉冲数
|
||
|
||
#define RTU_VALVE_CMD_CTL_MODE 0x00B1 // 阀门控制模式
|
||
#define RTU_VALVE_CMD_RUN_MODE 0x03C2 // 阀门运行模式
|
||
|
||
#define RTU_VALVE_CMD_PP_POS 0x03E7 // 阀门轮廓位置
|
||
#define RTU_VALVE_CMD_PP_SPEED 0x03F8 // 阀门轮廓速度
|
||
#define RTU_VALVE_CMD_PP_ACCEL 0x03FC // 阀门轮廓加速度
|
||
#define RTU_VALVE_CMD_PP_DECEL 0x03FE // 阀门轮廓减速度
|
||
#define RTU_VALVE_CMD_SC 0x0381 // 阀门运行状态
|
||
#define RTU_VALVE_CMD_AL 0x037F // 阀门运行告警
|
||
#define RTU_VALVE_CMD_POS 0x03C8 // 阀门运行位置，用户单位
|
||
#define RTU_VALVE_CMD_SPEED 0x03D5 // 阀门当前速度，用户单位/s，rpm
|
||
|
||
#define RTU_VALVE_CMD_HOME_MODE 0x0416 // 阀门原点回归方式
|
||
#define RTU_VALVE_CMD_HOME_SWT_SPEED 0x0417 // 阀门回归寻找开关的速度
|
||
#define RTU_VALVE_CMD_HOME_ORI_SPEED 0x0419 // 阀门回归寻找<E5AFBB><E689BE><EFBFBD>点的速度
|
||
#define RTU_VALVE_CMD_HOME_ACCEL 0x041B // 阀门回归加速度
|
||
#define RTU_VALVE_CMD_FUNC 0x0380 // 阀门控制
|
||
#define RTU_VALVE_CMD_HOME_TORQUE 0x0170 // 阀门原点回归堵转检测力矩
|
||
#define RTU_VALVE_CMD_HOME_TIME 0x0172 // 阀门原点回归堵转检测时间
|
||
|
||
#define RTU_VALVE_CFG_COMM_CIA402 0x0000 // 为CIA402模式
|
||
#define RTU_VALVE_CFG_MODE_HM 0x0006 // 原点回归模式(HM)
|
||
#define RTU_VALVE_CFG_MODE_PP 0x0001 // 轮廓位置模式(PP)
|
||
#define RTU_VALVE_CFG_PREPARE 0x0006 // 准备
|
||
#define RTU_VALVE_CFG_DISABLE 0x0007 // 失能
|
||
#define RTU_VALVE_CFG_ENABLE 0x000F // 使能
|
||
#define RTU_VALVE_CFG_RUN_ORIGIN 0x001F // 运行原点回归
|
||
|
||
// 错误码定义
|
||
typedef enum {
|
||
ERR_NONE = 0x00,
|
||
ERR_COMM = 0x01,
|
||
ERR_CTRL = 0x02,
|
||
// 其他错误码待定
|
||
} ErrorCode_t;
|
||
|
||
// 设备状态相关定义
|
||
|
||
// 1. 下挂设备状态
|
||
typedef enum {
|
||
SENSOR_OFFLINE = 0,
|
||
SENSOR_ONLINE = 1
|
||
} SensorStatus;
|
||
|
||
// 2. 三通阀角度
|
||
typedef enum {
|
||
VALVE_ANGLE_120 = 120,
|
||
VALVE_ANGLE_210 = 210
|
||
} ValveAngle;
|
||
|
||
// 3. 泵状态
|
||
typedef enum {
|
||
PUMP_STATUS_STOP = 0,
|
||
PUMP_STATUS_CLOCKWISE = 1,
|
||
PUMP_STATUS_ANTICLOCKWISE = 2
|
||
} PumpStatus;
|
||
|
||
// 4. 泵速度范围
|
||
#define PUMP_SPEED_MIN 0
|
||
#define PUMP_SPEED_MAX 100
|
||
|
||
// 5. 气泡状态
|
||
typedef enum {
|
||
BUBBLE_NONE = 0,
|
||
BUBBLE_DETECTED = 1
|
||
} BubbleStatus;
|
||
|
||
// 6. 急停按键状态
|
||
typedef enum {
|
||
ESTOP_NORMAL = 0,
|
||
ESTOP_PRESSED = 1
|
||
} EstopStatus_t;
|
||
|
||
// 7. 初始化状态
|
||
typedef enum {
|
||
INIT_IN_PROGRESS = 0,
|
||
INIT_SUCCESS = 1,
|
||
INIT_FAILED = 2
|
||
} InitStatus_t;
|
||
|
||
// 三通阀结构体
|
||
typedef struct {
|
||
uint8_t angle[2]; // 阀门角度 (120/210)
|
||
} ValveStatus_t;
|
||
|
||
// 泵结构体
|
||
typedef struct {
|
||
uint8_t status[2]; // 泵运行状态 (停止/顺时针/逆时针)
|
||
uint8_t speed[2]; // 泵速度百分比 (0-100)，设置值
|
||
} PumpStatus_t;
|
||
|
||
// 设备状态结构体，用于上报HOST
|
||
typedef struct {
|
||
uint8_t sensorStatus; // 下挂设备状态
|
||
ValveStatus_t valves; // 两个三通阀状态
|
||
PumpStatus_t pumps; // 两个泵状态
|
||
uint8_t bubbleStatus; // 气泡状态
|
||
uint16_t activityMeter; // 活度计mCi
|
||
uint8_t estopStatus; // 急停状态
|
||
uint8_t errorCode; // 错误码
|
||
uint8_t initStatus; // 初始化状态
|
||
} DeviceStatus_t;
|
||
|
||
|
||
typedef struct
|
||
{
|
||
DeviceStatus_t ds;
|
||
uint32_t valvesSpeed[2];//实时速度
|
||
uint8_t valvesSpeedPercent[2];//实时速度百分比
|
||
uint32_t valvesPos[2];//实时位置
|
||
uint32_t pumpsSpeed[2];//实时速度
|
||
uint8_t pumpsSpeedPercent[2];//实时速度百分比
|
||
uint32_t pumpsPos[2];//实时位置
|
||
uint16_t rst;//RTU命令执行结果
|
||
|
||
} SystemStatus_t;//包含需要上报的状态及附加状态
|
||
|
||
|
||
typedef struct {
|
||
uint8_t name[20];
|
||
uint8_t id;
|
||
uint32_t speed;//满速
|
||
uint32_t accel;
|
||
uint32_t decel;
|
||
uint16_t fullCount;//电机总步数，用于根据角度估算需要移动的步数
|
||
int16_t offsetPos;//电机偏移位置，用于补偿电机移动误差
|
||
uint8_t speedPercent;//设置速度百分比数，如100表示100%
|
||
uint8_t torque;//堵转力矩百分比,堵转转矩阈值应大于找寻原点过程中的实际运行转矩，且一般应小于最大转矩，以避免触发堵转故障保护
|
||
uint8_t timeout;//回归超时时间，单位s
|
||
} MotorDefaultParam_t;
|
||
|
||
|
||
// 定义设备默认参数
|
||
typedef struct {
|
||
MotorDefaultParam_t pump[2];
|
||
MotorDefaultParam_t valve[2];
|
||
} DeviceParam_t;
|
||
|
||
|
||
extern DeviceParam_t dp;
|
||
extern DeviceStatus_t deviceStatus;
|
||
|
||
// 函数声明
|
||
void ProcessHostCommand(uint8_t *rxBuf, uint8_t rxLen);
|
||
void runPumpDemo(void);
|
||
void runValveDemo(void);
|
||
void updateSystemStatus(void);
|
||
void initCTLSystem(void);
|
||
|
||
|
||
|
||
#endif // PROTOCOL_H
|