agrobot_base/AgroBase/AgroBase/Services/OIDCanService.cs

using AgroBase.Models;
using AgroBase.Models.Modules;
using System;
using System.Collections.Generic;
using System.Linq;
using System.Threading.Tasks;
using static AgroBase.Models.Enums;
using static AgroBase.Services.OIDCanService.OidHandler;

namespace AgroBase.Services
{
    public class OIDCanService
    {
        public static List<OIDModel> DadosLeitura = new List<OIDModel>();
        private static T_Code Dispositivo { get; set; } = T_Code.Oid;
        private static OidHandler _OidHandler { get; set; } = new OidHandler();
        private static AsyncTaskTimerModel tmrHeartbeat;

        public static bool Iniciado
        {
            get
            {
                return CanService.IsConnected && SerialService.DispositivosMapeados.Any(x => x.Dispositivo == T_Code.Mov) && DadosLeitura.Any(x => x.Iniciado);
            }
        }
        public static int TaxaAmostragem
        {
            get
            {
                return 1000;
            }
        }

        public static async Task<bool> VerificaDispositivoConectado()
        {
            if (!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.MovMotor.Inicializado))
            {
                CanService.RegistrarHandler(Modulo.MovMotor._EnderecoCAN_Rx, _OidHandler);

                RequisitarDado(Modulo.MovMotor._EnderecoCAN_Tx, Modulo.MovMotor._EnderecoCAN_Rx, OidParametros.CodErro);

                DateTime Inicio = DateTime.Now;
                bool Respondido() => DadosLeitura.Any(x => x.EnderecoRx == Modulo.MovMotor._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 Iniciado;
        }

        private static void DefinirDispositivo(List<string> Mod_IDs)
        {
            if (tmrHeartbeat == null || !(tmrHeartbeat?.IsRunning ?? false))
            {
                tmrHeartbeat?.Dispose();
                tmrHeartbeat = new AsyncTaskTimerModel("tmrOidHeartbeat", tmrHeartbeat_Tick, 500);
                tmrHeartbeat.Start();
            }

            foreach (var Mod_ID in Mod_IDs)
            {
                var Modulo = Variaveis.OperacaoEmAndamento.DispMvd.Dados.Modulos.FirstOrDefault(x => x.Modulo_ID == Mod_ID).MovMotor;

                if (DadosLeitura.Any(x => x.EnderecoTx == Modulo._EnderecoCAN_Tx && !x.Configurado))
                {
                    ConfigurarDriver(Modulo);
                }

                if (!SerialService.DispositivosMapeados.Any(x => x.Dispositivo == T_Code.Mov && x.Mod_ID == Mod_ID))
                {
                    var Leitura = DadosLeitura.FirstOrDefault(x => x.EnderecoTx == Modulo._EnderecoCAN_Tx);
                    if (Leitura != null)
                    {
                        SerialService.DispositivosMapeados.Add(new DispositivoDetalhesModel()
                        {
                            Endereco = CanService._portName,
                            Dispositivo = T_Code.Mov,
                            Erro = Leitura.CodigoAlarme.valor > 0,
                            Versao = "1.0",
                            Mod_ID = Mod_ID,
                        });
                    }
                }
            }
        }


        #region COMANDOS

        private static void EnviarComando(T_Code Dispositivo, byte idTx, byte idRx, byte funcCode, byte[] payload = null, bool get = true)
        {
            CanService.AdicionarMensagemNaFila(Dispositivo, idTx, idRx, funcCode, funcCode, payload, get);
        }

        public static void RequisitarDado(byte idTx, byte idRx, OidParametros dado)
        {
            EnviarComando(Dispositivo, idTx, idRx, (byte)OidFuncCode.Consulta, new byte[] { (byte)dado }, true);
        }

        public static void ComandoControle(byte idTx, byte idRx, OidFuncCode funcCode, double setPoint)
        {
            byte[] setPointBytes = null;

            switch (funcCode)
            {
                case OidFuncCode.ComandoCorrente:
                case OidFuncCode.ComandoCorrenteFreio:
                case OidFuncCode.AjusteCorrenteMaxima:
                    {
                        short corrente_10mA = (short)(setPoint * 100);
                        setPointBytes = BitConverter.GetBytes(corrente_10mA).Reverse().ToArray();
                        break;
                    }
                case OidFuncCode.ComandoVelocidade:
                    {
                        int erpm = (int)(setPoint * VariaveisEquipamento.NumeroPolosMotor);
                        setPointBytes = BitConverter.GetBytes(erpm).Reverse().ToArray();
                        break;
                    }
                case OidFuncCode.ComandoCicloTrabalho:
                    {
                        short duty = (short)(Math.Max(-100, Math.Min(setPoint, 100)) * 10);
                        setPointBytes = BitConverter.GetBytes(duty).Reverse().ToArray();
                        break;
                    }
                case OidFuncCode.AjusteAceleracaoVelocidade:
                case OidFuncCode.AjusteDesaceleracaoVelocidade:
                    {
                        if (setPoint <= 0) setPoint = 1;
                        double erpmMax = VariaveisEquipamento.RPM_Max_Roda * VariaveisEquipamento.RelacaoRPM * VariaveisEquipamento.NumeroPolosMotor;
                        int aceleracao = (int)(erpmMax / setPoint); // setPoint = tempo (s)

                        setPointBytes = BitConverter.GetBytes(aceleracao).Reverse().ToArray();
                        break;
                    }
                case OidFuncCode.Heartbeat:
                    {
                        setPointBytes = new byte[] { (byte)setPoint };
                        break;
                    }
                default:
                    {
                        Console.WriteLine($"[OID] Comando nao mapeado: {funcCode.ToString()}");
                        break;
                    }
            }

            if (setPointBytes == null)
                return;

            if (funcCode != OidFuncCode.Heartbeat)
            {
                //Console.WriteLine($"[OID] Comando enviado para fila: {idTx.ToString("X")} {((byte)funcCode).ToString("X")} {string.Join(" ", setPointBytes.Select(x => x.ToString("X")))}");
            }

            EnviarComando(Dispositivo, idTx, idRx, (byte)funcCode, setPointBytes, false);
        }

        private static void ConfigurarDriver(MovimentacaoModel Modulo)
        {
            byte addrTx = Modulo._EnderecoCAN_Tx;
            byte addrRx = Modulo._EnderecoCAN_Rx;

            ComandoControle(addrTx, addrRx, OidFuncCode.ComandoCorrenteFreio, 3.0);
            ComandoControle(addrTx, addrRx, OidFuncCode.ComandoVelocidade, 0.0);
            ComandoControle(addrTx, addrRx, OidFuncCode.AjusteCorrenteMaxima, Modulo.CorrenteTorqueMaximo);
            ComandoControle(addrTx, addrRx, OidFuncCode.AjusteAceleracaoVelocidade, Modulo.Rampa);
            ComandoControle(addrTx, addrRx, OidFuncCode.AjusteDesaceleracaoVelocidade, Modulo.Rampa);

            DadosLeitura.Where(x => x.EnderecoTx == addrTx).First().Configurado = true;
        }

        #endregion

        private static async Task tmrHeartbeat_Tick()
        {
            foreach (var Mod in Variaveis.OperacaoEmAndamento.DispMvd?.Dados?.Modulos)
            {
                if (Mod.MovMotor.Inicializado)
                {
                    int novoValor = Mod.MovMotor.heartBeatValue == 0 ? 1 : 0;

                    ComandoControle(Mod.MovMotor._EnderecoCAN_Tx, Mod.MovMotor._EnderecoCAN_Rx, OidFuncCode.Heartbeat, novoValor);

                    Mod.MovMotor.heartBeatValue = novoValor;
                }
            }
        }

        public class OidHandler : ICanMessageHandler
        {
            public T_Code Dispositivo { get; set; } = OIDCanService.Dispositivo;

            public enum OidFuncCode : byte
            {
                Heartbeat = 0x00, 
                ComandoCorrente = 0x01, 
                ComandoVelocidade = 0x02, 
                ComandoCicloTrabalho = 0x03, 
                ComandoPosicaoAbsoluta = 0x04, 
                ComandoIncrementoPosicaoRelativaAnterior = 0x05, 
                ComandoIncrementoPosicaoRelativaAtual = 0x06, 
                ComandoPosicaoAtual = 0x07, 
                ComandoCorrenteFreio= 0x08, 
                ComandoCorrenteFreioMao = 0x09, 
                AjusteAceleracaoVelocidade = 0x0A, 
                AjusteVelocidadeMaxima = 0x0B, 
                AjusteAceleracaoeMaxima = 0x0C, 
                AjusteDesaceleracaoeMaxima = 0x0D, 
                AlterarTabelaConfiguracao = 0x0E, 
                Consulta = 0x0F, 
                AjusteDesaceleracaoVelocidade = 0x10, 
                ComandoRetornarZero = 0x11, 
                ComandoCancelarRetornoZero = 0x12, 
                ConsultaStatusRetornoZero = 0x13, 
                AjusteCorrenteMaxima = 0x14, 
                AjusteAceleracaoSubidaTorque = 0x15, 
                AjusteRampaTorque = 0x16, 
            }

            public enum OidParametros : byte
            {
                CodErro = 0, 
                VelocidadeAtual = 1,
                CicloTrabalho = 2,
                Potencia = 3,
                Tensao = 4,
                CorrenteMotor = 5,
                CorrenteBarramento = 6,
                Temperatura = 7,
                PosicaoAbsoluta = 8,
                PosicaoRelativa = 9,
                ModoCodificador = 10,
                ErroLerCache = 11,
                IO_Status = 12
            }

            public enum OidCodigoErro : byte
            {
                NenhumErro = 0,
                Sobretensao = 1,
                Subtensao = 2,
                CorrenteMaximaExcedida = 3,
                TemperaturaMOS = 4,
                SubtensaoMCU = 5,
                Watchdog = 6,
                ErroSPI = 7,
                FlashDanificado = 8,
                DeslocamentoSensorU = 9,
                DeslocamentoSensorV = 10,
                DeslocamentoSensorW = 11,
                DesequilibrioTrifasico = 12,
                MotorFlashCorrompido = 13,
                AplicacaoFlashCorrompida = 14,
                PulsacaoCANOPEN = 15,
                Parado = 16,
                Parar = 17,
                ForaDeTolerancia = 18,
                SinalZNaoEncontrado = 19
            }

            private OIDModel ObterOuCriar(byte idTx, byte idRx)
            {
                lock (DadosLeitura)
                {
                    var item = DadosLeitura.FirstOrDefault(x => x.EnderecoTx == idTx && x.EnderecoRx == idRx);
                    if (item == null)
                    {
                        item = new OIDModel { EnderecoTx = idTx, EnderecoRx = idRx };
                        DadosLeitura.Add(item);
                    }
                    return item;
                }
            }

            public void ProcessarMensagem(CanMessage mensagem)
            {
                OidFuncCode func = (OidFuncCode)mensagem.DataRx[0];
                OidParametros parametro = (OidParametros)mensagem.DataRx[1];
                byte[] valorBytes = mensagem.DataRx.Skip(2).ToArray();

                if(BitConverter.IsLittleEndian)
                    Array.Reverse(valorBytes);

                //Console.WriteLine($"[OID] Dados brutos recebidos do driver {mensagem.IdTx} - {FuncoesGlobais.ConverterComandoBytesParaTexto(mensagem.DataRx)}");

                var item = ObterOuCriar(mensagem.IdTx, mensagem.IdRx);

                try
                {
                    switch (parametro)
                    {
                        case OidParametros.CodErro:
                            if (valorBytes.Length >= 2)
                            {
                                short codigo = BitConverter.ToInt16(valorBytes, 0);
                                item.CodigoAlarme.valor = (OidCodigoErro)codigo;
                            }
                            break;

                        case OidParametros.VelocidadeAtual:
                            if (valorBytes.Length == 4)
                                item.ERPM.valor = BitConverter.ToInt32(valorBytes, 0);
                            break;

                        case OidParametros.CicloTrabalho:
                            if (valorBytes.Length >= 2)
                                item.CicloTrabalho.valor = BitConverter.ToInt16(valorBytes, 0) / 10.0; // em %
                            break;

                        case OidParametros.Potencia:
                            if (valorBytes.Length >= 2)
                                item.Potencia.valor = BitConverter.ToInt16(valorBytes, 0); // em Watts
                            break;

                        case OidParametros.Tensao:
                            if (valorBytes.Length >= 2)
                                item.Tensao.valor = BitConverter.ToInt16(valorBytes, 0); // em Volts
                            break;

                        case OidParametros.CorrenteMotor:
                            if (valorBytes.Length >= 2)
                                item.CorrenteMotor.valor = BitConverter.ToInt16(valorBytes, 0) / 100.0; // em A
                            break;

                        case OidParametros.CorrenteBarramento:
                            if (valorBytes.Length >= 2)
                                item.CorrenteBarramento.valor = BitConverter.ToInt16(valorBytes, 0) / 100.0; // em A
                            break;

                        case OidParametros.Temperatura:
                            if (valorBytes.Length >= 2)
                                item.Temperatura.valor = BitConverter.ToInt16(valorBytes, 0); // em °C
                            break;

                        case OidParametros.PosicaoAbsoluta:
                            if (valorBytes.Length == 4)
                                item.PosicaoAbsoluta = BitConverter.ToInt32(valorBytes, 0) / 100.0; // em graus
                            break;

                        case OidParametros.PosicaoRelativa:
                            if (valorBytes.Length == 4)
                                item.PosicaoRelativa = BitConverter.ToInt32(valorBytes, 0) / 100.0; // em graus
                            break;

                        case OidParametros.ModoCodificador:
                            if (valorBytes.Length >= 2)
                                item.ModoCodificador = BitConverter.ToInt16(valorBytes, 0); // 0 ou 1 geralmente
                            break;

                        case OidParametros.ErroLerCache:
                            if (valorBytes.Length >= 2)
                                item.ErroHistorico = BitConverter.ToInt16(valorBytes, 0);
                            break;

                        case OidParametros.IO_Status:
                            if (valorBytes.Length >= 2)
                                item.IO_Status = BitConverter.ToUInt16(valorBytes, 0); // leitura binária
                            break;
                    }

                    item.UltimoComandoRecebido = DateTime.Now;
                }
                catch (Exception ex)
                {
                    Console.WriteLine($"[OID] Erro ao interpretar resposta CAN ({parametro}): {ex.Message}");
                }
            }
        }
    }

}