From 72fd77581d4c14db8d42cb3ad05696471ee41347 Mon Sep 17 00:00:00 2001
From: murmur <test@test.test>
Date: Fri, 13 Dec 2024 17:56:20 +0800
Subject: [PATCH] bug fix

---
 protocol.c | 43 +++++++++++++++++++++++++------------------
 1 file changed, 25 insertions(+), 18 deletions(-)

diff --git a/protocol.c b/protocol.c
index 2d44d8d..6fdad7d 100644
--- a/protocol.c
+++ b/protocol.c
@@ -421,24 +421,24 @@ static uint8_t SetPumpStepTarget(uint8_t index, uint32_t target) {
 }
 
 /**
- * 执行相对位置控制，目前仅用到相对移动
+ * 执行相对位置移动，目前协议仅用到相对移动
  * 写入FL(feed length)命令到操作码寄存器
  * 
  * @param index 泵索引
  * @return 0:成功 其他:失败
  */
-static uint8_t PumpRelativePositionControl(uint8_t index) {
+static uint8_t StartPumpRelativeMove(uint8_t index) {
     WritePump1Reg(index, RTU_PUMP_CMD_CO, 0x0066);
 }
 
 /**
- * 执行绝对位置控制，目前无使用
+ * 执行绝对位置移动，目前无使用
  * 写入FP(feed position)命令到操作码寄存器
  * 
  * @param index 泵索引
  * @return 0:成功 其他:失败
  */
-static uint8_t PumpAbsolutePositionControl(uint8_t index) {
+static uint8_t StartPumpAbsoluteMove(uint8_t index) {
     WritePump1Reg(index, RTU_PUMP_CMD_CO, 0x0067);
 }
 
@@ -585,19 +585,19 @@ void UpdatePumpStatus() {
 uint8_t InitPump(void) {
     // 初始化泵
     log_e("InitPump");
-    WriteJogAcc(dp.pump[0].id, dp.pump[0].maxAccel);
-    WriteJogDec(dp.pump[0].id, dp.pump[0].maxDecel);
-    WriteJogSpeed(dp.pump[0].id, dp.pump[0].maxSpeed);
-    WriteStepAcc(dp.pump[0].id, dp.pump[0].maxAccel);
-    WriteStepDec(dp.pump[0].id, dp.pump[0].maxDecel);
-    WriteStepSpeed(dp.pump[0].id, dp.pump[0].maxSpeed);
+    SetPumpJogAcc(dp.pump[0].id, dp.pump[0].maxAccel);
+    SetPumpJogDec(dp.pump[0].id, dp.pump[0].maxDecel);
+    SetPumpJogSpeed(dp.pump[0].id, dp.pump[0].maxSpeed);
+    SetPumpStepAcc(dp.pump[0].id, dp.pump[0].maxAccel);
+    SetPumpStepDec(dp.pump[0].id, dp.pump[0].maxDecel);
+    SetPumpStepSpeed(dp.pump[0].id, dp.pump[0].maxSpeed);
 
-    WriteJogAcc(dp.pump[1].id, dp.pump[1].maxAccel);
-    WriteJogDec(dp.pump[1].id, dp.pump[1].maxDecel);
-    WriteJogSpeed(dp.pump[1].id, dp.pump[1].maxSpeed);
-    WriteStepAcc(dp.pump[1].id, dp.pump[1].maxAccel);
-    WriteStepDec(dp.pump[1].id, dp.pump[1].maxDecel);
-    WriteStepSpeed(dp.pump[1].id, dp.pump[1].maxSpeed);
+    SetPumpJogAcc(dp.pump[1].id, dp.pump[1].maxAccel);
+    SetPumpJogDec(dp.pump[1].id, dp.pump[1].maxDecel);
+    SetPumpJogSpeed(dp.pump[1].id, dp.pump[1].maxSpeed);
+    SetPumpStepAcc(dp.pump[1].id, dp.pump[1].maxAccel);
+    SetPumpStepDec(dp.pump[1].id, dp.pump[1].maxDecel);
+    SetPumpStepSpeed(dp.pump[1].id, dp.pump[1].maxSpeed);
 }
 
 
@@ -996,6 +996,9 @@ static uint8_t HandlePumpSpeedControl(uint8_t *Buff, uint8_t len) {
         return 0;
     }    
     SetPumpJogSpeed(index, speed);
+    SetPumpStepSpeed(index, speed);
+    updatePumpSpeedStatus(index, speed);
+    dp.pump[index].maxSpeed = speed;
 
     index = Buff[2];
     speed = Buff[3];
@@ -1004,6 +1007,9 @@ static uint8_t HandlePumpSpeedControl(uint8_t *Buff, uint8_t len) {
         return 0;
     }
     SetPumpJogSpeed(index, speed);
+    SetPumpStepSpeed(index, speed);
+    updatePumpSpeedStatus(index, speed);
+    dp.pump[index].maxSpeed = speed;
 
     return 1;
 }
@@ -1024,6 +1030,7 @@ 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];
     SetPumpStepTarget(index, step);
+    StartPumpRelativeMove(index);
     
     return 0;
 }
@@ -1049,8 +1056,8 @@ static uint8_t HandleSoftStop(uint8_t *rxBuf, uint16_t rxLen) {
 // 急停状态
         StopPump(0);
         StopPump(1);
-        StopPumpJog(0);
-        StopPumpJog(1);
+        // StopPumpJog(0);
+        // StopPumpJog(1);
         updateEmergencyStop(ESTOP_PRESSED);
     }
     return 0;