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:
parent
bd1526cdaf
commit
5f04f9c5c6
|
|
@ -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;
|
||||||
|
|
|
||||||
|
|
@ -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
|
||||||
|
|
|
||||||
|
|
@ -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)
|
||||||
|
|
|
||||||
|
|
@ -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;
|
||||||
}
|
}
|
||||||
|
|
|
||||||
|
|
@ -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;
|
||||||
|
|
|
||||||
|
|
@ -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,
|
||||||
|
|
|
||||||
|
|
@ -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
|
||||||
|
|
|
||||||
Loading…
Reference in New Issue