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

fix bugs ,update ReadPumpSpeedPos

Browse Source
This commit is contained in:
murmur 2025-01-04 00:04:31 +08:00
parent ee58e1e28f
commit 000a8d7c4e
2 changed files with 24 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

36
protocol.c
View File

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

9
protocol.h
View File

@ -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;//电机总步数，用于根据角度估算需要移动的步数