add operation logic

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);
+    WriteStepTarget(index, step);
-
-    index = Buff[5];
-    step = (Buff[6]<<24) | (Buff[7]<<16) | (Buff[8]<<8) | Buff[9];
-    WritePumpStep(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;

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