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

revisado fluxo de referenciamento do direcional

This commit is contained in:
Diego Freitas 2026-08-31 14:18:05 -03:00
parent a5586d3ed1
commit a212e48c39
4 changed files with 850 additions and 154 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

56
AgroBase/AgroBase/Models/MKS057DModel.cs
View File

@ -66,20 +66,8 @@ namespace AgroBase.Models
private MksMovimentStatus _statusMovimento;
public MksMovimentStatus StatusMovimento
{
get
{
return _statusMovimento;
}
set
{
_statusMovimento = value;
if (_statusMovimento == MksMovimentStatus.Complete)
{
AtualizarAngulo(AnguloSP);
AtualizarAngulo(AnguloSP);
}
}
get => _statusMovimento;
set => _statusMovimento = value;
}
@ -93,28 +81,48 @@ namespace AgroBase.Models
{
DateTime agora = DateTime.Now;
double delta = novoAngulo - AnguloAtual;
double dtMs = (agora - MomentoUltimaLeitura).TotalMilliseconds;
if (dtMs > 0)
if (MomentoUltimaLeitura != DateTime.MinValue)
{
VelocidadeEstimada = delta / dtMs;
double delta = novoAngulo - AnguloAtual;
double dtMs = (agora - MomentoUltimaLeitura).TotalMilliseconds;
if (dtMs > 0 && dtMs <= 1000)
VelocidadeEstimada = delta / dtMs;
else
VelocidadeEstimada = 0;
}
else
{
VelocidadeEstimada = 0;
}
AnguloAnterior = AnguloAtual;
AnguloAtual = novoAngulo;
MomentoUltimaLeitura = agora;
}
public double AnguloEstimado()
{
double dt = (DateTime.Now - MomentoUltimaLeitura).TotalMilliseconds;
double estimado = AnguloAtual + (VelocidadeEstimada * dt);
if (MomentoUltimaLeitura == DateTime.MinValue)
return AnguloAtual;
double dtMs = (DateTime.Now - MomentoUltimaLeitura).TotalMilliseconds;
/*
* Durante referenciamento PulsosRecebidos trabalha rápido.
* Não deixamos uma leitura velha continuar "viajando" indefinidamente.
*/
dtMs = Math.Max(0, Math.Min(dtMs, 150));
double estimado = AnguloAtual + (VelocidadeEstimada * dtMs);
Variaveis.MostrarLog(
$"[MKS057DModel] [AnguloEstimado] [ADDR {EnderecoTx}] " +
$"{AnguloAtual:F2}° + ({VelocidadeEstimada:F4} * {dtMs:F0}ms) = {estimado:F2}°"
);
Variaveis.MostrarLog($"[MKS057DModel] [AnguloEstimado] [ADDR {EnderecoTx}] Estimativa {AnguloAtual:F2}° + ({VelocidadeEstimada:F4} * {dt:F0}ms) = {estimado:F2}°");
return estimado;
}
}
public class MKS057ComandoModel

880
AgroBase/AgroBase/Models/Modules/DirecionalModel.cs
View File

@ -482,44 +482,189 @@ namespace AgroBase.Models.Modules
}
public async Task ReferenciaMotor()
{
var op = Variaveis.OperacaoEmAndamento;
if (Referenciando || !Inicializado) return;
private const int VelocidadeReferenciamento = 10;
private const double ToleranciaZeroReferenciamento = 0.35;
private const double ToleranciaCentroReferenciamento = 0.50;
private const double MovimentoMinimoConfirmacao = 0.20;
/*
* Janela inicial propositalmente folgada.
* Depois dos ensaios podemos apertar com base nas folgas reais
* dos quatro conjuntos.
*/
private const double FolgaMinimaReferencia = 0.05;
private const double FolgaMaximaReferencia = 8.0;
private static readonly TimeSpan TimeoutPrimeiraBorda = TimeSpan.FromSeconds(120);
private static readonly TimeSpan TimeoutSegundaBorda = TimeSpan.FromSeconds(30);
private static readonly TimeSpan TimeoutCentro = TimeSpan.FromSeconds(30);
private async Task<bool> AtualizarPulsosAgoraAsync(int timeoutMs = 800)
{
var leitura = Leitura;
if (leitura == null || !Inicializado)
return false;
DateTime leituraAnterior = leitura.NumeroPulsos.lidoEm;
MKS057DCanService.RequisitarDadoForcado(_EnderecoCAN_Tx, _EnderecoCAN_Rx, MKS057DCanService.MksHandler.MksFuncCode.PulsosRecebidos);
bool NovaLeitura()
{
var l = Leitura;
return
l != null &&
l.NumeroPulsos.lidoEm > leituraAnterior;
}
await FuncoesGlobais.AguardarCondicaoAsync(NovaLeitura, timeoutMs, 20);
return NovaLeitura();
}
private async Task<bool> AtualizarIOAgoraAsync(int timeoutMs = 800)
{
var leitura = Leitura;
if (leitura == null || !Inicializado)
return false;
DateTime leituraAnterior = leitura.EntradaIN1.lidoEm;
MKS057DCanService.RequisitarDadoForcado(_EnderecoCAN_Tx, _EnderecoCAN_Rx, MKS057DCanService.MksHandler.MksFuncCode.IO);
bool NovaLeitura()
{
var l = Leitura;
return
l != null &&
l.EntradaIN1.lidoEm > leituraAnterior;
}
await FuncoesGlobais.AguardarCondicaoAsync(NovaLeitura, timeoutMs, 20);
return NovaLeitura();
}
private bool PulsosEstaoRecentes(double maxIdadeMs = 200)
{
var leitura = Leitura;
if (leitura == null || leitura.NumeroPulsos.lidoEm == DateTime.MinValue)
{
return false;
}
return (DateTime.Now - leitura.NumeroPulsos.lidoEm).TotalMilliseconds <= maxIdadeMs;
}
public async Task<bool> ReferenciaMotor()
{
var op =
Variaveis.OperacaoEmAndamento;
if (Referenciando ||
!Inicializado)
{
return false;
}
Referenciando = true;
await Task.Delay(1000);
Referenciado = false;
bool sensorAngRealConectado = (SensorAnguloReal?.Inicializado ?? false);
DateTime inicio =
DateTime.Now;
if (sensorAngRealConectado)
try
{
Referenciado = await ReferenciaMotorAbsoluto();
}
else
{
Referenciado = await ReferenciaMotorRelativo();
}
/*
* Atualmente o produto usa somente referência relativa MKS.
*
* Quando o sensor absoluto voltar para o produto,
* tratamos o outro fluxo com o mesmo rigor.
*/
bool sucesso =
await ReferenciaMotorRelativo();
if (Referenciado)
{
await SetZero();
Variaveis.MostrarLog($"[DirecionalModel] [ReferenciaMotor] [DIR {Mod_ID}] - Referenciamento Concluído!");
op.Sensoriamento.InserirLog(T_Code.Dir, StatusModulo.Operante, 100, $"Referenciamento Concluído!", id: Mod_ID);
}
else
{
Variaveis.MostrarLog($"[DirecionalModel] [ReferenciaMotor] [DIR {Mod_ID}] - Referenciamento Falhou!");
op.Sensoriamento.InserirLog(T_Code.Dir, StatusModulo.Falha, 0, $"Referenciamento Falhou!", id: Mod_ID);
}
Referenciado =
sucesso;
Referenciando = false;
if (!sucesso)
{
Variaveis.MostrarLog(
$"[DirecionalModel] [ReferenciaMotor] [DIR {Mod_ID}] " +
$"Referenciamento FALHOU."
);
UltimoReferenciamento = DateTime.Now;
op.Sensoriamento.InserirLog(
T_Code.Dir,
StatusModulo.Falha,
0,
"Referenciamento Falhou!",
id: Mod_ID
);
return false;
}
UltimoReferenciamento =
DateTime.Now;
/*
* Se essa propriedade global ainda for usada em algum lugar,
* agora ela representa um referenciamento REALMENTE concluído.
*/
MKS057DCanService.UltimoReferenciamento =
UltimoReferenciamento;
double tempo =
(DateTime.Now - inicio)
.TotalSeconds;
Variaveis.MostrarLog(
$"[DirecionalModel] [ReferenciaMotor] [DIR {Mod_ID}] " +
$"Referenciamento CONCLUÍDO E CONFIRMADO | " +
$"tempo={tempo:F1}s | " +
$"folga={FolgaMecanica:F3}°"
);
op.Sensoriamento.InserirLog(
T_Code.Dir,
StatusModulo.Operante,
100,
"Referenciamento Concluído!",
id: Mod_ID
);
return true;
}
catch (Exception ex)
{
FalharReferencia(
$"Exceção durante referenciamento: {ex.Message}"
);
op.Sensoriamento.InserirLog(
T_Code.Dir,
StatusModulo.Falha,
0,
$"Erro no referenciamento: {ex.Message}",
id: Mod_ID
);
return false;
}
finally
{
Referenciando = false;
}
}
public async Task<bool> ReferenciaMotorAbsoluto()
@ -544,124 +689,617 @@ namespace AgroBase.Models.Modules
public async Task<bool> ReferenciaMotorRelativo()
{
await SetZero();
/*
* ============================================================
* ETAPA 0 - Pré-condições
* ============================================================
*/
bool Sucesso = false;
bool EstadoInicial = IN1_Atuado;
double AnguloEntrada = 9999;
double AnguloSaida = 9999;
int verificacoesAngulo = 0;
double anguloAnterior = 0;
if (!Inicializado)
return false;
DateTime TimeoutGeral = DateTime.Now.AddMinutes(2); // Timeout geral para o processo.
Variaveis.MostrarLog(
$"[DirecionalModel] [REFERENCIA] [DIR {Mod_ID}] " +
$"INÍCIO REFERENCIAMENTO RELATIVO"
);
Sentido _sentido = SentidoReal == Sentido.Horario ? Sentido.Antihorario : Sentido.Horario;
double _anguloSP = 180 * (_sentido == Sentido.Antihorario ? -1 : 1);
double _anguloSpAnterior = 0;
/*
* ============================================================
* ETAPA 1 - Zero temporário
* ============================================================
*/
while (!Sucesso && TimeoutGeral > DateTime.Now && Inicializado)
if (!await SetZeroConfirmadoAsync("ZERO_TEMPORARIO"))
return FalharReferencia(
"Não foi possível confirmar o zero temporário."
);
/*
* Agora queremos IO e posição comprovadamente atuais.
*/
if (!await AtualizarIOAgoraAsync())
return FalharReferencia(
"Não foi possível obter leitura fresca do IN1."
);
if (!await AtualizarPulsosAgoraAsync())
return FalharReferencia(
"Não foi possível obter posição fresca após zero temporário."
);
bool estadoInicial =
IN1_Atuado;
Sentido sentidoBusca =
SentidoReal == Sentido.Horario
? Sentido.Antihorario
: Sentido.Horario;
double alvoBusca =
sentidoBusca == Sentido.Antihorario
? -180.0
: 180.0;
Variaveis.MostrarLog(
$"[DirecionalModel] [REFERENCIA] [DIR {Mod_ID}] " +
$"Estado inicial | " +
$"IN1={estadoInicial} | " +
$"angulo={AnguloDriver:F3}° | " +
$"sentidoBusca={sentidoBusca}"
);
/*
* ============================================================
* ETAPA 2 - Busca primeira borda
* ============================================================
*/
if (!await EnviarMovimentoConfirmadoAsync(
alvoBusca,
VelocidadeReferenciamento,
sentidoBusca,
"BUSCA_BORDA_1"))
{
int VelocidadeGiro = DefineVelocidadeReferenciamento();
if (_anguloSP != _anguloSpAnterior || (verificacoesAngulo > 5 && AnguloLeitura == anguloAnterior))
{
MKS057DCanService.Movimento(_EnderecoCAN_Tx, _EnderecoCAN_Rx, _anguloSP, VelocidadeGiro, _sentido);
_anguloSpAnterior = _anguloSP;
verificacoesAngulo = 0;
}
if (IN1_Atuado != EstadoInicial && AnguloEntrada == 9999)
{
AnguloEntrada = Leitura.AnguloEstimado();
Variaveis.MostrarLog($"[DirecionalModel] [ReferenciaMotorRelativo] [DIR {Mod_ID}] - Angulo de Entrada Definido: {AnguloEntrada} (Leitura: {AnguloLeitura})");
_sentido = _sentido == Sentido.Horario ? Sentido.Antihorario : Sentido.Horario;
_anguloSP *= -1;
Variaveis.MostrarLog($"[DirecionalModel] [ReferenciaMotorRelativo] [DIR {Mod_ID}] - Invertendo Sentido de Giro para {_sentido.ToString()}");
}
if (IN1_Atuado == EstadoInicial && AnguloEntrada != 9999 && AnguloSaida == 9999)
{
AnguloSaida = Leitura.AnguloEstimado();
Variaveis.MostrarLog($"[DirecionalModel] [ReferenciaMotorRelativo] [DIR {Mod_ID}] - Angulo de Saída Definido: {AnguloSaida} (Leitura: {AnguloLeitura})");
_sentido = _sentido == Sentido.Horario ? Sentido.Antihorario : Sentido.Horario;
Variaveis.MostrarLog($"[DirecionalModel] [ReferenciaMotorRelativo] [DIR {Mod_ID}] - Invertendo Sentido de Giro para {_sentido.ToString()}");
_anguloSP = CalcularCentroCompensado(AnguloEntrada, AnguloSaida, _sentido, OffsetReferenciamento);
}
verificacoesAngulo++;
anguloAnterior = AnguloLeitura;
Sucesso = AnguloEntrada != 9999 && AnguloSaida != 9999 && (FuncoesMatematicas.ValorEstaEntre(Leitura.AnguloEstimado(), _anguloSP, 0.5));
DateTime Inicio = DateTime.Now;
bool NovoDado() => MKS057DCanService.DadosLeitura.Any(x => x.EnderecoRx == _EnderecoCAN_Rx && x.UltimoComandoRecebido > Inicio);
await FuncoesGlobais.AguardarCondicaoAsync(NovoDado, 500, 100);
return FalharReferencia(
"Motor não iniciou corretamente a busca da primeira borda."
);
}
if (!Sucesso)
double? borda1 =
await AguardarTransicaoIN1Async(
estadoInicial,
alvoBusca,
TimeoutPrimeiraBorda,
"BORDA_1"
);
if (!borda1.HasValue)
return FalharReferencia(
"Primeira borda não foi encontrada."
);
bool estadoAposBorda1 =
IN1_Atuado;
/*
* ============================================================
* ETAPA 3 - Inverte e busca segunda borda
* ============================================================
*/
Sentido sentidoRetorno =
sentidoBusca == Sentido.Horario
? Sentido.Antihorario
: Sentido.Horario;
double alvoRetorno =
-alvoBusca;
if (!await EnviarMovimentoConfirmadoAsync(
alvoRetorno,
VelocidadeReferenciamento,
sentidoRetorno,
"BUSCA_BORDA_2"))
{
Variaveis.MostrarLog($"[DirecionalModel] [ReferenciaMotorRelativo] [DIR {Mod_ID}] - ReferenciaMotorRelativo falhou (timeout ou não inicializado). Entrada={AnguloEntrada}, Saida={AnguloSaida}");
Variaveis.OperacaoEmAndamento.Sensoriamento.InserirLog(T_Code.Dir, StatusModulo.Falha, 0, $"ReferenciaMotorRelativo falhou (timeout ou não inicializado). Entrada={AnguloEntrada}, Saida={AnguloSaida}", id: Mod_ID);
return FalharReferencia(
"Motor não iniciou corretamente a busca da segunda borda."
);
}
return Sucesso;
}
double? borda2 =
await AguardarTransicaoIN1Async(
estadoAposBorda1,
alvoRetorno,
TimeoutSegundaBorda,
"BORDA_2"
);
private int DefineVelocidadeReferenciamento()
{
int VelocidadeGiro = 10;
return VelocidadeGiro;
}
if (!borda2.HasValue)
return FalharReferencia(
"Segunda borda não foi encontrada."
);
private double CalcularCentroCompensado(double entrada, double saida, Sentido sentido, double offset)
{
double centro = (entrada + saida) / 2.0;
double folga = Math.Abs(entrada - saida);
// Determina o sentido do giro baseado na diferença
double direcao = sentido == Sentido.Horario ? -1 : 1;
double compensacao = 0; // (folga / 2.0) * direcao;
double centroCompensado = centro + compensacao + offset;
Variaveis.MostrarLog($"[DirecionalModel] [CalcularCentroCompensado] [DIR {Mod_ID}] - Folga: {folga}, Entrada: {entrada}, Saida: {saida}, Centro: {centro}, Compensacao: {compensacao}, Centro Compensado: {centroCompensado}");
FolgaMecanica = folga;
return centroCompensado;
}
public async Task SetZero()
{
MKS057DCanService.SetZero(_EnderecoCAN_Tx, _EnderecoCAN_Rx);
for (int i = 0; i < 5; i++)
/*
* Depois da segunda passagem devemos voltar ao estado lógico
* em que começamos.
*/
if (IN1_Atuado != estadoInicial)
{
DateTime Inicio = DateTime.Now;
bool NovoDado() => MKS057DCanService.DadosLeitura.Any(x => x.EnderecoRx == _EnderecoCAN_Rx && x.UltimoComandoRecebido > Inicio);
await FuncoesGlobais.AguardarCondicaoAsync(NovoDado, 500, 100);
return FalharReferencia(
$"Estado do IN1 inconsistente após segunda borda. " +
$"Inicial={estadoInicial}, Atual={IN1_Atuado}."
);
}
if (FuncoesMatematicas.ValorEstaEntre(AnguloLeitura, -1, 1))
/*
* ============================================================
* ETAPA 4 - Valida folga / calcula centro
* ============================================================
*/
double folga =
Math.Abs(
borda1.Value -
borda2.Value
);
if (folga < FolgaMinimaReferencia ||
folga > FolgaMaximaReferencia)
{
return FalharReferencia(
$"Folga medida fora da faixa esperada: {folga:F3}° " +
$"(esperado {FolgaMinimaReferencia:F2}..{FolgaMaximaReferencia:F2}°)."
);
}
double centro =
((borda1.Value + borda2.Value) / 2.0)
+ OffsetReferenciamento;
FolgaMecanica =
folga;
Variaveis.MostrarLog(
$"[DirecionalModel] [REFERENCIA] [DIR {Mod_ID}] " +
$"GEOMETRIA CONFIRMADA | " +
$"B1={borda1.Value:F3}° | " +
$"B2={borda2.Value:F3}° | " +
$"folga={folga:F3}° | " +
$"centro={centro:F3}° | " +
$"offset={OffsetReferenciamento:F3}°"
);
/*
* ============================================================
* ETAPA 5 - Vai para o centro calculado
* ============================================================
*/
Sentido sentidoCentro =
centro >= AnguloDriver
? Sentido.Horario
: Sentido.Antihorario;
if (!await EnviarMovimentoConfirmadoAsync(
centro,
VelocidadeReferenciamento,
sentidoCentro,
"CENTRALIZACAO"))
{
return FalharReferencia(
"Motor não iniciou movimento para o centro calculado."
);
}
if (!await AguardarAnguloAsync(
centro,
ToleranciaCentroReferenciamento,
TimeoutCentro,
"CENTRALIZACAO"))
{
return FalharReferencia(
"Motor não confirmou chegada ao centro calculado."
);
}
/*
* ============================================================
* ETAPA 6 - Zero absoluto final
* ============================================================
*/
if (!await SetZeroConfirmadoAsync("ZERO_FINAL"))
{
/*
* IMPORTANTÍSSIMO:
*
* NÃO mandamos Movimento(0).
*
* Se o zero não foi aceito, 0 ainda pertence ao sistema
* de coordenadas antigo.
*/
return FalharReferencia(
"Centro encontrado, porém o ZERO FINAL não foi confirmado."
);
}
/*
* Uma leitura extra para fechar o contrato.
*/
if (!await AtualizarPulsosAgoraAsync())
return FalharReferencia(
"Zero final aplicado, mas não foi possível confirmar posição posteriormente."
);
if (Math.Abs(AnguloDriver) >
ToleranciaZeroReferenciamento)
{
return FalharReferencia(
$"Zero final instável. Leitura final={AnguloDriver:F3}°."
);
}
Variaveis.MostrarLog(
$"[DirecionalModel] [REFERENCIA] [DIR {Mod_ID}] " +
$"REFERENCIAMENTO RELATIVO VALIDADO | " +
$"folga={FolgaMecanica:F3}° | " +
$"zero={AnguloDriver:F3}°"
);
return true;
}
private async Task<bool> SetZeroConfirmadoAsync(string etapa, int tentativas = 3)
{
for (int tentativa = 1; tentativa <= tentativas; tentativa++)
{
if (!Inicializado)
return false;
Variaveis.MostrarLog(
$"[DirecionalModel] [REFERENCIA] [DIR {Mod_ID}] " +
$"{etapa}: SetZero tentativa {tentativa}/{tentativas}"
);
MKS057DCanService.SetZero(_EnderecoCAN_Tx, _EnderecoCAN_Rx);
/*
* Deixa o comando entrar na fila/processar.
* Depois solicitamos explicitamente uma posição nova.
*/
await Task.Delay(100);
bool recebeuPulsos = await AtualizarPulsosAgoraAsync();
if (!recebeuPulsos)
{
Variaveis.MostrarLog($"[DirecionalModel] [SetZero] [DIR {Mod_ID}] - Set Zero");
break;
Variaveis.MostrarLog(
$"[DirecionalModel] [REFERENCIA] [DIR {Mod_ID}] " +
$"{etapa}: nenhuma leitura nova de pulsos após SetZero."
);
continue;
}
double angulo = AnguloDriver;
if (Math.Abs(angulo) <= ToleranciaZeroReferenciamento)
{
Variaveis.MostrarLog(
$"[DirecionalModel] [REFERENCIA] [DIR {Mod_ID}] " +
$"{etapa}: SetZero CONFIRMADO | angulo={angulo:F3}°"
);
return true;
}
Variaveis.MostrarLog(
$"[DirecionalModel] [REFERENCIA] [DIR {Mod_ID}] " +
$"{etapa}: SetZero NÃO confirmado | angulo={angulo:F3}°"
);
}
return false;
}
private async Task<bool> EnviarMovimentoConfirmadoAsync(double anguloAlvo, int velocidade, Sentido sentido, string etapa)
{
if (!await AtualizarPulsosAgoraAsync())
{
Variaveis.MostrarLog(
$"[DirecionalModel] [REFERENCIA] [DIR {Mod_ID}] " +
$"{etapa}: não foi possível obter posição inicial."
);
return false;
}
double anguloInicial = AnguloDriver;
if (Math.Abs(anguloAlvo - anguloInicial) <= ToleranciaCentroReferenciamento)
{
return true;
}
MKS057DCanService.Movimento(
_EnderecoCAN_Tx,
_EnderecoCAN_Rx,
anguloAlvo,
velocidade,
sentido
);
Variaveis.MostrarLog(
$"[DirecionalModel] [REFERENCIA] [DIR {Mod_ID}] " +
$"{etapa}: movimento enviado | " +
$"atual={anguloInicial:F2}° alvo={anguloAlvo:F2}° sentido={sentido}"
);
DateTime limite = DateTime.Now.AddSeconds(3);
DateTime ultimaLeitura = Leitura.NumeroPulsos.lidoEm;
while (DateTime.Now < limite && Inicializado)
{
bool NovaLeitura()
{
var l = Leitura;
return l != null && l.NumeroPulsos.lidoEm > ultimaLeitura;
}
await FuncoesGlobais.AguardarCondicaoAsync(NovaLeitura, 500, 20);
if (!NovaLeitura())
{
MKS057DCanService.RequisitarDadoForcado(_EnderecoCAN_Tx, _EnderecoCAN_Rx, MKS057DCanService.MksHandler.MksFuncCode.PulsosRecebidos);
continue;
}
ultimaLeitura = Leitura.NumeroPulsos.lidoEm;
double delta = AnguloDriver - anguloInicial;
if (Math.Abs(delta) < MovimentoMinimoConfirmacao)
{
continue;
}
double deltaEsperado = anguloAlvo - anguloInicial;
if (Math.Sign(delta) != Math.Sign(deltaEsperado))
{
Variaveis.MostrarLog(
$"[DirecionalModel] [REFERENCIA] [DIR {Mod_ID}] " +
$"{etapa}: movimento no sentido INESPERADO | " +
$"delta={delta:F3}° esperado={Math.Sign(deltaEsperado)}"
);
return false;
}
Variaveis.MostrarLog(
$"[DirecionalModel] [REFERENCIA] [DIR {Mod_ID}] " +
$"{etapa}: movimento CONFIRMADO | delta={delta:F3}°"
);
return true;
}
Variaveis.MostrarLog(
$"[DirecionalModel] [REFERENCIA] [DIR {Mod_ID}] " +
$"{etapa}: comando enviado, mas movimento não foi comprovado."
);
return false;
}
private async Task<double?> AguardarTransicaoIN1Async(bool estadoOrigem, double anguloLimite, TimeSpan timeout, string etapa)
{
DateTime limite = DateTime.Now.Add(timeout);
DateTime ultimoIO = Leitura?.EntradaIN1?.lidoEm ?? DateTime.MinValue;
while (DateTime.Now < limite && Inicializado)
{
bool NovoIO()
{
var l = Leitura;
return l != null && l.EntradaIN1.lidoEm > ultimoIO;
}
await FuncoesGlobais.AguardarCondicaoAsync(NovoIO, 500, 20);
/*
* Se o polling normal não trouxe IO,
* fazemos um poke específico.
*/
if (!NovoIO())
{
MKS057DCanService.RequisitarDadoForcado(_EnderecoCAN_Tx, _EnderecoCAN_Rx, MKS057DCanService.MksHandler.MksFuncCode.IO);
continue;
}
ultimoIO = Leitura.EntradaIN1.lidoEm;
bool estadoAtual = IN1_Atuado;
/*
* Ainda não cruzamos a referência.
*/
if (estadoAtual == estadoOrigem)
{
/*
* Se chegou ao limite de busca sem mudar IN1,
* não existe motivo para continuar esperando.
*/
if (PulsosEstaoRecentes(300) && Math.Abs(AnguloDriver - anguloLimite) <= 1.0)
{
Variaveis.MostrarLog(
$"[DirecionalModel] [REFERENCIA] [DIR {Mod_ID}] " +
$"{etapa}: atingiu limite {anguloLimite:F2}° " +
$"sem transição do IN1."
);
return null;
}
continue;
}
/*
* IO mudou.
*
* Só aceitamos a borda se a posição estiver recente.
* Nada de calcular referência com dado velho.
*/
if (!PulsosEstaoRecentes())
{
Variaveis.MostrarLog(
$"[DirecionalModel] [REFERENCIA] [DIR {Mod_ID}] " +
$"{etapa}: IN1 mudou, mas posição está desatualizada."
);
return null;
}
double anguloCandidato = Leitura.AnguloEstimado();
bool estadoCandidato = estadoAtual;
DateTime ioCandidato = Leitura.EntradaIN1.lidoEm;
/*
* Debounce lógico:
* exigimos outra amostra nova mantendo o mesmo estado.
*/
bool NovaConfirmacao()
{
var l = Leitura;
return l != null && l.EntradaIN1.lidoEm > ioCandidato;
}
await FuncoesGlobais.AguardarCondicaoAsync(NovaConfirmacao, 500, 20);
if (!NovaConfirmacao())
{
MKS057DCanService.RequisitarDadoForcado(_EnderecoCAN_Tx, _EnderecoCAN_Rx, MKS057DCanService.MksHandler.MksFuncCode.IO);
await FuncoesGlobais.AguardarCondicaoAsync(NovaConfirmacao, 500, 20);
}
if (!NovaConfirmacao())
return null;
if (IN1_Atuado != estadoCandidato)
{
Variaveis.MostrarLog(
$"[DirecionalModel] [REFERENCIA] [DIR {Mod_ID}] " +
$"{etapa}: transição descartada como instável."
);
ultimoIO = Leitura.EntradaIN1.lidoEm;
continue;
}
Variaveis.MostrarLog(
$"[DirecionalModel] [REFERENCIA] [DIR {Mod_ID}] " +
$"{etapa}: BORDA CONFIRMADA | " +
$"IN1 {estadoOrigem}->{estadoCandidato} | " +
$"angulo={anguloCandidato:F3}°"
);
return anguloCandidato;
}
Variaveis.MostrarLog(
$"[DirecionalModel] [REFERENCIA] [DIR {Mod_ID}] " +
$"{etapa}: timeout aguardando transição do IN1."
);
return null;
}
private async Task<bool> AguardarAnguloAsync(double alvo, double tolerancia, TimeSpan timeout, string etapa)
{
DateTime limite = DateTime.Now.Add(timeout);
DateTime ultimaLeitura = Leitura?.NumeroPulsos?.lidoEm ?? DateTime.MinValue;
int leiturasDentro = 0;
while (DateTime.Now < limite && Inicializado)
{
bool NovaLeitura()
{
var l = Leitura;
return l != null && l.NumeroPulsos.lidoEm > ultimaLeitura;
}
await FuncoesGlobais.AguardarCondicaoAsync(NovaLeitura, 500, 20);
if (!NovaLeitura())
{
MKS057DCanService.RequisitarDadoForcado(_EnderecoCAN_Tx, _EnderecoCAN_Rx, MKS057DCanService.MksHandler.MksFuncCode.PulsosRecebidos);
continue;
}
ultimaLeitura = Leitura.NumeroPulsos.lidoEm;
double erro = Math.Abs(AnguloDriver - alvo);
if (erro <= tolerancia)
{
leiturasDentro++;
/*
* Duas amostras consecutivas dentro da janela.
* Evita considerar "passou voando pelo alvo" como sucesso.
*/
if (leiturasDentro >= 2)
{
Variaveis.MostrarLog(
$"[DirecionalModel] [REFERENCIA] [DIR {Mod_ID}] " +
$"{etapa}: alvo CONFIRMADO | " +
$"alvo={alvo:F3}° atual={AnguloDriver:F3}°"
);
return true;
}
}
else
{
MKS057DCanService.SetZero(_EnderecoCAN_Tx, _EnderecoCAN_Rx);
leiturasDentro = 0;
}
}
MKS057DCanService.Movimento(_EnderecoCAN_Tx, _EnderecoCAN_Rx, 0, 10, Sentido.Parado);
Variaveis.MostrarLog($"[DirecionalModel] [SetZero] [DIR {Mod_ID}] - Comando Angulo Zero: {AnguloLeitura}");
Variaveis.MostrarLog(
$"[DirecionalModel] [REFERENCIA] [DIR {Mod_ID}] " +
$"{etapa}: timeout | alvo={alvo:F3}° atual={AnguloDriver:F3}°"
);
return false;
}
private bool FalharReferencia(string motivo)
{
try
{
MKS057DCanService.Parar(_EnderecoCAN_Tx, _EnderecoCAN_Rx);
}
catch
{
// Estamos justamente tratando uma falha.
}
Referenciado = false;
Variaveis.MostrarLog(
$"[DirecionalModel] [REFERENCIA] [DIR {Mod_ID}] " +
$"FALHA: {motivo}"
);
return false;
}
public void RequisitarDadosSensoresExternos()

66
AgroBase/AgroBase/Models/Modules/MovimentacaoUnificadoModel.cs
View File

@ -672,19 +672,71 @@ namespace AgroBase.Models.Modules
{
try
{
await modulo.DirMotor.ReferenciaMotor();
bool ok =
await modulo.DirMotor.ReferenciaMotor();
op.MarcarModuloCalibrado(T_Code.Dir, modulo.Modulo_ID, true);
op.MarcarModuloCalibrado(
T_Code.Dir,
modulo.Modulo_ID,
ok
);
Variaveis.MostrarLog($"[MovimentacaoUnificadaModel] [RealizarCalibragemDirAsync] Referenciamento do módulo {modulo.Modulo_ID} concluído com sucesso!");
op.Sensoriamento.InserirLog(T_Code.Dir, StatusModulo.Operante, 100, $"Referenciamento do módulo {modulo.Modulo_ID} concluído com sucesso!", id: modulo.Modulo_ID);
if (ok)
{
Variaveis.MostrarLog(
$"[MovimentacaoUnificadaModel] " +
$"[RealizarCalibragemDirAsync] " +
$"Referenciamento do módulo {modulo.Modulo_ID} " +
$"concluído com sucesso!"
);
op.Sensoriamento.InserirLog(
T_Code.Dir,
StatusModulo.Operante,
100,
$"Referenciamento do módulo {modulo.Modulo_ID} concluído com sucesso!",
id: modulo.Modulo_ID
);
}
else
{
Variaveis.MostrarLog(
$"[MovimentacaoUnificadaModel] " +
$"[RealizarCalibragemDirAsync] " +
$"Referenciamento do módulo {modulo.Modulo_ID} FALHOU."
);
op.Sensoriamento.InserirLog(
T_Code.Dir,
StatusModulo.Falha,
0,
$"Referenciamento do módulo {modulo.Modulo_ID} falhou.",
id: modulo.Modulo_ID
);
}
}
catch (Exception ex)
{
op.MarcarModuloCalibrado(T_Code.Dir, modulo.Modulo_ID, false);
op.MarcarModuloCalibrado(
T_Code.Dir,
modulo.Modulo_ID,
false
);
Variaveis.MostrarLog($"[MovimentacaoUnificadaModel] [RealizarCalibragemDirAsync] Erro no referenciamento do módulo {modulo.Modulo_ID}: {ex.Message}");
op.Sensoriamento.InserirLog(T_Code.Dir, StatusModulo.Falha, 0, $"Erro no referenciamento do módulo {modulo.Modulo_ID}: {ex.Message}", id: modulo.Modulo_ID);
Variaveis.MostrarLog(
$"[MovimentacaoUnificadaModel] " +
$"[RealizarCalibragemDirAsync] " +
$"Erro no referenciamento do módulo {modulo.Modulo_ID}: " +
$"{ex.Message}"
);
op.Sensoriamento.InserirLog(
T_Code.Dir,
StatusModulo.Falha,
0,
$"Erro no referenciamento do módulo {modulo.Modulo_ID}: {ex.Message}",
id: modulo.Modulo_ID
);
}
}));
}

2
AgroBase/AgroBase/Services/MKS057DCanService.cs
View File

@ -835,8 +835,6 @@ namespace AgroBase.Services
null,
dedupe: false
);
UltimoReferenciamento = DateTime.Now;
}
public static void Parar(