Merge branch 'op_control' of http://zendioninc.com.br:3000/Zendion/agrobot_base into op_control
This commit is contained in:
Diego Freitas
commit
f4e1a2fe2e
|
|
@ -773,7 +773,7 @@ namespace AgroBase.Models
|
|||
{
|
||||
if (CorredorAtual == null) return;
|
||||
|
||||
bool modoSimulador = false && Variaveis.OperacaoEmAndamento.Simulando;
|
||||
bool modoSimulador = true && Variaveis.OperacaoEmAndamento.Simulando;
|
||||
|
||||
// Limiar de "margem" para considerar proximidade do fim
|
||||
double limiarDistanciaMargem = DistanciaErroMapaPlantacao > -1 ? DistanciaErroMapaPlantacao : 5.0;
|
||||
|
|
@ -880,69 +880,74 @@ namespace AgroBase.Models
|
|||
|
||||
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
|
||||
double anguloControle = Variaveis.OperacaoEmAndamento.Controle.Angulo;
|
||||
|
||||
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++;
|
||||
}
|
||||
List<PontoTrajetoriaModel> pontosRemover = CorredorAtual.Pontos.Where(x => !x.Visitado).ToList();
|
||||
pontosRemover.ForEach(x => x.Visitado = true);
|
||||
|
||||
if (!CorredorAtual.Ultimo)
|
||||
{
|
||||
// Marca ponto de ligação de entrada do próximo corredor como visitado
|
||||
_TrajetoriaFixa[CorredorAtual.Pontos[CorredorAtual.QtdPontos - 1].idxPonto + 1].Visitado = true;
|
||||
foreach (var ponto in pontosRemover)
|
||||
{
|
||||
CorredorAtual.Pontos.Remove(ponto);
|
||||
_TrajetoriaFixa.Remove(ponto);
|
||||
}
|
||||
|
||||
// Avança bem a janela para "pular" o restinho do corredor
|
||||
AtualizarIndicesJanela((pontosMarcar + 2) * 2);
|
||||
AtualizarTrajetoriaJanela();
|
||||
DefinirPontoAtual();
|
||||
AtualizarCorredorAtual();
|
||||
if (CorredorAtual.Pontos.Count >= 2)
|
||||
{
|
||||
var pls = CorredorAtual.Pontos[CorredorAtual.Pontos.Count - 1];
|
||||
var pbs = CorredorAtual.Pontos[CorredorAtual.Pontos.Count - 2];
|
||||
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
|
||||
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();
|
||||
AtualizarTrajetoriaJanela();
|
||||
DefinirPontoAtual();
|
||||
DefinirProximoPonto();
|
||||
// garante que não vai remover tudo (pra sobrar pelo menos 2 pontos)
|
||||
int qtdRemoverProx = Math.Max(0, Math.Min(idxProx, pc.Pontos.Count - 2));
|
||||
|
||||
if (abrirCurva)
|
||||
{
|
||||
var pControle = Variaveis.OperacaoEmAndamento.Parametros.Controle;
|
||||
var pontosRemoverProx = pc.Pontos.Take(qtdRemoverProx).ToList();
|
||||
|
||||
if (pControle.MovimentoAutomatico)
|
||||
foreach (var p in pontosRemoverProx)
|
||||
{
|
||||
Variaveis.OperacaoEmAndamento.Controle.PercentualVelocidadeSP = Variaveis.OperacaoEmAndamento.Parametros.Controle.MovVelocidadeCErvasPercent;
|
||||
Variaveis.OperacaoEmAndamento.Controle.TiposControle.FirstOrDefault(x => x.Tipo == T_Code.Mov).DelayEnvioComandoExtra = 2000;
|
||||
Variaveis.OperacaoEmAndamento.AtualizarDadosControle(false, new List<T_Code>() { T_Code.Mov });
|
||||
pc.Pontos.Remove(p);
|
||||
_TrajetoriaFixa.Remove(p);
|
||||
}
|
||||
|
||||
if (pControle.DirecionalAutomatico)
|
||||
if (pc.Pontos.Count >= 2)
|
||||
{
|
||||
Console.WriteLine($"Angulo de controle definido de {Variaveis.OperacaoEmAndamento.Controle.Angulo} para {anguloControle}");
|
||||
Variaveis.OperacaoEmAndamento.Controle.Angulo = anguloControle;
|
||||
Variaveis.OperacaoEmAndamento.Controle.TipoMovimento = TipoMovimentoDirecional.MovimentoArco;
|
||||
Variaveis.OperacaoEmAndamento.Controle.TiposControle.FirstOrDefault(x => x.Tipo == T_Code.Dir).DelayEnvioComandoExtra = 1500;
|
||||
Variaveis.OperacaoEmAndamento.AtualizarDadosControle(false, new List<T_Code>() { T_Code.Dir });
|
||||
var ple = pc.Pontos[0];
|
||||
var pbe = pc.Pontos[1];
|
||||
ple.AlterarTipo(TipoPontoRua.LigacaoEntrada);
|
||||
pbe.AlterarTipo(TipoPontoRua.BordaEntrada);
|
||||
_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
|
||||
|
||||
|
||||
|
||||
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
|
||||
|
|
@ -2426,7 +2502,7 @@ namespace AgroBase.Models
|
|||
public GPSModel Posicao { get; set; }
|
||||
public double Orientacao { get; set; }
|
||||
public DirecaoCarroRua Direcao { get; set; }
|
||||
public TipoPontoRua Tipo { get; }
|
||||
public TipoPontoRua Tipo { private set; get; }
|
||||
public double LarguraCorredor { get; set; }
|
||||
public double DistanciaMargem { get; private set; }
|
||||
public bool Visitado { get; set; } = false;
|
||||
|
|
@ -2460,6 +2536,11 @@ namespace AgroBase.Models
|
|||
PontoLigacao = Tipo == Enums.TipoPontoRua.LigacaoEntrada || Tipo == Enums.TipoPontoRua.LigacaoSaida;
|
||||
}
|
||||
|
||||
public void AlterarTipo(Enums.TipoPontoRua novo)
|
||||
{
|
||||
Tipo = novo;
|
||||
}
|
||||
|
||||
public void AtualizarPropriedades()
|
||||
{
|
||||
AtualizarDistanciaAtual();
|
||||
|
|
|
|||
Loading…
Reference in New Issue