implementado MPC

AgroBase/AgroBase/Forms/IHM/frmDialogoMapa.cs

@@ -136,7 +136,8 @@ namespace AgroBase.Forms.IHM
                     _Trajetoria._TrajetoriaDinamica,
                     _Trajetoria.CorredorAtual.Pontos,
                     Variaveis.OperacaoEmAndamento.GPSTrajetoria,
-                    _Trajetoria.RuasPlantacao
+                    _Trajetoria.RuasPlantacao,
+                    _Sensoriamento.Controle.ControladorMPC?.ComandoAtual?.simulacao
                 );
             }
         }

AgroBase/AgroBase/Forms/frmSimulacaoMapaGPS.cs

@@ -138,7 +138,8 @@ namespace AgroBase.Forms
                     _Trajetoria._TrajetoriaDinamica,
                     _Trajetoria.CorredorAtual.Pontos,
                     Variaveis.OperacaoEmAndamento.GPSTrajetoria,
-                    _Trajetoria.RuasPlantacao
+                    _Trajetoria.RuasPlantacao,
+                    _Sensoriamento.Controle.ControladorMPC?.ComandoAtual?.simulacao
                 );
             }
 
@@ -197,58 +198,83 @@ namespace AgroBase.Forms
             GPSService.AtualizarCoordenadasGPS();
         }
 
-        private void btnAcrescentarGPS_Click(object sender, EventArgs e)
+        private async void btnAcrescentarGPS_Click(object sender, EventArgs e)
         {
             GPSModel novaPosicao = new GPSModel();
             double velocidadeMs = FuncoesMatematicas.CalculaVelocidadeMsPercentual(Variaveis.OperacaoEmAndamento.SimulacaoRpmControle);
             double velocidade = FuncoesMatematicas.ConverteMsParaKmh(velocidadeMs);
 
-            if (passosOperacao.Count > 0 && Variaveis.OperacaoEmAndamento.Simulando)
+            Variaveis.OperacaoEmAndamento.DispMvd.Dados.VelocidadeMedia = velocidadeMs;
-            {
-                novaPosicao = passosOperacao[Math.Min(passosOperacao.Count - 1, idxPassoOperacao)];
-                idxPassoOperacao++;
-            }
-            else
-            {
-                if (Variaveis.OperacaoEmAndamento.StatusAtual == StatusOperacao.Concluido || Variaveis.OperacaoEmAndamento.StatusAtual == StatusOperacao.Aguardando)
-                {
-                    return;
-                }
 
-                var ControleDir = Variaveis.OperacaoEmAndamento.Controle.TiposControle.FirstOrDefault(x => x.Tipo == T_Code.Dir);
+            TimeSpan tempoPassado =
-                double anguloControle = double.Parse(txtAnguloControle.Text);
-
-                double.TryParse(txtErroTempoComando.Text, out double fatorErro);
-                int ticksMax = (int)(ticksControleMax * Math.Max(1, fatorErro));
-
-                bool atualizarAnguloControle = (ticksControle >= ticksMax);
-                if (atualizarAnguloControle)
-                {
-                    ticksControle = 0;
-                }
-                else
-                {
-                    ticksControle++;
-                }
-
-                if (atualizarAnguloControle)
-                {
-                    btnCalcularAngulo_Click(new object(), new EventArgs());
-                    anguloControle = double.Parse(txtAnguloControle.Text);
-                }
-
-                // Obtém os parâmetros
-                double anguloAtual = double.Parse(txtAnguloGPS.Text);
-
-                TimeSpan tempoPassado =
                     !chbAutomatico.Checked || UltimaAtualizacaoGps == DateTime.MinValue ? // esta no modo manual
                     new TimeSpan((int)tempoGps * 10000) : // 1 tick do tempo gps
                     DateTime.Now - UltimaAtualizacaoGps; // tempo decorrido entre ticks do timer de leitura
 
-                // Calcula a nova posicao do robo
+            // Obtém os parâmetros
-                novaPosicao = VariaveisEquipamento.SimularNovaPosicao(Variaveis.OperacaoEmAndamento.Controle.TipoMovimento, anguloControle, velocidadeMs, tempoPassado.TotalSeconds, anguloAtual, GPSService.UltimaLeitura);
+            double anguloAtual = double.Parse(txtAnguloGPS.Text);
+
+            var _Gps = Variaveis.OperacaoEmAndamento.Sensoriamento.Gps;
+
+            double anguloControle = 0.0;
+            TipoMovimentoDirecional tipoMovimento = TipoMovimentoDirecional.RodasDianteiras;
+
+            var tipoControle = Variaveis.OperacaoEmAndamento.Controle.TipoControleDirecional;
+            switch (tipoControle)
+            {
+                case TiposControladorDirecional.PID:
+                    {
+                        tipoMovimento = Variaveis.OperacaoEmAndamento.Controle.TipoMovimento;
+
+                        if (passosOperacao.Count > 0 && Variaveis.OperacaoEmAndamento.Simulando)
+                        {
+                            novaPosicao = passosOperacao[Math.Min(passosOperacao.Count - 1, idxPassoOperacao)];
+                            idxPassoOperacao++;
+                        }
+                        else
+                        {
+                            if (Variaveis.OperacaoEmAndamento.StatusAtual == StatusOperacao.Concluido || Variaveis.OperacaoEmAndamento.StatusAtual == StatusOperacao.Aguardando)
+                            {
+                                return;
+                            }
+
+                            var ControleDir = Variaveis.OperacaoEmAndamento.Controle.TiposControle.FirstOrDefault(x => x.Tipo == T_Code.Dir);
+                            anguloControle = double.Parse(txtAnguloControle.Text);
+
+                            double.TryParse(txtErroTempoComando.Text, out double fatorErro);
+                            int ticksMax = (int)(ticksControleMax * Math.Max(1, fatorErro));
+
+                            bool atualizarAnguloControle = (ticksControle >= ticksMax);
+                            if (atualizarAnguloControle)
+                            {
+                                ticksControle = 0;
+                            }
+                            else
+                            {
+                                ticksControle++;
+                            }
+
+                            if (atualizarAnguloControle)
+                            {
+                                btnCalcularAngulo_Click(new object(), new EventArgs());
+                                anguloControle = double.Parse(txtAnguloControle.Text);
+                            }
+                        }
+                        break;
+                    }
+                case TiposControladorDirecional.MPC:
+                    {
+                        MPCComandoModel comando = Variaveis.OperacaoEmAndamento.Controle.ControladorMPC.ComandoAtual;
+                        anguloControle = comando?.angulo ?? 0;
+                        tipoMovimento = comando?.tipo ?? TipoMovimentoDirecional.RodasDianteiras;
+                        break;
+                    }
+                
             }
 
+            // Calcula a nova posicao do robo
+            novaPosicao = VariaveisEquipamento.SimularNovaPosicao(tipoMovimento, anguloControle, velocidadeMs, tempoPassado.TotalSeconds, anguloAtual, _Gps);
+
             // Atualiza a interface gráfica
             txtAnguloGPS.Text = novaPosicao.OrientacaoReal.ToString("0.00");
             txtVelocidade.Text = velocidade.ToString("0.00");
@@ -426,6 +452,12 @@ namespace AgroBase.Forms
             Variaveis.OperacaoEmAndamento.Controle.TipoControleDirecional = (TiposControladorDirecional)cmbTipoControleDirecional.SelectedIndex;
         }
 
-        
+
+
+
+
+       
     }
+
+    
 }

AgroBase/AgroBase/Models/Enums.cs

@@ -160,12 +160,12 @@
 
         public enum StatusCarroMapa
         {
-            Parado,
+            Parado = 0,
-            EntrandoRua, 
+            EntrandoRua = 1, 
-            CaminhandoRua,
+            CaminhandoRua = 2,
-            SaindoRua, 
+            SaindoRua = 3, 
-            Manobrando,
+            Manobrando = 4,
-            Direcionando
+            Direcionando = 5
         }
 
         public enum DirecaoCarroRua
@@ -241,6 +241,7 @@
             Script_GpsViewer = 13, 
             ArquivoModelo3D = 14,
             ScriptListOAKCaneras = 15,
+            ScriptMPCController = 16,
         }
 
         public enum TipoConexao

AgroBase/AgroBase/Models/MPCController.cs

@@ -1,224 +1,186 @@
 using AgroBase.Models;
 using AgroBase.Services;
+using Newtonsoft.Json;
 using System;
 using System.Collections.Generic;
+using System.Diagnostics;
+using System.Linq;
+using System.Threading.Tasks;
+using static AgroBase.Models.Enums;
 
 public class MPCController
 {
-    private int horizon;
+    private string TopicoRota = "mpc/rota";
-    private double maxSteeringAngle;
+    private string TopicoPosicao = "mpc/posicao";
-    private double maxSteeringAngleFourWheels;
+    private string TopicoComando = "mpc/comando";
-    private double minVelocity;
+    private int Horizonte = 20;
-    private double maxVelocity;
+    private double _dt = 0;
-    private double accelerationLimit;
-
-    // Pesos da função de custo ajustados
-    private double distancePredict = 3.0;
-    private double weightPosition = 1.0;
-    private double weightAlignment = 0.8;
-    private double weightSteering = 0.5;
-    private double weightVelocity = 0.3;
-    private double weightAcceleration = 1.0;  // Penaliza mudanças bruscas de velocidade
-    private double weightObstacle = 2.5; // Maior penalização para obstáculos
-    private double weightLaneCentering = 1.2; // Mantém o robô no centro do corredor
-
     private DateTime _ultimaAtualizacao = DateTime.MinValue;
+    public Process pythonProcess;
+    public readonly object processLock = new object();
+    public bool Iniciado = false;
+
+    public MPCComandoModel ComandoAtual = new MPCComandoModel();
 
     public MPCController()
     {
-        // Definições baseadas em variáveis globais
+        
-        this.horizon = Convert.ToInt32(distancePredict / TrajetoriaMapaOperacaoModel.DistanciaEntrePontos);
-        this.maxSteeringAngle = Variaveis.OperacaoEmAndamento?.Controle?.Angulo_Max ?? 30.0;
-        this.maxSteeringAngleFourWheels = this.maxSteeringAngle * 1.0;
-        this.minVelocity = Variaveis.OperacaoEmAndamento?.Controle?.PercentualVelocidadeMin ?? 20.0;
-        this.maxVelocity = Variaveis.OperacaoEmAndamento?.Controle?.PercentualVelocidadeMax ?? 100.0;
-        this.accelerationLimit = 1.5; // Limita aceleração a valores realistas
     }
 
-    public (double steeringAngle, Enums.TipoMovimentoDirecional mode) ComputeControl(
+    public async Task<bool> Inicializar()
-        double currentX, double currentY, double currentTheta,
-        double targetX, double targetY, double targetTheta,
-        double currentVelocity, double leftEdge, double rightEdge, List<Obstaculo> obstacles)
     {
-        double dt = (DateTime.Now - _ultimaAtualizacao).TotalSeconds;
+        await Variaveis.MqttService.AdicionarNovoTopico(TopicoRota);
-        dt = Math.Max(0.01, Math.Min(dt, 0.2));
+        await Variaveis.MqttService.AdicionarNovoTopico(TopicoPosicao);
-
+        await Variaveis.MqttService.AdicionarNovoTopico(TopicoComando, true, 2, async (mensagem) =>
-        // Define o número total de passos e passos por grupo
-        int totalSteps = 10;
-        int stepsPerGroup = 5;
-
-        // Calcula o número de grupos
-        int numGroups = totalSteps / stepsPerGroup;
-
-        // Pré-calcula todas as combinações de controle
-        List<(double steering, Enums.TipoMovimentoDirecional mode)> controlOptions = PreGenerateControlOptions();
-
-        // Variáveis para armazenar o melhor custo e a melhor sequência
-        double bestCost = double.MaxValue;
-        List<(double steering, Enums.TipoMovimentoDirecional mode)> bestSequence = null;
-
-        // Função de avaliação (passada como parâmetro)
-        Func<List<(double steering, Enums.TipoMovimentoDirecional mode)>, double> evaluateSequence = sequence =>
         {
-            return SimulateTrajectory(
+            if (!Iniciado && mensagem.Mensagem == "OK")
-                currentX, currentY, currentTheta,
+            {
-                targetX, targetY, targetTheta,
+                Iniciado = true;
-                currentVelocity, sequence,
+            }
-                leftEdge, rightEdge, obstacles, dt, Variaveis.OperacaoEmAndamento.Sensoriamento.Trajetoria.CorredorAtual.Dentro,
+            else
-                stepsPerGroup // Passa o número de passos por grupo
+            {
-            );
+                try
+                {
+                    ComandoAtual = JsonConvert.DeserializeObject<MPCComandoModel>(mensagem.Mensagem);
+                }
+                catch (Exception ex)
+                {
+                    Console.WriteLine("Erro ao deserializar resposta do MPC: " + ex.Message.ToString());
+                }
+                _ultimaAtualizacao = DateTime.Now;
+            }
+        });
+
+        lock (processLock)
+        {
+            pythonProcess = PythonService.RunScript(PythonService.ScriptMPCController, new string[] {});
+        }
+
+        bool ScriptIniciado() => Iniciado;
+        await FuncoesGlobais.AguardarCondicaoAsync(ScriptIniciado);
+
+        return Iniciado;
+    }
+
+    public async Task EnviarDadosTrajetoria(List<PontoTrajetoriaModel> _trajetoria)
+    {
+        var _Controle = Variaveis.OperacaoEmAndamento.Controle;
+
+        var trajetoria = new
+        {
+            pontos = _trajetoria
+                    .Select(p => new
+                    {
+                        lat = p.Posicao.Latitude,
+                        lon = p.Posicao.Longitude,
+                        tipo = (int)p.Tipo,
+                        distanciaMargem = p.LarguraCorredor * 0.8
+                    })
+                    .ToList(),
+            horizonte = Horizonte,
+            angulo_max_graus = _Controle.Angulo_Max,
+            distancia_entre_eixos = (VariaveisEquipamento.DistanciaEntreEixos / 100.0),
+            velocidade_min = FuncoesMatematicas.CalculaVelocidadeMsPercentual(_Controle.PercentualVelocidadeMin),
+            velocidade_max = FuncoesMatematicas.CalculaVelocidadeMsPercentual(_Controle.PercentualVelocidadeMax)
         };
 
-        // Gera e avalia todas as sequências dinamicamente
+        await Variaveis.MqttService.PublishAsync(
-        GenerateAndEvaluateSequences(
+            Variaveis.MqttService.Topicos.First(x => x.Topico == TopicoRota),
-            new List<(double steering, Enums.TipoMovimentoDirecional mode)>(),
+            JsonConvert.SerializeObject(trajetoria)
-            numGroups, // Número de grupos
+        );
-            controlOptions,
+    }
-            ref bestCost,
+
-            ref bestSequence,
+    public async Task<MPCComandoModel> Compute(double _lat, double _long, double _theta, double _velocidadeMs)
-            evaluateSequence
+    {
+        DateTime EnviadoEm = DateTime.Now;
+
+        double taxaHz = GPSService.TaxaAmostragemHz; // ex: 10 Hz
+        double tempoIdeal = 1.0 / taxaHz; // = 0.1 s
+        double tempoMinimo = tempoIdeal * 0.5; // 50% abaixo
+        double tempoMaximo = tempoIdeal * 2.0; // até 2x acima
+        _dt = (EnviadoEm - _ultimaAtualizacao).TotalSeconds;
+        double dtCalc = Math.Max(tempoMinimo, Math.Min(tempoMaximo, _dt));
+
+        var _Sensoriamento = Variaveis.OperacaoEmAndamento.Sensoriamento;
+
+        var dadosEnvio = new
+        {
+            lat = _lat,
+            lon = _long,
+            theta = _theta,
+            velocidade = _velocidadeMs,
+            dt = dtCalc,
+            contexto = new
+            {
+                StatusCarro = (int)_Sensoriamento.Trajetoria.StatusCarro, 
+                DentroCorredor = _Sensoriamento.Trajetoria.CorredorAtual.Dentro, 
+                ManobrandoEntreRuas = _Sensoriamento.Trajetoria.ManobrandoEntreRuas
+            }
+        };
+
+        await Variaveis.MqttService.PublishAsync(
+            Variaveis.MqttService.Topicos.First(x => x.Topico == TopicoPosicao),
+            JsonConvert.SerializeObject(dadosEnvio)
         );
 
-        _ultimaAtualizacao = DateTime.Now;
+        bool Respondido() => _ultimaAtualizacao > EnviadoEm;
+        await FuncoesGlobais.AguardarCondicaoAsync(Respondido, 200, 10);
 
-        // Retorna o primeiro comando da melhor sequência
+        return ComandoAtual;
-        return bestSequence[0];
     }
 
-
+    public MPCController Clone()
-    private List<(double steering, Enums.TipoMovimentoDirecional mode)> PreGenerateControlOptions()
     {
-        List<(double steering, Enums.TipoMovimentoDirecional mode)> controlOptions = new List<(double steering, Enums.TipoMovimentoDirecional mode)>();
+        return new MPCController()
-
-        // Gera todas as opções de steering (ângulos) e modos
-        List<double> steeringOptions = new List<double>();
-        for (double s = -maxSteeringAngle; s <= maxSteeringAngle; s += 5.0) // Passo de 5 graus
         {
-            steeringOptions.Add(s);
+            ComandoAtual = ComandoAtual?.Clone() ?? new MPCComandoModel(),
+            Horizonte = Horizonte, 
+            Iniciado = Iniciado,
+            _ultimaAtualizacao = _ultimaAtualizacao,
+            _dt = _dt
         }
+;    }
 
-        List<Enums.TipoMovimentoDirecional> modeOptions = new List<Enums.TipoMovimentoDirecional>()
+    public void Dispose()
-        {
-            Enums.TipoMovimentoDirecional.RodasDianteiras,
-            Enums.TipoMovimentoDirecional.MovimentoArco
-        };
-
-        // Cria todas as combinações de steering e mode
-        foreach (var steering in steeringOptions)
-        {
-            foreach (var mode in modeOptions)
-            {
-                controlOptions.Add((steering, mode));
-            }
-        }
-
-        return controlOptions;
-    }
-
-    private void GenerateControlSequences(List<(double steering, Enums.TipoMovimentoDirecional mode)> currentSequence, int remainingSteps, List<List<(double steering, Enums.TipoMovimentoDirecional mode)>> allSequences, List<(double steering, Enums.TipoMovimentoDirecional mode)> controlOptions) // Pré-geradas
     {
-        // Base da recursão: se não houver mais passos no horizonte, salva a sequência
+        lock (processLock)
-        if (remainingSteps == 0)
         {
-            allSequences.Add(new List<(double steering, Enums.TipoMovimentoDirecional mode)>(currentSequence));
+            pythonProcess?.Kill();
-            return;
+            pythonProcess?.Dispose();
+            pythonProcess = null;
         }
 
-        // Itera sobre as combinações pré-calculadas
+        Iniciado = false;
-        foreach (var control in controlOptions)
+        ComandoAtual = new MPCComandoModel();
-        {
+        Task.Run(async () => {
-            // Adiciona o controle atual à sequência
+            Variaveis.MqttService.Topicos.Remove(Variaveis.MqttService.Topicos.FirstOrDefault(x => x.Topico == TopicoRota));
-            currentSequence.Add(control);
+            Variaveis.MqttService.Topicos.Remove(Variaveis.MqttService.Topicos.FirstOrDefault(x => x.Topico == TopicoPosicao));
-
+            await Variaveis.MqttService.UnsubscribeAsync(Variaveis.MqttService.Topicos.FirstOrDefault(x => x.Topico == TopicoComando));
-            // Chama recursivamente para o próximo passo, diminuindo o horizonte
+            Variaveis.MqttService.Topicos.Remove(Variaveis.MqttService.Topicos.FirstOrDefault(x => x.Topico == TopicoComando));
-            GenerateControlSequences(currentSequence, remainingSteps - 1, allSequences, controlOptions);
+        });
-
-            // Remove o controle para explorar a próxima combinação
-            currentSequence.RemoveAt(currentSequence.Count - 1);
-        }
     }
 
-    private void GenerateAndEvaluateSequences(
+}
-        List<(double steering, Enums.TipoMovimentoDirecional mode)> currentSequence,
+
-        int remainingSteps,
+public class MPCComandoModel
-        List<(double steering, Enums.TipoMovimentoDirecional mode)> controlOptions,
+{
-        ref double bestCost,
+    public TipoMovimentoDirecional tipo { get; set; } = TipoMovimentoDirecional.RodasDianteiras;
-        ref List<(double steering, Enums.TipoMovimentoDirecional mode)> bestSequence,
+    public double angulo { get; set; } = 0;
-        Func<List<(double steering, Enums.TipoMovimentoDirecional mode)>, double> evaluateSequence)
+    public List<MPCSimulacaoModel> simulacao { get; set; } = new List<MPCSimulacaoModel>();
-    {
+
-        // Base da recursão: se não houver mais passos, avalia a sequência
+    public MPCComandoModel Clone()
-        if (remainingSteps == 0)
+    {
-        {
+        return new MPCComandoModel()
-            double cost = evaluateSequence(currentSequence);
+        {
-
+            tipo = tipo, 
-            // Verifica se a sequência atual tem o menor custo
+            angulo = angulo,
-            if (cost < bestCost)
+            simulacao = new List<MPCSimulacaoModel>(simulacao)
-            {
+        };
-                bestCost = cost;
+    }
-                bestSequence = new List<(double steering, Enums.TipoMovimentoDirecional mode)>(currentSequence);
+}
-            }
+
-
+public class MPCSimulacaoModel
-            return;
+{
-        }
+    public double latitude { get; set; }
-
+    public double longitude { get; set; }
-        // Itera sobre as combinações de controle
-        foreach (var control in controlOptions)
-        {
-            // Adiciona o controle atual à sequência
-            currentSequence.Add(control);
-
-            // Chama recursivamente para o próximo passo, diminuindo o horizonte
-            GenerateAndEvaluateSequences(currentSequence, remainingSteps - 1, controlOptions, ref bestCost, ref bestSequence, evaluateSequence);
-
-            // Remove o controle para explorar a próxima combinação
-            currentSequence.RemoveAt(currentSequence.Count - 1);
-        }
-    }
-
-
-
-
-    private double SimulateTrajectory(
-        double lat, double lon, double theta,
-        double targetLat, double targetLon, double targetTheta,
-        double currentVelocity,
-        List<(double steering, Enums.TipoMovimentoDirecional mode)> sequence,
-        double leftEdge, double rightEdge, List<Obstaculo> obstacles, double dt, bool insideStreet,
-        int stepsPerGroup)
-    {
-        double cost = 0;
-
-        var posicaoAtual = new GPSModel { Latitude = lat, Longitude = lon, OrientacaoReal = theta };
-        var posicaoAlvo = new GPSModel { Latitude = targetLat, Longitude = targetLon };
-        (double targetX, double targetY) = GPSUtils.ConverterLatLongParaMetros(posicaoAlvo, posicaoAtual);
-
-        foreach (var control in sequence)
-        {
-            double steering = control.steering;
-            Enums.TipoMovimentoDirecional mode = control.mode;
-
-            // Simula "stepsPerGroup" passos para o mesmo comando
-            for (int step = 0; step < stepsPerGroup; step++)
-            {
-                var novaPosicao = VariaveisEquipamento.SimularNovaPosicao(mode, steering, currentVelocity, dt, theta, posicaoAtual);
-
-                posicaoAtual = novaPosicao;
-                theta = novaPosicao.OrientacaoReal;
-
-                (double novaX, double novaY) = GPSUtils.ConverterLatLongParaMetros(novaPosicao, posicaoAlvo);
-
-                double distanceError = Math.Sqrt(Math.Pow(targetX - novaX, 2) + Math.Pow(targetY - novaY, 2));
-                double laneError = insideStreet ? Math.Abs(leftEdge - rightEdge) : 0.0;
-
-                // Acumula o custo
-                cost += (distanceError * 10.0) + (laneError * 20.0);
-            }
-        }
-
-        return cost;
-    }
-
-
-
-
 }

AgroBase/AgroBase/Models/MapaDinamicoModel.cs

@@ -20,6 +20,7 @@ namespace AgroBase.Models
         private List<PontoTrajetoriaModel> TrajetoriaDinamica { get; set; } = new List<PontoTrajetoriaModel>();
         private List<PontoTrajetoriaModel> TrajetoriaRuaProjetada { get; set; } = new List<PontoTrajetoriaModel>();
         private List<GPSModel> TrajetoriaRobo { get; set; } = new List<GPSModel>();
+        public List<MPCSimulacaoModel> TrajetoriaSimuladaMPC { get; set; } = new List<MPCSimulacaoModel>();
         private Obstaculo obstaculoDetectado { get; set; }
         private float AnguloCaminho { get; set; }
         private float AnguloCarro { get; set; }
@@ -42,7 +43,16 @@ namespace AgroBase.Models
             pnlZoomMapa.GetType().GetMethod("SetStyle", System.Reflection.BindingFlags.Instance | System.Reflection.BindingFlags.NonPublic).Invoke(pnlZoomMapa, new object[] { ControlStyles.UserPaint | ControlStyles.AllPaintingInWmPaint | ControlStyles.OptimizedDoubleBuffer, true });
         }
 
-        public void AtualizarDados(Obstaculo obstaculo, float anguloCaminho, float anguloCarro, GPSModel posicaoAtual, List<PontoTrajetoriaModel> trajetoriaDinamica, List<PontoTrajetoriaModel> trajetoriaRua, List<GPSModel> trajetoriaRobo, List<List<GPSModel>> ruasPlantacao)
+        public void AtualizarDados(
+            Obstaculo obstaculo, 
+            float anguloCaminho, 
+            float anguloCarro, 
+            GPSModel posicaoAtual, 
+            List<PontoTrajetoriaModel> trajetoriaDinamica, 
+            List<PontoTrajetoriaModel> trajetoriaRua, 
+            List<GPSModel> trajetoriaRobo, 
+            List<List<GPSModel>> ruasPlantacao,
+            List<MPCSimulacaoModel> trajetoriaSimulada = null)
         {
             obstaculoDetectado = obstaculo;
             AnguloCaminho = anguloCaminho;
@@ -52,6 +62,7 @@ namespace AgroBase.Models
             TrajetoriaRuaProjetada = trajetoriaRua;
             TrajetoriaRobo = trajetoriaRobo;
             RuasPlantacao = ruasPlantacao;
+            TrajetoriaSimuladaMPC = trajetoriaSimulada ?? new List<MPCSimulacaoModel>();
 
             pnlZoomMapa.Invalidate();
         }
@@ -151,6 +162,19 @@ namespace AgroBase.Models
                 {
                     DrawObstacle(g, centerX, centerY, Color.Cyan, Color.Cyan, AnguloCaminho - (float)obstaculoDetectado.AnguloParaDesvio, obstaculoDetectado, PxToCm);
                 }
+
+                // Desenha a simulação do MPC (linha tracejada verde-limão)
+                if (TrajetoriaSimuladaMPC != null && TrajetoriaSimuladaMPC.Count > 1)
+                {
+                    var _trajetoriaSimulada = TrajetoriaSimuladaMPC
+                        .Select(x => new PontoTrajetoriaModel(Enums.TipoPontoRua.Indefinido) { Posicao = new GPSModel() { Latitude = x.latitude, Longitude = x.longitude } })
+                        .ToList();
+
+                    Pen penTracejada = new Pen(Color.LimeGreen, 2);
+                    penTracejada.DashStyle = System.Drawing.Drawing2D.DashStyle.Dash;
+
+                    DrawTrajectory(g, _trajetoriaSimulada, centerX, centerY, Color.LimeGreen, penTracejada);
+                }
             }
             catch (Exception ex)
             {

AgroBase/AgroBase/Models/Operacoes/OperacaoMapaGPSModel.cs

@@ -13,7 +13,7 @@ namespace AgroBase.Models.Operacoes
 
         #region DIRECIONAL
 
-        public void DefinirDadosControleDir()
+        public async void DefinirDadosControleDir()
         {
             if (Variaveis.OperacaoEmAndamento.Modo != ModoOperacao.MapaGPS)
             {
@@ -37,13 +37,10 @@ namespace AgroBase.Models.Operacoes
                     }
                 case TiposControladorDirecional.MPC:
                     {
-                        var Trajetoria = Variaveis.OperacaoEmAndamento.Trajetoria;
+                        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);
-                        var MPC_A = new MPCControllerAprimorado();
+                        Controle.Angulo = comando?.angulo ?? 0;
-                        (double angulo, TipoMovimentoDirecional tipoControle) = MPC_A.CalcularControle(GPSService.UltimaLeitura, Controle.PercentualVelocidadeSP, Trajetoria.TrajetoriaDinamica, new TipoMovimentoDirecional[] { TipoMovimentoDirecional.RodasDianteiras });
+                        Controle.TipoMovimento = comando?.tipo ?? TipoMovimentoDirecional.RodasDianteiras;
-
-                        Controle.Angulo = angulo;
-                        Controle.TipoMovimento = tipoControle;
                         break;
                     }
             }
@@ -206,7 +203,7 @@ namespace AgroBase.Models.Operacoes
                 Controle.FiltroVelocidade.Reset(Controle.PercentualVelocidadeSP);
             }
 
-            Console.WriteLine($"Velocidade SP: {Controle.PercentualVelocidadeSP}, ms: {FuncoesMatematicas.CalculaVelocidadeMsPercentual(Controle.PercentualVelocidadeSP)}, km/h: {FuncoesMatematicas.ConverteMsParaKmh(FuncoesMatematicas.CalculaVelocidadeMsPercentual(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);

AgroBase/AgroBase/Models/Operacoes/OperacaoModel.cs

@@ -440,6 +440,8 @@ namespace AgroBase.Models
 
             tmrLogs?.Stop();
 
+            Variaveis.OperacaoEmAndamento.Controle.ControladorMPC?.Dispose();
+
             Variaveis.OperacaoEmAndamento = new OperacaoModel(Modo)
             {
                 Iniciado = false,
@@ -451,7 +453,6 @@ namespace AgroBase.Models
                 Sensoriamento = new OperacaoSensoriamentoLogModel(),
                 QuantidadeCamerasSolo = 1,
             };
-            
 
             switch (Modo)
             {
@@ -564,7 +565,7 @@ namespace AgroBase.Models
                         SegmentacaoSemantica = false,
                         PulverizadorAutomatico = true,
                         TipoMovimento = TipoMovimentoDirecional.RodasDianteiras,
-                        TipoControleDirecional = TiposControladorDirecional.PID,
+                        TipoControleDirecional = TiposControladorDirecional.MPC,
                     };
                     Variaveis.OperacaoEmAndamento.ModulosMandatorios = new List<OperacaoModulosMandatoriosModel>()
                     {
@@ -801,6 +802,11 @@ namespace AgroBase.Models
                 };
                 Controle.FiltroVelocidade = new KalmanFilter(erroMedicao: 2.0, erroProcesso: 0.05);
                 Controle.ControladorMPC = new MPCController();
+                bool mpcIniciado = await Controle.ControladorMPC.Inicializar();
+                if (mpcIniciado)
+                {
+                    await Controle.ControladorMPC.EnviarDadosTrajetoria(Variaveis.OperacaoEmAndamento.Trajetoria._TrajetoriaFixa);
+                }
             }
 
             Controle.Angulo = 0;
@@ -837,6 +843,8 @@ namespace AgroBase.Models
 
             Variaveis.OperacaoEmAndamento.PararCarroOperacaoInterrompida(true);
 
+            Variaveis.OperacaoEmAndamento.Controle.ControladorMPC?.Dispose();
+
             await Task.Delay(2000);
 
             Variaveis.OperacaoEmAndamento.Iniciado = false;
@@ -909,21 +917,12 @@ namespace AgroBase.Models
 
             ID = "";
 
-            if (DispMvd != null)
+            DispMvd?.Dados?.ReiniciarLeituras(DispMvd.Dados);
-            {
+            DispAtu?.Dados?.ReiniciarLeituras(DispAtu.Dados);
-                DispMvd.Dados.ReiniciarLeituras(DispMvd.Dados);
+            DispSen?.Dados?.ReiniciarLeituras(DispSen.Dados);
-            }
-            if (DispAtu != null)
-            {
-                DispAtu.Dados.ReiniciarLeituras(DispAtu.Dados);
-            }
-            if (DispSen != null)
-            {
-                DispSen.Dados.ReiniciarLeituras(DispSen.Dados);
-            }
 
-            Sensoriamento.AtualizarDados();
+            Sensoriamento?.AtualizarDados();
-            OpMapaGPS.Sensoriamento.ReiniciarLeituras();
+            OpMapaGPS?.Sensoriamento?.ReiniciarLeituras();
 
             if (Modo == ModoOperacao.MapaGPS)
             {
@@ -934,6 +933,14 @@ namespace AgroBase.Models
                 };
                 Controle.FiltroVelocidade = new KalmanFilter(erroMedicao: 2.0, erroProcesso: 0.05);
                 Controle.ControladorMPC = new MPCController();
+                Task.Run(async () =>
+                {
+                    bool mpcIniciado = await Controle.ControladorMPC.Inicializar();
+                    if (mpcIniciado)
+                    {
+                        await Controle.ControladorMPC.EnviarDadosTrajetoria(Variaveis.OperacaoEmAndamento.Trajetoria._TrajetoriaFixa);
+                    }
+                });
             }
 
             Controle.Angulo = 0;
@@ -944,6 +951,7 @@ namespace AgroBase.Models
 
         public void ReiniciarSimulacao()
         {
+            Variaveis.OperacaoEmAndamento.Controle.ControladorMPC?.Dispose();
             Variaveis.OperacaoEmAndamento.StatusAtual = StatusOperacao.Parametrizando;
             Variaveis.OperacaoEmAndamento.Iniciado = false;
             Variaveis.OperacaoEmAndamento.Simulando = false;
@@ -2879,6 +2887,8 @@ namespace AgroBase.Models
                 PercentualVelocidadeSP = PercentualVelocidadeSP, 
                 PIDdirecional = PIDdirecional.Clone(),
                 FiltroVelocidade = FiltroVelocidade.Clone(), 
+                ControladorMPC = ControladorMPC.Clone(),
+                _tipoMovimento = _tipoMovimento, 
                 PressaoLinha = PressaoLinha, 
                 PulverizadorAutomatico = PulverizadorAutomatico, 
                 RPM_Max = RPM_Max, 

AgroBase/AgroBase/Models/TrajetoriaMapaOperacaoModel.cs

@@ -217,8 +217,8 @@ namespace AgroBase.Models
         public double DistanciaDireita { get; private set; }
         private void AtualizarDistanciasLaterais()
         {
-            DistanciaEsquerda = CalcularDistanciaLateral(true, GPSService.UltimaLeitura, CorredorAtual.idxRuaEsquerda, PontoAtual.Direcao);
+            DistanciaEsquerda = CalcularDistanciaLateral(true, GPSService.UltimaLeitura, CorredorAtual?.idxRuaEsquerda ?? 0, PontoAtual?.Direcao ?? DirecaoCarroRua.Ida);
-            DistanciaDireita = CalcularDistanciaLateral(false, GPSService.UltimaLeitura, CorredorAtual.idxRuaDireita, PontoAtual.Direcao);
+            DistanciaDireita = CalcularDistanciaLateral(false, GPSService.UltimaLeitura, CorredorAtual?.idxRuaDireita ?? 0, PontoAtual?.Direcao ?? DirecaoCarroRua.Ida);
         }
 
         public double CalcularDistanciaLateral(bool ladoEsquerdo, GPSModel posicaoAtual, int idxRua, DirecaoCarroRua direcaoAtual)

AgroBase/AgroBase/Services/PythonService.cs

@@ -63,6 +63,13 @@ namespace AgroBase.Services
                 return VersionamentoService.ArquivoScript(Enums.TipoArquivoVersionado.ScriptListOAKCaneras).NomeArquivoLocal;
             }
         }
+        public static string ScriptMPCController
+        {
+            get
+            {
+                return VersionamentoService.ArquivoScript(Enums.TipoArquivoVersionado.ScriptMPCController).NomeArquivoLocal;
+            }
+        }
 
 
         private static List<Process> processosIniciados = new List<Process>();

AgroBase/AgroBase/bin/Debug/Parametros/version_files.vsf

@@ -225,5 +225,18 @@
     "Atualizado": true,
     "ArquivoAtualizado": true,
     "TipoArquivo": 15
+  },
+  {
+    "id": 20,
+    "Diretorio": "Python\\Scripts\\",
+    "Arquivo": "mpc_controller",
+    "Extensao": ".py",
+    "Versao": "1_0",
+    "ArquivoDownload": "mpc_controller-1_0.py",
+    "NomeArquivoLocal": "mpc_controller-1_0.py",
+    "CaminhoCompleto": "Python\\Scripts\\mpc_controller-1_0.py",
+    "Atualizado": true,
+    "ArquivoAtualizado": true,
+    "TipoArquivo": 16
   }
 ]

AgroBase/AgroBase/bin/Debug/Python/Output/GeoJson.json

@@ -1 +1,221 @@
-{"type":"FeatureCollection","features":[{"type":"Feature","properties":{"Id":"1","Name":"CidadeJardimTerreno2","Length":14.558011415731592,"Dist1":14.558011415731592,"Dist2":0.0},"geometry":{"id":"0","type":"LineString","coordinates":[[-47.395205344,-22.172559531333334],[-47.395212610166666,-22.172614638833334],[-47.395219157,-22.172656417833334],[-47.395223544833335,-22.1726892105],[-47.395224140984432,-22.172693691611649],[-47.395225326538004,-22.172702654611555]]}},{"type":"Feature","properties":{"Id":"2","Name":"CidadeJardimTerreno2","Length":14.771318274761821,"Dist1":14.558011415731592,"Dist2":0.0},"geometry":{"id":"1","type":"LineString","coordinates":[[-47.395210902333332,-22.172708800833334],[-47.395206943666665,-22.172679811],[-47.395202420666664,-22.1726491015],[-47.395198865666664,-22.172615782833333],[-47.395193255833334,-22.172577132833332],[-47.395192610024395,-22.172572657695412],[-47.395191325698136,-22.172563706509337]]}},{"type":"Feature","properties":{"Id":"3","Name":"CidadeJardimTerreno2","Length":14.827915524386164,"Dist1":14.558011415731592,"Dist2":0.0},"geometry":{"id":"2","type":"LineString","coordinates":[[-47.3951762925,-22.172561603],[-47.395180824166665,-22.172591686166665],[-47.395185745333336,-22.172623080166666],[-47.395190433,-22.172656367166667],[-47.395195199,-22.172693641833334],[-47.395195769044363,-22.172698125891035],[-47.395196902671593,-22.172707094716973]]}},{"type":"Feature","properties":{"Id":"4","Name":"CidadeJardimTerreno2","Length":14.785159385903514,"Dist1":14.558011415731592,"Dist2":0.0},"geometry":{"id":"3","type":"LineString","coordinates":[[-47.395182932166669,-22.1727127985],[-47.39517819266667,-22.172680514833335],[-47.3951732595,-22.172646752833334],[-47.395168805166669,-22.1726149155],[-47.395163812333337,-22.172581167166665],[-47.395163154299318,-22.172576693573966],[-47.395161845655842,-22.172567745443878]]}},{"type":"Feature","properties":{"Id":"5","Name":"CidadeJardimTerreno2","Length":14.80117692191233,"Dist1":14.558011415731592,"Dist2":0.0},"geometry":{"id":"4","type":"LineString","coordinates":[[-47.395147317833334,-22.172566031],[-47.395151503166666,-22.172595452333333],[-47.3951561855,-22.172628011166665],[-47.395161598666668,-22.172663757333332],[-47.395166727166668,-22.172697770833334],[-47.395167397572706,-22.172702242832194],[-47.39516873082615,-22.172711187810091]]}}]}
+{
+    "type": "FeatureCollection",
+    "features": [
+        {
+            "type": "Feature",
+            "properties": {
+                "Id": "1",
+                "Name": "CidadeJardimTerreno2",
+                "Length": 14.558011415731592,
+                "Dist1": 14.558011415731592,
+                "Dist2": 0.0
+            },
+            "geometry": {
+                "id": null,
+                "type": "LineString",
+                "coordinates": [
+                    [
+                        -47.395205344,
+                        -22.172559531333334
+                    ],
+                    [
+                        -47.395212610166666,
+                        -22.172614638833334
+                    ],
+                    [
+                        -47.395219157,
+                        -22.172656417833334
+                    ],
+                    [
+                        -47.395223544833335,
+                        -22.1726892105
+                    ],
+                    [
+                        -47.39522414098443,
+                        -22.17269369161165
+                    ],
+                    [
+                        -47.395225326538004,
+                        -22.172702654611555
+                    ]
+                ]
+            }
+        },
+        {
+            "type": "Feature",
+            "properties": {
+                "Id": "2",
+                "Name": "CidadeJardimTerreno2",
+                "Length": 14.771318274761821,
+                "Dist1": 14.558011415731592,
+                "Dist2": 0.0
+            },
+            "geometry": {
+                "id": null,
+                "type": "LineString",
+                "coordinates": [
+                    [
+                        -47.39521090233333,
+                        -22.172708800833334
+                    ],
+                    [
+                        -47.395206943666665,
+                        -22.172679811
+                    ],
+                    [
+                        -47.395202420666664,
+                        -22.1726491015
+                    ],
+                    [
+                        -47.395198865666664,
+                        -22.172615782833333
+                    ],
+                    [
+                        -47.395193255833334,
+                        -22.172577132833332
+                    ],
+                    [
+                        -47.395192610024395,
+                        -22.17257265769541
+                    ],
+                    [
+                        -47.395191325698136,
+                        -22.172563706509337
+                    ]
+                ]
+            }
+        },
+        {
+            "type": "Feature",
+            "properties": {
+                "Id": "3",
+                "Name": "CidadeJardimTerreno2",
+                "Length": 14.827915524386164,
+                "Dist1": 14.558011415731592,
+                "Dist2": 0.0
+            },
+            "geometry": {
+                "id": null,
+                "type": "LineString",
+                "coordinates": [
+                    [
+                        -47.3951762925,
+                        -22.172561603
+                    ],
+                    [
+                        -47.395180824166665,
+                        -22.172591686166665
+                    ],
+                    [
+                        -47.395185745333336,
+                        -22.172623080166666
+                    ],
+                    [
+                        -47.395190433,
+                        -22.172656367166667
+                    ],
+                    [
+                        -47.395195199,
+                        -22.172693641833334
+                    ],
+                    [
+                        -47.39519576904436,
+                        -22.172698125891035
+                    ],
+                    [
+                        -47.39519690267159,
+                        -22.172707094716973
+                    ]
+                ]
+            }
+        },
+        {
+            "type": "Feature",
+            "properties": {
+                "Id": "4",
+                "Name": "CidadeJardimTerreno2",
+                "Length": 14.785159385903514,
+                "Dist1": 14.558011415731592,
+                "Dist2": 0.0
+            },
+            "geometry": {
+                "id": null,
+                "type": "LineString",
+                "coordinates": [
+                    [
+                        -47.39518293216667,
+                        -22.1727127985
+                    ],
+                    [
+                        -47.39517819266667,
+                        -22.172680514833335
+                    ],
+                    [
+                        -47.3951732595,
+                        -22.172646752833334
+                    ],
+                    [
+                        -47.39516880516667,
+                        -22.1726149155
+                    ],
+                    [
+                        -47.39516381233334,
+                        -22.172581167166665
+                    ],
+                    [
+                        -47.39516315429932,
+                        -22.172576693573966
+                    ],
+                    [
+                        -47.39516184565584,
+                        -22.172567745443878
+                    ]
+                ]
+            }
+        },
+        {
+            "type": "Feature",
+            "properties": {
+                "Id": "5",
+                "Name": "CidadeJardimTerreno2",
+                "Length": 14.80117692191233,
+                "Dist1": 14.558011415731592,
+                "Dist2": 0.0
+            },
+            "geometry": {
+                "id": null,
+                "type": "LineString",
+                "coordinates": [
+                    [
+                        -47.395147317833334,
+                        -22.172566031
+                    ],
+                    [
+                        -47.395151503166666,
+                        -22.172595452333333
+                    ],
+                    [
+                        -47.3951561855,
+                        -22.172628011166665
+                    ],
+                    [
+                        -47.39516159866667,
+                        -22.172663757333332
+                    ],
+                    [
+                        -47.39516672716667,
+                        -22.172697770833334
+                    ],
+                    [
+                        -47.395167397572706,
+                        -22.172702242832194
+                    ],
+                    [
+                        -47.39516873082615,
+                        -22.17271118781009
+                    ]
+                ]
+            }
+        }
+    ]
+}

AgroBase/AgroBase/bin/Debug/Python/Output/mapa_interativo.html

@@ -25,7 +25,7 @@
             <meta name="viewport" content="width=device-width,
                 initial-scale=1.0, maximum-scale=1.0, user-scalable=no" />
             <style>
-                #map_5d2c7f135b188d679c1a4d3824a90e8f {
+                #map_99bb698933046267d33b2256e2758b03 {
                     position: relative;
                     width: 100.0%;
                     height: 100.0%;
@@ -39,14 +39,14 @@
 <body>
     
     
-            <div class="folium-map" id="map_5d2c7f135b188d679c1a4d3824a90e8f" ></div>
+            <div class="folium-map" id="map_99bb698933046267d33b2256e2758b03" ></div>
         
 </body>
 <script>
     
     
-            var map_5d2c7f135b188d679c1a4d3824a90e8f = L.map(
+            var map_99bb698933046267d33b2256e2758b03 = L.map(
-                "map_5d2c7f135b188d679c1a4d3824a90e8f",
+                "map_99bb698933046267d33b2256e2758b03",
                 {
                     center: [-22.172636164916668, -47.395186322185666],
                     crs: L.CRS.EPSG3857,
@@ -60,13 +60,13 @@
 
         
     
-            var tile_layer_c3102dc310a5c0966b042ddc1d0446a2 = L.tileLayer(
+            var tile_layer_0a630e1ac8e2fe468b74b905e056ba5f = L.tileLayer(
                 "https://tile.openstreetmap.org/{z}/{x}/{y}.png",
                 {"attribution": "\u0026copy; \u003ca href=\"https://www.openstreetmap.org/copyright\"\u003eOpenStreetMap\u003c/a\u003e contributors", "detectRetina": false, "maxNativeZoom": 19, "maxZoom": 19, "minZoom": 0, "noWrap": false, "opacity": 1, "subdomains": "abc", "tms": false}
             );
         
     
-            tile_layer_c3102dc310a5c0966b042ddc1d0446a2.addTo(map_5d2c7f135b188d679c1a4d3824a90e8f);
+            tile_layer_0a630e1ac8e2fe468b74b905e056ba5f.addTo(map_99bb698933046267d33b2256e2758b03);
         
     
 
@@ -82,7 +82,7 @@
                 }*/
             });
     }
-    function geo_json_eda399b26bc88766d3547dbe170f8acd_onEachFeature(feature, layer) {
+    function geo_json_778ae99174b146383df6fca12636dac2_onEachFeature(feature, layer) {
             layer.on({
             
     click: function(e) {
@@ -118,20 +118,20 @@
         }*/
     });
         };
-        var geo_json_eda399b26bc88766d3547dbe170f8acd = L.geoJson(null, {
+        var geo_json_778ae99174b146383df6fca12636dac2 = L.geoJson(null, {
-                onEachFeature: geo_json_eda399b26bc88766d3547dbe170f8acd_onEachFeature,
+                onEachFeature: geo_json_778ae99174b146383df6fca12636dac2_onEachFeature,
             
         });
 
-        function geo_json_eda399b26bc88766d3547dbe170f8acd_add (data) {
+        function geo_json_778ae99174b146383df6fca12636dac2_add (data) {
-            geo_json_eda399b26bc88766d3547dbe170f8acd
+            geo_json_778ae99174b146383df6fca12636dac2
                 .addData(data);
         }
-            geo_json_eda399b26bc88766d3547dbe170f8acd_add({"features": [{"geometry": {"coordinates": [[-47.395205344, -22.172559531333334], [-47.395212610166666, -22.172614638833334], [-47.395219157, -22.172656417833334], [-47.395223544833335, -22.1726892105], [-47.39522414098443, -22.17269369161165], [-47.395225326538004, -22.172702654611555]], "id": "0", "type": "LineString"}, "id": 0, "properties": {"Dist1": 14.558011415731592, "Dist2": 0.0, "Id": "1", "Length": 14.558011415731592, "Name": "CidadeJardimTerreno2"}, "type": "Feature"}, {"geometry": {"coordinates": [[-47.39521090233333, -22.172708800833334], [-47.395206943666665, -22.172679811], [-47.395202420666664, -22.1726491015], [-47.395198865666664, -22.172615782833333], [-47.395193255833334, -22.172577132833332], [-47.395192610024395, -22.17257265769541], [-47.395191325698136, -22.172563706509337]], "id": "1", "type": "LineString"}, "id": 1, "properties": {"Dist1": 14.558011415731592, "Dist2": 0.0, "Id": "2", "Length": 14.771318274761821, "Name": "CidadeJardimTerreno2"}, "type": "Feature"}, {"geometry": {"coordinates": [[-47.3951762925, -22.172561603], [-47.395180824166665, -22.172591686166665], [-47.395185745333336, -22.172623080166666], [-47.395190433, -22.172656367166667], [-47.395195199, -22.172693641833334], [-47.39519576904436, -22.172698125891035], [-47.39519690267159, -22.172707094716973]], "id": "2", "type": "LineString"}, "id": 2, "properties": {"Dist1": 14.558011415731592, "Dist2": 0.0, "Id": "3", "Length": 14.827915524386164, "Name": "CidadeJardimTerreno2"}, "type": "Feature"}, {"geometry": {"coordinates": [[-47.39518293216667, -22.1727127985], [-47.39517819266667, -22.172680514833335], [-47.3951732595, -22.172646752833334], [-47.39516880516667, -22.1726149155], [-47.39516381233334, -22.172581167166665], [-47.39516315429932, -22.172576693573966], [-47.39516184565584, -22.172567745443878]], "id": "3", "type": "LineString"}, "id": 3, "properties": {"Dist1": 14.558011415731592, "Dist2": 0.0, "Id": "4", "Length": 14.785159385903514, "Name": "CidadeJardimTerreno2"}, "type": "Feature"}, {"geometry": {"coordinates": [[-47.395147317833334, -22.172566031], [-47.395151503166666, -22.172595452333333], [-47.3951561855, -22.172628011166665], [-47.39516159866667, -22.172663757333332], [-47.39516672716667, -22.172697770833334], [-47.395167397572706, -22.172702242832194], [-47.39516873082615, -22.17271118781009]], "id": "4", "type": "LineString"}, "id": 4, "properties": {"Dist1": 14.558011415731592, "Dist2": 0.0, "Id": "5", "Length": 14.80117692191233, "Name": "CidadeJardimTerreno2"}, "type": "Feature"}], "type": "FeatureCollection"});
+            geo_json_778ae99174b146383df6fca12636dac2_add({"features": [{"geometry": {"coordinates": [[-47.395205344, -22.172559531333334], [-47.395212610166666, -22.172614638833334], [-47.395219157, -22.172656417833334], [-47.395223544833335, -22.1726892105], [-47.39522414098443, -22.17269369161165], [-47.395225326538004, -22.172702654611555]], "id": null, "type": "LineString"}, "id": 0, "properties": {"Dist1": 14.558011415731592, "Dist2": 0.0, "Id": "1", "Length": 14.558011415731592, "Name": "CidadeJardimTerreno2"}, "type": "Feature"}, {"geometry": {"coordinates": [[-47.39521090233333, -22.172708800833334], [-47.395206943666665, -22.172679811], [-47.395202420666664, -22.1726491015], [-47.395198865666664, -22.172615782833333], [-47.395193255833334, -22.172577132833332], [-47.395192610024395, -22.17257265769541], [-47.395191325698136, -22.172563706509337]], "id": null, "type": "LineString"}, "id": 1, "properties": {"Dist1": 14.558011415731592, "Dist2": 0.0, "Id": "2", "Length": 14.771318274761821, "Name": "CidadeJardimTerreno2"}, "type": "Feature"}, {"geometry": {"coordinates": [[-47.3951762925, -22.172561603], [-47.395180824166665, -22.172591686166665], [-47.395185745333336, -22.172623080166666], [-47.395190433, -22.172656367166667], [-47.395195199, -22.172693641833334], [-47.39519576904436, -22.172698125891035], [-47.39519690267159, -22.172707094716973]], "id": null, "type": "LineString"}, "id": 2, "properties": {"Dist1": 14.558011415731592, "Dist2": 0.0, "Id": "3", "Length": 14.827915524386164, "Name": "CidadeJardimTerreno2"}, "type": "Feature"}, {"geometry": {"coordinates": [[-47.39518293216667, -22.1727127985], [-47.39517819266667, -22.172680514833335], [-47.3951732595, -22.172646752833334], [-47.39516880516667, -22.1726149155], [-47.39516381233334, -22.172581167166665], [-47.39516315429932, -22.172576693573966], [-47.39516184565584, -22.172567745443878]], "id": null, "type": "LineString"}, "id": 3, "properties": {"Dist1": 14.558011415731592, "Dist2": 0.0, "Id": "4", "Length": 14.785159385903514, "Name": "CidadeJardimTerreno2"}, "type": "Feature"}, {"geometry": {"coordinates": [[-47.395147317833334, -22.172566031], [-47.395151503166666, -22.172595452333333], [-47.3951561855, -22.172628011166665], [-47.39516159866667, -22.172663757333332], [-47.39516672716667, -22.172697770833334], [-47.395167397572706, -22.172702242832194], [-47.39516873082615, -22.17271118781009]], "id": null, "type": "LineString"}, "id": 4, "properties": {"Dist1": 14.558011415731592, "Dist2": 0.0, "Id": "5", "Length": 14.80117692191233, "Name": "CidadeJardimTerreno2"}, "type": "Feature"}], "type": "FeatureCollection"});
 
         
     
-            geo_json_eda399b26bc88766d3547dbe170f8acd.addTo(map_5d2c7f135b188d679c1a4d3824a90e8f);
+            geo_json_778ae99174b146383df6fca12636dac2.addTo(map_99bb698933046267d33b2256e2758b03);
         
 </script>
 
@@ -152,7 +152,7 @@
         }
         trajeto_json_add({"features": []});
 
-        trajeto_json.addTo(map_5d2c7f135b188d679c1a4d3824a90e8f);
+        trajeto_json.addTo(map_99bb698933046267d33b2256e2758b03);
 
         function adicionarGeometria(novaGeometria) {
             trajeto_json.addData(novaGeometria);
@@ -210,7 +210,7 @@
         }
         trajeto_dinamico_json_add({"features": []});
 
-        trajeto_dinamico_json.addTo(map_5d2c7f135b188d679c1a4d3824a90e8f);
+        trajeto_dinamico_json.addTo(map_99bb698933046267d33b2256e2758b03);
 
         function adicionarGeometriaDinamica(novaGeometria) {
             trajeto_dinamico_json.addData(novaGeometria);
@@ -263,9 +263,9 @@
         
         var marcadorEquipamento = L.marker([0, 0], {
             icon: customIcon
-        }).addTo(map_5d2c7f135b188d679c1a4d3824a90e8f);
+        }).addTo(map_99bb698933046267d33b2256e2758b03);
         
-        var marcadorBase = L.marker([0, 0], {}).addTo(map_5d2c7f135b188d679c1a4d3824a90e8f);
+        var marcadorBase = L.marker([0, 0], {}).addTo(map_99bb698933046267d33b2256e2758b03);
         var icon = L.AwesomeMarkers.icon(
             {"extraClasses": "fa-rotate-0", "icon": "info-sign", "iconColor": "white", "markerColor": "red", "prefix": "glyphicon"}
         );
@@ -347,7 +347,7 @@
                 }
                 
                 if (foco) {
-                    map_5d2c7f135b188d679c1a4d3824a90e8f.setView(novaPosicao, map_5d2c7f135b188d679c1a4d3824a90e8f.getZoom());
+                    map_99bb698933046267d33b2256e2758b03.setView(novaPosicao, map_99bb698933046267d33b2256e2758b03.getZoom());
                 }
                 
             }
@@ -364,7 +364,7 @@
         function atualizarSelecaoRuas(selecionadas) {
             selecionadas = JSON.parse(selecionadas);
             RuasSelecionadas = Array.isArray(selecionadas) ? [...selecionadas] : [];
-            geo_json_eda399b26bc88766d3547dbe170f8acd.eachLayer(function (layer) {
+            geo_json_778ae99174b146383df6fca12636dac2.eachLayer(function (layer) {
                 if (RuasSelecionadas.includes(parseInt(layer.feature.id))) {
                     layer.setStyle({ color: 'blue' });
                 } else {

AgroBase/AgroBase/bin/Debug/Python/Scripts/mpc_controller-1_0.py

@@ -0,0 +1,318 @@
+# MPC com MQTT, simulação e visualização da trajetória
+import numpy as np
+import paho.mqtt.client as mqtt
+import json
+from enum import Enum
+#import matplotlib.pyplot as plt
+
+class StatusCarroMapa(Enum):
+    Parado = 0
+    EntrandoRua = 1
+    CaminhandoRua = 2
+    SaindoRua = 3
+    Manobrando = 4
+    Direcionando = 5
+
+class TipoMovimentoDirecional(Enum):
+    RodasDianteiras = 0
+    RodasTraseiras = 1
+    RotacionarNoEixo = 2
+    MovimentoArco = 3
+    MovimentoLateral = 4
+    MovimentoDiagonal = 5
+    Diagnostico = 6
+
+class TipoPontoRua(Enum):
+    Indefinido = -1
+    PosicaoRobo = 0
+    LigacaoEntrada = 1
+    BordaEntrada = 2
+    Rua = 3
+    BordaSaida = 4
+    LigacaoSaida = 5
+    CruvaEntreCorredores = 6
+    Desvio = 7
+
+
+topico_comando = "mpc/comando"
+topico_posicao = "mpc/posicao"
+topico_rota = "mpc/rota"
+
+raio_terra = 6371000
+
+# Parâmetros do MPC
+angulo_max_graus = 30.0
+velocidade_min = 0.4
+velocidade_max = 1.9
+distancia_entre_eixos = 0.92
+horizonte = 20
+
+# Estado compartilhado
+trajetoria_latlon = []
+pontos_info = []
+visitados_execucao = []
+lat0, lon0 = None, None
+
+# Visualização
+#plt.ion()
+#fig, ax = plt.subplots()
+
+# Funções matemáticas auxiliares
+def latlon_to_xy(lat, lon, lat0, lon0):
+    global raio_terra
+    R = raio_terra
+    dlat = np.radians(lat - lat0)
+    dlon = np.radians(lon - lon0)
+    x = R * dlon * np.cos(np.radians((lat + lat0) / 2))
+    y = R * dlat
+    return x, y
+
+def calcular_orientacao(p1, p2):
+    dx = p2[0] - p1[0]
+    dy = p2[1] - p1[1]
+    orient = np.arctan2(dy, dx) - np.radians(90)
+    return (orient + np.pi) % (2 * np.pi) - np.pi
+
+def calcular_omega(v, angulo_rad, tipo):
+    tan_delta = np.tan(angulo_rad)
+    if abs(angulo_rad) < 0.01:
+        return 0.0
+
+    k = 1.35 + 0.5 * np.exp(-abs(np.degrees(angulo_rad)) / 15.0)
+
+    R = distancia_entre_eixos / tan_delta
+    if tipo == TipoMovimentoDirecional.MovimentoArco:
+        R *= 0.5
+
+    R *= k  # Aplica fator de correção
+    return v / R
+
+def proximo_nao_visitado(visitados):
+    for i, v in enumerate(visitados):
+        if not v:
+            return i
+    return len(visitados) - 1
+
+def corrigir_pontos_visitados(x, y, pontos_visitados, limite_max_avanço=5):
+    for idx, ponto in enumerate(pontos_info):
+        if pontos_visitados[idx]:
+            continue
+        pos = ponto["xy"]
+        margem = ponto.get("distanciaMargem", 0.7)
+        dist = np.linalg.norm([x - pos[0], y - pos[1]])
+        if dist < margem:
+            for i in range(idx + 1):
+                pontos_visitados[i] = True
+            return idx
+        if idx > 0 and not pontos_visitados[idx - 1] and idx >= limite_max_avanço:
+            break
+    return proximo_nao_visitado(pontos_visitados)
+
+def calcular_melhor_candidato(x, y, theta, visitados_sim, contexto):
+    idx_alvo = corrigir_pontos_visitados(x, y, visitados_sim)
+    if idx_alvo >= len(pontos_info):
+        return 0.0, TipoMovimentoDirecional.RodasDianteiras
+
+    ponto_info = pontos_info[idx_alvo]
+    tipo_ponto = TipoPontoRua(ponto_info.get("tipo"))
+    ponto_alvo = ponto_info["xy"]
+
+    if np.linalg.norm([x - ponto_alvo[0], y - ponto_alvo[1]]) < ponto_info.get("distanciaMargem", 0.7):
+        visitados_sim[idx_alvo] = True
+        idx_alvo = corrigir_pontos_visitados(x, y, visitados_sim)
+        if idx_alvo >= len(pontos_info):
+            return 0.0, TipoMovimentoDirecional.RodasDianteiras
+        
+        ponto_info = pontos_info[idx_alvo]
+        tipo_ponto = TipoPontoRua(ponto_info.get("tipo"))
+        ponto_alvo = ponto_info["xy"]
+
+    orient = calcular_orientacao((x, y), ponto_alvo)
+    erro_ori = abs((orient - theta + np.pi) % (2 * np.pi) - np.pi)
+    delta_theta = ((-orient) - theta + np.pi) % (2 * np.pi) - np.pi
+    angulo = np.clip(delta_theta, -np.radians(angulo_max_graus), np.radians(angulo_max_graus))
+
+    # Pesos de custo base
+    peso_erro_pos, peso_erro_ori, peso_movimento, tipos_validos = calcular_pesos_movimento(contexto, erro_ori)
+
+    melhor_custo = float('inf')
+    melhor_tipo = TipoMovimentoDirecional.RodasDianteiras
+    melhor_angulo = angulo
+
+    for tipo in tipos_validos:
+        omega = calcular_omega(1.0, angulo, tipo)
+        x_sim = x + 1.0 * np.cos(theta) * 0.2
+        y_sim = y + 1.0 * np.sin(theta) * 0.2
+        theta_sim = theta + omega * 0.2
+
+        erro_pos = np.linalg.norm([x_sim - ponto_alvo[0], y_sim - ponto_alvo[1]])
+        orient_sim = calcular_orientacao((x_sim, y_sim), ponto_alvo)
+        erro_ori_sim = abs((orient_sim - theta_sim + np.pi) % (2 * np.pi) - np.pi)
+
+        vetor_alvo = np.array(ponto_alvo) - np.array([x_sim, y_sim])
+        vetor_alvo_norm = vetor_alvo / np.linalg.norm(vetor_alvo)
+        vetor_movel = np.array([np.cos(theta_sim), np.sin(theta_sim)])
+        cos_angulo = np.dot(vetor_alvo_norm, vetor_movel)
+        penalidade_orientacao = 9999 if cos_angulo < 0 else 0
+
+        custo = erro_pos * peso_erro_pos + erro_ori_sim * peso_erro_ori + penalidade_orientacao + peso_movimento[tipo]
+
+        if custo < melhor_custo:
+            melhor_custo = custo
+            melhor_tipo = tipo
+            melhor_angulo = angulo
+
+    return melhor_angulo, melhor_tipo.value
+
+def calcular_pesos_movimento(contexto, erro_ori):
+    # Lista de tipos sempre permitidos
+    tipos_validos = [
+        TipoMovimentoDirecional.RodasDianteiras, 
+        TipoMovimentoDirecional.MovimentoArco
+    ]
+
+    peso_erro_pos = 2.0
+    peso_erro_ori = 1.5
+    peso_movimento = {
+        TipoMovimentoDirecional.RodasDianteiras: 0.5,
+        TipoMovimentoDirecional.MovimentoArco: 1.0
+    }
+
+    status = StatusCarroMapa(contexto.get("StatusCarro", 0))
+    dentro = contexto.get("DentroCorredor", False)
+    manobrando = contexto.get("ManobrandoEntreRuas", False)
+    velocidade = contexto.get("Velocidade", velocidade_min)
+
+    # Penalidade proporcional à velocidade
+    penalidade_por_velocidade = velocidade / velocidade_max
+
+    if manobrando or status in [
+        StatusCarroMapa.EntrandoRua,
+        StatusCarroMapa.SaindoRua,
+        StatusCarroMapa.Manobrando
+    ]:
+        peso_movimento[TipoMovimentoDirecional.MovimentoArco] = 0.2
+        peso_movimento[TipoMovimentoDirecional.RodasDianteiras] = 2.0 + penalidade_por_velocidade
+    elif dentro:
+        peso_movimento[TipoMovimentoDirecional.MovimentoArco] = 0.5 + (erro_ori / np.radians(angulo_max_graus)) * 2.0
+        peso_movimento[TipoMovimentoDirecional.RodasDianteiras] = 1.0 + penalidade_por_velocidade
+    else:
+        peso_movimento[TipoMovimentoDirecional.MovimentoArco] = 0.3
+        peso_movimento[TipoMovimentoDirecional.RodasDianteiras] = 1.5 + penalidade_por_velocidade
+
+    return peso_erro_pos, peso_erro_ori, peso_movimento, tipos_validos
+
+def processar_mpc(pos_lat, pos_lon, theta, velocidade, dt, contexto):
+    #print(f"Posicao recebida: Velocidade={velocidade}, dt={dt}, Orientacao={np.degrees(theta)}")
+    global pontos_info, visitados_execucao, lat0, lon0, raio_terra
+    if not pontos_info:
+        return None
+
+    x, y = latlon_to_xy(pos_lat, pos_lon, lat0, lon0)
+    visitados_sim = visitados_execucao.copy()
+
+    sim = [(x, y)]
+    x_temp, y_temp, theta_temp = x, y, theta
+    for _ in range(horizonte):
+        angulo_mpc, tipo_mpc = calcular_melhor_candidato(x_temp, y_temp, theta_temp, visitados_sim, contexto)
+        omega = calcular_omega(velocidade, angulo_mpc, TipoMovimentoDirecional(tipo_mpc))
+        theta_plot = (-theta_temp) + np.radians(90)
+        x_temp += velocidade * np.cos(theta_plot) * dt
+        y_temp += velocidade * np.sin(theta_plot) * dt
+        theta_temp += omega * dt
+        sim.append((x_temp, y_temp))
+
+    # Converte os pontos simulados para lat/lon
+    simulacao_latlon = []
+    for sx, sy in sim:
+        dlat = sy / raio_terra
+        dlon = sx / (raio_terra * np.cos(np.radians(lat0)))
+        lat = lat0 + np.degrees(dlat)
+        lon = lon0 + np.degrees(dlon)
+        simulacao_latlon.append({"latitude": lat, "longitude": lon})
+
+    #ax.clear()
+    #if pontos_info:
+    #    tx, ty = zip(*[p["xy"] for p in pontos_info])
+    #    ax.plot(tx, ty, 'r.-', label="Trajetória alvo")
+    #if sim:
+    #    sx, sy = zip(*sim)
+    #    ax.plot(sx, sy, 'g:', label="Previsão MPC")
+    #ax.plot(sim[0][0], sim[0][1], 'bo', label="Posição atual")
+    #ax.set_title("Visualização MPC (tempo real)")
+    #ax.set_xlabel("X (m)")
+    #ax.set_ylabel("Y (m)")
+    #ax.axis("equal")
+    #ax.legend()
+    #plt.pause(0.001)
+
+    angulo_final, tipo_final = calcular_melhor_candidato(sim[0][0], sim[0][1], theta, visitados_execucao, contexto)
+    #print(f"angulo: {np.degrees(angulo_final)}, tipo: {tipo_final}")
+    return {
+        "angulo": np.degrees(angulo_final),
+        "tipo": tipo_final,
+        "simulacao": simulacao_latlon
+    }
+
+def on_connect(client, userdata, flags, rc):
+    #print("Conectado ao MQTT")
+    client.subscribe(topico_rota)
+    client.subscribe(topico_posicao)
+    client.publish(topico_comando, "OK")
+
+def on_message(client, userdata, msg):
+    global trajetoria_latlon, pontos_info, visitados_execucao, lat0, lon0, angulo_max_graus, distancia_entre_eixos, horizonte, velocidade_max, velocidade_min
+    
+    if (not msg.payload):
+        return
+    
+    try:
+        dados = json.loads(msg.payload.decode())
+    except json.JSONDecodeError as e:
+        print(f"Erro ao decodificar JSON em '{msg.topic}': {e}")
+        return
+
+    if msg.topic == topico_rota:
+        trajetoria_latlon = dados["pontos"]
+        angulo_max_graus = dados["angulo_max_graus"]
+        velocidade_min = dados["velocidade_min"]
+        velocidade_max = dados["velocidade_max"]
+        distancia_entre_eixos = dados["distancia_entre_eixos"]
+        horizonte = dados["horizonte"]
+        lat0, lon0 = trajetoria_latlon[0]["lat"], trajetoria_latlon[0]["lon"]
+        pontos_info = []
+        for p in trajetoria_latlon:
+            x, y = latlon_to_xy(p["lat"], p["lon"], lat0, lon0)
+            ponto = {
+                "xy": (x, y),
+                "tipo": p.get("tipo", 3),
+                "distanciaMargem": p.get("distanciaMargem", 0.7)
+            }
+            pontos_info.append(ponto)
+        visitados_execucao = [False] * len(pontos_info)
+        print(f"Trajetória recebida com: Pontos={len(pontos_info)}, angulo_max={angulo_max_graus}, vel_min={velocidade_min}, vel_max={velocidade_max}, dist_eixos={distancia_entre_eixos}, horizonte={horizonte}")
+
+    elif msg.topic == topico_posicao:
+        lat = dados["lat"]
+        lon = dados["lon"]
+        theta = np.radians(dados["theta"])
+        velocidade = dados["velocidade"]
+        dt = dados["dt"]
+        # Novo: contexto completo (com valores padrão caso não venha)
+        contexto = dados.get("contexto", {})
+        contexto.setdefault("StatusCarro", 0)
+        contexto.setdefault("DentroCorredor", False)
+        contexto.setdefault("ManobrandoEntreRuas", False)
+        contexto.setdefault("Velocidade", velocidade)  # já aproveita
+
+        comando = processar_mpc(lat, lon, theta, velocidade, dt, contexto)
+        if comando:
+            client.publish(topico_comando, json.dumps(comando))
+
+client = mqtt.Client()
+client.on_connect = on_connect
+client.on_message = on_message
+client.connect("localhost", 1883, 60)
+
+#print("Aguardando trajetória e posição...")
+client.loop_forever()

AgroBase/AgroBase/obj/Debug/AgroBase.csproj.FileListAbsolute.txt

@@ -482,4 +482,4 @@ C:\ZendionInc\agrobot_base\AgroBase\AgroBase\obj\Debug\AgroBase.csproj.GenerateR
 C:\ZendionInc\agrobot_base\AgroBase\AgroBase\obj\Debug\AgroBase.csproj.CoreCompileInputs.cache
 C:\ZendionInc\agrobot_base\AgroBase\AgroBase\obj\Debug\AgroBase.exe
 C:\ZendionInc\agrobot_base\AgroBase\AgroBase\obj\Debug\AgroBase.pdb
-C:\ZendionINC\agrobot_base\AgroBase\AgroBase\obj\Debug\AgroBase.csproj.Up2Date
+C:\ZendionInc\agrobot_base\AgroBase\AgroBase\obj\Debug\AgroBase.csproj.CopyComplete

Python/mpc/CidadeJardimTerreno2_Corrigido.json

@@ -0,0 +1,221 @@
+{
+    "type": "FeatureCollection",
+    "features": [
+        {
+            "type": "Feature",
+            "properties": {
+                "Id": "1",
+                "Name": "CidadeJardimTerreno2",
+                "Length": 14.558011415731592,
+                "Dist1": 14.558011415731592,
+                "Dist2": 0.0
+            },
+            "geometry": {
+                "id": null,
+                "type": "LineString",
+                "coordinates": [
+                    [
+                        -47.395205344,
+                        -22.172559531333334
+                    ],
+                    [
+                        -47.395212610166666,
+                        -22.172614638833334
+                    ],
+                    [
+                        -47.395219157,
+                        -22.172656417833334
+                    ],
+                    [
+                        -47.395223544833335,
+                        -22.1726892105
+                    ],
+                    [
+                        -47.39522414098443,
+                        -22.17269369161165
+                    ],
+                    [
+                        -47.395225326538004,
+                        -22.172702654611555
+                    ]
+                ]
+            }
+        },
+        {
+            "type": "Feature",
+            "properties": {
+                "Id": "2",
+                "Name": "CidadeJardimTerreno2",
+                "Length": 14.771318274761821,
+                "Dist1": 14.558011415731592,
+                "Dist2": 0.0
+            },
+            "geometry": {
+                "id": null,
+                "type": "LineString",
+                "coordinates": [
+                    [
+                        -47.39521090233333,
+                        -22.172708800833334
+                    ],
+                    [
+                        -47.395206943666665,
+                        -22.172679811
+                    ],
+                    [
+                        -47.395202420666664,
+                        -22.1726491015
+                    ],
+                    [
+                        -47.395198865666664,
+                        -22.172615782833333
+                    ],
+                    [
+                        -47.395193255833334,
+                        -22.172577132833332
+                    ],
+                    [
+                        -47.395192610024395,
+                        -22.17257265769541
+                    ],
+                    [
+                        -47.395191325698136,
+                        -22.172563706509337
+                    ]
+                ]
+            }
+        },
+        {
+            "type": "Feature",
+            "properties": {
+                "Id": "3",
+                "Name": "CidadeJardimTerreno2",
+                "Length": 14.827915524386164,
+                "Dist1": 14.558011415731592,
+                "Dist2": 0.0
+            },
+            "geometry": {
+                "id": null,
+                "type": "LineString",
+                "coordinates": [
+                    [
+                        -47.3951762925,
+                        -22.172561603
+                    ],
+                    [
+                        -47.395180824166665,
+                        -22.172591686166665
+                    ],
+                    [
+                        -47.395185745333336,
+                        -22.172623080166666
+                    ],
+                    [
+                        -47.395190433,
+                        -22.172656367166667
+                    ],
+                    [
+                        -47.395195199,
+                        -22.172693641833334
+                    ],
+                    [
+                        -47.39519576904436,
+                        -22.172698125891035
+                    ],
+                    [
+                        -47.39519690267159,
+                        -22.172707094716973
+                    ]
+                ]
+            }
+        },
+        {
+            "type": "Feature",
+            "properties": {
+                "Id": "4",
+                "Name": "CidadeJardimTerreno2",
+                "Length": 14.785159385903514,
+                "Dist1": 14.558011415731592,
+                "Dist2": 0.0
+            },
+            "geometry": {
+                "id": null,
+                "type": "LineString",
+                "coordinates": [
+                    [
+                        -47.39518293216667,
+                        -22.1727127985
+                    ],
+                    [
+                        -47.39517819266667,
+                        -22.172680514833335
+                    ],
+                    [
+                        -47.3951732595,
+                        -22.172646752833334
+                    ],
+                    [
+                        -47.39516880516667,
+                        -22.1726149155
+                    ],
+                    [
+                        -47.39516381233334,
+                        -22.172581167166665
+                    ],
+                    [
+                        -47.39516315429932,
+                        -22.172576693573966
+                    ],
+                    [
+                        -47.39516184565584,
+                        -22.172567745443878
+                    ]
+                ]
+            }
+        },
+        {
+            "type": "Feature",
+            "properties": {
+                "Id": "5",
+                "Name": "CidadeJardimTerreno2",
+                "Length": 14.80117692191233,
+                "Dist1": 14.558011415731592,
+                "Dist2": 0.0
+            },
+            "geometry": {
+                "id": null,
+                "type": "LineString",
+                "coordinates": [
+                    [
+                        -47.395147317833334,
+                        -22.172566031
+                    ],
+                    [
+                        -47.395151503166666,
+                        -22.172595452333333
+                    ],
+                    [
+                        -47.3951561855,
+                        -22.172628011166665
+                    ],
+                    [
+                        -47.39516159866667,
+                        -22.172663757333332
+                    ],
+                    [
+                        -47.39516672716667,
+                        -22.172697770833334
+                    ],
+                    [
+                        -47.395167397572706,
+                        -22.172702242832194
+                    ],
+                    [
+                        -47.39516873082615,
+                        -22.17271118781009
+                    ]
+                ]
+            }
+        }
+    ]
+}

Python/mpc/pontos.json

@@ -0,0 +1 @@
+{"pontos":[[-22.172546316494021,-47.395203906184044],[-22.172554486898338,-47.395197380933986],[-22.172561618921335,-47.39519833484907],[-22.172570764605126,-47.39519955809326],[-22.172579910288917,-47.395200781337451],[-22.172589055972708,-47.395202004581641],[-22.172599434069689,-47.395203559929712],[-22.172609812166666,-47.395205115277776],[-22.17261843375,-47.395206363531244],[-22.172627055333333,-47.39520761178472],[-22.172635676916666,-47.395208860038196],[-22.1726442985,-47.395210108291664],[-22.17265283328398,-47.395211099563944],[-22.172661368067963,-47.395212090836232],[-22.172669902851943,-47.395213082108512],[-22.172678700537212,-47.395214361615096],[-22.17268749822248,-47.395215641121681],[-22.172696612972462,-47.395216877778672],[-22.172705727722445,-47.395218114435664],[-22.172712121750248,-47.395221657035862],[-22.172712121750248,-47.395221657035862],[-22.172716653743002,-47.395219218133455],[-22.172719570120194,-47.395216634939345],[-22.172720870881825,-47.395213907453545],[-22.17272055602789,-47.395211035676063],[-22.1727186255584,-47.395208019606891],[-22.172715079473342,-47.395204859246029],[-22.172707947775152,-47.395203902502459],[-22.172698458110332,-47.395202629428987],[-22.172688968445517,-47.395201356355514],[-22.17268017005609,-47.395200083094423],[-22.172671371666667,-47.395198809833332],[-22.1726625475,-47.395197769708332],[-22.172653723333333,-47.395196729583333],[-22.172644899166666,-47.395195689458333],[-22.172636075,-47.395194649333334],[-22.1726288813,-47.395193619583331],[-22.1726216876,-47.395192589833336],[-22.1726144939,-47.395191560083333],[-22.1726073002,-47.395190530333338],[-22.1726001065,-47.395189500583335],[-22.17259113921552,-47.395188108839434],[-22.17258217193104,-47.395186717095527],[-22.172572413342856,-47.3951852630973],[-22.172562654754667,-47.395183809099066],[-22.172555629052788,-47.395185441649048],[-22.172555629052788,-47.395185441649048],[-22.172551986345685,-47.395181541050519],[-22.172549929165552,-47.395178038176475],[-22.172549457512385,-47.395174933026929],[-22.172550571386196,-47.395172225601875],[-22.172553270786981,-47.39516991590132],[-22.172557555714736,-47.395168003925257],[-22.172564674221938,-47.395169069077923],[-22.172574432046126,-47.395170529155457],[-22.172584189870314,-47.395171989232992],[-22.172593156768489,-47.395173384033164],[-22.172602123666664,-47.395174778833336],[-22.17261050308333,-47.395175988895836],[-22.172618882499997,-47.395177198958336],[-22.172627261916666,-47.395178409020836],[-22.172635641333333,-47.395179619083336],[-22.172644280333333,-47.395180771625],[-22.172652919333331,-47.395181924166664],[-22.172661558333331,-47.395183076708335],[-22.172670197333332,-47.39518422925],[-22.172679758847757,-47.39518560505276],[-22.172689320362185,-47.395186980855513],[-22.172699633485337,-47.395188449137322],[-22.172709946608485,-47.395189917419131],[-22.1727163205133,-47.395193502055555],[-22.1727163205133,-47.395193502055555],[-22.172720834453084,-47.395191044309996],[-22.172723729032157,-47.395188461546944],[-22.172725004250527,-47.395185753766377],[-22.172724660108191,-47.395182920968338],[-22.172722696605149,-47.3951799631528],[-22.172719113741397,-47.39517688031976],[-22.172711993155048,-47.39517583149641],[-22.172701685993907,-47.395174313308047],[-22.172691378832766,-47.395172795119692],[-22.17268182033305,-47.395171394226509],[-22.172672261833334,-47.395169993333333],[-22.172664030479169,-47.395168795479165],[-22.172655799125,-47.395167597625004],[-22.172647567770831,-47.395166399770837],[-22.172639336416665,-47.395165201916669],[-22.172630649604166,-47.395163901166669],[-22.172621962791666,-47.395162600416668],[-22.172613275979167,-47.395161299666668],[-22.172604589166667,-47.395159998916668],[-22.172595331060158,-47.39515866382483],[-22.172586072953649,-47.395157328732992],[-22.172576480587793,-47.395155955238792],[-22.172566888221937,-47.395154581744592],[-22.172552639889869,-47.395152541580387]]}

Python/mpc/test1.py

@@ -0,0 +1,70 @@
+import numpy as np
+import matplotlib.pyplot as plt
+
+# Parâmetros do modelo
+dt = 0.2       # intervalo de tempo (s)
+v = 1.0        # velocidade linear (m/s)
+horizonte = 50  # número de passos futuros
+
+# Ponto inicial
+x, y, theta = 0.0, 0.0, 0.0
+
+# Trajetória alvo (trajeto desejado)
+traj = np.array([
+    [0, 0],
+    [2, 0.5],
+    [4, 1.5],
+    [6, 3],
+    [8, 5],
+    [10, 7],
+    [12, 9],
+    [14, 10],
+    [16, 10],
+    [18, 9]
+])
+
+def calcular_orientacao(p1, p2):
+    dx = p2[0] - p1[0]
+    dy = p2[1] - p1[1]
+    return np.arctan2(dy, dx)
+
+# Trajetória gerada
+traj_gerada = [(x, y)]
+
+for step in range(horizonte):
+    # Acha ponto alvo mais próximo
+    distancias = np.linalg.norm(traj - np.array([x, y]), axis=1)
+    idx_alvo = np.argmin(distancias)
+    ponto_alvo = traj[min(idx_alvo + 1, len(traj) - 1)]
+
+    # Orientação desejada
+    orient_desejada = calcular_orientacao((x, y), ponto_alvo)
+    delta_theta = orient_desejada - theta
+
+    # Ajuste de ângulo para [-pi, pi]
+    delta_theta = (delta_theta + np.pi) % (2 * np.pi) - np.pi
+
+    # Calcular omega ideal
+    omega = delta_theta / dt
+    omega = np.clip(omega, -np.pi/4, np.pi/4)  # limitar a 45°/s
+
+    # Aplicar movimento
+    x += v * np.cos(theta) * dt
+    y += v * np.sin(theta) * dt
+    theta += omega * dt
+
+    traj_gerada.append((x, y))
+
+# Separar para plot
+traj_gerada = np.array(traj_gerada)
+
+# Plotar
+plt.plot(traj[:,0], traj[:,1], 'ro--', label='Trajetória desejada')
+plt.plot(traj_gerada[:,0], traj_gerada[:,1], 'bo-', label='Trajetória MPC (estimativa por passo)')
+plt.title("MPC passo a passo com orientação estimada")
+plt.xlabel("X")
+plt.ylabel("Y")
+plt.grid(True)
+plt.legend()
+plt.axis("equal")
+plt.show()

Python/mpc/test2.py

@@ -0,0 +1,84 @@
+import numpy as np
+import matplotlib.pyplot as plt
+from dataclasses import dataclass
+from typing import List
+
+# Simulação com base na estrutura GPSModel simplificada (Lat, Lon)
+@dataclass
+class GPSModel:
+    Latitude: float
+    Longitude: float
+    OrientacaoReal: float = 0.0  # Opcional, se necessário
+
+# Conversão simplificada de graus para metros (só para simular corretamente)
+def latlon_to_xy(lat, lon, lat0, lon0):
+    R = 6371000  # raio da Terra em metros
+    dlat = np.radians(lat - lat0)
+    dlon = np.radians(lon - lon0)
+    x = R * dlon * np.cos(np.radians((lat + lat0) / 2))
+    y = R * dlat
+    return x, y
+
+# Criar lista de pontos GPSModel simulando a trajetória alvo
+traj_gps = [
+    GPSModel(-22.0, -47.0),
+    GPSModel(-21.9999, -46.9998),
+    GPSModel(-21.9998, -46.9996),
+    GPSModel(-21.9995, -46.9990),
+    GPSModel(-21.9990, -46.9980),
+    GPSModel(-21.9985, -46.9970),
+    GPSModel(-21.9980, -46.9960),
+    GPSModel(-21.9978, -46.9955),
+    GPSModel(-21.9977, -46.9950),
+    GPSModel(-21.9976, -46.9945)
+]
+
+# Usar o primeiro ponto como referência para converter em metros
+lat0, lon0 = traj_gps[0].Latitude, traj_gps[0].Longitude
+traj_xy = np.array([latlon_to_xy(p.Latitude, p.Longitude, lat0, lon0) for p in traj_gps])
+
+# Estado inicial (x, y, theta)
+x, y, theta = 0.0, 0.0, 0.0
+dt = 0.2
+v = 1.0
+horizonte = 50
+
+def calcular_orientacao(p1, p2):
+    dx = p2[0] - p1[0]
+    dy = p2[1] - p1[1]
+    return np.arctan2(dy, dx)
+
+traj_gerada = [(x, y)]
+
+for step in range(horizonte):
+    # Encontrar ponto alvo mais próximo
+    distancias = np.linalg.norm(traj_xy - np.array([x, y]), axis=1)
+    idx_alvo = np.argmin(distancias)
+    ponto_alvo = traj_xy[min(idx_alvo + 1, len(traj_xy) - 1)]
+
+    # Calcular orientação ideal
+    orient_desejada = calcular_orientacao((x, y), ponto_alvo)
+    delta_theta = (orient_desejada - theta + np.pi) % (2 * np.pi) - np.pi
+
+    omega = delta_theta / dt
+    omega = np.clip(omega, -np.pi / 4, np.pi / 4)
+
+    # Aplicar movimento
+    x += v * np.cos(theta) * dt
+    y += v * np.sin(theta) * dt
+    theta += omega * dt
+
+    traj_gerada.append((x, y))
+
+traj_gerada = np.array(traj_gerada)
+
+# Plotagem
+plt.plot(traj_xy[:,0], traj_xy[:,1], 'ro--', label='Trajetória GPS (lat/lon convertida)')
+plt.plot(traj_gerada[:,0], traj_gerada[:,1], 'bo-', label='MPC passo a passo (estimado)')
+plt.xlabel("X (m)")
+plt.ylabel("Y (m)")
+plt.title("MPC usando GPSModel (Latitude/Longitude)")
+plt.legend()
+plt.grid(True)
+plt.axis("equal")
+plt.show()

Python/mpc/test3.py

@@ -0,0 +1,102 @@
+# Integração do cálculo físico de omega no script MPC passo a passo
+
+import numpy as np
+import matplotlib.pyplot as plt
+import json
+
+# Carregar mapa
+with open("CidadeJardimTerreno2_Corrigido.json", "r", encoding="utf-8") as f:
+    data = json.load(f)
+
+traj_gps = []
+for feature in data["features"]:
+    coords = feature["geometry"]["coordinates"]
+    for lon, lat in coords:
+        traj_gps.append((lat, lon))
+
+def latlon_to_xy(lat, lon, lat0, lon0):
+    R = 6371000
+    dlat = np.radians(lat - lat0)
+    dlon = np.radians(lon - lon0)
+    x = R * dlon * np.cos(np.radians((lat + lat0) / 2))
+    y = R * dlat
+    return x, y
+
+lat0, lon0 = traj_gps[0]
+traj_xy = np.array([latlon_to_xy(lat, lon, lat0, lon0) for lat, lon in traj_gps])
+
+# Parâmetros físicos do robô
+distancia_entre_eixos = 0.92  # L
+dt = 0.2
+v = 1.0
+angulo_max_graus = 30.0
+tipo_movimento = "movimentoArco"  # "rodasFrontais" ou "movimentoArco"
+horizonte = 100
+
+def calcular_orientacao(p1, p2):
+    dx = p2[0] - p1[0]
+    dy = p2[1] - p1[1]
+    return np.arctan2(dy, dx)
+
+def calcular_omega_fisico(v, angulo_rad, tipo_movimento):
+    L = distancia_entre_eixos
+    tan_delta = np.tan(angulo_rad)
+    if tipo_movimento == "rodasFrontais":
+        return (v * tan_delta) / L
+    elif tipo_movimento == "movimentoArco":
+        return (2 * v * tan_delta) / L
+    return 0.0
+
+# Início da simulação
+x, y, theta = traj_xy[0][0] - 0.5, traj_xy[0][1] - 0.5, 0.0
+traj_gerada = [(x, y)]
+indice_atual = 0
+dist_anterior = None
+angulo_max_rad = np.radians(angulo_max_graus)
+
+for step in range(horizonte):
+    if indice_atual >= len(traj_xy) - 1:
+        break
+
+    ponto_atual = traj_xy[indice_atual]
+    ponto_proximo = traj_xy[min(indice_atual + 1, len(traj_xy) - 1)]
+    pos_atual = np.array([x, y])
+    dist_atual = np.linalg.norm(pos_atual - ponto_atual)
+
+    if dist_anterior is not None:
+        orient_ponto = calcular_orientacao(ponto_atual, ponto_proximo)
+        orient_robo = calcular_orientacao(pos_atual, ponto_proximo)
+        erro_orient = np.abs((orient_robo - orient_ponto + np.pi) % (2 * np.pi) - np.pi)
+        if (dist_atual > dist_anterior and erro_orient < np.pi / 2) or dist_atual < 0.6:
+            indice_atual += 1
+            dist_anterior = None
+            continue
+
+    dist_anterior = dist_atual
+    orient_desejada = calcular_orientacao(pos_atual, ponto_proximo)
+    delta_theta = (orient_desejada - theta + np.pi) % (2 * np.pi) - np.pi
+
+    # Converter erro de orientação em um ângulo de direção
+    angulo_direcional = np.clip(delta_theta, -angulo_max_rad, angulo_max_rad)
+
+    # Calcular omega físico com base no tipo de movimento
+    omega = calcular_omega_fisico(v, angulo_direcional, tipo_movimento)
+
+    # Atualizar posição
+    x += v * np.cos(theta) * dt
+    y += v * np.sin(theta) * dt
+    theta += omega * dt
+    traj_gerada.append((x, y))
+
+traj_gerada = np.array(traj_gerada)
+
+# Plotagem final
+plt.plot(traj_xy[:,0], traj_xy[:,1], 'ro--', label='Trajetória original')
+plt.plot(traj_gerada[:,0], traj_gerada[:,1], 'bo-', label=f'MPC físico ({tipo_movimento}, {angulo_max_graus}°)')
+plt.xlabel("X (m)")
+plt.ylabel("Y (m)")
+plt.title("Simulação MPC física com tipo de movimento real")
+plt.grid(True)
+plt.axis("equal")
+plt.legend()
+plt.show()

Python/mpc/test4.py

@@ -0,0 +1,103 @@
+# Simulação MPC com recalculagem do delta_theta a cada passo (versão mais próxima do real)
+import numpy as np
+import matplotlib.pyplot as plt
+import json
+
+# Carregar mapa
+with open("CidadeJardimTerreno2_Corrigido.json", "r", encoding="utf-8") as f:
+    data = json.load(f)
+
+traj_gps = []
+for feature in data["features"]:
+    coords = feature["geometry"]["coordinates"]
+    for lon, lat in coords:
+        traj_gps.append((lat, lon))
+
+def latlon_to_xy(lat, lon, lat0, lon0):
+    R = 6371000
+    dlat = np.radians(lat - lat0)
+    dlon = np.radians(lon - lon0)
+    x = R * dlon * np.cos(np.radians((lat + lat0) / 2))
+    y = R * dlat
+    return x, y
+
+lat0, lon0 = traj_gps[0]
+traj_xy = np.array([latlon_to_xy(lat, lon, lat0, lon0) for lat, lon in traj_gps])
+
+# Parâmetros do robô
+distancia_entre_eixos = 0.92
+angulo_max_graus = 30.0
+angulo_max_rad = np.radians(angulo_max_graus)
+dt = 0.2
+horizonte = 10
+
+def calcular_orientacao(p1, p2):
+    dx = p2[0] - p1[0]
+    dy = p2[1] - p1[1]
+    return np.arctan2(dy, dx)
+
+def calcular_omega_fisico(v, angulo_rad, tipo_movimento):
+    L = distancia_entre_eixos
+    tan_delta = np.tan(angulo_rad)
+    if tipo_movimento == "rodasFrontais":
+        return (v * tan_delta) / L
+    elif tipo_movimento == "movimentoArco":
+        return (2 * v * tan_delta) / L
+    return 0.0
+
+def simular_trajetoria_recalculando(x0, y0, theta0, velocidade, tipo_movimento, traj_xy):
+    x, y, theta = x0, y0, theta0
+    pontos = [(x, y)]
+    custo_total = 0.0
+
+    for step in range(horizonte):
+        idx = min(step, len(traj_xy)-1)
+        ponto_alvo = traj_xy[idx]
+        erro_dist = np.linalg.norm([x - ponto_alvo[0], y - ponto_alvo[1]])
+        orient_alvo = calcular_orientacao((x, y), ponto_alvo)
+        delta_theta = (orient_alvo - theta + np.pi) % (2 * np.pi) - np.pi
+
+        # Limitando o ângulo de direção
+        angulo_direcional = np.clip(delta_theta, -angulo_max_rad, angulo_max_rad)
+        omega = calcular_omega_fisico(velocidade, angulo_direcional, tipo_movimento)
+
+        # Atualização de estado
+        x += velocidade * np.cos(theta) * dt
+        y += velocidade * np.sin(theta) * dt
+        theta += omega * dt
+
+        pontos.append((x, y))
+        erro_orient = np.abs((orient_alvo - theta + np.pi) % (2 * np.pi) - np.pi)
+        custo_total += erro_dist * 2.0 + erro_orient * 1.5
+
+    return np.array(pontos), custo_total
+
+# Estado inicial
+x0, y0, theta0 = traj_xy[0][0] - 0.5, traj_xy[0][1] - 0.5, 0.0
+trajetorias_testadas = []
+custos = []
+
+# Candidatos com recalculagem a cada passo
+for vel in [0.6, 1.0, 1.4]:
+    for tipo in ["rodasFrontais", "movimentoArco"]:
+        traj, custo = simular_trajetoria_recalculando(x0, y0, theta0, vel, tipo, traj_xy)
+        trajetorias_testadas.append((traj, vel, tipo))
+        custos.append(custo)
+
+# Melhor resultado
+melhor_idx = int(np.argmin(custos))
+traj_melhor, vel_melhor, tipo_melhor = trajetorias_testadas[melhor_idx]
+
+# Plot
+for traj, vel, tipo in trajetorias_testadas:
+    plt.plot(traj[:,0], traj[:,1], alpha=0.2, label=f"{vel} m/s | {tipo}")
+
+plt.plot(traj_melhor[:,0], traj_melhor[:,1], 'b-', linewidth=2.5, label=f"Melhor: {vel_melhor} m/s | {tipo_melhor}")
+plt.plot(traj_xy[:,0], traj_xy[:,1], 'ro--', label='Trajetória alvo')
+plt.xlabel("X (m)")
+plt.ylabel("Y (m)")
+plt.title("MPC com recalculagem por passo (delta_theta dinâmico)")
+plt.legend(loc="best", fontsize="small")
+plt.grid(True)
+plt.axis("equal")
+plt.show()

Python/mpc/test5.py

@@ -0,0 +1,116 @@
+# MPC híbrido com progressão inteligente: avança na trajetória conforme distância e orientação
+import numpy as np
+import matplotlib.pyplot as plt
+import json
+
+# Carrega o mapa
+with open("CidadeJardimTerreno2_Corrigido.json", "r", encoding="utf-8") as f:
+    data = json.load(f)
+
+traj_gps = []
+for feature in data["features"]:
+    coords = feature["geometry"]["coordinates"]
+    for lon, lat in coords:
+        traj_gps.append((lat, lon))
+
+def latlon_to_xy(lat, lon, lat0, lon0):
+    R = 6371000
+    dlat = np.radians(lat - lat0)
+    dlon = np.radians(lon - lon0)
+    x = R * dlon * np.cos(np.radians((lat + lat0) / 2))
+    y = R * dlat
+    return x, y
+
+lat0, lon0 = traj_gps[0]
+traj_xy = np.array([latlon_to_xy(lat, lon, lat0, lon0) for lat, lon in traj_gps])
+
+# Parâmetros
+distancia_entre_eixos = 0.92
+angulo_max_graus = 30.0
+angulo_max_rad = np.radians(angulo_max_graus)
+dt = 0.2
+horizonte = 150
+velocidade_param = 1.0
+
+def calcular_orientacao(p1, p2):
+    dx = p2[0] - p1[0]
+    dy = p2[1] - p1[1]
+    return np.arctan2(dy, dx)
+
+def calcular_omega_fisico(v, angulo_rad, tipo_movimento):
+    L = distancia_entre_eixos
+    tan_delta = np.tan(angulo_rad)
+    if tipo_movimento == "rodasFrontais":
+        return (v * tan_delta) / L
+    elif tipo_movimento == "movimentoArco":
+        return (2 * v * tan_delta) / L
+    return 0.0
+
+def simular_trajetoria_hibrida_com_progresso(x0, y0, theta0, v, traj_xy):
+    x, y, theta = x0, y0, theta0
+    pontos = [(x, y)]
+    custo_total = 0.0
+    tipos_usados = []
+    idx_atual = 0
+    dist_anterior = None
+
+    for step in range(horizonte):
+        if idx_atual >= len(traj_xy) - 1:
+            break
+
+        ponto_alvo = traj_xy[idx_atual]
+        orient_alvo = calcular_orientacao((x, y), ponto_alvo)
+        delta_theta = (orient_alvo - theta + np.pi) % (2 * np.pi) - np.pi
+        angulo_direcional = np.clip(delta_theta, -angulo_max_rad, angulo_max_rad)
+
+        melhor_tipo = None
+        melhor_estado = None
+        menor_custo = float('inf')
+
+        for tipo in ["rodasFrontais", "movimentoArco"]:
+            omega = calcular_omega_fisico(v, angulo_direcional, tipo)
+            x_sim = x + v * np.cos(theta) * dt
+            y_sim = y + v * np.sin(theta) * dt
+            theta_sim = theta + omega * dt
+
+            erro_dist = np.linalg.norm([x_sim - ponto_alvo[0], y_sim - ponto_alvo[1]])
+            orient_sim = calcular_orientacao((x_sim, y_sim), ponto_alvo)
+            erro_orient = np.abs((orient_sim - theta_sim + np.pi) % (2 * np.pi) - np.pi)
+            custo = erro_dist * 2.0 + erro_orient * 1.5
+
+            if custo < menor_custo:
+                menor_custo = custo
+                melhor_tipo = tipo
+                melhor_estado = (x_sim, y_sim, theta_sim)
+
+        x, y, theta = melhor_estado
+        pontos.append((x, y))
+        tipos_usados.append(melhor_tipo)
+        custo_total += menor_custo
+
+        # Lógica de progressão do índice
+        dist_atual = np.linalg.norm([x - ponto_alvo[0], y - ponto_alvo[1]])
+        if dist_anterior is not None:
+            orient_diff = np.abs((orient_alvo - theta + np.pi) % (2 * np.pi) - np.pi)
+            if (dist_atual > dist_anterior and orient_diff < np.pi / 2) or dist_atual < 0.6:
+                idx_atual += 1
+                dist_anterior = None
+                continue
+        dist_anterior = dist_atual
+
+    return np.array(pontos), custo_total, tipos_usados
+
+# Simular com progressão
+x0, y0, theta0 = traj_xy[0][0] - 0.5, traj_xy[0][1] - 0.5, 0.0
+traj_melhor, custo_final, tipos_melhor = simular_trajetoria_hibrida_com_progresso(x0, y0, theta0, velocidade_param, traj_xy)
+
+# Plot
+plt.plot(traj_melhor[:,0], traj_melhor[:,1], 'b-', linewidth=2.5, label='MPC híbrido com progressão')
+plt.plot(traj_xy[:,0], traj_xy[:,1], 'ro--', label='Trajetória alvo')
+plt.xlabel("X (m)")
+plt.ylabel("Y (m)")
+plt.title(f"MPC híbrido com progressão dinâmica (vel {velocidade_param} m/s)")
+plt.grid(True)
+plt.axis("equal")
+plt.legend()
+plt.show()

Python/mpc/test6.py

@@ -0,0 +1,123 @@
+# MPC híbrido com controle de pontos visitados via limiar de distância
+import numpy as np
+import matplotlib.pyplot as plt
+import json
+
+# Carrega o mapa
+with open("CidadeJardimTerreno2_Corrigido.json", "r", encoding="utf-8") as f:
+    data = json.load(f)
+
+traj_gps = []
+for feature in data["features"]:
+    coords = feature["geometry"]["coordinates"]
+    for lon, lat in coords:
+        traj_gps.append((lat, lon))
+
+def latlon_to_xy(lat, lon, lat0, lon0):
+    R = 6371000
+    dlat = np.radians(lat - lat0)
+    dlon = np.radians(lon - lon0)
+    x = R * dlon * np.cos(np.radians((lat + lat0) / 2))
+    y = R * dlat
+    return x, y
+
+lat0, lon0 = traj_gps[0]
+traj_xy = np.array([latlon_to_xy(lat, lon, lat0, lon0) for lat, lon in traj_gps])
+
+# Parâmetros
+distancia_entre_eixos = 0.92
+angulo_max_graus = 30.0
+angulo_max_rad = np.radians(angulo_max_graus)
+dt = 0.2
+horizonte = 250
+velocidade_param = 1.0
+limiar_entrada = 0.7  # distância para considerar ponto como visitado
+
+def calcular_orientacao(p1, p2):
+    dx = p2[0] - p1[0]
+    dy = p2[1] - p1[1]
+    return np.arctan2(dy, dx)
+
+def calcular_omega_fisico(v, angulo_rad, tipo_movimento):
+    L = distancia_entre_eixos
+    tan_delta = np.tan(angulo_rad)
+    if tipo_movimento == "rodasFrontais":
+        return (v * tan_delta) / L
+    elif tipo_movimento == "movimentoArco":
+        return (2 * v * tan_delta) / L
+    return 0.0
+
+def proximo_nao_visitado(visitados):
+    for i, v in enumerate(visitados):
+        if not v:
+            return i
+    return len(visitados) - 1  # fallback para o último ponto
+
+def simular_trajetoria_com_limiar(x0, y0, theta0, v, traj_xy):
+    x, y, theta = x0, y0, theta0
+    pontos = [(x, y)]
+    custo_total = 0.0
+    tipos_usados = []
+    visitados = [False] * len(traj_xy)
+
+    for step in range(horizonte):
+        idx_alvo = proximo_nao_visitado(visitados)
+        if idx_alvo >= len(traj_xy):
+            break
+
+        ponto_alvo = traj_xy[idx_alvo]
+        pos_atual = np.array([x, y])
+
+        # Verifica se o ponto foi alcançado
+        if np.linalg.norm(pos_atual - ponto_alvo) < limiar_entrada:
+            visitados[idx_alvo] = True
+            idx_alvo = proximo_nao_visitado(visitados)
+            if idx_alvo >= len(traj_xy):
+                break
+            ponto_alvo = traj_xy[idx_alvo]
+
+        orient_alvo = calcular_orientacao(pos_atual, ponto_alvo)
+        delta_theta = (orient_alvo - theta + np.pi) % (2 * np.pi) - np.pi
+        angulo_direcional = np.clip(delta_theta, -angulo_max_rad, angulo_max_rad)
+
+        melhor_tipo = None
+        melhor_estado = None
+        menor_custo = float('inf')
+
+        for tipo in ["rodasFrontais", "movimentoArco"]:
+            omega = calcular_omega_fisico(v, angulo_direcional, tipo)
+            x_sim = x + v * np.cos(theta) * dt
+            y_sim = y + v * np.sin(theta) * dt
+            theta_sim = theta + omega * dt
+
+            erro_dist = np.linalg.norm([x_sim - ponto_alvo[0], y_sim - ponto_alvo[1]])
+            orient_sim = calcular_orientacao((x_sim, y_sim), ponto_alvo)
+            erro_orient = np.abs((orient_sim - theta_sim + np.pi) % (2 * np.pi) - np.pi)
+            custo = erro_dist * 2.0 + erro_orient * 1.5
+
+            if custo < menor_custo:
+                menor_custo = custo
+                melhor_tipo = tipo
+                melhor_estado = (x_sim, y_sim, theta_sim)
+
+        x, y, theta = melhor_estado
+        pontos.append((x, y))
+        tipos_usados.append(melhor_tipo)
+        custo_total += menor_custo
+
+    return np.array(pontos), custo_total, tipos_usados
+
+# Simular com lógica de ponto visitado
+x0, y0, theta0 = traj_xy[0][0] - 0.5, traj_xy[0][1] - 0.5, 0.0
+traj_melhor, custo_final, tipos_melhor = simular_trajetoria_com_limiar(x0, y0, theta0, velocidade_param, traj_xy)
+
+# Plot
+plt.plot(traj_melhor[:,0], traj_melhor[:,1], 'b-', linewidth=2.5, label='MPC com visitação')
+plt.plot(traj_xy[:,0], traj_xy[:,1], 'ro--', label='Trajetória alvo')
+plt.xlabel("X (m)")
+plt.ylabel("Y (m)")
+plt.title(f"MPC com visitação de pontos (vel {velocidade_param} m/s)")
+plt.grid(True)
+plt.axis("equal")
+plt.legend()
+plt.show()

Python/mpc/test7.py

@@ -0,0 +1,242 @@
+# MPC com MQTT, simulação e visualização da trajetória
+import numpy as np
+import paho.mqtt.client as mqtt
+import json
+import matplotlib.pyplot as plt
+
+# Parâmetros do MPC
+angulo_max_graus = 30.0
+distancia_entre_eixos = 0.92
+limiar_entrada = 0.7
+horizonte = 20
+
+# Estado compartilhado
+trajetoria_latlon = []
+trajetoria_xy = []
+visitados_execucao = []
+lat0, lon0 = None, None
+
+# Visualização
+plt.ion()
+fig, ax = plt.subplots()
+
+# Funções matemáticas auxiliares
+def latlon_to_xy(lat, lon, lat0, lon0):
+    R = 6371000
+    dlat = np.radians(lat - lat0)
+    dlon = np.radians(lon - lon0)
+    x = R * dlon * np.cos(np.radians((lat + lat0) / 2))
+    y = R * dlat
+    return x, y
+
+def normalizar_angulo(angulo):
+    if angulo < 0:
+        angulo += 360
+    angulo %= 360
+    return angulo
+        
+def calcular_orientacao(p1, p2):
+    dx = p2[0] - p1[0]
+    dy = p2[1] - p1[1]
+    orient = np.arctan2(dy, dx)
+    orient -= np.radians(90)  # Corrige a base angular para bater com o seu theta ajustado
+    return (orient + np.pi) % (2 * np.pi) - np.pi  # Normaliza entre [-pi, pi]
+
+def calcular_orientacao_gps(p1_lat, p1_lon, p2_lat, p2_lon):
+    lat1 = np.radians(p1_lat)
+    lon1 = np.radians(p1_lon)
+    lat2 = np.radians(p2_lat)
+    lon2 = np.radians(p2_lon)
+
+    delta_lon = lon2 - lon1
+
+    y = np.sin(delta_lon) * np.cos(lat2)
+    x = np.cos(lat1) * np.sin(lat2) - np.sin(lat1) * np.cos(lat2) * np.cos(delta_lon)
+
+    theta = np.arctan2(y, x)
+    bearing = (np.degrees(theta) + 360) % 360
+
+    return bearing
+
+def calcular_omega(v, angulo_rad, tipo):
+    tan_delta = np.tan(angulo_rad)
+    if tipo == "rodasFrontais":
+        return (v * tan_delta) / distancia_entre_eixos
+    elif tipo == "movimentoArco":
+        return (2 * v * tan_delta) / distancia_entre_eixos
+    return 0.0
+
+def corrigir_pontos_visitados(x, y, limite_max_avanço=5):
+    global visitados_execucao
+
+    for idx, ponto in enumerate(trajetoria_xy):
+        if visitados_execucao[idx]:
+            continue
+
+        dist = np.linalg.norm([x - ponto[0], y - ponto[1]])
+
+        # Dentro do limiar: marca esse e os anteriores como visitados
+        if dist < limiar_entrada:
+            for i in range(idx + 1):
+                visitados_execucao[i] = True
+            return idx
+
+        # Se já passou do limite de avanço, para
+        if idx > 0 and not visitados_execucao[idx - 1] and idx >= limite_max_avanço:
+            break
+
+    # Se não marcou nenhum ponto, volta o próximo não visitado
+    return proximo_nao_visitado(visitados_execucao)
+
+def proximo_nao_visitado(visitados):
+    for i, v in enumerate(visitados):
+        if not v:
+            return i
+    return len(visitados) - 1
+
+def calcular_melhor_candidato(x, y, theta, visitados_sim):
+    idx_alvo = corrigir_pontos_visitados(x, y)
+    if idx_alvo >= len(trajetoria_xy):
+        return 0.0, "rodasFrontais"
+
+    ponto_alvo = trajetoria_xy[idx_alvo]
+
+    # Verifica se já alcançou o ponto
+    if np.linalg.norm([x - ponto_alvo[0], y - ponto_alvo[1]]) < limiar_entrada:
+        visitados_sim[idx_alvo] = True
+        idx_alvo = corrigir_pontos_visitados(x, y)
+        if idx_alvo >= len(trajetoria_xy):
+            return 0.0, "rodasFrontais"
+        ponto_alvo = trajetoria_xy[idx_alvo]
+
+    orient = calcular_orientacao((x, y), ponto_alvo)
+    erro_ori = abs((orient - theta + np.pi) % (2 * np.pi) - np.pi)
+    #delta_theta = (orient - theta + np.pi) % (2 * np.pi) - np.pi
+    delta_theta = ((-orient) - theta + np.pi) % (2 * np.pi) - np.pi
+    angulo = np.clip(delta_theta, -np.radians(angulo_max_graus), np.radians(angulo_max_graus))
+
+    # ✅ Se o erro for grande, prioriza curvas fechadas
+    if erro_ori > np.radians(10):
+        tipos_validos = ["movimentoArco"]
+    else:
+        tipos_validos = ["rodasFrontais", "movimentoArco"]
+
+    melhor_custo = float('inf')
+    melhor_tipo = "rodasFrontais"
+    melhor_angulo = angulo
+
+    for tipo in tipos_validos:
+        omega = calcular_omega(1.0, angulo, tipo)
+        x_sim = x + 1.0 * np.cos(theta) * 0.2
+        y_sim = y + 1.0 * np.sin(theta) * 0.2
+        theta_sim = theta + omega * 0.2
+
+        erro_pos = np.linalg.norm([x_sim - ponto_alvo[0], y_sim - ponto_alvo[1]])
+        orient_sim = calcular_orientacao((x_sim, y_sim), ponto_alvo)
+        erro_ori = np.abs((orient_sim - theta_sim + np.pi) % (2 * np.pi) - np.pi)
+
+        vetor_alvo = np.array(ponto_alvo) - np.array([x_sim, y_sim])
+        vetor_alvo_norm = vetor_alvo / np.linalg.norm(vetor_alvo)
+        vetor_movel = np.array([np.cos(theta_sim), np.sin(theta_sim)])
+        cos_angulo = np.dot(vetor_alvo_norm, vetor_movel)
+
+        # Se o ângulo for > 90° → cosseno < 0 → indo no sentido oposto
+        penalidade_orientacao = 9999 if cos_angulo < 0 else 0
+
+        custo = erro_pos * 2.0 + erro_ori * 1.5 + penalidade_orientacao
+
+        if custo < melhor_custo:
+            melhor_custo = custo
+            melhor_tipo = tipo
+            melhor_angulo = angulo
+
+    return melhor_angulo, melhor_tipo
+
+def processar_mpc(pos_lat, pos_lon, theta, velocidade, dt, angulo_controle_atual):
+    print(f"Posicao recebida: Velocidade={velocidade}, dt={dt}, Orientacao={np.degrees(theta)}")
+    global trajetoria_xy, visitados_execucao, lat0, lon0
+    if not trajetoria_xy:
+        return None
+
+    x, y = latlon_to_xy(pos_lat, pos_lon, lat0, lon0)
+
+    visitados_sim = visitados_execucao.copy()
+    sim = [(x, y)]
+    x_temp, y_temp, theta_temp = x, y, theta
+    for _ in range(horizonte):
+        angulo_mpc, tipo_mpc = calcular_melhor_candidato(x_temp, y_temp, theta_temp, visitados_sim)
+        omega = calcular_omega(velocidade, angulo_mpc, tipo_mpc)
+
+        # Corrige o ângulo apenas para visualização
+        theta_plot = theta_temp + np.radians(90)
+
+        # Avança a simulação com o ângulo corrigido para o traço
+        x_temp += velocidade * np.cos(theta_plot) * dt
+        y_temp += velocidade * np.sin(theta_plot) * dt
+        theta_temp += omega * dt
+
+        sim.append((x_temp, y_temp))
+
+    # Atualiza visualização
+    ax.clear()
+    if trajetoria_xy:
+        tx, ty = zip(*trajetoria_xy)
+        ax.plot(tx, ty, 'r.-', label="Trajetória alvo")
+    if sim:
+        sx, sy = zip(*sim)
+        ax.plot(sx, sy, 'g:', label="Previsão MPC")
+    ax.plot(sim[0][0], sim[0][1], 'bo', label="Posição atual")
+    ax.set_title("Visualização MPC (tempo real)")
+    ax.set_xlabel("X (m)")
+    ax.set_ylabel("Y (m)")
+    ax.axis("equal")
+    ax.legend()
+    plt.pause(0.001)
+
+    angulo_final, tipo_final = calcular_melhor_candidato(sim[0][0], sim[0][1], theta, visitados_execucao)
+    #angulo_final = angulo_final * -1
+    print(f"angulo: {np.degrees(angulo_final)}, tipo: {tipo_final}")
+    return {
+        "angulo": np.degrees(angulo_final),
+        "tipo": 0 if tipo_final == "rodasFrontais" else 3,
+        "velocidade": velocidade,
+        "dt": dt,
+        "orientacao": np.degrees(theta)
+    }
+
+# MQTT setup
+def on_connect(client, userdata, flags, rc):
+    print("Conectado ao MQTT")
+    client.subscribe("mpc/rota")
+    client.subscribe("mpc/posicao")
+
+def on_message(client, userdata, msg):
+    global trajetoria_latlon, trajetoria_xy, visitados_execucao, lat0, lon0
+    if msg.topic == "mpc/rota":
+        dados = json.loads(msg.payload.decode())
+        trajetoria_latlon = dados["pontos"]
+        lat0, lon0 = trajetoria_latlon[0][0], trajetoria_latlon[0][1]
+        trajetoria_xy = [latlon_to_xy(lat, lon, lat0, lon0) for lat, lon in trajetoria_latlon]
+        visitados_execucao = [False] * len(trajetoria_xy)
+        print(f"Trajetória recebida com {len(trajetoria_xy)} pontos")
+
+    elif msg.topic == "mpc/posicao":
+        dados = json.loads(msg.payload.decode())
+        lat = dados["lat"]
+        lon = dados["lon"]
+        #angulo = normalizar_angulo(dados["theta"] - 90)
+        theta = np.radians(dados["theta"])
+        velocidade = dados["velocidade"]
+        dt = dados["dt"]
+        angulo_controle = dados.get("anguloControle", 0.0)
+        comando = processar_mpc(lat, lon, theta, velocidade, dt, angulo_controle)
+        if comando:
+            client.publish("mpc/comando", json.dumps(comando))
+
+client = mqtt.Client()
+client.on_connect = on_connect
+client.on_message = on_message
+client.connect("localhost", 1883, 60)
+
+print("Aguardando trajetória e posição...")
+client.loop_forever()

Python/mpc/test7_mpc_tipo_ponto.py

@@ -0,0 +1,215 @@
+# MPC com MQTT, simulação e visualização da trajetória
+import numpy as np
+import paho.mqtt.client as mqtt
+import json
+import matplotlib.pyplot as plt
+
+# Parâmetros do MPC
+angulo_max_graus = 30.0
+distancia_entre_eixos = 0.92
+horizonte = 20
+
+# Estado compartilhado
+trajetoria_latlon = []
+pontos_info = []
+visitados_execucao = []
+lat0, lon0 = None, None
+
+# Visualização
+plt.ion()
+fig, ax = plt.subplots()
+
+# Funções matemáticas auxiliares
+def latlon_to_xy(lat, lon, lat0, lon0):
+    R = 6371000
+    dlat = np.radians(lat - lat0)
+    dlon = np.radians(lon - lon0)
+    x = R * dlon * np.cos(np.radians((lat + lat0) / 2))
+    y = R * dlat
+    return x, y
+
+def calcular_orientacao(p1, p2):
+    dx = p2[0] - p1[0]
+    dy = p2[1] - p1[1]
+    orient = np.arctan2(dy, dx) - np.radians(90)
+    return (orient + np.pi) % (2 * np.pi) - np.pi
+
+def calcular_omega(v, angulo_rad, tipo):
+    tan_delta = np.tan(angulo_rad)
+    if tipo == "rodasFrontais":
+        return (v * tan_delta) / distancia_entre_eixos
+    elif tipo == "movimentoArco":
+        return (2 * v * tan_delta) / distancia_entre_eixos
+    return 0.0
+
+def proximo_nao_visitado(visitados):
+    for i, v in enumerate(visitados):
+        if not v:
+            return i
+    return len(visitados) - 1
+
+def corrigir_pontos_visitados(x, y, pontos_visitados, limite_max_avanço=5):
+    for idx, ponto in enumerate(pontos_info):
+        if pontos_visitados[idx]:
+            continue
+        pos = ponto["xy"]
+        margem = ponto.get("distanciaMargem", 0.7)
+        dist = np.linalg.norm([x - pos[0], y - pos[1]])
+        if dist < margem:
+            for i in range(idx + 1):
+                pontos_visitados[i] = True
+            return idx
+        if idx > 0 and not pontos_visitados[idx - 1] and idx >= limite_max_avanço:
+            break
+    return proximo_nao_visitado(pontos_visitados)
+
+def calcular_melhor_candidato(x, y, theta, visitados_sim):
+    idx_alvo = corrigir_pontos_visitados(x, y, visitados_sim)
+    if idx_alvo >= len(pontos_info):
+        return 0.0, "movimentoArco"
+
+    ponto_info = pontos_info[idx_alvo]
+    tipo_ponto = ponto_info.get("tipo", 3)
+    ponto_alvo = ponto_info["xy"]
+
+    if np.linalg.norm([x - ponto_alvo[0], y - ponto_alvo[1]]) < ponto_info.get("distanciaMargem", 0.7):
+        visitados_sim[idx_alvo] = True
+        idx_alvo = corrigir_pontos_visitados(x, y, visitados_sim)
+        if idx_alvo >= len(pontos_info):
+            return 0.0, "movimentoArco"
+        ponto_info = pontos_info[idx_alvo]
+        tipo_ponto = ponto_info.get("tipo", 3)
+        ponto_alvo = ponto_info["xy"]
+
+    orient = calcular_orientacao((x, y), ponto_alvo)
+    erro_ori = abs((orient - theta + np.pi) % (2 * np.pi) - np.pi)
+    delta_theta = ((-orient) - theta + np.pi) % (2 * np.pi) - np.pi
+    angulo = np.clip(delta_theta, -np.radians(angulo_max_graus), np.radians(angulo_max_graus))
+
+    if erro_ori > np.radians(10):
+        tipos_validos = ["movimentoArco"]
+    elif tipo_ponto == 3:
+        tipos_validos = ["rodasFrontais", "movimentoArco"]
+    else:
+        tipos_validos = ["movimentoArco"]
+
+    melhor_custo = float('inf')
+    melhor_tipo = "movimentoArco"
+    melhor_angulo = angulo
+
+    for tipo in tipos_validos:
+        omega = calcular_omega(1.0, angulo, tipo)
+        x_sim = x + 1.0 * np.cos(theta) * 0.2
+        y_sim = y + 1.0 * np.sin(theta) * 0.2
+        theta_sim = theta + omega * 0.2
+
+        erro_pos = np.linalg.norm([x_sim - ponto_alvo[0], y_sim - ponto_alvo[1]])
+        orient_sim = calcular_orientacao((x_sim, y_sim), ponto_alvo)
+        erro_ori = abs((orient_sim - theta_sim + np.pi) % (2 * np.pi) - np.pi)
+
+        vetor_alvo = np.array(ponto_alvo) - np.array([x_sim, y_sim])
+        vetor_alvo_norm = vetor_alvo / np.linalg.norm(vetor_alvo)
+        vetor_movel = np.array([np.cos(theta_sim), np.sin(theta_sim)])
+        cos_angulo = np.dot(vetor_alvo_norm, vetor_movel)
+        penalidade_orientacao = 9999 if cos_angulo < 0 else 0
+
+        custo = erro_pos * 2.0 + erro_ori * 1.5 + penalidade_orientacao
+
+        if custo < melhor_custo:
+            melhor_custo = custo
+            melhor_tipo = tipo
+            melhor_angulo = angulo
+
+    return melhor_angulo, melhor_tipo
+
+def processar_mpc(pos_lat, pos_lon, theta, velocidade, dt, angulo_controle_atual):
+    print(f"Posicao recebida: Velocidade={velocidade}, dt={dt}, Orientacao={np.degrees(theta)}")
+    global pontos_info, visitados_execucao, lat0, lon0
+    if not pontos_info:
+        return None
+
+    x, y = latlon_to_xy(pos_lat, pos_lon, lat0, lon0)
+    visitados_sim = visitados_execucao.copy()
+
+    sim = [(x, y)]
+    x_temp, y_temp, theta_temp = x, y, theta
+    for _ in range(horizonte):
+        angulo_mpc, tipo_mpc = calcular_melhor_candidato(x_temp, y_temp, theta_temp, visitados_sim)
+        omega = calcular_omega(velocidade, angulo_mpc, tipo_mpc)
+        theta_plot = (-theta_temp) + np.radians(90)
+        x_temp += velocidade * np.cos(theta_plot) * dt
+        y_temp += velocidade * np.sin(theta_plot) * dt
+        theta_temp += omega * dt
+        sim.append((x_temp, y_temp))
+
+    ax.clear()
+    if pontos_info:
+        tx, ty = zip(*[p["xy"] for p in pontos_info])
+        ax.plot(tx, ty, 'r.-', label="Trajetória alvo")
+    if sim:
+        sx, sy = zip(*sim)
+        ax.plot(sx, sy, 'g:', label="Previsão MPC")
+    ax.plot(sim[0][0], sim[0][1], 'bo', label="Posição atual")
+    ax.set_title("Visualização MPC (tempo real)")
+    ax.set_xlabel("X (m)")
+    ax.set_ylabel("Y (m)")
+    ax.axis("equal")
+    ax.legend()
+    plt.pause(0.001)
+
+    angulo_final, tipo_final = calcular_melhor_candidato(sim[0][0], sim[0][1], theta, visitados_execucao)
+    print(f"angulo: {np.degrees(angulo_final)}, tipo: {tipo_final}")
+    return {
+        "angulo": np.degrees(angulo_final),
+        "tipo": 0 if tipo_final == "rodasFrontais" else 3,
+        "velocidade": velocidade,
+        "dt": dt,
+        "orientacao": np.degrees(theta)
+    }
+
+def on_connect(client, userdata, flags, rc):
+    print("Conectado ao MQTT")
+    client.subscribe("mpc/rota")
+    client.subscribe("mpc/posicao")
+    client.publish("mpc/comando", "OK")
+
+def on_message(client, userdata, msg):
+    global trajetoria_latlon, pontos_info, visitados_execucao, lat0, lon0, angulo_max_graus, distancia_entre_eixos, horizonte
+    if msg.topic == "mpc/rota":
+        dados = json.loads(msg.payload.decode())
+        trajetoria_latlon = dados["pontos"]
+        angulo_max_graus = dados["angulo_max_graus"]
+        distancia_entre_eixos = dados["distancia_entre_eixos"]
+        horizonte = dados["horizonte"]
+        lat0, lon0 = trajetoria_latlon[0]["lat"], trajetoria_latlon[0]["lon"]
+        pontos_info = []
+        for p in trajetoria_latlon:
+            x, y = latlon_to_xy(p["lat"], p["lon"], lat0, lon0)
+            ponto = {
+                "xy": (x, y),
+                "tipo": p.get("tipo", 3),
+                "distanciaMargem": p.get("distanciaMargem", 0.7)
+            }
+            pontos_info.append(ponto)
+        visitados_execucao = [False] * len(pontos_info)
+        print(f"Trajetória recebida com {len(pontos_info)} pontos")
+
+    elif msg.topic == "mpc/posicao":
+        dados = json.loads(msg.payload.decode())
+        lat = dados["lat"]
+        lon = dados["lon"]
+        theta = np.radians(dados["theta"])
+        velocidade = dados["velocidade"]
+        dt = dados["dt"]
+        angulo_controle = dados.get("anguloControle", 0.0)
+        comando = processar_mpc(lat, lon, theta, velocidade, dt, angulo_controle)
+        if comando:
+            client.publish("mpc/comando", json.dumps(comando))
+
+client = mqtt.Client()
+client.on_connect = on_connect
+client.on_message = on_message
+client.connect("localhost", 1883, 60)
+
+print("Aguardando trajetória e posição...")
+client.loop_forever()

Python/mpc/test8.py

@@ -0,0 +1,146 @@
+# Simulador interativo de MPC com separação de visitados (execução vs simulação)
+import numpy as np
+import matplotlib.pyplot as plt
+import json
+
+# Carrega o mapa a partir de pontos.json
+with open("pontos.json", "r", encoding="utf-8") as f:
+    data = json.load(f)
+
+traj_gps = data["pontos"]
+
+def latlon_to_xy(lat, lon, lat0, lon0):
+    R = 6371000
+    dlat = np.radians(lat - lat0)
+    dlon = np.radians(lon - lon0)
+    x = R * dlon * np.cos(np.radians((lat + lat0) / 2))
+    y = R * dlat
+    return x, y
+
+def calcular_orientacao(p1, p2):
+    dx = p2[0] - p1[0]
+    dy = p2[1] - p1[1]
+    return np.arctan2(dy, dx)
+
+def calcular_omega(v, angulo_rad, tipo):
+    tan_delta = np.tan(angulo_rad)
+    if tipo == "rodasFrontais":
+        return (v * tan_delta) / distancia_entre_eixos
+    elif tipo == "movimentoArco":
+        return (2 * v * tan_delta) / distancia_entre_eixos
+    return 0.0
+
+lat0, lon0 = traj_gps[0]
+traj_xy = np.array([latlon_to_xy(lat, lon, lat0, lon0) for lat, lon in traj_gps])
+
+# Parâmetros físicos
+distancia_entre_eixos = 0.92
+angulo_max_graus = 30.0
+angulo_max_rad = np.radians(angulo_max_graus)
+dt = 0.2
+velocidade = 1.0
+limiar_entrada = 0.7
+horizonte = 20
+
+# Estado atual do robô
+x, y = traj_xy[0][0] - 0.5, traj_xy[0][1] - 0.5
+theta = 0.0
+trajetoria_percorrida = [(x, y)]
+visitados_execucao = [False] * len(traj_xy)
+
+def proximo_nao_visitado(visitados):
+    for i, v in enumerate(visitados):
+        if not v:
+            return i
+    return len(visitados) - 1
+
+def calcular_melhor_candidato(x, y, theta, visitados):
+    visitados_sim = visitados.copy()
+    idx_alvo = proximo_nao_visitado(visitados_sim)
+    if idx_alvo >= len(traj_xy):
+        return 0.0, "rodasFrontais"
+
+    ponto_alvo = traj_xy[idx_alvo]
+    if np.linalg.norm([x - ponto_alvo[0], y - ponto_alvo[1]]) < limiar_entrada:
+        visitados_sim[idx_alvo] = True
+        idx_alvo = proximo_nao_visitado(visitados_sim)
+        if idx_alvo >= len(traj_xy):
+            return 0.0, "rodasFrontais"
+        ponto_alvo = traj_xy[idx_alvo]
+
+    orient = calcular_orientacao((x, y), ponto_alvo)
+    delta_theta = (orient - theta + np.pi) % (2 * np.pi) - np.pi
+    angulo = np.clip(delta_theta, -angulo_max_rad, angulo_max_rad)
+
+    melhor_custo = float('inf')
+    melhor_tipo = "rodasFrontais"
+    melhor_angulo = angulo
+
+    for tipo in ["rodasFrontais", "movimentoArco"]:
+        omega = calcular_omega(velocidade, angulo, tipo)
+        x_sim = x + velocidade * np.cos(theta) * dt
+        y_sim = y + velocidade * np.sin(theta) * dt
+        theta_sim = theta + omega * dt
+        erro = np.linalg.norm([x_sim - ponto_alvo[0], y_sim - ponto_alvo[1]])
+        orient_sim = calcular_orientacao((x_sim, y_sim), ponto_alvo)
+        erro_ori = np.abs((orient_sim - theta_sim + np.pi) % (2 * np.pi) - np.pi)
+        custo = erro * 2.0 + erro_ori * 1.5
+
+        if custo < melhor_custo:
+            melhor_custo = custo
+            melhor_tipo = tipo
+            melhor_angulo = angulo
+
+    return melhor_angulo, melhor_tipo
+
+def simular_dinamico(x, y, theta, visitados):
+    sim = [(x, y)]
+    visitados_sim = visitados.copy()
+    for _ in range(horizonte):
+        angulo_mpc, tipo_mpc = calcular_melhor_candidato(x, y, theta, visitados_sim)
+        omega = calcular_omega(velocidade, angulo_mpc, tipo_mpc)
+        x += velocidade * np.cos(theta) * dt
+        y += velocidade * np.sin(theta) * dt
+        theta += omega * dt
+        sim.append((x, y))
+    return np.array(sim)
+
+fig, ax = plt.subplots()
+plt.ion()
+
+while True:
+    angulo_mpc, tipo_mpc = calcular_melhor_candidato(x, y, theta, visitados_execucao)
+    caminho = simular_dinamico(x, y, theta, visitados_execucao)
+
+    ax.clear()
+    ax.plot(traj_xy[:,0], traj_xy[:,1], 'ro--', label='Trajetória alvo')
+    ax.plot(*zip(*trajetoria_percorrida), 'b-', label='Percurso real')
+    ax.plot(caminho[:,0], caminho[:,1], 'g:', label='Previsão MPC')
+    ax.set_title("MPC passo a passo (dinâmico por passo)")
+    ax.set_xlabel("X (m)")
+    ax.set_ylabel("Y (m)")
+    ax.axis("equal")
+    ax.legend()
+    plt.pause(0.01)
+
+    print(f"\n>>> Comando MPC: tipo = {tipo_mpc}, ângulo = {np.degrees(angulo_mpc):.2f}°")
+    input("Pressione Enter para o próximo passo...")
+
+    idx_real = proximo_nao_visitado(visitados_execucao)
+    if idx_real < len(traj_xy):
+        ponto_real = traj_xy[idx_real]
+        if np.linalg.norm([x - ponto_real[0], y - ponto_real[1]]) < limiar_entrada:
+            visitados_execucao[idx_real] = True
+
+    omega = calcular_omega(velocidade, angulo_mpc, tipo_mpc)
+    x += velocidade * np.cos(theta) * dt
+    y += velocidade * np.sin(theta) * dt
+    theta += omega * dt
+    trajetoria_percorrida.append((x, y))
+
+    if proximo_nao_visitado(visitados_execucao) >= len(traj_xy):
+        print("Trajetória completa!")
+        break
+
+plt.ioff()
+plt.show()