agrobot_base/Firmware/Modulos/SensorMassaModel.h

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

#ifndef SensorMassaModel
#define SensorMassaModel

class SensorMassa {
  public:

    static std::vector<uint8_t> ConfigurarSensor(std::vector<SensorMassa*>& lista, std::vector<uint8_t>& data, const String& Mod_ID);

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

    String Mod_ID;
    String _ID;
    int ID_Num;
    PinoModel _pinoDT;
    PinoModel _pinoSCK;

    float _Offset = 1895800.0;
    float _MassaRef = 0.8;
    float _MassaLeituraRef = 40876.37;

    HX711 balanca = HX711();
    SimpleKalmanFilter filtro = SimpleKalmanFilter(2, 2, 0.01);

    double leituraMassa = 0;
    float Massa = 0;

    bool Iniciado = false;

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

      if (_pinoDT.Definido() && _pinoSCK.Definido()) {
        balanca.begin(_pinoDT.num, _pinoSCK.num);
        balanca.set_scale();
        //balanca->tare();

        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
      _pinoDT.Desconectar();
      _pinoSCK.Desconectar();

      PrintTela(_ID + " Desligado");

      Iniciado = false;
    }

    void RequisitarDados() {
      AferirMassa();
    }

    std::vector<uint8_t> MontarMensagemCAN(CanMessagePosicaoDados posicao) {
      std::vector<uint8_t> data;
      data.push_back(ID_Num);
      data.push_back(static_cast<uint8_t>(posicao));
      switch (posicao) {
        case CanMessagePosicaoDados::Status: {
          data.push_back(Iniciado ? 1 : 0);
          break;
        }
        case CanMessagePosicaoDados::Dados1: {
          uint16_t massaEscalada = (uint16_t)(Massa * 100);
          data.push_back(massaEscalada >> 8);
          data.push_back(massaEscalada & 0xFF);
          break;
        }
      }
      return data;
    }



  private:
    void AferirMassa() {
      if (Iniciado) {
        float leitura = balanca.get_units(5);
        leituraMassa = filtro.updateEstimate(leitura);
        Massa = CalcularMassa(leituraMassa);
      }
    }

    float CalcularMassa(float _leitura) {
      double leituraMassa = _leitura * -1;
      leituraMassa += _Offset;

      float FatorLeitura = _MassaRef / _MassaLeituraRef;
      float massa = FatorLeitura * leituraMassa;
      return massa;
    }


};


std::vector<uint8_t> SensorMassa::ConfigurarSensor(std::vector<SensorMassa*>& lista, std::vector<uint8_t>& data, const String& Mod_ID) {
  std::vector<uint8_t> status;
  if (data.size() < 3) return status;
  uint8_t idNum = data[1];
  CanMessagePosicaoDados posicao = (CanMessagePosicaoDados)data[2];
  switch (posicao) {
    case CanMessagePosicaoDados::Config1: {
      if (data.size() < 7) return status;
      bool conectar = data[4] == 1;
      int pinoDt = data[5];
      int pinoSck = data[6];
      //uint16_t offsetInt = ((uint16_t)data[6] << 8) | data[7];
      //float offset = offsetInt / 100.0;
      auto it = std::find_if(lista.begin(), lista.end(), [idNum](SensorMassa* s) { return s->ID_Num == idNum; });
      if (conectar) {
        if (it == lista.end()) {
          SensorMassa* sensor = new SensorMassa(Mod_ID, "sMAS_" + String(idNum));
          sensor->ID_Num = idNum;
          sensor->_pinoDT = PinoModel(pinoDt, CONTROLADOR, _INPUT, DIGITAL);
          sensor->_pinoSCK = PinoModel(pinoSck, CONTROLADOR, _OUTPUT);
          //sensor->_Offset = offset;
          sensor->Inicializar();
          lista.push_back(sensor);
          PrintTela("Sensor de Massa adicionado via CAN: sMAS_" + String(idNum));
          status = sensor->MontarMensagemCAN(CanMessagePosicaoDados::Status);
        }
      } else {
        if (it != lista.end()) {
          SensorMassa* sensor = *it;
          sensor->Desligar();
          status = sensor->MontarMensagemCAN(CanMessagePosicaoDados::Status);
          delete sensor;
          lista.erase(it);
          PrintTela("Sensor de Massa removido via CAN: sMAS_" + String(idNum));
        }
      }
      break;
    }
  }
  return status;
}

#endif