agrobot_base/Firmware/Modulos/ServoFreioModel.h

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

#ifndef ServoFreioModel
#define ServoFreioModel

class ServoFreio {
  public:

    static std::vector<uint8_t> ConfigurarServo(std::vector<ServoFreio*>& lista, std::vector<uint8_t>& data, const String& Mod_ID);
    static void ComandarServo(std::vector<ServoFreio*>& lista, std::vector<uint8_t>& data);

    ServoFreio(String _modID, String _id) {
      Mod_ID = _modID;
      _ID = _id;
    }

    String Mod_ID;
    String _ID;
    int ID_Num;
    PinoModel _pino;

    bool Incremental = true;

    bool Iniciado = false;

    int _Angulo = 0;

    Servo _Servo;

    void Inicializar() {
      if (Iniciado) {
        PrintTela(_ID + " ja inicializado");
        return;
      }
      
      if (_pino.Definido()) {
        _Servo.attach(_pino.num);
        AtualizarAngulo(Incremental ? 0 : 180);

        Iniciado = true;

        PrintTela(_ID + " iniciado");
      }
      else {
        PrintTela(_ID + " nao iniciado");
      }
    }

    void Desligar() {
      if (!Iniciado) {
        PrintTela(_ID + " nao esta inicializado");
        return;
      }

      // Redefinir as configurações para os valores iniciais
      if (_pino.Definido()) {
        _Servo.detach();
      }

      PrintTela(_ID + " Desligado");

      Iniciado = false;
    }

    void AtualizarAngulo(int novoAngulo) {
      if (Iniciado) {
        PrintTela("Angulo de freio atualizado de " + String(_Angulo) + " para " + String(novoAngulo));

        _Angulo = novoAngulo;
      
        _Servo.write(_Angulo);
      }
    }

    void RequisitarDados() {
      
    }

    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);
          break;
        }
        case CanMessagePosicaoDados::Dados1: {
          data.push_back((uint8_t)(_Angulo & 0xFF)); // 0 a 180
          break;
        }
      }
      return data;
    }

};


std::vector<uint8_t> ServoFreio::ConfigurarServo(std::vector<ServoFreio*>& lista, std::vector<uint8_t>& data, const String& Mod_ID) {
  std::vector<uint8_t> status;
  if (data.size() < 2) return status;
  CanMessagePosicaoDados posicao = (CanMessagePosicaoDados)data[0];
  uint8_t idNum = data[1];
  auto it = std::find_if(lista.begin(), lista.end(), [idNum](ServoFreio* s) { return s->ID_Num == idNum; });
  bool jaExiste = it != lista.end();
  ServoFreio* sensor;
  switch (posicao) {
    case CanMessagePosicaoDados::Config1: {
      if (data.size() < 7) return status;
      bool conectar = data[3] == 1;
      bool incremental = data[4] == 1;
      TiposBarramentos tipoBarramento = (TiposBarramentos)data[5];
      int pino = data[6];
      if (conectar) {
        if (!jaExiste) {
          sensor = new ServoFreio(Mod_ID, "sFRO_" + String(idNum));
        }
        else {
          sensor = *it;
        }
        if (!sensor->Iniciado) {
          sensor->ID_Num = idNum;
          sensor->Incremental = incremental;
          sensor->_pino = PinoModel(pino, tipoBarramento, _OUTPUT);
          sensor->Inicializar();
          if (!jaExiste) {
            lista.push_back(sensor);
            PrintTela("Servo Freio adicionado via CAN: sFRO_" + String(idNum));
          }
        }
        status = sensor->MontarMensagemCAN(CanMessagePosicaoDados::Status);
      } else {
        if (jaExiste) {
          sensor = *it;
          sensor->Desligar();
          status = sensor->MontarMensagemCAN(CanMessagePosicaoDados::Status);
          delete sensor;
          lista.erase(it);
          PrintTela("Servo Freio removido via CAN: sFRO_" + String(idNum));
        }
      }
      break;
    }
  }
  return status;
}

void ServoFreio::ComandarServo(std::vector<ServoFreio*>& lista, std::vector<uint8_t>& data) {
  if (data.size() < 3) return;
  CanMessagePosicaoDados posicao = (CanMessagePosicaoDados)data[0];
  uint8_t idNum = data[1];
  for (auto* _freio : lista) {
    if (_freio->ID_Num == idNum) {
      switch (posicao) {
        case CanMessagePosicaoDados::Command1: {
          int novoAngulo = (int)data[2];
          _freio->AtualizarAngulo(novoAngulo);
          break;
        }
      }
      return;
    }
  }
}

#endif