421 lines
18 KiB
C#
421 lines
18 KiB
C#
using AgroBase.Models;
|
||
using System;
|
||
using System.Collections.Generic;
|
||
using System.Linq;
|
||
using System.Threading.Tasks;
|
||
using static AgroBase.Models.Enums;
|
||
using static AgroBase.Services.MKS057DCanService.MksHandler;
|
||
|
||
namespace AgroBase.Services
|
||
{
|
||
public class MKS057DCanService
|
||
{
|
||
public static List<MKS057DModel> DadosLeitura = new List<MKS057DModel>();
|
||
private static int _MStep { get; set; } = 16;
|
||
private static T_Code Dispositivo { get; set; } = T_Code.Mks;
|
||
public static MksHandler _MksHandler { get; set; } = new MksHandler();
|
||
|
||
public static bool Iniciado
|
||
{
|
||
get
|
||
{
|
||
return CanManager.CanService.IsConnected && SerialService.DispositivosMapeados.Any(x => x.Dispositivo == T_Code.Dir) && DadosLeitura.Any(x => x.Iniciado);
|
||
}
|
||
}
|
||
public static bool Referenciando
|
||
{
|
||
get
|
||
{
|
||
return Variaveis.OperacaoEmAndamento.DispMvd?.Dados?.Modulos?.Any(x => x?.DirMotor?.Referenciando ?? false) ?? false;
|
||
}
|
||
}
|
||
public static DateTime UltimoReferenciamento = DateTime.MinValue;
|
||
public static int TaxaAmostragem
|
||
{
|
||
get
|
||
{
|
||
return Referenciando ? 250 : 1000;
|
||
}
|
||
}
|
||
|
||
public static async Task<bool> VerificaDispositivoConectado()
|
||
{
|
||
if (!CanManager.CanService.IsConnected) return false;
|
||
|
||
if (Variaveis.OperacaoEmAndamento.DispMvd == null) return false;
|
||
|
||
List<string> Mod_IDs = new List<string>();
|
||
|
||
foreach (var Modulo in Variaveis.OperacaoEmAndamento.DispMvd.Dados.Modulos.Where(x => !x.DirMotor.Inicializado))
|
||
{
|
||
CanManager.CanService.RegistrarHandler(Modulo.DirMotor._EnderecoCAN_Rx, _MksHandler);
|
||
|
||
EnviarComandoDescoberta(Modulo.DirMotor._EnderecoCAN_Tx, Modulo.DirMotor._EnderecoCAN_Rx);
|
||
|
||
DateTime Inicio = DateTime.Now;
|
||
bool Respondido() => DadosLeitura.Any(x => x.EnderecoRx == Modulo.DirMotor._EnderecoCAN_Rx && x.UltimoComandoRecebido > Inicio);
|
||
await FuncoesGlobais.AguardarCondicaoAsync(Respondido, 500, 50);
|
||
|
||
if (Respondido())
|
||
{
|
||
Mod_IDs.Add(Modulo.Modulo_ID);
|
||
}
|
||
}
|
||
|
||
if (Mod_IDs.Any())
|
||
{
|
||
DefinirDispositivo(Mod_IDs);
|
||
}
|
||
|
||
return Mod_IDs.Any();
|
||
}
|
||
|
||
private static void DefinirDispositivo(List<string> Mod_IDs)
|
||
{
|
||
foreach (var Mod_ID in Mod_IDs)
|
||
{
|
||
var Modulo = Variaveis.OperacaoEmAndamento.DispMvd.Dados.Modulos.FirstOrDefault(x => x.Modulo_ID == Mod_ID);
|
||
|
||
if (!SerialService.DispositivosMapeados.Any(x => x.Dispositivo == T_Code.Dir && x.Mod_ID == Mod_ID))
|
||
{
|
||
var Leitura = DadosLeitura.FirstOrDefault(x => x.EnderecoTx == Modulo.DirMotor._EnderecoCAN_Tx);
|
||
if (Leitura != null)
|
||
{
|
||
SerialService.DispositivosMapeados.Add(new DispositivoDetalhesModel()
|
||
{
|
||
Dispositivo = T_Code.Dir,
|
||
Endereco = CanManager.CanService._portName,
|
||
Versao = (Leitura.FV ?? "").ToString(),
|
||
Mod_ID = Mod_ID,
|
||
});
|
||
|
||
Variaveis.OperacaoEmAndamento.ModsCalibragem.TryGetValue(T_Code.Dir, out var m);
|
||
if (m == null)
|
||
Variaveis.OperacaoEmAndamento.ModsCalibragem.Add(T_Code.Dir, new Dictionary<string, bool>());
|
||
Variaveis.OperacaoEmAndamento.ModsCalibragem[T_Code.Dir][Mod_ID] = false;
|
||
Variaveis.OperacaoEmAndamento.Sensoriamento.InserirLog(T_Code.Dir, StatusModulo.Conectado, 0, "Dispositivo conectado", Mod_ID);
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
|
||
#region UTILS
|
||
|
||
private static int ConverterAnguloParaValor(double _angulo, ModosControle modo)
|
||
{
|
||
int val = 0;
|
||
|
||
if (modo == ModosControle.Pulsos)
|
||
{
|
||
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;
|
||
}
|
||
|
||
#endregion
|
||
|
||
#region COMANDOS
|
||
|
||
private static void EnviarComandoDescoberta(byte idTx, byte idRx)
|
||
{
|
||
CanManager.CanService.AdicionarMensagemNaFila(Dispositivo, new byte[] { idTx }, new byte[] { idRx }, (byte)MksFuncCode.Versao, (byte)MksFuncCode.Versao, null, true, false, true);
|
||
}
|
||
|
||
private static void EnviarComando(T_Code Dispositivo, byte idTx, byte idRx, byte funcCode, byte[] payload = null, bool get = true)
|
||
{
|
||
CanManager.CanService.AdicionarMensagemNaFila(Dispositivo, new byte[] { idTx }, new byte[] { idRx }, funcCode, funcCode, payload, get);
|
||
}
|
||
|
||
public static void SetModoOperacao(byte idTx, byte idRx, byte modo)
|
||
{
|
||
EnviarComando(Dispositivo, idTx, idRx, 0x82, new byte[] { modo }, false);
|
||
}
|
||
|
||
public static void RequisitarDado(byte idTx, byte idRx, MksFuncCode dado)
|
||
{
|
||
EnviarComando(Dispositivo, idTx, idRx, (byte)dado);
|
||
}
|
||
|
||
public static void SetZero(byte idTx, byte idRx)
|
||
{
|
||
EnviarComando(Dispositivo, idTx, idRx, (byte)MksFuncCode.SetZero, null, false);
|
||
}
|
||
|
||
public static void Parar(byte idTx, byte idRx, ModosControle modo = ModosControle.AnguloAbsoluto)
|
||
{
|
||
byte funcCode =
|
||
modo == ModosControle.Pulsos ? (byte)MksFuncCode.ModoControlePulsos :
|
||
modo == ModosControle.AnguloRelativo ? (byte)MksFuncCode.ModoControleAnguloRelativo :
|
||
(byte)MksFuncCode.ModoControleAnguloAbsoluto;
|
||
|
||
EnviarComando(Dispositivo, idTx, idRx, funcCode, new byte[] { 0x00 }, false);
|
||
}
|
||
|
||
public static void Movimento(byte idTx, byte idRx, double angulo, int velocidade, Sentido sentido = Sentido.Horario, ModosControle modo = ModosControle.AnguloAbsoluto)
|
||
{
|
||
List<byte> payload = new List<byte>();
|
||
|
||
ushort vel = (ushort)velocidade;
|
||
byte[] VelocidadeBytes = BitConverter.GetBytes(vel);
|
||
|
||
byte acc = 0;
|
||
|
||
if (modo == ModosControle.Pulsos)
|
||
{
|
||
byte _Sentido = sentido == Sentido.Horario ? (byte)0x80 : (byte)0x00;
|
||
|
||
uint pulsos = (uint)ConverterAnguloParaValor(angulo, modo);
|
||
|
||
byte[] PulsosBytes = BitConverter.GetBytes(pulsos); // Little Endian padrão [0]=LSB
|
||
|
||
// Igual no RS485: sentido no bit alto da velocidade alta (VelocidadeBytes[1])
|
||
VelocidadeBytes[1] |= _Sentido;
|
||
|
||
// Ordem dos bytes: [MSB, LSB] da velocidade
|
||
payload.Add(VelocidadeBytes[1]);
|
||
payload.Add(VelocidadeBytes[0]);
|
||
payload.Add(acc);
|
||
|
||
// Agora: ordem dos pulsos → Big Endian (MSB → LSB), igual ao driver RS485
|
||
payload.Add(PulsosBytes[2]);
|
||
payload.Add(PulsosBytes[1]);
|
||
payload.Add(PulsosBytes[0]);
|
||
|
||
EnviarComando(Dispositivo, idTx, idRx, (byte)MksFuncCode.ModoControlePulsos, payload.ToArray(), false);
|
||
}
|
||
else
|
||
{
|
||
int pos = ConverterAnguloParaValor(angulo, modo);
|
||
|
||
long AbsAxisText = Convert.ToInt32(pos.ToString(), 10);
|
||
byte[] AbsAxis = BitConverter.GetBytes(AbsAxisText * 16384 / 360);
|
||
byte[] posInt24 = new byte[] { AbsAxis[2], AbsAxis[1], AbsAxis[0] };
|
||
|
||
payload.AddRange(new byte[] { VelocidadeBytes[1], VelocidadeBytes[0] }); // 2 bytes
|
||
payload.Add(acc); // 1 byte
|
||
payload.AddRange(posInt24); // 3 bytes
|
||
|
||
byte funcCode = modo == ModosControle.AnguloRelativo ? (byte)MksFuncCode.ModoControleAnguloRelativo : (byte)MksFuncCode.ModoControleAnguloAbsoluto;
|
||
EnviarComando(Dispositivo, idTx, idRx, funcCode, payload.ToArray(), false);
|
||
}
|
||
|
||
//DadosLeitura.FirstOrDefault(x => x.EnderecoTx == idTx).AnguloSP = angulo;
|
||
}
|
||
|
||
public static void AlterarStatusMotor(byte idTx, byte idRx, bool Habilitado)
|
||
{
|
||
byte comando = (Habilitado ? (byte)0x01 : (byte)0x00);
|
||
EnviarComando(Dispositivo, idTx, idRx, (byte)MksFuncCode.HabilitarMotor, new byte[] { comando }, false);
|
||
}
|
||
|
||
#endregion
|
||
|
||
public class MksHandler : ICanMessageHandler
|
||
{
|
||
public T_Code Dispositivo { get; set; } = MKS057DCanService.Dispositivo;
|
||
|
||
public enum MksFuncCode : byte
|
||
{
|
||
EncoderCarregado = 0x30,
|
||
EncoderAcumulado = 0x31,
|
||
RPM = 0x32,
|
||
PulsosRecebidos = 0x33,
|
||
IO = 0x34,
|
||
Versao = 0x40,
|
||
ErroAcumulado = 0x36,
|
||
SetZero = 0x92,
|
||
HabilitarMotor = 0xF3,
|
||
ModoControleAnguloRelativo = 0xF4,
|
||
ModoControleAnguloAbsoluto = 0xF5,
|
||
ModoControlePulsos = 0xFD
|
||
}
|
||
|
||
|
||
|
||
private long DecodeInt48(byte[] data, int startIndex)
|
||
{
|
||
byte[] buffer = new byte[8];
|
||
|
||
// inverter a ordem dos 6 bytes (big endian → little endian)
|
||
for (int i = 0; i < 6; i++)
|
||
buffer[i] = data[startIndex + 5 - i]; // inverter os 6 bytes
|
||
|
||
// Preencher os dois bytes restantes com sinal (baseado no MSB original)
|
||
buffer[6] = (buffer[5] & 0x80) != 0 ? (byte)0xFF : (byte)0x00;
|
||
buffer[7] = buffer[6]; // mesmo valor para byte 7
|
||
|
||
return BitConverter.ToInt64(buffer, 0);
|
||
}
|
||
|
||
private static ushort DecodeUInt16BE(byte[] data, int startIndex)
|
||
{
|
||
return (ushort)((data[startIndex] << 8) | data[startIndex + 1]);
|
||
}
|
||
|
||
private double PulsosRecebidosParaAngulo(int valor)
|
||
{
|
||
double reducao = VariaveisEquipamento.ReducaoDirecional;
|
||
|
||
double pulsos = _MStep * 200.0;
|
||
double relacao = (pulsos * reducao) / 360.0;
|
||
double angulo = valor / relacao;
|
||
|
||
return angulo;
|
||
}
|
||
|
||
private double PulsosEncoderParaAngulo(int valor)
|
||
{
|
||
return valor * 360.0 / 16384.0;
|
||
}
|
||
|
||
private void VerificarAnguloZero(MKS057DModel item, bool estadoAnterior)
|
||
{
|
||
if (item.EntradaIN1.valor != estadoAnterior && item.EntradaIN1.dt <= 0.5)
|
||
{
|
||
var driver = Variaveis.OperacaoEmAndamento.DispMvd.Dados.Modulos.FirstOrDefault(x => x.DirMotor._EnderecoCAN_Tx == item.EnderecoTx)?.DirMotor;
|
||
double tempoUltimaLeituraAngulo = (DateTime.Now - item.NumeroPulsos.lidoEm).TotalSeconds;
|
||
if (driver != null && Math.Abs(item.AnguloPulsos) >= driver.FolgaMecanica && tempoUltimaLeituraAngulo <= 1.0)
|
||
{
|
||
Task.Run(async () => await driver.SetZero());
|
||
}
|
||
}
|
||
}
|
||
|
||
private MKS057DModel ObterOuCriar(uint _idTx, uint _idRx)
|
||
{
|
||
lock (DadosLeitura)
|
||
{
|
||
byte idTx = CanManager.ConverterIdCan(_idTx)[0];
|
||
byte idRx = CanManager.ConverterIdCan(_idRx)[0];
|
||
var item = DadosLeitura.FirstOrDefault(x => x.EnderecoTx == idTx && x.EnderecoRx == idRx);
|
||
if (item == null)
|
||
{
|
||
item = new MKS057DModel { EnderecoTx = idTx, EnderecoRx = idRx };
|
||
DadosLeitura.Add(item);
|
||
}
|
||
return item;
|
||
}
|
||
}
|
||
|
||
public void ProcessarMensagem(CanMessage mensagem)
|
||
{
|
||
MksFuncCode func = (MksFuncCode)mensagem.DataRx[0];
|
||
var data = mensagem.DataRx;
|
||
|
||
var item = ObterOuCriar(mensagem.IdTx, mensagem.IdRx);
|
||
|
||
switch (func)
|
||
{
|
||
case MksFuncCode.EncoderCarregado:
|
||
if (data != null && data.Length >= 7)
|
||
{
|
||
int carry = (data[1] << 24) | (data[2] << 16) | (data[3] << 8) | data[4]; // bytes 1–4 Quantas voltas completas em torno do eixo
|
||
ushort value = (ushort)((data[5] << 8) | data[6]); // bytes 5–6 Quantos pulsos por volta
|
||
|
||
int pulsos = (int)(carry * 16384.0) + value;
|
||
|
||
item.NumeroPulsosRelativo = pulsos;
|
||
item.AnguloPulsosRelativo = PulsosEncoderParaAngulo(pulsos);
|
||
}
|
||
break;
|
||
|
||
case MksFuncCode.EncoderAcumulado:
|
||
if (data != null && data.Length >= 7)
|
||
{
|
||
long pulsos = DecodeInt48(data, 1); // lê [1]..[6]
|
||
item.NumeroPulsosAbsoluto = (int)pulsos;
|
||
item.AnguloPulsosAbsoluto = PulsosEncoderParaAngulo((int)pulsos);
|
||
}
|
||
break;
|
||
|
||
case MksFuncCode.RPM:
|
||
if (data != null && data.Length >= 3)
|
||
{
|
||
item.Velocidade = DecodeUInt16BE(data, 1);
|
||
|
||
//Console.WriteLine($"[DIR {Variaveis.OperacaoEmAndamento.DispMvd?.Dados?.Modulos?.FirstOrDefault(x => (x?.DirMotor?._EnderecoCAN_Tx ?? 0x00) == item.EnderecoTx)?.DirMotor?.Mod_ID ?? item.EnderecoTx.ToString()}] RPM atual: {item.Velocidade}");
|
||
}
|
||
break;
|
||
|
||
case MksFuncCode.PulsosRecebidos:
|
||
if (data != null && data.Length >= 6)
|
||
{
|
||
int pulsos = (data[1] << 24) | (data[2] << 16) | (data[3] << 8) | data[4];
|
||
if ((data[1] & 0x80) != 0) // se MSB estiver setado, é número negativo
|
||
{
|
||
pulsos |= unchecked((int)0xFF000000); // preencher sinal
|
||
}
|
||
|
||
item.NumeroPulsos.valor = pulsos;
|
||
item.AnguloPulsos = PulsosRecebidosParaAngulo(pulsos);
|
||
|
||
//Console.WriteLine($"[DIR {Variaveis.OperacaoEmAndamento.DispMvd?.Dados?.Modulos?.FirstOrDefault(x => (x?.DirMotor?._EnderecoCAN_Tx ?? 0x00) == item.EnderecoTx)?.DirMotor?.Mod_ID ?? item.EnderecoTx.ToString()}] Angulo atual: {item.AnguloPulsos} ({item.AnguloEstimado()})");
|
||
}
|
||
break;
|
||
|
||
case MksFuncCode.IO:
|
||
if (data != null && data.Length >= 3)
|
||
{
|
||
byte status = data[1];
|
||
item.Entradas = new List<bool>() { false, false };
|
||
item.Entradas[0] = (status & 0b00000001) != 0;
|
||
item.Entradas[1] = (status & 0b00000010) != 0;
|
||
item.Saidas = new List<bool>() { false, false };
|
||
item.Saidas[0] = (status & 0b00000100) != 0;
|
||
item.Saidas[1] = (status & 0b00001000) != 0;
|
||
|
||
bool estadoAnterior = item.EntradaIN1.valor;
|
||
|
||
item.EntradaIN1.valor = item.Entradas[0];
|
||
|
||
//VerificarAnguloZero(item, estadoAnterior);
|
||
|
||
//Console.WriteLine($"[DIR {Variaveis.OperacaoEmAndamento.DispMvd?.Dados?.Modulos?.FirstOrDefault(x => (x?.DirMotor?._EnderecoCAN_Tx ?? 0x00) == item.EnderecoTx)?.DirMotor?.Mod_ID ?? item.EnderecoTx.ToString()}] IO atual");
|
||
}
|
||
break;
|
||
|
||
case MksFuncCode.ErroAcumulado:
|
||
if (data != null && data.Length >= 3)
|
||
{
|
||
item.ErroAcumulado = DecodeUInt16BE(data, 1);
|
||
}
|
||
break;
|
||
|
||
case MksFuncCode.Versao:
|
||
if (data != null && data.Length >= 6)
|
||
{
|
||
string versao = $"{data[1]}.{data[2]}.{data[3]}.{data[4]}";
|
||
item.FV = versao;
|
||
|
||
//Console.WriteLine($"[DIR {Variaveis.OperacaoEmAndamento.DispMvd?.Dados?.Modulos?.FirstOrDefault(x => (x?.DirMotor?._EnderecoCAN_Tx ?? 0x00) == item.EnderecoTx)?.DirMotor?.Mod_ID ?? item.EnderecoTx.ToString()}] Versao atual");
|
||
}
|
||
break;
|
||
|
||
case MksFuncCode.ModoControleAnguloRelativo:
|
||
case MksFuncCode.ModoControleAnguloAbsoluto:
|
||
case MksFuncCode.ModoControlePulsos:
|
||
{
|
||
MksMovimentStatus statusMovimento = (MksMovimentStatus)data[1];
|
||
item.StatusMovimento = statusMovimento;
|
||
break;
|
||
}
|
||
}
|
||
|
||
item.UltimoComandoRecebido = DateTime.Now;
|
||
}
|
||
}
|
||
}
|
||
|
||
|
||
|
||
|
||
}
|