murmur/injectorCTL
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

add operation logic

Browse Source
This commit is contained in:
CSSC-WORK\murmur 2024-12-11 15:55:40 +08:00
parent a71606de2e
commit d391743361
2 changed files with 175 additions and 90 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

222
protocol.c
View File

@ -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;

43
protocol.h
View File

@ -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 // 告警寄存器