agrobot_base/Firmware/Modulos/BombaPressurizadoraModel_v2.h

#include "SerialService.h"
#include "Pinout.h"
#include "Utils.h"
#include "IComponenteCAN.h"
#include "SensorPressaoModel_v2.h"

#ifndef BombaPressurizadoraModel_v2_h
#define BombaPressurizadoraModel_v2_h

struct EstadoAnteriorBomba {
  Estado estado = Estado::Desligado;

  double potenciaSP = 0;
  double pressaoSP = 0;

  ModoControleBomba modoControle = ModoControleBomba::OnOff;

  double kp = 0;
  double ki = 0;
  double kd = 0;

  double potenciaMinPID = 0;
  double potenciaMaxPID = 100;
};

class BombaPressurizadora_v2 : public ComponenteCAN {
  public:
    BombaPressurizadora_v2(
      const String& id,
      uint8_t idNum,
      int pinoIN1,
      int canalIN1,
      int pinoIN2,
      int canalIN2,
      TiposBarramentos barramento = CONTROLADOR,
      Sentido sentido = Sentido::Horario,
      SensorPressao_v2* sensor = nullptr
    )
    : _ID(id),
      ID_Num(idNum),
      _pinoIN1(pinoIN1, barramento, _PWM),
      _pinoIN2(pinoIN2, barramento, _PWM),
      _canalIN1(canalIN1),
      _canalIN2(canalIN2),
      _sentido(sentido),
      sensorPressao(sensor) {
    }

    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;

    Estado EstadoLeitura = Estado::Desligado;
    double PWM_SP = 0;
    double PWM_Set = 0;
    double PercentualPotenciaLeitura = 0;

    Estado _EstadoControle = Estado::Desligado;
    double _PressaoSP = 0;
    double _PotenciaSP = 0;

    ModoControleBomba ModoControle = ModoControleBomba::OnOff;

    // PID
    double Kp = 8.0;
    double Ki = 0.4;
    double Kd = 0.0;

    double erroAnterior = 0;
    double integralErro = 0;
    unsigned long ultimoPID_ms = 0;

    // limites PID em percentual
    double PotenciaMinPID = 25.0;
    double PotenciaMaxPID = 100.0;

    // proteção contra integral absurda
    double IntegralMin = -100.0;
    double IntegralMax = 100.0;

    // zona morta para não ficar tremendo perto do SP
    double DeadbandPressao = 1.0;

    void VincularSensorPressao(SensorPressao_v2* sensor) {
      sensorPressao = sensor;
      MalhaFechada = (sensorPressao != nullptr);
    }

    void Inicializar() {
      if (Iniciado) {
        MostrarLog("Bomba ja inicializada");
        return;
      }

      MalhaFechada = (sensorPressao != nullptr);

      _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);
      }

      Iniciado = _pinoIN1.Definido() && _pinoIN2.Definido();

      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");
      }
    }

    void Desligar() {
      if (!Iniciado) {
        MostrarLog("Bomba nao esta inicializada");
        return;
      }

      _EstadoControle = Estado::Desligado;
      ledcWrite(_canalIN1, 0);
      ledcWrite(_canalIN2, 0);

      _pinoIN1.Desconectar();
      _pinoIN2.Desconectar();
      
      PWM_Set = ZeroRampa;
      PWM_SP = ZeroRampa;
      PercentualPotenciaLeitura = 0;
      EstadoLeitura = Estado::Desligado;
      bombaDesligadaPorPressao = false;
      Iniciado = false;

      MostrarLog("Bomba desligada");
    }

    void AtualizarControle(Estado novoEstado, double potenciaSP = -1, double pressaoSP = -1) {
      if (!Iniciado) {
        MostrarLog("Ignorando comando: bomba nao iniciada");
        return;
      }

      _EstadoControle = novoEstado;

      if (potenciaSP >= 0) {
        _PotenciaSP = constrain(potenciaSP, 0, 100);
        PWM_Set = int((_PotenciaSP / 100.0) * RampaMax);
      }

      if (pressaoSP >= 0 && MalhaFechada) {
        _PressaoSP = pressaoSP;
      }
    }

    void Loop() {
      if (!Iniciado) return;

      double pressaoAtual = 0;
      bool ligado = _EstadoControle == Estado::Ligado;

      if (ligado && MalhaFechada && sensorPressao != nullptr) {
        sensorPressao->RequisitarDados();
        pressaoAtual = sensorPressao->Pressao;
      }

      if (!ligado) {
        PWM_SP = ZeroRampa;
        ResetPID();
      } else {
        if (MalhaFechada && sensorPressao != nullptr && _PressaoSP > 0) {

          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;
            }

            PWM_SP = bombaDesligadaPorPressao ? ZeroRampa : PWM_Set;
          }

        } else {
          PWM_SP = PWM_Set;
        }
      }

      AplicarSaida(ligado, PWM_SP);
      AtualizarLeitura();
    }

    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] : -1;
          double pressao = (data.size() > 4) ? (double)data[4] : -1;
          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;
          }
          ResetPID();
          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 ? 1 : 0);
          break;
        }

        case CanMessagePosicaoDados::Dados1: {
          AtualizarLeitura();
          data.push_back(static_cast<uint8_t>(EstadoLeitura));
          data.push_back(static_cast<uint8_t>((int)_PotenciaSP));
          data.push_back(static_cast<uint8_t>((int)PercentualPotenciaLeitura));
          data.push_back(static_cast<uint8_t>((int)_PressaoSP));
          int pressaoAtual = 0;
          if (sensorPressao != nullptr) {
            pressaoAtual = (int)sensorPressao->Pressao;
          }
          data.push_back(static_cast<uint8_t>(pressaoAtual));
          data.push_back(static_cast<uint8_t>(ModoControle));
          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:
    bool DebugMode = false;
    int histerese = 3;
    int ZeroRampa = 0;
    int RampaMax = 1023;

    void AplicarSaida(bool ligado, int pwm) {
      if (!Iniciado && pwm != 0) return;

      if (!ligado) {
        pwm = 0;
      }

      pwm = LimitarPWM(pwm);
      ledcWrite(_canalIN1, _sentido == Sentido::Horario ? pwm : 0);
      ledcWrite(_canalIN2, _sentido == Sentido::Horario ? 0 : pwm);
    }

    int LimitarPWM(int valor) {
      if (valor < ZeroRampa) return ZeroRampa;
      if (valor > RampaMax) return RampaMax;
      return valor;
    }

    void AtualizarLeitura() {
      if (!Iniciado) return;

      uint32_t duty = ledcRead(_sentido == Sentido::Horario ? _canalIN1 : _canalIN2); 
      PercentualPotenciaLeitura = (duty / (double)RampaMax) * 100.0;
      EstadoLeitura = (duty > 5) ? Estado::Ligado : Estado::Desligado;
    }

    void ResetPID() {
      erroAnterior = 0;
      integralErro = 0;
      ultimoPID_ms = millis();
    }

    double CalcularPotenciaPID(double pressaoAtual) {
      unsigned long agora = millis();

      if (ultimoPID_ms == 0) {
        ultimoPID_ms = agora;
      }

      double dt = (agora - ultimoPID_ms) / 1000.0;
      ultimoPID_ms = agora;

      if (dt <= 0.001) {
        dt = 0.001;
      }

      double erro = _PressaoSP - pressaoAtual;

      // Zona morta: se está perto do SP, evita ficar oscilando demais
      if (abs(erro) <= DeadbandPressao) {
        erro = 0;
      }

      integralErro += erro * dt;
      integralErro = constrain(integralErro, IntegralMin, IntegralMax);

      double derivada = (erro - erroAnterior) / dt;
      erroAnterior = erro;

      double saida = (Kp * erro) + (Ki * integralErro) + (Kd * derivada);

      // Se passou da pressão desejada, deixa cortar a bomba
      if (pressaoAtual >= _PressaoSP) {
        saida = 0;
      }

      // Limite máximo vindo do comando
      double teto = _PotenciaSP > 0 ? _PotenciaSP : PotenciaMaxPID;
      teto = constrain(teto, PotenciaMinPID, PotenciaMaxPID);

      saida = constrain(saida, 0, teto);

      // Degrau mínimo: se o PID quer ligar, liga com potência útil
      if (saida > 0 && saida < PotenciaMinPID) {
        saida = PotenciaMinPID;
      }

      return saida;
    }

    void MostrarLog(String mensagem) {
      if (DebugMode) {
        PrintTela("[BMB " + _ID + "] " + mensagem);
      }
    }
};

#endif