diff --git a/protocol.c b/protocol.c index 2f5f73e..a9522fa 100644 --- a/protocol.c +++ b/protocol.c @@ -465,7 +465,7 @@ static uint8_t SetPumpStepDec(uint8_t index, uint16_t dec) { * @return 实际速度值 */ uint16_t transSpeedPercentToSpeed(uint8_t index, uint8_t speedPercent) { - return speedPercent * dp.pump[index].speed / 100; + return speedPercent * dp.pump[index].fullSpeed / 100; } /** @@ -476,7 +476,7 @@ uint16_t transSpeedPercentToSpeed(uint8_t index, uint8_t speedPercent) { * @return 百分比速度 */ uint8_t transSpeedToSpeedPercent(uint8_t index, uint32_t speed) { - return speed * 100 / dp.pump[index].speed; + return speed * 100 / dp.pump[index].fullSpeed; } /** @@ -661,7 +661,7 @@ void ReadPumpSpeedPos(void) uint16_t speed = ReadPump2Reg(dp.pump[index].id,RTU_PUMP_CMD_SPEED); systemStatus.pumpsSpeed[index] = speed; - systemStatus.pumpsSpeedPercent[index] = (uint8_t)(speed * 100 / dp.pump[index].speed); + systemStatus.pumpsSpeedPercent[index] = (uint8_t)(speed * 100 / dp.pump[index].fullSpeed); log_d("%s speed = %d",dp.pump[index].name,speed); //判断正转、反转 @@ -706,10 +706,10 @@ uint8_t InitPump(void) { SetPumpJogAcc(index, dp.pump[index].accel); SetPumpJogDec(index, dp.pump[index].decel); - SetPumpJogSpeed(index, dp.pump[index].speed*dp.pump[index].speedPercent/100); + SetPumpJogSpeed(index, dp.pump[index].fullSpeed*dp.pump[index].speedPercent/100); SetPumpStepAcc(index, dp.pump[index].accel); SetPumpStepDec(index, dp.pump[index].decel); - SetPumpStepSpeed(index, dp.pump[index].speed*dp.pump[index].speedPercent/100); + SetPumpStepSpeed(index, dp.pump[index].fullSpeed*dp.pump[index].speedPercent/100); if (rst != systemStatus.rst) { log_e("InitPump %s failed!", dp.pump[index].name); @@ -1399,19 +1399,22 @@ static uint8_t HandleValveControl(uint8_t *Buff, uint8_t len) { log_e("三通阀控制错误"); return 1; } - // for - uint8_t index = Buff[0]; - uint8_t direction = Buff[1];//此状态位无效,目前三通阀有硬件限位,且指定角度必须为120或210 - uint16_t angle = (Buff[2]<<8) | Buff[3]; - if (angle > 360 || angle != VALVE_ANGLE_120 || angle != VALVE_ANGLE_210) { - log_e("三通阀控制错误"); - return 1; + uint8_t rst = systemStatus.rst; + for(uint8_t index = 0; index < len; index++) { + + if(memcmp(Buff+index*2, "\xFF\xFF", 2) == 0) { + continue; + } + uint16_t angle = (Buff[index*2]<<8) | Buff[index*2+1]; + ValveRunToAngle(index,angle); + + if(rst != systemStatus.rst) { + log_e("泵角度控制错误"); + return ACK_FAILED; + } } - // 具体实现 - ValveRunToAngle(index,angle); - - return 0; + return ACK_OK; } // 泵时长控制处理 @@ -1443,11 +1446,12 @@ static uint8_t HandlePumpTimeControl(uint8_t *Buff, uint8_t len) { uint16_t time = (Buff[index*3+1]<<8) | Buff[index*3+2]; if(time == 0) { // 方向控制办法待确定 + SetPumpJogSpeed(index,-1*dp.pump[index].fullSpeed*dp.pump[index].speedPercent); StartPumpJog(index); continue; } // 使用步数方式更靠谱,通过时间和速度计算步数,结束时不用发送停止命令 - int32_t step = direction*time*dp.pump[index].speed*dp.pump[index].speedPercent/100; + int32_t step = direction*time*dp.pump[index].fullSpeed*dp.pump[index].speedPercent/100; SetPumpStepTarget(index, step); StartPumpRelativeMove(index); @@ -1484,8 +1488,7 @@ static uint8_t HandlePumpSpeedControl(uint8_t *Buff, uint8_t len) { log_e("泵速度设置错误"); return ACK_FAILED; } - //更新参数 - systemStatus.ds.pumps.speed[index] = speedPercent; + // 写入指令 uint16_t speed = transSpeedPercentToSpeed(index, speedPercent); @@ -1496,6 +1499,9 @@ static uint8_t HandlePumpSpeedControl(uint8_t *Buff, uint8_t len) { log_e("泵速度设置错误"); return ACK_FAILED; } + + //更新参数 + systemStatus.ds.pumps.speed[index] = speedPercent; } return ACK_OK; diff --git a/protocol.h b/protocol.h index 07e81f2..f28bb10 100644 --- a/protocol.h +++ b/protocol.h @@ -377,7 +377,7 @@ typedef enum { // 三通阀结构体 typedef struct { - uint16_t angle[2]; // 阀门角度 (120/210) + uint16_t angle[2]; // 阀门实时角度 } ValveStatus_t; // 泵结构体 @@ -392,7 +392,7 @@ typedef struct { ValveStatus_t valves; // 两个三通阀状态 PumpStatus_t pumps; // 两个泵状态 uint8_t bubbleStatus; // 气泡状态 - uint16_t activityMeter; // 活度计mCi + float activityMeter; // 活度计uCi uint8_t estopStatus; // 急停状态 uint8_t errorCode; // 错误码 uint8_t initStatus; // 初始化状态 @@ -417,7 +417,7 @@ typedef struct typedef struct { uint8_t name[20]; uint8_t id; - uint32_t speed;//满速 + uint32_t fullSpeed;//满速 uint32_t accel; uint32_t decel; uint16_t fullCount;//电机总步数,用于根据角度估算需要移动的步数