866 lines
39 KiB
C#
866 lines
39 KiB
C#
using AgroBase.Forms.Operacoes;
|
|
using AgroBase.Models.Modules;
|
|
using AgroBase.Services;
|
|
using CefSharp.DevTools.Database;
|
|
using Emgu.CV.ML;
|
|
using Newtonsoft.Json;
|
|
using System;
|
|
using System.Collections.Generic;
|
|
using System.ComponentModel;
|
|
using System.IO;
|
|
using System.Linq;
|
|
using System.Text;
|
|
using System.Threading.Tasks;
|
|
using System.Windows.Forms;
|
|
using static AgroBase.Models.Enuns;
|
|
|
|
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.Trajetoria.Count - 1;
|
|
}
|
|
}
|
|
public OperacaoRefRuaGPS ReferencialRuaGPS { get; set; } = new OperacaoRefRuaGPS();
|
|
public OperacaoMapaGPSSensoriamentoModel Sensoriamento { get; set; } = new OperacaoMapaGPSSensoriamentoModel();
|
|
public OperacaoControleModel Controle { get; set; } = new OperacaoControleModel();
|
|
public OperacaoControleModel ControleAnterior { get; set; } = new OperacaoControleModel();
|
|
public List<StatusCarroMapa> StatusDentroRua = new List<StatusCarroMapa>()
|
|
{
|
|
StatusCarroMapa.EntrandoRua,
|
|
StatusCarroMapa.CaminhandoRua,
|
|
StatusCarroMapa.SaindoRua
|
|
};
|
|
|
|
|
|
public void AtualizarDadosControle()
|
|
{
|
|
DateTime Agora = DateTime.Now;
|
|
OperacaoControleTipoModel ControleMov = Controle.TiposControle != null ? Controle.TiposControle.FirstOrDefault(x => x.Tipo == T_Code.Mov) : null;
|
|
OperacaoControleTipoModel ControleDir = Controle.TiposControle != null ? Controle.TiposControle.FirstOrDefault(x => x.Tipo == T_Code.Dir) : null;
|
|
OperacaoControleTipoModel ControleAtu = Controle.TiposControle != null ? Controle.TiposControle.FirstOrDefault(x => x.Tipo == T_Code.Atu) : null;
|
|
|
|
if (ControleMov != null && Controle.RPM != ControleAnterior.RPM && (Agora - ControleMov.UltimoComando).TotalMilliseconds > ControleMov.DelayEnvioComando)
|
|
{
|
|
// Enviar comando MOV
|
|
var Protocolos = EnviarProtocoloComando(Variaveis.OperacaoEmAndamento.DispMov);
|
|
ControleMov.UltimoComando = Agora;
|
|
ControleMov.Comandos.AddRange(Protocolos);
|
|
|
|
ControleAnterior.RPM = Controle.RPM;
|
|
}
|
|
if (ControleDir != null && Convert.ToInt32(Controle.Angulo) != Convert.ToInt32(ControleAnterior.Angulo) && (Agora - ControleDir.UltimoComando).TotalMilliseconds > ControleDir.DelayEnvioComando)
|
|
{
|
|
// Enviar comando DIR
|
|
var Protocolos = EnviarProtocoloComando(Variaveis.OperacaoEmAndamento.DispDir);
|
|
ControleDir.UltimoComando = Agora;
|
|
ControleDir.Comandos.AddRange(Protocolos);
|
|
|
|
ControleAnterior.Angulo = Controle.Angulo;
|
|
}
|
|
if (ControleAtu != null && !Controle.BicosAtuados.Select(x => x.Atuado).SequenceEqual(ControleAnterior.BicosAtuados.Select(x => x.Atuado)) && (Agora - ControleAtu.UltimoComando).TotalMilliseconds > ControleAtu.DelayEnvioComando)
|
|
{
|
|
// Enviar comando ATU
|
|
var Protocolos = EnviarProtocoloComando(Variaveis.OperacaoEmAndamento.DispAtu);
|
|
ControleAtu.UltimoComando = Agora;
|
|
ControleAtu.Comandos.AddRange(Protocolos);
|
|
|
|
ControleAnterior.BicosAtuados = new List<AtuadorBicoModel>();
|
|
foreach (var bico in Controle.BicosAtuados)
|
|
{
|
|
ControleAnterior.BicosAtuados.Add(bico.Clone());
|
|
}
|
|
}
|
|
|
|
AtualizarDadosOperacao();
|
|
}
|
|
|
|
public List<string> EnviarProtocoloComando(IDispositivosService Dispositivo)
|
|
{
|
|
if (Dispositivo == null)
|
|
{
|
|
return new List<string>();
|
|
}
|
|
|
|
if (Dispositivo.Dispositivo == T_Code.Mov)
|
|
{
|
|
GeneralJoystick.RPM = Controle.RPM;
|
|
if (Controle.RPM == 0)
|
|
{
|
|
Controle.Direcao = Direcao.Parado;
|
|
}
|
|
else
|
|
{
|
|
if (Controle.RPM < 0)
|
|
{
|
|
GeneralJoystick.RPM *= -1;
|
|
Controle.Direcao = Direcao.Tras;
|
|
}
|
|
else
|
|
{
|
|
Controle.Direcao = Direcao.Frente;
|
|
}
|
|
}
|
|
}
|
|
else if (Dispositivo.Dispositivo == T_Code.Dir)
|
|
{
|
|
GeneralJoystick.Angulo = Controle.Angulo;
|
|
if (Controle.Angulo < 0)
|
|
{
|
|
GeneralJoystick.Angulo *= -1;
|
|
if (ControleAnterior.Angulo < Controle.Angulo)
|
|
{
|
|
Controle.Direcao = Direcao.Parado;
|
|
}
|
|
else
|
|
{
|
|
Controle.Direcao = Direcao.Baixo;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
if (ControleAnterior.Angulo > Controle.Angulo)
|
|
{
|
|
Controle.Direcao = Direcao.Parado;
|
|
}
|
|
else
|
|
{
|
|
Controle.Direcao = Direcao.Cima;
|
|
}
|
|
}
|
|
}
|
|
|
|
var Protocolos =
|
|
Dispositivo.Dados is MovimentacaoModel ? ((MovimentacaoModel)Dispositivo.Dados).Motores.Select(x => x.ProtocoloComando(Controle.Direcao)) :
|
|
Dispositivo.Dados is DirecionalModel ? ((DirecionalModel)Dispositivo.Dados).Motores.Select(x => x.ProtocoloComando(Controle.Direcao)) :
|
|
Dispositivo.Dados is AtuadorModel ? ((AtuadorModel)Dispositivo.Dados).BicosPulverizadores.Select(x => x.ProtocoloComando()) :
|
|
null;
|
|
foreach (var ProtocoloMotor in Protocolos)
|
|
{
|
|
Dispositivo.AdicionarMensagemFila(ProtocoloMotor);
|
|
System.Threading.Thread.Sleep(10);
|
|
}
|
|
|
|
return Protocolos.ToList();
|
|
}
|
|
|
|
private void AtualizarDadosOperacao()
|
|
{
|
|
if (Variaveis.OperacaoEmAndamento.DispMov == null)
|
|
{
|
|
return;
|
|
}
|
|
try
|
|
{
|
|
//Sensoriamento.DistanciaPercorrida = DispMov.Dados.DistanciaPercorridaOperacao;
|
|
Sensoriamento.ProgressoTrajeto = Variaveis.OperacaoEmAndamento.Mapa.mapaService.pythonProcess == null ? 0 : Variaveis.OperacaoEmAndamento.Sensoriamento.DistanciaPercorrida.Sum() / Variaveis.OperacaoEmAndamento.Mapa.DistanciaTotal * 100;
|
|
if (Sensoriamento.ProgressoTrajeto < 0 && Sensoriamento.ProgressoTrajeto >= 0)
|
|
{
|
|
Sensoriamento.ProgressoTrajeto = 0;
|
|
}
|
|
Sensoriamento.ProgressoRua =
|
|
Variaveis.OperacaoEmAndamento.Mapa == null || Variaveis.OperacaoEmAndamento.Mapa.pnlMapa == null ? 0 :
|
|
(ReferencialRuaGPS.Direcao == DirecaoCarroRua.Ida ? ((double)ReferencialRuaGPS.IdxPontoAproximadoRua / (double)(Variaveis.OperacaoEmAndamento.Mapa.Trajetoria[Variaveis.OperacaoEmAndamento.Mapa.IdxRuaAtual].Count - 1)) :
|
|
ReferencialRuaGPS.Direcao == DirecaoCarroRua.Volta ? ((double)(Variaveis.OperacaoEmAndamento.Mapa.Trajetoria[Variaveis.OperacaoEmAndamento.Mapa.IdxRuaAtual].Count - 1 - ReferencialRuaGPS.IdxPontoAproximadoRua) / (double)(Variaveis.OperacaoEmAndamento.Mapa.Trajetoria[Variaveis.OperacaoEmAndamento.Mapa.IdxRuaAtual].Count - 1)) :
|
|
0) * 100;
|
|
}
|
|
catch
|
|
{
|
|
|
|
}
|
|
|
|
}
|
|
|
|
public void AtualizarControleAtuadores()
|
|
{
|
|
Controle.BicosAtuados = Variaveis.OperacaoEmAndamento.DispAtu.Dados.BicosPulverizadores;
|
|
}
|
|
|
|
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
|
|
double erroLateral = Variaveis.OperacaoEmAndamento.Mapa.DistanciaEsquerda - Variaveis.OperacaoEmAndamento.Mapa.DistanciaDireita;
|
|
|
|
|
|
|
|
double erroCombinado = CalcularErroCombinado(erroLateral, erroOrientacao, anguloCarro, statusCarroRua);
|
|
|
|
|
|
|
|
// Atualiza o PID com o erro combinado
|
|
if (statusCarroRua != StatusCarroMapa.Parado)
|
|
{
|
|
Controle.PIDdirecional.Atualizar(erroCombinado);
|
|
}
|
|
double correcaoAngulo = Controle.PIDdirecional.Saida;
|
|
|
|
// Aplica a correção. Isso pode incluir limites para evitar comandos excessivos.
|
|
if (correcaoAngulo > Controle.Angulo_Max)
|
|
{
|
|
correcaoAngulo = Controle.Angulo_Max;
|
|
}
|
|
else if (correcaoAngulo < Controle.Angulo_Min)
|
|
{
|
|
correcaoAngulo = Controle.Angulo_Min;
|
|
}
|
|
|
|
Controle.Angulo = correcaoAngulo;
|
|
|
|
if (statusCarroRua == StatusCarroMapa.Manobrando)
|
|
{
|
|
Variaveis.OperacaoEmAndamento.DispDir.Dados.Motores.ForEach(x => x.Controlar = true);
|
|
}
|
|
else
|
|
{
|
|
Variaveis.OperacaoEmAndamento.DispDir.Dados.Motores.ForEach(x => x.Controlar = false);
|
|
Variaveis.OperacaoEmAndamento.DispDir.Dados.Motores.Where(x => x.ID == "EF" || x.ID == "DF").ToList().ForEach(x => x.Controlar = true);
|
|
}
|
|
|
|
}
|
|
|
|
private double CalcularErroCombinado(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.Trajetoria.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;
|
|
}
|
|
|
|
public void CalculaDadosMovimentacaoAutonoma()
|
|
{
|
|
var statusAtual = ReferencialRuaGPS.StatusAtual;
|
|
|
|
CalcularAnguloInclinacao(statusAtual);
|
|
|
|
if (statusAtual == StatusCarroMapa.Parado)
|
|
{
|
|
var BicosNaoAtuados = new List<AtuadorBicoModel>(Variaveis.OperacaoEmAndamento.DispAtu.Dados.BicosPulverizadores);
|
|
BicosNaoAtuados.ForEach(x => x.Atuado = false);
|
|
Controle.BicosAtuados = BicosNaoAtuados.ToList();
|
|
Controle.RPM = 0;
|
|
Controle.Angulo = 0;
|
|
}
|
|
else if (statusAtual == StatusCarroMapa.Direcionando)
|
|
{
|
|
if (Variaveis.OperacaoEmAndamento.Sensoriamento.SonarFrontal.DesvioNecessario)
|
|
{
|
|
Controle.RPM = Controle.RPM_Min;
|
|
}
|
|
else
|
|
{
|
|
Controle.RPM = Controle.RPM_Max;
|
|
}
|
|
}
|
|
else if (statusAtual == StatusCarroMapa.EntrandoRua)
|
|
{
|
|
Controle.RPM = Controle.RPM_Min;
|
|
}
|
|
else if (statusAtual == StatusCarroMapa.SaindoRua)
|
|
{
|
|
Controle.RPM = Controle.RPM_Min;
|
|
}
|
|
else if (statusAtual == StatusCarroMapa.CaminhandoRua)
|
|
{
|
|
if (Sensoriamento.ErvasNoRadar == 0)
|
|
{
|
|
Controle.RPM = Controle.RPM_Max;
|
|
}
|
|
else
|
|
{
|
|
Controle.RPM = Convert.ToInt32(Controle.RPM_Max * 0.85);
|
|
}
|
|
|
|
double margemAngulo = 5;
|
|
if (Controle.Angulo > margemAngulo || Controle.Angulo < (margemAngulo * -1))
|
|
{
|
|
Controle.RPM = Controle.RPM_Min;
|
|
}
|
|
}
|
|
else if (statusAtual == StatusCarroMapa.Manobrando)
|
|
{
|
|
Controle.RPM = Controle.RPM_Max / 2;
|
|
}
|
|
}
|
|
|
|
public void AtualizarRuasSelecionadas()
|
|
{
|
|
List<string> ids = new List<string>();
|
|
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("\"", "");
|
|
ids = JsonConvert.DeserializeObject<int[]>(Mensagem.Mensagem).Select(x => x.ToString()).ToList();
|
|
}
|
|
if (ids.Count != Variaveis.OperacaoEmAndamento.Mapa.RuasPercorrer.Count)
|
|
{
|
|
Variaveis.OperacaoEmAndamento.Mapa.RuasPercorrer = ids;
|
|
Variaveis.OperacaoEmAndamento.Mapa.PopularTrajetoria();
|
|
}
|
|
}
|
|
|
|
|
|
|
|
}
|
|
|
|
public class OperacaoRefRuaGPS
|
|
{
|
|
public bool ManobraConcluida { get; set; } = true;
|
|
private StatusCarroMapa _EtapaAtual;
|
|
public StatusCarroMapa StatusAtual
|
|
{
|
|
get
|
|
{
|
|
bool dentroDaRua = Variaveis.OperacaoEmAndamento.Mapa.DentroDaRua;
|
|
StatusCarroMapa etapaAtual = _EtapaAtual;
|
|
|
|
// Se o status da operação não estiver Em Andamento
|
|
if (Variaveis.OperacaoEmAndamento.StatusAtual != StatusOperacao.EmAndamento)
|
|
{
|
|
etapaAtual = StatusCarroMapa.Parado;
|
|
}
|
|
else
|
|
{
|
|
// Robô está manobrando para seguir a próxima rota do mapa
|
|
if (_EtapaAtual == StatusCarroMapa.Manobrando)
|
|
{
|
|
// Aguarda a mudança de rua até que o robô tenha terminado a manobra e tenha ficado parado em frente à rua
|
|
/*if (ManobraConcluida && Direcao == DirecaoCarroRua.Parado)
|
|
{
|
|
Variaveis.OperacaoEmAndamento.Mapa.IdxRuaAtual++;
|
|
ManobraConcluida = false;
|
|
etapaAtual = StatusCarroMapa.EntrandoRua;
|
|
GPSService.AtualizaLeituraSimulacao();
|
|
System.Threading.Thread.Sleep(500);
|
|
}*/
|
|
int idxProximaRua = Variaveis.OperacaoEmAndamento.Mapa.IdxRuaAtual + 1;
|
|
List<GPSModel> trechoCarro = new List<GPSModel>()
|
|
{
|
|
GPSService.PenultimaLeitura,
|
|
GPSService.UltimaLeitura
|
|
};
|
|
List<GPSModel> trechoCaminho = new List<GPSModel>()
|
|
{
|
|
Variaveis.OperacaoEmAndamento.Mapa.TrajetoriaDinamica.Where(x => x.idxRua == idxProximaRua).FirstOrDefault(),
|
|
Variaveis.OperacaoEmAndamento.Mapa.TrajetoriaDinamica.Where(x => x.idxRua == idxProximaRua).Skip(1).FirstOrDefault()
|
|
};
|
|
if (GPSService.CompararDirecaoTrajeto(trechoCarro, trechoCaminho))
|
|
{
|
|
Variaveis.OperacaoEmAndamento.Mapa.IdxRuaAtual++;
|
|
Variaveis.OperacaoEmAndamento.Mapa.IdxUltimoPontoTrajetoria = 0;
|
|
ManobraConcluida = true;
|
|
etapaAtual = StatusCarroMapa.Direcionando;
|
|
Variaveis.OperacaoEmAndamento.Mapa.TrajetoriaDinamica.Clear();
|
|
Variaveis.OperacaoEmAndamento.Mapa.GerarTrajetoriaDinamica();
|
|
}
|
|
}
|
|
// Se o robô não estiver dentro da entrelinha de cana
|
|
else if (!dentroDaRua)
|
|
{
|
|
if (Variaveis.OperacaoEmAndamento.Mapa.TrajetoriaDinamica.Count() <= 3)
|
|
{
|
|
Variaveis.OperacaoEmAndamento.Mapa.GerarTrajetoriaDinamica();
|
|
if (Variaveis.OperacaoEmAndamento.Mapa.TrajetoriaDinamica.Count() <= 3)
|
|
{
|
|
etapaAtual = StatusCarroMapa.Parado;
|
|
Variaveis.OperacaoEmAndamento.Mapa.IdxRuaAtual++;
|
|
}
|
|
}
|
|
else
|
|
{// Está próximo o bastante e alinhado para entrar na rua
|
|
if (NaMargemEntradaRua)
|
|
{
|
|
DirecaoCarroRua direcao = Direcao;
|
|
// Está mais próximo do ponto inicial da rota, e a direção do carro é IDA ou então PARADO, logo está entrando na rua, do começo para o final
|
|
if (ExtremoMaisProximo == 0 && (direcao == DirecaoCarroRua.Ida || direcao == DirecaoCarroRua.Parado) && etapaAtual != StatusCarroMapa.SaindoRua)
|
|
{
|
|
etapaAtual = StatusCarroMapa.EntrandoRua;
|
|
}
|
|
// Está mais próximo do ponto final da rota, e a direção do carro é VOLTA ou então PARADO, logo está entrando na rua, do final para o começo
|
|
else if (ExtremoMaisProximo == 1 && (direcao == DirecaoCarroRua.Volta || direcao == DirecaoCarroRua.Parado) && etapaAtual != StatusCarroMapa.SaindoRua)
|
|
{
|
|
etapaAtual = StatusCarroMapa.EntrandoRua;
|
|
}
|
|
// Está mais próximo ao ponto final da rua, e a direção do carro é IDA, mas a rua será iniciada no sentido VOLTA, então para o carro para que ele vire e entre na rua
|
|
/*else if (ExtremoMaisProximo == 1 && (direcao == DirecaoCarroRua.Ida && Variaveis.OperacaoEmAndamento.DirecaoCaminho == DirecaoCarroRua.Volta))
|
|
{
|
|
etapaAtual = StatusCarroMapa.Manobrando;
|
|
}
|
|
else if (ExtremoMaisProximo == 0 && (direcao == DirecaoCarroRua.Volta && Variaveis.OperacaoEmAndamento.DirecaoCaminho == DirecaoCarroRua.Ida))
|
|
{
|
|
etapaAtual = StatusCarroMapa.Manobrando;
|
|
}*/
|
|
// Robô não está entrando na rua do começo para o fim, nem do fim para o começo
|
|
else
|
|
{
|
|
// Se a etapa anterior era CaminhandoRua, ou seja, robô estava na rua e agora não está mais, então esta saindo da rua
|
|
if (_EtapaAtual == StatusCarroMapa.CaminhandoRua)
|
|
{
|
|
etapaAtual = StatusCarroMapa.SaindoRua;
|
|
}
|
|
// Se a etapa anterior não for CaminhandoRua, então significa que ele já iniciou a etapa de sair da rua
|
|
else
|
|
{
|
|
/*// Ainda tem ruas a serem percorridas, logo, o robô precisa realizar a manobra para seguir para a próxima rua
|
|
if (!Variaveis.OperacaoEmAndamento.OpMapaGPS.Concluido)
|
|
{
|
|
etapaAtual = StatusCarroMapa.Manobrando;
|
|
ManobraConcluida = false;
|
|
}*/
|
|
}
|
|
}
|
|
}
|
|
// Estará se direcionando para o ponto inicial
|
|
else
|
|
{
|
|
// Saiu da margem de saída da rua
|
|
if (_EtapaAtual == StatusCarroMapa.SaindoRua)
|
|
{
|
|
// Ainda tem ruas a serem percorridas, logo, o robô precisa realizar a manobra para seguir para a próxima rua
|
|
if (!Variaveis.OperacaoEmAndamento.OpMapaGPS.Concluido)
|
|
{
|
|
etapaAtual = StatusCarroMapa.Manobrando;
|
|
ManobraConcluida = false;
|
|
}
|
|
}
|
|
// Não está na margem de entrada ou saída da rua, então está em estado de direcionamento até a rua da operação
|
|
else
|
|
{
|
|
etapaAtual = StatusCarroMapa.Direcionando;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
// Se o robô estiver dentro da entrelinha da cana
|
|
else if (dentroDaRua)
|
|
{
|
|
etapaAtual = StatusCarroMapa.CaminhandoRua;
|
|
}
|
|
}
|
|
|
|
|
|
_EtapaAtual = etapaAtual;
|
|
|
|
//_EtapaAtual = StatusCarroMapa.Direcionando;
|
|
|
|
return _EtapaAtual;
|
|
}
|
|
}
|
|
public int PontosConsiderarAngulo { get; set; } = 2;
|
|
public double Distancia { get; set; } = 0;
|
|
/*public double AnguloCarro
|
|
{
|
|
get
|
|
{
|
|
if (Variaveis.OperacaoEmAndamento.DispSen._Porta.IsOpen && Variaveis.OperacaoEmAndamento.DispSen.Dados.Sensores.Any(x => x.Componente == S_Code.sMAG && x.Inicializado))
|
|
{
|
|
return Variaveis.OperacaoEmAndamento.Sensoriamento.AnguloCarroBussola;
|
|
}
|
|
else
|
|
{
|
|
return Variaveis.OperacaoEmAndamento.Sensoriamento.AnguloCarroGPS;
|
|
}
|
|
}
|
|
}*/
|
|
/*public double AnguloCarroGPS
|
|
{
|
|
get
|
|
{
|
|
try
|
|
{
|
|
double somaAngulo = 0;
|
|
var Pontos = !Variaveis.OperacaoEmAndamento.GPSTrajetoriaRua.Any() || Variaveis.OperacaoEmAndamento.Mapa.IdxRuaAtual >= Variaveis.OperacaoEmAndamento.GPSTrajetoriaRua.Count() ? new List<GPSModel>() :
|
|
Variaveis.OperacaoEmAndamento.GPSTrajetoriaRua[Variaveis.OperacaoEmAndamento.Mapa.IdxRuaAtual].Count > PontosConsiderarAngulo ?
|
|
Variaveis.OperacaoEmAndamento.GPSTrajetoriaRua[Variaveis.OperacaoEmAndamento.Mapa.IdxRuaAtual].OrderByDescending(x => x.DataHora).Take(PontosConsiderarAngulo).ToList() :
|
|
Variaveis.OperacaoEmAndamento.GPSTrajetoriaRua[Variaveis.OperacaoEmAndamento.Mapa.IdxRuaAtual].OrderByDescending(x => x.DataHora).ToList();
|
|
Pontos = Pontos.OrderBy(x => x.DataHora).ToList();
|
|
for (int i = 0; i < Pontos.Count - 1; i++)
|
|
{
|
|
somaAngulo += GPSService.CalcularOrientacao(Pontos[i + 1], Pontos[i]);
|
|
}
|
|
double mediaAngulo = somaAngulo / (Pontos.Count - 1);
|
|
|
|
return (!(mediaAngulo >= 0 || mediaAngulo <= 0)) ? 0 : mediaAngulo;
|
|
}
|
|
catch
|
|
{
|
|
return 0;
|
|
}
|
|
}
|
|
}*/
|
|
public double AnguloCaminho
|
|
{
|
|
get
|
|
{
|
|
try
|
|
{
|
|
if (_EtapaAtual == StatusCarroMapa.Direcionando)
|
|
{
|
|
double somaAnguloRua = 0;
|
|
var Rua = Variaveis.OperacaoEmAndamento.Mapa.TrajetoriaDinamica;
|
|
|
|
|
|
var PontosRua = PontosConsiderarAngulo < Rua.Count ?
|
|
Rua.Take(PontosConsiderarAngulo).ToList() :
|
|
Rua.Skip(PontosConsiderarAngulo).ToList();
|
|
|
|
for (int i = 0; i < PontosRua.Count - 1; i++)
|
|
{
|
|
somaAnguloRua += GPSService.CalcularOrientacao(Rua[0], Rua[1]);
|
|
}
|
|
double mediaAnguloRua = somaAnguloRua / (PontosRua.Count - 1);
|
|
|
|
return (!(mediaAnguloRua >= 0 || mediaAnguloRua <= 0)) ? 0 : mediaAnguloRua;
|
|
}
|
|
else
|
|
{
|
|
double somaAnguloRua = 0;
|
|
var Rua = Variaveis.OperacaoEmAndamento.Mapa.Trajetoria.Skip(Variaveis.OperacaoEmAndamento.Mapa.IdxRuaAtual).First();
|
|
|
|
|
|
var PontosRua = IdxPontoAproximadoRua + PontosConsiderarAngulo < Rua.Count ?
|
|
Rua.Skip(IdxPontoAproximadoRua).Take(PontosConsiderarAngulo).ToList() :
|
|
Rua.Skip(IdxPontoAproximadoRua - PontosConsiderarAngulo).ToList();
|
|
|
|
/*var Pontos = Rua.Count > PontosConsiderarAngulo ?
|
|
Rua.OrderByDescending(x => x.DataHora).Take(PontosConsiderarAngulo).ToList() :
|
|
Rua.OrderByDescending(x => x.DataHora).ToList();*/
|
|
|
|
|
|
for (int i = 0; i < PontosRua.Count - 1; i++)
|
|
{
|
|
somaAnguloRua += GPSService.CalcularOrientacao(PontosRua[i + 1], PontosRua[i]);
|
|
}
|
|
|
|
double mediaAnguloRua = somaAnguloRua / (PontosRua.Count - 1);
|
|
double angulo = (!(mediaAnguloRua >= 0 || mediaAnguloRua <= 0)) ? 0 : mediaAnguloRua;
|
|
/*double anguloOposto = (angulo + 180) % 360;
|
|
|
|
if (Variaveis.OperacaoEmAndamento.DirecaoCaminho == DirecaoCarroRua.Volta)
|
|
{
|
|
angulo = anguloOposto;
|
|
}*/
|
|
|
|
return angulo;
|
|
}
|
|
}
|
|
catch
|
|
{
|
|
return 0;
|
|
}
|
|
}
|
|
}
|
|
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;
|
|
}
|
|
double mediaAngulo = Variaveis.OperacaoEmAndamento.Sensoriamento.AnguloCarro;
|
|
double anguloOposto = (mediaAngulo + 180) % 360;
|
|
double mediaAnguloRua = Variaveis.OperacaoEmAndamento.Mapa.AnguloCaminho;
|
|
|
|
/*if ((mediaAngulo + MargemErroAngulo) >= mediaAnguloRua && (mediaAngulo - MargemErroAngulo) <= mediaAnguloRua)
|
|
{
|
|
return DirecaoCarroRua.Ida;
|
|
}
|
|
else if ((anguloOposto + MargemErroAngulo) >= mediaAnguloRua && (anguloOposto - MargemErroAngulo) <= mediaAnguloRua)
|
|
{
|
|
return DirecaoCarroRua.Volta;
|
|
}
|
|
else
|
|
{
|
|
return DirecaoCarroRua.Parado;
|
|
}*/
|
|
DirecaoCarroRua direcaoTrecho = !Variaveis.OperacaoEmAndamento.Mapa.TrajetoriaDinamica.Any() ? DirecaoCarroRua.Ida :
|
|
Variaveis.OperacaoEmAndamento.Mapa.TrajetoriaDinamica.Any(x => x.idxRua == Variaveis.OperacaoEmAndamento.Mapa.IdxRuaAtual) ?
|
|
Variaveis.OperacaoEmAndamento.Mapa.TrajetoriaDinamica.FirstOrDefault(x => x.idxRua == Variaveis.OperacaoEmAndamento.Mapa.IdxRuaAtual).DirecaoTrecho :
|
|
Variaveis.OperacaoEmAndamento.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 GPSModel PrimeiroPontoRua
|
|
{
|
|
get
|
|
{
|
|
try
|
|
{
|
|
var primeiroPontoRua = Variaveis.OperacaoEmAndamento.Mapa.Trajetoria.Skip(Variaveis.OperacaoEmAndamento.Mapa.IdxRuaAtual).First().First();
|
|
return primeiroPontoRua;
|
|
}
|
|
catch
|
|
{
|
|
return new GPSModel();
|
|
}
|
|
|
|
}
|
|
}
|
|
public GPSModel UltimoPontoRua
|
|
{
|
|
get
|
|
{
|
|
try
|
|
{
|
|
var ultimoPontoRua = Variaveis.OperacaoEmAndamento.Mapa.Trajetoria.Skip(Variaveis.OperacaoEmAndamento.Mapa.IdxRuaAtual).First().Last();
|
|
return ultimoPontoRua;
|
|
}
|
|
catch
|
|
{
|
|
return new GPSModel();
|
|
}
|
|
}
|
|
}
|
|
public bool DentroDaRua
|
|
{
|
|
get
|
|
{
|
|
if (Variaveis.OperacaoEmAndamento.Mapa.RuasPercorrer.Count() > Variaveis.OperacaoEmAndamento.Mapa.IdxRuaAtual && !Variaveis.OperacaoEmAndamento.Mapa.TrajetoriaDinamica.Any())
|
|
{
|
|
try
|
|
{
|
|
string IDruaAtual = Variaveis.OperacaoEmAndamento.Mapa.RuasPercorrer[Variaveis.OperacaoEmAndamento.Mapa.IdxRuaAtual];
|
|
var Rua = Variaveis.OperacaoEmAndamento.Mapa.mapaService.DadosMapa.features.Where(x => x.geometry.id == IDruaAtual).FirstOrDefault();
|
|
if (Rua != null)
|
|
{
|
|
bool dentro = false;
|
|
List<GPSModel> Trecho = new List<GPSModel>();
|
|
var PontosRua = Variaveis.OperacaoEmAndamento.Mapa.Trajetoria[Variaveis.OperacaoEmAndamento.Mapa.IdxRuaAtual].ToList();
|
|
int idx = Variaveis.OperacaoEmAndamento.OpMapaGPS.ReferencialRuaGPS.IdxPontoAproximadoRua;
|
|
if (Variaveis.OperacaoEmAndamento.Mapa.IdxRuaAtual % 2 != 0)
|
|
{
|
|
PontosRua.Reverse();
|
|
idx = idx != (PontosRua.Count - 1) ? (PontosRua.Count - 1) - idx : idx;
|
|
}
|
|
for (int i = 0; i <= idx; i++)
|
|
{
|
|
Trecho.Add(PontosRua[i]);
|
|
}
|
|
double distTrecho = GPSService.DistanciaDoTrecho(Trecho);
|
|
dentro = distTrecho > 0 && distTrecho < Rua.properties.Length;
|
|
|
|
return dentro;
|
|
}
|
|
else
|
|
{
|
|
return false;
|
|
}
|
|
}
|
|
catch
|
|
{
|
|
return false;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
return false;
|
|
}
|
|
}
|
|
}
|
|
public int ExtremoMaisProximo
|
|
{
|
|
get
|
|
{
|
|
double distPP = GPSService.DistanciaEntrePontos(GPSService.UltimaLeitura, PrimeiroPontoRua);
|
|
double distUP = GPSService.DistanciaEntrePontos(GPSService.UltimaLeitura, UltimoPontoRua);
|
|
|
|
// 0 para mais próximo do primeiro ponto da rua (ENTRANDO)
|
|
// 1 para mais próximo do último ponto da rua (SAINDO)
|
|
return (distPP < distUP) ? 0 : 1;
|
|
}
|
|
}
|
|
private DateTime UltimaLeituraDistancia;
|
|
private int idxPontoAproximadoRua = 0;
|
|
public int IdxPontoAproximadoRua
|
|
{
|
|
get
|
|
{
|
|
try
|
|
{
|
|
if (UltimaLeituraDistancia == null)
|
|
{
|
|
UltimaLeituraDistancia = DateTime.Now.Add(new TimeSpan(0, 0, -10));
|
|
}
|
|
else if (UltimaLeituraDistancia.Add(new TimeSpan(0, 0, 5)) < DateTime.Now)
|
|
{
|
|
int idx = -1;
|
|
double menorDistancia = 9999;
|
|
double distanciaAnterior = 9999;
|
|
|
|
var Rua = Variaveis.OperacaoEmAndamento.Mapa.Trajetoria[Variaveis.OperacaoEmAndamento.Mapa.IdxRuaAtual];
|
|
|
|
foreach (var Ponto in Rua)
|
|
{
|
|
double dist = GPSService.DistanciaEntrePontos(GPSService.UltimaLeitura, Ponto);
|
|
if (dist < menorDistancia)
|
|
{
|
|
menorDistancia = dist;
|
|
idx = Rua.IndexOf(Ponto);
|
|
}
|
|
else if (dist > distanciaAnterior)
|
|
{
|
|
break;
|
|
}
|
|
distanciaAnterior = dist;
|
|
}
|
|
idxPontoAproximadoRua = idx;
|
|
|
|
UltimaLeituraDistancia = DateTime.Now;
|
|
}
|
|
}
|
|
catch
|
|
{
|
|
|
|
}
|
|
|
|
|
|
return idxPontoAproximadoRua;
|
|
}
|
|
}
|
|
public double MargemLimiteEntradaRua { get; } = 2.0;
|
|
public bool NaMargemEntradaRua
|
|
{
|
|
get
|
|
{
|
|
bool NaMargem = false;
|
|
List<GPSModel> Rua = Variaveis.OperacaoEmAndamento.Mapa.Trajetoria[Variaveis.OperacaoEmAndamento.Mapa.IdxRuaAtual];
|
|
GPSModel PontoRua = ExtremoMaisProximo == 0 ? PrimeiroPontoRua : UltimoPontoRua;
|
|
double Distancia = GPSService.DistanciaEntrePontos(GPSService.UltimaLeitura, PontoRua);
|
|
double DistanciaAnterior = GPSService.DistanciaEntrePontos(GPSService.PenultimaLeitura, PontoRua);
|
|
|
|
double DistanciaFinal = 0;
|
|
if (Distancia > DistanciaAnterior && (Distancia <= (MargemLimiteEntradaRua * 2)))
|
|
{
|
|
DistanciaFinal = Distancia * -1;
|
|
}
|
|
else
|
|
{
|
|
DistanciaFinal = Distancia;
|
|
}
|
|
|
|
if (DistanciaFinal <= MargemLimiteEntradaRua)
|
|
{
|
|
NaMargem = true;
|
|
}
|
|
else if (DistanciaFinal <= (MargemLimiteEntradaRua * 2) && (_EtapaAtual == StatusCarroMapa.CaminhandoRua || _EtapaAtual == StatusCarroMapa.SaindoRua))
|
|
{
|
|
NaMargem = true;
|
|
}
|
|
|
|
return NaMargem;
|
|
}
|
|
}
|
|
|
|
}
|
|
|
|
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
|
|
{
|
|
return AtuacoesPorBico.Sum();
|
|
}
|
|
}
|
|
public List<int> AtuacoesPorBico { get; set; } = new List<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;
|
|
}
|
|
|
|
}
|