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 pinoIN1,
      int canalIN1,
      int pinoIN2,
      int canalIN2,
      TiposBarramentos barramento = CONTROLADOR,
      Sentido sentido = Sentido::Horario,
      SensorPressao_v2* sensor = nullptr,
      int pressaoInicial = 0
    )
    : _ID(id),
      ID_Num(idNum),
      _pinoIN1(pinoIN1, barramento, _PWM),
      _pinoIN2(pinoIN2, barramento, _PWM),
      _canalIN1(canalIN1),
      _canalIN2(canalIN2),
      _sentido(sentido),
      sensorPressao(sensor),
      _PressaoSP(pressaoInicial) {
    }

    String _ID;
    uint8_t ID_Num = 0;

    PinoModel _pinoIN1;
    PinoModel _pinoIN2;
    
    int _canalIN1 = 1;
    int _canalIN2 = 2;

    Sentido _sentido = Sentido::Horario;

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

      _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;
        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;
      ledcWrite(_canalIN1, 0);
      ledcWrite(_canalIN2, 0);

      _pinoIN1.Desconectar();
      _pinoIN2.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::Ligado, 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;

      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;

      EstadoLeitura = (PotenciaLeitura > 0 && _EstadoControle == Estado::Ligado) ? Estado::Ligado : Estado::Desligado;
      PercentualPotenciaLeitura = (RampaMax > 0)
        ? (PotenciaLeitura / (double)RampaMax) * 100.0
        : 0.0;
    }

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

#endif