From d3917433612332a8357a3311002d865ce173b59a Mon Sep 17 00:00:00 2001 From: "CSSC-WORK\\murmur" Date: Wed, 11 Dec 2024 15:55:40 +0800 Subject: [PATCH] add operation logic --- protocol.c | 222 ++++++++++++++++++++++++++++++++--------------------- protocol.h | 43 ++++++++++- 2 files changed, 175 insertions(+), 90 deletions(-) diff --git a/protocol.c b/protocol.c index dbe80c6..d317ad0 100644 --- a/protocol.c +++ b/protocol.c @@ -104,6 +104,15 @@ void updateInitStatus(InitStatus_t status) { // 初始化设备状态 void InitDeviceStatus() { + // 初始化泵 + WriteJogAcc(1, 100); + WriteJogDec(1, 100); + WriteJogSpeed(1, 10); + WriteStepAcc(1, 100); + WriteStepDec(1, 100); + WriteStepSpeed(1, 10); + + // 更新设备状态 updateDeviceStatus(DEVICE_ONLINE); updateValveStatus(1, 120); @@ -156,85 +165,6 @@ static void FillBigEndian16(uint8_t *data, uint16_t value) { } } -// 泵加速、减速处理 -static uint8_t WritePumpAccDec(uint8_t index, uint8_t acc, uint8_t dec) { - // 实现泵加速、减速逻辑 - - RTU_Frame frame; - frame.data_cnt = 2; - uint8_t len = sizeof(RTU_Frame)+frame.data_cnt*sizeof(uint8_t); - - frame.device_id = 1; - frame.func = RTU_PUMP_FUNC_WRITE_REG; - FillBigEndian16(&frame.reg_addr[0], RTU_PUMP_CMD_AC); - FillBigEndian16(&frame.reg_cnt[0], 2); - - - FillBigEndian32(&frame.data[0], acc); - FillBigEndian32(&frame.data[4], dec); - - - uint16_t crc = CalculateCRC16(&frame, len - sizeof(frame.crc)); - // 大端序填充 - FillBigEndian16(&frame.crc[0], crc); - - writeCMD((uint8_t*)&frame, len); -} - -// 泵速度设置处理 -static uint8_t WritePumpSpeed(uint8_t index, uint8_t speed) { - // 实现泵速度设置逻辑 - - RTU_Frame frame; - frame.data_cnt = 2; - uint8_t len = sizeof(RTU_Frame)+frame.data_cnt*sizeof(uint8_t); - - frame.device_id = 1; - frame.func = RTU_PUMP_FUNC_WRITE_REG; - FillBigEndian16(&frame.reg_addr[0], RTU_PUMP_CMD_VE); - FillBigEndian16(&frame.reg_cnt[0], 2); - - FillBigEndian32(&frame.data[0], speed); - - uint16_t crc = CalculateCRC16(&frame, len - sizeof(frame.crc)); - // 大端序填充 - FillBigEndian16(&frame.crc[0], crc); - - writeCMD((uint8_t*)&frame, len); -} - -// 泵步进设置处理 -static uint8_t WritePumpStep(uint8_t index, int32_t step) { - // 实现泵步进设置逻辑 - - RTU_Frame frame; - frame.data_cnt = 2; - uint8_t len = sizeof(RTU_Frame)+frame.data_cnt*sizeof(uint8_t); - - frame.device_id = 1; - frame.func = RTU_PUMP_FUNC_WRITE_REG; - FillBigEndian16(&frame.reg_addr[0], RTU_PUMP_CMD_DI); - FillBigEndian16(&frame.reg_cnt[0], 2); - - // step为int32_t，正负表示方向，绝对值表示步数，0表示停止 - if(step != 0) { - FillBigEndian32(&frame.data[0], step); - } - else { - // 停止 - - } - - uint16_t crc = CalculateCRC16(&frame, len - sizeof(frame.crc)); - // 大端序填充 - FillBigEndian16(&frame.crc[0], crc); - - writeCMD((uint8_t*)&frame, len); - - - - return 1; -} // pump 读寄存器 uint16_t ReadPump1Reg(uint8_t index, uint16_t reg) { @@ -244,7 +174,7 @@ uint16_t ReadPump1Reg(uint8_t index, uint16_t reg) { data[0] = index; data[1] = RTU_PUMP_FUNC_READ_REG; FillBigEndian16(&data[2], reg); - FillBigEndian16(&data[4], 1); + FillBigEndian16(&data[4], 2); uint16_t crc = CalculateCRC16(data, 6); // 大端序填充 @@ -252,6 +182,120 @@ uint16_t ReadPump1Reg(uint8_t index, uint16_t reg) { writeCMD(data, 8); } +// 写泵1个寄存器 +uint8_t WritePump1Reg(uint8_t index, uint16_t reg, uint16_t value) { + // 写一个寄存器不需要指定寄存器长度 + uint8_t data[8] = {0}; + data[0] = index; + data[1] = RTU_PUMP_FUNC_WRITE_REG; + FillBigEndian16(&data[2], reg); + FillBigEndian16(&data[4], value); + uint16_t crc = CalculateCRC16(data, 6); + // 大端序填充 + FillBigEndian16(&data[6], crc); + + writeCMD(data, 8); +} +// 写泵2个寄存器 +uint8_t WritePump2Reg(uint8_t index, uint16_t reg, uint32_t value) { + // 写2个寄存器需要指定寄存器长度 + uint8_t data[13] = {0}; + data[0] = index; + data[1] = RTU_PUMP_FUNC_WRITE_MULTI_REG; + FillBigEndian16(&data[2], reg); + FillBigEndian16(&data[4], 2); + data[6] = 8; + FillBigEndian32(&data[7], value); + uint16_t crc = CalculateCRC16(data, 11); + // 大端序填充 + FillBigEndian16(&data[11], crc); + + writeCMD(data, 13); +} + +/* ++--------------+--------------------------------+ +| jogging设置顺序 | ++--------------+--------------------------------+ +| 1. | 设置加速度JA、减速度JL、速度JS | ++--------------+--------------------------------+ +| 2. | 启动Jog-CJ | ++--------------+--------------------------------+ +| 3. | 停止Jog-SJ | ++--------------+--------------------------------+ +*/ + +static uint8_t WriteJogAcc(uint8_t index, uint16_t acc) { + WritePump1Reg(index, RTU_PUMP_CMD_JA, acc); +} +static uint8_t WriteJogDec(uint8_t index, uint16_t dec) { + WritePump1Reg(index, RTU_PUMP_CMD_JL, dec); +} +static uint8_t WriteJogSpeed(uint8_t index, uint16_t speed) { + WritePump1Reg(index, RTU_PUMP_CMD_JS, speed); +} + +// Jog=慢跑 +// CJ=start jogging +// 写入命令操作码寄存器(40125)数据0x0096(CJ)，即执行启动Jog控制 +static uint8_t StartJogControl(uint8_t index) { + WritePump1Reg(index, RTU_PUMP_CMD_CO, 0x0096); +} + +// SJ=stop jogging +// 写入命令操作码寄存器(40125)数据0x00D8(SJ)，即执行停止Jog控制 +static uint8_t StopJogControl(uint8_t index) { + WritePump1Reg(index, RTU_PUMP_CMD_CO, 0x00D8); +} + +/* ++--------------+--------------------------------+ +| step设置顺序 | ++--------------+--------------------------------+ +| 1. | 设置加速度AC、减速度DE、速度VE | ++--------------+--------------------------------+ +| 2. | 设置步进DI | ++--------------+--------------------------------+ +| 3. | 启动相对位置FL，或绝对位置FP | ++--------------+--------------------------------+ +| 4. | 停止泵SK | ++--------------+--------------------------------+ +*/ +static uint8_t WriteStepAcc(uint8_t index, uint16_t acc) { + WritePump1Reg(index, RTU_PUMP_CMD_AC, acc); +} +static uint8_t WriteStepDec(uint8_t index, uint16_t dec) { + WritePump1Reg(index, RTU_PUMP_CMD_DE, dec); +} +static uint8_t WriteStepSpeed(uint8_t index, uint16_t speed) { + WritePump1Reg(index, RTU_PUMP_CMD_VE, speed); +} +static uint8_t WriteStepTarget(uint8_t index, uint32_t target) { + WritePump2Reg(index, RTU_PUMP_CMD_DI, target); +} + +// FL=feed length +// 写入命令操作码寄存器(40125)数据0x0066(FL)，即执行相对位置控制 +static uint8_t RelativePositionControl(uint8_t index) { + WritePump1Reg(index, RTU_PUMP_CMD_CO, 0x0066); +} + +// FP=feed position +// 写入命令操作码寄存器(40125)数据0x0067(FP)，即执行绝对位置控制 +static uint8_t AbsolutePositionControl(uint8_t index) { + WritePump1Reg(index, RTU_PUMP_CMD_CO, 0x0067); +} + +// SK=Stop Move & Kill Buffer, Max Decel +// 写入命令操作码寄存器(40125)数据0x00E1(SK)，即执行停止控制 +static uint8_t StopPump(uint8_t index) { + WritePump1Reg(index, RTU_PUMP_CMD_CO, 0x00E1); +} + +// 获取固件版本 +uint8_t ReadHWReg(uint8_t index) { + ReadPump1Reg(index, RTU_PUMP_CMD_HW); +} // 获取泵状态 void ReadPumpStatus(uint8_t index) { @@ -391,12 +435,12 @@ static uint8_t HandlePumpSpeedControl(uint8_t *Buff, uint8_t len) { return 0; } uint8_t index = Buff[0]; - uint8_t speed = Buff[1]; + uint16_t speed = Buff[1]; if (speed > 100) { printf("泵速度设置错误\r\n"); return 0; } - WritePumpSpeed(index, speed); + WriteJogSpeed(index, speed); index = Buff[2]; speed = Buff[3]; @@ -404,7 +448,7 @@ static uint8_t HandlePumpSpeedControl(uint8_t *Buff, uint8_t len) { printf("泵速度设置错误\r\n"); return 0; } - WritePumpSpeed(index, speed); + WriteJogSpeed(index, speed); return 1; } @@ -416,13 +460,9 @@ static uint8_t HandlePumpStepControl(uint8_t *Buff, uint8_t len) { } uint8_t index = Buff[0]; int32_t step = (Buff[1]<<24) | (Buff[2]<<16) | (Buff[3]<<8) | Buff[4]; - WritePumpStep(index, step); - - index = Buff[5]; - step = (Buff[6]<<24) | (Buff[7]<<16) | (Buff[8]<<8) | Buff[9]; - WritePumpStep(index, step); + WriteStepTarget(index, step); - return 1; + return 0; } // 软急停功能处理 @@ -438,6 +478,10 @@ static uint8_t HandleSoftStop(uint8_t *rxBuf, uint16_t rxLen) { } else { // 急停状态 + StopPump(0); + StopPump(1); + StopJogControl(0); + StopJogControl(1); updateEmergencyStop(ESTOP_PRESSED); } return 0; diff --git a/protocol.h b/protocol.h index 8631e5b..fe533b1 100644 --- a/protocol.h +++ b/protocol.h @@ -164,7 +164,7 @@ const uint8_t statusInfo[16][32]={ "初始化（步进系），伺服准备好（伺服系）", }; - +// 位置控制，点对点模式 /* 设置加速度、减速度、速度和目标位置，对应MOONS’ SCL指令如下： +----------+----------+----------+--------------+--------------+--------------+--------------------------------------------------+ | SCL指令 | 设定数值 | 单位 | 寄存器地址 | 十六进制格式 | 写入寄存器值 | 说明 | @@ -188,6 +188,47 @@ const uint8_t statusInfo[16][32]={ #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 + +// 速度控制，慢跑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 0x4002 // 状态寄存器 #define RTU_PUMP_CMD_AL 0x4001 // 告警寄存器