835 lines
30 KiB
C#
835 lines
30 KiB
C#
using AgroBase.Models;
|
|
using System;
|
|
using System.Collections;
|
|
using System.Collections.Generic;
|
|
using System.IO;
|
|
using System.Linq;
|
|
using System.Threading.Tasks;
|
|
using System.Windows.Forms;
|
|
using static AgroBase.Models.Enums;
|
|
|
|
namespace AgroBase.Services
|
|
{
|
|
public class MKS057DService
|
|
{
|
|
private static EthernetService _ethernetService;
|
|
public static bool Iniciado
|
|
{
|
|
get
|
|
{
|
|
return _ethernetService != null && _ethernetService.IsConnected && SerialService.DispositivosMapeados.Any(x => x.Dispositivo == T_Code.Dir) && DadosLeitura.Any(x => x.Iniciado);
|
|
}
|
|
}
|
|
public static int TaxaAmostragem { get; set; } = 1000;
|
|
private static AsyncTaskTimerModel tmrLoopComandos;
|
|
private static List<MKS057ComandoModel> FilaComandos = new List<MKS057ComandoModel>();
|
|
private static readonly object FilaLock = new object();
|
|
private static bool EnvioLiberado { get; set; } = true;
|
|
public static List<MKS057DModel> DadosLeitura = new List<MKS057DModel>();
|
|
public static bool FilaEmEspera = false;
|
|
public static byte AddrRef = 0x00;
|
|
public static bool Referenciando
|
|
{
|
|
get
|
|
{
|
|
return AddrRef > 0x00;
|
|
}
|
|
}
|
|
public static DateTime UltimoReferenciamento = DateTime.MinValue;
|
|
public static int AnguloOffsetSensor = 8;
|
|
|
|
public static void IniciarRotinas()
|
|
{
|
|
PararRotinas();
|
|
|
|
tmrLoopComandos = new AsyncTaskTimerModel("tmrLoopComandos", tmrLoopComandos_Tick, 100);
|
|
tmrLoopComandos.Start();
|
|
}
|
|
|
|
public static void PararRotinas()
|
|
{
|
|
tmrLoopComandos?.Dispose();
|
|
}
|
|
|
|
// Inicializa o serviço Ethernet com IP e porta específicos
|
|
public static async Task<bool> InicializarEthernetService()
|
|
{
|
|
string ipAddress = Variaveis.OperacaoEmAndamento.DispMvd.Dados.IP_Dir;
|
|
int port = VariaveisPortas.Ethernet_TCP;
|
|
|
|
_ethernetService?.Disconnect();
|
|
|
|
await Task.Delay(1000);
|
|
|
|
_ethernetService = new EthernetService(ipAddress, port);
|
|
return await _ethernetService.ConnectAsync();
|
|
}
|
|
|
|
public static async Task<bool> VerificaPortaMKS()
|
|
{
|
|
EnvioLiberado = false;
|
|
|
|
bool connect = await InicializarEthernetService();
|
|
if (!connect)
|
|
{
|
|
Console.WriteLine("Falha ao inicializar o serviço Ethernet.");
|
|
_ethernetService = null;
|
|
EnvioLiberado = true;
|
|
return false;
|
|
}
|
|
|
|
if (_ethernetService._isReconnecting)
|
|
{
|
|
return false;
|
|
}
|
|
|
|
if (!Iniciado && !await _ethernetService.CheckConnectionAsync())
|
|
{
|
|
Console.WriteLine("Falha ao conectar ao dispositivo via Ethernet.");
|
|
EnvioLiberado = true;
|
|
return false;
|
|
}
|
|
|
|
List<string> Mod_IDs = new List<string>();
|
|
|
|
// Verifica e inicializa cada módulo conectado
|
|
foreach (var Modulo in Variaveis.OperacaoEmAndamento.DispMvd.Dados.Modulos.Where(x => !x.DirMotor.Inicializado))
|
|
{
|
|
byte Addr = Modulo.DirMotor.EnderecoByte;
|
|
|
|
// Envia comando relevante para verificar o dispositivo
|
|
bool envioBemSucedido = await EnviarComandoDriver(MontarComandoRequisicaoVersao(Addr));
|
|
if (!envioBemSucedido)
|
|
{
|
|
Console.WriteLine($"Falha ao enviar dados para o dispositivo de endereço {Addr}.");
|
|
continue;
|
|
}
|
|
|
|
//var resposta = await _ethernetService.ReceiveDataAsync(8);
|
|
var resposta = await _ethernetService.ReceiveDataAsync(8);
|
|
|
|
//SerialService.MostrarComandoResposta(T_Code.Mks, comando, resposta);
|
|
|
|
DecifrarRespostaDriver(resposta);
|
|
|
|
if (DadosLeitura.Any(x => x.Endereco == Addr && x.Iniciado))
|
|
{
|
|
Mod_IDs.Add(Modulo.Modulo_ID);
|
|
}
|
|
}
|
|
|
|
// Define os dispositivos inicializados na lista de dispositivos mapeados
|
|
if (Mod_IDs.Any())
|
|
{
|
|
DefinirDispositivo(Mod_IDs);
|
|
}
|
|
|
|
EnvioLiberado = true;
|
|
return Iniciado;
|
|
}
|
|
|
|
private static void RegisrarErrosComunicacao(byte addr, string mensagem)
|
|
{
|
|
try
|
|
{
|
|
var Caminho = Variaveis.CaminhoLogsDispositivos;
|
|
if (!Directory.Exists(Caminho))
|
|
{
|
|
Directory.CreateDirectory(Caminho);
|
|
}
|
|
|
|
var Arquivo = T_Code.Mks.ToString() + "_err.txt";
|
|
Caminho += "/" + Arquivo;
|
|
|
|
mensagem = DateTime.Now.ToString("dd/MM/yyyy HH:mm:ss:fff") + " - Endereço " + addr + " (" + _ethernetService._ipAddress + "): " + mensagem + Environment.NewLine;
|
|
|
|
if (!File.Exists(Caminho))
|
|
{
|
|
File.WriteAllText(Caminho, mensagem);
|
|
}
|
|
else
|
|
{
|
|
File.AppendAllText(Caminho, mensagem);
|
|
}
|
|
}
|
|
catch
|
|
{
|
|
Console.WriteLine("Erro ao salvar log de erro MKS");
|
|
}
|
|
}
|
|
|
|
public static void RegisrarComandosComunicacao(byte addr, string mensagem)
|
|
{
|
|
try
|
|
{
|
|
var Caminho = Variaveis.CaminhoLogsDispositivos;
|
|
if (!Directory.Exists(Caminho))
|
|
{
|
|
Directory.CreateDirectory(Caminho);
|
|
}
|
|
|
|
var Arquivo = T_Code.Mks.ToString() + "_log.txt";
|
|
Caminho += "/" + Arquivo;
|
|
|
|
mensagem = DateTime.Now.ToString("dd/MM/yyyy HH:mm:ss:fff") + " - Endereço " + addr + " (" + _ethernetService._ipAddress + "): " + mensagem + Environment.NewLine;
|
|
|
|
if (!File.Exists(Caminho))
|
|
{
|
|
File.WriteAllText(Caminho, mensagem);
|
|
}
|
|
else
|
|
{
|
|
File.AppendAllText(Caminho, mensagem);
|
|
}
|
|
}
|
|
catch
|
|
{
|
|
Console.WriteLine("Erro ao salvar log de comunicacao MKS");
|
|
}
|
|
}
|
|
|
|
private static void DefinirDispositivo(List<string> Mod_IDs)
|
|
{
|
|
if (!SerialService.DispositivosMapeados.Any(x => x.Dispositivo == T_Code.Dir))
|
|
{
|
|
IniciarRotinas();
|
|
}
|
|
|
|
foreach (var Mod_ID in Mod_IDs)
|
|
{
|
|
var Modulo = Variaveis.OperacaoEmAndamento.DispMvd.Dados.Modulos.FirstOrDefault(x => x.Modulo_ID == Mod_ID);
|
|
byte Addr = Modulo.DirMotor.EnderecoByte;
|
|
|
|
if (!SerialService.DispositivosMapeados.Any(x => x.Dispositivo == T_Code.Dir && x.Mod_ID == Mod_ID))
|
|
{
|
|
var Leitura = DadosLeitura.FirstOrDefault(x => x.Endereco == Addr);
|
|
if (Leitura != null)
|
|
{
|
|
SerialService.DispositivosMapeados.Add(new DispositivoDetalhesModel()
|
|
{
|
|
Endereco = _ethernetService != null ? _ethernetService._ipAddress : "N/A",
|
|
Dispositivo = T_Code.Dir,
|
|
Erro = (Leitura.Mensagem ?? "") != "",
|
|
Versao = (Leitura.FV ?? "").ToString(),
|
|
Mod_ID = Mod_ID,
|
|
});
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
private static bool EnderecoValido(byte addr)
|
|
{
|
|
return Variaveis.OperacaoEmAndamento.DispMvd.Dados.Modulos.Any(x => x.DirMotor.EnderecoByte == addr);
|
|
}
|
|
|
|
|
|
public static void AdicionarComandoNaFila(MKS057ComandoModel Comando)
|
|
{
|
|
if (!EnderecoValido(Comando.Endereco))
|
|
{
|
|
return;
|
|
}
|
|
|
|
lock (FilaLock)
|
|
{
|
|
if (!FilaComandos.Any(x => x.Endereco == Comando.Endereco && x.Comando == Comando.Comando && x.Get == Comando.Get && !x.Enviado))
|
|
{
|
|
FilaComandos.Add(Comando);
|
|
}
|
|
}
|
|
}
|
|
|
|
private static async Task tmrLoopComandos_Tick()
|
|
{
|
|
if (!EnvioLiberado)
|
|
{
|
|
return;
|
|
}
|
|
|
|
EnvioLiberado = false;
|
|
|
|
// Verifica se a conexão foi perdida e tenta reconectar
|
|
if (_ethernetService != null && !_ethernetService.IsConnected)
|
|
{
|
|
if (!await _ethernetService.ReconnectAsync())
|
|
{
|
|
EnvioLiberado = true;
|
|
return;
|
|
}
|
|
}
|
|
|
|
try
|
|
{
|
|
await ProcessarFilaPrioridade();
|
|
|
|
List<MKS057ComandoModel> comandosParaEnviar;
|
|
|
|
lock (FilaLock)
|
|
{
|
|
comandosParaEnviar = FilaComandos.Where(x => !x.Enviado && x.Get).OrderBy(x => x.Momento).ToList();
|
|
}
|
|
|
|
foreach (var Comando in comandosParaEnviar)
|
|
{
|
|
await EnviarComandoControle(Comando);
|
|
await ProcessarFilaPrioridade();
|
|
}
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
RegisrarErrosComunicacao(0, $"Erro ao processar fila de comandos. Erro={ex.Message}");
|
|
}
|
|
finally
|
|
{
|
|
EnvioLiberado = true;
|
|
}
|
|
}
|
|
|
|
private static async Task ProcessarFilaPrioridade()
|
|
{
|
|
var FilaPrioridade = new List<MKS057ComandoModel>();
|
|
lock (FilaLock)
|
|
{
|
|
FilaPrioridade = FilaComandos.Where(x => !x.Enviado && !x.Get).OrderBy(x => x.Momento).ToList();
|
|
}
|
|
|
|
while (FilaPrioridade.Count > 0)
|
|
{
|
|
foreach (var Comando in FilaPrioridade)
|
|
{
|
|
await EnviarComandoControle(Comando);
|
|
}
|
|
|
|
lock (FilaLock)
|
|
{
|
|
FilaPrioridade = FilaComandos.Where(x => !x.Enviado && !x.Get).OrderBy(x => x.Momento).ToList();
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
|
|
#region Bytes
|
|
|
|
// Bytes de função
|
|
static readonly string[] HardVersion = { "未知电机", "MKS SERVO42D_485", "MKS SERVO42D_CAN", "MKS SERVO57D_485", "MKS SERVO57D_CAN", "MKS SERVO28D_485", "MKS SERVO28D_CAN", "MKS SERVO35D_485", "MKS SERVO35D_CAN" };
|
|
static byte FWD = 0x80;
|
|
static byte RWD = 0x00;
|
|
static byte[] AceleracaoInicio = { 0x00 };
|
|
static byte[] AceleracaoParada = { 0x00 };
|
|
static ushort Max_Current;
|
|
|
|
#endregion
|
|
|
|
#region Parametros
|
|
|
|
private static string ModoLeitura { get; set; } = "Decimal";
|
|
public static string _MStep { get; set; } = "16";
|
|
public static ModosControle _ModoControle { get; set; } = ModosControle.AnguloAbsoluto;
|
|
|
|
#endregion
|
|
|
|
|
|
|
|
|
|
public static MKS057ComandoModel MontarComandoRequisicaoVersao(byte Endereco)
|
|
{
|
|
return new MKS057ComandoModel()
|
|
{
|
|
Get = true,
|
|
Registro = 0x40,
|
|
ModoComando = ModosComando.GetVersao,
|
|
Endereco = Endereco,
|
|
AguardaResposta = true,
|
|
TamanhoResposta = 8,
|
|
};
|
|
}
|
|
|
|
public static MKS057ComandoModel MontarComandoRequisicaoPulsos(byte Endereco)
|
|
{
|
|
return new MKS057ComandoModel()
|
|
{
|
|
Get = true,
|
|
Comando = Convert.ToByte("1"),
|
|
Registro = 0x33,
|
|
ModoComando = ModosComando.GetPulsos,
|
|
Endereco = Endereco,
|
|
AguardaResposta = true,
|
|
TamanhoResposta = 8,
|
|
};
|
|
}
|
|
|
|
public static MKS057ComandoModel MontarComandoRequisicaoIO(byte Endereco)
|
|
{
|
|
return new MKS057ComandoModel()
|
|
{
|
|
Get = true,
|
|
Comando = Convert.ToByte("46"),
|
|
Registro = 0x34,
|
|
ModoComando = ModosComando.GetIO,
|
|
Endereco = Endereco,
|
|
AguardaResposta = true,
|
|
TamanhoResposta = 5,
|
|
};
|
|
}
|
|
|
|
public static MKS057ComandoModel MontarComandoRequisicaoRPM(byte Endereco)
|
|
{
|
|
return new MKS057ComandoModel()
|
|
{
|
|
Get = true,
|
|
Comando = Convert.ToByte("40"),
|
|
Registro = 0x32,
|
|
ModoComando = ModosComando.GetRPM,
|
|
Endereco = Endereco,
|
|
AguardaResposta = true,
|
|
TamanhoResposta = 5,
|
|
};
|
|
}
|
|
|
|
public static MKS057ComandoModel MontarComandoSetZero(byte Endereco)
|
|
{
|
|
return new MKS057ComandoModel()
|
|
{
|
|
Get = false,
|
|
Comando = Convert.ToByte("33"),
|
|
Registro = 0x92,
|
|
ModoComando = ModosComando.SetZero,
|
|
Endereco = Endereco,
|
|
AguardaResposta = true,
|
|
Angulo = 0,
|
|
_Sentido = Sentido.Parado,
|
|
Velocidade = 1,
|
|
ModoControle = ModosControle.AnguloAbsoluto
|
|
};
|
|
}
|
|
|
|
public static MKS057ComandoModel MontarComandoParada(byte Endereco)
|
|
{
|
|
return new MKS057ComandoModel()
|
|
{
|
|
Get = false,
|
|
Comando = Convert.ToByte("38"),
|
|
Registro =
|
|
(byte)(_ModoControle == ModosControle.Pulsos ? 0xFD :
|
|
_ModoControle == ModosControle.AnguloRelativo ? 0xF4 :
|
|
_ModoControle == ModosControle.AnguloAbsoluto ? 0xF5 :
|
|
0xFD),
|
|
ModoComando = ModosComando.SetParada,
|
|
Endereco = Endereco,
|
|
AguardaResposta = true,
|
|
Angulo = 0,
|
|
_Sentido = Sentido.Parado,
|
|
Velocidade = 1,
|
|
ModoControle = ModosControle.AnguloAbsoluto
|
|
};
|
|
}
|
|
|
|
public static MKS057ComandoModel MontarComandoMovimento(byte Endereco, double Angulo_SP, int Velocidade, Sentido Sentido_SP, ModosControle Controle = ModosControle.AnguloAbsoluto)
|
|
{
|
|
return new MKS057ComandoModel()
|
|
{
|
|
Get = false,
|
|
Comando = Convert.ToByte("13"),
|
|
Registro =
|
|
(byte)(_ModoControle == ModosControle.Pulsos ? 0xFD :
|
|
_ModoControle == ModosControle.AnguloRelativo ? 0xF4 :
|
|
_ModoControle == ModosControle.AnguloAbsoluto ? 0xF5 :
|
|
0xFD),
|
|
ModoComando = ModosComando.SetMovimento,
|
|
Endereco = Endereco,
|
|
Angulo = Angulo_SP,
|
|
Velocidade = Velocidade,
|
|
_Sentido = Sentido_SP,
|
|
ModoControle = Controle,
|
|
AguardaResposta = true,
|
|
TamanhoResposta = 5,
|
|
};
|
|
}
|
|
|
|
private static byte CalcularChecksum(byte[] Comando)
|
|
{
|
|
return (byte)(Comando.Aggregate(0, (acc, b) => acc + b) % 256);
|
|
}
|
|
|
|
|
|
public static int ConverterAnguloParaValor(double _angulo)
|
|
{
|
|
int val = 0;
|
|
|
|
|
|
if (_ModoControle == ModosControle.Pulsos)
|
|
{
|
|
int.TryParse(_MStep, out int mstep);
|
|
double pulsos = mstep * 200.0;
|
|
double relacao = (pulsos * VariaveisEquipamento.ReducaoDirecional) / 360.0;
|
|
val = Convert.ToInt32(_angulo * relacao);
|
|
}
|
|
else
|
|
{
|
|
val = Convert.ToInt32(_angulo * VariaveisEquipamento.ReducaoDirecional);
|
|
}
|
|
|
|
return val;
|
|
}
|
|
|
|
public static double PulsosParaAngulo(int valor)
|
|
{
|
|
double reducao = VariaveisEquipamento.ReducaoDirecional;
|
|
|
|
int mstep = 16;
|
|
double pulsos = mstep * 200.0;
|
|
double relacao = (pulsos * reducao) / 360.0;
|
|
double angulo = valor / relacao;
|
|
|
|
return angulo;
|
|
}
|
|
|
|
|
|
public static async Task EnviarComandoControle(MKS057ComandoModel Comando, bool resend = false)
|
|
{
|
|
while (FilaEmEspera && !resend)
|
|
{
|
|
await Task.Delay(Comando.Get ? 50 : 10);
|
|
}
|
|
|
|
FilaEmEspera = true;
|
|
|
|
try
|
|
{
|
|
byte Addr = Comando.Endereco;
|
|
|
|
// Envia o comando via Ethernet
|
|
bool sucessoEnvio = await EnviarComandoDriver(Comando);
|
|
Comando.ErroEnvio = !sucessoEnvio;
|
|
Comando.Enviado = true;
|
|
|
|
// Recebe a resposta via Ethernet, independentemente de precisar processá-la ou não
|
|
byte[] resposta = await _ethernetService.ReceiveDataAsync(Comando.TamanhoResposta, Comando.TimeoutResposta); // Timeout ajustável, aqui 1 segundo
|
|
RegisrarComandosComunicacao(Addr, $"RX: - {BitConverter.ToString(resposta ?? new byte[] { }).Replace("-", " ")}");
|
|
|
|
//SerialService.MostrarComandoResposta(T_Code.Mks, comando, resposta);
|
|
|
|
if (Comando.AguardaResposta)
|
|
{
|
|
// Processa a resposta, pois é esperada
|
|
bool Respondido = DecifrarRespostaDriver(resposta);
|
|
Comando.Respondido = Respondido;
|
|
if (!Respondido)
|
|
{
|
|
RegisrarErrosComunicacao(Addr, $"Timeout ao receber resposta. GET={Comando.Get}, Parametro={Comando.Comando}");
|
|
//Comando.Momento = DateTime.Now.AddMilliseconds(500);
|
|
|
|
if (!resend)
|
|
{
|
|
await EnviarComandoControle(Comando, true);
|
|
}
|
|
}
|
|
}
|
|
else
|
|
{
|
|
// Descarte da resposta, pois não era esperada
|
|
Console.WriteLine($"Resposta descartada para comando que não aguardava resposta.");
|
|
}
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
// Log de erro
|
|
RegisrarErrosComunicacao(Comando.Endereco, $"Erro ao enviar comando: {ex.Message}");
|
|
Comando.Enviado = false;
|
|
Comando.Momento = DateTime.Now.AddMilliseconds(500);
|
|
}
|
|
finally
|
|
{
|
|
FilaEmEspera = false;
|
|
}
|
|
}
|
|
|
|
private static async Task<bool> EnviarComandoDriver(MKS057ComandoModel Comando)
|
|
{
|
|
bool sucesso = false;
|
|
byte[] Buffer;
|
|
byte Addr = Comando.Endereco;
|
|
int data = Comando.Registro;
|
|
|
|
switch (Comando.ModoComando)
|
|
{
|
|
case ModosComando.GetPulsos:
|
|
case ModosComando.GetIO:
|
|
case ModosComando.GetRPM:
|
|
case ModosComando.GetVersao:
|
|
Buffer = new byte[] { 0xFA, Addr, (byte)data };
|
|
byte tCHK = CalcularChecksum(Buffer);
|
|
Buffer = Buffer.Concat(new byte[] { tCHK }).ToArray();
|
|
await WriteByteToEthernetAsync(Buffer, 0, (byte)Buffer.Length);
|
|
sucesso = true;
|
|
break;
|
|
|
|
case ModosComando.SetZero:
|
|
Buffer = new byte[] { 0xFA, Addr, (byte)data, 0x00 };
|
|
tCHK = CalcularChecksum(Buffer);
|
|
Buffer = Buffer.Concat(new byte[] { tCHK }).ToArray();
|
|
await WriteByteToEthernetAsync(Buffer, 0, (byte)Buffer.Length);
|
|
sucesso = true;
|
|
break;
|
|
|
|
case ModosComando.SetParada:
|
|
Buffer = new byte[] { 0xFA, Addr, (byte)data, 0x00, 0x00, AceleracaoParada[0], 0x00, 0x00, 0x00, 0x00 };
|
|
tCHK = CalcularChecksum(Buffer);
|
|
Buffer = Buffer.Concat(new byte[] { tCHK }).ToArray();
|
|
await WriteByteToEthernetAsync(Buffer, 0, (byte)Buffer.Length);
|
|
sucesso = true;
|
|
break;
|
|
|
|
case ModosComando.SetMovimento:
|
|
ushort velocidade = (ushort)Comando.Velocidade;
|
|
byte[] VelocidadeBytes = BitConverter.GetBytes(velocidade); // Little Endian
|
|
|
|
byte _Sentido = Comando._Sentido == Sentido.Horario ? (byte)0x01 : (byte)0x00;
|
|
|
|
string _AnguloStr = ConverterAnguloParaValor(Comando.Angulo).ToString();
|
|
|
|
if (Comando.ModoControle == ModosControle.Pulsos)
|
|
{
|
|
uint PulseText = Convert.ToUInt32(_AnguloStr, 10);
|
|
byte[] PulsosBytes = BitConverter.GetBytes(PulseText);
|
|
|
|
if (Comando._Sentido == Sentido.Antihorario)
|
|
{
|
|
VelocidadeBytes[1] |= RWD;
|
|
}
|
|
else if (Comando._Sentido == Sentido.Horario)
|
|
{
|
|
VelocidadeBytes[1] |= FWD;
|
|
}
|
|
|
|
// Construção do Buffer de Pulsos (idêntico ao original)
|
|
Buffer = new byte[]
|
|
{
|
|
0xFA, Addr, (byte)data,
|
|
VelocidadeBytes[1], VelocidadeBytes[0], // **Corrigido**
|
|
AceleracaoInicio[0],
|
|
PulsosBytes[3], PulsosBytes[2], PulsosBytes[1], PulsosBytes[0], // **Corrigido**
|
|
};
|
|
}
|
|
else
|
|
{
|
|
long AbsAxisText = Convert.ToInt32(_AnguloStr, 10);
|
|
byte[] AbsAxis = BitConverter.GetBytes(AbsAxisText * 16384 / 360);
|
|
|
|
// **Correção: Ordem de VelocidadeBytes e AnguloBytes**
|
|
Buffer = new byte[]
|
|
{
|
|
0xFA, Addr, (byte)data,
|
|
VelocidadeBytes[1], VelocidadeBytes[0], // **Mantendo a ordem**
|
|
AceleracaoInicio[0],
|
|
AbsAxis[3], AbsAxis[2], AbsAxis[1], AbsAxis[0], // **Mantendo a ordem**
|
|
};
|
|
}
|
|
|
|
tCHK = CalcularChecksum(Buffer);
|
|
Buffer = Buffer.Concat(new byte[] { tCHK }).ToArray();
|
|
await WriteByteToEthernetAsync(Buffer, 0, (byte)Buffer.Length);
|
|
sucesso = true;
|
|
break;
|
|
}
|
|
|
|
return sucesso;
|
|
}
|
|
|
|
private static bool DecifrarRespostaDriver(byte[] resposta)
|
|
{
|
|
if (resposta == null || resposta.Length == 0 || resposta.Length > 11)
|
|
{
|
|
return false;
|
|
}
|
|
|
|
return ProcessarResposta(resposta);
|
|
}
|
|
|
|
private static bool ProcessarResposta(byte[] _SerialBuffer_RX)
|
|
{
|
|
byte _rCHK = CalcularChecksum(_SerialBuffer_RX.Take(_SerialBuffer_RX.Length - 1).ToArray()); // Corrigido
|
|
|
|
byte addr = _SerialBuffer_RX[1];
|
|
if (!EnderecoValido(addr))
|
|
{
|
|
RegisrarErrosComunicacao(addr, $"Dados inválidos recebidos: [{BitConverter.ToString(_SerialBuffer_RX)}]");
|
|
return false;
|
|
}
|
|
|
|
if (_rCHK != _SerialBuffer_RX[_SerialBuffer_RX.Length - 1])
|
|
{
|
|
RegisrarErrosComunicacao(addr, "Erro de checksum na resposta recebida.");
|
|
return false;
|
|
}
|
|
|
|
MKS057DModel leitura = DadosLeitura.FirstOrDefault(x => x.Endereco == addr) ?? new MKS057DModel { Endereco = addr };
|
|
if (!DadosLeitura.Any(x => x.Endereco == leitura.Endereco))
|
|
{
|
|
DadosLeitura.Add(leitura);
|
|
}
|
|
|
|
switch (_SerialBuffer_RX[2])
|
|
{
|
|
case 0x33: // Número de pulsos acumulados
|
|
|
|
string str, word32;
|
|
int data32;
|
|
|
|
if (ModoLeitura == "Hex") // Hexadecimal
|
|
{
|
|
leitura.NumeroPulsos_Hex = "";
|
|
|
|
leitura.NumeroPulsos_Hex += "0x";
|
|
for (int a = 3; a < 7; a++)
|
|
{
|
|
str = Convert.ToString(_SerialBuffer_RX[a], 16).ToUpper();
|
|
leitura.NumeroPulsos_Hex += str.Length == 1 ? "0" + str : str;
|
|
}
|
|
}
|
|
else if (ModoLeitura == "Decimal") // Decimal
|
|
{
|
|
leitura.NumeroPulsos = 0;
|
|
|
|
word32 = "0";
|
|
for (int a = 3; a < 7; a++)
|
|
{
|
|
str = Convert.ToString(_SerialBuffer_RX[a], 16).ToUpper();
|
|
word32 += str.Length == 1 ? "0" + str : str;
|
|
}
|
|
data32 = Convert.ToInt32(word32, 16);
|
|
|
|
str = Convert.ToString(data32, 10);
|
|
int.TryParse(str, out int val);
|
|
leitura.NumeroPulsos = val;
|
|
}
|
|
|
|
break;
|
|
|
|
case 0x34: // Estado do Portão IO
|
|
leitura.Entradas = new List<bool>() { false, false };
|
|
leitura.Saidas = new List<bool>() { false, false };
|
|
|
|
byte[] IO = new byte[1];
|
|
IO[0] = _SerialBuffer_RX[3];
|
|
BitArray LimitIO = new BitArray(IO);
|
|
|
|
if (leitura.HV == HardVersion[3] || leitura.HV == HardVersion[4])
|
|
{
|
|
leitura.Entradas[0] = LimitIO.Get(0) ? false : true;
|
|
leitura.Entradas[1] = LimitIO.Get(1) ? false : true;
|
|
leitura.Saidas[0] = LimitIO.Get(2) ? false : true;
|
|
leitura.Saidas[1] = LimitIO.Get(3) ? false : true;
|
|
}
|
|
else if (leitura.HV == HardVersion[1] || leitura.HV == HardVersion[2])
|
|
{
|
|
leitura.Entradas[0] = LimitIO.Get(0) ? false : true;
|
|
}
|
|
break;
|
|
|
|
case 0x32: // Velocidade atual (RPM)
|
|
string word16;
|
|
int data16;
|
|
if (ModoLeitura == "Hex") // Hexadecimal
|
|
{
|
|
leitura.RPM_Hex = "";
|
|
leitura.RPM_Hex += "0x";
|
|
for (int a = 3; a < 5; a++)
|
|
{
|
|
str = Convert.ToString(_SerialBuffer_RX[a], 16).ToUpper();
|
|
leitura.RPM_Hex += str.Length == 1 ? "0" + str : str;
|
|
}
|
|
}
|
|
else if (ModoLeitura == "Decimal") // Decimal
|
|
{
|
|
leitura.RPM = 0;
|
|
word16 = "0";
|
|
for (int a = 3; a < 5; a++)
|
|
{
|
|
str = Convert.ToString(_SerialBuffer_RX[a], 16).ToUpper();
|
|
word16 += str.Length == 1 ? "0" + str : str;
|
|
}
|
|
data16 = Convert.ToInt16(word16, 16);
|
|
str = Convert.ToString(data16, 10);
|
|
double.TryParse(str, out double val);
|
|
leitura.RPM = val;
|
|
}
|
|
break;
|
|
|
|
case 0x40: // Informação da versão do hardware/firmware
|
|
int idx = _SerialBuffer_RX[3] < 9 ? _SerialBuffer_RX[3] : 22 - _SerialBuffer_RX[3];
|
|
leitura.HV = HardVersion[idx];
|
|
leitura.FV = $"V{_SerialBuffer_RX[4]}.{_SerialBuffer_RX[5]}.{_SerialBuffer_RX[6]}";
|
|
|
|
Max_Current = (ushort)((_SerialBuffer_RX[3] == 1 || _SerialBuffer_RX[3] == 2) ? 3000 :
|
|
(_SerialBuffer_RX[3] == 3 || _SerialBuffer_RX[3] == 4) ? 5200 : 3000);
|
|
break;
|
|
|
|
case 0xFD: // Resposta para SetMovimento
|
|
case 0xF4: // Resposta para Angulo Relativo
|
|
case 0xF5: // Resposta para Angulo Absoluto
|
|
if (_SerialBuffer_RX[3] == 0x02) // Concluído
|
|
{
|
|
leitura.StatusMovimentoMotor = StatusComando.Sucesso;
|
|
}
|
|
else if (_SerialBuffer_RX[3] == 0x01) // Movendo
|
|
{
|
|
leitura.StatusMovimentoMotor = StatusComando.Movendo;
|
|
}
|
|
else
|
|
{
|
|
leitura.StatusMovimentoMotor = StatusComando.Falha;
|
|
}
|
|
break;
|
|
|
|
case 0x92: // Resposta para SetZero
|
|
if (_SerialBuffer_RX[3] == 0x01)
|
|
{
|
|
leitura.StatusMovimentoMotor = StatusComando.Sucesso;
|
|
}
|
|
else
|
|
{
|
|
leitura.StatusMovimentoMotor = StatusComando.Falha;
|
|
}
|
|
break;
|
|
|
|
default:
|
|
MessageBox.Show("Erro ao decodificar resposta");
|
|
return false;
|
|
}
|
|
|
|
leitura.UltimoComandoRecebido = DateTime.Now;
|
|
return true;
|
|
}
|
|
|
|
|
|
private static async Task<(bool, byte[])> WriteByteToEthernetAsync(byte[] data, byte start, byte length)
|
|
{
|
|
byte Addr = data[1];
|
|
|
|
if (_ethernetService == null || !_ethernetService.IsConnected)
|
|
{
|
|
RegisrarErrosComunicacao(Addr, "Conexão Ethernet não está definida ou não está conectada");
|
|
return (false, data);
|
|
}
|
|
|
|
try
|
|
{
|
|
bool sucesso = await _ethernetService.SendDataAsync(data.Skip(start).Take(length).ToArray());
|
|
RegisrarComandosComunicacao(data[1], $"TX: {sucesso} - {BitConverter.ToString(data).Replace("-", " ")}");
|
|
return (sucesso, data);
|
|
}
|
|
catch
|
|
{
|
|
RegisrarErrosComunicacao(Addr, "Erro ao enviar dados para o dispositivo via Ethernet");
|
|
return (false, data);
|
|
}
|
|
}
|
|
|
|
|
|
}
|
|
|
|
|
|
} |