570 lines
25 KiB
C#
570 lines
25 KiB
C#
using AgroBase.Forms.Operacoes;
|
|
using AgroBase.Models.Modules;
|
|
using AgroBase.Services;
|
|
using Newtonsoft.Json;
|
|
using System;
|
|
using System.Collections.Generic;
|
|
using System.Linq;
|
|
using System.Windows.Forms;
|
|
using static AgroBase.Models.Enums;
|
|
|
|
namespace AgroBase.Models.Operacoes
|
|
{
|
|
public class OperacaoMapaGPSModel
|
|
{
|
|
public Form frmOperacao { get; set; } = new frmOperacaoMapaGPS();
|
|
public string frmOperacaoNome { get; set; } = "frmOperacaoMapaGPS";
|
|
public bool Concluido
|
|
{
|
|
get
|
|
{
|
|
return Variaveis.OperacaoEmAndamento.Mapa.IdxRuaAtual >= Variaveis.OperacaoEmAndamento.Mapa.TrajetoriaMapa.Count();
|
|
}
|
|
}
|
|
public OperacaoRefRuaGPS ReferencialRuaGPS { get; set; } = new OperacaoRefRuaGPS();
|
|
public OperacaoMapaGPSSensoriamentoModel Sensoriamento { get; set; } = new OperacaoMapaGPSSensoriamentoModel();
|
|
public List<StatusCarroMapa> StatusDentroRua = new List<StatusCarroMapa>()
|
|
{
|
|
StatusCarroMapa.EntrandoRua,
|
|
StatusCarroMapa.CaminhandoRua,
|
|
StatusCarroMapa.SaindoRua
|
|
};
|
|
|
|
public void CalcularAnguloInclinacao(StatusCarroMapa statusCarroRua)
|
|
{
|
|
double erroOrientacao = 0.0;
|
|
double anguloCarro = Variaveis.OperacaoEmAndamento.Sensoriamento.AnguloCarro;
|
|
double anguloOposto = (anguloCarro + 180) % 360;
|
|
double anguloRua = Variaveis.OperacaoEmAndamento.Mapa.AnguloCaminho;
|
|
double anguloCamera = Sensoriamento.AnguloRuaCamera;
|
|
|
|
// Calcula a diferença de ângulo entre o carro e a rua
|
|
double diferencaAngulo = GPSService.CalcularDiferencaAngulo(anguloCarro, anguloRua);
|
|
|
|
erroOrientacao = diferencaAngulo;
|
|
|
|
Sensoriamento.AnguloDif = erroOrientacao;
|
|
|
|
|
|
// Calcule o erro lateral baseado nas distâncias entre as ruas do mapa atraves do GPS
|
|
double erroLateral = statusCarroRua == StatusCarroMapa.CaminhandoRua && false ? Variaveis.OperacaoEmAndamento.Mapa.DistanciaEsquerda - Variaveis.OperacaoEmAndamento.Mapa.DistanciaDireita : 0;
|
|
|
|
double erroUltrassom = UltrasonicA05Helper.CalcularErroUltrassons();
|
|
|
|
double erroCombinado = CalcularErroCombinado(erroLateral, erroOrientacao, erroUltrassom, anguloCarro, anguloRua, statusCarroRua);
|
|
|
|
|
|
|
|
// Atualiza o PID com o erro combinado
|
|
if (statusCarroRua != StatusCarroMapa.Parado)
|
|
{
|
|
var PID = Variaveis.OperacaoEmAndamento.Controle.PIDdirecional;
|
|
if (Variaveis.OperacaoEmAndamento.OpMapaGPS.ReferencialRuaGPS.ManobrandoEntreRuas)
|
|
{
|
|
PID.LimiteSaida = Variaveis.OperacaoEmAndamento.Controle.Angulo_Max * 0.75;
|
|
}
|
|
else
|
|
{
|
|
PID.LimiteSaida = Variaveis.OperacaoEmAndamento.Controle.Angulo_Max;
|
|
}
|
|
PID.Atualizar(erroCombinado, 0);
|
|
}
|
|
double correcaoAngulo = Variaveis.OperacaoEmAndamento.Controle.PIDdirecional.Saida;
|
|
|
|
// Aplica a correção. Isso pode incluir limites para evitar comandos excessivos.
|
|
if (correcaoAngulo > Variaveis.OperacaoEmAndamento.Controle.Angulo_Max)
|
|
{
|
|
correcaoAngulo = Variaveis.OperacaoEmAndamento.Controle.Angulo_Max;
|
|
}
|
|
else if (correcaoAngulo < Variaveis.OperacaoEmAndamento.Controle.Angulo_Min)
|
|
{
|
|
correcaoAngulo = Variaveis.OperacaoEmAndamento.Controle.Angulo_Min;
|
|
}
|
|
|
|
Variaveis.OperacaoEmAndamento.Controle.Angulo = correcaoAngulo;
|
|
}
|
|
|
|
private double CalcularErroCombinado_bkp(double erroLateral, double erroOrientacao, double anguloCarro, StatusCarroMapa statusCarroRua)
|
|
{
|
|
bool carroDentroRua = StatusDentroRua.Contains(statusCarroRua);
|
|
|
|
// Manter o erro lateral sempre com duas casas decimais
|
|
double multiplicadorErroLateral = erroLateral < 1 ? 10 : erroLateral > 10 && erroLateral < 100 ? 0 : erroLateral > 100 ? 0.1 : 1;
|
|
double erroPosicaoLateral =
|
|
Variaveis.OperacaoEmAndamento.Mapa.TrajetoriaMapa.Count() == 1 ? 0 :
|
|
(erroLateral * (carroDentroRua ? multiplicadorErroLateral : 1));
|
|
|
|
double PesoLateralDentroRua = erroPosicaoLateral / 10;
|
|
if (PesoLateralDentroRua > 1)
|
|
{
|
|
PesoLateralDentroRua = 1;
|
|
}
|
|
if (PesoLateralDentroRua < 0)
|
|
{
|
|
PesoLateralDentroRua *= -1;
|
|
}
|
|
double PesoOrientacaoDentroRua = 1 - PesoLateralDentroRua;
|
|
|
|
// Combina os erros. Ajuste os pesos conforme necessário para o seu caso específico.
|
|
/*double pesoOrientacao = erroPosicaoLateral == 0 ? 1 : carroDentroRua ? PesoOrientacaoDentroRua : 1 - PesoOrientacaoDentroRua;
|
|
double pesoPosicaoLateral = erroPosicaoLateral == 0 ? 0 : carroDentroRua ? PesoLateralDentroRua : 1 - PesoLateralDentroRua;*/
|
|
double erroCombinado = (erroOrientacao * PesoOrientacaoDentroRua) + (erroPosicaoLateral * PesoLateralDentroRua);
|
|
|
|
|
|
Direcao direcaoVirar = erroCombinado < anguloCarro ? Direcao.Esquerda : Direcao.Direita;
|
|
double erroComparar = erroCombinado < 0 ? erroCombinado * -1 : erroCombinado;
|
|
if (erroCombinado > 30 && false)
|
|
{
|
|
if (direcaoVirar == Direcao.Esquerda && Variaveis.OperacaoEmAndamento.Mapa.DistanciaEsquerda > Variaveis.OperacaoEmAndamento.Mapa.DistanciaDireita)
|
|
{
|
|
erroCombinado -= 360;
|
|
/*if (erroCombinado < 0)
|
|
{
|
|
erroCombinado *= -1;
|
|
}*/
|
|
}
|
|
else if (direcaoVirar == Direcao.Direita && Variaveis.OperacaoEmAndamento.Mapa.DistanciaDireita > Variaveis.OperacaoEmAndamento.Mapa.DistanciaEsquerda)
|
|
{
|
|
erroCombinado -= 360;
|
|
/*if (erroCombinado < 0)
|
|
{
|
|
erroCombinado *= -1;
|
|
}*/
|
|
}
|
|
}
|
|
|
|
direcaoVirar = erroCombinado < anguloCarro ? Direcao.Esquerda : Direcao.Direita;
|
|
|
|
|
|
return erroCombinado;
|
|
}
|
|
|
|
private double CalcularErroCombinado(double erroLateral, double erroOrientacao, double erroUltrassons, double anguloCarro, double anguloRua, StatusCarroMapa statusCarroRua)
|
|
{
|
|
bool carroDentroRua = StatusDentroRua.Contains(statusCarroRua);
|
|
|
|
// Mantém o erro lateral sempre com duas casas decimais e ajusta o multiplicador
|
|
double multiplicadorErroLateral = erroLateral < 1 ? 10 : erroLateral > 10 && erroLateral < 100 ? 0 : erroLateral > 100 ? 0.1 : 1;
|
|
double erroPosicaoLateral =
|
|
Variaveis.OperacaoEmAndamento.Mapa.TrajetoriaMapa.Count() == 1 ? 0 :
|
|
(erroLateral * (carroDentroRua ? multiplicadorErroLateral : 1));
|
|
|
|
double PesoLateralDentroRua = erroPosicaoLateral / 10;
|
|
PesoLateralDentroRua = FuncoesMatematicas.Clamp(PesoLateralDentroRua, 0, 1);
|
|
double PesoOrientacaoDentroRua = (1 - PesoLateralDentroRua) * 0.7; // 70% para a orientação principal
|
|
double PesoUltrassons = 1 - PesoOrientacaoDentroRua - PesoLateralDentroRua; // Peso restante para o ultrassom
|
|
|
|
// Calcula o erro combinado, ponderando os três erros
|
|
double erroCombinado =
|
|
(erroOrientacao * PesoOrientacaoDentroRua) +
|
|
(erroPosicaoLateral * PesoLateralDentroRua) +
|
|
(erroUltrassons * PesoUltrassons);
|
|
|
|
//Direcao direcaoVirar = erroCombinado < anguloCarro ? Direcao.Esquerda : Direcao.Direita;
|
|
Direcao direcaoVirar = CalcularDirecaoParaVirar(anguloCarro, anguloRua);
|
|
double erroComparar = erroCombinado < 0 ? -erroCombinado : erroCombinado;
|
|
|
|
// Aplica correções extras quando o erro é significativo e comparável com a distância à borda da rua
|
|
if (erroCombinado > 30 && false)
|
|
{
|
|
if (direcaoVirar == Direcao.Esquerda && Variaveis.OperacaoEmAndamento.Mapa.DistanciaEsquerda > Variaveis.OperacaoEmAndamento.Mapa.DistanciaDireita)
|
|
{
|
|
erroCombinado -= 360;
|
|
}
|
|
else if (direcaoVirar == Direcao.Direita && Variaveis.OperacaoEmAndamento.Mapa.DistanciaDireita > Variaveis.OperacaoEmAndamento.Mapa.DistanciaEsquerda)
|
|
{
|
|
erroCombinado -= 360;
|
|
}
|
|
}
|
|
|
|
return erroCombinado;
|
|
}
|
|
|
|
public static Direcao CalcularDirecaoParaVirar(double anguloCarro, double anguloRua)
|
|
{
|
|
// Normaliza os ângulos para o intervalo [0, 360)
|
|
anguloCarro = ((anguloCarro % 360) + 360) % 360;
|
|
anguloRua = ((anguloRua % 360) + 360) % 360;
|
|
|
|
// Calcula a diferença angular nos dois sentidos
|
|
double diferencaHorario = (anguloRua - anguloCarro + 360) % 360;
|
|
double diferencaAntihorario = (anguloCarro - anguloRua + 360) % 360;
|
|
|
|
// Determina a direção com a menor distância
|
|
if (diferencaHorario <= diferencaAntihorario)
|
|
{
|
|
return Direcao.Direita; // Virar no sentido horário
|
|
}
|
|
else
|
|
{
|
|
return Direcao.Esquerda; // Virar no sentido antihorário
|
|
}
|
|
}
|
|
|
|
|
|
private void DefinirTipoMovimento(StatusCarroMapa statusCarro)
|
|
{
|
|
Variaveis.OperacaoEmAndamento.Controle.TipoMovimento =
|
|
Variaveis.OperacaoEmAndamento.OpMapaGPS.ReferencialRuaGPS.ManobrandoEntreRuas ? TipoMovimentoDirecional.MovimentoArco :
|
|
TipoMovimentoDirecional.RodasDianteiras;
|
|
}
|
|
|
|
public void CalculaDadosMovimentacaoAutonoma()
|
|
{
|
|
if (Variaveis.OperacaoEmAndamento.Modo != ModoOperacao.MapaGPS)
|
|
{
|
|
return;
|
|
}
|
|
|
|
var statusAtual = ReferencialRuaGPS.StatusAtual;
|
|
|
|
DefinirTipoMovimento(statusAtual);
|
|
|
|
CalcularAnguloInclinacao(statusAtual);
|
|
|
|
var Controle = Variaveis.OperacaoEmAndamento.Controle;
|
|
double limiteAnguloVelocidadeMin = Controle.Angulo_Max * 0.2;
|
|
|
|
if (statusAtual == StatusCarroMapa.Parado)
|
|
{
|
|
var BicosNaoAtuados = new List<AtuadorBicoModel>(Variaveis.OperacaoEmAndamento.DispAtu?.Dados?.BicosPulverizadores);
|
|
BicosNaoAtuados.ForEach(x => x.ComandoAtuar = false);
|
|
Controle.BicosAtuados = new List<AtuadorBicoModel>(BicosNaoAtuados.ToList());
|
|
Controle.PercentualVelocidadeSP = 0;
|
|
Controle.Angulo = 0;
|
|
}
|
|
else if (statusAtual == StatusCarroMapa.Direcionando)
|
|
{
|
|
//bool Manobrando = (ReferencialRuaGPS.EmManobra && Variaveis.OperacaoEmAndamento.Mapa.DistanciaAteProximoPonto <= Variaveis.OperacaoEmAndamento.Mapa.DistanciaProjecaoRua);
|
|
bool Manobrando = Variaveis.OperacaoEmAndamento.Mapa.DistanciaAteProximoPonto <= Variaveis.OperacaoEmAndamento.Mapa.DistanciaProjecaoRua;
|
|
bool DesvioSonar = (KinectService.Iniciado && Variaveis.OperacaoEmAndamento.Sensoriamento.SonarFrontal_Kinect.DesvioNecessario);
|
|
if (Manobrando || DesvioSonar)
|
|
{
|
|
Controle.PercentualVelocidadeSP = Controle.PercentualVelocidadeMin;
|
|
}
|
|
else if (Variaveis.OperacaoEmAndamento.Mapa.IdxRuaAtual > 0 && Variaveis.OperacaoEmAndamento.Mapa.DistanciaAteProximoPonto < 10.0)
|
|
{
|
|
Controle.PercentualVelocidadeSP = Controle.PercentualVelocidadeMin;
|
|
}
|
|
else if (Math.Abs(Controle.Angulo) > limiteAnguloVelocidadeMin)
|
|
{
|
|
Controle.PercentualVelocidadeSP = Controle.PercentualVelocidadeMin;
|
|
}
|
|
else
|
|
{
|
|
/*double ag0 = GPSService.CalcularOrientacao(GPSService.UltimaLeitura, Variaveis.OperacaoEmAndamento.Mapa.TrajetoriaDinamica[1]);
|
|
double ag1 = GPSService.CalcularOrientacao(GPSService.UltimaLeitura, Variaveis.OperacaoEmAndamento.Mapa.TrajetoriaDinamica[2]);
|
|
double ag2 = GPSService.CalcularOrientacao(GPSService.UltimaLeitura, Variaveis.OperacaoEmAndamento.Mapa.TrajetoriaDinamica[2]);
|
|
double ag3 = GPSService.CalcularOrientacao(GPSService.UltimaLeitura, Variaveis.OperacaoEmAndamento.Mapa.TrajetoriaDinamica[2]);
|
|
if (Math.Abs(ag0 - ag1) > (Controle.Angulo_Max * 0.4) || Math.Abs(ag1 - ag2) > (Controle.Angulo_Max * 0.4) || Math.Abs(ag2 - ag3) > (Controle.Angulo_Max * 0.4))
|
|
{
|
|
Controle.PercentualVelocidadeSP = Controle.PercentualVelocidadeMax * 0.6;
|
|
}
|
|
else
|
|
{
|
|
Controle.PercentualVelocidadeSP = Controle.PercentualVelocidadeMax;
|
|
}*/
|
|
Controle.PercentualVelocidadeSP = Controle.PercentualVelocidadeMax * 0.6;
|
|
}
|
|
}
|
|
else if (statusAtual == StatusCarroMapa.EntrandoRua)
|
|
{
|
|
Controle.PercentualVelocidadeSP = Controle.PercentualVelocidadeMin;
|
|
}
|
|
else if (statusAtual == StatusCarroMapa.SaindoRua)
|
|
{
|
|
Controle.PercentualVelocidadeSP = Controle.PercentualVelocidadeMin;
|
|
}
|
|
else if (statusAtual == StatusCarroMapa.CaminhandoRua)
|
|
{
|
|
if (Math.Abs(Controle.Angulo) > limiteAnguloVelocidadeMin)
|
|
{
|
|
Controle.PercentualVelocidadeSP = Controle.PercentualVelocidadeMin;
|
|
}
|
|
else if (Sensoriamento.ErvasNoRadar == 0)
|
|
{
|
|
Controle.PercentualVelocidadeSP = Controle.PercentualVelocidadeMax;
|
|
}
|
|
else
|
|
{
|
|
Controle.PercentualVelocidadeSP = Controle.PercentualVelocidadeMax * 0.85;
|
|
}
|
|
}
|
|
else if (statusAtual == StatusCarroMapa.Manobrando)
|
|
{
|
|
Controle.PercentualVelocidadeSP = Controle.PercentualVelocidadeMax / 2.0;
|
|
}
|
|
|
|
if (Variaveis.OperacaoEmAndamento.Iniciado)
|
|
{
|
|
Variaveis.OperacaoEmAndamento.Mapa.GerarTrajetoriaDinamica();
|
|
}
|
|
}
|
|
|
|
public void AtualizarRuasSelecionadas()
|
|
{
|
|
List<string> ids = new List<string>();
|
|
bool idsMensagem = false;
|
|
var Topico = Variaveis.MqttService.Topicos.FirstOrDefault(x => x.Topico == MapasVariaveisModel.TopicoSelecaoRuasMapa);
|
|
var Mensagem = Topico.Mensagens.LastOrDefault();
|
|
if (Mensagem != null && !Mensagem.Mensagem.Contains("[]"))
|
|
{
|
|
Mensagem.Mensagem = Mensagem.Mensagem.Replace("\n", "");
|
|
Mensagem.Mensagem = Mensagem.Mensagem.Replace("\"", "");
|
|
if (!string.IsNullOrEmpty(Mensagem.Mensagem))
|
|
{
|
|
ids = JsonConvert.DeserializeObject<int[]>(Mensagem.Mensagem).Select(x => x.ToString()).ToList();
|
|
idsMensagem = true;
|
|
}
|
|
}
|
|
if (idsMensagem && ids.Count != Variaveis.OperacaoEmAndamento.Mapa.RuasPercorrer.Count)
|
|
{
|
|
Variaveis.OperacaoEmAndamento.Mapa.RuasPercorrer = ids;
|
|
Variaveis.OperacaoEmAndamento.Mapa.PopularTrajetoriaMapa();
|
|
}
|
|
}
|
|
|
|
}
|
|
|
|
public class OperacaoRefRuaGPS
|
|
{
|
|
private List<StatusCarroMapa> _EtapasAnteriores = new List<StatusCarroMapa>() { StatusCarroMapa.Parado };
|
|
public StatusCarroMapa _EtapaAnterior
|
|
{
|
|
get
|
|
{
|
|
var _Anterior = _EtapasAnteriores.Last();
|
|
if (_Anterior == _EtapaAtual && _EtapasAnteriores.Count() > 1)
|
|
{
|
|
_Anterior = _EtapasAnteriores.Skip(_EtapasAnteriores.Count() - 2).First();
|
|
}
|
|
return _Anterior;
|
|
}
|
|
}
|
|
private void AtualizaEtapaAtual(StatusCarroMapa _Atual)
|
|
{
|
|
_EtapaAtual = _Atual;
|
|
var _EtapaAnterior = _EtapasAnteriores.Last();
|
|
if (_EtapaAtual != _EtapaAnterior)
|
|
{
|
|
_EtapasAnteriores.Add(_EtapaAtual);
|
|
}
|
|
}
|
|
public StatusCarroMapa _EtapaAtual;
|
|
public StatusCarroMapa StatusAtual
|
|
{
|
|
get
|
|
{
|
|
if (Variaveis.OperacaoEmAndamento.StatusAtual != StatusOperacao.EmAndamento)
|
|
{
|
|
AtualizaEtapaAtual(StatusCarroMapa.Parado);
|
|
return _EtapaAtual;
|
|
}
|
|
|
|
var Mapa = Variaveis.OperacaoEmAndamento.Mapa;
|
|
bool dentroDaRua = Mapa.DentroDaRua;
|
|
bool naMargem = Mapa.NaMargemEntradaRua;
|
|
|
|
if (dentroDaRua)
|
|
{
|
|
var sonar = Variaveis.OperacaoEmAndamento.Sensoriamento.SonarFrontal_Kinect;
|
|
// se o carro estiver dentro da rua, o sonar estiver ativado, o kinect estiver iniciado, o desvio for necessario, a direcao do desvio seja Parado
|
|
if (Variaveis.OperacaoEmAndamento.Controle.SonarAtivado && (sonar?.Iniciado ?? false) && (sonar?.DesvioNecessario ?? false))
|
|
{
|
|
_EtapaAtual = StatusCarroMapa.Parado;
|
|
}
|
|
else
|
|
{
|
|
if (!naMargem)
|
|
{
|
|
AtualizaEtapaAtual(StatusCarroMapa.CaminhandoRua);
|
|
}
|
|
else
|
|
{
|
|
if (_EtapaAtual == StatusCarroMapa.CaminhandoRua && (_EtapaAnterior == StatusCarroMapa.EntrandoRua))
|
|
{
|
|
AtualizaEtapaAtual(StatusCarroMapa.SaindoRua);
|
|
}
|
|
|
|
if (_EtapaAnterior == StatusCarroMapa.CaminhandoRua)
|
|
{
|
|
AtualizaEtapaAtual(StatusCarroMapa.SaindoRua);
|
|
}
|
|
else
|
|
{
|
|
AtualizaEtapaAtual(StatusCarroMapa.EntrandoRua);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
else
|
|
{
|
|
if (!naMargem)
|
|
{
|
|
AtualizaEtapaAtual(StatusCarroMapa.Direcionando);
|
|
}
|
|
else
|
|
{
|
|
if (_EtapaAtual == StatusCarroMapa.Direcionando && (_EtapaAnterior == StatusCarroMapa.Parado || _EtapaAnterior == StatusCarroMapa.SaindoRua))
|
|
{
|
|
AtualizaEtapaAtual(StatusCarroMapa.EntrandoRua);
|
|
}
|
|
|
|
if (_EtapaAnterior == StatusCarroMapa.Direcionando)
|
|
{
|
|
AtualizaEtapaAtual(StatusCarroMapa.EntrandoRua);
|
|
}
|
|
else
|
|
{
|
|
AtualizaEtapaAtual(StatusCarroMapa.SaindoRua);
|
|
}
|
|
}
|
|
}
|
|
|
|
return _EtapaAtual;
|
|
}
|
|
}
|
|
public double MargemErroAngulo { get; set; } = 90;
|
|
public DirecaoCarroRua Direcao
|
|
{
|
|
get
|
|
{
|
|
|
|
if (GPSService.UltimaLeitura.Latitude == GPSService.PenultimaLeitura.Latitude && GPSService.UltimaLeitura.Longitude == GPSService.PenultimaLeitura.Longitude && GPSService.UltimaLeitura.DataHora > GPSService.PenultimaLeitura.DataHora)
|
|
{
|
|
return DirecaoCarroRua.Parado;
|
|
}
|
|
|
|
var Mapa = Variaveis.OperacaoEmAndamento.Mapa;
|
|
|
|
double mediaAngulo = Variaveis.OperacaoEmAndamento.Sensoriamento.AnguloCarro;
|
|
double anguloOposto = (mediaAngulo + 180) % 360;
|
|
double mediaAnguloRua = Mapa.AnguloRuaAtual;
|
|
|
|
DirecaoCarroRua direcaoTrecho =
|
|
!Mapa.TrajetoriaDinamica.Any() && !Mapa.TrajetoriaProjetada.Any() ? DirecaoCarroRua.Ida :
|
|
Mapa.TrajetoriaProjetada[0] != null ? Mapa.TrajetoriaProjetada[0][0].DirecaoTrecho :
|
|
Mapa.TrajetoriaDinamica.Any(x => x.idxRua == Mapa.IdxRuaAtual) ?
|
|
Mapa.TrajetoriaDinamica.FirstOrDefault(x => x.idxRua == Mapa.IdxRuaAtual).DirecaoTrecho :
|
|
Mapa.TrajetoriaDinamica.FirstOrDefault().DirecaoTrecho;
|
|
|
|
if ((mediaAngulo + MargemErroAngulo) >= mediaAnguloRua && (mediaAngulo - MargemErroAngulo) <= mediaAnguloRua)
|
|
{
|
|
return direcaoTrecho;
|
|
}
|
|
else if ((anguloOposto + MargemErroAngulo) >= mediaAnguloRua && (anguloOposto - MargemErroAngulo) <= mediaAnguloRua)
|
|
{
|
|
return direcaoTrecho == DirecaoCarroRua.Ida ? DirecaoCarroRua.Volta : DirecaoCarroRua.Ida;
|
|
}
|
|
else
|
|
{
|
|
return DirecaoCarroRua.Parado;
|
|
}
|
|
}
|
|
}
|
|
public bool EmManobra
|
|
{
|
|
get
|
|
{
|
|
// Caso o robo esteja manobrando em direcao a uma rua, qualquer movimento feito sera considerado aproximacao do primeiro ponto da rua
|
|
return Variaveis.OperacaoEmAndamento.Mapa.IdxUltimoPontoTrajetoria == 0 && StatusAtual == StatusCarroMapa.Direcionando;
|
|
}
|
|
}
|
|
public bool ManobrandoEntreRuas
|
|
{
|
|
get
|
|
{
|
|
bool proximoEntradaRua = Variaveis.OperacaoEmAndamento.Mapa.DistanciaAteProximoPonto <= Variaveis.OperacaoEmAndamento.Mapa.DistanciaProjecaoRua;
|
|
bool Manobrando = proximoEntradaRua && EmManobra;
|
|
|
|
return Manobrando;
|
|
}
|
|
}
|
|
|
|
public void VerificaFimRuaAtual()
|
|
{
|
|
if (_EtapaAtual == StatusCarroMapa.Direcionando && (_EtapaAnterior == StatusCarroMapa.SaindoRua))
|
|
{
|
|
var Mapa = Variaveis.OperacaoEmAndamento.Mapa;
|
|
string RuaAtual = Mapa.RuasPercorrer[Mapa.IdxRuaAtual];
|
|
if (!Mapa.RuasPercorridas.Contains(RuaAtual))
|
|
{
|
|
Mapa.RuasPercorridas.Add(RuaAtual);
|
|
Mapa.IdxRuaAtual++;
|
|
Mapa.TrajetoriaDinamica.Clear();
|
|
Mapa.IdxUltimoPontoTrajetoria = 0;
|
|
Mapa.IdxUltimoPontoTrajetoriaDinamica = 0;
|
|
|
|
GPSService.PenultimaLeitura = new GPSModel()
|
|
{
|
|
DataHora = GPSService.UltimaLeitura.DataHora,
|
|
Longitude = GPSService.UltimaLeitura.Longitude,
|
|
Latitude = GPSService.UltimaLeitura.Latitude,
|
|
};
|
|
}
|
|
}
|
|
}
|
|
|
|
}
|
|
|
|
public class OperacaoMapaGPSSensoriamentoModel
|
|
{
|
|
public DateTime Momento { get; set; }
|
|
public int ErvasNoRadar { get; set; } = 0;
|
|
public double HerbicidaConsumido { get; set; } = 0;
|
|
public double HerbicidaPorErva { get; set; } = 0;
|
|
public int ErvasIdentificadas
|
|
{
|
|
get
|
|
{
|
|
int atuacoes = 0;
|
|
foreach (var bico in AtuacoesPorBico)
|
|
{
|
|
atuacoes += bico.Sum(x => x.Value);
|
|
}
|
|
return atuacoes;
|
|
}
|
|
}
|
|
public List<Dictionary<string, int>> AtuacoesPorBico { get; set; } = new List<Dictionary<string, int>>();
|
|
public double ProgressoTrajeto { get; set; } = 0;
|
|
public double ProgressoRua { get; set; } = 0;
|
|
public StatusCarroMapa Status { get; set; } = StatusCarroMapa.Parado;
|
|
public double AnguloCarro { get; set; } = 0;
|
|
public double AnguloRua { get; set; } = 0;
|
|
public double AnguloRuaCamera { get; set; } = 0;
|
|
public double AnguloDif { get; set; } = 0;
|
|
public DirecaoCarroRua Direcao { get; set; } = DirecaoCarroRua.Parado;
|
|
public bool DentroDaRua { get; set; } = false;
|
|
public int IdxPontoAproximadoRua { get; set; } = 0;
|
|
public int IdxRuaAtual { get; set; } = 0;
|
|
|
|
public void ReiniciarLeituras()
|
|
{
|
|
ErvasNoRadar = 0;
|
|
HerbicidaConsumido = 0;
|
|
HerbicidaPorErva = 0;
|
|
ProgressoTrajeto = 0;
|
|
ProgressoRua = 0;
|
|
Status = StatusCarroMapa.Parado;
|
|
AnguloCarro = 0;
|
|
AnguloRua = 0;
|
|
AnguloRuaCamera = 0;
|
|
AnguloDif = 0;
|
|
Direcao = DirecaoCarroRua.Parado;
|
|
DentroDaRua = false;
|
|
IdxPontoAproximadoRua = 0;
|
|
IdxRuaAtual = 0;
|
|
|
|
AtuacoesPorBico = new List<Dictionary<string, int>>();
|
|
if (Variaveis.OperacaoEmAndamento.DispAtu != null)
|
|
{
|
|
for (int i = 0; i < Variaveis.OperacaoEmAndamento.DispAtu.Dados.QuantidadeBicos; i++)
|
|
{
|
|
AtuacoesPorBico.Add(new Dictionary<string, int>());
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
}
|