169 lines
3.7 KiB
C++
169 lines
3.7 KiB
C++
#include "SerialService.h"
|
|
#include "Pinout.h"
|
|
#include "Utils.h"
|
|
#include "IComponenteCAN.h"
|
|
#include "HX711.h"
|
|
#include <SimpleKalmanFilter.h>
|
|
|
|
#ifndef SensorMassaModel_v2_h
|
|
#define SensorMassaModel_v2_h
|
|
|
|
class SensorMassa_v2 : public ComponenteCAN {
|
|
public:
|
|
SensorMassa_v2(
|
|
const String& id,
|
|
uint8_t idNum,
|
|
int pinoDT,
|
|
int pinoSCK,
|
|
TiposBarramentos barramento = CONTROLADOR,
|
|
float offsetMassa = 0.0,
|
|
long offsetHx = 1996283,
|
|
float escalaHx = 50857.316406
|
|
)
|
|
: _ID(id),
|
|
ID_Num(idNum),
|
|
_pinoDT(pinoDT, barramento, _INPUT, DIGITAL),
|
|
_pinoSCK(pinoSCK, barramento, _OUTPUT),
|
|
_Offset(offsetHx),
|
|
_Escala(escalaHx),
|
|
_OffsetMassa(offsetMassa),
|
|
filtro(0.05, 0.05, 0.25) {
|
|
}
|
|
|
|
String _ID;
|
|
uint8_t ID_Num = 0;
|
|
|
|
PinoModel _pinoDT;
|
|
PinoModel _pinoSCK;
|
|
|
|
long _Offset = 1996283;
|
|
float _Escala = 50857.316406f;
|
|
float _OffsetMassa = 0.0f;
|
|
|
|
HX711 balanca;
|
|
SimpleKalmanFilter filtro;
|
|
|
|
float Massa = 0.0f;
|
|
float leitura = 0.0f;
|
|
float leituraCorrigida = 0.0f;
|
|
|
|
bool Iniciado = false;
|
|
|
|
void Inicializar() {
|
|
if (Iniciado) {
|
|
MostrarLog("Sensor ja inicializado");
|
|
return;
|
|
}
|
|
|
|
_pinoDT.Conectar();
|
|
_pinoSCK.Conectar(false);
|
|
|
|
if (_pinoDT.Definido() && _pinoSCK.Definido()) {
|
|
balanca.begin(_pinoDT.num, _pinoSCK.num);
|
|
balanca.set_offset(_Offset);
|
|
balanca.set_scale(_Escala);
|
|
|
|
leitura = 0.0f;
|
|
leituraCorrigida = 0.0f;
|
|
Massa = 0.0f;
|
|
|
|
Iniciado = true;
|
|
MostrarLog("Sensor iniciado");
|
|
} else {
|
|
Iniciado = false;
|
|
MostrarLog("Sensor nao iniciado");
|
|
}
|
|
}
|
|
|
|
void Desligar() {
|
|
if (!Iniciado) {
|
|
MostrarLog("Sensor nao esta inicializado");
|
|
return;
|
|
}
|
|
|
|
_pinoDT.Desconectar();
|
|
_pinoSCK.Desconectar();
|
|
|
|
leitura = 0.0f;
|
|
leituraCorrigida = 0.0f;
|
|
Massa = 0.0f;
|
|
Iniciado = false;
|
|
|
|
MostrarLog("Sensor desligado");
|
|
}
|
|
|
|
void RequisitarDados() {
|
|
AtualizarLeitura();
|
|
|
|
MostrarLog("Leitura: " + String(leitura) + ", Massa: " + String(Massa));
|
|
}
|
|
|
|
void AtualizarLeitura() {
|
|
if (!Iniciado) return;
|
|
|
|
if (!balanca.is_ready()) {
|
|
MostrarLog("HX711 nao pronto");
|
|
return;
|
|
}
|
|
|
|
leitura = balanca.get_units(5);
|
|
|
|
float valor = -leitura;
|
|
valor += _OffsetMassa;
|
|
leituraCorrigida = valor;
|
|
|
|
Massa = filtro.updateEstimate(leituraCorrigida);
|
|
|
|
if (Massa < 0.0f) Massa = 0.0f;
|
|
}
|
|
|
|
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: {
|
|
uint16_t leituraEscalada = EscalarFloat2Bytes(leituraCorrigida);
|
|
uint16_t massaEscalada = EscalarFloat2Bytes(Massa);
|
|
|
|
data.push_back((uint8_t)(leituraEscalada >> 8));
|
|
data.push_back((uint8_t)(leituraEscalada & 0xFF));
|
|
data.push_back((uint8_t)(massaEscalada >> 8));
|
|
data.push_back((uint8_t)(massaEscalada & 0xFF));
|
|
break;
|
|
}
|
|
|
|
default:
|
|
break;
|
|
}
|
|
|
|
return data;
|
|
}
|
|
|
|
private:
|
|
bool DebugMode = false;
|
|
|
|
uint16_t EscalarFloat2Bytes(float valor) {
|
|
if (valor < 0.0f) valor = 0.0f;
|
|
|
|
float escalado = valor * 100.0f;
|
|
if (escalado > 65535.0f) escalado = 65535.0f;
|
|
|
|
return (uint16_t)(escalado);
|
|
}
|
|
|
|
void MostrarLog(String mensagem) {
|
|
if (DebugMode) {
|
|
PrintTela("[MAS " + _ID + "] " + mensagem);
|
|
}
|
|
}
|
|
};
|
|
|
|
#endif
|