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

class BombaPressurizadora_v2 : public ComponenteCAN {
  public:
    BombaPressurizadora_v2(
      const String& id,
      uint8_t idNum,
      int pinoEN,
      int pinoCS,
      int pinoINA,
      int pinoINB,
      int pinoPWM,
      TiposBarramentos barramento = CONTROLADOR,
      int canalPwm = 0,
      SensorPressao_v2* sensor = nullptr,
      int pressaoInicial = 0
    )
    : _ID(id),
      ID_Num(idNum),
      _pinoEN(pinoEN, barramento, _OUTPUT),
      _pinoCS(pinoCS, barramento, _INPUT),
      _pinoINA(pinoINA, barramento, _OUTPUT),
      _pinoINB(pinoINB, barramento, _OUTPUT),
      _pinoPWM(pinoPWM, barramento, _PWM),
      _canal(canalPwm),
      sensorPressao(sensor),
      _PressaoSP(pressaoInicial) {
    }

    String _ID;
    uint8_t ID_Num = 0;

    PinoModel _pinoEN;
    PinoModel _pinoCS;
    PinoModel _pinoINA;
    PinoModel _pinoINB;
    PinoModel _pinoPWM;

    int _canal = 0;

    float _Kp = 3.0;
    float _Ki = 2.2;
    float _Kd = 0.5;
    int histerese = 3;

    int ZeroRampa = 0;
    int RampaMax = 1023;

    bool Iniciado = false;
    bool MalhaFechada = false;
    bool bombaDesligadaPorPressao = false;

    SensorPressao_v2* sensorPressao = nullptr;

    Estado EstadoLeitura = Estado::Desligado;
    double PotenciaLeitura = 0;
    double PercentualPotenciaLeitura = 0;

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

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

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

      MalhaFechada = (sensorPressao != nullptr);

      _pinoEN.Conectar(false);
      _pinoCS.Conectar();
      _pinoINA.Conectar(false);
      _pinoINB.Conectar(true);
      _pinoPWM.Conectar(0);

      if (_pinoPWM.Definido()) {
        ledcSetup(_canal, 1000, 10);
        ledcAttachPin(_pinoPWM.num, _canal);
        ledcWrite(_canal, 0);
      }

      Iniciado =
        _pinoEN.Definido() &&
        _pinoINA.Definido() &&
        _pinoINB.Definido() &&
        _pinoPWM.Definido();

      if (Iniciado) {
        _EstadoControle = Estado::Desligado;
        PotenciaLeitura = 0;
        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;
      AplicarSaida(false, 0);

      _pinoPWM.Desconectar();
      _pinoEN.Desconectar();
      _pinoCS.Desconectar();
      _pinoINA.Desconectar();
      _pinoINB.Desconectar();

      PotenciaLeitura = 0;
      PercentualPotenciaLeitura = 0;
      EstadoLeitura = Estado::Desligado;
      bombaDesligadaPorPressao = false;
      Iniciado = false;

      MostrarLog("Bomba desligada");
    }

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

      _EstadoControle = novoEstado;

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

      if (_EstadoControle == Estado::Desligado) {
        bombaDesligadaPorPressao = false;
        AplicarSaida(false, 0);
      } else {
        AplicarSaida(true, PotenciaLeitura);
      }

      AtualizarLeitura();
    }

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

      double pressaoAtual = 0;

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

        if (pressaoAtual >= (_PressaoSP + histerese)) {
          bombaDesligadaPorPressao = true;
        } else if (pressaoAtual <= (_PressaoSP - histerese)) {
          bombaDesligadaPorPressao = false;
        }
      }

      if (_EstadoControle == Estado::Desligado) {
        PotenciaLeitura = ZeroRampa;
      } else {
        if (MalhaFechada) {
          PotenciaLeitura = bombaDesligadaPorPressao ? ZeroRampa : RampaMax;
        } else {
          PotenciaLeitura = RampaMax;
        }
      }

      AplicarSaida(_EstadoControle == Estado::Desligado, PotenciaLeitura);
      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 pressao = (data.size() > 3) ? (double)data[3] : _PressaoSP;
          AtualizarControle(novoEstado, pressao);
          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)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));
          break;
        }
      }

      return data;
    }

  private:
    bool DebugMode = false;

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

      _pinoEN.set(ligado);

      if (ligado) {
        _pinoINA.set(false);
        _pinoINB.set(true);
      } else {
        _pinoINA.set(false);
        _pinoINB.set(false);
        pwm = 0;
      }

      pwm = LimitarPWM(pwm);
      ledcWrite(_canal, pwm);
    }

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

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

      EstadoLeitura = _pinoEN.get() ? Ligado : Desligado;
      PercentualPotenciaLeitura = (RampaMax > 0)
        ? (PotenciaLeitura / (double)RampaMax) * 100.0
        : 0.0;
    }

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

#endif