ajustes no mpc e regras taticas
This commit is contained in:
Diego Freitas
parent
e401a31799
commit
c00b08dc88
|
|
@ -1085,17 +1085,17 @@ class ControladorMPC:
|
|||
|
||||
# 3) Flags de matriz
|
||||
usar_dados_sonar = contexto.get("VisualWorker") is not None
|
||||
dados_matriz_custo = contexto.get("VisualWorker", {}).get("MatrizCusto", {})
|
||||
dados_matriz_custo = contexto.get("VisualWorker", {}).get("MatrizCusto", {}) if usar_dados_sonar else {}
|
||||
matriz_custo_valida = bool(dados_matriz_custo.get("Valida", False))
|
||||
filtrar_matriz = usar_dados_sonar and matriz_custo_valida
|
||||
|
||||
|
||||
# 4) Geração bruta (graus)
|
||||
angulos_raw = self._gerar_candidatos_brutos(angulo_ideal_deg, filtrar_matriz)
|
||||
if not angulos_raw:
|
||||
# fallback mínimo em torno do ideal
|
||||
base = [angulo_ideal_deg + d for d in (-2.0, -1.0, 0.0, 1.0, 2.0)]
|
||||
angulos_raw = [float(np.clip(round(b, 2), -self.angulo_max_graus, self.angulo_max_graus)) for b in base]
|
||||
|
||||
|
||||
# ---------- QUICK GATE por LUT (por tipo) ----------
|
||||
permitidos = None # set de (tipo.value, grau_2c)
|
||||
if filtrar_matriz and (deadline is None or (deadline - now()) * 1000.0 > margem_ms):
|
||||
|
|
@ -1150,7 +1150,7 @@ class ControladorMPC:
|
|||
g2 = float(round(g, 2))
|
||||
if ok and g2 in allowed_angles:
|
||||
permitidos.add((t.value, g2))
|
||||
|
||||
|
||||
# ---------- RANKING RÁPIDO (barato) & LIMITE POR ORÇAMENTO ----------
|
||||
# Score(grau) = λ * |g - ideal| + (1-λ) * custo_quick (média nas últimas linhas, melhor tipo)
|
||||
if filtrar_matriz and angulos_raw:
|
||||
|
|
@ -1203,7 +1203,7 @@ class ControladorMPC:
|
|||
except Exception as e:
|
||||
# ranking rápido é best-effort; se der algo, segue com angulos_raw como está
|
||||
mostrar_log(f"⚠️ Ranking rápido falhou: {e}")
|
||||
|
||||
|
||||
# 5) Filtro pesado / matriz (usa 'permitidos' para pular combos bloqueados)
|
||||
tipos_final, angulos_final, custos_candidatos = self._filtrar_candidatos_validos_por_matriz(
|
||||
filtrar_matriz=filtrar_matriz,
|
||||
|
|
@ -1222,7 +1222,7 @@ class ControladorMPC:
|
|||
|
||||
# 6) Uniformização ao redor do ideal (rad) com N limite
|
||||
angulos_final = self._uniformizar_angulos_candidatos(angulos_final, angulo_ideal_rad, N=10)
|
||||
|
||||
|
||||
return tipos_final, angulos_final, custos_candidatos
|
||||
|
||||
except Exception as e:
|
||||
|
|
@ -1321,8 +1321,8 @@ class ControladorMPC:
|
|||
peso_angulo_ideal = float(ContextoGlobalRedis.get_pesos_mpc().get("ideal", 1.4))
|
||||
|
||||
# ---- dados da matriz / ensure LUTs (idempotente)
|
||||
dados_matriz_custo = contexto.get("VisualWorker", {}).get("MatrizCusto", {})
|
||||
custo_grid = dados_matriz_custo.get("Custo", None) if isinstance(dados_matriz_custo, dict) else dados_matriz_custo
|
||||
dados_matriz_custo = contexto.get("VisualWorker", {}).get("MatrizCusto", {}) if filtrar_matriz else {}
|
||||
custo_grid = dados_matriz_custo.get("Custo", None)
|
||||
|
||||
# conversões básicas
|
||||
angulos_deg = [float(a) for a in angulos_deg]
|
||||
|
|
@ -1342,7 +1342,7 @@ class ControladorMPC:
|
|||
custos_candidatos[(tipo.value, float(g2))] = float(c)
|
||||
# retorna o conjunto original (em rad)
|
||||
return tipos, angulos_rad.astype(float).tolist(), custos_candidatos
|
||||
|
||||
|
||||
# garante LUTs base (idempotente); algumas avaliações usam offsets/escala
|
||||
largura = float(contexto.get("Equipamento", {}).get("largura", 0.85))
|
||||
L = float(contexto.get("Equipamento", {}).get("entre_eixos", 0.94))
|
||||
|
|
|
|||
|
|
@ -39,13 +39,14 @@ class RegrasTaticas:
|
|||
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}")
|
||||
|
||||
_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.time() - _dados_vw.get("matriz_confianca", {}).get("ts", 10.0) <= 1.0)
|
||||
|
||||
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.time() - _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(
|
||||
|
|
@ -55,9 +56,7 @@ class RegrasTaticas:
|
|||
tempo_liberar=1.0
|
||||
)
|
||||
|
||||
if not snr_operante:
|
||||
motivos.append(f"Sonar mandatório não operante: {_snr_saude.name}")
|
||||
elif obstaculo_persistente:
|
||||
if obstaculo_persistente:
|
||||
motivos.append("Obstáculo próximo detectado no sonar")
|
||||
freio_necessario = True
|
||||
|
||||
|
|
@ -83,7 +82,7 @@ class RegrasTaticas:
|
|||
)
|
||||
|
||||
if not imu_operante:
|
||||
motivos.append(f"IMU mandatório não operante: {_imu_saude.name}")
|
||||
motivos.append(f"IMU mandatório por [ Parada por Inclinação ] 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
|
||||
|
|
@ -106,7 +105,7 @@ class RegrasTaticas:
|
|||
)
|
||||
|
||||
if not atu_operante:
|
||||
motivos.append(f"Pulverizador mandatório não operante: {_atu_saude.name}")
|
||||
motivos.append(f"Pulverizador mandatório por [ Pulverizador Automático ] não operante: {_atu_saude.name}")
|
||||
elif cavitacao_persistente:
|
||||
motivos.append(
|
||||
f"Cavitação na bomba pressurizadora da linha! "
|
||||
|
|
@ -114,6 +113,18 @@ class RegrasTaticas:
|
|||
)
|
||||
freio_necessario = False
|
||||
|
||||
mov_assistido_sonar = _controle.get("mov_auxilio_sonar", False)
|
||||
dir_assistido_sonar = _controle.get("dir_auxilio_sonar", False)
|
||||
if (mov_assistido_sonar or dir_assistido_sonar or oak_parada_por_bloqueio) and not snr_operante:
|
||||
motivos_ativos = []
|
||||
if mov_assistido_sonar:
|
||||
motivos_ativos.append("Movimento Assistido")
|
||||
if dir_assistido_sonar:
|
||||
motivos_ativos.append("Direcional Assistido")
|
||||
if oak_parada_por_bloqueio:
|
||||
motivos_ativos.append("Parada Por Bloqueio")
|
||||
motivos.append(f"Sonar mandatório por [ {', '.join(motivos_ativos)} ] não operante: {_snr_saude.name}")
|
||||
|
||||
return motivos, freio_necessario
|
||||
|
||||
except Exception as e:
|
||||
|
|
|
|||
Loading…
Reference in New Issue