506 lines
20 KiB
C#
506 lines
20 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 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
|
|
Angulo > 0 ? Sentido.Horario :
|
|
Angulo < 0 ? Sentido.Antihorario :
|
|
Sentido.Parado;
|
|
}
|
|
}
|
|
public Sentido SentidoReal
|
|
{
|
|
get
|
|
{
|
|
return IN1_Atuado ? Sentido.Horario : Sentido.Antihorario;
|
|
}
|
|
}
|
|
public double Angulo
|
|
{
|
|
get
|
|
{
|
|
return Leitura?.AnguloPulsos ?? 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 = MKS057DCanService.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
|
|
{
|
|
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, string _Endereco)
|
|
{
|
|
return new DirecionalModel()
|
|
{
|
|
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];
|
|
|
|
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;
|
|
}
|
|
|
|
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));
|
|
MKS057DCanService.Movimento(EnderecoByte, Angulo_SP, Velocidade, Sentido_SP);
|
|
UltimoComandoEnviado = DateTime.Now;
|
|
}
|
|
|
|
}
|
|
|
|
public async Task ReferenciaMotor()
|
|
{
|
|
byte Addr = EnderecoByte;
|
|
|
|
MKS057DCanService.AddrRef = Addr;
|
|
MKS057DCanService.UltimoReferenciamento = DateTime.Now;
|
|
|
|
await Task.Delay(2000);
|
|
|
|
//await MKS057DService.EnviarComandoControle(MKS057DService.MontarComandoSetZero(Addr));
|
|
MKS057DCanService.SetZero(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 (SentidoReal != Sentido.Parado)
|
|
{
|
|
_sentido = SentidoReal == Sentido.Horario ? Sentido.Antihorario : Sentido.Horario;
|
|
_angulo *= _sentido == Sentido.Antihorario ? -1 : 1;
|
|
}
|
|
|
|
if (!Sucesso)
|
|
{
|
|
while (!Sucesso && tentativas < MaxTentativas && DateTime.Now < TimeoutGeral && MKS057DCanService.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));
|
|
MKS057DCanService.Movimento(Addr, _angulo, VelocidadeGiro, _sentido);
|
|
|
|
DateTime Inicio = DateTime.Now;
|
|
bool inverterGiro = false;
|
|
while (!Sucesso && !inverterGiro && Inicio.AddSeconds(120) > DateTime.Now && MKS057DCanService.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));
|
|
}
|
|
|
|
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));
|
|
MKS057DCanService.Parar(Addr);
|
|
await Task.Delay(1500);
|
|
|
|
int _centro = Convert.ToInt32((_anguloEntrada + _anguloSaida) / 2.0);
|
|
|
|
//await MKS057DService.EnviarComandoControle(MKS057DService.MontarComandoMovimento(Addr, _centro, 36, Sentido.Horario));
|
|
MKS057DCanService.Movimento(Addr, _centro, 36, Sentido.Horario);
|
|
await Task.Delay(2500);
|
|
//await MKS057DService.EnviarComandoControle(MKS057DService.MontarComandoSetZero(Addr));
|
|
MKS057DCanService.SetZero(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));
|
|
MKS057DCanService.Movimento(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));
|
|
MKS057DCanService.Parar(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));
|
|
MKS057DCanService.Parar(Addr);
|
|
}
|
|
|
|
MKS057DCanService.AddrRef = 0x00;
|
|
MKS057DCanService.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;
|
|
}
|
|
|
|
}
|
|
}
|