1562 lines
60 KiB
C#
1562 lines
60 KiB
C#
using AgroBase.Services;
|
|
using System;
|
|
using System.Collections.Generic;
|
|
using System.Linq;
|
|
using System.Threading.Tasks;
|
|
using static AgroBase.Models.Enums;
|
|
|
|
namespace AgroBase.Models.Modules
|
|
{
|
|
public class MovimentacaoUnificadaModel : DispositivoBaseModel
|
|
{
|
|
public bool Conectado
|
|
{
|
|
get
|
|
{
|
|
return Modulos.Any(x => x.Conectado);
|
|
}
|
|
set
|
|
{
|
|
|
|
}
|
|
}
|
|
|
|
public static int _TaxaAmostragem { get; set; } = 1000;
|
|
|
|
public double DistanciaPercorridaOperacao { get; set; } = 0;
|
|
public int TempoEmAtividade { get; set; } = 0;
|
|
public double DistanciaPercorridaTotal { get; set; } = 0;
|
|
public double DistanciaPercorridaParcial { get; set; } = 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_Bld { get; set; } = Variaveis.FaixaIPethernet + ".10";
|
|
public string IP_Mks { get; set; } = Variaveis.FaixaIPethernet + ".11";
|
|
|
|
public static int LogsManter { get; set; } = 60;
|
|
public AsyncTaskTimerModel tmrRegistrador;
|
|
public AsyncTaskTimerModel tmrRegistradorBLD;
|
|
public AsyncTaskTimerModel tmrRegistradorMKS;
|
|
|
|
|
|
public List<ModuloMvdModel> Modulos { get; set; } = new List<ModuloMvdModel>();
|
|
|
|
public static Direcao UltimaDirecao { get; set; } = Direcao.Parado;
|
|
|
|
public (bool, List<string>) StatusModulosBLD(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.BLD_Inicializado ||
|
|
Mod.MovMotor.CodigoAlarme != 0 ||
|
|
Mod.MovMotor.AlarmeSensores
|
|
)
|
|
.ToList();
|
|
|
|
bool _operante = ModsComFalha.Count() < Mods.Count();
|
|
List<string> ModsInoperantes = ModsComFalha.Select(x => x.Modulo_ID).ToList();
|
|
|
|
return (_operante, ModsInoperantes);
|
|
}
|
|
|
|
public (bool, List<string>) StatusModulosMKS(TipoMovimentoDirecional TipoMovimento)
|
|
{
|
|
var Mods = Modulos.Where(Mod => Mod.DirMotor.Comandar).ToList();
|
|
var ModsComFalha = Mods
|
|
.Where(Mod =>
|
|
!Mod.DirMotor.MKS_Inicializado && (
|
|
Mod.DirMotor.ConfigSentidos[TipoMovimento].Esquerda != Sentido.Parado &&
|
|
Mod.DirMotor.ConfigSentidos[TipoMovimento].Direita != Sentido.Parado
|
|
)
|
|
);
|
|
|
|
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 = MvdMotorBLDC.CarregarParametrosIniciais("ET", "0x01"),
|
|
DirMotor = MvdMotorPasso.CarregarParametrosIniciais("ET", "0x01"),
|
|
},
|
|
new ModuloMvdModel()
|
|
{
|
|
Modulo_ID = "EF",
|
|
MovMotor = MvdMotorBLDC.CarregarParametrosIniciais("EF", "0x03"),
|
|
DirMotor = MvdMotorPasso.CarregarParametrosIniciais("EF", "0x03")
|
|
},
|
|
new ModuloMvdModel()
|
|
{
|
|
Modulo_ID = "DT",
|
|
MovMotor = MvdMotorBLDC.CarregarParametrosIniciais("DT", "0x02"),
|
|
DirMotor = MvdMotorPasso.CarregarParametrosIniciais("DT", "0x02")
|
|
},
|
|
new ModuloMvdModel()
|
|
{
|
|
Modulo_ID = "DF",
|
|
MovMotor = MvdMotorBLDC.CarregarParametrosIniciais("DF", "0x04"),
|
|
DirMotor = MvdMotorPasso.CarregarParametrosIniciais("DF", "0x04")
|
|
},
|
|
},
|
|
};
|
|
}
|
|
|
|
public void LimparDados<T>(T _dados)
|
|
{
|
|
var Dados = _dados as MovimentacaoUnificadaModel;
|
|
Dados.Modulos.ForEach(x =>
|
|
{
|
|
x.MovMotor.Leitura = null;
|
|
x.DirMotor.Leitura = null;
|
|
});
|
|
|
|
ReiniciarLeituras(Dados);
|
|
}
|
|
|
|
public void ReiniciarLeituras(MovimentacaoUnificadaModel Dados)
|
|
{
|
|
Dados.DistanciaPercorridaParcial = 0;
|
|
Dados.DistanciaPercorridaTotal = 0;
|
|
Dados.DistanciaPercorridaOperacao = 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.MovMotor.Corrente = 0;
|
|
|
|
x.DirMotor.Angulo_SP = 0;
|
|
x.DirMotor.Sentido_SP = Sentido.Parado;
|
|
});
|
|
}
|
|
|
|
public Dictionary<string, List<string>> ProtocoloConfiguracao(bool Conectar)
|
|
{
|
|
var Config = Modulos.Where(x => x.MovMotor.Comandar || x.DirMotor.Comandar).ToDictionary(x => x.Modulo_ID, x => x.ProtocoloConfiguracao(Conectar).ToList());
|
|
return Config;
|
|
}
|
|
|
|
public string ProtocoloVerificacao(bool Completo)
|
|
{
|
|
return "";
|
|
}
|
|
|
|
public void RecebeProtocoloModulo(string Protocolo)
|
|
{
|
|
|
|
}
|
|
|
|
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,
|
|
Sentido = Modulo.MovMotor.Sentido,
|
|
Temperatura = Modulo.MovMotor.Temperatura,
|
|
TemperaturaDriver = Modulo.MovMotor.TemperaturaDriver,
|
|
Tensao = Modulo.MovMotor.Tensao,
|
|
Velocidade = Modulo.MovMotor.VelocidadeInstantanea
|
|
});
|
|
|
|
if (Modulo.MovMotor.LogGrafico.Count > 60)
|
|
{
|
|
Modulo.MovMotor.LogGrafico.RemoveAt(0);
|
|
}
|
|
}
|
|
|
|
public void DefineStatusConexao(string Mod_ID, bool conecatdo)
|
|
{
|
|
var Modulo = Modulos.FirstOrDefault(x => x.Modulo_ID == Mod_ID);
|
|
if (Modulo != null)
|
|
{
|
|
//Modulo.Conectado = conecatdo;
|
|
}
|
|
}
|
|
|
|
public string GetTaxaAmostragem()
|
|
{
|
|
return _TaxaAmostragem.ToString("00000");
|
|
}
|
|
|
|
public void SetTaxaAmostragem(int TaxaAmostragem)
|
|
{
|
|
_TaxaAmostragem = TaxaAmostragem;
|
|
}
|
|
|
|
public bool GetStatusConexao()
|
|
{
|
|
return Conectado;
|
|
}
|
|
|
|
public string GetIP()
|
|
{
|
|
return IP_Bld;
|
|
}
|
|
|
|
public void IniciarRegistrador()
|
|
{
|
|
if (!(tmrRegistrador?.IsRunning ?? false))
|
|
{
|
|
LimparDados(this);
|
|
|
|
PararRegistrador();
|
|
|
|
tmrRegistrador = new AsyncTaskTimerModel("tmrRegistrador_" + T_Code.Mvd.ToString(), tmrRegistrador_Tick, _TaxaAmostragem);
|
|
tmrRegistrador.Start();
|
|
|
|
tmrRegistradorBLD = new AsyncTaskTimerModel("tmrRegistrador_" + T_Code.Bld.ToString(), tmrRegistradorBLD_Tick, BLD300RService.TaxaAmostragem);
|
|
tmrRegistradorBLD.Start();
|
|
|
|
tmrRegistradorMKS = new AsyncTaskTimerModel("tmrRegistrador_" + T_Code.Mks.ToString(), tmrRegistradorMKS_Tick, MKS057DService.TaxaAmostragem);
|
|
tmrRegistradorMKS.Start();
|
|
}
|
|
}
|
|
|
|
public void PararRegistrador()
|
|
{
|
|
tmrRegistrador?.Dispose();
|
|
tmrRegistradorBLD?.Dispose();
|
|
tmrRegistradorMKS?.Dispose();
|
|
}
|
|
|
|
public async Task tmrRegistrador_Tick()
|
|
{
|
|
tmrRegistrador.SetInterval(_TaxaAmostragem);
|
|
|
|
if (UltimaDirecao != Direcao.Parado)
|
|
{
|
|
TempoEmAtividade++;
|
|
double VelocidadeMedia = Modulos.Where(x => x.Conectado).Average(x => x.MovMotor.VelocidadeInstantanea * _TaxaAmostragem / 3600.0);
|
|
DistanciaPercorridaTotal += 1 * VelocidadeMedia;
|
|
DistanciaPercorridaParcial += (UltimaDirecao == Direcao.Frente ? 1 : UltimaDirecao == Direcao.Tras ? -1 : 0) * VelocidadeMedia;
|
|
DistanciaPercorridaOperacao += 1 * VelocidadeMedia;
|
|
Variaveis.OperacaoEmAndamento.Sensoriamento.VelocidadeMedia = VelocidadeMedia;
|
|
}
|
|
}
|
|
|
|
private async Task tmrRegistradorBLD_Tick()
|
|
{
|
|
if (!BLD300RService.Iniciado && SerialService.DispositivosMapeados.Any(x => x.Dispositivo == T_Code.Bld))
|
|
{
|
|
await BLD300RService.InicializarEthernetService();
|
|
|
|
foreach (var Leitura in BLD300RService.DadosLeitura.Where(x => x.UltimoComandoRecebido.AddSeconds(10) < DateTime.Now))
|
|
{
|
|
BLD300RService.AdicionarComandoNaFila(new BLD300RService.BLDComandoModel()
|
|
{
|
|
Endereco = Leitura.Endereco,
|
|
Parametro = BLD300RService.Parametros.First(x => x.Chave == "Versao"),
|
|
Get = true,
|
|
AguardaResposta = true
|
|
});
|
|
}
|
|
|
|
tmrRegistradorBLD.SetInterval(3000);
|
|
return;
|
|
}
|
|
|
|
var Controle = Variaveis.OperacaoEmAndamento.Controle.TiposControle.FirstOrDefault(x => x.Tipo == T_Code.Mov);
|
|
|
|
foreach (var Modulo in Modulos.Where(x => x.MovMotor.BLD_Inicializado).OrderBy(x => x.MovMotor.EnderecoByte))
|
|
{
|
|
foreach (var Parametro in BLD300RService.Parametros.Where(x => x.LeituraLoop))
|
|
{
|
|
var Comando = new BLD300RService.BLDComandoModel()
|
|
{
|
|
Endereco = Modulo.MovMotor.EnderecoByte,
|
|
AguardaResposta = true,
|
|
Parametro = Parametro,
|
|
Get = true,
|
|
TimeoutResposta = 500
|
|
};
|
|
BLD300RService.AdicionarComandoNaFila(Comando);
|
|
}
|
|
|
|
Modulo.MovMotor.EnviarComandoControle(Controle.UltimaDirecao);
|
|
|
|
Modulo.MovMotor.ConfigFreio.Leitura = Modulo.MovMotor.Leitura?.Freado ?? false;
|
|
|
|
Variaveis.OperacaoEmAndamento.DispMvd.Dados.RegistrarLogMov(Modulo);
|
|
}
|
|
|
|
tmrRegistradorBLD.SetInterval(BLD300RService.TaxaAmostragem);
|
|
}
|
|
|
|
private async Task tmrRegistradorMKS_Tick()
|
|
{
|
|
if (!MKS057DService.Iniciado && SerialService.DispositivosMapeados.Any(x => x.Dispositivo == T_Code.Mks))
|
|
{
|
|
await MKS057DService.InicializarEthernetService();
|
|
|
|
foreach (var Leitura in MKS057DService.DadosLeitura.Where(x => x.UltimoComandoRecebido.AddSeconds(10) < DateTime.Now))
|
|
{
|
|
MKS057DService.AdicionarComandoNaFila(MKS057DService.MontarComandoRequisicaoPulsos(Leitura.Endereco));
|
|
}
|
|
|
|
tmrRegistradorMKS.SetInterval(3000);
|
|
return;
|
|
}
|
|
|
|
var Controle = Variaveis.OperacaoEmAndamento.Controle.TiposControle.FirstOrDefault(x => x.Tipo == T_Code.Dir);
|
|
foreach (var Modulo in Modulos.Where(x => x.DirMotor.MKS_Inicializado))
|
|
{
|
|
byte modAddr = Modulo.DirMotor.EnderecoByte;
|
|
if (!MKS057DService.Referenciando || (MKS057DService.Referenciando && MKS057DService.AddrRef == modAddr))
|
|
{
|
|
bool reqParametros = true; // (!MKS057DService.Referenciando && (tmrRegistradorMKS.Ticks % 5 == 0)) || MKS057DService.Referenciando;
|
|
if (reqParametros)
|
|
{
|
|
MKS057DService.AdicionarComandoNaFila(MKS057DService.MontarComandoRequisicaoPulsos(modAddr));
|
|
MKS057DService.AdicionarComandoNaFila(MKS057DService.MontarComandoRequisicaoIO(modAddr));
|
|
}
|
|
|
|
// 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 (!MKS057DService.Referenciando && (Modulo.DirMotor.UltimoComandoEnviado.AddMilliseconds(2000) < DateTime.Now) && !Modulo.DirMotor.AtingiuAnguloSP)
|
|
{
|
|
Modulo.DirMotor.EnviarComandoControle(Controle.UltimaDirecao);
|
|
}
|
|
}
|
|
}
|
|
|
|
MKS057DService.TaxaAmostragem = MKS057DService.Referenciando ? 200 : 1000;
|
|
tmrRegistradorMKS.SetInterval(MKS057DService.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.MKS_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 RealizarReferenciamentoDirecional()
|
|
{
|
|
if (!MKS057DService.Referenciando)
|
|
{
|
|
TipoMovimentoDirecional tipoControle = Variaveis.OperacaoEmAndamento.Controle.TipoMovimento;
|
|
|
|
Variaveis.OperacaoEmAndamento.Controle.TipoMovimento = TipoMovimentoDirecional.Diagnostico;
|
|
foreach (var Modulo in Modulos.Where(x => x.DirMotor.Comandar && x.DirMotor.MKS_Inicializado && !x.DirMotor.IN1_Atuado))
|
|
{
|
|
await Modulo.DirMotor.ReferenciaMotor();
|
|
}
|
|
Variaveis.OperacaoEmAndamento.Controle.TipoMovimento = tipoControle;
|
|
}
|
|
}
|
|
|
|
|
|
|
|
}
|
|
|
|
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.BLD_Inicializado || DirMotor.MKS_Inicializado;
|
|
}
|
|
}
|
|
|
|
public MvdMotorBLDC MovMotor { get; set; }
|
|
public MvdMotorPasso 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
|
|
};
|
|
|
|
|
|
public List<string> ProtocoloConfiguracao(bool Conectar)
|
|
{
|
|
List<string> Config = new List<string>();
|
|
|
|
var _pinout = new PinoutDataModel()
|
|
{
|
|
ExpansorGPIO = Pinout.ExpansorGPIO,
|
|
EnderecoExpansor = Pinout.EnderecoExpansor,
|
|
Pinos = Pinout.Pinos.ToList(),
|
|
};
|
|
_pinout.Pinos = _pinout.Pinos.Where(x => x.ComponenteID == T_Code.Mvd.ToString()).ToList();
|
|
string ProtocoloMOD =
|
|
((int)F_Code.Cfg).ToString() + SerialService.SplitMessage +
|
|
Modulo_ID + SerialService.SplitParams +
|
|
(Conectar ? "1" : "0") + SerialService.SplitParams +
|
|
MovimentacaoUnificadaModel._TaxaAmostragem.ToString() + SerialService.SplitParams +
|
|
(RequisitarDados ? "1" : "0") + SerialService.SplitParams +
|
|
PinoutModel.PinoProtocolo(_pinout, Funcoes, SerialService.SplitParams);
|
|
|
|
if (Conectar)
|
|
{
|
|
Config.Add(ProtocoloMOD);
|
|
}
|
|
|
|
if (MovMotor.Comandar)
|
|
{
|
|
Config.Add(MovMotor.ProtocoloConfiguracao(Conectar));
|
|
}
|
|
/*if (DirMotor.Comandar)
|
|
{
|
|
Config.Add(DirMotor.ProtocoloConfiguracao(Conectar));
|
|
}*/
|
|
|
|
if (!Conectar)
|
|
{
|
|
Config.Add(ProtocoloMOD);
|
|
}
|
|
|
|
Config.Add(ProtocoloVerificacao(false));
|
|
|
|
return Config;
|
|
}
|
|
|
|
public string ProtocoloVerificacao(bool Completo)
|
|
{
|
|
if (Completo)
|
|
{
|
|
string ProtocoloCheck =
|
|
SerialService.BeginLine +
|
|
0 + SerialService.SplitMessage +
|
|
Modulo_ID + SerialService.SplitMessage +
|
|
((int)F_Code.Chk).ToString() + SerialService.SplitMessage +
|
|
"check" +
|
|
SerialService.EndLine;
|
|
|
|
return ProtocoloCheck;
|
|
}
|
|
else
|
|
{
|
|
string ProtocoloCheck =
|
|
((int)F_Code.Chk).ToString() + SerialService.SplitMessage +
|
|
"check";
|
|
|
|
return ProtocoloCheck;
|
|
}
|
|
}
|
|
|
|
public void AtualizarSensoresMotor()
|
|
{
|
|
var sensorTmp = Variaveis.OperacaoEmAndamento.DispSen.Dados.DadosLeitura.SensoresTemperatura.FirstOrDefault(x => x.ID.Contains(Modulo_ID));
|
|
if (sensorTmp != null)
|
|
{
|
|
MovMotor.Temperatura = sensorTmp.Temperatura;
|
|
|
|
AlarmeModel _Alarme = new AlarmeModel()
|
|
{
|
|
Tipo = S_Code.sTMP,
|
|
Valor = sensorTmp.Temperatura,
|
|
Maximo = sensorTmp.Temperatura > sensorTmp.LimiteMaximo,
|
|
Minimo = sensorTmp.Temperatura < sensorTmp.LimiteMinimo,
|
|
ParaMaximo = false,
|
|
ParaMinimo = false
|
|
};
|
|
VerificarAlarme(_Alarme);
|
|
}
|
|
var sensorCor = Variaveis.OperacaoEmAndamento.DispSen.Dados.DadosLeitura.SensoresCorrente.FirstOrDefault(x => x.ID.Contains(Modulo_ID));
|
|
if (sensorCor != null)
|
|
{
|
|
MovMotor.Corrente = (double)sensorCor.Current;
|
|
|
|
AlarmeModel _Alarme = new AlarmeModel()
|
|
{
|
|
Tipo = S_Code.sCOR,
|
|
Valor = (double)sensorCor.Current,
|
|
Maximo = (double)sensorCor.Current > sensorCor.LimiteMaximo,
|
|
Minimo = (double)sensorCor.Current < sensorCor.LimiteMinimo,
|
|
ParaMaximo = false,
|
|
ParaMinimo = false
|
|
};
|
|
VerificarAlarme(_Alarme);
|
|
}
|
|
}
|
|
|
|
private void VerificarAlarme(AlarmeModel _Alarme)
|
|
{
|
|
bool Adicionado = false;
|
|
var Alm = MovMotor.Alarmes.FirstOrDefault(x => x.Tipo == _Alarme.Tipo);
|
|
if (Alm == null)
|
|
{
|
|
Alm = _Alarme;
|
|
MovMotor.Alarmes.Add(Alm);
|
|
Adicionado = true;
|
|
}
|
|
|
|
bool AlarmeAnterior = (Alm.Maximo && Alm.ParaMaximo) || (Alm.Minimo && Alm.ParaMinimo);
|
|
bool AlarmeAtual = (_Alarme.Maximo && _Alarme.ParaMaximo) || (_Alarme.Minimo && _Alarme.ParaMinimo);
|
|
|
|
if (MovMotor.BLD_Inicializado && (!AlarmeAnterior || Adicionado) && AlarmeAtual) // Motor iniciado, Não estava em alarme anteriormente ou foi adicionado agora, alarme atual
|
|
{
|
|
MovMotor.EnviarComandoControle(Direcao.Parado);
|
|
}
|
|
|
|
Alm.Valor = _Alarme.Valor;
|
|
Alm.Maximo = _Alarme.Maximo;
|
|
Alm.Minimo = _Alarme.Minimo;
|
|
|
|
MovMotor.AlarmeSensores = MovMotor.Alarmes.Any(x => (x.Maximo && x.ParaMaximo) || (x.Minimo && x.ParaMinimo));
|
|
}
|
|
|
|
}
|
|
|
|
public class MvdMotorBLDC
|
|
{
|
|
public string ID { get; set; } = T_Code.Mov.ToString();
|
|
|
|
// SETs
|
|
public string Endereco { get; set; }
|
|
public byte EnderecoByte
|
|
{
|
|
get
|
|
{
|
|
return Convert.ToByte(Endereco.Replace("0x", ""), 16);
|
|
}
|
|
}
|
|
public bool LeituraLigado
|
|
{
|
|
get
|
|
{
|
|
return Leitura?.Ativado ?? false;
|
|
}
|
|
}
|
|
public bool ComandoLigado { get; set; } = false;
|
|
//public bool Freio { get; set; } = false;
|
|
public int CodigoAlarme
|
|
{
|
|
get
|
|
{
|
|
return Leitura?.CodigoAlarme ?? 0;
|
|
}
|
|
}
|
|
public bool AlarmeMotor
|
|
{
|
|
get
|
|
{
|
|
return CodigoAlarme > 0 && CodigoAlarme < 99;
|
|
}
|
|
}
|
|
public int RPM_SP { get; set; } = 0;
|
|
public int RPM_SP_Controle { get; set; } = 0;
|
|
public int RPM_SP_Leitura
|
|
{
|
|
get
|
|
{
|
|
return RPM_SP / Convert.ToInt32(Variaveis.OperacaoEmAndamento.DispMvd.Dados.ReducaoMotorBLDC);
|
|
}
|
|
}
|
|
public Sentido Sentido_SP { get; set; } = Sentido.Parado;
|
|
public int NumeroPolos { get; set; } = 10;
|
|
public int Rampa { get; set; } = 0;
|
|
public double OffsetLeitura { get; set; } = 1.0;
|
|
|
|
|
|
// GETs
|
|
public double RPM_Roda
|
|
{
|
|
get
|
|
{
|
|
return (Leitura?.RPM ?? 0) * OffsetLeitura;
|
|
}
|
|
}
|
|
public Sentido Sentido
|
|
{
|
|
get
|
|
{
|
|
return Leitura?.Direcao ?? Sentido.Parado;
|
|
}
|
|
}
|
|
public double Temperatura { get; set; } = 0;
|
|
public double TemperaturaDriver
|
|
{
|
|
get
|
|
{
|
|
return Math.Round(Leitura?.Temperatura ?? 0, 2);
|
|
}
|
|
}
|
|
public double Tensao
|
|
{
|
|
get
|
|
{
|
|
return Math.Round(Leitura?.Tensao ?? 0, 2);
|
|
}
|
|
}
|
|
public double Corrente_Max
|
|
{
|
|
get
|
|
{
|
|
return Math.Round(Leitura?.CorrenteMax ?? 0, 2);
|
|
}
|
|
}
|
|
public double Corrente { get; set; } = 0;
|
|
public double RPM_Motor
|
|
{
|
|
get
|
|
{
|
|
//return Convert.ToInt32(RPM_Motor / VariaveisEquipamento.RelacaoRPM);
|
|
return RPM_Roda * VariaveisEquipamento.RelacaoRPM;
|
|
//return RPM_Roda * 0.74;
|
|
}
|
|
}
|
|
public double VelocidadeInstantanea
|
|
{
|
|
get
|
|
{
|
|
return FuncoesMatematicas.CalculaVelocidadeRPM(RPM_Roda);
|
|
}
|
|
}
|
|
|
|
|
|
public bool Comandar { get; set; } = false;
|
|
public bool AlarmeSensores { get; set; } = false;
|
|
public bool BLD_Inicializado
|
|
{
|
|
get
|
|
{
|
|
return Leitura?.Iniciado ?? false;
|
|
}
|
|
}
|
|
private BLD300RModel _leitura = null;
|
|
public BLD300RModel Leitura
|
|
{
|
|
get
|
|
{
|
|
_leitura = BLD300RService.DadosLeitura?.FirstOrDefault(x => x.Endereco == EnderecoByte);
|
|
return _leitura;
|
|
}
|
|
set
|
|
{
|
|
_leitura = value;
|
|
}
|
|
}
|
|
|
|
|
|
public Dictionary<TipoMovimentoDirecional,ConfiguracaoSentidoMotor> ConfigSentidos { get; set; }
|
|
public List<FuncoesPinout> Funcoes { get; set; }
|
|
|
|
public List<MvdMotorMOVSensorimanetoGrafico> LogGrafico { get; set; } = new List<MvdMotorMOVSensorimanetoGrafico>();
|
|
public List<AlarmeModel> Alarmes { get; set; } = new List<AlarmeModel>();
|
|
|
|
public MvdServoFreioModel ConfigFreio { get; set; } = new MvdServoFreioModel();
|
|
|
|
|
|
public static MvdMotorBLDC CarregarParametrosIniciais(string Mod_ID, string _Endereco)
|
|
{
|
|
return new MvdMotorBLDC()
|
|
{
|
|
ID = T_Code.Mov.ToString(),
|
|
Endereco = _Endereco,
|
|
Funcoes = new List<FuncoesPinout>()
|
|
{
|
|
FuncoesPinout.TMP,
|
|
FuncoesPinout.ENA,
|
|
FuncoesPinout.DIR,
|
|
FuncoesPinout.PWM,
|
|
FuncoesPinout.BRK,
|
|
},
|
|
ConfigFreio = new MvdServoFreioModel()
|
|
{
|
|
AnguloInicial = 10,
|
|
Incremental = Mod_ID.Contains("E"),
|
|
Leitura = false,
|
|
},
|
|
ConfigSentidos = new Dictionary<TipoMovimentoDirecional, ConfiguracaoSentidoMotor>()
|
|
{
|
|
{
|
|
TipoMovimentoDirecional.RodasDianteiras,
|
|
new ConfiguracaoSentidoMotor()
|
|
{
|
|
Direita = Sentido.Parado,
|
|
Esquerda = Sentido.Parado,
|
|
Frente = Mod_ID.Contains("E") ? Sentido.Horario : Sentido.Antihorario,
|
|
Tras = Mod_ID.Contains("E") ? Sentido.Antihorario : Sentido.Horario
|
|
}
|
|
},
|
|
{
|
|
TipoMovimentoDirecional.RodasTraseiras,
|
|
new ConfiguracaoSentidoMotor()
|
|
{
|
|
Direita = Sentido.Parado,
|
|
Esquerda = Sentido.Parado,
|
|
Frente = Mod_ID.Contains("E") ? Sentido.Horario : Sentido.Antihorario,
|
|
Tras = Mod_ID.Contains("E") ? Sentido.Antihorario : Sentido.Horario
|
|
}
|
|
},
|
|
{
|
|
TipoMovimentoDirecional.MovimentoDiagonal,
|
|
new ConfiguracaoSentidoMotor()
|
|
{
|
|
Direita = Sentido.Parado,
|
|
Esquerda = Sentido.Parado,
|
|
Frente = Mod_ID.Contains("E") ? Sentido.Horario : Sentido.Antihorario,
|
|
Tras = Mod_ID.Contains("E") ? Sentido.Antihorario : Sentido.Horario
|
|
}
|
|
},
|
|
{
|
|
TipoMovimentoDirecional.MovimentoArco,
|
|
new ConfiguracaoSentidoMotor()
|
|
{
|
|
Direita = Sentido.Parado,
|
|
Esquerda = Sentido.Parado,
|
|
Frente = Mod_ID.Contains("E") ? Sentido.Horario : Sentido.Antihorario,
|
|
Tras = Mod_ID.Contains("E") ? Sentido.Antihorario : Sentido.Horario
|
|
}
|
|
},
|
|
{
|
|
TipoMovimentoDirecional.RotacionarNoEixo,
|
|
new ConfiguracaoSentidoMotor()
|
|
{
|
|
Direita = Sentido.Parado,
|
|
Esquerda = Sentido.Parado,
|
|
Frente = Sentido.Horario,
|
|
Tras = Sentido.Antihorario
|
|
}
|
|
},
|
|
{
|
|
TipoMovimentoDirecional.MovimentoLateral,
|
|
new ConfiguracaoSentidoMotor()
|
|
{
|
|
Direita = Sentido.Parado,
|
|
Esquerda = Sentido.Parado,
|
|
Frente = Mod_ID.Contains("T") ? Sentido.Horario : Sentido.Antihorario,
|
|
Tras = Mod_ID.Contains("T") ? Sentido.Antihorario : Sentido.Horario
|
|
}
|
|
},
|
|
{
|
|
TipoMovimentoDirecional.Diagnostico,
|
|
new ConfiguracaoSentidoMotor()
|
|
{
|
|
Direita = Sentido.Parado,
|
|
Esquerda = Sentido.Parado,
|
|
Frente = Mod_ID.Contains("E") ? Sentido.Horario : Sentido.Antihorario,
|
|
Tras = Mod_ID.Contains("E") ? Sentido.Antihorario : Sentido.Horario
|
|
}
|
|
},
|
|
},
|
|
};
|
|
}
|
|
|
|
|
|
public string ProtocoloConfiguracao(bool Conectar)
|
|
{
|
|
var _pinout = new PinoutDataModel()
|
|
{
|
|
ExpansorGPIO = ModuloMvdModel.Pinout.ExpansorGPIO,
|
|
EnderecoExpansor = ModuloMvdModel.Pinout.EnderecoExpansor,
|
|
Pinos = ModuloMvdModel.Pinout.Pinos.ToList(),
|
|
};
|
|
_pinout.Pinos = _pinout.Pinos.Where(x => x.ComponenteID == ID).ToList();
|
|
|
|
string Protocolo =
|
|
((int)F_Code.Cfg).ToString() + SerialService.SplitMessage +
|
|
ID + SerialService.SplitParams +
|
|
(Conectar ? "1" : "0") + SerialService.SplitParams +
|
|
Endereco + SerialService.SplitParams +
|
|
NumeroPolos + SerialService.SplitParams +
|
|
Rampa + SerialService.SplitParams +
|
|
Variaveis.OperacaoEmAndamento.DispMvd.Dados.RPM_Max_MotorBLDC + SerialService.SplitParams +
|
|
PinoutModel.PinoProtocolo(_pinout, Funcoes, SerialService.SplitParams);
|
|
|
|
return Protocolo;
|
|
}
|
|
|
|
public void EnviarComandoControle(Direcao DirecaoAtual)
|
|
{
|
|
bool Freado = DirecaoAtual == Direcao.EmFreio;
|
|
(bool AtuarServo, int AnguloFreio) = ConfigFreio.AtualizarDadosFreio(Freado, 0, -1, 5);
|
|
|
|
if (Comandar && BLD_Inicializado)
|
|
{
|
|
if (AlarmeSensores || AlarmeMotor)
|
|
{
|
|
DirecaoAtual = Direcao.Parado;
|
|
Freado = false;
|
|
(AtuarServo, AnguloFreio) = ConfigFreio.AtualizarDadosFreio(Freado, 0, -1, 5);
|
|
|
|
if (AlarmeMotor)
|
|
{
|
|
BLD300RService.RegisrarErrosComunicacao(EnderecoByte, $"Módulo com código de alarme {CodigoAlarme}, reiniciando o módulo...");
|
|
BLD300RService.AdicionarComandoNaFila(new BLD300RService.BLDComandoModel()
|
|
{
|
|
Endereco = EnderecoByte,
|
|
AguardaResposta = true,
|
|
TimeoutResposta = 500,
|
|
Get = false,
|
|
Parametro = BLD300RService.Parametros.First(x => x.Chave == "AlarmeCode"),
|
|
Valor = 0
|
|
});
|
|
}
|
|
}
|
|
else
|
|
{
|
|
MovimentacaoUnificadaModel.UltimaDirecao = DirecaoAtual;
|
|
}
|
|
|
|
RPM_SP_Controle = Variaveis.OperacaoEmAndamento.Controle.RPM_Motor;
|
|
RPM_SP =
|
|
DirecaoAtual == Direcao.Parado || Freado ? 0 :
|
|
RPM_SP_Controle;
|
|
|
|
ComandoLigado = RPM_SP > 0;
|
|
|
|
switch (DirecaoAtual)
|
|
{
|
|
case Direcao.Frente:
|
|
Sentido_SP = ConfigSentidos[Variaveis.OperacaoEmAndamento.Controle.TipoMovimento].Frente;
|
|
break;
|
|
case Direcao.Tras:
|
|
Sentido_SP = ConfigSentidos[Variaveis.OperacaoEmAndamento.Controle.TipoMovimento].Tras;
|
|
break;
|
|
default:
|
|
Sentido_SP = Sentido.Parado;
|
|
break;
|
|
}
|
|
|
|
byte Addr = EnderecoByte;
|
|
DateTime Agora = DateTime.Now;
|
|
|
|
if (Sentido_SP != Sentido.Parado && (Leitura == null || Leitura.Direcao != Sentido_SP))
|
|
{
|
|
BLD300RService.AdicionarComandoNaFila(new BLD300RService.BLDComandoModel()
|
|
{
|
|
Endereco = Addr,
|
|
Parametro = BLD300RService.Parametros.First(x => x.Chave == "Direction"),
|
|
Valor = Sentido_SP == Sentido.Horario ? 1 : 0,
|
|
Get = false,
|
|
AguardaResposta = true,
|
|
TimeoutResposta = 500,
|
|
});
|
|
}
|
|
if (ComandoLigado && (Leitura == null || Leitura.RPM_SP != RPM_SP_Controle))
|
|
{
|
|
BLD300RService.AdicionarComandoNaFila(new BLD300RService.BLDComandoModel()
|
|
{
|
|
Endereco = Addr,
|
|
Parametro = BLD300RService.Parametros.First(x => x.Chave == "RPM_SP"),
|
|
Valor = RPM_SP,
|
|
Get = false,
|
|
AguardaResposta = true,
|
|
TimeoutResposta = 500,
|
|
});
|
|
}
|
|
if (Leitura == null || Leitura.Freado != Freado)
|
|
{
|
|
BLD300RService.AdicionarComandoNaFila(new BLD300RService.BLDComandoModel()
|
|
{
|
|
Endereco = Addr,
|
|
Parametro = BLD300RService.Parametros.First(x => x.Chave == "Breaked"),
|
|
Valor = Freado ? 1 : 0,
|
|
Get = false,
|
|
AguardaResposta = true,
|
|
TimeoutResposta = 500,
|
|
});
|
|
}
|
|
if (Leitura == null || ComandoLigado != LeituraLigado)
|
|
{
|
|
BLD300RService.AdicionarComandoNaFila(new BLD300RService.BLDComandoModel()
|
|
{
|
|
Endereco = Addr,
|
|
Parametro = BLD300RService.Parametros.First(x => x.Chave == "Enabled"),
|
|
Valor = ComandoLigado ? 1 : 0,
|
|
Get = false,
|
|
AguardaResposta = true,
|
|
TimeoutResposta = 500,
|
|
});
|
|
}
|
|
}
|
|
|
|
if (AtuarServo)
|
|
{
|
|
string Mod_ID = Variaveis.OperacaoEmAndamento.DispMvd.Dados.Modulos.FirstOrDefault(x => x.MovMotor.Endereco == Endereco)?.Modulo_ID ?? "";
|
|
var ServoFreio = Variaveis.OperacaoEmAndamento.DispSen.Dados.Servos.FirstOrDefault(x => x.ID.Contains(Mod_ID) && x.Inicializado && x.Controlar);
|
|
if (ServoFreio != null)
|
|
{
|
|
GeneralJoystick.EnviarComandoSensoriamento(S_Code.sFRO, ServoFreio.ID, AnguloFreio);
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
|
|
}
|
|
|
|
public class MvdMotorPasso
|
|
{
|
|
public string ID { get; set; } = T_Code.Dir.ToString();
|
|
|
|
// SETs
|
|
public string Endereco { get; set; }
|
|
public byte EnderecoByte
|
|
{
|
|
get
|
|
{
|
|
return Convert.ToByte(Endereco.Replace("0x", ""), 16);
|
|
}
|
|
}
|
|
public Sentido Sentido_SP { get; set; } = Sentido.Parado;
|
|
public double Angulo_SP { get; set; } = 0;
|
|
public double Angulo_Offset { get; set; } = 0;
|
|
public double Velocidade_Max { get; set; } = 600;
|
|
public int Velocidade
|
|
{
|
|
get
|
|
{
|
|
return Convert.ToInt32(((Variaveis.OperacaoEmAndamento.Controle?.VelocidadeMP ?? 0) / 100.0) * Velocidade_Max);
|
|
}
|
|
}
|
|
|
|
// GETs
|
|
public Sentido Sentido
|
|
{
|
|
get
|
|
{
|
|
return Leitura?.Direcao ?? Sentido.Parado;
|
|
}
|
|
}
|
|
public double Angulo
|
|
{
|
|
get
|
|
{
|
|
return MKS057DService.PulsosParaAngulo(Leitura?.NumeroPulsos ?? 0);
|
|
}
|
|
}
|
|
public double RPM
|
|
{
|
|
get
|
|
{
|
|
return Leitura?.RPM ?? 0;
|
|
}
|
|
}
|
|
public bool IN1_Atuado
|
|
{
|
|
get
|
|
{
|
|
return !(Leitura?.Entradas?[0] ?? true);
|
|
}
|
|
}
|
|
public bool IN2_Atuado
|
|
{
|
|
get
|
|
{
|
|
return !(Leitura?.Entradas?[1] ?? true);
|
|
}
|
|
}
|
|
|
|
|
|
public bool Comandar { get; set; } = false;
|
|
public bool MKS_Inicializado
|
|
{
|
|
get
|
|
{
|
|
return Leitura?.Iniciado ?? false;
|
|
}
|
|
}
|
|
private MKS057DModel _leitura = null;
|
|
public MKS057DModel Leitura
|
|
{
|
|
get
|
|
{
|
|
_leitura = MKS057DService.DadosLeitura?.FirstOrDefault(x => x.Endereco == EnderecoByte);
|
|
return _leitura;
|
|
}
|
|
set
|
|
{
|
|
_leitura = value;
|
|
}
|
|
}
|
|
|
|
public Direcao UltimaDirecao { get; set; } = Direcao.Parado;
|
|
public DateTime UltimoComandoEnviado { get; set; } = DateTime.MinValue;
|
|
public bool RetornandoAzero
|
|
{
|
|
get
|
|
{
|
|
return (UltimaDirecao != Direcao.Esquerda && UltimaDirecao != Direcao.Direita);
|
|
}
|
|
}
|
|
public bool EmMovimento
|
|
{
|
|
get
|
|
{
|
|
return RPM != 0;
|
|
}
|
|
}
|
|
public bool AtingiuAnguloSP
|
|
{
|
|
get
|
|
{
|
|
return FuncoesMatematicas.ValorEstaEntre(Angulo, Angulo_SP, 1);
|
|
}
|
|
}
|
|
|
|
public bool PosicaoTipoMovimentoConcluido(Direcao DirecaoAtual)
|
|
{
|
|
var TipoMovimento = Variaveis.OperacaoEmAndamento.Controle.TipoMovimento;
|
|
switch (TipoMovimento)
|
|
{
|
|
case TipoMovimentoDirecional.RotacionarNoEixo:
|
|
case TipoMovimentoDirecional.MovimentoLateral:
|
|
int agSP = DirecaoAtual != Direcao.Parado ? VariaveisEquipamento.AnguloMovimento[TipoMovimento] : 0;
|
|
bool anguloCorreto = agSP == Angulo;
|
|
if (!anguloCorreto && agSP != 0)
|
|
{
|
|
if (agSP > 0)
|
|
{
|
|
anguloCorreto = Angulo >= agSP;
|
|
}
|
|
else
|
|
{
|
|
anguloCorreto = Angulo <= agSP;
|
|
}
|
|
}
|
|
return !Comandar || anguloCorreto;
|
|
default:
|
|
return !Comandar || Angulo_SP == Angulo;
|
|
}
|
|
}
|
|
|
|
|
|
public Dictionary<TipoMovimentoDirecional, ConfiguracaoSentidoMotor> ConfigSentidos { get; set; }
|
|
public List<FuncoesPinout> Funcoes { get; set; }
|
|
|
|
|
|
public static MvdMotorPasso CarregarParametrosIniciais(string Mod_ID, string _Endereco)
|
|
{
|
|
return new MvdMotorPasso()
|
|
{
|
|
ID = T_Code.Dir.ToString(),
|
|
Endereco = _Endereco,
|
|
Funcoes = new List<FuncoesPinout>()
|
|
{
|
|
FuncoesPinout.PUL,
|
|
FuncoesPinout.DIR,
|
|
FuncoesPinout.ENA,
|
|
FuncoesPinout.EncoderAbsoluto
|
|
},
|
|
ConfigSentidos = new Dictionary<TipoMovimentoDirecional, ConfiguracaoSentidoMotor>()
|
|
{
|
|
{
|
|
TipoMovimentoDirecional.RodasDianteiras,
|
|
new ConfiguracaoSentidoMotor()
|
|
{
|
|
Direita = Mod_ID.Contains("E") ? Sentido.Horario : Sentido.Antihorario,
|
|
Esquerda = Mod_ID.Contains("E") ? Sentido.Antihorario : Sentido.Horario,
|
|
Frente = Sentido.Parado,
|
|
Tras = Sentido.Parado
|
|
}
|
|
},
|
|
{
|
|
TipoMovimentoDirecional.RodasTraseiras,
|
|
new ConfiguracaoSentidoMotor()
|
|
{
|
|
Direita = Mod_ID.Contains("E") ? Sentido.Horario : Sentido.Antihorario,
|
|
Esquerda = Mod_ID.Contains("E") ? Sentido.Antihorario : Sentido.Horario,
|
|
Frente = Sentido.Parado,
|
|
Tras = Sentido.Parado
|
|
}
|
|
},
|
|
{
|
|
TipoMovimentoDirecional.MovimentoDiagonal,
|
|
new ConfiguracaoSentidoMotor()
|
|
{
|
|
Direita = Mod_ID == "EF" || Mod_ID == "DT" ? Sentido.Horario : Sentido.Antihorario,
|
|
Esquerda = Mod_ID == "EF" || Mod_ID == "DT" ? Sentido.Antihorario : Sentido.Horario,
|
|
Frente = Sentido.Parado,
|
|
Tras = Sentido.Parado
|
|
}
|
|
},
|
|
{
|
|
TipoMovimentoDirecional.MovimentoArco,
|
|
new ConfiguracaoSentidoMotor()
|
|
{
|
|
Direita = Mod_ID.Contains("E") ? Sentido.Horario : Sentido.Antihorario,
|
|
Esquerda = Mod_ID.Contains("E") ? Sentido.Antihorario : Sentido.Horario,
|
|
Frente = Sentido.Parado,
|
|
Tras = Sentido.Parado
|
|
}
|
|
},
|
|
{
|
|
TipoMovimentoDirecional.RotacionarNoEixo,
|
|
new ConfiguracaoSentidoMotor()
|
|
{
|
|
Direita = Sentido.Horario,
|
|
Esquerda = Sentido.Horario,
|
|
Frente = Sentido.Parado,
|
|
Tras = Sentido.Parado
|
|
}
|
|
},
|
|
{
|
|
TipoMovimentoDirecional.MovimentoLateral,
|
|
new ConfiguracaoSentidoMotor()
|
|
{
|
|
Direita = Mod_ID == "EF" || Mod_ID == "DT" ? Sentido.Horario : Sentido.Antihorario,
|
|
Esquerda = Mod_ID == "EF" || Mod_ID == "DT" ? Sentido.Antihorario : Sentido.Horario,
|
|
Frente = Sentido.Parado,
|
|
Tras = Sentido.Parado
|
|
}
|
|
},
|
|
{
|
|
TipoMovimentoDirecional.Diagnostico,
|
|
new ConfiguracaoSentidoMotor()
|
|
{
|
|
Direita = Sentido.Horario,
|
|
Esquerda = Sentido.Antihorario,
|
|
Frente = Sentido.Parado,
|
|
Tras = Sentido.Parado
|
|
}
|
|
},
|
|
}
|
|
};
|
|
}
|
|
|
|
public void EnviarComandoControle(Direcao Direcao)
|
|
{
|
|
if (Comandar && MKS_Inicializado)
|
|
{
|
|
int AnguloTipoMovimento = VariaveisEquipamento.AnguloMovimento[Variaveis.OperacaoEmAndamento.Controle.TipoMovimento];
|
|
|
|
if (UltimaDirecao != Direcao)
|
|
{
|
|
Sentido_SP =
|
|
Direcao == Direcao.Direita ? ConfigSentidos[Variaveis.OperacaoEmAndamento.Controle.TipoMovimento].Direita :
|
|
Direcao == Direcao.Esquerda ? ConfigSentidos[Variaveis.OperacaoEmAndamento.Controle.TipoMovimento].Esquerda :
|
|
Sentido_SP == Sentido.Horario ? Sentido.Antihorario :
|
|
Sentido_SP == Sentido.Antihorario ? Sentido.Horario :
|
|
Sentido.Parado;
|
|
|
|
UltimaDirecao = Direcao;
|
|
}
|
|
else if (AtingiuAnguloSP && RetornandoAzero && Angulo == 0)
|
|
{
|
|
Sentido_SP = Sentido.Parado;
|
|
}
|
|
|
|
Angulo_SP =
|
|
RetornandoAzero ? 0 :
|
|
AnguloTipoMovimento != -1 ? AnguloTipoMovimento :
|
|
Variaveis.OperacaoEmAndamento.Controle.Angulo;
|
|
|
|
int mx = Sentido_SP == Sentido.Antihorario ? -1 : 1;
|
|
|
|
Angulo_SP = Math.Abs(Angulo_SP) * mx;
|
|
|
|
if (true || !AtingiuAnguloSP)
|
|
{
|
|
MKS057DService.AdicionarComandoNaFila(MKS057DService.MontarComandoMovimento(EnderecoByte, Angulo_SP, Velocidade, Sentido_SP));
|
|
UltimoComandoEnviado = DateTime.Now;
|
|
}
|
|
}
|
|
}
|
|
|
|
/*public async Task EnviarComandoControleAsync(Direcao Direcao)
|
|
{
|
|
if (Comandar && MKS_Inicializado)
|
|
{
|
|
int AnguloTipoMovimento = VariaveisEquipamento.AnguloMovimento[Variaveis.OperacaoEmAndamento.Controle.TipoMovimento];
|
|
|
|
if (UltimaDirecao != Direcao)
|
|
{
|
|
Sentido_SP =
|
|
Direcao == Direcao.Direita ? ConfigSentidos[Variaveis.OperacaoEmAndamento.Controle.TipoMovimento].Direita :
|
|
Direcao == Direcao.Esquerda ? ConfigSentidos[Variaveis.OperacaoEmAndamento.Controle.TipoMovimento].Esquerda :
|
|
Sentido_SP == Sentido.Horario ? Sentido.Antihorario :
|
|
Sentido_SP == Sentido.Antihorario ? Sentido.Horario :
|
|
Sentido.Parado;
|
|
|
|
UltimaDirecao = Direcao;
|
|
}
|
|
else if (AtingiuAnguloSP && RetornandoAzero && Angulo == 0)
|
|
{
|
|
Sentido_SP = Sentido.Parado;
|
|
}
|
|
|
|
Angulo_SP =
|
|
RetornandoAzero ? 0 :
|
|
AnguloTipoMovimento != -1 ? AnguloTipoMovimento :
|
|
Variaveis.OperacaoEmAndamento.Controle.Angulo;
|
|
|
|
int mx = Sentido_SP == Sentido.Antihorario ? -1 : 1;
|
|
|
|
Angulo_SP = Math.Abs(Angulo_SP) * mx;
|
|
|
|
if (!AtingiuAnguloSP || true)
|
|
{
|
|
//await MKS057DService.EnviarComandoControle(MKS057DService.MontarComandoMovimento(EnderecoByte, Angulo_SP, Velocidade, Sentido_SP));
|
|
MKS057DService.AdicionarComandoNaFila(MKS057DService.MontarComandoMovimento(EnderecoByte, Angulo_SP, Velocidade, Sentido_SP));
|
|
UltimoComandoEnviado = DateTime.Now;
|
|
}
|
|
}
|
|
}*/
|
|
|
|
public async Task ReferenciaMotor()
|
|
{
|
|
byte Addr = EnderecoByte;
|
|
|
|
MKS057DService.AddrRef = Addr;
|
|
MKS057DService.UltimoReferenciamento = DateTime.Now;
|
|
|
|
await Task.Delay(2000);
|
|
|
|
await MKS057DService.EnviarComandoControle(MKS057DService.MontarComandoSetZero(Addr));
|
|
|
|
await Task.Delay(500);
|
|
|
|
bool Sucesso = false;
|
|
bool EstadoInicial = IN1_Atuado;
|
|
bool EstadoAnterior = EstadoInicial;
|
|
|
|
double _anguloAnterior = Angulo;
|
|
double _velocidadeAnterior = 0;
|
|
int _angulo = 45;
|
|
int _offset = 0;
|
|
Sentido _sentido = Sentido.Horario;
|
|
int tentativas = 0;
|
|
double _anguloEntrada = 9999;
|
|
double _anguloSaida = 9999;
|
|
|
|
const int MaxTentativas = 10;
|
|
DateTime TimeoutGeral = DateTime.Now.AddMinutes(2); // Timeout geral para o processo.
|
|
|
|
double anguloMaxVisitado = 0;
|
|
double anguloMinVisitado = 0;
|
|
|
|
if (!Sucesso)
|
|
{
|
|
while (!Sucesso && tentativas < MaxTentativas && DateTime.Now < TimeoutGeral && MKS057DService.Referenciando)
|
|
{
|
|
// Se o angulo ja foi visitado, entao pode se mover mais rapido, senao, se move mais devagar
|
|
int VelocidadeGiro = DefineVelocidadeReferenciamento(EstadoInicial, anguloMaxVisitado, anguloMinVisitado);
|
|
|
|
await MKS057DService.EnviarComandoControle(MKS057DService.MontarComandoMovimento(Addr, _angulo, VelocidadeGiro, _sentido));
|
|
|
|
DateTime Inicio = DateTime.Now;
|
|
bool inverterGiro = false;
|
|
while (!Sucesso && !inverterGiro && Inicio.AddSeconds(120) > DateTime.Now && MKS057DService.Referenciando)
|
|
{
|
|
bool _ref = IN1_Atuado;
|
|
|
|
//Sucesso = (EstadoInicial && !_ref) || (!EstadoInicial && _ref);
|
|
|
|
// Comecou com o sensor ja atuado
|
|
if (EstadoInicial)
|
|
{
|
|
// Quando houver alteracao na leitura do sensor
|
|
if (_ref != EstadoAnterior)
|
|
{
|
|
// Se estava atuado e agora nao esta mais, e ainda nao definiu o angulo de entrada
|
|
if (!_ref && _anguloEntrada == 9999)
|
|
{
|
|
_anguloEntrada = Angulo;
|
|
inverterGiro = true;
|
|
}
|
|
// Se estava atuado e agora nao esta mais, e ja definiu o angulo de entrada, e ainda nao definiu o angulo de saida
|
|
else if (!_ref && _anguloEntrada != 9999 && _anguloSaida == 9999)
|
|
{
|
|
_anguloSaida = Angulo;
|
|
}
|
|
}
|
|
}
|
|
// Comecou com o sensor nao atuado
|
|
else
|
|
{
|
|
// Quando houver alteracao na leitura do sensor
|
|
if (_ref != EstadoAnterior)
|
|
{
|
|
// Se nao estava atuado e agora esta, e ainda nao definiu o angulo de entrada
|
|
if (_ref && _anguloEntrada == 9999)
|
|
{
|
|
_anguloEntrada = Angulo;
|
|
}
|
|
// Se estava atuado e agora nao esta mais, e ja definiu o angulo de entrada, e ainda nao definiu o angulo de saida
|
|
else if (!_ref && _anguloEntrada != 9999 && _anguloSaida == 9999)
|
|
{
|
|
_anguloSaida = Angulo;
|
|
inverterGiro = true;
|
|
}
|
|
}
|
|
}
|
|
|
|
// Realiza a verificacao de necessidade de reverter a rotacao do motor apenas se ainda nao encontrou o angulo de entrada do sensor
|
|
if (_anguloEntrada == 9999)
|
|
{
|
|
inverterGiro = (_angulo > 0 && Angulo >= (_angulo - 0.5)) || (_angulo < 0 && Angulo <= (_angulo + 0.5));
|
|
}
|
|
|
|
Sucesso = _anguloEntrada != 9999 && _anguloSaida != 9999;
|
|
|
|
EstadoAnterior = _ref;
|
|
|
|
Console.WriteLine($"Ref {Addr}: Angulo SP = {_angulo}, Velocidade: {VelocidadeGiro}, Angulo Atual = {Angulo}, Sentido SP = {_sentido.ToString()}, Passou = {inverterGiro}, Sucesso = {Sucesso}, Entrada: {_ref}");
|
|
|
|
if (Sucesso)
|
|
{
|
|
await MKS057DService.EnviarComandoControle(MKS057DService.MontarComandoParada(Addr));
|
|
await Task.Delay(1500);
|
|
|
|
int _centro = Convert.ToInt32((_anguloEntrada + _anguloSaida) / 2.0);
|
|
|
|
await MKS057DService.EnviarComandoControle(MKS057DService.MontarComandoMovimento(Addr, _centro, 36, Sentido.Horario));
|
|
await Task.Delay(2500);
|
|
await MKS057DService.EnviarComandoControle(MKS057DService.MontarComandoSetZero(Addr));
|
|
await Task.Delay(1000);
|
|
break;
|
|
}
|
|
|
|
await Task.Delay(200); // Verificações rápidas.
|
|
|
|
VelocidadeGiro = DefineVelocidadeReferenciamento(EstadoInicial, anguloMaxVisitado, anguloMinVisitado);
|
|
|
|
if (
|
|
(_anguloAnterior == Angulo && RPM == 0) || // Verifica se o angulo nao foi alterado desde a ultima leitura, e o motor nao esta em movimento
|
|
(_velocidadeAnterior != VelocidadeGiro) // Verifica se mudou a velocidade de giro do motor de acordo com o angulo atual
|
|
)
|
|
{
|
|
await MKS057DService.EnviarComandoControle(MKS057DService.MontarComandoMovimento(Addr, _angulo, VelocidadeGiro, _sentido));
|
|
}
|
|
|
|
_anguloAnterior = Angulo;
|
|
_velocidadeAnterior = VelocidadeGiro;
|
|
if (Angulo > anguloMaxVisitado)
|
|
{
|
|
anguloMaxVisitado = Angulo;
|
|
}
|
|
else if (Angulo < anguloMinVisitado)
|
|
{
|
|
anguloMinVisitado = Angulo;
|
|
}
|
|
}
|
|
|
|
if (!Sucesso && inverterGiro)
|
|
{
|
|
await MKS057DService.EnviarComandoControle(MKS057DService.MontarComandoParada(Addr));
|
|
await Task.Delay(500);
|
|
|
|
_sentido = _sentido == Sentido.Horario ? Sentido.Antihorario : Sentido.Horario;
|
|
_offset = Math.Min(100, _offset + 25); // Limita o offset máximo para evitar valores extremos.
|
|
int mx = _sentido == Sentido.Antihorario ? -1 : 1;
|
|
_angulo = (Math.Abs(_angulo) + _offset) * mx;
|
|
tentativas++;
|
|
}
|
|
}
|
|
}
|
|
|
|
if (!Sucesso)
|
|
{
|
|
await MKS057DService.EnviarComandoControle(MKS057DService.MontarComandoParada(Addr));
|
|
}
|
|
|
|
MKS057DService.AddrRef = 0x00;
|
|
MKS057DService.UltimoReferenciamento = DateTime.Now;
|
|
}
|
|
|
|
private int DefineVelocidadeReferenciamento(bool EstadoInicial, double anguloMaxVisitado, double anguloMinVisitado)
|
|
{
|
|
int VelocidadeGiro =
|
|
EstadoInicial || IN1_Atuado ? 10 : // Se o sensor estiver atuado ou o fluxo tiver iniciado com ele ja atuado, entao deve se mover lentamente para melhor precisao dos angulos de entrada e saida
|
|
Angulo > (anguloMaxVisitado + 5) || Angulo < (anguloMinVisitado - 5) ? 18 : // Se o angulo atual ainda nao foi visitado, deve se mover em velocidade media
|
|
20; // Se o angulo ja foi visitado, deve se mover em uma velocidade maior
|
|
return VelocidadeGiro;
|
|
}
|
|
|
|
|
|
|
|
}
|
|
|
|
public class MvdMotorMOVSensorimanetoGrafico
|
|
{
|
|
public DateTime Momento { get; set; }
|
|
public string Modulo_ID { get; set; }
|
|
public int RPM_SP { get; set; }
|
|
public double RPM_Motor { get; set; }
|
|
public double RPM_Roda { get; set; }
|
|
public double Temperatura { get; set; }
|
|
public double TemperaturaDriver { get; set; }
|
|
public Sentido Sentido { get; set; }
|
|
public double Velocidade { get; set; }
|
|
public double Tensao { get; set; }
|
|
public double Corrente { get; set; }
|
|
}
|
|
|
|
public class MvdServoFreioModel
|
|
{
|
|
public bool Incremental { get; set; }
|
|
public int AnguloInicial { get; set; }
|
|
private int AnguloControle { get; set; } = -1;
|
|
private int AnguloControleMin
|
|
{
|
|
get
|
|
{
|
|
if (Incremental)
|
|
{
|
|
return AnguloMin;
|
|
}
|
|
else
|
|
{
|
|
return AnguloMax;
|
|
}
|
|
}
|
|
}
|
|
private int AnguloControleMax { get; set; } = 20;
|
|
public bool Leitura { get; set; }
|
|
private bool _controle { get; set; }
|
|
public bool Controle
|
|
{
|
|
get
|
|
{
|
|
return _controle;
|
|
}
|
|
}
|
|
private int RPM_SP { get; set; }
|
|
private int MargemRPM { get; set; } = 2; // Margem de tolerância no RPM
|
|
private int AnguloMin { get; set; } = 0; // Ângulo mínimo permitido pelo servo
|
|
private int AnguloMax { get; set; } = 180; // Ângulo máximo permitido pelo servo
|
|
private int FatorEscala { get; set; } = 10; // Fator de ajuste do controle proporcional
|
|
|
|
public (bool, int) AtualizarDadosFreio(bool status, int rpm_sp = 0, int anguloInicial = -1, int anguloSP = -1)
|
|
{
|
|
bool mudou = _controle != status;
|
|
_controle = status;
|
|
RPM_SP = rpm_sp;
|
|
|
|
if (!_controle)
|
|
{
|
|
AnguloControle = AnguloControleMin; // Solta o freio se não estiver no modo de controle
|
|
return (mudou, AnguloControle);
|
|
}
|
|
|
|
if (anguloInicial == -1)
|
|
{
|
|
anguloInicial = AnguloInicial;
|
|
}
|
|
|
|
if (AnguloControle == -1)
|
|
{
|
|
AnguloControle = AnguloControleMin;
|
|
}
|
|
|
|
if (AnguloControle == AnguloControleMin)
|
|
{
|
|
AnguloControle = anguloInicial;
|
|
}
|
|
|
|
int ajuste = anguloSP;
|
|
if (anguloSP == -1)
|
|
{
|
|
double Rpm_Atual = FuncoesMatematicas.CalculaRPMVelocidade(Variaveis.OperacaoEmAndamento.Sensoriamento.VelocidadeMedia);
|
|
|
|
// Garante que os valores de RPM sejam válidos
|
|
Rpm_Atual = Math.Max(Rpm_Atual, 0);
|
|
RPM_SP = Math.Max(RPM_SP, 0);
|
|
|
|
// Verifica se o RPM atual está dentro da margem de tolerância
|
|
if (Math.Abs(Rpm_Atual - RPM_SP) <= MargemRPM)
|
|
{
|
|
return (false, AnguloControle); // Se dentro da margem, não ajusta o ângulo
|
|
}
|
|
|
|
// Calcula o erro e ajusta proporcionalmente
|
|
int erroRPM = RPM_SP - (int)Rpm_Atual;
|
|
ajuste = erroRPM / FatorEscala;
|
|
|
|
// Se incremental, aplica um ajuste limitado
|
|
if (Incremental)
|
|
{
|
|
ajuste = Math.Abs(ajuste); // Garante que o ajuste seja positivo
|
|
}
|
|
else
|
|
{
|
|
ajuste = -Math.Abs(ajuste); // Garante que o ajuste seja negativo
|
|
}
|
|
}
|
|
|
|
// Atualiza o ângulo de controle, garantindo que fique dentro dos limites
|
|
int limiteMax = AnguloInicial + (Incremental ? AnguloControleMax : -AnguloControleMax);
|
|
if (Incremental)
|
|
{
|
|
AnguloControle = (int)FuncoesMatematicas.Clamp(AnguloControle + ajuste, AnguloMin, limiteMax);
|
|
}
|
|
else
|
|
{
|
|
AnguloControle = (int)FuncoesMatematicas.Clamp(AnguloControle - ajuste, limiteMax, AnguloMax);
|
|
}
|
|
|
|
return (true, AnguloControle);
|
|
}
|
|
}
|
|
|
|
|
|
|
|
}
|