ajustado algoritmo para movimentacao autonoma

AgroBase/AgroBase/AgroBase.csproj

@@ -451,6 +451,12 @@
     <Content Include="Python\Models\yolov7\utils\__init__.py">
       <CopyToOutputDirectory>PreserveNewest</CopyToOutputDirectory>
     </Content>
+    <Content Include="Python\Output\folium\images\position_marker.png">
+      <CopyToOutputDirectory>PreserveNewest</CopyToOutputDirectory>
+    </Content>
+    <Content Include="Python\Output\folium\leaflet.rotatedMarker.js">
+      <CopyToOutputDirectory>PreserveNewest</CopyToOutputDirectory>
+    </Content>
     <Content Include="Python\Output\folium\mqtt.min.js">
       <CopyToOutputDirectory>PreserveNewest</CopyToOutputDirectory>
     </Content>

AgroBase/AgroBase/Forms/Operacoes/frmOperacaoMapaGPS.cs

@@ -623,6 +623,12 @@ namespace AgroBase.Forms.Operacoes
 
         private void btnIniciarOperacao_Click(object sender, EventArgs e)
         {
+            if (!Variaveis.OperacaoEmAndamento.Mapa.RuasPercorrer.Any())
+            {
+                MessageBox.Show("Selecione as ruas para realizar a operação!");
+                return;
+            }
+
             if (btnIniciarOperacao.Text == "Finalizar Operação")
             {
                 Variaveis.OperacaoEmAndamento.FinalizarOperacao();

AgroBase/AgroBase/Forms/frmSimulacaoMapaGPS.Designer.cs

@@ -29,6 +29,7 @@
         private void InitializeComponent()
         {
             this.pnlOpcoes = new System.Windows.Forms.Panel();
+            this.btnAtualizarLeitura = new System.Windows.Forms.Button();
             this.lblDistanciaFinal = new System.Windows.Forms.Label();
             this.btnCarregarMapa = new System.Windows.Forms.Button();
             this.lblDistanciaOperacao = new System.Windows.Forms.Label();
@@ -46,6 +47,7 @@
             this.lblDirecao = new System.Windows.Forms.Label();
             this.lblStatus = new System.Windows.Forms.Label();
             this.gpbGPS = new System.Windows.Forms.GroupBox();
+            this.btnCalcularAngulo = new System.Windows.Forms.Button();
             this.btnAcrescentarGPS = new System.Windows.Forms.Button();
             this.lblDistancia = new System.Windows.Forms.Label();
             this.txtDistanciaGPS = new System.Windows.Forms.TextBox();
@@ -60,8 +62,7 @@
             this.pnlOrientacaoTrajeto = new System.Windows.Forms.Panel();
             this.pnlSonar = new System.Windows.Forms.Panel();
             this.picSonar = new System.Windows.Forms.PictureBox();
-            this.btnAtualizarLeitura = new System.Windows.Forms.Button();
+            this.lblDistanciaLateral = new System.Windows.Forms.Label();
-            this.btnCalcularAngulo = new System.Windows.Forms.Button();
             this.pnlOpcoes.SuspendLayout();
             this.gpbSonar.SuspendLayout();
             this.gpbGPS.SuspendLayout();
@@ -72,6 +73,7 @@
             // 
             // pnlOpcoes
             // 
+            this.pnlOpcoes.Controls.Add(this.lblDistanciaLateral);
             this.pnlOpcoes.Controls.Add(this.btnAtualizarLeitura);
             this.pnlOpcoes.Controls.Add(this.lblDistanciaFinal);
             this.pnlOpcoes.Controls.Add(this.btnCarregarMapa);
@@ -83,11 +85,22 @@
             this.pnlOpcoes.Controls.Add(this.gpbGPS);
             this.pnlOpcoes.Dock = System.Windows.Forms.DockStyle.Bottom;
             this.pnlOpcoes.Location = new System.Drawing.Point(0, 479);
-            this.pnlOpcoes.Margin = new System.Windows.Forms.Padding(2, 2, 2, 2);
+            this.pnlOpcoes.Margin = new System.Windows.Forms.Padding(2);
             this.pnlOpcoes.Name = "pnlOpcoes";
             this.pnlOpcoes.Size = new System.Drawing.Size(1028, 100);
             this.pnlOpcoes.TabIndex = 0;
             // 
+            // btnAtualizarLeitura
+            // 
+            this.btnAtualizarLeitura.Location = new System.Drawing.Point(134, 30);
+            this.btnAtualizarLeitura.Margin = new System.Windows.Forms.Padding(2);
+            this.btnAtualizarLeitura.Name = "btnAtualizarLeitura";
+            this.btnAtualizarLeitura.Size = new System.Drawing.Size(121, 20);
+            this.btnAtualizarLeitura.TabIndex = 22;
+            this.btnAtualizarLeitura.Text = "Atualizar leitura";
+            this.btnAtualizarLeitura.UseVisualStyleBackColor = true;
+            this.btnAtualizarLeitura.Click += new System.EventHandler(this.btnAtualizarLeitura_Click);
+            // 
             // lblDistanciaFinal
             // 
             this.lblDistanciaFinal.AutoSize = true;
@@ -102,7 +115,7 @@
             // btnCarregarMapa
             // 
             this.btnCarregarMapa.Location = new System.Drawing.Point(9, 6);
-            this.btnCarregarMapa.Margin = new System.Windows.Forms.Padding(2, 2, 2, 2);
+            this.btnCarregarMapa.Margin = new System.Windows.Forms.Padding(2);
             this.btnCarregarMapa.Name = "btnCarregarMapa";
             this.btnCarregarMapa.Size = new System.Drawing.Size(121, 19);
             this.btnCarregarMapa.TabIndex = 20;
@@ -124,7 +137,7 @@
             // btnIniciarSimulacao
             // 
             this.btnIniciarSimulacao.Location = new System.Drawing.Point(9, 30);
-            this.btnIniciarSimulacao.Margin = new System.Windows.Forms.Padding(2, 2, 2, 2);
+            this.btnIniciarSimulacao.Margin = new System.Windows.Forms.Padding(2);
             this.btnIniciarSimulacao.Name = "btnIniciarSimulacao";
             this.btnIniciarSimulacao.Size = new System.Drawing.Size(121, 20);
             this.btnIniciarSimulacao.TabIndex = 18;
@@ -144,9 +157,9 @@
             this.gpbSonar.Controls.Add(this.lblAnguloI);
             this.gpbSonar.Controls.Add(this.txtAnguloInicial);
             this.gpbSonar.Location = new System.Drawing.Point(785, 2);
-            this.gpbSonar.Margin = new System.Windows.Forms.Padding(2, 2, 2, 2);
+            this.gpbSonar.Margin = new System.Windows.Forms.Padding(2);
             this.gpbSonar.Name = "gpbSonar";
-            this.gpbSonar.Padding = new System.Windows.Forms.Padding(2, 2, 2, 2);
+            this.gpbSonar.Padding = new System.Windows.Forms.Padding(2);
             this.gpbSonar.Size = new System.Drawing.Size(232, 95);
             this.gpbSonar.TabIndex = 17;
             this.gpbSonar.TabStop = false;
@@ -156,7 +169,7 @@
             // 
             this.chbMoverObstaculo.AutoSize = true;
             this.chbMoverObstaculo.Location = new System.Drawing.Point(124, 35);
-            this.chbMoverObstaculo.Margin = new System.Windows.Forms.Padding(2, 2, 2, 2);
+            this.chbMoverObstaculo.Margin = new System.Windows.Forms.Padding(2);
             this.chbMoverObstaculo.Name = "chbMoverObstaculo";
             this.chbMoverObstaculo.Size = new System.Drawing.Size(105, 17);
             this.chbMoverObstaculo.TabIndex = 29;
@@ -167,7 +180,7 @@
             // 
             this.chbSonarVirando.AutoSize = true;
             this.chbSonarVirando.Location = new System.Drawing.Point(124, 68);
-            this.chbSonarVirando.Margin = new System.Windows.Forms.Padding(2, 2, 2, 2);
+            this.chbSonarVirando.Margin = new System.Windows.Forms.Padding(2);
             this.chbSonarVirando.Name = "chbSonarVirando";
             this.chbSonarVirando.Size = new System.Drawing.Size(96, 17);
             this.chbSonarVirando.TabIndex = 28;
@@ -177,7 +190,7 @@
             // btnAdicionarDeteccao
             // 
             this.btnAdicionarDeteccao.Location = new System.Drawing.Point(70, 64);
-            this.btnAdicionarDeteccao.Margin = new System.Windows.Forms.Padding(2, 2, 2, 2);
+            this.btnAdicionarDeteccao.Margin = new System.Windows.Forms.Padding(2);
             this.btnAdicionarDeteccao.Name = "btnAdicionarDeteccao";
             this.btnAdicionarDeteccao.Size = new System.Drawing.Size(45, 23);
             this.btnAdicionarDeteccao.TabIndex = 25;
@@ -198,7 +211,7 @@
             // txtAnguloFinal
             // 
             this.txtAnguloFinal.Location = new System.Drawing.Point(70, 33);
-            this.txtAnguloFinal.Margin = new System.Windows.Forms.Padding(2, 2, 2, 2);
+            this.txtAnguloFinal.Margin = new System.Windows.Forms.Padding(2);
             this.txtAnguloFinal.Name = "txtAnguloFinal";
             this.txtAnguloFinal.Size = new System.Drawing.Size(46, 20);
             this.txtAnguloFinal.TabIndex = 23;
@@ -218,7 +231,7 @@
             // txtDistanciaSonar
             // 
             this.txtDistanciaSonar.Location = new System.Drawing.Point(11, 69);
-            this.txtDistanciaSonar.Margin = new System.Windows.Forms.Padding(2, 2, 2, 2);
+            this.txtDistanciaSonar.Margin = new System.Windows.Forms.Padding(2);
             this.txtDistanciaSonar.Name = "txtDistanciaSonar";
             this.txtDistanciaSonar.Size = new System.Drawing.Size(46, 20);
             this.txtDistanciaSonar.TabIndex = 24;
@@ -238,7 +251,7 @@
             // txtAnguloInicial
             // 
             this.txtAnguloInicial.Location = new System.Drawing.Point(11, 33);
-            this.txtAnguloInicial.Margin = new System.Windows.Forms.Padding(2, 2, 2, 2);
+            this.txtAnguloInicial.Margin = new System.Windows.Forms.Padding(2);
             this.txtAnguloInicial.Name = "txtAnguloInicial";
             this.txtAnguloInicial.Size = new System.Drawing.Size(46, 20);
             this.txtAnguloInicial.TabIndex = 22;
@@ -281,19 +294,30 @@
             this.gpbGPS.Controls.Add(this.txtLatitude);
             this.gpbGPS.Controls.Add(this.btnIniciarGPS);
             this.gpbGPS.Location = new System.Drawing.Point(289, 2);
-            this.gpbGPS.Margin = new System.Windows.Forms.Padding(2, 2, 2, 2);
+            this.gpbGPS.Margin = new System.Windows.Forms.Padding(2);
             this.gpbGPS.Name = "gpbGPS";
-            this.gpbGPS.Padding = new System.Windows.Forms.Padding(2, 2, 2, 2);
+            this.gpbGPS.Padding = new System.Windows.Forms.Padding(2);
             this.gpbGPS.Size = new System.Drawing.Size(400, 95);
             this.gpbGPS.TabIndex = 14;
             this.gpbGPS.TabStop = false;
             this.gpbGPS.Text = "GPS";
             // 
+            // btnCalcularAngulo
+            // 
+            this.btnCalcularAngulo.Location = new System.Drawing.Point(308, 67);
+            this.btnCalcularAngulo.Margin = new System.Windows.Forms.Padding(2);
+            this.btnCalcularAngulo.Name = "btnCalcularAngulo";
+            this.btnCalcularAngulo.Size = new System.Drawing.Size(74, 23);
+            this.btnCalcularAngulo.TabIndex = 23;
+            this.btnCalcularAngulo.Text = "Calcular";
+            this.btnCalcularAngulo.UseVisualStyleBackColor = true;
+            this.btnCalcularAngulo.Click += new System.EventHandler(this.btnCalcularAngulo_Click);
+            // 
             // btnAcrescentarGPS
             // 
             this.btnAcrescentarGPS.Enabled = false;
             this.btnAcrescentarGPS.Location = new System.Drawing.Point(230, 67);
-            this.btnAcrescentarGPS.Margin = new System.Windows.Forms.Padding(2, 2, 2, 2);
+            this.btnAcrescentarGPS.Margin = new System.Windows.Forms.Padding(2);
             this.btnAcrescentarGPS.Name = "btnAcrescentarGPS";
             this.btnAcrescentarGPS.Size = new System.Drawing.Size(74, 23);
             this.btnAcrescentarGPS.TabIndex = 22;
@@ -315,7 +339,7 @@
             // 
             this.txtDistanciaGPS.Enabled = false;
             this.txtDistanciaGPS.Location = new System.Drawing.Point(161, 69);
-            this.txtDistanciaGPS.Margin = new System.Windows.Forms.Padding(2, 2, 2, 2);
+            this.txtDistanciaGPS.Margin = new System.Windows.Forms.Padding(2);
             this.txtDistanciaGPS.Name = "txtDistanciaGPS";
             this.txtDistanciaGPS.Size = new System.Drawing.Size(54, 20);
             this.txtDistanciaGPS.TabIndex = 20;
@@ -336,7 +360,7 @@
             // 
             this.txtAnguloGPS.Enabled = false;
             this.txtAnguloGPS.Location = new System.Drawing.Point(161, 33);
-            this.txtAnguloGPS.Margin = new System.Windows.Forms.Padding(2, 2, 2, 2);
+            this.txtAnguloGPS.Margin = new System.Windows.Forms.Padding(2);
             this.txtAnguloGPS.Name = "txtAnguloGPS";
             this.txtAnguloGPS.Size = new System.Drawing.Size(54, 20);
             this.txtAnguloGPS.TabIndex = 18;
@@ -366,27 +390,27 @@
             // txtLongitude
             // 
             this.txtLongitude.Location = new System.Drawing.Point(9, 69);
-            this.txtLongitude.Margin = new System.Windows.Forms.Padding(2, 2, 2, 2);
+            this.txtLongitude.Margin = new System.Windows.Forms.Padding(2);
             this.txtLongitude.Name = "txtLongitude";
             this.txtLongitude.Size = new System.Drawing.Size(135, 20);
             this.txtLongitude.TabIndex = 15;
-            this.txtLongitude.Text = "-47.26406713810977";
+            this.txtLongitude.Text = "-47.2639156835239";
             this.txtLongitude.TextAlign = System.Windows.Forms.HorizontalAlignment.Center;
             // 
             // txtLatitude
             // 
             this.txtLatitude.Location = new System.Drawing.Point(9, 33);
-            this.txtLatitude.Margin = new System.Windows.Forms.Padding(2, 2, 2, 2);
+            this.txtLatitude.Margin = new System.Windows.Forms.Padding(2);
             this.txtLatitude.Name = "txtLatitude";
             this.txtLatitude.Size = new System.Drawing.Size(135, 20);
             this.txtLatitude.TabIndex = 14;
-            this.txtLatitude.Text = "-22.339442335994334";
+            this.txtLatitude.Text = "-22.3397915668518";
             this.txtLatitude.TextAlign = System.Windows.Forms.HorizontalAlignment.Center;
             // 
             // btnIniciarGPS
             // 
             this.btnIniciarGPS.Location = new System.Drawing.Point(230, 31);
-            this.btnIniciarGPS.Margin = new System.Windows.Forms.Padding(2, 2, 2, 2);
+            this.btnIniciarGPS.Margin = new System.Windows.Forms.Padding(2);
             this.btnIniciarGPS.Name = "btnIniciarGPS";
             this.btnIniciarGPS.Size = new System.Drawing.Size(74, 23);
             this.btnIniciarGPS.TabIndex = 13;
@@ -401,7 +425,7 @@
             | System.Windows.Forms.AnchorStyles.Right)));
             this.pnlMapa.Controls.Add(this.pnlOrientacaoTrajeto);
             this.pnlMapa.Location = new System.Drawing.Point(9, 10);
-            this.pnlMapa.Margin = new System.Windows.Forms.Padding(2, 2, 2, 2);
+            this.pnlMapa.Margin = new System.Windows.Forms.Padding(2);
             this.pnlMapa.Name = "pnlMapa";
             this.pnlMapa.Size = new System.Drawing.Size(506, 465);
             this.pnlMapa.TabIndex = 1;
@@ -413,7 +437,7 @@
             this.pnlOrientacaoTrajeto.BackgroundImage = global::AgroBase.Properties.Resources.rosa_dos_ventos;
             this.pnlOrientacaoTrajeto.BackgroundImageLayout = System.Windows.Forms.ImageLayout.Zoom;
             this.pnlOrientacaoTrajeto.Location = new System.Drawing.Point(425, 2);
-            this.pnlOrientacaoTrajeto.Margin = new System.Windows.Forms.Padding(2, 2, 2, 2);
+            this.pnlOrientacaoTrajeto.Margin = new System.Windows.Forms.Padding(2);
             this.pnlOrientacaoTrajeto.Name = "pnlOrientacaoTrajeto";
             this.pnlOrientacaoTrajeto.Size = new System.Drawing.Size(78, 84);
             this.pnlOrientacaoTrajeto.TabIndex = 12;
@@ -425,7 +449,7 @@
             | System.Windows.Forms.AnchorStyles.Right)));
             this.pnlSonar.Controls.Add(this.picSonar);
             this.pnlSonar.Location = new System.Drawing.Point(519, 10);
-            this.pnlSonar.Margin = new System.Windows.Forms.Padding(2, 2, 2, 2);
+            this.pnlSonar.Margin = new System.Windows.Forms.Padding(2);
             this.pnlSonar.Name = "pnlSonar";
             this.pnlSonar.Size = new System.Drawing.Size(500, 465);
             this.pnlSonar.TabIndex = 2;
@@ -434,33 +458,22 @@
             // 
             this.picSonar.Dock = System.Windows.Forms.DockStyle.Fill;
             this.picSonar.Location = new System.Drawing.Point(0, 0);
-            this.picSonar.Margin = new System.Windows.Forms.Padding(2, 2, 2, 2);
+            this.picSonar.Margin = new System.Windows.Forms.Padding(2);
             this.picSonar.Name = "picSonar";
             this.picSonar.Size = new System.Drawing.Size(500, 465);
             this.picSonar.TabIndex = 0;
             this.picSonar.TabStop = false;
             // 
-            // btnAtualizarLeitura
+            // lblDistanciaLateral
             // 
-            this.btnAtualizarLeitura.Location = new System.Drawing.Point(134, 30);
+            this.lblDistanciaLateral.AutoSize = true;
-            this.btnAtualizarLeitura.Margin = new System.Windows.Forms.Padding(2);
+            this.lblDistanciaLateral.Font = new System.Drawing.Font("Microsoft Sans Serif", 7.8F);
-            this.btnAtualizarLeitura.Name = "btnAtualizarLeitura";
+            this.lblDistanciaLateral.Location = new System.Drawing.Point(154, 84);
-            this.btnAtualizarLeitura.Size = new System.Drawing.Size(121, 20);
+            this.lblDistanciaLateral.Margin = new System.Windows.Forms.Padding(2, 0, 2, 0);
-            this.btnAtualizarLeitura.TabIndex = 22;
+            this.lblDistanciaLateral.Name = "lblDistanciaLateral";
-            this.btnAtualizarLeitura.Text = "Atualizar leitura";
+            this.lblDistanciaLateral.Size = new System.Drawing.Size(117, 13);
-            this.btnAtualizarLeitura.UseVisualStyleBackColor = true;
+            this.lblDistanciaLateral.TabIndex = 23;
-            this.btnAtualizarLeitura.Click += new System.EventHandler(this.btnAtualizarLeitura_Click);
+            this.lblDistanciaLateral.Text = "Esq: 0.00 m Dir: 0.00 m";
-            // 
-            // btnCalcularAngulo
-            // 
-            this.btnCalcularAngulo.Location = new System.Drawing.Point(308, 67);
-            this.btnCalcularAngulo.Margin = new System.Windows.Forms.Padding(2);
-            this.btnCalcularAngulo.Name = "btnCalcularAngulo";
-            this.btnCalcularAngulo.Size = new System.Drawing.Size(74, 23);
-            this.btnCalcularAngulo.TabIndex = 23;
-            this.btnCalcularAngulo.Text = "Calcular";
-            this.btnCalcularAngulo.UseVisualStyleBackColor = true;
-            this.btnCalcularAngulo.Click += new System.EventHandler(this.btnCalcularAngulo_Click);
             // 
             // frmSimulacaoMapaGPS
             // 
@@ -470,7 +483,7 @@
             this.Controls.Add(this.pnlSonar);
             this.Controls.Add(this.pnlMapa);
             this.Controls.Add(this.pnlOpcoes);
-            this.Margin = new System.Windows.Forms.Padding(2, 2, 2, 2);
+            this.Margin = new System.Windows.Forms.Padding(2);
             this.Name = "frmSimulacaoMapaGPS";
             this.Text = "frmSimulacaoMapaGPS";
             this.FormClosing += new System.Windows.Forms.FormClosingEventHandler(this.frmSimulacaoMapaGPS_FormClosing);
@@ -524,5 +537,6 @@
         private System.Windows.Forms.CheckBox chbSonarVirando;
         private System.Windows.Forms.Button btnAtualizarLeitura;
         private System.Windows.Forms.Button btnCalcularAngulo;
+        private System.Windows.Forms.Label lblDistanciaLateral;
     }
 }

AgroBase/AgroBase/Forms/frmSimulacaoMapaGPS.cs

@@ -43,27 +43,13 @@ namespace AgroBase.Forms
             lblDirecao.Text = "Direção: " + Enum.GetName(typeof(DirecaoCarroRua), Variaveis.OperacaoEmAndamento.OpMapaGPS.ReferencialRuaGPS.Direcao);
             lblDistanciaOperacao.Text = "Distância Operação: " + (Variaveis.OperacaoEmAndamento.Mapa == null ? 0 : Variaveis.OperacaoEmAndamento.Mapa.DistanciaTotal).ToString("00.00") + " m";
             lblDistanciaFinal.Text = "Distância Final: " + (GPSService.DistanciaDoTrecho(Variaveis.OperacaoEmAndamento.Mapa.TrajetoriaDinamica)).ToString("00.00") + " m";
+            lblDistanciaLateral.Text = "Esq: " + Variaveis.OperacaoEmAndamento.Mapa.DistanciaEsquerda.ToString("0.00") + " m - Dir: " + Variaveis.OperacaoEmAndamento.Mapa.DistanciaDireita.ToString("0.00") + " m";
 
             if (Variaveis.OperacaoEmAndamento.Iniciado)
             {
-                if (StatusCarro == StatusCarroMapa.Direcionando)
+                if (StatusCarro != StatusCarroMapa.Manobrando && StatusCarro != StatusCarroMapa.SaindoRua)
                 {
-                    var Rua = Variaveis.OperacaoEmAndamento.Mapa.Trajetoria[Variaveis.OperacaoEmAndamento.Mapa.IdxRuaAtual];
+                    Variaveis.OperacaoEmAndamento.Mapa.GerarTrajetoriaDinamica();
-                    var PontoInicial = Rua.First();
-                    var PontoFinal = Rua.Last();
-                    Variaveis.OperacaoEmAndamento.Mapa.GerarCaminhoEntreDoisPontos(Variaveis.OperacaoEmAndamento.DirecaoCaminho == DirecaoCarroRua.Volta ? PontoFinal : PontoInicial);
-                }
-                else if (StatusCarro == StatusCarroMapa.Manobrando)
-                {
-                    //Variaveis.OperacaoEmAndamento.Mapa.GerarCaminhoEntreDoisPontos(Variaveis.OperacaoEmAndamento.Mapa.Trajetoria[Variaveis.OperacaoEmAndamento.Mapa.IdxRuaAtual].First());
-                }
-                else
-                {
-                    if (Variaveis.OperacaoEmAndamento.Mapa.TrajetoriaDinamica.Any())
-                    {
-                        Variaveis.OperacaoEmAndamento.Mapa.TrajetoriaDinamica.Clear();
-                        GPSService.AtualizarTrajetoriaDinamica();
-                    }
                 }
 
                 pnlOrientacaoTrajeto.Invalidate();
@@ -126,13 +112,13 @@ namespace AgroBase.Forms
 
         private void btnAcrescentarGPS_Click(object sender, EventArgs e)
         {
-            double angulo = Variaveis.OperacaoEmAndamento.OpMapaGPS.ReferencialRuaGPS.AnguloCaminho;
+            double angulo = Variaveis.OperacaoEmAndamento.Mapa.AnguloCaminho;
             if (txtAnguloGPS.Text != "")
             {
                 angulo = double.Parse(txtAnguloGPS.Text);
             }
             double distancia = double.Parse(txtDistanciaGPS.Text); // Distância em metros
-            angulo -= 180; // Ângulo em graus
+            //angulo -= 180; // Ângulo em graus
 
             double latitude = GPSService.UltimaLeitura.Latitude;
             double longitude = GPSService.UltimaLeitura.Longitude;
@@ -141,7 +127,7 @@ namespace AgroBase.Forms
             double anguloRad = angulo * (Math.PI / 180);
 
             // Raio da Terra em metros
-            double raioTerra = 6371000;
+            double raioTerra = GPSService.RaioDaTerra;
 
             // Calcular deslocamento de latitude em radianos
             double deltaLat = distancia * Math.Cos(anguloRad) / raioTerra;
@@ -166,12 +152,20 @@ namespace AgroBase.Forms
             }
 
             btnCalcularAngulo_Click(sender, e);
+
+            btnAtualizarLeitura_Click(sender, e);
         }
 
         private void btnIniciarSimulacao_Click(object sender, EventArgs e)
         {
+            if (!Variaveis.OperacaoEmAndamento.Mapa.RuasPercorrer.Any())
+            {
+                MessageBox.Show("Selecione as ruas para realizar a operação!");
+                return;
+            }
             Variaveis.OperacaoEmAndamento.Iniciado = !Variaveis.OperacaoEmAndamento.Iniciado;
             btnIniciarSimulacao.Text = Variaveis.OperacaoEmAndamento.Iniciado ? "Parar Simulação" : "Iniciar Simulação";
+            Variaveis.OperacaoEmAndamento.Mapa.GerarTrajetoriaDinamica();
         }
 
         private void btnCarregarMapa_Click(object sender, EventArgs e)
@@ -183,8 +177,8 @@ namespace AgroBase.Forms
         {
             try
             {
-                double angulo = Variaveis.OperacaoEmAndamento.OpMapaGPS.ReferencialRuaGPS.AnguloCaminho;
+                double angulo = Variaveis.OperacaoEmAndamento.Mapa.AnguloCaminho;
-                DesenharInclinacao((Panel)sender, e, (float)angulo, -90);
+                DesenharInclinacao((Panel)sender, e, (float)angulo, 90);
             }
             catch { }
         }
@@ -313,11 +307,17 @@ namespace AgroBase.Forms
 
         private void btnCalcularAngulo_Click(object sender, EventArgs e)
         {
-            Variaveis.OperacaoEmAndamento.OpMapaGPS.CalcularAnguloInclinacao(StatusCarro);
+            Variaveis.OperacaoEmAndamento.OpMapaGPS.CalculaDadosMovimentacaoAutonoma();
 
-            double anguloAnterior = double.Parse(txtAnguloGPS.Text);
+            double anguloAtual = double.Parse(txtAnguloGPS.Text);
+            double novoAngulo = (anguloAtual + Variaveis.OperacaoEmAndamento.OpMapaGPS.Controle.Angulo) % 360;
 
-            txtAnguloGPS.Text = (Variaveis.OperacaoEmAndamento.OpMapaGPS.Controle.Angulo + anguloAnterior).ToString("0.00");
+            txtAnguloGPS.Text = novoAngulo.ToString("0.00");
+
+            double distancia = Math.Round((double)Variaveis.OperacaoEmAndamento.OpMapaGPS.Controle.RPM / 45.0, 2);
+            txtDistanciaGPS.Text = distancia.ToString("0.00");
+
+            pnlOrientacaoTrajeto.Invalidate();
         }
     }
 }

AgroBase/AgroBase/Models/Enuns.cs

@@ -147,7 +147,8 @@ namespace AgroBase.Models
         {
             Parado, 
             Ida, 
-            Volta
+            Volta, 
+            Manobra
         }
 
         public enum ParametrosOperacao

AgroBase/AgroBase/Models/GPSModel.cs

@@ -4,6 +4,7 @@ using System.Collections.Generic;
 using System.Linq;
 using System.Text;
 using System.Threading.Tasks;
+using static AgroBase.Models.Enuns;
 
 namespace AgroBase.Models
 {
@@ -20,8 +21,10 @@ namespace AgroBase.Models
         {
             get
             {
-                return GPSService.CalcularOrientacao(GPSService.UltimaLeitura, GPSService.PenultimaLeitura);
+                return GPSService.CalcularOrientacao(GPSService.PenultimaLeitura, GPSService.UltimaLeitura);
             }
         }
+        public int idxRua { get; set; } = -1;
+        public DirecaoCarroRua DirecaoTrecho { get; set; } = DirecaoCarroRua.Parado;
     }
 }

AgroBase/AgroBase/Models/MapasModel.cs

@@ -1,5 +1,6 @@
 using AgroBase.Properties;
 using AgroBase.Services;
+using CefSharp.DevTools.Page;
 using CefSharp.WinForms;
 using Emgu.CV.Features2D;
 using Newtonsoft.Json;
@@ -26,6 +27,242 @@ namespace AgroBase.Models
         public List<string> RuasPercorrer { get; set; } = new List<string>();
         public int IdxRuaAtual { get; set; } = 0;
         public double DistanciaTotal { get; set; } = 0;
+        public DirecaoCarroRua DirecaoCaminho
+        {
+            get
+            {
+                DirecaoCarroRua direcaoCaminho = DirecaoCarroRua.Parado;
+                if (TrajetoriaDinamica.Any())
+                {
+                    GPSModel PontoMaisProximo = GPSService.PontoMaisProximoRuaProjetada(TrajetoriaDinamica);
+                    int idxPontoMaisProximo = TrajetoriaDinamica.IndexOf(PontoMaisProximo);
+                    direcaoCaminho = TrajetoriaDinamica[idxPontoMaisProximo].DirecaoTrecho;
+                }
+                else
+                {
+                    if (Variaveis.OperacaoEmAndamento.OpMapaGPS.ReferencialRuaGPS.ExtremoMaisProximo == 0)
+                    {
+                        direcaoCaminho = DirecaoCarroRua.Ida;
+                    }
+                    else
+                    {
+                        direcaoCaminho = DirecaoCarroRua.Volta;
+                    }
+                }
+                return direcaoCaminho;
+            }
+        }
+        public bool DentroDaRua
+        {
+            get
+            {
+                if (RuasPercorrer.Count() > IdxRuaAtual)
+                {
+                    try
+                    {
+                        string IDruaAtual = RuasPercorrer[IdxRuaAtual];
+                        var Rua = mapaService.DadosMapa.features.Where(x => x.geometry.id == IDruaAtual).FirstOrDefault();
+                        if (Rua != null)
+                        {
+                            bool dentro = false;
+                            List<GPSModel> Trecho = new List<GPSModel>();
+                            List<GPSModel> PontosRua = new List<GPSModel>();
+                            Trajetoria[IdxRuaAtual].ToList().ForEach(x => PontosRua.Add(x));
+                            if (DirecaoCaminho == DirecaoCarroRua.Volta)
+                            {
+                                PontosRua.Reverse();
+                            }
+                            GPSModel PontoMaisProximo = GPSService.PontoMaisProximoRuaProjetada(Trajetoria[IdxRuaAtual]);
+                            int idx = PontosRua.IndexOf(PontoMaisProximo);
+                            foreach (var Ponto in PontosRua)
+                            {
+                                // Adiciona os pontos que o robô já passou por eles no trecho, e quando chegar no ponto mais próximo, substitui ele pela posição atual
+                                int idxPonto = PontosRua.IndexOf(Ponto);
+                                if (idxPonto < idx)
+                                {
+                                    Trecho.Add(Ponto);
+                                }
+                                else
+                                {
+                                    if (idx > 0)
+                                    {
+                                        Trecho.Add(GPSService.UltimaLeitura);
+                                    }
+
+                                    break;
+                                }
+                            }
+                            double distTrecho = GPSService.DistanciaDoTrecho(Trecho);
+                            dentro = distTrecho > 0 && distTrecho < Rua.properties.Length;
+
+                            return dentro;
+                        }
+                        else
+                        {
+                            return false;
+                        }
+                    }
+                    catch
+                    {
+                        return false;
+                    }
+                }
+                else
+                {
+                    return false;
+                }
+            }
+        }
+        public GPSModel PrimeiroPontoRua
+        {
+            get
+            {
+                try
+                {
+                    var primeiroPontoRua = Trajetoria.Skip(IdxRuaAtual).First().First();
+                    return primeiroPontoRua;
+                }
+                catch
+                {
+                    return new GPSModel();
+                }
+
+            }
+        }
+        public GPSModel UltimoPontoRua
+        {
+            get
+            {
+                try
+                {
+                    var ultimoPontoRua = Trajetoria.Skip(IdxRuaAtual).First().Last();
+                    return ultimoPontoRua;
+                }
+                catch
+                {
+                    return new GPSModel();
+                }
+            }
+        }
+        public int ExtremoMaisProximo
+        {
+            get
+            {
+                double distPP = GPSService.DistanciaEntrePontos(GPSService.UltimaLeitura, PrimeiroPontoRua);
+                double distUP = GPSService.DistanciaEntrePontos(GPSService.UltimaLeitura, UltimoPontoRua);
+
+                // 0 para mais próximo do primeiro ponto da rua (ENTRANDO)
+                // 1 para mais próximo do último ponto da rua (SAINDO)
+                return (distPP < distUP) ? 0 : 1;
+            }
+        }
+        public double AnguloCaminho
+        {
+            get
+            {
+                try
+                {
+                    if (TrajetoriaDinamica.Any())
+                    {
+                        GPSModel PontoMaisProximo = GPSService.PontoMaisProximoRuaProjetada(TrajetoriaDinamica);
+                        int idx = TrajetoriaDinamica.IndexOf(PontoMaisProximo);
+                        double angulo = GPSService.CalcularOrientacao(TrajetoriaDinamica[idx], TrajetoriaDinamica[idx + 1]);
+                        if (angulo == 0)
+                        {
+                            angulo = GPSService.CalcularOrientacao(TrajetoriaDinamica[idx + 1], TrajetoriaDinamica[idx + 2]);
+                        }
+
+                        return (!(angulo >= 0 || angulo <= 0)) ? 0 : angulo;
+                    }
+                    else
+                    {
+                        return 0;
+                    }
+                }
+                catch
+                {
+                    return 0;
+                }
+            }
+        }
+        public double DistanciaEsquerda
+        {
+            get
+            {
+                double distancia = 0;
+
+                List<GPSModel> RuaEsquerda = new List<GPSModel>();
+                if (Trajetoria.Any())
+                {
+                    if (DirecaoCaminho == DirecaoCarroRua.Ida)
+                    {
+                        RuaEsquerda = Trajetoria[IdxRuaAtual];
+                    }
+                    else if (DirecaoCaminho == DirecaoCarroRua.Volta)
+                    {
+                        if (IdxRuaAtual > 0)
+                        {
+                            GPSModel pontoMaisProximo = GPSService.PontoMaisProximoRuaProjetada(Trajetoria[IdxRuaAtual - 1]);
+                            double distanciaP0 = GPSService.DistanciaEntrePontos(GPSService.UltimaLeitura, pontoMaisProximo);
+                            if (distanciaP0 < Variaveis.OperacaoEmAndamento.OpMapaGPS.ReferencialRuaGPS.MargemLimiteEntradaRua)
+                            {
+                                RuaEsquerda = Trajetoria[IdxRuaAtual - 1];
+                            }
+                        }
+                    }
+                }
+
+                if (RuaEsquerda.Any())
+                {
+                    distancia = DistanciaAteRua(RuaEsquerda);
+                }
+                else
+                {
+                    distancia = DistanciaEntreRuas(Trajetoria) / 2;
+                }
+
+                return distancia;
+            }
+        }
+        public double DistanciaDireita
+        {
+            get
+            {
+                double distancia = 0;
+
+                List<GPSModel> RuaDireita = new List<GPSModel>();
+                if (Trajetoria.Any())
+                {
+                    if (DirecaoCaminho == DirecaoCarroRua.Volta)
+                    {
+                        RuaDireita = Trajetoria[IdxRuaAtual];
+                    }
+                    else if (DirecaoCaminho == DirecaoCarroRua.Ida)
+                    {
+                        if (IdxRuaAtual > 0)
+                        {
+                            GPSModel pontoMaisProximo = GPSService.PontoMaisProximoRuaProjetada(Trajetoria[IdxRuaAtual - 1]);
+                            double distanciaP0 = GPSService.DistanciaEntrePontos(GPSService.UltimaLeitura, pontoMaisProximo);
+                            if (distanciaP0 < Variaveis.OperacaoEmAndamento.OpMapaGPS.ReferencialRuaGPS.MargemLimiteEntradaRua)
+                            {
+                                RuaDireita = Trajetoria[IdxRuaAtual - 1];
+                            }
+                        }
+                    }
+                }
+                
+                if (RuaDireita.Any())
+                {
+                    distancia = DistanciaAteRua(RuaDireita);
+                }
+                else
+                {
+                    distancia = DistanciaEntreRuas(Trajetoria) / 2;
+                }
+
+                return distancia;
+            }
+        }
 
         public void btnCarregar_Click(object sender, EventArgs e)
         {
@@ -161,6 +398,244 @@ namespace AgroBase.Models
         }
 
 
+        public double DistanciaEntreRuas(List<List<GPSModel>> ruas)
+        {
+            double distanciaTotal = 0;
+
+            if (ruas.Count < 2)
+            {
+                // Se houver menos de duas ruas, não há distância a calcular
+                return distanciaTotal;
+            }
+
+            double menorDistancia = double.MaxValue;
+
+            for (int i = 0; i < ruas.Count - 1; i++)
+            {
+                List<GPSModel> ruaAtual = ruas[i];
+                List<GPSModel> proximaRua = ruas[i + 1];
+
+                // Calcule a distância entre as duas ruas (rua atual e próxima rua)
+                // Aqui você pode usar o método GPSService.CalcularDistanciaPontos
+                // ou uma função personalizada para calcular a distância entre as ruas
+                
+
+                foreach (GPSModel pontoAtual in ruaAtual)
+                {
+                    foreach (GPSModel pontoProxima in proximaRua)
+                    {
+                        // Calcular a distância entre pontoAtual e pontoProxima
+                        double distancia = GPSService.DistanciaEntrePontos(pontoAtual, pontoProxima);
+
+                        // Verifique se esta é a menor distância
+                        if (distancia < menorDistancia)
+                        {
+                            menorDistancia = distancia;
+                        }
+                    }
+                }
+
+                // Adiciona a menor distância encontrada à distância total
+                distanciaTotal += menorDistancia;
+            }
+
+            double distanciaMedia = distanciaTotal / ruas.Count();
+
+            return menorDistancia;
+        }
+
+        public double DistanciaAteRua(List<GPSModel> Rua)
+        {
+            GPSModel pontoAtual = GPSService.UltimaLeitura;
+
+            GPSModel P0 = GPSService.PontoMaisProximoRuaProjetada(Rua);
+            int idxP0 = Rua.IndexOf(P0);
+            bool PontoProximo = idxP0 < Rua.Count() - 1;
+            bool PontoAnterior = idxP0 > 0;
+            GPSModel P1 = PontoProximo ? Rua[idxP0 + 1] : PontoAnterior ? Rua[idxP0 - 1] : pontoAtual;
+            
+            double a = GPSService.DistanciaEntrePontos(pontoAtual, P0);
+            double b = GPSService.DistanciaEntrePontos(pontoAtual, P1);
+            double c = GPSService.DistanciaEntrePontos(P0, P1);
+
+            double distancia = FuncoesGlobais.CalcularAlturaTriangulo(a, b, c);
+
+            distancia -= (VariaveisEquipamento.Largura / 2.0 / 100.0);
+
+            return distancia;
+        }
+
+
+        public void GerarTrajetoriaDinamica()
+        {
+            double distanciaEntreRuas = DistanciaEntreRuas(Trajetoria) / 2;
+            List<GPSModel> Caminho = new List<GPSModel>();
+
+            DirecaoCarroRua direcaoRua = TrajetoriaDinamica.Any() ? TrajetoriaDinamica[0].DirecaoTrecho : ExtremoMaisProximo == 0 ? DirecaoCarroRua.Ida : DirecaoCarroRua.Volta;
+
+            for (int i = IdxRuaAtual; i < Trajetoria.Count(); i++)
+            {
+                List<GPSModel> RuaAtual = Trajetoria.Skip(i).First();
+
+                double anguloProjetar = GPSService.CalcularOrientacao(RuaAtual.First(), RuaAtual.Skip(1).First()) + 90;
+
+                List<GPSModel> RuaProjetada = ProjetarRua(RuaAtual, distanciaEntreRuas, anguloProjetar);
+                RuaProjetada.ForEach(x =>
+                {
+                    x.idxRua = i;
+                    x.DirecaoTrecho = direcaoRua;
+                });
+                if (direcaoRua == DirecaoCarroRua.Volta)
+                {
+                    RuaProjetada.Reverse();
+                }
+                double anguloProjetar1 = GPSService.CalcularOrientacao(RuaProjetada.Skip(1).First(), RuaProjetada.First());
+                GPSModel PrimeiroPonto = GPSService.ProjetarPontoDeslocado(RuaProjetada.First(), 3, anguloProjetar1);
+                double anguloProjetar2 = GPSService.CalcularOrientacao(RuaProjetada.Skip(RuasPercorrer.Count() - 2).First(), RuaProjetada.Last());
+                GPSModel UltimoPonto = GPSService.ProjetarPontoDeslocado(RuaProjetada.Last(), 3, anguloProjetar2);
+
+                if (!Caminho.Any())
+                {
+                    GPSModel PontoMaisProximo = GPSService.PontoMaisProximoRuaProjetada(RuaProjetada);
+                    int idxPontoMaisProximo = RuaProjetada.IndexOf(PontoMaisProximo);
+
+                    GPSModel inicio = GPSService.UltimaLeitura;
+
+                    List<GPSModel> _caminho = new List<GPSModel>();
+                    if (idxPontoMaisProximo == 0)
+                    {
+                        //_caminho.Add(PrimeiroPonto);
+                        List<GPSModel> _pontos = GPSService.InterpolarPontos(inicio, PrimeiroPonto, 1.0);
+                        if (GPSService.CompararDirecaoTrajeto(_pontos, RuaProjetada))
+                        {
+                            _caminho.AddRange(_pontos);
+                        }
+                    }
+                    
+                    if (!Variaveis.OperacaoEmAndamento.OpMapaGPS.ReferencialRuaGPS.ManobraConcluida)
+                    {
+                        _caminho.Add(inicio);
+                    }
+                    else
+                    {
+                        _caminho.AddRange(RuaProjetada.Skip(idxPontoMaisProximo));
+                        _caminho.Add(UltimoPonto);
+                    }
+                    _caminho.ForEach(x => x.DirecaoTrecho = direcaoRua);
+                    Caminho.AddRange(_caminho);
+
+                    direcaoRua = direcaoRua == DirecaoCarroRua.Ida ? DirecaoCarroRua.Volta : DirecaoCarroRua.Ida;
+                }
+                else
+                {
+                    List<GPSModel> _caminho = new List<GPSModel>();
+                    double distancia = GPSService.DistanciaEntrePontos(GPSService.UltimaLeitura, PrimeiroPonto);
+                    if (distancia > Variaveis.OperacaoEmAndamento.OpMapaGPS.ReferencialRuaGPS.MargemLimiteEntradaRua * 2)
+                    {
+                        GPSModel P0 = CalcularPontoControle(Caminho.Last(), PrimeiroPonto, distanciaEntreRuas * 2);
+                        List<GPSModel> CurvaConexao = GerarCurvaConexao(Caminho.Last(), PrimeiroPonto, P0, 6);
+
+                        _caminho.AddRange(CurvaConexao);
+                    }
+                    else
+                    {
+                        _caminho.Add(PrimeiroPonto);
+                    }
+                    //_caminho.AddRange(GPSService.InterpolarPontos(inicio, PrimeiroPonto, 1.0));
+                    _caminho.AddRange(RuaProjetada);
+                    _caminho.Add(UltimoPonto);
+                    _caminho.ForEach(x => x.DirecaoTrecho = DirecaoCarroRua.Manobra);
+                    Caminho.AddRange(_caminho);
+
+                    direcaoRua = direcaoRua == DirecaoCarroRua.Ida ? DirecaoCarroRua.Volta : DirecaoCarroRua.Ida;
+                }
+            }
+
+
+            // Limpa a trajetória dinâmica existente e adiciona os novos pontos
+            TrajetoriaDinamica.Clear();
+            Caminho.ForEach(x =>
+            {
+                // Impede inclusão de pontos duplicados
+                if (!TrajetoriaDinamica.Contains(x))
+                {
+                    TrajetoriaDinamica.Add(x);
+                }
+            });
+
+            if (Variaveis.OperacaoEmAndamento.Sensoriamento.SonarFrontal.DesvioNecessario)
+            {
+                AjustarTrajetoriaParaDesvio(Variaveis.OperacaoEmAndamento.Sensoriamento.SonarFrontal.ObstaculoCritico);
+            }
+
+            GPSService.AtualizarTrajetoriaDinamica();
+        }
+
+        public List<GPSModel> ProjetarRua(List<GPSModel> ruaOriginal, double offset, double anguloOffset)
+        {
+            // Certifique-se de que a lista da rua original não está vazia
+            if (ruaOriginal == null || ruaOriginal.Count == 0)
+            {
+                return new List<GPSModel>();
+            }
+
+            List<GPSModel> RuaProjetada = new List<GPSModel>();
+
+            foreach (var Ponto in  ruaOriginal)
+            {
+                int idx = ruaOriginal.IndexOf(Ponto);
+                GPSModel Ponto2 = idx + 1 < ruaOriginal.Count() ? ruaOriginal[idx + 1] : ruaOriginal[idx - 1];
+                double angulo = 0;
+                if (idx + 1 < ruaOriginal.Count())
+                {
+                    angulo = GPSService.CalcularOrientacao(Ponto, Ponto2);
+                }
+                else
+                {
+                    angulo = GPSService.CalcularOrientacao(Ponto2, Ponto);
+                }
+                RuaProjetada.Add(GPSService.ProjetarPontoDeslocado(Ponto, offset, angulo + 90));
+            }
+
+            return RuaProjetada;
+        }
+
+
+        public static GPSModel CalcularBezierQuadratica(GPSModel p0, GPSModel p1, GPSModel p2, double t)
+        {
+            double x = Math.Pow(1 - t, 2) * p0.Latitude + 2 * (1 - t) * t * p1.Latitude + Math.Pow(t, 2) * p2.Latitude;
+            double y = Math.Pow(1 - t, 2) * p0.Longitude + 2 * (1 - t) * t * p1.Longitude + Math.Pow(t, 2) * p2.Longitude;
+            return new GPSModel { Latitude = x, Longitude = y };
+        }
+
+        public static GPSModel CalcularPontoControle(GPSModel ultimoPontoRuaAtual, GPSModel primeiroPontoProximaRua, double distancia)
+        {
+            double angulo = GPSService.CalcularOrientacao(ultimoPontoRuaAtual, primeiroPontoProximaRua);
+            double lat = (ultimoPontoRuaAtual.Latitude + primeiroPontoProximaRua.Latitude) / 2;
+            double lon = (ultimoPontoRuaAtual.Longitude + primeiroPontoProximaRua.Longitude) / 2;
+            GPSModel pontoMedio = new GPSModel()
+            {
+                Latitude = lat, 
+                Longitude = lon,
+            };
+            // Projetar um ponto a uma certa distância na direção calculada
+            return GPSService.ProjetarPontoDeslocado(pontoMedio, distancia, angulo + 270);
+        }
+
+        public static List<GPSModel> GerarCurvaConexao(GPSModel ultimoPontoRuaAtual, GPSModel primeiroPontoProximaRua, GPSModel pontoControle, int numeroPontos)
+        {
+            List<GPSModel> pontosCurva = new List<GPSModel>();
+            for (int i = 0; i <= numeroPontos; i++)
+            {
+                double t = i / (double)numeroPontos;
+                pontosCurva.Add(CalcularBezierQuadratica(ultimoPontoRuaAtual, pontoControle, primeiroPontoProximaRua, t));
+            }
+            return pontosCurva;
+        }
+
+
+
+
 
         // Método para gerar o caminho entre dois pontos
         public void GerarCaminhoEntreDoisPontos(GPSModel destino)
@@ -240,8 +715,10 @@ namespace AgroBase.Models
         {
             List<GPSModel> waypointsDesvio = new List<GPSModel>();
 
+            double anguloCaminho = Variaveis.OperacaoEmAndamento.Mapa.AnguloCaminho;
+
             // Calcular o ângulo e distância para o desvio
-            double anguloDesvio = Variaveis.OperacaoEmAndamento.OpMapaGPS.ReferencialRuaGPS.AnguloCaminho + ((obstaculo.DirecaoDesvio == Enuns.Direcao.Direita ? 270 : 90) + obstaculo.AnguloParaDesvio);
+            double anguloDesvio = anguloCaminho + ((obstaculo.DirecaoDesvio == Enuns.Direcao.Direita ? 270 : 90) + obstaculo.AnguloParaDesvio);
             double deslocamento = (double)obstaculo.DistanciaMedia / 100; // Math.Sqrt(Math.Pow(((double)obstaculo.DeslocamentoNecessarioDesvio / 100), 2) + Math.Pow(((double)obstaculo.DistanciaMedia / 100), 2));
 
             // Waypoint de início do desvio
@@ -249,12 +726,12 @@ namespace AgroBase.Models
             waypointsDesvio.Add(waypointInicioDesvio);
 
             // Waypoint de contorno do obstáculo
-            double anguloContorno = 270 - Variaveis.OperacaoEmAndamento.OpMapaGPS.ReferencialRuaGPS.AnguloCaminho; // Ajustar conforme a orientação do desvio
+            double anguloContorno = 270 - anguloCaminho; // Ajustar conforme a orientação do desvio
             GPSModel waypointContornoObstaculo = CalcularNovoWaypoint(waypointInicioDesvio, anguloContorno, ((double)obstaculo.Largura / 100)); //((double)obstaculo.Largura / 100) 
             waypointsDesvio.Add(waypointContornoObstaculo);
 
             // Waypoint de retorno à trajetória
-            double anguloRetorno = Variaveis.OperacaoEmAndamento.OpMapaGPS.ReferencialRuaGPS.AnguloCaminho + ((obstaculo.DirecaoDesvio == Enuns.Direcao.Direita ? 90 : 270) - obstaculo.AnguloParaDesvio); //anguloDesvio - obstaculo.AnguloParaDesvio - 180;// Variaveis.OperacaoEmAndamento.OpMapaGPS.ReferencialRuaGPS.AnguloCaminho + (obstaculo.AnguloParaDesvio); // Pode precisar ajustar baseado na orientação do desvio
+            double anguloRetorno = anguloCaminho + ((obstaculo.DirecaoDesvio == Enuns.Direcao.Direita ? 90 : 270) - obstaculo.AnguloParaDesvio); //anguloDesvio - obstaculo.AnguloParaDesvio - 180;// Variaveis.OperacaoEmAndamento.OpMapaGPS.ReferencialRuaGPS.AnguloCaminho + (obstaculo.AnguloParaDesvio); // Pode precisar ajustar baseado na orientação do desvio
             GPSModel waypointRetorno = CalcularNovoWaypoint(waypointContornoObstaculo, anguloRetorno, deslocamento);
             waypointsDesvio.Add(waypointRetorno);
 

AgroBase/AgroBase/Models/Operacoes/OperacaoMapaGPSModel.cs

@@ -29,6 +29,14 @@ namespace AgroBase.Models.Operacoes
         public OperacaoControleModel Controle { get; set; } = new OperacaoControleModel();
         public OperacaoControleModel ControleAnterior { get; set; } = new OperacaoControleModel();
         public List<OperacaoMapaGPSSensoriamentoModel> Logs { get; set; } = new List<OperacaoMapaGPSSensoriamentoModel>();
+        public List<StatusCarroMapa> StatusDentroRua = new List<StatusCarroMapa>()
+        {
+            StatusCarroMapa.EntrandoRua, 
+            StatusCarroMapa.CaminhandoRua, 
+            StatusCarroMapa.SaindoRua
+        };
+        public double PesoOrientacaoDentroRua { get; } = 0.3;
+        public double PesoLateralDentroRua { get; } = 0.7;
 
 
         public void AtualizarDadosControle()
@@ -183,7 +191,7 @@ namespace AgroBase.Models.Operacoes
                 Status = ReferencialRuaGPS.StatusAtual,
                 Direcao = ReferencialRuaGPS.Direcao,
                 AnguloCarro = Variaveis.OperacaoEmAndamento.Sensoriamento.AnguloCarro, 
-                AnguloRua = ReferencialRuaGPS.AnguloCaminho,
+                AnguloRua = Variaveis.OperacaoEmAndamento.Mapa.AnguloCaminho,
                 DentroDaRua = ReferencialRuaGPS.DentroDaRua,
                 IdxPontoAproximadoRua = ReferencialRuaGPS.IdxPontoAproximadoRua,
                 IdxRuaAtual = Variaveis.OperacaoEmAndamento.Mapa.IdxRuaAtual,
@@ -217,39 +225,44 @@ namespace AgroBase.Models.Operacoes
             double erroOrientacao = 0.0;
             double anguloCarro = Variaveis.OperacaoEmAndamento.Sensoriamento.AnguloCarro;
             double anguloOposto = (anguloCarro + 180) % 360;
-            double anguloRua = ReferencialRuaGPS.AnguloCaminho;
+            double anguloRua = Variaveis.OperacaoEmAndamento.Mapa.AnguloCaminho;
             double anguloCamera = Sensoriamento.AnguloRuaCamera;
-            double margem = ReferencialRuaGPS.MargemErroAngulo;
 
-            // Calcula o erro de orientação
+            // Calcula a diferença de ângulo entre o carro e a rua
-            if ((anguloCarro + margem) >= anguloRua && (anguloCarro - margem) <= anguloRua)
+            double diferencaAngulo = GPSService.CalcularDiferencaAngulo(anguloCarro, anguloRua);
-            {
+            
-                erroOrientacao = anguloCarro - anguloRua;
+            erroOrientacao = diferencaAngulo;
-            }
+
-            else if ((anguloOposto + margem) >= anguloRua && (anguloOposto - margem) <= anguloRua)
-            {
-                erroOrientacao = anguloOposto - anguloRua;
-            }
             Sensoriamento.AnguloDif = erroOrientacao;
 
-            // Assume-se que o anguloCamera reflete o desvio lateral. 
+            // Calcule o erro lateral baseado nas distâncias
-            // Isso pode precisar de ajuste dependendo de como o ângulo é medido.
+            double erroLateral = Variaveis.OperacaoEmAndamento.Mapa.DistanciaEsquerda - Variaveis.OperacaoEmAndamento.Mapa.DistanciaDireita;
-            double erroPosicaoLateral = anguloCamera;
+ 
+            double erroPosicaoLateral = erroLateral;
+            
 
             // Combina os erros. Ajuste os pesos conforme necessário para o seu caso específico.
-            double pesoOrientacao = statusCarroRua == StatusCarroMapa.Direcionando ? 0.95 : 0.3;
+            double pesoOrientacao = StatusDentroRua.Contains(statusCarroRua) ? PesoOrientacaoDentroRua : 1 - PesoOrientacaoDentroRua;
-            double pesoPosicaoLateral = statusCarroRua == StatusCarroMapa.Direcionando ? 0.05 : 0.7;
+            double pesoPosicaoLateral = StatusDentroRua.Contains(statusCarroRua) ? PesoLateralDentroRua : 1 - PesoLateralDentroRua;
             double erroCombinado = (erroOrientacao * pesoOrientacao) + (erroPosicaoLateral * pesoPosicaoLateral);
 
-            //var statusAtual = ReferencialRuaGPS.StatusAtual;
             // Atualiza o PID com o erro combinado
-            if (statusCarroRua != StatusCarroMapa.Parado && statusCarroRua != StatusCarroMapa.Manobrando)
+            if (statusCarroRua != StatusCarroMapa.Parado)
             {
                 Controle.PIDdirecional.Atualizar(erroCombinado);
             }
             double correcaoAngulo = Controle.PIDdirecional.Saida;
 
             // Aplica a correção. Isso pode incluir limites para evitar comandos excessivos.
+            if (correcaoAngulo > Controle.Angulo_Max)
+            {
+                correcaoAngulo = Controle.Angulo_Max;
+            }
+            else if (correcaoAngulo < Controle.Angulo_Min)
+            {
+                correcaoAngulo = Controle.Angulo_Min;
+            }
+            
             Controle.Angulo = correcaoAngulo;
         }
 
@@ -305,16 +318,10 @@ namespace AgroBase.Models.Operacoes
             }
             else if (statusAtual == StatusCarroMapa.Manobrando)
             {
-                Controle.RPM = 0;
+                Controle.RPM = Controle.RPM_Min;
-                CalcularManobra();
             }
         }
 
-        public void CalcularManobra()
-        {
-
-        }
-
         public void AtualizarRuasSelecionadas()
         {
             List<string> ids = new List<string>();
@@ -339,13 +346,13 @@ namespace AgroBase.Models.Operacoes
 
     public class OperacaoRefRuaGPS
     {
-        public bool ManobraConcluida { get; set; } = false;
+        public bool ManobraConcluida { get; set; } = true;
         private StatusCarroMapa _EtapaAtual;
         public StatusCarroMapa StatusAtual
         {
             get
             {
-                bool dentroDaRua = DentroDaRua;
+                bool dentroDaRua = Variaveis.OperacaoEmAndamento.Mapa.DentroDaRua;
                 StatusCarroMapa etapaAtual = _EtapaAtual;
 
                 // Se o status da operação não estiver Em Andamento
@@ -367,10 +374,24 @@ namespace AgroBase.Models.Operacoes
                             GPSService.AtualizaLeituraSimulacao();
                             System.Threading.Thread.Sleep(500);
                         }*/
-                        if (Variaveis.OperacaoEmAndamento.DirecaoCaminho == Direcao)
+                        int idxProximaRua = Variaveis.OperacaoEmAndamento.Mapa.IdxRuaAtual + 1;
+                        List<GPSModel> trechoCarro = new List<GPSModel>() 
                         { 
-                            ManobraConcluida = false;
+                            GPSService.PenultimaLeitura, 
-                            etapaAtual = StatusCarroMapa.EntrandoRua;
+                            GPSService.UltimaLeitura 
+                        };
+                        List<GPSModel> trechoCaminho = new List<GPSModel>() 
+                        { 
+                            Variaveis.OperacaoEmAndamento.Mapa.TrajetoriaDinamica.Where(x => x.idxRua == idxProximaRua).FirstOrDefault(), 
+                            Variaveis.OperacaoEmAndamento.Mapa.TrajetoriaDinamica.Where(x => x.idxRua == idxProximaRua).Skip(1).FirstOrDefault()
+                        };
+                        if (GPSService.CompararDirecaoTrajeto(trechoCarro, trechoCaminho))
+                        {
+                            Variaveis.OperacaoEmAndamento.Mapa.IdxRuaAtual++;
+                            ManobraConcluida = true;
+                            etapaAtual = StatusCarroMapa.Direcionando;
+                            Variaveis.OperacaoEmAndamento.Mapa.TrajetoriaDinamica.Clear();
+                            Variaveis.OperacaoEmAndamento.Mapa.GerarTrajetoriaDinamica();
                         }
                     }
                     // Se o robô não estiver dentro da entrelinha de cana
@@ -391,14 +412,14 @@ namespace AgroBase.Models.Operacoes
                                 etapaAtual = StatusCarroMapa.EntrandoRua;
                             }
                             // Está mais próximo ao ponto final da rua, e a direção do carro é IDA, mas a rua será iniciada no sentido VOLTA, então para o carro para que ele vire e entre na rua
-                            else if (ExtremoMaisProximo == 1 && (direcao == DirecaoCarroRua.Ida && Variaveis.OperacaoEmAndamento.DirecaoCaminho == DirecaoCarroRua.Volta))
+                            /*else if (ExtremoMaisProximo == 1 && (direcao == DirecaoCarroRua.Ida && Variaveis.OperacaoEmAndamento.DirecaoCaminho == DirecaoCarroRua.Volta))
                             {
                                 etapaAtual = StatusCarroMapa.Manobrando;
                             }
                             else if (ExtremoMaisProximo == 0 && (direcao == DirecaoCarroRua.Volta && Variaveis.OperacaoEmAndamento.DirecaoCaminho == DirecaoCarroRua.Ida))
                             {
                                 etapaAtual = StatusCarroMapa.Manobrando;
-                            }
+                            }*/
                             // Robô não está entrando na rua do começo para o fim, nem do fim para o começo
                             else
                             {
@@ -410,22 +431,35 @@ namespace AgroBase.Models.Operacoes
                                 // Se a etapa anterior não for CaminhandoRua, então significa que ele já iniciou a etapa de sair da rua
                                 else
                                 {
-                                    // Ainda tem ruas a serem percorridas, logo, o robô precisa realizar a manobra para seguir para a próxima rua
+                                    /*// Ainda tem ruas a serem percorridas, logo, o robô precisa realizar a manobra para seguir para a próxima rua
                                     if (!Variaveis.OperacaoEmAndamento.OpMapaGPS.Concluido)
                                     {
-                                        //etapaAtual = StatusCarroMapa.Manobrando;
+                                        etapaAtual = StatusCarroMapa.Manobrando;
-                                        Variaveis.OperacaoEmAndamento.Mapa.IdxRuaAtual++;
+                                        ManobraConcluida = false;
-                                        etapaAtual = StatusCarroMapa.Direcionando;
+                                    }*/
-                                    }
                                 }
                             }
                         }
                         // Estará se direcionando para o ponto inicial
                         else
+                        {
+                            // Saiu da margem de saída da rua
+                            if (_EtapaAtual == StatusCarroMapa.SaindoRua)
+                            {
+                                // Ainda tem ruas a serem percorridas, logo, o robô precisa realizar a manobra para seguir para a próxima rua
+                                if (!Variaveis.OperacaoEmAndamento.OpMapaGPS.Concluido)
+                                {
+                                    etapaAtual = StatusCarroMapa.Manobrando;
+                                    ManobraConcluida = false;
+                                }
+                            }
+                            // Não está na margem de entrada ou saída da rua, então está em estado de direcionamento até a rua da operação
+                            else
                             {
                                 etapaAtual = StatusCarroMapa.Direcionando;
                             }
                         }
+                    }
                     // Se o robô estiver dentro da entrelinha da cana
                     else if (dentroDaRua)
                     {
@@ -501,7 +535,7 @@ namespace AgroBase.Models.Operacoes
 
                         for (int i = 0; i < PontosRua.Count - 1; i++)
                         {
-                            somaAnguloRua += GPSService.CalcularOrientacao(PontosRua[i + 1], PontosRua[i]);
+                            somaAnguloRua += GPSService.CalcularOrientacao(Rua[0], Rua[1]);
                         }
                         double mediaAnguloRua = somaAnguloRua / (PontosRua.Count - 1);
 
@@ -545,7 +579,7 @@ namespace AgroBase.Models.Operacoes
                 }
             }
         }
-        public double MargemErroAngulo { get; set; } = 45;
+        public double MargemErroAngulo { get; set; } = 90;
         public DirecaoCarroRua Direcao
         {
             get
@@ -557,9 +591,9 @@ namespace AgroBase.Models.Operacoes
                 }
                 double mediaAngulo = Variaveis.OperacaoEmAndamento.Sensoriamento.AnguloCarro;
                 double anguloOposto = (mediaAngulo + 180) % 360;
-                double mediaAnguloRua = AnguloCaminho;
+                double mediaAnguloRua = Variaveis.OperacaoEmAndamento.Mapa.AnguloCaminho;
 
-                if ((mediaAngulo + MargemErroAngulo) >= mediaAnguloRua && (mediaAngulo - MargemErroAngulo) <= mediaAnguloRua)
+                /*if ((mediaAngulo + MargemErroAngulo) >= mediaAnguloRua && (mediaAngulo - MargemErroAngulo) <= mediaAnguloRua)
                 {
                     return DirecaoCarroRua.Ida;
                 }
@@ -568,6 +602,22 @@ namespace AgroBase.Models.Operacoes
                     return DirecaoCarroRua.Volta;
                 }
                 else
+                {
+                    return DirecaoCarroRua.Parado;
+                }*/
+                DirecaoCarroRua direcaoTrecho = !Variaveis.OperacaoEmAndamento.Mapa.TrajetoriaDinamica.Any() ? DirecaoCarroRua.Ida : 
+                    Variaveis.OperacaoEmAndamento.Mapa.TrajetoriaDinamica.Any(x => x.idxRua == Variaveis.OperacaoEmAndamento.Mapa.IdxRuaAtual) ?
+                    Variaveis.OperacaoEmAndamento.Mapa.TrajetoriaDinamica.FirstOrDefault(x => x.idxRua == Variaveis.OperacaoEmAndamento.Mapa.IdxRuaAtual).DirecaoTrecho : 
+                    Variaveis.OperacaoEmAndamento.Mapa.TrajetoriaDinamica.FirstOrDefault().DirecaoTrecho;
+                if ((mediaAngulo + MargemErroAngulo) >= mediaAnguloRua && (mediaAngulo - MargemErroAngulo) <= mediaAnguloRua)
+                {
+                    return direcaoTrecho;
+                }
+                else if ((anguloOposto + MargemErroAngulo) >= mediaAnguloRua && (anguloOposto - MargemErroAngulo) <= mediaAnguloRua)
+                {
+                    return direcaoTrecho == DirecaoCarroRua.Ida ? DirecaoCarroRua.Volta : DirecaoCarroRua.Ida;
+                }
+                else
                 {
                     return DirecaoCarroRua.Parado;
                 }
@@ -608,7 +658,7 @@ namespace AgroBase.Models.Operacoes
         {
             get
             {
-                if (Variaveis.OperacaoEmAndamento.Mapa.RuasPercorrer.Count() > Variaveis.OperacaoEmAndamento.Mapa.IdxRuaAtual)
+                if (Variaveis.OperacaoEmAndamento.Mapa.RuasPercorrer.Count() > Variaveis.OperacaoEmAndamento.Mapa.IdxRuaAtual && !Variaveis.OperacaoEmAndamento.Mapa.TrajetoriaDinamica.Any())
                 {
                     try
                     {
@@ -716,12 +766,13 @@ namespace AgroBase.Models.Operacoes
             get
             {
                 bool NaMargem = false;
+                List<GPSModel> Rua = Variaveis.OperacaoEmAndamento.Mapa.Trajetoria[Variaveis.OperacaoEmAndamento.Mapa.IdxRuaAtual];
                 GPSModel PontoRua = ExtremoMaisProximo == 0 ? PrimeiroPontoRua : UltimoPontoRua;
                 double Distancia = GPSService.DistanciaEntrePontos(GPSService.UltimaLeitura, PontoRua);
                 double DistanciaAnterior = GPSService.DistanciaEntrePontos(GPSService.PenultimaLeitura, PontoRua);
 
                 double DistanciaFinal = 0;
-                if (Distancia > DistanciaAnterior)
+                if (Distancia > DistanciaAnterior && (Distancia <= MargemLimiteEntradaRua * 2))
                 {
                     DistanciaFinal = Distancia * -1;
                 }

AgroBase/AgroBase/Models/Operacoes/OperacaoModel.cs

@@ -238,14 +238,6 @@ namespace AgroBase.Models
         public List<OperacaoSensoriamentoModel> Logs { get; set; } = new List<OperacaoSensoriamentoModel>();
         public List<GPSModel> GPSTrajetoria { get; set; } = new List<GPSModel>();
         public List<List<GPSModel>> GPSTrajetoriaRua { get; set; } = new List<List<GPSModel>>();
-        public DirecaoCarroRua DirecaoCaminho
-        {
-            get
-            {
-                DirecaoCarroRua direcaoCaminho = Mapa.IdxRuaAtual % 2 == 0 ? DirecaoCarroRua.Ida : DirecaoCarroRua.Volta;
-                return direcaoCaminho;
-            }
-        }
         public int QuantidadeCamerasSolo { get; set; } = 2;
         public List<CameraSoloModel> CamerasSolo { get; set; } = new List<CameraSoloModel>();
         public CameraCaminhoModel CameraCaminho { get; set; }
@@ -751,7 +743,7 @@ namespace AgroBase.Models
                     Pontos = Pontos.OrderBy(x => x.DataHora).ToList();
                     for (int i = 0; i < Pontos.Count() - 1; i++)
                     {
-                        somaAngulo += GPSService.CalcularOrientacao(Pontos[i + 1], Pontos[i]);
+                        somaAngulo += GPSService.CalcularOrientacao(Pontos[i], Pontos[i + 1]);
                     }
                     double mediaAngulo = somaAngulo / (Pontos.Count() - 1);
 

AgroBase/AgroBase/Models/Variaveis.cs

@@ -155,6 +155,25 @@ namespace AgroBase.Models
                 pgb.Minimum;
         }
 
+        public static double CalcularAlturaTriangulo(double a, double b, double c)
+        {
+            // Verifica se os lados formam um triângulo válido
+            if (a + b <= c || a + c <= b || b + c <= a)
+            {
+                return 0;
+            }
+
+            // Calcule a área do triângulo usando a fórmula de Herão
+            double s = (a + b + c) / 2.0; // Semiperímetro
+            double area = Math.Sqrt(s * (s - a) * (s - b) * (s - c));
+
+            // Calcule a altura em relação à base c
+            double altura = (2 * area) / c;
+
+            return altura;
+        }
+
+
     }
 
     public static class VariaveisPortas

AgroBase/AgroBase/Python/Output/folium/leaflet.rotatedMarker.js

@@ -0,0 +1,57 @@
+(function() {
+    // save these original methods before they are overwritten
+    var proto_initIcon = L.Marker.prototype._initIcon;
+    var proto_setPos = L.Marker.prototype._setPos;
+
+    var oldIE = (L.DomUtil.TRANSFORM === 'msTransform');
+
+    L.Marker.addInitHook(function () {
+        var iconOptions = this.options.icon && this.options.icon.options;
+        var iconAnchor = iconOptions && this.options.icon.options.iconAnchor;
+        if (iconAnchor) {
+            iconAnchor = (iconAnchor[0] + 'px ' + iconAnchor[1] + 'px');
+        }
+        this.options.rotationOrigin = this.options.rotationOrigin || iconAnchor || 'center bottom' ;
+        this.options.rotationAngle = this.options.rotationAngle || 0;
+
+        // Ensure marker keeps rotated during dragging
+        this.on('drag', function(e) { e.target._applyRotation(); });
+    });
+
+    L.Marker.include({
+        _initIcon: function() {
+            proto_initIcon.call(this);
+        },
+
+        _setPos: function (pos) {
+            proto_setPos.call(this, pos);
+            this._applyRotation();
+        },
+
+        _applyRotation: function () {
+            if(this.options.rotationAngle) {
+                this._icon.style[L.DomUtil.TRANSFORM+'Origin'] = this.options.rotationOrigin;
+
+                if(oldIE) {
+                    // for IE 9, use the 2D rotation
+                    this._icon.style[L.DomUtil.TRANSFORM] = 'rotate(' + this.options.rotationAngle + 'deg)';
+                } else {
+                    // for modern browsers, prefer the 3D accelerated version
+                    this._icon.style[L.DomUtil.TRANSFORM] += ' rotateZ(' + this.options.rotationAngle + 'deg)';
+                }
+            }
+        },
+
+        setRotationAngle: function(angle) {
+            this.options.rotationAngle = angle;
+            this.update();
+            return this;
+        },
+
+        setRotationOrigin: function(origin) {
+            this.options.rotationOrigin = origin;
+            this.update();
+            return this;
+        }
+    });
+})();

AgroBase/AgroBase/Python/Scripts/map-load.py

@@ -183,13 +183,22 @@ with open(caminho_completo_html, 'r+') as arquivo_html:
     # Script JavaScript para adicionar ao HTML, com o ID do mapa substituído
     script_atualizacao_marcador = f"""
     <script src="folium/mqtt.min.js"></script>
+    <script src="folium/leaflet.rotatedMarker.js"></script>
     <script>
+        let posicaoAtual = {{
+            lat: 0, 
+            long: 0
+        }};
+        
+        var customIcon = L.icon({{
+            iconUrl: 'folium/images/position_marker.png',
+            iconSize: [32, 32], 
+            iconAnchor: [16, 16], 
+            popupAnchor: [0, -16] 
+        }});
+        
         var marcadorDinamico = L.marker([0, 0], {{
-            icon: L.AwesomeMarkers.icon({{
+            icon: customIcon
-                icon: 'info-sign',
-                markerColor: 'red',
-                prefix: 'glyphicon'
-            }})
         }}).addTo({id_mapa});
     
         // Conectar ao broker MQTT
@@ -229,11 +238,46 @@ with open(caminho_completo_html, 'r+') as arquivo_html:
                 var novaLongitude = dados.longitude;
                 var novaPosicao = [novaLatitude, novaLongitude];
                 marcadorDinamico.setLatLng(novaPosicao);
+                
+                // Calcular angulo de rotacao
+                var angulo = calcularOrientacao(novaPosicao[0], novaPosicao[1], posicaoAtual.lat, posicaoAtual.long);
+
+                posicaoAtual.lat = novaPosicao[0];
+                posicaoAtual.long = novaPosicao[1];
+
+                // Rotacionar o marcador para o angulo calculado
+                marcadorDinamico.setRotationAngle(angulo);
+                
                 adicionarCoordenada("Tj", [novaLongitude, novaLatitude]);
                 {id_mapa}.setView(novaPosicao, {id_mapa}.getZoom());
             }}
             
         }});
+        
+        function calcularOrientacao(P1latitude, P1longitude, P2latitude, P2longitude) {{
+            // Converter latitudes e longitudes de graus para radianos
+            const latA = P1latitude * (Math.PI / 180.0);
+            const lonA = P1longitude * (Math.PI / 180.0);
+            const latB = P2latitude * (Math.PI / 180.0);
+            const lonB = P2longitude * (Math.PI / 180.0);
+
+            // Calcular a diferença de longitude
+            const deltaLon = lonB - lonA;
+
+            // Calcular a direção
+            const y = Math.sin(deltaLon) * Math.cos(latB);
+            const x = Math.cos(latA) * Math.sin(latB) - Math.sin(latA) * Math.cos(latB) * Math.cos(deltaLon);
+            const direcaoRadianos = Math.atan2(y, x);
+
+            // Converter a direção de radianos para graus
+            let direcaoGraus = direcaoRadianos * (180.0 / Math.PI);
+
+            // Normalizar a direção para que esteja no intervalo de 0 a 360 graus
+            direcaoGraus = (direcaoGraus + 360) % 360;
+
+            return direcaoGraus;
+        }}
+
     </script>
     """
     

AgroBase/AgroBase/Services/GPSService.cs

@@ -11,11 +11,13 @@ using System.Net.Http;
 using System.Text;
 using System.Threading.Tasks;
 using System.Windows.Forms;
+using static AgroBase.Models.Enuns;
 
 namespace AgroBase.Services
 {
     public class GPSService
     {
+        public static double RaioDaTerra = 6378137; // Raio da Terra em Km
         public static bool Iniciado = false;
         public static SerialPort PortaGPS = null;
         public static GPSModel UltimaLeitura = new GPSModel();
@@ -200,6 +202,144 @@ namespace AgroBase.Services
             return direcaoGraus;
         }
 
+        public static double NormalizarAngulo(double angulo)
+        {
+            if (angulo < 0)
+            {
+                angulo += 360;
+            }
+            angulo %= 360;
+            return angulo;
+        }
+
+        public static double CalcularDiferencaAngulo(double anguloVariavel, double anguloRef)
+        {
+            // Calcula a diferença entre o ângulo do carro e o ângulo da rua
+            double diferenca = anguloVariavel - anguloRef;
+
+            // Normaliza a diferença para estar entre -180 e 180 graus
+            diferenca = ((diferenca + 180) % 360) - 180;
+
+            return diferenca;
+        }
+
+        public static GPSModel PontoMaisProximoRuaProjetada(List<GPSModel> RuaProjetada)
+        {
+            /*if (!RuaProjetada.Any())
+            {
+                return new GPSModel();
+            }
+            int idx = -1;
+            double menorDistancia = 9999;
+            double distanciaAnterior = 9999;
+            bool Afastando = false;
+            bool Aproximando = false;
+            bool Inverteu = false;
+
+            var Rua = RuaProjetada;
+
+            foreach (var Ponto in Rua)
+            {
+                int idxPonto = Rua.IndexOf(Ponto);
+
+
+                double Distancia = DistanciaEntrePontos(UltimaLeitura, Ponto);
+                double DistanciaAnterior = DistanciaEntrePontos(PenultimaLeitura, Ponto);
+
+                double DistanciaFinal = Distancia;
+                if (Distancia > DistanciaAnterior)
+                {
+                    if (!Aproximando)
+                    {
+                        Afastando = true;
+                    }
+                    else
+                    {
+                        Inverteu = true;
+                    }
+                    DistanciaFinal = Distancia * -1;
+                }
+                else
+                {
+                    if (!Afastando)
+                    {
+                        Aproximando = true;
+                    }
+                    else
+                    {
+                        Inverteu = true;
+                    }
+                    DistanciaFinal = Distancia;
+                }
+
+
+
+                double distPonto = DistanciaEntrePontos(UltimaLeitura, Rua.First());
+                double distTrecho = DistanciaDoTrecho(Rua.Where(x => Rua.IndexOf(x) <= idxPonto).ToList());
+                double dist = distPonto + distTrecho;
+                if ((Aproximando && DistanciaFinal < menorDistancia) || (Afastando && DistanciaFinal > menorDistancia))
+                {
+                    menorDistancia = DistanciaFinal;
+                    idx = Rua.IndexOf(Ponto);
+                }
+                else if (dist > distanciaAnterior)
+                {
+                    break;
+                }
+                if (Inverteu)
+                {
+                    break;
+                }
+                distanciaAnterior = DistanciaFinal;
+            }
+            return Rua[idx];*/
+
+            // Verifica se a lista está vazia
+            if (RuaProjetada == null || RuaProjetada.Count == 0)
+            {
+                return null;
+            }
+
+            // Inicializa variáveis
+            GPSModel pontoMaisProximo = null;
+            double menorDistancia = double.MaxValue;
+            bool estaAproximando = false;
+
+            // Calcula a direção (aproximando ou afastando) com base nas primeiras distâncias
+            double distanciaAtual = DistanciaEntrePontos(UltimaLeitura, RuaProjetada[0]);
+            double distanciaAnterior = DistanciaEntrePontos(PenultimaLeitura, RuaProjetada[0]);
+            estaAproximando = distanciaAtual < distanciaAnterior;
+
+            // Itera sobre os pontos da rua projetada
+            foreach (var ponto in RuaProjetada)
+            {
+                // Calcula a distância entre a última leitura e o ponto atual
+                distanciaAtual = DistanciaEntrePontos(UltimaLeitura, ponto);
+                distanciaAnterior = DistanciaEntrePontos(PenultimaLeitura, ponto);
+
+                // Atualiza o ponto mais próximo e a menor distância se apropriado
+                if (distanciaAtual <= menorDistancia)
+                {
+                    menorDistancia = distanciaAtual;
+                    pontoMaisProximo = ponto;
+                }
+
+                estaAproximando = distanciaAtual < distanciaAnterior;
+
+                
+
+                // Verifica se houve inversão na direção
+                if (estaAproximando && pontoMaisProximo != ponto)
+                {
+                    // O robô começou a se afastar
+                    break;
+                }
+            }
+
+            return pontoMaisProximo;
+
+        }
+
         public static double DistanciaEntrePontos(GPSModel P1, GPSModel P2)
         {
             double lat1 = P1.Latitude;
@@ -207,7 +347,7 @@ namespace AgroBase.Services
             double lon1 = P1.Longitude;
             double lon2 = P2.Longitude;
 
-            double R = 6371.0; // Raio da Terra em Km
+            
             double dLat = ToRadians(lat2 - lat1);
             double dLon = ToRadians(lon2 - lon1);
             lat1 = ToRadians(lat1);
@@ -216,10 +356,7 @@ namespace AgroBase.Services
             double a = Math.Sin(dLat / 2) * Math.Sin(dLat / 2) +
                        Math.Sin(dLon / 2) * Math.Sin(dLon / 2) * Math.Cos(lat1) * Math.Cos(lat2);
             double c = 2 * Math.Atan2(Math.Sqrt(a), Math.Sqrt(1 - a));
-            double distance = R * c;
+            double distance = RaioDaTerra * c;
-
-            // Converter para centímetros
-            distance = distance * 1000;
 
             //distance = distance * 0.5;
 
@@ -236,6 +373,50 @@ namespace AgroBase.Services
             return d;
         }
 
+        public static bool CompararDirecaoTrajeto(List<GPSModel> Trecho1, List<GPSModel> Trecho2)
+        {
+            double MargemErroAngulo = 45;
+            double anguloTrecho1 = CalcularOrientacao(Trecho1[0], Trecho1[Trecho1.Count - 1]);
+            double anguloTrecho2 = CalcularOrientacao(Trecho2[0], Trecho2[Trecho2.Count - 1]);
+
+            if ((anguloTrecho1 + MargemErroAngulo) >= anguloTrecho2 && (anguloTrecho1 - MargemErroAngulo) <= anguloTrecho2)
+            {
+                return true;
+            }
+            else
+            {
+                return false;
+            }
+        }
+
+        public static GPSModel ProjetarPontoDeslocado(GPSModel pontoOriginal, double distancia, double angulo)
+        {
+            // Convertendo ângulo em radianos
+            double anguloRad = (Math.PI / 180) * angulo;
+
+            // Calculando deslocamento
+            double deltaLat = (distancia * Math.Cos(anguloRad)) / RaioDaTerra;
+            double deltaLon = (distancia * Math.Sin(anguloRad)) / (RaioDaTerra * Math.Cos(pontoOriginal.Latitude * Math.PI / 180));
+
+            // Convertendo deslocamento de radianos para graus
+            deltaLat = deltaLat * (180 / Math.PI);
+            deltaLon = deltaLon * (180 / Math.PI);
+
+            // Criando novo ponto com o deslocamento
+            GPSModel pontoDeslocado = new GPSModel
+            {
+                Latitude = pontoOriginal.Latitude + deltaLat,
+                Longitude = pontoOriginal.Longitude + deltaLon,
+                Altitude = pontoOriginal.Altitude,
+                DataHora = pontoOriginal.DataHora,
+                NumeroSatelites = pontoOriginal.NumeroSatelites,
+                PrecisaoHorizontal = pontoOriginal.PrecisaoHorizontal,
+                Velocidade = pontoOriginal.Velocidade
+            };
+
+            return pontoDeslocado;
+        }
+
         private static double ToRadians(double angle)
         {
             return Math.PI / 180 * angle;
@@ -307,6 +488,15 @@ namespace AgroBase.Services
             return pontos;
         }
 
+        public static GPSModel PontoMedio(GPSModel P1, GPSModel P2)
+        {
+            return new GPSModel()
+            {
+                Latitude = (P1.Latitude + P2.Latitude) / 2, 
+                Longitude = (P1.Longitude + P2.Longitude) / 2, 
+            };
+        }
+
 
         private static MapaFeatureCollectionModel TrajetoFake = new MapaFeatureCollectionModel();
         private static int idxPonto = 0;

AgroBase/AgroBase/bin/Debug/Python/Output/folium/leaflet.rotatedMarker.js

@@ -0,0 +1,57 @@
+(function() {
+    // save these original methods before they are overwritten
+    var proto_initIcon = L.Marker.prototype._initIcon;
+    var proto_setPos = L.Marker.prototype._setPos;
+
+    var oldIE = (L.DomUtil.TRANSFORM === 'msTransform');
+
+    L.Marker.addInitHook(function () {
+        var iconOptions = this.options.icon && this.options.icon.options;
+        var iconAnchor = iconOptions && this.options.icon.options.iconAnchor;
+        if (iconAnchor) {
+            iconAnchor = (iconAnchor[0] + 'px ' + iconAnchor[1] + 'px');
+        }
+        this.options.rotationOrigin = this.options.rotationOrigin || iconAnchor || 'center bottom' ;
+        this.options.rotationAngle = this.options.rotationAngle || 0;
+
+        // Ensure marker keeps rotated during dragging
+        this.on('drag', function(e) { e.target._applyRotation(); });
+    });
+
+    L.Marker.include({
+        _initIcon: function() {
+            proto_initIcon.call(this);
+        },
+
+        _setPos: function (pos) {
+            proto_setPos.call(this, pos);
+            this._applyRotation();
+        },
+
+        _applyRotation: function () {
+            if(this.options.rotationAngle) {
+                this._icon.style[L.DomUtil.TRANSFORM+'Origin'] = this.options.rotationOrigin;
+
+                if(oldIE) {
+                    // for IE 9, use the 2D rotation
+                    this._icon.style[L.DomUtil.TRANSFORM] = 'rotate(' + this.options.rotationAngle + 'deg)';
+                } else {
+                    // for modern browsers, prefer the 3D accelerated version
+                    this._icon.style[L.DomUtil.TRANSFORM] += ' rotateZ(' + this.options.rotationAngle + 'deg)';
+                }
+            }
+        },
+
+        setRotationAngle: function(angle) {
+            this.options.rotationAngle = angle;
+            this.update();
+            return this;
+        },
+
+        setRotationOrigin: function(origin) {
+            this.options.rotationOrigin = origin;
+            this.update();
+            return this;
+        }
+    });
+})();

AgroBase/AgroBase/bin/Debug/Python/Scripts/map-load.py

@@ -183,13 +183,22 @@ with open(caminho_completo_html, 'r+') as arquivo_html:
     # Script JavaScript para adicionar ao HTML, com o ID do mapa substituído
     script_atualizacao_marcador = f"""
     <script src="folium/mqtt.min.js"></script>
+    <script src="folium/leaflet.rotatedMarker.js"></script>
     <script>
+        let posicaoAtual = {{
+            lat: 0, 
+            long: 0
+        }};
+        
+        var customIcon = L.icon({{
+            iconUrl: 'folium/images/position_marker.png',
+            iconSize: [32, 32], 
+            iconAnchor: [16, 16], 
+            popupAnchor: [0, -16] 
+        }});
+        
         var marcadorDinamico = L.marker([0, 0], {{
-            icon: L.AwesomeMarkers.icon({{
+            icon: customIcon
-                icon: 'info-sign',
-                markerColor: 'red',
-                prefix: 'glyphicon'
-            }})
         }}).addTo({id_mapa});
     
         // Conectar ao broker MQTT
@@ -229,11 +238,46 @@ with open(caminho_completo_html, 'r+') as arquivo_html:
                 var novaLongitude = dados.longitude;
                 var novaPosicao = [novaLatitude, novaLongitude];
                 marcadorDinamico.setLatLng(novaPosicao);
+                
+                // Calcular angulo de rotacao
+                var angulo = calcularOrientacao(novaPosicao[0], novaPosicao[1], posicaoAtual.lat, posicaoAtual.long);
+
+                posicaoAtual.lat = novaPosicao[0];
+                posicaoAtual.long = novaPosicao[1];
+
+                // Rotacionar o marcador para o angulo calculado
+                marcadorDinamico.setRotationAngle(angulo);
+                
                 adicionarCoordenada("Tj", [novaLongitude, novaLatitude]);
                 {id_mapa}.setView(novaPosicao, {id_mapa}.getZoom());
             }}
             
         }});
+        
+        function calcularOrientacao(P1latitude, P1longitude, P2latitude, P2longitude) {{
+            // Converter latitudes e longitudes de graus para radianos
+            const latA = P1latitude * (Math.PI / 180.0);
+            const lonA = P1longitude * (Math.PI / 180.0);
+            const latB = P2latitude * (Math.PI / 180.0);
+            const lonB = P2longitude * (Math.PI / 180.0);
+
+            // Calcular a diferença de longitude
+            const deltaLon = lonB - lonA;
+
+            // Calcular a direção
+            const y = Math.sin(deltaLon) * Math.cos(latB);
+            const x = Math.cos(latA) * Math.sin(latB) - Math.sin(latA) * Math.cos(latB) * Math.cos(deltaLon);
+            const direcaoRadianos = Math.atan2(y, x);
+
+            // Converter a direção de radianos para graus
+            let direcaoGraus = direcaoRadianos * (180.0 / Math.PI);
+
+            // Normalizar a direção para que esteja no intervalo de 0 a 360 graus
+            direcaoGraus = (direcaoGraus + 360) % 360;
+
+            return direcaoGraus;
+        }}
+
     </script>
     """
     

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

@@ -455,3 +455,5 @@ C:\ZendionInc\agrobot_base\AgroBase\AgroBase\bin\Debug\Python\Models\yolov7\mode
 C:\ZendionInc\agrobot_base\AgroBase\AgroBase\bin\Debug\Python\Models\yolov7\models\__pycache__\yolo.cpython-311.pyc
 C:\ZendionInc\agrobot_base\AgroBase\AgroBase\bin\Debug\Python\Models\yolov7\models\__pycache__\__init__.cpython-311.pyc
 C:\ZendionInc\agrobot_base\AgroBase\AgroBase\bin\Debug\Python\Scripts\weed-detector-v7.py
+C:\ZendionInc\agrobot_base\AgroBase\AgroBase\bin\Debug\Python\Output\folium\images\position_marker.png
+C:\ZendionInc\agrobot_base\AgroBase\AgroBase\bin\Debug\Python\Output\folium\leaflet.rotatedMarker.js