1628 lines
44 KiB
C++
1628 lines
44 KiB
C++
#ifndef BombaPressurizadoraModel_v2_h
|
|
#define BombaPressurizadoraModel_v2_h
|
|
|
|
#include <Arduino.h>
|
|
#include <vector>
|
|
#include <math.h>
|
|
|
|
#include "SerialService.h"
|
|
#include "Pinout.h"
|
|
#include "Utils.h"
|
|
#include "IComponenteCAN.h"
|
|
#include "SensorPressaoModel_v2.h"
|
|
#include "SensorFluxoModel_v2.h"
|
|
#include "BicoPulverizadorModel_v2.h"
|
|
|
|
/*
|
|
BombaPressurizadora_v2 - controle suave por pressao com supervisor de fluxo.
|
|
|
|
Ideia desta versao:
|
|
- A pressao e o controle principal.
|
|
- A bomba NAO fica ligando/desligando dentro da malha fechada.
|
|
- Se existem bicos abertos e nao existe pulso/fluxo recente, o controle aumenta
|
|
potencia por boost progressivo, mesmo que a pressao esteja acima do SP.
|
|
- O sensor de fluxo nunca deve ser usado como "chave nervosa" para cortar a bomba.
|
|
Ele apenas adiciona potencia enquanto nao houver indicio de produto em movimento.
|
|
- O ON/OFF antigo fica preservado somente quando ModoControle == 0.
|
|
- O contrato CAN Dados1/Dados2 foi mantido.
|
|
*/
|
|
|
|
struct EstadoAnteriorBomba {
|
|
Estado estado = Estado::Desligado;
|
|
double potenciaSP = 0.0;
|
|
double pressaoSP = 0.0;
|
|
|
|
ModoControleBomba modoControle = static_cast<ModoControleBomba>(0);
|
|
|
|
double kp = 0.0;
|
|
double ki = 0.0;
|
|
double kd = 0.0;
|
|
|
|
double potenciaMinPID = 0.0;
|
|
double potenciaMaxPID = 100.0;
|
|
};
|
|
|
|
class BombaPressurizadora_v2 : public ComponenteCAN {
|
|
public:
|
|
BombaPressurizadora_v2(
|
|
const String& id,
|
|
uint8_t idNum,
|
|
int pinoIN1,
|
|
int canalIN1,
|
|
int pinoIN2,
|
|
int canalIN2,
|
|
TiposBarramentos barramento = TiposBarramentos::CONTROLADOR,
|
|
Sentido sentido = Sentido::Horario,
|
|
SensorPressao_v2* sensor = nullptr,
|
|
BicoPulverizador_v2** bicos = nullptr,
|
|
size_t qtdBicos = 0,
|
|
SensorFluxo_v2* sensorFluxoLinha = nullptr
|
|
)
|
|
: _ID(id),
|
|
ID_Num(idNum),
|
|
_pinoIN1(pinoIN1, barramento, _OUTPUT),
|
|
_pinoIN2(pinoIN2, barramento, _OUTPUT),
|
|
_canalIN1(canalIN1),
|
|
_canalIN2(canalIN2),
|
|
_sentido(sentido),
|
|
sensorPressao(sensor),
|
|
sensorFluxo(sensorFluxoLinha),
|
|
_bicos(bicos),
|
|
_qtdBicos(qtdBicos) {
|
|
if (_bicos == nullptr || _qtdBicos == 0) {
|
|
DesligarSemBicosAbertos = false;
|
|
HabilitarSupervisorFluxo = false;
|
|
HabilitarKickInicial = false;
|
|
HabilitarDetectorCavitacao = false;
|
|
}
|
|
}
|
|
|
|
String _ID;
|
|
uint8_t ID_Num = 0;
|
|
|
|
PinoModel _pinoIN1;
|
|
PinoModel _pinoIN2;
|
|
|
|
int _canalIN1 = 1;
|
|
int _canalIN2 = 2;
|
|
Sentido _sentido = Sentido::Horario;
|
|
|
|
bool Iniciado = false;
|
|
bool MalhaFechada = false;
|
|
bool bombaDesligadaPorPressao = false;
|
|
|
|
SensorPressao_v2* sensorPressao = nullptr;
|
|
SensorFluxo_v2* sensorFluxo = nullptr;
|
|
|
|
Estado EstadoLeitura = Estado::Desligado;
|
|
Estado _EstadoControle = Estado::Desligado;
|
|
|
|
int PWM_SP = 0;
|
|
int PWM_Set = 0;
|
|
int PotenciaLeitura = 0;
|
|
double PercentualPotenciaLeitura = 0.0;
|
|
|
|
double _PotenciaSP = 0.0;
|
|
double _PressaoSP = 0.0;
|
|
|
|
ModoControleBomba ModoControle = static_cast<ModoControleBomba>(0);
|
|
|
|
/*
|
|
Ganhos da malha suave.
|
|
Observacao: o Command2 continua atualizando Kp/Ki/Kd.
|
|
Para esta versao, comece conservador:
|
|
Kp ~= 1.0 a 2.5
|
|
Ki ~= 0.10 a 0.40
|
|
Kd = 0
|
|
*/
|
|
double Kp = 1.10;
|
|
double Ki = 0.12;
|
|
double Kd = 0.0;
|
|
|
|
double PotenciaMinPID = 0.0;
|
|
double PotenciaMaxPID = 100.0;
|
|
|
|
/*
|
|
Na malha fechada, a saida nunca deve ficar passeando entre 0 e 100.
|
|
Ela segue uma rampa.
|
|
*/
|
|
double TaxaSubidaPctPorSegundo = 220.0;
|
|
double TaxaDescidaPctPorSegundo = 55.0;
|
|
unsigned long IntervaloControleMs = 50;
|
|
|
|
/*
|
|
Se true, com nenhum bico aberto a bomba desliga.
|
|
Se futuramente houver bypass/retorno e quiser pre-pressurizar linha,
|
|
altere para false.
|
|
*/
|
|
bool DesligarSemBicosAbertos = true;
|
|
|
|
/*
|
|
Piso eletrico/hidraulico.
|
|
Nao confundir com boost de fluxo. Este piso apenas evita PWM inutil.
|
|
*/
|
|
double PotenciaMinAtiva = 18.0;
|
|
|
|
/*
|
|
Piso suave por quantidade de bicos.
|
|
Mantive baixo para nao forcar sobrepressao permanente.
|
|
O boost de fluxo e quem sobe quando nao houver vazao.
|
|
*/
|
|
double PotenciaMinimaComBicosPorQtd[8] = {
|
|
0.0,
|
|
28.0,
|
|
30.0,
|
|
38.0,
|
|
42.0,
|
|
46.0,
|
|
50.0,
|
|
55.0
|
|
};
|
|
|
|
/*
|
|
Kick inicial quando sai de 0 bicos para N bicos.
|
|
Diferente do supervisor de fluxo, isto e apenas uma pancada inicial curta.
|
|
*/
|
|
bool HabilitarKickInicial = true;
|
|
double PotenciaKickPoucosBicosPct = 65.0;
|
|
double PotenciaKickMuitosBicosPct = 58.0;
|
|
unsigned long DuracaoKickPoucosBicosMs = 1000;
|
|
unsigned long DuracaoKickMuitosBicosMs = 650;
|
|
|
|
/*
|
|
Supervisor de fluxo.
|
|
Nao corta a bomba. Apenas adiciona boost se houver bico aberto e nao houver
|
|
indicio recente de fluxo/pulso.
|
|
*/
|
|
bool HabilitarSupervisorFluxo = true;
|
|
double FatorCalibracaoFluxoFirmware = 0.776;
|
|
double VazaoMinimaOperacaoMLs = 3.0;
|
|
unsigned long TimeoutPulsoFluxoRecenteMs = 700;
|
|
unsigned long TempoSemFluxoParaBoostMs = 300;
|
|
unsigned long TempoComFluxoParaAliviarMs = 800;
|
|
|
|
double BoostFluxoMaxPoucosBicosPct = 40.0;
|
|
double BoostFluxoMaxMuitosBicosPct = 38.0;
|
|
double TaxaBoostFluxoSubidaPctPorSegundo = 45.0;
|
|
double TaxaBoostFluxoDescidaPctPorSegundo = 16.0;
|
|
|
|
/*
|
|
Enquanto nao houver fluxo, a malha de pressao usa um SP auxiliar minimo.
|
|
Isto ajuda o PI a subir potencia sem depender apenas do boost.
|
|
Nao e usado para cortar a bomba.
|
|
*/
|
|
double PressaoSPMinSemFluxoPoucosBicosPsi = 22.0;
|
|
double PressaoSPMinSemFluxoMuitosBicosPsi = 22.0;
|
|
|
|
/*
|
|
Se nao houver fluxo e a pressao ja estiver muito alta, o boost para de subir.
|
|
A bomba ainda nao fica ligando/desligando, apenas evita empurrar boost infinito.
|
|
*/
|
|
double PressaoMaxAssistenciaFluxoPsi = 85.0;
|
|
|
|
/*
|
|
Corte duro de protecao. A linha/trava mecanica foram informadas como seguras
|
|
proximo de 120 psi.
|
|
*/
|
|
double PressaoCorteSegurancaPsi = 120.0;
|
|
double HistereseRearmeSegurancaPsi = 12.0;
|
|
bool CorteSegurancaAtivo = false;
|
|
|
|
/*
|
|
ON/OFF legado.
|
|
Use apenas quando ModoControle == 0.
|
|
*/
|
|
double HistereseOnOffPsi = 3.0;
|
|
|
|
/*
|
|
Mapa empirico mantido como chute inicial/feedforward, nao como dono da decisao.
|
|
A malha suave funciona mesmo se HabilitarFeedForward == false.
|
|
*/
|
|
bool HabilitarFeedForward = true;
|
|
double PesoFeedForward = 1.0;
|
|
|
|
double PotenciaBase15Psi[8] = {
|
|
0.0,
|
|
20.0,
|
|
40.0,
|
|
56.0,
|
|
61.0,
|
|
68.0,
|
|
72.0, // antes estava perto de 75
|
|
78.0 // antes estava perto de 88
|
|
};
|
|
|
|
double PotenciaBase22Psi[8] = {
|
|
0.0,
|
|
35.0,
|
|
56.4,
|
|
62.5,
|
|
65.8,
|
|
71.7,
|
|
77.5,
|
|
100.0
|
|
};
|
|
|
|
double PressaoReferenciaBaixaPsi = 15.0;
|
|
double PressaoReferenciaAltaPsi = 22.0;
|
|
|
|
double PressaoMaximaMapeadaPorBicos[8] = {
|
|
0.0,
|
|
92.0,
|
|
75.0,
|
|
56.0,
|
|
41.0,
|
|
32.0,
|
|
26.0,
|
|
22.0
|
|
};
|
|
|
|
double PotenciaBaseMaxPsi[8] = {
|
|
0.0,
|
|
100.0,
|
|
100.0,
|
|
100.0,
|
|
100.0,
|
|
100.0,
|
|
100.0,
|
|
100.0
|
|
};
|
|
|
|
/*
|
|
Correção opcional da pressão para aproximar ponto de aplicação/manômetro.
|
|
Se quiser controlar exatamente o sensor, zere esta tabela.
|
|
*/
|
|
double CorrecaoPressaoAplicacaoPorBicos[8] = {
|
|
0.0,
|
|
-4.0,
|
|
-2.0,
|
|
0.0,
|
|
1.0,
|
|
2.0,
|
|
3.5,
|
|
4.5
|
|
};
|
|
|
|
double AlphaFiltroPressao = 0.35;
|
|
double DeadbandPressaoPsi = 1.2;
|
|
|
|
/*
|
|
Estados/telemetria expostos via CAN/log.
|
|
*/
|
|
uint8_t QtdBicosLigados = 0;
|
|
double PressaoSensorAtual = 0.0;
|
|
double PressaoAtual = 0.0;
|
|
double PressaoFiltrada = 0.0;
|
|
double PressaoSPControleAtual = 0.0;
|
|
bool PressaoSPLimitadoPorCapacidade = false;
|
|
bool PressaoSPForaDoMapaAtual = false;
|
|
double PressaoMaximaMapeadaAtual = 0.0;
|
|
|
|
double PotenciaFeedForwardAtual = 0.0;
|
|
double CorrecaoPIAtual = 0.0;
|
|
double PotenciaFinalAtual = 0.0;
|
|
|
|
double VazaoFluxoAtualMLs = 0.0;
|
|
double VazaoFluxoCorrigidaAtualMLs = 0.0;
|
|
bool FluxoPresenteAtual = false;
|
|
bool FluxoInsuficienteAtual = false;
|
|
bool RecuperacaoFluxoAtiva = false;
|
|
double BoostFluxoAtualPct = 0.0;
|
|
|
|
/*
|
|
Compatibilidade com versões anteriores.
|
|
*/
|
|
bool ArmamentoLinhaAtivo = false;
|
|
bool LinhaArmadaPorFluxo = false;
|
|
bool FalhaArmamentoLinhaAtual = false;
|
|
|
|
/*
|
|
Detector de cavitacao consolidado na firmware.
|
|
Nao corta a bomba neste primeiro momento.
|
|
Apenas informa estado diagnostico para o C# via CAN.
|
|
*/
|
|
bool HabilitarDetectorCavitacao = true;
|
|
|
|
double PressaoMinimaCavitacaoPsi = 8.0;
|
|
double MargemPressaoSPCavitacaoPsi = 6.0;
|
|
double PotenciaMinimaParaDetectarCavitacaoPct = 25.0;
|
|
double VazaoMinimaCavitacaoMLs = 2.0;
|
|
|
|
unsigned long TempoIgnorarAposLigarBombaMs = 1500;
|
|
unsigned long TempoIgnorarAposMudancaBicosMs = 900;
|
|
unsigned long TempoParaSuspeitarCavitacaoMs = 900;
|
|
unsigned long TempoParaConfirmarCavitacaoMs = 2500;
|
|
unsigned long TempoParaLimparCavitacaoMs = 1800;
|
|
|
|
bool CavitacaoSuspeitaAtual = false;
|
|
bool CavitadaAtual = false;
|
|
|
|
void ConfigurarBicos(BicoPulverizador_v2** bicos, size_t qtdBicos) {
|
|
_bicos = bicos;
|
|
_qtdBicos = qtdBicos;
|
|
}
|
|
|
|
void ConfigurarSensorFluxo(SensorFluxo_v2* sensor) {
|
|
sensorFluxo = sensor;
|
|
ResetarFluxo();
|
|
}
|
|
|
|
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
|
|
) {
|
|
PotenciaBase22Psi[0] = 0.0;
|
|
PotenciaBase22Psi[1] = b1;
|
|
PotenciaBase22Psi[2] = b2;
|
|
PotenciaBase22Psi[3] = b3;
|
|
PotenciaBase22Psi[4] = b4;
|
|
PotenciaBase22Psi[5] = b5;
|
|
PotenciaBase22Psi[6] = b6;
|
|
PotenciaBase22Psi[7] = b7;
|
|
}
|
|
|
|
void ConfigurarMapaPotenciaAltaPsi(
|
|
double pressaoAltaPsi,
|
|
double b1,
|
|
double b2,
|
|
double b3,
|
|
double b4,
|
|
double b5,
|
|
double b6,
|
|
double b7
|
|
) {
|
|
PressaoReferenciaAltaPsi = constrain(pressaoAltaPsi, PressaoReferenciaBaixaPsi + 0.1, 250.0);
|
|
ConfigurarMapaPotencia30Psi(b1, b2, b3, b4, b5, b6, b7);
|
|
}
|
|
|
|
void ConfigurarPressaoMaximaPorBicos(
|
|
double b1,
|
|
double b2,
|
|
double b3,
|
|
double b4,
|
|
double b5,
|
|
double b6,
|
|
double b7
|
|
) {
|
|
PressaoMaximaMapeadaPorBicos[0] = 0.0;
|
|
PressaoMaximaMapeadaPorBicos[1] = b1;
|
|
PressaoMaximaMapeadaPorBicos[2] = b2;
|
|
PressaoMaximaMapeadaPorBicos[3] = b3;
|
|
PressaoMaximaMapeadaPorBicos[4] = b4;
|
|
PressaoMaximaMapeadaPorBicos[5] = b5;
|
|
PressaoMaximaMapeadaPorBicos[6] = b6;
|
|
PressaoMaximaMapeadaPorBicos[7] = b7;
|
|
}
|
|
|
|
void ConfigurarMapaPotenciaMaxPsi(
|
|
double b1,
|
|
double b2,
|
|
double b3,
|
|
double b4,
|
|
double b5,
|
|
double b6,
|
|
double b7
|
|
) {
|
|
PotenciaBaseMaxPsi[0] = 0.0;
|
|
PotenciaBaseMaxPsi[1] = b1;
|
|
PotenciaBaseMaxPsi[2] = b2;
|
|
PotenciaBaseMaxPsi[3] = b3;
|
|
PotenciaBaseMaxPsi[4] = b4;
|
|
PotenciaBaseMaxPsi[5] = b5;
|
|
PotenciaBaseMaxPsi[6] = b6;
|
|
PotenciaBaseMaxPsi[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);
|
|
}
|
|
|
|
void ConfigurarModoControle(ModoControleBomba modo) {
|
|
ModoControle = modo;
|
|
ResetarControle();
|
|
}
|
|
|
|
void Inicializar() {
|
|
if (Iniciado) {
|
|
MostrarLog("Bomba ja inicializada");
|
|
return;
|
|
}
|
|
|
|
_pinoIN1.Conectar(false);
|
|
_pinoIN2.Conectar(false);
|
|
|
|
ledcSetup(_canalIN1, PWM_FREQ_HZ, PWM_RESOLUTION_BITS);
|
|
ledcSetup(_canalIN2, PWM_FREQ_HZ, PWM_RESOLUTION_BITS);
|
|
|
|
ledcAttachPin(_pinoIN1.num, _canalIN1);
|
|
ledcAttachPin(_pinoIN2.num, _canalIN2);
|
|
|
|
bool pinosOk = _pinoIN1.Definido() && _pinoIN2.Definido();
|
|
|
|
if (!pinosOk) {
|
|
Iniciado = false;
|
|
MostrarLog("Falha ao iniciar bomba");
|
|
return;
|
|
}
|
|
|
|
Iniciado = true;
|
|
ResetarControle();
|
|
AplicarSaida(false, 0);
|
|
MostrarLog("Bomba iniciada");
|
|
}
|
|
|
|
void Desligar() {
|
|
if (!Iniciado) {
|
|
MostrarLog("Bomba nao esta inicializada");
|
|
return;
|
|
}
|
|
|
|
AplicarSaida(false, 0);
|
|
|
|
_pinoIN1.Desconectar();
|
|
_pinoIN2.Desconectar();
|
|
|
|
Iniciado = false;
|
|
EstadoLeitura = Estado::Desligado;
|
|
_EstadoControle = Estado::Desligado;
|
|
ResetarControle();
|
|
|
|
MostrarLog("Bomba desligada");
|
|
}
|
|
|
|
void AtualizarControle(
|
|
Estado estado,
|
|
double potenciaSP,
|
|
double pressaoSP = 0.0
|
|
) {
|
|
bool mudouEstado = (_EstadoControle != estado);
|
|
bool mudouPressao = fabs(_PressaoSP - pressaoSP) > 0.01;
|
|
|
|
_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<uint8_t>(ModoControle) != 0
|
|
);
|
|
|
|
if (mudouEstado || mudouPressao) {
|
|
ResetarIntegralPressao();
|
|
}
|
|
|
|
if (_EstadoControle != Estado::Ligado) {
|
|
AplicarSaida(false, 0);
|
|
ResetarControle();
|
|
AtualizarLeitura();
|
|
return;
|
|
}
|
|
|
|
if (!MalhaFechada) {
|
|
double potencia = _PotenciaSP;
|
|
uint8_t idx = LimitarIndiceBicos(ContarBicosLigados());
|
|
|
|
if (idx > 0) {
|
|
potencia = MaiorDouble(potencia, ObterPotenciaMinimaOperacional(idx));
|
|
}
|
|
|
|
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;
|
|
}
|
|
|
|
unsigned long agora = millis();
|
|
|
|
if (agora - _ultimoControleMs < IntervaloControleMs) {
|
|
return;
|
|
}
|
|
|
|
double dt = CalcularDtSegundos(agora);
|
|
_ultimoControleMs = agora;
|
|
|
|
QtdBicosLigados = ContarBicosLigados();
|
|
AtualizarPressao();
|
|
AtualizarFluxo();
|
|
|
|
if (_EstadoControle != Estado::Ligado) {
|
|
AplicarSaida(false, 0);
|
|
ResetarControle();
|
|
AtualizarLeitura();
|
|
return;
|
|
}
|
|
|
|
if (TratarCorteSeguranca()) {
|
|
AtualizarLeitura();
|
|
return;
|
|
}
|
|
|
|
MalhaFechada = (
|
|
sensorPressao != nullptr &&
|
|
_PressaoSP > 0.0 &&
|
|
static_cast<uint8_t>(ModoControle) != 0
|
|
);
|
|
|
|
if (TemBicosConfigurados() && QtdBicosLigados == 0 && DesligarSemBicosAbertos) {
|
|
AplicarSaida(false, 0);
|
|
ResetarEstadosDeBicosFechados();
|
|
AtualizarLeitura();
|
|
return;
|
|
}
|
|
|
|
if (!MalhaFechada) {
|
|
double potencia = _PotenciaSP;
|
|
|
|
if (QtdBicosLigados > 0) {
|
|
potencia = MaiorDouble(potencia, ObterPotenciaMinimaOperacional(QtdBicosLigados));
|
|
potencia = AplicarSupervisorFluxo(potencia, dt);
|
|
} else {
|
|
ResetarSupervisorFluxo();
|
|
}
|
|
|
|
double potenciaComRampa = AplicarRampaPercentual(potencia, dt);
|
|
AplicarPotenciaPercentual(potenciaComRampa);
|
|
AtualizarLeitura();
|
|
return;
|
|
}
|
|
|
|
if (static_cast<uint8_t>(ModoControle) == 0) {
|
|
ControlarOnOffLegado(_PressaoSP, _PotenciaSP, HistereseOnOffPsi);
|
|
AtualizarLeitura();
|
|
return;
|
|
}
|
|
|
|
ControlarPressaoSuaveComFluxo(dt);
|
|
AtualizarLeitura();
|
|
}
|
|
|
|
void AtualizarLeitura() {
|
|
if (!Iniciado) {
|
|
EstadoLeitura = Estado::Desligado;
|
|
PercentualPotenciaLeitura = 0.0;
|
|
PotenciaLeitura = 0;
|
|
return;
|
|
}
|
|
|
|
QtdBicosLigados = ContarBicosLigados();
|
|
|
|
if (sensorPressao != nullptr) {
|
|
AtualizarPressao();
|
|
}
|
|
|
|
AtualizarFluxo();
|
|
|
|
PercentualPotenciaLeitura = constrain(PotenciaFinalAtual, 0.0, 100.0);
|
|
PotenciaLeitura = (int)round(PercentualPotenciaLeitura);
|
|
EstadoLeitura = (PercentualPotenciaLeitura > 2.0) ? Estado::Ligado : Estado::Desligado;
|
|
|
|
AtualizarDetectorCavitacao();
|
|
}
|
|
|
|
void RequisitarDados() {
|
|
AtualizarLeitura();
|
|
}
|
|
|
|
bool ProcessarComando(const std::vector<uint8_t>& data) {
|
|
if (data.size() < 3) {
|
|
MostrarLog("Comando invalido");
|
|
return false;
|
|
}
|
|
|
|
CanMessagePosicaoDados posicao = (CanMessagePosicaoDados)data[0];
|
|
uint8_t idNum = data[1];
|
|
|
|
if (idNum != ID_Num) {
|
|
return false;
|
|
}
|
|
|
|
switch (posicao) {
|
|
case CanMessagePosicaoDados::Command1: {
|
|
Estado novoEstado = (Estado)data[2];
|
|
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() < 7) {
|
|
MostrarLog("Command2 invalido para bomba");
|
|
return false;
|
|
}
|
|
|
|
ModoControleBomba modo = static_cast<ModoControleBomba>(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;
|
|
}
|
|
|
|
default:
|
|
return false;
|
|
}
|
|
}
|
|
|
|
std::vector<uint8_t> MontarMensagemCAN(CanMessagePosicaoDados posicao) {
|
|
std::vector<uint8_t> data;
|
|
data.push_back(static_cast<uint8_t>(posicao));
|
|
data.push_back(ID_Num);
|
|
|
|
switch (posicao) {
|
|
case CanMessagePosicaoDados::Status: {
|
|
data.push_back(Iniciado ? 1 : 0);
|
|
data.push_back((MalhaFechada && sensorPressao != nullptr) ? 1 : 0);
|
|
break;
|
|
}
|
|
|
|
case CanMessagePosicaoDados::Dados1: {
|
|
data.push_back(static_cast<uint8_t>(EstadoLeitura)); // data[2]
|
|
data.push_back(LimitarByte(_PotenciaSP)); // data[3] potenciaSp
|
|
data.push_back(LimitarByte(PercentualPotenciaLeitura)); // data[4] percentualPotencia real
|
|
data.push_back(LimitarByte(_PressaoSP)); // data[5] pressaoSp pedido
|
|
data.push_back(LimitarByte(PressaoAtual)); // data[6] pressaoAtual
|
|
data.push_back(static_cast<uint8_t>(ModoControle)); // data[7] modo
|
|
break;
|
|
}
|
|
|
|
case CanMessagePosicaoDados::Dados2: {
|
|
data.push_back(QtdBicosLigados); // data[2]
|
|
data.push_back(LimitarByte(PotenciaFeedForwardAtual)); // data[3]
|
|
data.push_back(LimitarByte(PotenciaFinalAtual)); // data[4]
|
|
data.push_back(LimitarByte(PressaoSensorAtual)); // data[5]
|
|
data.push_back(LimitarByte(PressaoSPControleAtual)); // data[6]
|
|
data.push_back(MontarFlagsDiagnosticoBomba()); // data[7]
|
|
break;
|
|
}
|
|
|
|
default:
|
|
break;
|
|
}
|
|
|
|
return data;
|
|
}
|
|
|
|
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;
|
|
|
|
BicoPulverizador_v2** _bicos = nullptr;
|
|
size_t _qtdBicos = 0;
|
|
|
|
unsigned long _ultimoControleMs = 0;
|
|
unsigned long _ultimoTempoDtMs = 0;
|
|
|
|
bool _filtroInicializado = false;
|
|
double _ultimaPressaoFiltrada = 0.0;
|
|
double _integralPressao = 0.0;
|
|
double _ultimoErroPressao = 0.0;
|
|
|
|
uint8_t _ultimaQtdBicosLigados = 0;
|
|
|
|
bool _kickAtivo = false;
|
|
unsigned long _inicioKickMs = 0;
|
|
|
|
unsigned long _ultimoPulsoFluxoMs = 0;
|
|
unsigned long _inicioSemFluxoMs = 0;
|
|
unsigned long _inicioComFluxoMs = 0;
|
|
|
|
unsigned long _ultimoComandoLigadoMs = 0;
|
|
unsigned long _ultimaMudancaBicosCavitacaoMs = 0;
|
|
unsigned long _inicioCondicaoCavitacaoMs = 0;
|
|
unsigned long _inicioCondicaoNormalCavitacaoMs = 0;
|
|
|
|
uint8_t _ultimaQtdBicosCavitacao = 0;
|
|
Estado _ultimoEstadoControleCavitacao = Estado::Desligado;
|
|
|
|
void ResetarControle() {
|
|
ResetarIntegralPressao();
|
|
ResetarSupervisorFluxo();
|
|
ResetarDetectorCavitacao();
|
|
ResetarKick();
|
|
_ultimoControleMs = 0;
|
|
_ultimoTempoDtMs = 0;
|
|
_ultimaQtdBicosLigados = 0;
|
|
_filtroInicializado = false;
|
|
PressaoSensorAtual = 0.0;
|
|
PressaoAtual = 0.0;
|
|
PressaoFiltrada = 0.0;
|
|
PressaoSPControleAtual = 0.0;
|
|
PressaoSPLimitadoPorCapacidade = false;
|
|
PressaoSPForaDoMapaAtual = false;
|
|
PressaoMaximaMapeadaAtual = 0.0;
|
|
PotenciaFeedForwardAtual = 0.0;
|
|
CorrecaoPIAtual = 0.0;
|
|
bombaDesligadaPorPressao = false;
|
|
CorteSegurancaAtivo = false;
|
|
ArmamentoLinhaAtivo = false;
|
|
LinhaArmadaPorFluxo = false;
|
|
FalhaArmamentoLinhaAtual = false;
|
|
ResetarFluxo();
|
|
}
|
|
|
|
void ResetarEstadosDeBicosFechados() {
|
|
ResetarIntegralPressao();
|
|
ResetarSupervisorFluxo();
|
|
ResetarDetectorCavitacao();
|
|
ResetarKick();
|
|
PotenciaFeedForwardAtual = 0.0;
|
|
CorrecaoPIAtual = 0.0;
|
|
PressaoSPControleAtual = 0.0;
|
|
PressaoSPForaDoMapaAtual = false;
|
|
bombaDesligadaPorPressao = false;
|
|
ArmamentoLinhaAtivo = false;
|
|
LinhaArmadaPorFluxo = false;
|
|
FalhaArmamentoLinhaAtual = false;
|
|
}
|
|
|
|
void ResetarIntegralPressao() {
|
|
_integralPressao = 0.0;
|
|
_ultimoErroPressao = 0.0;
|
|
CorrecaoPIAtual = 0.0;
|
|
}
|
|
|
|
void ResetarKick() {
|
|
_kickAtivo = false;
|
|
_inicioKickMs = 0;
|
|
}
|
|
|
|
void ResetarFluxo() {
|
|
VazaoFluxoAtualMLs = 0.0;
|
|
VazaoFluxoCorrigidaAtualMLs = 0.0;
|
|
FluxoPresenteAtual = false;
|
|
FluxoInsuficienteAtual = false;
|
|
RecuperacaoFluxoAtiva = false;
|
|
_ultimoPulsoFluxoMs = 0;
|
|
_inicioSemFluxoMs = 0;
|
|
_inicioComFluxoMs = 0;
|
|
BoostFluxoAtualPct = 0.0;
|
|
}
|
|
|
|
void ResetarSupervisorFluxo() {
|
|
FluxoInsuficienteAtual = false;
|
|
RecuperacaoFluxoAtiva = false;
|
|
_inicioSemFluxoMs = 0;
|
|
_inicioComFluxoMs = 0;
|
|
BoostFluxoAtualPct = 0.0;
|
|
}
|
|
|
|
uint8_t LimitarIndiceBicos(uint8_t idx) {
|
|
if (idx > 7) {
|
|
return 7;
|
|
}
|
|
return idx;
|
|
}
|
|
|
|
uint8_t ContarBicosLigados() {
|
|
if (_bicos == nullptr || _qtdBicos == 0) {
|
|
return 0;
|
|
}
|
|
|
|
uint8_t qtd = 0;
|
|
|
|
for (size_t i = 0; i < _qtdBicos; i++) {
|
|
if (_bicos[i] == nullptr) {
|
|
continue;
|
|
}
|
|
|
|
/*
|
|
A leitura fisica do bico e atualizada no proprio model do bico.
|
|
Chamamos aqui para que a bomba enxergue o estado fisico mais recente.
|
|
*/
|
|
_bicos[i]->AtualizarLeitura();
|
|
|
|
if (_bicos[i]->_Status == Estado::Ligado) {
|
|
qtd++;
|
|
}
|
|
}
|
|
|
|
return LimitarIndiceBicos(qtd);
|
|
}
|
|
|
|
void AtualizarPressao() {
|
|
if (sensorPressao == nullptr) {
|
|
PressaoSensorAtual = 0.0;
|
|
PressaoAtual = 0.0;
|
|
return;
|
|
}
|
|
|
|
sensorPressao->RequisitarDados();
|
|
PressaoSensorAtual = sensorPressao->Pressao;
|
|
|
|
uint8_t idx = LimitarIndiceBicos(QtdBicosLigados);
|
|
double pressaoAplicacao = PressaoSensorAtual + CorrecaoPressaoAplicacaoPorBicos[idx];
|
|
|
|
if (pressaoAplicacao < 0.0) {
|
|
pressaoAplicacao = 0.0;
|
|
}
|
|
|
|
PressaoAtual = pressaoAplicacao;
|
|
|
|
if (!_filtroInicializado) {
|
|
PressaoFiltrada = PressaoAtual;
|
|
_ultimaPressaoFiltrada = PressaoFiltrada;
|
|
_filtroInicializado = true;
|
|
return;
|
|
}
|
|
|
|
double alpha = constrain(AlphaFiltroPressao, 0.01, 1.0);
|
|
PressaoFiltrada = (alpha * PressaoAtual) + ((1.0 - alpha) * PressaoFiltrada);
|
|
}
|
|
|
|
bool SensorFluxoDisponivel() {
|
|
return sensorFluxo != nullptr && sensorFluxo->Iniciado;
|
|
}
|
|
|
|
void AtualizarFluxo() {
|
|
VazaoFluxoAtualMLs = 0.0;
|
|
VazaoFluxoCorrigidaAtualMLs = 0.0;
|
|
FluxoPresenteAtual = false;
|
|
|
|
if (!SensorFluxoDisponivel()) {
|
|
return;
|
|
}
|
|
|
|
/*
|
|
Nao chama RequisitarDados(), porque aquilo reinicia a janela de amostragem
|
|
do sensor e pode atrapalhar o log/CAN do FLX.
|
|
*/
|
|
sensorFluxo->Loop();
|
|
|
|
unsigned long agora = millis();
|
|
unsigned int pulsos = sensorFluxo->_Pulsos;
|
|
unsigned long periodoMs = sensorFluxo->_PeriodoTotal;
|
|
|
|
if (pulsos > 0) {
|
|
_ultimoPulsoFluxoMs = agora;
|
|
}
|
|
|
|
double vazaoMLs = 0.0;
|
|
|
|
if (pulsos > 0 && periodoMs > 0) {
|
|
double frequenciaHz = ((double)pulsos) * 1000.0 / ((double)periodoMs);
|
|
double q_L_min = (frequenciaHz + 3.0) / 8.1;
|
|
|
|
if (q_L_min < 0.0) {
|
|
q_L_min = 0.0;
|
|
}
|
|
|
|
if (q_L_min > 20.0) {
|
|
q_L_min = 20.0;
|
|
}
|
|
|
|
vazaoMLs = q_L_min * (1000.0 / 60.0);
|
|
}
|
|
|
|
bool pulsoRecente =
|
|
_ultimoPulsoFluxoMs > 0 &&
|
|
(agora - _ultimoPulsoFluxoMs) <= TimeoutPulsoFluxoRecenteMs;
|
|
|
|
if (!pulsoRecente) {
|
|
vazaoMLs = 0.0;
|
|
}
|
|
|
|
VazaoFluxoAtualMLs = vazaoMLs;
|
|
VazaoFluxoCorrigidaAtualMLs = vazaoMLs * FatorCalibracaoFluxoFirmware;
|
|
|
|
FluxoPresenteAtual =
|
|
pulsoRecente ||
|
|
VazaoFluxoCorrigidaAtualMLs >= VazaoMinimaOperacaoMLs;
|
|
}
|
|
|
|
void IniciarKick(uint8_t idx) {
|
|
if (!HabilitarKickInicial || idx == 0) {
|
|
return;
|
|
}
|
|
|
|
_kickAtivo = true;
|
|
_inicioKickMs = millis();
|
|
ArmamentoLinhaAtivo = true;
|
|
}
|
|
|
|
double ObterPotenciaKick(uint8_t idx) {
|
|
idx = LimitarIndiceBicos(idx);
|
|
|
|
if (idx > 0 && idx <= 2) {
|
|
return constrain(PotenciaKickPoucosBicosPct, 0.0, 100.0);
|
|
}
|
|
|
|
if (idx >= 3) {
|
|
return constrain(PotenciaKickMuitosBicosPct, 0.0, 100.0);
|
|
}
|
|
|
|
return 0.0;
|
|
}
|
|
|
|
unsigned long ObterDuracaoKickMs(uint8_t idx) {
|
|
idx = LimitarIndiceBicos(idx);
|
|
|
|
if (idx > 0 && idx <= 2) {
|
|
return DuracaoKickPoucosBicosMs;
|
|
}
|
|
|
|
if (idx >= 3) {
|
|
return DuracaoKickMuitosBicosMs;
|
|
}
|
|
|
|
return 0;
|
|
}
|
|
|
|
double AplicarKickInicial(double potenciaDesejada, uint8_t idx) {
|
|
if (!_kickAtivo || idx == 0) {
|
|
return potenciaDesejada;
|
|
}
|
|
|
|
unsigned long duracao = ObterDuracaoKickMs(idx);
|
|
unsigned long agora = millis();
|
|
|
|
if (duracao == 0 || (agora - _inicioKickMs) > duracao) {
|
|
ResetarKick();
|
|
ArmamentoLinhaAtivo = false;
|
|
return potenciaDesejada;
|
|
}
|
|
|
|
return MaiorDouble(potenciaDesejada, ObterPotenciaKick(idx));
|
|
}
|
|
|
|
double ObterPotenciaMinimaOperacional(uint8_t idx) {
|
|
idx = LimitarIndiceBicos(idx);
|
|
|
|
double minimo = PotenciaMinAtiva;
|
|
|
|
if (idx > 0) {
|
|
minimo = MaiorDouble(minimo, PotenciaMinimaComBicosPorQtd[idx]);
|
|
}
|
|
|
|
minimo = MaiorDouble(minimo, PotenciaMinPID);
|
|
return constrain(minimo, 0.0, PotenciaMaxPID);
|
|
}
|
|
|
|
double ObterPressaoMaximaMapeada(uint8_t idx) {
|
|
idx = LimitarIndiceBicos(idx);
|
|
return PressaoMaximaMapeadaPorBicos[idx];
|
|
}
|
|
|
|
double ObterPotenciaMaximaMapeada(uint8_t idx) {
|
|
idx = LimitarIndiceBicos(idx);
|
|
|
|
double potencia = PotenciaBaseMaxPsi[idx];
|
|
|
|
if (potencia <= 0.1) {
|
|
potencia = PotenciaBase22Psi[idx];
|
|
}
|
|
|
|
if (potencia <= 0.1) {
|
|
potencia = PotenciaBase15Psi[idx];
|
|
}
|
|
|
|
return constrain(potencia, 0.0, 100.0);
|
|
}
|
|
|
|
double Interpolar(double x1, double y1, double x2, double y2, double x) {
|
|
if (fabs(x2 - x1) < 0.001) {
|
|
return constrain(y2, 0.0, 100.0);
|
|
}
|
|
|
|
double t = (x - x1) / (x2 - x1);
|
|
t = constrain(t, 0.0, 1.0);
|
|
|
|
double y = y1 + ((y2 - y1) * t);
|
|
return constrain(y, 0.0, 100.0);
|
|
}
|
|
|
|
double CalcularPotenciaFeedForward(uint8_t idx, double pressaoSP) {
|
|
idx = LimitarIndiceBicos(idx);
|
|
|
|
if (!HabilitarFeedForward || idx == 0 || pressaoSP <= 0.0) {
|
|
return 0.0;
|
|
}
|
|
|
|
double p15 = constrain(PotenciaBase15Psi[idx], 0.0, 100.0);
|
|
double p22 = constrain(PotenciaBase22Psi[idx], 0.0, 100.0);
|
|
double pMax = ObterPotenciaMaximaMapeada(idx);
|
|
double pressaoMax = ObterPressaoMaximaMapeada(idx);
|
|
|
|
double potencia = 0.0;
|
|
|
|
if (pressaoMax > 0.0 && pressaoSP >= pressaoMax) {
|
|
potencia = pMax;
|
|
} else if (pressaoMax > 0.0 && pressaoMax <= PressaoReferenciaBaixaPsi) {
|
|
potencia = Interpolar(0.0, 0.0, pressaoMax, pMax, pressaoSP);
|
|
} else if (pressaoSP <= PressaoReferenciaBaixaPsi) {
|
|
potencia = p15;
|
|
} else if (pressaoMax > PressaoReferenciaBaixaPsi && pressaoMax <= PressaoReferenciaAltaPsi) {
|
|
potencia = Interpolar(
|
|
PressaoReferenciaBaixaPsi,
|
|
p15,
|
|
pressaoMax,
|
|
pMax,
|
|
pressaoSP
|
|
);
|
|
} else if (pressaoSP <= PressaoReferenciaAltaPsi) {
|
|
potencia = Interpolar(
|
|
PressaoReferenciaBaixaPsi,
|
|
p15,
|
|
PressaoReferenciaAltaPsi,
|
|
p22,
|
|
pressaoSP
|
|
);
|
|
} else if (pressaoMax > PressaoReferenciaAltaPsi && pressaoSP <= pressaoMax) {
|
|
potencia = Interpolar(
|
|
PressaoReferenciaAltaPsi,
|
|
p22,
|
|
pressaoMax,
|
|
pMax,
|
|
pressaoSP
|
|
);
|
|
} else {
|
|
potencia = pMax;
|
|
}
|
|
|
|
return constrain(potencia * constrain(PesoFeedForward, 0.0, 1.0), 0.0, 100.0);
|
|
}
|
|
|
|
double CalcularPressaoSPControle(uint8_t idx, double pressaoSP) {
|
|
idx = LimitarIndiceBicos(idx);
|
|
|
|
double spControle = pressaoSP;
|
|
|
|
bool precisaFluxo =
|
|
idx > 0 &&
|
|
HabilitarSupervisorFluxo &&
|
|
SensorFluxoDisponivel() &&
|
|
!FluxoPresenteAtual;
|
|
|
|
if (precisaFluxo) {
|
|
if (idx <= 2 && PressaoSPMinSemFluxoPoucosBicosPsi > 0.0) {
|
|
spControle = MaiorDouble(spControle, PressaoSPMinSemFluxoPoucosBicosPsi);
|
|
} else if (idx >= 3 && PressaoSPMinSemFluxoMuitosBicosPsi > 0.0) {
|
|
spControle = MaiorDouble(spControle, PressaoSPMinSemFluxoMuitosBicosPsi);
|
|
}
|
|
}
|
|
|
|
return constrain(spControle, 0.0, 250.0);
|
|
}
|
|
|
|
bool SPForaDoMapa(uint8_t idx, double pressaoSP) {
|
|
if (idx == 0 || pressaoSP <= 0.0) {
|
|
return false;
|
|
}
|
|
|
|
double pressaoMax = ObterPressaoMaximaMapeada(idx);
|
|
|
|
if (pressaoMax <= 0.0) {
|
|
return false;
|
|
}
|
|
|
|
return pressaoSP > pressaoMax;
|
|
}
|
|
|
|
bool TratarCorteSeguranca() {
|
|
if (sensorPressao == nullptr || PressaoCorteSegurancaPsi <= 0.0) {
|
|
CorteSegurancaAtivo = false;
|
|
return false;
|
|
}
|
|
|
|
double pressaoProtecao = MaiorDouble(PressaoFiltrada, PressaoSensorAtual);
|
|
|
|
if (pressaoProtecao >= PressaoCorteSegurancaPsi) {
|
|
CorteSegurancaAtivo = true;
|
|
}
|
|
|
|
if (CorteSegurancaAtivo) {
|
|
AplicarSaida(false, 0);
|
|
ResetarIntegralPressao();
|
|
bombaDesligadaPorPressao = true;
|
|
|
|
if (pressaoProtecao <= (PressaoCorteSegurancaPsi - HistereseRearmeSegurancaPsi)) {
|
|
CorteSegurancaAtivo = false;
|
|
bombaDesligadaPorPressao = false;
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
return false;
|
|
}
|
|
|
|
void ControlarOnOffLegado(double pressaoSP, double potenciaOn, double histeresePsi) {
|
|
PressaoSPControleAtual = pressaoSP;
|
|
PotenciaFeedForwardAtual = 0.0;
|
|
CorrecaoPIAtual = 0.0;
|
|
|
|
double histerese = constrain(histeresePsi, 0.5, 30.0);
|
|
|
|
if (PressaoFiltrada >= (pressaoSP + histerese)) {
|
|
bombaDesligadaPorPressao = true;
|
|
}
|
|
|
|
if (PressaoFiltrada <= (pressaoSP - histerese)) {
|
|
bombaDesligadaPorPressao = false;
|
|
}
|
|
|
|
if (bombaDesligadaPorPressao) {
|
|
AplicarSaida(false, 0);
|
|
return;
|
|
}
|
|
|
|
double potencia = potenciaOn;
|
|
|
|
if (potencia <= 0.0) {
|
|
potencia = PotenciaMaxPID;
|
|
}
|
|
|
|
if (QtdBicosLigados > 0) {
|
|
potencia = MaiorDouble(potencia, ObterPotenciaMinimaOperacional(QtdBicosLigados));
|
|
}
|
|
|
|
potencia = constrain(potencia, 0.0, PotenciaMaxPID);
|
|
AplicarPotenciaPercentual(potencia);
|
|
}
|
|
|
|
void ControlarPressaoSuaveComFluxo(double dt) {
|
|
uint8_t idx = LimitarIndiceBicos(QtdBicosLigados);
|
|
|
|
if (idx != _ultimaQtdBicosLigados) {
|
|
uint8_t anterior = _ultimaQtdBicosLigados;
|
|
_ultimaQtdBicosLigados = idx;
|
|
ResetarIntegralPressao();
|
|
|
|
if (anterior == 0 && idx > 0) {
|
|
IniciarKick(idx);
|
|
}
|
|
}
|
|
|
|
if (idx == 0) {
|
|
ResetarEstadosDeBicosFechados();
|
|
return;
|
|
}
|
|
|
|
double spControle = CalcularPressaoSPControle(idx, _PressaoSP);
|
|
PressaoSPControleAtual = spControle;
|
|
PressaoMaximaMapeadaAtual = ObterPressaoMaximaMapeada(idx);
|
|
PressaoSPForaDoMapaAtual = SPForaDoMapa(idx, spControle);
|
|
PressaoSPLimitadoPorCapacidade = false;
|
|
|
|
double potenciaBase = CalcularPotenciaFeedForward(idx, spControle);
|
|
PotenciaFeedForwardAtual = potenciaBase;
|
|
|
|
double erro = spControle - PressaoFiltrada;
|
|
double erroParaPI = erro;
|
|
|
|
if (fabs(erroParaPI) <= DeadbandPressaoPsi) {
|
|
erroParaPI = 0.0;
|
|
}
|
|
|
|
double derivadaMedicao = 0.0;
|
|
|
|
if (dt > 0.001) {
|
|
derivadaMedicao = (PressaoFiltrada - _ultimaPressaoFiltrada) / dt;
|
|
}
|
|
|
|
double minimo = ObterPotenciaMinimaOperacional(idx);
|
|
double maximo = PotenciaMaxPID;
|
|
|
|
/*
|
|
Integrador anti-windup. Integra pouco quando saturado.
|
|
*/
|
|
double saidaTeste =
|
|
potenciaBase +
|
|
(Kp * erroParaPI) +
|
|
(Ki * _integralPressao) -
|
|
(Kd * derivadaMedicao);
|
|
|
|
bool saturadoAlto = saidaTeste >= maximo && erroParaPI > 0.0;
|
|
bool saturadoBaixo = saidaTeste <= minimo && erroParaPI < 0.0;
|
|
|
|
if (!saturadoAlto && !saturadoBaixo) {
|
|
_integralPressao += erroParaPI * dt;
|
|
_integralPressao = constrain(_integralPressao, -220.0, 220.0);
|
|
}
|
|
|
|
double correcao =
|
|
(Kp * erroParaPI) +
|
|
(Ki * _integralPressao) -
|
|
(Kd * derivadaMedicao);
|
|
|
|
CorrecaoPIAtual = correcao;
|
|
|
|
double potenciaDesejada = potenciaBase + correcao;
|
|
potenciaDesejada = constrain(potenciaDesejada, minimo, maximo);
|
|
|
|
potenciaDesejada = AplicarSupervisorFluxo(potenciaDesejada, dt);
|
|
potenciaDesejada = AplicarKickInicial(potenciaDesejada, idx);
|
|
|
|
double potenciaComRampa = AplicarRampaPercentual(potenciaDesejada, dt);
|
|
AplicarPotenciaPercentual(potenciaComRampa);
|
|
|
|
_ultimoErroPressao = erro;
|
|
_ultimaPressaoFiltrada = PressaoFiltrada;
|
|
}
|
|
|
|
double AplicarSupervisorFluxo(double potenciaDesejada, double dt) {
|
|
uint8_t idx = LimitarIndiceBicos(QtdBicosLigados);
|
|
|
|
if (!HabilitarSupervisorFluxo || !SensorFluxoDisponivel() || idx == 0) {
|
|
ResetarSupervisorFluxo();
|
|
return potenciaDesejada;
|
|
}
|
|
|
|
unsigned long agora = millis();
|
|
bool fluxoOk = FluxoPresenteAtual || VazaoFluxoCorrigidaAtualMLs >= VazaoMinimaOperacaoMLs;
|
|
|
|
if (!fluxoOk) {
|
|
FluxoInsuficienteAtual = true;
|
|
|
|
if (_inicioSemFluxoMs == 0) {
|
|
_inicioSemFluxoMs = agora;
|
|
}
|
|
|
|
_inicioComFluxoMs = 0;
|
|
|
|
if ((agora - _inicioSemFluxoMs) >= TempoSemFluxoParaBoostMs) {
|
|
RecuperacaoFluxoAtiva = true;
|
|
}
|
|
} else {
|
|
FluxoInsuficienteAtual = false;
|
|
_inicioSemFluxoMs = 0;
|
|
|
|
if (_inicioComFluxoMs == 0) {
|
|
_inicioComFluxoMs = agora;
|
|
}
|
|
|
|
if ((agora - _inicioComFluxoMs) >= TempoComFluxoParaAliviarMs) {
|
|
RecuperacaoFluxoAtiva = false;
|
|
}
|
|
}
|
|
|
|
double maxBoost = (idx <= 2)
|
|
? BoostFluxoMaxPoucosBicosPct
|
|
: BoostFluxoMaxMuitosBicosPct;
|
|
|
|
maxBoost = constrain(maxBoost, 0.0, 100.0);
|
|
|
|
bool permitirSubirBoost =
|
|
RecuperacaoFluxoAtiva &&
|
|
PressaoFiltrada < PressaoMaxAssistenciaFluxoPsi;
|
|
|
|
if (permitirSubirBoost) {
|
|
BoostFluxoAtualPct += TaxaBoostFluxoSubidaPctPorSegundo * dt;
|
|
} else {
|
|
BoostFluxoAtualPct -= TaxaBoostFluxoDescidaPctPorSegundo * dt;
|
|
}
|
|
|
|
BoostFluxoAtualPct = constrain(BoostFluxoAtualPct, 0.0, maxBoost);
|
|
|
|
if (BoostFluxoAtualPct > 0.1) {
|
|
potenciaDesejada = MaiorDouble(
|
|
potenciaDesejada,
|
|
ObterPotenciaMinimaOperacional(idx)
|
|
);
|
|
|
|
potenciaDesejada += BoostFluxoAtualPct;
|
|
}
|
|
|
|
return constrain(potenciaDesejada, 0.0, PotenciaMaxPID);
|
|
}
|
|
|
|
double AplicarRampaPercentual(double potenciaDesejada, double dt) {
|
|
potenciaDesejada = constrain(potenciaDesejada, 0.0, PotenciaMaxPID);
|
|
|
|
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;
|
|
}
|
|
|
|
uint8_t LimitarByte(double valor) {
|
|
valor = constrain(valor, 0.0, 255.0);
|
|
return static_cast<uint8_t>(round(valor));
|
|
}
|
|
|
|
double MaiorDouble(double a, double b) {
|
|
return (a > b) ? a : b;
|
|
}
|
|
|
|
void ResetarDetectorCavitacao() {
|
|
CavitacaoSuspeitaAtual = false;
|
|
CavitadaAtual = false;
|
|
|
|
_inicioCondicaoCavitacaoMs = 0;
|
|
_inicioCondicaoNormalCavitacaoMs = 0;
|
|
_ultimaMudancaBicosCavitacaoMs = 0;
|
|
_ultimoComandoLigadoMs = 0;
|
|
|
|
_ultimaQtdBicosCavitacao = 0;
|
|
_ultimoEstadoControleCavitacao = _EstadoControle;
|
|
}
|
|
|
|
void AtualizarDetectorCavitacao() {
|
|
unsigned long agora = millis();
|
|
|
|
if (!HabilitarDetectorCavitacao) {
|
|
ResetarDetectorCavitacao();
|
|
return;
|
|
}
|
|
|
|
if (_EstadoControle != _ultimoEstadoControleCavitacao) {
|
|
_ultimoEstadoControleCavitacao = _EstadoControle;
|
|
|
|
if (_EstadoControle == Estado::Ligado) {
|
|
_ultimoComandoLigadoMs = agora;
|
|
} else {
|
|
ResetarDetectorCavitacao();
|
|
return;
|
|
}
|
|
}
|
|
|
|
if (QtdBicosLigados != _ultimaQtdBicosCavitacao) {
|
|
_ultimaQtdBicosCavitacao = QtdBicosLigados;
|
|
_ultimaMudancaBicosCavitacaoMs = agora;
|
|
|
|
_inicioCondicaoCavitacaoMs = 0;
|
|
CavitacaoSuspeitaAtual = false;
|
|
}
|
|
|
|
if (_EstadoControle != Estado::Ligado) {
|
|
ResetarDetectorCavitacao();
|
|
return;
|
|
}
|
|
|
|
if (QtdBicosLigados == 0) {
|
|
ResetarDetectorCavitacao();
|
|
return;
|
|
}
|
|
|
|
if (!SensorFluxoDisponivel() || sensorPressao == nullptr) {
|
|
CavitacaoSuspeitaAtual = false;
|
|
CavitadaAtual = false;
|
|
_inicioCondicaoCavitacaoMs = 0;
|
|
_inicioCondicaoNormalCavitacaoMs = 0;
|
|
return;
|
|
}
|
|
|
|
bool emJanelaIgnorada = false;
|
|
|
|
if (_ultimoComandoLigadoMs > 0 && (agora - _ultimoComandoLigadoMs) < TempoIgnorarAposLigarBombaMs) {
|
|
emJanelaIgnorada = true;
|
|
}
|
|
|
|
if (_ultimaMudancaBicosCavitacaoMs > 0 && (agora - _ultimaMudancaBicosCavitacaoMs) < TempoIgnorarAposMudancaBicosMs) {
|
|
emJanelaIgnorada = true;
|
|
}
|
|
|
|
if (emJanelaIgnorada) {
|
|
_inicioCondicaoCavitacaoMs = 0;
|
|
CavitacaoSuspeitaAtual = false;
|
|
return;
|
|
}
|
|
|
|
bool bombaRealmenteAtiva =
|
|
EstadoLeitura == Estado::Ligado &&
|
|
PotenciaFinalAtual >= PotenciaMinimaParaDetectarCavitacaoPct;
|
|
|
|
bool pressaoMuitoBaixa =
|
|
PressaoFiltrada <= PressaoMinimaCavitacaoPsi;
|
|
|
|
bool pressaoAbaixoDoSP =
|
|
_PressaoSP > 0.0 &&
|
|
PressaoFiltrada <= (_PressaoSP - MargemPressaoSPCavitacaoPsi);
|
|
|
|
bool semFluxo =
|
|
!FluxoPresenteAtual &&
|
|
VazaoFluxoCorrigidaAtualMLs <= VazaoMinimaCavitacaoMLs;
|
|
|
|
bool condicaoCavitacao =
|
|
bombaRealmenteAtiva &&
|
|
pressaoMuitoBaixa &&
|
|
pressaoAbaixoDoSP &&
|
|
semFluxo;
|
|
|
|
if (condicaoCavitacao) {
|
|
if (_inicioCondicaoCavitacaoMs == 0) {
|
|
_inicioCondicaoCavitacaoMs = agora;
|
|
}
|
|
|
|
_inicioCondicaoNormalCavitacaoMs = 0;
|
|
|
|
unsigned long tempoCondicao = agora - _inicioCondicaoCavitacaoMs;
|
|
|
|
CavitacaoSuspeitaAtual =
|
|
tempoCondicao >= TempoParaSuspeitarCavitacaoMs;
|
|
|
|
if (tempoCondicao >= TempoParaConfirmarCavitacaoMs) {
|
|
CavitadaAtual = true;
|
|
}
|
|
|
|
return;
|
|
}
|
|
|
|
_inicioCondicaoCavitacaoMs = 0;
|
|
CavitacaoSuspeitaAtual = false;
|
|
|
|
bool condicaoNormal =
|
|
_EstadoControle != Estado::Ligado ||
|
|
QtdBicosLigados == 0 ||
|
|
FluxoPresenteAtual ||
|
|
VazaoFluxoCorrigidaAtualMLs > VazaoMinimaCavitacaoMLs ||
|
|
PressaoFiltrada >= (PressaoMinimaCavitacaoPsi + 3.0) ||
|
|
PotenciaFinalAtual < PotenciaMinimaParaDetectarCavitacaoPct;
|
|
|
|
if (CavitadaAtual && condicaoNormal) {
|
|
if (_inicioCondicaoNormalCavitacaoMs == 0) {
|
|
_inicioCondicaoNormalCavitacaoMs = agora;
|
|
}
|
|
|
|
if ((agora - _inicioCondicaoNormalCavitacaoMs) >= TempoParaLimparCavitacaoMs) {
|
|
CavitadaAtual = false;
|
|
_inicioCondicaoNormalCavitacaoMs = 0;
|
|
}
|
|
} else if (!condicaoNormal) {
|
|
_inicioCondicaoNormalCavitacaoMs = 0;
|
|
}
|
|
}
|
|
|
|
uint8_t MontarFlagsDiagnosticoBomba() {
|
|
uint8_t flags = 0;
|
|
|
|
if (PressaoSPForaDoMapaAtual) flags |= (1 << 0);
|
|
if (FluxoInsuficienteAtual) flags |= (1 << 1);
|
|
if (RecuperacaoFluxoAtiva) flags |= (1 << 2);
|
|
if (CavitacaoSuspeitaAtual) flags |= (1 << 3);
|
|
if (CavitadaAtual) flags |= (1 << 4);
|
|
if (CorteSegurancaAtivo) flags |= (1 << 5);
|
|
if (PressaoSPLimitadoPorCapacidade) flags |= (1 << 6);
|
|
|
|
return flags;
|
|
}
|
|
|
|
bool TemBicosConfigurados() {
|
|
return _bicos != nullptr && _qtdBicos > 0;
|
|
}
|
|
|
|
void MostrarLog(String mensagem) {
|
|
if (DebugMode) {
|
|
PrintTela("[BMB " + _ID + "] " + mensagem);
|
|
}
|
|
}
|
|
};
|
|
|
|
#endif
|