295 lines
12 KiB
C#
295 lines
12 KiB
C#
using AgroBase.Models.Modules;
|
|
using AgroBase.Services;
|
|
using System;
|
|
using System.Collections.Generic;
|
|
using System.Linq;
|
|
using static AgroBase.Models.Enums;
|
|
|
|
namespace AgroBase.Models.Operacoes
|
|
{
|
|
public class OperacaoMapaGPSModel
|
|
{
|
|
public OperacaoMapaGPSSensoriamentoModel Sensoriamento { get; set; } = new OperacaoMapaGPSSensoriamentoModel();
|
|
|
|
#region DIRECIONAL
|
|
|
|
public async void DefinirDadosControleDir()
|
|
{
|
|
if (Variaveis.OperacaoEmAndamento.Modo != ModoOperacao.MapaGPS)
|
|
{
|
|
return;
|
|
}
|
|
|
|
var Controle = Variaveis.OperacaoEmAndamento.Controle;
|
|
|
|
if (Variaveis.OperacaoEmAndamento.StatusAtual != StatusOperacao.EmAndamento || Variaveis.OperacaoEmAndamento.Finalizando)
|
|
{
|
|
Controle.Angulo = 0;
|
|
return;
|
|
}
|
|
|
|
switch (Controle.TipoControleDirecional)
|
|
{
|
|
case TiposControladorDirecional.PID:
|
|
{
|
|
DefinirAnguloSP();
|
|
break;
|
|
}
|
|
case TiposControladorDirecional.MPC:
|
|
{
|
|
var _Sensoriamento = Variaveis.OperacaoEmAndamento.Sensoriamento;
|
|
MPCComandoModel comando = await Variaveis.OperacaoEmAndamento.Controle.ControladorMPC.Compute(_Sensoriamento.Gps.Latitude, _Sensoriamento.Gps.Longitude, _Sensoriamento.Gps.AnguloCarroDefinido, _Sensoriamento.Movimentacao.VelocidadeMedia);
|
|
Controle.Angulo = comando?.angulo ?? 0;
|
|
Controle.TipoMovimento = comando?.tipo ?? TipoMovimentoDirecional.RodasDianteiras;
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
|
|
private void DefinirAnguloSP()
|
|
{
|
|
CalcularAnguloInclinacao();
|
|
|
|
DefinirTipoMovimento();
|
|
}
|
|
|
|
private void DefinirTipoMovimento()
|
|
{
|
|
var Trajetoria = Variaveis.OperacaoEmAndamento.Sensoriamento.Trajetoria;
|
|
var Controle = Variaveis.OperacaoEmAndamento.Controle;
|
|
|
|
bool arcoNecessario =
|
|
Trajetoria.ManobrandoEntreRuas ||
|
|
Trajetoria.StatusCarro == StatusCarroMapa.SaindoRua ||
|
|
Trajetoria.StatusCarro == StatusCarroMapa.EntrandoRua ||
|
|
(Trajetoria.CorredorAtual.Dentro && Math.Abs(Trajetoria.ErroOrientacaoAngularCaminho) >= Controle.Angulo_Max) ||
|
|
(!Trajetoria.CorredorAtual.Dentro && Math.Abs(Trajetoria.ErroOrientacaoAngularProximoPonto) >= Controle.Angulo_Max);
|
|
|
|
Controle.TipoMovimento =
|
|
arcoNecessario ? TipoMovimentoDirecional.MovimentoArco :
|
|
TipoMovimentoDirecional.RodasDianteiras;
|
|
}
|
|
|
|
private void CalcularAnguloInclinacao()
|
|
{
|
|
var _Trajetoria = Variaveis.OperacaoEmAndamento.Sensoriamento.Trajetoria;
|
|
var _Controle = Variaveis.OperacaoEmAndamento.Controle;
|
|
|
|
bool consideraAnguloCorredor =
|
|
(_Trajetoria.CorredorAtual.Dentro && Math.Abs(_Trajetoria.ErroLateralAngular) < 30.0 && _Trajetoria.PontoAtual.Tipo != TipoPontoRua.BordaSaida) ||
|
|
_Trajetoria.PontoAtual.Tipo == TipoPontoRua.BordaEntrada ||
|
|
_Trajetoria.PontoAtual.Tipo == TipoPontoRua.LigacaoEntrada;
|
|
|
|
double erroCombinado =
|
|
consideraAnguloCorredor ? _Trajetoria.ErroOrientacaoAngularCombinado :
|
|
_Trajetoria.ErroOrientacaoAngularProximoPonto;
|
|
|
|
var PID = _Controle.PIDdirecional;
|
|
if (_Trajetoria.StatusCarro != StatusCarroMapa.Parado)
|
|
{
|
|
// Atualiza o PID com o erro combinado
|
|
AjustarGanhosPID(_Controle);
|
|
|
|
PID.LimiteSaida = _Controle.Angulo_Max;
|
|
PID.Atualizar(erroCombinado, 0);
|
|
}
|
|
|
|
double correcaoAngulo = PID.Saida;
|
|
|
|
// Aplica a correção. Isso pode incluir limites para evitar comandos excessivos.
|
|
correcaoAngulo = Math.Min(_Controle.Angulo_Max, correcaoAngulo);
|
|
correcaoAngulo = Math.Max(_Controle.Angulo_Min, correcaoAngulo);
|
|
correcaoAngulo = Math.Round(correcaoAngulo, 2);
|
|
|
|
_Controle.Angulo = correcaoAngulo;
|
|
}
|
|
|
|
private void AjustarGanhosPID(OperacaoControleModel Controle)
|
|
{
|
|
var PID = Controle.PIDdirecional;
|
|
double velocidadePercnt = (Controle.PercentualVelocidadeMax - Controle.PercentualVelocidadeMin) * Controle.PercentualVelocidadeSP / 100.0;
|
|
|
|
double v = FuncoesMatematicas.Clamp(velocidadePercnt / 100.0, 0.0, 1.0);
|
|
|
|
// 🔹 Kp reduz suavemente com o quadrado da velocidade (menos agressivo em alta)
|
|
PID.Kp = PID.KpBase * (1.0 - 0.4 * Math.Pow(v, 2));
|
|
|
|
// 🔹 Ki cai mais rápido e zera a partir de 60%
|
|
PID.Ki = v >= 0.6 ? 0.0 : PID.KiBase * (1.0 - Math.Pow(v, 1.5));
|
|
|
|
// 🔹 Kd cresce suavemente (forte amortecimento em alta velocidade)
|
|
PID.Kd = PID.KdBase * (1.0 + 1.2 * Math.Pow(v, 1.2));
|
|
|
|
Console.WriteLine($"[PID ADAPT] Vel={velocidadePercnt:F1}% | Kp={PID.Kp:F3}, Ki={PID.Ki:F3}, Kd={PID.Kd:F3}");
|
|
}
|
|
|
|
|
|
#endregion
|
|
|
|
#region MOVIMENTACAO
|
|
|
|
public void DefinirDadosControleMov()
|
|
{
|
|
if (Variaveis.OperacaoEmAndamento.Modo != ModoOperacao.MapaGPS)
|
|
{
|
|
return;
|
|
}
|
|
|
|
var Controle = Variaveis.OperacaoEmAndamento.Controle;
|
|
|
|
if (Variaveis.OperacaoEmAndamento.StatusAtual != StatusOperacao.EmAndamento || Variaveis.OperacaoEmAndamento.Finalizando)
|
|
{
|
|
Controle.PercentualVelocidadeSP = 0;
|
|
Controle.FiltroVelocidade.Reset(Controle.PercentualVelocidadeSP);
|
|
return;
|
|
}
|
|
|
|
switch (Variaveis.OperacaoEmAndamento.Trajetoria.StatusAtual)
|
|
{
|
|
case StatusCarroMapa.Parado:
|
|
{
|
|
Controle.PercentualVelocidadeSP = 0;
|
|
break;
|
|
}
|
|
case StatusCarroMapa.Direcionando:
|
|
{
|
|
var Sonar = Variaveis.OperacaoEmAndamento.Sensoriamento.Sonar;
|
|
bool Manobrando = Variaveis.OperacaoEmAndamento.Trajetoria.ManobrandoEntreRuas;
|
|
bool DesvioSonar = Sonar.Iniciado && Sonar.DesvioNecessario;
|
|
if (Manobrando || DesvioSonar)
|
|
{
|
|
Controle.PercentualVelocidadeSP = Controle.PercentualVelocidadeMin;
|
|
}
|
|
else
|
|
{
|
|
Controle.PercentualVelocidadeSP = CalcularVelocidadeRelativa(Controle);
|
|
}
|
|
break;
|
|
}
|
|
case StatusCarroMapa.EntrandoRua:
|
|
{
|
|
Controle.PercentualVelocidadeSP = Controle.PercentualVelocidadeMin;
|
|
break;
|
|
}
|
|
case StatusCarroMapa.SaindoRua:
|
|
{
|
|
Controle.PercentualVelocidadeSP = Controle.PercentualVelocidadeMin;
|
|
break;
|
|
}
|
|
case StatusCarroMapa.CaminhandoRua:
|
|
{
|
|
if (Sensoriamento.ErvasNoRadar > 0)
|
|
{
|
|
Controle.PercentualVelocidadeSP = Controle.PercentualVelocidadeMin;
|
|
}
|
|
else
|
|
{
|
|
Controle.PercentualVelocidadeSP = CalcularVelocidadeRelativa(Controle);
|
|
}
|
|
break;
|
|
}
|
|
case StatusCarroMapa.Manobrando:
|
|
{
|
|
Controle.PercentualVelocidadeSP = Controle.PercentualVelocidadeMin;
|
|
break;
|
|
}
|
|
}
|
|
|
|
Controle.PercentualVelocidadeSP = Math.Max(Controle.PercentualVelocidadeMin, Controle.PercentualVelocidadeSP);
|
|
Controle.PercentualVelocidadeSP = Math.Min(Controle.PercentualVelocidadeMax, Controle.PercentualVelocidadeSP);
|
|
|
|
if (Controle.PercentualVelocidadeSP == Controle.PercentualVelocidadeMin)
|
|
{
|
|
Controle.FiltroVelocidade.Reset(Controle.PercentualVelocidadeSP);
|
|
}
|
|
|
|
//Console.WriteLine($"Velocidade SP: {Controle.PercentualVelocidadeSP}, ms: {FuncoesMatematicas.CalculaVelocidadeMsPercentual(Controle.PercentualVelocidadeSP)}, km/h: {FuncoesMatematicas.ConverteMsParaKmh(FuncoesMatematicas.CalculaVelocidadeMsPercentual(Controle.PercentualVelocidadeSP))}");
|
|
|
|
//double velocidadeFiltrada = Controle.FiltroVelocidade.Filtrar(Controle.PercentualVelocidadeSP);
|
|
//Controle.PercentualVelocidadeSP = Math.Max(Controle.PercentualVelocidadeMin, velocidadeFiltrada);
|
|
}
|
|
|
|
private double CalcularVelocidadeRelativa(OperacaoControleModel Controle)
|
|
{
|
|
double erroCombinado = Variaveis.OperacaoEmAndamento.Sensoriamento.Trajetoria.ErroOrientacaoAngularCombinado;
|
|
|
|
// Converte erro para uma penalização de velocidade (até 1.5x mais severa em curvas)
|
|
double severidade = Math.Min(Math.Abs(erroCombinado) / 30.0, 1.0); // Normaliza até 30°
|
|
double penalidade = severidade * 0.85; // até 85% de penalização
|
|
|
|
// Calcula velocidade proporcional
|
|
double faixaVel = Controle.PercentualVelocidadeMax - Controle.PercentualVelocidadeMin;
|
|
double velocidadeFinal = Controle.PercentualVelocidadeMax - (faixaVel * penalidade);
|
|
|
|
double velocidadeRelativa = Math.Max(Controle.PercentualVelocidadeMin, velocidadeFinal);
|
|
|
|
double velocidadeFiltrada = Math.Round(Controle.FiltroVelocidade.Filtrar(velocidadeRelativa), 2);
|
|
|
|
return velocidadeFiltrada;
|
|
}
|
|
|
|
|
|
#endregion
|
|
|
|
#region ATUADOR
|
|
|
|
public void DefinirDadosControleAtu()
|
|
{
|
|
var Controle = Variaveis.OperacaoEmAndamento.Controle;
|
|
|
|
if (Variaveis.OperacaoEmAndamento.StatusAtual != StatusOperacao.EmAndamento || Variaveis.OperacaoEmAndamento.Finalizando)
|
|
{
|
|
var BicosNaoAtuados = new List<AtuadorBicoModel>(Variaveis.OperacaoEmAndamento.DispAtu?.Dados?.BicosPulverizadores ?? new List<AtuadorBicoModel>());
|
|
BicosNaoAtuados.ForEach(x => x.ComandoAtuar = false);
|
|
Controle.BicosAtuados = new List<AtuadorBicoModel>(BicosNaoAtuados.ToList());
|
|
return;
|
|
}
|
|
|
|
Controle.BicosAtuados = Variaveis.OperacaoEmAndamento.DispAtu?.Dados?.BicosPulverizadores?.Select(x => x?.Clone() ?? new AtuadorBicoModel()).DefaultIfEmpty().ToList();
|
|
}
|
|
|
|
#endregion
|
|
|
|
}
|
|
|
|
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 double AnguloRuaCamera { 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 void ReiniciarLeituras()
|
|
{
|
|
ErvasNoRadar = 0;
|
|
HerbicidaConsumido = 0;
|
|
HerbicidaPorErva = 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>());
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
}
|