284 lines
14 KiB
C#
284 lines
14 KiB
C#
using AgroBase.Services;
|
|
using OpenHardwareMonitor.Hardware;
|
|
using System;
|
|
using System.Collections.Generic;
|
|
using System.Linq;
|
|
|
|
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.FrontalEsquerdo, true, true, 10.0, 30.0, 2.0, 2.0, true, true));
|
|
ConfigPadrao.Sensores.Add(DefineDadosSensor(2, PosicaoSensorUltrassonico.FrontalDireito, true, true, 10.0, 30.0, 2.0, 2.0, true, true));
|
|
ConfigPadrao.Sensores.Add(DefineDadosSensor(3, PosicaoSensorUltrassonico.TraseiroEsquerdo, true, true, 10.0, 30.0, 2.0, 2.0, true, true));
|
|
ConfigPadrao.Sensores.Add(DefineDadosSensor(4, PosicaoSensorUltrassonico.TraseiroDireito, true, true, 10.0, 30.0, 2.0, 2.0, true, true));
|
|
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 || Config == null || Config.Configuracao == ConfiguracaoSensorUltrassonico.NaoUtilizado || Config.Sensores.All(x => x.Posicao == PosicaoSensorUltrassonico.NaoConectado))
|
|
{
|
|
return 0.0;
|
|
}
|
|
|
|
// Processa alinhamento lateral
|
|
if (Config.Configuracao == ConfiguracaoSensorUltrassonico.AlinhamentoLateral)
|
|
{
|
|
var sensorEsquerdoFrente = Config.Sensores.FirstOrDefault(s => s.Posicao == PosicaoSensorUltrassonico.LateralEsquerdaFrontal);
|
|
var sensorEsquerdoTras = Config.Sensores.FirstOrDefault(s => s.Posicao == PosicaoSensorUltrassonico.LateralEsquerdaTraseiro);
|
|
var sensorDireitoFrente = Config.Sensores.FirstOrDefault(s => s.Posicao == PosicaoSensorUltrassonico.LateralDireitaFrontal);
|
|
var sensorDireitoTras = Config.Sensores.FirstOrDefault(s => s.Posicao == PosicaoSensorUltrassonico.LateralDireitaTraseiro);
|
|
|
|
if (sensorEsquerdoFrente != null && sensorEsquerdoTras != null)
|
|
{
|
|
erroAlinhamento += (sensorEsquerdoFrente.Leitura - sensorEsquerdoTras.Leitura) * -1;
|
|
pesoTotal += 0.5; // Peso para o lado esquerdo
|
|
}
|
|
|
|
if (sensorDireitoFrente != null && sensorDireitoTras != null)
|
|
{
|
|
erroAlinhamento += (sensorDireitoFrente.Leitura - sensorDireitoTras.Leitura);
|
|
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)
|
|
{
|
|
if (sensor.Posicao == PosicaoSensorUltrassonico.NaoConectado) continue;
|
|
|
|
if (sensor.Parada && sensor.SensorAtuado)
|
|
{
|
|
paradaNecessaria = true;
|
|
break; // Parada imediata, não precisa de mais verificações
|
|
}
|
|
|
|
if (sensor.Desvio && sensor.SensorAtuado)
|
|
{
|
|
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 class UltrasonicA05Model
|
|
{
|
|
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 class UltrasonicA05Configuracoes
|
|
{
|
|
public ConfiguracaoSensorUltrassonico Configuracao { get; set; } = ConfiguracaoSensorUltrassonico.NaoUtilizado;
|
|
public List<UltrasonicA05ConfiguracoesSensor> Sensores { get; set; }
|
|
}
|
|
|
|
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 Leitura { get; set; }
|
|
public bool SensorAtuado
|
|
{
|
|
get
|
|
{
|
|
// Caso LimiteMinimo e LimiteMaximo estejam configurados
|
|
if (LimiteMinimo && LimiteMaximo)
|
|
{
|
|
return Leitura < (LimiteMinimoVal - MargemMinimo) || Leitura > (LimiteMaximoVal + MargemMaximo);
|
|
}
|
|
// Caso apenas LimiteMinimo esteja configurado
|
|
else if (LimiteMinimo)
|
|
{
|
|
return Leitura < (LimiteMinimoVal - MargemMinimo);
|
|
}
|
|
// Caso apenas LimiteMaximo esteja configurado
|
|
else if (LimiteMaximo)
|
|
{
|
|
return Leitura > (LimiteMaximoVal + MargemMaximo);
|
|
}
|
|
return false;
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
|
|
|
|
}
|