From 13781723e047b8df30eb4f459fd304ef21f042b2 Mon Sep 17 00:00:00 2001 From: "CSSC-WORK\\murmur" Date: Thu, 26 Dec 2024 22:27:14 +0800 Subject: [PATCH] A lot of logic has been updated for the v1.6 protocol --- .gitignore | 1 + protocol.c | 839 +++++++++++++++++++++++++++++------------------------ protocol.h | 45 +-- 3 files changed, 485 insertions(+), 400 deletions(-) diff --git a/.gitignore b/.gitignore index a3062be..e4db7fd 100644 --- a/.gitignore +++ b/.gitignore @@ -1 +1,2 @@ .vscode/* +tasks.json diff --git a/protocol.c b/protocol.c index 47edb1a..3826852 100644 --- a/protocol.c +++ b/protocol.c @@ -18,16 +18,21 @@ void elog_hexdump(const char *name, uint8_t width, const void *buf, uint16_t siz DeviceStatus_t deviceStatus = { .sensorStatus = 1, - .valves = {210, 120}, - .pumps = {0, 0, 50, 50}, + .valves = {{210, 120}}, + .pumps = {{0, 0},{50, 50}}, .bubbleStatus = 0, - .stopStatus = 0, + .activityMeter = 0, + .estopStatus = 0, .errorCode = 0, .initStatus = 1 }; SystemStatus_t systemStatus = { - .speed = {0}, - .pos = {0}, + .valvesSpeed = {0}, + .valvesPos = {0}, + .valvesSpeedPercent = {0}, + .pumpsSpeed = {0}, + .pumpsPos = {0}, + .pumpsSpeedPercent = {0}, .rst = 0 }; @@ -107,9 +112,9 @@ uint16_t CalculateCRC16(uint8_t *data, uint16_t length) { * * @param status 设备状态值(在线/离线) */ -void updateDeviceStatus(SensorStatus status) { - deviceStatus.sensorStatus = status; -} +// void updateDeviceStatus(SensorStatus status) { +// deviceStatus.sensorStatus = status; +// } /** * 更新三通阀状态 @@ -117,27 +122,13 @@ void updateDeviceStatus(SensorStatus status) { * @param index 阀门索引(1或2) * @param angle 阀门角度值(0-360，绝对角度) */ -void updateValveStatus(uint8_t index, ValveAngle angle) { - if (index == 1) { - deviceStatus.valves.angle1 = angle; - } else if (index == 2) { - deviceStatus.valves.angle2 = angle; - } -} - -/** - * 更新泵运行状态 - * - * @param index 泵索引(1或2) - * @param status 泵状态值(停止/顺时针/逆时针) - */ -void updatePumpStatus(uint8_t index, PumpStatus status) { - if (index == 1) { - deviceStatus.pumps.status1 = status; - } else if (index == 2) { - deviceStatus.pumps.status2 = status; - } -} +// void updateValveStatus(uint8_t index, ValveAngle angle) { +// if (index == 1) { +// deviceStatus.valves.angle1 = angle; +// } else if (index == 2) { +// deviceStatus.valves.angle2 = angle; +// } +// } /** * 更新泵速度状态 @@ -145,49 +136,49 @@ void updatePumpStatus(uint8_t index, PumpStatus status) { * @param index 泵索引(1或2) * @param speed 泵速度值(0-100%) */ -void updatePumpSpeedStatus(uint8_t index, uint8_t speed) { - if (index == 1) { - deviceStatus.pumps.speed1 = speed; - } else if (index == 2) { - deviceStatus.pumps.speed2 = speed; - } -} +// void updatePumpSpeedStatus(uint8_t index, uint8_t speed) { +// if (index == 1) { +// deviceStatus.pumps.speed1 = speed; +// } else if (index == 2) { +// deviceStatus.pumps.speed2 = speed; +// } +// } /** * 更新气泡传感器状态 * * @param value 气泡传感器状态值(有/无气泡) */ -void updateBubbleSensor(BubbleStatus value) { - deviceStatus.bubbleStatus = value; -} +// void updateBubbleSensor(BubbleStatus value) { +// deviceStatus.bubbleStatus = value; +// } /** * 更新急停状态 * * @param status 急停状态值(正常/按下) */ -void updateEmergencyStop(EstopStatus_t status) { - deviceStatus.stopStatus = status; -} +// void updateEmergencyStop(EstopStatus_t status) { +// deviceStatus.stopStatus = status; +// } /** * 更新错误码 * * @param errorCode 错误码值 */ -void updateErrorCode(ErrorCode_t errorCode) { - deviceStatus.errorCode = errorCode; -} +// void updateErrorCode(ErrorCode_t errorCode) { +// deviceStatus.errorCode = errorCode; +// } /** * 更新初始化状态 * * @param status 初始化状态值(进行中/成功/失败) */ -void updateInitStatus(InitStatus_t status) { - deviceStatus.initStatus = status; -} +// void updateInitStatus(InitStatus_t status) { +// deviceStatus.initStatus = status; +// } @@ -208,7 +199,7 @@ static uint8_t writeCMD(uint8_t *txBuf, uint16_t txLen) { transDataToMotorValve(txBuf, txLen); uint8_t rxBuf[30] = {0}; uint16_t rxLen = txLen; - uint8_t hal_rst = readDataFromMotorValve(rxBuf, rxLen, READ_ACK_TIMEOUT); + readDataFromMotorValve(rxBuf, rxLen, READ_ACK_TIMEOUT); if(memcmp(rxBuf, txBuf, 2) != 0) {//正常情况下，返回的前2个字节应与发送的相同 log_e("writeCMD error!"); elog_hexdump("writeCMD error", 16, rxBuf, rxLen); @@ -296,7 +287,7 @@ uint16_t ReadPump1Reg(uint8_t id, uint16_t reg) { writeCMD(data, 8); uint8_t rxBuf[30] = {0}; uint16_t rxLen = 8; - uint8_t rst = HAL_UART_Receive(&huart2, rxBuf, rxLen, READ_ACK_TIMEOUT); + readDataFromMotorValve(rxBuf, rxLen, READ_ACK_TIMEOUT); return rxBuf[3]<<8|rxBuf[4]; } @@ -320,7 +311,7 @@ uint32_t ReadPump2Reg(uint8_t id, uint16_t reg) { writeCMD(data, 12); uint8_t rxBuf[30] = {0}; uint16_t rxLen = 12; - uint8_t rst = HAL_UART_Receive(&huart2, rxBuf, rxLen, READ_ACK_TIMEOUT*2); + readDataFromMotorValve(rxBuf, rxLen, READ_ACK_TIMEOUT*2); // 逻辑需进一步完善 return rxBuf[6]<<24|rxBuf[7]<<16|rxBuf[8]<<8|rxBuf[9]; } @@ -464,6 +455,29 @@ static uint8_t SetPumpStepAcc(uint8_t index, uint16_t acc) { static uint8_t SetPumpStepDec(uint8_t index, uint16_t dec) { return WritePump1Reg(dp.pump[index].id, RTU_PUMP_CMD_DE, dec*PUMP_DECEL_RPS); } + +/** + * 将百分比速度转换为实际速度值 + * + * @param index 泵索引 + * @param speedPercent 百分比速度 + * @return 实际速度值 + */ +uint16_t transSpeedPercentToSpeed(uint8_t index, uint8_t speedPercent) { + return speedPercent * dp.pump[index].speed / 100; +} + +/** + * 将实际速度转换为百分比速度 + * + * @param index 泵索引 + * @param speed 实际速度 + * @return 百分比速度 + */ +uint8_t transSpeedToSpeedPercent(uint8_t index, uint32_t speed) { + return speed * 100 / dp.pump[index].speed; +} + /** * 设置泵的步进速度 * 将百分比速度转换为实际速度值 @@ -580,14 +594,14 @@ void DecodePumpAlarmMsg(uint16_t reg4001) { // 与上次告警信息相同，则不更新，仅打印一次 if (alarmCode.all == reg4001) { - return 0; + return; } alarmCode.all = reg4001; if(alarmCode.all == 0) { //暂时屏蔽，避免刷屏 // printf("\r\n%s无报警信息\r\n", pumpName[index]); - return 0; + return; } // 打印表格头部 @@ -601,7 +615,7 @@ void DecodePumpAlarmMsg(uint16_t reg4001) { printf("+--------+------------------+\r\n"); } } - return 1; + return; } /** @@ -631,26 +645,50 @@ void DecodePumpStatusMsg(uint16_t reg4002) { return 0; } - -uint32_t readPumpPos(uint8_t index) +/** + * 读取泵的实时速度和位置 + * + */ +void ReadPumpSpeedPos(void) { - uint16_t speed = ReadPump2Reg(dp.pump[index].id,RTU_PUMP_CMD_SPEED); - log_d("speed[40001] = %d",speed); - uint32_t pos = ReadPump2Reg(dp.pump[index].id,RTU_PUMP_CMD_POS); - log_d("pos[] = %d",pos); + for(uint8_t index = 0; index < 2; index++) { + + + uint32_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 = ReadPump2Reg(dp.pump[index].id,RTU_PUMP_CMD_SPEED); + systemStatus.pumpsSpeed[index] = speed; + systemStatus.pumpsSpeedPercent[index] = (uint8_t)(speed * 100 / dp.pump[index].speed); + 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 { + systemStatus.ds.pumps.status[index] = PUMP_STATUS_STOP; + } + } } /** - * 定时更新泵的状态 - * 读取泵的运行状态和告警信息 + * 更新泵的故障状态和运动状态，在轮询中调用 + * */ -void UpdatePumpStatus() { +void updatePumpStatus(void) { // 更新设备状态 - ReadPumpStatus(0); - ReadPumpStatus(1); - ReadPumpAlarm(0); - ReadPumpAlarm(1); + for(uint8_t index = 0; index < 2; index++) { + uint16_t reg4001 = ReadPumpAlarm(index); + DecodePumpAlarmMsg(reg4001); + uint16_t reg4002 = ReadPumpStatus(index); + DecodePumpStatusMsg(reg4002); + } + } /** @@ -662,43 +700,24 @@ void UpdatePumpStatus() { uint8_t InitPump(void) { // 初始化泵 log_i("InitPump"); - uint8_t rst = systemStatus.rst; + for(uint8_t index = 0; index < 2; index++) { + uint8_t rst = systemStatus.rst; - SetPumpJogAcc(0, dp.pump[0].maxAccel); - SetPumpJogDec(0, dp.pump[0].maxDecel); - SetPumpJogSpeed(0, dp.pump[0].maxSpeed); - SetPumpStepAcc(0, dp.pump[0].maxAccel); - SetPumpStepDec(0, dp.pump[0].maxDecel); - SetPumpStepSpeed(0, dp.pump[0].maxSpeed); - if (rst != systemStatus.rst) - { - log_e("InitPump[0] failed!"); - systemStatus.ds.initStatus = INIT_FAILED; + SetPumpJogAcc(index, dp.pump[index].accel); + SetPumpJogDec(index, dp.pump[index].decel); + SetPumpJogSpeed(index, dp.pump[index].speed*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); + if (rst != systemStatus.rst) + { + log_e("InitPump %s failed!", dp.pump[index].name); + systemStatus.ds.initStatus = INIT_FAILED; + return 1; + } } - SetPumpJogAcc(1, dp.pump[1].maxAccel); - SetPumpJogDec(1, dp.pump[1].maxDecel); - SetPumpJogSpeed(1, dp.pump[1].maxSpeed); - SetPumpStepAcc(1, dp.pump[1].maxAccel); - SetPumpStepDec(1, dp.pump[1].maxDecel); - SetPumpStepSpeed(1, dp.pump[1].maxSpeed); - if (rst != systemStatus.rst) - { - log_e("InitPump[1] failed!"); - systemStatus.ds.initStatus = INIT_FAILED; - } - - uint16_t reg4001 = ReadPumpAlarm(0); - DecodePumpAlarmMsg(reg4001); - uint16_t reg4002 = ReadPumpStatus(0); - DecodePumpStatusMsg(reg4002); - - reg4001 = ReadPumpAlarm(1); - DecodePumpAlarmMsg(reg4001); - reg4002 = ReadPumpStatus(1); - DecodePumpStatusMsg(reg4002); - - return rst; + return 0; } @@ -952,39 +971,15 @@ static uint32_t ReadValvePos(uint8_t index) { } /** - * 检查原点回归结果 - * @param index - * @return 0:成功 1:运动中 2:失败 + * 读取阀门速度 + * + * @param index 阀门索引 + * @return 速度值(用户单位/s) */ -static uint8_t valveCheckBTOResult(uint8_t index, uint32_t startTime) -{ - if (HAL_GetTick() - startTime > 1000*20) { - // 超时=失败 - return 2; - } - - uint16_t rst = ReadValve1InputReg(dp.valve[index].id,RTU_VALVE_CMD_SC); - // 如果原点回归完成，状态字第12位会从0变为1， - // 如果原点回归失败，状态字第13位会从0变为1。 - // 此外也可以附加判断电机当前位置是否在0附近的200个脉冲以内。 - if(rst & 0x0001<<12) { - // 成功 - uint32_t pos = ReadValvePos(index); - if(pos > 200 || pos < (VALVE_PULSE_PER_ROUND-200)) { - // 位置超出范围 - return 2; - } - return 0; - } - if(rst & 0x0001<<13) { - // 失败 - return 2; - } - - // 运动中，未超时 - return 1; - +static uint32_t ReadValveSpeed(uint8_t index) { + return ReadValve2InputReg(dp.valve[index].id, RTU_VALVE_CMD_SPEED); } + /** * 阀门回归原点控制 * 包含设置原点回归方式、堵转检测、运行模式等配置 @@ -1036,6 +1031,55 @@ void ValveBackToOrigin(uint8_t index,int8_t direction) { // 不同在于负堵转时，目标位置10000为正0x2710，正堵转时，目标位置为负0xffffd8f0 } + +/** + * 检查原点回归结果 + * @param index + * @return 0:成功 1:运动中 2:失败 + */ +static void valveCheckBTOResult(uint8_t index) +{ + static uint8_t retryCnt = 0; + uint8_t isSuccess = 0; + uint16_t rst = ReadValve1InputReg(dp.valve[index].id,RTU_VALVE_CMD_SC); + // 如果原点回归完成，状态字第12位会从0变为1， + // 如果原点回归失败，状态字第13位会从0变为1。 + // 此外也可以附加判断电机当前位置是否在0附近的200个脉冲以内。 + if(rst & 0x0001<<12) { + // 成功 + uint32_t pos = ReadValvePos(index); + if(pos > 200 || pos < (VALVE_PULSE_PER_ROUND-200)) { + // 位置超出范围 + isSuccess = 0; + } + else { + isSuccess = 1; + } + } + if(rst & 0x0001<<13) { + // 失败 + isSuccess = 0; + } + + if (isSuccess) + { + retryCnt = 0; + systemStatus.ds.initStatus = INIT_SUCCESS; + return; + } + else { + retryCnt++; + if(retryCnt > 2) {//执行两次回归，都失败则认为初始化失败 + systemStatus.ds.initStatus = INIT_FAILED; + retryCnt = 0; + return; + } + ValveBackToOrigin(index, -1); + } + + +} + /** * 阀门运行初始化 * 配置通信模式、运行模式、速度和加减速等参数 @@ -1112,73 +1156,72 @@ uint8_t InitValve(void) { // ValvePPInit(1); } +void ReadValveSpeedPos(void) +{ + for(uint8_t index = 0; index < 2; index++) { + systemStatus.valvesSpeed[index] = ReadValveSpeed(index); + // systemStatus.valvesSpeedPercent[index] = transSpeedToSpeedPercent(index, systemStatus.valvesSpeed[index]); + systemStatus.valvesPos[index] = ReadValvePos(index); + systemStatus.ds.valves.angle[index] = systemStatus.valvesPos[index]*360/dp.valve[index].fullCount; + } +} + +/** + * 更新阀门状态，包括回归状态、运行状态和告警，在轮询中调用 + */ +void updateValveStatus(void) +{ + valveCheckBTOResult(0); + valveCheckBTOResult(1); + + //alarm + +} + +/** + * 停止阀门 + * + * @param index 阀门索引 + */ +void stopValve(uint8_t index) { + SetValveFunc(index, RTU_VALVE_CFG_DISABLE); + SetValveFunc(index, RTU_VALVE_CFG_ENABLE); +} + /** * 初始化设备状态 * 设置设备在线状态、阀门角度、泵运行状态等 */ -void InitDeviceStatus() { - // 初始化泵 +// void InitDeviceStatus() { +// // 初始化泵 - // 更新设备状态 - updateDeviceStatus(SENSOR_ONLINE); - updateValveStatus(1, 120); - updateValveStatus(2, 210); - updatePumpStatus(1, PUMP_CLOCKWISE); - updatePumpStatus(2, PUMP_ANTICLOCKWISE); - updatePumpSpeedStatus(1, 100); - updatePumpSpeedStatus(2, 100); - updateBubbleSensor(BUBBLE_DETECTED); - updateEmergencyStop(ESTOP_NORMAL); - updateInitStatus(INIT_SUCCESS); -} +// // 更新设备状态 +// updateDeviceStatus(SENSOR_ONLINE); +// updateValveStatus(1, 120); +// updateValveStatus(2, 210); +// updatePumpStatus(1, PUMP_CLOCKWISE); +// updatePumpStatus(2, PUMP_ANTICLOCKWISE); +// updatePumpSpeedStatus(1, 100); +// updatePumpSpeedStatus(2, 100); +// updateBubbleSensor(BUBBLE_DETECTED); +// updateEmergencyStop(ESTOP_NORMAL); +// updateInitStatus(INIT_SUCCESS); +// } /** - * 更新valve和pump的状态 + * 更新valve和pump的信息，如位置、角度、速度等 + * 在轮询中调用 */ -void updateVPStatus(uint8_t index) +void updateVPInfo(void) { - static uint32_t st[2] = {0, 0}; // Initialize with constants - if (isValveMovingBackToOrigin[index]) - { - if (st[index] == 0) { // First time initialization - st[index] = HAL_GetTick(); - } - uint8_t rst = valveCheckBTOResult(index, st[index]); - switch (rst) - { - case 0: - log_i("BTO success!delta=%d",HAL_GetTick()-st[index]); - updateInitStatus(INIT_SUCCESS); - break; - case 1: - log_d("BTO moving..."); - updateInitStatus(INIT_IN_PROGRESS); - break; - case 2: - log_e("BTO failure!!!"); - updateInitStatus(INIT_FAILED); - default: - break; - } - } - else - { - log_d("NOT in BTO."); - st[index] = HAL_GetTick(); - } + // 获取回归状态 - // 获取实时角度 - readPumpPos(index); - uint32_t pos = ReadValve2InputReg(index,RTU_VALVE_CMD_POS); - log_d("pos[%d]=%d",index,pos); - systemStatus.pos[index] = pos; + // 获取泵实时速度、位置 + ReadPumpSpeedPos(); - - // 获取运动状态 - uint32_t speed = ReadValve2InputReg(index,RTU_VALVE_CMD_SPEED); - log_d("speed[%d]=",index,speed); - systemStatus.speed[index] = speed; + // 获取阀门实时速度、位置 + ReadValveSpeedPos(); } @@ -1188,12 +1231,13 @@ void updateVPStatus(uint8_t index) */ void updateSystemStatus(void) { - updateVPStatus(0); - updateVPStatus(1); + updateVPInfo(); + updatePumpStatus(); + updateValveStatus(); } /** - * 初始化系统，初始化阀门和泵的默认参数， + * 初始化控制系统，初始化阀门和泵的默认参数， * 有别于 HOST_CMD_SYSTEM_INIT=0x0007 指令对应的初始化功能 */ void initCTLSystem(void) @@ -1205,193 +1249,14 @@ void initCTLSystem(void) InitPump(); } -// 初始化处理 -static uint8_t HandleInit(void) { - // 实现初始化逻辑 - // 1.更新状态为“初始化中” - // 2.执行默认的初始化内容，此步骤系统上电后会自动执行 - // 3.执行协议初始化流程 - // 4.检查初始化结果，更新状态“成功”或“失败” - - // 3.协议要求内容为：2个三通阀步进电机堵转找原点，重复至少2次，然后各自转至120°。 - - systemStatus.ds.initStatus = INIT_IN_PROGRESS; - systemStatus.rst = 0; - - initCTLSystem(); - - ValveBackToOrigin(0,-1); - ValveRunToAngle(0,60); - ValveBackToOrigin(0,-1); - ValveRunToAngle(0,120); - - ValveBackToOrigin(1,-1); - ValveRunToAngle(1,60); - ValveBackToOrigin(1,-1); - ValveRunToAngle(1,120); - return systemStatus.rst; -} - - -// 状态查询处理 -/** - * 处理状态查询命令 - * - * @return 0:成功 其他:失败 - */ -static uint8_t HandleStatusQuery(void) { - - packMsgToHost(HOST_CMD_STATUS_QUERY, 0); - return 0; -} - -// 三通阀控制处理 -/** - * 处理三通阀控制命令 - * - * @param Buff 接收到的数据缓冲区 - * @param len 接收到的数据长度 - * @return 0:成功 其他:失败 - */ -static uint8_t HandleValveControl(uint8_t *Buff, uint8_t len) { - // 实现三通阀控制逻辑 - - if(len != 8) { - log_e("三通阀控制错误"); - return 0; - } - uint8_t index = Buff[0]; - uint8_t direction = Buff[1];//此状态位无效，目前三通阀有硬件限位，且指定角度必须为120或210 - uint16_t angle = (Buff[2]<<8) | Buff[3]; - if(angle > 360) { - log_e("三通阀控制错误"); - return 1; - } - if (angle != VALVE_ANGLE_120 && angle != VALVE_ANGLE_210) { - log_e("三通阀控制错误"); - return 1; - } - - // 具体实现 - ValveRunToAngle(index,angle); - - // 更新三通阀状态 - updateValveStatus(index, angle); - return 0; -} - -// 泵时长控制处理 -/** - * 处理泵时长控制命令 - * - * @param Buff 接收��的数据缓冲区 - * @param len 接收到的数据长度 - * @return 0:成功 其他:失败 - */ -static uint8_t HandlePumpTimeControl(uint8_t *Buff, uint8_t len) { - // 实现泵时长控制逻辑 - - // 使用步数方式更靠谱，通过时间和速度计算步数，结束时不用发送停止命令 - - return 1; -} - -// 泵速度设置处理 -/** - * 处理泵速度设置命令 - * - * @param Buff 接收到的数据缓冲区 - * @param len 接收到的数据长度 - * @return 0:成功 其他:失败 - */ -static uint8_t HandlePumpSpeedControl(uint8_t *Buff, uint8_t len) { - // 实现速度设置逻辑 - if(len != 4) { - log_e("泵速度设置错误"); - return 0; - } - uint8_t index = Buff[0]; - uint16_t speed = Buff[1]; - if (speed > 100) { - log_e("泵速度设置错误"); - return 0; - } - SetPumpJogSpeed(index, speed); - SetPumpStepSpeed(index, speed); - updatePumpSpeedStatus(index, speed); - dp.pump[index].maxSpeed = speed; - - index = Buff[2]; - speed = Buff[3]; - if (speed > 100) { - log_e("泵速度设置错误"); - return 0; - } - SetPumpJogSpeed(index, speed); - SetPumpStepSpeed(index, speed); - updatePumpSpeedStatus(index, speed); - dp.pump[index].maxSpeed = speed; - - return 1; -} - -// 泵步进控制处理 -/** - * 处理泵步进控制命令 - * - * @param Buff 接收到的数据缓冲区 - * @param len 接收到的数据长度 - * @return 0:成功 其他:失败 - */ -static uint8_t HandlePumpStepControl(uint8_t *Buff, uint8_t len) { - if(len != 10) { - log_e("泵步进设置错误"); - return 0; - } - 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; -} - -/** - * 处理软急停命令 - * - * @param rxBuf 接收到的数据缓冲区 - * @param rxLen 接收到的数据长度 - * @return 0:成功 1:失败 - */ -static uint8_t HandleSoftStop(uint8_t *rxBuf, uint16_t rxLen) { - if(rxLen != 1) { - log_e("软急停设置错误"); - return 1; - } - // 实现软急停功能逻辑 - if(rxBuf[0] == 0) { -// 正常状态 - updateEmergencyStop(ESTOP_NORMAL); - } - else { -// 急停状态 - StopPump(0); - StopPump(1); - // StopPumpJog(0); - // StopPumpJog(1); - updateEmergencyStop(ESTOP_PRESSED); - } - return 0; -} /** * 将消息打包并发送给上位机 * 帧格式:帧头+功能码(2Byte)+数据长度(1Byte)+具体数据(NByte)+CRC16校验位+帧尾 * * @param funcCode 功能码 - * @param isOK 执行结果，1成功，0失败 */ -void packMsgToHost(uint16_t funcCode, uint8_t isOK) { +static void packMsgToHost(uint16_t funcCode, uint8_t isOK) { // 实现打包消息到上位机逻辑 // 帧头+功能码(2Byte)+数据长度(1Byte)+ 具体数据(NByte)+CRC16校验位+帧尾 uint8_t msgBuf[64];//最大为4+2+1+15+2+4=28 @@ -1431,6 +1296,224 @@ void packMsgToHost(uint16_t funcCode, uint8_t isOK) { sendMsgToHost(msgBuf, len); } +// 初始化处理 +static uint8_t HandleInit(void) { + // 实现初始化逻辑 + // 1.更新状态为“初始化中” + // 2.执行默认的初始化内容，此步骤系统上电后会自动执行 + // 3.执行协议初始化流程 + // 4.检查初始化结果，更新状态“成功”或“失败” + + // 3.协议要求内容为：2个三通阀步进电机堵转找原点，重复至少2次，然后各自转至120°。 + + systemStatus.ds.initStatus = INIT_IN_PROGRESS; + systemStatus.rst = 0; + + initCTLSystem(); + + ValveBackToOrigin(0,-1); + ValveBackToOrigin(1,-1); + + if(systemStatus.rst != 0) { + log_e("系统初始化失败"); + systemStatus.ds.initStatus = INIT_FAILED; + return ACK_FAILED; + } + return ACK_OK; +} + + +// 状态查询处理 +/** + * 处理状态查询命令 + * + * @return 0:成功 其他:失败 + */ +static void HandleStatusQuery(void) { + packMsgToHost(HOST_CMD_STATUS_QUERY, ACK_OK); +} + +// 三通阀控制处理 +/** + * 处理三通阀控制命令，正反转、目标角度 + * + * @param Buff 接收到的数据缓冲区 + * @param len 接收到的数据长度 + * @return 0:成功 其他:失败 + */ +static uint8_t HandleValveControl(uint8_t *Buff, uint8_t len) { + // 实现三通阀控制逻辑 + + if(len != 8) { + 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; + } + + // 具体实现 + ValveRunToAngle(index,angle); + + return 0; +} + +// 泵时长控制处理 +/** + * 处理泵时长控制命令 + * + * @param Buff 接收到的数据缓冲区 + * @param len 接收到的数据长度 + * @return 0:成功 其他:失败 + */ +static uint8_t HandlePumpTimeControl(uint8_t *Buff, uint8_t len) { + // 实现泵时长控制逻辑，方向1字节，时长2字节。全FF跳过 + // 1表示启动泵顺时针转动，2表示启动泵的逆时针转动，0表示停止泵 + // 时间为0则表示一直转 + uint8_t rst = systemStatus.rst; + for(uint8_t index = 0; index < len; index++) { + + if(memcmp(Buff+index*3, "\xFF\xFF\xFF", 3) == 0) { + continue; + } + int8_t direction = Buff[index*3]; + if(direction == 0) { + StopPump(index); + continue; + } + if(direction == 2) { + direction = -1; + } + uint16_t time = (Buff[index*3+1]<<8) | Buff[index*3+2]; + if(time == 0) { + // 方向控制办法待确定 + StartPumpJog(index); + continue; + } + // 使用步数方式更靠谱，通过时间和速度计算步数，结束时不用发送停止命令 + int32_t step = direction*time*dp.pump[index].speed*dp.pump[index].speedPercent/100; + SetPumpStepTarget(index, step); + StartPumpRelativeMove(index); + + if(rst != systemStatus.rst) { + log_e("泵时长控制错误"); + return ACK_FAILED; + } + } + + return ACK_OK; +} + +// 泵速度设置处理 +/** + * 处理泵速度设置命令 + * + * @param Buff 接收到的数据缓冲区 + * @param len 接收到的数据长度 + * @return 0:成功 其他:失败 + */ +static uint8_t HandlePumpSpeedControl(uint8_t *Buff, uint8_t len) { + // 1个字节，为速度百分比，全FF跳过 + + for (size_t index = 0; index < len; index++) + { + uint8_t rst = systemStatus.rst; + uint8_t speedPercent = Buff[index]; + if (speedPercent == 0xFF) + { + continue; + } + + if (speedPercent > 100) { + log_e("泵速度设置错误"); + return ACK_FAILED; + } + //更新参数 + systemStatus.ds.pumps.speed[index] = speedPercent; + + // 写入指令 + uint16_t speed = transSpeedPercentToSpeed(index, speedPercent); + SetPumpJogSpeed(index, speed); + SetPumpStepSpeed(index, speed); + + if(rst != systemStatus.rst) { + log_e("泵速度设置错误"); + return ACK_FAILED; + } + } + + return ACK_OK; +} + +// 泵步进控制处理 +/** + * 处理泵步进控制命令 + * + * @param Buff 接收到的数据缓冲区 + * @param len 接收到的数据长度 + * @return 0:成功 其他:失败 + */ +static uint8_t HandlePumpStepControl(uint8_t *Buff, uint8_t len) { + // 4字节步进，全FF跳过 + for (size_t index = 0; index < len; index++) + { + if(memcmp(Buff+index*4, "\xFF\xFF\xFF\xFF", 4) == 0) { + continue; + } + uint8_t rst = systemStatus.rst; + uint8_t index = Buff[index*4]; + int32_t step = (Buff[index*4+1]<<24) | (Buff[index*4+2]<<16) | (Buff[index*4+3]<<8) | Buff[index*4+4]; + SetPumpStepTarget(index, step); + StartPumpRelativeMove(index); + if(rst != systemStatus.rst) { + log_e("泵步进设置错误"); + return ACK_FAILED; + } + } + + return ACK_OK; +} + +/** + * 处理软急停命令 + * + * @param rxBuf 接收到的数据缓冲区 + * @param rxLen 接收到的数据长度 + * @return 0:成功 1:失败 + */ +static uint8_t HandleSoftStop(uint8_t *rxBuf, uint16_t rxLen) { + if(rxLen != 1) { + log_e("软急停设置错误"); + return ACK_FAILED; + } + // 实现软急停功能逻辑 + if(rxBuf[0] == 0) { + // 正常状态 + systemStatus.ds.estopStatus = ESTOP_NORMAL; + } + else { + // 急停状态 + uint8_t rst = systemStatus.rst; + StopPump(0); + StopPump(1); + // StopPumpJog(0); + // StopPumpJog(1); + stopValve(0); + stopValve(1); + if(rst != systemStatus.rst) { + log_e("软急停错误"); + return ACK_FAILED; + } + systemStatus.ds.estopStatus = ESTOP_PRESSED; + } + return ACK_OK; +} + /** * 检查接收到的命令帧格式是否正确 * @@ -1490,33 +1573,31 @@ void ProcessHostCommand(uint8_t *rxBuf, uint8_t rxLen) { switch(cmdCode) { case HOST_CMD_STATUS_QUERY: - error = HandleStatusQuery(); + HandleStatusQuery(); break; case HOST_CMD_VALVE_CTRL: error = HandleValveControl(data, dataLen); + packMsgToHost(HOST_CMD_VALVE_CTRL, error); break; case HOST_CMD_PUMP_RUN_TIME: error = HandlePumpTimeControl(data, dataLen); + packMsgToHost(HOST_CMD_PUMP_RUN_TIME, error); break; case HOST_CMD_PUMP_RUN_SPEED: error = HandlePumpSpeedControl(data, dataLen); + packMsgToHost(HOST_CMD_PUMP_RUN_SPEED, error); break; case HOST_CMD_SOFT_STOP: error = HandleSoftStop(data, dataLen); + packMsgToHost(HOST_CMD_SOFT_STOP, error); break; case HOST_CMD_PUMP_RUN_STEP: error = HandlePumpStepControl(data, dataLen); + packMsgToHost(HOST_CMD_PUMP_RUN_STEP, error); break; case HOST_CMD_SYSTEM_INIT: error = HandleInit(); - if(error != 0) { - log_e("系统初始化失败"); - packMsgToHost(HOST_CMD_SYSTEM_INIT, ACK_OK); - } - else { - packMsgToHost(HOST_CMD_SYSTEM_INIT, ACK_FAILED); - } - + packMsgToHost(HOST_CMD_SYSTEM_INIT, error); break; default: error = CMD_FRAME_CMD_ERROR; diff --git a/protocol.h b/protocol.h index e79049d..1b3d238 100644 --- a/protocol.h +++ b/protocol.h @@ -19,8 +19,8 @@ #define FRAME_HEADER 0xA55A5AA5 #define FRAME_TAIL 0x5AA5A55A #define READ_ACK_TIMEOUT 50 -#define ACK_OK 0x0001 -#define ACK_FAILED 0x0000 +#define ACK_OK 0x0000 +#define ACK_FAILED 0x0001 #define ACK_OTHER 0x0002 // 功能码定义 @@ -347,9 +347,9 @@ typedef enum { // 3. 泵状态 typedef enum { - PUMP_STOP = 0, - PUMP_CLOCKWISE = 1, - PUMP_ANTICLOCKWISE = 2 + PUMP_STATUS_STOP = 0, + PUMP_STATUS_CLOCKWISE = 1, + PUMP_STATUS_ANTICLOCKWISE = 2 } PumpStatus; // 4. 泵速度范围 @@ -377,16 +377,13 @@ typedef enum { // 三通阀结构体 typedef struct { - uint8_t angle1; // 阀门1角度 (120/210) - uint8_t angle2; // 阀门2角度 (120/210) + uint8_t angle[2]; // 阀门角度 (120/210) } ValveStatus_t; // 泵结构体 typedef struct { - uint8_t status1; // 泵1运行状态 (停止/顺时针/逆时针) - uint8_t status2; // 泵2运行状态 (停止/顺时针/逆时针) - uint8_t speed1; // 泵1速度百分比 (0-100) - uint8_t speed2; // 泵2速度百分比 (0-100) + uint8_t status[2]; // 泵运行状态 (停止/顺时针/逆时针) + uint8_t speed[2]; // 泵速度百分比 (0-100)，设置值 } PumpStatus_t; // 设备状态结构体，用于上报HOST @@ -395,7 +392,8 @@ typedef struct { ValveStatus_t valves; // 两个三通阀状态 PumpStatus_t pumps; // 两个泵状态 uint8_t bubbleStatus; // 气泡状态 - uint8_t stopStatus; // 急停状态 + uint16_t activityMeter; // 活度计mCi + uint8_t estopStatus; // 急停状态 uint8_t errorCode; // 错误码 uint8_t initStatus; // 初始化状态 } DeviceStatus_t; @@ -404,21 +402,28 @@ typedef struct { typedef struct { DeviceStatus_t ds; - uint32_t speed[4];//实时速度 - uint32_t pos[4];//实时位置 + uint32_t valvesSpeed[2];//实时速度 + uint8_t valvesSpeedPercent[2];//实时速度百分比 + uint32_t valvesPos[2];//实时位置 + uint32_t pumpsSpeed[2];//实时速度 + uint8_t pumpsSpeedPercent[2];//实时速度百分比 + uint32_t pumpsPos[2];//实时位置 uint16_t rst;//RTU命令执行结果 -} SystemStatus_t; +} SystemStatus_t;//包含需要上报的状态及附加状态 typedef struct { uint8_t name[20]; uint8_t id; - uint32_t maxSpeed; - uint32_t maxAccel; - uint32_t maxDecel; + uint32_t speed;//满速 + uint32_t accel; + uint32_t decel; uint16_t fullCount;//电机总步数，用于根据角度估算需要移动的步数 int16_t offsetPos;//电机偏移位置，用于补偿电机移动误差 + uint8_t speedPercent;//设置速度百分比数，如100表示100% + uint8_t torque;//堵转力矩百分比,堵转转矩阈值应大于找寻原点过程中的实际运行转矩，且一般应小于最大转矩，以避免触发堵转故障保护 + uint8_t timeout;//回归超时时间，单位s } MotorDefaultParam_t; @@ -434,10 +439,8 @@ extern DeviceStatus_t deviceStatus; // 函数声明 void ProcessHostCommand(uint8_t *rxBuf, uint8_t rxLen); -void DecodePumpAlarmMsg(uint16_t reg4001); -void DecodePumpStatusMsg(uint16_t reg4002); void runPumpDemo(void); -void runVavleDemo(void); +void runValveDemo(void); void updateSystemStatus(void); void initCTLSystem(void);