agrobot_base/AgroBase/AgroBase/Models/Modules/MovimentacaoUnificadoModel.cs

497 lines
21 KiB
C#

using AgroBase.Services;
using System;
using System.Collections.Generic;
using System.Data;
using System.Linq;
using System.Threading.Tasks;
using static AgroBase.Models.Enums;
namespace AgroBase.Models.Modules
{
public class MovimentacaoUnificadaModel : DispositivoBaseModel
{
public T_Code Dispositivo { get; set; } = T_Code.Mvd;
public byte _EnderecoCAN_Tx { get; set; } = 0x00;
public byte _EnderecoCAN_Rx { get; set; } = 0x00;
public bool ConexaoAtiva
{
get
{
return Modulos.Any(x => x.Conectado);
}
}
public bool DirPronto { get; set; } = false;
public bool MovPronto { get; set; } = false;
public int _TaxaAmostragem { get; set; } = 250;
public double DistanciaPercorridaOperacao { get; set; } = 0;
public double TempoEmAtividade { get; set; } = 0;
public double DistanciaPercorridaTotal { get; set; } = 0;
public double DistanciaPercorridaParcial { get; set; } = 0;
public double VelocidadeMedia { get; set; } = 0.0;
public double DiametroRoda { get; set; } = 0.3556;
public double ReducaoMotorBLDC { get; set; } = 5;
public double RPM_Max_MotorBLDC { get; set; } = 500;
public string IP_Mov { get; set; } = Variaveis.FaixaIPethernet + ".10";
public string IP_Dir { get; set; } = Variaveis.FaixaIPethernet + ".11";
public static int LogsManter { get; set; } = 60;
public AsyncTaskTimerModel tmrRegistrador;
public AsyncTaskTimerModel tmrRegistradorMOV;
public AsyncTaskTimerModel tmrRegistradorDIR;
public List<ModuloMvdModel> Modulos { get; set; } = new List<ModuloMvdModel>();
public (bool, List<string>) StatusModulosMOV(Direcao Lado)
{
string Prefixo = Lado == Direcao.Esquerda ? "E" : Lado == Direcao.Direita ? "D" : "";
var Mods = Modulos.Where(Mod => Mod.Modulo_ID.Contains(Prefixo) && Mod.MovMotor.Comandar).ToList();
var ModsComFalha = Mods
.Where(Mod =>
!Mod.MovMotor.Inicializado || // nao inicializado
Mod.MovMotor.CodigoAlarme != OIDCanService.OidHandler.OidCodigoErro.NenhumErro || // com leitura de erro
Mod.MovMotor.AlarmeSensores || // com alarme atuado por sensor
Mod.MovMotor.AlarmeComando // com alarme atuado por comando de movimento mas o movimento nao acontece
)
.ToList();
int total = Mods.Count;
int falhas = ModsComFalha.Count;
bool _operante = falhas < total;
List<string> ModsInoperantes = ModsComFalha.Select(x => x.Modulo_ID).ToList();
return (_operante, ModsInoperantes);
}
public (bool, List<string>) StatusModulosDIR(TipoMovimentoDirecional TipoMovimento)
{
var Mods = Modulos.Where(Mod => Mod.DirMotor.Comandar).ToList();
var ModsComFalha = Mods.Where(Mod => !Mod.DirMotor.Inicializado); // nao inicializado
bool _operante = !ModsComFalha.Any();
List<string> ModsInoperantes = ModsComFalha.Select(x => x.Modulo_ID).ToList();
return (_operante, ModsInoperantes);
}
public static MovimentacaoUnificadaModel CarregarParametrosIniciais()
{
return new MovimentacaoUnificadaModel()
{
Modulos = new List<ModuloMvdModel>()
{
new ModuloMvdModel()
{
Modulo_ID = "ET",
MovMotor = MovimentacaoModel.CarregarParametrosIniciais("ET", 0x05, 0x05),
DirMotor = DirecionalModel.CarregarParametrosIniciais("ET", 0x01, 0x01),
},
new ModuloMvdModel()
{
Modulo_ID = "EF",
MovMotor = MovimentacaoModel.CarregarParametrosIniciais("EF", 0x07, 0x07),
DirMotor = DirecionalModel.CarregarParametrosIniciais("EF", 0x03, 0x03)
},
new ModuloMvdModel()
{
Modulo_ID = "DT",
MovMotor = MovimentacaoModel.CarregarParametrosIniciais("DT", 0x06, 0x06),
DirMotor = DirecionalModel.CarregarParametrosIniciais("DT", 0x02, 0x02)
},
new ModuloMvdModel()
{
Modulo_ID = "DF",
MovMotor = MovimentacaoModel.CarregarParametrosIniciais("DF", 0x08, 0x08),
DirMotor = DirecionalModel.CarregarParametrosIniciais("DF", 0x04, 0x04)
},
},
};
}
public void LimparDados<T>(T _dados)
{
var Dados = _dados as MovimentacaoUnificadaModel;
Dados.DirPronto = false;
Dados.MovPronto = false;
Dados.Modulos.ForEach(x =>
{
x.MovMotor.Leitura = null;
x.DirMotor.Leitura = null;
x.DirMotor.UltimoReferenciamento = DateTime.MinValue;
x.DirMotor.Referenciado = false;
x.DirMotor.Referenciando = false;
});
ReiniciarLeituras(Dados);
}
public void ReiniciarLeituras(MovimentacaoUnificadaModel Dados)
{
Dados.DistanciaPercorridaParcial = 0;
Dados.DistanciaPercorridaTotal = 0;
Dados.DistanciaPercorridaOperacao = 0;
Dados.VelocidadeMedia = 0;
Dados.TempoEmAtividade = 0;
Dados.Modulos.ForEach(x =>
{
x.MovMotor.AlarmeSensores = false;
x.MovMotor.ConfigFreio.Leitura = false;
x.MovMotor.LogGrafico = new List<MvdMotorMOVSensorimanetoGrafico>();
x.MovMotor.Temperatura = 0;
x.MovMotor.RPM_SP = 0;
x.MovMotor.RPM_SP_Controle = 0;
x.DirMotor.Angulo_SP = 0;
x.DirMotor.Sentido_SP = Sentido.Parado;
});
}
public List<(int, CanMessagePosicaoDados, byte[])> ProtocoloConfiguracao(bool Conectar)
{
return new List<(int, CanMessagePosicaoDados, byte[])>();
}
public void RegistrarLogMov(ModuloMvdModel Modulo)
{
Modulo.MovMotor.LogGrafico.Add(new MvdMotorMOVSensorimanetoGrafico()
{
Momento = DateTime.Now,
Modulo_ID = Modulo.Modulo_ID,
RPM_SP = Modulo.MovMotor.RPM_SP_Leitura,
RPM_Motor = Modulo.MovMotor.RPM_Motor,
RPM_Roda = Modulo.MovMotor.RPM_Roda,
Corrente = Modulo.MovMotor.Corrente_Motor,
Sentido = Modulo.MovMotor.Sentido,
Temperatura = Modulo.MovMotor.Temperatura,
TemperaturaDriver = Modulo.MovMotor.TemperaturaDriver,
Tensao = Modulo.MovMotor.Tensao,
Velocidade = Modulo.MovMotor.VelocidadeInstantanea,
Potencia = Modulo.MovMotor.Potencia,
CicloTrabalho = Modulo.MovMotor.CicloTrabalho
});
if (Modulo.MovMotor.LogGrafico.Count > 60)
{
Modulo.MovMotor.LogGrafico.RemoveAt(0);
}
}
public void IniciarRegistrador()
{
if (!(tmrRegistrador?.IsRunning ?? false))
{
LimparDados(this);
PararRegistrador();
tmrRegistrador = new AsyncTaskTimerModel("tmrRegistrador_" + T_Code.Mvd.ToString(), tmrRegistrador_Tick, _TaxaAmostragem);
tmrRegistrador.Start();
tmrRegistradorMOV = new AsyncTaskTimerModel("tmrRegistrador_" + T_Code.Mov.ToString(), tmrRegistradorMOV_Tick, OIDCanService.TaxaAmostragem);
tmrRegistradorMOV.Start();
tmrRegistradorDIR = new AsyncTaskTimerModel("tmrRegistrador_" + T_Code.Dir.ToString(), tmrRegistradorDIR_Tick, MKS057DCanService.TaxaAmostragem);
tmrRegistradorDIR.Start();
}
}
public void PararRegistrador()
{
tmrRegistrador?.Dispose();
tmrRegistradorMOV?.Dispose();
tmrRegistradorDIR?.Dispose();
}
public async Task tmrRegistrador_Tick()
{
tmrRegistrador.SetInterval(_TaxaAmostragem);
if (!Variaveis.OperacaoEmAndamento.Simulando)
{
VelocidadeMedia = FuncoesMatematicas.ConverteKmhParaMs(Modulos.Where(x => x.Conectado && x.MovMotor.ComandoLigado).DefaultIfEmpty().Average(x => x?.MovMotor?.VelocidadeInstantanea ?? 0));
}
var UltimaDirecao = Variaveis.OperacaoEmAndamento.Controle.TiposControle.FirstOrDefault(x => x.Tipo == T_Code.Mov)?.UltimaDirecao ?? Direcao.Parado;
if (UltimaDirecao != Direcao.Parado)
{
double tempo = _TaxaAmostragem / 1000.0;
double distancia = tempo * VelocidadeMedia;
DistanciaPercorridaTotal += distancia;
DistanciaPercorridaParcial += ((UltimaDirecao == Direcao.Frente ? 1 : UltimaDirecao == Direcao.Tras ? -1 : 0) * distancia);
DistanciaPercorridaOperacao += distancia;
TempoEmAtividade += tempo;
}
}
private async Task tmrRegistradorMOV_Tick()
{
if (Variaveis.Fechando)
{
return;
}
if (!OIDCanService.Iniciado && SerialService.DispositivosMapeados.Any(x => x.Dispositivo == T_Code.Mov))
{
foreach (var Leitura in OIDCanService.DadosLeitura.Where(x => x.UltimoComandoRecebido.AddSeconds(10) < DateTime.Now))
{
OIDCanService.RequisitarDado(_EnderecoCAN_Tx, _EnderecoCAN_Rx, OIDCanService.OidHandler.OidParametros.CodErro);
}
tmrRegistradorMOV.SetInterval(3000);
return;
}
foreach (var Modulo in Modulos.Where(x => x.MovMotor.Inicializado).OrderBy(x => x.MovMotor._EnderecoCAN_Tx))
{
byte addrTx = Modulo.MovMotor._EnderecoCAN_Tx;
byte addrRx = Modulo.MovMotor._EnderecoCAN_Rx;
OIDCanService.RequisitarDado(addrTx, addrRx, OIDCanService.OidHandler.OidParametros.VelocidadeAtual);
OIDCanService.RequisitarDado(addrTx, addrRx, OIDCanService.OidHandler.OidParametros.Potencia);
OIDCanService.RequisitarDado(addrTx, addrRx, OIDCanService.OidHandler.OidParametros.Temperatura);
OIDCanService.RequisitarDado(addrTx, addrRx, OIDCanService.OidHandler.OidParametros.CicloTrabalho);
OIDCanService.RequisitarDado(addrTx, addrRx, OIDCanService.OidHandler.OidParametros.CodErro);
OIDCanService.RequisitarDado(addrTx, addrRx, OIDCanService.OidHandler.OidParametros.CorrenteBarramento);
OIDCanService.RequisitarDado(addrTx, addrRx, OIDCanService.OidHandler.OidParametros.CorrenteMotor);
OIDCanService.RequisitarDado(addrTx, addrRx, OIDCanService.OidHandler.OidParametros.Tensao);
var Controle = Variaveis.OperacaoEmAndamento.Controle.TiposControle?.FirstOrDefault(x => x.Tipo == T_Code.Mov);
if (Controle != null)
{
Modulo.MovMotor.EnviarComandoControle(Controle.UltimaDirecao);
}
Modulo.MovMotor.ConfigFreio.Leitura = Modulo.MovMotor.Leitura?.Freado ?? false;
Variaveis.OperacaoEmAndamento.DispMvd.Dados.RegistrarLogMov(Modulo);
var sTMP = Variaveis.OperacaoEmAndamento.DispSen?.Dados?.Sensores?.FirstOrDefault(x => x.ID.Contains(Modulo.Modulo_ID));
if (sTMP != null)
{
Modulo.MovMotor.Temperatura = sTMP.ValoresLeituras.FirstOrDefault(x => x.funcao == FuncoesPinout.Temperatura)?.atual?.valor ?? -1;
}
}
tmrRegistradorMOV.SetInterval(OIDCanService.TaxaAmostragem);
}
private async Task tmrRegistradorDIR_Tick()
{
if (Variaveis.Fechando)
{
return;
}
if (!MKS057DCanService.Iniciado && SerialService.DispositivosMapeados.Any(x => x.Dispositivo == T_Code.Dir))
{
foreach (var Leitura in MKS057DCanService.DadosLeitura.Where(x => x.UltimoComandoRecebido.AddSeconds(10) < DateTime.Now))
{
MKS057DCanService.RequisitarDado(Leitura.EnderecoTx, Leitura.EnderecoRx, MKS057DCanService.MksHandler.MksFuncCode.PulsosRecebidos);
}
tmrRegistradorDIR.SetInterval(3000);
return;
}
foreach (var Modulo in Modulos.Where(x => x.DirMotor.Inicializado))
{
byte addrTx = Modulo.DirMotor._EnderecoCAN_Tx;
byte addrRx = Modulo.DirMotor._EnderecoCAN_Rx;
MKS057DCanService.RequisitarDado(addrTx, addrRx, MKS057DCanService.MksHandler.MksFuncCode.PulsosRecebidos);
MKS057DCanService.RequisitarDado(addrTx, addrRx, MKS057DCanService.MksHandler.MksFuncCode.IO);
// Verifica se o angulo de set point do motor em questao e diferente do angulo atual, e se for, reenvia o comando para garantir que o motor esteja coerente com o set point
if (!MKS057DCanService.Referenciando && (Modulo.DirMotor.UltimoComandoEnviado.AddMilliseconds(MKS057DCanService.TaxaAmostragem) < DateTime.Now) && !Modulo.DirMotor.AtingiuAnguloSP)
{
var Controle = Variaveis.OperacaoEmAndamento.Controle.TiposControle?.FirstOrDefault(x => x.Tipo == T_Code.Dir);
if (Controle != null)
{
Modulo.DirMotor.EnviarComandoControle(Controle.UltimaDirecao);
}
}
}
tmrRegistradorDIR.SetInterval(MKS057DCanService.TaxaAmostragem);
}
public async Task ExecutarMovimentoDirecionalCombinado(Direcao DirecaoAtual)
{
if (DirecaoAtual == Direcao.Parado)
{
foreach (var Modulo in Modulos)
{
Modulo.MovMotor.EnviarComandoControle(Direcao.Parado);
}
await Task.Delay(1000);
}
switch (Variaveis.OperacaoEmAndamento.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)
{
Variaveis.OperacaoEmAndamento.Controle.PercentualVelocidadeSP = 20;
foreach (var Modulo in Modulos)
{
Modulo.MovMotor.EnviarComandoControle(
DirecaoAtual == Direcao.Direita ? Direcao.Frente :
DirecaoAtual == Direcao.Esquerda ? Direcao.Tras :
Direcao.Parado
);
}
}
break;
}
}
public async Task<bool> RealizarReferenciamentoDirecional()
{
if (!MKS057DCanService.Referenciando)
{
TipoMovimentoDirecional tipoControle = Variaveis.OperacaoEmAndamento.Controle.TipoMovimento;
Variaveis.OperacaoEmAndamento.Controle.TipoMovimento = TipoMovimentoDirecional.Diagnostico;
var ModsRef = Modulos.Where(x => x.DirMotor.Comandar && x.DirMotor.Inicializado).ToList();
foreach (var Modulo in ModsRef)
{
Task.Run(async () => await Modulo.DirMotor.ReferenciaMotor());
}
bool ReferenciamentoConcluido() => !MKS057DCanService.Referenciando;
await FuncoesGlobais.AguardarCondicaoAsync(ReferenciamentoConcluido, 2 * 1000 * 60, 1000);
Variaveis.OperacaoEmAndamento.Controle.TipoMovimento = tipoControle;
}
return Modulos.All(x => x.DirMotor.Referenciado);
}
public List<Task> RealizarCalibragemMovAsync(bool forcar)
{
List<Task> tasks = new List<Task>();
var DispMvd = Variaveis.OperacaoEmAndamento.DispMvd;
if (DispMvd != null)
{
var calMov = Variaveis.OperacaoEmAndamento.ModsCalibragem[T_Code.Mov];
foreach (var mod in DispMvd.Dados.Modulos)
{
calMov.TryGetValue(mod.Modulo_ID, out var cal);
if (!forcar && cal && calMov[mod.Modulo_ID]) continue;
calMov[mod.Modulo_ID] = mod.MovMotor.Inicializado;
}
DispMvd.Dados.MovPronto = DispMvd.Dados.Modulos.All(x => x.MovMotor.Inicializado);
}
return tasks;
}
public List<Task> RealizarCalibragemDirAsync(bool forcar)
{
List<Task> tasks = new List<Task>();
var DispMvd = Variaveis.OperacaoEmAndamento.DispMvd;
if (DispMvd != null)
{
var calDir = Variaveis.OperacaoEmAndamento.ModsCalibragem[T_Code.Dir];
foreach (var mod in DispMvd.Dados.Modulos)
{
calDir.TryGetValue(mod.Modulo_ID, out var cal);
if (!forcar && cal && calDir[mod.Modulo_ID]) continue;
calDir[mod.Modulo_ID] = false;
}
DispMvd.Dados.DirPronto = DispMvd.Dados.Modulos.All(x => x.DirMotor.Inicializado);
if (DispMvd.Dados.Modulos.Any(x => x.DirMotor.Inicializado))
{
Variaveis.OperacaoEmAndamento.Controle.TipoMovimento = TipoMovimentoDirecional.Diagnostico;
var ModsRef = Modulos.Where(x => x.DirMotor.Comandar).ToList(); // && x.DirMotor.Inicializado
foreach (var Modulo in ModsRef)
{
if (calDir[Modulo.Modulo_ID]) continue;
tasks.Add(Modulo.DirMotor.ReferenciaMotor().ContinueWith(t => {
if (t.IsFaulted)
Console.WriteLine($"Erro no referenciamento do módulo {Modulo.Modulo_ID}: {t.Exception?.InnerException?.Message}");
if (t.IsCompleted)
{
Console.WriteLine($"Referenciamento do módulo {Modulo.Modulo_ID} concluído com sucesso!");
calDir[Modulo.Modulo_ID] = true;
}
}));
}
}
}
return tasks;
}
public void AtualizarDadosFreio()
{
foreach (var Mod in Modulos)
{
var Freio = Mod.MovMotor.ConfigFreio;
Freio.AtualizarDados();
if (Freio.EnviarComando && Freio.UltimoEnvio.AddMilliseconds(Freio.DelayEnvio) < DateTime.Now)
{
GeneralJoystick.EnviarComandoSensoriamento(S_Code.sFRO, Freio.Servo.ID, Freio.AnguloControle);
Freio.UltimoEnvio = DateTime.Now;
}
}
}
}
public class ModuloMvdModel
{
public string Modulo_ID { get; set; }
public bool Disponivel { get; set; }
public bool RequisitarDados { get; set; } = false;
public bool Conectado
{
get
{
return MovMotor.Inicializado || DirMotor.Inicializado;
}
}
public MovimentacaoModel MovMotor { get; set; }
public DirecionalModel DirMotor { get; set; }
public static PinoutDataModel Pinout { get; set; }
public static List<FuncoesPinout> Funcoes { get; set; } = new List<FuncoesPinout>()
{
FuncoesPinout.TX,
FuncoesPinout.RX,
FuncoesPinout.ModbusDI,
FuncoesPinout.ModbusRO,
FuncoesPinout.ModbusDE_RE
};
}
}