374 lines
18 KiB
C#
374 lines
18 KiB
C#
using AgroBase.Services;
|
|
using System;
|
|
using System.Collections.Generic;
|
|
using System.Linq;
|
|
using System.Windows.Forms;
|
|
using static AgroBase.Models.Enums;
|
|
|
|
namespace AgroBase.Models
|
|
{
|
|
public enum ConfiguracaoSensorUltrassonico
|
|
{
|
|
NaoUtilizado,
|
|
AlinhamentoLateral,
|
|
DeteccaoObstaculosFrenteETras,
|
|
DeteccaoSulcosEBuracos,
|
|
NavegacaoAmbienteEstreito,
|
|
SupervisaoAreaComObstaculos
|
|
}
|
|
|
|
public enum PosicaoSensorUltrassonico
|
|
{
|
|
NaoConectado,
|
|
TraseiroEsquerdo,
|
|
TraseiroCentro,
|
|
TraseiroDireito,
|
|
FrontalEsquerdo,
|
|
FrontalCentro,
|
|
FrontalDireito,
|
|
LateralEsquerdaFrontal,
|
|
LateralEsquerdaCentral,
|
|
LateralEsquerdaTraseiro,
|
|
LateralDireitaFrontal,
|
|
LateralDireitaCentral,
|
|
LateralDireitaTraseiro
|
|
}
|
|
|
|
public class UltrasonicA05Helper
|
|
{
|
|
public static Dictionary<ConfiguracaoSensorUltrassonico, string> DescricaoConfiguracoes = new Dictionary<ConfiguracaoSensorUltrassonico, string>
|
|
{
|
|
{ ConfiguracaoSensorUltrassonico.NaoUtilizado, "Não ultiliza os sensores ultrassonicos durante a execução da operação." },
|
|
|
|
{ ConfiguracaoSensorUltrassonico.AlinhamentoLateral, "Usa sensores nas laterais para manter o robô centralizado na entrelinha da plantação, garantindo alinhamento constante." },
|
|
|
|
{ ConfiguracaoSensorUltrassonico.DeteccaoObstaculosFrenteETras, "Configuração para detectar obstáculos na frente e atrás do robô, permitindo desvio de objetos inesperados no caminho." },
|
|
|
|
{ ConfiguracaoSensorUltrassonico.DeteccaoSulcosEBuracos, "Sensores na frente inclinados para baixo, ideais para identificar buracos ou sulcos no solo e evitar áreas acidentadas." },
|
|
|
|
{ ConfiguracaoSensorUltrassonico.NavegacaoAmbienteEstreito, "Sensores laterais nas extremidades para navegação em áreas estreitas, mantendo uma distância segura das fileiras e evitando colisões." },
|
|
|
|
{ ConfiguracaoSensorUltrassonico.SupervisaoAreaComObstaculos, "Configuração que monitora objetos ao redor do robô, oferecendo uma visão de 360 graus para navegação em áreas com alta densidade de obstáculos." }
|
|
};
|
|
|
|
public static UltrasonicA05Configuracoes CarregarConfiguracaoPadrao(ConfiguracaoSensorUltrassonico Config)
|
|
{
|
|
UltrasonicA05Configuracoes ConfigPadrao = new UltrasonicA05Configuracoes()
|
|
{
|
|
Configuracao = Config,
|
|
Sensores = new List<UltrasonicA05ConfiguracoesSensor>()
|
|
};
|
|
|
|
switch (Config)
|
|
{
|
|
case ConfiguracaoSensorUltrassonico.NaoUtilizado:
|
|
ConfigPadrao.Sensores.Add(DefineDadosSensor(1));
|
|
ConfigPadrao.Sensores.Add(DefineDadosSensor(2));
|
|
ConfigPadrao.Sensores.Add(DefineDadosSensor(3));
|
|
ConfigPadrao.Sensores.Add(DefineDadosSensor(4));
|
|
break;
|
|
|
|
case ConfiguracaoSensorUltrassonico.AlinhamentoLateral:
|
|
ConfigPadrao.Sensores.Add(DefineDadosSensor(1, PosicaoSensorUltrassonico.LateralEsquerdaTraseiro, true, true, 25.0, 45.0, 5.0, 5.0, true, false));
|
|
ConfigPadrao.Sensores.Add(DefineDadosSensor(2, PosicaoSensorUltrassonico.LateralDireitaTraseiro, true, true, 25.0, 45.0, 5.0, 5.0, true, false));
|
|
ConfigPadrao.Sensores.Add(DefineDadosSensor(3, PosicaoSensorUltrassonico.LateralEsquerdaFrontal, true, true, 25.0, 45.0, 5.0, 5.0, true, false));
|
|
ConfigPadrao.Sensores.Add(DefineDadosSensor(4, PosicaoSensorUltrassonico.LateralDireitaFrontal, true, true, 25.0, 45.0, 5.0, 5.0, true, false));
|
|
break;
|
|
|
|
case ConfiguracaoSensorUltrassonico.DeteccaoObstaculosFrenteETras:
|
|
ConfigPadrao.Sensores.Add(DefineDadosSensor(1, PosicaoSensorUltrassonico.TraseiroEsquerdo, true, false, 50.0, 0.0, 2.0, 0.0, true, false));
|
|
ConfigPadrao.Sensores.Add(DefineDadosSensor(2, PosicaoSensorUltrassonico.TraseiroDireito, true, false, 50.0, 0.0, 2.0, 0.0, true, false));
|
|
ConfigPadrao.Sensores.Add(DefineDadosSensor(3, PosicaoSensorUltrassonico.FrontalEsquerdo, true, false, 50.0, 0.0, 2.0, 0.0, true, false));
|
|
ConfigPadrao.Sensores.Add(DefineDadosSensor(4, PosicaoSensorUltrassonico.FrontalDireito, true, false, 50.0, 0.0, 2.0, 0.0, true, false));
|
|
break;
|
|
|
|
case ConfiguracaoSensorUltrassonico.DeteccaoSulcosEBuracos:
|
|
ConfigPadrao.Sensores.Add(DefineDadosSensor(1, PosicaoSensorUltrassonico.FrontalCentro, true, true, 5.0, 20.0, 1.0, 1.0, false, true));
|
|
ConfigPadrao.Sensores.Add(DefineDadosSensor(2, PosicaoSensorUltrassonico.FrontalEsquerdo, true, true, 5.0, 20.0, 1.0, 1.0, false, false));
|
|
ConfigPadrao.Sensores.Add(DefineDadosSensor(3, PosicaoSensorUltrassonico.FrontalDireito, true, true, 5.0, 20.0, 1.0, 1.0, false, false));
|
|
break;
|
|
|
|
case ConfiguracaoSensorUltrassonico.NavegacaoAmbienteEstreito:
|
|
ConfigPadrao.Sensores.Add(DefineDadosSensor(1, PosicaoSensorUltrassonico.LateralEsquerdaFrontal, true, true, 15.0, 30.0, 3.0, 3.0, true, false));
|
|
ConfigPadrao.Sensores.Add(DefineDadosSensor(2, PosicaoSensorUltrassonico.LateralDireitaFrontal, true, true, 15.0, 30.0, 3.0, 3.0, true, false));
|
|
ConfigPadrao.Sensores.Add(DefineDadosSensor(3, PosicaoSensorUltrassonico.LateralEsquerdaTraseiro, true, true, 15.0, 30.0, 3.0, 3.0, true, false));
|
|
ConfigPadrao.Sensores.Add(DefineDadosSensor(4, PosicaoSensorUltrassonico.LateralDireitaTraseiro, true, true, 15.0, 30.0, 3.0, 3.0, true, false));
|
|
break;
|
|
|
|
case ConfiguracaoSensorUltrassonico.SupervisaoAreaComObstaculos:
|
|
ConfigPadrao.Sensores.Add(DefineDadosSensor(1, PosicaoSensorUltrassonico.FrontalCentro, true, true, 20.0, 50.0, 4.0, 4.0, true, true));
|
|
ConfigPadrao.Sensores.Add(DefineDadosSensor(2, PosicaoSensorUltrassonico.LateralEsquerdaCentral, true, true, 20.0, 50.0, 4.0, 4.0, true, false));
|
|
ConfigPadrao.Sensores.Add(DefineDadosSensor(3, PosicaoSensorUltrassonico.LateralDireitaCentral, true, true, 20.0, 50.0, 4.0, 4.0, true, false));
|
|
ConfigPadrao.Sensores.Add(DefineDadosSensor(4, PosicaoSensorUltrassonico.TraseiroCentro, true, true, 20.0, 50.0, 4.0, 4.0, true, true));
|
|
break;
|
|
}
|
|
|
|
return ConfigPadrao;
|
|
}
|
|
|
|
private static UltrasonicA05ConfiguracoesSensor DefineDadosSensor(int _id, PosicaoSensorUltrassonico _posicao = PosicaoSensorUltrassonico.NaoConectado, bool _limiteMin = false, bool _limiteMax = false, double _limiteMinVal = 0, double _limiteMaxVal = 0, double _margemMin = 0, double _margemMax = 0, bool _desvio = false, bool _parada = false)
|
|
{
|
|
return new UltrasonicA05ConfiguracoesSensor()
|
|
{
|
|
ID = _id,
|
|
Posicao = _posicao,
|
|
LimiteMinimo = _limiteMin,
|
|
LimiteMaximo = _limiteMax,
|
|
LimiteMinimoVal = _limiteMinVal,
|
|
LimiteMaximoVal = _limiteMaxVal,
|
|
MargemMinimo = _margemMin,
|
|
MargemMaximo = _margemMax,
|
|
Desvio = _desvio,
|
|
Parada = _parada
|
|
};
|
|
}
|
|
|
|
public static double CalcularErroUltrassons()
|
|
{
|
|
double erroUltrassons = 0.0;
|
|
double pesoTotal = 0.0;
|
|
double erroAlinhamento = 0.0;
|
|
bool paradaNecessaria = false;
|
|
bool desvioEsquerdaNecessario = false;
|
|
bool desvioDireitaNecessario = false;
|
|
|
|
var Config = Variaveis.OperacaoEmAndamento.DispSen?.Dados?.ConfiguracaoSensorUltrassom;
|
|
|
|
if (
|
|
!UltrasonicA05Service.Iniciado || // ultrassom nao iniciado
|
|
Config == null || // configuracao nao definida
|
|
Config.Configuracao == ConfiguracaoSensorUltrassonico.NaoUtilizado || // configuracao para nao utilizar
|
|
Config.Sensores.All(x => x.Posicao == PosicaoSensorUltrassonico.NaoConectado) || // configuracao com todos os sensores desconectados
|
|
Config.Sensores.All(x => !x.Parada && !x.Desvio) // configuracao onde os sensores nao tem acao
|
|
)
|
|
{
|
|
return 0.0;
|
|
}
|
|
|
|
// Processa alinhamento lateral
|
|
if (Config.Configuracao == ConfiguracaoSensorUltrassonico.AlinhamentoLateral)
|
|
{
|
|
var sensorEsquerdoFrente = Config.Sensores.FirstOrDefault(s => s.Posicao == PosicaoSensorUltrassonico.LateralEsquerdaFrontal && s.SensorAtuado);
|
|
var sensorEsquerdoTras = Config.Sensores.FirstOrDefault(s => s.Posicao == PosicaoSensorUltrassonico.LateralEsquerdaTraseiro && s.SensorAtuado);
|
|
var sensorDireitoFrente = Config.Sensores.FirstOrDefault(s => s.Posicao == PosicaoSensorUltrassonico.LateralDireitaFrontal && s.SensorAtuado);
|
|
var sensorDireitoTras = Config.Sensores.FirstOrDefault(s => s.Posicao == PosicaoSensorUltrassonico.LateralDireitaTraseiro && s.SensorAtuado);
|
|
|
|
if (sensorEsquerdoFrente != null && sensorEsquerdoTras != null)
|
|
{
|
|
erroAlinhamento += (sensorEsquerdoFrente.LeituraCm - sensorEsquerdoTras.LeituraCm) * -1;
|
|
pesoTotal += 0.5; // Peso para o lado esquerdo
|
|
}
|
|
|
|
if (sensorDireitoFrente != null && sensorDireitoTras != null)
|
|
{
|
|
erroAlinhamento += (sensorDireitoFrente.LeituraCm - sensorDireitoTras.LeituraCm);
|
|
pesoTotal += 0.5; // Peso para o lado direito
|
|
}
|
|
|
|
erroUltrassons += erroAlinhamento * (pesoTotal > 0 ? 0.5 : 1.0);
|
|
pesoTotal += 0.5;
|
|
}
|
|
|
|
// Verifica condições de parada e desvio
|
|
foreach (var sensor in Config.Sensores.Where(s => s.SensorAtuado))
|
|
{
|
|
if (sensor.Posicao == PosicaoSensorUltrassonico.NaoConectado || (!sensor.Desvio && !sensor.Parada)) continue;
|
|
|
|
if (sensor.Parada)
|
|
{
|
|
paradaNecessaria = true;
|
|
break; // Parada imediata, não precisa de mais verificações
|
|
}
|
|
|
|
if (sensor.Desvio)
|
|
{
|
|
if (sensor.Posicao == PosicaoSensorUltrassonico.LateralEsquerdaFrontal || sensor.Posicao == PosicaoSensorUltrassonico.LateralEsquerdaTraseiro)
|
|
{
|
|
desvioEsquerdaNecessario = true;
|
|
}
|
|
else if (sensor.Posicao == PosicaoSensorUltrassonico.LateralDireitaFrontal || sensor.Posicao == PosicaoSensorUltrassonico.LateralDireitaTraseiro)
|
|
{
|
|
desvioDireitaNecessario = true;
|
|
}
|
|
}
|
|
}
|
|
|
|
// Decisões de parada e desvio
|
|
if (paradaNecessaria || (desvioEsquerdaNecessario && desvioDireitaNecessario))
|
|
{
|
|
//Variaveis.OperacaoEmAndamento.Controle.PercentualVelocidadeSP = 0; // Parada imediata
|
|
return double.MaxValue;
|
|
}
|
|
else if (desvioEsquerdaNecessario)
|
|
{
|
|
erroUltrassons = -1.0; // Desvio para a direita
|
|
pesoTotal = 0.3; // Peso maior para desvio
|
|
}
|
|
else if (desvioDireitaNecessario)
|
|
{
|
|
erroUltrassons = 1.0; // Desvio para a esquerda
|
|
pesoTotal = 0.3;
|
|
}
|
|
|
|
return pesoTotal > 0 ? erroUltrassons / pesoTotal : 0;
|
|
}
|
|
|
|
public static string ObterCategoriaSensor(PosicaoSensorUltrassonico posicao)
|
|
{
|
|
if (posicao.ToString().Contains("Esquerdo")) return "Esquerda";
|
|
if (posicao.ToString().Contains("Direito")) return "Direita";
|
|
if (posicao.ToString().Contains("Frontal")) return "Frontal";
|
|
if (posicao.ToString().Contains("Traseiro")) return "Traseira";
|
|
return "Desconhecida";
|
|
}
|
|
|
|
}
|
|
|
|
public class UltrasonicA05Model
|
|
{
|
|
public bool Conectado { get; set; }
|
|
public DateTime Momento { get; set; }
|
|
public byte Endereco { get; set; }
|
|
public int U1 { get; set; }
|
|
public int U2 { get; set; }
|
|
public int U3 { get; set; }
|
|
public int U4 { get; set; }
|
|
public UltrasonicA05Configuracoes Configuracao { get; set; }
|
|
public DateTime UltimoComandoRespondido { get; set; }
|
|
}
|
|
|
|
public class UltrasonicA05Configuracoes
|
|
{
|
|
public ConfiguracaoSensorUltrassonico Configuracao { get; set; } = ConfiguracaoSensorUltrassonico.NaoUtilizado;
|
|
public List<UltrasonicA05ConfiguracoesSensor> Sensores { get; set; }
|
|
private IEnumerable<IGrouping<string, UltrasonicA05ConfiguracoesSensor>> _sensoresAtuados
|
|
{
|
|
get
|
|
{
|
|
var sensoresAtuados = Sensores
|
|
.Where(x => x.SensorAtuado)
|
|
.GroupBy(x => UltrasonicA05Helper.ObterCategoriaSensor(x.Posicao));
|
|
return sensoresAtuados;
|
|
}
|
|
}
|
|
|
|
private bool _desvioEsquerda => _sensoresAtuados.Where(g => g.Key == "Esquerda").Any(g => g.Any(s => s.Desvio));
|
|
private bool _desvioDireita => _sensoresAtuados.Where(g => g.Key == "Direita").Any(g => g.Any(s => s.Desvio));
|
|
private bool _deteccaoFrontal => _sensoresAtuados.Any(g => g.Key == "Frontal");
|
|
public bool _paradaFrontal => _sensoresAtuados.Where(g => g.Key == "Frontal").Any(g => g.Any(s => s.Parada));
|
|
private bool _deteccaoTraseira => _sensoresAtuados.Any(g => g.Key == "Traseira");
|
|
public bool _paradaTraseira => _sensoresAtuados.Where(g => g.Key == "Traseira").Any(g => g.Any(s => s.Parada));
|
|
private bool _deteccaoEsquerda => _sensoresAtuados.Any(g => g.Key == "Esquerda");
|
|
public bool _paradaEsquerda => _sensoresAtuados.Where(g => g.Key == "Esquerda").Any(g => g.Any(s => s.Parada));
|
|
private bool _deteccaoDireita => _sensoresAtuados.Any(g => g.Key == "Direita");
|
|
public bool _paradaDireita => _sensoresAtuados.Where(g => g.Key == "Direita").Any(g => g.Any(s => s.Parada));
|
|
|
|
public bool DesvioNecessario
|
|
{
|
|
get
|
|
{
|
|
return DirecaoDesvio != Direcao.Frente;
|
|
}
|
|
}
|
|
public bool ParadaNecessaria => _sensoresAtuados.Any(x => x.Any(g => g.Parada));
|
|
public List<Direcao> DirecoesObstaculos
|
|
{
|
|
get
|
|
{
|
|
List<Direcao> Direcoes = new List<Direcao>();
|
|
|
|
if (_deteccaoFrontal)
|
|
{
|
|
Direcoes.Add(Direcao.Frente);
|
|
}
|
|
if (_deteccaoTraseira)
|
|
{
|
|
Direcoes.Add(Direcao.Tras);
|
|
}
|
|
if (_deteccaoEsquerda)
|
|
{
|
|
Direcoes.Add(Direcao.Esquerda);
|
|
}
|
|
if (_deteccaoDireita)
|
|
{
|
|
Direcoes.Add(Direcao.Direita);
|
|
}
|
|
|
|
return Direcoes;
|
|
}
|
|
}
|
|
public Direcao DirecaoDesvio
|
|
{
|
|
get
|
|
{
|
|
if (_paradaFrontal || _paradaTraseira || _paradaEsquerda || _paradaDireita || (_deteccaoEsquerda && _deteccaoDireita))
|
|
{
|
|
return Direcao.Parado;
|
|
}
|
|
else if (_deteccaoEsquerda && _desvioEsquerda)
|
|
{
|
|
return Direcao.Direita;
|
|
}
|
|
else if (_deteccaoDireita && _desvioDireita)
|
|
{
|
|
return Direcao.Esquerda;
|
|
}
|
|
|
|
return Direcao.Frente;
|
|
}
|
|
}
|
|
}
|
|
|
|
public class UltrasonicA05ConfiguracoesSensor
|
|
{
|
|
public int ID { get; set; }
|
|
public PosicaoSensorUltrassonico Posicao { get; set; }
|
|
public bool LimiteMinimo { get; set; }
|
|
public double LimiteMinimoVal { get; set; }
|
|
public double MargemMinimo { get; set; }
|
|
public bool LimiteMaximo { get; set; }
|
|
public double LimiteMaximoVal { get; set; }
|
|
public double MargemMaximo { get; set; }
|
|
public bool Desvio { get; set; }
|
|
public bool Parada { get; set; }
|
|
public double DistanciaIdeal
|
|
{
|
|
get
|
|
{
|
|
// Calcula o ponto médio entre os limites, ou usa o limite ativo como valor ideal
|
|
return
|
|
LimiteMaximo && LimiteMinimo ? (LimiteMaximoVal + LimiteMinimoVal) / 2.0
|
|
: LimiteMinimo ? LimiteMinimoVal
|
|
: LimiteMaximo ? LimiteMaximoVal
|
|
: 0.0;
|
|
}
|
|
}
|
|
public bool EmUso => LimiteMaximo || LimiteMinimo;
|
|
public double LeituraCm { get; set; }
|
|
public bool SensorAtuado
|
|
{
|
|
get
|
|
{
|
|
// Caso LimiteMinimo e LimiteMaximo estejam configurados
|
|
if (LimiteMinimo && LimiteMaximo)
|
|
{
|
|
return LeituraCm < (LimiteMinimoVal - MargemMinimo) || LeituraCm > (LimiteMaximoVal + MargemMaximo);
|
|
}
|
|
// Caso apenas LimiteMinimo esteja configurado
|
|
else if (LimiteMinimo)
|
|
{
|
|
return LeituraCm < (LimiteMinimoVal - MargemMinimo);
|
|
}
|
|
// Caso apenas LimiteMaximo esteja configurado
|
|
else if (LimiteMaximo)
|
|
{
|
|
return LeituraCm > (LimiteMaximoVal + MargemMaximo);
|
|
}
|
|
return false;
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
}
|