Zendion/agrobot_base
Zendion
/
agrobot_base
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Criado método para abrir curva ao antecipar a saída do corredor

This commit is contained in:
Diego Freitas 2026-02-26 21:51:16 -03:00
parent 89e5f1ae41
commit 163c95adfd
1 changed files with 128 additions and 47 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

175
AgroBase/AgroBase/Models/TrajetoriaMapaOperacaoModel.cs
View File

@ -773,7 +773,7 @@ namespace AgroBase.Models
{ {
if (CorredorAtual == null) return; if (CorredorAtual == null) return;
bool modoSimulador = false && Variaveis.OperacaoEmAndamento.Simulando; bool modoSimulador = true && Variaveis.OperacaoEmAndamento.Simulando;
// Limiar de "margem" para considerar proximidade do fim // Limiar de "margem" para considerar proximidade do fim
double limiarDistanciaMargem = DistanciaErroMapaPlantacao > -1 ? DistanciaErroMapaPlantacao : 5.0; double limiarDistanciaMargem = DistanciaErroMapaPlantacao > -1 ? DistanciaErroMapaPlantacao : 5.0;
@ -880,69 +880,74 @@ namespace AgroBase.Models
bool podeUsarSegmentacao = modoSimulador || (dadoFresco && dv.CorredorConfirmado); bool podeUsarSegmentacao = modoSimulador || (dadoFresco && dv.CorredorConfirmado);
bool abrirCurva = false; // mantém seu flag por enquanto bool fimAntecipado = podeUsarSegmentacao && pertoDoFim && foraDoCorredorAgora && foraContinuoSuficiente;
if (podeUsarSegmentacao && pertoDoFim && foraDoCorredorAgora && foraContinuoSuficiente) Console.WriteLine($"podeUsarSegmentacao: {podeUsarSegmentacao}, pertoDoFim: {pertoDoFim}, foraDoCorredorAgora: {foraDoCorredorAgora}, foraContinuoSuficiente: {foraContinuoSuficiente}");
if (fimAntecipado)
{ {
// 4) Aqui consideramos que o corredor acabou antes do fim do mapa List<PontoTrajetoriaModel> pontosRemover = CorredorAtual.Pontos.Where(x => !x.Visitado).ToList();
double anguloControle = Variaveis.OperacaoEmAndamento.Controle.Angulo; pontosRemover.ForEach(x => x.Visitado = true);
if (abrirCurva && !CorredorAtual.Ultimo)
{
anguloControle = CorredorAtual.idxRuaDireita > CorredorAtual.idxRuaEsquerda
? -Variaveis.OperacaoEmAndamento.Parametros.Controle.DirAnguloMaximo
: Variaveis.OperacaoEmAndamento.Parametros.Controle.DirAnguloMaximo;
}
int pontosMarcar = 0;
foreach (var ponto in CorredorAtual.Pontos.Where(x => !x.Visitado))
{
ponto.Visitado = true;
pontosMarcar++;
}
if (!CorredorAtual.Ultimo) if (!CorredorAtual.Ultimo)
{ {
// Marca ponto de ligação de entrada do próximo corredor como visitado foreach (var ponto in pontosRemover)
_TrajetoriaFixa[CorredorAtual.Pontos[CorredorAtual.QtdPontos - 1].idxPonto + 1].Visitado = true; {
CorredorAtual.Pontos.Remove(ponto);
_TrajetoriaFixa.Remove(ponto);
}
// Avança bem a janela para "pular" o restinho do corredor if (CorredorAtual.Pontos.Count >= 2)
AtualizarIndicesJanela((pontosMarcar + 2) * 2); {
AtualizarTrajetoriaJanela(); var pls = CorredorAtual.Pontos[CorredorAtual.Pontos.Count - 1];
DefinirPontoAtual(); var pbs = CorredorAtual.Pontos[CorredorAtual.Pontos.Count - 2];
AtualizarCorredorAtual(); pls.AlterarTipo(TipoPontoRua.LigacaoSaida);
pbs.AlterarTipo(TipoPontoRua.BordaSaida);
_TrajetoriaFixa.FirstOrDefault(x => ReferenceEquals(x, pls))?.AlterarTipo(pls.Tipo);
_TrajetoriaFixa.FirstOrDefault(x => ReferenceEquals(x, pbs))?.AlterarTipo(pbs.Tipo);
}
double anguloControle = CorredorAtual.idxRuaDireita > CorredorAtual.idxRuaEsquerda ? -Variaveis.OperacaoEmAndamento.Parametros.Controle.DirAnguloMaximo : Variaveis.OperacaoEmAndamento.Parametros.Controle.DirAnguloMaximo;
double anguloDesvio = PontoAtual.Orientacao + anguloControle;
var pontoDesvio = GPSUtils.GerarPontoDeslocado(PontoAtual.Posicao, anguloDesvio, 1.5);
IncluirDesvioNaTrajetoria(new List<GPSModel>() { pontoDesvio }, PontoAtual.idxPonto);
// Garante que os primeiros pontos do corredor atual estão como visitados // Garante que os primeiros pontos do corredor atual estão como visitados
for (int i = 0; i < pontosMarcar && i < CorredorAtual.Pontos.Count; i++) var pc = _Corredores[CorredorAtual.Idx + 1];
// acha o ponto mais próximo
(GPSModel pp, double distanciaAtual) = GPSUtils.CalcularPontoMaisProximoTrajetoria(Variaveis.OperacaoEmAndamento.Sensoriamento.Gps, pc.Pontos.Select(x => x.Posicao).ToList());
var _pp = pc.Pontos.FirstOrDefault(x => Math.Abs(x.Posicao.Latitude - pp.Latitude) < 1e-8 && Math.Abs(x.Posicao.Longitude - pp.Longitude) < 1e-8);
if (_pp != null)
{ {
CorredorAtual.Pontos[i].Visitado = true; int idxProx = _pp.idxPontoCorredor;
}
AtualizarIndicesJanela(); // garante que não vai remover tudo (pra sobrar pelo menos 2 pontos)
AtualizarTrajetoriaJanela(); int qtdRemoverProx = Math.Max(0, Math.Min(idxProx, pc.Pontos.Count - 2));
DefinirPontoAtual();
DefinirProximoPonto();
if (abrirCurva) var pontosRemoverProx = pc.Pontos.Take(qtdRemoverProx).ToList();
{
var pControle = Variaveis.OperacaoEmAndamento.Parametros.Controle;
if (pControle.MovimentoAutomatico) foreach (var p in pontosRemoverProx)
{ {
Variaveis.OperacaoEmAndamento.Controle.PercentualVelocidadeSP = Variaveis.OperacaoEmAndamento.Parametros.Controle.MovVelocidadeCErvasPercent; pc.Pontos.Remove(p);
Variaveis.OperacaoEmAndamento.Controle.TiposControle.FirstOrDefault(x => x.Tipo == T_Code.Mov).DelayEnvioComandoExtra = 2000; _TrajetoriaFixa.Remove(p);
Variaveis.OperacaoEmAndamento.AtualizarDadosControle(false, new List<T_Code>() { T_Code.Mov });
} }
if (pControle.DirecionalAutomatico) if (pc.Pontos.Count >= 2)
{ {
Console.WriteLine($"Angulo de controle definido de {Variaveis.OperacaoEmAndamento.Controle.Angulo} para {anguloControle}"); var ple = pc.Pontos[0];
Variaveis.OperacaoEmAndamento.Controle.Angulo = anguloControle; var pbe = pc.Pontos[1];
Variaveis.OperacaoEmAndamento.Controle.TipoMovimento = TipoMovimentoDirecional.MovimentoArco; ple.AlterarTipo(TipoPontoRua.LigacaoEntrada);
Variaveis.OperacaoEmAndamento.Controle.TiposControle.FirstOrDefault(x => x.Tipo == T_Code.Dir).DelayEnvioComandoExtra = 1500; pbe.AlterarTipo(TipoPontoRua.BordaEntrada);
Variaveis.OperacaoEmAndamento.AtualizarDadosControle(false, new List<T_Code>() { T_Code.Dir }); _TrajetoriaFixa.FirstOrDefault(x => ReferenceEquals(x, ple))?.AlterarTipo(ple.Tipo);
_TrajetoriaFixa.FirstOrDefault(x => ReferenceEquals(x, pbe))?.AlterarTipo(pbe.Tipo);
} }
} }
ReindexarTrajetoriaFixa();
RedisService.AtualizarCampos(CtxKey.DadosOperacao, ("configurado", false));
Variaveis.OperacaoEmAndamento.Sensoriamento.AtualizarDados();
} }
} }
} }
@ -2300,6 +2305,77 @@ namespace AgroBase.Models
#endregion #endregion
private void IncluirDesvioNaTrajetoria(List<GPSModel> pontos, int idxPontoApartir)
{
if (pontos == null || pontos.Count == 0)
return;
// 1. Achar o ponto base na trajetória fixa
var pontoBase = _TrajetoriaFixa.FirstOrDefault(x => x.idxPonto == idxPontoApartir);
if (pontoBase == null)
return;
// Conferir corredor
var corredor = _Corredores[pontoBase.idxCorredor];
// 2. Gerar novos pontos de desvio (sem se preocupar ainda com idxPonto / idxPontoCorredor)
var novosPontos = new List<PontoTrajetoriaModel>();
var pa = pontoBase;
foreach (var pontoGps in pontos)
{
var novoPonto = new PontoTrajetoriaModel(TipoPontoRua.Desvio)
{
// idxPonto e idxPontoCorredor serão ajustados depois
idxCorredor = pa.idxCorredor,
Posicao = pontoGps,
Direcao = pa.Direcao,
LarguraCorredor = pa.LarguraCorredor,
Orientacao = GPSUtils.CalcularOrientacao(pontoGps, pa.Posicao),
Visitado = false
};
novosPontos.Add(novoPonto);
pa = novoPonto;
}
// 3. Inserir na lista global a partir do ponto base
int insertIdxGlobal = _TrajetoriaFixa.IndexOf(pontoBase) + 1;
int insertIdxCorredor = corredor.Pontos.IndexOf(pontoBase) + 1;
foreach (var np in novosPontos)
{
_TrajetoriaFixa.Insert(insertIdxGlobal, np);
corredor.Pontos.Insert(insertIdxCorredor, np);
insertIdxGlobal++;
insertIdxCorredor++;
}
// 4. Reindexar listas
ReindexarTrajetoriaFixa();
}
private void ReindexarTrajetoriaFixa()
{
// 1. Reindexar idxPonto globalmente
for (int i = 0; i < _TrajetoriaFixa.Count; i++)
{
_TrajetoriaFixa[i].idxPonto = i;
}
// 2. Reindexar idxPontoCorredor para cada corredor
for (int c = 0; c < _Corredores.Count; c++)
{
var corr = _Corredores[c];
for (int i = 0; i < corr.Pontos.Count; i++)
{
corr.Pontos[i].idxPontoCorredor = i;
}
}
}
} }
public class CorredorTrajetoriaModel public class CorredorTrajetoriaModel
@ -2426,7 +2502,7 @@ namespace AgroBase.Models
public GPSModel Posicao { get; set; } public GPSModel Posicao { get; set; }
public double Orientacao { get; set; } public double Orientacao { get; set; }
public DirecaoCarroRua Direcao { get; set; } public DirecaoCarroRua Direcao { get; set; }
public TipoPontoRua Tipo { get; } public TipoPontoRua Tipo { private set; get; }
public double LarguraCorredor { get; set; } public double LarguraCorredor { get; set; }
public double DistanciaMargem { get; private set; } public double DistanciaMargem { get; private set; }
public bool Visitado { get; set; } = false; public bool Visitado { get; set; } = false;
@ -2460,6 +2536,11 @@ namespace AgroBase.Models
PontoLigacao = Tipo == Enums.TipoPontoRua.LigacaoEntrada || Tipo == Enums.TipoPontoRua.LigacaoSaida; PontoLigacao = Tipo == Enums.TipoPontoRua.LigacaoEntrada || Tipo == Enums.TipoPontoRua.LigacaoSaida;
} }
public void AlterarTipo(Enums.TipoPontoRua novo)
{
Tipo = novo;
}
public void AtualizarPropriedades() public void AtualizarPropriedades()
{ {
AtualizarDistanciaAtual(); AtualizarDistanciaAtual();