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

ajustes no mpc

This commit is contained in:
Diego Freitas 2025-04-23 15:41:11 -03:00
parent 9b9636545d
commit a039e3c6a6
24 changed files with 217 additions and 111 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

BIN
AgroBase/.vs/AgroBase/FileContentIndex/5ac8e0ec-7da6-47aa-9bd1-360cf85d479f.vsidx → AgroBase/.vs/AgroBase/FileContentIndex/d49ce5da-b2e5-477c-b22d-f7488c1c0c03.vsidx
View File

Binary file not shown.

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

Binary file not shown.

5
AgroBase/AgroBase/Forms/Operacoes/frmResultadosOperacao.cs
View File

@ -1203,7 +1203,8 @@ namespace AgroBase.Forms.Operacoes
_trajaetoria,
null, //Variaveis.OperacaoEmAndamento.Mapa.TrajetoriaProjetada.FirstOrDefault(),
new List<GPSModel>(LogsGPS.Take(LogsGPS.IndexOf(Log) + 1)),
new List<List<GPSModel>>()
new List<List<GPSModel>>(),
LogOpe.Controle.ControladorMPC?.ComandoAtual?.simulacao
);
}
@ -1540,7 +1541,7 @@ namespace AgroBase.Forms.Operacoes
private void pnlOrientacaoMagnetometro_Paint(object sender, PaintEventArgs e)
{
var Log = LogsSensoriamento.FirstOrDefault(x => x.Momento == MomentoAtual).SensoresIMU.FirstOrDefault();
var Log = LogsSensoriamento.FirstOrDefault(x => x.Momento == MomentoAtual)?.SensoresIMU?.FirstOrDefault();
double Angulo = (Log?.Yaw ?? 0);
double AnguloCorrigido = (Log?.YawFixed ?? 0);

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

@ -217,59 +217,42 @@ namespace AgroBase.Forms
var _Gps = Variaveis.OperacaoEmAndamento.Sensoriamento.Gps;
double anguloControle = 0.0;
TipoMovimentoDirecional tipoMovimento = TipoMovimentoDirecional.RodasDianteiras;
TipoMovimentoDirecional tipoMovimento = Variaveis.OperacaoEmAndamento.Controle.TipoMovimento;
var tipoControle = Variaveis.OperacaoEmAndamento.Controle.TipoControleDirecional;
switch (tipoControle)
if (passosOperacao.Count > 0 && Variaveis.OperacaoEmAndamento.Simulando)
{
case TiposControladorDirecional.PID:
{
tipoMovimento = Variaveis.OperacaoEmAndamento.Controle.TipoMovimento;
novaPosicao = passosOperacao[Math.Min(passosOperacao.Count - 1, idxPassoOperacao)];
idxPassoOperacao++;
}
else
{
if (Variaveis.OperacaoEmAndamento.StatusAtual == StatusOperacao.Concluido || Variaveis.OperacaoEmAndamento.StatusAtual == StatusOperacao.Aguardando)
{
return;
}
if (passosOperacao.Count > 0 && Variaveis.OperacaoEmAndamento.Simulando)
{
novaPosicao = passosOperacao[Math.Min(passosOperacao.Count - 1, idxPassoOperacao)];
idxPassoOperacao++;
}
else
{
if (Variaveis.OperacaoEmAndamento.StatusAtual == StatusOperacao.Concluido || Variaveis.OperacaoEmAndamento.StatusAtual == StatusOperacao.Aguardando)
{
return;
}
anguloControle = double.Parse(txtAnguloControle.Text);
var ControleDir = Variaveis.OperacaoEmAndamento.Controle.TiposControle.FirstOrDefault(x => x.Tipo == T_Code.Dir);
anguloControle = double.Parse(txtAnguloControle.Text);
double.TryParse(txtErroTempoComando.Text, out double fatorErro);
int ticksMax = (int)(ticksControleMax * Math.Max(1, fatorErro));
double.TryParse(txtErroTempoComando.Text, out double fatorErro);
int ticksMax = (int)(ticksControleMax * Math.Max(1, fatorErro));
bool atualizarAnguloControle = (ticksControle >= ticksMax);
if (atualizarAnguloControle)
{
ticksControle = 0;
}
else
{
ticksControle++;
}
bool atualizarAnguloControle = (ticksControle >= ticksMax);
if (atualizarAnguloControle)
{
ticksControle = 0;
}
else
{
ticksControle++;
}
if (atualizarAnguloControle)
{
btnCalcularAngulo_Click(new object(), new EventArgs());
anguloControle = double.Parse(txtAnguloControle.Text);
}
}
break;
}
case TiposControladorDirecional.MPC:
{
MPCComandoModel comando = Variaveis.OperacaoEmAndamento.Controle.ControladorMPC.ComandoAtual;
anguloControle = comando?.angulo ?? 0;
tipoMovimento = comando?.tipo ?? TipoMovimentoDirecional.RodasDianteiras;
break;
}
if (atualizarAnguloControle)
{
btnCalcularAngulo_Click(new object(), new EventArgs());
anguloControle = double.Parse(txtAnguloControle.Text);
}
}
// Calcula a nova posicao do robo
@ -416,7 +399,7 @@ namespace AgroBase.Forms
double anguloControle = Variaveis.OperacaoEmAndamento.SimulacaoAnguloControle;
if (historicoAngulosControle.Count >= errosConsiderar && errosConsiderar > 0)
if (historicoAngulosControle.Count >= errosConsiderar || errosConsiderar == 0)
historicoAngulosControle.Dequeue(); // Remove o valor mais antigo
historicoAngulosControle.Enqueue(anguloControle);

9
AgroBase/AgroBase/Models/MPCController.cs
View File

@ -13,8 +13,8 @@ public class MPCController
private string TopicoRota = "mpc/rota";
private string TopicoPosicao = "mpc/posicao";
private string TopicoComando = "mpc/comando";
private int Horizonte = 20;
private double _dt = 0;
private double Horizonte = 4.0;
private double _dt = 0.0;
private DateTime _ultimaAtualizacao = DateTime.MinValue;
public Process pythonProcess;
public readonly object processLock = new object();
@ -110,6 +110,11 @@ public class MPCController
theta = _theta,
velocidade = _velocidadeMs,
dt = dtCalc,
comando_anterior = new
{
angulo = _Sensoriamento.Controle.Angulo,
tipo = _Sensoriamento.Controle.TipoMovimento,
},
contexto = new
{
StatusCarro = (int)_Sensoriamento.Trajetoria.StatusCarro,

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

@ -300,7 +300,20 @@ namespace AgroBase.Models
#region PARAMETRIZACAO
public bool SalvarParametrizacaoOperacao(string descricao, string capacidade, string pressao, string duracao, string percentual, string angulo, string velocidadeMp, int velocidadeServasPercent, int velocidadeCervasPercent, UltrasonicA05Configuracoes ConfigUltrassom, bool sonarAtivado, int qtdCamerasSolo)
public bool SalvarParametrizacaoOperacao(
string descricao,
string capacidade,
string pressao,
string duracao,
string percentual,
string angulo,
string velocidadeMp,
int velocidadeServasPercent,
int velocidadeCervasPercent,
UltrasonicA05Configuracoes ConfigUltrassom,
bool sonarAtivado,
int qtdCamerasSolo,
TiposControladorDirecional tipoControleDirecional)
{
if (string.IsNullOrEmpty(descricao))
{
@ -325,6 +338,7 @@ namespace AgroBase.Models
AtuPressaoLinha = pressaoLinha,
DirAnguloMaximo = anguloMaximo,
DirVelocidadeMovimento = velocidadeMovimento,
DirTipoMovimento = tipoControleDirecional,
MovVelocidadeCErvasPercent = velocidadeCervasPercent,
MovVelocidadeSErvasPercent = velocidadeServasPercent,
ModulosMandatorios = Variaveis.OperacaoEmAndamento.ModulosMandatorios,
@ -388,9 +402,14 @@ namespace AgroBase.Models
PercentualVelocidadeSP = 0,
PIDdirecional = new PIDModel(),
FiltroVelocidade = new KalmanFilter(),
ControladorMPC = new MPCController(),
TiposControle = Variaveis.OperacaoEmAndamento.Controle.TiposControle.ToList(),
SonarAtivado = Parametros.SonarAtivado,
SegmentacaoSemantica = Parametros.Modo == ModoOperacao.MapaGPS,
TipoControleDirecional = Parametros.DirTipoMovimento,
PulverizadorAutomatico = Parametros.Modo == ModoOperacao.MapaGPS,
TipoMovimento = TipoMovimentoDirecional.RodasDianteiras,
SonarDesvioAutomatico = false,
};
Variaveis.OperacaoEmAndamento.QuantidadeCamerasSolo = Parametros.QtdCamerasSolo;
@ -548,7 +567,7 @@ namespace AgroBase.Models
new OperacaoControleTipoModel()
{
Tipo = T_Code.Dir,
DelayEnvioComando = 500,
DelayEnvioComando = 200,
Comandos = new List<string>(),
UltimoComando = DateTime.Now
},

1
AgroBase/AgroBase/Models/Operacoes/OperacaoParametrosModel.cs
View File

@ -12,6 +12,7 @@ namespace AgroBase.Models.Operacoes
public int MovVelocidadeCErvasPercent { get; set; }
public int DirAnguloMaximo { get; set; }
public int DirVelocidadeMovimento { get; set; }
public TiposControladorDirecional DirTipoMovimento { get; set; }
public int AtuPercentualDeteccaoVertical { get; set; }
public double AtuCapacidadeReservatorio { get; set; }
public double AtuPressaoLinha { get; set; }

1
AgroBase/AgroBase/Services/SerialService.cs
View File

@ -680,6 +680,7 @@ namespace AgroBase.Services
{
try
{
Porta.BaudRate = 115200;
Porta.Open();
await Task.Delay(100);
AtualizarConsole($"{Porta.PortName} - Procurando dispositivo GPS");

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.

2
AgroBase/AgroBase/bin/Debug/OperacoesSalvas/OP_CJ_Terreno.opr
View File

File diff suppressed because one or more lines are too long

1
AgroBase/AgroBase/bin/Debug/OperacoesSalvas/OP_CJ_Terreno_Debug.opr
View File

File diff suppressed because one or more lines are too long

1
AgroBase/AgroBase/bin/Debug/OperacoesSalvas/OP_CJ_Terreno_MPC.opr Normal file
View File

File diff suppressed because one or more lines are too long

1
AgroBase/AgroBase/bin/Debug/OperacoesSalvas/OP_CJ_Terreno_PID.opr Normal file
View File

File diff suppressed because one or more lines are too long

2
AgroBase/AgroBase/bin/Debug/Parametros/parametersMvd-2_0.par
View File

File diff suppressed because one or more lines are too long

2
AgroBase/AgroBase/bin/Debug/Parametros/parametersSen-1_2.par
View File

File diff suppressed because one or more lines are too long

18
AgroBase/AgroBase/bin/Debug/Python/Output/mapa_gps.html
View File

@ -25,7 +25,7 @@
<meta name="viewport" content="width=device-width,
initial-scale=1.0, maximum-scale=1.0, user-scalable=no" />
<style>
#map_d89f9773ee899027809e6745084649e1 {
#map_7f2f505618d18ff23c11f62484118a2c {
position: relative;
width: 100.0%;
height: 100.0%;
@ -39,14 +39,14 @@
<body>
<div class="folium-map" id="map_d89f9773ee899027809e6745084649e1" ></div>
<div class="folium-map" id="map_7f2f505618d18ff23c11f62484118a2c" ></div>
</body>
<script>
var map_d89f9773ee899027809e6745084649e1 = L.map(
"map_d89f9773ee899027809e6745084649e1",
var map_7f2f505618d18ff23c11f62484118a2c = L.map(
"map_7f2f505618d18ff23c11f62484118a2c",
{
center: [0.0, 0.0],
crs: L.CRS.EPSG3857,
@ -60,13 +60,13 @@
var tile_layer_eb04a0a8d26a9cc90463b40733770d06 = L.tileLayer(
var tile_layer_4c72109091f8651b6e2b819865fbc3d5 = L.tileLayer(
"https://tile.openstreetmap.org/{z}/{x}/{y}.png",
{"attribution": "\u0026copy; \u003ca href=\"https://www.openstreetmap.org/copyright\"\u003eOpenStreetMap\u003c/a\u003e contributors", "detectRetina": false, "maxNativeZoom": 19, "maxZoom": 19, "minZoom": 0, "noWrap": false, "opacity": 1, "subdomains": "abc", "tms": false}
);
tile_layer_eb04a0a8d26a9cc90463b40733770d06.addTo(map_d89f9773ee899027809e6745084649e1);
tile_layer_4c72109091f8651b6e2b819865fbc3d5.addTo(map_7f2f505618d18ff23c11f62484118a2c);
</script>
@ -87,7 +87,7 @@
}
trajeto_json_add({"features": []});
trajeto_json.addTo(map_d89f9773ee899027809e6745084649e1);
trajeto_json.addTo(map_7f2f505618d18ff23c11f62484118a2c);
function adicionarGeometria(novaGeometria) {
trajeto_json.addData(novaGeometria);
@ -145,7 +145,7 @@
var marcadorDinamico = L.marker([0, 0], {
icon: customIcon
}).addTo(map_d89f9773ee899027809e6745084649e1);
}).addTo(map_7f2f505618d18ff23c11f62484118a2c);
// Conectar ao broker MQTT
const client = mqtt.connect('ws://localhost:9001'); // Use wss para conexão segura
@ -183,7 +183,7 @@
marcadorDinamico.setRotationAngle(angulo);
adicionarCoordenada("Tj", [novaLongitude, novaLatitude]);
map_d89f9773ee899027809e6745084649e1.setView(novaPosicao, map_d89f9773ee899027809e6745084649e1.getZoom());
map_7f2f505618d18ff23c11f62484118a2c.setView(novaPosicao, map_7f2f505618d18ff23c11f62484118a2c.getZoom());
});
function calcularOrientacao(P1latitude, P1longitude, P2latitude, P2longitude) {

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

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

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

@ -45,7 +45,7 @@ angulo_max_graus = 30.0
velocidade_min = 0.4
velocidade_max = 1.9
distancia_entre_eixos = 0.92
horizonte = 20
horizonte = 4.0
# Estado compartilhado
trajetoria_latlon = []
@ -108,7 +108,7 @@ def corrigir_pontos_visitados(x, y, pontos_visitados, limite_max_avanço=5):
break
return proximo_nao_visitado(pontos_visitados)
def calcular_melhor_candidato(x, y, theta, visitados_sim, contexto):
def calcular_melhor_candidato(x, y, theta, visitados_sim, contexto, comando_anterior):
idx_alvo = corrigir_pontos_visitados(x, y, visitados_sim)
if idx_alvo >= len(pontos_info):
return 0.0, TipoMovimentoDirecional.RodasDianteiras
@ -133,7 +133,7 @@ def calcular_melhor_candidato(x, y, theta, visitados_sim, contexto):
angulo = np.clip(delta_theta, -np.radians(angulo_max_graus), np.radians(angulo_max_graus))
# Pesos de custo base
peso_erro_pos, peso_erro_ori, peso_movimento, tipos_validos = calcular_pesos_movimento(contexto, erro_ori)
peso_erro_pos, peso_erro_ori, peso_suavidade, peso_movimento, tipos_validos = calcular_pesos_movimento(contexto, erro_ori)
melhor_custo = float('inf')
melhor_tipo = TipoMovimentoDirecional.RodasDianteiras
@ -155,7 +155,14 @@ def calcular_melhor_candidato(x, y, theta, visitados_sim, contexto):
cos_angulo = np.dot(vetor_alvo_norm, vetor_movel)
penalidade_orientacao = 9999 if cos_angulo < 0 else 0
custo = erro_pos * peso_erro_pos + erro_ori_sim * peso_erro_ori + penalidade_orientacao + peso_movimento[tipo]
delta_angulo = abs(np.degrees(angulo) - comando_anterior.get("angulo", 0))
custo_pos = erro_pos * peso_erro_pos
custo_ori = erro_ori_sim * peso_erro_ori
custo_mov = peso_movimento[tipo]
custo_suavidade = delta_angulo * peso_suavidade
custo = custo_pos + custo_ori + penalidade_orientacao + custo_mov + custo_suavidade
if custo < melhor_custo:
melhor_custo = custo
@ -171,9 +178,10 @@ def calcular_pesos_movimento(contexto, erro_ori):
TipoMovimentoDirecional.MovimentoArco
]
peso_erro_pos = 2.0
peso_erro_ori = 1.5
peso_movimento = {
peso_erro_pos = 1.2
peso_erro_ori = 2.5
peso_suavidade = 0.25
custo_movimento = {
TipoMovimentoDirecional.RodasDianteiras: 0.5,
TipoMovimentoDirecional.MovimentoArco: 1.0
}
@ -186,23 +194,22 @@ def calcular_pesos_movimento(contexto, erro_ori):
# Penalidade proporcional à velocidade
penalidade_por_velocidade = velocidade / velocidade_max
if manobrando or status in [
StatusCarroMapa.EntrandoRua,
StatusCarroMapa.SaindoRua,
StatusCarroMapa.Manobrando
]:
peso_movimento[TipoMovimentoDirecional.MovimentoArco] = 0.2
peso_movimento[TipoMovimentoDirecional.RodasDianteiras] = 2.0 + penalidade_por_velocidade
if dentro and erro_ori < np.radians(10):
custo_movimento[TipoMovimentoDirecional.MovimentoArco] = 5.0 # Desencoraja fortemente
custo_movimento[TipoMovimentoDirecional.RodasDianteiras] = 0.1 + penalidade_por_velocidade
elif manobrando or status in [ StatusCarroMapa.EntrandoRua, StatusCarroMapa.SaindoRua, StatusCarroMapa.Manobrando ]:
custo_movimento[TipoMovimentoDirecional.MovimentoArco] = 0.2
custo_movimento[TipoMovimentoDirecional.RodasDianteiras] = 2.0 + penalidade_por_velocidade
elif dentro:
peso_movimento[TipoMovimentoDirecional.MovimentoArco] = 0.5 + (erro_ori / np.radians(angulo_max_graus)) * 2.0
peso_movimento[TipoMovimentoDirecional.RodasDianteiras] = 1.0 + penalidade_por_velocidade
custo_movimento[TipoMovimentoDirecional.MovimentoArco] = 2.0 + (erro_ori / np.radians(angulo_max_graus)) * 2.0
custo_movimento[TipoMovimentoDirecional.RodasDianteiras] = 0.5 + penalidade_por_velocidade
else:
peso_movimento[TipoMovimentoDirecional.MovimentoArco] = 0.3
peso_movimento[TipoMovimentoDirecional.RodasDianteiras] = 1.5 + penalidade_por_velocidade
custo_movimento[TipoMovimentoDirecional.MovimentoArco] = 1.0
custo_movimento[TipoMovimentoDirecional.RodasDianteiras] = 0.5 + penalidade_por_velocidade
return peso_erro_pos, peso_erro_ori, peso_movimento, tipos_validos
return peso_erro_pos, peso_erro_ori, peso_suavidade, custo_movimento, tipos_validos
def processar_mpc(pos_lat, pos_lon, theta, velocidade, dt, contexto):
def processar_mpc_old(pos_lat, pos_lon, theta, velocidade, dt, contexto):
#print(f"Posicao recebida: Velocidade={velocidade}, dt={dt}, Orientacao={np.degrees(theta)}")
global pontos_info, visitados_execucao, lat0, lon0, raio_terra
if not pontos_info:
@ -254,6 +261,62 @@ def processar_mpc(pos_lat, pos_lon, theta, velocidade, dt, contexto):
"simulacao": simulacao_latlon
}
def processar_mpc(pos_lat, pos_lon, theta, velocidade, dt, contexto, comando_anterior=None):
global pontos_info, visitados_execucao, lat0, lon0, raio_terra
if not pontos_info:
return None
x, y = latlon_to_xy(pos_lat, pos_lon, lat0, lon0)
visitados_sim = visitados_execucao.copy()
sim = [(x, y)]
x_temp, y_temp, theta_temp = x, y, theta
atraso_simulado_passos = 2 # Delay estimado de resposta do robô
if velocidade * dt > 0:
pontos_horizonte = int(horizonte / (velocidade * dt))
else:
pontos_horizonte = 10
pontos_horizonte = max(10, min(pontos_horizonte, 120)) # entre 10 e 120 pontos
for i in range(pontos_horizonte):
if i < atraso_simulado_passos and comando_anterior:
angulo_mpc = np.radians(comando_anterior["angulo"])
tipo_mpc = TipoMovimentoDirecional(comando_anterior["tipo"])
else:
angulo_mpc, tipo_mpc = calcular_melhor_candidato(x_temp, y_temp, theta_temp, visitados_sim, contexto, comando_anterior)
tipo_mpc = TipoMovimentoDirecional(tipo_mpc)
omega = calcular_omega(velocidade, angulo_mpc, tipo_mpc)
# Simula a movimentação usando theta com base rotacionada (-theta + 90)
theta_plot = (-theta_temp) + np.radians(90)
x_temp += velocidade * np.cos(theta_plot) * dt
y_temp += velocidade * np.sin(theta_plot) * dt
theta_temp += omega * dt
sim.append((x_temp, y_temp))
# Converte os pontos simulados para lat/lon
simulacao_latlon = []
for sx, sy in sim:
dlat = sy / raio_terra
dlon = sx / (raio_terra * np.cos(np.radians(lat0)))
lat = lat0 + np.degrees(dlat)
lon = lon0 + np.degrees(dlon)
simulacao_latlon.append({"latitude": lat, "longitude": lon})
# Calcula o novo melhor candidato, já considerando o novo estado do robô
angulo_final, tipo_final = calcular_melhor_candidato(sim[0][0], sim[0][1], theta, visitados_execucao, contexto, comando_anterior)
return {
"angulo": np.degrees(angulo_final),
"tipo": tipo_final,
"simulacao": simulacao_latlon
}
def on_connect(client, userdata, flags, rc):
#print("Conectado ao MQTT")
client.subscribe(topico_rota)
@ -298,6 +361,7 @@ def on_message(client, userdata, msg):
theta = np.radians(dados["theta"])
velocidade = dados["velocidade"]
dt = dados["dt"]
comando_anterior = dados.get("comando_anterior", None)
# Novo: contexto completo (com valores padrão caso não venha)
contexto = dados.get("contexto", {})
contexto.setdefault("StatusCarro", 0)
@ -305,7 +369,7 @@ def on_message(client, userdata, msg):
contexto.setdefault("ManobrandoEntreRuas", False)
contexto.setdefault("Velocidade", velocidade) # já aproveita
comando = processar_mpc(lat, lon, theta, velocidade, dt, contexto)
comando = processar_mpc(lat, lon, theta, velocidade, dt, contexto, comando_anterior)
if comando:
client.publish(topico_comando, json.dumps(comando))

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

Binary file not shown.

32
AgroBase/AgroMonitor/Forms/frmAjustesOperacao.Designer.cs generated
View File

@ -107,6 +107,8 @@
this.btnCarregar = new System.Windows.Forms.Button();
this.txtDescricao = new System.Windows.Forms.TextBox();
this.lblDescricao = new System.Windows.Forms.Label();
this.cmbTipoMovimentoDirecional = new System.Windows.Forms.ComboBox();
this.lblTipoMovimentoDirecional = new System.Windows.Forms.Label();
this.gpbModulos.SuspendLayout();
this.pnlModulos.SuspendLayout();
this.tabControl1.SuspendLayout();
@ -443,7 +445,7 @@
//
// btnProximo
//
this.btnProximo.Location = new System.Drawing.Point(483, 388);
this.btnProximo.Location = new System.Drawing.Point(481, 388);
this.btnProximo.Name = "btnProximo";
this.btnProximo.Size = new System.Drawing.Size(75, 23);
this.btnProximo.TabIndex = 12;
@ -471,7 +473,7 @@
this.gpbSensoriamento.Controls.Add(this.cmbConfiguracaoUltrassom);
this.gpbSensoriamento.Controls.Add(this.btnConfigurar);
this.gpbSensoriamento.Enabled = false;
this.gpbSensoriamento.Location = new System.Drawing.Point(3, 405);
this.gpbSensoriamento.Location = new System.Drawing.Point(3, 439);
this.gpbSensoriamento.Name = "gpbSensoriamento";
this.gpbSensoriamento.Size = new System.Drawing.Size(529, 121);
this.gpbSensoriamento.TabIndex = 6;
@ -636,7 +638,7 @@
this.gpbAtuador.Controls.Add(this.txtPercentualAtuacao);
this.gpbAtuador.Controls.Add(this.lblCapacidadeReservatorio);
this.gpbAtuador.Controls.Add(this.lblPercentualAtuacao1);
this.gpbAtuador.Location = new System.Drawing.Point(3, 197);
this.gpbAtuador.Location = new System.Drawing.Point(3, 231);
this.gpbAtuador.Name = "gpbAtuador";
this.gpbAtuador.Size = new System.Drawing.Size(529, 202);
this.gpbAtuador.TabIndex = 7;
@ -780,6 +782,8 @@
//
// gpbDirecional
//
this.gpbDirecional.Controls.Add(this.lblTipoMovimentoDirecional);
this.gpbDirecional.Controls.Add(this.cmbTipoMovimentoDirecional);
this.gpbDirecional.Controls.Add(this.lblPercentual1);
this.gpbDirecional.Controls.Add(this.lblAngulo);
this.gpbDirecional.Controls.Add(this.txtVelocidadeDirecional);
@ -788,7 +792,7 @@
this.gpbDirecional.Controls.Add(this.lblAnguloMaximo);
this.gpbDirecional.Location = new System.Drawing.Point(3, 103);
this.gpbDirecional.Name = "gpbDirecional";
this.gpbDirecional.Size = new System.Drawing.Size(529, 88);
this.gpbDirecional.Size = new System.Drawing.Size(529, 122);
this.gpbDirecional.TabIndex = 6;
this.gpbDirecional.TabStop = false;
this.gpbDirecional.Text = "Direcional";
@ -983,6 +987,24 @@
this.lblDescricao.TabIndex = 7;
this.lblDescricao.Text = "Descrição";
//
// cmbTipoMovimentoDirecional
//
this.cmbTipoMovimentoDirecional.DropDownStyle = System.Windows.Forms.ComboBoxStyle.DropDownList;
this.cmbTipoMovimentoDirecional.FormattingEnabled = true;
this.cmbTipoMovimentoDirecional.Location = new System.Drawing.Point(358, 91);
this.cmbTipoMovimentoDirecional.Name = "cmbTipoMovimentoDirecional";
this.cmbTipoMovimentoDirecional.Size = new System.Drawing.Size(100, 21);
this.cmbTipoMovimentoDirecional.TabIndex = 6;
//
// lblTipoMovimentoDirecional
//
this.lblTipoMovimentoDirecional.AutoSize = true;
this.lblTipoMovimentoDirecional.Location = new System.Drawing.Point(6, 94);
this.lblTipoMovimentoDirecional.Name = "lblTipoMovimentoDirecional";
this.lblTipoMovimentoDirecional.Size = new System.Drawing.Size(330, 13);
this.lblTipoMovimentoDirecional.TabIndex = 7;
this.lblTipoMovimentoDirecional.Text = "Tipo de controle usado na tomada de decições do módulo direcional";
//
// frmAjustesOperacao
//
this.AutoScaleDimensions = new System.Drawing.SizeF(6F, 13F);
@ -1106,5 +1128,7 @@
private System.Windows.Forms.Label lblQuantidadeCamerasSolo;
private System.Windows.Forms.Panel pnlZoomMapa;
private System.Windows.Forms.CheckBox chbMapaDinamico;
private System.Windows.Forms.Label lblTipoMovimentoDirecional;
private System.Windows.Forms.ComboBox cmbTipoMovimentoDirecional;
}
}

9
AgroBase/AgroMonitor/Forms/frmAjustesOperacao.cs
View File

@ -35,6 +35,10 @@ namespace AgroMonitor.Forms
cmbModosOperacao.Items.Clear();
cmbModosOperacao.Items.AddRange(Enum.GetNames(typeof(ModoOperacao)));
cmbModosOperacao.SelectedIndex = 0;
cmbTipoMovimentoDirecional.Items.Clear();
cmbTipoMovimentoDirecional.Items.AddRange(Enum.GetNames(typeof(TiposControladorDirecional)));
cmbModosOperacao.SelectedIndex = 1;
}
private void frmAjustesOperacao_FormClosing(object sender, FormClosingEventArgs e)
@ -170,7 +174,8 @@ namespace AgroMonitor.Forms
(int)nudVelMin.Value,
ConfigUltrassom,
chbSonarAtivado.Checked,
qtdCameras
qtdCameras,
(TiposControladorDirecional)cmbTipoMovimentoDirecional.SelectedIndex
);
if (!sucesso)
{
@ -202,6 +207,8 @@ namespace AgroMonitor.Forms
carregarOpPadrao = false;
cmbModosOperacao.SelectedIndex = (int)Variaveis.OperacaoEmAndamento.Modo;
cmbTipoMovimentoDirecional.SelectedIndex = (int)Variaveis.OperacaoEmAndamento.Controle.TipoControleDirecional;
cmbConfiguracaoUltrassom.SelectedIndex = (int)Variaveis.OperacaoEmAndamento.DispSen.Dados.ConfiguracaoSensorUltrassom.Configuracao;
MessageBox.Show("Operação carregada com sucesso!", "Sucesso", MessageBoxButtons.OK, MessageBoxIcon.Information);

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

Binary file not shown.