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
parent
bd1526cdaf
commit
5f04f9c5c6
|
|
@ -1,6 +1,7 @@
|
|||
using AgroBase.Forms.IHM.Controls;
|
||||
using AgroBase.Forms.IHM.Operacao.Parametros;
|
||||
using AgroBase.Models;
|
||||
using AgroBase.Models.Operacoes;
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Drawing;
|
||||
|
|
|
|||
|
|
@ -6,6 +6,8 @@ using System;
|
|||
using System.Collections.Generic;
|
||||
using System.Drawing;
|
||||
using System.Linq;
|
||||
using System.Diagnostics;
|
||||
using System.Threading;
|
||||
using System.Threading.Tasks;
|
||||
using System.Windows.Forms;
|
||||
|
||||
|
|
@ -17,6 +19,46 @@ namespace AgroBase.Forms.IHM
|
|||
private OperacaoModel op => Variaveis.OperacaoEmAndamento;
|
||||
|
||||
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();
|
||||
private frmAjustes _frmAjustes;
|
||||
|
|
@ -61,6 +103,8 @@ namespace AgroBase.Forms.IHM
|
|||
|
||||
private void FrmMenu_TelaSolicitada(object sender, TelaSolicitadaEventArgs e)
|
||||
{
|
||||
if (!_consumoEstadoLiberado) return;
|
||||
|
||||
switch (e.Tela)
|
||||
{
|
||||
case TipoTelaIHM.Operacao:
|
||||
|
|
@ -426,6 +470,8 @@ namespace AgroBase.Forms.IHM
|
|||
private void frmIHM_Load(object sender, EventArgs e)
|
||||
{
|
||||
lblVersao.Text = "IHM Rover • v " + Variaveis.Versao;
|
||||
IniciarDiagnosticoUi();
|
||||
AtualizarControlesUI();
|
||||
AbrirTela(frmMenu);
|
||||
|
||||
if (tmrDispositivos != null)
|
||||
|
|
@ -447,29 +493,55 @@ namespace AgroBase.Forms.IHM
|
|||
|
||||
private Task tmrDispositivos_Tick()
|
||||
{
|
||||
try
|
||||
{
|
||||
if (IsDisposed || Disposing || !IsHandleCreated)
|
||||
return Task.CompletedTask;
|
||||
|
||||
if (InvokeRequired)
|
||||
{
|
||||
BeginInvoke(new Action(AtualizarControlesUI));
|
||||
}
|
||||
else
|
||||
{
|
||||
AtualizarControlesUI();
|
||||
}
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
Variaveis.MostrarLog("Erro no tmrDispositivos_Tick: " + ex.Message);
|
||||
}
|
||||
|
||||
AtualizarControlesUI();
|
||||
return Task.CompletedTask;
|
||||
}
|
||||
|
||||
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 dadosOperacao = operacao?.Sensoriamento?.Operacao;
|
||||
|
|
@ -552,7 +624,7 @@ namespace AgroBase.Forms.IHM
|
|||
bool operacaoPausada = ObterPausaParaExibicao(pausaModelo);
|
||||
bool operacaoEmergencia = ObterEmergenciaParaExibicao(emergenciaModelo);
|
||||
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);
|
||||
|
||||
// =========================================================
|
||||
|
|
@ -560,7 +632,7 @@ namespace AgroBase.Forms.IHM
|
|||
// =========================================================
|
||||
if (_telaAtual != null && !_telaAtual.IsDisposed)
|
||||
{
|
||||
(_telaAtual as ITelaNavegavel).AtualizarDadosTela();
|
||||
(_telaAtual as ITelaNavegavel)?.AtualizarDadosTela();
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -666,7 +738,7 @@ namespace AgroBase.Forms.IHM
|
|||
if (card == null)
|
||||
return;
|
||||
|
||||
if (card.Valor == valor || card.CorValor == cor)
|
||||
if (card.Valor == valor && card.CorValor == cor)
|
||||
return;
|
||||
|
||||
ExecutarNaUI(delegate
|
||||
|
|
|
|||
|
|
@ -15,6 +15,7 @@ namespace AgroBase.Forms.IHM
|
|||
public partial class frmMenu : Form, ITelaNavegavel
|
||||
{
|
||||
private bool _sincronizando;
|
||||
private bool _referenciamentoEmAndamento;
|
||||
|
||||
/// <summary>
|
||||
/// Estes eventos deixam os atalhos de campo prontos para receber
|
||||
|
|
@ -52,7 +53,9 @@ namespace AgroBase.Forms.IHM
|
|||
});
|
||||
|
||||
btnDebugMode.Click += BtnDebugMode_Click;
|
||||
btnAjustarLeverArm.Click += BtnAjustarLeverArm_Click;
|
||||
btnAjustarLeverArm.Texto = "Referenciar robô";
|
||||
btnAjustarLeverArm.IconText = "↻";
|
||||
btnAjustarLeverArm.Click += BtnReferenciarRobo_Click;
|
||||
rowFalhas.Click += LblCodigoFalha_Click;
|
||||
}
|
||||
|
||||
|
|
@ -73,8 +76,19 @@ namespace AgroBase.Forms.IHM
|
|||
AtualizarControlesUI();
|
||||
}
|
||||
|
||||
private bool _startupVisualLiberado;
|
||||
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 iaConectada = VariaveisOperacao.Operadores != null && VariaveisOperacao.Operadores.Conectado;
|
||||
var operacao = Variaveis.OperacaoEmAndamento;
|
||||
|
|
@ -134,7 +148,7 @@ namespace AgroBase.Forms.IHM
|
|||
|
||||
private bool VerificaEquipamentoIniciado()
|
||||
{
|
||||
if (!VariaveisEquipamento.Pronto)
|
||||
if (!frmInstancial.StartupConcluido || !VariaveisEquipamento.Pronto)
|
||||
{
|
||||
FuncoesIHM.MostrarAlerta(
|
||||
"Equipamento iniciando",
|
||||
|
|
@ -297,18 +311,82 @@ namespace AgroBase.Forms.IHM
|
|||
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)
|
||||
{
|
||||
AjustarLeverArmSolicitado(this, EventArgs.Empty);
|
||||
return;
|
||||
}
|
||||
if (!frmInstancial.StartupConcluido || Variaveis.Fechando)
|
||||
return "Aguarde a inicialização do equipamento.";
|
||||
if (operacao?.Controle == null || operacao.Parametros?.Controle == null ||
|
||||
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(
|
||||
"Ajustar Lever Arm",
|
||||
"O layout já está pronto. Vincule a tela ou o diálogo no evento " +
|
||||
"AjustarLeverArmSolicitado.");
|
||||
private async void BtnReferenciarRobo_Click(object sender, EventArgs e)
|
||||
{
|
||||
if (_referenciamentoEmAndamento) return;
|
||||
_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)
|
||||
|
|
|
|||
|
|
@ -94,14 +94,14 @@ namespace AgroBase.Forms.IHM
|
|||
AtualizarParametrosTela
|
||||
), op, recarregarMapa);
|
||||
}
|
||||
catch (ObjectDisposedException)
|
||||
{
|
||||
}
|
||||
catch (InvalidOperationException)
|
||||
{
|
||||
// O formulário pode estar encerrando entre a verificação
|
||||
// e o BeginInvoke. A próxima abertura sincroniza a fonte.
|
||||
}
|
||||
catch (ObjectDisposedException)
|
||||
{
|
||||
}
|
||||
|
||||
return;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -1,4 +1,4 @@
|
|||
using AgroBase.Forms.IHM;
|
||||
using AgroBase.Forms.IHM;
|
||||
using AgroBase.Models;
|
||||
using AgroBase.Services;
|
||||
using System;
|
||||
|
|
@ -27,7 +27,9 @@ namespace AgroBase.Forms
|
|||
|
||||
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 bool _allowClose;
|
||||
|
|
|
|||
|
|
@ -662,18 +662,18 @@ namespace AgroBase.Models
|
|||
DirecionalAutomatico = true,
|
||||
PulverizadorAutomatico = true,
|
||||
|
||||
FrenagemAutomaticaAoParar = true,
|
||||
FrenagemAutomaticaAoParar = false,
|
||||
ImuParadaPorInclinacao = true,
|
||||
ImuAuxilioMovimento = true,
|
||||
ImuAuxilioDirecional = true,
|
||||
ImuDirecionalPreferirArco = true,
|
||||
OakParadaPorObstaculo = true,
|
||||
DirAuxilioSonar = true,
|
||||
ImuAuxilioDirecional = false,
|
||||
ImuDirecionalPreferirArco = false,
|
||||
OakParadaPorObstaculo = false,
|
||||
DirAuxilioSonar = false,
|
||||
MovAuxilioSonar = true,
|
||||
MpcHorizonte = 4.0,
|
||||
AnteciparManobraCorredorM = 0.0,
|
||||
|
||||
MovVelocidadeSErvasPercent = 50,
|
||||
MovVelocidadeSErvasPercent = 45,
|
||||
MovVelocidadeCErvasPercent = 20,
|
||||
DirAnguloMaximo = 30,
|
||||
DirVelocidadeMovimento = 80,
|
||||
|
|
@ -685,8 +685,8 @@ namespace AgroBase.Models
|
|||
AtuDuracaoAtuacao = 0,
|
||||
AtuPercentualErvasBicoOff = 2,
|
||||
AtuPercentualErvasBicoOn = 1,
|
||||
AtuPercentualInicioPulverizacao = 85,
|
||||
AtuPressaoLinha = 22,
|
||||
AtuPercentualInicioPulverizacao = 70,
|
||||
AtuPressaoLinha = 35,
|
||||
AtuAgitadorModo = ModoAgitadorCalda.Continuo,
|
||||
AtuModoControle = ModoControleBomba.PID,
|
||||
AtuModeloCabeca = CabecasModelo.Target,
|
||||
|
|
@ -715,7 +715,7 @@ namespace AgroBase.Models
|
|||
new OperacaoModulosMandatoriosModel()
|
||||
{
|
||||
Dispositivo = T_Code.Sen,
|
||||
Mandatorio = true,
|
||||
Mandatorio = false,
|
||||
Utilizar = true,
|
||||
},
|
||||
new OperacaoModulosMandatoriosModel()
|
||||
|
|
@ -1050,31 +1050,31 @@ namespace AgroBase.Models
|
|||
DirecionalAutomatico = true,
|
||||
PulverizadorAutomatico = false,
|
||||
|
||||
FrenagemAutomaticaAoParar = true,
|
||||
FrenagemAutomaticaAoParar = false,
|
||||
ImuParadaPorInclinacao = true,
|
||||
ImuAuxilioMovimento = true,
|
||||
ImuAuxilioDirecional = true,
|
||||
ImuDirecionalPreferirArco = true,
|
||||
OakParadaPorObstaculo = true,
|
||||
DirAuxilioSonar = true,
|
||||
ImuAuxilioDirecional = false,
|
||||
ImuDirecionalPreferirArco = false,
|
||||
OakParadaPorObstaculo = false,
|
||||
DirAuxilioSonar = false,
|
||||
MovAuxilioSonar = true,
|
||||
MpcHorizonte = 4.0,
|
||||
AnteciparManobraCorredorM = 0,
|
||||
|
||||
MovVelocidadeSErvasPercent = 60,
|
||||
MovVelocidadeSErvasPercent = 50,
|
||||
MovVelocidadeCErvasPercent = 20,
|
||||
DirAnguloMaximo = 30,
|
||||
DirVelocidadeMovimento = 80,
|
||||
DirTipoMovimento = TiposControladorDirecional.MPC,
|
||||
|
||||
DistanciaManobra = 3.0,
|
||||
DistanciaManobra = 2.2,
|
||||
|
||||
AtuAlturaAreaPulverizacao = 10,
|
||||
AtuDuracaoAtuacao = 0,
|
||||
AtuPercentualErvasBicoOff = 2,
|
||||
AtuPercentualErvasBicoOn = 1,
|
||||
AtuPercentualInicioPulverizacao = 85,
|
||||
AtuPressaoLinha = 18,
|
||||
AtuPercentualInicioPulverizacao = 70,
|
||||
AtuPressaoLinha = 22,
|
||||
AtuAgitadorModo = ModoAgitadorCalda.SemAgitacao,
|
||||
AtuModoControle = ModoControleBomba.PID,
|
||||
AtuModeloCabeca = CabecasModelo.Target,
|
||||
|
|
@ -1103,13 +1103,13 @@ namespace AgroBase.Models
|
|||
new OperacaoModulosMandatoriosModel()
|
||||
{
|
||||
Dispositivo = T_Code.Sen,
|
||||
Mandatorio = true,
|
||||
Mandatorio = false,
|
||||
Utilizar = true,
|
||||
},
|
||||
new OperacaoModulosMandatoriosModel()
|
||||
{
|
||||
Dispositivo = T_Code.Atu,
|
||||
Mandatorio = true,
|
||||
Mandatorio = false,
|
||||
Utilizar = true,
|
||||
},
|
||||
new OperacaoModulosMandatoriosModel()
|
||||
|
|
@ -1119,6 +1119,12 @@ namespace AgroBase.Models
|
|||
Utilizar = true,
|
||||
},
|
||||
new OperacaoModulosMandatoriosModel()
|
||||
{
|
||||
Dispositivo = T_Code.Cam,
|
||||
Mandatorio = false,
|
||||
Utilizar = true,
|
||||
},
|
||||
new OperacaoModulosMandatoriosModel()
|
||||
{
|
||||
Dispositivo = T_Code.Snr,
|
||||
Mandatorio = true,
|
||||
|
|
|
|||
|
|
@ -111,6 +111,18 @@ namespace AgroBase.Models
|
|||
private double ErroOrientacaoMaximoRecuperacaoRetornoGraus { get; set; } = 65.0;
|
||||
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 ConfirmacoesMinimasEntradaProximoCorredor { get; set; } = 2;
|
||||
|
||||
|
|
@ -6118,7 +6130,8 @@ namespace AgroBase.Models
|
|||
|
||||
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];
|
||||
|
||||
|
|
@ -6128,7 +6141,7 @@ namespace AgroBase.Models
|
|||
if (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);
|
||||
|
||||
|
|
@ -6494,6 +6507,44 @@ namespace AgroBase.Models
|
|||
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);
|
||||
op?.Sensoriamento?.InserirLog(
|
||||
T_Code.Trj,
|
||||
|
|
@ -7032,6 +7083,43 @@ namespace AgroBase.Models
|
|||
);
|
||||
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);
|
||||
|
||||
return (trecho.Any(), pontos);
|
||||
|
|
@ -7178,6 +7266,358 @@ namespace AgroBase.Models
|
|||
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
|
||||
|
||||
#region CASO 2
|
||||
|
|
|
|||
Loading…
Reference in New Issue