agrobot_base/Firmware/Modulos/SensorPotenciometroModel.h

#include "SerialService.h"
#include "ReleModel.h"
#include "Pinout.h"
#include "Utils.h"
#include <SimpleKalmanFilter.h>

#ifndef SensorPotenciometroModel
#define SensorPotenciometroModel

class SensorPotenciometro {
  public:

    static void InicializarLista(std::vector<SensorPotenciometro*>& lista, const String& parametros, const String& Mod_ID, std::vector<Rele*>& listaReles);

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

    // Definições
    String Mod_ID = "";
    String _ID;
    int ID_Num;
    PinoModel _pino;

    Rele* releControle;

    double _anguloPotenciometro = 270;
    double _limitePotenciometro = _anguloPotenciometro / 2;
    double _anguloMin = -110;
    double _anguloMeio = 0;
    double _anguloMax = 110;
    double _margem = 10;

    double leitura = 0;
    double angulo = 0;
    Sentido sentido = Parado;
    bool limiteExcedido = false;

    unsigned long millisLimite = -1;
    int tempoReiniciarRele = 5000;
    double anguloAtuacaoLimite = 0;

    SimpleKalmanFilter filtro = SimpleKalmanFilter(5, 15, 0.01);

    bool Iniciado = false;
    bool Checando = false;

    void Inicializar() {
      if (Iniciado) {
        PrintTela(_ID + " ja inicializado");
        return;
      }

      leitura = 0;

      _pino.Conectar(); // Conecta o pino antes de criar a tarefa

      Iniciado = true;

      PrintTela(_ID + " iniciado");
    }

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

      _pino.Desconectar(); // Desconecta o pino

      PrintTela(_ID + " Desligado");

      Iniciado = false;
    }

    void AferirAngulo() {
      leitura = _pino.get(5);
      double leituraNormalizada = filtro.updateEstimate(leitura);

      angulo = map(leituraNormalizada, 0, 4095, -_limitePotenciometro, _limitePotenciometro);

      sentido = 
        (angulo <= (_anguloMeio - _margem)) ? Antihorario :
        (angulo >= (_anguloMeio + _margem)) ? Horario : 
        Parado;

      limiteExcedido = angulo <= (_anguloMin - _margem) || angulo >= (_anguloMax + _margem);
    }

    void RequisitarDados() {
      AferirAngulo();
    }

    void Loop() {
      if (releControle != nullptr) {
        AferirAngulo();

        releControle->AtualizarLeitura();

        bool tempoResetAtingido = millis() > (millisLimite + tempoReiniciarRele);

        // Determina se o ângulo continua avançando no mesmo sentido do limite
        bool avancandoMesmoSentido = 
          tempoResetAtingido && 
          ((sentido == Antihorario && angulo < (anguloAtuacaoLimite - (_margem * 1.5))) || 
          (sentido == Horario && angulo > (anguloAtuacaoLimite + (_margem * 1.5))));

        // Reinicia limite caso:
        // - O limite não está mais excedido
        // - OU o motor continua no mesmo sentido após reset
        bool reiniciarLimite = !limiteExcedido || avancandoMesmoSentido;

        // Angulo limite excedido mas ainda nao enviou o comando para ligar o rele
        if (limiteExcedido && releControle->_Status == Desligado && millisLimite == -1) {
          millisLimite = millis();
          anguloAtuacaoLimite = angulo;
          releControle->AtualizarStatus(Ligado);
        }
        // Angulo limite excedido, o rele esta ligado, ja passou o tempo para reiniciar o rele
        else if (releControle->_Status == Ligado && tempoResetAtingido) {
          releControle->AtualizarStatus(Desligado);
        }
        // Reiniciar millis quando o angulo voltar para a margem segura ou se continuar a virar no sentido do limite
        else if (avancandoMesmoSentido) {
          millisLimite = -1;
        }
      }
    }
    
    std::vector<uint8_t> MontarMensagemCAN() {
      std::vector<uint8_t> data;
      data.push_back(ID_Num);
      data.push_back(Iniciado ? 1 : 0);
  
      uint16_t leituraInt = static_cast<uint16_t>(leitura);
      data.push_back((uint8_t)(leituraInt >> 8));
      data.push_back((uint8_t)(leituraInt & 0xFF));
  
      uint8_t anguloByte = static_cast<uint8_t>(angulo);
      data.push_back(anguloByte);
      data.push_back(sentido); // já é uint8_t pelo enum?
      data.push_back(limiteExcedido ? 1 : 0);
  
      return data;
    }
  

};

// Função para inicializar os sensores de potenciometros com IDs e pinos específicos
void SensorPotenciometro::InicializarLista(std::vector<SensorPotenciometro*>& lista, const String& parametros, const String& Mod_ID, std::vector<Rele*>& listaReles) {
  // Verifica se há sensores já na lista
  if (!lista.empty()) {
    PrintTela("Limpando lista de sensores de potenciometro existente...");

    for (auto* sensor : lista) {
      sensor->Desligar();  // Desliga cada sensor antes de remover
      PrintTela("Sensor de Potenciometro " + sensor->_ID + " desligado.");
      delete sensor;
    }

    lista.clear();  // Limpa a lista após desligar todos os itens
  }

  if (parametros.length() > 0) {
    std::vector<String> _config = SplitString(parametros, SplitConfig);  // Divide a configuração por vírgulas

    for (const String& config : _config) {
      std::vector<String> partes = SplitString(config, SplitSubParams);  // Divide cada configuração pelo '-'

      if (partes.size() >= 4) {
        String id = partes[0];
        bool conectar = partes[1] == "1";
        TiposBarramentos pinoB = (TiposBarramentos)partes[2].toInt();
        int pino = partes[3].toInt();

        if (conectar) {
          SensorPotenciometro* sensor = new SensorPotenciometro(Mod_ID, id);
          sensor->ID_Num = lista.size();
          sensor->_pino = PinoModel(pino, pinoB, _INPUT, ANALOGICO);
          sensor->releControle = nullptr;

          if (partes.size() >= 8) {
            double angMin = partes[4].toDouble();
            double angMeio = partes[5].toDouble();
            double angMax = partes[6].toDouble();
            double angMargem = partes[7].toDouble();

            sensor->_anguloMin = angMin;
            sensor->_anguloMeio = angMeio;
            sensor->_anguloMax = angMax;
            sensor->_margem = angMargem;
          }

          // Atribui um rele de controle
          for (auto* _rele : listaReles) {
            String idRele = _rele->_ID;
            if (idRele.length() > 2 && idRele.startsWith("RL") && idRele.substring(2) == id) {
              sensor->releControle = _rele;
              break;
            }
          }

          sensor->Inicializar();
          lista.push_back(sensor);
          PrintTela("Sensor de potenciometro " + id + " inicializado no pino " + String(pino) + (sensor->releControle != nullptr ? (" e rele " + sensor->releControle->_ID) : ""));
        }
      } else {
        PrintTela("Configuração inválida para sensor de potenciometro: " + config);
      }
    }
  } else {
    PrintTela("Configuração de sensor de potenciometro não encontrada.");
  }
}

#endif