Criado firmware estatica do sensoriamento
This commit is contained in:
Diego Freitas
parent
fc89b3a90e
commit
f77d5daf3e
|
|
@ -174,14 +174,7 @@ namespace AgroBase.Models.Modules
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
public SensorModel SensorTemperaturaMotor
|
public SensorModel SensorTemperaturaMotor => Variaveis.OperacaoEmAndamento.DispSen?.Dados?.Sensores?.FirstOrDefault(x => x.Inicializado && x.Componente == S_Code.sNTC && x.ID.Contains(Mod_ID));
|
||||||
{
|
|
||||||
get
|
|
||||||
{
|
|
||||||
var sNTC = Variaveis.OperacaoEmAndamento.DispSen?.Dados?.Sensores?.FirstOrDefault(x => x.Componente == S_Code.sNTC && x.ID.Contains(Mod_ID));
|
|
||||||
return sNTC;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
public bool Comandar { get; set; } = false;
|
public bool Comandar { get; set; } = false;
|
||||||
public bool AlarmeSensores { get; set; } = false;
|
public bool AlarmeSensores { get; set; } = false;
|
||||||
|
|
@ -511,34 +504,10 @@ namespace AgroBase.Models.Modules
|
||||||
public DateTime UltimoEnvio { get; set; }
|
public DateTime UltimoEnvio { get; set; }
|
||||||
public int DelayEnvio { get; set; } = 500;
|
public int DelayEnvio { get; set; } = 500;
|
||||||
|
|
||||||
public SensorServoModel Servo
|
public SensorServoModel Servo => Variaveis.OperacaoEmAndamento.DispSen?.Dados?.Servos?.FirstOrDefault(x => x.Inicializado && x.ID.Contains(Mod_ID));
|
||||||
{
|
private double VelocidadeAtual => FuncoesMatematicas.CalculaVelocidadePercentualMs(Variaveis.OperacaoEmAndamento.Sensoriamento?.Movimentacao?.VelocidadeMedia ?? 0);
|
||||||
get
|
private bool StatusControleEmFreio => Variaveis.OperacaoEmAndamento.DispMvd?.Dados?.Modulos?.FirstOrDefault(x => x.Modulo_ID == Mod_ID)?.MovMotor?.Leitura?.Freado ?? false;
|
||||||
{
|
private int? AnguloLido => (int?)Servo?.ValoresLeituras?.FirstOrDefault(x => x.funcao == FuncoesPinout.ServoAnguloLeitura)?.atual?.valor;
|
||||||
return Variaveis.OperacaoEmAndamento.DispSen?.Dados?.Servos?
|
|
||||||
.FirstOrDefault(x => x.ID.Contains(Mod_ID));
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
private double VelocidadeAtual =>
|
|
||||||
FuncoesMatematicas.CalculaVelocidadePercentualMs(Variaveis.OperacaoEmAndamento.Sensoriamento?.Movimentacao?.VelocidadeMedia ?? 0);
|
|
||||||
|
|
||||||
private bool StatusControleEmFreio =>
|
|
||||||
(Variaveis.OperacaoEmAndamento.Controle?.TiposControle?
|
|
||||||
.FirstOrDefault(x => x.Tipo == T_Code.Mov)?.UltimaDirecao
|
|
||||||
?? Direcao.Parado) == Direcao.EmFreio;
|
|
||||||
|
|
||||||
// leitura atual do ângulo (ou null)
|
|
||||||
private int AnguloLido
|
|
||||||
{
|
|
||||||
get
|
|
||||||
{
|
|
||||||
var v = Servo?.ValoresLeituras?
|
|
||||||
.FirstOrDefault(x => x.funcao == FuncoesPinout.ServoAnguloLeitura)?
|
|
||||||
.atual?.valor ?? 0;
|
|
||||||
return (int)v;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
// histerese de velocidade
|
// histerese de velocidade
|
||||||
private bool VelocidadeOkHisterese(int sp, double atual)
|
private bool VelocidadeOkHisterese(int sp, double atual)
|
||||||
|
|
@ -637,7 +606,7 @@ namespace AgroBase.Models.Modules
|
||||||
// Regra: envia quando (a) houve transição de modo, (b) alvo mudou significativamente,
|
// Regra: envia quando (a) houve transição de modo, (b) alvo mudou significativamente,
|
||||||
// ou (c) não atingiu o alvo atual (com tolerância).
|
// ou (c) não atingiu o alvo atual (com tolerância).
|
||||||
bool alvoMudou = Math.Abs(AnguloControle - anguloAntigo) > 0;
|
bool alvoMudou = Math.Abs(AnguloControle - anguloAntigo) > 0;
|
||||||
bool atingiu = AtingiuSPComTol(AnguloLido, AnguloControle);
|
bool atingiu = AnguloLido != null && AtingiuSPComTol(AnguloLido, AnguloControle);
|
||||||
|
|
||||||
// histerese de velocidade apenas para DEFINIR o alvo (já considerado acima).
|
// histerese de velocidade apenas para DEFINIR o alvo (já considerado acima).
|
||||||
// Não use VelocidadeOk para forçar reenvio — isso causa “chatters” de comando.
|
// Não use VelocidadeOk para forçar reenvio — isso causa “chatters” de comando.
|
||||||
|
|
@ -655,9 +624,9 @@ namespace AgroBase.Models.Modules
|
||||||
|
|
||||||
private void EnviarComandoModulo()
|
private void EnviarComandoModulo()
|
||||||
{
|
{
|
||||||
if (EnviarComando && UltimoEnvio.AddMilliseconds(DelayEnvio) < DateTime.Now)
|
if (EnviarComando && UltimoEnvio.AddMilliseconds(DelayEnvio) < DateTime.Now && Servo != null)
|
||||||
{
|
{
|
||||||
GeneralJoystick.EnviarComandoSensoriamento(S_Code.sSRV, Servo.ID, AnguloControle);
|
GeneralJoystick.EnviarComandoSensoriamento(Servo.Componente, Servo.ID, AnguloControle, Forcar: true);
|
||||||
UltimoEnvio = DateTime.Now;
|
UltimoEnvio = DateTime.Now;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
|
||||||
|
|
@ -41,6 +41,39 @@ public:
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
template<typename T>
|
||||||
|
static void ProcessarComponenteCAN(
|
||||||
|
T& componente,
|
||||||
|
const std::vector<CanMessagePosicaoDados>& posicoes,
|
||||||
|
bool todosIDs,
|
||||||
|
int id_num,
|
||||||
|
bool enviarStatus,
|
||||||
|
bool todosDados,
|
||||||
|
CanMessagePosicaoDados posicao,
|
||||||
|
std::function<void(const std::vector<uint8_t>&)> enviarDadosFunc
|
||||||
|
) {
|
||||||
|
if (todosIDs || componente.ID_Num == id_num) {
|
||||||
|
componente.RequisitarDados();
|
||||||
|
|
||||||
|
if (enviarStatus) {
|
||||||
|
enviarDadosFunc(componente.MontarMensagemCAN(CanMessagePosicaoDados::Status));
|
||||||
|
}
|
||||||
|
|
||||||
|
if (componente.Iniciado) {
|
||||||
|
if (todosDados) {
|
||||||
|
for (auto p : posicoes) {
|
||||||
|
if (p == CanMessagePosicaoDados::Status) continue;
|
||||||
|
enviarDadosFunc(componente.MontarMensagemCAN(p));
|
||||||
|
}
|
||||||
|
} else {
|
||||||
|
if (posicao != CanMessagePosicaoDados::Status || !enviarStatus) {
|
||||||
|
enviarDadosFunc(componente.MontarMensagemCAN(posicao));
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
};
|
};
|
||||||
|
|
||||||
#endif
|
#endif
|
||||||
|
|
@ -0,0 +1,129 @@
|
||||||
|
#include "SerialService.h"
|
||||||
|
#include "Pinout.h"
|
||||||
|
#include "Utils.h"
|
||||||
|
#include "IComponenteCAN.h"
|
||||||
|
|
||||||
|
#ifndef ReleModel_v2
|
||||||
|
#define ReleModel_v2
|
||||||
|
|
||||||
|
class Rele_v2 : public ComponenteCAN {
|
||||||
|
public:
|
||||||
|
String ID;
|
||||||
|
uint8_t ID_Num = 0;
|
||||||
|
PinoModel _pino;
|
||||||
|
bool Iniciado = false;
|
||||||
|
|
||||||
|
Rele_v2() {}
|
||||||
|
|
||||||
|
Rele_v2(
|
||||||
|
const String& id,
|
||||||
|
uint8_t idNum,
|
||||||
|
bool atuacaoNivelAlto,
|
||||||
|
int pino,
|
||||||
|
TiposBarramentos tipoBarramento
|
||||||
|
) :
|
||||||
|
ID(id),
|
||||||
|
ID_Num(idNum),
|
||||||
|
_pino(PinoModel(pino, tipoBarramento, _OUTPUT)),
|
||||||
|
AtuacaoNivelAlto(atuacaoNivelAlto)
|
||||||
|
{
|
||||||
|
}
|
||||||
|
|
||||||
|
void Inicializar() {
|
||||||
|
if (Iniciado) {
|
||||||
|
MostrarLog("Rele ja inicializado");
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
if (_pino.Definido()) {
|
||||||
|
bool nivelInicial = AtuacaoNivelAlto ? false : true;
|
||||||
|
_pino.Conectar(nivelInicial);
|
||||||
|
_Status = Estado::Desligado;
|
||||||
|
Iniciado = true;
|
||||||
|
MostrarLog("Rele iniciado");
|
||||||
|
}
|
||||||
|
else {
|
||||||
|
MostrarLog("Rele nao iniciado");
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void Desligar() {
|
||||||
|
if (!Iniciado) {
|
||||||
|
MostrarLog("Rele nao esta inicializado");
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
AtualizarStatus(Estado::Desligado);
|
||||||
|
_pino.Desconectar();
|
||||||
|
Iniciado = false;
|
||||||
|
MostrarLog("Rele desligado");
|
||||||
|
}
|
||||||
|
|
||||||
|
void RequisitarDados() {
|
||||||
|
if (!Iniciado) return;
|
||||||
|
AtualizarLeitura();
|
||||||
|
}
|
||||||
|
|
||||||
|
void ProcessarComando(std::vector<uint8_t>& data) {
|
||||||
|
if (!Iniciado) return;
|
||||||
|
if (data.size() < 3) return;
|
||||||
|
CanMessagePosicaoDados posicao = (CanMessagePosicaoDados)data[0];
|
||||||
|
uint8_t idNum = data[1];
|
||||||
|
if (idNum != ID_Num) return;
|
||||||
|
switch (posicao) {
|
||||||
|
case CanMessagePosicaoDados::Command1: {
|
||||||
|
Estado novoEstado = (Estado)data[2];
|
||||||
|
AtualizarStatus(novoEstado);
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
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(static_cast<uint8_t>(_Status));
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
return data;
|
||||||
|
}
|
||||||
|
|
||||||
|
private:
|
||||||
|
bool DebugMode = true;
|
||||||
|
|
||||||
|
bool AtuacaoNivelAlto = true;
|
||||||
|
Estado _Status = Desligado;
|
||||||
|
|
||||||
|
void AtualizarStatus(Estado novoEstado) {
|
||||||
|
bool ligado = (novoEstado == Estado::Ligado);
|
||||||
|
bool nivelPino = AtuacaoNivelAlto ? ligado : !ligado;
|
||||||
|
MostrarLog("Atualizando status do rele de " + String(_Status) + " para " + String(novoEstado));
|
||||||
|
_pino.set(nivelPino);
|
||||||
|
_Status = novoEstado;
|
||||||
|
}
|
||||||
|
|
||||||
|
void AtualizarLeitura() {
|
||||||
|
bool nivelPino = _pino.get();
|
||||||
|
bool ligado = AtuacaoNivelAlto ? nivelPino : !nivelPino;
|
||||||
|
_Status = ligado ? Estado::Ligado : Estado::Desligado;
|
||||||
|
}
|
||||||
|
|
||||||
|
void MostrarLog(String mensagem) {
|
||||||
|
if (DebugMode) {
|
||||||
|
PrintTela("[RLE " + ID + "] " + mensagem);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
};
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
#endif
|
||||||
|
|
@ -0,0 +1,218 @@
|
||||||
|
#include "SerialService.h"
|
||||||
|
#include "I2CService.h"
|
||||||
|
#include "Utils.h"
|
||||||
|
#include "IComponenteCAN.h"
|
||||||
|
#include <Wire.h>
|
||||||
|
#include <vector>
|
||||||
|
#include <Adafruit_INA219.h>
|
||||||
|
#include <Adafruit_INA228.h>
|
||||||
|
#include <SimpleKalmanFilter.h>
|
||||||
|
|
||||||
|
#ifndef SensorCorrenteModel_v2
|
||||||
|
#define SensorCorrenteModel_v2
|
||||||
|
|
||||||
|
class SensorCorrente_v2 : public ComponenteCAN {
|
||||||
|
public:
|
||||||
|
String ID;
|
||||||
|
uint8_t ID_Num = 0;
|
||||||
|
uint8_t _Endereco = 0;
|
||||||
|
TiposShuntCorrente _A_Shunt = TiposShuntCorrente::INA228_10A;
|
||||||
|
int _CanalMUX = -1;
|
||||||
|
bool isINA219 = false;
|
||||||
|
|
||||||
|
bool Iniciado = false;
|
||||||
|
|
||||||
|
float busVoltage = 0;
|
||||||
|
float current = 0;
|
||||||
|
float power = 0;
|
||||||
|
float energy = 0;
|
||||||
|
float charge = 0;
|
||||||
|
uint16_t codAlarme = 0;
|
||||||
|
float temperature = 0;
|
||||||
|
|
||||||
|
SensorCorrente_v2() {}
|
||||||
|
|
||||||
|
SensorCorrente_v2(
|
||||||
|
const String& id,
|
||||||
|
uint8_t idNum,
|
||||||
|
uint8_t endereco,
|
||||||
|
TiposShuntCorrente tipoShunt,
|
||||||
|
int canalMux = -1
|
||||||
|
) :
|
||||||
|
ID(id),
|
||||||
|
ID_Num(idNum),
|
||||||
|
_Endereco(endereco),
|
||||||
|
_A_Shunt(tipoShunt),
|
||||||
|
_CanalMUX(canalMux)
|
||||||
|
{
|
||||||
|
}
|
||||||
|
|
||||||
|
void Inicializar() {
|
||||||
|
if (Iniciado) {
|
||||||
|
MostrarLog("Sensor ja inicializado");
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (!I2CService::SolicitarAcessoI2C(ID_Num, _CanalMUX)) {
|
||||||
|
MostrarLog("Falha ao solicitar acesso I2C");
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
isINA219 = (_A_Shunt == TiposShuntCorrente::INA219_3A2);
|
||||||
|
|
||||||
|
bool encontrado = I2CService::VerificaEnderecoBarramento(_Endereco);
|
||||||
|
if (!encontrado) {
|
||||||
|
Iniciado = false;
|
||||||
|
MostrarLog("Endereco I2C nao encontrado: " + String(_Endereco));
|
||||||
|
I2CService::LiberarAcessoI2C(ID_Num);
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (isINA219) {
|
||||||
|
sINA219 = Adafruit_INA219(_Endereco);
|
||||||
|
Iniciado = sINA219.begin();
|
||||||
|
|
||||||
|
if (Iniciado) {
|
||||||
|
sINA219.setCalibration_32V_2A();
|
||||||
|
MostrarLog("INA219 iniciado no canal " + String(_CanalMUX));
|
||||||
|
} else {
|
||||||
|
MostrarLog("Falha ao iniciar INA219");
|
||||||
|
}
|
||||||
|
}
|
||||||
|
else {
|
||||||
|
sINA228 = Adafruit_INA228();
|
||||||
|
Iniciado = sINA228.begin(_Endereco);
|
||||||
|
|
||||||
|
if (Iniciado) {
|
||||||
|
ConfigurarShuntINA228();
|
||||||
|
sINA228.setAveragingCount(INA228_COUNT_16);
|
||||||
|
sINA228.setCurrentConversionTime(INA228_TIME_280_us);
|
||||||
|
MostrarLog("INA228 iniciado no canal " + String(_CanalMUX));
|
||||||
|
} else {
|
||||||
|
MostrarLog("Falha ao iniciar INA228");
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
I2CService::LiberarAcessoI2C(ID_Num);
|
||||||
|
}
|
||||||
|
|
||||||
|
void Desligar() {
|
||||||
|
if (!Iniciado) {
|
||||||
|
MostrarLog("Sensor nao esta inicializado");
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
Iniciado = false;
|
||||||
|
MostrarLog("Sensor desligado");
|
||||||
|
}
|
||||||
|
|
||||||
|
void RequisitarDados() {
|
||||||
|
if (!Iniciado) return;
|
||||||
|
AferirCorrente();
|
||||||
|
}
|
||||||
|
|
||||||
|
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 volts = (uint16_t)(busVoltage * 100.0f);
|
||||||
|
uint16_t amps = (uint16_t)(current / 10.0f);
|
||||||
|
uint16_t watts = (uint16_t)(power / 10.0f);
|
||||||
|
|
||||||
|
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: {
|
||||||
|
if (isINA219) break;
|
||||||
|
|
||||||
|
uint16_t wh = (uint16_t)(energy / 10.0f);
|
||||||
|
uint16_t temp = (uint16_t)(temperature * 100.0f);
|
||||||
|
|
||||||
|
data.push_back(wh >> 8); data.push_back(wh & 0xFF);
|
||||||
|
data.push_back(temp >> 8); data.push_back(temp & 0xFF);
|
||||||
|
data.push_back(codAlarme >> 8); data.push_back(codAlarme & 0xFF);
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
return data;
|
||||||
|
}
|
||||||
|
|
||||||
|
private:
|
||||||
|
bool DebugMode = true;
|
||||||
|
SimpleKalmanFilter filtro = SimpleKalmanFilter(10, 10, 0.05);
|
||||||
|
Adafruit_INA219 sINA219;
|
||||||
|
Adafruit_INA228 sINA228;
|
||||||
|
|
||||||
|
void MostrarLog(String mensagem) {
|
||||||
|
if (DebugMode) {
|
||||||
|
PrintTela("[COR " + ID + "] " + mensagem);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void ConfigurarShuntINA228() {
|
||||||
|
if (_A_Shunt == TiposShuntCorrente::INA228_10A) {
|
||||||
|
sINA228.setShunt(0.015, 10.0);
|
||||||
|
}
|
||||||
|
else if (_A_Shunt == TiposShuntCorrente::INA228_50A) {
|
||||||
|
sINA228.setShunt(0.0015, 50.0);
|
||||||
|
}
|
||||||
|
else if (_A_Shunt == TiposShuntCorrente::INA228_100A) {
|
||||||
|
sINA228.setShunt(0.00075, 100.0);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void AferirCorrente() {
|
||||||
|
if (!Iniciado) return;
|
||||||
|
if (!I2CService::SolicitarAcessoI2C(ID_Num, _CanalMUX)) return;
|
||||||
|
|
||||||
|
if (isINA219) {
|
||||||
|
busVoltage = sINA219.getBusVoltage_V();
|
||||||
|
float leituraCorrente = sINA219.getCurrent_mA();
|
||||||
|
current = filtro.updateEstimate(leituraCorrente);
|
||||||
|
power = sINA219.getPower_mW();
|
||||||
|
}
|
||||||
|
else {
|
||||||
|
busVoltage = sINA228.getBusVoltage_V();
|
||||||
|
float leituraCorrente = sINA228.getCurrent_mA();
|
||||||
|
current = filtro.updateEstimate(leituraCorrente);
|
||||||
|
power = sINA228.getPower_mW();
|
||||||
|
temperature = sINA228.readDieTemp();
|
||||||
|
energy = sINA228.readEnergy();
|
||||||
|
charge = sINA228.readCharge();
|
||||||
|
codAlarme = (uint16_t)LerRegistradorINA(0x0B, 2);
|
||||||
|
}
|
||||||
|
|
||||||
|
I2CService::LiberarAcessoI2C(ID_Num);
|
||||||
|
}
|
||||||
|
|
||||||
|
uint32_t LerRegistradorINA(uint8_t reg, uint8_t bytes) {
|
||||||
|
uint8_t buffer[4] = {0};
|
||||||
|
if (bytes > 4) return 0;
|
||||||
|
|
||||||
|
if (!I2CService::LeituraSegura(_Endereco, buffer, bytes, reg)) {
|
||||||
|
MostrarLog("Falha na leitura do registrador 0x" + String(reg, HEX));
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
uint32_t valor = 0;
|
||||||
|
for (uint8_t i = 0; i < bytes; i++) {
|
||||||
|
valor = (valor << 8) | buffer[i];
|
||||||
|
}
|
||||||
|
|
||||||
|
return valor;
|
||||||
|
}
|
||||||
|
};
|
||||||
|
|
||||||
|
#endif
|
||||||
|
|
@ -0,0 +1,136 @@
|
||||||
|
#include "SerialService.h"
|
||||||
|
#include "I2CService.h"
|
||||||
|
#include "Utils.h"
|
||||||
|
#include "IComponenteCAN.h"
|
||||||
|
#include <Wire.h>
|
||||||
|
|
||||||
|
#ifndef SensorMagneticoModel_v2
|
||||||
|
#define SensorMagneticoModel_v2
|
||||||
|
|
||||||
|
class SensorMagnetico_v2 : public ComponenteCAN {
|
||||||
|
public:
|
||||||
|
String ID;
|
||||||
|
uint8_t ID_Num = 0;
|
||||||
|
uint8_t _Endereco = 0x36;
|
||||||
|
int _CanalMUX = -1;
|
||||||
|
|
||||||
|
bool Iniciado = false;
|
||||||
|
uint16_t raw = 0;
|
||||||
|
float angle = 0;
|
||||||
|
|
||||||
|
SensorMagnetico_v2() {}
|
||||||
|
|
||||||
|
SensorMagnetico_v2(
|
||||||
|
const String& id,
|
||||||
|
uint8_t idNum,
|
||||||
|
uint8_t endereco = 0x36,
|
||||||
|
int canalMux = -1
|
||||||
|
) :
|
||||||
|
ID(id),
|
||||||
|
ID_Num(idNum),
|
||||||
|
_Endereco(endereco),
|
||||||
|
_CanalMUX(canalMux)
|
||||||
|
{
|
||||||
|
}
|
||||||
|
|
||||||
|
void Inicializar() {
|
||||||
|
if (Iniciado) {
|
||||||
|
MostrarLog("Sensor ja inicializado");
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (!I2CService::SolicitarAcessoI2C(ID_Num, _CanalMUX)) {
|
||||||
|
MostrarLog("Falha ao solicitar acesso I2C");
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
bool encontrado = I2CService::VerificaEnderecoBarramento(_Endereco);
|
||||||
|
Iniciado = encontrado;
|
||||||
|
|
||||||
|
if (!Iniciado) {
|
||||||
|
MostrarLog("Sensor nao iniciado, endereco " + String(_Endereco) + " nao encontrado");
|
||||||
|
} else {
|
||||||
|
MostrarLog("Sensor iniciado");
|
||||||
|
}
|
||||||
|
|
||||||
|
I2CService::LiberarAcessoI2C(ID_Num);
|
||||||
|
}
|
||||||
|
|
||||||
|
void Desligar() {
|
||||||
|
if (!Iniciado) {
|
||||||
|
MostrarLog("Sensor nao esta inicializado");
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
Iniciado = false;
|
||||||
|
MostrarLog("Sensor desligado");
|
||||||
|
}
|
||||||
|
|
||||||
|
void RequisitarDados() {
|
||||||
|
if (!Iniciado) return;
|
||||||
|
AferirAngulo();
|
||||||
|
}
|
||||||
|
|
||||||
|
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 ang100 = (uint16_t)round(angle * 100.0f);
|
||||||
|
data.push_back(raw >> 8);
|
||||||
|
data.push_back(raw & 0xFF);
|
||||||
|
data.push_back(ang100 >> 8);
|
||||||
|
data.push_back(ang100 & 0xFF);
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
return data;
|
||||||
|
}
|
||||||
|
|
||||||
|
private:
|
||||||
|
bool DebugMode = true;
|
||||||
|
static const uint8_t REG_RAW_ANGLE = 0x0C;
|
||||||
|
|
||||||
|
void MostrarLog(String mensagem) {
|
||||||
|
if (DebugMode) {
|
||||||
|
PrintTela("[MAG " + ID + "] " + mensagem);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void AferirAngulo() {
|
||||||
|
if (!Iniciado) return;
|
||||||
|
if (!I2CService::SolicitarAcessoI2C(ID_Num, _CanalMUX)) return;
|
||||||
|
|
||||||
|
raw = LerRawAngle();
|
||||||
|
angle = RawToDegrees(raw);
|
||||||
|
|
||||||
|
I2CService::LiberarAcessoI2C(ID_Num);
|
||||||
|
}
|
||||||
|
|
||||||
|
uint16_t LerRawAngle() {
|
||||||
|
uint8_t buffer[2] = {0};
|
||||||
|
|
||||||
|
if (!I2CService::LeituraSegura(_Endereco, buffer, 2, REG_RAW_ANGLE)) {
|
||||||
|
MostrarLog("Falha na leitura do RAW_ANGLE");
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
uint16_t valor = ((uint16_t)buffer[0] << 8) | buffer[1];
|
||||||
|
valor &= 0x0FFF;
|
||||||
|
return valor;
|
||||||
|
}
|
||||||
|
|
||||||
|
float RawToDegrees(uint16_t valorRaw) {
|
||||||
|
return (valorRaw * 360.0f) / 4096.0f;
|
||||||
|
}
|
||||||
|
};
|
||||||
|
|
||||||
|
#endif
|
||||||
|
|
@ -0,0 +1,151 @@
|
||||||
|
#include "SerialService.h"
|
||||||
|
#include "Pinout.h"
|
||||||
|
#include "Utils.h"
|
||||||
|
#include "IComponenteCAN.h"
|
||||||
|
#include <SimpleKalmanFilter.h>
|
||||||
|
|
||||||
|
#ifndef SensorTemperaturaNtcModel
|
||||||
|
#define SensorTemperaturaNtcModel
|
||||||
|
|
||||||
|
class SensorTemperaturaNtc : public ComponenteCAN {
|
||||||
|
public:
|
||||||
|
String ID;
|
||||||
|
uint8_t ID_Num = 0;
|
||||||
|
PinoModel _pino;
|
||||||
|
|
||||||
|
int leitura = 0;
|
||||||
|
double temperatura = -1;
|
||||||
|
double tensao = 0;
|
||||||
|
|
||||||
|
bool Iniciado = false;
|
||||||
|
bool Checando = false;
|
||||||
|
|
||||||
|
float OffsetTemperatura = 0.0f;
|
||||||
|
|
||||||
|
SensorTemperaturaNtc() {}
|
||||||
|
|
||||||
|
SensorTemperaturaNtc(
|
||||||
|
const String& id,
|
||||||
|
uint8_t idNum,
|
||||||
|
int pino,
|
||||||
|
TiposBarramentos tipoBarramento,
|
||||||
|
int canalMux = -1,
|
||||||
|
float offsetTemperatura = 0.0f
|
||||||
|
) :
|
||||||
|
ID(id),
|
||||||
|
ID_Num(idNum),
|
||||||
|
_pino(PinoModel(pino, tipoBarramento, _INPUT, ANALOGICO, idNum, canalMux)),
|
||||||
|
OffsetTemperatura(offsetTemperatura)
|
||||||
|
{
|
||||||
|
}
|
||||||
|
|
||||||
|
void Inicializar() {
|
||||||
|
if (Iniciado) {
|
||||||
|
MostrarLog("Sensor ja inicializado");
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
leitura = 0;
|
||||||
|
temperatura = -1;
|
||||||
|
tensao = 0;
|
||||||
|
|
||||||
|
_pino.Conectar();
|
||||||
|
|
||||||
|
Iniciado = _pino.Definido();
|
||||||
|
|
||||||
|
MostrarLog(Iniciado ? "Sensor iniciado" : "Sensor nao iniciado");
|
||||||
|
}
|
||||||
|
|
||||||
|
void Desligar() {
|
||||||
|
if (!Iniciado) {
|
||||||
|
MostrarLog("Sensor nao esta inicializado");
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
_pino.Desconectar();
|
||||||
|
|
||||||
|
Iniciado = false;
|
||||||
|
|
||||||
|
MostrarLog("Sensor desligado");
|
||||||
|
}
|
||||||
|
|
||||||
|
void RequisitarDados() {
|
||||||
|
if (!Iniciado) return;
|
||||||
|
AferirTemperatura();
|
||||||
|
}
|
||||||
|
|
||||||
|
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: {
|
||||||
|
int16_t leituraInt = static_cast<int16_t>(round(temperatura * 100.0f));
|
||||||
|
data.push_back((uint8_t)(leitura >> 8));
|
||||||
|
data.push_back((uint8_t)(leitura & 0xFF));
|
||||||
|
data.push_back((uint8_t)(leituraInt >> 8));
|
||||||
|
data.push_back((uint8_t)(leituraInt & 0xFF));
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
return data;
|
||||||
|
}
|
||||||
|
|
||||||
|
private:
|
||||||
|
bool DebugMode = true;
|
||||||
|
// SimpleKalmanFilter filtro = SimpleKalmanFilter(5, 15, 0.01);
|
||||||
|
|
||||||
|
void MostrarLog(String mensagem) {
|
||||||
|
if (DebugMode) {
|
||||||
|
PrintTela("[TMP " + ID + "] " + mensagem);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void AferirTemperatura() {
|
||||||
|
leitura = _pino.get(1);
|
||||||
|
// leitura = filtro.updateEstimate(leitura);
|
||||||
|
|
||||||
|
temperatura = CalcularTemperatura((float)leitura, OffsetTemperatura);
|
||||||
|
|
||||||
|
MostrarLog("Leitura: " + String(leitura) +
|
||||||
|
", Tensao: " + String(tensao) +
|
||||||
|
", Temperatura: " + String(temperatura));
|
||||||
|
}
|
||||||
|
|
||||||
|
float CalcularTemperatura(float leituraAdc, float offset = 0.0f) {
|
||||||
|
const float Vs = 5.0f;
|
||||||
|
const float Beta = 3950.0f;
|
||||||
|
const float To = 298.15f;
|
||||||
|
const float Ro = 10000.0f;
|
||||||
|
const float adcMax = 4095.0f;
|
||||||
|
static constexpr float divisor = 1.0f;
|
||||||
|
static constexpr float vOffset = (2.516f / 3.036f);
|
||||||
|
|
||||||
|
if (leituraAdc < 200.0f) return -1.0f;
|
||||||
|
|
||||||
|
float Vadc = leituraAdc / adcMax * Vs;
|
||||||
|
Vadc = Vadc * divisor * vOffset;
|
||||||
|
|
||||||
|
tensao = Vadc;
|
||||||
|
|
||||||
|
if (Vadc <= 0.001f) return -100.0f;
|
||||||
|
if (Vadc >= Vs) return NAN;
|
||||||
|
|
||||||
|
float Rt = Ro * (Vadc / (Vs - Vadc));
|
||||||
|
float T = 1.0f / (1.0f / To + log(Rt / Ro) / Beta);
|
||||||
|
float Tc = T - 273.15f + offset;
|
||||||
|
|
||||||
|
return roundf(Tc * 100.0f) / 100.0f;
|
||||||
|
}
|
||||||
|
};
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
#endif
|
||||||
|
|
@ -0,0 +1,128 @@
|
||||||
|
#include "SerialService.h"
|
||||||
|
#include "Pinout.h"
|
||||||
|
#include "Utils.h"
|
||||||
|
#include "IComponenteCAN.h"
|
||||||
|
#include <ESP32Servo.h>
|
||||||
|
|
||||||
|
#ifndef ServoAgroModel_v2
|
||||||
|
#define ServoAgroModel_v2
|
||||||
|
|
||||||
|
class ServoAgro_v2 : public ComponenteCAN {
|
||||||
|
public:
|
||||||
|
String ID;
|
||||||
|
uint8_t ID_Num = 0;
|
||||||
|
bool Incremental = false;
|
||||||
|
bool Iniciado = false;
|
||||||
|
PinoModel _pino;
|
||||||
|
|
||||||
|
ServoAgro_v2() {}
|
||||||
|
|
||||||
|
ServoAgro_v2(
|
||||||
|
const String& id,
|
||||||
|
uint8_t idNum,
|
||||||
|
bool incremental,
|
||||||
|
int pino,
|
||||||
|
TiposBarramentos tipoBarramento
|
||||||
|
) :
|
||||||
|
ID(id),
|
||||||
|
ID_Num(idNum),
|
||||||
|
Incremental(incremental),
|
||||||
|
_pino(PinoModel(pino, tipoBarramento, _OUTPUT))
|
||||||
|
{
|
||||||
|
}
|
||||||
|
|
||||||
|
void Inicializar() {
|
||||||
|
if (Iniciado) {
|
||||||
|
MostrarLog("Servo ja inicializado");
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
if (_pino.Definido()) {
|
||||||
|
_Servo.attach(_pino.num);
|
||||||
|
AtualizarAngulo(Incremental ? 0 : 180);
|
||||||
|
Iniciado = true;
|
||||||
|
MostrarLog("Servo iniciado");
|
||||||
|
}
|
||||||
|
else {
|
||||||
|
MostrarLog("Servo nao iniciado");
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void Desligar() {
|
||||||
|
if (!Iniciado) {
|
||||||
|
MostrarLog("Servo nao esta inicializado");
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (_pino.Definido()) {
|
||||||
|
_Servo.detach();
|
||||||
|
}
|
||||||
|
|
||||||
|
MostrarLog("Servo Desligado");
|
||||||
|
|
||||||
|
Iniciado = false;
|
||||||
|
}
|
||||||
|
|
||||||
|
void RequisitarDados() {
|
||||||
|
if (!Iniciado) return;
|
||||||
|
}
|
||||||
|
|
||||||
|
void ProcessarComando(std::vector<uint8_t>& data) {
|
||||||
|
if (!Iniciado) return;
|
||||||
|
if (data.size() < 3) return;
|
||||||
|
CanMessagePosicaoDados posicao = (CanMessagePosicaoDados)data[0];
|
||||||
|
uint8_t idNum = data[1];
|
||||||
|
if (idNum != ID_Num) return;
|
||||||
|
switch (posicao) {
|
||||||
|
case CanMessagePosicaoDados::Command1: {
|
||||||
|
int novoAngulo = (int)data[2];
|
||||||
|
AtualizarAngulo(novoAngulo);
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
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(static_cast<uint8_t>(_Angulo));
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
return data;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
private:
|
||||||
|
bool DebugMode = true;
|
||||||
|
|
||||||
|
Servo _Servo;
|
||||||
|
int _Angulo = 0;
|
||||||
|
|
||||||
|
void AtualizarAngulo(int novoAngulo) {
|
||||||
|
novoAngulo = constrain(novoAngulo, 0, 180);
|
||||||
|
MostrarLog("Angulo atualizado de " + String(_Angulo) + " para " + String(novoAngulo));
|
||||||
|
_Angulo = novoAngulo;
|
||||||
|
_Servo.write(_Angulo);
|
||||||
|
}
|
||||||
|
|
||||||
|
void MostrarLog(String mensagem) {
|
||||||
|
if (DebugMode) {
|
||||||
|
PrintTela("[SRV " + ID + "] " + mensagem);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
};
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
#endif
|
||||||
|
|
@ -0,0 +1,153 @@
|
||||||
|
#include "SerialService.h"
|
||||||
|
#include "Pinout.h"
|
||||||
|
#include "Utils.h"
|
||||||
|
#include "IComponenteCAN.h"
|
||||||
|
|
||||||
|
#ifndef SinaleiroModel
|
||||||
|
#define SinaleiroModel
|
||||||
|
|
||||||
|
class Sinaleiro_v2 : public ComponenteCAN {
|
||||||
|
public:
|
||||||
|
String ID;
|
||||||
|
uint8_t ID_Num = 0;
|
||||||
|
PinoModel _pino;
|
||||||
|
bool Iniciado = false;
|
||||||
|
|
||||||
|
int _StatusID = 0;
|
||||||
|
StatusLED _Status = Apagado;
|
||||||
|
StatusLED _StatusLeitura = Apagado;
|
||||||
|
|
||||||
|
int _Qtd = 0;
|
||||||
|
int _TempoMs = 0;
|
||||||
|
int _PausaMs = 0;
|
||||||
|
|
||||||
|
// controle interno (não era exposto antes)
|
||||||
|
bool _estadoAtual = false;
|
||||||
|
int _piscadasAtual = 0;
|
||||||
|
bool _emPausa = false;
|
||||||
|
unsigned long _lastMillis = 0;
|
||||||
|
|
||||||
|
Sinaleiro_v2() {}
|
||||||
|
|
||||||
|
Sinaleiro_v2(
|
||||||
|
const String& id,
|
||||||
|
uint8_t idNum,
|
||||||
|
int pino,
|
||||||
|
TiposBarramentos tipoBarramento
|
||||||
|
) :
|
||||||
|
ID(id),
|
||||||
|
ID_Num(idNum),
|
||||||
|
_pino(PinoModel(pino, tipoBarramento, _OUTPUT))
|
||||||
|
{}
|
||||||
|
|
||||||
|
void Inicializar() {
|
||||||
|
if (Iniciado) return;
|
||||||
|
if (_pino.Definido()) {
|
||||||
|
_pino.Conectar(false);
|
||||||
|
Iniciado = true;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void AtualizarEstado(unsigned long agora) {
|
||||||
|
if (!Iniciado) return;
|
||||||
|
|
||||||
|
if (_Status != StatusLED::Piscando) {
|
||||||
|
_pino.set(_Status == StatusLED::Aceso);
|
||||||
|
_StatusLeitura = _Status;
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
// modo piscando
|
||||||
|
if (_piscadasAtual < _Qtd) {
|
||||||
|
if (!_emPausa && (agora - _lastMillis >= _TempoMs)) {
|
||||||
|
_estadoAtual = !_estadoAtual;
|
||||||
|
_pino.set(_estadoAtual);
|
||||||
|
_lastMillis = agora;
|
||||||
|
|
||||||
|
if (!_estadoAtual) {
|
||||||
|
_piscadasAtual++;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
else {
|
||||||
|
// pausa
|
||||||
|
if (!_emPausa) {
|
||||||
|
_pino.set(false);
|
||||||
|
_emPausa = true;
|
||||||
|
_lastMillis = agora;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (agora - _lastMillis >= _PausaMs) {
|
||||||
|
_emPausa = false;
|
||||||
|
_piscadasAtual = 0;
|
||||||
|
_lastMillis = agora;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
_StatusLeitura = StatusLED::Piscando;
|
||||||
|
}
|
||||||
|
|
||||||
|
void ProcessarComando(std::vector<uint8_t>& data) {
|
||||||
|
if (!Iniciado) return;
|
||||||
|
if (data.size() < 7) return;
|
||||||
|
|
||||||
|
CanMessagePosicaoDados posicao = (CanMessagePosicaoDados)data[0];
|
||||||
|
uint8_t idNum = data[1];
|
||||||
|
if (idNum != ID_Num) return;
|
||||||
|
|
||||||
|
if (posicao == CanMessagePosicaoDados::Command1) {
|
||||||
|
_StatusID = data[2];
|
||||||
|
_Status = (StatusLED)data[3];
|
||||||
|
_Qtd = data[4];
|
||||||
|
_TempoMs = (uint16_t)data[5] * 10;
|
||||||
|
_PausaMs = (uint16_t)data[6] * 10;
|
||||||
|
|
||||||
|
// reset ciclo
|
||||||
|
_piscadasAtual = 0;
|
||||||
|
_emPausa = false;
|
||||||
|
_lastMillis = millis();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void RequisitarDados() {}
|
||||||
|
|
||||||
|
std::vector<uint8_t> MontarMensagemCAN(CanMessagePosicaoDados posicao) {
|
||||||
|
std::vector<uint8_t> data;
|
||||||
|
data.push_back((uint8_t)posicao);
|
||||||
|
data.push_back(ID_Num);
|
||||||
|
|
||||||
|
switch (posicao) {
|
||||||
|
case Status:
|
||||||
|
data.push_back(Iniciado ? 1 : 0);
|
||||||
|
break;
|
||||||
|
case Dados1:
|
||||||
|
data.push_back(_StatusID);
|
||||||
|
data.push_back(_StatusLeitura);
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
|
return data;
|
||||||
|
}
|
||||||
|
};
|
||||||
|
|
||||||
|
class LedManager {
|
||||||
|
public:
|
||||||
|
std::vector<Sinaleiro_v2*> leds;
|
||||||
|
|
||||||
|
void Adicionar(Sinaleiro_v2* led) {
|
||||||
|
leds.push_back(led);
|
||||||
|
}
|
||||||
|
|
||||||
|
void Atualizar() {
|
||||||
|
unsigned long agora = millis();
|
||||||
|
for (auto* led : leds) {
|
||||||
|
if (led != nullptr && led->Iniciado) {
|
||||||
|
led->AtualizarEstado(agora);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
};
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
#endif
|
||||||
|
|
@ -0,0 +1,8 @@
|
||||||
|
{
|
||||||
|
// Use IntelliSense to learn about possible attributes.
|
||||||
|
// Hover to view descriptions of existing attributes.
|
||||||
|
"version": "0.2.0",
|
||||||
|
"configurations": [
|
||||||
|
|
||||||
|
]
|
||||||
|
}
|
||||||
|
|
@ -0,0 +1,416 @@
|
||||||
|
// Dispositivo: Sensoriamento
|
||||||
|
// Versão Firmware: 22
|
||||||
|
// Ultima atualização: 20/03/2026
|
||||||
|
// Atualização: Firmware mock
|
||||||
|
|
||||||
|
#include "C:\ZendionInc\agrobot_base\Firmware\Modulos\SerialService.h"
|
||||||
|
#include "C:\ZendionInc\agrobot_base\Firmware\Modulos\CanService.h"
|
||||||
|
#include "C:\ZendionInc\agrobot_base\Firmware\Modulos\EepromService.h"
|
||||||
|
#include "C:\ZendionInc\agrobot_base\Firmware\Modulos\I2CService.h"
|
||||||
|
#include "C:\ZendionInc\agrobot_base\Firmware\Modulos\Utils.h"
|
||||||
|
#include "C:\ZendionInc\agrobot_base\Firmware\Modulos\Pinout.h"
|
||||||
|
#include "C:\ZendionInc\agrobot_base\Firmware\Modulos\ServoAgroModel_v2.h"
|
||||||
|
#include "C:\ZendionInc\agrobot_base\Firmware\Modulos\ReleModel_v2.h"
|
||||||
|
#include "C:\ZendionInc\agrobot_base\Firmware\Modulos\SinaleiroModel_v2.h"
|
||||||
|
#include "C:\ZendionInc\agrobot_base\Firmware\Modulos\SensorTemperaturaNtcModel.h"
|
||||||
|
#include "C:\ZendionInc\agrobot_base\Firmware\Modulos\SensorCorrenteModel_v2.h"
|
||||||
|
#include "C:\ZendionInc\agrobot_base\Firmware\Modulos\SensorMagneticoModel_v2.h"
|
||||||
|
|
||||||
|
|
||||||
|
// Configurações do CAN
|
||||||
|
uint8_t CanID = 0x11;
|
||||||
|
CanService canService(CanID);
|
||||||
|
|
||||||
|
//LoRaService loraService(&canService);
|
||||||
|
|
||||||
|
#define D_Code Sen
|
||||||
|
int VERSION = 22;
|
||||||
|
String Mod_ID = "Sen";
|
||||||
|
|
||||||
|
#define _pinoLED RGB_BUILTIN
|
||||||
|
|
||||||
|
bool Conectado = false;
|
||||||
|
bool Conectar = false;
|
||||||
|
|
||||||
|
int _TaxaAmostragem = 1000;
|
||||||
|
volatile bool RequisitarDados = false;
|
||||||
|
int latenciaLoop = 0;
|
||||||
|
int qtdEnviosStatus = 0;
|
||||||
|
int maxEnviosStatus = 5;
|
||||||
|
|
||||||
|
#pragma region SINALEIROS
|
||||||
|
Sinaleiro_v2 sLED_Manual("LEDMN", 21, 37, TiposBarramentos::CONTROLADOR);
|
||||||
|
Sinaleiro_v2 sLED_Automatico("LEDAT", 22, 35, TiposBarramentos::CONTROLADOR);
|
||||||
|
Sinaleiro_v2 sLED_Operacao("LEDOP", 23, 36, TiposBarramentos::CONTROLADOR);
|
||||||
|
|
||||||
|
Sinaleiro_v2* ObterSinaleiroPorId(uint8_t idNum) {
|
||||||
|
switch (idNum) {
|
||||||
|
case 21: return &sLED_Manual;
|
||||||
|
case 22: return &sLED_Automatico;
|
||||||
|
case 23: return &sLED_Operacao;
|
||||||
|
default: return nullptr;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
LedManager ledManager;
|
||||||
|
void LedTask(void* pv) {
|
||||||
|
while (true) {
|
||||||
|
ledManager.Atualizar();
|
||||||
|
vTaskDelay(pdMS_TO_TICKS(10)); // 100 Hz
|
||||||
|
}
|
||||||
|
}
|
||||||
|
#pragma endregion
|
||||||
|
|
||||||
|
#pragma region SERVOS
|
||||||
|
ServoAgro_v2 sSRV_FreioET("FROET", 41, true, 45, TiposBarramentos::CONTROLADOR);
|
||||||
|
ServoAgro_v2 sSRV_FreioDT("FRODT", 43, false, 47, TiposBarramentos::CONTROLADOR);
|
||||||
|
|
||||||
|
ServoAgro_v2* ObterServoPorId(uint8_t idNum) {
|
||||||
|
switch (idNum) {
|
||||||
|
case 41: return &sSRV_FreioET;
|
||||||
|
case 43: return &sSRV_FreioDT;
|
||||||
|
default: return nullptr;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
#pragma endregion
|
||||||
|
|
||||||
|
#pragma region RELES
|
||||||
|
Rele_v2 sRLE_CoolerIn("RLCIN", 81, true, 39, TiposBarramentos::CONTROLADOR);
|
||||||
|
Rele_v2 sRLE_CoolerOut("RLCOUT", 82, true, 40, TiposBarramentos::CONTROLADOR);
|
||||||
|
|
||||||
|
Rele_v2* ObterRelePorId(uint8_t idNum) {
|
||||||
|
switch (idNum) {
|
||||||
|
case 81: return &sRLE_CoolerIn;
|
||||||
|
case 82: return &sRLE_CoolerOut;
|
||||||
|
default: return nullptr;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
#pragma endregion
|
||||||
|
|
||||||
|
#pragma region TEMPERATURA NTC
|
||||||
|
SensorTemperaturaNtc sNTC_MotorET("TMVET", 1, 0, TiposBarramentos::EXPANSOR_ADS, -1, 0.0f);
|
||||||
|
SensorTemperaturaNtc sNTC_MotorDT("TMVDT", 3, 1, TiposBarramentos::EXPANSOR_ADS, -1, 0.0f);
|
||||||
|
SensorTemperaturaNtc sNTC_MotorEF("TMVEF", 2, 2, TiposBarramentos::EXPANSOR_ADS, -1, 0.0f);
|
||||||
|
SensorTemperaturaNtc sNTC_MotorDF("TMVDF", 4, 3, TiposBarramentos::EXPANSOR_ADS, -1, 0.0f);
|
||||||
|
|
||||||
|
SensorTemperaturaNtc* ObterTempNtcPorId(uint8_t idNum) {
|
||||||
|
switch (idNum) {
|
||||||
|
case 1: return &sNTC_MotorET;
|
||||||
|
case 2: return &sNTC_MotorEF;
|
||||||
|
case 3: return &sNTC_MotorDT;
|
||||||
|
case 4: return &sNTC_MotorDF;
|
||||||
|
default: return nullptr;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
#pragma endregion
|
||||||
|
|
||||||
|
#pragma region CORRENTE
|
||||||
|
SensorCorrente_v2 sCOR_3V0 ("COR_AB3V", 11, 0x40, TiposShuntCorrente::INA219_3A2, 5);
|
||||||
|
SensorCorrente_v2 sCOR_5V0 ("COR_AB5V", 12, 0x40, TiposShuntCorrente::INA219_3A2, 4);
|
||||||
|
SensorCorrente_v2 sCOR_7V2 ("COR_AB7V2", 13, 0x40, TiposShuntCorrente::INA219_3A2, 2);
|
||||||
|
SensorCorrente_v2 sCOR_12V ("COR_AB12V", 14, 0x40, TiposShuntCorrente::INA228_50A, 3);
|
||||||
|
SensorCorrente_v2 sCOR_19V ("COR_AB19V", 15, 0x40, TiposShuntCorrente::INA228_10A, 1);
|
||||||
|
SensorCorrente_v2 sCOR_24V ("COR_AB24V", 16, 0x40, TiposShuntCorrente::INA228_50A, 6);
|
||||||
|
SensorCorrente_v2 sCOR_36V ("COR_AB36V", 17, 0x40, TiposShuntCorrente::INA228_100A, 7);
|
||||||
|
|
||||||
|
SensorCorrente_v2* ObterSensorCorrentePorId(uint8_t idNum) {
|
||||||
|
switch (idNum) {
|
||||||
|
case 11: return &sCOR_3V0;
|
||||||
|
case 12: return &sCOR_5V0;
|
||||||
|
case 13: return &sCOR_7V2;
|
||||||
|
case 14: return &sCOR_12V;
|
||||||
|
case 15: return &sCOR_19V;
|
||||||
|
case 16: return &sCOR_24V;
|
||||||
|
case 17: return &sCOR_36V;
|
||||||
|
default: return nullptr;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
#pragma endregion
|
||||||
|
|
||||||
|
#pragma region MAGNETICOS
|
||||||
|
SensorMagnetico_v2 sMAG_MotorET("MAG_ET", 91, 0x36, 1);
|
||||||
|
SensorMagnetico_v2 sMAG_MotorDT("MAG_DT", 92, 0x36, 2);
|
||||||
|
SensorMagnetico_v2 sMAG_MotorEF("MAG_EF", 93, 0x36, 3);
|
||||||
|
SensorMagnetico_v2 sMAG_MotorDF("MAG_DF", 94, 0x36, 4);
|
||||||
|
|
||||||
|
SensorMagnetico_v2* ObterSensorMagneticoPorId(uint8_t idNum) {
|
||||||
|
switch (idNum) {
|
||||||
|
case 91: return &sMAG_MotorET;
|
||||||
|
case 92: return &sMAG_MotorDT;
|
||||||
|
case 93: return &sMAG_MotorEF;
|
||||||
|
case 94: return &sMAG_MotorDF;
|
||||||
|
default: return nullptr;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
#pragma endregion
|
||||||
|
|
||||||
|
void MostrarLog(String mensagem) {
|
||||||
|
bool _debugMode = false;
|
||||||
|
if (_debugMode) {
|
||||||
|
PrintTela(String("[SEN] ") + mensagem);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void inicializarDependenciasI2C() {
|
||||||
|
I2CService::IniciarI2C();
|
||||||
|
I2CService::IniciarMUX();
|
||||||
|
I2CService::IniciarMCP();
|
||||||
|
}
|
||||||
|
|
||||||
|
void setup() {
|
||||||
|
delay(2000);
|
||||||
|
|
||||||
|
inicializarDependenciasI2C();
|
||||||
|
|
||||||
|
sSRV_FreioET.Inicializar();
|
||||||
|
sSRV_FreioDT.Inicializar();
|
||||||
|
sRLE_CoolerIn.Inicializar();
|
||||||
|
sRLE_CoolerOut.Inicializar();
|
||||||
|
sLED_Manual.Inicializar();
|
||||||
|
sLED_Automatico.Inicializar();
|
||||||
|
sLED_Operacao.Inicializar();
|
||||||
|
sNTC_MotorET.Inicializar();
|
||||||
|
sNTC_MotorDT.Inicializar();
|
||||||
|
sNTC_MotorEF.Inicializar();
|
||||||
|
sNTC_MotorDF.Inicializar();
|
||||||
|
sCOR_3V0.Inicializar();
|
||||||
|
sCOR_5V0.Inicializar();
|
||||||
|
sCOR_7V2.Inicializar();
|
||||||
|
sCOR_12V.Inicializar();
|
||||||
|
sCOR_19V.Inicializar();
|
||||||
|
sCOR_24V.Inicializar();
|
||||||
|
sCOR_36V.Inicializar();
|
||||||
|
sMAG_MotorET.Inicializar();
|
||||||
|
sMAG_MotorDT.Inicializar();
|
||||||
|
sMAG_MotorEF.Inicializar();
|
||||||
|
sMAG_MotorDF.Inicializar();
|
||||||
|
|
||||||
|
ledManager.Adicionar(&sLED_Manual);
|
||||||
|
ledManager.Adicionar(&sLED_Automatico);
|
||||||
|
ledManager.Adicionar(&sLED_Operacao);
|
||||||
|
xTaskCreatePinnedToCore(LedTask, "LedTask", 3000, NULL, 1, NULL, tskNO_AFFINITY);
|
||||||
|
|
||||||
|
// Registrar callback para processar mensagens CAN recebidas
|
||||||
|
canService.setReceiveCallback([](int packetSize, int senderId, CanMessagePosicaoDados posicao, byte* data, int dataLength) {
|
||||||
|
std::vector<uint8_t> fullData;
|
||||||
|
fullData.push_back(static_cast<uint8_t>(posicao)); // Primeiro byte é sempre a posicao
|
||||||
|
for (int i = 0; i < dataLength; i++) {
|
||||||
|
fullData.push_back(data[i]);
|
||||||
|
}
|
||||||
|
|
||||||
|
ProcessarFrameCAN(posicao, fullData);
|
||||||
|
});
|
||||||
|
|
||||||
|
canService.begin();
|
||||||
|
}
|
||||||
|
|
||||||
|
void loop() {
|
||||||
|
if (!RequisitarDados) {
|
||||||
|
enviarLogsPeriodicamente();
|
||||||
|
}
|
||||||
|
|
||||||
|
delay(1);
|
||||||
|
}
|
||||||
|
|
||||||
|
// Função para enviar logs do sensor periodicamente
|
||||||
|
unsigned long ultimoEnvioLogs = 0;
|
||||||
|
const unsigned long intervaloEnvioLogs = 1000; // 1 segundo
|
||||||
|
void enviarLogsPeriodicamente() {
|
||||||
|
unsigned long tempoAtual = millis();
|
||||||
|
// Verifica se o intervalo de 5 segundos já passou
|
||||||
|
if (tempoAtual - ultimoEnvioLogs >= intervaloEnvioLogs) {
|
||||||
|
ultimoEnvioLogs = tempoAtual; // Atualiza o tempo do último envio
|
||||||
|
|
||||||
|
enviarDadosSensores(canService.ID_Num_sTOD, CanMessagePosicaoDados::DadosAll);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void enviarDadosSensores(uint8_t id_num, CanMessagePosicaoDados posicao) {
|
||||||
|
if (Conectado || posicao == CanMessagePosicaoDados::Status) {
|
||||||
|
unsigned long startLoop = millis();
|
||||||
|
|
||||||
|
bool enviarStatus = qtdEnviosStatus >= maxEnviosStatus && posicao != CanMessagePosicaoDados::Status;
|
||||||
|
bool todosDados = posicao == CanMessagePosicaoDados::DadosAll;
|
||||||
|
bool todosIDs = id_num == canService.ID_Num_sTOD;
|
||||||
|
|
||||||
|
if (id_num == canService.ID_Num_sMOD) {
|
||||||
|
EnviarDadosCAN(canService.MontarFrameReqStatusMod(D_Code, Conectado, VERSION));
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
MostrarLog("Atualizando dados dos sensores...");
|
||||||
|
|
||||||
|
if (todosIDs && enviarStatus) {
|
||||||
|
EnviarDadosCAN(canService.MontarFrameReqStatusMod(D_Code, Conectado, VERSION));
|
||||||
|
}
|
||||||
|
|
||||||
|
ComponenteCAN::ProcessarComponenteCAN(sSRV_FreioET, {CanMessagePosicaoDados::Dados1}, todosIDs, id_num, enviarStatus, todosDados, posicao, EnviarDadosCAN);
|
||||||
|
ComponenteCAN::ProcessarComponenteCAN(sSRV_FreioDT, {CanMessagePosicaoDados::Dados1}, todosIDs, id_num, enviarStatus, todosDados, posicao, EnviarDadosCAN);
|
||||||
|
|
||||||
|
ComponenteCAN::ProcessarComponenteCAN(sRLE_CoolerIn, {CanMessagePosicaoDados::Dados1}, todosIDs, id_num, enviarStatus, todosDados, posicao, EnviarDadosCAN);
|
||||||
|
ComponenteCAN::ProcessarComponenteCAN(sRLE_CoolerOut, {CanMessagePosicaoDados::Dados1}, todosIDs, id_num, enviarStatus, todosDados, posicao, EnviarDadosCAN);
|
||||||
|
|
||||||
|
ComponenteCAN::ProcessarComponenteCAN(sLED_Manual, {CanMessagePosicaoDados::Dados1}, todosIDs, id_num, enviarStatus, todosDados, posicao, EnviarDadosCAN);
|
||||||
|
ComponenteCAN::ProcessarComponenteCAN(sLED_Automatico, {CanMessagePosicaoDados::Dados1}, todosIDs, id_num, enviarStatus, todosDados, posicao, EnviarDadosCAN);
|
||||||
|
ComponenteCAN::ProcessarComponenteCAN(sLED_Operacao, {CanMessagePosicaoDados::Dados1}, todosIDs, id_num, enviarStatus, todosDados, posicao, EnviarDadosCAN);
|
||||||
|
|
||||||
|
ComponenteCAN::ProcessarComponenteCAN(sNTC_MotorET, {CanMessagePosicaoDados::Dados1}, todosIDs, id_num, enviarStatus, todosDados, posicao, EnviarDadosCAN);
|
||||||
|
ComponenteCAN::ProcessarComponenteCAN(sNTC_MotorDT, {CanMessagePosicaoDados::Dados1}, todosIDs, id_num, enviarStatus, todosDados, posicao, EnviarDadosCAN);
|
||||||
|
ComponenteCAN::ProcessarComponenteCAN(sNTC_MotorEF, {CanMessagePosicaoDados::Dados1}, todosIDs, id_num, enviarStatus, todosDados, posicao, EnviarDadosCAN);
|
||||||
|
ComponenteCAN::ProcessarComponenteCAN(sNTC_MotorDF, {CanMessagePosicaoDados::Dados1}, todosIDs, id_num, enviarStatus, todosDados, posicao, EnviarDadosCAN);
|
||||||
|
|
||||||
|
ComponenteCAN::ProcessarComponenteCAN(sCOR_3V0, {CanMessagePosicaoDados::Dados1}, todosIDs, id_num, enviarStatus, todosDados, posicao, EnviarDadosCAN);
|
||||||
|
ComponenteCAN::ProcessarComponenteCAN(sCOR_5V0, {CanMessagePosicaoDados::Dados1}, todosIDs, id_num, enviarStatus, todosDados, posicao, EnviarDadosCAN);
|
||||||
|
ComponenteCAN::ProcessarComponenteCAN(sCOR_7V2, {CanMessagePosicaoDados::Dados1}, todosIDs, id_num, enviarStatus, todosDados, posicao, EnviarDadosCAN);
|
||||||
|
ComponenteCAN::ProcessarComponenteCAN(sCOR_12V, {CanMessagePosicaoDados::Dados1, CanMessagePosicaoDados::Dados2}, todosIDs, id_num, enviarStatus, todosDados, posicao, EnviarDadosCAN);
|
||||||
|
ComponenteCAN::ProcessarComponenteCAN(sCOR_19V, {CanMessagePosicaoDados::Dados1, CanMessagePosicaoDados::Dados2}, todosIDs, id_num, enviarStatus, todosDados, posicao, EnviarDadosCAN);
|
||||||
|
ComponenteCAN::ProcessarComponenteCAN(sCOR_24V, {CanMessagePosicaoDados::Dados1, CanMessagePosicaoDados::Dados2}, todosIDs, id_num, enviarStatus, todosDados, posicao, EnviarDadosCAN);
|
||||||
|
ComponenteCAN::ProcessarComponenteCAN(sCOR_36V, {CanMessagePosicaoDados::Dados1, CanMessagePosicaoDados::Dados2}, todosIDs, id_num, enviarStatus, todosDados, posicao, EnviarDadosCAN);
|
||||||
|
|
||||||
|
ComponenteCAN::ProcessarComponenteCAN(sMAG_MotorET, {CanMessagePosicaoDados::Dados1}, todosIDs, id_num, enviarStatus, todosDados, posicao, EnviarDadosCAN);
|
||||||
|
ComponenteCAN::ProcessarComponenteCAN(sMAG_MotorDT, {CanMessagePosicaoDados::Dados1}, todosIDs, id_num, enviarStatus, todosDados, posicao, EnviarDadosCAN);
|
||||||
|
ComponenteCAN::ProcessarComponenteCAN(sMAG_MotorEF, {CanMessagePosicaoDados::Dados1}, todosIDs, id_num, enviarStatus, todosDados, posicao, EnviarDadosCAN);
|
||||||
|
ComponenteCAN::ProcessarComponenteCAN(sMAG_MotorDF, {CanMessagePosicaoDados::Dados1}, todosIDs, id_num, enviarStatus, todosDados, posicao, EnviarDadosCAN);
|
||||||
|
|
||||||
|
if (enviarStatus) {
|
||||||
|
qtdEnviosStatus = 0;
|
||||||
|
}
|
||||||
|
else {
|
||||||
|
qtdEnviosStatus++;
|
||||||
|
}
|
||||||
|
|
||||||
|
latenciaLoop = millis() - startLoop;
|
||||||
|
|
||||||
|
if (posicao == CanMessagePosicaoDados::DadosAll) {
|
||||||
|
EnviarDadosCAN(canService.MontarFrameReqDadosFim(latenciaLoop));
|
||||||
|
}
|
||||||
|
|
||||||
|
MostrarLog("Ciclo concluido, latencia de loop: " + String(latenciaLoop));
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void EnviarDadosCAN(std::vector<uint8_t> data) {
|
||||||
|
if (data.size() > 2) {
|
||||||
|
canService.adicionarMensagemFila(data);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void ProcessarFrameCAN(CanMessagePosicaoDados posicao, std::vector<uint8_t> data) {
|
||||||
|
if (data.size() < 1) return;
|
||||||
|
|
||||||
|
F_Code funcao = canService.FuncaoPorPosicao(posicao);
|
||||||
|
|
||||||
|
switch (funcao) {
|
||||||
|
case F_Code::ReqTx: ProcessarReq(data); break;
|
||||||
|
case F_Code::CmdTx: ProcessarCmd(data); break;
|
||||||
|
case F_Code::CfgTx: ProcessarCfg(data); break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void ProcessarReq(std::vector<uint8_t> data) {
|
||||||
|
if (data.size() < 2) return;
|
||||||
|
CanMessagePosicaoDados posicao = (CanMessagePosicaoDados)data[0];
|
||||||
|
uint8_t id = data[1];
|
||||||
|
enviarDadosSensores(id, posicao);
|
||||||
|
}
|
||||||
|
|
||||||
|
void ProcessarCfg(std::vector<uint8_t> data) {
|
||||||
|
if (data.size() < 4) return;
|
||||||
|
std::vector<uint8_t> _chkRx;
|
||||||
|
CanMessagePosicaoDados posicao = (CanMessagePosicaoDados)data[0];
|
||||||
|
uint8_t id = data[1];
|
||||||
|
S_Code componente = DetectarComponente(id);
|
||||||
|
if (componente == S_Code::sMOD) {
|
||||||
|
switch (posicao) {
|
||||||
|
case CanMessagePosicaoDados::Config1: {
|
||||||
|
if (data.size() < 8) return;
|
||||||
|
bool conectar = data[2] == 1;
|
||||||
|
int taxa = data[3] * 10;
|
||||||
|
bool requisitar = data[4] == 1;
|
||||||
|
//loraService.AddressBase = data[5];
|
||||||
|
int pinoSDA = data[6];
|
||||||
|
int pinoSCL = data[7];
|
||||||
|
|
||||||
|
_TaxaAmostragem = taxa;
|
||||||
|
RequisitarDados = requisitar;
|
||||||
|
|
||||||
|
if (pinoSDA != PINO_INVALIDO && pinoSCL != PINO_INVALIDO) {
|
||||||
|
if (I2CService::DefinirPinos(pinoSDA, pinoSCL) || !I2CService::I2CIniciado) {
|
||||||
|
inicializarDependenciasI2C();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
vTaskDelay(pdMS_TO_TICKS(100));
|
||||||
|
|
||||||
|
Conectado = conectar;
|
||||||
|
_chkRx = canService.MontarFrameReqStatusMod(D_Code, Conectado, VERSION);
|
||||||
|
|
||||||
|
MostrarLog("Configuracao do modulo concluida");
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
EnviarDadosCAN(_chkRx);
|
||||||
|
}
|
||||||
|
|
||||||
|
void ProcessarCmd(std::vector<uint8_t> data) {
|
||||||
|
if (data.size() < 2) return;
|
||||||
|
CanMessagePosicaoDados posicao = (CanMessagePosicaoDados)data[0];
|
||||||
|
uint8_t id = data[1];
|
||||||
|
S_Code componente = DetectarComponente(id);
|
||||||
|
switch (componente) {
|
||||||
|
case sMOD: {
|
||||||
|
if (data.size() < 3) return;
|
||||||
|
switch (posicao) {
|
||||||
|
case CanMessagePosicaoDados::Command1: {
|
||||||
|
Estado estado = (Estado)data[2];
|
||||||
|
if (estado == Estado::Reset) {
|
||||||
|
delay(100);
|
||||||
|
esp_restart();
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
case sSRV: {
|
||||||
|
ServoAgro_v2* servo = ObterServoPorId(id);
|
||||||
|
if (servo == nullptr)
|
||||||
|
return;
|
||||||
|
servo->ProcessarComando(data);
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
case sRLE: {
|
||||||
|
Rele_v2* rele = ObterRelePorId(id);
|
||||||
|
if (rele == nullptr)
|
||||||
|
return;
|
||||||
|
rele->ProcessarComando(data);
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
case sLED: {
|
||||||
|
Sinaleiro_v2* sinaleiro = ObterSinaleiroPorId(id);
|
||||||
|
if (sinaleiro == nullptr)
|
||||||
|
return;
|
||||||
|
sinaleiro->ProcessarComando(data);
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
default:
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
S_Code DetectarComponente(uint8_t idNum) {
|
||||||
|
if (idNum == canService.ID_Num_sMOD) return S_Code::sMOD;
|
||||||
|
if (idNum == canService.ID_Num_sTOD) return S_Code::sTOD;
|
||||||
|
if (idNum == canService.ID_Num_sLRA) return S_Code::sLRA;
|
||||||
|
if (idNum >= 01 && idNum <= 10) return S_Code::sNTC;
|
||||||
|
if (idNum >= 11 && idNum <= 20) return S_Code::sCOR;
|
||||||
|
if (idNum >= 21 && idNum <= 30) return S_Code::sLED;
|
||||||
|
if (idNum >= 41 && idNum <= 50) return S_Code::sSRV;
|
||||||
|
if (idNum >= 81 && idNum <= 90) return S_Code::sRLE;
|
||||||
|
if (idNum >= 91 && idNum <= 100) return S_Code::sMAG;
|
||||||
|
return S_Code::sVZO;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
Loading…
Reference in New Issue