agrobot_base/Firmware/Modulos/BicoPulverizadorModel_v2.h

#include <ESP32Servo.h>
#include "SerialService.h"
#include "Pinout.h"
#include "Utils.h"
#include "IComponenteCAN.h"

#ifndef BicoPulverizadorModel_v2_h
#define BicoPulverizadorModel_v2_h

struct EstadoAnteriorBico {
  Estado estado = Estado::Desligado;
  int angulo = 0;
};

class BicoPulverizador_v2 : public ComponenteCAN {
  public:
    BicoPulverizador_v2(
      const String& id,
      uint8_t idNum,
      int pinoSaida,
      bool atuacaoNivelAlto,
      TiposBarramentos barramentoSaida,
      int pinoServo = -1,
      int anguloMin = 0,
      int anguloMax = 180,
      int anguloInicial = 90
    )
    : _ID(id),
      ID_Num(idNum),
      _pinoSaida(pinoSaida, barramentoSaida, _OUTPUT),
      _pinoServo(pinoServo),
      AtuacaoNivelAlto(atuacaoNivelAlto),
      _anguloMin(anguloMin),
      _anguloMax(anguloMax),
      _anguloInicial(anguloInicial) {
    }

    String _ID;
    uint8_t ID_Num = 0;

    bool Iniciado = false;
    bool ServoHabilitado = false;
    bool AtuacaoNivelAlto = true;

    Estado _Status = Estado::Desligado;
    int _AnguloAtual = 0;

    void Inicializar() {
      if (Iniciado) {
        MostrarLog("Bico ja inicializado");
        return;
      }

      _pinoSaida.Conectar(!AtuacaoNivelAlto);
      bool saidaOk = _pinoSaida.Definido();

      ServoHabilitado = false;

      if (_pinoServo >= 0) {
        try {
          _servo.attach(_pinoServo);
          _servo.write(_anguloInicial);
          _AnguloAtual = _anguloInicial;
          ServoHabilitado = true;
          MostrarLog("Servo iniciado no pino " + String(_pinoServo));
        } catch (...) {
          ServoHabilitado = false;
          _AnguloAtual = 0;
          MostrarLog("Falha ao iniciar servo");
        }
      } else {
        _AnguloAtual = 0;
        MostrarLog("Bico sem servo");
      }

      if (saidaOk) {
        DesativarSaidaFisica();
        _Status = Desligado;
        Iniciado = true;
        MostrarLog("Bico iniciado com sucesso");
      } else {
        Iniciado = false;
        MostrarLog("Falha ao iniciar bico");
      }
    }

    void Desligar() {
      if (!Iniciado) {
        MostrarLog("Bico nao esta inicializado");
        return;
      }

      DesativarSaidaFisica();

      if (ServoHabilitado) {
        _servo.detach();
        ServoHabilitado = false;
      }

      _pinoSaida.Desconectar();

      Iniciado = false;
      _Status = Desligado;
      _AnguloAtual = 0;

      MostrarLog("Bico desligado");
    }

    void AtualizarStatus(Estado novoEstado, int novoAngulo = -1) {
      if (!Iniciado) {
        MostrarLog("Ignorando comando: bico nao iniciado");
        return;
      }

      bool saidaFisica = (novoEstado == Ligado);
      if (!AtuacaoNivelAlto) {
        saidaFisica = !saidaFisica;
      }

      _pinoSaida.set(saidaFisica);
      _Status = novoEstado;

      if (ServoHabilitado && novoAngulo >= 0) {
        novoAngulo = LimitarAngulo(novoAngulo);
        _servo.write(novoAngulo);
        _AnguloAtual = novoAngulo;
      }

      MostrarLog("Status atualizado | ligado=" + String((int)_Status) + " ang=" + String(_AnguloAtual));
    }

    void Ligar(int angulo = -1) {
      AtualizarStatus(Ligado, angulo);
    }

    void DesligarBico(int angulo = -1) {
      AtualizarStatus(Desligado, angulo);
    }

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

      bool estadoFisico = _pinoSaida.get();
      bool estadoLogico = AtuacaoNivelAlto ? estadoFisico : !estadoFisico;
      _Status = estadoLogico ? Ligado : Desligado;
    }

    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];
          int novoAngulo = (data.size() > 3) ? (int)data[3] : -1;
          AtualizarStatus(novoEstado, novoAngulo);
          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(ServoHabilitado ? 1 : 0);
          break;
        }

        case CanMessagePosicaoDados::Dados1: {
          AtualizarLeitura();
          data.push_back(static_cast<uint8_t>(_Status));
          data.push_back(static_cast<uint8_t>(ServoHabilitado ? _AnguloAtual : 0));
          break;
        }

        default:
          break;
      }

      return data;
    }

  private:
    bool DebugMode = false;

    PinoModel _pinoSaida;
    Servo _servo;

    int _pinoServo = -1;
    int _anguloMin = 0;
    int _anguloMax = 180;
    int _anguloInicial = 90;

    int LimitarAngulo(int valor) {
      if (valor < _anguloMin) return _anguloMin;
      if (valor > _anguloMax) return _anguloMax;
      return valor;
    }

    void DesativarSaidaFisica() {
      bool saidaFisica = AtuacaoNivelAlto ? false : true;
      _pinoSaida.set(saidaFisica);
    }

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

#endif