From a212e48c39f9108af55944a60d18ab0bfcdf0eed Mon Sep 17 00:00:00 2001 From: Diego Freitas Date: Mon, 31 Aug 2026 14:18:05 -0300 Subject: [PATCH] revisado fluxo de referenciamento do direcional --- AgroBase/AgroBase/Models/MKS057DModel.cs | 56 +- .../Models/Modules/DirecionalModel.cs | 880 +++++++++++++++--- .../Modules/MovimentacaoUnificadoModel.cs | 66 +- .../AgroBase/Services/MKS057DCanService.cs | 2 - 4 files changed, 850 insertions(+), 154 deletions(-) diff --git a/AgroBase/AgroBase/Models/MKS057DModel.cs b/AgroBase/AgroBase/Models/MKS057DModel.cs index 47757d6b9..b522c9cf2 100644 --- a/AgroBase/AgroBase/Models/MKS057DModel.cs +++ b/AgroBase/AgroBase/Models/MKS057DModel.cs @@ -66,20 +66,8 @@ namespace AgroBase.Models private MksMovimentStatus _statusMovimento; public MksMovimentStatus StatusMovimento { - get - { - return _statusMovimento; - } - set - { - _statusMovimento = value; - - if (_statusMovimento == MksMovimentStatus.Complete) - { - AtualizarAngulo(AnguloSP); - AtualizarAngulo(AnguloSP); - } - } + get => _statusMovimento; + set => _statusMovimento = value; } @@ -93,28 +81,48 @@ namespace AgroBase.Models { DateTime agora = DateTime.Now; - double delta = novoAngulo - AnguloAtual; - - double dtMs = (agora - MomentoUltimaLeitura).TotalMilliseconds; - if (dtMs > 0) + if (MomentoUltimaLeitura != DateTime.MinValue) { - VelocidadeEstimada = delta / dtMs; + double delta = novoAngulo - AnguloAtual; + double dtMs = (agora - MomentoUltimaLeitura).TotalMilliseconds; + + if (dtMs > 0 && dtMs <= 1000) + VelocidadeEstimada = delta / dtMs; + else + VelocidadeEstimada = 0; + } + else + { + VelocidadeEstimada = 0; } AnguloAnterior = AnguloAtual; AnguloAtual = novoAngulo; MomentoUltimaLeitura = agora; } - public double AnguloEstimado() { - double dt = (DateTime.Now - MomentoUltimaLeitura).TotalMilliseconds; - double estimado = AnguloAtual + (VelocidadeEstimada * dt); + if (MomentoUltimaLeitura == DateTime.MinValue) + return AnguloAtual; + + double dtMs = (DateTime.Now - MomentoUltimaLeitura).TotalMilliseconds; + + /* + * Durante referenciamento PulsosRecebidos trabalha rápido. + * Não deixamos uma leitura velha continuar "viajando" indefinidamente. + */ + dtMs = Math.Max(0, Math.Min(dtMs, 150)); + + double estimado = AnguloAtual + (VelocidadeEstimada * dtMs); + + Variaveis.MostrarLog( + $"[MKS057DModel] [AnguloEstimado] [ADDR {EnderecoTx}] " + + $"{AnguloAtual:F2}° + ({VelocidadeEstimada:F4} * {dtMs:F0}ms) = {estimado:F2}°" + ); - Variaveis.MostrarLog($"[MKS057DModel] [AnguloEstimado] [ADDR {EnderecoTx}] Estimativa {AnguloAtual:F2}° + ({VelocidadeEstimada:F4} * {dt:F0}ms) = {estimado:F2}°"); return estimado; } - + } public class MKS057ComandoModel diff --git a/AgroBase/AgroBase/Models/Modules/DirecionalModel.cs b/AgroBase/AgroBase/Models/Modules/DirecionalModel.cs index 36df2de18..be09673a8 100644 --- a/AgroBase/AgroBase/Models/Modules/DirecionalModel.cs +++ b/AgroBase/AgroBase/Models/Modules/DirecionalModel.cs @@ -482,44 +482,189 @@ namespace AgroBase.Models.Modules } - public async Task ReferenciaMotor() - { - var op = Variaveis.OperacaoEmAndamento; - if (Referenciando || !Inicializado) return; + + + + private const int VelocidadeReferenciamento = 10; + private const double ToleranciaZeroReferenciamento = 0.35; + private const double ToleranciaCentroReferenciamento = 0.50; + private const double MovimentoMinimoConfirmacao = 0.20; + /* + * Janela inicial propositalmente folgada. + * Depois dos ensaios podemos apertar com base nas folgas reais + * dos quatro conjuntos. + */ + private const double FolgaMinimaReferencia = 0.05; + private const double FolgaMaximaReferencia = 8.0; + private static readonly TimeSpan TimeoutPrimeiraBorda = TimeSpan.FromSeconds(120); + private static readonly TimeSpan TimeoutSegundaBorda = TimeSpan.FromSeconds(30); + private static readonly TimeSpan TimeoutCentro = TimeSpan.FromSeconds(30); + + private async Task AtualizarPulsosAgoraAsync(int timeoutMs = 800) + { + var leitura = Leitura; + + if (leitura == null || !Inicializado) + return false; + + DateTime leituraAnterior = leitura.NumeroPulsos.lidoEm; + + MKS057DCanService.RequisitarDadoForcado(_EnderecoCAN_Tx, _EnderecoCAN_Rx, MKS057DCanService.MksHandler.MksFuncCode.PulsosRecebidos); + + bool NovaLeitura() + { + var l = Leitura; + + return + l != null && + l.NumeroPulsos.lidoEm > leituraAnterior; + } + + await FuncoesGlobais.AguardarCondicaoAsync(NovaLeitura, timeoutMs, 20); + + return NovaLeitura(); + } + + private async Task AtualizarIOAgoraAsync(int timeoutMs = 800) + { + var leitura = Leitura; + + if (leitura == null || !Inicializado) + return false; + + DateTime leituraAnterior = leitura.EntradaIN1.lidoEm; + + MKS057DCanService.RequisitarDadoForcado(_EnderecoCAN_Tx, _EnderecoCAN_Rx, MKS057DCanService.MksHandler.MksFuncCode.IO); + + bool NovaLeitura() + { + var l = Leitura; + + return + l != null && + l.EntradaIN1.lidoEm > leituraAnterior; + } + + await FuncoesGlobais.AguardarCondicaoAsync(NovaLeitura, timeoutMs, 20); + + return NovaLeitura(); + } + + private bool PulsosEstaoRecentes(double maxIdadeMs = 200) + { + var leitura = Leitura; + + if (leitura == null || leitura.NumeroPulsos.lidoEm == DateTime.MinValue) + { + return false; + } + + return (DateTime.Now - leitura.NumeroPulsos.lidoEm).TotalMilliseconds <= maxIdadeMs; + } + + + + public async Task ReferenciaMotor() + { + var op = + Variaveis.OperacaoEmAndamento; + + if (Referenciando || + !Inicializado) + { + return false; + } Referenciando = true; - - await Task.Delay(1000); - Referenciado = false; - bool sensorAngRealConectado = (SensorAnguloReal?.Inicializado ?? false); + DateTime inicio = + DateTime.Now; - if (sensorAngRealConectado) + try { - Referenciado = await ReferenciaMotorAbsoluto(); - } - else - { - Referenciado = await ReferenciaMotorRelativo(); - } + /* + * Atualmente o produto usa somente referência relativa MKS. + * + * Quando o sensor absoluto voltar para o produto, + * tratamos o outro fluxo com o mesmo rigor. + */ + bool sucesso = + await ReferenciaMotorRelativo(); - if (Referenciado) - { - await SetZero(); - Variaveis.MostrarLog($"[DirecionalModel] [ReferenciaMotor] [DIR {Mod_ID}] - Referenciamento Concluído!"); - op.Sensoriamento.InserirLog(T_Code.Dir, StatusModulo.Operante, 100, $"Referenciamento Concluído!", id: Mod_ID); - } - else - { - Variaveis.MostrarLog($"[DirecionalModel] [ReferenciaMotor] [DIR {Mod_ID}] - Referenciamento Falhou!"); - op.Sensoriamento.InserirLog(T_Code.Dir, StatusModulo.Falha, 0, $"Referenciamento Falhou!", id: Mod_ID); - } + Referenciado = + sucesso; - Referenciando = false; + if (!sucesso) + { + Variaveis.MostrarLog( + $"[DirecionalModel] [ReferenciaMotor] [DIR {Mod_ID}] " + + $"Referenciamento FALHOU." + ); - UltimoReferenciamento = DateTime.Now; + op.Sensoriamento.InserirLog( + T_Code.Dir, + StatusModulo.Falha, + 0, + "Referenciamento Falhou!", + id: Mod_ID + ); + + return false; + } + + UltimoReferenciamento = + DateTime.Now; + + /* + * Se essa propriedade global ainda for usada em algum lugar, + * agora ela representa um referenciamento REALMENTE concluído. + */ + MKS057DCanService.UltimoReferenciamento = + UltimoReferenciamento; + + double tempo = + (DateTime.Now - inicio) + .TotalSeconds; + + Variaveis.MostrarLog( + $"[DirecionalModel] [ReferenciaMotor] [DIR {Mod_ID}] " + + $"Referenciamento CONCLUÍDO E CONFIRMADO | " + + $"tempo={tempo:F1}s | " + + $"folga={FolgaMecanica:F3}°" + ); + + op.Sensoriamento.InserirLog( + T_Code.Dir, + StatusModulo.Operante, + 100, + "Referenciamento Concluído!", + id: Mod_ID + ); + + return true; + } + catch (Exception ex) + { + FalharReferencia( + $"Exceção durante referenciamento: {ex.Message}" + ); + + op.Sensoriamento.InserirLog( + T_Code.Dir, + StatusModulo.Falha, + 0, + $"Erro no referenciamento: {ex.Message}", + id: Mod_ID + ); + + return false; + } + finally + { + Referenciando = false; + } } public async Task ReferenciaMotorAbsoluto() @@ -544,124 +689,617 @@ namespace AgroBase.Models.Modules public async Task ReferenciaMotorRelativo() { - await SetZero(); + /* + * ============================================================ + * ETAPA 0 - Pré-condições + * ============================================================ + */ - bool Sucesso = false; - bool EstadoInicial = IN1_Atuado; - double AnguloEntrada = 9999; - double AnguloSaida = 9999; - int verificacoesAngulo = 0; - double anguloAnterior = 0; + if (!Inicializado) + return false; - DateTime TimeoutGeral = DateTime.Now.AddMinutes(2); // Timeout geral para o processo. + Variaveis.MostrarLog( + $"[DirecionalModel] [REFERENCIA] [DIR {Mod_ID}] " + + $"INÍCIO REFERENCIAMENTO RELATIVO" + ); - Sentido _sentido = SentidoReal == Sentido.Horario ? Sentido.Antihorario : Sentido.Horario; - double _anguloSP = 180 * (_sentido == Sentido.Antihorario ? -1 : 1); - double _anguloSpAnterior = 0; + /* + * ============================================================ + * ETAPA 1 - Zero temporário + * ============================================================ + */ - while (!Sucesso && TimeoutGeral > DateTime.Now && Inicializado) + if (!await SetZeroConfirmadoAsync("ZERO_TEMPORARIO")) + return FalharReferencia( + "Não foi possível confirmar o zero temporário." + ); + + /* + * Agora queremos IO e posição comprovadamente atuais. + */ + if (!await AtualizarIOAgoraAsync()) + return FalharReferencia( + "Não foi possível obter leitura fresca do IN1." + ); + + if (!await AtualizarPulsosAgoraAsync()) + return FalharReferencia( + "Não foi possível obter posição fresca após zero temporário." + ); + + bool estadoInicial = + IN1_Atuado; + + Sentido sentidoBusca = + SentidoReal == Sentido.Horario + ? Sentido.Antihorario + : Sentido.Horario; + + double alvoBusca = + sentidoBusca == Sentido.Antihorario + ? -180.0 + : 180.0; + + Variaveis.MostrarLog( + $"[DirecionalModel] [REFERENCIA] [DIR {Mod_ID}] " + + $"Estado inicial | " + + $"IN1={estadoInicial} | " + + $"angulo={AnguloDriver:F3}° | " + + $"sentidoBusca={sentidoBusca}" + ); + + /* + * ============================================================ + * ETAPA 2 - Busca primeira borda + * ============================================================ + */ + + if (!await EnviarMovimentoConfirmadoAsync( + alvoBusca, + VelocidadeReferenciamento, + sentidoBusca, + "BUSCA_BORDA_1")) { - int VelocidadeGiro = DefineVelocidadeReferenciamento(); - - if (_anguloSP != _anguloSpAnterior || (verificacoesAngulo > 5 && AnguloLeitura == anguloAnterior)) - { - MKS057DCanService.Movimento(_EnderecoCAN_Tx, _EnderecoCAN_Rx, _anguloSP, VelocidadeGiro, _sentido); - _anguloSpAnterior = _anguloSP; - verificacoesAngulo = 0; - } - - if (IN1_Atuado != EstadoInicial && AnguloEntrada == 9999) - { - AnguloEntrada = Leitura.AnguloEstimado(); - Variaveis.MostrarLog($"[DirecionalModel] [ReferenciaMotorRelativo] [DIR {Mod_ID}] - Angulo de Entrada Definido: {AnguloEntrada} (Leitura: {AnguloLeitura})"); - - _sentido = _sentido == Sentido.Horario ? Sentido.Antihorario : Sentido.Horario; - _anguloSP *= -1; - Variaveis.MostrarLog($"[DirecionalModel] [ReferenciaMotorRelativo] [DIR {Mod_ID}] - Invertendo Sentido de Giro para {_sentido.ToString()}"); - } - if (IN1_Atuado == EstadoInicial && AnguloEntrada != 9999 && AnguloSaida == 9999) - { - AnguloSaida = Leitura.AnguloEstimado(); - Variaveis.MostrarLog($"[DirecionalModel] [ReferenciaMotorRelativo] [DIR {Mod_ID}] - Angulo de Saída Definido: {AnguloSaida} (Leitura: {AnguloLeitura})"); - - _sentido = _sentido == Sentido.Horario ? Sentido.Antihorario : Sentido.Horario; - Variaveis.MostrarLog($"[DirecionalModel] [ReferenciaMotorRelativo] [DIR {Mod_ID}] - Invertendo Sentido de Giro para {_sentido.ToString()}"); - - _anguloSP = CalcularCentroCompensado(AnguloEntrada, AnguloSaida, _sentido, OffsetReferenciamento); - } - - verificacoesAngulo++; - anguloAnterior = AnguloLeitura; - - Sucesso = AnguloEntrada != 9999 && AnguloSaida != 9999 && (FuncoesMatematicas.ValorEstaEntre(Leitura.AnguloEstimado(), _anguloSP, 0.5)); - - DateTime Inicio = DateTime.Now; - bool NovoDado() => MKS057DCanService.DadosLeitura.Any(x => x.EnderecoRx == _EnderecoCAN_Rx && x.UltimoComandoRecebido > Inicio); - await FuncoesGlobais.AguardarCondicaoAsync(NovoDado, 500, 100); + return FalharReferencia( + "Motor não iniciou corretamente a busca da primeira borda." + ); } - if (!Sucesso) + double? borda1 = + await AguardarTransicaoIN1Async( + estadoInicial, + alvoBusca, + TimeoutPrimeiraBorda, + "BORDA_1" + ); + + if (!borda1.HasValue) + return FalharReferencia( + "Primeira borda não foi encontrada." + ); + + bool estadoAposBorda1 = + IN1_Atuado; + + /* + * ============================================================ + * ETAPA 3 - Inverte e busca segunda borda + * ============================================================ + */ + + Sentido sentidoRetorno = + sentidoBusca == Sentido.Horario + ? Sentido.Antihorario + : Sentido.Horario; + + double alvoRetorno = + -alvoBusca; + + if (!await EnviarMovimentoConfirmadoAsync( + alvoRetorno, + VelocidadeReferenciamento, + sentidoRetorno, + "BUSCA_BORDA_2")) { - Variaveis.MostrarLog($"[DirecionalModel] [ReferenciaMotorRelativo] [DIR {Mod_ID}] - ReferenciaMotorRelativo falhou (timeout ou não inicializado). Entrada={AnguloEntrada}, Saida={AnguloSaida}"); - Variaveis.OperacaoEmAndamento.Sensoriamento.InserirLog(T_Code.Dir, StatusModulo.Falha, 0, $"ReferenciaMotorRelativo falhou (timeout ou não inicializado). Entrada={AnguloEntrada}, Saida={AnguloSaida}", id: Mod_ID); + return FalharReferencia( + "Motor não iniciou corretamente a busca da segunda borda." + ); } - return Sucesso; - } + double? borda2 = + await AguardarTransicaoIN1Async( + estadoAposBorda1, + alvoRetorno, + TimeoutSegundaBorda, + "BORDA_2" + ); - private int DefineVelocidadeReferenciamento() - { - int VelocidadeGiro = 10; - return VelocidadeGiro; - } + if (!borda2.HasValue) + return FalharReferencia( + "Segunda borda não foi encontrada." + ); - private double CalcularCentroCompensado(double entrada, double saida, Sentido sentido, double offset) - { - double centro = (entrada + saida) / 2.0; - double folga = Math.Abs(entrada - saida); - - // Determina o sentido do giro baseado na diferença - double direcao = sentido == Sentido.Horario ? -1 : 1; - - double compensacao = 0; // (folga / 2.0) * direcao; - - double centroCompensado = centro + compensacao + offset; - - Variaveis.MostrarLog($"[DirecionalModel] [CalcularCentroCompensado] [DIR {Mod_ID}] - Folga: {folga}, Entrada: {entrada}, Saida: {saida}, Centro: {centro}, Compensacao: {compensacao}, Centro Compensado: {centroCompensado}"); - - FolgaMecanica = folga; - - return centroCompensado; - } - - public async Task SetZero() - { - MKS057DCanService.SetZero(_EnderecoCAN_Tx, _EnderecoCAN_Rx); - - for (int i = 0; i < 5; i++) + /* + * Depois da segunda passagem devemos voltar ao estado lógico + * em que começamos. + */ + if (IN1_Atuado != estadoInicial) { - DateTime Inicio = DateTime.Now; - bool NovoDado() => MKS057DCanService.DadosLeitura.Any(x => x.EnderecoRx == _EnderecoCAN_Rx && x.UltimoComandoRecebido > Inicio); - await FuncoesGlobais.AguardarCondicaoAsync(NovoDado, 500, 100); + return FalharReferencia( + $"Estado do IN1 inconsistente após segunda borda. " + + $"Inicial={estadoInicial}, Atual={IN1_Atuado}." + ); + } - if (FuncoesMatematicas.ValorEstaEntre(AnguloLeitura, -1, 1)) + /* + * ============================================================ + * ETAPA 4 - Valida folga / calcula centro + * ============================================================ + */ + + double folga = + Math.Abs( + borda1.Value - + borda2.Value + ); + + if (folga < FolgaMinimaReferencia || + folga > FolgaMaximaReferencia) + { + return FalharReferencia( + $"Folga medida fora da faixa esperada: {folga:F3}° " + + $"(esperado {FolgaMinimaReferencia:F2}..{FolgaMaximaReferencia:F2}°)." + ); + } + + double centro = + ((borda1.Value + borda2.Value) / 2.0) + + OffsetReferenciamento; + + FolgaMecanica = + folga; + + Variaveis.MostrarLog( + $"[DirecionalModel] [REFERENCIA] [DIR {Mod_ID}] " + + $"GEOMETRIA CONFIRMADA | " + + $"B1={borda1.Value:F3}° | " + + $"B2={borda2.Value:F3}° | " + + $"folga={folga:F3}° | " + + $"centro={centro:F3}° | " + + $"offset={OffsetReferenciamento:F3}°" + ); + + /* + * ============================================================ + * ETAPA 5 - Vai para o centro calculado + * ============================================================ + */ + + Sentido sentidoCentro = + centro >= AnguloDriver + ? Sentido.Horario + : Sentido.Antihorario; + + if (!await EnviarMovimentoConfirmadoAsync( + centro, + VelocidadeReferenciamento, + sentidoCentro, + "CENTRALIZACAO")) + { + return FalharReferencia( + "Motor não iniciou movimento para o centro calculado." + ); + } + + if (!await AguardarAnguloAsync( + centro, + ToleranciaCentroReferenciamento, + TimeoutCentro, + "CENTRALIZACAO")) + { + return FalharReferencia( + "Motor não confirmou chegada ao centro calculado." + ); + } + + /* + * ============================================================ + * ETAPA 6 - Zero absoluto final + * ============================================================ + */ + + if (!await SetZeroConfirmadoAsync("ZERO_FINAL")) + { + /* + * IMPORTANTÍSSIMO: + * + * NÃO mandamos Movimento(0). + * + * Se o zero não foi aceito, 0 ainda pertence ao sistema + * de coordenadas antigo. + */ + return FalharReferencia( + "Centro encontrado, porém o ZERO FINAL não foi confirmado." + ); + } + + /* + * Uma leitura extra para fechar o contrato. + */ + if (!await AtualizarPulsosAgoraAsync()) + return FalharReferencia( + "Zero final aplicado, mas não foi possível confirmar posição posteriormente." + ); + + if (Math.Abs(AnguloDriver) > + ToleranciaZeroReferenciamento) + { + return FalharReferencia( + $"Zero final instável. Leitura final={AnguloDriver:F3}°." + ); + } + + Variaveis.MostrarLog( + $"[DirecionalModel] [REFERENCIA] [DIR {Mod_ID}] " + + $"REFERENCIAMENTO RELATIVO VALIDADO | " + + $"folga={FolgaMecanica:F3}° | " + + $"zero={AnguloDriver:F3}°" + ); + + return true; + } + + private async Task SetZeroConfirmadoAsync(string etapa, int tentativas = 3) + { + for (int tentativa = 1; tentativa <= tentativas; tentativa++) + { + if (!Inicializado) + return false; + + Variaveis.MostrarLog( + $"[DirecionalModel] [REFERENCIA] [DIR {Mod_ID}] " + + $"{etapa}: SetZero tentativa {tentativa}/{tentativas}" + ); + + MKS057DCanService.SetZero(_EnderecoCAN_Tx, _EnderecoCAN_Rx); + + /* + * Deixa o comando entrar na fila/processar. + * Depois solicitamos explicitamente uma posição nova. + */ + await Task.Delay(100); + + bool recebeuPulsos = await AtualizarPulsosAgoraAsync(); + + if (!recebeuPulsos) { - Variaveis.MostrarLog($"[DirecionalModel] [SetZero] [DIR {Mod_ID}] - Set Zero"); - break; + Variaveis.MostrarLog( + $"[DirecionalModel] [REFERENCIA] [DIR {Mod_ID}] " + + $"{etapa}: nenhuma leitura nova de pulsos após SetZero." + ); + + continue; + } + + double angulo = AnguloDriver; + + if (Math.Abs(angulo) <= ToleranciaZeroReferenciamento) + { + Variaveis.MostrarLog( + $"[DirecionalModel] [REFERENCIA] [DIR {Mod_ID}] " + + $"{etapa}: SetZero CONFIRMADO | angulo={angulo:F3}°" + ); + + return true; + } + + Variaveis.MostrarLog( + $"[DirecionalModel] [REFERENCIA] [DIR {Mod_ID}] " + + $"{etapa}: SetZero NÃO confirmado | angulo={angulo:F3}°" + ); + } + + return false; + } + + private async Task EnviarMovimentoConfirmadoAsync(double anguloAlvo, int velocidade, Sentido sentido, string etapa) + { + if (!await AtualizarPulsosAgoraAsync()) + { + Variaveis.MostrarLog( + $"[DirecionalModel] [REFERENCIA] [DIR {Mod_ID}] " + + $"{etapa}: não foi possível obter posição inicial." + ); + + return false; + } + + double anguloInicial = AnguloDriver; + + if (Math.Abs(anguloAlvo - anguloInicial) <= ToleranciaCentroReferenciamento) + { + return true; + } + + MKS057DCanService.Movimento( + _EnderecoCAN_Tx, + _EnderecoCAN_Rx, + anguloAlvo, + velocidade, + sentido + ); + + Variaveis.MostrarLog( + $"[DirecionalModel] [REFERENCIA] [DIR {Mod_ID}] " + + $"{etapa}: movimento enviado | " + + $"atual={anguloInicial:F2}° alvo={anguloAlvo:F2}° sentido={sentido}" + ); + + DateTime limite = DateTime.Now.AddSeconds(3); + + DateTime ultimaLeitura = Leitura.NumeroPulsos.lidoEm; + + while (DateTime.Now < limite && Inicializado) + { + bool NovaLeitura() + { + var l = Leitura; + return l != null && l.NumeroPulsos.lidoEm > ultimaLeitura; + } + + await FuncoesGlobais.AguardarCondicaoAsync(NovaLeitura, 500, 20); + + if (!NovaLeitura()) + { + MKS057DCanService.RequisitarDadoForcado(_EnderecoCAN_Tx, _EnderecoCAN_Rx, MKS057DCanService.MksHandler.MksFuncCode.PulsosRecebidos); + continue; + } + + ultimaLeitura = Leitura.NumeroPulsos.lidoEm; + + double delta = AnguloDriver - anguloInicial; + + if (Math.Abs(delta) < MovimentoMinimoConfirmacao) + { + continue; + } + + double deltaEsperado = anguloAlvo - anguloInicial; + + if (Math.Sign(delta) != Math.Sign(deltaEsperado)) + { + Variaveis.MostrarLog( + $"[DirecionalModel] [REFERENCIA] [DIR {Mod_ID}] " + + $"{etapa}: movimento no sentido INESPERADO | " + + $"delta={delta:F3}° esperado={Math.Sign(deltaEsperado)}" + ); + + return false; + } + + Variaveis.MostrarLog( + $"[DirecionalModel] [REFERENCIA] [DIR {Mod_ID}] " + + $"{etapa}: movimento CONFIRMADO | delta={delta:F3}°" + ); + + return true; + } + + Variaveis.MostrarLog( + $"[DirecionalModel] [REFERENCIA] [DIR {Mod_ID}] " + + $"{etapa}: comando enviado, mas movimento não foi comprovado." + ); + + return false; + } + + private async Task AguardarTransicaoIN1Async(bool estadoOrigem, double anguloLimite, TimeSpan timeout, string etapa) + { + DateTime limite = DateTime.Now.Add(timeout); + + DateTime ultimoIO = Leitura?.EntradaIN1?.lidoEm ?? DateTime.MinValue; + + while (DateTime.Now < limite && Inicializado) + { + bool NovoIO() + { + var l = Leitura; + return l != null && l.EntradaIN1.lidoEm > ultimoIO; + } + + await FuncoesGlobais.AguardarCondicaoAsync(NovoIO, 500, 20); + + /* + * Se o polling normal não trouxe IO, + * fazemos um poke específico. + */ + if (!NovoIO()) + { + MKS057DCanService.RequisitarDadoForcado(_EnderecoCAN_Tx, _EnderecoCAN_Rx, MKS057DCanService.MksHandler.MksFuncCode.IO); + continue; + } + + ultimoIO = Leitura.EntradaIN1.lidoEm; + + bool estadoAtual = IN1_Atuado; + + /* + * Ainda não cruzamos a referência. + */ + if (estadoAtual == estadoOrigem) + { + /* + * Se chegou ao limite de busca sem mudar IN1, + * não existe motivo para continuar esperando. + */ + if (PulsosEstaoRecentes(300) && Math.Abs(AnguloDriver - anguloLimite) <= 1.0) + { + Variaveis.MostrarLog( + $"[DirecionalModel] [REFERENCIA] [DIR {Mod_ID}] " + + $"{etapa}: atingiu limite {anguloLimite:F2}° " + + $"sem transição do IN1." + ); + + return null; + } + + continue; + } + + /* + * IO mudou. + * + * Só aceitamos a borda se a posição estiver recente. + * Nada de calcular referência com dado velho. + */ + if (!PulsosEstaoRecentes()) + { + Variaveis.MostrarLog( + $"[DirecionalModel] [REFERENCIA] [DIR {Mod_ID}] " + + $"{etapa}: IN1 mudou, mas posição está desatualizada." + ); + + return null; + } + + double anguloCandidato = Leitura.AnguloEstimado(); + + bool estadoCandidato = estadoAtual; + + DateTime ioCandidato = Leitura.EntradaIN1.lidoEm; + + /* + * Debounce lógico: + * exigimos outra amostra nova mantendo o mesmo estado. + */ + bool NovaConfirmacao() + { + var l = Leitura; + return l != null && l.EntradaIN1.lidoEm > ioCandidato; + } + + await FuncoesGlobais.AguardarCondicaoAsync(NovaConfirmacao, 500, 20); + + if (!NovaConfirmacao()) + { + MKS057DCanService.RequisitarDadoForcado(_EnderecoCAN_Tx, _EnderecoCAN_Rx, MKS057DCanService.MksHandler.MksFuncCode.IO); + await FuncoesGlobais.AguardarCondicaoAsync(NovaConfirmacao, 500, 20); + } + + if (!NovaConfirmacao()) + return null; + + if (IN1_Atuado != estadoCandidato) + { + Variaveis.MostrarLog( + $"[DirecionalModel] [REFERENCIA] [DIR {Mod_ID}] " + + $"{etapa}: transição descartada como instável." + ); + + ultimoIO = Leitura.EntradaIN1.lidoEm; + + continue; + } + + Variaveis.MostrarLog( + $"[DirecionalModel] [REFERENCIA] [DIR {Mod_ID}] " + + $"{etapa}: BORDA CONFIRMADA | " + + $"IN1 {estadoOrigem}->{estadoCandidato} | " + + $"angulo={anguloCandidato:F3}°" + ); + + return anguloCandidato; + } + + Variaveis.MostrarLog( + $"[DirecionalModel] [REFERENCIA] [DIR {Mod_ID}] " + + $"{etapa}: timeout aguardando transição do IN1." + ); + + return null; + } + + private async Task AguardarAnguloAsync(double alvo, double tolerancia, TimeSpan timeout, string etapa) + { + DateTime limite = DateTime.Now.Add(timeout); + + DateTime ultimaLeitura = Leitura?.NumeroPulsos?.lidoEm ?? DateTime.MinValue; + + int leiturasDentro = 0; + + while (DateTime.Now < limite && Inicializado) + { + bool NovaLeitura() + { + var l = Leitura; + return l != null && l.NumeroPulsos.lidoEm > ultimaLeitura; + } + + await FuncoesGlobais.AguardarCondicaoAsync(NovaLeitura, 500, 20); + + if (!NovaLeitura()) + { + MKS057DCanService.RequisitarDadoForcado(_EnderecoCAN_Tx, _EnderecoCAN_Rx, MKS057DCanService.MksHandler.MksFuncCode.PulsosRecebidos); + continue; + } + + ultimaLeitura = Leitura.NumeroPulsos.lidoEm; + + double erro = Math.Abs(AnguloDriver - alvo); + + if (erro <= tolerancia) + { + leiturasDentro++; + + /* + * Duas amostras consecutivas dentro da janela. + * Evita considerar "passou voando pelo alvo" como sucesso. + */ + if (leiturasDentro >= 2) + { + Variaveis.MostrarLog( + $"[DirecionalModel] [REFERENCIA] [DIR {Mod_ID}] " + + $"{etapa}: alvo CONFIRMADO | " + + $"alvo={alvo:F3}° atual={AnguloDriver:F3}°" + ); + + return true; + } } else { - MKS057DCanService.SetZero(_EnderecoCAN_Tx, _EnderecoCAN_Rx); + leiturasDentro = 0; } } - MKS057DCanService.Movimento(_EnderecoCAN_Tx, _EnderecoCAN_Rx, 0, 10, Sentido.Parado); - Variaveis.MostrarLog($"[DirecionalModel] [SetZero] [DIR {Mod_ID}] - Comando Angulo Zero: {AnguloLeitura}"); + Variaveis.MostrarLog( + $"[DirecionalModel] [REFERENCIA] [DIR {Mod_ID}] " + + $"{etapa}: timeout | alvo={alvo:F3}° atual={AnguloDriver:F3}°" + ); + + return false; } + private bool FalharReferencia(string motivo) + { + try + { + MKS057DCanService.Parar(_EnderecoCAN_Tx, _EnderecoCAN_Rx); + } + catch + { + // Estamos justamente tratando uma falha. + } - + Referenciado = false; + Variaveis.MostrarLog( + $"[DirecionalModel] [REFERENCIA] [DIR {Mod_ID}] " + + $"FALHA: {motivo}" + ); + + return false; + } public void RequisitarDadosSensoresExternos() diff --git a/AgroBase/AgroBase/Models/Modules/MovimentacaoUnificadoModel.cs b/AgroBase/AgroBase/Models/Modules/MovimentacaoUnificadoModel.cs index bf684d46a..2403bd416 100644 --- a/AgroBase/AgroBase/Models/Modules/MovimentacaoUnificadoModel.cs +++ b/AgroBase/AgroBase/Models/Modules/MovimentacaoUnificadoModel.cs @@ -672,19 +672,71 @@ namespace AgroBase.Models.Modules { try { - await modulo.DirMotor.ReferenciaMotor(); + bool ok = + await modulo.DirMotor.ReferenciaMotor(); - op.MarcarModuloCalibrado(T_Code.Dir, modulo.Modulo_ID, true); + op.MarcarModuloCalibrado( + T_Code.Dir, + modulo.Modulo_ID, + ok + ); - Variaveis.MostrarLog($"[MovimentacaoUnificadaModel] [RealizarCalibragemDirAsync] Referenciamento do módulo {modulo.Modulo_ID} concluído com sucesso!"); - op.Sensoriamento.InserirLog(T_Code.Dir, StatusModulo.Operante, 100, $"Referenciamento do módulo {modulo.Modulo_ID} concluído com sucesso!", id: modulo.Modulo_ID); + if (ok) + { + Variaveis.MostrarLog( + $"[MovimentacaoUnificadaModel] " + + $"[RealizarCalibragemDirAsync] " + + $"Referenciamento do módulo {modulo.Modulo_ID} " + + $"concluído com sucesso!" + ); + + op.Sensoriamento.InserirLog( + T_Code.Dir, + StatusModulo.Operante, + 100, + $"Referenciamento do módulo {modulo.Modulo_ID} concluído com sucesso!", + id: modulo.Modulo_ID + ); + } + else + { + Variaveis.MostrarLog( + $"[MovimentacaoUnificadaModel] " + + $"[RealizarCalibragemDirAsync] " + + $"Referenciamento do módulo {modulo.Modulo_ID} FALHOU." + ); + + op.Sensoriamento.InserirLog( + T_Code.Dir, + StatusModulo.Falha, + 0, + $"Referenciamento do módulo {modulo.Modulo_ID} falhou.", + id: modulo.Modulo_ID + ); + } } catch (Exception ex) { - op.MarcarModuloCalibrado(T_Code.Dir, modulo.Modulo_ID, false); + op.MarcarModuloCalibrado( + T_Code.Dir, + modulo.Modulo_ID, + false + ); - Variaveis.MostrarLog($"[MovimentacaoUnificadaModel] [RealizarCalibragemDirAsync] Erro no referenciamento do módulo {modulo.Modulo_ID}: {ex.Message}"); - op.Sensoriamento.InserirLog(T_Code.Dir, StatusModulo.Falha, 0, $"Erro no referenciamento do módulo {modulo.Modulo_ID}: {ex.Message}", id: modulo.Modulo_ID); + Variaveis.MostrarLog( + $"[MovimentacaoUnificadaModel] " + + $"[RealizarCalibragemDirAsync] " + + $"Erro no referenciamento do módulo {modulo.Modulo_ID}: " + + $"{ex.Message}" + ); + + op.Sensoriamento.InserirLog( + T_Code.Dir, + StatusModulo.Falha, + 0, + $"Erro no referenciamento do módulo {modulo.Modulo_ID}: {ex.Message}", + id: modulo.Modulo_ID + ); } })); } diff --git a/AgroBase/AgroBase/Services/MKS057DCanService.cs b/AgroBase/AgroBase/Services/MKS057DCanService.cs index b1d085d43..16c9d471c 100644 --- a/AgroBase/AgroBase/Services/MKS057DCanService.cs +++ b/AgroBase/AgroBase/Services/MKS057DCanService.cs @@ -835,8 +835,6 @@ namespace AgroBase.Services null, dedupe: false ); - - UltimoReferenciamento = DateTime.Now; } public static void Parar(