1621 lines
62 KiB
C#
1621 lines
62 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 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 static Direcao UltimaDirecao { get; set; } = Direcao.Parado;
|
|
|
|
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 ||
|
|
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>) StatusModulosDIR(TipoMovimentoDirecional TipoMovimento)
|
|
{
|
|
var Mods = Modulos.Where(Mod => Mod.DirMotor.Comandar).ToList();
|
|
var ModsComFalha = Mods
|
|
.Where(Mod =>
|
|
!Mod.DirMotor.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.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 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_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 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_Mov;
|
|
}
|
|
|
|
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, OIDService.TaxaAmostragem);
|
|
tmrRegistradorMOV.Start();
|
|
|
|
tmrRegistradorDIR = new AsyncTaskTimerModel("tmrRegistrador_" + T_Code.Dir.ToString(), tmrRegistradorDIR_Tick, MKS057DService.TaxaAmostragem);
|
|
tmrRegistradorDIR.Start();
|
|
}
|
|
}
|
|
|
|
public void PararRegistrador()
|
|
{
|
|
tmrRegistrador?.Dispose();
|
|
tmrRegistradorMOV?.Dispose();
|
|
tmrRegistradorDIR?.Dispose();
|
|
}
|
|
|
|
public async Task tmrRegistrador_Tick()
|
|
{
|
|
tmrRegistrador.SetInterval(_TaxaAmostragem);
|
|
|
|
if (UltimaDirecao != Direcao.Parado)
|
|
{
|
|
VelocidadeMedia = FuncoesMatematicas.ConverteKmhParaMs(Modulos.Where(x => x.Conectado && x.MovMotor.ComandoLigado).DefaultIfEmpty().Average(x => x?.MovMotor?.VelocidadeInstantanea ?? 0));
|
|
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 (!OIDService.Iniciado && SerialService.DispositivosMapeados.Any(x => x.Dispositivo == T_Code.Mov))
|
|
{
|
|
await OIDService.InicializarEthernetService();
|
|
|
|
foreach (var Leitura in OIDService.DadosLeitura.Where(x => x.UltimoComandoRecebido.AddSeconds(10) < DateTime.Now))
|
|
{
|
|
OIDService.AdicionarComandoNaFila(new OIDComandoModel()
|
|
{
|
|
Endereco = Leitura.Endereco,
|
|
Parametro = OIDService.Parametros.First(x => x.Chave == "CodErro"),
|
|
Get = true,
|
|
AguardaResposta = true
|
|
});
|
|
}
|
|
|
|
tmrRegistradorMOV.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.Inicializado).OrderBy(x => x.MovMotor.EnderecoByte))
|
|
{
|
|
foreach (var Parametro in OIDService.Parametros.Where(x => x.LeituraLoop))
|
|
{
|
|
var Comando = new OIDComandoModel()
|
|
{
|
|
Endereco = Modulo.MovMotor.EnderecoByte,
|
|
AguardaResposta = true,
|
|
Parametro = Parametro,
|
|
Get = true,
|
|
TimeoutResposta = 200
|
|
};
|
|
OIDService.AdicionarComandoNaFila(Comando);
|
|
}
|
|
|
|
if (Controle != null)
|
|
{
|
|
Modulo.MovMotor.EnviarComandoControle(Controle.UltimaDirecao);
|
|
}
|
|
|
|
Modulo.MovMotor.ConfigFreio.Leitura = Modulo.MovMotor.Leitura?.Freado ?? false;
|
|
|
|
Variaveis.OperacaoEmAndamento.DispMvd.Dados.RegistrarLogMov(Modulo);
|
|
}
|
|
|
|
tmrRegistradorMOV.SetInterval(OIDService.TaxaAmostragem);
|
|
}
|
|
|
|
private async Task tmrRegistradorDIR_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));
|
|
}
|
|
|
|
tmrRegistradorDIR.SetInterval(3000);
|
|
return;
|
|
}
|
|
|
|
foreach (var Modulo in Modulos.Where(x => x.DirMotor.Inicializado))
|
|
{
|
|
byte modAddr = Modulo.DirMotor.EnderecoByte;
|
|
if (!MKS057DService.Referenciando || (MKS057DService.Referenciando && MKS057DService.AddrRef == modAddr))
|
|
{
|
|
MKS057DService.AdicionarComandoNaFila(MKS057DService.MontarComandoRequisicaoPulsos(modAddr));
|
|
MKS057DService.AdicionarComandoNaFila(MKS057DService.MontarComandoRequisicaoIO(modAddr));
|
|
|
|
if (Modulo.DirMotor.SensorAnguloAuxiliar != null && Modulo.DirMotor.SensorAnguloAuxiliar.LimiteExcedido)
|
|
{
|
|
if (Modulo.DirMotor.SensorAnguloAuxiliar.ReleControle != null)
|
|
{
|
|
GeneralJoystick.EnviarComandoSensoriamento(S_Code.sRLE, Modulo.DirMotor.SensorAnguloAuxiliar.ReleControle.ID, Estado.Desligado);
|
|
await Task.Delay(2000);
|
|
}
|
|
Direcao direcaoContraria = Modulo.DirMotor.SensorAnguloAuxiliar.Sentido == Sentido.Horario ? Direcao.Esquerda : Direcao.Direita;
|
|
Modulo.DirMotor.EnviarComandoControle(direcaoContraria);
|
|
}
|
|
|
|
// 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(1000) < 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);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
MKS057DService.TaxaAmostragem = MKS057DService.Referenciando ? 200 : 1000;
|
|
tmrRegistradorDIR.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.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;
|
|
var ModsRef = Modulos.Where(x => x.DirMotor.Comandar && x.DirMotor.Inicializado && !x.DirMotor.IN1_Atuado).ToList();
|
|
foreach (var Modulo in ModsRef)
|
|
{
|
|
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.Inicializado || DirMotor.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 _Sensores = Variaveis.OperacaoEmAndamento.DispSen.Dados.DadosLeitura;
|
|
|
|
var sensorTmp = _Sensores.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 = _Sensores.SensoresCorrente.FirstOrDefault(x => x.ID.Contains(Modulo_ID));
|
|
if (sensorCor != null)
|
|
{
|
|
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.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?.CorrenteMotor != 0;
|
|
}
|
|
}
|
|
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 ?? 1);
|
|
}
|
|
}
|
|
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;
|
|
return RPM_Motor / VariaveisEquipamento.RelacaoRPM;
|
|
}
|
|
}
|
|
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_Barramento
|
|
{
|
|
get
|
|
{
|
|
return Math.Round(Leitura?.CorrenteBarramento ?? 0, 2);
|
|
}
|
|
}
|
|
public double Corrente_Motor
|
|
{
|
|
get
|
|
{
|
|
return Math.Round(Leitura?.CorrenteMotor ?? 0, 2);
|
|
}
|
|
}
|
|
public double Potencia
|
|
{
|
|
get
|
|
{
|
|
return Math.Round(Leitura?.Potencia ?? 0, 2);
|
|
}
|
|
}
|
|
public double CicloTrabalho
|
|
{
|
|
get
|
|
{
|
|
return Math.Round(Leitura?.CicloTrabalho ?? 0, 2);
|
|
}
|
|
}
|
|
public double RPM_Motor
|
|
{
|
|
get
|
|
{
|
|
//return Convert.ToInt32(RPM_Motor / VariaveisEquipamento.RelacaoRPM);
|
|
//return RPM_Roda * VariaveisEquipamento.RelacaoRPM;
|
|
//return RPM_Roda * 0.74;
|
|
|
|
return (Leitura?.RPM ?? 0);
|
|
}
|
|
}
|
|
public double VelocidadeInstantanea
|
|
{
|
|
get
|
|
{
|
|
return FuncoesMatematicas.CalculaVelocidadeRPM(RPM_Roda);
|
|
}
|
|
}
|
|
|
|
|
|
public bool Comandar { get; set; } = false;
|
|
public bool AlarmeSensores { get; set; } = false;
|
|
public bool Inicializado
|
|
{
|
|
get
|
|
{
|
|
return Leitura?.Iniciado ?? false;
|
|
}
|
|
}
|
|
private OIDModel _leitura = null;
|
|
public OIDModel Leitura
|
|
{
|
|
get
|
|
{
|
|
_leitura = OIDService.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)
|
|
{
|
|
var simulando = Variaveis.OperacaoEmAndamento.Simulando || Variaveis.OperacaoEmAndamento.Treinando;
|
|
|
|
bool Freado = DirecaoAtual == Direcao.EmFreio;
|
|
(bool AtuarServo, int AnguloFreio) = ConfigFreio.AtualizarDadosFreio(Freado, 0, -1, 5);
|
|
|
|
if ((Comandar && Inicializado) || simulando)
|
|
{
|
|
if (AlarmeSensores || AlarmeMotor)
|
|
{
|
|
DirecaoAtual = Direcao.Parado;
|
|
Freado = false;
|
|
(AtuarServo, AnguloFreio) = ConfigFreio.AtualizarDadosFreio(Freado, 0, -1, 5);
|
|
|
|
if (AlarmeMotor)
|
|
{
|
|
OIDService.RegisrarErrosComunicacao(EnderecoByte, $"Módulo com código de alarme {CodigoAlarme}, reiniciando o módulo...");
|
|
OIDService.AdicionarComandoNaFila(new OIDComandoModel()
|
|
{
|
|
Endereco = EnderecoByte,
|
|
AguardaResposta = true,
|
|
TimeoutResposta = 500,
|
|
Get = false,
|
|
Parametro = OIDService.Parametros.First(x => x.Chave == "CodErro"),
|
|
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;
|
|
|
|
Variaveis.OperacaoEmAndamento.SimulacaoRpmControle = (RPM_SP / VariaveisEquipamento.RelacaoRPM);
|
|
|
|
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;
|
|
}
|
|
|
|
if (simulando)
|
|
{
|
|
return;
|
|
}
|
|
|
|
byte Addr = EnderecoByte;
|
|
DateTime Agora = DateTime.Now;
|
|
|
|
bool SentidoAlterado = Sentido_SP != Sentido.Parado && (Leitura == null || Leitura.Direcao != Sentido_SP);
|
|
bool RPMAlterado = ComandoLigado && (Leitura == null || Leitura.ControleVelocidade != RPM_SP_Controle);
|
|
bool LigadoAlterado = Leitura == null || ComandoLigado != LeituraLigado;
|
|
bool FreioAlterado = Leitura == null || Leitura.Freado != Freado;
|
|
bool EnviaComando = SentidoAlterado || RPMAlterado || LigadoAlterado;
|
|
|
|
OIDModoControle modoControle =
|
|
Freado ? OIDModoControle.Freio :
|
|
ComandoLigado ? OIDModoControle.Velocidade :
|
|
OIDModoControle.Corrente;
|
|
|
|
if (Leitura.ModoControle != modoControle)
|
|
{
|
|
OIDService.AdicionarComandoNaFila(new OIDComandoModel()
|
|
{
|
|
Endereco = Addr,
|
|
Parametro = OIDService.Parametros.First(x => x.Chave == "ModoControle"),
|
|
Valor = (int)modoControle,
|
|
Get = false,
|
|
AguardaResposta = false,
|
|
TimeoutResposta = 50,
|
|
});
|
|
}
|
|
|
|
if (EnviaComando && !Freado)
|
|
{
|
|
int mx =
|
|
Sentido_SP == Sentido.Horario ? 1 :
|
|
Sentido_SP == Sentido.Antihorario ? -1 :
|
|
0;
|
|
float velocidade = RPM_SP * mx;
|
|
|
|
string regisro = modoControle == OIDModoControle.Velocidade ? "ControleVelocidade" : "ControleCorrente";
|
|
|
|
OIDService.AdicionarComandoNaFila(new OIDComandoModel()
|
|
{
|
|
Endereco = Addr,
|
|
Parametro = OIDService.Parametros.First(x => x.Chave == regisro),
|
|
Valor = velocidade,
|
|
Get = false,
|
|
AguardaResposta = false,
|
|
TimeoutResposta = 50,
|
|
});
|
|
}
|
|
}
|
|
|
|
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 string Mod_ID
|
|
{
|
|
get
|
|
{
|
|
return Variaveis.OperacaoEmAndamento.DispMvd?.Dados?.Modulos?.FirstOrDefault(x => x.DirMotor.EnderecoByte == EnderecoByte)?.Modulo_ID ?? "";
|
|
}
|
|
}
|
|
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 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 FirmwareSensorPotenciometro SensorAnguloAuxiliar
|
|
{
|
|
get
|
|
{
|
|
var sensor = Variaveis.OperacaoEmAndamento.DispSen?.Dados?.Sensores?.FirstOrDefault(x => x.Componente == S_Code.sPOT && x.ID.Contains(Mod_ID));
|
|
if (sensor != null && sensor.Inicializado && sensor.Leitura != null)
|
|
{
|
|
return sensor.Leitura as FirmwareSensorPotenciometro;
|
|
}
|
|
return null;
|
|
}
|
|
}
|
|
|
|
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
|
|
{
|
|
double anguloSp = Comandar ? Angulo_SP : 0;
|
|
return FuncoesMatematicas.ValorEstaEntre(Angulo, anguloSp, 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)
|
|
{
|
|
var _Simulando = Variaveis.OperacaoEmAndamento.Simulando;
|
|
|
|
if ((Inicializado && (Comandar || !AtingiuAnguloSP)) || _Simulando)
|
|
{
|
|
var _Controle = Variaveis.OperacaoEmAndamento.Controle;
|
|
int AnguloTipoMovimento = VariaveisEquipamento.AnguloMovimento[_Controle.TipoMovimento];
|
|
|
|
bool limiteLateralExcedido = SensorAnguloAuxiliar?.LimiteExcedido ?? false;
|
|
|
|
if (UltimaDirecao != Direcao)
|
|
{
|
|
Sentido_SP =
|
|
Direcao == Direcao.Direita ? ConfigSentidos[_Controle.TipoMovimento].Direita :
|
|
Direcao == Direcao.Esquerda ? ConfigSentidos[_Controle.TipoMovimento].Esquerda :
|
|
Sentido_SP == Sentido.Horario ? Sentido.Antihorario :
|
|
Sentido_SP == Sentido.Antihorario ? Sentido.Horario :
|
|
Sentido.Parado;
|
|
|
|
if (!limiteLateralExcedido)
|
|
{
|
|
UltimaDirecao = Direcao;
|
|
}
|
|
}
|
|
|
|
if (RetornandoAzero || !Comandar || limiteLateralExcedido)
|
|
{
|
|
Angulo_SP = 0;
|
|
Sentido_SP = Sentido.Parado;
|
|
UltimaDirecao = Direcao.Parado;
|
|
}
|
|
else if (AnguloTipoMovimento != -1)
|
|
{
|
|
Angulo_SP = AnguloTipoMovimento;
|
|
}
|
|
else
|
|
{
|
|
Angulo_SP = _Controle.Angulo;
|
|
}
|
|
|
|
if (Comandar)
|
|
{
|
|
Variaveis.OperacaoEmAndamento.SimulacaoAnguloControle = Angulo_SP;
|
|
}
|
|
|
|
int mx = Sentido_SP == Sentido.Antihorario ? -1 : 1;
|
|
|
|
Angulo_SP = Math.Abs(Angulo_SP) * mx;
|
|
|
|
if (_Simulando)
|
|
{
|
|
return;
|
|
}
|
|
|
|
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;
|
|
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;
|
|
|
|
Sentido _sentido = Sentido.Horario;
|
|
if (SensorAnguloAuxiliar != null && SensorAnguloAuxiliar.Sentido != Sentido.Parado)
|
|
{
|
|
_sentido = SensorAnguloAuxiliar.Sentido == Sentido.Horario ? Sentido.Antihorario : Sentido.Horario;
|
|
}
|
|
|
|
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;
|
|
|
|
// 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));
|
|
if (!inverterGiro && SensorAnguloAuxiliar != null)
|
|
{
|
|
if (SensorAnguloAuxiliar.Angulo < (SensorAnguloAuxiliar.LimiteMinimo / 2.0) || SensorAnguloAuxiliar.Angulo > (SensorAnguloAuxiliar.LimiteMaximo / 2.0))
|
|
{
|
|
inverterGiro = true;
|
|
}
|
|
}
|
|
}
|
|
|
|
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 double Potencia { get; set; }
|
|
public double CicloTrabalho { 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.DispMvd.Dados.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);
|
|
}
|
|
}
|
|
|
|
|
|
|
|
}
|