revisão tipo movimento lateral e rotacionar no eixo, novas métricas can, comunicacao can mov e dir, compensacao de rpm em curvas
This commit is contained in:
Diego Freitas
parent
66f019b6e8
commit
3891aaf819
|
|
@ -416,7 +416,7 @@ namespace AgroBase.Forms.Operacoes
|
|||
$"{id} Fator Curva",
|
||||
LogsMovimentacao,
|
||||
id,
|
||||
x => x.FatorCompensacaoCurva,
|
||||
x => x.FatorCompensacao,
|
||||
visivel: false
|
||||
));
|
||||
}
|
||||
|
|
|
|||
|
|
@ -696,7 +696,8 @@ namespace AgroBase
|
|||
if (cmd == null) return;
|
||||
cmd.CalcularValor(direcao == Direcao.Parado, valorJoy: valor);
|
||||
bool mudou = direcao != _ultimaDirecaoMovEnviada || double.IsNaN(_ultimoValorMovEnviado) || Math.Abs(cmd.ValorAplicar - _ultimoValorMovEnviado) >= VariacaoMinimaMov;
|
||||
bool heartbeat = (DateTime.Now - _ultimoEnvioMov).TotalMilliseconds >= HeartbeatComandoMs;
|
||||
bool combinado = MovimentosCombinados.Contains(Variaveis.OperacaoEmAndamento.Controle.TipoMovimento);
|
||||
bool heartbeat = !combinado && (DateTime.Now - _ultimoEnvioDir).TotalMilliseconds >= HeartbeatComandoMs;
|
||||
if (!mudou && !heartbeat) return;
|
||||
EnviarComandoMotorAtualizado(cmd, ForcarComando: false);
|
||||
_ultimaDirecaoMovEnviada = direcao; _ultimoValorMovEnviado = cmd.ValorAplicar; _ultimoEnvioMov = DateTime.Now;
|
||||
|
|
|
|||
|
|
@ -101,23 +101,44 @@ namespace AgroBase.Models
|
|||
MomentoUltimaLeitura = agora;
|
||||
}
|
||||
public double AnguloEstimado()
|
||||
{
|
||||
return AnguloEstimadoEm(DateTime.Now);
|
||||
}
|
||||
|
||||
public double AnguloEstimadoEm(DateTime momento, double maxExtrapolacaoMs = 500)
|
||||
{
|
||||
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.
|
||||
* Positivo:
|
||||
* o evento aconteceu depois da última leitura de pulsos.
|
||||
*
|
||||
* Negativo:
|
||||
* já recebemos uma leitura de pulsos posterior ao evento
|
||||
* e estamos estimando para trás.
|
||||
*/
|
||||
dtMs = Math.Max(0, Math.Min(dtMs, 150));
|
||||
double dtMs = (momento - MomentoUltimaLeitura).TotalMilliseconds;
|
||||
|
||||
if (Math.Abs(dtMs) > maxExtrapolacaoMs)
|
||||
{
|
||||
Variaveis.MostrarLog(
|
||||
$"[MKS057DModel] [AnguloEstimadoEm] [ADDR {EnderecoTx}] " +
|
||||
$"Estimativa recusada | " +
|
||||
$"delta={dtMs:F0}ms > limite={maxExtrapolacaoMs:F0}ms"
|
||||
);
|
||||
|
||||
return double.NaN;
|
||||
}
|
||||
|
||||
double estimado = AnguloAtual + (VelocidadeEstimada * dtMs);
|
||||
|
||||
Variaveis.MostrarLog(
|
||||
$"[MKS057DModel] [AnguloEstimado] [ADDR {EnderecoTx}] " +
|
||||
$"{AnguloAtual:F2}° + ({VelocidadeEstimada:F4} * {dtMs:F0}ms) = {estimado:F2}°"
|
||||
$"[MKS057DModel] [AnguloEstimadoEm] [ADDR {EnderecoTx}] " +
|
||||
$"base={AnguloAtual:F3}° | " +
|
||||
$"vel={VelocidadeEstimada:F5}°/ms | " +
|
||||
$"delta={dtMs:F0}ms | " +
|
||||
$"estimado={estimado:F3}°"
|
||||
);
|
||||
|
||||
return estimado;
|
||||
|
|
|
|||
|
|
@ -496,73 +496,282 @@ namespace AgroBase.Models.Modules
|
|||
* dos quatro conjuntos.
|
||||
*/
|
||||
private const double FolgaMinimaReferencia = 0.05;
|
||||
private const double FolgaAlertaReferencia = 6.0;
|
||||
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 const int TentativasLeituraReferencia = 3;
|
||||
private const int TimeoutLeituraReferenciaMs = 900;
|
||||
private const int EsperaEntreTentativasLeituraMs = 80;
|
||||
private const double DeltaIdealIoPulsoMs = 350;
|
||||
private const double DeltaMaximoIoPulsoMs = 500;
|
||||
private const int TentativasReferenciamento = 3;
|
||||
private const int EsperaEntreTentativasReferenciaMs = 500;
|
||||
private bool _falhaCriticaReferenciamento = false;
|
||||
private const double MovimentoMaximoConsideradoParado = 0.12;
|
||||
private const int TentativasParadaReferencia = 3;
|
||||
private static readonly TimeSpan IntervaloConfirmacaoParada = TimeSpan.FromMilliseconds(250);
|
||||
|
||||
private async Task<bool> AtualizarPulsosAgoraAsync(int timeoutMs = 800)
|
||||
private async Task<bool> AtualizarPulsosAgoraAsync(int timeoutMs = TimeoutLeituraReferenciaMs, int tentativas = TentativasLeituraReferencia)
|
||||
{
|
||||
for (int tentativa = 1;
|
||||
tentativa <= tentativas;
|
||||
tentativa++)
|
||||
{
|
||||
if (!Inicializado || Leitura == null)
|
||||
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
|
||||
);
|
||||
|
||||
if (NovaLeitura())
|
||||
{
|
||||
if (tentativa > 1)
|
||||
{
|
||||
Variaveis.MostrarLog(
|
||||
$"[DirecionalModel] [REFERENCIA] [DIR {Mod_ID}] " +
|
||||
$"PULSOS recuperados na tentativa " +
|
||||
$"{tentativa}/{tentativas} | " +
|
||||
$"dt={Leitura.NumeroPulsos.dt * 1000.0:F0}ms | " +
|
||||
$"freq={Leitura.NumeroPulsos.frequencia:F1}Hz"
|
||||
);
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
Variaveis.MostrarLog(
|
||||
$"[DirecionalModel] [REFERENCIA] [DIR {Mod_ID}] " +
|
||||
$"PULSOS sem resposta nova | " +
|
||||
$"tentativa={tentativa}/{tentativas} | " +
|
||||
$"idade={ObterIdadePulsosMs():F0}ms"
|
||||
);
|
||||
|
||||
if (tentativa < tentativas)
|
||||
await Task.Delay(EsperaEntreTentativasLeituraMs);
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
private async Task<bool> AtualizarIOAgoraAsync(int timeoutMs = TimeoutLeituraReferenciaMs, int tentativas = TentativasLeituraReferencia)
|
||||
{
|
||||
for (int tentativa = 1;
|
||||
tentativa <= tentativas;
|
||||
tentativa++)
|
||||
{
|
||||
if (!Inicializado || Leitura == null)
|
||||
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
|
||||
);
|
||||
|
||||
if (NovaLeitura())
|
||||
{
|
||||
if (tentativa > 1)
|
||||
{
|
||||
Variaveis.MostrarLog(
|
||||
$"[DirecionalModel] [REFERENCIA] [DIR {Mod_ID}] " +
|
||||
$"IO recuperado na tentativa " +
|
||||
$"{tentativa}/{tentativas} | " +
|
||||
$"IN1={IN1_Atuado} | " +
|
||||
$"dt={Leitura.EntradaIN1.dt * 1000.0:F0}ms | " +
|
||||
$"freq={Leitura.EntradaIN1.frequencia:F1}Hz"
|
||||
);
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
double idade =
|
||||
Leitura.EntradaIN1.lidoEm == DateTime.MinValue
|
||||
? double.PositiveInfinity
|
||||
: (DateTime.Now -
|
||||
Leitura.EntradaIN1.lidoEm)
|
||||
.TotalMilliseconds;
|
||||
|
||||
Variaveis.MostrarLog(
|
||||
$"[DirecionalModel] [REFERENCIA] [DIR {Mod_ID}] " +
|
||||
$"IO sem resposta nova | " +
|
||||
$"tentativa={tentativa}/{tentativas} | " +
|
||||
$"idade={idade:F0}ms"
|
||||
);
|
||||
|
||||
if (tentativa < tentativas)
|
||||
await Task.Delay(EsperaEntreTentativasLeituraMs);
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
private double ObterIdadePulsosMs()
|
||||
{
|
||||
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()
|
||||
if (leitura == null ||
|
||||
leitura.NumeroPulsos.lidoEm == DateTime.MinValue)
|
||||
{
|
||||
var l = Leitura;
|
||||
|
||||
return
|
||||
l != null &&
|
||||
l.NumeroPulsos.lidoEm > leituraAnterior;
|
||||
return double.PositiveInfinity;
|
||||
}
|
||||
|
||||
await FuncoesGlobais.AguardarCondicaoAsync(NovaLeitura, timeoutMs, 20);
|
||||
|
||||
return NovaLeitura();
|
||||
return
|
||||
(DateTime.Now -
|
||||
leitura.NumeroPulsos.lidoEm)
|
||||
.TotalMilliseconds;
|
||||
}
|
||||
|
||||
private async Task<bool> AtualizarIOAgoraAsync(int timeoutMs = 800)
|
||||
private async Task<double?> ObterAnguloNaBordaAsync(DateTime momentoIO, string etapa)
|
||||
{
|
||||
var leitura = Leitura;
|
||||
if (!Inicializado || Leitura == null)
|
||||
return null;
|
||||
|
||||
if (leitura == null || !Inicializado)
|
||||
return false;
|
||||
DateTime momentoPulsos =
|
||||
Leitura.NumeroPulsos.lidoEm;
|
||||
|
||||
DateTime leituraAnterior = leitura.EntradaIN1.lidoEm;
|
||||
double deltaIoPulsoMs =
|
||||
momentoPulsos == DateTime.MinValue
|
||||
? double.PositiveInfinity
|
||||
: (momentoIO - momentoPulsos)
|
||||
.TotalMilliseconds;
|
||||
|
||||
MKS057DCanService.RequisitarDadoForcado(_EnderecoCAN_Tx, _EnderecoCAN_Rx, MKS057DCanService.MksHandler.MksFuncCode.IO);
|
||||
Variaveis.MostrarLog(
|
||||
$"[DirecionalModel] [REFERENCIA] [DIR {Mod_ID}] " +
|
||||
$"{etapa}: sincronismo inicial | " +
|
||||
$"IO={momentoIO:HH:mm:ss.fff} | " +
|
||||
$"PULSOS={momentoPulsos:HH:mm:ss.fff} | " +
|
||||
$"delta_io_pulso={deltaIoPulsoMs:F0}ms | " +
|
||||
$"idade_pulsos_agora={ObterIdadePulsosMs():F0}ms | " +
|
||||
$"dt_pulsos={Leitura.NumeroPulsos.dt * 1000.0:F0}ms | " +
|
||||
$"freq_pulsos={Leitura.NumeroPulsos.frequencia:F1}Hz"
|
||||
);
|
||||
|
||||
bool NovaLeitura()
|
||||
/*
|
||||
* A posição está relativamente próxima do evento.
|
||||
* Podemos estimar diretamente.
|
||||
*/
|
||||
if (Math.Abs(deltaIoPulsoMs) <= DeltaIdealIoPulsoMs)
|
||||
{
|
||||
var l = Leitura;
|
||||
double angulo =
|
||||
Leitura.AnguloEstimadoEm(
|
||||
momentoIO,
|
||||
DeltaMaximoIoPulsoMs
|
||||
);
|
||||
|
||||
return
|
||||
l != null &&
|
||||
l.EntradaIN1.lidoEm > leituraAnterior;
|
||||
if (!double.IsNaN(angulo))
|
||||
return angulo;
|
||||
}
|
||||
|
||||
await FuncoesGlobais.AguardarCondicaoAsync(NovaLeitura, timeoutMs, 20);
|
||||
/*
|
||||
* A última posição ficou velha demais.
|
||||
*
|
||||
* Não descartamos imediatamente a borda.
|
||||
* Buscamos uma leitura nova de pulsos e então estimamos
|
||||
* para trás até o timestamp exato do IO.
|
||||
*/
|
||||
Variaveis.MostrarLog(
|
||||
$"[DirecionalModel] [REFERENCIA] [DIR {Mod_ID}] " +
|
||||
$"{etapa}: posição distante do evento; " +
|
||||
$"solicitando PULSOS para reconstruir a borda."
|
||||
);
|
||||
|
||||
return NovaLeitura();
|
||||
}
|
||||
bool atualizou =
|
||||
await AtualizarPulsosAgoraAsync(
|
||||
timeoutMs: 600,
|
||||
tentativas: 2
|
||||
);
|
||||
|
||||
private bool PulsosEstaoRecentes(double maxIdadeMs = 200)
|
||||
{
|
||||
var leitura = Leitura;
|
||||
|
||||
if (leitura == null || leitura.NumeroPulsos.lidoEm == DateTime.MinValue)
|
||||
if (!atualizou || Leitura == null)
|
||||
{
|
||||
return false;
|
||||
Variaveis.MostrarLog(
|
||||
$"[DirecionalModel] [REFERENCIA] [DIR {Mod_ID}] " +
|
||||
$"{etapa}: não foi possível obter pulsos " +
|
||||
$"para reconstruir a borda."
|
||||
);
|
||||
|
||||
return null;
|
||||
}
|
||||
|
||||
return (DateTime.Now - leitura.NumeroPulsos.lidoEm).TotalMilliseconds <= maxIdadeMs;
|
||||
}
|
||||
momentoPulsos =
|
||||
Leitura.NumeroPulsos.lidoEm;
|
||||
|
||||
deltaIoPulsoMs =
|
||||
(momentoIO - momentoPulsos)
|
||||
.TotalMilliseconds;
|
||||
|
||||
Variaveis.MostrarLog(
|
||||
$"[DirecionalModel] [REFERENCIA] [DIR {Mod_ID}] " +
|
||||
$"{etapa}: sincronismo após atualização | " +
|
||||
$"delta_io_pulso={deltaIoPulsoMs:F0}ms | " +
|
||||
$"angulo_pulsos={AnguloDriver:F3}°"
|
||||
);
|
||||
|
||||
if (Math.Abs(deltaIoPulsoMs) >
|
||||
DeltaMaximoIoPulsoMs)
|
||||
{
|
||||
Variaveis.MostrarLog(
|
||||
$"[DirecionalModel] [REFERENCIA] [DIR {Mod_ID}] " +
|
||||
$"{etapa}: borda recusada | " +
|
||||
$"|delta_io_pulso|={Math.Abs(deltaIoPulsoMs):F0}ms > " +
|
||||
$"{DeltaMaximoIoPulsoMs:F0}ms"
|
||||
);
|
||||
|
||||
return null;
|
||||
}
|
||||
|
||||
double estimado =
|
||||
Leitura.AnguloEstimadoEm(
|
||||
momentoIO,
|
||||
DeltaMaximoIoPulsoMs
|
||||
);
|
||||
|
||||
if (double.IsNaN(estimado))
|
||||
return null;
|
||||
|
||||
return estimado;
|
||||
}
|
||||
|
||||
|
||||
public async Task<bool> ReferenciaMotor()
|
||||
|
|
@ -579,71 +788,201 @@ namespace AgroBase.Models.Modules
|
|||
Referenciando = true;
|
||||
Referenciado = false;
|
||||
|
||||
DateTime inicio =
|
||||
DateTime inicioGeral =
|
||||
DateTime.Now;
|
||||
|
||||
try
|
||||
{
|
||||
/*
|
||||
* 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();
|
||||
|
||||
Referenciado =
|
||||
sucesso;
|
||||
|
||||
if (!sucesso)
|
||||
for (int tentativa = 1;
|
||||
tentativa <= TentativasReferenciamento;
|
||||
tentativa++)
|
||||
{
|
||||
_falhaCriticaReferenciamento = false;
|
||||
|
||||
Variaveis.MostrarLog(
|
||||
$"[DirecionalModel] [ReferenciaMotor] [DIR {Mod_ID}] " +
|
||||
$"Referenciamento FALHOU."
|
||||
$"===== TENTATIVA {tentativa}/{TentativasReferenciamento} ====="
|
||||
);
|
||||
|
||||
op.Sensoriamento.InserirLog(
|
||||
T_Code.Dir,
|
||||
StatusModulo.Falha,
|
||||
0,
|
||||
"Referenciamento Falhou!",
|
||||
id: Mod_ID
|
||||
);
|
||||
DateTime inicioTentativa =
|
||||
DateTime.Now;
|
||||
|
||||
return false;
|
||||
bool sucesso =
|
||||
await ReferenciaMotorRelativo();
|
||||
|
||||
if (sucesso)
|
||||
{
|
||||
Referenciado = true;
|
||||
|
||||
UltimoReferenciamento =
|
||||
DateTime.Now;
|
||||
|
||||
MKS057DCanService.UltimoReferenciamento =
|
||||
UltimoReferenciamento;
|
||||
|
||||
double tempoTentativa =
|
||||
(DateTime.Now - inicioTentativa)
|
||||
.TotalSeconds;
|
||||
|
||||
double tempoTotal =
|
||||
(DateTime.Now - inicioGeral)
|
||||
.TotalSeconds;
|
||||
|
||||
Variaveis.MostrarLog(
|
||||
$"[DirecionalModel] [ReferenciaMotor] [DIR {Mod_ID}] " +
|
||||
$"REFERENCIAMENTO CONCLUÍDO E CONFIRMADO | " +
|
||||
$"tentativa={tentativa}/{TentativasReferenciamento} | " +
|
||||
$"tempo_tentativa={tempoTentativa:F1}s | " +
|
||||
$"tempo_total={tempoTotal:F1}s | " +
|
||||
$"folga={FolgaMecanica:F3}°"
|
||||
);
|
||||
|
||||
op.Sensoriamento.InserirLog(
|
||||
T_Code.Dir,
|
||||
StatusModulo.Operante,
|
||||
100,
|
||||
$"Referenciamento concluído na tentativa " +
|
||||
$"{tentativa}/{TentativasReferenciamento}.",
|
||||
id: Mod_ID
|
||||
);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
Referenciado = false;
|
||||
|
||||
/*
|
||||
* Driver desapareceu durante o processo.
|
||||
*
|
||||
* Não faz sentido ficar tentando referência
|
||||
* sem comunicação.
|
||||
*/
|
||||
if (!Inicializado)
|
||||
{
|
||||
Variaveis.MostrarLog(
|
||||
$"[DirecionalModel] [ReferenciaMotor] [DIR {Mod_ID}] " +
|
||||
$"Retry CANCELADO: driver não está inicializado."
|
||||
);
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
/*
|
||||
* Falha física/lógica grave.
|
||||
*
|
||||
* Exemplo: eixo movimentou no sentido contrário
|
||||
* ao comando.
|
||||
*/
|
||||
if (_falhaCriticaReferenciamento)
|
||||
{
|
||||
Variaveis.MostrarLog(
|
||||
$"[DirecionalModel] [ReferenciaMotor] [DIR {Mod_ID}] " +
|
||||
$"Retry CANCELADO por falha crítica."
|
||||
);
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
if (tentativa <
|
||||
TentativasReferenciamento)
|
||||
{
|
||||
Variaveis.MostrarLog(
|
||||
$"[DirecionalModel] [ReferenciaMotor] [DIR {Mod_ID}] " +
|
||||
$"Tentativa {tentativa} falhou. " +
|
||||
$"Preparando retry automático..."
|
||||
);
|
||||
|
||||
bool parou =
|
||||
await PararEConfirmarAsync(
|
||||
$"RETRY_{tentativa}_PARADA"
|
||||
);
|
||||
|
||||
if (!parou)
|
||||
{
|
||||
_falhaCriticaReferenciamento = true;
|
||||
|
||||
Variaveis.MostrarLog(
|
||||
$"[DirecionalModel] [ReferenciaMotor] [DIR {Mod_ID}] " +
|
||||
$"Retry CANCELADO: não foi possível " +
|
||||
$"comprovar parada do eixo."
|
||||
);
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
await Task.Delay(
|
||||
EsperaEntreTentativasReferenciaMs
|
||||
);
|
||||
|
||||
/*
|
||||
* Pequeno precheck antes de começar outra tentativa.
|
||||
*/
|
||||
bool ioOk =
|
||||
await AtualizarIOAgoraAsync(
|
||||
timeoutMs: 700,
|
||||
tentativas: 2
|
||||
);
|
||||
|
||||
bool pulsosOk =
|
||||
await AtualizarPulsosAgoraAsync(
|
||||
timeoutMs: 700,
|
||||
tentativas: 2
|
||||
);
|
||||
|
||||
Variaveis.MostrarLog(
|
||||
$"[DirecionalModel] [ReferenciaMotor] [DIR {Mod_ID}] " +
|
||||
$"PRECHECK RETRY | " +
|
||||
$"IO={ioOk} | " +
|
||||
$"PULSOS={pulsosOk} | " +
|
||||
$"IN1={IN1_Atuado} | " +
|
||||
$"angulo={AnguloDriver:F3}°"
|
||||
);
|
||||
|
||||
if (!ioOk ||
|
||||
!pulsosOk)
|
||||
{
|
||||
Variaveis.MostrarLog(
|
||||
$"[DirecionalModel] [ReferenciaMotor] [DIR {Mod_ID}] " +
|
||||
$"Precheck do retry incompleto. " +
|
||||
$"A próxima tentativa ainda fará seu próprio precheck."
|
||||
);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
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)
|
||||
double tempoTotalFalha =
|
||||
(DateTime.Now - inicioGeral)
|
||||
.TotalSeconds;
|
||||
|
||||
bool paradaFinal =
|
||||
await PararEConfirmarAsync(
|
||||
"FALHA_FINAL_PARADA"
|
||||
);
|
||||
|
||||
Variaveis.MostrarLog(
|
||||
$"[DirecionalModel] [ReferenciaMotor] [DIR {Mod_ID}] " +
|
||||
$"Referenciamento CONCLUÍDO E CONFIRMADO | " +
|
||||
$"tempo={tempo:F1}s | " +
|
||||
$"folga={FolgaMecanica:F3}°"
|
||||
$"Estado seguro após falha | " +
|
||||
$"parada_confirmada={paradaFinal} | " +
|
||||
$"angulo={AnguloDriver:F3}°"
|
||||
);
|
||||
|
||||
Variaveis.MostrarLog(
|
||||
$"[DirecionalModel] [ReferenciaMotor] [DIR {Mod_ID}] " +
|
||||
$"REFERENCIAMENTO FALHOU DEFINITIVAMENTE | " +
|
||||
$"tentativas={TentativasReferenciamento} | " +
|
||||
$"tempo={tempoTotalFalha:F1}s"
|
||||
);
|
||||
|
||||
op.Sensoriamento.InserirLog(
|
||||
T_Code.Dir,
|
||||
StatusModulo.Operante,
|
||||
100,
|
||||
"Referenciamento Concluído!",
|
||||
StatusModulo.Falha,
|
||||
0,
|
||||
$"Referenciamento falhou após " +
|
||||
$"{TentativasReferenciamento} tentativa(s).",
|
||||
id: Mod_ID
|
||||
);
|
||||
|
||||
return true;
|
||||
return false;
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
|
|
@ -837,21 +1176,27 @@ namespace AgroBase.Models.Modules
|
|||
* ============================================================
|
||||
*/
|
||||
|
||||
double folga =
|
||||
Math.Abs(
|
||||
borda1.Value -
|
||||
borda2.Value
|
||||
);
|
||||
double folga = Math.Abs(borda1.Value - borda2.Value);
|
||||
|
||||
if (folga < FolgaMinimaReferencia ||
|
||||
folga > FolgaMaximaReferencia)
|
||||
if (folga < FolgaMinimaReferencia || folga > FolgaMaximaReferencia)
|
||||
{
|
||||
return FalharReferencia(
|
||||
$"Folga medida fora da faixa esperada: {folga:F3}° " +
|
||||
$"Folga medida fora da faixa física admissível: {folga:F3}° " +
|
||||
$"(esperado {FolgaMinimaReferencia:F2}..{FolgaMaximaReferencia:F2}°)."
|
||||
);
|
||||
}
|
||||
|
||||
if (folga > FolgaAlertaReferencia)
|
||||
{
|
||||
Variaveis.MostrarLog(
|
||||
$"[DirecionalModel] [REFERENCIA] [DIR {Mod_ID}] " +
|
||||
$"ALERTA DE FOLGA ELEVADA | " +
|
||||
$"folga={folga:F3}° | " +
|
||||
$"limite_alerta={FolgaAlertaReferencia:F2}° | " +
|
||||
$"limite_max={FolgaMaximaReferencia:F2}°"
|
||||
);
|
||||
}
|
||||
|
||||
double centro =
|
||||
((borda1.Value + borda2.Value) / 2.0)
|
||||
+ OffsetReferenciamento;
|
||||
|
|
@ -951,43 +1296,65 @@ namespace AgroBase.Models.Modules
|
|||
|
||||
private async Task<bool> SetZeroConfirmadoAsync(string etapa, int tentativas = 3)
|
||||
{
|
||||
for (int tentativa = 1; tentativa <= tentativas; tentativa++)
|
||||
/*
|
||||
* ZERO somente com motor comprovadamente imóvel.
|
||||
*/
|
||||
if (!await PararEConfirmarAsync(
|
||||
etapa + "_PRE_ZERO"))
|
||||
{
|
||||
Variaveis.MostrarLog(
|
||||
$"[DirecionalModel] [REFERENCIA] [DIR {Mod_ID}] " +
|
||||
$"{etapa}: SetZero ABORTADO porque " +
|
||||
$"a parada não foi confirmada."
|
||||
);
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
for (int tentativa = 1;
|
||||
tentativa <= tentativas;
|
||||
tentativa++)
|
||||
{
|
||||
if (!Inicializado)
|
||||
return false;
|
||||
|
||||
Variaveis.MostrarLog(
|
||||
$"[DirecionalModel] [REFERENCIA] [DIR {Mod_ID}] " +
|
||||
$"{etapa}: SetZero tentativa {tentativa}/{tentativas}"
|
||||
$"{etapa}: SetZero tentativa " +
|
||||
$"{tentativa}/{tentativas}"
|
||||
);
|
||||
|
||||
MKS057DCanService.SetZero(_EnderecoCAN_Tx, _EnderecoCAN_Rx);
|
||||
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();
|
||||
bool recebeuPulsos =
|
||||
await AtualizarPulsosAgoraAsync();
|
||||
|
||||
if (!recebeuPulsos)
|
||||
{
|
||||
Variaveis.MostrarLog(
|
||||
$"[DirecionalModel] [REFERENCIA] [DIR {Mod_ID}] " +
|
||||
$"{etapa}: nenhuma leitura nova de pulsos após SetZero."
|
||||
$"{etapa}: nenhuma leitura nova " +
|
||||
$"de pulsos após SetZero."
|
||||
);
|
||||
|
||||
continue;
|
||||
}
|
||||
|
||||
double angulo = AnguloDriver;
|
||||
double angulo =
|
||||
AnguloDriver;
|
||||
|
||||
if (Math.Abs(angulo) <= ToleranciaZeroReferenciamento)
|
||||
if (Math.Abs(angulo) <=
|
||||
ToleranciaZeroReferenciamento)
|
||||
{
|
||||
Variaveis.MostrarLog(
|
||||
$"[DirecionalModel] [REFERENCIA] [DIR {Mod_ID}] " +
|
||||
$"{etapa}: SetZero CONFIRMADO | angulo={angulo:F3}°"
|
||||
$"{etapa}: SetZero CONFIRMADO | " +
|
||||
$"angulo={angulo:F3}°"
|
||||
);
|
||||
|
||||
return true;
|
||||
|
|
@ -995,7 +1362,8 @@ namespace AgroBase.Models.Modules
|
|||
|
||||
Variaveis.MostrarLog(
|
||||
$"[DirecionalModel] [REFERENCIA] [DIR {Mod_ID}] " +
|
||||
$"{etapa}: SetZero NÃO confirmado | angulo={angulo:F3}°"
|
||||
$"{etapa}: SetZero NÃO confirmado | " +
|
||||
$"angulo={angulo:F3}°"
|
||||
);
|
||||
}
|
||||
|
||||
|
|
@ -1068,10 +1436,14 @@ namespace AgroBase.Models.Modules
|
|||
|
||||
if (Math.Sign(delta) != Math.Sign(deltaEsperado))
|
||||
{
|
||||
_falhaCriticaReferenciamento = true;
|
||||
|
||||
Variaveis.MostrarLog(
|
||||
$"[DirecionalModel] [REFERENCIA] [DIR {Mod_ID}] " +
|
||||
$"{etapa}: movimento no sentido INESPERADO | " +
|
||||
$"delta={delta:F3}° esperado={Math.Sign(deltaEsperado)}"
|
||||
$"delta={delta:F3}° | " +
|
||||
$"delta_esperado={deltaEsperado:F3}° | " +
|
||||
$"FALHA CRÍTICA, retry bloqueado"
|
||||
);
|
||||
|
||||
return false;
|
||||
|
|
@ -1093,6 +1465,174 @@ namespace AgroBase.Models.Modules
|
|||
return false;
|
||||
}
|
||||
|
||||
private async Task<bool> PararEConfirmarAsync(string etapa, int tentativas = TentativasParadaReferencia)
|
||||
{
|
||||
if (!Inicializado || Leitura == null)
|
||||
return false;
|
||||
|
||||
for (int tentativa = 1;
|
||||
tentativa <= tentativas;
|
||||
tentativa++)
|
||||
{
|
||||
/*
|
||||
* Primeiro tentamos o comando de parada normal do protocolo.
|
||||
*/
|
||||
MKS057DCanService.Parar(
|
||||
_EnderecoCAN_Tx,
|
||||
_EnderecoCAN_Rx
|
||||
);
|
||||
|
||||
Variaveis.MostrarLog(
|
||||
$"[DirecionalModel] [REFERENCIA] [DIR {Mod_ID}] " +
|
||||
$"{etapa}: PARAR enviado | " +
|
||||
$"tentativa={tentativa}/{tentativas}"
|
||||
);
|
||||
|
||||
await Task.Delay(150);
|
||||
|
||||
if (!await AtualizarPulsosAgoraAsync(
|
||||
timeoutMs: 600,
|
||||
tentativas: 2))
|
||||
{
|
||||
Variaveis.MostrarLog(
|
||||
$"[DirecionalModel] [REFERENCIA] [DIR {Mod_ID}] " +
|
||||
$"{etapa}: sem posição após PARAR."
|
||||
);
|
||||
|
||||
continue;
|
||||
}
|
||||
|
||||
double angulo1 =
|
||||
AnguloDriver;
|
||||
|
||||
await Task.Delay(
|
||||
IntervaloConfirmacaoParada
|
||||
);
|
||||
|
||||
if (!await AtualizarPulsosAgoraAsync(
|
||||
timeoutMs: 600,
|
||||
tentativas: 2))
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
double angulo2 =
|
||||
AnguloDriver;
|
||||
|
||||
double deslocamento =
|
||||
Math.Abs(
|
||||
angulo2 -
|
||||
angulo1
|
||||
);
|
||||
|
||||
if (deslocamento <=
|
||||
MovimentoMaximoConsideradoParado)
|
||||
{
|
||||
Variaveis.MostrarLog(
|
||||
$"[DirecionalModel] [REFERENCIA] [DIR {Mod_ID}] " +
|
||||
$"{etapa}: PARADA CONFIRMADA | " +
|
||||
$"a1={angulo1:F3}° | " +
|
||||
$"a2={angulo2:F3}° | " +
|
||||
$"delta={deslocamento:F3}°"
|
||||
);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
Variaveis.MostrarLog(
|
||||
$"[DirecionalModel] [REFERENCIA] [DIR {Mod_ID}] " +
|
||||
$"{etapa}: motor ainda em movimento após PARAR | " +
|
||||
$"a1={angulo1:F3}° | " +
|
||||
$"a2={angulo2:F3}° | " +
|
||||
$"delta={deslocamento:F3}°"
|
||||
);
|
||||
|
||||
/*
|
||||
* Fallback robusto:
|
||||
*
|
||||
* se o "Parar" do protocolo não matou a trajetória,
|
||||
* substituímos o alvo antigo pela posição atual.
|
||||
*
|
||||
* Sabemos que o modo de posição absoluta funciona.
|
||||
*/
|
||||
double anguloHold =
|
||||
angulo2;
|
||||
|
||||
MKS057DCanService.Movimento(
|
||||
_EnderecoCAN_Tx,
|
||||
_EnderecoCAN_Rx,
|
||||
anguloHold,
|
||||
VelocidadeReferenciamento,
|
||||
Sentido.Parado
|
||||
);
|
||||
|
||||
Variaveis.MostrarLog(
|
||||
$"[DirecionalModel] [REFERENCIA] [DIR {Mod_ID}] " +
|
||||
$"{etapa}: HOLD POSITION enviado | " +
|
||||
$"alvo={anguloHold:F3}°"
|
||||
);
|
||||
|
||||
await Task.Delay(250);
|
||||
|
||||
if (!await AtualizarPulsosAgoraAsync(
|
||||
timeoutMs: 600,
|
||||
tentativas: 2))
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
double hold1 =
|
||||
AnguloDriver;
|
||||
|
||||
await Task.Delay(
|
||||
IntervaloConfirmacaoParada
|
||||
);
|
||||
|
||||
if (!await AtualizarPulsosAgoraAsync(
|
||||
timeoutMs: 600,
|
||||
tentativas: 2))
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
double hold2 =
|
||||
AnguloDriver;
|
||||
|
||||
double deslocamentoHold =
|
||||
Math.Abs(
|
||||
hold2 -
|
||||
hold1
|
||||
);
|
||||
|
||||
if (deslocamentoHold <=
|
||||
MovimentoMaximoConsideradoParado)
|
||||
{
|
||||
Variaveis.MostrarLog(
|
||||
$"[DirecionalModel] [REFERENCIA] [DIR {Mod_ID}] " +
|
||||
$"{etapa}: PARADA CONFIRMADA VIA HOLD | " +
|
||||
$"a1={hold1:F3}° | " +
|
||||
$"a2={hold2:F3}° | " +
|
||||
$"delta={deslocamentoHold:F3}°"
|
||||
);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
Variaveis.MostrarLog(
|
||||
$"[DirecionalModel] [REFERENCIA] [DIR {Mod_ID}] " +
|
||||
$"{etapa}: HOLD não estabilizou o eixo | " +
|
||||
$"delta={deslocamentoHold:F3}°"
|
||||
);
|
||||
}
|
||||
|
||||
Variaveis.MostrarLog(
|
||||
$"[DirecionalModel] [REFERENCIA] [DIR {Mod_ID}] " +
|
||||
$"{etapa}: NÃO foi possível confirmar parada."
|
||||
);
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
private async Task<double?> AguardarTransicaoIN1Async(bool estadoOrigem, double anguloLimite, TimeSpan timeout, string etapa)
|
||||
{
|
||||
DateTime limite = DateTime.Now.Add(timeout);
|
||||
|
|
@ -1132,7 +1672,7 @@ namespace AgroBase.Models.Modules
|
|||
* 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)
|
||||
if (ObterIdadePulsosMs() <= DeltaMaximoIoPulsoMs && Math.Abs(AnguloDriver - anguloLimite) <= 1.0)
|
||||
{
|
||||
Variaveis.MostrarLog(
|
||||
$"[DirecionalModel] [REFERENCIA] [DIR {Mod_ID}] " +
|
||||
|
|
@ -1152,21 +1692,37 @@ namespace AgroBase.Models.Modules
|
|||
* Só aceitamos a borda se a posição estiver recente.
|
||||
* Nada de calcular referência com dado velho.
|
||||
*/
|
||||
if (!PulsosEstaoRecentes())
|
||||
/*
|
||||
* IMPORTANTE:
|
||||
*
|
||||
* Capturamos primeiro o timestamp EXATO da leitura
|
||||
* em que detectamos a mudança do IN1.
|
||||
*/
|
||||
DateTime ioCandidato =
|
||||
Leitura.EntradaIN1.lidoEm;
|
||||
|
||||
bool estadoCandidato =
|
||||
estadoAtual;
|
||||
|
||||
double? anguloBorda =
|
||||
await ObterAnguloNaBordaAsync(
|
||||
ioCandidato,
|
||||
etapa
|
||||
);
|
||||
|
||||
if (!anguloBorda.HasValue)
|
||||
{
|
||||
Variaveis.MostrarLog(
|
||||
$"[DirecionalModel] [REFERENCIA] [DIR {Mod_ID}] " +
|
||||
$"{etapa}: IN1 mudou, mas posição está desatualizada."
|
||||
$"{etapa}: transição válida de IN1, " +
|
||||
$"mas não foi possível reconstruir posição confiável."
|
||||
);
|
||||
|
||||
return null;
|
||||
}
|
||||
|
||||
double anguloCandidato = Leitura.AnguloEstimado();
|
||||
|
||||
bool estadoCandidato = estadoAtual;
|
||||
|
||||
DateTime ioCandidato = Leitura.EntradaIN1.lidoEm;
|
||||
double anguloCandidato =
|
||||
anguloBorda.Value;
|
||||
|
||||
/*
|
||||
* Debounce lógico:
|
||||
|
|
|
|||
|
|
@ -22,7 +22,7 @@ namespace AgroBase.Models.Modules
|
|||
return Variaveis.OperacaoEmAndamento.DispMvd?.Dados?.Modulos?.FirstOrDefault(x => x.MovMotor._EnderecoCAN_Tx == _EnderecoCAN_Tx)?.Modulo_ID ?? "";
|
||||
}
|
||||
}
|
||||
public double LeituraRPM { get; set; } = 0;
|
||||
public double UltimoRPMComandado { get; set; } = 0;
|
||||
public bool ComandoLigado
|
||||
{
|
||||
get
|
||||
|
|
@ -304,65 +304,7 @@ namespace AgroBase.Models.Modules
|
|||
}
|
||||
}
|
||||
|
||||
public bool CompensacaoNecessaria
|
||||
{
|
||||
get
|
||||
{
|
||||
if (ComandoLigado)
|
||||
{
|
||||
var op = Variaveis.OperacaoEmAndamento;
|
||||
|
||||
var _Controle = op.Controle;
|
||||
var _ControleDir = _Controle.TiposControle.FirstOrDefault(x => x.Tipo == T_Code.Dir);
|
||||
|
||||
bool movimentoCompensado =
|
||||
(_Controle.TipoMovimento == TipoMovimentoDirecional.MovimentoArco) ||
|
||||
(_Controle.TipoMovimento == TipoMovimentoDirecional.RodasDianteiras && Mod_ID.Contains("F")) ||
|
||||
(_Controle.TipoMovimento == TipoMovimentoDirecional.RodasTraseiras && Mod_ID.Contains("T"));
|
||||
bool emCurva = _ControleDir.UltimaDirecao != Direcao.Parado;
|
||||
double anguloDir = Math.Abs(op.DispMvd?.Dados?.Modulos?.FirstOrDefault(x => x.Modulo_ID == Mod_ID)?.DirMotor?.Angulo_SP ?? 0);
|
||||
bool direcionalAtuado = anguloDir > 0.0;
|
||||
|
||||
if (movimentoCompensado && emCurva && direcionalAtuado)
|
||||
{
|
||||
if (
|
||||
(_ControleDir.UltimaDirecao == Direcao.Esquerda && Mod_ID.Contains("D")) ||
|
||||
(_ControleDir.UltimaDirecao == Direcao.Direita && Mod_ID.Contains("E"))
|
||||
)
|
||||
{
|
||||
return true;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
private float FatorCompensacaoCurvas
|
||||
{
|
||||
get
|
||||
{
|
||||
if (CompensacaoNecessaria)
|
||||
{
|
||||
//double compensacao = 1 + FuncoesMatematicas.Map(Math.Abs(Variaveis.OperacaoEmAndamento.Controle.Angulo), 0.0, Variaveis.OperacaoEmAndamento.Controle.Angulo_Max, 0.0, 0.3);
|
||||
double compensacao = MovimentacaoMovimentoCompensadoModel.compensacoes[Mod_ID];
|
||||
return (float)compensacao;
|
||||
}
|
||||
else
|
||||
{
|
||||
return 1;
|
||||
}
|
||||
}
|
||||
}
|
||||
private float FatorCompensacaoCurvasAnterior { get; set; } = 1.0f;
|
||||
public bool NovoRPMNecessario
|
||||
{
|
||||
get
|
||||
{
|
||||
return FatorCompensacaoCurvas != FatorCompensacaoCurvasAnterior;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
public Dictionary<TipoMovimentoDirecional, ConfiguracaoSentidoMotor> ConfigSentidos { get; set; }
|
||||
public List<FuncoesPinout> Funcoes { get; set; }
|
||||
|
|
@ -377,6 +319,8 @@ namespace AgroBase.Models.Modules
|
|||
public double UltimoSetPointEnviado { get; set; } = 0;
|
||||
public Direcao UltimaDirecao { get; set; } = Direcao.Parado;
|
||||
public bool UltimoFreioSP { get; set; } = false;
|
||||
private static readonly TimeSpan IntervaloReafirmacaoControle = TimeSpan.FromSeconds(1.5);
|
||||
private const int DeltaMinimoRpmNovoComando = 3;
|
||||
|
||||
|
||||
public static MovimentacaoModel CarregarParametrosIniciais(string Mod_ID, byte _EnderecoTx, byte _EnderecoRx)
|
||||
|
|
@ -504,14 +448,17 @@ namespace AgroBase.Models.Modules
|
|||
}
|
||||
|
||||
RPM_SP_Controle = _Controle.RPM_Motor;
|
||||
RPM_SP =
|
||||
DirecaoAtual == Direcao.Parado || Freado ? 0 :
|
||||
RPM_SP_Controle;
|
||||
|
||||
int RpmCorrigido = Math.Min((int)(VariaveisEquipamento.RPM_Max_Roda * VariaveisEquipamento.RelacaoRPM), Math.Max(RPM_SP != 0 ? (int)(VariaveisEquipamento.RPM_Min_Roda * VariaveisEquipamento.RelacaoRPM) : 0, Convert.ToInt32(RPM_SP * FatorCompensacaoCurvas)));
|
||||
//Console.WriteLine($"[{Mod_ID}] Direcao: {DirecaoAtual}, RPM_SP: {RPM_SP / VariaveisEquipamento.RelacaoRPM}, FC: {FatorCompensacaoCurvas.ToString("0.00")}, Novo RPM: {RpmCorrigido / VariaveisEquipamento.RelacaoRPM}");
|
||||
RPM_SP = Math.Abs(RpmCorrigido);
|
||||
FatorCompensacaoCurvasAnterior = FatorCompensacaoCurvas;
|
||||
if (DirecaoAtual == Direcao.Parado || Freado)
|
||||
{
|
||||
RPM_SP = 0;
|
||||
}
|
||||
else
|
||||
{
|
||||
int rpmMinMotor = Convert.ToInt32(VariaveisEquipamento.RPM_Min_Roda * VariaveisEquipamento.RelacaoRPM);
|
||||
int rpmMaxMotor = Convert.ToInt32(VariaveisEquipamento.RPM_Max_Roda * VariaveisEquipamento.RelacaoRPM);
|
||||
RPM_SP = MovimentacaoMovimentoCompensadoModel.CalcularRpmMotor(Mod_ID, RPM_SP_Controle, rpmMinMotor, rpmMaxMotor);
|
||||
}
|
||||
|
||||
op.SimulacaoRpmControle = _Controle.PercentualVelocidadeSP;
|
||||
//Console.WriteLine($"RPM Atualizado para {Variaveis.OperacaoEmAndamento.SimulacaoRpmControle}");
|
||||
|
|
@ -536,11 +483,20 @@ namespace AgroBase.Models.Modules
|
|||
|
||||
bool PrimeiraLeitura = Leitura == null;
|
||||
bool SentidoAlterado = (Leitura?.Direcao ?? Sentido.Parado) != Sentido_SP;
|
||||
bool RPMAlterado = LeituraRPM != RPM_SP;
|
||||
bool RPMAlterado = Math.Abs(UltimoRPMComandado - RPM_SP) >= DeltaMinimoRpmNovoComando;
|
||||
bool LigadoAlterado = LeituraLigado != ComandoLigado;
|
||||
bool FreioAlterado = (Leitura?.Freado ?? false) != Freado;
|
||||
bool EnviaComando = PrimeiraLeitura || SentidoAlterado || RPMAlterado || LigadoAlterado || FreioAlterado;
|
||||
|
||||
bool ReafirmacaoNecessaria = UltimoComandoEnviado == DateTime.MinValue || DateTime.Now - UltimoComandoEnviado >= IntervaloReafirmacaoControle;
|
||||
|
||||
bool EnviaComando =
|
||||
PrimeiraLeitura ||
|
||||
SentidoAlterado ||
|
||||
RPMAlterado ||
|
||||
LigadoAlterado ||
|
||||
FreioAlterado ||
|
||||
ReafirmacaoNecessaria;
|
||||
|
||||
OidFuncCode modoControle =
|
||||
Freado ? OidFuncCode.ComandoCorrenteFreio :
|
||||
ComandoLigado ? OidFuncCode.ComandoVelocidade :
|
||||
|
|
@ -565,7 +521,7 @@ namespace AgroBase.Models.Modules
|
|||
|
||||
OIDCanService.ComandoControle(_EnderecoCAN_Tx, _EnderecoCAN_Rx, modoControle, setPoint);
|
||||
|
||||
LeituraRPM = RPM_SP;
|
||||
UltimoRPMComandado = RPM_SP;
|
||||
UltimoComandoEnviado = DateTime.Now;
|
||||
UltimoModoControleEnviado = modoControle;
|
||||
UltimoSetPointEnviado = setPoint;
|
||||
|
|
@ -771,192 +727,558 @@ namespace AgroBase.Models.Modules
|
|||
public double CicloTrabalho { get; set; }
|
||||
}
|
||||
|
||||
public class MovimentacaoMovimentoCompensadoModel
|
||||
public static class MovimentacaoMovimentoCompensadoModel
|
||||
{
|
||||
static List<TipoMovimentoDirecional> MovimentosCompensar = new List<TipoMovimentoDirecional>() { TipoMovimentoDirecional.RodasDianteiras, TipoMovimentoDirecional.RodasTraseiras, TipoMovimentoDirecional.MovimentoArco };
|
||||
static GeometriaRobo g = new GeometriaRobo { L = VariaveisEquipamento.DistanciaEntreEixosCm / 100.0, W = VariaveisEquipamento.LarguraEquipamentoMm / 1000.0 };
|
||||
public static Dictionary<string, double> compensacoes { get; set; } = new Dictionary<string, double>();
|
||||
public static Dictionary<string, double> _compAnt { get; set; }
|
||||
private static readonly object _sync = new object();
|
||||
|
||||
private static readonly HashSet<TipoMovimentoDirecional> ModosCompensados = new HashSet<TipoMovimentoDirecional>
|
||||
{
|
||||
TipoMovimentoDirecional.RodasDianteiras,
|
||||
TipoMovimentoDirecional.RodasTraseiras,
|
||||
TipoMovimentoDirecional.MovimentoArco
|
||||
};
|
||||
|
||||
/*
|
||||
* Proteção contra geometria patológica.
|
||||
* Depois podemos derivar esse teto exatamente do ângulo máximo
|
||||
* permitido pelo rover.
|
||||
*/
|
||||
private const double FatorMaximo = 2.50;
|
||||
|
||||
/*
|
||||
* Tempo máximo para considerar posição do MKS fresca.
|
||||
*/
|
||||
private const double IdadeMaximaAnguloMs = 1000.0;
|
||||
|
||||
private static DateTime _ultimoCalculo = DateTime.MinValue;
|
||||
|
||||
/*
|
||||
* Filtro no ÂNGULO, não no fator.
|
||||
*
|
||||
* Isso mantém todos os fatores pertencendo à mesma geometria.
|
||||
*/
|
||||
private const double TauFiltroAnguloSeg = 0.15;
|
||||
|
||||
private static double _angFrenteFiltrado;
|
||||
private static double _angTrasFiltrado;
|
||||
|
||||
private static Dictionary<string, EstadoCompensacaoRoda> _estado = new Dictionary<string, EstadoCompensacaoRoda>();
|
||||
|
||||
private const double AnguloEntradaCurvaGraus = 1.0;
|
||||
private const double AnguloSaidaCurvaGraus = 0.5;
|
||||
private static bool _curvaAtiva;
|
||||
|
||||
|
||||
public class EstadoCompensacaoRoda
|
||||
{
|
||||
public string ModuloId { get; set; }
|
||||
public double RaioM { get; set; }
|
||||
public double Fator { get; set; } = 1.0;
|
||||
public bool Saturado { get; set; }
|
||||
public double AnguloFrenteGraus { get; set; }
|
||||
public double AnguloTrasGraus { get; set; }
|
||||
public double IccX { get; set; }
|
||||
public double IccY { get; set; }
|
||||
}
|
||||
|
||||
|
||||
private struct GeometriaRobo
|
||||
{
|
||||
public double L;
|
||||
public double W;
|
||||
}
|
||||
|
||||
|
||||
public static void AtualizarDados()
|
||||
{
|
||||
var op = Variaveis.OperacaoEmAndamento;
|
||||
var modulos = op?.DispMvd?.Dados?.Modulos;
|
||||
|
||||
var ctrl = op?.Controle;
|
||||
var disp = op?.DispMvd?.Dados?.Modulos;
|
||||
if (op?.Sensoriamento?.Operacao?.StatusOperacaoAtual != StatusOperacao.EmAndamento || ctrl == null || disp == null || !MovimentosCompensar.Contains(ctrl.TipoMovimento))
|
||||
if (modulos == null || modulos.Count == 0)
|
||||
{
|
||||
compensacoes = (disp ?? Enumerable.Empty<ModuloMvdModel>()).ToDictionary(x => x.Modulo_ID, x => 1.0);
|
||||
return;
|
||||
ResetarTudo();
|
||||
return;
|
||||
}
|
||||
|
||||
// Coleta ângulos dos módulos
|
||||
var frente = disp.Where(m => m.Modulo_ID?.Contains("F") == true).Select(m => (double)(m.DirMotor?.Angulo_SP ?? 0) * (m.DirMotor.Sentido_SP == Sentido.Antihorario ? -1 : 1));
|
||||
var tras = disp.Where(m => m.Modulo_ID?.Contains("T") == true).Select(m => (double)(m.DirMotor?.Angulo_SP ?? 0) * (m.DirMotor.Sentido_SP == Sentido.Antihorario ? 1 : -1));
|
||||
var controle = op?.Controle;
|
||||
|
||||
// Ângulo equivalente do eixo (média em tan-space)
|
||||
double angFrenteDeg = EqAngleEixo(frente);
|
||||
double angTrasDeg = EqAngleEixo(tras);
|
||||
|
||||
compensacoes = FatoresVelocidade(
|
||||
ctrl.TipoMovimento,
|
||||
angFrenteDeg,
|
||||
angTrasDeg,
|
||||
g,
|
||||
PoliticaFator.ReferenciaNoCentro,
|
||||
fatorMaximo: 1.6
|
||||
);
|
||||
|
||||
const double alpha = 0.2;
|
||||
if (_compAnt != null && _compAnt.Count == compensacoes.Count)
|
||||
compensacoes = compensacoes.ToDictionary(kv => kv.Key,
|
||||
kv => _compAnt.TryGetValue(kv.Key, out var prev) ? prev * (1 - alpha) + kv.Value * alpha : kv.Value);
|
||||
_compAnt = new Dictionary<string, double>(compensacoes);
|
||||
}
|
||||
|
||||
public struct GeometriaRobo
|
||||
{
|
||||
public double L; // entre-eixos (m)
|
||||
public double W; // bitola (m)
|
||||
}
|
||||
|
||||
// assuma ângulos em graus no seu sistema
|
||||
static double Deg2Rad(double deg) => deg * Math.PI / 180.0;
|
||||
|
||||
static double EqAngleEixo(IEnumerable<double> angsDeg)
|
||||
{
|
||||
// Trata sequência vazia
|
||||
if (angsDeg == null) return 0;
|
||||
var list = angsDeg.ToList();
|
||||
if (list.Count == 0) return 0;
|
||||
|
||||
// média de tan e volta pra graus
|
||||
double mediaTan = list.Select(a => Math.Tan(a * Math.PI / 180.0)).Average();
|
||||
return Math.Atan(mediaTan) * 180.0 / Math.PI;
|
||||
}
|
||||
|
||||
static bool TryCalcularRaioCentro(TipoMovimentoDirecional modo, double deltaFrontDeg, double deltaRearDeg, GeometriaRobo g, out double Rc)
|
||||
{
|
||||
double tf = Math.Tan(Deg2Rad(deltaFrontDeg));
|
||||
double tr = Math.Tan(Deg2Rad(deltaRearDeg));
|
||||
|
||||
Rc = double.PositiveInfinity;
|
||||
|
||||
switch (modo)
|
||||
if (controle == null || !ModosCompensados.Contains(controle.TipoMovimento))
|
||||
{
|
||||
case TipoMovimentoDirecional.RodasDianteiras:
|
||||
if (Math.Abs(tf) < 1e-6) return false; // reta
|
||||
Rc = g.L / tf;
|
||||
return true;
|
||||
ResetarTudo(modulos);
|
||||
return;
|
||||
}
|
||||
|
||||
case TipoMovimentoDirecional.RodasTraseiras:
|
||||
if (Math.Abs(tr) < 1e-6) return false;
|
||||
Rc = g.L / tr;
|
||||
return true;
|
||||
GeometriaRobo g = ObterGeometriaAtual();
|
||||
|
||||
case TipoMovimentoDirecional.MovimentoArco:
|
||||
// uso geral: Rc = L / (tan(df) - tan(dr))
|
||||
double denom = (tf - tr);
|
||||
if (Math.Abs(denom) < 1e-6) return false;
|
||||
Rc = g.L / denom;
|
||||
return true;
|
||||
if (g.L <= 0 || g.W <= 0)
|
||||
{
|
||||
ResetarTudo(modulos);
|
||||
return;
|
||||
}
|
||||
|
||||
/*
|
||||
* Pega posição física REAL das rodas.
|
||||
*/
|
||||
double ef = 0;
|
||||
double df = 0;
|
||||
double et = 0;
|
||||
double dt = 0;
|
||||
|
||||
bool angulosValidos =
|
||||
TryObterAnguloCanonico(modulos, "EF", out ef) &&
|
||||
TryObterAnguloCanonico(modulos, "DF", out df) &&
|
||||
TryObterAnguloCanonico(modulos, "ET", out et) &&
|
||||
TryObterAnguloCanonico(modulos, "DT", out dt);
|
||||
|
||||
if (!angulosValidos)
|
||||
{
|
||||
/*
|
||||
* Não inventa geometria quando não sabemos
|
||||
* fisicamente onde as rodas estão.
|
||||
*
|
||||
* A saúde DIR decide em outro nível se a
|
||||
* movimentação pode continuar.
|
||||
*/
|
||||
ResetarTudo(modulos);
|
||||
return;
|
||||
}
|
||||
|
||||
/*
|
||||
* Transforma as duas rodas em um ângulo equivalente
|
||||
* de cada eixo.
|
||||
*
|
||||
* Média em tan-space é melhor do que média simples
|
||||
* para geometria de esterçamento.
|
||||
*/
|
||||
double angFrente = AnguloEquivalenteEixo(ef, df);
|
||||
double angTras = AnguloEquivalenteEixo(et, dt);
|
||||
|
||||
/*
|
||||
* Filtro temporal independente da frequência do timer.
|
||||
*/
|
||||
DateTime agora = DateTime.Now;
|
||||
|
||||
double dtSeg =
|
||||
_ultimoCalculo == DateTime.MinValue
|
||||
? 0
|
||||
: (agora - _ultimoCalculo).TotalSeconds;
|
||||
|
||||
_ultimoCalculo = agora;
|
||||
|
||||
if (dtSeg <= 0 || dtSeg > 1.0)
|
||||
{
|
||||
_angFrenteFiltrado = angFrente;
|
||||
_angTrasFiltrado = angTras;
|
||||
}
|
||||
else
|
||||
{
|
||||
double alpha = 1.0 - Math.Exp(-dtSeg / TauFiltroAnguloSeg);
|
||||
_angFrenteFiltrado += alpha * (angFrente - _angFrenteFiltrado);
|
||||
_angTrasFiltrado += alpha * (angTras - _angTrasFiltrado);
|
||||
}
|
||||
|
||||
double deltaCurvatura = Math.Abs(_angFrenteFiltrado - _angTrasFiltrado);
|
||||
|
||||
if (_curvaAtiva)
|
||||
{
|
||||
if (deltaCurvatura <= AnguloSaidaCurvaGraus)
|
||||
{
|
||||
_curvaAtiva = false;
|
||||
ResetarCompensacao(modulos);
|
||||
return;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
if (deltaCurvatura < AnguloEntradaCurvaGraus)
|
||||
{
|
||||
ResetarCompensacao(modulos);
|
||||
return;
|
||||
}
|
||||
|
||||
_curvaAtiva = true;
|
||||
}
|
||||
|
||||
double iccX;
|
||||
double iccY;
|
||||
|
||||
if (!TryCalcularICC(_angFrenteFiltrado, _angTrasFiltrado, g, out iccX, out iccY))
|
||||
{
|
||||
ResetarTudo(modulos);
|
||||
return;
|
||||
}
|
||||
|
||||
var raios = CalcularRaios(g, iccX, iccY);
|
||||
|
||||
if (raios.Count == 0)
|
||||
{
|
||||
ResetarTudo(modulos);
|
||||
return;
|
||||
}
|
||||
|
||||
double raioMin = raios.Values.Min();
|
||||
|
||||
/*
|
||||
* ICC praticamente sobre uma roda.
|
||||
* Geometria extrema demais para nossa política.
|
||||
*/
|
||||
if (raioMin < 0.05)
|
||||
{
|
||||
ResetarTudo(modulos);
|
||||
return;
|
||||
}
|
||||
|
||||
var novoEstado = new Dictionary<string, EstadoCompensacaoRoda>();
|
||||
|
||||
foreach (var kv in raios)
|
||||
{
|
||||
double fatorBruto = kv.Value / raioMin;
|
||||
|
||||
bool saturado = fatorBruto > FatorMaximo;
|
||||
|
||||
double fator = Math.Max(1.0, Math.Min(fatorBruto, FatorMaximo));
|
||||
|
||||
novoEstado[kv.Key] = new EstadoCompensacaoRoda
|
||||
{
|
||||
ModuloId = kv.Key,
|
||||
RaioM = kv.Value,
|
||||
Fator = fator,
|
||||
Saturado = saturado,
|
||||
|
||||
AnguloFrenteGraus = _angFrenteFiltrado,
|
||||
AnguloTrasGraus = _angTrasFiltrado,
|
||||
|
||||
IccX = iccX,
|
||||
IccY = iccY
|
||||
};
|
||||
}
|
||||
|
||||
lock (_sync)
|
||||
{
|
||||
_estado = novoEstado;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
private static GeometriaRobo ObterGeometriaAtual()
|
||||
{
|
||||
return new GeometriaRobo
|
||||
{
|
||||
L = VariaveisEquipamento.DistanciaEntreEixosCm / 100.0,
|
||||
|
||||
/*
|
||||
* CRIAR esta propriedade.
|
||||
* Centro da roda esquerda até centro
|
||||
* da roda direita.
|
||||
*/
|
||||
W = VariaveisEquipamento.BitolaRodasCm / 100.0
|
||||
};
|
||||
}
|
||||
|
||||
|
||||
private static bool TryObterAnguloCanonico(List<ModuloMvdModel> modulos, string modId, out double angulo)
|
||||
{
|
||||
angulo = 0.0;
|
||||
|
||||
var modulo = modulos.FirstOrDefault(x => x.Modulo_ID == modId);
|
||||
|
||||
var dir = modulo?.DirMotor;
|
||||
|
||||
if (dir == null ||
|
||||
!dir.Inicializado ||
|
||||
dir.Leitura == null ||
|
||||
dir.Leitura.NumeroPulsos == null ||
|
||||
dir.Leitura.NumeroPulsos.lidoEm == DateTime.MinValue)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
DateTime agora = DateTime.Now;
|
||||
|
||||
double idadeMs = (agora - dir.Leitura.NumeroPulsos.lidoEm).TotalMilliseconds;
|
||||
|
||||
if (idadeMs < 0 || idadeMs > IdadeMaximaAnguloMs)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
angulo = ConverterAnguloMotorParaVeiculo(modId, dir.AnguloLeitura);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
private static double ConverterAnguloMotorParaVeiculo(string modId, double anguloMotor)
|
||||
{
|
||||
switch (modId)
|
||||
{
|
||||
case "EF":
|
||||
return -anguloMotor;
|
||||
|
||||
case "DF":
|
||||
return anguloMotor;
|
||||
|
||||
case "ET":
|
||||
return anguloMotor;
|
||||
|
||||
case "DT":
|
||||
return -anguloMotor;
|
||||
|
||||
default:
|
||||
return false;
|
||||
return 0.0;
|
||||
}
|
||||
}
|
||||
|
||||
// Raio de trajetória por roda para curva à esquerda (Rc > 0) ou direita (Rc < 0)
|
||||
// Posições das rodas em relação ao centro do chassi: x = ±L/2, y = ±W/2
|
||||
static void CalcularRaiosPorRoda(TipoMovimentoDirecional modo, double Rc, GeometriaRobo g, out double rFL, out double rFR, out double rRL, out double rRR)
|
||||
|
||||
private static double AnguloEquivalenteEixo(double esquerdo, double direito)
|
||||
{
|
||||
// Por simplicidade, tratamos três casos:
|
||||
// 1) Dianteiras: ICC está sobre a linha do eixo traseiro
|
||||
// 2) Traseiras: ICC está sobre a linha do eixo dianteiro
|
||||
// 3) ArcoOposto (simétrico): ICC está próximo ao centro; distância axial = L/2 para ambos
|
||||
double ay = Math.Abs(Rc); // magnitude do Rc; sinal define esquerda/direita
|
||||
double tanE = Math.Tan(Deg2Rad(esquerdo));
|
||||
double tanD = Math.Tan(Deg2Rad(direito));
|
||||
|
||||
if (modo == TipoMovimentoDirecional.RodasDianteiras)
|
||||
{
|
||||
// Traseiro: raio = Rc ± W/2
|
||||
rRL = Math.Max(1e-6, ay - g.W / 2.0);
|
||||
rRR = Math.Max(1e-6, ay + g.W / 2.0);
|
||||
// Dianteiro: distância diagonal até ICC (pitágoras)
|
||||
rFL = Math.Sqrt(Math.Pow(ay - g.W / 2.0, 2) + Math.Pow(g.L, 2));
|
||||
rFR = Math.Sqrt(Math.Pow(ay + g.W / 2.0, 2) + Math.Pow(g.L, 2));
|
||||
}
|
||||
else if (modo == TipoMovimentoDirecional.RodasTraseiras)
|
||||
{
|
||||
// Dianteiro: raio = Rc ± W/2
|
||||
rFL = Math.Max(1e-6, ay - g.W / 2.0);
|
||||
rFR = Math.Max(1e-6, ay + g.W / 2.0);
|
||||
// Traseiro: distância diagonal
|
||||
rRL = Math.Sqrt(Math.Pow(ay - g.W / 2.0, 2) + Math.Pow(g.L, 2));
|
||||
rRR = Math.Sqrt(Math.Pow(ay + g.W / 2.0, 2) + Math.Pow(g.L, 2));
|
||||
}
|
||||
else // Arco (simétrico)
|
||||
{
|
||||
// Ambos eixos a L/2 do centro
|
||||
double a = g.L / 2.0;
|
||||
rFL = Math.Sqrt(Math.Pow(ay - g.W / 2.0, 2) + a * a);
|
||||
rFR = Math.Sqrt(Math.Pow(ay + g.W / 2.0, 2) + a * a);
|
||||
rRL = Math.Sqrt(Math.Pow(ay - g.W / 2.0, 2) + a * a);
|
||||
rRR = Math.Sqrt(Math.Pow(ay + g.W / 2.0, 2) + a * a);
|
||||
}
|
||||
|
||||
// Observação: sinal de Rc indica sentido da curva:
|
||||
// Rc > 0 -> curva à esquerda (rodas esquerdas são internas)
|
||||
// Rc < 0 -> curva à direita (troque "interna"↔"externa")
|
||||
// Aqui usamos |Rc|; para decidir quem é interna/externa, use o sinal fora daqui.
|
||||
return Rad2Deg(Math.Atan((tanE + tanD) / 2.0));
|
||||
}
|
||||
|
||||
// Retorna fatores por roda. Modo "SomenteAumentarExterna" mantém a interna em 1.0
|
||||
public enum PoliticaFator { ReferenciaNoCentro, SomenteAumentarExterna }
|
||||
|
||||
public static Dictionary<string, double> FatoresVelocidade(TipoMovimentoDirecional modo, double deltaFrontDeg, double deltaRearDeg, GeometriaRobo g, PoliticaFator politica, double fatorMaximo = 1.5)
|
||||
private static bool TryCalcularICC(double deltaFrenteGraus, double deltaTrasGraus, GeometriaRobo g, out double iccX, out double iccY)
|
||||
{
|
||||
if (!TryCalcularRaioCentro(modo, deltaFrontDeg, deltaRearDeg, g, out double Rc))
|
||||
return Variaveis.OperacaoEmAndamento.DispMvd.Dados.Modulos.ToDictionary(x => x.Modulo_ID, x => 1.0);
|
||||
iccX = 0;
|
||||
iccY = 0;
|
||||
|
||||
CalcularRaiosPorRoda(modo, Rc, g, out var rFL, out var rFR, out var rRL, out var rRR);
|
||||
double df = Deg2Rad(deltaFrenteGraus);
|
||||
double dr = Deg2Rad(deltaTrasGraus);
|
||||
|
||||
// Centro do robô percorre raio |Rc|
|
||||
double RcAbs = Math.Abs(Rc);
|
||||
/*
|
||||
* Vetor normal à orientação de cada eixo.
|
||||
*/
|
||||
double nfx = -Math.Sin(df);
|
||||
double nfy = Math.Cos(df);
|
||||
double nrx = -Math.Sin(dr);
|
||||
double nry = Math.Cos(dr);
|
||||
|
||||
// Descobrir lado interno pela curva (sinal de Rc)
|
||||
bool esquerda = Variaveis.OperacaoEmAndamento.Controle.TiposControle.FirstOrDefault(x => x.Tipo == T_Code.Dir).UltimaDirecao == Direcao.Esquerda; // Rc > 0;
|
||||
/*
|
||||
* cross(nf, nr)
|
||||
*/
|
||||
double denominador = nfx * nry - nfy * nrx;
|
||||
|
||||
double rIntFront = esquerda ? rFL : rFR;
|
||||
double rExtFront = esquerda ? rFR : rFL;
|
||||
double rIntRear = esquerda ? rRL : rRR;
|
||||
double rExtRear = esquerda ? rRR : rRL;
|
||||
|
||||
double fFL, fFR, fRL, fRR;
|
||||
|
||||
if (politica == PoliticaFator.ReferenciaNoCentro)
|
||||
/*
|
||||
* Normais quase paralelas:
|
||||
* raio tende ao infinito.
|
||||
*/
|
||||
if (Math.Abs(denominador) < 1e-6)
|
||||
{
|
||||
fFL = rIntFront / RcAbs;
|
||||
fFR = rExtFront / RcAbs;
|
||||
fRL = rIntRear / RcAbs;
|
||||
fRR = rExtRear / RcAbs;
|
||||
}
|
||||
else // SomenteAumentarExterna
|
||||
{
|
||||
fFL = 1.0;
|
||||
fFR = rExtFront / rIntFront;
|
||||
fRL = 1.0;
|
||||
fRR = rExtRear / rIntRear;
|
||||
return false;
|
||||
}
|
||||
|
||||
// Saturação e leve suavização (ex: EMA) podem ser aplicadas aqui
|
||||
fFL = Math.Min(fatorMaximo, fFL);
|
||||
fFR = Math.Min(fatorMaximo, fFR);
|
||||
fRL = Math.Min(fatorMaximo, fRL);
|
||||
fRR = Math.Min(fatorMaximo, fRR);
|
||||
/*
|
||||
* Centro do eixo dianteiro:
|
||||
* F = (+L/2, 0)
|
||||
*
|
||||
* Centro do eixo traseiro:
|
||||
* R = (-L/2, 0)
|
||||
*
|
||||
* vetor R-F = (-L,0)
|
||||
*/
|
||||
double vx = -g.L;
|
||||
|
||||
return Variaveis.OperacaoEmAndamento.DispMvd.Dados.Modulos.ToDictionary(x => x.Modulo_ID, x => (double)(x.Modulo_ID == "ET" ? fRL : x.Modulo_ID == "DT" ? fRR : x.Modulo_ID == "EF" ? fFL : x.Modulo_ID == "DF" ? fFR : 1.0));
|
||||
double vy = 0.0;
|
||||
|
||||
/*
|
||||
* t =
|
||||
* cross(R-F, nr) /
|
||||
* cross(nf, nr)
|
||||
*/
|
||||
double t = (vx * nry - vy * nrx) / denominador;
|
||||
|
||||
iccX = g.L / 2.0 + t * nfx;
|
||||
|
||||
iccY = t * nfy;
|
||||
|
||||
return
|
||||
!double.IsNaN(iccX) &&
|
||||
!double.IsNaN(iccY) &&
|
||||
!double.IsInfinity(iccX) &&
|
||||
!double.IsInfinity(iccY);
|
||||
}
|
||||
|
||||
|
||||
private static Dictionary<string, double> CalcularRaios(GeometriaRobo g, double iccX, double iccY)
|
||||
{
|
||||
double xF = g.L / 2.0;
|
||||
double xT = -g.L / 2.0;
|
||||
double yE = g.W / 2.0;
|
||||
double yD = -g.W / 2.0;
|
||||
|
||||
return new Dictionary<string, double>
|
||||
{
|
||||
{
|
||||
"EF",
|
||||
Distancia(xF, yE, iccX, iccY)
|
||||
},
|
||||
{
|
||||
"DF",
|
||||
Distancia(xF, yD, iccX, iccY)
|
||||
},
|
||||
{
|
||||
"ET",
|
||||
Distancia(xT, yE, iccX, iccY)
|
||||
},
|
||||
{
|
||||
"DT",
|
||||
Distancia(xT, yD, iccX, iccY)
|
||||
}
|
||||
};
|
||||
}
|
||||
|
||||
|
||||
public static double ObterFator(string modId)
|
||||
{
|
||||
lock (_sync)
|
||||
{
|
||||
EstadoCompensacaoRoda e;
|
||||
|
||||
if (_estado.TryGetValue(modId, out e))
|
||||
{
|
||||
return Math.Max(1.0, e.Fator);
|
||||
}
|
||||
}
|
||||
|
||||
return 1.0;
|
||||
}
|
||||
|
||||
|
||||
public static int CalcularRpmMotor(string modId, int rpmBase, int rpmMin, int rpmMax)
|
||||
{
|
||||
if (rpmBase <= 0)
|
||||
return 0;
|
||||
|
||||
double fatorRoda = ObterFator(modId);
|
||||
|
||||
double maiorFator;
|
||||
|
||||
lock (_sync)
|
||||
{
|
||||
maiorFator =
|
||||
_estado.Count == 0
|
||||
? 1.0
|
||||
: _estado.Values.Max(x => x.Fator);
|
||||
}
|
||||
|
||||
/*
|
||||
* Primeiro limita o RPM base válido.
|
||||
*/
|
||||
double baseSolicitada = Math.Max(rpmMin, Math.Min(rpmBase, rpmMax));
|
||||
|
||||
/*
|
||||
* Se a roda externa estouraria RPM máximo,
|
||||
* reduzimos TODAS proporcionalmente.
|
||||
*
|
||||
* Assim preservamos a geometria.
|
||||
*/
|
||||
double baseEfetiva = baseSolicitada;
|
||||
|
||||
if (maiorFator > 1.0)
|
||||
{
|
||||
double baseMaximaPermitida = rpmMax / maiorFator;
|
||||
|
||||
baseEfetiva = Math.Min(baseEfetiva, baseMaximaPermitida);
|
||||
}
|
||||
|
||||
/*
|
||||
* Normalmente respeitamos rpmMin.
|
||||
*
|
||||
* Porém, se a própria geometria tornar impossível manter
|
||||
* simultaneamente rpmMin e rpmMax, priorizamos a proporção
|
||||
* cinemática e o limite máximo.
|
||||
*/
|
||||
bool geometriaIncompativelComRpmMin = maiorFator > 1.0 && (rpmMax / maiorFator) < rpmMin;
|
||||
|
||||
if (!geometriaIncompativelComRpmMin)
|
||||
{
|
||||
baseEfetiva = Math.Max(baseEfetiva, rpmMin);
|
||||
}
|
||||
|
||||
double rpm = baseEfetiva * fatorRoda;
|
||||
|
||||
return Convert.ToInt32(Math.Max(0, Math.Min(rpm, rpmMax)));
|
||||
}
|
||||
|
||||
|
||||
public static EstadoCompensacaoRoda ObterEstado(string modId)
|
||||
{
|
||||
lock (_sync)
|
||||
{
|
||||
EstadoCompensacaoRoda e;
|
||||
|
||||
if (!_estado.TryGetValue(modId, out e))
|
||||
{
|
||||
return new EstadoCompensacaoRoda
|
||||
{
|
||||
ModuloId = modId,
|
||||
Fator = 1.0
|
||||
};
|
||||
}
|
||||
|
||||
return e;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
private static void ResetarTudo(List<ModuloMvdModel> modulos = null)
|
||||
{
|
||||
ResetarCompensacao(modulos);
|
||||
|
||||
_angFrenteFiltrado = 0.0;
|
||||
_angTrasFiltrado = 0.0;
|
||||
_ultimoCalculo = DateTime.MinValue;
|
||||
_curvaAtiva = false;
|
||||
}
|
||||
|
||||
|
||||
private static void ResetarCompensacao(List<ModuloMvdModel> modulos)
|
||||
{
|
||||
lock (_sync)
|
||||
{
|
||||
_estado = new Dictionary<string, EstadoCompensacaoRoda>();
|
||||
|
||||
if (modulos != null)
|
||||
{
|
||||
foreach (var m in modulos)
|
||||
{
|
||||
if (m == null || string.IsNullOrWhiteSpace(m.Modulo_ID))
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
_estado[m.Modulo_ID] = new EstadoCompensacaoRoda
|
||||
{
|
||||
ModuloId = m.Modulo_ID,
|
||||
Fator = 1.0
|
||||
};
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
private static double Distancia(double x1, double y1, double x2, double y2)
|
||||
{
|
||||
double dx = x1 - x2;
|
||||
double dy = y1 - y2;
|
||||
|
||||
return Math.Sqrt(dx * dx + dy * dy);
|
||||
}
|
||||
|
||||
|
||||
private static double Deg2Rad(double graus)
|
||||
{
|
||||
return graus * Math.PI / 180.0;
|
||||
}
|
||||
|
||||
|
||||
private static double Rad2Deg(double rad)
|
||||
{
|
||||
return rad * 180.0 / Math.PI;
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
|
|
|||
|
|
@ -4,6 +4,7 @@ using System;
|
|||
using System.Collections.Generic;
|
||||
using System.Data;
|
||||
using System.Linq;
|
||||
using System.Threading;
|
||||
using System.Threading.Tasks;
|
||||
using static AgroBase.Models.Enums;
|
||||
|
||||
|
|
@ -46,6 +47,10 @@ namespace AgroBase.Models.Modules
|
|||
[JsonIgnore]
|
||||
public AsyncTaskTimerModel tmrRegistradorDIR;
|
||||
|
||||
public bool MovimentoDirecionalCombinadoEmExecucao { get; private set; }
|
||||
private readonly SemaphoreSlim _gateMovimentoCombinado = new SemaphoreSlim(1, 1);
|
||||
private CancellationTokenSource _ctsMovimentoCombinado;
|
||||
|
||||
|
||||
public List<ModuloMvdModel> Modulos { get; set; } = new List<ModuloMvdModel>();
|
||||
|
||||
|
|
@ -150,6 +155,10 @@ namespace AgroBase.Models.Modules
|
|||
x.MovMotor.Temperatura = 0;
|
||||
x.MovMotor.RPM_SP = 0;
|
||||
x.MovMotor.RPM_SP_Controle = 0;
|
||||
x.MovMotor.UltimoRPMComandado = 0;
|
||||
x.MovMotor.UltimoComandoEnviado = DateTime.MinValue;
|
||||
x.MovMotor.UltimoSetPointEnviado = 0;
|
||||
x.MovMotor.UltimaDirecao = Direcao.Parado;
|
||||
|
||||
x.DirMotor.Angulo_SP = 0;
|
||||
x.DirMotor.Sentido_SP = Sentido.Parado;
|
||||
|
|
@ -163,6 +172,8 @@ namespace AgroBase.Models.Modules
|
|||
|
||||
public void RegistrarLogMov(ModuloMvdModel Modulo)
|
||||
{
|
||||
var comp = MovimentacaoMovimentoCompensadoModel.ObterEstado(Modulo.Modulo_ID);
|
||||
|
||||
Modulo.MovMotor.LogGrafico.Add(new MvdMotorMOVSensorimanetoGrafico()
|
||||
{
|
||||
Momento = DateTime.Now,
|
||||
|
|
@ -177,7 +188,7 @@ namespace AgroBase.Models.Modules
|
|||
Tensao = Modulo.MovMotor.Tensao,
|
||||
Velocidade = Modulo.MovMotor.VelocidadeInstantanea,
|
||||
Potencia = Modulo.MovMotor.Potencia,
|
||||
CicloTrabalho = Modulo.MovMotor.CicloTrabalho
|
||||
CicloTrabalho = Modulo.MovMotor.CicloTrabalho,
|
||||
});
|
||||
|
||||
if (Modulo.MovMotor.LogGrafico.Count > 60)
|
||||
|
|
@ -332,8 +343,11 @@ namespace AgroBase.Models.Modules
|
|||
//}
|
||||
private DateTime _ultimoEnvioControleMov = DateTime.MinValue;
|
||||
private DateTime _ultimoRegistroLogMov = DateTime.MinValue;
|
||||
private static readonly TimeSpan IntervaloControleMov = TimeSpan.FromSeconds(1);
|
||||
private static readonly TimeSpan IntervaloControleMov = TimeSpan.FromMilliseconds(100);
|
||||
private static readonly TimeSpan IntervaloLogMov = TimeSpan.FromSeconds(1);
|
||||
private DateTime _ultimaAtualizacaoCompensacaoMov = DateTime.MinValue;
|
||||
private static readonly TimeSpan IntervaloCompensacaoMov = TimeSpan.FromMilliseconds(50);
|
||||
|
||||
private async Task tmrRegistradorMOV_Tick()
|
||||
{
|
||||
var op = Variaveis.OperacaoEmAndamento;
|
||||
|
|
@ -343,40 +357,27 @@ namespace AgroBase.Models.Modules
|
|||
|
||||
DateTime agora = DateTime.Now;
|
||||
|
||||
if (!OIDCanService.Iniciado && SerialService.DispositivosMapeados.Any(x => x.Dispositivo == T_Code.Mov))
|
||||
{
|
||||
foreach (var leitura in OIDCanService.DadosLeitura.Where(x => x.UltimoComandoRecebido.AddSeconds(10) < agora))
|
||||
{
|
||||
OIDCanService.RequisitarDado(
|
||||
leitura.EnderecoTx,
|
||||
leitura.EnderecoRx,
|
||||
OIDCanService.OidHandler.OidParametros.CodErro
|
||||
);
|
||||
}
|
||||
|
||||
tmrRegistradorMOV.SetInterval(3000);
|
||||
return;
|
||||
}
|
||||
|
||||
bool deveEnviarControle =
|
||||
_ultimoEnvioControleMov == DateTime.MinValue ||
|
||||
agora - _ultimoEnvioControleMov >= IntervaloControleMov;
|
||||
|
||||
bool deveRegistrarLog =
|
||||
_ultimoRegistroLogMov == DateTime.MinValue ||
|
||||
agora - _ultimoRegistroLogMov >= IntervaloLogMov;
|
||||
|
||||
bool enviouControle = false;
|
||||
bool registrouLog = false;
|
||||
|
||||
var controleMov = deveEnviarControle ? op.Controle.TiposControle?.FirstOrDefault(x => x.Tipo == T_Code.Mov) : null;
|
||||
|
||||
/*
|
||||
* Uma única tentativa global por tick.
|
||||
* O OIDCanService escolhe o driver e o parâmetro mais adequado.
|
||||
*/
|
||||
OIDCanService.AtualizarDados();
|
||||
|
||||
bool deveEnviarControle = _ultimoEnvioControleMov == DateTime.MinValue || agora - _ultimoEnvioControleMov >= IntervaloControleMov;
|
||||
bool deveRegistrarLog = _ultimoRegistroLogMov == DateTime.MinValue || agora - _ultimoRegistroLogMov >= IntervaloLogMov;
|
||||
bool deveAtualizarCompensacao = _ultimaAtualizacaoCompensacaoMov == DateTime.MinValue || agora - _ultimaAtualizacaoCompensacaoMov >= IntervaloCompensacaoMov;
|
||||
|
||||
if (deveAtualizarCompensacao)
|
||||
{
|
||||
MovimentacaoMovimentoCompensadoModel.AtualizarDados();
|
||||
_ultimaAtualizacaoCompensacaoMov = agora;
|
||||
}
|
||||
|
||||
bool enviouControle = false;
|
||||
bool registrouLog = false;
|
||||
|
||||
var controleMov = deveEnviarControle ? op.Controle.TiposControle?.FirstOrDefault(x => x.Tipo == T_Code.Mov) : null;
|
||||
|
||||
foreach (var modulo in Modulos)
|
||||
{
|
||||
if (!modulo.MovMotor.Inicializado)
|
||||
|
|
@ -387,7 +388,7 @@ namespace AgroBase.Models.Modules
|
|||
* O polling agora é centralizado em AtualizarDados().
|
||||
*/
|
||||
|
||||
if (deveEnviarControle && controleMov != null)
|
||||
if (!MovimentoDirecionalCombinadoEmExecucao && deveEnviarControle && controleMov != null)
|
||||
{
|
||||
modulo.MovMotor.EnviarComandoControle(controleMov.UltimaDirecao);
|
||||
|
||||
|
|
@ -477,7 +478,7 @@ namespace AgroBase.Models.Modules
|
|||
* Durante referenciamento não interferimos no comando,
|
||||
* pois o próprio fluxo de referência assume o controle do MKS.
|
||||
*/
|
||||
if (!MKS057DCanService.Referenciando && deveEnviarControle && controleDir != null && !driver.AtingiuAnguloSP)
|
||||
if (!MovimentoDirecionalCombinadoEmExecucao && !MKS057DCanService.Referenciando && deveEnviarControle && controleDir != null && !driver.AtingiuAnguloSP)
|
||||
{
|
||||
driver.EnviarComandoControle(controleDir.UltimaDirecao);
|
||||
enviouControle = true;
|
||||
|
|
@ -532,51 +533,100 @@ namespace AgroBase.Models.Modules
|
|||
|
||||
public async Task ExecutarMovimentoDirecionalCombinado(Direcao DirecaoAtual)
|
||||
{
|
||||
var op = Variaveis.OperacaoEmAndamento;
|
||||
_ctsMovimentoCombinado?.Cancel();
|
||||
|
||||
if (DirecaoAtual == Direcao.Parado)
|
||||
var cts = new CancellationTokenSource();
|
||||
_ctsMovimentoCombinado = cts;
|
||||
|
||||
await _gateMovimentoCombinado.WaitAsync();
|
||||
|
||||
try
|
||||
{
|
||||
foreach (var Modulo in Modulos)
|
||||
if (cts.IsCancellationRequested)
|
||||
return;
|
||||
|
||||
MovimentoDirecionalCombinadoEmExecucao = true;
|
||||
|
||||
var op = Variaveis.OperacaoEmAndamento;
|
||||
|
||||
var ctrlMov = op.Controle.TiposControle.First(x => x.Tipo == T_Code.Mov);
|
||||
var ctrlDir = op.Controle.TiposControle.First(x => x.Tipo == T_Code.Dir);
|
||||
|
||||
ctrlDir.DirecaoAtual = DirecaoAtual;
|
||||
ctrlDir.UltimaDirecao = DirecaoAtual;
|
||||
|
||||
// Sempre parar a tração antes de alterar a geometria das rodas
|
||||
ctrlMov.DirecaoAtual = Direcao.Parado;
|
||||
ctrlMov.UltimaDirecao = Direcao.Parado;
|
||||
|
||||
foreach (var modulo in Modulos)
|
||||
{
|
||||
Modulo.MovMotor.EnviarComandoControle(Direcao.Parado);
|
||||
modulo.MovMotor.EnviarComandoControle(Direcao.Parado);
|
||||
}
|
||||
|
||||
await Task.Delay(1000);
|
||||
}
|
||||
if (DirecaoAtual == Direcao.Parado)
|
||||
{
|
||||
await Task.Delay(1000, cts.Token);
|
||||
}
|
||||
|
||||
switch (op.Controle.TipoMovimento)
|
||||
{
|
||||
case TipoMovimentoDirecional.RotacionarNoEixo:
|
||||
case TipoMovimentoDirecional.MovimentoLateral:
|
||||
switch (op.Controle.TipoMovimento)
|
||||
{
|
||||
case TipoMovimentoDirecional.RotacionarNoEixo:
|
||||
case TipoMovimentoDirecional.MovimentoLateral:
|
||||
|
||||
foreach (var Modulo in Modulos.Where(x => x.DirMotor.Inicializado && !x.DirMotor.PosicaoTipoMovimentoConcluido(DirecaoAtual)))
|
||||
{
|
||||
Modulo.DirMotor.EnviarComandoControle(DirecaoAtual);
|
||||
}
|
||||
|
||||
DateTime Agora = DateTime.Now;
|
||||
bool Sucesso = Modulos.All(x => x.DirMotor.PosicaoTipoMovimentoConcluido(DirecaoAtual));
|
||||
while (Agora.AddMilliseconds(5000) > DateTime.Now && !Sucesso)
|
||||
{
|
||||
await Task.Delay(100);
|
||||
Sucesso = Modulos.All(x => x.DirMotor.PosicaoTipoMovimentoConcluido(DirecaoAtual));
|
||||
}
|
||||
|
||||
if (Sucesso && DirecaoAtual != Direcao.Parado)
|
||||
{
|
||||
op.Controle.PercentualVelocidadeSP = 20;
|
||||
foreach (var Modulo in Modulos)
|
||||
foreach (var Modulo in Modulos.Where(x => x.DirMotor.Inicializado && !x.DirMotor.PosicaoTipoMovimentoConcluido(DirecaoAtual)))
|
||||
{
|
||||
Modulo.MovMotor.EnviarComandoControle(
|
||||
DirecaoAtual == Direcao.Direita ? Direcao.Frente :
|
||||
DirecaoAtual == Direcao.Esquerda ? Direcao.Tras :
|
||||
Direcao.Parado
|
||||
);
|
||||
Modulo.DirMotor.EnviarComandoControle(DirecaoAtual);
|
||||
}
|
||||
}
|
||||
|
||||
break;
|
||||
DateTime Agora = DateTime.Now;
|
||||
bool Sucesso = Modulos.All(x => x.DirMotor.PosicaoTipoMovimentoConcluido(DirecaoAtual));
|
||||
while (Agora.AddMilliseconds(5000) > DateTime.Now && !Sucesso)
|
||||
{
|
||||
await Task.Delay(100, cts.Token);
|
||||
Sucesso = Modulos.All(x => x.DirMotor.PosicaoTipoMovimentoConcluido(DirecaoAtual));
|
||||
}
|
||||
|
||||
if (Sucesso && DirecaoAtual != Direcao.Parado)
|
||||
{
|
||||
op.Controle.PercentualVelocidadeSP = 20;
|
||||
|
||||
Direcao direcaoMov = DirecaoAtual == Direcao.Direita ? Direcao.Frente : DirecaoAtual == Direcao.Esquerda ? Direcao.Tras : Direcao.Parado;
|
||||
ctrlMov.DirecaoAtual = direcaoMov;
|
||||
ctrlMov.UltimaDirecao = direcaoMov;
|
||||
|
||||
foreach (var Modulo in Modulos)
|
||||
{
|
||||
Modulo.MovMotor.EnviarComandoControle(
|
||||
DirecaoAtual == Direcao.Direita ? Direcao.Frente :
|
||||
DirecaoAtual == Direcao.Esquerda ? Direcao.Tras :
|
||||
Direcao.Parado
|
||||
);
|
||||
}
|
||||
}
|
||||
|
||||
if (!Sucesso)
|
||||
{
|
||||
ctrlMov.DirecaoAtual = Direcao.Parado;
|
||||
ctrlMov.UltimaDirecao = Direcao.Parado;
|
||||
|
||||
Variaveis.MostrarLog(
|
||||
$"[MVD] Movimento combinado não atingiu posição | " +
|
||||
$"Tipo={op.Controle.TipoMovimento} | " +
|
||||
$"Direcao={DirecaoAtual}"
|
||||
);
|
||||
|
||||
return;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
}
|
||||
}
|
||||
finally
|
||||
{
|
||||
MovimentoDirecionalCombinadoEmExecucao = false;
|
||||
_gateMovimentoCombinado.Release();
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -2115,6 +2115,8 @@ namespace AgroBase.Models.Modules
|
|||
Mod_ID = Modulo_ID,
|
||||
});
|
||||
|
||||
if (op.Sensoriamento?.Operacao?.ModsCalibragem == null)
|
||||
op.Sensoriamento.Operacao.ModsCalibragem = new Dictionary<T_Code, Dictionary<string, bool>>();
|
||||
op.Sensoriamento.Operacao.ModsCalibragem[Dispositivo] = new Dictionary<string, bool>();
|
||||
op.Sensoriamento.InserirLog(Dispositivo, StatusModulo.Conectado, 0, "Dispositivo conectado");
|
||||
}
|
||||
|
|
@ -3094,7 +3096,7 @@ namespace AgroBase.Models.Modules
|
|||
double temperatura = ConverterTemperaturaNtc(data, 4);
|
||||
double tensao = ConverterTensaoNtc(data, 6);
|
||||
AtualizarDados(ID_Num, posicao, componente, new List<dynamic>() { raw, temperatura, tensao });
|
||||
Variaveis.MostrarLog($"[SEN] sNTC | ID_Num: {ID_Num} | Posicao: {posicao} | Leitura: {raw} | Temperatura: {temperatura} | Tensao: {tensao}");
|
||||
//Variaveis.MostrarLog($"[SEN] sNTC | ID_Num: {ID_Num} | Posicao: {posicao} | Leitura: {raw} | Temperatura: {temperatura} | Tensao: {tensao}");
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -2045,11 +2045,10 @@ namespace AgroBase.Models
|
|||
{
|
||||
case T_Code.Mov:
|
||||
{
|
||||
var modsAtualizar = op.DispMvd.Dados.Modulos.Where(Mod => Mod.MovMotor.NovoRPMNecessario).Select(x => x.Modulo_ID).ToList();
|
||||
bool envioNecessario = ForcarEnvio || _Controle.RPM_SP != _ControleAnterior.RPM_SP || _Controle.EmFreio != _ControleAnterior.EmFreio;
|
||||
if (envioNecessario || modsAtualizar.Any())
|
||||
if (envioNecessario)
|
||||
{
|
||||
foreach (var Mod in op.DispMvd.Dados.Modulos.Where(x => envioNecessario ? true : modsAtualizar.Contains(x.Modulo_ID)))
|
||||
foreach (var Mod in op.DispMvd.Dados.Modulos)
|
||||
{
|
||||
Mod.MovMotor.EnviarComandoControle(DirecaoAtual);
|
||||
}
|
||||
|
|
@ -4740,6 +4739,8 @@ namespace AgroBase.Models
|
|||
|
||||
string moduloId = modulo?.Modulo_ID ?? mov?.Mod_ID ?? "";
|
||||
|
||||
var comp = MovimentacaoMovimentoCompensadoModel.ObterEstado(moduloId);
|
||||
|
||||
return new MovimentacaoModuloLogModel
|
||||
{
|
||||
Modulo_ID = moduloId,
|
||||
|
|
@ -4790,7 +4791,12 @@ namespace AgroBase.Models
|
|||
UltimoSetPointEnviado = mov?.UltimoSetPointEnviado ?? 0,
|
||||
UltimaDirecao = mov?.UltimaDirecao ?? Direcao.Parado,
|
||||
|
||||
FatorCompensacaoCurva = BuscarFatorCompensacao(moduloId)
|
||||
RPM_SP_Base = mov.RPM_SP_Controle,
|
||||
FatorCompensacao = comp.Fator,
|
||||
CompensacaoSaturada = comp.Saturado,
|
||||
IccX = comp.IccX,
|
||||
IccY = comp.IccY,
|
||||
RaioTrajetoriaM = comp.RaioM,
|
||||
};
|
||||
}
|
||||
|
||||
|
|
@ -4827,19 +4833,6 @@ namespace AgroBase.Models
|
|||
return valores.Count == 0 ? 0 : valores.Average();
|
||||
}
|
||||
|
||||
double BuscarFatorCompensacao(string moduloId)
|
||||
{
|
||||
if (string.IsNullOrWhiteSpace(moduloId))
|
||||
return 1.0;
|
||||
|
||||
if (MovimentacaoMovimentoCompensadoModel.compensacoes != null && MovimentacaoMovimentoCompensadoModel.compensacoes.TryGetValue(moduloId, out var fator))
|
||||
{
|
||||
return fator;
|
||||
}
|
||||
|
||||
return 1.0;
|
||||
}
|
||||
|
||||
var dados = op.DispMvd?.Dados;
|
||||
|
||||
var modulos = dados?.Modulos?
|
||||
|
|
@ -5245,7 +5238,6 @@ namespace AgroBase.Models
|
|||
CameraWorkerService.AtualizarDadosCameras();
|
||||
GPSService.AtualizaDadosRedis();
|
||||
LivoxManagerProcess.AtualizarLeitura();
|
||||
MovimentacaoMovimentoCompensadoModel.AtualizarDados();
|
||||
DalyBMSService.AtualizarDados();
|
||||
SerialService.DispositivosMapeados.ForEach(d => d.AtualizarDados());
|
||||
|
||||
|
|
@ -5974,7 +5966,12 @@ namespace AgroBase.Models
|
|||
public Direcao UltimaDirecao { get; set; }
|
||||
|
||||
// Compensação em curva
|
||||
public double FatorCompensacaoCurva { get; set; } = 1.0;
|
||||
public int RPM_SP_Base { get; set; }
|
||||
public double FatorCompensacao { get; set; }
|
||||
public double RaioTrajetoriaM { get; set; }
|
||||
public double IccX { get; set; }
|
||||
public double IccY { get; set; }
|
||||
public bool CompensacaoSaturada { get; set; }
|
||||
|
||||
public MovimentacaoModuloLogModel Clone()
|
||||
{
|
||||
|
|
|
|||
|
|
@ -2437,6 +2437,9 @@ namespace AgroBase.Models
|
|||
|
||||
public void ProjetarTrajetoriaFixa(GPSModel posicaoRobo = null)
|
||||
{
|
||||
if (Variaveis.OperacaoEmAndamento.Sensoriamento?.Operacao?.Modo != ModoOperacao.MapaGPS)
|
||||
return;
|
||||
|
||||
List<PontoTrajetoriaModel> _trajetoriaFixa = new List<PontoTrajetoriaModel>();
|
||||
List<List<GPSModel>> corredores;
|
||||
List<double> CorredoresLarguras;
|
||||
|
|
|
|||
|
|
@ -826,6 +826,7 @@ namespace AgroBase.Models
|
|||
public static double ComprimentoFrente { get; } = 90.0; // 7
|
||||
public static double ComprimentoTras { get; } = 40.0; // 107
|
||||
public static double DistanciaEntreEixosCm { get; set; } = 92.0;
|
||||
public static double BitolaRodasCm { get; set; } = 75.0;
|
||||
public static double LeverArmFrontalCm { get; set; } = 37.0; // cm
|
||||
public static double LeverArmLateralCm { get; set; } = 0.0; // cm
|
||||
public static double LarguraEquipamentoMm
|
||||
|
|
|
|||
|
|
@ -322,7 +322,7 @@ namespace AgroBase.Services
|
|||
bool PortaIsCan(SerialPort Porta = null);
|
||||
void RegistrarHandler(uint endereco_rx, ICanMessageHandler handler);
|
||||
bool Inicializar(int BitRate = 250);
|
||||
void AdicionarMensagemNaFila(T_Code Dispositivo, byte[] idTx, byte[] idRx, byte funcCodeTx, byte funcCodeRx, byte[] payload = null, bool get = true, bool contabilizarTimeoutHealth = true, bool mensagemDescoberta = false);
|
||||
bool AdicionarMensagemNaFila(T_Code Dispositivo, byte[] idTx, byte[] idRx, byte funcCodeTx, byte funcCodeRx, byte[] payload = null, bool get = true, bool contabilizarTimeoutHealth = true, bool mensagemDescoberta = false);
|
||||
void Fechar();
|
||||
bool PossuiMensagemPendente(T_Code dispositivo);
|
||||
|
||||
|
|
@ -373,6 +373,10 @@ namespace AgroBase.Services
|
|||
int PendentesNaoEnviadas { get; }
|
||||
int PendentesAguardandoResposta { get; }
|
||||
int FramesRxNaFila { get; }
|
||||
|
||||
double IdadeMaxFilaNaoEnviadaMs { get; }
|
||||
int PendentesNaoEnviadasAtrasadas { get; }
|
||||
double IdadeMediaFilaNaoEnviadaMs { get; }
|
||||
}
|
||||
|
||||
}
|
||||
|
|
@ -158,6 +158,54 @@ namespace AgroBase.Services
|
|||
|
||||
public int FramesRxNaFila => FramesRecebidos?.Count ?? 0;
|
||||
|
||||
public double IdadeMaxFilaNaoEnviadaMs
|
||||
{
|
||||
get
|
||||
{
|
||||
lock (_LockMensagens)
|
||||
{
|
||||
var pendentes = MensagensPendentes.Where(x => !x.Enviado).ToList();
|
||||
|
||||
if (!pendentes.Any())
|
||||
return 0;
|
||||
|
||||
DateTime agora = DateTime.Now;
|
||||
|
||||
return pendentes.Max(x => (agora - x.Momento).TotalMilliseconds);
|
||||
}
|
||||
}
|
||||
}
|
||||
public int PendentesNaoEnviadasAtrasadas
|
||||
{
|
||||
get
|
||||
{
|
||||
lock (_LockMensagens)
|
||||
{
|
||||
DateTime limite = DateTime.Now.AddMilliseconds(-500);
|
||||
|
||||
return MensagensPendentes.Count(x => !x.Enviado && x.Momento < limite);
|
||||
}
|
||||
}
|
||||
}
|
||||
public double IdadeMediaFilaNaoEnviadaMs
|
||||
{
|
||||
get
|
||||
{
|
||||
lock (_LockMensagens)
|
||||
{
|
||||
var pendentes = MensagensPendentes.Where(x => !x.Enviado).ToList();
|
||||
|
||||
if (!pendentes.Any())
|
||||
return 0;
|
||||
|
||||
DateTime agora = DateTime.Now;
|
||||
|
||||
return pendentes.Average(x => (agora - x.Momento).TotalMilliseconds);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
private bool DebugMode = false;
|
||||
private void MostrarLog(string message, uint id = 0)
|
||||
{
|
||||
|
|
@ -607,9 +655,9 @@ namespace AgroBase.Services
|
|||
}
|
||||
}
|
||||
|
||||
public void AdicionarMensagemNaFila(T_Code Dispositivo, byte[] _idTx, byte[] _idRx, byte funcCodeTx, byte funcCodeRx, byte[] payload = null, bool get = true, bool contabilizarTimeoutHealth = true, bool mensagemDescoberta = false)
|
||||
public bool AdicionarMensagemNaFila(T_Code Dispositivo, byte[] _idTx, byte[] _idRx, byte funcCodeTx, byte funcCodeRx, byte[] payload = null, bool get = true, bool contabilizarTimeoutHealth = true, bool mensagemDescoberta = false)
|
||||
{
|
||||
if (_PortaCAN?.IsOpen != true) return;
|
||||
if (_PortaCAN?.IsOpen != true) return false;
|
||||
|
||||
uint idTx = CanManager.ConverterIdCan(_idTx);
|
||||
uint idRx = CanManager.ConverterIdCan(_idRx);
|
||||
|
|
@ -653,12 +701,16 @@ namespace AgroBase.Services
|
|||
//{
|
||||
// Console.WriteLine($"{DateTime.Now.ToString("dd/MM/yyyy HH:mm:ss.fff")} - MENSAGEM LORA ADICIONADO NA FILA CAN, FUNC_CODE: {((CanMessagePosicaoDados)dados[0]).ToString()} ({((DadosLoRaParse)dados[0]).ToString()}) " + string.Join(" ", dados));
|
||||
//}
|
||||
|
||||
return true;
|
||||
}
|
||||
else
|
||||
{
|
||||
MostrarLog($"Mensagem já pendente e idêntica. ID: {novaMensagem.IdTx}, Data: {FuncoesGlobais.ConverterComandoBytesParaTexto(novaMensagem.DataTx)}", idTx);
|
||||
}
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
private CanMessage ObterProximaMensagem()
|
||||
|
|
|
|||
|
|
@ -120,6 +120,54 @@ namespace AgroBase.Services
|
|||
private long _rxTotalUltimaJanela = 0;
|
||||
private DateTime _ultimaJanelaFreq = DateTime.UtcNow;
|
||||
|
||||
public double IdadeMaxFilaNaoEnviadaMs
|
||||
{
|
||||
get
|
||||
{
|
||||
lock (_LockMensagens)
|
||||
{
|
||||
var pendentes = MensagensPendentes.Where(x => !x.Enviado).ToList();
|
||||
|
||||
if (!pendentes.Any())
|
||||
return 0;
|
||||
|
||||
DateTime agora = DateTime.Now;
|
||||
|
||||
return pendentes.Max(x => (agora - x.Momento).TotalMilliseconds);
|
||||
}
|
||||
}
|
||||
}
|
||||
public int PendentesNaoEnviadasAtrasadas
|
||||
{
|
||||
get
|
||||
{
|
||||
lock (_LockMensagens)
|
||||
{
|
||||
DateTime limite = DateTime.Now.AddMilliseconds(-500);
|
||||
|
||||
return MensagensPendentes.Count(x => !x.Enviado && x.Momento < limite);
|
||||
}
|
||||
}
|
||||
}
|
||||
public double IdadeMediaFilaNaoEnviadaMs
|
||||
{
|
||||
get
|
||||
{
|
||||
lock (_LockMensagens)
|
||||
{
|
||||
var pendentes = MensagensPendentes.Where(x => !x.Enviado).ToList();
|
||||
|
||||
if (!pendentes.Any())
|
||||
return 0;
|
||||
|
||||
DateTime agora = DateTime.Now;
|
||||
|
||||
return pendentes.Average(x => (agora - x.Momento).TotalMilliseconds);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
private static long NowUnixMs()
|
||||
{
|
||||
return DateTimeOffset.UtcNow.ToUnixTimeMilliseconds();
|
||||
|
|
@ -418,9 +466,9 @@ namespace AgroBase.Services
|
|||
}
|
||||
|
||||
|
||||
public void AdicionarMensagemNaFila(T_Code Dispositivo, byte[] _idTx, byte[] _idRx, byte funcCodeTx, byte funcCodeRx, byte[] payload = null, bool get = true, bool contabilizarTimeoutHealth = true, bool mensagemDescoberta = false)
|
||||
public bool AdicionarMensagemNaFila(T_Code Dispositivo, byte[] _idTx, byte[] _idRx, byte funcCodeTx, byte funcCodeRx, byte[] payload = null, bool get = true, bool contabilizarTimeoutHealth = true, bool mensagemDescoberta = false)
|
||||
{
|
||||
if (!Connector.IsOpen) return;
|
||||
if (!Connector.IsOpen) return false;
|
||||
|
||||
uint idTx = CanManager.ConverterIdCan(_idTx);
|
||||
uint idRx = CanManager.ConverterIdCan(_idRx);
|
||||
|
|
@ -467,12 +515,14 @@ namespace AgroBase.Services
|
|||
//{
|
||||
// Console.WriteLine($"{DateTime.Now.ToString("dd/MM/yyyy HH:mm:ss.fff")} - MENSAGEM LORA ADICIONADO NA FILA CAN, FUNC_CODE: {((CanMessagePosicaoDados)payload[1]).ToString()} ({((DadosLoRaParse)payload[1]).ToString()})" + string.Join(" ", payload));
|
||||
//}
|
||||
return true;
|
||||
}
|
||||
else
|
||||
{
|
||||
MostrarLog($"Mensagem já pendente e idêntica. ID: {novaMensagem.IdTx}, Data: {FuncoesGlobais.ConverterComandoBytesParaTexto(novaMensagem.DataTx)}", idTx);
|
||||
}
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
private CanMessage ObterProximaMensagem()
|
||||
|
|
|
|||
|
|
@ -65,9 +65,7 @@ namespace AgroBase.Services
|
|||
{
|
||||
lock (_LockMensagens)
|
||||
{
|
||||
return MensagensPendentes == null ||
|
||||
MensagensPendentes.Count == 0 ||
|
||||
!MensagensPendentes.Any(x => !x.Enviado && x.Momento >= DateTime.Now.AddSeconds(-10));
|
||||
return !MensagensPendentes.Any(x => !x.Enviado);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -196,6 +194,53 @@ namespace AgroBase.Services
|
|||
|
||||
private const int USR_FRAME_SIZE = 13;
|
||||
|
||||
public double IdadeMaxFilaNaoEnviadaMs
|
||||
{
|
||||
get
|
||||
{
|
||||
lock (_LockMensagens)
|
||||
{
|
||||
var pendentes = MensagensPendentes.Where(x => !x.Enviado).ToList();
|
||||
|
||||
if (!pendentes.Any())
|
||||
return 0;
|
||||
|
||||
DateTime agora = DateTime.Now;
|
||||
|
||||
return pendentes.Max(x => (agora - x.Momento).TotalMilliseconds);
|
||||
}
|
||||
}
|
||||
}
|
||||
public int PendentesNaoEnviadasAtrasadas
|
||||
{
|
||||
get
|
||||
{
|
||||
lock (_LockMensagens)
|
||||
{
|
||||
DateTime limite = DateTime.Now.AddMilliseconds(-500);
|
||||
|
||||
return MensagensPendentes.Count(x => !x.Enviado && x.Momento < limite);
|
||||
}
|
||||
}
|
||||
}
|
||||
public double IdadeMediaFilaNaoEnviadaMs
|
||||
{
|
||||
get
|
||||
{
|
||||
lock (_LockMensagens)
|
||||
{
|
||||
var pendentes = MensagensPendentes.Where(x => !x.Enviado).ToList();
|
||||
|
||||
if (!pendentes.Any())
|
||||
return 0;
|
||||
|
||||
DateTime agora = DateTime.Now;
|
||||
|
||||
return pendentes.Average(x => (agora - x.Momento).TotalMilliseconds);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
private void MostrarLog(string message, uint id = 0)
|
||||
{
|
||||
if (DebugMode)
|
||||
|
|
@ -337,9 +382,10 @@ namespace AgroBase.Services
|
|||
}
|
||||
}
|
||||
|
||||
public void AdicionarMensagemNaFila(T_Code Dispositivo, byte[] _idTx, byte[] _idRx, byte funcCodeTx, byte funcCodeRx, byte[] payload = null, bool get = true, bool contabilizarTimeoutHealth = true, bool mensagemDescoberta = false)
|
||||
public bool AdicionarMensagemNaFila(T_Code Dispositivo, byte[] _idTx, byte[] _idRx, byte funcCodeTx, byte funcCodeRx, byte[] payload = null, bool get = true, bool contabilizarTimeoutHealth = true, bool mensagemDescoberta = false)
|
||||
{
|
||||
if (!IsConnected) return;
|
||||
if (!IsConnected)
|
||||
return false;
|
||||
|
||||
uint idTx = CanManager.ConverterIdCan(_idTx);
|
||||
uint idRx = CanManager.ConverterIdCan(_idRx);
|
||||
|
|
@ -355,21 +401,19 @@ namespace AgroBase.Services
|
|||
if (Dispositivo == T_Code.Mks)
|
||||
{
|
||||
byte crc = CanManager.CalcularChecksum(idTx, dados.ToArray());
|
||||
|
||||
dados.Add(crc);
|
||||
}
|
||||
|
||||
if (dados.Count > 8)
|
||||
throw new InvalidOperationException($"Payload CAN maior que 8 bytes. ID=0x{idTx:X}, Len={dados.Count}");
|
||||
|
||||
string chaveCorrelacao =
|
||||
CanManager.GerarChaveCorrelacao(
|
||||
Dispositivo,
|
||||
idTx,
|
||||
idRx,
|
||||
funcCodeTx,
|
||||
funcCodeRx,
|
||||
dados.ToArray()
|
||||
{
|
||||
throw new InvalidOperationException(
|
||||
$"Payload CAN maior que 8 bytes. " +
|
||||
$"ID=0x{idTx:X}, Len={dados.Count}"
|
||||
);
|
||||
}
|
||||
|
||||
string chaveCorrelacao = CanManager.GerarChaveCorrelacao(Dispositivo, idTx, idRx, funcCodeTx, funcCodeRx, dados.ToArray());
|
||||
|
||||
var novaMensagem = new CanMessage
|
||||
{
|
||||
|
|
@ -390,13 +434,6 @@ namespace AgroBase.Services
|
|||
{
|
||||
DateTime agora = DateTime.Now;
|
||||
|
||||
/*
|
||||
* O OID não deve possuir mais de uma consulta aguardando
|
||||
* resposta para o mesmo driver.
|
||||
*
|
||||
* Diferente do dedupe por ChaveInterna, esta proteção vale
|
||||
* mesmo quando o parâmetro consultado é diferente.
|
||||
*/
|
||||
if (get && Dispositivo == T_Code.Oid)
|
||||
{
|
||||
bool consultaOidAberta =
|
||||
|
|
@ -407,14 +444,9 @@ namespace AgroBase.Services
|
|||
x.IdTx == idTx);
|
||||
|
||||
if (consultaOidAberta)
|
||||
return;
|
||||
return false;
|
||||
}
|
||||
|
||||
/*
|
||||
* Para request com resposta, não deixa duas pendências iguais
|
||||
* ficarem esperando a mesma resposta. Isso é vital para OID, porque
|
||||
* todas as consultas respondem com funcCode 0x0F e diferem pelo param.
|
||||
*/
|
||||
bool mesmaPendenciaAberta =
|
||||
MensagensPendentes.Any(x =>
|
||||
x.ComResposta &&
|
||||
|
|
@ -422,42 +454,37 @@ namespace AgroBase.Services
|
|||
x.ChaveInterna == chaveCorrelacao);
|
||||
|
||||
if (get && mesmaPendenciaAberta)
|
||||
return;
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
/*
|
||||
* Para comando sem resposta, evita empilhar comandos idênticos
|
||||
* ainda não enviados. Depois de enviado, ele será limpo normalmente.
|
||||
*/
|
||||
bool comandoIdenticoNaoEnviado =
|
||||
MensagensPendentes.Any(x =>
|
||||
!x.ComResposta &&
|
||||
!x.Enviado &&
|
||||
x.IdTx == idTx &&
|
||||
x.DataTx != null &&
|
||||
x.DataTx.SequenceEqual(dados.ToArray()));
|
||||
x.DataTx.SequenceEqual(
|
||||
dados.ToArray()
|
||||
));
|
||||
|
||||
if (!get && comandoIdenticoNaoEnviado)
|
||||
return;
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
/*
|
||||
* Dedupe temporal extra, útil para comandos de controle que chegam
|
||||
* repetidos por loop rápido.
|
||||
*/
|
||||
TimeSpan janela = get ? DEDUPE_JANELA_GET : DEDUPE_JANELA_SET;
|
||||
|
||||
if (_ultimaFilaPorChave.TryGetValue(chaveCorrelacao, out DateTime ultima) && agora - ultima < janela)
|
||||
{
|
||||
return;
|
||||
return false;
|
||||
}
|
||||
|
||||
_ultimaFilaPorChave[chaveCorrelacao] = agora;
|
||||
|
||||
MensagensPendentes.Add(novaMensagem);
|
||||
|
||||
MostrarLog(
|
||||
$"Mensagem adicionada. ID=0x{idTx:X}, Data={BitConverter.ToString(novaMensagem.DataTx)}",
|
||||
idTx
|
||||
);
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -696,6 +696,9 @@ namespace AgroBase.Services
|
|||
{
|
||||
try
|
||||
{
|
||||
if (!CanManager.CanService.IsConnected)
|
||||
return;
|
||||
|
||||
var modulos = Variaveis.OperacaoEmAndamento.DispMvd?.Dados?.Modulos;
|
||||
if (modulos == null)
|
||||
return;
|
||||
|
|
@ -862,7 +865,8 @@ namespace AgroBase.Services
|
|||
double angulo,
|
||||
int velocidade,
|
||||
Sentido sentido = Sentido.Horario,
|
||||
ModosControle modo = ModosControle.AnguloAbsoluto)
|
||||
ModosControle modo = ModosControle.AnguloAbsoluto,
|
||||
int aceleracao = 0)
|
||||
{
|
||||
byte funcCode;
|
||||
double reducao = Variaveis.OperacaoEmAndamento?.DispMvd?.Dados?.Modulos?.FirstOrDefault(x => x.DirMotor._EnderecoCAN_Tx == idTx)?.DirMotor?.Reducao ?? VariaveisEquipamento.ReducaoDirecional;
|
||||
|
|
@ -873,6 +877,7 @@ namespace AgroBase.Services
|
|||
sentido,
|
||||
modo,
|
||||
reducao,
|
||||
aceleracao,
|
||||
out funcCode
|
||||
);
|
||||
|
||||
|
|
@ -956,6 +961,7 @@ namespace AgroBase.Services
|
|||
Sentido sentido,
|
||||
ModosControle modo,
|
||||
double reducao,
|
||||
int aceleracao,
|
||||
out byte funcCode)
|
||||
{
|
||||
List<byte> payload = new List<byte>();
|
||||
|
|
@ -963,7 +969,7 @@ namespace AgroBase.Services
|
|||
ushort vel = (ushort)Math.Max(0, Math.Min(velocidade, ushort.MaxValue));
|
||||
byte[] velocidadeBytes = BitConverter.GetBytes(vel);
|
||||
|
||||
byte acc = 0;
|
||||
byte acc = (byte)aceleracao;
|
||||
|
||||
if (modo == ModosControle.Pulsos)
|
||||
{
|
||||
|
|
|
|||
|
|
@ -22,8 +22,7 @@ namespace AgroBase.Services
|
|||
private static readonly object _sync = new object();
|
||||
private static readonly HashSet<byte> _handlersRegistrados = new HashSet<byte>();
|
||||
private static readonly Dictionary<byte, DateTime> _ultimoEnvioConsultaPorDriver = new Dictionary<byte, DateTime>();
|
||||
private static readonly Dictionary<string, DateTime> _ultimoEnvioParametro = new Dictionary<string, DateTime>();
|
||||
private static readonly Dictionary<byte, OidParametros> _ultimaConsultaPendente = new Dictionary<byte, OidParametros>();
|
||||
private static readonly Dictionary<string, DateTime> _ultimaConsultaParametro = new Dictionary<string, DateTime>();
|
||||
private static int _cursorDriverPolling = 0;
|
||||
private static readonly Dictionary<string, string> _ultimoComandoControleAssinatura = new Dictionary<string, string>();
|
||||
private static readonly Dictionary<string, DateTime> _ultimoComandoControleQuando = new Dictionary<string, DateTime>();
|
||||
|
|
@ -35,11 +34,12 @@ namespace AgroBase.Services
|
|||
*/
|
||||
private static readonly TimeSpan IntervaloMinimoConsultaPorDriver = TimeSpan.FromMilliseconds(40);
|
||||
private static readonly TimeSpan IntervaloMinimoMesmoParametro = TimeSpan.FromMilliseconds(300);
|
||||
private static readonly TimeSpan JanelaConsultaPendente = TimeSpan.FromMilliseconds(1200);
|
||||
private static readonly TimeSpan JanelaDedupeComandoControle = TimeSpan.FromMilliseconds(80);
|
||||
private static readonly TimeSpan TempoRespostaRecente = TimeSpan.FromSeconds(3);
|
||||
private static readonly TimeSpan TempoRemoverDriver = TimeSpan.FromSeconds(15);
|
||||
private static readonly TimeSpan TempoRemoverDriver = TimeSpan.FromSeconds(5);
|
||||
private static readonly TimeSpan PeriodoHeartbeat = TimeSpan.FromMilliseconds(500);
|
||||
private static readonly TimeSpan IntervaloTentativaReconexao = TimeSpan.FromSeconds(1);
|
||||
private static readonly Dictionary<byte, DateTime> _ultimaTentativaReconexaoPorDriver = new Dictionary<byte, DateTime>();
|
||||
|
||||
/*
|
||||
* Polling recomendado. Se outro ponto do sistema ainda chamar
|
||||
|
|
@ -314,8 +314,8 @@ namespace AgroBase.Services
|
|||
));
|
||||
|
||||
campos.Add((
|
||||
$"{enderecoMov}.polling.timeout_pendente_ms",
|
||||
JanelaConsultaPendente.TotalMilliseconds
|
||||
$"{enderecoMov}.polling.timeout_resposta_gerenciado_por",
|
||||
"CanService"
|
||||
));
|
||||
|
||||
campos.Add((
|
||||
|
|
@ -499,6 +499,65 @@ namespace AgroBase.Services
|
|||
tmrHeartbeat.Start();
|
||||
}
|
||||
|
||||
private static void GarantirMapeamentoDriver(MovimentacaoModel motor)
|
||||
{
|
||||
if (motor == null)
|
||||
return;
|
||||
|
||||
string modId = motor.Mod_ID;
|
||||
|
||||
if (string.IsNullOrWhiteSpace(modId))
|
||||
return;
|
||||
|
||||
if (SerialService.DispositivosMapeados.Any(
|
||||
x =>
|
||||
x.Dispositivo == T_Code.Mov &&
|
||||
x.Mod_ID == modId))
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
SerialService.DispositivosMapeados.Add(
|
||||
new DispositivoDetalhesModel
|
||||
{
|
||||
Dispositivo = T_Code.Mov,
|
||||
Endereco = CanManager.CanService._portName,
|
||||
Versao = "1.0",
|
||||
Mod_ID = modId,
|
||||
}
|
||||
);
|
||||
|
||||
var op = Variaveis.OperacaoEmAndamento;
|
||||
|
||||
if (op.Sensoriamento?.Operacao?.ModsCalibragem == null)
|
||||
op.Sensoriamento.Operacao.ModsCalibragem = new Dictionary<T_Code, Dictionary<string, bool>>();
|
||||
|
||||
if (!op.Sensoriamento.Operacao.ModsCalibragem.ContainsKey(T_Code.Mov))
|
||||
{
|
||||
op.Sensoriamento.Operacao.ModsCalibragem.Add(
|
||||
T_Code.Mov,
|
||||
new Dictionary<string, bool>()
|
||||
);
|
||||
}
|
||||
|
||||
op.Sensoriamento.Operacao
|
||||
.ModsCalibragem[T_Code.Mov][modId] = false;
|
||||
|
||||
op.Sensoriamento.InserirLog(
|
||||
T_Code.Mov,
|
||||
StatusModulo.Conectado,
|
||||
0,
|
||||
"Dispositivo conectado",
|
||||
modId
|
||||
);
|
||||
|
||||
Variaveis.MostrarLog(
|
||||
$"[OIDCanService] [CONEXAO] " +
|
||||
$"MOV {modId} mapeado | " +
|
||||
$"CAN=0x{motor._EnderecoCAN_Tx:X2}"
|
||||
);
|
||||
}
|
||||
|
||||
private static void RegistrarHandler(byte idRx)
|
||||
{
|
||||
lock (_sync)
|
||||
|
|
@ -542,61 +601,44 @@ namespace AgroBase.Services
|
|||
);
|
||||
}
|
||||
|
||||
private static void EnviarComando(
|
||||
T_Code dispositivo,
|
||||
byte idTx,
|
||||
byte idRx,
|
||||
byte funcCode,
|
||||
byte[] payload = null,
|
||||
bool get = true,
|
||||
bool contabilizarTimeoutHealth = true)
|
||||
private static bool EnviarComando(T_Code dispositivo, byte idTx, byte idRx, byte funcCode, byte[] payload = null, bool get = true, bool contabilizarTimeoutHealth = true)
|
||||
{
|
||||
RegistrarHandlersMotor(idTx, idRx);
|
||||
|
||||
CanManager.CanService.AdicionarMensagemNaFila(
|
||||
dispositivo,
|
||||
new byte[] { idTx },
|
||||
new byte[] { idRx },
|
||||
funcCode,
|
||||
funcCode,
|
||||
payload ?? new byte[0],
|
||||
get,
|
||||
contabilizarTimeoutHealth
|
||||
);
|
||||
return CanManager.CanService.AdicionarMensagemNaFila(
|
||||
dispositivo,
|
||||
new byte[] { idTx },
|
||||
new byte[] { idRx },
|
||||
funcCode,
|
||||
funcCode,
|
||||
payload ?? new byte[0],
|
||||
get,
|
||||
contabilizarTimeoutHealth
|
||||
);
|
||||
}
|
||||
|
||||
public static void RequisitarDado(
|
||||
byte idTx,
|
||||
byte idRx,
|
||||
OidParametros dado)
|
||||
public static void RequisitarDado(byte idTx, byte idRx, OidParametros dado)
|
||||
{
|
||||
RequisitarDadoInterno(
|
||||
idTx,
|
||||
idRx,
|
||||
dado,
|
||||
forcar: false,
|
||||
contabilizarTimeoutHealth:
|
||||
DeveContabilizarTimeoutHealth(dado)
|
||||
contabilizarTimeoutHealth: DeveContabilizarTimeoutHealth(dado)
|
||||
);
|
||||
}
|
||||
|
||||
private static bool RequisitarDadoInterno(
|
||||
byte idTx,
|
||||
byte idRx,
|
||||
OidParametros dado,
|
||||
bool forcar,
|
||||
bool contabilizarTimeoutHealth)
|
||||
private static bool RequisitarDadoInterno(byte idTx, byte idRx, OidParametros dado, bool forcar, bool contabilizarTimeoutHealth)
|
||||
{
|
||||
DateTime agora =
|
||||
DateTime.Now;
|
||||
DateTime agora = DateTime.Now;
|
||||
|
||||
string chaveParametro =
|
||||
idTx.ToString("X2") +
|
||||
":" +
|
||||
((byte)dado).ToString("X2");
|
||||
|
||||
lock (_sync)
|
||||
{
|
||||
string chaveParametro =
|
||||
idTx.ToString("X2") +
|
||||
":" +
|
||||
((byte)dado).ToString("X2");
|
||||
|
||||
if (!forcar)
|
||||
{
|
||||
if (_ultimoEnvioConsultaPorDriver.TryGetValue(idTx, out DateTime ultimoDriver) && agora - ultimoDriver < IntervaloMinimoConsultaPorDriver)
|
||||
|
|
@ -604,80 +646,59 @@ namespace AgroBase.Services
|
|||
return false;
|
||||
}
|
||||
|
||||
if (_ultimaConsultaPendente.TryGetValue(idTx, out OidParametros pendente))
|
||||
{
|
||||
bool possuiMomentoEnvio = _ultimoEnvioConsultaPorDriver.TryGetValue(idTx, out DateTime ultimoPendente);
|
||||
|
||||
bool aindaDentroDaJanela =
|
||||
possuiMomentoEnvio &&
|
||||
agora - ultimoPendente < JanelaConsultaPendente;
|
||||
|
||||
if (aindaDentroDaJanela)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
/*
|
||||
* A consulta anterior venceu. Libera o driver para uma nova
|
||||
* tentativa sem manter pendência órfã.
|
||||
*/
|
||||
_ultimaConsultaPendente.Remove(idTx);
|
||||
|
||||
Variaveis.MostrarLog(
|
||||
"[OID] Consulta pendente expirada: " +
|
||||
"driver=" + idTx.ToString("X2") +
|
||||
", parametro=" + pendente
|
||||
);
|
||||
}
|
||||
|
||||
TimeSpan intervalo =
|
||||
IntervaloParametro.TryGetValue(
|
||||
dado,
|
||||
out TimeSpan valor
|
||||
)
|
||||
out TimeSpan valor)
|
||||
? valor
|
||||
: IntervaloMinimoMesmoParametro;
|
||||
|
||||
if (_ultimoEnvioParametro
|
||||
.TryGetValue(chaveParametro, out DateTime ultimoParam) &&
|
||||
agora - ultimoParam < intervalo)
|
||||
if (_ultimaConsultaParametro.TryGetValue(chaveParametro, out DateTime ultimoParam) && agora - ultimoParam < intervalo)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
_ultimoEnvioConsultaPorDriver[idTx] = agora;
|
||||
_ultimoEnvioParametro[chaveParametro] = agora;
|
||||
_ultimaConsultaPendente[idTx] = dado;
|
||||
}
|
||||
|
||||
//Console.WriteLine(
|
||||
// "[OID/TX] idTx=" +
|
||||
// idTx.ToString("X2") +
|
||||
// " idRx=" +
|
||||
// idRx.ToString("X2") +
|
||||
// " param=" +
|
||||
// dado +
|
||||
// " data=0F-" +
|
||||
// ((byte)dado).ToString("X2")
|
||||
//);
|
||||
|
||||
EnviarComando(
|
||||
/*
|
||||
* A partir daqui quem decide se existe espaço
|
||||
* para esta transação é o CanService.
|
||||
*/
|
||||
bool aceitoNaFila = EnviarComando(
|
||||
Dispositivo,
|
||||
idTx,
|
||||
idRx,
|
||||
(byte)OidFuncCode.Consulta,
|
||||
new byte[] { (byte)dado },
|
||||
new byte[]
|
||||
{
|
||||
(byte)dado
|
||||
},
|
||||
get: true,
|
||||
contabilizarTimeoutHealth:
|
||||
contabilizarTimeoutHealth
|
||||
contabilizarTimeoutHealth: contabilizarTimeoutHealth
|
||||
);
|
||||
|
||||
if (!aceitoNaFila)
|
||||
return false;
|
||||
|
||||
/*
|
||||
* IMPORTANTE:
|
||||
* isso significa "aceito pela fila",
|
||||
* não "fisicamente transmitido".
|
||||
*
|
||||
* Timeout de resposta não é contado aqui.
|
||||
*/
|
||||
DateTime momentoAceite = DateTime.Now;
|
||||
|
||||
lock (_sync)
|
||||
{
|
||||
_ultimoEnvioConsultaPorDriver[idTx] = momentoAceite;
|
||||
_ultimaConsultaParametro[chaveParametro] = momentoAceite;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
private static bool DeveContabilizarTimeoutHealth(
|
||||
OidParametros dado)
|
||||
private static bool DeveContabilizarTimeoutHealth(OidParametros dado)
|
||||
{
|
||||
/*
|
||||
* Saúde CAN deve refletir comandos/leituras críticas.
|
||||
|
|
@ -709,63 +730,132 @@ namespace AgroBase.Services
|
|||
{
|
||||
try
|
||||
{
|
||||
var modulos =
|
||||
Variaveis
|
||||
.OperacaoEmAndamento
|
||||
.DispMvd?
|
||||
.Dados?
|
||||
.Modulos?
|
||||
.Where(x =>
|
||||
x?.MovMotor != null &&
|
||||
x.MovMotor.Inicializado
|
||||
)
|
||||
.ToList();
|
||||
if (!CanManager.CanService.IsConnected)
|
||||
return;
|
||||
|
||||
var modulos = Variaveis.OperacaoEmAndamento.DispMvd?.Dados?.Modulos?.Where(x => x?.MovMotor != null).ToList();
|
||||
if (modulos == null || modulos.Count == 0)
|
||||
{
|
||||
RemoverDriversMuitoAntigos();
|
||||
return;
|
||||
}
|
||||
|
||||
/*
|
||||
* Existem OIDs configurados nesta operação e o CAN está ativo.
|
||||
* O heartbeat deve existir independentemente de termos encontrado
|
||||
* algum driver no discovery inicial.
|
||||
*/
|
||||
GarantirTimerHeartbeat();
|
||||
|
||||
/*
|
||||
* 1. Primeiro atualiza lifecycle.
|
||||
*/
|
||||
MarcarDriversInativosPorTimeout();
|
||||
|
||||
/*
|
||||
* 2. Offline recebe tentativa de reconexão.
|
||||
*/
|
||||
foreach (var modulo in modulos)
|
||||
{
|
||||
var motor =
|
||||
modulo.MovMotor;
|
||||
|
||||
if (motor.Inicializado)
|
||||
continue;
|
||||
|
||||
TentarReconectarDriver(
|
||||
motor._EnderecoCAN_Tx,
|
||||
motor._EnderecoCAN_Rx
|
||||
);
|
||||
}
|
||||
|
||||
/*
|
||||
* 3. Drivers online que acabaram de voltar
|
||||
* precisam recuperar configuração.
|
||||
*/
|
||||
foreach (var modulo in modulos)
|
||||
{
|
||||
var motor =
|
||||
modulo.MovMotor;
|
||||
|
||||
if (!motor.Inicializado)
|
||||
continue;
|
||||
|
||||
GarantirMapeamentoDriver(motor);
|
||||
|
||||
var leitura =
|
||||
DadosLeitura.FirstOrDefault(
|
||||
x => x.EnderecoTx ==
|
||||
motor._EnderecoCAN_Tx
|
||||
);
|
||||
|
||||
if (leitura != null &&
|
||||
!leitura.Configurado)
|
||||
{
|
||||
ConfigurarDriver(motor);
|
||||
|
||||
Variaveis.MostrarLog(
|
||||
$"[OIDCanService] [CONFIG] " +
|
||||
$"Driver CAN 0x{motor._EnderecoCAN_Tx:X2} " +
|
||||
$"reconfigurado."
|
||||
);
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
* 4. Polling global dos online.
|
||||
*/
|
||||
var online =
|
||||
modulos
|
||||
.Where(x =>
|
||||
x.MovMotor.Inicializado)
|
||||
.ToList();
|
||||
|
||||
if (online.Count == 0)
|
||||
return;
|
||||
|
||||
int inicio;
|
||||
|
||||
lock (_sync)
|
||||
{
|
||||
if (_cursorDriverPolling < 0 ||
|
||||
_cursorDriverPolling >= modulos.Count)
|
||||
_cursorDriverPolling >= online.Count)
|
||||
{
|
||||
_cursorDriverPolling = 0;
|
||||
}
|
||||
|
||||
inicio = _cursorDriverPolling;
|
||||
inicio =
|
||||
_cursorDriverPolling;
|
||||
}
|
||||
|
||||
for (int deslocamento = 0;
|
||||
deslocamento < modulos.Count;
|
||||
deslocamento < online.Count;
|
||||
deslocamento++)
|
||||
{
|
||||
int index =
|
||||
(inicio + deslocamento) % modulos.Count;
|
||||
(inicio + deslocamento) %
|
||||
online.Count;
|
||||
|
||||
var motor = modulos[index].MovMotor;
|
||||
|
||||
byte idTx = motor._EnderecoCAN_Tx;
|
||||
byte idRx = motor._EnderecoCAN_Rx;
|
||||
var motor =
|
||||
online[index].MovMotor;
|
||||
|
||||
OidParametros? proximo =
|
||||
EscolherProximoParametro(idTx);
|
||||
EscolherProximoParametro(
|
||||
motor._EnderecoCAN_Tx
|
||||
);
|
||||
|
||||
if (!proximo.HasValue)
|
||||
continue;
|
||||
|
||||
bool enviado = RequisitarDadoInterno(
|
||||
idTx,
|
||||
idRx,
|
||||
proximo.Value,
|
||||
forcar: false,
|
||||
contabilizarTimeoutHealth:
|
||||
DeveContabilizarTimeoutHealth(proximo.Value)
|
||||
);
|
||||
bool enviado =
|
||||
RequisitarDadoInterno(
|
||||
motor._EnderecoCAN_Tx,
|
||||
motor._EnderecoCAN_Rx,
|
||||
proximo.Value,
|
||||
forcar: false,
|
||||
contabilizarTimeoutHealth:
|
||||
DeveContabilizarTimeoutHealth(
|
||||
proximo.Value)
|
||||
);
|
||||
|
||||
if (!enviado)
|
||||
continue;
|
||||
|
|
@ -773,22 +863,20 @@ namespace AgroBase.Services
|
|||
lock (_sync)
|
||||
{
|
||||
_cursorDriverPolling =
|
||||
(index + 1) % modulos.Count;
|
||||
(index + 1) %
|
||||
online.Count;
|
||||
}
|
||||
|
||||
// Uma consulta nova por tick.
|
||||
/*
|
||||
* Uma consulta nova global por tick.
|
||||
*/
|
||||
break;
|
||||
}
|
||||
|
||||
/*
|
||||
* Mesmo quando ninguém estava apto a enviar, fazemos a limpeza.
|
||||
*/
|
||||
RemoverDriversMuitoAntigos();
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
Console.WriteLine(
|
||||
"[OID] Erro em AtualizarDados: " +
|
||||
Variaveis.MostrarLog(
|
||||
"[OIDCanService] Erro em AtualizarDados: " +
|
||||
ex.Message
|
||||
);
|
||||
}
|
||||
|
|
@ -811,22 +899,17 @@ namespace AgroBase.Services
|
|||
((byte)parametro).ToString("X2");
|
||||
|
||||
TimeSpan intervalo =
|
||||
IntervaloParametro.TryGetValue(
|
||||
parametro,
|
||||
out TimeSpan configurado
|
||||
)
|
||||
IntervaloParametro.TryGetValue(parametro, out TimeSpan configurado)
|
||||
? configurado
|
||||
: IntervaloMinimoMesmoParametro;
|
||||
|
||||
if (!_ultimoEnvioParametro.TryGetValue(
|
||||
chave,
|
||||
out DateTime ultimoEnvio))
|
||||
if (!_ultimaConsultaParametro.TryGetValue(chave, out DateTime ultimaConsulta))
|
||||
{
|
||||
// Parâmetro nunca consultado tem prioridade máxima.
|
||||
return parametro;
|
||||
}
|
||||
|
||||
TimeSpan decorrido = agora - ultimoEnvio;
|
||||
TimeSpan decorrido = agora - ultimaConsulta;
|
||||
|
||||
if (decorrido < intervalo)
|
||||
continue;
|
||||
|
|
@ -853,10 +936,9 @@ namespace AgroBase.Services
|
|||
}
|
||||
}
|
||||
|
||||
private static void RemoverDriversMuitoAntigos()
|
||||
private static void MarcarDriversInativosPorTimeout()
|
||||
{
|
||||
DateTime agora =
|
||||
DateTime.Now;
|
||||
DateTime agora = DateTime.Now;
|
||||
|
||||
List<OIDModel> antigos;
|
||||
|
||||
|
|
@ -868,19 +950,69 @@ namespace AgroBase.Services
|
|||
x.Iniciado &&
|
||||
x.UltimoComandoRecebido != DateTime.MinValue &&
|
||||
agora - x.UltimoComandoRecebido >
|
||||
TempoRemoverDriver)
|
||||
TempoRemoverDriver)
|
||||
.ToList();
|
||||
}
|
||||
|
||||
foreach (var antigo in antigos)
|
||||
{
|
||||
antigo.Iniciado = false;
|
||||
|
||||
/*
|
||||
* Importantíssimo:
|
||||
* não sabemos se houve apenas perda CAN
|
||||
* ou reboot/power-cycle do OID.
|
||||
*/
|
||||
antigo.Configurado = false;
|
||||
|
||||
lock (_sync)
|
||||
{
|
||||
_ultimoEnvioConsultaPorDriver.Remove(
|
||||
antigo.EnderecoTx
|
||||
);
|
||||
|
||||
string prefixo =
|
||||
antigo.EnderecoTx.ToString("X2") +
|
||||
":";
|
||||
|
||||
var chaves =
|
||||
_ultimaConsultaParametro.Keys
|
||||
.Where(x =>
|
||||
x.StartsWith(prefixo))
|
||||
.ToList();
|
||||
|
||||
foreach (var chave in chaves)
|
||||
_ultimaConsultaParametro.Remove(chave);
|
||||
}
|
||||
|
||||
Variaveis.MostrarLog(
|
||||
$"[OIDCanService] [CONEXAO] " +
|
||||
$"Driver CAN 0x{antigo.EnderecoTx:X2} OFFLINE | " +
|
||||
$"sem resposta há " +
|
||||
$"{(agora - antigo.UltimoComandoRecebido).TotalSeconds:F1}s"
|
||||
);
|
||||
}
|
||||
}
|
||||
|
||||
public static void ComandoControle(
|
||||
byte idTx,
|
||||
byte idRx,
|
||||
OidFuncCode funcCode,
|
||||
double setPoint)
|
||||
private static void TentarReconectarDriver(byte idTx, byte idRx)
|
||||
{
|
||||
DateTime agora = DateTime.Now;
|
||||
|
||||
lock (_sync)
|
||||
{
|
||||
if (_ultimaTentativaReconexaoPorDriver.TryGetValue(idTx, out DateTime ultima) && agora - ultima < IntervaloTentativaReconexao)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
_ultimaTentativaReconexaoPorDriver[idTx] = agora;
|
||||
}
|
||||
|
||||
RegistrarHandlersMotor(idTx, idRx);
|
||||
EnviarComandoDescoberta(idTx, idRx);
|
||||
}
|
||||
|
||||
public static void ComandoControle(byte idTx, byte idRx, OidFuncCode funcCode, double setPoint)
|
||||
{
|
||||
byte[] setPointBytes =
|
||||
MontarPayloadControle(
|
||||
|
|
@ -914,10 +1046,7 @@ namespace AgroBase.Services
|
|||
);
|
||||
}
|
||||
|
||||
private static bool DeveDroparComandoDuplicado(
|
||||
byte idTx,
|
||||
OidFuncCode funcCode,
|
||||
byte[] payload)
|
||||
private static bool DeveDroparComandoDuplicado(byte idTx, OidFuncCode funcCode, byte[] payload)
|
||||
{
|
||||
if (funcCode == OidFuncCode.Heartbeat)
|
||||
return false;
|
||||
|
|
@ -955,10 +1084,7 @@ namespace AgroBase.Services
|
|||
}
|
||||
}
|
||||
|
||||
private static byte[] MontarPayloadControle(
|
||||
byte idTx,
|
||||
OidFuncCode funcCode,
|
||||
double setPoint)
|
||||
private static byte[] MontarPayloadControle(byte idTx, OidFuncCode funcCode, double setPoint)
|
||||
{
|
||||
OIDModel item =
|
||||
DadosLeitura.FirstOrDefault(
|
||||
|
|
@ -1057,8 +1183,8 @@ namespace AgroBase.Services
|
|||
}
|
||||
|
||||
default:
|
||||
Console.WriteLine(
|
||||
"[OID] Comando não mapeado: " +
|
||||
Variaveis.MostrarLog(
|
||||
"[OIDCanService] Comando não mapeado: " +
|
||||
funcCode
|
||||
);
|
||||
return null;
|
||||
|
|
@ -1400,18 +1526,14 @@ namespace AgroBase.Services
|
|||
{
|
||||
try
|
||||
{
|
||||
if (mensagem == null ||
|
||||
mensagem.DataRx == null ||
|
||||
mensagem.DataRx.Length < 2)
|
||||
if (mensagem == null || mensagem.DataRx == null || mensagem.DataRx.Length < 2)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
OidFuncCode func =
|
||||
(OidFuncCode)mensagem.DataRx[0];
|
||||
OidFuncCode func = (OidFuncCode)mensagem.DataRx[0];
|
||||
|
||||
if (func != OidFuncCode.Consulta &&
|
||||
func != OidFuncCode.ConsultaStatusRetornoZero)
|
||||
if (func != OidFuncCode.Consulta && func != OidFuncCode.ConsultaStatusRetornoZero)
|
||||
{
|
||||
/*
|
||||
* Controle não deveria responder. Se responder algo
|
||||
|
|
@ -1434,7 +1556,7 @@ namespace AgroBase.Services
|
|||
return;
|
||||
}
|
||||
|
||||
//Console.WriteLine(
|
||||
//Variaveis.MostrarLog(
|
||||
// "[OID/RX] idTx=" +
|
||||
// mensagem.IdTx.ToString("X") +
|
||||
// " idRx=" +
|
||||
|
|
@ -1443,120 +1565,135 @@ namespace AgroBase.Services
|
|||
// BitConverter.ToString(mensagem.DataRx ?? new byte[0])
|
||||
//);
|
||||
|
||||
OidParametros parametro =
|
||||
(OidParametros)mensagem.DataRx[1];
|
||||
OidParametros parametro = (OidParametros)mensagem.DataRx[1];
|
||||
|
||||
ProcessarParametro(
|
||||
item,
|
||||
parametro,
|
||||
mensagem.DataRx
|
||||
);
|
||||
bool processado = ProcessarParametro(item, parametro, mensagem.DataRx);
|
||||
|
||||
if (!processado)
|
||||
{
|
||||
Variaveis.MostrarLog(
|
||||
$"[OIDCanService] [RX] " +
|
||||
$"Resposta inválida ignorada | " +
|
||||
$"driver=0x{item.EnderecoTx:X2} | " +
|
||||
$"parametro={parametro}"
|
||||
);
|
||||
|
||||
return;
|
||||
}
|
||||
|
||||
bool jaFoiVisto = item.UltimoComandoRecebido != DateTime.MinValue;
|
||||
bool estavaOffline = jaFoiVisto && !item.Iniciado;
|
||||
|
||||
item.UltimoComandoRecebido = DateTime.Now;
|
||||
item.Iniciado = true;
|
||||
|
||||
DateTime agoraRx = DateTime.Now;
|
||||
string chaveParametro = item.EnderecoTx.ToString("X2") + ":" + ((byte)parametro).ToString("X2");
|
||||
lock (_sync)
|
||||
{
|
||||
RemoverPendenteSeCorresponder(
|
||||
item.EnderecoTx,
|
||||
parametro
|
||||
);
|
||||
_ultimaConsultaParametro[chaveParametro] = agoraRx;
|
||||
}
|
||||
|
||||
if (item.EnderecoRx != item.EnderecoTx)
|
||||
{
|
||||
RemoverPendenteSeCorresponder(
|
||||
item.EnderecoRx,
|
||||
parametro
|
||||
);
|
||||
}
|
||||
if (estavaOffline)
|
||||
{
|
||||
Variaveis.MostrarLog(
|
||||
$"[OIDCanService] [CONEXAO] " +
|
||||
$"Driver CAN 0x{item.EnderecoTx:X2} ONLINE novamente"
|
||||
);
|
||||
}
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
Console.WriteLine(
|
||||
"[OID] Erro ao interpretar resposta CAN: " +
|
||||
Variaveis.MostrarLog(
|
||||
"[OIDCanService] Erro ao interpretar resposta CAN: " +
|
||||
ex.Message
|
||||
);
|
||||
}
|
||||
}
|
||||
|
||||
private static void RemoverPendenteSeCorresponder(byte endereco, OidParametros parametroRecebido)
|
||||
{
|
||||
if (_ultimaConsultaPendente.TryGetValue(endereco, out OidParametros pendente) && pendente == parametroRecebido)
|
||||
{
|
||||
_ultimaConsultaPendente.Remove(endereco);
|
||||
}
|
||||
}
|
||||
|
||||
private void ProcessarParametro(
|
||||
OIDModel item,
|
||||
OidParametros parametro,
|
||||
byte[] data)
|
||||
private bool ProcessarParametro(OIDModel item, OidParametros parametro, byte[] data)
|
||||
{
|
||||
switch (parametro)
|
||||
{
|
||||
case OidParametros.CodErro:
|
||||
if (data.Length >= 4)
|
||||
{
|
||||
short codigo =
|
||||
ReadInt16BE(data, 2);
|
||||
|
||||
item.CodigoAlarme.valor =
|
||||
(OidCodigoErro)codigo;
|
||||
short codigo = ReadInt16BE(data, 2);
|
||||
item.CodigoAlarme.valor = (OidCodigoErro)codigo;
|
||||
return true;
|
||||
}
|
||||
break;
|
||||
|
||||
case OidParametros.VelocidadeAtual:
|
||||
if (data.Length >= 6)
|
||||
{
|
||||
item.ERPM.valor = ReadInt32BE(data, 2);
|
||||
return true;
|
||||
}
|
||||
break;
|
||||
|
||||
case OidParametros.CicloTrabalho:
|
||||
if (data.Length >= 4)
|
||||
item.CicloTrabalho.valor =
|
||||
ReadInt16BE(data, 2) / 10.0;
|
||||
{
|
||||
item.CicloTrabalho.valor = ReadInt16BE(data, 2) / 10.0;
|
||||
return true;
|
||||
}
|
||||
break;
|
||||
|
||||
case OidParametros.Potencia:
|
||||
if (data.Length >= 4)
|
||||
item.Potencia.valor =
|
||||
ReadInt16BE(data, 2);
|
||||
{
|
||||
item.Potencia.valor = ReadInt16BE(data, 2);
|
||||
return true;
|
||||
}
|
||||
break;
|
||||
|
||||
case OidParametros.Tensao:
|
||||
if (data.Length >= 4)
|
||||
item.Tensao.valor =
|
||||
ReadInt16BE(data, 2);
|
||||
{
|
||||
item.Tensao.valor = ReadInt16BE(data, 2);
|
||||
return true;
|
||||
}
|
||||
break;
|
||||
|
||||
case OidParametros.CorrenteMotor:
|
||||
if (data.Length >= 4)
|
||||
item.CorrenteMotor.valor =
|
||||
ReadInt16BE(data, 2) / 100.0;
|
||||
{
|
||||
item.CorrenteMotor.valor = ReadInt16BE(data, 2) / 100.0;
|
||||
return true;
|
||||
}
|
||||
break;
|
||||
|
||||
case OidParametros.CorrenteBarramento:
|
||||
if (data.Length >= 4)
|
||||
item.CorrenteBarramento.valor =
|
||||
ReadInt16BE(data, 2) / 100.0;
|
||||
{
|
||||
item.CorrenteBarramento.valor = ReadInt16BE(data, 2) / 100.0;
|
||||
return true;
|
||||
}
|
||||
break;
|
||||
|
||||
case OidParametros.Temperatura:
|
||||
if (data.Length >= 4)
|
||||
item.Temperatura.valor =
|
||||
ReadInt16BE(data, 2);
|
||||
{
|
||||
item.Temperatura.valor = ReadInt16BE(data, 2);
|
||||
return true;
|
||||
}
|
||||
break;
|
||||
|
||||
case OidParametros.PosicaoAbsoluta:
|
||||
if (data.Length >= 6)
|
||||
item.PosicaoAbsoluta =
|
||||
ReadInt32BE(data, 2) / 100.0;
|
||||
{
|
||||
item.PosicaoAbsoluta = ReadInt32BE(data, 2) / 100.0;
|
||||
return true;
|
||||
}
|
||||
break;
|
||||
|
||||
case OidParametros.PosicaoRelativa:
|
||||
if (data.Length >= 6)
|
||||
item.PosicaoRelativa =
|
||||
ReadInt32BE(data, 2) / 100.0;
|
||||
{
|
||||
item.PosicaoRelativa = ReadInt32BE(data, 2) / 100.0;
|
||||
return true;
|
||||
}
|
||||
break;
|
||||
|
||||
case OidParametros.ModoCodificador:
|
||||
|
|
@ -1564,7 +1701,10 @@ namespace AgroBase.Services
|
|||
* Manual: resposta DLC 3, DATA[2]=0/1.
|
||||
*/
|
||||
if (data.Length >= 3)
|
||||
{
|
||||
item.ModoCodificador = data[2];
|
||||
return true;
|
||||
}
|
||||
break;
|
||||
|
||||
case OidParametros.ErroLerCache:
|
||||
|
|
@ -1573,20 +1713,25 @@ namespace AgroBase.Services
|
|||
* Mantém o primeiro para compatibilidade.
|
||||
*/
|
||||
if (data.Length >= 3)
|
||||
{
|
||||
item.ErroHistorico = data[2];
|
||||
return true;
|
||||
}
|
||||
break;
|
||||
|
||||
case OidParametros.IO_Status:
|
||||
if (data.Length >= 4)
|
||||
item.IO_Status =
|
||||
ReadUInt16BE(data, 2);
|
||||
{
|
||||
item.IO_Status = ReadUInt16BE(data, 2);
|
||||
return true;
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
private void ProcessarStatusRetornoZero(
|
||||
OIDModel item,
|
||||
byte[] data)
|
||||
private void ProcessarStatusRetornoZero(OIDModel item, byte[] data)
|
||||
{
|
||||
/*
|
||||
* Manual:
|
||||
|
|
@ -1603,9 +1748,7 @@ namespace AgroBase.Services
|
|||
|
||||
internal static class OidModelExtensions
|
||||
{
|
||||
public static void SetModoControleSeguro(
|
||||
this OIDModel model,
|
||||
OIDModoControle modo)
|
||||
public static void SetModoControleSeguro(this OIDModel model, OIDModoControle modo)
|
||||
{
|
||||
if (model == null)
|
||||
return;
|
||||
|
|
|
|||
|
|
@ -1430,6 +1430,9 @@ namespace AgroBase.Services.Operadores
|
|||
("pendentes_nao_enviadas", DadosCan.PendentesNaoEnviadas),
|
||||
("pendentes_aguardando_resposta", DadosCan.PendentesAguardandoResposta),
|
||||
("frames_rx_fila", DadosCan.FramesRxNaFila),
|
||||
("idade_max_fila_nao_enviada_ms", DadosCan.IdadeMaxFilaNaoEnviadaMs),
|
||||
("pendentes_nao_enviadas_atrasadas", DadosCan.PendentesNaoEnviadasAtrasadas),
|
||||
("idade_media_fila_nao_enviada_ms", DadosCan.IdadeMediaFilaNaoEnviadaMs),
|
||||
|
||||
// Latência
|
||||
("latencia_media_ms", DadosCan.LatenciaMediaMs),
|
||||
|
|
|
|||
|
|
@ -77,6 +77,9 @@ class ModuloCAN(ModuloDiagnosticoBase):
|
|||
pend_nao_env = int(m.get("pendentes_nao_enviadas", 0) or 0)
|
||||
pend_resp = int(m.get("pendentes_aguardando_resposta", 0) or 0)
|
||||
frames_rx_fila = int(m.get("frames_rx_fila", 0) or 0)
|
||||
idade_max_fila_nao_enviada_ms = float(m.get("idade_max_fila_nao_enviada_ms", 0.0) or 0.0)
|
||||
pendentes_nao_enviadas_atrasadas = int(m.get("pendentes_nao_enviadas_atrasadas", 0) or 0)
|
||||
idade_media_fila_nao_enviada_ms = float(m.get("idade_media_fila_nao_enviada_ms", 0.0) or 0.0)
|
||||
|
||||
timeouts_total = int(m.get("timeouts_resposta_total", 0) or 0)
|
||||
timeouts_por_id_total = m.get("timeouts_por_id_total", {}) or {}
|
||||
|
|
@ -276,6 +279,9 @@ class ModuloCAN(ModuloDiagnosticoBase):
|
|||
"pendentes_total": pend_total,
|
||||
"pendentes_nao_enviadas": pend_nao_env,
|
||||
"pendentes_aguardando_resposta": pend_resp,
|
||||
"idade_max_fila_nao_enviada_ms": idade_max_fila_nao_enviada_ms,
|
||||
"idade_media_fila_nao_enviada_ms": idade_media_fila_nao_enviada_ms,
|
||||
"pendentes_nao_enviadas_atrasadas": pendentes_nao_enviadas_atrasadas,
|
||||
"frames_rx_fila": frames_rx_fila,
|
||||
"timeouts_resposta_total": timeouts_total,
|
||||
"timeouts_por_id_total": timeouts_por_id_total,
|
||||
|
|
|
|||
Loading…
Reference in New Issue