454 lines
17 KiB
C#
454 lines
17 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 DirecionalModel
|
|
{
|
|
public string ID { get; set; } = T_Code.Dir.ToString();
|
|
|
|
// SETs
|
|
public byte _EnderecoCAN_Tx { get; set; }
|
|
public byte _EnderecoCAN_Rx { get; set; }
|
|
public string Mod_ID
|
|
{
|
|
get
|
|
{
|
|
return Variaveis.OperacaoEmAndamento.DispMvd?.Dados?.Modulos?.FirstOrDefault(x => x.DirMotor._EnderecoCAN_Tx == _EnderecoCAN_Tx)?.Modulo_ID ?? "";
|
|
}
|
|
}
|
|
public Sentido Sentido_SP { get; set; } = Sentido.Parado;
|
|
public double Angulo_SP { get; set; } = 0;
|
|
public double FolgaMecanica { 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
|
|
Angulo > 0 ? Sentido.Horario :
|
|
Angulo < 0 ? Sentido.Antihorario :
|
|
Sentido.Parado;
|
|
}
|
|
}
|
|
public Sentido SentidoReal
|
|
{
|
|
get
|
|
{
|
|
if (Mod_ID == "EF" || Mod_ID == "DT")
|
|
{
|
|
return IN1_Atuado ? Sentido.Antihorario : Sentido.Horario;
|
|
}
|
|
else
|
|
{
|
|
return IN1_Atuado ? Sentido.Horario : Sentido.Antihorario;
|
|
}
|
|
}
|
|
}
|
|
public double Angulo
|
|
{
|
|
get
|
|
{
|
|
return Leitura?.AnguloPulsos ?? 0;
|
|
}
|
|
}
|
|
public double RPM
|
|
{
|
|
get
|
|
{
|
|
return Leitura?.RPM?.valor ?? 0;
|
|
}
|
|
}
|
|
public bool IN1_Atuado
|
|
{
|
|
get
|
|
{
|
|
return !(Leitura?.Entradas?[0] ?? false);
|
|
}
|
|
}
|
|
public bool IN2_Atuado
|
|
{
|
|
get
|
|
{
|
|
return !(Leitura?.Entradas?[1] ?? false);
|
|
}
|
|
}
|
|
public bool Referenciando { get; set; } = false;
|
|
public DateTime UltimoReferenciamento { get; set; }
|
|
public bool Referenciado { get; set; } = false;
|
|
|
|
public bool Comandar { get; set; } = false;
|
|
public bool Inicializado
|
|
{
|
|
get
|
|
{
|
|
return Leitura?.Iniciado ?? false;
|
|
}
|
|
}
|
|
private MKS057DModel _leitura = null;
|
|
public MKS057DModel Leitura
|
|
{
|
|
get
|
|
{
|
|
_leitura = MKS057DCanService.DadosLeitura?.FirstOrDefault(x => x.EnderecoTx == _EnderecoCAN_Tx);
|
|
return _leitura;
|
|
}
|
|
set
|
|
{
|
|
_leitura = value;
|
|
}
|
|
}
|
|
|
|
public Direcao UltimaDirecao { get; set; } = Direcao.Parado;
|
|
public DateTime UltimoComandoEnviado { get; set; } = DateTime.MinValue;
|
|
public bool RetornandoAzero
|
|
{
|
|
get
|
|
{
|
|
bool foraZero = !FuncoesMatematicas.ValorEstaEntre(Angulo, 0, 1);
|
|
bool parar = UltimaDirecao == Direcao.Parado;
|
|
return foraZero && parar;
|
|
}
|
|
}
|
|
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 DirecionalModel CarregarParametrosIniciais(string Mod_ID, byte _EnderecoTx, byte _EnderecoRx)
|
|
{
|
|
return new DirecionalModel()
|
|
{
|
|
ID = T_Code.Dir.ToString(),
|
|
_EnderecoCAN_Tx = _EnderecoTx,
|
|
_EnderecoCAN_Rx = _EnderecoRx,
|
|
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];
|
|
|
|
if (!Comandar)
|
|
{
|
|
Sentido_SP = Sentido.Parado;
|
|
}
|
|
else if (Direcao == Direcao.Direita)
|
|
{
|
|
Sentido_SP = ConfigSentidos[_Controle.TipoMovimento].Direita;
|
|
}
|
|
else if (Direcao == Direcao.Esquerda)
|
|
{
|
|
Sentido_SP = ConfigSentidos[_Controle.TipoMovimento].Esquerda;
|
|
}
|
|
else
|
|
{
|
|
Sentido_SP = Sentido.Parado;
|
|
}
|
|
|
|
UltimaDirecao = Direcao;
|
|
|
|
if (Sentido_SP == Sentido.Parado)
|
|
{
|
|
Angulo_SP = 0;
|
|
UltimaDirecao = Direcao.Parado;
|
|
}
|
|
else if (AnguloTipoMovimento != -1)
|
|
{
|
|
Angulo_SP = AnguloTipoMovimento;
|
|
}
|
|
else
|
|
{
|
|
Angulo_SP = _Controle.Angulo + (FolgaMecanica / 2.0);
|
|
}
|
|
|
|
if (Comandar)
|
|
{
|
|
Variaveis.OperacaoEmAndamento.SimulacaoAnguloControle = Variaveis.OperacaoEmAndamento.Controle.Angulo;
|
|
//Console.WriteLine($"[{Mod_ID}] Angulo Atualizado para {Variaveis.OperacaoEmAndamento.SimulacaoAnguloControle}");
|
|
}
|
|
|
|
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));
|
|
MKS057DCanService.Movimento(_EnderecoCAN_Tx, _EnderecoCAN_Rx, Angulo_SP, Velocidade, Sentido_SP);
|
|
UltimoComandoEnviado = DateTime.Now;
|
|
}
|
|
|
|
}
|
|
|
|
public async Task ReferenciaMotor()
|
|
{
|
|
Referenciando = true;
|
|
|
|
await Task.Delay(1000);
|
|
|
|
Referenciado = false;
|
|
|
|
await SetZero();
|
|
|
|
bool Sucesso = false;
|
|
bool EstadoInicial = IN1_Atuado;
|
|
double AnguloEntrada = 9999;
|
|
double AnguloSaida = 9999;
|
|
int verificacoesAngulo = 0;
|
|
double anguloAnterior = 0;
|
|
|
|
DateTime TimeoutGeral = DateTime.Now.AddMinutes(2); // Timeout geral para o processo.
|
|
|
|
Sentido _sentido = SentidoReal == Sentido.Horario ? Sentido.Antihorario : Sentido.Horario;
|
|
double _anguloSP = 360 * (_sentido == Sentido.Antihorario ? -1 : 1);
|
|
double _anguloSpAnterior = 0;
|
|
|
|
while (!Sucesso && TimeoutGeral > DateTime.Now && Inicializado)
|
|
{
|
|
int VelocidadeGiro = DefineVelocidadeReferenciamento();
|
|
|
|
if (_anguloSP != _anguloSpAnterior || (verificacoesAngulo > 5 && Angulo == anguloAnterior))
|
|
{
|
|
MKS057DCanService.Movimento(_EnderecoCAN_Tx, _EnderecoCAN_Rx, _anguloSP, VelocidadeGiro, _sentido);
|
|
_anguloSpAnterior = _anguloSP;
|
|
verificacoesAngulo = 0;
|
|
}
|
|
|
|
if (IN1_Atuado != EstadoInicial && AnguloEntrada == 9999)
|
|
{
|
|
AnguloEntrada = Leitura.AnguloEstimado();
|
|
Console.WriteLine($"[DIR {Mod_ID}] - Angulo de Entrada Definido: {AnguloEntrada} (Leitura: {Angulo})");
|
|
|
|
_sentido = _sentido == Sentido.Horario ? Sentido.Antihorario : Sentido.Horario;
|
|
_anguloSP *= -1;
|
|
Console.WriteLine($"[DIR {Mod_ID}] - Invertendo Sentido de Giro para {_sentido.ToString()}");
|
|
}
|
|
if (IN1_Atuado == EstadoInicial && AnguloEntrada != 9999 && AnguloSaida == 9999)
|
|
{
|
|
AnguloSaida = Leitura.AnguloEstimado();
|
|
Console.WriteLine($"[DIR {Mod_ID}] - Angulo de Saída Definido: {AnguloSaida} (Leitura: {Angulo})");
|
|
|
|
_sentido = _sentido == Sentido.Horario ? Sentido.Antihorario : Sentido.Horario;
|
|
Console.WriteLine($"[DIR {Mod_ID}] - Invertendo Sentido de Giro para {_sentido.ToString()}");
|
|
|
|
_anguloSP = CalcularCentroCompensado(AnguloEntrada, AnguloSaida, _sentido);
|
|
}
|
|
|
|
verificacoesAngulo++;
|
|
anguloAnterior = Angulo;
|
|
|
|
Sucesso = AnguloEntrada != 9999 && AnguloSaida != 9999 && (FuncoesMatematicas.ValorEstaEntre(Leitura.AnguloEstimado(), _anguloSP, 0.5));
|
|
if (Sucesso)
|
|
{
|
|
await SetZero();
|
|
Console.WriteLine($"[DIR {Mod_ID}] - Referenciamento Concluído!");
|
|
Referenciado = true;
|
|
}
|
|
|
|
DateTime Inicio = DateTime.Now;
|
|
bool NovoDado() => MKS057DCanService.DadosLeitura.Any(x => x.EnderecoRx == _EnderecoCAN_Rx && x.UltimoComandoRecebido > Inicio);
|
|
await FuncoesGlobais.AguardarCondicaoAsync(NovoDado, 500, 100);
|
|
}
|
|
|
|
Referenciando = false;
|
|
|
|
UltimoReferenciamento = DateTime.Now;
|
|
}
|
|
|
|
private int DefineVelocidadeReferenciamento()
|
|
{
|
|
int VelocidadeGiro = 10;
|
|
return VelocidadeGiro;
|
|
}
|
|
|
|
private double CalcularCentroCompensado(double entrada, double saida, Sentido sentido)
|
|
{
|
|
double centro = (entrada + saida) / 2.0;
|
|
double folga = Math.Abs(entrada - saida);
|
|
|
|
// Determina o sentido do giro baseado na diferença
|
|
double direcao = sentido == Sentido.Horario ? -1 : 1;
|
|
|
|
double compensacao = 0; // (folga / 2.0) * direcao;
|
|
|
|
double centroCompensado = centro + compensacao;
|
|
|
|
Console.WriteLine($"[DIR {Mod_ID}] - Folga: {folga}, Entrada: {entrada}, Saida: {saida}, Centro: {centro}, Compensacao: {compensacao}, Centro Compensado: {centroCompensado}");
|
|
|
|
FolgaMecanica = folga;
|
|
|
|
return centroCompensado;
|
|
}
|
|
|
|
public async Task SetZero()
|
|
{
|
|
MKS057DCanService.SetZero(_EnderecoCAN_Tx, _EnderecoCAN_Rx);
|
|
|
|
for (int i = 0; i < 5; i++)
|
|
{
|
|
DateTime Inicio = DateTime.Now;
|
|
bool NovoDado() => MKS057DCanService.DadosLeitura.Any(x => x.EnderecoRx == _EnderecoCAN_Rx && x.UltimoComandoRecebido > Inicio);
|
|
await FuncoesGlobais.AguardarCondicaoAsync(NovoDado, 500, 100);
|
|
|
|
if (FuncoesMatematicas.ValorEstaEntre(Angulo, -1, 1))
|
|
{
|
|
Console.WriteLine($"[DIR {Mod_ID}] - Set Zero");
|
|
break;
|
|
}
|
|
else
|
|
{
|
|
MKS057DCanService.SetZero(_EnderecoCAN_Tx, _EnderecoCAN_Rx);
|
|
}
|
|
}
|
|
|
|
MKS057DCanService.Movimento(_EnderecoCAN_Tx, _EnderecoCAN_Rx, 0, 10, Sentido.Parado);
|
|
Console.WriteLine($"[DIR {Mod_ID}] - Comando Angulo Zero: {Angulo}");
|
|
}
|
|
|
|
}
|
|
}
|