From 000a8d7c4e2d4227bf25df1565fcac46c61e0c72 Mon Sep 17 00:00:00 2001 From: murmur Date: Sat, 4 Jan 2025 00:04:31 +0800 Subject: [PATCH] fix bugs ,update ReadPumpSpeedPos --- protocol.c | 36 +++++++++++++++++++----------------- protocol.h | 9 +++++---- 2 files changed, 24 insertions(+), 21 deletions(-) diff --git a/protocol.c b/protocol.c index 90c0537..dd66314 100644 --- a/protocol.c +++ b/protocol.c @@ -718,22 +718,24 @@ void ReadPumpSpeedPos(void) for(uint8_t index = 0; index < 2; index++) { - uint32_t pos = ReadPump2Reg(dp.pump[index].id,RTU_PUMP_CMD_POS); + int32_t pos = ReadPump2Reg(dp.pump[index].id,RTU_PUMP_CMD_POS); systemStatus.pumpsPos[index] = pos; log_d("%s pos = %d",dp.pump[index].name,pos); - uint16_t speed = ReadPump1Reg(dp.pump[index].id,RTU_PUMP_CMD_SPEED); + int16_t speed = ReadPump1Reg(dp.pump[index].id,RTU_PUMP_CMD_SPEED); systemStatus.pumpsSpeed[index] = speed; - systemStatus.pumpsSpeedPercent[index] = (uint8_t)(speed * 100 / dp.pump[index].fullSpeed); + systemStatus.pumpsSpeedPercent[index] = (uint8_t)(abs(speed) *100 / PUMP_SPEED_RPS / dp.pump[index].fullSpeed); log_d("%s speed = %d",dp.pump[index].name,speed); //判断正转、反转 - if(speed > 0) { - if(ReadPump2Reg(dp.pump[index].id,RTU_PUMP_CMD_POS) > pos) - systemStatus.ds->pumps.status[index] = PUMP_STATUS_CLOCKWISE; - else - systemStatus.ds->pumps.status[index] = PUMP_STATUS_ANTICLOCKWISE; - } else { + if(pos > 0 || speed > 0) { + systemStatus.ds->pumps.status[index] = PUMP_STATUS_CLOCKWISE; + } + if(pos < 0 || speed < 0) { + systemStatus.ds->pumps.status[index] = PUMP_STATUS_ANTICLOCKWISE; + } + if (!speed) + { systemStatus.ds->pumps.status[index] = PUMP_STATUS_STOP; } } @@ -1362,7 +1364,7 @@ void updateVPInfo(void) // 获取回归状态 // 获取泵实时速度、位置 - // ReadPumpSpeedPos(); + ReadPumpSpeedPos(); // 获取阀门实时速度、位置 // if(systemStatus.ds->initStatus == INIT_IN_PROGRESS) { @@ -1401,7 +1403,6 @@ void dumpSystemStatus(void) void updateSystemStatus(void) { updateVPInfo(); - ReadPumpSpeedPos(); updatePumpStatus(); updateValveStatus(); dumpSystemStatus(); @@ -1420,7 +1421,7 @@ void initCTLSystem(void) for (size_t index = 0; index < 2; index++) { //更新参数 - systemStatus.pumpsSpeed[index] = transSpeedPercentToSpeed(index, systemStatus.ds->pumps.speedPercent[index]); + systemStatus.pumpsCfgSpeed[index] = transSpeedPercentToSpeed(index, systemStatus.ds->pumps.speedPercent[index]); } InitValve(); InitPump(); @@ -1597,11 +1598,12 @@ static uint8_t HandlePumpTimeControl(uint8_t *Buff, uint8_t len) { } else { // 使用步数方式更靠谱,通过时间和速度计算步数,结束时不用发送停止命令 - uint16_t cnt = ReadPumpCNT(index);//10000 + uint16_t cnt = ReadPumpCNT(index)*2;//10000 log_d("cnt=%d",cnt); - int32_t step = direction*systemStatus.pumpsSpeed[index]/PUMP_SPEED_RPS*cnt*time; + int64_t step = ((int64_t)direction * systemStatus.pumpsCfgSpeed[index] * time * cnt) / PUMP_SPEED_RPS; + log_d("step=%d,",step); StopPump(index);//stop first - SetPumpStepTarget(index, step); + SetPumpStepTarget(index, (int32_t)step); StartPumpRelativeMove(index); } @@ -1655,7 +1657,7 @@ static uint8_t HandlePumpSpeedControl(uint8_t *Buff, uint8_t len) { } //更新参数 - systemStatus.pumpsSpeed[index] = speed; + systemStatus.pumpsCfgSpeed[index] = speed; systemStatus.ds->pumps.speedPercent[index] = speedPercent; } @@ -1678,7 +1680,7 @@ static uint8_t HandlePumpStepControl(uint8_t *Buff, uint8_t len) { continue; } uint8_t rst = systemStatus.rst; - int32_t step = (Buff[index*4+1]<<24) | (Buff[index*4+2]<<16) | (Buff[index*4+3]<<8) | Buff[index*4+4]; + int32_t step = (Buff[index*4]<<24) | (Buff[index*4+1]<<16) | (Buff[index*4+2]<<8) | Buff[index*4+3]; log_d("泵步进设置: %d[%d], %d",index, dp.pump[index].id, step); SetPumpStepTarget(index, step); StartPumpRelativeMove(index); diff --git a/protocol.h b/protocol.h index a2abbf6..f7a3218 100644 --- a/protocol.h +++ b/protocol.h @@ -218,7 +218,7 @@ static const uint8_t statusInfo[16][60]={ #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 +#define RTU_PUMP_CMD_POS 0x0013 //相对位置,40013-40014 // 速度控制,慢跑Jogging模式 /* @@ -408,9 +408,10 @@ typedef struct int32_t valvesSpeed[2];//实时速度 uint8_t valvesSpeedPercent[2];//实时速度百分比 uint32_t valvesPos[2];//实时位置,实时角度已有,在DeviceStatus_t - uint32_t pumpsSpeed[2];//设定速度 + int32_t pumpsCfgSpeed[2];//设定速度 + int32_t pumpsSpeed[2];//实时速度 uint8_t pumpsSpeedPercent[2];//实时速度百分比 - uint32_t pumpsPos[2];//实时位置 + int32_t pumpsPos[2];//实时位置 uint8_t isValveMovingBackToOrigin[2];//阀门是否在回归原点 uint8_t isBTOOk[2];//回归原点是否完成 uint16_t rst;//RTU命令执行结果 @@ -421,7 +422,7 @@ typedef struct typedef struct { uint8_t name[20]; uint8_t id; - uint32_t fullSpeed;//满速 + uint8_t fullSpeed;//满速,max=60 uint32_t accel; uint32_t decel; uint16_t fullCount;//电机总步数,用于根据角度估算需要移动的步数