novo metodo de referenciamento
This commit is contained in:
Diego Freitas
parent
4b8a37f2e4
commit
dd63a55523
Binary file not shown.
Binary file not shown.
Binary file not shown.
|
|
@ -430,6 +430,13 @@ namespace AgroBase.Forms.Operacoes
|
|||
gridDados.Rows.Add("Vazão Instantanea", "", "mL/s");
|
||||
gridDados.Rows.Add("Vazão Media", "", "mL/s");
|
||||
gridDados.Rows.Add("Volume Vazado", "", "mL");
|
||||
gridDados.Rows.Add("", "", "");
|
||||
gridDados.Rows.Add("Roll", "", "");
|
||||
gridDados.Rows.Add("Pitch", "", "");
|
||||
gridDados.Rows.Add("Yaw", "", "");
|
||||
gridDados.Rows.Add("Temperatura", "", "");
|
||||
gridDados.Rows.Add("Pressao", "", "");
|
||||
gridDados.Rows.Add("Altitude", "", "");
|
||||
|
||||
AtualizarInformacoesGerais();
|
||||
}
|
||||
|
|
@ -442,6 +449,7 @@ namespace AgroBase.Forms.Operacoes
|
|||
var AB7V2 = _SensoresCorrente.FirstOrDefault(x => x.ID == "AB7V2");
|
||||
var AB12V = _SensoresCorrente.FirstOrDefault(x => x.ID == "AB12V");
|
||||
var _Sensoriamento = Variaveis.OperacaoEmAndamento.Sensoriamento;
|
||||
var _SensorIMU = Variaveis.OperacaoEmAndamento.DispSen.Dados.DadosLeitura.SensoresIMU.FirstOrDefault();
|
||||
|
||||
gridDados.Rows[0].Cells[1].Value = _Sensoriamento.TempoDecorrido.ToString("HH:mm:ss");
|
||||
gridDados.Rows[1].Cells[1].Value = _Sensoriamento.Movimentacao.TempoMovimento.ToString("HH:mm:ss");
|
||||
|
|
@ -465,6 +473,13 @@ namespace AgroBase.Forms.Operacoes
|
|||
gridDados.Rows[19].Cells[1].Value = _Sensoriamento.Atuador.VazaoInstantanea.ToString("0.00");
|
||||
gridDados.Rows[20].Cells[1].Value = _Sensoriamento.Atuador.VazaoMedia.ToString("0.00");
|
||||
gridDados.Rows[21].Cells[1].Value = _Sensoriamento.Atuador.VolumeVazaoMl.ToString("0.00");
|
||||
gridDados.Rows[22].Cells[1].Value = "";
|
||||
gridDados.Rows[23].Cells[1].Value = _SensorIMU.Roll.ToString("0.00");
|
||||
gridDados.Rows[24].Cells[1].Value = _SensorIMU.Pitch.ToString("0.00");
|
||||
gridDados.Rows[25].Cells[1].Value = _SensorIMU.Yaw.ToString("0.00");
|
||||
gridDados.Rows[26].Cells[1].Value = _SensorIMU.Temperatura.ToString("0.00");
|
||||
gridDados.Rows[27].Cells[1].Value = _SensorIMU.Pressao.ToString("0.00");
|
||||
gridDados.Rows[28].Cells[1].Value = _SensorIMU.Altitude.ToString("0.00");
|
||||
}
|
||||
|
||||
#endregion
|
||||
|
|
|
|||
|
|
@ -1224,7 +1224,6 @@ namespace AgroBase.Forms.Operacoes
|
|||
{
|
||||
var Log = LogsDirecional.Where(x => x.Modulo_ID == ID).FirstOrDefault(x => x.Momento == MomentoAtual);
|
||||
var LogSen = LogsSensoriamento.FirstOrDefault(x => x.Momento == MomentoAtual);
|
||||
var SensorPot = LogSen?.SensoresPotenciometro?.FirstOrDefault(x => x.ID.Contains(ID));
|
||||
|
||||
double Angulo = Log?.Angulo ?? 0;
|
||||
double Multiplicador = -1;
|
||||
|
|
@ -1259,11 +1258,6 @@ namespace AgroBase.Forms.Operacoes
|
|||
TextBox txtControlar = FuncoesGlobais.FindControlRecursive<TextBox>(tabDirecional, "txtDIR_Controlar_" + ID);
|
||||
txtControlar.Text = (Log?.Controlar ?? false) ? "Sim" : "Não";
|
||||
TextBox txtStatus = FuncoesGlobais.FindControlRecursive<TextBox>(tabDirecional, "txtDIR_Status_" + ID);
|
||||
txtStatus.Text = (SensorPot?.Status ?? Enums.StatusSensorPotenciometro.Indefinido).ToString();
|
||||
TextBox txtAgPot = FuncoesGlobais.FindControlRecursive<TextBox>(tabDirecional, "txtDIR_AgPot_" + ID);
|
||||
txtAgPot.Text = (SensorPot?.Angulo ?? -1).ToString("0.00");
|
||||
Label lblAgPot = FuncoesGlobais.FindControlRecursive<Label>(tabDirecional, "lblDIR_AgPot_" + ID);
|
||||
lblAgPot.ForeColor = (SensorPot?.Iniciado ?? false) ? Color.Green : Color.Red;
|
||||
}
|
||||
|
||||
private void AtualizarInformacoesMovimentacao()
|
||||
|
|
|
|||
|
|
@ -109,6 +109,7 @@
|
|||
sFRO = 22,
|
||||
sPOT = 23,
|
||||
sMOD = 24,
|
||||
sIMU = 25,
|
||||
}
|
||||
|
||||
public enum BotoesJoystick
|
||||
|
|
|
|||
|
|
@ -1389,7 +1389,7 @@ namespace AgroBase.Models.Modules
|
|||
|
||||
default:
|
||||
{
|
||||
Console.WriteLine($"[ATU] Resposta do {ID_Num} recebida. Parametros nao mapeados: {FuncoesGlobais.ConverterComandoBytesParaTexto(data)}");
|
||||
Console.WriteLine($"[ATU] Resposta do {ID_Num} na posicao {posicao.ToString()} recebida. Parametros nao mapeados: {FuncoesGlobais.ConverterComandoBytesParaTexto(data)}");
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -328,175 +328,49 @@ namespace AgroBase.Models.Modules
|
|||
await Task.Delay(1000);
|
||||
|
||||
//await MKS057DService.EnviarComandoControle(MKS057DService.MontarComandoSetZero(Addr));
|
||||
|
||||
for (int i = 0; i < 5; i++)
|
||||
{
|
||||
MKS057DCanService.SetZero(Addr);
|
||||
|
||||
if (FuncoesMatematicas.ValorEstaEntre(Angulo, 0, 1))
|
||||
{
|
||||
break;
|
||||
}
|
||||
|
||||
await Task.Delay(1000);
|
||||
}
|
||||
await SetZero(Addr);
|
||||
|
||||
bool Sucesso = false;
|
||||
bool EstadoInicial = IN1_Atuado;
|
||||
bool EstadoAnterior = EstadoInicial;
|
||||
|
||||
double _velocidadeAnterior = 0;
|
||||
int _anguloSP = 45;
|
||||
int _offset = 0;
|
||||
int tentativas = 0;
|
||||
double _anguloEntrada = 9999;
|
||||
double _anguloSaida = 9999;
|
||||
|
||||
const int MaxTentativas = 10;
|
||||
double AnguloEntrada = 9999;
|
||||
double AnguloSaida = 9999;
|
||||
|
||||
DateTime TimeoutGeral = DateTime.Now.AddMinutes(2); // Timeout geral para o processo.
|
||||
|
||||
double anguloMaxVisitado = 0;
|
||||
double anguloMinVisitado = 0;
|
||||
Sentido _sentido = SentidoReal == Sentido.Horario ? Sentido.Antihorario : Sentido.Horario;
|
||||
double _anguloSP = 180 * (_sentido == Sentido.Antihorario ? -1 : 1);
|
||||
|
||||
Sentido _sentido = Sentido.Horario;
|
||||
if (SentidoReal != Sentido.Parado)
|
||||
while (!Sucesso && TimeoutGeral > DateTime.Now)
|
||||
{
|
||||
_sentido = SentidoReal == Sentido.Horario ? Sentido.Antihorario : Sentido.Horario;
|
||||
_anguloSP *= _sentido == Sentido.Antihorario ? -1 : 1;
|
||||
}
|
||||
|
||||
while (!Sucesso && tentativas < MaxTentativas && TimeoutGeral > DateTime.Now)
|
||||
{
|
||||
// Se o angulo ja foi visitado, entao pode se mover mais rapido, senao, se move mais devagar
|
||||
int VelocidadeGiro = DefineVelocidadeReferenciamento(EstadoInicial, anguloMaxVisitado, anguloMinVisitado);
|
||||
int VelocidadeGiro = DefineVelocidadeReferenciamento();
|
||||
|
||||
//await MKS057DService.EnviarComandoControle(MKS057DService.MontarComandoMovimento(Addr, _angulo, VelocidadeGiro, _sentido));
|
||||
MKS057DCanService.Movimento(Addr, _anguloSP, VelocidadeGiro, _sentido);
|
||||
|
||||
DateTime Inicio = DateTime.Now;
|
||||
bool inverterGiro = false;
|
||||
while (!Sucesso && !inverterGiro && Inicio.AddSeconds(120) > DateTime.Now)
|
||||
if (IN1_Atuado != EstadoInicial && AnguloEntrada == 9999)
|
||||
{
|
||||
bool _ref = IN1_Atuado;
|
||||
AnguloEntrada = Angulo;
|
||||
MKS057DCanService.Movimento(Addr, Angulo, VelocidadeGiro, _sentido);
|
||||
await SetZero(Addr);
|
||||
_sentido = SentidoReal == Sentido.Horario ? Sentido.Antihorario : Sentido.Horario;
|
||||
_anguloSP = 180 * (_sentido == Sentido.Antihorario ? -1 : 1);
|
||||
}
|
||||
else if (IN1_Atuado == EstadoInicial && AnguloEntrada != 9999 && AnguloSaida == 9999)
|
||||
{
|
||||
AnguloSaida = Angulo;
|
||||
MKS057DCanService.Movimento(Addr, Angulo, VelocidadeGiro, _sentido);
|
||||
|
||||
// Comecou com o sensor ja atuado
|
||||
if (EstadoInicial)
|
||||
{
|
||||
// Quando houver alteracao na leitura do sensor
|
||||
if (_ref != EstadoAnterior)
|
||||
{
|
||||
// Se estava atuado e agora nao esta mais, e ainda nao definiu o angulo de entrada
|
||||
if (!_ref && _anguloEntrada == 9999)
|
||||
{
|
||||
_anguloEntrada = Angulo;
|
||||
inverterGiro = true;
|
||||
}
|
||||
// Se estava atuado e agora nao esta mais, e ja definiu o angulo de entrada, e ainda nao definiu o angulo de saida
|
||||
else if (!_ref && _anguloEntrada != 9999 && _anguloSaida == 9999)
|
||||
{
|
||||
_anguloSaida = Angulo;
|
||||
}
|
||||
}
|
||||
}
|
||||
// Comecou com o sensor nao atuado
|
||||
else
|
||||
{
|
||||
// Quando houver alteracao na leitura do sensor
|
||||
if (_ref != EstadoAnterior)
|
||||
{
|
||||
// Se nao estava atuado e agora esta, e ainda nao definiu o angulo de entrada
|
||||
if (_ref && _anguloEntrada == 9999)
|
||||
{
|
||||
_anguloEntrada = Angulo;
|
||||
}
|
||||
// Se estava atuado e agora nao esta mais, e ja definiu o angulo de entrada, e ainda nao definiu o angulo de saida
|
||||
else if (!_ref && _anguloEntrada != 9999 && _anguloSaida == 9999)
|
||||
{
|
||||
_anguloSaida = Angulo;
|
||||
inverterGiro = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
_sentido = SentidoReal == Sentido.Horario ? Sentido.Antihorario : Sentido.Horario;
|
||||
|
||||
// Realiza a verificacao de necessidade de reverter a rotacao do motor apenas se ainda nao encontrou o angulo de entrada do sensor
|
||||
if (_anguloEntrada == 9999)
|
||||
{
|
||||
inverterGiro = (_anguloSP > 0 && Angulo >= (_anguloSP - 0.5)) || (_anguloSP < 0 && Angulo <= (_anguloSP + 0.5));
|
||||
}
|
||||
_anguloSP = AnguloSaida / 2.0;
|
||||
|
||||
Sucesso = _anguloEntrada != 9999 && _anguloSaida != 9999;
|
||||
|
||||
EstadoAnterior = _ref;
|
||||
|
||||
//Console.WriteLine($"Ref {Addr}: Angulo SP = {_angulo}, Velocidade: {VelocidadeGiro}, Angulo Atual = {Angulo}, Sentido SP = {_sentido.ToString()}, Passou = {inverterGiro}, Sucesso = {Sucesso}, Entrada: {_ref}");
|
||||
|
||||
if (Sucesso)
|
||||
{
|
||||
//await MKS057DService.EnviarComandoControle(MKS057DService.MontarComandoParada(Addr));
|
||||
|
||||
//MKS057DCanService.Parar(Addr);
|
||||
//await Task.Delay(1500);
|
||||
|
||||
_sentido = _sentido == Sentido.Horario ? Sentido.Antihorario : Sentido.Horario;
|
||||
double _centro = CalcularCentroCompensado(_anguloEntrada, _anguloSaida, FolgaMecanica, _sentido);
|
||||
|
||||
//await MKS057DService.EnviarComandoControle(MKS057DService.MontarComandoMovimento(Addr, _centro, 36, Sentido.Horario));
|
||||
MKS057DCanService.Movimento(Addr, _centro, VelocidadeGiro, _sentido);
|
||||
await Task.Delay(2500);
|
||||
//await MKS057DService.EnviarComandoControle(MKS057DService.MontarComandoSetZero(Addr));
|
||||
|
||||
for (int i = 0; i < 5; i++)
|
||||
{
|
||||
MKS057DCanService.SetZero(Addr);
|
||||
|
||||
if (FuncoesMatematicas.ValorEstaEntre(Angulo, 0, 1))
|
||||
{
|
||||
break;
|
||||
}
|
||||
|
||||
await Task.Delay(1000);
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
//await Task.Delay(200); // Verificações rápidas.
|
||||
|
||||
VelocidadeGiro = DefineVelocidadeReferenciamento(EstadoInicial, anguloMaxVisitado, anguloMinVisitado);
|
||||
|
||||
if (
|
||||
//(_anguloAnterior == Angulo && RPM == 0) || // Verifica se o angulo nao foi alterado desde a ultima leitura, e o motor nao esta em movimento
|
||||
(_velocidadeAnterior != VelocidadeGiro) // Verifica se mudou a velocidade de giro do motor de acordo com o angulo atual
|
||||
)
|
||||
{
|
||||
//await MKS057DService.EnviarComandoControle(MKS057DService.MontarComandoMovimento(Addr, _angulo, VelocidadeGiro, _sentido));
|
||||
MKS057DCanService.Movimento(Addr, _anguloSP, VelocidadeGiro, _sentido);
|
||||
}
|
||||
|
||||
_velocidadeAnterior = VelocidadeGiro;
|
||||
if (Angulo > anguloMaxVisitado)
|
||||
{
|
||||
anguloMaxVisitado = Angulo;
|
||||
}
|
||||
else if (Angulo < anguloMinVisitado)
|
||||
{
|
||||
anguloMinVisitado = Angulo;
|
||||
}
|
||||
//await MKS057DService.EnviarComandoControle(MKS057DService.MontarComandoMovimento(Addr, _centro, 36, Sentido.Horario));
|
||||
MKS057DCanService.Movimento(Addr, _anguloSP, VelocidadeGiro, _sentido);
|
||||
await Task.Delay(2000);
|
||||
await SetZero(Addr);
|
||||
}
|
||||
|
||||
if (!Sucesso && inverterGiro)
|
||||
{
|
||||
//await MKS057DService.EnviarComandoControle(MKS057DService.MontarComandoParada(Addr));
|
||||
|
||||
//MKS057DCanService.Parar(Addr);
|
||||
//await Task.Delay(500);
|
||||
|
||||
_sentido = _sentido == Sentido.Horario ? Sentido.Antihorario : Sentido.Horario;
|
||||
_offset = Math.Min(100, _offset + 25); // Limita o offset máximo para evitar valores extremos.
|
||||
int mx = _sentido == Sentido.Antihorario ? -1 : 1;
|
||||
_anguloSP = (Math.Abs(_anguloSP) + _offset) * mx;
|
||||
tentativas++;
|
||||
}
|
||||
Sucesso = AnguloEntrada != 9999 && AnguloSaida != 9999 && FuncoesMatematicas.ValorEstaEntre(AnguloSaida, 0, 0.5);
|
||||
}
|
||||
|
||||
if (!Sucesso)
|
||||
|
|
@ -508,12 +382,9 @@ namespace AgroBase.Models.Modules
|
|||
MKS057DCanService.FinalizarReferenciamento();
|
||||
}
|
||||
|
||||
private int DefineVelocidadeReferenciamento(bool EstadoInicial, double anguloMaxVisitado, double anguloMinVisitado)
|
||||
private int DefineVelocidadeReferenciamento()
|
||||
{
|
||||
int VelocidadeGiro =
|
||||
EstadoInicial || IN1_Atuado ? 10 : // Se o sensor estiver atuado ou o fluxo tiver iniciado com ele ja atuado, entao deve se mover lentamente para melhor precisao dos angulos de entrada e saida
|
||||
Angulo > (anguloMaxVisitado + 5) || Angulo < (anguloMinVisitado - 5) ? 18 : // Se o angulo atual ainda nao foi visitado, deve se mover em velocidade media
|
||||
20; // Se o angulo ja foi visitado, deve se mover em uma velocidade maior
|
||||
int VelocidadeGiro = 10;
|
||||
return VelocidadeGiro;
|
||||
}
|
||||
|
||||
|
|
@ -533,5 +404,20 @@ namespace AgroBase.Models.Modules
|
|||
return centroCompensado;
|
||||
}
|
||||
|
||||
private async Task SetZero(byte Addr)
|
||||
{
|
||||
for (int i = 0; i < 5; i++)
|
||||
{
|
||||
MKS057DCanService.SetZero(Addr);
|
||||
|
||||
if (FuncoesMatematicas.ValorEstaEntre(Angulo, 0, 1))
|
||||
{
|
||||
break;
|
||||
}
|
||||
|
||||
await Task.Delay(1000);
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -364,6 +364,38 @@ namespace AgroBase.Models.Modules
|
|||
},
|
||||
}
|
||||
},
|
||||
new SensorModel()
|
||||
{
|
||||
ID = "GY91",
|
||||
Descricao = "Inclinometro",
|
||||
ID_Num = 20,
|
||||
Componente = S_Code.sIMU,
|
||||
Aferir = true,
|
||||
Funcoes = new List<FuncoesPinout>()
|
||||
{
|
||||
FuncoesPinout.I2C,
|
||||
},
|
||||
UnidadeMedida = "",
|
||||
Parametros = new List<SensorParametroModel>()
|
||||
{
|
||||
new SensorParametroModel()
|
||||
{
|
||||
Descricao = "Endereço MPU",
|
||||
Valor = "0x68",
|
||||
QtdBytes = 1,
|
||||
Escalar = false,
|
||||
Enviar = true
|
||||
},
|
||||
new SensorParametroModel()
|
||||
{
|
||||
Descricao = "Endereço BMP",
|
||||
Valor = "0x76",
|
||||
QtdBytes = 1,
|
||||
Escalar = false,
|
||||
Enviar = true
|
||||
},
|
||||
}
|
||||
},
|
||||
|
||||
/*new SensorModel()
|
||||
{
|
||||
|
|
@ -665,7 +697,7 @@ namespace AgroBase.Models.Modules
|
|||
var config2 = new List<byte>
|
||||
{
|
||||
Variaveis.ID_Num_sMOD,
|
||||
(byte)CanMessagePosicaoDados.Config1,
|
||||
(byte)CanMessagePosicaoDados.Config2,
|
||||
(byte)S_Code.sMOD,
|
||||
};
|
||||
config2.AddRange(PinoutModel.PinoProtocolo(Pinout, Funcoes, false));
|
||||
|
|
@ -955,8 +987,8 @@ namespace AgroBase.Models.Modules
|
|||
case S_Code.sMAS:
|
||||
_leitura = Variaveis.OperacaoEmAndamento.DispAtu?.Dados?.DadosLeitura?.SensoresMassa?.FirstOrDefault(x => x.ID == ID);
|
||||
break;
|
||||
case S_Code.sPOT:
|
||||
_leitura = Variaveis.OperacaoEmAndamento.DispSen?.Dados?.DadosLeitura?.SensoresPotenciometro?.FirstOrDefault(x => x.ID == ID);
|
||||
case S_Code.sIMU:
|
||||
_leitura = Variaveis.OperacaoEmAndamento.DispSen?.Dados?.DadosLeitura?.SensoresIMU?.FirstOrDefault(x => x.ID == ID);
|
||||
break;
|
||||
default:
|
||||
_leitura = null;
|
||||
|
|
@ -1697,6 +1729,27 @@ namespace AgroBase.Models.Modules
|
|||
}
|
||||
}
|
||||
|
||||
public class FirmwareSensorIMU : IFirmwareSensorBase
|
||||
{
|
||||
public string ID { get; set; }
|
||||
public bool Iniciado { get; set; }
|
||||
public double AccX { get; set; }
|
||||
public double AccY { get; set; }
|
||||
public double AccZ { get; set; }
|
||||
public double GyrX { get; set; }
|
||||
public double GyrY { get; set; }
|
||||
public double GyrZ { get; set; }
|
||||
public double MagX { get; set; }
|
||||
public double MagY { get; set; }
|
||||
public double MagZ { get; set; }
|
||||
public double Temperatura { get; set; }
|
||||
public double Pressao { get; set; }
|
||||
public double Altitude { get; set; }
|
||||
public double Roll { get; set; }
|
||||
public double Pitch { get; set; }
|
||||
public double Yaw { get; set; }
|
||||
}
|
||||
|
||||
|
||||
public class SensoriamentoMensagemJsonModel
|
||||
{
|
||||
|
|
@ -1712,7 +1765,7 @@ namespace AgroBase.Models.Modules
|
|||
public List<FirmwareSinaleiro> Sinaleiros { get; set; } = new List<FirmwareSinaleiro>();
|
||||
public List<FirmwareRele> Reles { get; set; } = new List<FirmwareRele>();
|
||||
public List<FirmwareServoFreio> ServoFreios { get; set; } = new List<FirmwareServoFreio>();
|
||||
public List<FirmwareSensorPotenciometro> SensoresPotenciometro { get; set; } = new List<FirmwareSensorPotenciometro>();
|
||||
public List<FirmwareSensorIMU> SensoresIMU { get; set; } = new List<FirmwareSensorIMU>();
|
||||
public DateTime UltimoComandoRespondido { get; set; }
|
||||
}
|
||||
|
||||
|
|
@ -1740,7 +1793,7 @@ namespace AgroBase.Models.Modules
|
|||
return sensor?.Componente ?? rele?.Componente ?? sinaleiro?.Componente ?? servo?.Componente ?? S_Code.sVZO;
|
||||
}
|
||||
|
||||
private void DefinirOuCriar(int CompID_Num, S_Code componente, bool Iniciado, List<dynamic> Parametros)
|
||||
private void DefinirOuCriar(int CompID_Num, CanMessagePosicaoDados Posicao, S_Code componente, List<dynamic> Parametros)
|
||||
{
|
||||
var Disp = Variaveis.OperacaoEmAndamento.DispSen;
|
||||
var item = Disp.Dados.DadosLeitura;
|
||||
|
|
@ -1756,15 +1809,26 @@ namespace AgroBase.Models.Modules
|
|||
_sensor = new FirmwareSensorTemperatura()
|
||||
{
|
||||
ID = sensor.ID,
|
||||
Iniciado = Iniciado,
|
||||
Iniciado = false,
|
||||
Temperatura = -1,
|
||||
};
|
||||
item.SensoresTemperatura.Add(_sensor);
|
||||
}
|
||||
if (_sensor != null)
|
||||
{
|
||||
_sensor.Iniciado = Iniciado;
|
||||
_sensor.Temperatura = Parametros.Count > 0 ? Parametros[0] : -1;
|
||||
switch (Posicao)
|
||||
{
|
||||
case CanMessagePosicaoDados.Status:
|
||||
{
|
||||
_sensor.Iniciado = Parametros.Count > 0 ? Parametros[0] : false;
|
||||
break;
|
||||
}
|
||||
case CanMessagePosicaoDados.Dados1:
|
||||
{
|
||||
_sensor.Temperatura = Parametros.Count > 0 ? Parametros[0] : -1;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
|
@ -1777,7 +1841,7 @@ namespace AgroBase.Models.Modules
|
|||
_sensor = new FirmwareSensorCorrente()
|
||||
{
|
||||
ID = sensor.ID,
|
||||
Iniciado = Iniciado,
|
||||
Iniciado = false,
|
||||
BusVoltage = -1,
|
||||
Current = -1,
|
||||
Power = -1,
|
||||
|
|
@ -1787,10 +1851,21 @@ namespace AgroBase.Models.Modules
|
|||
}
|
||||
if (_sensor != null)
|
||||
{
|
||||
_sensor.Iniciado = Iniciado;
|
||||
_sensor.BusVoltage = Parametros.Count > 0 ? Parametros[0] : -1;
|
||||
_sensor.Current = Parametros.Count > 1 ? Parametros[1] : -1;
|
||||
_sensor.Power = Parametros.Count > 2 ? Parametros[2] : -1;
|
||||
switch (Posicao)
|
||||
{
|
||||
case CanMessagePosicaoDados.Status:
|
||||
{
|
||||
_sensor.Iniciado = Parametros.Count > 0 ? Parametros[0] : false;
|
||||
break;
|
||||
}
|
||||
case CanMessagePosicaoDados.Dados1:
|
||||
{
|
||||
_sensor.BusVoltage = Parametros.Count > 0 ? Parametros[0] : -1;
|
||||
_sensor.Current = Parametros.Count > 1 ? Parametros[1] : -1;
|
||||
_sensor.Power = Parametros.Count > 2 ? Parametros[2] : -1;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
|
@ -1803,7 +1878,7 @@ namespace AgroBase.Models.Modules
|
|||
_sensor = new FirmwareSinaleiro()
|
||||
{
|
||||
ID = sensor.ID,
|
||||
Iniciado = Iniciado,
|
||||
Iniciado = false,
|
||||
Comando = StatusLED.Apagado,
|
||||
Leitura = StatusLED.Apagado,
|
||||
};
|
||||
|
|
@ -1811,9 +1886,20 @@ namespace AgroBase.Models.Modules
|
|||
}
|
||||
if (_sensor != null)
|
||||
{
|
||||
_sensor.Iniciado = Iniciado;
|
||||
_sensor.Leitura = Parametros.Count > 0 ? Parametros[0] : StatusLED.Apagado;
|
||||
_sensor.Comando = Parametros.Count > 1 ? Parametros[1] : StatusLED.Apagado;
|
||||
switch (Posicao)
|
||||
{
|
||||
case CanMessagePosicaoDados.Status:
|
||||
{
|
||||
_sensor.Iniciado = Parametros.Count > 0 ? Parametros[0] : false;
|
||||
break;
|
||||
}
|
||||
case CanMessagePosicaoDados.Dados1:
|
||||
{
|
||||
_sensor.Leitura = Parametros.Count > 0 ? Parametros[0] : StatusLED.Apagado;
|
||||
_sensor.Comando = Parametros.Count > 1 ? Parametros[1] : StatusLED.Apagado;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
|
@ -1826,15 +1912,26 @@ namespace AgroBase.Models.Modules
|
|||
_sensor = new FirmwareRele()
|
||||
{
|
||||
ID = sensor.ID,
|
||||
Iniciado = Iniciado,
|
||||
Iniciado = false,
|
||||
Leitura = Estado.Desligado,
|
||||
};
|
||||
item.Reles.Add(_sensor);
|
||||
}
|
||||
if (_sensor != null)
|
||||
{
|
||||
_sensor.Iniciado = Iniciado;
|
||||
_sensor.Leitura = Parametros.Count > 0 ? Parametros[0] : Estado.Desligado;
|
||||
switch (Posicao)
|
||||
{
|
||||
case CanMessagePosicaoDados.Status:
|
||||
{
|
||||
_sensor.Iniciado = Parametros.Count > 0 ? Parametros[0] : false;
|
||||
break;
|
||||
}
|
||||
case CanMessagePosicaoDados.Dados1:
|
||||
{
|
||||
_sensor.Leitura = Parametros.Count > 0 ? Parametros[0] : Estado.Desligado;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
|
@ -1847,18 +1944,106 @@ namespace AgroBase.Models.Modules
|
|||
_sensor = new FirmwareServoFreio()
|
||||
{
|
||||
ID = sensor.ID,
|
||||
Iniciado = Iniciado,
|
||||
Iniciado = false,
|
||||
Angulo = -1,
|
||||
};
|
||||
item.ServoFreios.Add(_sensor);
|
||||
}
|
||||
if (_sensor != null)
|
||||
{
|
||||
_sensor.Iniciado = Iniciado;
|
||||
_sensor.Angulo = Parametros.Count > 0 ? Parametros[0] : -1;
|
||||
switch (Posicao)
|
||||
{
|
||||
case CanMessagePosicaoDados.Status:
|
||||
{
|
||||
_sensor.Iniciado = Parametros.Count > 0 ? Parametros[0] : false;
|
||||
break;
|
||||
}
|
||||
case CanMessagePosicaoDados.Dados1:
|
||||
{
|
||||
_sensor.Angulo = Parametros.Count > 0 ? Parametros[0] : -1;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
break;
|
||||
}
|
||||
case S_Code.sIMU:
|
||||
{
|
||||
var sensor = Disp.Dados.Sensores.FirstOrDefault(x => x.ID_Num == CompID_Num);
|
||||
var _sensor = item.SensoresIMU.FirstOrDefault(x => x.ID == sensor.ID);
|
||||
if (_sensor == null)
|
||||
{
|
||||
_sensor = new FirmwareSensorIMU()
|
||||
{
|
||||
ID = sensor.ID,
|
||||
Iniciado = false,
|
||||
AccX = -1,
|
||||
AccY = -1,
|
||||
AccZ = -1,
|
||||
GyrX = -1,
|
||||
GyrY = -1,
|
||||
GyrZ = -1,
|
||||
MagX = -1,
|
||||
MagY = -1,
|
||||
MagZ = -1,
|
||||
Altitude = -1,
|
||||
Temperatura = -1,
|
||||
Pressao = -1,
|
||||
Pitch = -1,
|
||||
Roll = -1,
|
||||
Yaw = -1,
|
||||
};
|
||||
item.SensoresIMU.Add(_sensor);
|
||||
}
|
||||
if (_sensor != null)
|
||||
{
|
||||
switch (Posicao)
|
||||
{
|
||||
case CanMessagePosicaoDados.Status:
|
||||
{
|
||||
_sensor.Iniciado = Parametros.Count > 0 ? Parametros[0] : false;
|
||||
break;
|
||||
}
|
||||
case CanMessagePosicaoDados.Dados1:
|
||||
{
|
||||
_sensor.Roll = Parametros.Count > 0 ? Parametros[0] : -1;
|
||||
_sensor.Pitch = Parametros.Count > 1 ? Parametros[1] : -1;
|
||||
_sensor.Yaw = Parametros.Count > 2 ? Parametros[2] : -1;
|
||||
break;
|
||||
}
|
||||
case CanMessagePosicaoDados.Dados2:
|
||||
{
|
||||
_sensor.Temperatura = Parametros.Count > 0 ? Parametros[0] : -1;
|
||||
_sensor.Pressao = Parametros.Count > 1 ? Parametros[1] : -1;
|
||||
_sensor.Altitude = Parametros.Count > 2 ? Parametros[2] : -1;
|
||||
break;
|
||||
}
|
||||
case CanMessagePosicaoDados.Dados3:
|
||||
{
|
||||
_sensor.AccX = Parametros.Count > 0 ? Parametros[0] : -1;
|
||||
_sensor.AccY = Parametros.Count > 1 ? Parametros[1] : -1;
|
||||
_sensor.AccZ = Parametros.Count > 2 ? Parametros[2] : -1;
|
||||
break;
|
||||
}
|
||||
case CanMessagePosicaoDados.Dados4:
|
||||
{
|
||||
_sensor.GyrX = Parametros.Count > 0 ? Parametros[0] : -1;
|
||||
_sensor.GyrY = Parametros.Count > 1 ? Parametros[1] : -1;
|
||||
_sensor.GyrZ = Parametros.Count > 2 ? Parametros[2] : -1;
|
||||
break;
|
||||
}
|
||||
case CanMessagePosicaoDados.Dados5:
|
||||
{
|
||||
_sensor.MagX = Parametros.Count > 0 ? Parametros[0] : -1;
|
||||
_sensor.MagY = Parametros.Count > 1 ? Parametros[1] : -1;
|
||||
_sensor.MagZ = Parametros.Count > 2 ? Parametros[2] : -1;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -1873,7 +2058,7 @@ namespace AgroBase.Models.Modules
|
|||
|
||||
S_Code componente = DetectarComponenteDoID_Num(ID_Num);
|
||||
|
||||
//Console.WriteLine($"[SEN] Componente={componente.ToString()}, ID_Num={CompID_Num}, Data={FuncoesGlobais.ConverterComandoBytesParaTexto(data)}");
|
||||
Console.WriteLine($"[SEN] Componente={componente.ToString()}, ID_Num={ID_Num}, Posicao={posicao.ToString()}, Data={FuncoesGlobais.ConverterComandoBytesParaTexto(data)}");
|
||||
|
||||
switch (posicao)
|
||||
{
|
||||
|
|
@ -1894,7 +2079,7 @@ namespace AgroBase.Models.Modules
|
|||
else
|
||||
{
|
||||
bool Iniciado = data[2] == 1;
|
||||
DefinirOuCriar(ID_Num, componente, Iniciado, new List<dynamic>());
|
||||
DefinirOuCriar(ID_Num, posicao, componente, new List<dynamic>() { Iniciado });
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
|
@ -1905,7 +2090,7 @@ namespace AgroBase.Models.Modules
|
|||
case S_Code.sTMP:
|
||||
{
|
||||
double temperatura = ConverterByteParaDouble(data, 2);
|
||||
DefinirOuCriar(ID_Num, componente, true, new List<dynamic>() { temperatura });
|
||||
DefinirOuCriar(ID_Num, posicao, componente, new List<dynamic>() { temperatura });
|
||||
break;
|
||||
}
|
||||
case S_Code.sCOR:
|
||||
|
|
@ -1913,32 +2098,128 @@ namespace AgroBase.Models.Modules
|
|||
double busVoltage = ConverterByteParaDouble(data, 2);
|
||||
double current = ConverterByteParaDouble(data, 4);
|
||||
double power = ConverterByteParaDouble(data, 6);
|
||||
DefinirOuCriar(ID_Num, componente, true, new List<dynamic>() { busVoltage, current, power });
|
||||
DefinirOuCriar(ID_Num, posicao, componente, new List<dynamic>() { busVoltage, current, power });
|
||||
break;
|
||||
}
|
||||
case S_Code.sLED:
|
||||
{
|
||||
StatusLED statusLeitura = (StatusLED)data[2];
|
||||
StatusLED statusComando = (StatusLED)data[3];
|
||||
DefinirOuCriar(ID_Num, componente, true, new List<dynamic>() { statusLeitura, statusComando });
|
||||
DefinirOuCriar(ID_Num, posicao, componente, new List<dynamic>() { statusLeitura, statusComando });
|
||||
break;
|
||||
}
|
||||
case S_Code.sRLE:
|
||||
{
|
||||
Estado estado = (Estado)data[2];
|
||||
DefinirOuCriar(ID_Num, componente, true, new List<dynamic>() { estado });
|
||||
DefinirOuCriar(ID_Num, posicao, componente, new List<dynamic>() { estado });
|
||||
break;
|
||||
}
|
||||
case S_Code.sFRO:
|
||||
{
|
||||
int angulo = data[2];
|
||||
DefinirOuCriar(ID_Num, componente, true, new List<dynamic>() { angulo });
|
||||
DefinirOuCriar(ID_Num, posicao, componente, new List<dynamic>() { angulo });
|
||||
break;
|
||||
}
|
||||
case S_Code.sIMU:
|
||||
{
|
||||
double roll = ConverterByteParaDouble(data, 2);
|
||||
double pitch = ConverterByteParaDouble(data, 4);
|
||||
double yaw = ConverterByteParaDouble(data, 6);
|
||||
DefinirOuCriar(ID_Num, posicao, componente, new List<dynamic>() { roll, pitch, yaw });
|
||||
break;
|
||||
}
|
||||
|
||||
default:
|
||||
{
|
||||
Console.WriteLine($"[SEN] Resposta de {ID_Num} recebida. Parametros nao mapeados: {FuncoesGlobais.ConverterComandoBytesParaTexto(data)}");
|
||||
Console.WriteLine($"[SEN] Resposta de {ID_Num} na posicao {posicao.ToString()} recebida. Parametros nao mapeados: {FuncoesGlobais.ConverterComandoBytesParaTexto(data)}");
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
case CanMessagePosicaoDados.Dados2:
|
||||
{
|
||||
switch (componente)
|
||||
{
|
||||
case S_Code.sIMU:
|
||||
{
|
||||
double temperatura = ConverterByteParaDouble(data, 2);
|
||||
double pressao = ConverterByteParaDouble(data, 4);
|
||||
double altitude = ConverterByteParaDouble(data, 6);
|
||||
DefinirOuCriar(ID_Num, posicao, componente, new List<dynamic>() { temperatura, pressao, altitude });
|
||||
break;
|
||||
}
|
||||
|
||||
default:
|
||||
{
|
||||
Console.WriteLine($"[SEN] Resposta de {ID_Num} na posicao {posicao.ToString()} recebida. Parametros nao mapeados: {FuncoesGlobais.ConverterComandoBytesParaTexto(data)}");
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
case CanMessagePosicaoDados.Dados3:
|
||||
{
|
||||
switch (componente)
|
||||
{
|
||||
case S_Code.sIMU:
|
||||
{
|
||||
double accX = ConverterByteParaDouble(data, 2);
|
||||
double accY = ConverterByteParaDouble(data, 4);
|
||||
double accZ = ConverterByteParaDouble(data, 6);
|
||||
DefinirOuCriar(ID_Num, posicao, componente, new List<dynamic>() { accX, accY, accZ});
|
||||
break;
|
||||
}
|
||||
|
||||
default:
|
||||
{
|
||||
Console.WriteLine($"[SEN] Resposta de {ID_Num} na posicao {posicao.ToString()} recebida. Parametros nao mapeados: {FuncoesGlobais.ConverterComandoBytesParaTexto(data)}");
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
case CanMessagePosicaoDados.Dados4:
|
||||
{
|
||||
switch (componente)
|
||||
{
|
||||
case S_Code.sIMU:
|
||||
{
|
||||
double gyrX = ConverterByteParaDouble(data, 2);
|
||||
double gyrY = ConverterByteParaDouble(data, 4);
|
||||
double gyrZ = ConverterByteParaDouble(data, 6);
|
||||
DefinirOuCriar(ID_Num, posicao, componente, new List<dynamic>() { gyrX, gyrY, gyrZ });
|
||||
break;
|
||||
}
|
||||
|
||||
default:
|
||||
{
|
||||
Console.WriteLine($"[SEN] Resposta de {ID_Num} na posicao {posicao.ToString()} recebida. Parametros nao mapeados: {FuncoesGlobais.ConverterComandoBytesParaTexto(data)}");
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
case CanMessagePosicaoDados.Dados5:
|
||||
{
|
||||
switch (componente)
|
||||
{
|
||||
case S_Code.sIMU:
|
||||
{
|
||||
double magX = ConverterByteParaDouble(data, 2);
|
||||
double magY = ConverterByteParaDouble(data, 4);
|
||||
double magZ = ConverterByteParaDouble(data, 6);
|
||||
DefinirOuCriar(ID_Num, posicao, componente, new List<dynamic>() { magX, magY, magZ });
|
||||
break;
|
||||
}
|
||||
|
||||
default:
|
||||
{
|
||||
Console.WriteLine($"[SEN] Resposta de {ID_Num} na posicao {posicao.ToString()} recebida. Parametros nao mapeados: {FuncoesGlobais.ConverterComandoBytesParaTexto(data)}");
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -1968,7 +1968,7 @@ namespace AgroBase.Models
|
|||
SensoresTemperatura = new List<FirmwareSensorTemperatura>(senLeitura.SensoresTemperatura),
|
||||
ServoFreios = new List<FirmwareServoFreio>(senLeitura.ServoFreios),
|
||||
Sinaleiros = new List<FirmwareSinaleiro>(senLeitura.Sinaleiros),
|
||||
SensoresPotenciometro = new List<FirmwareSensorPotenciometro>(senLeitura.SensoresPotenciometro),
|
||||
SensoresIMU = new List<FirmwareSensorIMU>(senLeitura.SensoresIMU),
|
||||
UltimoComandoRespondido = senLeitura.Momento,
|
||||
};
|
||||
SalvarLog(T_Code.Sen.ToString(), logSen, false);
|
||||
|
|
|
|||
Binary file not shown.
File diff suppressed because one or more lines are too long
|
|
@ -25,7 +25,7 @@
|
|||
<meta name="viewport" content="width=device-width,
|
||||
initial-scale=1.0, maximum-scale=1.0, user-scalable=no" />
|
||||
<style>
|
||||
#map_beecccdcf5c786b58b81527b87a76393 {
|
||||
#map_9732a3a481e157ff8ba61a30462a8ce8 {
|
||||
position: relative;
|
||||
width: 100.0%;
|
||||
height: 100.0%;
|
||||
|
|
@ -39,14 +39,14 @@
|
|||
<body>
|
||||
|
||||
|
||||
<div class="folium-map" id="map_beecccdcf5c786b58b81527b87a76393" ></div>
|
||||
<div class="folium-map" id="map_9732a3a481e157ff8ba61a30462a8ce8" ></div>
|
||||
|
||||
</body>
|
||||
<script>
|
||||
|
||||
|
||||
var map_beecccdcf5c786b58b81527b87a76393 = L.map(
|
||||
"map_beecccdcf5c786b58b81527b87a76393",
|
||||
var map_9732a3a481e157ff8ba61a30462a8ce8 = L.map(
|
||||
"map_9732a3a481e157ff8ba61a30462a8ce8",
|
||||
{
|
||||
center: [0.0, 0.0],
|
||||
crs: L.CRS.EPSG3857,
|
||||
|
|
@ -60,13 +60,13 @@
|
|||
|
||||
|
||||
|
||||
var tile_layer_78c150513a8dfbf34ae6dc2bceb87edd = L.tileLayer(
|
||||
var tile_layer_b28cc1434b76a05db69db0ade9dd15a6 = L.tileLayer(
|
||||
"https://tile.openstreetmap.org/{z}/{x}/{y}.png",
|
||||
{"attribution": "\u0026copy; \u003ca href=\"https://www.openstreetmap.org/copyright\"\u003eOpenStreetMap\u003c/a\u003e contributors", "detectRetina": false, "maxNativeZoom": 19, "maxZoom": 19, "minZoom": 0, "noWrap": false, "opacity": 1, "subdomains": "abc", "tms": false}
|
||||
);
|
||||
|
||||
|
||||
tile_layer_78c150513a8dfbf34ae6dc2bceb87edd.addTo(map_beecccdcf5c786b58b81527b87a76393);
|
||||
tile_layer_b28cc1434b76a05db69db0ade9dd15a6.addTo(map_9732a3a481e157ff8ba61a30462a8ce8);
|
||||
|
||||
</script>
|
||||
|
||||
|
|
@ -87,7 +87,7 @@
|
|||
}
|
||||
trajeto_json_add({"features": []});
|
||||
|
||||
trajeto_json.addTo(map_beecccdcf5c786b58b81527b87a76393);
|
||||
trajeto_json.addTo(map_9732a3a481e157ff8ba61a30462a8ce8);
|
||||
|
||||
function adicionarGeometria(novaGeometria) {
|
||||
trajeto_json.addData(novaGeometria);
|
||||
|
|
@ -145,7 +145,7 @@
|
|||
|
||||
var marcadorDinamico = L.marker([0, 0], {
|
||||
icon: customIcon
|
||||
}).addTo(map_beecccdcf5c786b58b81527b87a76393);
|
||||
}).addTo(map_9732a3a481e157ff8ba61a30462a8ce8);
|
||||
|
||||
// Conectar ao broker MQTT
|
||||
const client = mqtt.connect('ws://localhost:9001'); // Use wss para conexão segura
|
||||
|
|
@ -183,7 +183,7 @@
|
|||
marcadorDinamico.setRotationAngle(angulo);
|
||||
|
||||
adicionarCoordenada("Tj", [novaLongitude, novaLatitude]);
|
||||
map_beecccdcf5c786b58b81527b87a76393.setView(novaPosicao, map_beecccdcf5c786b58b81527b87a76393.getZoom());
|
||||
map_9732a3a481e157ff8ba61a30462a8ce8.setView(novaPosicao, map_9732a3a481e157ff8ba61a30462a8ce8.getZoom());
|
||||
});
|
||||
|
||||
function calcularOrientacao(P1latitude, P1longitude, P2latitude, P2longitude) {
|
||||
|
|
|
|||
Binary file not shown.
|
|
@ -482,4 +482,4 @@ C:\ZendionInc\agrobot_base\AgroBase\AgroBase\obj\Debug\AgroBase.csproj.GenerateR
|
|||
C:\ZendionInc\agrobot_base\AgroBase\AgroBase\obj\Debug\AgroBase.csproj.CoreCompileInputs.cache
|
||||
C:\ZendionInc\agrobot_base\AgroBase\AgroBase\obj\Debug\AgroBase.exe
|
||||
C:\ZendionInc\agrobot_base\AgroBase\AgroBase\obj\Debug\AgroBase.pdb
|
||||
C:\ZendionINC\agrobot_base\AgroBase\AgroBase\obj\Debug\AgroBase.csproj.Up2Date
|
||||
C:\ZendionInc\agrobot_base\AgroBase\AgroBase\obj\Debug\AgroBase.csproj.CopyComplete
|
||||
|
|
|
|||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
|
|
@ -65,6 +65,7 @@ enum S_Code {
|
|||
sFRO = 22,
|
||||
sPOT = 23,
|
||||
sMOD = 24,
|
||||
sIMU = 25,
|
||||
};
|
||||
|
||||
enum Estado {
|
||||
|
|
|
|||
|
|
@ -140,19 +140,26 @@ std::vector<uint8_t> SensorCorrente::ConfigurarSensor(std::vector<SensorCorrente
|
|||
uint16_t aMaxInt = ((uint16_t)data[6] << 8) | data[7];
|
||||
float aMax = aMaxInt / 100.0;
|
||||
auto it = std::find_if(lista.begin(), lista.end(), [idNum](SensorCorrente* s) { return s->ID_Num == idNum; });
|
||||
bool existente = (it != lista.end());
|
||||
if (conectar) {
|
||||
if (it == lista.end()) {
|
||||
SensorCorrente* sensor = new SensorCorrente(Mod_ID, "sCOR_" + String(idNum));
|
||||
sensor->ID_Num = idNum;
|
||||
sensor->_Endereco = endereco;
|
||||
sensor->_A_Shunt = aMax;
|
||||
sensor->Inicializar();
|
||||
SensorCorrente* sensor;
|
||||
if (!existente) {
|
||||
sensor = new SensorCorrente(Mod_ID, "sCOR_" + String(idNum));
|
||||
}
|
||||
else {
|
||||
sensor = *it;
|
||||
}
|
||||
sensor->ID_Num = idNum;
|
||||
sensor->_Endereco = endereco;
|
||||
sensor->_A_Shunt = aMax;
|
||||
sensor->Inicializar();
|
||||
if (!existente) {
|
||||
lista.push_back(sensor);
|
||||
PrintTela("Sensor de corrente adicionado via CAN: sCOR_" + String(idNum));
|
||||
status = sensor->MontarMensagemCAN(CanMessagePosicaoDados::Status);
|
||||
}
|
||||
status = sensor->MontarMensagemCAN(CanMessagePosicaoDados::Status);
|
||||
} else {
|
||||
if (it != lista.end()) {
|
||||
if (existente) {
|
||||
SensorCorrente* sensor = *it;
|
||||
sensor->Desligar();
|
||||
status = sensor->MontarMensagemCAN(CanMessagePosicaoDados::Status);
|
||||
|
|
|
|||
|
|
@ -3,27 +3,33 @@
|
|||
|
||||
#include <MPU9250_WE.h>
|
||||
#include <Adafruit_BMP280.h>
|
||||
#include <MadgwickAHRS.h>
|
||||
#include <Wire.h>
|
||||
#include "SerialService.h"
|
||||
#include "Pinout.h"
|
||||
|
||||
class SensorIMU {
|
||||
public:
|
||||
|
||||
static std::vector<uint8_t> ConfigurarSensor(std::vector<SensorIMU*>& lista, std::vector<uint8_t>& data, const String& Mod_ID);
|
||||
|
||||
String Mod_ID;
|
||||
String _ID;
|
||||
int ID_Num;
|
||||
bool Iniciado = false;
|
||||
|
||||
// Endereços I2C
|
||||
#define MPU_ADDR 0x68
|
||||
#define BMP_ADDR 0x76
|
||||
byte _EnderecoMPU; // 0x68
|
||||
byte _EnderecoBMP; // 0x76
|
||||
|
||||
// MPU9250 (IMU 9 eixos)
|
||||
MPU9250_WE mpu = MPU9250_WE(MPU_ADDR);
|
||||
MPU9250_WE* mpu;
|
||||
|
||||
// BMP280 (Temperatura, Pressão, Altitude)
|
||||
Adafruit_BMP280 bmp;
|
||||
|
||||
Madgwick filter;
|
||||
|
||||
// Leituras principais
|
||||
float AccX = 0, AccY = 0, AccZ = 0;
|
||||
float GyroX = 0, GyroY = 0, GyroZ = 0;
|
||||
|
|
@ -31,6 +37,9 @@ class SensorIMU {
|
|||
float Temp = 0;
|
||||
float Pressao = 0;
|
||||
float Altitude = 0;
|
||||
float Roll = 0;
|
||||
float Pitch = 0;
|
||||
float Yaw = 0;
|
||||
|
||||
SensorIMU(String _modID, String _id) {
|
||||
Mod_ID = _modID;
|
||||
|
|
@ -43,20 +52,21 @@ class SensorIMU {
|
|||
return;
|
||||
}
|
||||
|
||||
if (!mpu.init()) {
|
||||
PrintTela("MPU9250 nao encontrado no endereco " + String(MPU_ADDR));
|
||||
mpu = new MPU9250_WE(_EnderecoMPU);
|
||||
if (!mpu->init()) {
|
||||
PrintTela("MPU9250 nao encontrado no endereco " + String(_EnderecoMPU));
|
||||
Iniciado = false;
|
||||
return;
|
||||
//return;
|
||||
}
|
||||
|
||||
mpu.autoOffsets();
|
||||
mpu.setSampleRateDivider(5);
|
||||
mpu.setAccRange(MPU9250_ACC_RANGE_2G);
|
||||
mpu.enableAccDLPF(true);
|
||||
mpu.setAccDLPF(MPU9250_DLPF_6);
|
||||
mpu->autoOffsets();
|
||||
mpu->setSampleRateDivider(5);
|
||||
mpu->setAccRange(MPU9250_ACC_RANGE_2G);
|
||||
mpu->enableAccDLPF(true);
|
||||
mpu->setAccDLPF(MPU9250_DLPF_6);
|
||||
|
||||
if (!bmp.begin(BMP_ADDR)) {
|
||||
PrintTela("BMP280 nao encontrado no endereco " + String(BMP_ADDR));
|
||||
if (!bmp.begin(_EnderecoBMP)) {
|
||||
PrintTela("BMP280 nao encontrado no endereco " + String(_EnderecoBMP));
|
||||
Iniciado = false;
|
||||
return;
|
||||
}
|
||||
|
|
@ -67,6 +77,10 @@ class SensorIMU {
|
|||
Adafruit_BMP280::FILTER_X16,
|
||||
Adafruit_BMP280::STANDBY_MS_500);
|
||||
|
||||
filter.begin(100); // 100 Hz
|
||||
|
||||
xTaskCreatePinnedToCore(&SensorIMU::IMUTaskWrapper, "IMUTask", 5000, this, 20, &IMUTaskHandle, tskNO_AFFINITY);
|
||||
|
||||
Iniciado = true;
|
||||
PrintTela(_ID + " iniciado");
|
||||
}
|
||||
|
|
@ -77,43 +91,124 @@ class SensorIMU {
|
|||
return;
|
||||
}
|
||||
|
||||
delete mpu;
|
||||
mpu = nullptr;
|
||||
|
||||
// Parar a execução das tarefas
|
||||
if (IMUTaskHandle != NULL) {
|
||||
vTaskDelete(IMUTaskHandle);
|
||||
IMUTaskHandle = NULL;
|
||||
}
|
||||
|
||||
PrintTela(_ID + " Desligado");
|
||||
|
||||
Iniciado = false;
|
||||
}
|
||||
|
||||
void RequisitarDados() {
|
||||
AferirDados();
|
||||
Serial.print("Temp: "); Serial.println(Temp);
|
||||
Serial.print("Pressao: "); Serial.println(Pressao);
|
||||
Serial.print("Altitude: "); Serial.println(Altitude);
|
||||
Serial.print("Roll: "); Serial.println(Roll);
|
||||
Serial.print("Pitch: "); Serial.println(Pitch);
|
||||
Serial.print("Yaw: "); Serial.println(Yaw);
|
||||
Serial.println();
|
||||
}
|
||||
|
||||
std::vector<uint8_t> MontarMensagemCAN() {
|
||||
std::vector<uint8_t> MontarMensagemCAN(CanMessagePosicaoDados posicao) {
|
||||
std::vector<uint8_t> data;
|
||||
data.push_back(ID_Num); // Byte 0 - ID
|
||||
data.push_back(Iniciado ? 1 : 0); // Byte 1 - Status
|
||||
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;
|
||||
}
|
||||
|
||||
// Exemplo com AccX, AccY, Temp, Pressao, Altitude
|
||||
int16_t accX_int = static_cast<int16_t>(AccX * 100);
|
||||
int16_t accY_int = static_cast<int16_t>(AccY * 100);
|
||||
int16_t temp_int = static_cast<int16_t>(Temp * 100);
|
||||
uint16_t pressao_int = static_cast<uint16_t>(Pressao / 10);
|
||||
int16_t alt_int = static_cast<int16_t>(Altitude * 10);
|
||||
case CanMessagePosicaoDados::Dados1: { // Roll, Pitch, Yaw
|
||||
int16_t roll = Roll * 100;
|
||||
int16_t pitch = Pitch * 100;
|
||||
int16_t yaw = Yaw * 100;
|
||||
|
||||
data.push_back(accX_int >> 8); data.push_back(accX_int & 0xFF);
|
||||
data.push_back(accY_int >> 8); data.push_back(accY_int & 0xFF);
|
||||
data.push_back(temp_int >> 8); data.push_back(temp_int & 0xFF);
|
||||
data.push_back(pressao_int >> 8); data.push_back(pressao_int & 0xFF);
|
||||
data.push_back(alt_int >> 8); data.push_back(alt_int & 0xFF);
|
||||
data.push_back(roll >> 8); data.push_back(roll & 0xFF);
|
||||
data.push_back(pitch >> 8); data.push_back(pitch & 0xFF);
|
||||
data.push_back(yaw >> 8); data.push_back(yaw & 0xFF);
|
||||
break;
|
||||
}
|
||||
|
||||
case CanMessagePosicaoDados::Dados2: { // Temp, Pressão, Altitude
|
||||
int16_t temp = Temp * 100;
|
||||
uint16_t pressao = Pressao / 10; // Ex: 100000 Pa → 10000 (precisão: 10 Pa)
|
||||
int16_t altitude = Altitude * 100;
|
||||
|
||||
data.push_back(temp >> 8); data.push_back(temp & 0xFF);
|
||||
data.push_back(pressao >> 8); data.push_back(pressao & 0xFF);
|
||||
data.push_back(altitude >> 8); data.push_back(altitude & 0xFF);
|
||||
break;
|
||||
}
|
||||
|
||||
case CanMessagePosicaoDados::Dados3: { // Acc
|
||||
int16_t accX = AccX * 100;
|
||||
int16_t accY = AccY * 100;
|
||||
int16_t accZ = AccZ * 100;
|
||||
|
||||
data.push_back(accX >> 8); data.push_back(accX & 0xFF);
|
||||
data.push_back(accY >> 8); data.push_back(accY & 0xFF);
|
||||
data.push_back(accZ >> 8); data.push_back(accZ & 0xFF);
|
||||
break;
|
||||
}
|
||||
|
||||
case CanMessagePosicaoDados::Dados4: { // Gyro
|
||||
int16_t gyroX = GyroX * 100;
|
||||
int16_t gyroY = GyroY * 100;
|
||||
int16_t gyroZ = GyroZ * 100;
|
||||
|
||||
data.push_back(gyroX >> 8); data.push_back(gyroX & 0xFF);
|
||||
data.push_back(gyroY >> 8); data.push_back(gyroY & 0xFF);
|
||||
data.push_back(gyroZ >> 8); data.push_back(gyroZ & 0xFF);
|
||||
break;
|
||||
}
|
||||
|
||||
case CanMessagePosicaoDados::Dados5: { // Mag
|
||||
int16_t magX = MagX * 100;
|
||||
int16_t magY = MagY * 100;
|
||||
int16_t magZ = MagZ * 100;
|
||||
|
||||
data.push_back(magX >> 8); data.push_back(magX & 0xFF);
|
||||
data.push_back(magY >> 8); data.push_back(magY & 0xFF);
|
||||
data.push_back(magZ >> 8); data.push_back(magZ & 0xFF);
|
||||
break;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
return data;
|
||||
}
|
||||
|
||||
|
||||
private:
|
||||
TaskHandle_t IMUTaskHandle = NULL;
|
||||
static void IMUTaskWrapper(void *pvParameters) {
|
||||
SensorIMU *sensor = static_cast<SensorIMU*>(pvParameters);
|
||||
sensor->IMUTask();
|
||||
}
|
||||
|
||||
void IMUTask() {
|
||||
while (1) {
|
||||
if (Iniciado) {
|
||||
AferirDados();
|
||||
vTaskDelay(pdMS_TO_TICKS(10)); // 10ms → 100 Hz
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void AferirDados() {
|
||||
if (!Iniciado) return;
|
||||
|
||||
xyzFloat acc = mpu.getGValues();
|
||||
xyzFloat gyr = mpu.getGyrValues();
|
||||
xyzFloat mag = mpu.getMagValues();
|
||||
xyzFloat acc = mpu->getGValues();
|
||||
xyzFloat gyr = mpu->getGyrValues();
|
||||
xyzFloat mag = mpu->getMagValues();
|
||||
|
||||
AccX = acc.x;
|
||||
AccY = acc.y;
|
||||
|
|
@ -130,7 +225,59 @@ class SensorIMU {
|
|||
Temp = bmp.readTemperature();
|
||||
Pressao = bmp.readPressure();
|
||||
Altitude = bmp.readAltitude();
|
||||
|
||||
filter.update(GyroX, GyroY, GyroZ, AccX, AccY, AccZ, MagX, MagY, MagZ);\
|
||||
Roll = filter.getRoll();
|
||||
Pitch = filter.getPitch();
|
||||
Yaw = filter.getYaw();
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
std::vector<uint8_t> SensorIMU::ConfigurarSensor(std::vector<SensorIMU*>& 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;
|
||||
uint8_t enderecoMpu = data[5];
|
||||
uint8_t enderecoBmp = data[6];
|
||||
auto it = std::find_if(lista.begin(), lista.end(), [idNum](SensorIMU* s) { return s->ID_Num == idNum; });
|
||||
bool existente = (it != lista.end());
|
||||
if (conectar) {
|
||||
SensorIMU* sensor;
|
||||
if (!existente) {
|
||||
sensor = new SensorIMU(Mod_ID, "sIMU_" + String(idNum));
|
||||
}
|
||||
else {
|
||||
sensor = *it;
|
||||
}
|
||||
sensor->ID_Num = idNum;
|
||||
sensor->_EnderecoMPU = enderecoMpu;
|
||||
sensor->_EnderecoBMP = enderecoBmp;
|
||||
sensor->Inicializar();
|
||||
if (!existente) {
|
||||
lista.push_back(sensor);
|
||||
PrintTela("Sensor IMU adicionado via CAN: sIMU_" + String(idNum));
|
||||
}
|
||||
status = sensor->MontarMensagemCAN(CanMessagePosicaoDados::Status);
|
||||
} else {
|
||||
if (existente) {
|
||||
SensorIMU* sensor = *it;
|
||||
sensor->Desligar();
|
||||
status = sensor->MontarMensagemCAN(CanMessagePosicaoDados::Status);
|
||||
delete sensor;
|
||||
lista.erase(it);
|
||||
PrintTela("Sensor IMU removido via CAN: sIMU_" + String(idNum));
|
||||
}
|
||||
}
|
||||
break;
|
||||
}
|
||||
}
|
||||
return status;
|
||||
}
|
||||
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -10,6 +10,7 @@
|
|||
#include "C:\ZendionInc\agrobot_base\Firmware\Modulos\Pinout.h"
|
||||
#include "C:\ZendionInc\agrobot_base\Firmware\Modulos\SensorTemperaturaModel.h"
|
||||
#include "C:\ZendionInc\agrobot_base\Firmware\Modulos\SensorCorrenteModel.h"
|
||||
#include "C:\ZendionInc\agrobot_base\Firmware\Modulos\SensorIMUModel.h"
|
||||
#include "C:\ZendionInc\agrobot_base\Firmware\Modulos\SinaleiroModel.h"
|
||||
#include "C:\ZendionInc\agrobot_base\Firmware\Modulos\ReleModel.h"
|
||||
#include "C:\ZendionInc\agrobot_base\Firmware\Modulos\ServoFreioModel.h"
|
||||
|
|
@ -42,6 +43,7 @@ std::vector<SensorCorrente*> listaSensoresCorrente;
|
|||
std::vector<Sinaleiro*> listaSinaleiros;
|
||||
std::vector<Rele*> listaReles;
|
||||
std::vector<ServoFreio*> listaServoFreios;
|
||||
std::vector<SensorIMU*> listaSensoresIMU;
|
||||
|
||||
|
||||
void setup() {
|
||||
|
|
@ -130,6 +132,14 @@ void enviarDadosSensores() {
|
|||
}
|
||||
}
|
||||
|
||||
for (auto* sensor : listaSensoresIMU) {
|
||||
if (sensor != nullptr) {
|
||||
sensor->RequisitarDados();
|
||||
EnviarDadosCAN(sensor->MontarMensagemCAN(CanMessagePosicaoDados::Dados1));
|
||||
EnviarDadosCAN(sensor->MontarMensagemCAN(CanMessagePosicaoDados::Dados2));
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -225,6 +235,10 @@ void ProcessarCfg(std::vector<uint8_t> data) {
|
|||
_chkRx = ServoFreio::ConfigurarServo(listaServoFreios, data, Mod_ID);
|
||||
break;
|
||||
}
|
||||
case sIMU: {
|
||||
_chkRx = SensorIMU::ConfigurarSensor(listaSensoresIMU, data, Mod_ID);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
EnviarDadosCAN(_chkRx);
|
||||
|
|
@ -288,6 +302,13 @@ void LimparListasComponentes() {
|
|||
delete sensor;
|
||||
}
|
||||
listaSensoresTemperatura.clear();
|
||||
|
||||
for (auto* sensor : listaSensoresIMU) {
|
||||
sensor->Desligar();
|
||||
PrintTela("Sensor IMU " + sensor->_ID + " desligado.");
|
||||
delete sensor;
|
||||
}
|
||||
listaSensoresIMU.clear();
|
||||
}
|
||||
|
||||
S_Code DetectarComponente(uint8_t idNum) {
|
||||
|
|
@ -307,5 +328,8 @@ S_Code DetectarComponente(uint8_t idNum) {
|
|||
for (auto* s : listaServoFreios) {
|
||||
if (s->ID_Num == idNum) return S_Code::sFRO;
|
||||
}
|
||||
for (auto* s : listaSensoresIMU) {
|
||||
if (s->ID_Num == idNum) return S_Code::sIMU;
|
||||
}
|
||||
return S_Code::sVZO;
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue