Zendion/agrobot_base
Zendion
/
agrobot_base
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

ajustes em erro do mpc no ultimo ponto da rua, simulador agora clona o penultimo ponto, corrigido distancias das laterais, corrigido dentro do corredor

This commit is contained in:
Diego Freitas 2026-09-09 15:50:14 -03:00
parent 8e8f06ebb2
commit f35553ec08
9 changed files with 250 additions and 113 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
AgroBase/AgroBase/Forms/Simuladores/frmSimulacaoMapaGPS.cs
View File

@ -770,9 +770,9 @@ namespace AgroBase.Forms
? "Aproximando"
: "Afastando";
lblDistProx.Text =
$"Distância Próximo Ponto: {(proximoPonto?.DistanciaTrajeto ?? 0):F2} m";
$"Distância Próximo Ponto: {(proximoPonto?.DistanciaAtual ?? 0):F2} m";
lblDistAnt.Text =
$"Distância Ponto Anterior: {(pontoMaisProximo?.DistanciaTrajeto ?? 0):F2} m";
$"Dist Esquerda: {dadosTrajetoria.DistanciaEsquerda * 100:F0} cm | Dist Direita: {dadosTrajetoria.DistanciaDireita * 100:F0} cm";
lblTempoRestante.Text =
$"Tempo Restante: {dadosTrajetoria.TempoEstimadoRestante}";
lblTempoOperacao.Text =

4
AgroBase/AgroBase/Models/MPCControllerAprimorado.cs
View File

@ -134,10 +134,10 @@ public class MPCControllerAprimorado
PontoTrajetoriaModel pontoAtual = op.Trajetoria._TrajetoriaFixa[idxNovoPonto].Clone();
int idxRuaEsquerda = op.Trajetoria._Corredores[pontoAtual.idxCorredor].idxRuaEsquerda;
double distEsq = op.Trajetoria.CalcularDistanciaLateral(true, posAtual, idxRuaEsquerda, pontoAtual.Direcao);
double distEsq = op.Trajetoria.CalcularDistanciaLateral(true, posAtual, idxRuaEsquerda, pontoAtual.Posicao);
int idxRuaDireita = op.Trajetoria._Corredores[pontoAtual.idxCorredor].idxRuaDireita;
double distDir = op.Trajetoria.CalcularDistanciaLateral(false, posAtual, idxRuaDireita, pontoAtual.Direcao);
double distDir = op.Trajetoria.CalcularDistanciaLateral(false, posAtual, idxRuaDireita, pontoAtual.Posicao);
double erroLat = Math.Abs(distEsq - distDir);

8
AgroBase/AgroBase/Models/MapaDinamicoModel.cs
View File

@ -1068,16 +1068,16 @@ namespace AgroBase.Models
*/
float larguraEsquerdaCm =
(float)VariaveisEquipamento.LarguraEsquerda;
(float)VariaveisEquipamento.LarguraEsquerdaCm;
float larguraDireitaCm =
(float)VariaveisEquipamento.LarguraDireita;
(float)VariaveisEquipamento.LarguraDireitaCm;
float comprimentoFrenteCm =
(float)VariaveisEquipamento.ComprimentoFrente;
(float)VariaveisEquipamento.ComprimentoFrenteCm;
float comprimentoTrasCm =
(float)VariaveisEquipamento.ComprimentoTras;
(float)VariaveisEquipamento.ComprimentoTrasCm;
float larguraTotalCm =
larguraEsquerdaCm + larguraDireitaCm;

6
AgroBase/AgroBase/Models/SonarCamModel.cs
View File

@ -245,7 +245,7 @@ namespace AgroBase.Models
}
// Definir a direção do desvio com base nos percentuais
if (DistanciaMedia_mm < (minDepth * 1.1) && (DistanciaDaLateralDireita < VariaveisEquipamento.LarguraDireita || DistanciaLivreEsquerda < VariaveisEquipamento.LarguraEsquerda))
if (DistanciaMedia_mm < (minDepth * 1.1) && (DistanciaDaLateralDireita < VariaveisEquipamento.LarguraDireitaCm || DistanciaLivreEsquerda < VariaveisEquipamento.LarguraEsquerdaCm))
{
direcaoDesvio = Direcao.Parado;
}
@ -331,7 +331,7 @@ namespace AgroBase.Models
{
if (PercentualDireita > 0 || PercentualEsquerda > 0)
{
double deslocamento = ((((VariaveisEquipamento.LarguraEsquerda + VariaveisEquipamento.LarguraDireita) / 2.0) + DistanciaDaLateralEsquerda) * (1 + PercentualEsquerda)) + SonarCamModel.MargemSegurancaDesvio;
double deslocamento = ((((VariaveisEquipamento.LarguraEsquerdaCm + VariaveisEquipamento.LarguraDireitaCm) / 2.0) + DistanciaDaLateralEsquerda) * (1 + PercentualEsquerda)) + SonarCamModel.MargemSegurancaDesvio;
if (deslocamento < 0)
{
deslocamento = 0;
@ -348,7 +348,7 @@ namespace AgroBase.Models
{
if (PercentualDireita > 0 || PercentualEsquerda > 0)
{
double deslocamento = ((((VariaveisEquipamento.LarguraEsquerda + VariaveisEquipamento.LarguraDireita) / 2.0) + DistanciaDaLateralDireita) * (1 + PercentualDireita)) + SonarCamModel.MargemSegurancaDesvio;
double deslocamento = ((((VariaveisEquipamento.LarguraEsquerdaCm + VariaveisEquipamento.LarguraDireitaCm) / 2.0) + DistanciaDaLateralDireita) * (1 + PercentualDireita)) + SonarCamModel.MargemSegurancaDesvio;
if (deslocamento < 0)
{
deslocamento = 0;

12
AgroBase/AgroBase/Models/SonarModel.cs
View File

@ -627,7 +627,7 @@ namespace AgroBase.Models
if (Adireita && !Aesquerda)
{
//return DistanciaInicial + SonarModel.MargemSegurancaDesvio;
double deslocamento = VariaveisEquipamento.LarguraDireita - DistanciaFinal + SonarModel.MargemSegurancaDesvio;
double deslocamento = VariaveisEquipamento.LarguraDireitaCm - DistanciaFinal + SonarModel.MargemSegurancaDesvio;
if (deslocamento < 0)
{
deslocamento = 0;
@ -637,7 +637,7 @@ namespace AgroBase.Models
else if (Aesquerda && !Adireita)
{
//return DistanciaFinal + SonarModel.MargemSegurancaDesvio + (VariaveisEquipamento.Largura / 2);
double deslocamento = VariaveisEquipamento.LarguraEsquerda + DistanciaFinal + SonarModel.MargemSegurancaDesvio;
double deslocamento = VariaveisEquipamento.LarguraEsquerdaCm + DistanciaFinal + SonarModel.MargemSegurancaDesvio;
if (deslocamento < 0)
{
deslocamento = 0;
@ -646,7 +646,7 @@ namespace AgroBase.Models
}
else if (Adireita && Aesquerda)
{
double deslocamento = ((VariaveisEquipamento.LarguraEsquerda + VariaveisEquipamento.LarguraDireita) / 2) + DistanciaFinal + SonarModel.MargemSegurancaDesvio;
double deslocamento = ((VariaveisEquipamento.LarguraEsquerdaCm + VariaveisEquipamento.LarguraDireitaCm) / 2) + DistanciaFinal + SonarModel.MargemSegurancaDesvio;
if (deslocamento < 0)
{
deslocamento = 0;
@ -666,7 +666,7 @@ namespace AgroBase.Models
if (Adireita && !Aesquerda)
{
//return DistanciaInicial + SonarModel.MargemSegurancaDesvio + (VariaveisEquipamento.Largura / 2);
double deslocamento = VariaveisEquipamento.LarguraDireita + DistanciaInicial + SonarModel.MargemSegurancaDesvio;
double deslocamento = VariaveisEquipamento.LarguraDireitaCm + DistanciaInicial + SonarModel.MargemSegurancaDesvio;
if(deslocamento < 0)
{
deslocamento = 0;
@ -676,7 +676,7 @@ namespace AgroBase.Models
else if (Aesquerda && !Adireita)
{
//return DistanciaFinal + SonarModel.MargemSegurancaDesvio;
double deslocamento = VariaveisEquipamento.LarguraEsquerda - DistanciaInicial + SonarModel.MargemSegurancaDesvio;
double deslocamento = VariaveisEquipamento.LarguraEsquerdaCm - DistanciaInicial + SonarModel.MargemSegurancaDesvio;
if (deslocamento < 0)
{
deslocamento = 0;
@ -685,7 +685,7 @@ namespace AgroBase.Models
}
else if (Adireita && Aesquerda)
{
double deslocamento = ((VariaveisEquipamento.LarguraEsquerda + VariaveisEquipamento.LarguraDireita) / 2) + DistanciaInicial + SonarModel.MargemSegurancaDesvio;
double deslocamento = ((VariaveisEquipamento.LarguraEsquerdaCm + VariaveisEquipamento.LarguraDireitaCm) / 2) + DistanciaInicial + SonarModel.MargemSegurancaDesvio;
if (deslocamento < 0)
{
deslocamento = 0;

302
AgroBase/AgroBase/Models/TrajetoriaMapaOperacaoModel.cs
View File

@ -416,33 +416,43 @@ namespace AgroBase.Models
public double DistanciaDireita { get; private set; }
private void AtualizarDistanciasLaterais()
{
DistanciaEsquerda = CalcularDistanciaLateral(true, GPSPosicaoAtual, CorredorAtual?.idxRuaEsquerda ?? 0, PontoAtual?.Direcao ?? DirecaoCarroRua.Ida);
DistanciaDireita = CalcularDistanciaLateral(false, GPSPosicaoAtual, CorredorAtual?.idxRuaDireita ?? 0, PontoAtual?.Direcao ?? DirecaoCarroRua.Ida);
var referenciaCorredor = PontoMaisProximo?.Posicao ?? PontoAtual?.Posicao;
DistanciaEsquerda = CalcularDistanciaLateral(true, GPSPosicaoAtual, CorredorAtual?.idxRuaEsquerda ?? -1, referenciaCorredor);
DistanciaDireita = CalcularDistanciaLateral(false, GPSPosicaoAtual, CorredorAtual?.idxRuaDireita ?? -1, referenciaCorredor);
}
public double CalcularDistanciaLateral(bool ladoEsquerdo, GPSModel posicaoAtual, int idxRua, DirecaoCarroRua direcaoAtual)
public double CalcularDistanciaLateral(bool ladoEsquerdo, GPSModel posicaoAtual, int idxRua, GPSModel referenciaCorredor)
{
if (posicaoAtual == null) return 0;
if (posicaoAtual == null)
return 0;
// Verifica se o corredor atual existe
if (!_CorredorAtualDefinido) return 0;
if (!_CorredorAtualDefinido)
return 0;
if (RuasPlantacao == null || idxRua < 0 || idxRua >= RuasPlantacao.Count)
return 0;
if (referenciaCorredor == null)
return 0;
List<GPSModel> rua = new List<GPSModel>(RuasPlantacao[idxRua]);
if (rua.Count < 2)
return 0;
double d0 = double.MaxValue;
// =========================================================
// 1. Encontrar região da rua mais próxima do rover
// =========================================================
double menorDistancia = double.MaxValue;
int idx = -1;
for (int i = 0; i < rua.Count; i++)
{
double dist = GPSUtils.DistanciaEntrePontos(posicaoAtual, rua[i]);
if (dist < d0)
if (dist < menorDistancia)
{
d0 = dist;
menorDistancia = dist;
idx = i;
}
}
@ -450,52 +460,86 @@ namespace AgroBase.Models
if (idx < 0)
return 0;
GPSModel p0 = rua[idx - (idx > 0 ? 1 : 0)];
GPSModel p1 = rua[idx + (idx < (rua.Count - 1) ? 1 : 0)];
int idxPa = rua.IndexOf(p0);
int idxPb = rua.IndexOf(p1);
double anguloAprojetar = GPSUtils.CalcularOrientacao(rua[idxPb], rua[idxPa]);
double anguloBprojetar = GPSUtils.CalcularOrientacao(rua[idxPa], rua[idxPb]);
int idxPa = Math.Max(0, idx - 1);
int idxPb = Math.Min(rua.Count - 1, idx + 1);
GPSModel pontoA = GPSUtils.GerarPontoDeslocado(rua[idxPa], anguloAprojetar, DistanciaManobraEntreRuas);
GPSModel pontoB = GPSUtils.GerarPontoDeslocado(rua[idxPb], anguloBprojetar, DistanciaManobraEntreRuas);
if (idxPa == idxPb)
return 0;
GPSModel pa = rua[idxPa];
GPSModel pb = rua[idxPb];
// =========================================================
// 2. Prolongar o segmento local
// =========================================================
double anguloA = GPSUtils.CalcularOrientacao(pb, pa);
double anguloB = GPSUtils.CalcularOrientacao(pa, pb);
GPSModel pontoA = GPSUtils.GerarPontoDeslocado(pa, anguloA, DistanciaManobraEntreRuas);
GPSModel pontoB = GPSUtils.GerarPontoDeslocado(pb, anguloB, DistanciaManobraEntreRuas);
var trechoRua = new List<GPSModel>
{
pontoA
};
List<GPSModel> novaRua = new List<GPSModel>();
novaRua.Add(pontoA);
for (int i = idxPa; i <= idxPb; i++)
{
novaRua.Add(rua[i]);
}
novaRua.Add(pontoB);
trechoRua.Add(rua[i]);
rua = novaRua;
trechoRua.Add(pontoB);
int qtdPontos = Math.Max(2, Convert.ToInt32(Math.Ceiling(GPSUtils.DistanciaDoTrecho(trechoRua))) + 1);
trechoRua = InterpolarRotaPorDistancia(trechoRua, qtdPontos);
int qtdPontosRua = Math.Max(2, Convert.ToInt32(Math.Ceiling(GPSUtils.DistanciaDoTrecho(rua))) + 1);
rua = InterpolarRotaPorDistancia(rua, Math.Max(2, qtdPontosRua));
// =========================================================
// 3. Distância absoluta centro do rover -> rua
// =========================================================
// Calcula a menor distância entre o robô e a rua
double distancia = GPSUtils.CalcularMenorDistanciaAteTrecho(posicaoAtual, rua);
double distanciaCentro = GPSUtils.CalcularMenorDistanciaAteTrecho(posicaoAtual, trechoRua);
// Calcula o vetor entre os pontos da rua e o vetor entre o ponto A e o GPS
double crossProduct =
(pontoB.Longitude - pontoA.Longitude) * (posicaoAtual.Latitude - pontoA.Latitude) -
(pontoB.Latitude - pontoA.Latitude) * (posicaoAtual.Longitude - pontoA.Longitude);
// =========================================================
// 4. Descobrir qual lado da rua é o INTERIOR do corredor
//
// Não depende do sentido A->B da polyline.
// =========================================================
// Ajusta a distância com base no sentido de deslocamento do robô
if ((direcaoAtual == DirecaoCarroRua.Volta && ladoEsquerdo) || (direcaoAtual == DirecaoCarroRua.Ida && !ladoEsquerdo))
{
distancia *= crossProduct > 0 ? 1 : -1;
}
else
{
distancia *= crossProduct > 0 ? -1 : 1;
}
double crossReferencia =
(pontoB.Longitude - pontoA.Longitude) *
(referenciaCorredor.Latitude - pontoA.Latitude) -
(pontoB.Latitude - pontoA.Latitude) *
(referenciaCorredor.Longitude - pontoA.Longitude);
// Ajusta a largura do robô no cálculo final
double larguraAjuste = ladoEsquerdo ? VariaveisEquipamento.LarguraEsquerda : VariaveisEquipamento.LarguraDireita;
distancia -= (larguraAjuste / 100.0);
double crossRobo =
(pontoB.Longitude - pontoA.Longitude) *
(posicaoAtual.Latitude - pontoA.Latitude) -
(pontoB.Latitude - pontoA.Latitude) *
(posicaoAtual.Longitude - pontoA.Longitude);
return distancia;
/*
* A trajetória central define o lado interno da rua.
*
* Mesmo sinal:
* rover está do lado interno.
*
* Sinal diferente:
* rover atravessou a rua e está fora do corredor.
*/
const double epsilon = 1e-15;
bool referenciaSobreLinha = Math.Abs(crossReferencia) <= epsilon;
bool roboSobreLinha = Math.Abs(crossRobo) <= epsilon;
bool mesmoLado = referenciaSobreLinha || roboSobreLinha || Math.Sign(crossReferencia) == Math.Sign(crossRobo);
double distanciaAssinada = mesmoLado ? distanciaCentro : -distanciaCentro;
// =========================================================
// 5. Converter distância do CENTRO para folga da CARROCERIA
// =========================================================
double larguraAjuste = ladoEsquerdo ? VariaveisEquipamento.LarguraEsquerdaCm : VariaveisEquipamento.LarguraDireitaCm;
double folga = distanciaAssinada - larguraAjuste / 100.0;
return folga;
}
[JsonProperty]
public PontoTrajetoriaModel PontoAtual { get; private set; }
@ -3432,7 +3476,7 @@ namespace AgroBase.Models
double larguraMaximaSegura = Math.Max(6.0, LarguraCorredorPadrao * 4.0);
double larguraRover = Math.Max(
0,
(VariaveisEquipamento.LarguraEsquerda + VariaveisEquipamento.LarguraDireita) / 100.0
(VariaveisEquipamento.LarguraEsquerdaCm + VariaveisEquipamento.LarguraDireitaCm) / 100.0
);
double larguraMinimaSegura = Math.Max(0.50, larguraRover + 0.15);
@ -3765,7 +3809,7 @@ namespace AgroBase.Models
Corredores[idx] = CorredorAtual;
double anguloProjetar1 = GPSUtils.CalcularOrientacao(CorredorAtual.Skip(1).FirstOrDefault(), CorredorAtual.FirstOrDefault());
GPSModel PrimeiroPonto = GPSUtils.ProjetarPontoDeslocado(CorredorAtual.FirstOrDefault(), DistanciaProjecaoRua, anguloProjetar1);
GPSModel PrimeiroPonto = GPSUtils.ProjetarPontoDeslocado(CorredorAtual.FirstOrDefault(), DistanciaProjecaoRua * 0.8, anguloProjetar1);
var ultimoPontoTrajetoria = _trajetoriaFixa.LastOrDefault();
@ -3837,7 +3881,7 @@ namespace AgroBase.Models
Direcao = direcaoAtual,
Orientacao = GPSUtils.CalcularOrientacao(_trajetoriaFixa.Last().Posicao, UltimoPonto),
Visitado = false,
LarguraCorredor = ultimoCorredor ? larguraCorredorMenor : (larguraCorredorMenor * 0.8)
LarguraCorredor = larguraCorredorMenor // ultimoCorredor ? larguraCorredorMenor : (larguraCorredorMenor * 0.8)
};
_trajetoriaFixa.Add(PontoFinal);
@ -5422,12 +5466,16 @@ namespace AgroBase.Models
public double FatorLarguraCorredor { get; set; }
public DadosVisuaisCorredor DadosVisuais { get; set; } = new DadosVisuaisCorredor();
private int _confirmacoesForaCorredor = 0;
private const int ConfirmacoesSaidaCorredor = 3;
public void AtualizarDados()
{
Dentro = false;
if (Pontos == null || Pontos.Count == 0)
{
Dentro = false;
_confirmacoesForaCorredor = 0;
DistanciaRestante = 0;
Progresso = 0;
Concluido = true;
@ -5436,46 +5484,108 @@ namespace AgroBase.Models
int idxReferencia = Pontos.FindLastIndex(x => x.Visitado);
idxReferencia = Math.Max(0, idxReferencia);
int idxInicio = Math.Max(0, idxReferencia - 2);
int idxFim = Math.Min(Pontos.Count - 1, idxReferencia + 3);
/*
* Somente a vizinhanca do progresso atual pode definir Dentro.
* Pontos visitados antigos conservam propriedades diagnosticas,
* mas nao podem manter o rover eternamente dentro da rua.
*/
bool dentroAgora = false;
for (int i = idxInicio; i <= idxFim; i++)
{
var ponto = Pontos[i];
if (
if (ponto == null)
continue;
/*
* Ponto Rua representa geometria física.
* Não precisa já ter sido confirmado como Visitado
* para provar que o rover está dentro da passada.
*/
bool ruaValida =
ponto.Tipo == TipoPontoRua.Rua &&
ponto.NaMargem;
/*
* Bordas continuam exigindo progresso confirmado,
* para não declarar entrada/saída prematuramente.
*/
bool bordaValida =
ponto.Visitado &&
ponto.NaMargem &&
(ponto.Tipo == TipoPontoRua.Rua ||
(
(ponto.Tipo == TipoPontoRua.BordaEntrada && !ponto.Aproximando) ||
(ponto.Tipo == TipoPontoRua.BordaSaida && ponto.Aproximando)
)
)
)
(
(ponto.Tipo == TipoPontoRua.BordaEntrada &&
!ponto.Aproximando)
||
(ponto.Tipo == TipoPontoRua.BordaSaida &&
ponto.Aproximando)
);
if (ruaValida || bordaValida)
{
Dentro = true;
dentroAgora = true;
break;
}
}
var naoVisitados = Pontos.Where(x => !x.Visitado).ToList();
DistanciaRestante = GPSUtils.DistanciaDoTrecho(naoVisitados.Select(x => x.Posicao).ToList());
/*
* Entrada é imediata.
* Saída exige algumas amostras consecutivas.
*
* Isso impede jitter GNSS/waypoint de transformar
* uma passada reta em Direcionando por um único ciclo.
*/
if (dentroAgora)
{
Dentro = true;
_confirmacoesForaCorredor = 0;
}
else if (Dentro)
{
_confirmacoesForaCorredor++;
if (_confirmacoesForaCorredor >= ConfirmacoesSaidaCorredor)
{
Dentro = false;
_confirmacoesForaCorredor = 0;
}
}
else
{
Dentro = false;
}
var naoVisitados = Pontos
.Where(x => !x.Visitado)
.ToList();
DistanciaRestante =
GPSUtils.DistanciaDoTrecho(
naoVisitados.Select(x => x.Posicao).ToList()
);
if (naoVisitados.Count > 0)
{
double distanciaEntrada = naoVisitados[0].DistanciaAtual;
double distanciaEntrada =
naoVisitados[0].DistanciaAtual;
if (!double.IsNaN(distanciaEntrada) && !double.IsInfinity(distanciaEntrada) && distanciaEntrada > 0)
if (!double.IsNaN(distanciaEntrada) &&
!double.IsInfinity(distanciaEntrada) &&
distanciaEntrada > 0)
{
DistanciaRestante += distanciaEntrada;
}
}
Progresso = (DistanciaTotal > 0) ? FuncoesMatematicas.Clamp((DistanciaPercorridaCorredor / DistanciaTotal) * 100.0, 0, 100) : 0;
Progresso =
DistanciaTotal > 0
? FuncoesMatematicas.Clamp(
(DistanciaPercorridaCorredor / DistanciaTotal) * 100.0,
0,
100
)
: 0;
Concluido = naoVisitados.Count == 0;
}
@ -5592,11 +5702,7 @@ namespace AgroBase.Models
);
}
public void AtualizarPropriedades(
GPSModel posicaoAtual,
GPSModel posicaoAnterior,
bool permitirMarcarVisitado = true
)
public void AtualizarPropriedades(GPSModel posicaoAtual, GPSModel posicaoAnterior, bool permitirMarcarVisitado = true)
{
AtualizarDistanciaAtual(posicaoAtual);
AtualizarDistanciaAnterior(posicaoAnterior);
@ -5612,31 +5718,49 @@ namespace AgroBase.Models
private void AtualizarVisitado()
{
if (Visitado) return; // Se já foi visitado, não faz nada
if (Visitado)
return;
double espacamento =
Tipo == TipoPontoRua.CruvaEntreCorredores || Tipo == TipoPontoRua.Desvio
Tipo == TipoPontoRua.CruvaEntreCorredores ||
Tipo == TipoPontoRua.Desvio
? TrajetoriaMapaOperacaoModel.DistanciaEntrePontosCurva
: TrajetoriaMapaOperacaoModel.DistanciaEntrePontos;
espacamento = Math.Max(0.10, espacamento);
double limiteDistancia =
espacamento * 0.75 +
Math.Max(0, TrajetoriaMapaOperacaoModel.DistanciaMaximaEntreLeituras);
double limiteDistancia = espacamento * 0.75 + Math.Max(0, TrajetoriaMapaOperacaoModel.DistanciaMaximaEntreLeituras);
limiteDistancia = Math.Max(0.25, Math.Min(DistanciaMargem, limiteDistancia));
if (PontoBorda || PontoLigacao)
bool estrutural = PontoBorda || PontoLigacao;
if (estrutural)
{
/*
* Bordas e ligações são marcos estruturais.
*
* Quando este método é chamado com autorização de visita
* no fluxo normal, o ponto já é o próximo ponto
* autoritativo da trajetória.
*
* Em manobras o bearing para o ponto pode mudar rapidamente,
* tornando "Aproximando" instável mesmo quando o rover passa
* fisicamente sobre o waypoint.
*/
limiteDistancia = Math.Min(limiteDistancia, 0.80);
bool cruzouJanelaDeAceitacao =
DistanciaAtual <= limiteDistancia &&
(
Aproximando ||
DistanciaAnterior <= limiteDistancia ||
DistanciaAtual <= 0.25
);
if (NaMargem && DistanciaAtual <= limiteDistancia)
{
Visitado = true;
}
return;
}
/*
* Pontos comuns continuam usando a janela endurecida.
*/
bool cruzouJanelaDeAceitacao = DistanciaAtual <= limiteDistancia && (Aproximando || DistanciaAnterior <= limiteDistancia || DistanciaAtual <= 0.25);
if (NaMargem && cruzouJanelaDeAceitacao)
{

10
AgroBase/AgroBase/Models/Variaveis.cs
View File

@ -821,10 +821,10 @@ namespace AgroBase.Models
public static string TopicoMqttComandos { get; } = $"agrobot/v1/rover/<id>/cmd";
public static string TopicoMqttTelemetria { get; } = $"agrobot/v1/rover/<id>/telemetry";
public static string TopicoMqttParametros { get; } = $"agrobot/v1/rover/<id>/parameters";
public static double LarguraEsquerda { get; } = 53.0; // 62
public static double LarguraDireita { get; } = 53.0; // 22
public static double ComprimentoFrente { get; } = 90.0; // 7
public static double ComprimentoTras { get; } = 40.0; // 107
public static double LarguraEsquerdaCm { get; } = 53.0; // 62
public static double LarguraDireitaCm { get; } = 53.0; // 22
public static double ComprimentoFrenteCm { get; } = 90.0; // 7
public static double ComprimentoTrasCm { get; } = 40.0; // 107
public static double DistanciaEntreEixosCm { get; set; } = 92.0;
public static double BitolaRodasCm { get; set; } = 75.0;
public static double LeverArmFrontalCm { get; set; } = 37.0; // cm
@ -833,7 +833,7 @@ namespace AgroBase.Models
{
get
{
return ((LarguraEsquerda + LarguraDireita + 10.0) * 10.0);
return ((LarguraEsquerdaCm + LarguraDireitaCm + 10.0) * 10.0);
}
}
public static double TensaoMinimaBateria { get; set; } = 30.0;

16
AgroBase/AgroBase/Services/GPSService.cs
View File

@ -209,6 +209,19 @@ namespace AgroBase.Services
lock (_stateLock)
{
/*
* A leitura simulada deve obedecer à mesma semântica temporal
* da leitura GNSS real:
*
* PenultimaLeitura = amostra imediatamente anterior
* UltimaLeitura = nova amostra
*
* Sem isso, a posição anterior fica congelada durante toda
* a simulação e contamina Aproximando, orientação anterior,
* deslocamento GNSS e recuperação da trajetória.
*/
CopiarPosicaoParaPenultima();
double orientacaoReal =
GPSUtils.NormalizarAngulo(leitura.OrientacaoReal);
@ -248,11 +261,10 @@ namespace AgroBase.Services
UltimaLeitura.Heartbeat = leitura.Heartbeat;
UltimaLeitura.Inicializado = true;
// Monitor/lock do C# é reentrante; os dois métodos abaixo usam o
// mesmo lock e permanecem em uma única transação de leitura GNSS.
AtualizarAtrasoPosicoes(
Math.Max(0, atrasoPosicaoPassos)
);
AtualizarCoordenadasGPS();
}
}

1
AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/manager_worker/modulos/mpc.py
View File

@ -1597,6 +1597,7 @@ class ControladorMPC:
tipo_anterior = TipoMovimentoDirecional(comando_anterior.get("tipo", TipoMovimentoDirecional.RodasDianteiras.value))
simulacao_latlon = []
debug_custo = {}
dados_costmap_ciclo = {}
ultimo_ponto = idx_proximo_ponto_real >= len(self.pontos_info)
if ultimo_ponto: