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

Ajustes no IMU, regras taticas, joystick e controle da base

This commit is contained in:
Diego Freitas 2026-03-19 14:09:39 -03:00
parent d898b493ce
commit ecf4b571bf
13 changed files with 308 additions and 145 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
AgroBase/AgroBase/Forms/frmJoystick.cs
View File

@ -1,4 +1,5 @@
using System;
using System.Linq;
using System.Windows.Forms;
using static AgroBase.Models.Enums;
@ -25,14 +26,14 @@ namespace AgroBase.Forms
private void frmJoystick_KeyDown(object sender, KeyEventArgs e)
{
T_Code disp = GeneralJoystick.GetDispositivoComando(e.KeyCode);
GeneralJoystick.EnviaComandoMotor(e.KeyCode, disp);
var comando = GeneralJoystick.DeParaComandos.FirstOrDefault(x => x.key == e.KeyCode);
GeneralJoystick.EnviaComandoMotor(comando?.key ?? e.KeyCode, comando?.Dispositivo ?? T_Code.Vzo);
}
private void frmJoystick_KeyUp(object sender, KeyEventArgs e)
{
T_Code disp = GeneralJoystick.GetDispositivoComando(e.KeyCode);
GeneralJoystick.EnviaComandoMotor(Keys.Escape, disp);
var comando = GeneralJoystick.DeParaComandos.FirstOrDefault(x => x.key == e.KeyCode);
GeneralJoystick.EnviaComandoMotor(comando?.key ?? Keys.Escape, comando?.Dispositivo ?? T_Code.Vzo);
}
}

87
AgroBase/AgroBase/Models/GeneralJoystick.cs
View File

@ -22,22 +22,22 @@ namespace AgroBase
public static List<ComandosDispositivo> Comandos = new List<ComandosDispositivo>() { };
public static List<DeParaComandos> DeParaComandos = new List<DeParaComandos>()
{
new DeParaComandos() { key = Keys.Left, Dispositivo = T_Code.Dir, botaoJoy = BotoesJoystick.SEsquerda, PararAoSoltar = true },
new DeParaComandos() { key = Keys.Left, Dispositivo = T_Code.Dir, botaoJoy = BotoesJoystick.AEEsquerda, PararAoSoltar = true },
new DeParaComandos() { key = Keys.Right, Dispositivo = T_Code.Dir, botaoJoy = BotoesJoystick.SDireita, PararAoSoltar = true },
new DeParaComandos() { key = Keys.Right, Dispositivo = T_Code.Dir, botaoJoy = BotoesJoystick.AEDireita, PararAoSoltar = true },
new DeParaComandos() { key = Keys.R, Dispositivo = T_Code.Dir, botaoJoy = BotoesJoystick.R3, PararAoSoltar = false },
new DeParaComandos() { key = Keys.Up, Dispositivo = T_Code.Mov, botaoJoy = BotoesJoystick.Xis, PararAoSoltar = true },
new DeParaComandos() { key = Keys.Up, Dispositivo = T_Code.Mov, botaoJoy = BotoesJoystick.R2A, PararAoSoltar = true },
new DeParaComandos() { key = Keys.Down, Dispositivo = T_Code.Mov, botaoJoy = BotoesJoystick.Quadrado, PararAoSoltar = true },
new DeParaComandos() { key = Keys.Down, Dispositivo = T_Code.Mov, botaoJoy = BotoesJoystick.L2A, PararAoSoltar = true },
new DeParaComandos() { key = Keys.Space, Dispositivo = T_Code.Mov, botaoJoy = BotoesJoystick.Bolinha, PararAoSoltar = true },
new DeParaComandos() { key = Keys.Subtract, Dispositivo = T_Code.Mov, botaoJoy = BotoesJoystick.L1, PararAoSoltar = false },
new DeParaComandos() { key = Keys.Add, Dispositivo = T_Code.Mov, botaoJoy = BotoesJoystick.R1, PararAoSoltar = false },
new DeParaComandos() { key = Keys.D1, Dispositivo = T_Code.Atu, botaoJoy = BotoesJoystick.ADEsquerda, PararAoSoltar = true, IDcontrolar = "B01" },
new DeParaComandos() { key = Keys.D2, Dispositivo = T_Code.Atu, botaoJoy = BotoesJoystick.ADBaixo, PararAoSoltar = true, IDcontrolar = "B02" },
new DeParaComandos() { key = Keys.D3, Dispositivo = T_Code.Atu, botaoJoy = BotoesJoystick.ADCima, PararAoSoltar = true, IDcontrolar = "B03" },
new DeParaComandos() { key = Keys.D4, Dispositivo = T_Code.Atu, botaoJoy = BotoesJoystick.ADDireita, PararAoSoltar = true, IDcontrolar = "B04" },
new DeParaComandos() { key = Keys.Left, Dispositivo = T_Code.Dir, botaoJoy = BotoesJoystick.SEsquerda, PararAoSoltar = true },
new DeParaComandos() { key = Keys.Left, Dispositivo = T_Code.Dir, botaoJoy = BotoesJoystick.AEEsquerda, PararAoSoltar = true },
new DeParaComandos() { key = Keys.Right, Dispositivo = T_Code.Dir, botaoJoy = BotoesJoystick.SDireita, PararAoSoltar = true },
new DeParaComandos() { key = Keys.Right, Dispositivo = T_Code.Dir, botaoJoy = BotoesJoystick.AEDireita, PararAoSoltar = true },
new DeParaComandos() { key = Keys.R, Dispositivo = T_Code.Dir, botaoJoy = BotoesJoystick.R3, PararAoSoltar = false },
new DeParaComandos() { key = Keys.Up, Dispositivo = T_Code.Mov, botaoJoy = BotoesJoystick.Xis, PararAoSoltar = true },
new DeParaComandos() { key = Keys.Up, Dispositivo = T_Code.Mov, botaoJoy = BotoesJoystick.R2A, PararAoSoltar = true },
new DeParaComandos() { key = Keys.Down, Dispositivo = T_Code.Mov, botaoJoy = BotoesJoystick.Quadrado, PararAoSoltar = true },
new DeParaComandos() { key = Keys.Down, Dispositivo = T_Code.Mov, botaoJoy = BotoesJoystick.L2A, PararAoSoltar = true },
new DeParaComandos() { key = Keys.Space, Dispositivo = T_Code.Mov, botaoJoy = BotoesJoystick.Bolinha, PararAoSoltar = true },
new DeParaComandos() { key = Keys.Subtract, Dispositivo = T_Code.Mov, botaoJoy = BotoesJoystick.L1, PararAoSoltar = false },
new DeParaComandos() { key = Keys.Add, Dispositivo = T_Code.Mov, botaoJoy = BotoesJoystick.R1, PararAoSoltar = false },
new DeParaComandos() { key = Keys.D1, Dispositivo = T_Code.Atu, botaoJoy = BotoesJoystick.ADEsquerda, PararAoSoltar = true, IDcontrolar = "B01" },
new DeParaComandos() { key = Keys.D2, Dispositivo = T_Code.Atu, botaoJoy = BotoesJoystick.ADBaixo, PararAoSoltar = true, IDcontrolar = "B02" },
new DeParaComandos() { key = Keys.D3, Dispositivo = T_Code.Atu, botaoJoy = BotoesJoystick.ADCima, PararAoSoltar = true, IDcontrolar = "B03" },
new DeParaComandos() { key = Keys.D4, Dispositivo = T_Code.Atu, botaoJoy = BotoesJoystick.ADDireita, PararAoSoltar = true, IDcontrolar = "B04" },
};
public static List<BotoesJoystick> BotoesJoyPressionados = new List<BotoesJoystick>() { };
public static List<TipoMovimentoDirecional> MovimentosCombinados = new List<TipoMovimentoDirecional>()
@ -523,7 +523,7 @@ namespace AgroBase
var Comando = DeParaComandos.FirstOrDefault(x => x.botaoJoy == botao.botao);
if (Comando != null)
{
EnviaComandoMotor(Comando.key, Comando.Dispositivo, ID: Comando.IDcontrolar);
EnviaComandoMotor(Comando.key, Comando.Dispositivo, ID: Comando.IDcontrolar, botao: botao.botao);
}
});
_botoesSoltos.ForEach(botao =>
@ -534,7 +534,7 @@ namespace AgroBase
var Comando = DeParaComandos.FirstOrDefault(x => x.botaoJoy == botao);
if (Comando != null && Comando.PararAoSoltar)
{
EnviaComandoMotor(Keys.Escape, Comando.Dispositivo, ID: Comando.IDcontrolar);
EnviaComandoMotor(Keys.Escape, Comando.Dispositivo, ID: Comando.IDcontrolar, botao: botao, ForcarComando: true);
}
});
}
@ -547,6 +547,7 @@ namespace AgroBase
double centro = 32767;
double deadzone = 3000;
double maximo = 65535.0;
var pControle = Variaveis.OperacaoEmAndamento.Parametros.Controle;
switch (botao)
{
@ -609,13 +610,13 @@ namespace AgroBase
case BotoesJoystick.Xis:
direcao = Direcao.Frente;
dispositivo = T_Code.Mov;
percent = 1.0;
percent = Math.Max(pControle.MovVelocidadeCErvasPercent, (pControle.MovVelocidadeSErvasPercent - pControle.MovVelocidadeCErvasPercent) / 2.0);
break;
case BotoesJoystick.Quadrado:
direcao = Direcao.Tras;
dispositivo = T_Code.Mov;
percent = 1.0;
percent = Math.Max(pControle.MovVelocidadeCErvasPercent, (pControle.MovVelocidadeSErvasPercent - pControle.MovVelocidadeCErvasPercent) / 2.0);
break;
}
@ -624,7 +625,6 @@ namespace AgroBase
//Console.WriteLine($"botao: {botao}, valor: {valor}, percentual: {percent * 100.0:F2}%");
var pControle = Variaveis.OperacaoEmAndamento.Parametros.Controle;
var _Controle = Variaveis.OperacaoEmAndamento.Controle;
_Controle.Direcao = direcao;
int mx = direcao == Direcao.Esquerda || direcao == Direcao.Tras ? -1 : 1;
@ -639,7 +639,7 @@ namespace AgroBase
}
}
public static void EnviaComandoMotor(Keys Tecla, T_Code Disp = T_Code.Vzo, string ID = null, bool ForcarComando = false)
public static void EnviaComandoMotor(Keys Tecla, T_Code Disp = T_Code.Vzo, string ID = null, BotoesJoystick botao = BotoesJoystick.Vazio, bool ForcarComando = false)
{
// Verifica se deve interromper o fluxo
// interrompe se nao estiver diagnosticando, nao forca o envio, e se o modo for manual vai verificar o status da operacao e do sonar, pois automatico faz esse controle em outro lugar
@ -668,7 +668,8 @@ namespace AgroBase
Direcao DirecaoAtual = Direcao.Parado;
var Controle = Variaveis.OperacaoEmAndamento.Controle.TiposControle.FirstOrDefault(x => x.Tipo == Comando.Dispositivo);
var _Controle = Variaveis.OperacaoEmAndamento.Controle;
var tipoControle = _Controle?.TiposControle?.FirstOrDefault(x => x?.Tipo == Comando.Dispositivo);
// Cancelar Comando Anterior
if (Comando.Comando == Keys.Escape)
@ -688,6 +689,14 @@ namespace AgroBase
Comando.IDcontrolar = !string.IsNullOrEmpty(ID) ? ID : ComandosDisp[ComandosDisp.Count - 1].IDcontrolar;
}
DirecaoAtual = Direcao.Parado;
switch (botao)
{
case BotoesJoystick.Bolinha:
_Controle.EmFreio = false;
break;
}
Pressionado = true;
}
}
@ -695,17 +704,15 @@ namespace AgroBase
else if (Comando.Comando == Keys.Add)
{
Comando.Dispositivo = T_Code.Mov;
Controle = Variaveis.OperacaoEmAndamento.Controle.TiposControle.FirstOrDefault(x => x.Tipo == Comando.Dispositivo);
DirecaoAtual = Controle?.UltimaDirecao ?? Direcao.Parado;
Variaveis.OperacaoEmAndamento.Controle.PercentualVelocidadeSP++;
DirecaoAtual = tipoControle?.UltimaDirecao ?? Direcao.Parado;
_Controle.PercentualVelocidadeSP++;
Pressionado = true;
}
else if (Comando.Comando == Keys.Subtract)
{
Comando.Dispositivo = T_Code.Mov;
Controle = Variaveis.OperacaoEmAndamento.Controle.TiposControle.FirstOrDefault(x => x.Tipo == Comando.Dispositivo);
DirecaoAtual = Controle?.UltimaDirecao ?? Direcao.Parado;
Variaveis.OperacaoEmAndamento.Controle.PercentualVelocidadeSP--;
DirecaoAtual = tipoControle?.UltimaDirecao ?? Direcao.Parado;
_Controle.PercentualVelocidadeSP--;
Pressionado = true;
}
// Controle Direcao dos Motores BLDC (MOV)
@ -725,6 +732,7 @@ namespace AgroBase
{
Comando.Dispositivo = T_Code.Mov;
DirecaoAtual = Direcao.EmFreio;
_Controle.EmFreio = true;
Pressionado = true;
}
// Controle de Rotação dos Motores de Passo (DIR)
@ -788,7 +796,7 @@ namespace AgroBase
case T_Code.Mov:
{
//Variaveis.OperacaoEmAndamento.DispMvd.Dados.Modulos.ForEach(x => x.MovMotor.EnviarComandoControle(DirecaoAtual));
Variaveis.OperacaoEmAndamento.AtualizarDadosControle(false, new List<T_Code>() { Comando.Dispositivo });
Variaveis.OperacaoEmAndamento.AtualizarDadosControle(ForcarComando, new List<T_Code>() { Comando.Dispositivo });
break;
}
case T_Code.Dir:
@ -806,7 +814,7 @@ namespace AgroBase
}
else
{
Variaveis.OperacaoEmAndamento.AtualizarDadosControle(false, new List<T_Code>() { Comando.Dispositivo });
Variaveis.OperacaoEmAndamento.AtualizarDadosControle(ForcarComando, new List<T_Code>() { Comando.Dispositivo });
//foreach (var mod in Variaveis.OperacaoEmAndamento.DispMvd.Dados.Modulos)
//{
// if (modsControlar == null || modsControlar.Contains(mod.DirMotor.Mod_ID))
@ -830,12 +838,11 @@ namespace AgroBase
}
}
Controle = Variaveis.OperacaoEmAndamento.Controle.TiposControle.FirstOrDefault(x => x.Tipo == Comando.Dispositivo);
if (Controle != null)
{
Controle.UltimaDirecao = DirecaoAtual;
}
//Controle = Variaveis.OperacaoEmAndamento.Controle.TiposControle.FirstOrDefault(x => x.Tipo == Comando.Dispositivo);
//if (Controle != null)
//{
// Controle.UltimaDirecao = DirecaoAtual;
//}
}
UltimaDirecao = DirecaoAtual;
@ -978,7 +985,6 @@ namespace AgroBase
{
List<Keys> ComandosImpedidos = new List<Keys>();
var Sonar = Variaveis.OperacaoEmAndamento.Sensoriamento.OperadorVisual;
Sonar?.Resumo?.AtualizarStatusBloqueio(false);
var DeveParar = Sonar?.Resumo?.DeveParar ?? false;
if (DeveParar)
{
@ -1020,11 +1026,6 @@ namespace AgroBase
return !ComandosImpedidos.Any(x => x == Comando);
}
public static T_Code GetDispositivoComando(Keys Tecla, BotoesJoystick botao = BotoesJoystick.Vazio)
{
T_Code disp = DeParaComandos.FirstOrDefault(x => x.key == Tecla || x.botaoJoy == botao)?.Dispositivo ?? T_Code.Vzo;
return disp;
}
}

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

@ -1334,7 +1334,7 @@ namespace AgroBase.Models
public List<string> EnviarProtocoloComando(T_Code Tipo, bool ForcarEnvio)
{
if (!Variaveis.OperacaoEmAndamento.Iniciado)
if (!Variaveis.OperacaoEmAndamento.Iniciado && !ForcarEnvio)
{
return new List<string>();
}
@ -1488,11 +1488,11 @@ namespace AgroBase.Models
Sonar?.Resumo?.AtualizarStatusBloqueio(true);
if (Sonar?.Resumo?.EnviarComandoParada ?? false)
{
GeneralJoystick.EnviaComandoMotor(Keys.Space, T_Code.Mov);
GeneralJoystick.EnviaComandoMotor(Keys.Escape, Disp: T_Code.Mov, ForcarComando: true);
}
else if (Sonar?.Resumo?.EnviarComandoRetomada ?? false)
{
GeneralJoystick.EnviaComandoMotor(Keys.Escape, T_Code.Mov);
//GeneralJoystick.EnviaComandoMotor(Keys.Escape, T_Code.Mov);
}
if (!Variaveis.OperacaoEmAndamento.OperacaoLiberada)
@ -1501,12 +1501,12 @@ namespace AgroBase.Models
if ((_Controle.TiposControle?.FirstOrDefault(x => x?.Tipo == T_Code.Mov)?.UltimaDirecao ?? Direcao.Parado) != Direcao.Parado)
{
_Controle.PercentualVelocidadeSP = 0;
GeneralJoystick.EnviaComandoMotor(Keys.Escape, T_Code.Mov, ForcarComando: true);
GeneralJoystick.EnviaComandoMotor(Keys.Escape, Disp: T_Code.Mov, ForcarComando: true);
}
if ((_Controle.TiposControle?.FirstOrDefault(x => x?.Tipo == T_Code.Dir)?.UltimaDirecao ?? Direcao.Parado) != Direcao.Parado)
{
_Controle.Angulo = 0;
GeneralJoystick.EnviaComandoMotor(Keys.Escape, T_Code.Dir, ForcarComando: true);
GeneralJoystick.EnviaComandoMotor(Keys.Escape, Disp: T_Code.Dir, ForcarComando: true);
}
}
}
@ -1719,8 +1719,9 @@ namespace AgroBase.Models
if (cmdRover?.Dispositivo == T_Code.Dir && Variaveis.OperacaoEmAndamento.Parametros.Controle.DirecionalAutomatico && Variaveis.OperacaoEmAndamento.Iniciado) return;
var _Controle = Variaveis.OperacaoEmAndamento.Controle;
if (controleBase.Controle.PercentualVelocidadeSP != null) _Controle.PercentualVelocidadeSP = (double)controleBase.Controle.PercentualVelocidadeSP;
if (controleBase.Controle.AnguloSP != null) _Controle.Angulo = (double)controleBase.Controle.AnguloSP;
int mx = cmdRover.key == Keys.Escape || cmdRover.key == Keys.Down ? -1 : 1;
if (controleBase.Controle.PercentualVelocidadeSP != null) _Controle.PercentualVelocidadeSP = (double)controleBase.Controle.PercentualVelocidadeSP * mx;
if (controleBase.Controle.AnguloSP != null) _Controle.Angulo = (double)controleBase.Controle.AnguloSP * mx;
if (controleBase.Controle.TipoMovimentoDirecional != null) _Controle.TipoMovimento = (TipoMovimentoDirecional)controleBase.Controle.TipoMovimentoDirecional;
if (controleBase.Controle.EmFreio != null) _Controle.EmFreio = (bool)controleBase.Controle.EmFreio;

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

@ -284,12 +284,12 @@ namespace AgroBase.Models
{
if (disp == T_Code.Dir)
{
controle.Controle.AnguloSP = msg.P1 / 100.0;
controle.Controle.AnguloSP = (msg.P1 / 100.0);
controle.Controle.TipoMovimentoDirecional = (TipoMovimentoDirecional)(byte)msg.P2;
}
else if (disp == T_Code.Mov)
{
controle.Controle.PercentualVelocidadeSP = msg.P1 / 100.0;
controle.Controle.PercentualVelocidadeSP = (msg.P1 / 100.0);
controle.Controle.EmFreio = msg.P2 == 1;
}
}

120
AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/health_worker/modulos/imu.py
View File

@ -12,6 +12,11 @@ class IMUCamera(ModuloDiagnosticoBase):
def __init__(self, queue=None, freq=100, angulo_inicial=26.3):
self.freq = freq
self.ativo = False
self.last_packet_ts = 0.0
self.last_publish_ts = 0.0
self.ultimo_erro_ts = 0.0
if queue is None:
return
@ -56,8 +61,8 @@ class IMUCamera(ModuloDiagnosticoBase):
self.rugosidade_idx = 0.0
self.impacto_idx = 0.0
self.estabilidade_idx = 100.0
self.last_redis_ts = 0.0
self.redis_period = 0.10 # 100 ms
self.last_redis_ts = 0.0
self.ativo = True
self.imu_thread = threading.Thread(target=self.imu_task_loop, daemon=True)
@ -189,6 +194,7 @@ class IMUCamera(ModuloDiagnosticoBase):
self.imu_em_falha = False
if imuData is not None and len(imuData.packets) > 0:
self.last_packet_ts = time.time()
now = time.perf_counter()
if self.last_loop_ts is None:
dt_total = 1.0 / self.freq
@ -297,6 +303,7 @@ class IMUCamera(ModuloDiagnosticoBase):
t_pub = time.perf_counter()
if (t_pub - self.last_redis_ts) >= self.redis_period:
self.last_publish_ts = time.time()
self.last_redis_ts = t_pub
ContextoGlobalRedis.atualizar_ctx_dict(
@ -318,6 +325,7 @@ class IMUCamera(ModuloDiagnosticoBase):
)
except Exception as e:
self.ultimo_erro_ts = time.time()
self.mostrar_log(f"Erro no loop: {e}")
if not self.imu_em_falha:
self.imu_em_falha = True
@ -334,43 +342,110 @@ class IMUCamera(ModuloDiagnosticoBase):
def atualizar_saude(self):
try:
modulo = ContextoGlobalRedis.get_modulo(T_Code.Imu)
agora = time.time()
modulo = ContextoGlobalRedis.get_modulo(T_Code.Imu) or {}
FREQ_BASE = self.freq
FREQ_MIN = FREQ_BASE * 0.5
SAUDE_MIN_ALERTA = 80
SAUDE_MIN_FALHA = 45
conectado = (time.time() * 1000) - modulo.get("timestamp", 0) < 2000
freq_hz = modulo.get("frequencia", 0.0)
latencia = modulo.get("latencia", 0.0)
conectado = bool(
self.ativo and
self.imu_thread is not None and
self.imu_thread.is_alive()
)
freq_hz = float(modulo.get("frequencia", 0.0) or 0.0)
latencia = float(modulo.get("latencia", 0.0) or 0.0)
last_packet_ts = float(self.last_packet_ts)
last_publish_ts = float(self.last_publish_ts)
ultimo_erro_ts = float(self.ultimo_erro_ts)
tempo_sem_packet = (agora - last_packet_ts) if last_packet_ts > 0 else 999.0
tempo_sem_publicar = (agora - last_publish_ts) if last_publish_ts > 0 else 999.0
tempo_desde_erro = (agora - ultimo_erro_ts) if ultimo_erro_ts > 0 else 999.0
saude = 100
motivos = []
if not conectado:
motivos.append("IMU desconectado ou sem dados recentes")
motivos.append("Thread da IMU não está viva")
saude = 0
elif freq_hz <= FREQ_MIN:
motivos.append(f"Frequência baixa: {freq_hz:.2f} Hz")
saude -= 50
else:
f = min(freq_hz, FREQ_BASE)
erro = (FREQ_BASE - f) / FREQ_BASE
p = min(int(erro * 100), 30)
saude -= abs(p)
if p > 0:
motivos.append(f"Frequência abaixo do ideal: {freq_hz:.2f} Hz (penalidade {p}%)")
# ============================
# 1) Penalização por ausência de packets
# ============================
if tempo_sem_packet > 1.5:
saude -= 45
motivos.append(f"Sem packets há {tempo_sem_packet:.2f}s")
elif tempo_sem_packet > 0.7:
saude -= 25
motivos.append(f"Packets atrasados há {tempo_sem_packet:.2f}s")
elif tempo_sem_packet > 0.3:
saude -= 10
motivos.append(f"Leve atraso de packets: {tempo_sem_packet:.2f}s")
if latencia > 0.5:
p = int(min(latencia * 100, 30))
saude -= p
motivos.append(f"Latência alta: {latencia:.2f} s (penalidade {p}%)")
# ============================
# 2) Penalização por ausência de publicação
# ============================
if tempo_sem_publicar > 1.5:
saude -= 30
motivos.append(f"Sem publicar no Redis há {tempo_sem_publicar:.2f}s")
elif tempo_sem_publicar > 0.8:
saude -= 15
motivos.append(f"Publicação atrasada há {tempo_sem_publicar:.2f}s")
elif tempo_sem_publicar > 0.3:
saude -= 5
motivos.append(f"Leve atraso na publicação: {tempo_sem_publicar:.2f}s")
saude = max(saude, 0)
# ============================
# 3) Frequência
# ============================
if freq_hz <= 0:
saude -= 20
motivos.append("Frequência zerada")
elif freq_hz <= FREQ_MIN:
saude -= 35
motivos.append(f"Frequência baixa: {freq_hz:.2f} Hz")
else:
f = min(freq_hz, FREQ_BASE)
erro = (FREQ_BASE - f) / FREQ_BASE
p = min(int(erro * 100), 20)
saude -= p
if p > 0:
motivos.append(f"Frequência abaixo do ideal: {freq_hz:.2f} Hz")
# ============================
# 4) Latência
# ============================
if latencia > 1.0:
saude -= 25
motivos.append(f"Latência muito alta: {latencia:.2f}s")
elif latencia > 0.5:
saude -= 15
motivos.append(f"Latência alta: {latencia:.2f}s")
elif latencia > 0.2:
saude -= 5
motivos.append(f"Latência moderada: {latencia:.2f}s")
# ============================
# 5) Erro recente
# ============================
if tempo_desde_erro < 2.0:
saude -= 20
motivos.append("Erro muito recente no loop")
elif tempo_desde_erro < 5.0:
saude -= 10
motivos.append("Erro recente no loop")
saude = max(0, min(100, saude))
status = StatusModulo.OPERANTE
if not conectado:
status = StatusModulo.DESCONECTADO
elif saude <= 0:
elif saude <= SAUDE_MIN_FALHA:
status = StatusModulo.FALHA
elif saude < SAUDE_MIN_ALERTA:
status = StatusModulo.ALERTA
@ -380,13 +455,14 @@ class IMUCamera(ModuloDiagnosticoBase):
"status": status.value,
"saude": saude,
"motivos": motivos,
"saude_idividual": []
"saude_idividual": [],
}
ContextoGlobalRedis.atualizar_ctx_dict(
ContextoGlobalRedis.ModKey(T_Code.Imu),
saude=saude_geral
)
except Exception as e:
self.mostrar_log(f"Erro ao atualizar saude: {e}")

150
AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/manager_worker/modulos/regras_taticas.py
View File

@ -3,49 +3,113 @@ from shared.enums import StatusModulo, StatusOperacao, T_Code
from shared.contexto_global_redis import ContextoGlobalRedis, CtxKey
from manager_worker.config import mostrar_log
def verifica_controle_liberado(ignorar_parada=False):
try:
motivos = []
_operacao = ContextoGlobalRedis.get_operacao()
status_operacao = StatusOperacao(_operacao.get("status", StatusOperacao.NaoIniciado.value))
finalizando = _operacao.get("finalizando", False)
if not ignorar_parada and ((status_operacao != StatusOperacao.EmAndamento) or finalizando):
motivos.append(f"Operação não está em andamento ou finalizando: {status_operacao.name}")
_controle = ContextoGlobalRedis.get_controle()
oak_parada_por_bloqueio = _controle.get("oak_parada_por_bloqueio", False)
if oak_parada_por_bloqueio:
_snr = ContextoGlobalRedis.get_modulo(T_Code.Snr)
_snr_saude = StatusModulo(_snr.get("saude", {}).get("status", StatusModulo.DESCONECTADO.value))
snr_operante = _snr_saude in [StatusModulo.OPERANTE, StatusModulo.ALERTA]
_dados_vw = ContextoGlobalRedis.get(CtxKey.DadosVisualWorker, {})
analise_deteccao_atualizada = (time.perf_counter() - _dados_vw.get("matriz_confianca", {}).get("ts", 10.0) <= 1.0)
obstaculo_detectado = _dados_vw.get("matriz_confianca", {}).get("block", {}).get("decision", {}).get("parar", False) if analise_deteccao_atualizada else False
class RegrasTaticas:
def __init__(self):
self.persistencias = {
"obstaculo_sonar": {
"ativo": False,
"ts_inicio_true": None,
"ts_inicio_false": None,
"ultimo_estado_bruto": False,
},
"inclinacao_perigosa": {
"ativo": False,
"ts_inicio_true": None,
"ts_inicio_false": None,
"ultimo_estado_bruto": False,
},
}
def verifica_controle_liberado(self, ignorar_parada=False):
try:
motivos = []
freio_necessario = False
_controle = ContextoGlobalRedis.get_controle()
_operacao = ContextoGlobalRedis.get_operacao()
status_operacao = StatusOperacao(_operacao.get("status", StatusOperacao.NaoIniciado.value))
finalizando = _operacao.get("finalizando", False)
if (not snr_operante):
motivos.append(f"Sonar mandatório não operante: {_snr_saude.name}")
elif (snr_operante and obstaculo_detectado):
motivos.append(f"Obstáculo próximo detectado no sonar")
imu_parada_por_inclinacao = _controle.get("imu_parada_por_inclinacao", False)
if imu_parada_por_inclinacao:
_imu = ContextoGlobalRedis.get_modulo(T_Code.Imu)
_imu_saude = StatusModulo(_imu.get("saude", {}).get("status", StatusModulo.DESCONECTADO.value))
imu_operante = _imu_saude in [StatusModulo.OPERANTE, StatusModulo.ALERTA]
_equipamento = ContextoGlobalRedis.get_equipamento()
ang_roll_max = _equipamento.get("angulo_roll_max", 15.0)
ang_pitch_max = _equipamento.get("angulo_pitch_max", 30.0)
inclinacao_perigosa = abs(_imu.get("pitch_seg", 0.0)) > ang_pitch_max or abs(_imu.get("roll_seg", 0.0)) > ang_roll_max
if not ignorar_parada and ((status_operacao != StatusOperacao.EmAndamento) or finalizando):
motivos.append(f"Operação não está em andamento ou finalizando: {status_operacao.name}")
oak_parada_por_bloqueio = _controle.get("oak_parada_por_bloqueio", False)
if oak_parada_por_bloqueio:
_snr = ContextoGlobalRedis.get_modulo(T_Code.Snr)
_snr_saude = StatusModulo(_snr.get("saude", {}).get("status", StatusModulo.DESCONECTADO.value))
snr_operante = _snr_saude in [StatusModulo.OPERANTE, StatusModulo.ALERTA]
_dados_vw = ContextoGlobalRedis.get(CtxKey.DadosVisualWorker, {})
analise_deteccao_atualizada = (time.perf_counter() - _dados_vw.get("matriz_confianca", {}).get("ts", 10.0) <= 1.0)
obstaculo_detectado = _dados_vw.get("matriz_confianca", {}).get("block", {}).get("decision", {}).get("parar", False)
obstaculo_persistente = self._avaliar_persistencia(
nome="obstaculo_sonar",
estado_bruto=(snr_operante and analise_deteccao_atualizada and obstaculo_detectado),
tempo_acionar=0.35,
tempo_liberar=1.0
)
if not imu_operante:
motivos.append(f"IMU mandatório não operante: {_imu_saude.name}")
elif imu_operante and inclinacao_perigosa:
motivos.append(f"Inclinação perigosa detectada. roll: {_imu.get('roll')}, pitch: {_imu.get('pitch')}, yaw: {_imu.get('yaw')}")
return motivos
except Exception as e:
mostrar_log(f"Erro ao verificar liberacao do controle: {e}")
if not snr_operante:
motivos.append(f"Sonar mandatório não operante: {_snr_saude.name}")
elif obstaculo_persistente:
motivos.append("Obstáculo próximo detectado no sonar")
freio_necessario = True
imu_parada_por_inclinacao = _controle.get("imu_parada_por_inclinacao", False)
if imu_parada_por_inclinacao:
_imu = ContextoGlobalRedis.get_modulo(T_Code.Imu)
_imu_saude = StatusModulo(_imu.get("saude", {}).get("status", StatusModulo.DESCONECTADO.value))
imu_operante = _imu_saude in [StatusModulo.OPERANTE, StatusModulo.ALERTA]
imu_ts = _imu.get("timestamp", 0) / 1000.0
imu_atualizada = (time.time() - imu_ts) <= 1.0
_equipamento = ContextoGlobalRedis.get_equipamento()
ang_roll_max = _equipamento.get("angulo_roll_max", 15.0)
ang_pitch_max = _equipamento.get("angulo_pitch_max", 30.0)
roll = _imu.get("roll_seg", 0.0)
pitch = _imu.get("pitch_seg", 0.0)
inclinacao_perigosa = abs(pitch) > ang_pitch_max or abs(roll) > ang_roll_max
inclinacao_persistente = self._avaliar_persistencia(
nome="inclinacao_perigosa",
estado_bruto=(imu_operante and imu_atualizada and inclinacao_perigosa),
tempo_acionar=0.20,
tempo_liberar=1.2
)
if not imu_operante:
motivos.append(f"IMU mandatório não operante: {_imu_saude.name}")
elif inclinacao_persistente:
motivos.append(f"Inclinação perigosa detectada. roll: {roll:.2f}/{ang_roll_max}, pitch: {pitch:.2f}/{ang_pitch_max}")
freio_necessario = True
return motivos, freio_necessario
except Exception as e:
mostrar_log(f"Erro ao verificar liberacao do controle: {e}")
def _avaliar_persistencia(self, nome, estado_bruto, tempo_acionar, tempo_liberar):
agora = time.perf_counter()
p = self.persistencias[nome]
if estado_bruto:
if p["ultimo_estado_bruto"] != True:
p["ts_inicio_true"] = agora
p["ts_inicio_false"] = None
elif p["ts_inicio_true"] is None:
p["ts_inicio_true"] = agora
if not p["ativo"] and p["ts_inicio_true"] is not None:
if (agora - p["ts_inicio_true"]) >= tempo_acionar:
p["ativo"] = True
else:
if p["ultimo_estado_bruto"] != False:
p["ts_inicio_false"] = agora
p["ts_inicio_true"] = None
elif p["ts_inicio_false"] is None:
p["ts_inicio_false"] = agora
if p["ativo"] and p["ts_inicio_false"] is not None:
if (agora - p["ts_inicio_false"]) >= tempo_liberar:
p["ativo"] = False
p["ultimo_estado_bruto"] = estado_bruto
return p["ativo"]

9
AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/manager_worker/processadores/_3_aguardando.py
View File

@ -1,12 +1,15 @@
from .base import ProcessadorBase
from manager_worker.processadores.padroes import comando_parado
from manager_worker.modulos.regras_taticas import verifica_controle_liberado
from manager_worker.modulos.regras_taticas import RegrasTaticas
class ProcessadorAguardando(ProcessadorBase):
def __init__(self):
self.regras = RegrasTaticas()
def processar(self):
try:
motivos_parada = verifica_controle_liberado(ignorar_parada=True)
return comando_parado(motivos=motivos_parada)
motivos_parada, freio_necessario = self.regras.verifica_controle_liberado(ignorar_parada=True)
return comando_parado(motivos=motivos_parada, frear=freio_necessario)
except Exception as e:
from manager_worker.config import mostrar_log
mostrar_log(f"Erro no processador Aguardando: {e}")

9
AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/manager_worker/processadores/_4_em_andamento.py
View File

@ -5,7 +5,7 @@ from manager_worker.modulos.pid import PIDAdaptativo
from .base import ProcessadorBase
from manager_worker.filtros import FiltroVelocidade
from manager_worker.processadores.padroes import comando_controle
from manager_worker.modulos.regras_taticas import verifica_controle_liberado
from manager_worker.modulos.regras_taticas import RegrasTaticas
from manager_worker.modulos.direcional import definir_comando as definir_comando_dir
from manager_worker.modulos.movimentacao import definir_comando as definir_comando_mov
@ -24,6 +24,7 @@ class ProcessadorEmAndamento(ProcessadorBase):
saida_max=ang_max
)
self.ultimo_comando_enviado = time.perf_counter()
self.regras = RegrasTaticas()
def processar(self):
@ -31,13 +32,13 @@ class ProcessadorEmAndamento(ProcessadorBase):
agora = time.perf_counter()
envio_necessario = (agora - self.ultimo_comando_enviado) > 0.3
motivos_parada = verifica_controle_liberado()
motivos_parada, freio_necessario = self.regras.verifica_controle_liberado()
if len(motivos_parada) > 0:
from manager_worker.config import mostrar_log
mostrar_log(f"⛔ Controle bloqueado: {' | '.join(motivos_parada)}")
return comando_controle(
percentual_velocidade=0,
frear=True,
frear=freio_necessario,
angulo=0,
tipo_movimento=TipoMovimentoDirecional.RodasDianteiras.value,
simulacao=[],
@ -59,7 +60,7 @@ class ProcessadorEmAndamento(ProcessadorBase):
if direcional_automatico:
comando_dir = definir_comando_dir(self.pid_dir, envio_necessario)
enviar_comando = (direcional_automatico and comando_dir.get("enviar_comando", False)) or not direcional_automatico
enviar_comando = comando_dir.get("enviar_comando", False) or movimento_automatico
if enviar_comando:
self.ultimo_comando_enviado = agora

9
AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/manager_worker/processadores/_5_parado.py
View File

@ -1,12 +1,15 @@
from .base import ProcessadorBase
from manager_worker.processadores.padroes import comando_parado
from manager_worker.modulos.regras_taticas import verifica_controle_liberado
from manager_worker.modulos.regras_taticas import RegrasTaticas
class ProcessadorParado(ProcessadorBase):
def __init__(self):
self.regras = RegrasTaticas()
def processar(self):
try:
motivos_parada = verifica_controle_liberado(ignorar_parada=True)
return comando_parado(motivos=motivos_parada)
motivos_parada, freio_necessario = self.regras.verifica_controle_liberado(ignorar_parada=True)
return comando_parado(motivos=motivos_parada, frear=freio_necessario)
except Exception as e:
from manager_worker.config import mostrar_log
mostrar_log(f"Erro no processador Parado: {e}")

4
AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/manager_worker/processadores/padroes.py
View File

@ -44,8 +44,8 @@ def comando_controle(percentual_velocidade: float, frear: bool, angulo: float, t
"motivos": motivos
}
def comando_parado(motivos=None):
return comando_controle(0, False, 0, TipoMovimentoDirecional.RodasDianteiras, motivos=motivos)
def comando_parado(motivos=None, frear=False):
return comando_controle(0, frear, 0, TipoMovimentoDirecional.RodasDianteiras, motivos=motivos)
def log_status_operacao():
modo = ModoOperacao(ContextoGlobalRedis.get_operacao().get("modo"))

2
AgroBase/AgroBase/bin/x64/Debug/Python/Scripts/workers/shared/contexto_global_redis.py
View File

@ -392,7 +392,7 @@ class ContextoGlobalRedis:
controle_ok = False
motivos_dos_modulos_mandatorios.extend(_motivos_controle)
liberou = ((base_ok and parametros_ok) or debug_mode) and emergencia_ok and pausa_ok and trajetoria_ok and controle_ok
liberou = ((base_ok and parametros_ok) or debug_mode) and emergencia_ok and pausa_ok and trajetoria_ok # and controle_ok
cls.atualizar_ctx_dict(
CtxKey.DadosOperacao,

16
AgroBase/OperationControl/ViewModels/Views/Operacao/Monitoramento/MonitoramentoViewModel.cs
View File

@ -1,5 +1,4 @@
using AgroBase.Models;
using AgroBase.Models.Operadores;
using AgroBase.Models.Operadores;
using OperationControl.Helpers;
using OperationControl.Models;
using System.Collections.ObjectModel;
@ -149,7 +148,7 @@ namespace OperationControl.ViewModels.Views.Operacao.Monitoramento
{
if (System.Windows.MessageBox.Show("Deseja solicitar o retorno do equipamento até a base?", "Retorno à base", MessageBoxButton.YesNo, MessageBoxImage.Question) == MessageBoxResult.Yes)
{
var gps = OperationControl.Models.Variaveis.GpsService.UltimaLeitura;
var gps = Variaveis.GpsService.UltimaLeitura;
double? lat = gps?.Latitude;
double? lon = gps?.Longitude;
if (lat == null || lon == null)
@ -158,7 +157,16 @@ namespace OperationControl.ViewModels.Views.Operacao.Monitoramento
return;
}
VariaveisControleOperacao.EnviarComandoRetornoBase(lat, lon);
Task.Run(async () =>
{
await VariaveisControleOperacao.EnviarComandoParadaUDP(pausa: true);
await Task.Delay(500);
VariaveisControleOperacao.EnviarComandoRetornoBase(lat, lon);
await Task.Delay(1000);
await VariaveisControleOperacao.EnviarComandoParadaUDP(pausa: false);
});
}
}

19
AgroBase/OperationControl/ViewModels/Views/Operacao/OperacaoLeftViewModel.cs
View File

@ -319,15 +319,16 @@ namespace OperationControl.ViewModels.Views.Operacao
private void AlternarOperacao()
{
if (VariaveisControleOperacao.RoverEmFoco?.DadosLeitura?.Operacao?.Iniciada ?? false)
var dadosRover = VariaveisControleOperacao.RoverEmFoco?.DadosLeitura;
if (dadosRover?.Operacao?.Iniciada ?? false)
{
bool PauseActivated = VariaveisControleOperacao.RoverEmFoco?.DadosLeitura?.Operacao?.Pausa ?? false;
bool PauseActivated = dadosRover?.Operacao?.Pausa ?? false;
PauseActivated = !PauseActivated;
VariaveisControleOperacao.EnviarComandoParada(pausa: PauseActivated);
}
else
{
if (((VariaveisControleOperacao.RoverEmFoco?.DadosLeitura?.Operacao?.Modo ?? AgroBase.Models.Enums.ModoOperacao.NaoDefinido) == AgroBase.Models.Enums.ModoOperacao.MapaGPS) && !(VariaveisControleOperacao.RoverEmFoco?.RuasPercorrer?.Any() ?? false))
if (((dadosRover?.Operacao?.Modo ?? AgroBase.Models.Enums.ModoOperacao.NaoDefinido) == AgroBase.Models.Enums.ModoOperacao.MapaGPS) && !(VariaveisControleOperacao.RoverEmFoco?.RuasPercorrer?.Any() ?? false))
{
System.Windows.MessageBox.Show("Para iniciar a operação automática, é necessário carregar um mapa e selecionar as ruas onde o equipamento irá operar!", "Parametrização", MessageBoxButton.OK, MessageBoxImage.Warning);
return;
@ -335,13 +336,17 @@ namespace OperationControl.ViewModels.Views.Operacao
if (System.Windows.MessageBox.Show("Deseja iniciar a operação com os parâmetros atuais?", "Iniciar Operação", MessageBoxButton.YesNo, MessageBoxImage.Question) == MessageBoxResult.Yes)
{
bool simulador = VariaveisControleOperacao.RoverEmFoco?.DadosLeitura?.Gnss?.Iniciado != true;
VariaveisControleOperacao.EnviarComandoIniciarOperacao(true, simulador);
Task.Run(async () =>
{
if ((dadosRover?.Operacao?.Status ?? StatusOperacao.NaoIniciado) == StatusOperacao.Concluido)
{
VariaveisControleOperacao.EnviarComandoIniciarOperacao(false, false);
await Task.Delay(500);
}
bool simulador = VariaveisControleOperacao.RoverEmFoco?.DadosLeitura?.Gnss?.Iniciado != true;
VariaveisControleOperacao.EnviarComandoIniciarOperacao(true, simulador);
await Task.Delay(1000);
if (!(VariaveisControleOperacao.RoverEmFoco?.DadosLeitura?.Operacao?.Liberada ?? false))
if (!(dadosRover?.Operacao?.Liberada ?? false))
{
System.Windows.MessageBox.Show($"{VariaveisControleOperacao.RoverEmFoco?.DadosLeitura?.Operacao?.ImpedimentoErro}", "Operação iniciada com impedimento!", MessageBoxButton.OK, MessageBoxImage.Warning);
}