From 357e36b24df1c6adbd90524c2e6dc2f576db1990 Mon Sep 17 00:00:00 2001 From: babayaga Date: Sat, 17 Jan 2026 06:25:31 +0100 Subject: [PATCH] Add binary WebSocket updates and lag compensation; extend PlotStatus --- src/NetworkValue.h | 110 +- src/components/Delta_VFD.cpp | 55 +- src/components/OmronE5.cpp | 2 + src/components/OperatorSwitch.cpp | 71 +- src/components/PressCylinder.cpp | 2568 +++++++++++++-------------- src/components/PressCylinder.h | 26 +- src/components/RestServer.cpp | 113 +- src/components/RestServer.h | 30 +- src/modbus/ModbusRTU.cpp | 2 +- src/profiles/PlotBase.cpp | 32 +- src/profiles/PlotBase.h | 24 +- src/profiles/SignalPlot.cpp | 41 +- src/profiles/TemperatureProfile.cpp | 4 + src/profiles/TemperatureProfile.h | 11 + 14 files changed, 1626 insertions(+), 1463 deletions(-) diff --git a/src/NetworkValue.h b/src/NetworkValue.h index d76216f5..7c8acc18 100644 --- a/src/NetworkValue.h +++ b/src/NetworkValue.h @@ -14,7 +14,7 @@ #define NETWORKVALUE_PERSISTENCE_ENABLED #ifdef NETWORKVALUE_PERSISTENCE_ENABLED - #include +#include #endif /* @@ -44,7 +44,7 @@ #include "NetworkValuePB.h" #define NETWORKVALUE_PROTOBUF_INHERITANCE , public maybe #else -#define NETWORKVALUE_PROTOBUF_INHERITANCE +#define NETWORKVALUE_PROTOBUF_INHERITANCE #endif enum class NetworkValue_ThresholdMode : uint8_t @@ -87,10 +87,10 @@ public: } template - void log(int level, const char *componentName, const char *fmt, Args... args) const { } + void log(int level, const char *componentName, const char *fmt, Args... args) const {} void setup_feature() {} void loop_feature() {} - void info_feature() const { } + void info_feature() const {} }; /** @@ -124,7 +124,6 @@ public: void loop_feature() {} void info_feature() const { - } }; @@ -222,7 +221,8 @@ public: bool applyUpdate(const T &newValue) { - if (!checkChanged(newValue)) { + if (!checkChanged(newValue)) + { return false; } T oldValue = m_lastValueOnUpdate; @@ -236,7 +236,7 @@ public: } T getValue() const { return m_value; } - T& getValueRef() { return m_value; } + T &getValueRef() { return m_value; } void setup_feature() {} void loop_feature() {} @@ -281,8 +281,10 @@ public: // This specialized version of checkChanged now compares the new value against the last known state. bool checkChanged(const std::array &newValue) const { - for (size_t i = 0; i < N; ++i) { - if (newValue[i] != m_lastValueOnUpdate[i]) { + for (size_t i = 0; i < N; ++i) + { + if (newValue[i] != m_lastValueOnUpdate[i]) + { return true; } } @@ -291,7 +293,8 @@ public: bool applyUpdate(const std::array &newValue) { - if (!checkChanged(newValue)) { + if (!checkChanged(newValue)) + { return false; } std::array oldValue = m_lastValueOnUpdate; @@ -305,7 +308,7 @@ public: } std::array getValue() const { return m_value; } - std::array& getValueRef() { return m_value; } + std::array &getValueRef() { return m_value; } void setup_feature() {} void loop_feature() {} @@ -349,8 +352,10 @@ public: bool checkChanged(const std::array &newValue) const { - for (size_t i = 0; i < N; ++i) { - if (newValue[i] != m_lastValueOnUpdate[i]) { + for (size_t i = 0; i < N; ++i) + { + if (newValue[i] != m_lastValueOnUpdate[i]) + { return true; } } @@ -359,7 +364,8 @@ public: bool applyUpdate(const std::array &newValue) { - if (!checkChanged(newValue)) { + if (!checkChanged(newValue)) + { return false; } std::array oldValue = m_lastValueOnUpdate; @@ -373,7 +379,7 @@ public: } std::array getValue() const { return m_value; } - std::array& getValueRef() { return m_value; } + std::array &getValueRef() { return m_value; } void setup_feature() {} void loop_feature() {} @@ -399,8 +405,7 @@ template class NetworkValue : public NetworkValueBase, public maybe, public maybe, - public maybe> - NETWORKVALUE_PROTOBUF_INHERITANCE + public maybe> NETWORKVALUE_PROTOBUF_INHERITANCE { private: // --- Tag Dispatching for update() method --- @@ -429,18 +434,22 @@ private: if (this->owner) { #if (NETWORKVALUE_ENABLE_PROTOBUF == 1) - if (hasFeature(E_NetworkValueFeatureFlags::E_NVFF_PROTOBUF)) { + if (hasFeature(E_NetworkValueFeatureFlags::E_NVFF_PROTOBUF)) + { uint8_t buffer[64]; // Static buffer for PB encoding pb_ostream_t stream = pb_ostream_from_buffer(buffer, sizeof(buffer)); - + MB_Registers regInfo = this->getRegisterInfo(); - if (this->encode(&stream, regInfo, newValue)) { + if (this->encode(&stream, regInfo, newValue)) + { PB_UpdateData pb_msg; pb_msg.data = buffer; pb_msg.len = stream.bytes_written; pb_msg.componentId = this->id; this->owner->onMessage(this->id, E_CALLS::EC_PROTOBUF_UPDATE, E_MessageFlags::E_MF_NONE, &pb_msg, this->owner); - } else { + } + else + { Log.warningln(F(" [update_impl] Protobuf encoding failed for '%s'"), this->name.c_str()); } } @@ -471,18 +480,22 @@ private: if (this->owner) { #if (NETWORKVALUE_ENABLE_PROTOBUF == 1) - if (hasFeature(E_NetworkValueFeatureFlags::E_NVFF_PROTOBUF)) { + if (hasFeature(E_NetworkValueFeatureFlags::E_NVFF_PROTOBUF)) + { uint8_t buffer[256]; // Larger buffer for arrays pb_ostream_t stream = pb_ostream_from_buffer(buffer, sizeof(buffer)); - + MB_Registers regInfo = this->getRegisterInfo(); - if (this->encode(&stream, regInfo, newValue)) { + if (this->encode(&stream, regInfo, newValue)) + { PB_UpdateData pb_msg; pb_msg.data = buffer; pb_msg.len = stream.bytes_written; pb_msg.componentId = this->id; this->owner->onMessage(this->id, E_CALLS::EC_PROTOBUF_UPDATE, E_MessageFlags::E_MF_NONE, &pb_msg, this->owner); - } else { + } + else + { Log.warningln(F(" [update_impl] Protobuf encoding failed for array '%s'"), this->name.c_str()); } } @@ -518,12 +531,10 @@ private: } public: - - - NetworkValue(Component *owner, ushort id, - const char *name, - T initial, T threshold, NetworkValue_ThresholdMode mode, void (*cb)(const T &, const T &) = nullptr, - uint8_t featureFlags = static_cast(E_NetworkValueFeatureFlags::E_NVFF_ALL)) + NetworkValue(Component *owner, ushort id, + const char *name, + T initial, T threshold, NetworkValue_ThresholdMode mode, void (*cb)(const T &, const T &) = nullptr, + uint8_t featureFlags = static_cast(E_NetworkValueFeatureFlags::E_NVFF_ALL)) : NetworkValueBase(name, id, COMPONENT_DEFAULT, owner, featureFlags) { initNotify(initial, threshold, mode, cb); @@ -539,10 +550,10 @@ public: } #endif } - - NetworkValue(Component *owner, ushort id, - const char *name, - uint8_t featureFlags = static_cast(E_NetworkValueFeatureFlags::E_NVFF_ALL)) + + NetworkValue(Component *owner, ushort id, + const char *name, + uint8_t featureFlags = static_cast(E_NetworkValueFeatureFlags::E_NVFF_ALL)) : NetworkValueBase(name, id, COMPONENT_DEFAULT, owner, featureFlags) { #if (NETWORKVALUE_ENABLE_LOGGING == 1) @@ -610,7 +621,8 @@ public: return E_OK; } - short info() override { + short info() override + { Component::info(); #if (NETWORKVALUE_ENABLE_LOGGING == 1) if (hasFeature(E_NetworkValueFeatureFlags::E_NVFF_LOGGING)) @@ -629,10 +641,11 @@ public: template bool update(const U &newValue, E_PRIORITY priority = E_PRIORITY::E_PRIORITY_LOWEST) { - if (!this->applyUpdate(newValue)) { + if (!this->applyUpdate(newValue)) + { return false; // If no change, do nothing further. } - + #if (NETWORKVALUE_ENABLE_LOGGING == 1) if (hasFeature(E_NetworkValueFeatureFlags::E_NVFF_LOGGING)) { @@ -641,10 +654,11 @@ public: #endif // Dispatch notifications - if (this->owner) { + if (this->owner) + { update_impl(newValue, typename get_update_tag::type(), priority); } - + return true; } @@ -685,18 +699,20 @@ public: if (NETWORKVALUE_ENABLE_NOTIFY && hasFeature(E_NetworkValueFeatureFlags::E_NVFF_NOTIFY)) { NV_Notify::init_feature(initial, threshold, mode, cb); - } + } } - bool applyUpdate(const T& newValue) { - if (NETWORKVALUE_ENABLE_NOTIFY && hasFeature(E_NetworkValueFeatureFlags::E_NVFF_NOTIFY)) { + bool applyUpdate(const T &newValue) + { + if (NETWORKVALUE_ENABLE_NOTIFY && hasFeature(E_NetworkValueFeatureFlags::E_NVFF_NOTIFY)) + { return NV_Notify::applyUpdate(newValue); } return false; } T getValue() const { return NV_Notify::getValue(); } - T& getValueRef() { return NV_Notify::getValueRef(); } + T &getValueRef() { return NV_Notify::getValueRef(); } using NV_Notify::getValue; using NV_Notify::getValueRef; @@ -750,14 +766,14 @@ public: void fromJSON(const JsonObjT &obj) { #if NETWORKVALUE_ENABLE_LOGGING - if (hasFeature(E_NetworkValueFeatureFlags::E_NVFF_LOGGING) && obj.containsKey("logging_enabled")) + if (hasFeature(E_NetworkValueFeatureFlags::E_NVFF_LOGGING) && !obj["logging_enabled"].isNull()) { this->enableLogging(obj["logging_enabled"].template as()); } #endif #if NETWORKVALUE_ENABLE_MODBUS - if (hasFeature(E_NetworkValueFeatureFlags::E_NVFF_MODBUS) && obj.containsKey("modbus")) + if (hasFeature(E_NetworkValueFeatureFlags::E_NVFF_MODBUS) && !obj["modbus"].isNull()) { auto mod = obj["modbus"].template as(); ushort startAddress = mod["startAddress"].template as(); @@ -771,7 +787,7 @@ public: #endif #if NETWORKVALUE_ENABLE_NOTIFY - if (hasFeature(E_NetworkValueFeatureFlags::E_NVFF_NOTIFY) && obj.containsKey("notify")) + if (hasFeature(E_NetworkValueFeatureFlags::E_NVFF_NOTIFY) && !obj["notify"].isNull()) { auto notify = obj["notify"].template as(); T threshold = notify["threshold"].template as(); diff --git a/src/components/Delta_VFD.cpp b/src/components/Delta_VFD.cpp index 1c85ae73..6b58df6f 100644 --- a/src/components/Delta_VFD.cpp +++ b/src/components/Delta_VFD.cpp @@ -54,10 +54,9 @@ enum class E_DELTA_MS300_FAULT_STATUS_FLAGS : uint16_t // MS300 Frequency Command Register (2001H) #define DELTA_REG_SET_FREQ static_cast(E_DELTA_MS300_REGISTERS::E_DELTA_MS300_FREQUENCY_COMMAND) - // Enum for TCP Offsets is now in DELTA_VFD.h -DELTA_VFD::DELTA_VFD(Component* owner, uint8_t slaveId, millis_t readInterval) +DELTA_VFD::DELTA_VFD(Component *owner, uint8_t slaveId, millis_t readInterval) : VFD_Base(owner, slaveId, COMPONENT_KEY_DELTA_VFD, readInterval) { componentId = id; @@ -87,7 +86,7 @@ DELTA_VFD::DELTA_VFD(Component* owner, uint8_t slaveId, millis_t readInterval) short DELTA_VFD::setup() { L_INFO(F("DELTA_VFD[%d]: Setting up..."), slaveId); - + // --- Configure Mandatory Read Blocks --- // Block 1: Status and monitoring registers (0x2100-0x2106) - Status Codes, Fault Status, Freq Cmd, Output Freq, Current, DC Bus, Output Voltage ModbusReadBlock *statusMonitorBlock = addMandatoryReadBlock( @@ -117,12 +116,12 @@ short DELTA_VFD::setup() } #endif // Add output registers for VFD control - this->addOutputRegister(DELTA_REG_SET_FREQ, E_FN_CODE::FN_WRITE_HOLD_REGISTER, 0, PRIORITY_HIGH); - this->addOutputRegister(static_cast(E_DELTA_MS300_REGISTERS::E_DELTA_MS300_CONTROL_COMMAND), + this->addOutputRegister(DELTA_REG_SET_FREQ, E_FN_CODE::FN_WRITE_HOLD_REGISTER, 0, PRIORITY_HIGH); + this->addOutputRegister(static_cast(E_DELTA_MS300_REGISTERS::E_DELTA_MS300_CONTROL_COMMAND), E_FN_CODE::FN_WRITE_HOLD_REGISTER, 0, // Default to no command (stopped) PRIORITY_HIGH); - + return E_OK; } @@ -181,7 +180,6 @@ bool DELTA_VFD::onRegisterUpdate(uint16_t address, uint16_t newValue) bool updated = false; // Update local state based on the register address using MS300 registers - switch (address) { case static_cast(E_DELTA_MS300_REGISTERS::E_DELTA_MS300_MON_OUTPUT_FREQ): @@ -214,10 +212,11 @@ bool DELTA_VFD::onRegisterUpdate(uint16_t address, uint16_t newValue) updated = true; break; case static_cast(E_DELTA_MS300_REGISTERS::E_DELTA_MS300_STATUS_CODES): // 0x2100 - Status Codes - _faultCode = newValue & 0xFF; // Low byte = error code + _faultCode = newValue & 0xFF; // Low byte = error code _faultValid = true; // Check for specific fault flags if high byte has fault status flags - if (newValue & 0xFF00) { + if (newValue & 0xFF00) + { _faultStatusFlags = (newValue >> 8) & 0xFF; // High byte contains fault status flags } _updateVfdState(); @@ -225,7 +224,7 @@ bool DELTA_VFD::onRegisterUpdate(uint16_t address, uint16_t newValue) break; default: // Check if it's within the monitoring range - if (address >= static_cast(E_DELTA_MS300_REGISTERS::E_DELTA_MS300_MON_FREQ_CMD) && + if (address >= static_cast(E_DELTA_MS300_REGISTERS::E_DELTA_MS300_MON_FREQ_CMD) && address <= static_cast(E_DELTA_MS300_REGISTERS::E_DELTA_MS300_MON_OUTPUT_TORQUE)) { // Log.traceln(F("DELTA_VFD[%d]: MS300 monitoring register update (Addr: 0x%04X) -> %d"), slaveId, address, newValue); @@ -234,9 +233,10 @@ bool DELTA_VFD::onRegisterUpdate(uint16_t address, uint16_t newValue) } // Call base class method - if (RTU_Base::onRegisterUpdate(address, newValue)) updated = true; + if (RTU_Base::onRegisterUpdate(address, newValue)) + updated = true; - //L_INFO(F("DELTA_VFD[%d]: Register Update (Addr: 0x%04X) -> %d"), slaveId, address, newValue); + // L_INFO(F("DELTA_VFD[%d]: Register Update (Addr: 0x%04X) -> %d"), slaveId, address, newValue); return updated; } @@ -299,7 +299,7 @@ bool DELTA_VFD::hasFaultType(E_DELTA_MS300_FAULT_STATUS_FLAGS faultType) const { if (!_faultValid) return false; - + return (_faultStatusFlags & static_cast(faultType)) != 0; } @@ -324,8 +324,7 @@ bool DELTA_VFD::run() static_cast(E_DELTA_MS300_CMD_ACCEL_DECEL::ACCEL_DECEL_1), static_cast(E_DELTA_MS300_CMD_SPEED_SELECT::MASTER_SPEED), static_cast(E_DELTA_MS300_CMD_ENABLE::DISABLE), - static_cast(E_DELTA_MS300_CMD_OPERATION_SOURCE::PR_00_21_SETTING) - ); + static_cast(E_DELTA_MS300_CMD_OPERATION_SOURCE::PR_00_21_SETTING)); uint16_t reg = static_cast(E_DELTA_MS300_REGISTERS::E_DELTA_MS300_CONTROL_COMMAND); this->setOutputRegisterValue(reg, cmd); return true; @@ -339,8 +338,7 @@ bool DELTA_VFD::reverse() static_cast(E_DELTA_MS300_CMD_ACCEL_DECEL::ACCEL_DECEL_1), static_cast(E_DELTA_MS300_CMD_SPEED_SELECT::MASTER_SPEED), static_cast(E_DELTA_MS300_CMD_ENABLE::DISABLE), - static_cast(E_DELTA_MS300_CMD_OPERATION_SOURCE::PR_00_21_SETTING) - ); + static_cast(E_DELTA_MS300_CMD_OPERATION_SOURCE::PR_00_21_SETTING)); uint16_t reg = static_cast(E_DELTA_MS300_REGISTERS::E_DELTA_MS300_CONTROL_COMMAND); this->setOutputRegisterValue(reg, cmd); return true; @@ -354,8 +352,7 @@ short DELTA_VFD::stop() static_cast(E_DELTA_MS300_CMD_ACCEL_DECEL::ACCEL_DECEL_1), static_cast(E_DELTA_MS300_CMD_SPEED_SELECT::MASTER_SPEED), static_cast(E_DELTA_MS300_CMD_ENABLE::DISABLE), - static_cast(E_DELTA_MS300_CMD_OPERATION_SOURCE::PR_00_21_SETTING) - ); + static_cast(E_DELTA_MS300_CMD_OPERATION_SOURCE::PR_00_21_SETTING)); uint16_t reg = static_cast(E_DELTA_MS300_REGISTERS::E_DELTA_MS300_CONTROL_COMMAND); this->setOutputRegisterValue(reg, cmd); return true; @@ -505,7 +502,7 @@ short DELTA_VFD::mb_tcp_read(MB_Registers *reg) success = _setFrequencyValid; // Corrected: was _setFrequencyValid / 100 value = success ? (_setFrequency / 100) : 0xFFFF; // Display in Hz break; - case E_DELTA_TCP_OFFSET::CMD_DIRECTION: // Read associated with command write returns current running state? + case E_DELTA_TCP_OFFSET::CMD_DIRECTION: // Read associated with command write returns current running state? value = _statusValid ? _statusRegister : 0xFFFF; // Return raw status success = _statusValid; break; @@ -533,15 +530,15 @@ short DELTA_VFD::mb_tcp_read(MB_Registers *reg) short DELTA_VFD::setupVFD() { RS485 *rs485 = (RS485 *)owner; - + // Configure MS300 parameters for communication control - + // /rs485->modbus.writeRegister(this->slaveId, static_cast(E_DELTA_MS300_REGISTERS::E_DELTA_MS300_MASTER_FREQ_CMD_SOURCE), 9, true); // Master Freq Cmd Source (00-20) - rs485->modbus.writeRegister(this->slaveId, static_cast(E_DELTA_MS300_REGISTERS::E_DELTA_MS300_OPERATION_CMD_SOURCE), 2, true); // Operation Cmd Source (00-21) + rs485->modbus.writeRegister(this->slaveId, static_cast(E_DELTA_MS300_REGISTERS::E_DELTA_MS300_OPERATION_CMD_SOURCE), 2, true); // Operation Cmd Source (00-21) rs485->modbus.writeRegister(this->slaveId, static_cast(E_DELTA_MS300_REGISTERS::E_DELTA_MS300_MAX_OP_FREQ_MOTOR_1), DELTA_MAX_OP_FREQ_MOTOR_1, true); // Max Op Freq Motor 1 (01-00) = 75.00 Hz - rs485->modbus.writeRegister(this->slaveId, static_cast(E_DELTA_MS300_REGISTERS::E_DELTA_MS300_ACCEL_TIME_1_MOTOR_1), 150, true); // Accel Time 1 (01-12) = 1.50 sec - rs485->modbus.writeRegister(this->slaveId, static_cast(E_DELTA_MS300_REGISTERS::E_DELTA_MS300_DECEL_TIME_1_MOTOR_1), 150, true); // Decel Time 1 (01-13) = 1.50 sec - rs485->modbus.writeRegister(this->slaveId, static_cast(E_DELTA_MS300_REGISTERS::E_DELTA_MS300_OUTPUT_VOLTAGE_MOTOR_1), 2200, true); // Motor Voltage (01-02) = 220.0V + rs485->modbus.writeRegister(this->slaveId, static_cast(E_DELTA_MS300_REGISTERS::E_DELTA_MS300_ACCEL_TIME_1_MOTOR_1), 150, true); // Accel Time 1 (01-12) = 1.50 sec + rs485->modbus.writeRegister(this->slaveId, static_cast(E_DELTA_MS300_REGISTERS::E_DELTA_MS300_DECEL_TIME_1_MOTOR_1), 150, true); // Decel Time 1 (01-13) = 1.50 sec + rs485->modbus.writeRegister(this->slaveId, static_cast(E_DELTA_MS300_REGISTERS::E_DELTA_MS300_OUTPUT_VOLTAGE_MOTOR_1), 2200, true); // Motor Voltage (01-02) = 220.0V return E_OK; } @@ -583,7 +580,7 @@ short DELTA_VFD::mb_tcp_write(MB_Registers *reg, short value) case 1: // Run Forward commandSuccess = run(); break; - case 2: // Run Reverse + case 2: // Run Reverse commandSuccess = reverse(); break; case 0: // Stop @@ -604,7 +601,7 @@ short DELTA_VFD::mb_tcp_write(MB_Registers *reg, short value) // Handle custom commands E_DELTA_CMD command = static_cast(value); _lastCommand = command; - + switch (command) { case E_DELTA_CMD::E_DTC_INFO: @@ -811,7 +808,7 @@ bool DELTA_VFD::getOutputTorquePercent(uint16_t &value) const } #endif -void DELTA_VFD::onError(ushort errorCode, const char* errorMessage) +void DELTA_VFD::onError(ushort errorCode, const char *errorMessage) { // L_ERROR(F("DELTA_VFD[%d]: Modbus Error %d - %s"), slaveId, errorCode, errorMessage ? errorMessage : "Unknown"); RTU_Base::onError(errorCode, errorMessage); diff --git a/src/components/OmronE5.cpp b/src/components/OmronE5.cpp index b8f230c5..7630e68b 100644 --- a/src/components/OmronE5.cpp +++ b/src/components/OmronE5.cpp @@ -52,9 +52,11 @@ OmronE5::OmronE5(Component *owner, uint8_t slaveId, millis_t readInterval) m_pv.initModbus(tcpBaseAddr + (ushort)E_OmronTcpOffset::PV, 1, this->id, this->slaveId, FN_READ_HOLD_REGISTER, "PV", name.c_str()); m_sp.initModbus(tcpBaseAddr + (ushort)E_OmronTcpOffset::SP, 1, this->id, this->slaveId, FN_READ_HOLD_REGISTER, "SP", name.c_str()); + m_statusLow.initModbus(tcpBaseAddr + (ushort)E_OmronTcpOffset::STATUS_LOW, 1, this->id, this->slaveId, FN_READ_HOLD_REGISTER, "Status Low", name.c_str()); m_statusHigh.initModbus(tcpBaseAddr + (ushort)E_OmronTcpOffset::STATUS_HIGH, 1, this->id, this->slaveId, FN_READ_HOLD_REGISTER, "Status High", name.c_str()); m_enabled.initModbus(tcpBaseAddr + (ushort)E_OmronTcpOffset::ENABLED, 1, this->id, this->slaveId, FN_WRITE_COIL, "Enabled", name.c_str()); + m_commsWritingEnabled.initModbus(tcpBaseAddr + (ushort)E_OmronTcpOffset::CMD_COMMS_WRITE, 1, this->id, this->slaveId, FN_WRITE_COIL, "Comms Write", name.c_str()); m_runState.initModbus(tcpBaseAddr + (ushort)E_OmronTcpOffset::CMD_STOP, 1, this->id, this->slaveId, FN_WRITE_COIL, "Run/Stop Coil", name.c_str()); diff --git a/src/components/OperatorSwitch.cpp b/src/components/OperatorSwitch.cpp index bf4ac4ca..7d73c089 100644 --- a/src/components/OperatorSwitch.cpp +++ b/src/components/OperatorSwitch.cpp @@ -5,16 +5,26 @@ #include "./OperatorSwitch.h" // Helper function to convert State enum to string for logging -const char* stateToString(OperatorSwitch::State state) { - switch (state) { - case OperatorSwitch::State::IDLE: return "IDLE"; - case OperatorSwitch::State::STOP_PRESSED: return "STOP_PRESSED"; - case OperatorSwitch::State::CYCLE_PRESSED: return "CYCLE_PRESSED"; - case OperatorSwitch::State::BOTH_PRESSED: return "BOTH_PRESSED"; - case OperatorSwitch::State::STOP_HELD: return "STOP_HELD"; - case OperatorSwitch::State::CYCLE_HELD: return "CYCLE_HELD"; - case OperatorSwitch::State::BOTH_HELD: return "BOTH_HELD"; - default: return "UNKNOWN"; +const char *stateToString(OperatorSwitch::State state) +{ + switch (state) + { + case OperatorSwitch::State::IDLE: + return "IDLE"; + case OperatorSwitch::State::STOP_PRESSED: + return "STOP_PRESSED"; + case OperatorSwitch::State::CYCLE_PRESSED: + return "CYCLE_PRESSED"; + case OperatorSwitch::State::BOTH_PRESSED: + return "BOTH_PRESSED"; + case OperatorSwitch::State::STOP_HELD: + return "STOP_HELD"; + case OperatorSwitch::State::CYCLE_HELD: + return "CYCLE_HELD"; + case OperatorSwitch::State::BOTH_HELD: + return "BOTH_HELD"; + default: + return "UNKNOWN"; } } @@ -116,9 +126,11 @@ short OperatorSwitch::loop() if (rawInputState != State::IDLE) { unsigned long mostRecentPressTime = 0; - if (stopPressed) mostRecentPressTime = max(mostRecentPressTime, buttons[BUTTON_STOP].pressTime); - if (cyclePressed) mostRecentPressTime = max(mostRecentPressTime, buttons[BUTTON_CYCLE].pressTime); - + if (stopPressed) + mostRecentPressTime = max(mostRecentPressTime, buttons[BUTTON_STOP].pressTime); + if (cyclePressed) + mostRecentPressTime = max(mostRecentPressTime, buttons[BUTTON_CYCLE].pressTime); + if ((now - mostRecentPressTime >= PRESS_TIME_MS)) { newState = rawInputState; @@ -143,35 +155,42 @@ short OperatorSwitch::loop() } else if (!holdEventTriggered && (now - pressStartTime >= HOLD_TIME_MS)) { - if (currentState == State::STOP_PRESSED) newState = State::STOP_HELD; - else if (currentState == State::CYCLE_PRESSED) newState = State::CYCLE_HELD; - else if (currentState == State::BOTH_PRESSED) newState = State::BOTH_HELD; - - if (newState != currentState) { + if (currentState == State::STOP_PRESSED) + newState = State::STOP_HELD; + else if (currentState == State::CYCLE_PRESSED) + newState = State::CYCLE_HELD; + else if (currentState == State::BOTH_PRESSED) + newState = State::BOTH_HELD; + + if (newState != currentState) + { holdEventTriggered = true; } } break; case State::STOP_HELD: - if (!stopPressed) newState = State::IDLE; + if (!stopPressed) + newState = State::IDLE; break; case State::CYCLE_HELD: - if (!cyclePressed) newState = State::IDLE; + if (!cyclePressed) + newState = State::IDLE; break; case State::BOTH_HELD: - if (!stopPressed || !cyclePressed) newState = State::IDLE; + if (!stopPressed || !cyclePressed) + newState = State::IDLE; break; default: break; } - if (newState != currentState) { + if (newState != currentState) + { m_state.update(newState); notifyStateChange(); } - return E_OK; } @@ -184,7 +203,6 @@ short OperatorSwitch::info(short val0, short val1) void OperatorSwitch::notifyStateChange() { - Log.verboseln("OperatorSwitch::notifyStateChange, state: %s", stateToString(m_state.getValue())); Component::notifyStateChange(); } @@ -196,12 +214,13 @@ short OperatorSwitch::mb_tcp_write(MB_Registers *reg, short networkValue) short OperatorSwitch::mb_tcp_read(MB_Registers *reg) { short result = NetworkComponent::mb_tcp_read(reg); - if (result != E_NOT_IMPLEMENTED) { + if (result != E_NOT_IMPLEMENTED) + { return result; } uint16_t address = reg->startAddress; - + if (address == (_baseAddress + (ushort)E_MB_Offset::STATE)) { return (ushort)m_state.getValue(); diff --git a/src/components/PressCylinder.cpp b/src/components/PressCylinder.cpp index 5fb39d07..602e8cdf 100644 --- a/src/components/PressCylinder.cpp +++ b/src/components/PressCylinder.cpp @@ -1,1300 +1,1268 @@ -#include "PressCylinder.h" - -#ifdef ENABLE_PRESS_CYLINDER - -#include -#include -#include -#include -#include "config.h" -#include "features.h" -#include "StringUtils.h" -#include "xmath.h" -#include "macros.h" - -#include -#include -#include - -constexpr unsigned long PressCylinder::JOYSTICK_DOUBLE_CLICK_MS; -constexpr PressCylinder::E_Mode PressCylinder::DEFAULT_AUTO_MODE_NORMAL; -constexpr PressCylinder::E_Mode PressCylinder::DEFAULT_AUTO_MODE_INTERLOCKED; - -// #define ENABLE_MIN_LOAD_CHECK - -void PressCylinderSettings::fromJSON(const JsonVariantConst &doc) -{ - interlocked = doc["interlocked"] | true; - mode = doc["mode"] | (uint8_t)PressCylinder::MODE_MANUAL; - homing_threshold = doc["homing_threshold"] | PRESS_CYLINDER_DEFAULT_HOMING_THRESHOLD; - pressing_threshold = doc["pressing_threshold"] | PRESS_CYLINDER_DEFAULT_PRESSING_THRESHOLD; - highload_threshold = doc["highload_threshold"] | PRESS_CYLINDER_DEFAULT_HIGHLOAD_THRESHOLD; - maxload_threshold = doc["maxload_threshold"] | PRESS_CYLINDER_DEFAULT_MAXLOAD_THRESHOLD; - maxload_threshold = clamp(maxload_threshold, (uint32_t)1, (uint32_t)PRESS_CYLINDER_MAX_LOAD_CLAMP); - sp_deadband_percent = doc["sp_deadband_percent"] | PRESS_CYLINDER_DEFAULT_SP_DEADBAND_PERCENT; - min_load = doc["min_load"] | PRESS_CYLINDER_DEFAULT_MIN_LOAD; - balance_interval = doc["balance_interval"] | BALANCE_INTERVAL; - balance_min_pv_diff = doc["balance_min_pv_diff"] | BALANCE_MIN_PV_DIFF; - balance_max_pv_diff = doc["balance_max_pv_diff"] | BALANCE_MAX_PV_DIFF; - max_stall_activations = doc["max_stall_activations"] | PRESS_CYLINDER_MAX_STALL_ACTIVATIONS; -} - -void PressCylinderSettings::toJSON(JsonVariant doc) -{ - doc["homing_threshold"] = homing_threshold; - doc["pressing_threshold"] = pressing_threshold; - doc["highload_threshold"] = highload_threshold; - doc["maxload_threshold"] = maxload_threshold; - doc["sp_deadband_percent"] = sp_deadband_percent; - doc["min_load"] = min_load; - doc["balance_interval"] = balance_interval; - doc["balance_min_pv_diff"] = balance_min_pv_diff; - doc["balance_max_pv_diff"] = balance_max_pv_diff; - doc["max_stall_activations"] = max_stall_activations; - doc["interlocked"] = interlocked; - doc["mode"] = mode; -} - -void PressCylinder::_applyDefaultSettings() -{ - _settings.homing_threshold = PRESS_CYLINDER_DEFAULT_HOMING_THRESHOLD; - _settings.pressing_threshold = PRESS_CYLINDER_DEFAULT_PRESSING_THRESHOLD; - _settings.highload_threshold = PRESS_CYLINDER_DEFAULT_HIGHLOAD_THRESHOLD; - _settings.maxload_threshold = PRESS_CYLINDER_DEFAULT_MAXLOAD_THRESHOLD; - _settings.sp_deadband_percent = PRESS_CYLINDER_DEFAULT_SP_DEADBAND_PERCENT; - _settings.min_load = PRESS_CYLINDER_DEFAULT_MIN_LOAD; - _settings.balance_interval = BALANCE_INTERVAL; - _settings.balance_min_pv_diff = BALANCE_MIN_PV_DIFF; - _settings.balance_max_pv_diff = BALANCE_MAX_PV_DIFF; - _settings.max_stall_activations = PRESS_CYLINDER_MAX_STALL_ACTIVATIONS; - _settings.max_stall_activations = PRESS_CYLINDER_MAX_STALL_ACTIVATIONS; - _settings.interlocked = true; - _settings.mode = MODE_MANUAL; -} - -PressCylinder::PressCylinder(Component *owner, short id, short mbAddress, Joystick *joystick) - : NetworkComponent(mbAddress, "PressCylinder", id, COMPONENT_DEFAULT, owner), - m_targetSP(this, id, "Target SP"), - m_mode(this, id, "Mode(0:MANUAL,1:AUTO,2:MANUAL_MULTI,3:AUTO_MULTI,4:AUTO_MULTI_BALANCED,5:REMOTE)"), - m_state(this, id, "State(0:IDLE,1:HOMING,2:PRESSING,3:HIGHLOAD,4:MAXLOAD,5:ERROR)"), - m_error_code(this, id, "Error(0:OK,1:MINLOAD,2:OVERLOAD,3:BALANCE_MAX_DIFF,4:LOADCELL,5:MAX_TIME,6:STALLED,7:AUTO_TIMEOUT)"), - m_cflags(this, id, "CFlags(1:MINLOAD,2:MAX_TIME,4:STALLED,8:BALANCE,16:LOADCELL,32:MULTI_TIMEOUT)"), - m_outputMode(this, id, "OutputMode(1:HOLD)"), - m_interlocked(this, id, "Interlocked"), - _joystick(joystick), - _last_error_code(E_PC_OK), - _last_balance_time(0), - _auto_interlocked_start_time(0), - _balanced_mode_active_cylinder(0), - _previous_mode(MODE_MANUAL), - _hold_sp(0), - _last_joy_up_time(0), - _joy_was_up(false) -{ - _applyDefaultSettings(); - for (uint8_t i = 0; i < PRESS_CYLINDER_MAX_PAIRS; ++i) - { - _loadcells[i] = nullptr; - _solenoids[i] = nullptr; - _pv_names[i].format("PV %d", i); - m_pvs[i] = new PressCylinderValue(this, id, _pv_names[i].c_str()); - _pvs_raw_previous[i] = 0; - } -} - -short PressCylinder::addLoadcell(Loadcell *loadcell) -{ - for (uint8_t i = 0; i < PRESS_CYLINDER_MAX_PAIRS; ++i) - { - if (!_loadcells[i]) - { - _loadcells[i] = loadcell; - return E_OK; - } - } - L_ERROR("Cannot add more loadcells to PressCylinder"); - return E_INVALID_PARAMETER; -} - -short PressCylinder::addSolenoid(Solenoid *solenoid) -{ - for (uint8_t i = 0; i < PRESS_CYLINDER_MAX_SOLENOIDS; ++i) - { - if (!_solenoids[i]) - { - _solenoids[i] = solenoid; - return E_OK; - } - } - L_ERROR("Cannot add more solenoids to PressCylinder"); - return E_INVALID_PARAMETER; -} - -short PressCylinder::setup() -{ - NetworkComponent::setup(); - - for (int i = 0; i < PRESS_CYLINDER_MAX_PAIRS; ++i) - { - if (m_pvs[i]) - { - SETUP_NETWORK_VALUE((*m_pvs[i]), (MB_OFS_HR_PVS + i), FN_READ_HOLD_REGISTER, _pv_names[i].c_str(), 0, 2, NetworkValue_ThresholdMode::DIFFERENCE); - } - } - SETUP_NETWORK_VALUE(m_targetSP, MB_OFS_HR_TARGET_SP, FN_WRITE_HOLD_REGISTER, "Target SP", 0, 1, NetworkValue_ThresholdMode::DIFFERENCE); - SETUP_NETWORK_VALUE(m_mode, MB_OFS_HR_MODE, FN_WRITE_HOLD_REGISTER, "Mode(0:NONE,1:MANUAL,2:AUTO,3:MANUAL_MULTI,4:AUTO_MULTI,5:AUTO_MULTI_BALANCED,6:REMOTE)", MODE_MANUAL, 1, NetworkValue_ThresholdMode::DIFFERENCE); - SETUP_NETWORK_VALUE(m_state, MB_OFS_HR_STATE, FN_READ_HOLD_REGISTER, "State(0:IDLE,1:MAXLOAD,2:ERROR)", STATE_IDLE, 1, NetworkValue_ThresholdMode::DIFFERENCE); - SETUP_NETWORK_VALUE(m_error_code, MB_OFS_HR_ERROR_CODE, FN_READ_HOLD_REGISTER, "Error(0:OK,1:MINLOAD,2:OVERLOAD,3:BALANCE_MAX_DIFF,4:LOADCELL,5:MAX_TIME,6:STALLED,7:AUTO_TIMEOUT)", E_PC_OK, 1, NetworkValue_ThresholdMode::DIFFERENCE); - SETUP_NETWORK_VALUE(m_cflags, MB_OFS_HR_CFLAGS, FN_WRITE_HOLD_REGISTER, "CFlags(1:MINLOAD,2:MAX_TIME,4:STALLED,8:BALANCE,16:LOADCELL,32:MULTI_TIMEOUT)", E_PC_OP_ALL, 1, NetworkValue_ThresholdMode::DIFFERENCE); - SETUP_NETWORK_VALUE(m_outputMode, MB_OFS_HR_OUTPUT_MODE, FN_WRITE_HOLD_REGISTER, "OutputMode(1:HOLD)", E_PC_OM_NONE, 1, NetworkValue_ThresholdMode::DIFFERENCE); - SETUP_NETWORK_VALUE(m_interlocked, MB_OFS_C_INTERLOCKED, FN_WRITE_COIL, "Interlocked", true, 1, NetworkValue_ThresholdMode::DIFFERENCE); - - MB_Registers cmdReg(_baseAddress + MB_OFS_HR_CMD, 1, FN_WRITE_HOLD_REGISTER, MB_ACCESS_WRITE_ONLY, this->id, this->slaveId, "Cmd(1:INFO,2:RESET)", this->name.c_str()); - registerBlock(cmdReg); - - if (!_loadcells[0]) - { - L_ERROR("PressCylinder setup failed: No Loadcell provided"); - return E_INVALID_PARAMETER; - } - if (!_solenoids[0]) - { - L_ERROR("PressCylinder setup failed: No Solenoid provided"); - return E_INVALID_PARAMETER; - } - loadSettings(); - m_interlocked.update(_settings.interlocked); - m_mode.update(_settings.mode); - SOLENOIDS_OFF(); - return E_OK; -} - -bool PressCylinder::pvsOk() -{ - uint8_t validCnt = 0; - for (int i = 0; i < PRESS_CYLINDER_MAX_PAIRS; ++i) - { - if (_loadcells[i] && _loadcells[i]->enabled()) - { - if (_loadcells[i]->getWeight(_pvs_raw[i])) - { - ++validCnt; - } - else - { - return false; // sensor error - } - } - } - return validCnt > 0; -} - -/** - * @brief Handles control logic for single-cylinder operational modes. - * - * This function is responsible for modes that only operate on the first loadcell/solenoid pair (`_loadcells[0]`, `_solenoids[0]`). - * - In `MODE_MANUAL`, it directly controls the solenoid based on joystick position. - * - In `MODE_AUTO`, it compares the PV against the target setpoint to turn the solenoid on or off. - * - `MODE_REMOTE` is a stub and ensures the solenoid is off. - * - * @return short E_OK on success, or an error code. - */ -short PressCylinder::loopSingle() -{ - uint32_t pv_raw = _pvs_raw[0]; - - // MIN_LOAD safety check - prevent any activation below minimum load - if ((m_cflags.getValue() & E_PC_OP_CHECK_MINLOAD) && pv_raw < _settings.min_load) - { - SOLENOID_OFF(0); - m_state.update(STATE_ERROR); - m_error_code.update(E_PC_MINLOAD); - return E_PC_MINLOAD; - } - - switch (m_mode.getValue()) - { - case MODE_MANUAL: - if (_joystick) - { - Joystick::E_POSITION joyPos = _joystick->getPosition(); - if (joyPos == Joystick::E_POSITION::UP) - { - onJoystickUp(); - } - else - { // DOWN, CENTER, or any other state - SOLENOID_OFF(0); - } - } - else - { - SOLENOID_OFF(0); - } - break; - case MODE_AUTO: - if (pv_raw < ((_settings.maxload_threshold * m_targetSP.getValue()) / 100)) - { - SOLENOID_ON(0); - } - else - { - SOLENOID_OFF(0); - } - break; - default: - break; - } - return E_OK; -} - -/** - * @brief Handles control logic for multi-cylinder operational modes. - * - * This function manages coordinated pressing using multiple loadcell/solenoid pairs. - * It uses several lambda functions to encapsulate safety and control logic: - * - `is_unbalanced`: Checks if the pressure difference between cylinders exceeds a threshold. - * - `has_stalled`: Detects if pressing has stalled with no progress. - * - `canPress`/`shouldRelease`/`manage_solenoid`: Encapsulate the core press/release logic for an individual cylinder. - * - * It implements the following modes: - * - `MODE_MANUAL_MULTI`: Activates all solenoids based on joystick input. - * - `MODE_AUTO_MULTI`: Presses with all cylinders simultaneously, aborting if an imbalance or stall is detected. - * - `MODE_AUTO_MULTI_BALANCED`: Alternately presses with individual cylinders to correct imbalances and reach the target SP. - * - * A fatal balance error (`E_PC_BALANCE_MAX_DIFF`) will halt the system if the PV difference is too large. - * - * @return short E_OK on success, or an error code. - */ -short PressCylinder::loopMulti() -{ - auto is_lc_active = [&](int i) - { - return _loadcells[i] && _loadcells[i]->enabled(); - }; - - // MIN_LOAD safety check - prevent any activation below minimum load - if (m_cflags.getValue() & E_PC_OP_CHECK_MINLOAD) - { - for (int i = 0; i < PRESS_CYLINDER_MAX_PAIRS; ++i) - { - if (is_lc_active(i) && _pvs_raw[i] < _settings.min_load) - { - SOLENOIDS_OFF(); - m_state.update(STATE_ERROR); - m_error_code.update(E_PC_MINLOAD); - return E_PC_MINLOAD; - } - } - } - - auto is_unbalanced = [&](uint16_t diff_threshold) - { - uint16_t min_pv = -1, max_pv = 0; - for (int i = 0; i < PRESS_CYLINDER_MAX_PAIRS; ++i) - { - if (is_lc_active(i)) - { - if (_pvs_raw[i] < min_pv) - min_pv = _pvs_raw[i]; - if (_pvs_raw[i] > max_pv) - max_pv = _pvs_raw[i]; - } - } - return (max_pv - min_pv) > diff_threshold; - }; - - auto has_stalled = [&](uint32_t last_pv) - { - if (!(m_cflags.getValue() & E_PC_OP_CHECK_STALLED)) - return false; - - uint32_t total_activations = 0; - for (int i = 0; i < PRESS_CYLINDER_MAX_PAIRS; ++i) - { - if (_solenoids[i]) - total_activations += _solenoids[i]->getActivationCount(); - } - - if (total_activations < _settings.max_stall_activations) - return false; // Not enough data - - uint32_t min_pv_stalled = UINT32_MAX; - for (int i = 0; i < PRESS_CYLINDER_MAX_PAIRS; ++i) - { - if (_solenoids[i] && _solenoids[i]->getActivationCount() > _settings.max_stall_activations) - { - if (is_lc_active(i) && _pvs_raw[i] < last_pv + _settings.balance_min_pv_diff) - { - return true; - } - } - } - return false; - }; - - uint32_t target_sp_raw = (_settings.maxload_threshold * m_targetSP.getValue()) / 100; - uint32_t target_with_deadband = target_sp_raw * (100 + _settings.sp_deadband_percent) / 100; - - auto canPress = [&](uint32_t pv_raw) - { - return pv_raw < target_sp_raw; - }; - - auto shouldRelease = [&](uint32_t pv_raw) - { - return pv_raw >= target_with_deadband; - }; - - auto errorCodeToString = [](int error_code) -> const char * - { - switch ((E_PC_ErrorCode)error_code) - { - case E_PC_OK: - return "OK"; - case E_PC_MINLOAD: - return "MINLOAD"; - case E_PC_OVERLOAD: - return "OVERLOAD"; - case E_PC_BALANCE_MAX_DIFF: - return "BALANCE_MAX_DIFF"; - case E_PC_LOADCELL_ERROR: - return "LOADCELL_ERROR"; - case E_PC_MAX_TIME: - return "MAX_TIME"; - case E_PC_STALLED: - return "STALLED"; - case E_PC_AUTO_TIMEOUT: - return "AUTO_TIMEOUT"; - default: - return "UNKNOWN"; - } - }; - - auto abort = [&](int error_code) - { - if (error_code == E_PC_OVERLOAD) - { - info(0, 0); - } - SOLENOIDS_OFF(); - m_state.update(STATE_ERROR); - m_error_code.update(_last_error_code); - m_mode.update(MODE_MANUAL); - }; - - auto manage_solenoid = [&](int i) - { - if (is_lc_active(i) && _solenoids[i]) - { - if (_solenoids[i]->getValue()) - { // Solenoid is ON - if (shouldRelease(_pvs_raw[i])) - { - SOLENOID_OFF(i); - } - } - else - { // Solenoid is OFF - if (canPress(_pvs_raw[i])) - { - SOLENOID_ON(i); - } - } - } - else if (_solenoids[i]) - { - SOLENOID_OFF(i); - } - }; - - /** @brief Safety Check: Fatal Pressure Imbalance. Halts the system if the pressure difference between cylinders is dangerously high. */ - if (m_interlocked.getValue() && (m_cflags.getValue() & E_PC_OP_CHECK_BALANCE) && is_unbalanced(_settings.balance_max_pv_diff)) - { - _last_error_code = E_PC_BALANCE_MAX_DIFF; - m_error_code.update(_last_error_code); - m_state.update(STATE_ERROR); - L_ERROR("Fatal balance error! PV difference > %d. System halted. Operator reset required.", _settings.balance_max_pv_diff); - abort(_last_error_code); - return _last_error_code; - } - - switch (m_mode.getValue()) - { - case MODE_MANUAL_MULTI: - if (_joystick) - { - Joystick::E_POSITION joyPos = _joystick->getPosition(); - if (joyPos == Joystick::E_POSITION::UP) - { - onJoystickUp(); - } - else - { - SOLENOIDS_OFF(); - } - } - else - { - SOLENOIDS_OFF(); - } - break; - case MODE_AUTO_MULTI: - { - if (m_interlocked.getValue() && is_unbalanced(_settings.balance_min_pv_diff)) - { - L_WARN("PV difference too high in AUTO_MULTI, aborting."); - abort(E_PC_BALANCE_MAX_DIFF); - break; - } - uint32_t min_pv = UINT32_MAX; - for (int i = 0; i < PRESS_CYLINDER_MAX_PAIRS; ++i) - { - if (is_lc_active(i) && _pvs_raw[i] < min_pv) - min_pv = _pvs_raw[i]; - } - if (has_stalled(min_pv)) - { - abort(E_PC_STALLED); - break; - } - for (int i = 0; i < PRESS_CYLINDER_MAX_PAIRS; i++) - { - manage_solenoid(i); - } - } - break; - case MODE_AUTO_MULTI_BALANCED: - { - uint32_t min_pv = UINT32_MAX; - for (int i = 0; i < PRESS_CYLINDER_MAX_PAIRS; ++i) - { - if (is_lc_active(i) && _pvs_raw[i] < min_pv) - min_pv = _pvs_raw[i]; - } - if (has_stalled(min_pv)) - { - L_WARN("No progress in AUTO_MULTI_BALANCED, aborting."); - abort(E_PC_STALLED); - break; - } - - // Check if cylinders are in balance using deadband - bool cylinders_balanced = !is_unbalanced(PRESS_CYLINDER_DEFAULT_SP_DEADBAND_RAW); - - if (!cylinders_balanced) - { - // Out of balance: press on cylinder with lowest PV (most urgent) - uint32_t lowest_pv = UINT32_MAX; - int most_urgent_cylinder = 0; - for (int i = 0; i < PRESS_CYLINDER_MAX_PAIRS; ++i) - { - if (is_lc_active(i) && _pvs_raw[i] < lowest_pv) - { - lowest_pv = _pvs_raw[i]; - most_urgent_cylinder = i; - } - } - if (_balanced_mode_active_cylinder != most_urgent_cylinder) - { - L_INFO("AUTO_MULTI_BALANCED: Switching to most urgent cylinder %d (PV=%d)", most_urgent_cylinder, lowest_pv); - SOLENOID_OFF(_balanced_mode_active_cylinder); - _balanced_mode_active_cylinder = most_urgent_cylinder; - } - manage_solenoid(_balanced_mode_active_cylinder); - } - else - { - // Cylinders are balanced: use periodic pressing - if (m_interlocked.getValue() && is_unbalanced(_settings.balance_min_pv_diff)) - { - if (millis() - _last_balance_time > _settings.balance_interval) - { - L_INFO("AUTO_MULTI_BALANCED: Balanced cylinders, periodic switch from %d to %d", - _balanced_mode_active_cylinder, 1 - _balanced_mode_active_cylinder); - _last_balance_time = millis(); - _balanced_mode_active_cylinder = 1 - _balanced_mode_active_cylinder; - SOLENOID_OFF(1 - _balanced_mode_active_cylinder); - } - } - else - { - _last_balance_time = millis(); - } - manage_solenoid(_balanced_mode_active_cylinder); - } - } - break; - default: - break; - } - return E_OK; -} - -/** - * @brief Main loop function for the PressCylinder component. - * - * This function orchestrates the overall control logic of the PressCylinder. - * It handles mode switching, safety checks, and calls the appropriate loopSingle or loopMulti functions. - * - * @return short E_OK on success, or an error code. - */ -void PressCylinder::onJoystickDoubleUp() -{ - // Trigger HOLD - uint32_t max_pv = 0; - for (int i = 0; i < PRESS_CYLINDER_MAX_PAIRS; ++i) - { - if (_loadcells[i] && _loadcells[i]->enabled() && _pvs_raw[i] > max_pv) - { - max_pv = _pvs_raw[i]; - } - } - if (_settings.maxload_threshold > 0) - { - // Use NCLAMP to normalize and then scale to 100 - _hold_sp = (uint32_t)(NCLAMP(max_pv, 0, _settings.maxload_threshold) * 100); - - m_targetSP.update(_hold_sp); - L_INFO("HOLD triggered via Joystick Double-Up: SP=%d", _hold_sp); - - // Set flag momentarily (optional, but good for feedback) - uint16_t mode = m_outputMode.getValue(); - SBI_MASK(mode, E_PC_OM_HOLD); - m_outputMode.update(mode); - - // Switch to AUTO if in MANUAL - E_Mode cur_mode = (E_Mode)m_mode.getValue(); - if (cur_mode == MODE_MANUAL) - { - if (m_interlocked.getValue()) - { - m_mode.update(DEFAULT_AUTO_MODE_INTERLOCKED); - } - else - { - m_mode.update(DEFAULT_AUTO_MODE_NORMAL); - } - } - - // Clear flag immediately as requested "once set in motion" - CBI_MASK(mode, E_PC_OM_HOLD); - m_outputMode.update(mode); - } -} - -void PressCylinder::onJoystickUp() -{ - E_Mode current_mode = (E_Mode)m_mode.getValue(); - - if (current_mode == MODE_MANUAL) - { - uint32_t pv_raw = _pvs_raw[0]; - int32_t pv_signed = (int32_t)pv_raw; - if (pv_signed >= 0 && pv_raw >= _settings.maxload_threshold) - { - SOLENOID_OFF(0); - m_state.update(STATE_ERROR); - m_error_code.update(E_PC_OVERLOAD); - m_mode.update(MODE_MANUAL); - return; - } - SOLENOID_ON(0); - } - else if (current_mode == MODE_MANUAL_MULTI) - { - for (int i = 0; i < PRESS_CYLINDER_MAX_PAIRS; ++i) - { - if (_loadcells[i] && _loadcells[i]->enabled()) - { - int32_t pv_signed = (int32_t)_pvs_raw[i]; - if (pv_signed >= 0 && _pvs_raw[i] >= _settings.maxload_threshold) - { - SOLENOIDS_OFF(); - m_state.update(STATE_ERROR); - m_error_code.update(E_PC_OVERLOAD); - m_mode.update(MODE_MANUAL); - return; - } - } - } - SOLENOIDS_ON(); - } -} - -short PressCylinder::loop() -{ - Component::loop(); - - if (millis() - _last_loop_time < PRESSCYLINDER_INTERVAL) - { - return E_OK; - } - _last_loop_time = millis(); - - if (_joystick && _joystick->getPosition() == Joystick::E_POSITION::DOWN) - { - E_Mode current_mode = (E_Mode)m_mode.getValue(); - if (current_mode == MODE_AUTO || current_mode == MODE_AUTO_MULTI || current_mode == MODE_AUTO_MULTI_BALANCED) - { - m_targetSP.update(0); - } - SOLENOIDS_OFF(); - m_mode.update(MODE_MANUAL); - m_state.update(STATE_IDLE); - return E_OK; - } - - // Joystick Double-Up Detection for HOLD - if (_joystick && (m_cflags.getValue() & E_PC_OP_ENABLE_DOUBLE_CLICK)) - { - bool is_up = (_joystick->getPosition() == Joystick::E_POSITION::UP); - if (is_up && !_joy_was_up) // Rising edge - { - if (millis() - _last_joy_up_time < JOYSTICK_DOUBLE_CLICK_MS) - { - onJoystickDoubleUp(); - } - _last_joy_up_time = millis(); - } - _joy_was_up = is_up; - } - - auto errorCodeToString = [](int error_code) -> const char * - { - switch ((E_PC_ErrorCode)error_code) - { - case E_PC_OK: - return "OK"; - case E_PC_MINLOAD: - return "MINLOAD"; - case E_PC_OVERLOAD: - return "OVERLOAD"; - case E_PC_BALANCE_MAX_DIFF: - return "BALANCE_MAX_DIFF"; - case E_PC_LOADCELL_ERROR: - return "LOADCELL_ERROR"; - case E_PC_MAX_TIME: - return "MAX_TIME"; - case E_PC_STALLED: - return "STALLED"; - case E_PC_AUTO_TIMEOUT: - return "AUTO_TIMEOUT"; - default: - return "UNKNOWN"; - } - }; - - auto abort = [&](int error_code) - { - if (error_code == E_PC_OVERLOAD) - { - // info(0, 0); - } - SOLENOIDS_OFF(); - m_state.update(STATE_ERROR); - m_error_code.update(error_code); - m_mode.update(MODE_MANUAL); - L_ERROR("PressCylinder aborting: %s (%d)", errorCodeToString(error_code), error_code); - }; - - E_Mode current_mode = (E_Mode)m_mode.getValue(); - if (current_mode != _previous_mode) - { - /* - if ((current_mode == MODE_AUTO || current_mode == MODE_AUTO_MULTI || current_mode == MODE_AUTO_MULTI_BALANCED) && m_targetSP.getValue() == 0) - { - SOLENOIDS_OFF(); - m_mode.update(MODE_MANUAL); - m_state.update(STATE_IDLE); - _previous_mode = MODE_MANUAL; - return E_OK; - } - */ - if (current_mode == MODE_AUTO || current_mode == MODE_AUTO_MULTI || current_mode == MODE_AUTO_MULTI_BALANCED) - { - if (m_interlocked.getValue()) - { - _auto_interlocked_start_time = millis(); - for (int i = 0; i < PRESS_CYLINDER_MAX_PAIRS; ++i) - { - _pvs_raw_previous[i] = _pvs_raw[i]; - } - L_INFO("Auto mode started while interlocked, timeout timer started"); - } - } - else - { - _auto_interlocked_start_time = 0; - } - _previous_mode = current_mode; - } - - /** @brief Safety timeout for the solenoid. */ - if (m_cflags.getValue() & E_PC_OP_CHECK_MAX_TIME) - { - for (int i = 0; i < PRESS_CYLINDER_MAX_PAIRS; i++) - { - if (_solenoids[i] && _solenoids[i]->getCurrentOnDurationMs() > PRESS_MAX_PRESS_TIME) - { - L_WARN("Solenoid %d on for too long (%ums), turning off all solenoids for safety.", i, PRESS_MAX_PRESS_TIME); - abort(E_PC_MAX_TIME); - break; - } - } - } - - /** @brief Auto mode timeout when interlocked. */ - if (current_mode == MODE_AUTO && m_interlocked.getValue() && _auto_interlocked_start_time > 0) - { - if (millis() - _auto_interlocked_start_time > PRESS_AUTO_TIMEOUT) - { - L_WARN("Auto mode interlocked timeout (%ums), aborting.", PRESS_AUTO_TIMEOUT); - abort(E_PC_AUTO_TIMEOUT); - return E_OK; - } - } - - /** @brief Multi auto mode timeout when interlocked (only if flag enabled). */ - if ((m_cflags.getValue() & E_PC_OP_CHECK_MULTI_TIMEOUT) && - (current_mode == MODE_AUTO_MULTI || current_mode == MODE_AUTO_MULTI_BALANCED) && - m_interlocked.getValue() && _auto_interlocked_start_time > 0) - { - if (millis() - _auto_interlocked_start_time > PRESS_AUTO_TIMEOUT) - { - L_WARN("Multi auto mode interlocked timeout (%ums), aborting.", PRESS_AUTO_TIMEOUT); - abort(E_PC_AUTO_TIMEOUT); - return E_OK; - } - } - - /** @brief 1. Handle disabled state */ - if (!enabled()) - { - SOLENOIDS_OFF(); - m_state.update(STATE_IDLE); - return E_OK; - } - - if (m_cflags.getValue() & E_PC_OP_CHECK_LOADCELL) - { - for (int i = 0; i < PRESS_CYLINDER_MAX_PAIRS; i++) - { - if (_loadcells[i] && _loadcells[i]->enabled() && _loadcells[i]->lastErrorCode != 0) - { - abort(E_PC_LOADCELL_ERROR); - return E_OK; - } - } - } - - bool pv_ok = pvsOk(); - - for (int i = 0; i < PRESS_CYLINDER_MAX_PAIRS; ++i) - { - if (_loadcells[i] && _loadcells[i]->enabled() && _pvs_raw[i] <= INT32_MAX) - { - int32_t pv_signed = (int32_t)_pvs_raw[i]; - if (pv_signed >= 0 && _pvs_raw[i] >= _settings.maxload_threshold) - { - if (current_mode == MODE_AUTO_MULTI_BALANCED) - { - uint32_t balanced_threshold = (_settings.maxload_threshold * PRESS_CYLINDER_DEFAULT_OVERLOAD) / 100; - if (_pvs_raw[i] >= balanced_threshold) - { - abort(E_PC_OVERLOAD); - return E_OK; - } - } - else - { - abort(E_PC_OVERLOAD); - return E_OK; - } - } - } - } - - _last_error_code = E_PC_OK; - m_state.update(STATE_IDLE); - - if (!pv_ok && (m_cflags.getValue() & E_PC_OP_CHECK_MINLOAD)) - { - _last_error_code = E_PC_MINLOAD; - } - m_error_code.update(_last_error_code); - - if (_last_error_code != E_PC_OK) - { - m_state.update(STATE_ERROR); - SOLENOIDS_OFF(); - return E_OK; - } - - for (int i = 0; i < PRESS_CYLINDER_MAX_PAIRS; ++i) - { - if (m_pvs[i]) - { - m_pvs[i]->update(_loadcells[i] && _loadcells[i]->enabled() && _pvs_raw[i] <= INT32_MAX ? (_pvs_raw[i] * 100) / _settings.maxload_threshold : 0); - } - } - - /** @brief Advance timeout timer if significant PV progress detected during auto interlocked mode */ - if ((current_mode == MODE_AUTO || current_mode == MODE_AUTO_MULTI || current_mode == MODE_AUTO_MULTI_BALANCED) && - m_interlocked.getValue() && _auto_interlocked_start_time > 0) - { - bool should_advance_timeout = false; - - // Check for significant PV changes - for (int i = 0; i < PRESS_CYLINDER_MAX_PAIRS; ++i) - { - if (_loadcells[i] && _loadcells[i]->enabled()) - { - uint32_t pv_diff = (_pvs_raw[i] > _pvs_raw_previous[i]) ? (_pvs_raw[i] - _pvs_raw_previous[i]) : (_pvs_raw_previous[i] - _pvs_raw[i]); - if (pv_diff > _settings.balance_min_pv_diff) - { - should_advance_timeout = true; - L_INFO("Significant PV change detected (cylinder %d: %d->%d, diff=%d), advancing timeout timer", - i, _pvs_raw_previous[i], _pvs_raw[i], pv_diff); - break; - } - } - } - - // For MODE_AUTO_MULTI_BALANCED: advance timeout if cylinders are balanced and near target - if (!should_advance_timeout && current_mode == MODE_AUTO_MULTI_BALANCED) - { - uint32_t target_sp_raw = (_settings.maxload_threshold * m_targetSP.getValue()) / 100; - uint16_t min_pv = -1, max_pv = 0; - bool near_target = true; - - for (int i = 0; i < PRESS_CYLINDER_MAX_PAIRS; ++i) - { - if (_loadcells[i] && _loadcells[i]->enabled()) - { - if (_pvs_raw[i] < min_pv) - min_pv = _pvs_raw[i]; - if (_pvs_raw[i] > max_pv) - max_pv = _pvs_raw[i]; - // Check if this cylinder is reasonably close to target (within 2x deadband) - if (_pvs_raw[i] < (target_sp_raw - (2 * _settings.sp_deadband_percent * target_sp_raw / 100))) - { - near_target = false; - } - } - } - - bool cylinders_balanced = (max_pv - min_pv) <= PRESS_CYLINDER_DEFAULT_SP_DEADBAND_RAW; - - if (cylinders_balanced && near_target) - { - should_advance_timeout = true; - } - } - - if (should_advance_timeout) - { - _auto_interlocked_start_time = millis(); - } - } - - /** @brief Update previous PV values for next cycle */ - for (int i = 0; i < PRESS_CYLINDER_MAX_PAIRS; ++i) - { - if (_loadcells[i] && _loadcells[i]->enabled()) - { - _pvs_raw_previous[i] = _pvs_raw[i]; - } - } - - if (current_mode == MODE_MANUAL_MULTI || current_mode == MODE_AUTO_MULTI || current_mode == MODE_AUTO_MULTI_BALANCED) - { - loopMulti(); - } - else - { - loopSingle(); - } - - // Final safety override for manual modes - if ((current_mode == MODE_MANUAL || current_mode == MODE_MANUAL_MULTI) && (!_joystick || _joystick->getPosition() != Joystick::E_POSITION::UP)) - { - SOLENOIDS_OFF(); - } - - return E_OK; -} - -short PressCylinder::mb_tcp_write(MB_Registers *reg, short networkValue) -{ - if (!reg) - return E_INVALID_PARAMETER; - - short result = NetworkComponent::mb_tcp_write(reg, networkValue); - if (result != E_NOT_IMPLEMENTED) - { - // The base class handles enabled/disabled, and loop() will enforce safety. - if (reg->startAddress == (mb_tcp_base_address() + E_NVC_ENABLED) && networkValue == 0) - { - SOLENOIDS_OFF(); - } - return result; - } - - uint16_t address = reg->startAddress; - - if (address == (mb_tcp_base_address() + MB_OFS_HR_TARGET_SP)) - { - m_targetSP.update(networkValue); - return E_OK; - } - if (address == (mb_tcp_base_address() + MB_OFS_HR_MODE)) - { - int sp = m_targetSP.getValue(); - if (m_mode.getValue() != (E_Mode)networkValue) - { - reset(); - } - m_mode.update((E_Mode)networkValue); - _settings.mode = (uint8_t)networkValue; - saveSettings(); - if (sp > 0) - { - m_targetSP.update(sp); - } - return E_OK; - } - if (address == (mb_tcp_base_address() + MB_OFS_HR_CMD)) - { - E_Command cmd = (E_Command)networkValue; - switch (cmd) - { - case CMD_INFO: - info(0, 0); - return E_OK; - case CMD_RESET: - reset(); - return E_OK; - default: - return E_INVALID_PARAMETER; - } - } - if (address == (mb_tcp_base_address() + MB_OFS_HR_CFLAGS)) - { - m_cflags.update(networkValue); - return E_OK; - } - if (address == (mb_tcp_base_address() + MB_OFS_HR_OUTPUT_MODE)) - { - uint16_t old_flags = m_outputMode.getValue(); - uint16_t new_flags = networkValue; - - // Check for HOLD flag rising edge - if (!TEST_MASK(old_flags, E_PC_OM_HOLD) && TEST_MASK(new_flags, E_PC_OM_HOLD)) - { - // Calculate SP from current load - uint32_t max_pv = 0; - for (int i = 0; i < PRESS_CYLINDER_MAX_PAIRS; ++i) - { - if (_loadcells[i] && _loadcells[i]->enabled() && _pvs_raw[i] > max_pv) - { - max_pv = _pvs_raw[i]; - } - } - // Convert raw load to SP (0-100 scale based on maxload_threshold) - if (_settings.maxload_threshold > 0) - { - _hold_sp = (max_pv * 100) / _settings.maxload_threshold; - // Clamp to 100 - if (_hold_sp > 100) - _hold_sp = 100; - - m_targetSP.update(_hold_sp); - L_INFO("HOLD activated: Captured SP=%d from PV=%d", _hold_sp, max_pv); - } - } - - m_outputMode.update(new_flags); - return E_OK; - } - if (address == (mb_tcp_base_address() + MB_OFS_C_INTERLOCKED)) - { - bool was_interlocked = m_interlocked.getValue(); - m_interlocked.update(networkValue); - _settings.interlocked = m_interlocked.getValue(); - saveSettings(); - - E_Mode current_mode = (E_Mode)m_mode.getValue(); - if ((current_mode == MODE_AUTO || current_mode == MODE_AUTO_MULTI || current_mode == MODE_AUTO_MULTI_BALANCED)) - { - if (networkValue && !was_interlocked) - { - _auto_interlocked_start_time = millis(); - for (int i = 0; i < PRESS_CYLINDER_MAX_PAIRS; ++i) - { - _pvs_raw_previous[i] = _pvs_raw[i]; - } - L_INFO("Interlocked activated during auto mode, timeout timer started"); - } - else if (!networkValue && was_interlocked) - { - _auto_interlocked_start_time = 0; - L_INFO("Interlocked deactivated, timeout timer reset"); - } - } - return E_OK; - } - return E_INVALID_PARAMETER; -} - -short PressCylinder::mb_tcp_read(MB_Registers *reg) -{ - if (!reg) - return 0; - - short result = NetworkComponent::mb_tcp_read(reg); - if (result != E_NOT_IMPLEMENTED) - { - return result; - } - - uint16_t address = reg->startAddress; - - if (address == (mb_tcp_base_address() + MB_OFS_HR_TARGET_SP)) - { - return m_targetSP.getValue(); - } - if (address == (mb_tcp_base_address() + MB_OFS_HR_MODE)) - { - return m_mode.getValue(); - } - if (address >= (_baseAddress + MB_OFS_HR_PVS) && address < (_baseAddress + MB_OFS_HR_PVS + PRESS_CYLINDER_MAX_PAIRS)) - { - int index = address - (_baseAddress + MB_OFS_HR_PVS); - if (m_pvs[index]) - { - return m_pvs[index]->getValue(); - } - } - if (address == (mb_tcp_base_address() + MB_OFS_HR_STATE)) - { - return m_state.getValue(); - } - if (address == (mb_tcp_base_address() + MB_OFS_HR_ERROR_CODE)) - { - return m_error_code.getValue(); - } - if (address == (mb_tcp_base_address() + MB_OFS_HR_CFLAGS)) - { - return m_cflags.getValue(); - } - if (address == (mb_tcp_base_address() + MB_OFS_C_INTERLOCKED)) - { - return m_interlocked.getValue(); - } - if (address == (mb_tcp_base_address() + MB_OFS_HR_CMD)) - { - return 0; // Write-only register - } - return 0; -} - -bool PressCylinder::loadSettings(const char *filename) -{ - File file = LittleFS.open(filename, "r"); - if (!file) - { - L_WARN("Failed to open settings file: %s. Using default values.", filename); - _applyDefaultSettings(); - return false; - } - - JsonDocument doc; - DeserializationError error = deserializeJson(doc, file); - file.close(); - - if (error) - { - L_ERROR("Failed to parse settings file: %s", error.c_str()); - _applyDefaultSettings(); - return false; - } - - _settings.fromJSON(doc.as()); - L_INFO("Successfully loaded settings from %s", filename); - return true; -} - -bool PressCylinder::saveSettings(const char *filename) -{ - File file = LittleFS.open(filename, "w"); - if (!file) - { - L_ERROR("Failed to create settings file: %s", filename); - return false; - } - - JsonDocument doc; - _settings.toJSON(doc.to()); - - if (serializeJson(doc, file) == 0) - { - file.close(); - L_ERROR("Failed to write to settings file: %s", filename); - return false; - } - - file.close(); - L_INFO("Successfully saved settings to %s", filename); - return true; -} - -short PressCylinder::stop() -{ - SOLENOIDS_OFF(); - _last_error_code = E_PC_OK; - m_error_code.update(_last_error_code); - m_state.update(STATE_IDLE); - m_mode.update(MODE_MANUAL); - for (int i = 0; i < PRESS_CYLINDER_MAX_PAIRS; ++i) - { - if (_solenoids[i]) - { - _solenoids[i]->resetActivationCount(); - } - } - L_INFO("PressCylinder stopped"); - return E_OK; -} - -short PressCylinder::reset() -{ - L_INFO("PressCylinder reset"); - stop(); - _auto_interlocked_start_time = 0; - for (int i = 0; i < PRESS_CYLINDER_MAX_PAIRS; ++i) - { - _pvs_raw_previous[i] = 0; - } - return E_OK; -} - -/* -short PressCylinder::onError(short errorCode, const char* errorMessage) { - L_ERROR("PressCylinder error: %d - %s", errorCode, errorMessage); - return E_OK; -} -*/ - -short PressCylinder::info(short val0, short val1) -{ - L_INFO("PressCylinder info"); - bool has_loadcells = false; - for (uint8_t i = 0; i < PRESS_CYLINDER_MAX_PAIRS; ++i) - { - if (_loadcells[i]) - { - has_loadcells = true; - uint32_t weight = 0; - _loadcells[i]->getWeight(weight); - L_INFO("Loadcell %d PV: %d, Local PV: %d", i, weight, m_pvs[i] ? m_pvs[i]->getValue() : -1); - } - } - if (!has_loadcells) - { - L_INFO("Loadcell: null"); - } - - bool has_solenoids = false; - for (uint8_t i = 0; i < PRESS_CYLINDER_MAX_SOLENOIDS; ++i) - { - if (_solenoids[i]) - { - has_solenoids = true; - L_INFO("Solenoid %d: %s", i, _solenoids[i]->getValue() ? "ON" : "OFF"); - } - } - if (!has_solenoids) - { - L_INFO("Solenoid: null"); - } - if (_joystick) - { - L_INFO("Joystick: %d", _joystick->getPosition()); // TODO: add joystick position to info - } - else - { - L_INFO("Joystick: null"); - } - L_INFO("PressCylinder: Mode=%d, State=%d, Error=%d, Interlocked=%d", m_mode.getValue(), m_state.getValue(), _last_error_code, m_interlocked.getValue()); - if (m_mode.getValue() == MODE_AUTO_MULTI_BALANCED) - { - uint16_t min_pv = -1, max_pv = 0; - for (int i = 0; i < PRESS_CYLINDER_MAX_PAIRS; ++i) - { - if (_loadcells[i]) - { - uint32_t weight = 0; - if (_loadcells[i]->getWeight(weight)) - { - if (weight < min_pv) - min_pv = weight; - if (weight > max_pv) - max_pv = weight; - } - } - } - bool cylinders_balanced = (max_pv - min_pv) <= PRESS_CYLINDER_DEFAULT_SP_DEADBAND_RAW; - L_INFO("Balanced Mode: Active cylinder=%d, Status=%s (diff=%d)", - _balanced_mode_active_cylinder, - cylinders_balanced ? "Periodic" : "Urgent", - max_pv - min_pv); - } - L_INFO("Settings: TargetSP=%d, Homing=%d, Pressing=%d, Highload=%d, Maxload=%d, MinLoad=%d", - m_targetSP.getValue(), - _settings.homing_threshold, - _settings.pressing_threshold, - _settings.highload_threshold, - _settings.maxload_threshold, - _settings.min_load); - L_INFO("Balance Settings: Interval=%d, MinDiff=%d, MaxDiff=%d, StallActivations=%d", - _settings.balance_interval, - _settings.balance_min_pv_diff, - _settings.balance_max_pv_diff, - _settings.max_stall_activations); - if (_auto_interlocked_start_time > 0) - { - unsigned long elapsed = millis() - _auto_interlocked_start_time; - L_INFO("Auto Mode Interlocked: Elapsed=%ums, Timeout=%ums, Remaining=%ums", - elapsed, PRESS_AUTO_TIMEOUT, PRESS_AUTO_TIMEOUT - elapsed); - L_INFO("Previous PV values: %d, %d (for progress tracking)", - _pvs_raw_previous[0], _pvs_raw_previous[1]); - } - if (_loadcells[0]) - { - L_INFO("Loadcell 0: %d", _loadcells[0]->info()); - } - if (_loadcells[1]) - { - L_INFO("Loadcell 1: %d", _loadcells[1]->info()); - } - return E_OK; -} - -bool PressCylinder::isOverloaded() -{ - for (int i = 0; i < PRESS_CYLINDER_MAX_PAIRS; ++i) - { - if (_loadcells[i] && _pvs_raw[i] <= INT32_MAX) - { - int32_t pv_signed = (int32_t)_pvs_raw[i]; - if (pv_signed >= 0 && _pvs_raw[i] >= _settings.maxload_threshold) - { - return true; - } - } - } - return false; -} - -#endif // ENABLE_PRESS_CYLINDER +#include "PressCylinder.h" + +#ifdef ENABLE_PRESS_CYLINDER + +#include +#include +#include +#include +#include "config.h" +#include "features.h" +#include "StringUtils.h" +#include "xmath.h" +#include "macros.h" + +#include +#include +#include + +constexpr unsigned long PressCylinder::JOYSTICK_DOUBLE_CLICK_MS; +constexpr PressCylinder::E_Mode PressCylinder::DEFAULT_AUTO_MODE_NORMAL; +constexpr PressCylinder::E_Mode PressCylinder::DEFAULT_AUTO_MODE_INTERLOCKED; + +// #define ENABLE_MIN_LOAD_CHECK + +void PressCylinderSettings::fromJSON(const JsonVariantConst &doc) +{ + interlocked = doc["interlocked"] | true; + mode = doc["mode"] | (uint8_t)PressCylinder::MODE_MANUAL; + homing_threshold = doc["homing_threshold"] | PRESS_CYLINDER_DEFAULT_HOMING_THRESHOLD; + pressing_threshold = doc["pressing_threshold"] | PRESS_CYLINDER_DEFAULT_PRESSING_THRESHOLD; + highload_threshold = doc["highload_threshold"] | PRESS_CYLINDER_DEFAULT_HIGHLOAD_THRESHOLD; + maxload_threshold = doc["maxload_threshold"] | PRESS_CYLINDER_DEFAULT_MAXLOAD_THRESHOLD; + maxload_threshold = clamp(maxload_threshold, (uint32_t)1, (uint32_t)PRESS_CYLINDER_MAX_LOAD_CLAMP); + sp_deadband_percent = doc["sp_deadband_percent"] | PRESS_CYLINDER_DEFAULT_SP_DEADBAND_PERCENT; + min_load = doc["min_load"] | PRESS_CYLINDER_DEFAULT_MIN_LOAD; + balance_interval = doc["balance_interval"] | BALANCE_INTERVAL; + balance_min_pv_diff = doc["balance_min_pv_diff"] | BALANCE_MIN_PV_DIFF; + balance_max_pv_diff = doc["balance_max_pv_diff"] | BALANCE_MAX_PV_DIFF; + max_stall_activations = doc["max_stall_activations"] | PRESS_CYLINDER_MAX_STALL_ACTIVATIONS; +} + +void PressCylinderSettings::toJSON(JsonVariant doc) +{ + doc["homing_threshold"] = homing_threshold; + doc["pressing_threshold"] = pressing_threshold; + doc["highload_threshold"] = highload_threshold; + doc["maxload_threshold"] = maxload_threshold; + doc["sp_deadband_percent"] = sp_deadband_percent; + doc["min_load"] = min_load; + doc["balance_interval"] = balance_interval; + doc["balance_min_pv_diff"] = balance_min_pv_diff; + doc["balance_max_pv_diff"] = balance_max_pv_diff; + doc["max_stall_activations"] = max_stall_activations; + doc["interlocked"] = interlocked; + doc["mode"] = mode; +} + +void PressCylinder::_applyDefaultSettings() +{ + _settings.homing_threshold = PRESS_CYLINDER_DEFAULT_HOMING_THRESHOLD; + _settings.pressing_threshold = PRESS_CYLINDER_DEFAULT_PRESSING_THRESHOLD; + _settings.highload_threshold = PRESS_CYLINDER_DEFAULT_HIGHLOAD_THRESHOLD; + _settings.maxload_threshold = PRESS_CYLINDER_DEFAULT_MAXLOAD_THRESHOLD; + _settings.sp_deadband_percent = PRESS_CYLINDER_DEFAULT_SP_DEADBAND_PERCENT; + _settings.min_load = PRESS_CYLINDER_DEFAULT_MIN_LOAD; + _settings.balance_interval = BALANCE_INTERVAL; + _settings.balance_min_pv_diff = BALANCE_MIN_PV_DIFF; + _settings.balance_max_pv_diff = BALANCE_MAX_PV_DIFF; + _settings.max_stall_activations = PRESS_CYLINDER_MAX_STALL_ACTIVATIONS; + _settings.max_stall_activations = PRESS_CYLINDER_MAX_STALL_ACTIVATIONS; + _settings.interlocked = true; + _settings.mode = MODE_MANUAL; +} + +PressCylinder::PressCylinder(Component *owner, short id, short mbAddress, Joystick *joystick, PushButton *pushButton) + : NetworkComponent(mbAddress, "PressCylinder", id, COMPONENT_DEFAULT, owner), + m_targetSP(this, id, "Target SP"), + m_mode(this, id, "Mode(0:MANUAL,1:AUTO,2:MANUAL_MULTI,3:AUTO_MULTI,4:AUTO_MULTI_BALANCED,5:REMOTE)"), + m_state(this, id, "State(0:IDLE,1:HOMING,2:PRESSING,3:HIGHLOAD,4:MAXLOAD,5:ERROR)"), + m_error_code(this, id, "Error(0:OK,1:MINLOAD,2:OVERLOAD,3:BALANCE_MAX_DIFF,4:LOADCELL,5:MAX_TIME,6:STALLED,7:AUTO_TIMEOUT)"), + m_cflags(this, id, "CFlags(1:MINLOAD,2:MAX_TIME,4:STALLED,8:BALANCE,16:LOADCELL,32:MULTI_TIMEOUT,64:DBL_CLK)"), + m_outputMode(this, id, "OutputMode(1:HOLD)"), + m_cmd(this, id, "Cmd(1:INFO,2:RESET)"), + m_interlocked(this, id, "Interlocked"), + _joystick(joystick), + _pushButton(pushButton), + _last_error_code(E_PC_OK), + _last_balance_time(0), + _auto_interlocked_start_time(0), + _balanced_mode_active_cylinder(0), + _previous_mode(MODE_MANUAL), + _hold_sp(0), + _last_joy_up_time(0), + _joy_was_up(false) +{ + m_cflags.update(E_PC_OP_ALL); + _applyDefaultSettings(); + for (uint8_t i = 0; i < PRESS_CYLINDER_MAX_PAIRS; ++i) + { + _loadcells[i] = nullptr; + _solenoids[i] = nullptr; + _pv_names[i].format("PV %d", i); + m_pvs[i] = new PressCylinderValue(this, id, _pv_names[i].c_str()); + _pvs_raw_previous[i] = 0; + } +} + +short PressCylinder::addLoadcell(Loadcell *loadcell) +{ + for (uint8_t i = 0; i < PRESS_CYLINDER_MAX_PAIRS; ++i) + { + if (!_loadcells[i]) + { + _loadcells[i] = loadcell; + return E_OK; + } + } + L_ERROR("Cannot add more loadcells to PressCylinder"); + return E_INVALID_PARAMETER; +} + +short PressCylinder::addSolenoid(Solenoid *solenoid) +{ + for (uint8_t i = 0; i < PRESS_CYLINDER_MAX_SOLENOIDS; ++i) + { + if (!_solenoids[i]) + { + _solenoids[i] = solenoid; + return E_OK; + } + } + L_ERROR("Cannot add more solenoids to PressCylinder"); + return E_INVALID_PARAMETER; +} + +short PressCylinder::setup() +{ + NetworkComponent::setup(); + + for (int i = 0; i < PRESS_CYLINDER_MAX_PAIRS; ++i) + { + if (m_pvs[i]) + { + SETUP_NETWORK_VALUE((*m_pvs[i]), (MB_OFS_HR_PVS + i), FN_READ_HOLD_REGISTER, _pv_names[i].c_str(), 0, 2, NetworkValue_ThresholdMode::DIFFERENCE); + } + } + SETUP_NETWORK_VALUE(m_targetSP, MB_OFS_HR_TARGET_SP, FN_WRITE_HOLD_REGISTER, "Target SP", 0, 1, NetworkValue_ThresholdMode::DIFFERENCE); + SETUP_NETWORK_VALUE(m_mode, MB_OFS_HR_MODE, FN_WRITE_HOLD_REGISTER, "Mode(0:NONE,1:MANUAL,2:AUTO,3:MANUAL_MULTI,4:AUTO_MULTI,5:AUTO_MULTI_BALANCED,6:REMOTE)", MODE_MANUAL, 1, NetworkValue_ThresholdMode::DIFFERENCE); + SETUP_NETWORK_VALUE(m_state, MB_OFS_HR_STATE, FN_READ_HOLD_REGISTER, "State(0:IDLE,1:MAXLOAD,2:ERROR)", STATE_IDLE, 1, NetworkValue_ThresholdMode::DIFFERENCE); + SETUP_NETWORK_VALUE(m_error_code, MB_OFS_HR_ERROR_CODE, FN_READ_HOLD_REGISTER, "Error(0:OK,1:MINLOAD,2:OVERLOAD,3:BALANCE_MAX_DIFF,4:LOADCELL,5:MAX_TIME,6:STALLED,7:AUTO_TIMEOUT)", E_PC_OK, 1, NetworkValue_ThresholdMode::DIFFERENCE); + SETUP_NETWORK_VALUE(m_cflags, MB_OFS_HR_CFLAGS, FN_WRITE_HOLD_REGISTER, "CFlags(1:MINLOAD,2:MAX_TIME,4:STALLED,8:BALANCE,16:LOADCELL,32:MULTI_TIMEOUT,64:DBL_CLK)", E_PC_OP_ALL, 1, NetworkValue_ThresholdMode::DIFFERENCE); + SETUP_NETWORK_VALUE(m_outputMode, MB_OFS_HR_OUTPUT_MODE, FN_WRITE_HOLD_REGISTER, "OutputMode(1:HOLD)", E_PC_OM_NONE, 1, NetworkValue_ThresholdMode::DIFFERENCE); + SETUP_NETWORK_VALUE(m_cmd, MB_OFS_HR_CMD, FN_WRITE_HOLD_REGISTER, "Cmd(1:INFO,2:RESET)", 0, 1, NetworkValue_ThresholdMode::DIFFERENCE); + SETUP_NETWORK_VALUE(m_interlocked, MB_OFS_C_INTERLOCKED, FN_WRITE_COIL, "Interlocked", true, 1, NetworkValue_ThresholdMode::DIFFERENCE); + + if (!_loadcells[0]) + { + L_ERROR("PressCylinder setup failed: No Loadcell provided"); + return E_INVALID_PARAMETER; + } + if (!_solenoids[0]) + { + L_ERROR("PressCylinder setup failed: No Solenoid provided"); + return E_INVALID_PARAMETER; + } + loadSettings(); + m_interlocked.update(_settings.interlocked); + m_mode.update(_settings.mode); + SOLENOIDS_OFF(); + return E_OK; +} + +bool PressCylinder::pvsOk() +{ + uint8_t validCnt = 0; + for (int i = 0; i < PRESS_CYLINDER_MAX_PAIRS; ++i) + { + if (_loadcells[i] && _loadcells[i]->enabled()) + { + if (_loadcells[i]->getWeight(_pvs_raw[i])) + { + ++validCnt; + } + else + { + return false; // sensor error + } + } + } + return validCnt > 0; +} + +const char *PressCylinder::getErrorString(int error_code) +{ + switch ((E_PC_ErrorCode)error_code) + { + case E_PC_OK: + return "OK"; + case E_PC_MINLOAD: + return "MINLOAD"; + case E_PC_OVERLOAD: + return "OVERLOAD"; + case E_PC_BALANCE_MAX_DIFF: + return "BALANCE_MAX_DIFF"; + case E_PC_LOADCELL_ERROR: + return "LOADCELL_ERROR"; + case E_PC_MAX_TIME: + return "MAX_TIME"; + case E_PC_STALLED: + return "STALLED"; + case E_PC_AUTO_TIMEOUT: + return "AUTO_TIMEOUT"; + default: + return "UNKNOWN"; + } +} + +void PressCylinder::onError(int error_code) +{ + // Log full state for debug + info(0, 0); + + SOLENOIDS_OFF(); + m_state.update(STATE_ERROR); + m_error_code.update(error_code); + _last_error_code = (E_PC_ErrorCode)error_code; + m_mode.update(MODE_MANUAL); + // L_ERROR("PressCylinder aborting: %s (%d)", getErrorString(error_code), error_code); + + JsonDocument doc; + doc["type"] = "error"; + doc["source"] = this->name; + doc["code"] = error_code; + doc["msg"] = getErrorString(error_code); + + if (this->owner) + { + this->owner->onMessage(this->id, EC_DISPATCH, E_MessageFlags::E_MF_NEW, &doc, this); + } +} + +/** + * @brief Handles control logic for single-cylinder operational modes. + * + * This function is responsible for modes that only operate on the first loadcell/solenoid pair (`_loadcells[0]`, `_solenoids[0]`). + * + * @param mode The operational mode to execute (e.g. MODE_AUTO, MODE_MANUAL). + * @param target_sp The target setpoint to control against. + * @return short E_OK on success, or an error code. + */ +short PressCylinder::loopSingle(E_Mode mode, uint32_t target_sp) +{ + uint32_t pv_raw = _pvs_raw[0]; + + // MIN_LOAD safety check - prevent any activation below minimum load + if ((m_cflags.getValue() & E_PC_OP_CHECK_MINLOAD) && pv_raw < _settings.min_load) + { + SOLENOID_OFF(0); + m_state.update(STATE_ERROR); + m_error_code.update(E_PC_MINLOAD); + return E_PC_MINLOAD; + } + + switch (mode) + { + case MODE_MANUAL: + case MODE_AUTO: + { + uint32_t target_val = (target_sp > 0) ? ((_settings.maxload_threshold * target_sp) / 100) : 0; + if (pv_raw < target_val) + { + SOLENOID_ON(0); + } + else + { + SOLENOID_OFF(0); + } + } + break; + default: + break; + } + return E_OK; +} + +/** + * @brief Handles control logic for multi-cylinder operational modes. + * + * @param mode The operational mode to execute. + * @param target_sp The target setpoint to control against. + * @return short E_OK on success, or an error code. + */ +short PressCylinder::loopMulti(E_Mode mode, uint32_t target_sp) +{ + if (target_sp == 0) + { + SOLENOIDS_OFF(); + return E_OK; + } + + auto is_lc_active = [&](int i) + { + return _loadcells[i] && _loadcells[i]->enabled(); + }; + + // MIN_LOAD safety check - prevent any activation below minimum load + if (m_cflags.getValue() & E_PC_OP_CHECK_MINLOAD) + { + for (int i = 0; i < PRESS_CYLINDER_MAX_PAIRS; ++i) + { + if (is_lc_active(i) && _pvs_raw[i] < _settings.min_load) + { + SOLENOIDS_OFF(); + m_state.update(STATE_ERROR); + m_error_code.update(E_PC_MINLOAD); + return E_PC_MINLOAD; + } + } + } + + auto is_unbalanced = [&](uint16_t diff_threshold) + { + uint16_t min_pv = -1, max_pv = 0; + for (int i = 0; i < PRESS_CYLINDER_MAX_PAIRS; ++i) + { + if (is_lc_active(i)) + { + if (_pvs_raw[i] < min_pv) + min_pv = _pvs_raw[i]; + if (_pvs_raw[i] > max_pv) + max_pv = _pvs_raw[i]; + } + } + return (max_pv - min_pv) > diff_threshold; + }; + + auto has_stalled = [&](uint32_t last_pv) + { + if (!(m_cflags.getValue() & E_PC_OP_CHECK_STALLED)) + return false; + + uint32_t total_activations = 0; + for (int i = 0; i < PRESS_CYLINDER_MAX_PAIRS; ++i) + { + if (_solenoids[i]) + total_activations += _solenoids[i]->getActivationCount(); + } + + if (total_activations < _settings.max_stall_activations) + return false; // Not enough data + + uint32_t min_pv_stalled = UINT32_MAX; + for (int i = 0; i < PRESS_CYLINDER_MAX_PAIRS; ++i) + { + if (_solenoids[i] && _solenoids[i]->getActivationCount() > _settings.max_stall_activations) + { + if (is_lc_active(i) && _pvs_raw[i] < last_pv + _settings.balance_min_pv_diff) + { + return true; + } + } + } + return false; + }; + + uint32_t target_sp_raw = (_settings.maxload_threshold * target_sp) / 100; + uint32_t target_with_deadband = target_sp_raw * (100 + _settings.sp_deadband_percent) / 100; + + auto canPress = [&](uint32_t pv_raw) + { + return pv_raw < target_sp_raw; + }; + + auto shouldRelease = [&](uint32_t pv_raw) + { + return pv_raw >= target_with_deadband; + }; + + auto manage_solenoid = [&](int i) + { + if (is_lc_active(i) && _solenoids[i]) + { + if (_solenoids[i]->getValue()) + { // Solenoid is ON + if (shouldRelease(_pvs_raw[i])) + { + SOLENOID_OFF(i); + } + } + else + { // Solenoid is OFF + if (canPress(_pvs_raw[i])) + { + SOLENOID_ON(i); + } + } + } + else if (_solenoids[i]) + { + SOLENOID_OFF(i); + } + }; + + /** @brief Safety Check: Fatal Pressure Imbalance. Halts the system if the pressure difference between cylinders is dangerously high. */ + if (m_interlocked.getValue() && (m_cflags.getValue() & E_PC_OP_CHECK_BALANCE) && is_unbalanced(_settings.balance_max_pv_diff)) + { + _last_error_code = E_PC_BALANCE_MAX_DIFF; + L_ERROR("Fatal balance error! PV difference > %d. System halted. Operator reset required.", _settings.balance_max_pv_diff); + onError(_last_error_code); + return _last_error_code; + } + + switch (mode) + { + case MODE_MANUAL_MULTI: + case MODE_AUTO_MULTI: + { + if (m_interlocked.getValue() && is_unbalanced(_settings.balance_min_pv_diff)) + { + L_WARN("PV difference too high in AUTO_MULTI, aborting."); + onError(E_PC_BALANCE_MAX_DIFF); + break; + } + uint32_t min_pv = UINT32_MAX; + for (int i = 0; i < PRESS_CYLINDER_MAX_PAIRS; ++i) + { + if (is_lc_active(i) && _pvs_raw[i] < min_pv) + min_pv = _pvs_raw[i]; + } + if (has_stalled(min_pv)) + { + onError(E_PC_STALLED); + break; + } + for (int i = 0; i < PRESS_CYLINDER_MAX_PAIRS; i++) + { + manage_solenoid(i); + } + } + break; + case MODE_AUTO_MULTI_BALANCED: + { + uint32_t min_pv = UINT32_MAX; + for (int i = 0; i < PRESS_CYLINDER_MAX_PAIRS; ++i) + { + if (is_lc_active(i) && _pvs_raw[i] < min_pv) + min_pv = _pvs_raw[i]; + } + if (has_stalled(min_pv)) + { + L_WARN("No progress in AUTO_MULTI_BALANCED, aborting."); + onError(E_PC_STALLED); + break; + } + + // Check if cylinders are in balance using deadband + bool cylinders_balanced = !is_unbalanced(PRESS_CYLINDER_DEFAULT_SP_DEADBAND_RAW); + + if (!cylinders_balanced) + { + // Out of balance: press on cylinder with lowest PV (most urgent) + uint32_t lowest_pv = UINT32_MAX; + int most_urgent_cylinder = 0; + for (int i = 0; i < PRESS_CYLINDER_MAX_PAIRS; ++i) + { + if (is_lc_active(i) && _pvs_raw[i] < lowest_pv) + { + lowest_pv = _pvs_raw[i]; + most_urgent_cylinder = i; + } + } + if (_balanced_mode_active_cylinder != most_urgent_cylinder) + { + L_INFO("AUTO_MULTI_BALANCED: Switching to most urgent cylinder %d (PV=%d)", most_urgent_cylinder, lowest_pv); + SOLENOID_OFF(_balanced_mode_active_cylinder); + _balanced_mode_active_cylinder = most_urgent_cylinder; + } + manage_solenoid(_balanced_mode_active_cylinder); + } + else + { + // Cylinders are balanced: use periodic pressing + if (m_interlocked.getValue() && is_unbalanced(_settings.balance_min_pv_diff)) + { + if (millis() - _last_balance_time > _settings.balance_interval) + { + L_INFO("AUTO_MULTI_BALANCED: Balanced cylinders, periodic switch from %d to %d", + _balanced_mode_active_cylinder, 1 - _balanced_mode_active_cylinder); + _last_balance_time = millis(); + _balanced_mode_active_cylinder = 1 - _balanced_mode_active_cylinder; + SOLENOID_OFF(1 - _balanced_mode_active_cylinder); + } + } + else + { + _last_balance_time = millis(); + } + manage_solenoid(_balanced_mode_active_cylinder); + } + } + break; + default: + break; + } + return E_OK; +} + +/** + * @brief Main loop function for the PressCylinder component. + * + * This function orchestrates the overall control logic of the PressCylinder. + * It handles mode switching, safety checks, and calls the appropriate loopSingle or loopMulti functions. + * + * @return short E_OK on success, or an error code. + */ +void PressCylinder::onJoystickDoubleUp() +{ + // Trigger HOLD + uint32_t max_pv = 0; + for (int i = 0; i < PRESS_CYLINDER_MAX_PAIRS; ++i) + { + if (_loadcells[i] && _loadcells[i]->enabled() && _pvs_raw[i] > max_pv) + { + max_pv = _pvs_raw[i]; + } + } + if (_settings.maxload_threshold > 0) + { + // Use NCLAMP to normalize and then scale to 100 + _hold_sp = (uint32_t)(NCLAMP(max_pv, 0, _settings.maxload_threshold) * 100); + + m_targetSP.update(_hold_sp); + L_INFO("HOLD triggered via Joystick Double-Up: SP=%d", _hold_sp); + + // Set flag momentarily (optional, but good for feedback) + uint16_t mode = m_outputMode.getValue(); + SBI_MASK(mode, E_PC_OM_HOLD); + m_outputMode.update(mode); + + // Switch to AUTO if in MANUAL + E_Mode cur_mode = (E_Mode)m_mode.getValue(); + if (cur_mode == MODE_MANUAL) + { + if (m_interlocked.getValue()) + { + m_mode.update(DEFAULT_AUTO_MODE_INTERLOCKED); + } + else + { + m_mode.update(DEFAULT_AUTO_MODE_NORMAL); + } + } + + // Clear flag immediately as requested "once set in motion" + CBI_MASK(mode, E_PC_OM_HOLD); + m_outputMode.update(mode); + } +} + +void PressCylinder::onJoystickUp() +{ + // Deprecated: Logic moved to loop() with virtual target SP +} + +short PressCylinder::loop() +{ + Component::loop(); + + if (millis() - _last_loop_time < PRESSCYLINDER_INTERVAL) + { + return E_OK; + } + _last_loop_time = millis(); + + if (_joystick && _joystick->getPosition() == Joystick::E_POSITION::DOWN) + { + E_Mode current_mode = (E_Mode)m_mode.getValue(); + if (current_mode == MODE_AUTO || current_mode == MODE_AUTO_MULTI || current_mode == MODE_AUTO_MULTI_BALANCED) + { + m_targetSP.update(0); + } + SOLENOIDS_OFF(); + m_mode.update(MODE_MANUAL); + m_state.update(STATE_IDLE); + return E_OK; + } + + /* + // Joystick Double-Up Detection for HOLD + if (_joystick && (m_cflags.getValue() & E_PC_OP_ENABLE_DOUBLE_CLICK)) + { + bool is_up = (_joystick->getPosition() == Joystick::E_POSITION::UP); + if (is_up && !_joy_was_up) // Rising edge + { + if (millis() - _last_joy_up_time < JOYSTICK_DOUBLE_CLICK_MS) + { + onJoystickDoubleUp(); + } + _last_joy_up_time = millis(); + } + _joy_was_up = is_up; + } + */ + + E_Mode current_mode = (E_Mode)m_mode.getValue(); + if (current_mode != _previous_mode) + { + /* + if ((current_mode == MODE_AUTO || current_mode == MODE_AUTO_MULTI || current_mode == MODE_AUTO_MULTI_BALANCED) && m_targetSP.getValue() == 0) + { + SOLENOIDS_OFF(); + m_mode.update(MODE_MANUAL); + m_state.update(STATE_IDLE); + _previous_mode = MODE_MANUAL; + return E_OK; + } + */ + if (current_mode == MODE_AUTO || current_mode == MODE_AUTO_MULTI || current_mode == MODE_AUTO_MULTI_BALANCED) + { + if (m_interlocked.getValue()) + { + _auto_interlocked_start_time = millis(); + for (int i = 0; i < PRESS_CYLINDER_MAX_PAIRS; ++i) + { + _pvs_raw_previous[i] = _pvs_raw[i]; + } + L_INFO("Auto mode started while interlocked, timeout timer started"); + } + } + else + { + _auto_interlocked_start_time = 0; + } + _previous_mode = current_mode; + } + + /** @brief Safety timeout for the solenoid. */ + if (m_cflags.getValue() & E_PC_OP_CHECK_MAX_TIME) + { + for (int i = 0; i < PRESS_CYLINDER_MAX_PAIRS; i++) + { + if (_solenoids[i] && _solenoids[i]->getCurrentOnDurationMs() > PRESS_MAX_PRESS_TIME) + { + L_WARN("Solenoid %d on for too long (%ums), turning off all solenoids for safety.", i, PRESS_MAX_PRESS_TIME); + onError(E_PC_MAX_TIME); + break; + } + } + } + + /** @brief Auto mode timeout when interlocked. */ + if (current_mode == MODE_AUTO && m_interlocked.getValue() && _auto_interlocked_start_time > 0) + { + if (millis() - _auto_interlocked_start_time > PRESS_AUTO_TIMEOUT) + { + L_WARN("Auto mode interlocked timeout (%ums), aborting.", PRESS_AUTO_TIMEOUT); + onError(E_PC_AUTO_TIMEOUT); + return E_OK; + } + } + + /** @brief Multi auto mode timeout when interlocked (only if flag enabled). */ + if ((m_cflags.getValue() & E_PC_OP_CHECK_MULTI_TIMEOUT) && + (current_mode == MODE_AUTO_MULTI || current_mode == MODE_AUTO_MULTI_BALANCED) && + m_interlocked.getValue() && _auto_interlocked_start_time > 0) + { + if (millis() - _auto_interlocked_start_time > PRESS_AUTO_TIMEOUT) + { + L_WARN("Multi auto mode interlocked timeout (%ums), aborting.", PRESS_AUTO_TIMEOUT); + onError(E_PC_AUTO_TIMEOUT); + return E_OK; + } + } + + /** @brief 1. Handle disabled state */ + if (!enabled()) + { + SOLENOIDS_OFF(); + m_state.update(STATE_IDLE); + return E_OK; + } + + if (m_cflags.getValue() & E_PC_OP_CHECK_LOADCELL) + { + uint8_t healthy_count = 0; + for (int i = 0; i < PRESS_CYLINDER_MAX_PAIRS; i++) + { + if (_loadcells[i] && _loadcells[i]->enabled()) + { + if (_loadcells[i]->lastErrorCode != 0) + { + m_interlocked.update(false); + return E_OK; + } + healthy_count++; + } + } + + if (!m_interlocked.getValue() && healthy_count == PRESS_CYLINDER_MAX_PAIRS) + { + m_interlocked.update(true); + } + } + + bool pv_ok = pvsOk(); + + for (int i = 0; i < PRESS_CYLINDER_MAX_PAIRS; ++i) + { + if (_loadcells[i] && _loadcells[i]->enabled() && _pvs_raw[i] <= INT32_MAX) + { + int32_t pv_signed = (int32_t)_pvs_raw[i]; + if (pv_signed >= 0 && _pvs_raw[i] >= _settings.maxload_threshold) + { + if (current_mode == MODE_AUTO_MULTI_BALANCED) + { + uint32_t balanced_threshold = (_settings.maxload_threshold * PRESS_CYLINDER_DEFAULT_OVERLOAD) / 100; + if (_pvs_raw[i] >= balanced_threshold) + { + onError(E_PC_OVERLOAD); + return E_OK; + } + } + else + { + onError(E_PC_OVERLOAD); + return E_OK; + } + } + } + } + + _last_error_code = E_PC_OK; + m_state.update(STATE_IDLE); + + if (!pv_ok && (m_cflags.getValue() & E_PC_OP_CHECK_MINLOAD)) + { + _last_error_code = E_PC_MINLOAD; + } + m_error_code.update(_last_error_code); + + if (_last_error_code != E_PC_OK) + { + m_state.update(STATE_ERROR); + SOLENOIDS_OFF(); + return E_OK; + } + + for (int i = 0; i < PRESS_CYLINDER_MAX_PAIRS; ++i) + { + if (m_pvs[i]) + { + m_pvs[i]->update(_loadcells[i] && _loadcells[i]->enabled() && _pvs_raw[i] <= INT32_MAX ? (_pvs_raw[i] * 100) / _settings.maxload_threshold : 0); + } + } + + /** @brief Advance timeout timer if significant PV progress detected during auto interlocked mode */ + if ((current_mode == MODE_AUTO || current_mode == MODE_AUTO_MULTI || current_mode == MODE_AUTO_MULTI_BALANCED) && + m_interlocked.getValue() && _auto_interlocked_start_time > 0) + { + bool should_advance_timeout = false; + + // Check for significant PV changes + for (int i = 0; i < PRESS_CYLINDER_MAX_PAIRS; ++i) + { + if (_loadcells[i] && _loadcells[i]->enabled()) + { + uint32_t pv_diff = (_pvs_raw[i] > _pvs_raw_previous[i]) ? (_pvs_raw[i] - _pvs_raw_previous[i]) : (_pvs_raw_previous[i] - _pvs_raw[i]); + if (pv_diff > _settings.balance_min_pv_diff) + { + should_advance_timeout = true; + L_INFO("Significant PV change detected (cylinder %d: %d->%d, diff=%d), advancing timeout timer", + i, _pvs_raw_previous[i], _pvs_raw[i], pv_diff); + break; + } + } + } + + // For MODE_AUTO_MULTI_BALANCED: advance timeout if cylinders are balanced and near target + if (!should_advance_timeout && current_mode == MODE_AUTO_MULTI_BALANCED) + { + uint32_t target_sp_raw = (_settings.maxload_threshold * m_targetSP.getValue()) / 100; + uint16_t min_pv = -1, max_pv = 0; + bool near_target = true; + + for (int i = 0; i < PRESS_CYLINDER_MAX_PAIRS; ++i) + { + if (_loadcells[i] && _loadcells[i]->enabled()) + { + if (_pvs_raw[i] < min_pv) + min_pv = _pvs_raw[i]; + if (_pvs_raw[i] > max_pv) + max_pv = _pvs_raw[i]; + // Check if this cylinder is reasonably close to target (within 2x deadband) + if (_pvs_raw[i] < (target_sp_raw - (2 * _settings.sp_deadband_percent * target_sp_raw / 100))) + { + near_target = false; + } + } + } + + bool cylinders_balanced = (max_pv - min_pv) <= PRESS_CYLINDER_DEFAULT_SP_DEADBAND_RAW; + + if (cylinders_balanced && near_target) + { + should_advance_timeout = true; + } + } + + if (should_advance_timeout) + { + _auto_interlocked_start_time = millis(); + } + } + + /** @brief Update previous PV values for next cycle */ + for (int i = 0; i < PRESS_CYLINDER_MAX_PAIRS; ++i) + { + if (_loadcells[i] && _loadcells[i]->enabled()) + { + _pvs_raw_previous[i] = _pvs_raw[i]; + } + } + + // Unified Control Logic: Determine Effective Mode and Target + E_Mode base_mode = (E_Mode)m_mode.getValue(); + E_Mode effective_mode = base_mode; + uint32_t effective_sp = m_targetSP.getValue(); + bool manual_input_active = false; + + // Check Inputs (Joystick only - PushButton is a modifier) + if (_joystick && _joystick->getPosition() == Joystick::E_POSITION::UP) + { + manual_input_active = true; + effective_sp = MANUAL_VIRTUAL_TARGET_PERCENT; + + // Check PushButton modifier + bool button_pressed = _pushButton && _pushButton->enabled() && (_pushButton->getState() == PushButton::State::PRESSED || _pushButton->getState() == PushButton::State::HELD); + + if (button_pressed) + { + // Joystick UP + Button Pressed -> Force Manual Single Cylinder + effective_mode = MODE_MANUAL; + } + else + { + // Joystick UP + Button Released -> Smart Auto / Interlock Dependent + // Only apply "Smart Auto" if we are currently in a Manual mode (not overriding an existing Auto run) + if (effective_mode == MODE_MANUAL || effective_mode == MODE_MANUAL_MULTI) + { + if (m_interlocked.getValue()) + { + effective_mode = DEFAULT_AUTO_MODE_INTERLOCKED; // e.g. AUTO_MULTI_BALANCED + } + else + { + effective_mode = DEFAULT_AUTO_MODE_NORMAL; // e.g. AUTO + } + } + } + } + + if (manual_input_active || current_mode == MODE_AUTO || current_mode == MODE_AUTO_MULTI || current_mode == MODE_AUTO_MULTI_BALANCED) + { + if (effective_mode == MODE_MANUAL_MULTI || effective_mode == MODE_AUTO_MULTI || effective_mode == MODE_AUTO_MULTI_BALANCED) + { + loopMulti(effective_mode, effective_sp); + } + else + { + loopSingle(effective_mode, effective_sp); + } + } + else + { + // Idle / Stop (Manual input released and not in Auto mode) + if (current_mode == MODE_MANUAL || current_mode == MODE_MANUAL_MULTI) + { + SOLENOIDS_OFF(); + } + else + { + // For other modes, we might need to be careful not to turn off if they have their own logic, + // but loopSingle/Multi covers most. REMOTE case is separate. + // Existing logic had a 'loopSingle()' fallback for 'else'. + // Here we explicitly turn off if standard manual/auto logic isn't running. + SOLENOIDS_OFF(); + } + } + + // Final safety override for manual modes + // Final safety override for manual modes - REMOVED (Handled by Unified Logic above) + + return E_OK; +} + +short PressCylinder::mb_tcp_write(MB_Registers *reg, short networkValue) +{ + if (!reg) + return E_INVALID_PARAMETER; + + short result = NetworkComponent::mb_tcp_write(reg, networkValue); + if (result != E_NOT_IMPLEMENTED) + { + // The base class handles enabled/disabled, and loop() will enforce safety. + if (reg->startAddress == (mb_tcp_base_address() + E_NVC_ENABLED) && networkValue == 0) + { + SOLENOIDS_OFF(); + } + return result; + } + + uint16_t address = reg->startAddress; + + if (address == (mb_tcp_base_address() + MB_OFS_HR_TARGET_SP)) + { + m_targetSP.update(networkValue); + return E_OK; + } + if (address == (mb_tcp_base_address() + MB_OFS_HR_MODE)) + { + int sp = m_targetSP.getValue(); + if (m_mode.getValue() != (E_Mode)networkValue) + { + reset(); + } + m_mode.update((E_Mode)networkValue); + _settings.mode = (uint8_t)networkValue; + saveSettings(); + if (sp > 0) + { + m_targetSP.update(sp); + } + return E_OK; + } + if (address == (mb_tcp_base_address() + MB_OFS_HR_CMD)) + { + E_Command cmd = (E_Command)networkValue; + switch (cmd) + { + case CMD_INFO: + info(0, 0); + return E_OK; + case CMD_RESET: + reset(); + return E_OK; + default: + return E_INVALID_PARAMETER; + } + } + if (address == (mb_tcp_base_address() + MB_OFS_HR_CFLAGS)) + { + m_cflags.update(networkValue); + return E_OK; + } + if (address == (mb_tcp_base_address() + MB_OFS_HR_OUTPUT_MODE)) + { + uint16_t old_flags = m_outputMode.getValue(); + uint16_t new_flags = networkValue; + + // Check for HOLD flag rising edge + if (!TEST_MASK(old_flags, E_PC_OM_HOLD) && TEST_MASK(new_flags, E_PC_OM_HOLD)) + { + // Calculate SP from current load + uint32_t max_pv = 0; + for (int i = 0; i < PRESS_CYLINDER_MAX_PAIRS; ++i) + { + if (_loadcells[i] && _loadcells[i]->enabled() && _pvs_raw[i] > max_pv) + { + max_pv = _pvs_raw[i]; + } + } + // Convert raw load to SP (0-100 scale based on maxload_threshold) + if (_settings.maxload_threshold > 0) + { + _hold_sp = (max_pv * 100) / _settings.maxload_threshold; + // Clamp to 100 + if (_hold_sp > 100) + _hold_sp = 100; + + m_targetSP.update(_hold_sp); + L_INFO("HOLD activated: Captured SP=%d from PV=%d", _hold_sp, max_pv); + } + } + + m_outputMode.update(new_flags); + return E_OK; + } + if (address == (mb_tcp_base_address() + MB_OFS_C_INTERLOCKED)) + { + bool was_interlocked = m_interlocked.getValue(); + m_interlocked.update(networkValue); + _settings.interlocked = m_interlocked.getValue(); + saveSettings(); + + E_Mode current_mode = (E_Mode)m_mode.getValue(); + if ((current_mode == MODE_AUTO || current_mode == MODE_AUTO_MULTI || current_mode == MODE_AUTO_MULTI_BALANCED)) + { + if (networkValue && !was_interlocked) + { + _auto_interlocked_start_time = millis(); + for (int i = 0; i < PRESS_CYLINDER_MAX_PAIRS; ++i) + { + _pvs_raw_previous[i] = _pvs_raw[i]; + } + L_INFO("Interlocked activated during auto mode, timeout timer started"); + } + else if (!networkValue && was_interlocked) + { + _auto_interlocked_start_time = 0; + L_INFO("Interlocked deactivated, timeout timer reset"); + } + } + return E_OK; + } + return E_INVALID_PARAMETER; +} + +short PressCylinder::mb_tcp_read(MB_Registers *reg) +{ + if (!reg) + return 0; + + short result = NetworkComponent::mb_tcp_read(reg); + if (result != E_NOT_IMPLEMENTED) + { + return result; + } + + uint16_t address = reg->startAddress; + + if (address == (mb_tcp_base_address() + MB_OFS_HR_TARGET_SP)) + { + return m_targetSP.getValue(); + } + if (address == (mb_tcp_base_address() + MB_OFS_HR_MODE)) + { + return m_mode.getValue(); + } + if (address >= (_baseAddress + MB_OFS_HR_PVS) && address < (_baseAddress + MB_OFS_HR_PVS + PRESS_CYLINDER_MAX_PAIRS)) + { + int index = address - (_baseAddress + MB_OFS_HR_PVS); + if (m_pvs[index]) + { + return m_pvs[index]->getValue(); + } + } + if (address == (mb_tcp_base_address() + MB_OFS_HR_STATE)) + { + return m_state.getValue(); + } + if (address == (mb_tcp_base_address() + MB_OFS_HR_ERROR_CODE)) + { + return m_error_code.getValue(); + } + if (address == (mb_tcp_base_address() + MB_OFS_HR_CFLAGS)) + { + return m_cflags.getValue(); + } + if (address == (mb_tcp_base_address() + MB_OFS_C_INTERLOCKED)) + { + return m_interlocked.getValue(); + } + if (address == (mb_tcp_base_address() + MB_OFS_HR_CMD)) + { + return 0; // Write-only register + } + return 0; +} + +bool PressCylinder::loadSettings(const char *filename) +{ + File file = LittleFS.open(filename, "r"); + if (!file) + { + L_WARN("Failed to open settings file: %s. Using default values.", filename); + _applyDefaultSettings(); + return false; + } + + JsonDocument doc; + DeserializationError error = deserializeJson(doc, file); + file.close(); + + if (error) + { + L_ERROR("Failed to parse settings file: %s", error.c_str()); + _applyDefaultSettings(); + return false; + } + + _settings.fromJSON(doc.as()); + L_INFO("Successfully loaded settings from %s", filename); + return true; +} + +bool PressCylinder::saveSettings(const char *filename) +{ + File file = LittleFS.open(filename, "w"); + if (!file) + { + L_ERROR("Failed to create settings file: %s", filename); + return false; + } + + JsonDocument doc; + _settings.toJSON(doc.to()); + + if (serializeJson(doc, file) == 0) + { + file.close(); + L_ERROR("Failed to write to settings file: %s", filename); + return false; + } + + file.close(); + L_INFO("Successfully saved settings to %s", filename); + return true; +} + +short PressCylinder::stop() +{ + SOLENOIDS_OFF(); + _last_error_code = E_PC_OK; + m_error_code.update(_last_error_code); + m_state.update(STATE_IDLE); + m_mode.update(MODE_MANUAL); + for (int i = 0; i < PRESS_CYLINDER_MAX_PAIRS; ++i) + { + if (_solenoids[i]) + { + _solenoids[i]->resetActivationCount(); + } + } + L_INFO("PressCylinder stopped"); + return E_OK; +} + +short PressCylinder::reset() +{ + L_INFO("PressCylinder reset"); + stop(); + _auto_interlocked_start_time = 0; + for (int i = 0; i < PRESS_CYLINDER_MAX_PAIRS; ++i) + { + _pvs_raw_previous[i] = 0; + } + return E_OK; +} + +/* +short PressCylinder::onError(short errorCode, const char* errorMessage) { + L_ERROR("PressCylinder error: %d - %s", errorCode, errorMessage); + return E_OK; +} +*/ + +short PressCylinder::info(short val0, short val1) +{ + L_INFO("PressCylinder info"); + bool has_loadcells = false; + for (uint8_t i = 0; i < PRESS_CYLINDER_MAX_PAIRS; ++i) + { + if (_loadcells[i]) + { + has_loadcells = true; + uint32_t weight = 0; + _loadcells[i]->getWeight(weight); + L_INFO("Loadcell %d PV: %d, Local PV: %d", i, weight, m_pvs[i] ? m_pvs[i]->getValue() : -1); + } + } + if (!has_loadcells) + { + L_INFO("Loadcell: null"); + } + + bool has_solenoids = false; + for (uint8_t i = 0; i < PRESS_CYLINDER_MAX_SOLENOIDS; ++i) + { + if (_solenoids[i]) + { + has_solenoids = true; + L_INFO("Solenoid %d: %s", i, _solenoids[i]->getValue() ? "ON" : "OFF"); + } + } + if (!has_solenoids) + { + L_INFO("Solenoid: null"); + } + if (_joystick) + { + L_INFO("Joystick: %d", _joystick->getPosition()); // TODO: add joystick position to info + } + else + { + L_INFO("Joystick: null"); + } + L_INFO("PressCylinder: Mode=%d, State=%d, Error=%d, Interlocked=%d", m_mode.getValue(), m_state.getValue(), _last_error_code, m_interlocked.getValue()); + if (m_mode.getValue() == MODE_AUTO_MULTI_BALANCED) + { + uint16_t min_pv = -1, max_pv = 0; + for (int i = 0; i < PRESS_CYLINDER_MAX_PAIRS; ++i) + { + if (_loadcells[i]) + { + uint32_t weight = 0; + if (_loadcells[i]->getWeight(weight)) + { + if (weight < min_pv) + min_pv = weight; + if (weight > max_pv) + max_pv = weight; + } + } + } + bool cylinders_balanced = (max_pv - min_pv) <= PRESS_CYLINDER_DEFAULT_SP_DEADBAND_RAW; + L_INFO("Balanced Mode: Active cylinder=%d, Status=%s (diff=%d)", + _balanced_mode_active_cylinder, + cylinders_balanced ? "Periodic" : "Urgent", + max_pv - min_pv); + } + L_INFO("Settings: TargetSP=%d, Homing=%d, Pressing=%d, Highload=%d, Maxload=%d, MinLoad=%d", + m_targetSP.getValue(), + _settings.homing_threshold, + _settings.pressing_threshold, + _settings.highload_threshold, + _settings.maxload_threshold, + _settings.min_load); + L_INFO("Balance Settings: Interval=%d, MinDiff=%d, MaxDiff=%d, StallActivations=%d", + _settings.balance_interval, + _settings.balance_min_pv_diff, + _settings.balance_max_pv_diff, + _settings.max_stall_activations); + if (_auto_interlocked_start_time > 0) + { + unsigned long elapsed = millis() - _auto_interlocked_start_time; + L_INFO("Auto Mode Interlocked: Elapsed=%ums, Timeout=%ums, Remaining=%ums", + elapsed, PRESS_AUTO_TIMEOUT, PRESS_AUTO_TIMEOUT - elapsed); + L_INFO("Previous PV values: %d, %d (for progress tracking)", + _pvs_raw_previous[0], _pvs_raw_previous[1]); + } + if (_loadcells[0]) + { + L_INFO("Loadcell 0: %d", _loadcells[0]->info()); + } + if (_loadcells[1]) + { + L_INFO("Loadcell 1: %d", _loadcells[1]->info()); + } + return E_OK; +} + +bool PressCylinder::isOverloaded() +{ + for (int i = 0; i < PRESS_CYLINDER_MAX_PAIRS; ++i) + { + if (_loadcells[i] && _pvs_raw[i] <= INT32_MAX) + { + int32_t pv_signed = (int32_t)_pvs_raw[i]; + if (pv_signed >= 0 && _pvs_raw[i] >= _settings.maxload_threshold) + { + return true; + } + } + } + return false; +} + +#endif // ENABLE_PRESS_CYLINDER diff --git a/src/components/PressCylinder.h b/src/components/PressCylinder.h index 25ab5e9b..518315be 100644 --- a/src/components/PressCylinder.h +++ b/src/components/PressCylinder.h @@ -14,6 +14,7 @@ class Loadcell; class Solenoid; +class PushButton; // Default pressure thresholds for states #define PRESS_CYLINDER_DEFAULT_HOMING_THRESHOLD 5 @@ -21,22 +22,22 @@ class Solenoid; #define PRESS_CYLINDER_DEFAULT_MAXLOAD_THRESHOLD 3200 #define PRESS_CYLINDER_DEFAULT_HIGHLOAD_THRESHOLD 3000 #define PRESS_CYLINDER_DEFAULT_OVERLOAD 120 -#define PRESS_CYLINDER_DEFAULT_SP_DEADBAND_PERCENT 7 -#define PRESS_CYLINDER_DEFAULT_SP_DEADBAND_RAW 100 +#define PRESS_CYLINDER_DEFAULT_SP_DEADBAND_PERCENT 5 +#define PRESS_CYLINDER_DEFAULT_SP_DEADBAND_RAW 50 #define PRESS_CYLINDER_DEFAULT_MIN_LOAD 10 #define PRESS_CYLINDER_MAX_LOAD_CLAMP 3500 #define PRESS_MAX_PRESS_TIME 35000 #define PRESS_AUTO_TIMEOUT 35000 #define PRESS_CYLINDER_AUTO_MODE_HOLD_DURATION_MS 1500 -#define BALANCE_INTERVAL 3000 -#define BALANCE_MIN_PV_DIFF 50 +#define BALANCE_INTERVAL 500 +#define BALANCE_MIN_PV_DIFF 200 #define BALANCE_MAX_PV_DIFF 350 -#define BALANCE_MIN_STEP_DIFF 300 +#define BALANCE_MIN_STEP_DIFF 200 #define PRESSCYLINDER_INTERVAL 20 #define PRESS_CYLINDER_MAX_STALL_ACTIVATIONS 4 -#define PRESS_CYLINDER_MB_COUNT 10 +#define PRESS_CYLINDER_MB_COUNT 12 #define PRESS_CYLINDER_MAX_PAIRS 2 #define PRESS_CYLINDER_MAX_LOADCELLS PRESS_CYLINDER_MAX_PAIRS #define PRESS_CYLINDER_MAX_SOLENOIDS PRESS_CYLINDER_MAX_PAIRS @@ -124,7 +125,7 @@ public: E_PC_OP_CHECK_MULTI_TIMEOUT = 1 << 5, E_PC_OP_ENABLE_DOUBLE_CLICK = 1 << 6, // E_PC_OP_ALL = E_PC_OP_CHECK_MAX_TIME | E_PC_OP_CHECK_STALLED | E_PC_OP_CHECK_BALANCE | E_PC_OP_CHECK_LOADCELL, - E_PC_OP_ALL = E_PC_OP_CHECK_MAX_TIME | E_PC_OP_ENABLE_DOUBLE_CLICK + E_PC_OP_ALL = E_PC_OP_CHECK_MAX_TIME | E_PC_OP_ENABLE_DOUBLE_CLICK | E_PC_OP_CHECK_LOADCELL }; enum E_PC_OutputMode { @@ -150,6 +151,7 @@ private: Loadcell *_loadcells[PRESS_CYLINDER_MAX_PAIRS]; Solenoid *_solenoids[PRESS_CYLINDER_MAX_PAIRS]; Joystick *_joystick; + PushButton *_pushButton; unsigned long _last_balance_time; unsigned long _auto_interlocked_start_time; uint32_t _pvs_raw_previous[PRESS_CYLINDER_MAX_PAIRS]; @@ -169,10 +171,11 @@ public: PressCylinderValue m_error_code; PressCylinderValue m_cflags; PressCylinderValue m_outputMode; + PressCylinderValue m_cmd; NetworkValue m_interlocked; PressCylinderSettings _settings; - PressCylinder(Component *owner, short id, short mbAddress, Joystick *joystick); + PressCylinder(Component *owner, short id, short mbAddress, Joystick *joystick, PushButton *pushButton); short addLoadcell(Loadcell *loadcell); short addSolenoid(Solenoid *solenoid); @@ -186,6 +189,7 @@ public: static constexpr unsigned long JOYSTICK_DOUBLE_CLICK_MS = 1200; static constexpr E_Mode DEFAULT_AUTO_MODE_NORMAL = MODE_AUTO; static constexpr E_Mode DEFAULT_AUTO_MODE_INTERLOCKED = MODE_AUTO_MULTI_BALANCED; + static constexpr uint8_t MANUAL_VIRTUAL_TARGET_PERCENT = 70; short mb_tcp_write(MB_Registers *reg, short networkValue) override; short mb_tcp_read(MB_Registers *reg) override; @@ -203,8 +207,10 @@ public: private: void _applyDefaultSettings(); bool pvsOk(); - short loopSingle(); - short loopMulti(); + const char *getErrorString(int error_code); + void onError(int error_code); + short loopSingle(E_Mode mode, uint32_t targetVal); + short loopMulti(E_Mode mode, uint32_t targetVal); }; #endif // ENABLE_PRESS_CYLINDER diff --git a/src/components/RestServer.cpp b/src/components/RestServer.cpp index 8ad897a8..1c0d1603 100644 --- a/src/components/RestServer.cpp +++ b/src/components/RestServer.cpp @@ -1558,20 +1558,7 @@ void RESTServer::handleWebSocketMessage(AsyncWebSocketClient *client, void *arg, if (len > 0 && Log.getLevel() >= LOG_LEVEL_VERBOSE) { - String data_str; - // Limit hex dump to a reasonable size to avoid flooding logs - size_t dump_len = len > 128 ? 128 : len; - for (size_t i = 0; i < dump_len; i++) - { - if (data[i] < 16) - data_str += "0"; - data_str += String(data[i], HEX); - data_str += " "; - } - if (len > 128) - { - data_str += "..."; - } + // Log.verbose("WS Message len: %d", len); - unsafe string build removed } if (info->final && info->index == 0 && info->len == len && info->opcode == WS_TEXT) @@ -1945,8 +1932,8 @@ void RESTServer::handleWebSocketMessage(AsyncWebSocketClient *client, void *arg, #ifdef ENABLE_WEBSOCKET_PB else if (command == "get_registers_pb") { - int currentPage = requestDoc.containsKey("page") ? requestDoc["page"].as() : 0; - int pageSize = requestDoc.containsKey("pageSize") ? requestDoc["pageSize"].as() : DEFAULT_WEBSOCKET_PAGE_SIZE; + int currentPage = !requestDoc["page"].isNull() ? requestDoc["page"].as() : 0; + int pageSize = !requestDoc["pageSize"].isNull() ? requestDoc["pageSize"].as() : DEFAULT_WEBSOCKET_PAGE_SIZE; if (pageSize <= 0) pageSize = DEFAULT_WEBSOCKET_PAGE_SIZE; if (pageSize > MAX_PAGE_SIZE) @@ -2240,37 +2227,37 @@ void RESTServer::broadcast(BroadcastMessageType type, const JsonDocument &data) case BROADCAST_USER_MESSAGE: typeStr = "user_message"; break; + case BROADCAST_ERROR_MESSAGE: + typeStr = "error_message"; + break; default: break; } doc["type"] = typeStr; doc["data"] = data; - - if (type == BROADCAST_USER_MESSAGE) - { - doc["timestamp"] = millis(); - } - String output; serializeJson(doc, output); - if (type == BROADCAST_USER_MESSAGE) + if (type == BROADCAST_USER_MESSAGE || type == BROADCAST_ERROR_MESSAGE) { + doc["timestamp"] = millis(); userMessageHistory.addMessage(output.c_str()); } queueWsMessage(0, output); } +#ifdef ENABLE_WEBSOCKET void RESTServer::broadcastBinary(const uint8_t *data, size_t len) { - if (!ws.availableForWriteAll()) + // Check rate limit if needed, or simply broadcast + if (ws.count() > 0 && ws.availableForWriteAll()) { - return; + ws.binaryAll((uint8_t *)data, len); } - ws.binaryAll(data, len); } +#endif short RESTServer::onMessage(int id, E_CALLS verb, E_MessageFlags flags, void *user, Component *src) { @@ -2284,6 +2271,75 @@ short RESTServer::onMessage(int id, E_CALLS verb, E_MessageFlags flags, void *us if (verb == E_CALLS::EC_USER && user != nullptr) { MB_UpdateData *info = static_cast(user); + + // --- Binary Protocol Implementation --- +#ifdef USE_BINARY_UPDATES + if (info->functionCode == E_FN_CODE::FN_READ_HOLD_REGISTER || + info->functionCode == E_FN_CODE::FN_READ_INPUT_REGISTER || + info->functionCode == E_FN_CODE::FN_WRITE_HOLD_REGISTER || + info->functionCode == E_FN_CODE::FN_WRITE_MULT_REGISTERS) + { + BinaryRegisterUpdate update; + // update.type is initialized to 0x02 + update.slaveId = info->slaveId; + update.fc = (uint8_t)info->functionCode; + update.componentId = src ? src->id : 0; + update.address = info->address; + update.count = info->count; + update.reserved = 0; + + // Handle value extraction + if (info->functionCode == E_FN_CODE::FN_WRITE_MULT_REGISTERS && info->userData != nullptr) + { + // For multi-write, we might need a different packet or just send the first one? + // The current spec assumes single register update mostly. + // Let's take the first value for now as the 'value' field. + uint16_t *data = static_cast(info->userData); + update.value = data[0]; + } + else + { + update.value = (uint16_t)info->value; + } + + this->broadcastBinary((uint8_t *)&update, sizeof(update)); + lastBroadcastTime = millis(); + return E_OK; + } + else if (info->functionCode == E_FN_CODE::FN_READ_COIL || + info->functionCode == E_FN_CODE::FN_READ_DISCR_INPUT || + info->functionCode == E_FN_CODE::FN_WRITE_COIL || + info->functionCode == E_FN_CODE::FN_WRITE_MULT_COILS) + { + BinaryCoilUpdate update; + // update.type is initialized to 0x01 + update.slaveId = info->slaveId; + update.fc = (uint8_t)info->functionCode; + update.componentId = src ? src->id : 0; + update.address = info->address; + update.count = info->count; + + if (info->functionCode == E_FN_CODE::FN_WRITE_MULT_COILS && info->userData != nullptr) + { + bool *data = static_cast(info->userData); + update.value = data[0] ? 1 : 0; + } + else + { + update.value = info->value ? 1 : 0; + } + + this->broadcastBinary((uint8_t *)&update, sizeof(update)); + lastBroadcastTime = millis(); + return E_OK; + } +#endif + // --- End Binary Protocol --- + + // --- JSON Fallback (Original Logic) --- + // If USE_BINARY_UPDATES is undefined, OR if the message type wasn't handled above (though all types are covered) + // we fall through to here. Note: If USE_BINARY_UPDATES is defined, we returned E_OK above. + JsonDocument doc; doc["slaveId"] = info->slaveId; doc["address"] = info->address; @@ -2358,6 +2414,7 @@ uint8_t RESTServer::getConnectedClientsCount() const return ws.count(); } #endif + #ifdef ENABLE_WEBSOCKET void RESTServer::queueWsMessage(uint32_t clientId, const String &message) { @@ -2376,8 +2433,10 @@ void RESTServer::queueWsMessage(uint32_t clientId, const String &message) void RESTServer::processWsQueue() { - if (millis() - lastWsSendTime < WS_MIN_SEND_INTERVAL_MS) + if (millis() - lastWsSendTime < 100) + { return; + } WsMessage msg; if (wsTxQueue.peek(msg)) diff --git a/src/components/RestServer.h b/src/components/RestServer.h index 1bd8638c..f7130621 100644 --- a/src/components/RestServer.h +++ b/src/components/RestServer.h @@ -33,7 +33,8 @@ typedef enum : uint8_t BROADCAST_LOG_ENTRY, BROADCAST_SYSTEM_STATUS, BROADCAST_USER_DEFINED, - BROADCAST_USER_MESSAGE + BROADCAST_USER_MESSAGE, + BROADCAST_ERROR_MESSAGE } BroadcastMessageType; struct WsMessage @@ -42,6 +43,33 @@ struct WsMessage String message; }; +// --- Binary Protocol Structs --- +#pragma pack(push, 1) +struct BinaryCoilUpdate +{ + uint8_t type = 0x01; // BROADCAST_COIL_UPDATE + uint8_t slaveId; + uint8_t fc; + uint8_t value; // 0 or 1 + uint16_t componentId; + uint16_t address; + uint16_t count; +}; + +struct BinaryRegisterUpdate +{ + uint8_t type = 0x02; // BROADCAST_REGISTER_UPDATE + uint8_t slaveId; + uint8_t fc; + uint8_t reserved; // Padding + uint16_t componentId; + uint16_t address; + uint16_t count; + uint16_t value; +}; +#pragma pack(pop) +// ---------------------------- + class WsTxQueue { public: diff --git a/src/modbus/ModbusRTU.cpp b/src/modbus/ModbusRTU.cpp index b434f9e5..10e1ce99 100644 --- a/src/modbus/ModbusRTU.cpp +++ b/src/modbus/ModbusRTU.cpp @@ -1355,7 +1355,7 @@ void ModbusRTU::onErrorReceived(Error error, uint32_t token) const char *errorString = modbusErrorToString(mbError); if (mbError != MB_Error::Timeout && MB_PRINT_ERRORS) { - Log.errorln("Modbus Error: Token=%u, Error=%d (%s) Slave %d, address %d", token, (int)error, errorString, slaveIdxOperation, address); + // Log.errorln("Modbus Error: Token=%u, Error=%d (%s) Slave %d, address %d", token, (int)error, errorString, slaveIdxOperation, address); } onErrorCallback(*operation, (int)error, errorString); CBI(operation->flags, OP_USED_BIT); diff --git a/src/profiles/PlotBase.cpp b/src/profiles/PlotBase.cpp index df55b270..5dbe3f8b 100644 --- a/src/profiles/PlotBase.cpp +++ b/src/profiles/PlotBase.cpp @@ -126,6 +126,31 @@ void PlotBase::seek(uint32_t targetMs) _startTimeMs = ULONG_MAX - (targetMs - now - 1); } } + + // Recursively seek children + for (int i = 0; i < MAX_PLOTS; ++i) + { + if (_plots[i] != nullptr) + { + _plots[i]->seek(targetMs); + } + } +} + +void PlotBase::slipTime(uint32_t deltaMs) +{ + if (_running && !_paused) + { + _startTimeMs += deltaMs; + } + // Recursively slip children + for (int i = 0; i < MAX_PLOTS; ++i) + { + if (_plots[i] != nullptr) + { + _plots[i]->slipTime(deltaMs); + } + } } uint32_t PlotBase::getElapsedMs() const @@ -216,6 +241,10 @@ PlotStatus PlotBase::getCurrentStatus() const { return PlotStatus::PAUSED; } + if (_waiting && _running) + { + return PlotStatus::WAITING; + } if (_running) { return PlotStatus::RUNNING; @@ -308,6 +337,7 @@ short PlotBase::startPlots() _plots[i]->start(); } } + return E_OK; } void PlotBase::onStop() { @@ -755,7 +785,7 @@ short PlotBase::setup() const char *group = getModbusGroupName(); - m_status.initModbus(_baseAddress + (ushort)PlotBaseRegisterOffset::STATUS, 1, this->id, this->slaveId, E_FN_CODE::FN_READ_HOLD_REGISTER, "Status(0:IDLE, 1:INITIALIZING, 2:RUNNING, 3:PAUSED, 4:STOPPED, 5:FINISHED)", group); + m_status.initModbus(_baseAddress + (ushort)PlotBaseRegisterOffset::STATUS, 1, this->id, this->slaveId, E_FN_CODE::FN_READ_HOLD_REGISTER, "Status(0:IDLE, 1:INITIALIZING, 2:RUNNING, 3:PAUSED, 4:STOPPED, 5:FINISHED, 6:WAITING)", group); m_status.initNotify(PlotStatus::IDLE, PlotStatus::IDLE, NetworkValue_ThresholdMode::DIFFERENCE); registerBlock(m_status.getRegisterInfo()); diff --git a/src/profiles/PlotBase.h b/src/profiles/PlotBase.h index de238260..e3f110ba 100644 --- a/src/profiles/PlotBase.h +++ b/src/profiles/PlotBase.h @@ -28,7 +28,8 @@ enum class PlotStatus : uint8_t RUNNING, // Actively running PAUSED, // Started, but currently paused STOPPED, // Explicitly stopped by user/logic - FINISHED // Reached or exceeded duration + FINISHED, // Reached or exceeded duration + WAITING, // Waiting for active delays to finish }; enum class PlotType : uint8_t @@ -82,7 +83,7 @@ public: PlotBase(Component *owner, ushort componentId, ushort modbusAddress, PlotType plotType = PlotType::Base) : NetworkComponent(modbusAddress, "PlotBase", componentId, Component::COMPONENT_DEFAULT, owner), _durationMs(0), _startTimeMs(0), _elapsedMsAtPause(0), _running(false), _paused(false), _explicitlyStopped(false), _userData(nullptr), _parent(nullptr), _eventsDelegate(nullptr), _plotType(plotType), _numControlPoints(0), max(0), - m_status(this, componentId, "Status(0:IDLE, 1:INITIALIZING, 2:RUNNING, 3:PAUSED, 4:STOPPED, 5:FINISHED)", static_cast(E_NetworkValueFeatureFlags::E_NVFF_ALL)), + m_status(this, componentId, "Status(0:IDLE, 1:INITIALIZING, 2:RUNNING, 3:PAUSED, 4:STOPPED, 5:FINISHED, 6:WAITING)", static_cast(E_NetworkValueFeatureFlags::E_NVFF_ALL)), m_currentValue(this, componentId, "CurrentValue", static_cast(E_NetworkValueFeatureFlags::E_NVFF_ALL)), m_duration(this, componentId, "Duration", static_cast(E_NetworkValueFeatureFlags::E_NVFF_ALL)), m_elapsed(this, componentId, "Elapsed", static_cast(E_NetworkValueFeatureFlags::E_NVFF_ALL)), @@ -151,6 +152,13 @@ public: */ virtual void seek(uint32_t targetMs); + /** + * @brief Slips the start time by the specified delta to compensate for lag. + * Recursively slips all child plots. + * @param deltaMs The amount of time to slip in milliseconds. + */ + virtual void slipTime(uint32_t deltaMs); + void setEventsDelegate(IPlotEvents *delegate) { _eventsDelegate = delegate; } // --- Sub-Plot Management --- @@ -170,6 +178,17 @@ public: */ bool isPaused() const { return _paused; } + /** + * @brief Checks if the plot is currently waiting (lagging). + * @return true if waiting, false otherwise. + */ + bool isWaiting() const { return _waiting; } + + /** + * @brief Sets the waiting status of the plot. + * @param waiting True if waiting, false otherwise. + */ + void setWaiting(bool waiting) { _waiting = waiting; } /** * @brief Gets the total duration of the plot. * @return Plot duration in milliseconds. @@ -311,6 +330,7 @@ protected: uint32_t _startTimeMs; bool _running; // True if started and not stopped bool _paused; // True if pause() called while running + bool _waiting = false; // True if waiting for lag compensation bool _explicitlyStopped; // True if stop() was called and not superseded by start() void *_userData; // Pointer for arbitrary user data diff --git a/src/profiles/SignalPlot.cpp b/src/profiles/SignalPlot.cpp index 86215dfe..26b97e03 100644 --- a/src/profiles/SignalPlot.cpp +++ b/src/profiles/SignalPlot.cpp @@ -40,18 +40,18 @@ short SignalPlot::setup() short SignalPlot::loop() { PlotBase::loop(); - + // Update all relevant NetworkValues from the base class PlotStatus currentStatus = getCurrentStatus(); m_status.update(currentStatus); - m_duration.update(getDuration() / 1000); // Assuming duration is in seconds for modbus + m_duration.update(getDuration() / 1000); // Assuming duration is in seconds for modbus m_remaining.update(getRemainingTime() / 1000); // Same for remaining - + uint32_t currentElapsedMs = getElapsedMs(); m_elapsed.update(currentElapsedMs / 1000); // Same for elapsed // Since SignalPlot doesn't have a value curve, we'll just update with 0 - m_currentValue.update(0); + m_currentValue.update(0); if (!isRunning() || !enabled()) { @@ -70,7 +70,8 @@ short SignalPlot::loop() return E_OK; } -bool SignalPlot::load(const JsonObject& config) { +bool SignalPlot::load(const JsonObject &config) +{ PlotBase::load(config); _numControlPoints = 0; // Reset count before loading new points for (int i = 0; i < MAX_SIGNAL_POINTS; ++i) @@ -242,12 +243,12 @@ void SignalPlot::executeControlPointAction(uint8_t cpIndex) return; } S_SignalControlPoint &cp = _controlPoints[cpIndex]; - + if (cp.state == E_SIGNAL_STATE::STATE_ON) { - return; // Already executed, skip + return; // Already executed, skip } - + switch (cp.type) { case E_SIGNAL_TYPE::MB_WRITE_COIL: @@ -276,7 +277,7 @@ void SignalPlot::executeControlPointAction(uint8_t cpIndex) if (gpioMode == E_GpioWriteMode::DIGITAL) { digitalWrite(pin, (value != 0) ? HIGH : LOW); - cp.state = E_SIGNAL_STATE::STATE_ON; + cp.state = E_SIGNAL_STATE::STATE_ON; } else if (gpioMode == E_GpioWriteMode::ANALOG_PWM) { @@ -302,7 +303,7 @@ void SignalPlot::executeControlPointAction(uint8_t cpIndex) } String cmdStr = String((char *)cp.user); - + Bridge *bridge = static_cast(owner->byId(COMPONENT_KEY_MB_BRIDGE)); if (!bridge) { @@ -320,7 +321,7 @@ void SignalPlot::executeControlPointAction(uint8_t cpIndex) } case E_SIGNAL_TYPE::DISPLAY_MESSAGE: { - #ifdef ENABLE_WEBSOCKET +#ifdef ENABLE_WEBSOCKET String message = cp.description; if (message.isEmpty()) { @@ -336,7 +337,7 @@ void SignalPlot::executeControlPointAction(uint8_t cpIndex) phApp->broadcast(BROADCAST_USER_MESSAGE, doc); } cp.state = E_SIGNAL_STATE::STATE_ON; - #endif +#endif break; } case E_SIGNAL_TYPE::PAUSE: @@ -369,7 +370,8 @@ void SignalPlot::executeControlPointAction(uint8_t cpIndex) case E_SIGNAL_TYPE::STOP_PIDS: { PlotBase *parent = getParent(); - if(parent){ + if (parent) + { PHApp *phApp = static_cast(owner); if (phApp) { @@ -383,7 +385,8 @@ void SignalPlot::executeControlPointAction(uint8_t cpIndex) case E_SIGNAL_TYPE::START_PIDS: { PlotBase *parent = getParent(); - if(parent){ + if (parent) + { PHApp *phApp = static_cast(owner); if (phApp) { @@ -393,7 +396,7 @@ void SignalPlot::executeControlPointAction(uint8_t cpIndex) cp.state = E_SIGNAL_STATE::STATE_ON; break; } - #ifdef ENABLE_FEEDBACK_BUZZER +#ifdef ENABLE_FEEDBACK_BUZZER case E_SIGNAL_TYPE::BUZZER_OFF: case E_SIGNAL_TYPE::BUZZER_SOLID: case E_SIGNAL_TYPE::BUZZER_SLOW_BLINK: @@ -431,11 +434,11 @@ void SignalPlot::executeControlPointAction(uint8_t cpIndex) } else { - cp.state = E_SIGNAL_STATE::STATE_ERROR; - } - break; + cp.state = E_SIGNAL_STATE::STATE_ERROR; } - #endif + break; + } +#endif case E_SIGNAL_TYPE::IFTTT_WEBHOOK: { #ifdef ENABLE_IFTTT diff --git a/src/profiles/TemperatureProfile.cpp b/src/profiles/TemperatureProfile.cpp index 055ce6a7..d5d6c177 100644 --- a/src/profiles/TemperatureProfile.cpp +++ b/src/profiles/TemperatureProfile.cpp @@ -79,6 +79,7 @@ short TemperatureProfile::loop() auto handleRunning = [&]() { + // --- Lag Compensation --- if (now - _lastLoopExecutionMs < TEMPERATURE_PROFILE_LOOP_INTERVAL_MS) { return; @@ -91,6 +92,9 @@ short TemperatureProfile::loop() } }; + // Check if initializing to reset cache if needed, or do it on start. + // Ideally we clear _omron on stop/start to handle config changes, but it's unlikely to change at runtime. + // --- State Machine --- switch (currentStatus) { diff --git a/src/profiles/TemperatureProfile.h b/src/profiles/TemperatureProfile.h index 8bdfc107..7c7c46a0 100644 --- a/src/profiles/TemperatureProfile.h +++ b/src/profiles/TemperatureProfile.h @@ -19,6 +19,7 @@ class ModbusTCP; #define TEMPERATURE_PROFILE_LOOP_INTERVAL_MS 1000 #define TEMP_PROFILE_ID_BASE 2000 #define TEMP_REGISTER_NAME_PREFIX "TProf" +#define TEMP_MAX_LAG 1000 * 60 * 5 // 5 minutes const uint16_t TEMP_PROFILE_REGISTER_COUNT = static_cast(PlotBaseRegisterOffset::_COUNT); @@ -39,6 +40,10 @@ enum class TemperatureProfileCommand : uint16_t class TemperatureProfile : public PlotBase { public: + uint32_t getLagDuration() const { return _lagDurationMs; } + void setLagDuration(uint32_t duration) { _lagDurationMs = duration; } + uint32_t getLastLogMs() const { return _lastLogMs; } + void setLastLogMs(uint32_t timestamp) { _lastLogMs = timestamp; } TemperatureProfile(Component *owner, short slot, ushort componentId, ushort modbusBaseAddress); virtual ~TemperatureProfile() = default; @@ -134,6 +139,12 @@ private: short _pressureProfileSlotId; PlotStatus _previousStatus; + + // Cached pointer to OmronE5 device for lag compensation + Component *_omron = nullptr; + + // Track how long we have been slipping due to lag + uint32_t _lagDurationMs = 0; }; #endif // TEMPERATURE_PROFILE_H \ No newline at end of file