revisao do retorno a base quando alinhado com a entrada do corredor e do mesmo lado que a base, inicializacao limpa e sincrona, inclusao do botao de referenciamento no menu da ihm, aleterado padroes da nova operacao

This commit is contained in:
Diego Freitas 2026-09-21 10:38:11 -03:00
parent bd1526cdaf
commit 5f04f9c5c6
7 changed files with 660 additions and 61 deletions

View File

@ -1,6 +1,7 @@
using AgroBase.Forms.IHM.Controls; using AgroBase.Forms.IHM.Controls;
using AgroBase.Forms.IHM.Operacao.Parametros; using AgroBase.Forms.IHM.Operacao.Parametros;
using AgroBase.Models; using AgroBase.Models;
using AgroBase.Models.Operacoes;
using System; using System;
using System.Collections.Generic; using System.Collections.Generic;
using System.Drawing; using System.Drawing;

View File

@ -6,6 +6,8 @@ using System;
using System.Collections.Generic; using System.Collections.Generic;
using System.Drawing; using System.Drawing;
using System.Linq; using System.Linq;
using System.Diagnostics;
using System.Threading;
using System.Threading.Tasks; using System.Threading.Tasks;
using System.Windows.Forms; using System.Windows.Forms;
@ -17,6 +19,46 @@ namespace AgroBase.Forms.IHM
private OperacaoModel op => Variaveis.OperacaoEmAndamento; private OperacaoModel op => Variaveis.OperacaoEmAndamento;
private Form _telaAtual; private Form _telaAtual;
private int _atualizacaoPendente;
private bool _consumoEstadoLiberado;
private System.Windows.Forms.Timer _pulsoUi;
private System.Threading.Timer _monitorUi;
private long _ultimoPulsoUi;
private int _monitorOcupado;
private bool _uiAtrasada;
private long _ultimoLogMonitor;
private void IniciarDiagnosticoUi()
{
Interlocked.Exchange(ref _ultimoPulsoUi, Stopwatch.GetTimestamp());
_pulsoUi = new System.Windows.Forms.Timer { Interval = 1000 };
_pulsoUi.Tick += (s, e) => Interlocked.Exchange(ref _ultimoPulsoUi, Stopwatch.GetTimestamp());
_pulsoUi.Start();
_monitorUi = new System.Threading.Timer(_ =>
{
if (Interlocked.Exchange(ref _monitorOcupado, 1) != 0) return;
try
{
if (Variaveis.Fechando) return;
long agora = Stopwatch.GetTimestamp();
double atraso = (agora - Interlocked.Read(ref _ultimoPulsoUi)) / (double)Stopwatch.Frequency;
bool atrasada = atraso >= 5;
if (atrasada != _uiAtrasada || (agora - _ultimoLogMonitor) / (double)Stopwatch.Frequency >= 30)
{
_uiAtrasada = atrasada;
_ultimoLogMonitor = agora;
Variaveis.MostrarLog($"[IHM/UI] {(atrasada ? "ATRASO" : "RESPONSIVA")} | ultimoPulso={atraso:F1}s | startup={frmInstancial.StartupConcluido}");
}
}
catch { /* Diagnóstico não pode derrubar o processo. */ }
finally { Volatile.Write(ref _monitorOcupado, 0); }
}, null, 2000, 2000);
Disposed += (s, e) =>
{
_monitorUi?.Dispose();
_pulsoUi?.Dispose();
};
}
public frmMenu frmMenu = new frmMenu(); public frmMenu frmMenu = new frmMenu();
private frmAjustes _frmAjustes; private frmAjustes _frmAjustes;
@ -61,6 +103,8 @@ namespace AgroBase.Forms.IHM
private void FrmMenu_TelaSolicitada(object sender, TelaSolicitadaEventArgs e) private void FrmMenu_TelaSolicitada(object sender, TelaSolicitadaEventArgs e)
{ {
if (!_consumoEstadoLiberado) return;
switch (e.Tela) switch (e.Tela)
{ {
case TipoTelaIHM.Operacao: case TipoTelaIHM.Operacao:
@ -426,6 +470,8 @@ namespace AgroBase.Forms.IHM
private void frmIHM_Load(object sender, EventArgs e) private void frmIHM_Load(object sender, EventArgs e)
{ {
lblVersao.Text = "IHM Rover • v " + Variaveis.Versao; lblVersao.Text = "IHM Rover • v " + Variaveis.Versao;
IniciarDiagnosticoUi();
AtualizarControlesUI();
AbrirTela(frmMenu); AbrirTela(frmMenu);
if (tmrDispositivos != null) if (tmrDispositivos != null)
@ -446,30 +492,56 @@ namespace AgroBase.Forms.IHM
} }
private Task tmrDispositivos_Tick() private Task tmrDispositivos_Tick()
{
try
{
if (IsDisposed || Disposing || !IsHandleCreated)
return Task.CompletedTask;
if (InvokeRequired)
{
BeginInvoke(new Action(AtualizarControlesUI));
}
else
{ {
AtualizarControlesUI(); AtualizarControlesUI();
}
}
catch (Exception ex)
{
Variaveis.MostrarLog("Erro no tmrDispositivos_Tick: " + ex.Message);
}
return Task.CompletedTask; return Task.CompletedTask;
} }
public void AtualizarControlesUI() public void AtualizarControlesUI()
{
if (IsDisposed || Disposing || !IsHandleCreated || Variaveis.Fechando) return;
if (Interlocked.Exchange(ref _atualizacaoPendente, 1) != 0) return;
if (InvokeRequired)
{
try { BeginInvoke(new Action(AtualizarControlesProtegidos)); }
catch { Volatile.Write(ref _atualizacaoPendente, 0); }
return;
}
AtualizarControlesProtegidos();
}
private void AtualizarControlesProtegidos()
{
var tempo = Stopwatch.StartNew();
try
{
if (IsDisposed || Disposing || Variaveis.Fechando) return;
// Latch: perda posterior de prontidão deve continuar visível na IHM.
if (!_consumoEstadoLiberado && frmInstancial.StartupConcluido && VariaveisEquipamento.Pronto)
{
_consumoEstadoLiberado = true;
Variaveis.MostrarLog("[IHM_STARTUP] LIBERADA: startup concluído e núcleo pronto.");
}
if (!_consumoEstadoLiberado)
{
AtualizarCabecalhoOperacao("INICIALIZANDO", "Inicializando equipamento. Aguarde...", HmiTheme.Warning);
DefinirAcoesHabilitadas(false, false, false);
foreach (var card in new[] { cardBateria, cardReservatorio, cardTemperatura, cardRede, cardGnss, cardBase })
AtualizarCard(card, "--", HmiTheme.TextMuted);
return;
}
AtualizarControlesCompletos();
}
catch (Exception ex) { Variaveis.MostrarLog("[IHM/UI] ERRO atualização: " + ex); }
finally
{
Volatile.Write(ref _atualizacaoPendente, 0);
if (tempo.ElapsedMilliseconds >= 500)
Variaveis.MostrarLog($"[IHM/UI] ATUALIZACAO_LENTA | duracao={tempo.ElapsedMilliseconds}ms | tela={_telaAtual?.Name}");
}
}
private void AtualizarControlesCompletos()
{ {
var operacao = Variaveis.OperacaoEmAndamento; var operacao = Variaveis.OperacaoEmAndamento;
var dadosOperacao = operacao?.Sensoriamento?.Operacao; var dadosOperacao = operacao?.Sensoriamento?.Operacao;
@ -552,7 +624,7 @@ namespace AgroBase.Forms.IHM
bool operacaoPausada = ObterPausaParaExibicao(pausaModelo); bool operacaoPausada = ObterPausaParaExibicao(pausaModelo);
bool operacaoEmergencia = ObterEmergenciaParaExibicao(emergenciaModelo); bool operacaoEmergencia = ObterEmergenciaParaExibicao(emergenciaModelo);
bool operacaoIniciada = dadosOperacao?.OperacaoIniciada ?? false; bool operacaoIniciada = dadosOperacao?.OperacaoIniciada ?? false;
AtualizarStatusOperacao(statusOperacao, operacaoPausada, operacaoEmergencia, (int)dadosOperacao.TempoAguardandoSeg); AtualizarStatusOperacao(statusOperacao, operacaoPausada, operacaoEmergencia, (int)(dadosOperacao?.TempoAguardandoSeg ?? 0));
AtualizarAcoesOperacao(statusOperacao, operacaoIniciada, operacaoPausada, operacaoEmergencia); AtualizarAcoesOperacao(statusOperacao, operacaoIniciada, operacaoPausada, operacaoEmergencia);
// ========================================================= // =========================================================
@ -560,7 +632,7 @@ namespace AgroBase.Forms.IHM
// ========================================================= // =========================================================
if (_telaAtual != null && !_telaAtual.IsDisposed) if (_telaAtual != null && !_telaAtual.IsDisposed)
{ {
(_telaAtual as ITelaNavegavel).AtualizarDadosTela(); (_telaAtual as ITelaNavegavel)?.AtualizarDadosTela();
} }
} }
@ -666,7 +738,7 @@ namespace AgroBase.Forms.IHM
if (card == null) if (card == null)
return; return;
if (card.Valor == valor || card.CorValor == cor) if (card.Valor == valor && card.CorValor == cor)
return; return;
ExecutarNaUI(delegate ExecutarNaUI(delegate

View File

@ -15,6 +15,7 @@ namespace AgroBase.Forms.IHM
public partial class frmMenu : Form, ITelaNavegavel public partial class frmMenu : Form, ITelaNavegavel
{ {
private bool _sincronizando; private bool _sincronizando;
private bool _referenciamentoEmAndamento;
/// <summary> /// <summary>
/// Estes eventos deixam os atalhos de campo prontos para receber /// Estes eventos deixam os atalhos de campo prontos para receber
@ -52,7 +53,9 @@ namespace AgroBase.Forms.IHM
}); });
btnDebugMode.Click += BtnDebugMode_Click; btnDebugMode.Click += BtnDebugMode_Click;
btnAjustarLeverArm.Click += BtnAjustarLeverArm_Click; btnAjustarLeverArm.Texto = "Referenciar robô";
btnAjustarLeverArm.IconText = "↻";
btnAjustarLeverArm.Click += BtnReferenciarRobo_Click;
rowFalhas.Click += LblCodigoFalha_Click; rowFalhas.Click += LblCodigoFalha_Click;
} }
@ -73,8 +76,19 @@ namespace AgroBase.Forms.IHM
AtualizarControlesUI(); AtualizarControlesUI();
} }
private bool _startupVisualLiberado;
public void AtualizarControlesUI() public void AtualizarControlesUI()
{ {
if (!_startupVisualLiberado)
{
_startupVisualLiberado = frmInstancial.StartupConcluido && VariaveisEquipamento.Pronto;
if (!_startupVisualLiberado)
{
lblRoverProntidao.Text = "Inicializando equipamento...";
lblStatus.Text = "Aguardando inicialização do núcleo.";
return;
}
}
bool mqttConectado = Variaveis.MqttServiceLocal != null && Variaveis.MqttServiceLocal.StatusConexao(); bool mqttConectado = Variaveis.MqttServiceLocal != null && Variaveis.MqttServiceLocal.StatusConexao();
bool iaConectada = VariaveisOperacao.Operadores != null && VariaveisOperacao.Operadores.Conectado; bool iaConectada = VariaveisOperacao.Operadores != null && VariaveisOperacao.Operadores.Conectado;
var operacao = Variaveis.OperacaoEmAndamento; var operacao = Variaveis.OperacaoEmAndamento;
@ -134,7 +148,7 @@ namespace AgroBase.Forms.IHM
private bool VerificaEquipamentoIniciado() private bool VerificaEquipamentoIniciado()
{ {
if (!VariaveisEquipamento.Pronto) if (!frmInstancial.StartupConcluido || !VariaveisEquipamento.Pronto)
{ {
FuncoesIHM.MostrarAlerta( FuncoesIHM.MostrarAlerta(
"Equipamento iniciando", "Equipamento iniciando",
@ -297,18 +311,82 @@ namespace AgroBase.Forms.IHM
btnDebugMode.Texto = Variaveis.DebugMode ? "Desligar Debug Mode" : "Ligar Debug Mode"; btnDebugMode.Texto = Variaveis.DebugMode ? "Desligar Debug Mode" : "Ligar Debug Mode";
} }
private void BtnAjustarLeverArm_Click(object sender, EventArgs e) private string MotivoBloqueioReferenciamento(OperacaoModel operacao)
{ {
if (AjustarLeverArmSolicitado != null) if (!frmInstancial.StartupConcluido || Variaveis.Fechando)
{ return "Aguarde a inicialização do equipamento.";
AjustarLeverArmSolicitado(this, EventArgs.Empty); if (operacao?.Controle == null || operacao.Parametros?.Controle == null ||
return; operacao.Sensoriamento?.Operacao == null || operacao.DispMvd?.Dados?.Modulos == null)
return "Dados de controle ou módulos indisponíveis.";
if (operacao.CalibrandoRuntime || MKS057DCanService.Referenciando)
return "Já existe calibragem ou referenciamento em andamento.";
var estado = operacao.Sensoriamento.Operacao;
if (estado.Emergencia)
return "Resolva a emergência antes de referenciar.";
if (estado.OperacaoIniciada && !estado.Pausa)
return "Pause a operação e aguarde o robô parar antes de referenciar.";
if (Math.Abs(operacao.Controle.PercentualVelocidadeSP) > 0 ||
Math.Abs(operacao.Controle.RPM_SP) > 0)
return "Neutralize os comandos de movimento antes de referenciar.";
var modulos = operacao.DispMvd.Dados.Modulos.ToList();
if (modulos.Count == 0 || modulos.Any(m => m == null || !m.Conectado ||
m.MovMotor == null || m.DirMotor == null || !m.DirMotor.Inicializado || !m.DirMotor.Comandar))
return "Todos os módulos devem estar conectados e a direção inicializada e habilitada.";
if (modulos.Any(m => double.IsNaN(m.MovMotor.VelocidadeInstantanea) ||
double.IsInfinity(m.MovMotor.VelocidadeInstantanea) ||
Math.Abs(m.MovMotor.VelocidadeInstantanea) > 0.05))
return "Aguarde todas as rodas pararem antes de referenciar.";
return null;
} }
ExibirAtalhoNaoVinculado( private async void BtnReferenciarRobo_Click(object sender, EventArgs e)
"Ajustar Lever Arm", {
"O layout já está pronto. Vincule a tela ou o diálogo no evento " + if (_referenciamentoEmAndamento) return;
"AjustarLeverArmSolicitado."); _referenciamentoEmAndamento = true;
btnAjustarLeverArm.Enabled = false;
try
{
var operacao = Variaveis.OperacaoEmAndamento;
string motivo = MotivoBloqueioReferenciamento(operacao);
if (motivo != null)
{
FuncoesIHM.MostrarAlerta("Referenciamento indisponível", motivo, MessageBoxIcon.Warning);
return;
}
if (!FuncoesIHM.ConfirmarAcao("Referenciar robô",
"Refazer a referência de todas as rodas? A direção irá se movimentar. " +
"Mantenha o robô parado e a área das rodas livre.", MessageBoxIcon.Question)) return;
// A confirmação abre um diálogo: revalidar o estado antes de executar.
if (!ReferenceEquals(operacao, Variaveis.OperacaoEmAndamento)) return;
motivo = MotivoBloqueioReferenciamento(operacao);
if (motivo != null)
{
FuncoesIHM.MostrarAlerta("Referenciamento indisponível", motivo, MessageBoxIcon.Warning);
return;
}
btnAjustarLeverArm.Texto = "Referenciando...";
Variaveis.MostrarLog("[IHM/REFERENCIAR] Solicitado referenciamento de todas as rodas pelo menu.");
// Mesma rotina usada pelo R3; evita o desvio para movimento combinado.
bool resultado = await operacao.DispMvd.Dados.RealizarReferenciamentoDirecional();
Variaveis.MostrarLog($"[IHM/REFERENCIAR] Rotina retornou resultado={resultado}; confira os logs de cada módulo.");
// Não retomar a operação nem liberar pausa automaticamente.
}
catch (Exception ex)
{
Variaveis.MostrarLog("[IHM/REFERENCIAR] Falha: " + ex);
if (!IsDisposed && !Disposing)
FuncoesIHM.MostrarAlerta("Falha no referenciamento", ex.Message, MessageBoxIcon.Error);
}
finally
{
_referenciamentoEmAndamento = false;
if (!IsDisposed && !Disposing)
{
btnAjustarLeverArm.Texto = "Referenciar robô";
btnAjustarLeverArm.Enabled = true;
}
}
} }
private void LblCodigoFalha_Click(object sender, EventArgs e) private void LblCodigoFalha_Click(object sender, EventArgs e)

View File

@ -94,14 +94,14 @@ namespace AgroBase.Forms.IHM
AtualizarParametrosTela AtualizarParametrosTela
), op, recarregarMapa); ), op, recarregarMapa);
} }
catch (ObjectDisposedException)
{
}
catch (InvalidOperationException) catch (InvalidOperationException)
{ {
// O formulário pode estar encerrando entre a verificação // O formulário pode estar encerrando entre a verificação
// e o BeginInvoke. A próxima abertura sincroniza a fonte. // e o BeginInvoke. A próxima abertura sincroniza a fonte.
} }
catch (ObjectDisposedException)
{
}
return; return;
} }

View File

@ -1,4 +1,4 @@
using AgroBase.Forms.IHM; using AgroBase.Forms.IHM;
using AgroBase.Models; using AgroBase.Models;
using AgroBase.Services; using AgroBase.Services;
using System; using System;
@ -27,7 +27,9 @@ namespace AgroBase.Forms
private static CancellationTokenSource _appCts; private static CancellationTokenSource _appCts;
private static bool _startupConcluido; private static volatile bool _startupConcluido;
// Prontidão visual não altera os intertravamentos operacionais.
public static bool StartupConcluido => _startupConcluido;
private static bool _shutdownIniciado; private static bool _shutdownIniciado;
private bool _allowClose; private bool _allowClose;

View File

@ -662,18 +662,18 @@ namespace AgroBase.Models
DirecionalAutomatico = true, DirecionalAutomatico = true,
PulverizadorAutomatico = true, PulverizadorAutomatico = true,
FrenagemAutomaticaAoParar = true, FrenagemAutomaticaAoParar = false,
ImuParadaPorInclinacao = true, ImuParadaPorInclinacao = true,
ImuAuxilioMovimento = true, ImuAuxilioMovimento = true,
ImuAuxilioDirecional = true, ImuAuxilioDirecional = false,
ImuDirecionalPreferirArco = true, ImuDirecionalPreferirArco = false,
OakParadaPorObstaculo = true, OakParadaPorObstaculo = false,
DirAuxilioSonar = true, DirAuxilioSonar = false,
MovAuxilioSonar = true, MovAuxilioSonar = true,
MpcHorizonte = 4.0, MpcHorizonte = 4.0,
AnteciparManobraCorredorM = 0.0, AnteciparManobraCorredorM = 0.0,
MovVelocidadeSErvasPercent = 50, MovVelocidadeSErvasPercent = 45,
MovVelocidadeCErvasPercent = 20, MovVelocidadeCErvasPercent = 20,
DirAnguloMaximo = 30, DirAnguloMaximo = 30,
DirVelocidadeMovimento = 80, DirVelocidadeMovimento = 80,
@ -685,8 +685,8 @@ namespace AgroBase.Models
AtuDuracaoAtuacao = 0, AtuDuracaoAtuacao = 0,
AtuPercentualErvasBicoOff = 2, AtuPercentualErvasBicoOff = 2,
AtuPercentualErvasBicoOn = 1, AtuPercentualErvasBicoOn = 1,
AtuPercentualInicioPulverizacao = 85, AtuPercentualInicioPulverizacao = 70,
AtuPressaoLinha = 22, AtuPressaoLinha = 35,
AtuAgitadorModo = ModoAgitadorCalda.Continuo, AtuAgitadorModo = ModoAgitadorCalda.Continuo,
AtuModoControle = ModoControleBomba.PID, AtuModoControle = ModoControleBomba.PID,
AtuModeloCabeca = CabecasModelo.Target, AtuModeloCabeca = CabecasModelo.Target,
@ -715,7 +715,7 @@ namespace AgroBase.Models
new OperacaoModulosMandatoriosModel() new OperacaoModulosMandatoriosModel()
{ {
Dispositivo = T_Code.Sen, Dispositivo = T_Code.Sen,
Mandatorio = true, Mandatorio = false,
Utilizar = true, Utilizar = true,
}, },
new OperacaoModulosMandatoriosModel() new OperacaoModulosMandatoriosModel()
@ -1050,31 +1050,31 @@ namespace AgroBase.Models
DirecionalAutomatico = true, DirecionalAutomatico = true,
PulverizadorAutomatico = false, PulverizadorAutomatico = false,
FrenagemAutomaticaAoParar = true, FrenagemAutomaticaAoParar = false,
ImuParadaPorInclinacao = true, ImuParadaPorInclinacao = true,
ImuAuxilioMovimento = true, ImuAuxilioMovimento = true,
ImuAuxilioDirecional = true, ImuAuxilioDirecional = false,
ImuDirecionalPreferirArco = true, ImuDirecionalPreferirArco = false,
OakParadaPorObstaculo = true, OakParadaPorObstaculo = false,
DirAuxilioSonar = true, DirAuxilioSonar = false,
MovAuxilioSonar = true, MovAuxilioSonar = true,
MpcHorizonte = 4.0, MpcHorizonte = 4.0,
AnteciparManobraCorredorM = 0, AnteciparManobraCorredorM = 0,
MovVelocidadeSErvasPercent = 60, MovVelocidadeSErvasPercent = 50,
MovVelocidadeCErvasPercent = 20, MovVelocidadeCErvasPercent = 20,
DirAnguloMaximo = 30, DirAnguloMaximo = 30,
DirVelocidadeMovimento = 80, DirVelocidadeMovimento = 80,
DirTipoMovimento = TiposControladorDirecional.MPC, DirTipoMovimento = TiposControladorDirecional.MPC,
DistanciaManobra = 3.0, DistanciaManobra = 2.2,
AtuAlturaAreaPulverizacao = 10, AtuAlturaAreaPulverizacao = 10,
AtuDuracaoAtuacao = 0, AtuDuracaoAtuacao = 0,
AtuPercentualErvasBicoOff = 2, AtuPercentualErvasBicoOff = 2,
AtuPercentualErvasBicoOn = 1, AtuPercentualErvasBicoOn = 1,
AtuPercentualInicioPulverizacao = 85, AtuPercentualInicioPulverizacao = 70,
AtuPressaoLinha = 18, AtuPressaoLinha = 22,
AtuAgitadorModo = ModoAgitadorCalda.SemAgitacao, AtuAgitadorModo = ModoAgitadorCalda.SemAgitacao,
AtuModoControle = ModoControleBomba.PID, AtuModoControle = ModoControleBomba.PID,
AtuModeloCabeca = CabecasModelo.Target, AtuModeloCabeca = CabecasModelo.Target,
@ -1103,13 +1103,13 @@ namespace AgroBase.Models
new OperacaoModulosMandatoriosModel() new OperacaoModulosMandatoriosModel()
{ {
Dispositivo = T_Code.Sen, Dispositivo = T_Code.Sen,
Mandatorio = true, Mandatorio = false,
Utilizar = true, Utilizar = true,
}, },
new OperacaoModulosMandatoriosModel() new OperacaoModulosMandatoriosModel()
{ {
Dispositivo = T_Code.Atu, Dispositivo = T_Code.Atu,
Mandatorio = true, Mandatorio = false,
Utilizar = true, Utilizar = true,
}, },
new OperacaoModulosMandatoriosModel() new OperacaoModulosMandatoriosModel()
@ -1119,6 +1119,12 @@ namespace AgroBase.Models
Utilizar = true, Utilizar = true,
}, },
new OperacaoModulosMandatoriosModel() new OperacaoModulosMandatoriosModel()
{
Dispositivo = T_Code.Cam,
Mandatorio = false,
Utilizar = true,
},
new OperacaoModulosMandatoriosModel()
{ {
Dispositivo = T_Code.Snr, Dispositivo = T_Code.Snr,
Mandatorio = true, Mandatorio = true,

View File

@ -111,6 +111,18 @@ namespace AgroBase.Models
private double ErroOrientacaoMaximoRecuperacaoRetornoGraus { get; set; } = 65.0; private double ErroOrientacaoMaximoRecuperacaoRetornoGraus { get; set; } = 65.0;
private double ToleranciaParametroRecuperacaoRetorno { get; set; } = 0.25; private double ToleranciaParametroRecuperacaoRetorno { get; set; } = 0.25;
// RetornoBase V6 - tentativa de escape cinematicamente viavel quando
// rover e base estao na mesma cabeceira. O envelope convexo continua
// sendo uma barreira conservadora para rotas retas/perimetrais, mas
// deixa de ocultar uma curva curta que caiba na area livre da borda.
private double DistanciaMaximaCabecieraRetornoM { get; set; } = 6.0;
// As ruas do mapa sao polilinhas sem largura agronomica confiavel.
// Portanto a barreira autoritativa e NAO CRUZAR a linha. Os 5 cm
// abaixo servem apenas para rejeitar tangencias numericas/raspadas;
// nao tentam representar a largura fisica da cultura.
private double MargemLinhaCulturaRetornoM { get; set; } = 0.05;
private double ComprimentoMaximoRelativoEscapeRetorno { get; set; } = 4.0;
private int MaxPontosEntradaRecuperacaoTransicao { get; set; } = 12; private int MaxPontosEntradaRecuperacaoTransicao { get; set; } = 12;
private int ConfirmacoesMinimasEntradaProximoCorredor { get; set; } = 2; private int ConfirmacoesMinimasEntradaProximoCorredor { get; set; } = 2;
@ -6118,7 +6130,8 @@ namespace AgroBase.Models
int ultimoCorredor = -1; int ultimoCorredor = -1;
for (int i = 0; i < pontos.Count; i++) // Ponto 0 é a poição do robô, não considera distancia entre pontos
for (int i = 1; i < pontos.Count; i++)
{ {
var ponto = pontos[i]; var ponto = pontos[i];
@ -6128,7 +6141,7 @@ namespace AgroBase.Models
if (ponto.idxPonto != i) if (ponto.idxPonto != i)
throw new InvalidOperationException($"Indice global inconsistente na trajetoria: {ponto.idxPonto}/{i}."); throw new InvalidOperationException($"Indice global inconsistente na trajetoria: {ponto.idxPonto}/{i}.");
if (i > 0) if (i > 1)
{ {
double salto = GPSUtils.DistanciaEntrePontos(pontos[i - 1].Posicao, ponto.Posicao); double salto = GPSUtils.DistanciaEntrePontos(pontos[i - 1].Posicao, ponto.Posicao);
@ -6494,6 +6507,44 @@ namespace AgroBase.Models
op.Sensoriamento.Operacao.OperacaoIniciada = operacaoIniciadaAnterior.Value; op.Sensoriamento.Operacao.OperacaoIniciada = operacaoIniciadaAnterior.Value;
} }
/*
* Falha fechada do retorno.
*
* A rota agricola anterior e preservada apenas para
* diagnostico/retentativa. Ela NAO pode voltar a ser uma
* rota automatica liberada depois que o retorno falhou.
* O operador ainda pode assumir o modo manual, reposicionar
* o rover e solicitar um novo retorno a partir da nova pose.
*/
try
{
op?.DefinirPausa(true);
}
catch (Exception exPausa)
{
Variaveis.MostrarLog(
"[RETORNO] Falha ao solicitar pausa local: " +
exPausa.Message
);
}
try
{
RedisService.AtualizarCampos(
CtxKey.DadosOperacao,
("configurado", false),
("liberado", false),
("motivo_nao_liberado", "Retorno a base rejeitado: " + ex.Message)
);
}
catch (Exception exRedis)
{
Variaveis.MostrarLog(
"[RETORNO] Falha ao publicar bloqueio no Redis: " +
exRedis.Message
);
}
Variaveis.MostrarLog("[RETORNO] Trajetoria rejeitada: " + ex.Message); Variaveis.MostrarLog("[RETORNO] Trajetoria rejeitada: " + ex.Message);
op?.Sensoriamento?.InserirLog( op?.Sensoriamento?.InserirLog(
T_Code.Trj, T_Code.Trj,
@ -7032,6 +7083,43 @@ namespace AgroBase.Models
); );
break; break;
} }
/*
* CASO 4 - ESCAPE DE CABECEIRA
*
* O caso perimetral pode ser recusado quando o rover ainda esta
* matematicamente dentro do envelope convexo, mesmo estando na
* area livre da cabeceira. Nesse ponto tentamos uma Dubins a
* partir da pose REAL do rover ate a base. Cada candidato e
* amostrado e validado contra as polilinhas das ruas; nenhuma
* curva que cruze uma linha de cultura e aceita.
*/
if (!trecho.Any())
{
string diagnosticoEscape;
var escape = GerarEscapeSeguroCabeciera(
posicaoAtual,
baseGps,
out diagnosticoEscape
);
if (escape.Any())
{
trecho.AddRange(escape);
Variaveis.MostrarLog(
"[RETORNO] Escape seguro de cabeceira aceito | " +
diagnosticoEscape
);
}
else
{
Variaveis.MostrarLog(
"[RETORNO] Escape de cabeceira indisponivel | " +
diagnosticoEscape
);
}
}
pontos.AddRange(trecho); pontos.AddRange(trecho);
return (trecho.Any(), pontos); return (trecho.Any(), pontos);
@ -7178,6 +7266,358 @@ namespace AgroBase.Models
return lista; return lista;
} }
private List<GPSModel> GerarEscapeSeguroCabeciera(
GPSModel posRobo,
GPSModel posBase,
out string diagnostico
)
{
diagnostico = "nao avaliado";
if (!PontoGpsValido(posRobo) || !PontoGpsValido(posBase))
{
diagnostico = "coordenada do rover ou da base invalida";
return new List<GPSModel>();
}
if (CorredorAtual?.Dentro == true)
{
diagnostico = "rover ainda confirmado dentro de corredor";
return new List<GPSModel>();
}
double distanciaCabeciera = DistanciaAteExtremidadeMaisProximaRua(posRobo);
if (
double.IsNaN(distanciaCabeciera) ||
double.IsInfinity(distanciaCabeciera) ||
distanciaCabeciera > DistanciaMaximaCabecieraRetornoM
)
{
diagnostico = $"fora da zona de cabeceira ({distanciaCabeciera:F2}m)";
return new List<GPSModel>();
}
double headingInicial = posRobo.AnguloCarroDefinido;
if (double.IsNaN(headingInicial) || double.IsInfinity(headingInicial))
headingInicial = posRobo.OrientacaoReal;
if (double.IsNaN(headingInicial) || double.IsInfinity(headingInicial))
{
diagnostico = "heading atual invalido";
return new List<GPSModel>();
}
double headingFinal = GPSUtils.CalcularOrientacao(posRobo, posBase);
double distanciaDireta = GPSUtils.DistanciaEntrePontos(posRobo, posBase);
double entreEixosM = Math.Max(
0.30,
VariaveisEquipamento.DistanciaEntreEixosCm / 100.0
);
// Mesmo raio conservador ja usado nas ferraduras validadas.
const double anguloProjetoGraus = 28.0;
double raioSeguro = Math.Max(
0.75,
(entreEixosM /
(2.0 * Math.Tan(anguloProjetoGraus * Math.PI / 180.0))) *
1.05
);
var candidatos = GerarCurvasDubinsCandidatas(
posRobo,
headingInicial,
posBase,
headingFinal,
raioSeguro,
Math.Max(0.15, DistanciaEntrePontosCurva)
);
int rejeitadosGeometria = 0;
int rejeitadosComprimento = 0;
foreach (var candidato in candidatos.OrderBy(x => x.custo))
{
if (
candidato.pontos == null ||
candidato.pontos.Count < 2 ||
candidato.pontos.Any(x => !PontoGpsValido(x))
)
{
continue;
}
double limiteComprimento =
distanciaDireta * ComprimentoMaximoRelativoEscapeRetorno +
10.0;
if (candidato.custo > limiteComprimento)
{
rejeitadosComprimento++;
continue;
}
string motivoGeometria;
if (!TrajetoEscapeLivreDaCultura(candidato.pontos, out motivoGeometria))
{
rejeitadosGeometria++;
continue;
}
diagnostico =
$"dubins={candidato.tipo}, raio={raioSeguro:F2}m, " +
$"comprimento={candidato.custo:F2}m, " +
$"cabeciera={distanciaCabeciera:F2}m, " +
$"heading={headingInicial:F1}->{headingFinal:F1}graus";
return candidato.pontos;
}
diagnostico =
$"nenhum candidato seguro; total={candidatos.Count}, " +
$"geometria={rejeitadosGeometria}, " +
$"comprimento={rejeitadosComprimento}, " +
$"cabeciera={distanciaCabeciera:F2}m";
return new List<GPSModel>();
}
private double DistanciaAteExtremidadeMaisProximaRua(GPSModel ponto)
{
if (RuasPlantacao == null || RuasPlantacao.Count == 0)
return double.PositiveInfinity;
double melhor = double.PositiveInfinity;
foreach (var rua in RuasPlantacao)
{
if (rua == null || rua.Count == 0)
continue;
melhor = Math.Min(
melhor,
GPSUtils.DistanciaEntrePontos(ponto, rua[0])
);
melhor = Math.Min(
melhor,
GPSUtils.DistanciaEntrePontos(ponto, rua[rua.Count - 1])
);
}
return melhor;
}
private List<(string tipo, double custo, List<GPSModel> pontos)>
GerarCurvasDubinsCandidatas(
GPSModel inicio,
double headingInicioGraus,
GPSModel fim,
double headingFimGraus,
double raioM,
double espacamentoM
)
{
double refLat = inicio.Latitude;
double refLon = inicio.Longitude;
var fimXY = LatLonToXY(fim, refLat, refLon);
double theta0 = Mod2Pi(
Math.PI / 2.0 - headingInicioGraus * Math.PI / 180.0
);
double theta1 = Mod2Pi(
Math.PI / 2.0 - headingFimGraus * Math.PI / 180.0
);
double dx = fimXY.x;
double dy = fimXY.y;
double d = Math.Sqrt(dx * dx + dy * dy) / raioM;
double theta = Mod2Pi(Math.Atan2(dy, dx));
double alpha = Mod2Pi(theta0 - theta);
double beta = Mod2Pi(theta1 - theta);
var parametros = new List<(string tipo, double t, double p, double q, double custo)>();
AdicionarDubins(parametros, "LSL", DubinsLSL(alpha, beta, d), raioM);
AdicionarDubins(parametros, "RSR", DubinsRSR(alpha, beta, d), raioM);
AdicionarDubins(parametros, "LSR", DubinsLSR(alpha, beta, d), raioM);
AdicionarDubins(parametros, "RSL", DubinsRSL(alpha, beta, d), raioM);
AdicionarDubins(parametros, "RLR", DubinsRLR(alpha, beta, d), raioM);
AdicionarDubins(parametros, "LRL", DubinsLRL(alpha, beta, d), raioM);
var resultado = new List<(string tipo, double custo, List<GPSModel> pontos)>();
foreach (var candidato in parametros)
{
double[] segmentos = { candidato.t, candidato.p, candidato.q };
var pontos = new List<GPSModel> { ClonarPontoGeometrico(inicio) };
double x = 0.0;
double y = 0.0;
double th = theta0;
double passo = Math.Max(0.10, espacamentoM);
for (int seg = 0; seg < 3; seg++)
{
char tipoSegmento = candidato.tipo[seg];
double restante = segmentos[seg] * raioM;
while (restante > 1e-6)
{
double ds = Math.Min(passo, restante);
restante -= ds;
if (tipoSegmento == 'S')
{
x += Math.Cos(th) * ds;
y += Math.Sin(th) * ds;
}
else
{
double k = tipoSegmento == 'L'
? 1.0 / raioM
: -1.0 / raioM;
double dth = k * ds;
x += (Math.Sin(th + dth) - Math.Sin(th)) / k;
y += -(Math.Cos(th + dth) - Math.Cos(th)) / k;
th = Mod2Pi(th + dth);
}
pontos.Add(XYToLatLon(x, y, refLat, refLon));
}
}
if (pontos.Count > 0)
pontos[pontos.Count - 1] = ClonarPontoGeometrico(fim);
resultado.Add((candidato.tipo, candidato.custo, pontos));
}
return resultado;
}
private bool TrajetoEscapeLivreDaCultura(
List<GPSModel> trajeto,
out string motivo
)
{
motivo = null;
if (trajeto == null || trajeto.Count < 2)
{
motivo = "trajeto vazio";
return false;
}
if (RuasPlantacao == null || RuasPlantacao.Count == 0)
{
motivo = "mapa de ruas indisponivel";
return false;
}
GPSModel referencia = trajeto[0];
double distanciaAcumulada = 0.0;
double distanciaInicialLinha = DistanciaMinimaAteLinhasCultura(
trajeto[0]
);
for (int i = 1; i < trajeto.Count; i++)
{
GPSModel a = trajeto[i - 1];
GPSModel b = trajeto[i];
distanciaAcumulada += GPSUtils.DistanciaEntrePontos(a, b);
foreach (var rua in RuasPlantacao)
{
if (rua == null || rua.Count < 2)
continue;
for (int j = 1; j < rua.Count; j++)
{
var aXY = LatLonToXY(a, referencia.Latitude, referencia.Longitude);
var bXY = LatLonToXY(b, referencia.Latitude, referencia.Longitude);
var cXY = LatLonToXY(rua[j - 1], referencia.Latitude, referencia.Longitude);
var dXY = LatLonToXY(rua[j], referencia.Latitude, referencia.Longitude);
if (SegmentosIntersectam(aXY, bXY, cXY, dXY))
{
motivo = $"segmento {i - 1}->{i} cruza linha de cultura";
return false;
}
}
}
double distanciaLinha = DistanciaMinimaAteLinhasCultura(b);
double margemExigida = MargemLinhaCulturaRetornoM;
// O rover pode iniciar proximo da ultima linha. Durante o
// primeiro metro aceitamos a distancia inicial, mas nunca uma
// aproximacao adicional relevante. Depois disso vale a margem
// integral configurada.
if (distanciaAcumulada <= 1.0)
margemExigida = Math.Min(
margemExigida,
Math.Max(0.10, distanciaInicialLinha - 0.05)
);
if (distanciaLinha + 1e-6 < margemExigida)
{
motivo =
$"distancia insuficiente da cultura: " +
$"{distanciaLinha:F2}m < {margemExigida:F2}m";
return false;
}
}
return true;
}
private double DistanciaMinimaAteLinhasCultura(GPSModel ponto)
{
double melhor = double.PositiveInfinity;
foreach (var rua in RuasPlantacao)
{
if (rua == null || rua.Count < 2)
continue;
for (int i = 1; i < rua.Count; i++)
{
melhor = Math.Min(
melhor,
DistanciaPontoSegmentoMetros(
ponto,
rua[i - 1],
rua[i]
)
);
}
}
return melhor;
}
private double DistanciaPontoSegmentoMetros(
GPSModel ponto,
GPSModel inicio,
GPSModel fim
)
{
var p = LatLonToXY(ponto, ponto.Latitude, ponto.Longitude);
var a = LatLonToXY(inicio, ponto.Latitude, ponto.Longitude);
var b = LatLonToXY(fim, ponto.Latitude, ponto.Longitude);
double dx = b.x - a.x;
double dy = b.y - a.y;
double norma2 = dx * dx + dy * dy;
if (norma2 <= 1e-12)
return Math.Sqrt(a.x * a.x + a.y * a.y);
double t = ((p.x - a.x) * dx + (p.y - a.y) * dy) / norma2;
t = Math.Max(0.0, Math.Min(1.0, t));
double qx = a.x + t * dx;
double qy = a.y + t * dy;
return Math.Sqrt(
Math.Pow(p.x - qx, 2) +
Math.Pow(p.y - qy, 2)
);
}
#endregion #endregion
#region CASO 2 #region CASO 2