diff --git a/Firmware/Atuador/Atuador_v14/Atuador_v14.ino b/Firmware/Atuador/Atuador_v14/Atuador_v14.ino index 7c000129b..3d14df39d 100644 --- a/Firmware/Atuador/Atuador_v14/Atuador_v14.ino +++ b/Firmware/Atuador/Atuador_v14/Atuador_v14.ino @@ -1,7 +1,7 @@ // Dispositivo: Atuador // Versão Firmware: 14 -// Ultima atualização: 08/06/2026 -// Atualização: Firmware mock +// Ultima atualização: 24/06/2026 +// Atualização: Inclusao do PID feedforward na bomba pressurizadora #include "C:\ZendionInc\agrobot_base\Firmware\Modulos\SerialService.h" #include "C:\ZendionInc\agrobot_base\Firmware\Modulos\CanService.h" @@ -130,8 +130,29 @@ BicoPulverizador_v2* BicosPulverizadores[QUANTIDADE_BICOS] = { #pragma endregion #pragma region BOMBAS -BombaPressurizadora_v2 sBMB_Linha("BMBLN", 51, 35, 1, 36, 2, TiposBarramentos::CONTROLADOR, Sentido::Horario, &sPRS_Linha); -BombaPressurizadora_v2 sBMB_Agitador("BMBAGT", 52, 37, 3, 38, 4, TiposBarramentos::CONTROLADOR, Sentido::Horario); +BombaPressurizadora_v2 sBMB_Linha( + "BMBLN", + 51, + 35, + 1, + 36, + 2, + TiposBarramentos::CONTROLADOR, + Sentido::Horario, + &sPRS_Linha, + BicosPulverizadores, + QUANTIDADE_BICOS +); +BombaPressurizadora_v2 sBMB_Agitador( + "BMBAGT", + 52, + 37, + 3, + 38, + 4, + TiposBarramentos::CONTROLADOR, + Sentido::Horario +); BombaPressurizadora_v2* ObterBombaPorId(uint8_t idNum) { switch (idNum) { diff --git a/Firmware/Modulos/BombaPressurizadoraModel_v2.h b/Firmware/Modulos/BombaPressurizadoraModel_v2.h index 16fc15a34..ecf5e4462 100644 --- a/Firmware/Modulos/BombaPressurizadoraModel_v2.h +++ b/Firmware/Modulos/BombaPressurizadoraModel_v2.h @@ -1,26 +1,30 @@ +#ifndef BombaPressurizadoraModel_v2_h +#define BombaPressurizadoraModel_v2_h + +#include +#include +#include + #include "SerialService.h" #include "Pinout.h" #include "Utils.h" #include "IComponenteCAN.h" #include "SensorPressaoModel_v2.h" - -#ifndef BombaPressurizadoraModel_v2_h -#define BombaPressurizadoraModel_v2_h +#include "BicoPulverizadorModel_v2.h" struct EstadoAnteriorBomba { Estado estado = Estado::Desligado; + double potenciaSP = 0.0; + double pressaoSP = 0.0; - double potenciaSP = 0; - double pressaoSP = 0; + ModoControleBomba modoControle = static_cast(0); - ModoControleBomba modoControle = ModoControleBomba::OnOff; + double kp = 0.0; + double ki = 0.0; + double kd = 0.0; - double kp = 0; - double ki = 0; - double kd = 0; - - double potenciaMinPID = 0; - double potenciaMaxPID = 100; + double potenciaMinPID = 0.0; + double potenciaMaxPID = 100.0; }; class BombaPressurizadora_v2 : public ComponenteCAN { @@ -32,18 +36,22 @@ class BombaPressurizadora_v2 : public ComponenteCAN { int canalIN1, int pinoIN2, int canalIN2, - TiposBarramentos barramento = CONTROLADOR, + TiposBarramentos barramento = TiposBarramentos::CONTROLADOR, Sentido sentido = Sentido::Horario, - SensorPressao_v2* sensor = nullptr + SensorPressao_v2* sensor = nullptr, + BicoPulverizador_v2** bicos = nullptr, + size_t qtdBicos = 0 ) : _ID(id), ID_Num(idNum), - _pinoIN1(pinoIN1, barramento, _PWM), - _pinoIN2(pinoIN2, barramento, _PWM), + _pinoIN1(pinoIN1, barramento, _OUTPUT), + _pinoIN2(pinoIN2, barramento, _OUTPUT), _canalIN1(canalIN1), _canalIN2(canalIN2), _sentido(sentido), - sensorPressao(sensor) { + sensorPressao(sensor), + _bicos(bicos), + _qtdBicos(qtdBicos) { } String _ID; @@ -51,10 +59,9 @@ class BombaPressurizadora_v2 : public ComponenteCAN { PinoModel _pinoIN1; PinoModel _pinoIN2; - + int _canalIN1 = 1; int _canalIN2 = 2; - Sentido _sentido = Sentido::Horario; bool Iniciado = false; @@ -64,40 +71,178 @@ class BombaPressurizadora_v2 : public ComponenteCAN { SensorPressao_v2* sensorPressao = nullptr; Estado EstadoLeitura = Estado::Desligado; - double PWM_SP = 0; - double PWM_Set = 0; - double PercentualPotenciaLeitura = 0; + int PWM_SP = 0; + int PWM_Set = 0; + int PotenciaLeitura = 0; + double PercentualPotenciaLeitura = 0.0; Estado _EstadoControle = Estado::Desligado; - double _PressaoSP = 0; - double _PotenciaSP = 0; + double _PressaoSP = 0.0; + double _PotenciaSP = 0.0; - ModoControleBomba ModoControle = ModoControleBomba::OnOff; + ModoControleBomba ModoControle = static_cast(0); - // PID - double Kp = 8.0; - double Ki = 0.4; + /* + Ganhos do controle fechado. + + Recomendacao inicial para campo: + - Comece com Kd = 0.0. + - Use o feedforward para carregar a demanda principal. + - Use Kp/Ki apenas para corrigir diferencas pequenas. + */ + double Kp = 1.8; + double Ki = 0.35; double Kd = 0.0; - double erroAnterior = 0; - double pressaoAnterior = 0.0; - double integralErro = 0; - unsigned long ultimoPID_ms = 0; - - // limites PID em percentual - double PotenciaMinPID = 25.0; + double PotenciaMinPID = 18.0; double PotenciaMaxPID = 100.0; - // proteção contra integral absurda - double IntegralMin = -100.0; - double IntegralMax = 100.0; + /* + Potencia minima realmente util da bomba. + Abaixo disso a bomba pode girar, mas nao pressurizar de verdade. + */ + double PotenciaMinAtiva = 22.0; - // zona morta para não ficar tremendo perto do SP - double DeadbandPressao = 1.0; + /* + Tabelas de feedforward. - void VincularSensorPressao(SensorPressao_v2* sensor) { - sensorPressao = sensor; - MalhaFechada = (sensorPressao != nullptr); + Indice = quantidade de bicos ligados. + Exemplo: PotenciaBase30Psi[4] = potencia base para 4 bicos ligados em 30 psi. + + Valores abaixo sao CHUTES INICIAIS seguros para teste. + Substitua pelos valores medidos em bancada/campo. + */ + double PotenciaBase15Psi[8] = { + 0.0, // 0 bicos + 20.0, // 1 bico + 26.0, // 2 bicos + 33.0, // 3 bicos + 41.0, // 4 bicos + 50.0, // 5 bicos + 60.0, // 6 bicos + 72.0 // 7 bicos + }; + + double PotenciaBase30Psi[8] = { + 0.0, // 0 bicos + 34.0, // 1 bico + 45.0, // 2 bicos + 57.0, // 3 bicos + 69.0, // 4 bicos + 81.0, // 5 bicos + 93.0, // 6 bicos + 100.0 // 7 bicos + }; + + double PressaoReferenciaBaixaPsi = 15.0; + double PressaoReferenciaAltaPsi = 30.0; + + /* + Histerese usada somente no modo ON/OFF antigo. + */ + double HistereseOnOffPsi = 3.0; + + /* + Filtro leve para pressao. + 0.0 = nao atualiza; 1.0 = sem filtro. + */ + double AlphaFiltroPressao = 0.35; + + /* + Limites da correcao PI em cima da tabela. + A tabela deve fazer o grosso. O PI so afina. + */ + double CorrecaoMinPI = -25.0; + double CorrecaoMaxPI = 25.0; + + /* + Rampa de potencia. + Subida mais rapida para nao deixar a linha morrer quando abrem bicos. + Descida mais suave para evitar pico quando fecham bicos. + */ + double TaxaSubidaPctPorSegundo = 350.0; + double TaxaDescidaPctPorSegundo = 180.0; + + unsigned long IntervaloControleMs = 50; + + /* + Por seguranca, a bomba desliga quando nao existe bico aberto em malha fechada. + Se houver bypass/retorno ou valvula reguladora e voce quiser manter linha pre-pressurizada, + altere para false. + */ + bool DesligarSemBicosAbertos = true; + + uint8_t QtdBicosLigados = 0; + double PressaoAtual = 0.0; + double PressaoFiltrada = 0.0; + double PotenciaFeedForwardAtual = 0.0; + double CorrecaoPIAtual = 0.0; + double PotenciaFinalAtual = 0.0; + + void ConfigurarBicos(BicoPulverizador_v2** bicos, size_t qtdBicos) { + _bicos = bicos; + _qtdBicos = qtdBicos; + } + + void ConfigurarMapaPotencia15Psi( + double b1, + double b2, + double b3, + double b4, + double b5, + double b6, + double b7 + ) { + PotenciaBase15Psi[0] = 0.0; + PotenciaBase15Psi[1] = b1; + PotenciaBase15Psi[2] = b2; + PotenciaBase15Psi[3] = b3; + PotenciaBase15Psi[4] = b4; + PotenciaBase15Psi[5] = b5; + PotenciaBase15Psi[6] = b6; + PotenciaBase15Psi[7] = b7; + } + + void ConfigurarMapaPotencia30Psi( + double b1, + double b2, + double b3, + double b4, + double b5, + double b6, + double b7 + ) { + PotenciaBase30Psi[0] = 0.0; + PotenciaBase30Psi[1] = b1; + PotenciaBase30Psi[2] = b2; + PotenciaBase30Psi[3] = b3; + PotenciaBase30Psi[4] = b4; + PotenciaBase30Psi[5] = b5; + PotenciaBase30Psi[6] = b6; + PotenciaBase30Psi[7] = b7; + } + + void ConfigurarPID( + double kp, + double ki, + double kd, + double potenciaMinPID, + double potenciaMaxPID + ) { + Kp = kp; + Ki = ki; + Kd = kd; + PotenciaMinPID = constrain(potenciaMinPID, 0.0, 100.0); + PotenciaMaxPID = constrain(potenciaMaxPID, PotenciaMinPID, 100.0); + + if (PotenciaMinAtiva < PotenciaMinPID) { + PotenciaMinAtiva = PotenciaMinPID; + } + } + + void ConfigurarModoControle(ModoControleBomba modo) { + ModoControle = modo; + ResetarControleFechado(); } void Inicializar() { @@ -106,117 +251,177 @@ class BombaPressurizadora_v2 : public ComponenteCAN { return; } - MalhaFechada = (sensorPressao != nullptr); + _pinoIN1.Conectar(false); + _pinoIN2.Conectar(false); - _pinoIN1.Conectar(0); - if (_pinoIN1.Definido()) { - ledcSetup(_canalIN1, 1000, 10); - ledcAttachPin(_pinoIN1.num, _canalIN1); - ledcWrite(_canalIN1, 0); - } - _pinoIN2.Conectar(0); - if (_pinoIN2.Definido()) { - ledcSetup(_canalIN2, 1000, 10); - ledcAttachPin(_pinoIN2.num, _canalIN2); - ledcWrite(_canalIN2, 0); + bool in1Ok = _pinoIN1.Definido(); + bool in2Ok = _pinoIN2.Definido(); + + if (!in1Ok || !in2Ok) { + Iniciado = false; + MostrarLog("Falha ao iniciar bomba: pinos invalidos"); + return; } - Iniciado = _pinoIN1.Definido() && _pinoIN2.Definido(); + ledcSetup(_canalIN1, PWM_FREQ_HZ, PWM_RESOLUTION_BITS); + ledcSetup(_canalIN2, PWM_FREQ_HZ, PWM_RESOLUTION_BITS); - if (Iniciado) { - _EstadoControle = Estado::Desligado; - PWM_Set = RampaMax; - PWM_SP = ZeroRampa; - PercentualPotenciaLeitura = 0; - EstadoLeitura = Estado::Desligado; - bombaDesligadaPorPressao = false; - MostrarLog(String("Bomba iniciada | malha ") + (MalhaFechada ? "fechada" : "aberta")); - } else { - MostrarLog("Falha ao iniciar bomba"); - } + ledcAttachPin(_pinoIN1.num, _canalIN1); + ledcAttachPin(_pinoIN2.num, _canalIN2); + + AplicarSaida(false, 0); + + Iniciado = true; + EstadoLeitura = Estado::Desligado; + _EstadoControle = Estado::Desligado; + + _ultimoControleMs = millis(); + _ultimoTempoDtMs = _ultimoControleMs; + + MostrarLog("Bomba iniciada com sucesso"); } void Desligar() { if (!Iniciado) { - MostrarLog("Bomba nao esta inicializada"); return; } - _EstadoControle = Estado::Desligado; - ledcWrite(_canalIN1, 0); - ledcWrite(_canalIN2, 0); + AplicarSaida(false, 0); _pinoIN1.Desconectar(); _pinoIN2.Desconectar(); - - PWM_Set = ZeroRampa; - PWM_SP = ZeroRampa; - PercentualPotenciaLeitura = 0; - EstadoLeitura = Estado::Desligado; - bombaDesligadaPorPressao = false; + Iniciado = false; + EstadoLeitura = Estado::Desligado; + _EstadoControle = Estado::Desligado; + ResetarControleFechado(); MostrarLog("Bomba desligada"); } - void AtualizarControle(Estado novoEstado, double potenciaSP = -1, double pressaoSP = -1) { - if (!Iniciado) { - MostrarLog("Ignorando comando: bomba nao iniciada"); + void AtualizarControle( + Estado estado, + double potenciaSP, + double pressaoSP = 0.0 + ) { + _EstadoControle = estado; + _PotenciaSP = constrain(potenciaSP, 0.0, 100.0); + _PressaoSP = constrain(pressaoSP, 0.0, 250.0); + + MalhaFechada = ( + _EstadoControle == Estado::Ligado && + sensorPressao != nullptr && + _PressaoSP > 0.0 && + static_cast(ModoControle) != 0 + ); + + if (_EstadoControle != Estado::Ligado) { + AplicarSaida(false, 0); + ResetarControleFechado(); + AtualizarLeitura(); return; } - _EstadoControle = novoEstado; + if (!MalhaFechada) { + double potencia = _PotenciaSP; - if (potenciaSP >= 0) { - _PotenciaSP = constrain(potenciaSP, 0, 100); - PWM_Set = int((_PotenciaSP / 100.0) * RampaMax); - } + if (potencia > 0.0 && potencia < PotenciaMinAtiva) { + potencia = PotenciaMinAtiva; + } - if (pressaoSP >= 0 && MalhaFechada) { - _PressaoSP = pressaoSP; + AplicarPotenciaPercentual(potencia); } } + void Ligar(double potenciaSP = 100.0, double pressaoSP = 0.0) { + AtualizarControle(Estado::Ligado, potenciaSP, pressaoSP); + } + + void DesligarBomba() { + AtualizarControle(Estado::Desligado, 0.0, 0.0); + } + void Loop() { - if (!Iniciado) return; - - double pressaoAtual = 0; - bool ligado = _EstadoControle == Estado::Ligado; - - if (ligado && MalhaFechada && sensorPressao != nullptr) { - sensorPressao->RequisitarDados(); - pressaoAtual = sensorPressao->Pressao; + if (!Iniciado) { + return; } - if (!ligado) { - PWM_SP = ZeroRampa; - ResetPID(); - } else { - if (MalhaFechada && sensorPressao != nullptr && _PressaoSP > 0) { + unsigned long agora = millis(); - if (ModoControle == ModoControleBomba::PID) { - double potenciaPID = CalcularPotenciaPID(pressaoAtual); - PWM_SP = int((potenciaPID / 100.0) * RampaMax); - } - else { - if (pressaoAtual >= (_PressaoSP + histerese)) { - bombaDesligadaPorPressao = true; - } else if (pressaoAtual <= (_PressaoSP - histerese)) { - bombaDesligadaPorPressao = false; - } + if (agora - _ultimoControleMs < IntervaloControleMs) { + return; + } - PWM_SP = bombaDesligadaPorPressao ? ZeroRampa : PWM_Set; - } + double dt = CalcularDtSegundos(agora); + _ultimoControleMs = agora; - } else { - PWM_SP = PWM_Set; + if (_EstadoControle != Estado::Ligado) { + AplicarSaida(false, 0); + ResetarControleFechado(); + AtualizarLeitura(); + return; + } + + AtualizarPressao(); + QtdBicosLigados = ContarBicosLigados(); + + if (QtdBicosLigados == 0 && DesligarSemBicosAbertos) { + /* + Sem bico aberto nao existe demanda de aplicacao. + Por padrao, mantem a bomba desligada para nao pressurizar linha fechada. + + Se houver bypass/retorno/regulador e voce quiser manter a linha pre-pressurizada, + defina DesligarSemBicosAbertos = false. + + A rotina de lavagem continua funcionando, porque liga todos os bicos antes da bomba. + */ + if (_PressaoSP > 0.0 && sensorPressao != nullptr) { + AplicarSaida(false, 0); + ResetarControleFechado(); + AtualizarLeitura(); + return; } } - AplicarSaida(ligado, PWM_SP); + if (sensorPressao == nullptr || _PressaoSP <= 0.0) { + double potencia = _PotenciaSP; + + if (potencia > 0.0 && potencia < PotenciaMinAtiva) { + potencia = PotenciaMinAtiva; + } + + AplicarPotenciaPercentual(potencia); + AtualizarLeitura(); + return; + } + + if (static_cast(ModoControle) == 0) { + ControlarOnOff(); + AtualizarLeitura(); + return; + } + + ControlarFeedForwardPI(dt); AtualizarLeitura(); } + void AtualizarLeitura() { + if (!Iniciado) { + EstadoLeitura = Estado::Desligado; + PercentualPotenciaLeitura = 0.0; + PotenciaLeitura = 0; + return; + } + + PercentualPotenciaLeitura = constrain(PotenciaFinalAtual, 0.0, 100.0); + PotenciaLeitura = (int)round(PercentualPotenciaLeitura); + EstadoLeitura = (PercentualPotenciaLeitura > 2.0) ? Estado::Ligado : Estado::Desligado; + + if (sensorPressao != nullptr) { + AtualizarPressao(); + } + } + void RequisitarDados() { AtualizarLeitura(); } @@ -237,21 +442,27 @@ class BombaPressurizadora_v2 : public ComponenteCAN { switch (posicao) { case CanMessagePosicaoDados::Command1: { Estado novoEstado = (Estado)data[2]; - double potencia = (data.size() > 3) ? (double)data[3] : -1; - double pressao = (data.size() > 4) ? (double)data[4] : -1; + double potencia = (data.size() > 3) ? (double)data[3] : _PotenciaSP; + double pressao = (data.size() > 4) ? (double)data[4] : _PressaoSP; + AtualizarControle(novoEstado, potencia, pressao); return true; } + case CanMessagePosicaoDados::Command2: { - if (data.size() < 4) return false; - ModoControle = (ModoControleBomba)data[2]; - if (data.size() > 3) PotenciaMinPID = constrain((double)data[3], 0, 100); - if (data.size() > 6) { - Kp = data[4] / 10.0; - Ki = data[5] / 10.0; - Kd = data[6] / 10.0; + if (data.size() < 7) { + MostrarLog("Command2 invalido para bomba"); + return false; } - ResetPID(); + + ModoControleBomba modo = static_cast(data[2]); + double potenciaMin = (double)data[3]; + double kp = ((double)data[4]) / 10.0; + double ki = ((double)data[5]) / 10.0; + double kd = ((double)data[6]) / 10.0; + + ConfigurarModoControle(modo); + ConfigurarPID(kp, ki, kd, potenciaMin, PotenciaMaxPID); return true; } @@ -268,182 +479,329 @@ class BombaPressurizadora_v2 : public ComponenteCAN { switch (posicao) { case CanMessagePosicaoDados::Status: { data.push_back(Iniciado ? 1 : 0); - data.push_back(MalhaFechada ? 1 : 0); break; } case CanMessagePosicaoDados::Dados1: { AtualizarLeitura(); + data.push_back(static_cast(EstadoLeitura)); - data.push_back(static_cast((int)_PotenciaSP)); - data.push_back(static_cast((int)PercentualPotenciaLeitura)); - data.push_back(static_cast((int)_PressaoSP)); - int pressaoAtual = 0; - if (sensorPressao != nullptr) { - pressaoAtual = (int)sensorPressao->Pressao; - } - data.push_back(static_cast(pressaoAtual)); - data.push_back(static_cast(ModoControle)); + data.push_back(LimitarByte(PercentualPotenciaLeitura)); + data.push_back(LimitarByte(_PressaoSP)); + data.push_back(LimitarByte(PressaoAtual)); + + /* + Bytes extras mantendo o pacote em 8 bytes: + - qtd bicos ligados + - potencia feedforward usada antes da correcao PI + */ + data.push_back(QtdBicosLigados); + data.push_back(LimitarByte(PotenciaFeedForwardAtual)); break; } + + default: + break; } return data; } - - void ConfigurarModoControle(ModoControleBomba modo) { - ModoControle = modo; - ResetPID(); - } - - void ConfigurarPID(double kp, double ki, double kd, double potenciaMin, double potenciaMax = 100.0) { - Kp = kp; - Ki = ki; - Kd = kd; - - PotenciaMinPID = constrain(potenciaMin, 0, 100); - PotenciaMaxPID = constrain(potenciaMax, PotenciaMinPID, 100); - - ResetPID(); - } - - private: + static const int PWM_FREQ_HZ = 1000; + static const int PWM_RESOLUTION_BITS = 10; + static const int PWM_MAX_VALUE = 1023; + bool DebugMode = false; - int histerese = 3; - int ZeroRampa = 0; - int RampaMax = 1023; - void AplicarSaida(bool ligado, int pwm) { - if (!Iniciado && pwm != 0) return; + BicoPulverizador_v2** _bicos = nullptr; + size_t _qtdBicos = 0; - if (!ligado) { - pwm = 0; + unsigned long _ultimoControleMs = 0; + unsigned long _ultimoTempoDtMs = 0; + + double _integral = 0.0; + double _ultimoErro = 0.0; + double _ultimaPressaoFiltrada = 0.0; + uint8_t _ultimaQtdBicosLigados = 0; + bool _filtroInicializado = false; + + uint8_t ContarBicosLigados() { + if (_bicos == nullptr || _qtdBicos == 0) { + return 0; } - pwm = LimitarPWM(pwm); - ledcWrite(_canalIN1, _sentido == Sentido::Horario ? pwm : 0); - ledcWrite(_canalIN2, _sentido == Sentido::Horario ? 0 : pwm); + uint8_t qtd = 0; + + for (size_t i = 0; i < _qtdBicos; i++) { + BicoPulverizador_v2* bico = _bicos[i]; + + if (bico == nullptr || !bico->Iniciado) { + continue; + } + + bico->AtualizarLeitura(); + + if (bico->_Status == Estado::Ligado) { + qtd++; + } + } + + return qtd; } - int LimitarPWM(int valor) { - if (valor < ZeroRampa) return ZeroRampa; - if (valor > RampaMax) return RampaMax; - return valor; + void AtualizarPressao() { + if (sensorPressao == nullptr) { + PressaoAtual = 0.0; + return; + } + + sensorPressao->RequisitarDados(); + PressaoAtual = sensorPressao->Pressao; + + if (!_filtroInicializado) { + PressaoFiltrada = PressaoAtual; + _ultimaPressaoFiltrada = PressaoFiltrada; + _filtroInicializado = true; + return; + } + + double alpha = constrain(AlphaFiltroPressao, 0.01, 1.0); + PressaoFiltrada = (alpha * PressaoAtual) + ((1.0 - alpha) * PressaoFiltrada); } - void AtualizarLeitura() { - if (!Iniciado) return; + void ControlarOnOff() { + if (PressaoAtual >= (_PressaoSP + HistereseOnOffPsi)) { + bombaDesligadaPorPressao = true; + } - uint32_t duty = ledcRead(_sentido == Sentido::Horario ? _canalIN1 : _canalIN2); - PercentualPotenciaLeitura = (duty / (double)RampaMax) * 100.0; - EstadoLeitura = (duty > 5) ? Estado::Ligado : Estado::Desligado; + if (PressaoAtual <= (_PressaoSP - HistereseOnOffPsi)) { + bombaDesligadaPorPressao = false; + } + + if (bombaDesligadaPorPressao) { + AplicarSaida(false, 0); + return; + } + + double potencia = _PotenciaSP; + + if (potencia > 0.0 && potencia < PotenciaMinAtiva) { + potencia = PotenciaMinAtiva; + } + + AplicarPotenciaPercentual(potencia); } - void ResetPID() { - erroAnterior = 0.0; - integralErro = 0.0; - pressaoAnterior = 0.0; - ultimoPID_ms = millis(); + void ControlarFeedForwardPI(double dt) { + if (QtdBicosLigados != _ultimaQtdBicosLigados) { + _ultimaQtdBicosLigados = QtdBicosLigados; + + /* + Mudou a demanda hidraulica. + Zera o erro acumulado para evitar integral velha brigando com a nova tabela. + */ + _integral = 0.0; + _ultimoErro = 0.0; + } + + double potenciaBase = CalcularPotenciaFeedForward(QtdBicosLigados, _PressaoSP); + PotenciaFeedForwardAtual = potenciaBase; + + double erro = _PressaoSP - PressaoFiltrada; + double derivadaMedicao = 0.0; + + if (dt > 0.001) { + derivadaMedicao = (PressaoFiltrada - _ultimaPressaoFiltrada) / dt; + } + + double correcaoSemIntegralNova = + (Kp * erro) + + (Ki * _integral) - + (Kd * derivadaMedicao); + + correcaoSemIntegralNova = constrain( + correcaoSemIntegralNova, + CorrecaoMinPI, + CorrecaoMaxPI + ); + + double saidaSemIntegralNova = potenciaBase + correcaoSemIntegralNova; + double minimo = (QtdBicosLigados > 0) ? PotenciaMinAtiva : 0.0; + double maximo = PotenciaMaxPID; + + bool saturouAlto = saidaSemIntegralNova >= maximo && erro > 0.0; + bool saturouBaixo = saidaSemIntegralNova <= minimo && erro < 0.0; + + if (!saturouAlto && !saturouBaixo) { + _integral += erro * dt; + _integral = constrain(_integral, -120.0, 120.0); + } + + double correcao = + (Kp * erro) + + (Ki * _integral) - + (Kd * derivadaMedicao); + + correcao = constrain(correcao, CorrecaoMinPI, CorrecaoMaxPI); + CorrecaoPIAtual = correcao; + + double potenciaDesejada = potenciaBase + correcao; + potenciaDesejada = constrain(potenciaDesejada, minimo, maximo); + + double potenciaComRampa = AplicarRampaPercentual(potenciaDesejada, dt); + + AplicarPotenciaPercentual(potenciaComRampa); + + _ultimoErro = erro; + _ultimaPressaoFiltrada = PressaoFiltrada; } - double CalcularPotenciaPID(double pressaoAtual) { - const unsigned long agora = millis(); - - if (ultimoPID_ms == 0) { - ultimoPID_ms = agora; - erroAnterior = _PressaoSP - pressaoAtual; - pressaoAnterior = pressaoAtual; + double CalcularPotenciaFeedForward(uint8_t qtdBicos, double pressaoSP) { + if (qtdBicos == 0) { return 0.0; } - double dt = (agora - ultimoPID_ms) / 1000.0; - ultimoPID_ms = agora; - - if (dt < 0.01) { - dt = 0.01; + if (qtdBicos > 7) { + qtdBicos = 7; } - if (dt > 0.5) { - dt = 0.5; + double p15 = PotenciaBase15Psi[qtdBicos]; + double p30 = PotenciaBase30Psi[qtdBicos]; + + bool p15Valida = p15 > 0.1; + bool p30Valida = p30 > 0.1; + + if (!p15Valida && !p30Valida) { + return _PotenciaSP; } - double erro = _PressaoSP - pressaoAtual; - - if (fabs(erro) <= DeadbandPressao) { - erro = 0.0; + if (!p15Valida) { + return constrain(p30, 0.0, 100.0); } - /* - Derivada feita sobre a medição, e não sobre o erro. - Isso reduz o tranco quando o SP muda. - */ - double derivadaMedicao = - (pressaoAtual - pressaoAnterior) / dt; - - pressaoAnterior = pressaoAtual; - - /* - Calcula primeiro sem atualizar a integral. - */ - double saidaSemIntegral = - (Kp * erro) - - (Kd * derivadaMedicao); - - double integralCandidata = - integralErro + (erro * dt); - - integralCandidata = constrain( - integralCandidata, - IntegralMin, - IntegralMax - ); - - double saidaCandidata = - saidaSemIntegral - + (Ki * integralCandidata); - - double teto = constrain( - _PotenciaSP, - 0.0, - PotenciaMaxPID - ); - - /* - Anti-windup: - só integra se não estiver saturado, - ou se o erro estiver ajudando a sair da saturação. - */ - bool saturadoSuperior = saidaCandidata >= teto; - bool saturadoInferior = saidaCandidata <= 0.0; - - bool podeIntegrar = - (!saturadoSuperior && !saturadoInferior) - || (saturadoSuperior && erro < 0) - || (saturadoInferior && erro > 0); - - if (podeIntegrar) { - integralErro = integralCandidata; + if (!p30Valida) { + return constrain(p15, 0.0, 100.0); } - double saida = - saidaSemIntegral - + (Ki * integralErro); - - saida = constrain(saida, 0.0, teto); - - /* - Só aplica o mínimo quando existe demanda real - para ligar a bomba. - */ - if (saida > 0.0 && saida < PotenciaMinPID) { - saida = PotenciaMinPID; + if (pressaoSP <= PressaoReferenciaBaixaPsi) { + return constrain(p15, 0.0, 100.0); } - erroAnterior = erro; + if (pressaoSP >= PressaoReferenciaAltaPsi) { + return constrain(p30, 0.0, 100.0); + } - return saida; + double t = + (pressaoSP - PressaoReferenciaBaixaPsi) / + (PressaoReferenciaAltaPsi - PressaoReferenciaBaixaPsi); + + double potencia = p15 + ((p30 - p15) * t); + return constrain(potencia, 0.0, 100.0); + } + + double AplicarRampaPercentual(double potenciaDesejada, double dt) { + double atual = PotenciaFinalAtual; + double delta = potenciaDesejada - atual; + + if (fabs(delta) < 0.01) { + return potenciaDesejada; + } + + double taxa = (delta > 0.0) ? TaxaSubidaPctPorSegundo : TaxaDescidaPctPorSegundo; + double maxDelta = taxa * dt; + + if (fabs(delta) <= maxDelta) { + return potenciaDesejada; + } + + return atual + ((delta > 0.0) ? maxDelta : -maxDelta); + } + + double CalcularDtSegundos(unsigned long agoraMs) { + if (_ultimoTempoDtMs == 0) { + _ultimoTempoDtMs = agoraMs; + return ((double)IntervaloControleMs) / 1000.0; + } + + unsigned long deltaMs = agoraMs - _ultimoTempoDtMs; + _ultimoTempoDtMs = agoraMs; + + if (deltaMs == 0) { + return ((double)IntervaloControleMs) / 1000.0; + } + + double dt = ((double)deltaMs) / 1000.0; + return constrain(dt, 0.005, 0.500); + } + + void AplicarPotenciaPercentual(double percentual) { + percentual = constrain(percentual, 0.0, 100.0); + + if (_EstadoControle != Estado::Ligado || percentual <= 0.0) { + AplicarSaida(false, 0); + return; + } + + int pwm = PercentualParaPWM(percentual); + AplicarSaida(true, pwm); + } + + int PercentualParaPWM(double percentual) { + percentual = constrain(percentual, 0.0, 100.0); + return (int)round((percentual / 100.0) * PWM_MAX_VALUE); + } + + double PWMParaPercentual(int pwm) { + pwm = constrain(pwm, 0, PWM_MAX_VALUE); + return (((double)pwm) * 100.0) / ((double)PWM_MAX_VALUE); + } + + void AplicarSaida(bool ligado, int pwm) { + pwm = constrain(pwm, 0, PWM_MAX_VALUE); + + if (!ligado || pwm <= 0) { + ledcWrite(_canalIN1, 0); + ledcWrite(_canalIN2, 0); + + PWM_SP = 0; + PWM_Set = 0; + PotenciaFinalAtual = 0.0; + PercentualPotenciaLeitura = 0.0; + PotenciaLeitura = 0; + EstadoLeitura = Estado::Desligado; + return; + } + + if (_sentido == Sentido::Horario) { + ledcWrite(_canalIN1, pwm); + ledcWrite(_canalIN2, 0); + } else { + ledcWrite(_canalIN1, 0); + ledcWrite(_canalIN2, pwm); + } + + PWM_SP = pwm; + PWM_Set = pwm; + PotenciaFinalAtual = PWMParaPercentual(pwm); + PercentualPotenciaLeitura = PotenciaFinalAtual; + PotenciaLeitura = (int)round(PercentualPotenciaLeitura); + EstadoLeitura = Estado::Ligado; + } + + void ResetarControleFechado() { + _integral = 0.0; + _ultimoErro = 0.0; + _ultimaPressaoFiltrada = PressaoFiltrada; + _ultimaQtdBicosLigados = 0; + CorrecaoPIAtual = 0.0; + PotenciaFeedForwardAtual = 0.0; + bombaDesligadaPorPressao = false; + } + + uint8_t LimitarByte(double valor) { + valor = constrain(valor, 0.0, 255.0); + return static_cast(round(valor)); } void MostrarLog(String mensagem) { @@ -453,4 +811,4 @@ class BombaPressurizadora_v2 : public ComponenteCAN { } }; -#endif \ No newline at end of file +#endif diff --git a/Firmware/Modulos/SensorFluxoModel_v2.h b/Firmware/Modulos/SensorFluxoModel_v2.h index 804a07121..88be81812 100644 --- a/Firmware/Modulos/SensorFluxoModel_v2.h +++ b/Firmware/Modulos/SensorFluxoModel_v2.h @@ -152,8 +152,6 @@ class SensorFluxo_v2 : public ComponenteCAN { float CalcularVazao(uint16_t pulsos, unsigned long periodoMs) { if (periodoMs == 0) return 0.0f; - const float FATOR_CALIBRACAO_FLUXO = 0.776f; - float frequenciaHz = ((float)pulsos) * 1000.0f / (float)periodoMs; float q_L_min = (frequenciaHz + 3.0f) / 8.1f; @@ -162,8 +160,6 @@ class SensorFluxo_v2 : public ComponenteCAN { float q_mL_s = q_L_min * (1000.0f / 60.0f); - q_mL_s *= FATOR_CALIBRACAO_FLUXO; - return q_mL_s; }