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

ajustado algoritmo para movimentacao autonoma

This commit is contained in:
Diego Freitas 2024-05-03 16:55:06 -03:00
parent 800a5530c6
commit 1bcfee7c23
33 changed files with 1176 additions and 169 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

BIN
AgroBase/.vs/AgroBase/FileContentIndex/156227cf-6d01-4901-9bf2-a3fde9507533.vsidx Normal file
View File

Binary file not shown.

BIN
AgroBase/.vs/AgroBase/FileContentIndex/35338bfa-1c68-463a-8e26-3ce56d34c1d3.vsidx
View File

Binary file not shown.

BIN
AgroBase/.vs/AgroBase/FileContentIndex/88e0d923-5b71-4f41-94b0-1ca5eccd2dea.vsidx Normal file
View File

Binary file not shown.

BIN
AgroBase/.vs/AgroBase/FileContentIndex/91ead007-94ea-4bfe-8c01-63da7bd12c73.vsidx
View File

Binary file not shown.

BIN
AgroBase/.vs/AgroBase/FileContentIndex/936e34ae-a1d1-4a09-a436-cbf616ea53aa.vsidx Normal file
View File

Binary file not shown.

BIN
AgroBase/.vs/AgroBase/FileContentIndex/aea47805-1e35-470a-83c7-678c5404eae5.vsidx
View File

Binary file not shown.

BIN
AgroBase/.vs/AgroBase/v17/.suo
View File

Binary file not shown.

6
AgroBase/AgroBase/AgroBase.csproj
View File

@ -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>

6
AgroBase/AgroBase/Forms/Operacoes/frmOperacaoMapaGPS.cs
View File

@ -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();

110
AgroBase/AgroBase/Forms/frmSimulacaoMapaGPS.Designer.cs generated
View File

@ -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.btnCalcularAngulo = new System.Windows.Forms.Button();
this.lblDistanciaLateral = new System.Windows.Forms.Label();
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.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);
//
// 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);
this.lblDistanciaLateral.AutoSize = true;
this.lblDistanciaLateral.Font = new System.Drawing.Font("Microsoft Sans Serif", 7.8F);
this.lblDistanciaLateral.Location = new System.Drawing.Point(154, 84);
this.lblDistanciaLateral.Margin = new System.Windows.Forms.Padding(2, 0, 2, 0);
this.lblDistanciaLateral.Name = "lblDistanciaLateral";
this.lblDistanciaLateral.Size = new System.Drawing.Size(117, 13);
this.lblDistanciaLateral.TabIndex = 23;
this.lblDistanciaLateral.Text = "Esq: 0.00 m Dir: 0.00 m";
//
// 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;
}
}

50
AgroBase/AgroBase/Forms/frmSimulacaoMapaGPS.cs
View File

@ -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];
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();
}
Variaveis.OperacaoEmAndamento.Mapa.GerarTrajetoriaDinamica();
}
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;
DesenharInclinacao((Panel)sender, e, (float)angulo, -90);
double angulo = Variaveis.OperacaoEmAndamento.Mapa.AnguloCaminho;
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();
}
}
}

3
AgroBase/AgroBase/Models/Enuns.cs
View File

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

5
AgroBase/AgroBase/Models/GPSModel.cs
View File

@ -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;
}
}

483
AgroBase/AgroBase/Models/MapasModel.cs
View File

@ -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);

141
AgroBase/AgroBase/Models/Operacoes/OperacaoMapaGPSModel.cs
View File

@ -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
if ((anguloCarro + margem) >= anguloRua && (anguloCarro - margem) <= anguloRua)
{
erroOrientacao = anguloCarro - anguloRua;
}
else if ((anguloOposto + margem) >= anguloRua && (anguloOposto - margem) <= anguloRua)
{
erroOrientacao = anguloOposto - anguloRua;
}
// Calcula a diferença de ângulo entre o carro e a rua
double diferencaAngulo = GPSService.CalcularDiferencaAngulo(anguloCarro, anguloRua);
erroOrientacao = diferencaAngulo;
Sensoriamento.AnguloDif = erroOrientacao;
// Assume-se que o anguloCamera reflete o desvio lateral.
// Isso pode precisar de ajuste dependendo de como o ângulo é medido.
double erroPosicaoLateral = anguloCamera;
// Calcule o erro lateral baseado nas distâncias
double erroLateral = Variaveis.OperacaoEmAndamento.Mapa.DistanciaEsquerda - Variaveis.OperacaoEmAndamento.Mapa.DistanciaDireita;
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 pesoPosicaoLateral = statusCarroRua == StatusCarroMapa.Direcionando ? 0.05 : 0.7;
double pesoOrientacao = StatusDentroRua.Contains(statusCarroRua) ? PesoOrientacaoDentroRua : 1 - PesoOrientacaoDentroRua;
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;
CalcularManobra();
Controle.RPM = Controle.RPM_Min;
}
}
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>()
{
GPSService.PenultimaLeitura,
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))
{
ManobraConcluida = false;
etapaAtual = StatusCarroMapa.EntrandoRua;
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,20 +431,33 @@ 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;
Variaveis.OperacaoEmAndamento.Mapa.IdxRuaAtual++;
etapaAtual = StatusCarroMapa.Direcionando;
}
etapaAtual = StatusCarroMapa.Manobrando;
ManobraConcluida = false;
}*/
}
}
}
// Estará se direcionando para o ponto inicial
else
{
etapaAtual = StatusCarroMapa.Direcionando;
// 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
@ -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;
}

10
AgroBase/AgroBase/Models/Operacoes/OperacaoModel.cs
View File

@ -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);

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

@ -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

BIN
AgroBase/AgroBase/Python/Output/folium/images/position_marker.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 13 KiB

57
AgroBase/AgroBase/Python/Output/folium/leaflet.rotatedMarker.js Normal file
View File

@ -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;
}
});
})();

54
AgroBase/AgroBase/Python/Scripts/map-load.py
View File

@ -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: 'info-sign',
markerColor: 'red',
prefix: 'glyphicon'
}})
icon: customIcon
}}).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>
"""

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

@ -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;
// Converter para centímetros
distance = distance * 1000;
double distance = RaioDaTerra * c;
//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;

BIN
AgroBase/AgroBase/bin/Debug/AgroBase.pdb
View File

Binary file not shown.

BIN
AgroBase/AgroBase/bin/Debug/DawnCache/data_1
View File

Binary file not shown.

BIN
AgroBase/AgroBase/bin/Debug/GPUCache/data_1
View File

Binary file not shown.

BIN
AgroBase/AgroBase/bin/Debug/Python/Output/folium/images/position_marker.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 13 KiB

57
AgroBase/AgroBase/bin/Debug/Python/Output/folium/leaflet.rotatedMarker.js Normal file
View File

@ -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;
}
});
})();

88
AgroBase/AgroBase/bin/Debug/Python/Output/mapa_interativo.html
View File

File diff suppressed because one or more lines are too long

54
AgroBase/AgroBase/bin/Debug/Python/Scripts/map-load.py
View File

@ -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: 'info-sign',
markerColor: 'red',
prefix: 'glyphicon'
}})
icon: customIcon
}}).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>
"""

2
AgroBase/AgroBase/obj/Debug/AgroBase.csproj.FileListAbsolute.txt
View File

@ -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

BIN
AgroBase/AgroBase/obj/Debug/AgroBase.csproj.GenerateResource.cache
View File

Binary file not shown.

BIN
AgroBase/AgroBase/obj/Debug/AgroBase.pdb
View File

Binary file not shown.

BIN
AgroBase/AgroBase/obj/Debug/DesignTimeResolveAssemblyReferences.cache
View File

Binary file not shown.

BIN
AgroBase/AgroBase/obj/Debug/DesignTimeResolveAssemblyReferencesInput.cache
View File

Binary file not shown.