agrobot_base/Firmware/Modulos/SensorCorrenteINA228Model.h

#ifndef SensorCorrenteINA228Model
#define SensorCorrenteINA228Model

#include "SerialService.h"
#include "Pinout.h"
#include "Utils.h"
#include <Wire.h>
#include <vector>

class SensorCorrenteINA228 {
  public:
    SensorCorrenteINA228(String _modID, String _id) {
      Mod_ID = _modID;
      _ID = _id;
    }

    String Mod_ID = "";
    String _ID;
    int ID_Num;
    byte _Endereco;
    float _A_Shunt = 10.0;

    bool Iniciado = false;
    float busVoltage = 0;
    float current = 0;
    float power = 0;
    float energy = 0;
    float temperature = 0;

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

      Wire.beginTransmission(_Endereco);
      byte error = Wire.endTransmission();
      if (error == 0) {
        Iniciado = true;
        EscreverRegistradorINA(0x00, 0x8000);  // Bit 15 = 1 → RESET
        delay(2);
        if (_A_Shunt == 10.0) {
          currentLSB = 0.0001;     // Mantém como já aprovado nos testes
        } else if (_A_Shunt == 50.0) {
          currentLSB = 0.00005;    // 50 / 0.00005 = 1.000.000 → perto do limite
        } else if (_A_Shunt == 100.0) {
          currentLSB = 0.0001;     // 100 / 0.0001 = 1.000.000 → ótimo
        }
      } else {
        Iniciado = false;
        PrintTela(_ID + " nao iniciado, endereco " + _Endereco + " nao encontrado!");
      }
    }

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

    void RequisitarDados() {
      if (!Iniciado) return;
      AferirCorrente();
    }

    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 volts = busVoltage * 100;
          uint16_t amps = current * 100;
          uint16_t watts = power * 10;
          data.push_back(volts >> 8); data.push_back(volts & 0xFF);
          data.push_back(amps >> 8); data.push_back(amps & 0xFF);
          data.push_back(watts >> 8); data.push_back(watts & 0xFF);
          break;
        }
        case CanMessagePosicaoDados::Dados2: {
          uint16_t wh = energy * 10;
          uint16_t temp = temperature * 100;
          data.push_back(wh >> 8); data.push_back(wh & 0xFF);
          data.push_back(temp >> 8); data.push_back(temp & 0xFF);
          break;
        }
      }
      return data;
    }

    static std::vector<uint8_t> ConfigurarSensor(std::vector<SensorCorrenteINA228*>& 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];
      auto it = std::find_if(lista.begin(), lista.end(), [idNum](SensorCorrenteINA228* s) { return s->ID_Num == idNum; });
      bool jaExiste = it != lista.end();
      SensorCorrenteINA228* sensor;

      switch (posicao) {
        case CanMessagePosicaoDados::Config1: {
          if (data.size() < 8) return status;
          bool conectar = data[4] == 1;
          uint8_t endereco = data[5];
          uint16_t aMaxInt = ((uint16_t)data[6] << 8) | data[7];
          float aMax = aMaxInt / 100.0;

          if (conectar) {
            if (!jaExiste) {
              sensor = new SensorCorrenteINA228(Mod_ID, "sCOR_" + String(idNum));
            } else {
              sensor = *it;
            }
            if (!sensor->Iniciado) {
              sensor->ID_Num = idNum;
              sensor->_Endereco = endereco;
              sensor->_A_Shunt = aMax;
              sensor->Inicializar();
              if (!jaExiste) {
                lista.push_back(sensor);
                PrintTela("Sensor INA228 adicionado via CAN: sCOR_" + 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("Sensor INA228 removido via CAN: sCOR_" + String(idNum));
            }
          }
          break;
        }
      }
      return status;
    }

  private:
    float shuntLSB = 1.0e-6;
    float currentLSB = 0.000003;
    float VbusLSB = 1.23e-5;

    uint32_t LerRegistradorINA(uint8_t reg, uint8_t bytes) {
      Wire.beginTransmission(_Endereco);
      Wire.write(reg);
      Wire.endTransmission(false);
      Wire.requestFrom(_Endereco, bytes);

      uint32_t valor = 0;
      for (uint8_t i = 0; i < bytes; i++) {
        valor = (valor << 8) | Wire.read();
      }
      return valor;
    }

    void EscreverRegistradorINA(uint8_t reg, uint16_t valor) {
      Wire.beginTransmission(_Endereco);
      Wire.write(reg);
      Wire.write((valor >> 8) & 0xFF);  // MSB
      Wire.write(valor & 0xFF);         // LSB
      Wire.endTransmission();
    }

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

      uint32_t rawBus = LerRegistradorINA(0x05, 3);
      uint32_t rawCurrent = LerRegistradorINA(0x07, 3);
      uint32_t rawPower = LerRegistradorINA(0x08, 3);
      uint64_t rawEnergy = ((uint64_t)LerRegistradorINA(0x0A, 3) << 16) | LerRegistradorINA(0x0B, 2);
      uint16_t rawTemp = LerRegistradorINA(0x0F, 2);

      //busVoltage = rawBus * 3.125e-6;
      busVoltage = rawBus * VbusLSB;
      power = rawPower * currentLSB * 3.125;
      current = rawCurrent * currentLSB;
      energy = rawEnergy * currentLSB * 3.125 * 16 / 3600.0;
      temperature = ((int16_t)rawTemp) * 0.0078125;
    }
};

#endif