795 lines
35 KiB
C#
795 lines
35 KiB
C#
using AgroBase.Models.Modules;
|
|
using AgroBase.Services;
|
|
using System;
|
|
using System.Collections.Generic;
|
|
using System.Drawing;
|
|
using System.Linq;
|
|
using System.Windows.Forms;
|
|
using static AgroBase.Models.Enums;
|
|
|
|
namespace AgroBase.Models
|
|
{
|
|
public class SonarModel
|
|
{
|
|
public SonarModel(PictureBox pictureSonar)
|
|
{
|
|
picSonar = pictureSonar;
|
|
}
|
|
|
|
|
|
public List<SonarRegistroModel> Deteccoes { get; set; } = new List<SonarRegistroModel>();
|
|
public List<SonarObstaculoDetectadoModel> ObstaculosDetectados { get; set; } = new List<SonarObstaculoDetectadoModel>();
|
|
public SonarObstaculoDetectadoModel ObstaculoCritico
|
|
{
|
|
get
|
|
{
|
|
if (ObstaculosDetectados.Any())
|
|
{
|
|
return ObstaculosDetectados.OrderBy(x => x.DistanciaMedia).FirstOrDefault(x => x.DirecaoDesvio == Direcao.Parado || x.DeslocamentoNecessarioDesvio > 0);
|
|
}
|
|
else
|
|
{
|
|
return null;
|
|
}
|
|
}
|
|
}
|
|
public int maxDistanceCm { get; set; } = 200;
|
|
public static int anguloAcrescentar { get; set; } = 5;
|
|
public int PosicaoAtual { get; set; } = 0;
|
|
public static int AnguloDesvioMaximo { get; set; } = 60;
|
|
public static double MargemSegurancaDesvio { get; set; } = 5;
|
|
private int LarguraMinimaObstaculo { get; set; } = 10;
|
|
public bool DesvioNecessario { get; set;} = false;
|
|
public Direcao DirecaoDesvio
|
|
{
|
|
get
|
|
{
|
|
if (!ObstaculosDetectados.Any())
|
|
{
|
|
AnguloDesvio = 0;
|
|
return Direcao.Frente;
|
|
}
|
|
else if (ObstaculosDetectados.Any(x => x.DirecaoDesvio == Direcao.Parado))
|
|
{
|
|
AnguloDesvio = 0;
|
|
return Direcao.Parado;
|
|
}
|
|
else if (ObstaculosDetectados.Count() == 1)
|
|
{
|
|
AnguloDesvio = ObstaculoCritico.AnguloParaDesvio;
|
|
return ObstaculoCritico.DirecaoDesvio;
|
|
}
|
|
else
|
|
{
|
|
if (ObstaculosDetectados.Count(x => x.Aesquerda) > ObstaculosDetectados.Count(x => x.Adireita))
|
|
{
|
|
if (ObstaculoCritico.DirecaoDesvio == Direcao.Esquerda)
|
|
{
|
|
AnguloDesvio = ObstaculoCritico.AnguloParaDesvioDireita;
|
|
}
|
|
return Direcao.Direita;
|
|
}
|
|
else
|
|
{
|
|
if (ObstaculoCritico.DirecaoDesvio == Direcao.Direita)
|
|
{
|
|
AnguloDesvio = ObstaculoCritico.AnguloParaDesvioEsquerda;
|
|
}
|
|
return Direcao.Esquerda;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
public double AnguloDesvio { get; set; }
|
|
public double DistanciaEsquerda { get; set; }
|
|
public double DistanciaDireita { get; set; }
|
|
|
|
|
|
#region Informacoes Visiais
|
|
|
|
PictureBox picSonar;
|
|
int radarRadiusPixels = 0; // Raio do radar em pixels
|
|
double pixelsPerCm = 0; // Calcula pixels por cm
|
|
int leiturasManter = 20;
|
|
|
|
public void AtualizarSonar()
|
|
{
|
|
// Atualiza o desenho do radar
|
|
DrawRadar();
|
|
ProcessarDados();
|
|
}
|
|
private void DrawRadar()
|
|
{
|
|
radarRadiusPixels = ((picSonar.Height < picSonar.Width ? picSonar.Height : picSonar.Width) / 2) - 50; // Raio do radar em pixels
|
|
pixelsPerCm = radarRadiusPixels / (double)maxDistanceCm; // Calcula pixels por cm
|
|
|
|
Bitmap bmp = new Bitmap(picSonar.Width, picSonar.Height);
|
|
Graphics g = Graphics.FromImage(bmp);
|
|
g.Clear(Color.Black);
|
|
|
|
int centerX = picSonar.Width / 2;
|
|
int centerY = picSonar.Height / 2;
|
|
|
|
// Desenha a grade e os ângulos
|
|
DrawGrid(g, centerX, centerY);
|
|
DrawAngles(g, centerX, centerY);
|
|
|
|
var detections = Deteccoes.First(x => x.Posicao == PosicaoAtual).Leituras.OrderByDescending(d => d.Momento);
|
|
// Aqui continua o código para desenhar as detecções e o arco do sonar...
|
|
// Filtra e mantém apenas as últimas 2 detecções de cada ângulo
|
|
var recentDetections = detections.ToList();
|
|
int i = 0;
|
|
foreach (var detection in recentDetections)
|
|
{
|
|
// Ajusta a distância para a escala do display
|
|
double scaledDistance = (i == 0 ? maxDistanceCm : detection.Distancia) * pixelsPerCm;
|
|
|
|
// Calcula as posições finais ajustadas para a escala
|
|
int x = (int)(Math.Cos((180 - detection.Angulo) * Math.PI / 180) * scaledDistance);
|
|
int y = (int)(Math.Sin((180 - detection.Angulo) * Math.PI / 180) * scaledDistance);
|
|
|
|
// Desenha uma linha do centro até o ponto detectado, ajustado para a escala
|
|
g.DrawLine(i == 0 ? new Pen(Color.Red, 7) : Pens.Red, centerX, centerY, centerX + x, centerY - y);
|
|
i++;
|
|
}
|
|
|
|
// Desenha os obstáculos detectados
|
|
DrawObstacles(g, centerX, centerY);
|
|
|
|
// Adiciona as novas informações visuais aqui
|
|
DrawAdditionalInfo(g, 180 - detections.First().Angulo, detections.First().Distancia);
|
|
|
|
// Desenha a trajetória de desvio
|
|
DrawTrajectory(g, centerX, centerY);
|
|
|
|
// Ao final, atribui o bitmap ao PictureBox
|
|
picSonar.Image?.Dispose(); // Libera o recurso da imagem anterior
|
|
picSonar.Image = bmp;
|
|
}
|
|
|
|
private void DrawGrid(Graphics g, int centerX, int centerY)
|
|
{
|
|
int scaleDistanceCm = 50; // Distância em cm que cada círculo da grade representa
|
|
|
|
// Calcula o espaçamento entre as linhas da grade em pixels
|
|
int stepPixels = (int)(scaleDistanceCm * pixelsPerCm);
|
|
|
|
// Desenha linhas circulares baseadas na escala
|
|
for (int r = stepPixels; r <= radarRadiusPixels; r += stepPixels)
|
|
{
|
|
g.DrawEllipse(Pens.Gray, centerX - r, centerY - r, 2 * r, 2 * r);
|
|
|
|
// Calcula a posição da legenda
|
|
int legendX = centerX + 5; // Um pequeno deslocamento à direita do centro
|
|
int legendY = centerY + r - 10; // Posiciona acima da linha da grade
|
|
|
|
// Desenha a legenda de distância
|
|
string legendText = $"{(r / pixelsPerCm).ToString("0")} cm";
|
|
g.DrawString(legendText, new Font("Arial", 8), Brushes.White, legendX, legendY);
|
|
}
|
|
|
|
// Desenha linhas radiais (Opcional: ajuste conforme necessário)
|
|
for (int angle = 0; angle < 360; angle += 30)
|
|
{
|
|
double radian = angle * Math.PI / 180;
|
|
int x = (int)(Math.Cos(radian) * radarRadiusPixels);
|
|
int y = (int)(Math.Sin(radian) * radarRadiusPixels);
|
|
g.DrawLine(Pens.Gray, centerX, centerY, centerX + x, centerY - y);
|
|
}
|
|
|
|
// Desenha o arco do sonar
|
|
g.DrawEllipse(Pens.Green, centerX - radarRadiusPixels, centerY - radarRadiusPixels, radarRadiusPixels * 2, radarRadiusPixels * 2);
|
|
}
|
|
|
|
private void DrawAngles(Graphics g, int centerX, int centerY)
|
|
{
|
|
int radius = radarRadiusPixels + 10; // Posiciona o texto um pouco fora do radar para não sobrepor as linhas
|
|
using (Font font = new Font("Arial", 8))
|
|
{
|
|
for (int angle = 0; angle <= 180; angle += 30) // Ajuste o passo conforme necessário
|
|
{
|
|
// Ajuste aqui para o sentido anti-horário
|
|
double radian = (360 - angle) * Math.PI / 180;
|
|
int x = (int)(Math.Cos(radian) * radius) + centerX;
|
|
int y = (int)(Math.Sin(radian) * radius) + centerY;
|
|
|
|
// Ajusta a posição do texto para melhor alinhamento
|
|
SizeF textSize = g.MeasureString(angle.ToString(), font);
|
|
g.DrawString(angle.ToString(), font, Brushes.White, x - textSize.Width / 2, y - textSize.Height / 2);
|
|
}
|
|
}
|
|
}
|
|
|
|
private void DrawObstacles(Graphics g, int centerX, int centerY)
|
|
{
|
|
// Desenha obstáculos detectados
|
|
foreach (var obstaculo in ObstaculosDetectados)
|
|
{
|
|
// Calcula a posição e o tamanho do obstáculo no radar
|
|
double anguloInicialRad = (180 - obstaculo.AnguloInicial) * Math.PI / 180;
|
|
double anguloFinalRad = (180 - obstaculo.AnguloFinal) * Math.PI / 180;
|
|
|
|
// Calcula a largura do obstáculo no radar em pixels
|
|
double larguraObstaculoPixels = obstaculo.Largura * pixelsPerCm;
|
|
//var Sonar = Variaveis.OperacaoEmAndamento.Sensoriamento.SonarFrontal_old;
|
|
var Sonar = new SonarModel(new PictureBox());
|
|
int posicaoAtual = Sonar.PosicaoAtual;
|
|
var deteccoes = Sonar.Deteccoes.First(x => x.Posicao == posicaoAtual).Leituras.OrderBy(x => x.Angulo).ToList();
|
|
var leituraInicial = deteccoes.FirstOrDefault(x => x.Angulo == obstaculo.AnguloInicial);
|
|
var leituraFinal = deteccoes.FirstOrDefault(x => x.Angulo == obstaculo.AnguloFinal);
|
|
if (leituraInicial == null || leituraFinal == null)
|
|
{
|
|
continue;
|
|
}
|
|
|
|
int idxPontoInicial = deteccoes.IndexOf(leituraInicial);
|
|
int idxPontoFinal = deteccoes.IndexOf(leituraFinal);
|
|
|
|
List<Point> pontos = new List<Point>();
|
|
for (int i = idxPontoInicial; i <= idxPontoFinal; i++)
|
|
{
|
|
double anguloRad = (180 - deteccoes[i].Angulo) * Math.PI / 180;
|
|
var ponto = new Point(centerX + ((int)(Math.Cos(anguloRad) * (deteccoes[i].Distancia * pixelsPerCm))),
|
|
centerY - ((int)(Math.Sin(anguloRad) * (deteccoes[i].Distancia * pixelsPerCm))));
|
|
pontos.Add(ponto);
|
|
}
|
|
|
|
pontos.Add(new Point(
|
|
pontos[pontos.Count - 1].X + ((int)(Math.Cos(anguloFinalRad) * larguraObstaculoPixels)),
|
|
pontos[pontos.Count - 1].Y - ((int)(Math.Sin(anguloFinalRad) * larguraObstaculoPixels)))
|
|
);
|
|
pontos.Add(new Point(
|
|
pontos[0].X + ((int)(Math.Cos(anguloInicialRad) * larguraObstaculoPixels)),
|
|
pontos[0].Y - ((int)(Math.Sin(anguloInicialRad) * larguraObstaculoPixels)))
|
|
);
|
|
|
|
g.FillPolygon(Brushes.Yellow, pontos.ToArray());
|
|
}
|
|
}
|
|
|
|
private void DrawAdditionalInfo(Graphics g, double angulo, double distancia)
|
|
{
|
|
// Adiciona a informação de ângulo e distância atuais
|
|
string infoText = $"Posição: {PosicaoAtual}, Ângulo: {angulo}°, Distância: {distancia} cm";
|
|
// Escolha a posição onde deseja que o texto apareça. Exemplo: canto superior esquerdo.
|
|
int infoPosX = 10; // 10 pixels do canto esquerdo
|
|
int infoPosY = 10; // 10 pixels do topo
|
|
g.DrawString(infoText, new Font("Arial", 10), Brushes.White, infoPosX, infoPosY);
|
|
string distText = $"DE: {DistanciaEsquerda.ToString("0.0")} cm, DD: {DistanciaDireita.ToString("0.0")} cm";
|
|
g.DrawString(distText, new Font("Arial", 7), Brushes.White, picSonar.Width - 180, picSonar.Height - 20);
|
|
|
|
// Verifica se há obstáculos detectados para evitar erros
|
|
if (ObstaculosDetectados.Any())
|
|
{
|
|
SonarObstaculoDetectadoModel obstaculoCritico = ObstaculoCritico;
|
|
|
|
if (obstaculoCritico != null)
|
|
{
|
|
string anguloDesvioText = $"Ângulo de Desvio: Ob {obstaculoCritico.AnguloParaDesvio.ToString("0.00")} Tj {AnguloDesvio.ToString("0.00")}°";
|
|
string distanciaObstaculoText = $"Distância até Obstáculo: {obstaculoCritico.DistanciaMedia.ToString("0.00")} cm";
|
|
string larguraObstaculoText = $"Largura do Obstáculo: {obstaculoCritico.Largura.ToString("0.00")} cm";
|
|
string tempoColisaoText = $"Tempo até Colisão: {obstaculoCritico.TempoAteColisao.ToString("0.00")} s";
|
|
string direcaoDesvioText = $"Direção para Desvio: Ob {obstaculoCritico.DirecaoDesvio.ToString()} Tj {DirecaoDesvio.ToString()}";
|
|
|
|
// Define a posição inicial para as informações adicionais
|
|
int startX = 10;
|
|
int startY = picSonar.Height - 100; // Ajuste conforme necessário para não sobrepor outros elementos
|
|
|
|
// Desenha o texto no radar
|
|
g.DrawString(anguloDesvioText, new Font("Arial", 10), Brushes.White, startX, startY);
|
|
g.DrawString(distanciaObstaculoText, new Font("Arial", 10), Brushes.White, startX, startY + 20);
|
|
g.DrawString(larguraObstaculoText, new Font("Arial", 10), Brushes.White, startX, startY + 40);
|
|
g.DrawString(tempoColisaoText, new Font("Arial", 10), Brushes.White, startX, startY + 60);
|
|
g.DrawString(direcaoDesvioText, new Font("Arial", 10), Brushes.White, startX, startY + 80);
|
|
}
|
|
}
|
|
}
|
|
|
|
private void DrawTrajectory(Graphics g, int centerX, int centerY)
|
|
{
|
|
if (DesvioNecessario)
|
|
{
|
|
// Calcula o ângulo final considerando o ângulo de desvio
|
|
double anguloDesvioRad = (AnguloDesvio + 90) * (Math.PI / 180); // +90 para ajustar a referência
|
|
|
|
// Calcula a posição final da trajetória
|
|
double distanciaObstaculo = maxDistanceCm * pixelsPerCm;
|
|
int desvioX = (int)(Math.Cos(anguloDesvioRad) * distanciaObstaculo);
|
|
int desvioY = (int)(Math.Sin(anguloDesvioRad) * distanciaObstaculo);
|
|
|
|
// Cria um Pen tracejado azul
|
|
Pen penTrajectory = new Pen(Color.Blue, 3) { DashPattern = new float[] { 4, 2 } }; // Ajuste o padrão de traço conforme necessário
|
|
|
|
// Desenha a linha de trajetória
|
|
g.DrawLine(penTrajectory, centerX, centerY, centerX + desvioX, centerY - desvioY);
|
|
|
|
// Calcula o ângulo final considerando o ângulo de desvio
|
|
double anguloDesvioObRad = (ObstaculoCritico.AnguloParaDesvio + 90) * (Math.PI / 180); // +90 para ajustar a referência
|
|
|
|
// Calcula a posição final da trajetória
|
|
int desvioXob = (int)(Math.Cos(anguloDesvioObRad) * distanciaObstaculo);
|
|
int desvioYob = (int)(Math.Sin(anguloDesvioObRad) * distanciaObstaculo);
|
|
|
|
// Cria um Pen tracejado azul
|
|
Pen penTrajectoryOb = new Pen(Color.Cyan, 3) { DashPattern = new float[] { 4, 2 } }; // Ajuste o padrão de traço conforme necessário
|
|
|
|
// Desenha a linha de trajetória
|
|
g.DrawLine(penTrajectoryOb, centerX, centerY, centerX + desvioXob, centerY - desvioYob);
|
|
}
|
|
}
|
|
|
|
#endregion
|
|
|
|
#region Processamento de Dados
|
|
|
|
public void ProcessarDados()
|
|
{
|
|
CalcularDistanciasLaterais();
|
|
|
|
DesvioNecessario = false;
|
|
|
|
// Limpa a lista de obstáculos para o novo processamento
|
|
ObstaculosDetectados.Clear();
|
|
|
|
// Agrupa leituras por proximidade
|
|
var grupos = AgruparLeiturasPorProximidade(Deteccoes.First(x => x.Posicao == PosicaoAtual).Leituras);
|
|
|
|
foreach (var grupo in grupos)
|
|
{
|
|
var leiturasGrupo = grupo.OrderBy(x => x.Angulo).ToList();
|
|
var primeiraLeitura = leiturasGrupo.First();
|
|
var ultimaLeitura = leiturasGrupo.Last();
|
|
|
|
// Calcula a largura aproximada do obstáculo
|
|
//double larguraObstaculo = CalcularLarguraObstaculo(primeiraLeitura, ultimaLeitura);
|
|
double distanciaMedia = leiturasGrupo.Average(x => x.Distancia);
|
|
var Objeto = new SonarObstaculoDetectadoModel(primeiraLeitura.Angulo, ultimaLeitura.Angulo, distanciaMedia);
|
|
|
|
// Verifica se a largura do obstáculo é significativa
|
|
if (Objeto.Largura > LarguraMinimaObstaculo)
|
|
{
|
|
// Adiciona o obstáculo detectado à lista
|
|
ObstaculosDetectados.Add(Objeto);
|
|
}
|
|
}
|
|
|
|
|
|
|
|
CalcularDirecaoDesvio();
|
|
}
|
|
|
|
/*private List<List<SonarLeituraModel>> AgruparLeiturasPorProximidade(List<SonarLeituraModel> leituras)
|
|
{
|
|
int maxAnguloDiferenca = anguloAcrescentar * 3; // Diferença máxima de ângulo (em graus) para considerar leituras no mesmo grupo
|
|
const int maxDistanciaDiferenca = 20; // Diferença máxima de distância (em cm) para considerar leituras no mesmo grupo
|
|
|
|
List<List<SonarLeituraModel>> grupos = new List<List<SonarLeituraModel>>();
|
|
List<SonarLeituraModel> grupoAtual = new List<SonarLeituraModel>();
|
|
|
|
// Ordena as leituras por ângulo para processamento sequencial
|
|
var leiturasOrdenadas = leituras.OrderBy(l => l.Angulo).ToList();
|
|
|
|
foreach (var leitura in leiturasOrdenadas)
|
|
{
|
|
if (grupoAtual.Count == 0)
|
|
{
|
|
// Se o grupo atual está vazio, simplesmente adiciona a leitura atual ao grupo
|
|
grupoAtual.Add(leitura);
|
|
}
|
|
else
|
|
{
|
|
// Compara a leitura atual com a última leitura do grupo atual
|
|
var ultimaLeitura = grupoAtual.Last();
|
|
if (Math.Abs(leitura.Angulo - ultimaLeitura.Angulo) <= maxAnguloDiferenca &&
|
|
Math.Abs(leitura.Distancia - ultimaLeitura.Distancia) <= maxDistanciaDiferenca)
|
|
{
|
|
// Se a leitura atual está próxima da última leitura, adiciona ao mesmo grupo
|
|
grupoAtual.Add(leitura);
|
|
}
|
|
else
|
|
{
|
|
// Se a leitura atual está longe da última leitura, considera como um novo grupo
|
|
grupos.Add(new List<SonarLeituraModel>(grupoAtual)); // Adiciona o grupo atual aos grupos
|
|
grupoAtual.Clear(); // Limpa o grupo atual
|
|
grupoAtual.Add(leitura); // Inicia um novo grupo com a leitura atual
|
|
}
|
|
}
|
|
}
|
|
|
|
if (grupoAtual.Count > 0)
|
|
{
|
|
// Adiciona o último grupo aos grupos, se não estiver vazio
|
|
grupos.Add(grupoAtual);
|
|
}
|
|
|
|
return grupos.Where(x => x.All(y => y.Distancia <= maxDistanceCm)).ToList();
|
|
}*/
|
|
|
|
public List<List<SonarLeituraModel>> AgruparLeiturasPorProximidade(List<SonarLeituraModel> leituras)
|
|
{
|
|
int maxAnguloDiferenca = anguloAcrescentar * 3; // Diferença máxima de ângulo (em graus) para considerar leituras no mesmo grupo
|
|
const int maxDistanciaDiferenca = 20; // Diferença máxima de distância (em cm) para considerar leituras no mesmo grupo
|
|
|
|
List<List<SonarLeituraModel>> grupos = new List<List<SonarLeituraModel>>();
|
|
List<SonarLeituraModel> grupoAtual = new List<SonarLeituraModel>();
|
|
|
|
// Ordena as leituras por ângulo para processamento sequencial
|
|
var leiturasOrdenadas = leituras.OrderBy(l => l.Angulo).ToList();
|
|
|
|
foreach (var leitura in leiturasOrdenadas)
|
|
{
|
|
if (grupoAtual.Count == 0)
|
|
{
|
|
// Se o grupo atual está vazio, simplesmente adiciona a leitura atual ao grupo
|
|
grupoAtual.Add(leitura);
|
|
}
|
|
else
|
|
{
|
|
// Compara a leitura atual com a última leitura do grupo atual
|
|
var ultimaLeitura = grupoAtual.Last();
|
|
if (Math.Abs(leitura.Angulo - ultimaLeitura.Angulo) <= maxAnguloDiferenca &&
|
|
Math.Abs(leitura.Distancia - ultimaLeitura.Distancia) <= maxDistanciaDiferenca)
|
|
{
|
|
// Se a leitura atual está próxima da última leitura, adiciona ao mesmo grupo
|
|
grupoAtual.Add(leitura);
|
|
}
|
|
else
|
|
{
|
|
// Se a leitura atual está longe da última leitura, considera como um novo grupo
|
|
grupos.Add(new List<SonarLeituraModel>(grupoAtual)); // Adiciona o grupo atual aos grupos
|
|
grupoAtual.Clear(); // Limpa o grupo atual
|
|
grupoAtual.Add(leitura); // Inicia um novo grupo com a leitura atual
|
|
}
|
|
}
|
|
}
|
|
|
|
if (grupoAtual.Count > 0)
|
|
{
|
|
// Adiciona o último grupo aos grupos, se não estiver vazio
|
|
grupos.Add(grupoAtual);
|
|
}
|
|
|
|
return grupos.Where(x => x.All(y => y.Distancia <= maxDistanceCm)).ToList();
|
|
}
|
|
|
|
|
|
// Método para calcular a média de distâncias nas laterais do robô
|
|
private void CalcularDistanciasLaterais()
|
|
{
|
|
// Filtra leituras correspondentes aos ângulos de interesse
|
|
//var leiturasDireita = Deteccoes.SelectMany(d => d.Leituras.Where(l => l.Angulo <= 4)).ToList();
|
|
//var leiturasEsquerda = Deteccoes.SelectMany(d => d.Leituras.Where(l => l.Angulo >= 176)).ToList();
|
|
var leiturasEsquerda = Deteccoes.Where(d => d.Posicao == 1).SelectMany(d => d.Leituras.Where(l => l.Angulo <= anguloAcrescentar)).ToList();
|
|
var leiturasDireita = Deteccoes.Where(d => d.Posicao == 1).SelectMany(d => d.Leituras.Where(l => l.Angulo >= 180 - anguloAcrescentar)).ToList();
|
|
|
|
// Calcula as médias
|
|
if (leiturasDireita.Any() && leiturasEsquerda.Any())
|
|
{
|
|
DistanciaDireita = leiturasDireita.Any() ? leiturasDireita.Average(l => l.Distancia) : 0;
|
|
DistanciaEsquerda = leiturasEsquerda.Any() ? leiturasEsquerda.Average(l => l.Distancia) : 0;
|
|
}
|
|
}
|
|
|
|
// Método para decidir a direção do desvio
|
|
public void CalcularDirecaoDesvio()
|
|
{
|
|
foreach (var obstaculo in ObstaculosDetectados)
|
|
{
|
|
// Inicializa variáveis para determinar a melhor direção de desvio
|
|
double espacoLivreDireita = DistanciaDireita - obstaculo.DeslocamentoNecessarioDireita;
|
|
double espacoLivreEsquerda = DistanciaEsquerda - obstaculo.DeslocamentoNecessarioEsquerda;
|
|
|
|
var Sensor = Variaveis.OperacaoEmAndamento.DispSen.Dados.Sensores.FirstOrDefault(x => x.Componente == S_Code.sSNR);
|
|
if (Sensor != null && obstaculo.DistanciaMedia < (Sensor.ValoresLeituras.FirstOrDefault()?.minimo ?? 0))
|
|
{
|
|
obstaculo.DirecaoDesvio = Direcao.Parado;
|
|
}
|
|
// Decisão de direção baseada no espaço livre ajustado
|
|
else if (espacoLivreDireita > espacoLivreEsquerda && espacoLivreDireita > 0)
|
|
{
|
|
obstaculo.DirecaoDesvio = Direcao.Direita;
|
|
}
|
|
else if (espacoLivreEsquerda > 0)
|
|
{
|
|
obstaculo.DirecaoDesvio = Direcao.Esquerda;
|
|
}
|
|
else
|
|
{
|
|
// Não há espaço suficiente para desviar
|
|
obstaculo.DirecaoDesvio = Direcao.Parado;
|
|
}
|
|
}
|
|
|
|
DesvioNecessario = ObstaculosDetectados.Any(obstaculo => obstaculo.DeslocamentoNecessarioDesvio > 0);
|
|
}
|
|
|
|
|
|
#endregion
|
|
|
|
}
|
|
|
|
public class SonarRegistroModel
|
|
{
|
|
public int Posicao { get; set; }
|
|
public List<SonarLeituraModel> Leituras { get; set; }
|
|
}
|
|
|
|
public class SonarLeituraModel
|
|
{
|
|
public int Angulo { get; set; }
|
|
public int Distancia { get; set; }
|
|
public DateTime Momento { get; set; }
|
|
|
|
public SonarLeituraModel(int angle, int distance)
|
|
{
|
|
Angulo = angle;
|
|
Distancia = distance;
|
|
Momento = DateTime.Now;
|
|
}
|
|
}
|
|
|
|
public class SonarObstaculoDetectadoModel
|
|
{
|
|
public double AnguloInicial { get; set; }
|
|
public double AnguloFinal { get; set; }
|
|
public double Largura
|
|
{
|
|
get
|
|
{
|
|
double largura = 0;
|
|
if ((Aesquerda && !Adireita) || (!Aesquerda && Adireita))
|
|
{
|
|
largura = DistanciaInicial - DistanciaFinal;
|
|
}
|
|
else if (Aesquerda && Adireita)
|
|
{
|
|
largura = DistanciaInicial + DistanciaFinal;
|
|
}
|
|
if (largura < 0)
|
|
{
|
|
largura *= -1;
|
|
}
|
|
return largura;
|
|
}
|
|
}
|
|
public double DistanciaMedia { get; set; }
|
|
public double DistanciaInicial
|
|
{
|
|
get
|
|
{
|
|
// Cálculo simplificado, assumindo que podemos determinar a distância direta até o ponto inicial do obstáculo.
|
|
// Esta fórmula precisa ser adaptada com base na geometria específica e na localização do obstáculo.
|
|
double anguloRelativo = AnguloInicial - 0; // Calculando o ângulo relativo ao centro do robô
|
|
// Convertendo o ângulo para radianos para uso em funções trigonométricas
|
|
double anguloRad = anguloRelativo * (Math.PI / 180);
|
|
|
|
// Usando trigonometria para calcular a componente da distância na direção do obstáculo
|
|
// Esta é uma simplificação e pode não refletir a distância exata sem mais contextos
|
|
double distanciaInicial = Math.Cos(anguloRad) * DistanciaMedia;
|
|
if (distanciaInicial < 0)
|
|
{
|
|
distanciaInicial *= -1;
|
|
}
|
|
|
|
return distanciaInicial;
|
|
}
|
|
}
|
|
public double DistanciaFinal
|
|
{
|
|
get
|
|
{
|
|
// Cálculo simplificado, assumindo que podemos determinar a distância direta até o ponto inicial do obstáculo.
|
|
// Esta fórmula precisa ser adaptada com base na geometria específica e na localização do obstáculo.
|
|
double anguloRelativo = AnguloFinal - 0; // Calculando o ângulo relativo ao centro do robô
|
|
// Convertendo o ângulo para radianos para uso em funções trigonométricas
|
|
double anguloRad = anguloRelativo * (Math.PI / 180);
|
|
|
|
// Usando trigonometria para calcular a componente da distância na direção do obstáculo
|
|
// Esta é uma simplificação e pode não refletir a distância exata sem mais contextos
|
|
double distanciaFinal = Math.Cos(anguloRad) * DistanciaMedia;
|
|
if (distanciaFinal < 0)
|
|
{
|
|
distanciaFinal *= -1;
|
|
}
|
|
|
|
return distanciaFinal;
|
|
}
|
|
}
|
|
public double TempoAteColisao
|
|
{
|
|
get
|
|
{
|
|
return DistanciaMedia / FuncoesMatematicas.ConverteKmhParaMs(Variaveis.OperacaoEmAndamento.Sensoriamento.Movimentacao.VelocidadeMediaMs);
|
|
}
|
|
}
|
|
public double DeslocamentoNecessarioDesvio
|
|
{
|
|
get
|
|
{
|
|
if (DirecaoDesvio == Direcao.Direita)
|
|
{
|
|
return DeslocamentoNecessarioDireita;
|
|
}
|
|
else if (DirecaoDesvio == Direcao.Esquerda)
|
|
{
|
|
return DeslocamentoNecessarioEsquerda;
|
|
}
|
|
else
|
|
{
|
|
return 0;
|
|
}
|
|
}
|
|
}
|
|
public double DeslocamentoNecessarioEsquerda
|
|
{
|
|
get
|
|
{
|
|
if (Adireita && !Aesquerda)
|
|
{
|
|
//return DistanciaInicial + SonarModel.MargemSegurancaDesvio;
|
|
double deslocamento = VariaveisEquipamento.LarguraDireitaCm - DistanciaFinal + SonarModel.MargemSegurancaDesvio;
|
|
if (deslocamento < 0)
|
|
{
|
|
deslocamento = 0;
|
|
}
|
|
return deslocamento;
|
|
}
|
|
else if (Aesquerda && !Adireita)
|
|
{
|
|
//return DistanciaFinal + SonarModel.MargemSegurancaDesvio + (VariaveisEquipamento.Largura / 2);
|
|
double deslocamento = VariaveisEquipamento.LarguraEsquerdaCm + DistanciaFinal + SonarModel.MargemSegurancaDesvio;
|
|
if (deslocamento < 0)
|
|
{
|
|
deslocamento = 0;
|
|
}
|
|
return deslocamento;
|
|
}
|
|
else if (Adireita && Aesquerda)
|
|
{
|
|
double deslocamento = ((VariaveisEquipamento.LarguraEsquerdaCm + VariaveisEquipamento.LarguraDireitaCm) / 2) + DistanciaFinal + SonarModel.MargemSegurancaDesvio;
|
|
if (deslocamento < 0)
|
|
{
|
|
deslocamento = 0;
|
|
}
|
|
return deslocamento;
|
|
}
|
|
else
|
|
{
|
|
return 0;
|
|
}
|
|
}
|
|
}
|
|
public double DeslocamentoNecessarioDireita
|
|
{
|
|
get
|
|
{
|
|
if (Adireita && !Aesquerda)
|
|
{
|
|
//return DistanciaInicial + SonarModel.MargemSegurancaDesvio + (VariaveisEquipamento.Largura / 2);
|
|
double deslocamento = VariaveisEquipamento.LarguraDireitaCm + DistanciaInicial + SonarModel.MargemSegurancaDesvio;
|
|
if(deslocamento < 0)
|
|
{
|
|
deslocamento = 0;
|
|
}
|
|
return deslocamento;
|
|
}
|
|
else if (Aesquerda && !Adireita)
|
|
{
|
|
//return DistanciaFinal + SonarModel.MargemSegurancaDesvio;
|
|
double deslocamento = VariaveisEquipamento.LarguraEsquerdaCm - DistanciaInicial + SonarModel.MargemSegurancaDesvio;
|
|
if (deslocamento < 0)
|
|
{
|
|
deslocamento = 0;
|
|
}
|
|
return deslocamento;
|
|
}
|
|
else if (Adireita && Aesquerda)
|
|
{
|
|
double deslocamento = ((VariaveisEquipamento.LarguraEsquerdaCm + VariaveisEquipamento.LarguraDireitaCm) / 2) + DistanciaInicial + SonarModel.MargemSegurancaDesvio;
|
|
if (deslocamento < 0)
|
|
{
|
|
deslocamento = 0;
|
|
}
|
|
return deslocamento;
|
|
}
|
|
else
|
|
{
|
|
return 0;
|
|
}
|
|
}
|
|
}
|
|
public double AnguloParaDesvio
|
|
{
|
|
get
|
|
{
|
|
double anguloDesvio = 0;
|
|
Direcao direcaoDesvio = DirecaoDesvio;
|
|
|
|
if (direcaoDesvio == Direcao.Esquerda)
|
|
{
|
|
anguloDesvio = AnguloParaDesvioEsquerda;
|
|
}
|
|
else if (direcaoDesvio == Direcao.Direita)
|
|
{
|
|
anguloDesvio = AnguloParaDesvioDireita;
|
|
}
|
|
|
|
// Atribuição do ângulo calculado ao ângulo de desvio, com verificação de limites
|
|
return Math.Min(anguloDesvio, SonarModel.AnguloDesvioMaximo);
|
|
}
|
|
}
|
|
public double AnguloParaDesvioEsquerda
|
|
{
|
|
get
|
|
{
|
|
// Usando trigonometria para calcular o ângulo necessário
|
|
// Nota: Assegure-se de converter o resultado de arco seno para graus, se necessário
|
|
double anguloRad = Math.Atan(DistanciaMedia / DeslocamentoNecessarioEsquerda);
|
|
double anguloDesvio = (anguloRad * (180 / Math.PI)); // Convertendo radianos para graus
|
|
|
|
anguloDesvio = 90 - anguloDesvio;
|
|
|
|
// Atribuição do ângulo calculado ao ângulo de desvio, com verificação de limites
|
|
return Math.Min(anguloDesvio, SonarModel.AnguloDesvioMaximo);
|
|
}
|
|
}
|
|
public double AnguloParaDesvioDireita
|
|
{
|
|
get
|
|
{
|
|
// Usando trigonometria para calcular o ângulo necessário
|
|
// Nota: Assegure-se de converter o resultado de arco seno para graus, se necessário
|
|
double anguloRad = Math.Atan(DistanciaMedia / DeslocamentoNecessarioDireita);
|
|
double anguloDesvio = (anguloRad * (180 / Math.PI)); // Convertendo radianos para graus
|
|
|
|
anguloDesvio = anguloDesvio - 90;
|
|
|
|
// Atribuição do ângulo calculado ao ângulo de desvio, com verificação de limites
|
|
return Math.Min(anguloDesvio, SonarModel.AnguloDesvioMaximo);
|
|
}
|
|
}
|
|
public Direcao DirecaoDesvio { get; set; }
|
|
public bool Adireita
|
|
{
|
|
get
|
|
{
|
|
return AnguloFinal > 90 && AnguloInicial < 270;
|
|
}
|
|
}
|
|
public bool Aesquerda
|
|
{
|
|
get
|
|
{
|
|
return AnguloInicial < 90 || AnguloFinal > 270;
|
|
}
|
|
}
|
|
public GPSModel Coordenadas
|
|
{
|
|
get
|
|
{
|
|
double anguloAbsolutoObstaculo = (Variaveis.OperacaoEmAndamento.Sensoriamento.Gps.AnguloCarroDefinido + AnguloInicial + AnguloFinal) / 2.0;
|
|
double anguloRad = anguloAbsolutoObstaculo * (Math.PI / 180.0);
|
|
|
|
// Supondo que a distância seja em metros e utilizando uma aproximação simples da Terra como esfera
|
|
// para converter distância e ângulo em mudança de latitude/longitude.
|
|
double deltaLatitude = (DistanciaMedia * Math.Cos(anguloRad)) / 111111; // Dividindo por 111111 para converter metros em graus
|
|
double deltaLongitude = (DistanciaMedia * Math.Sin(anguloRad)) / (111111 * Math.Cos(Variaveis.OperacaoEmAndamento.Sensoriamento.Gps.Latitude * (Math.PI / 180.0)));
|
|
|
|
return new GPSModel
|
|
{
|
|
Latitude = Variaveis.OperacaoEmAndamento.Sensoriamento.Gps.Latitude + deltaLatitude,
|
|
Longitude = Variaveis.OperacaoEmAndamento.Sensoriamento.Gps.Longitude + deltaLongitude
|
|
};
|
|
}
|
|
}
|
|
|
|
public SonarObstaculoDetectadoModel(double anguloInicial, double anguloFinal, double distanciaMedia)
|
|
{
|
|
AnguloInicial = anguloInicial;
|
|
AnguloFinal = anguloFinal;
|
|
DistanciaMedia = distanciaMedia;
|
|
}
|
|
}
|
|
|
|
}
|