// Dispositivo: Movimentação e Direcional Unificados
// Versão Firmware: 3
// Ultima atualização: 18/06/2024
// Atualização: Controle do driver BLD300R através dos pinos

#include "C:\ZendionInc\agrobot_base\Firmware\Modulos\SerialService.h"
#include "C:\ZendionInc\agrobot_base\Firmware\Modulos\ModbusService.h"
#include "C:\ZendionInc\agrobot_base\Firmware\Modulos\utils.h"
#include "C:\ZendionInc\agrobot_base\Firmware\Modulos\Pinout.h"
#include <SimpleKalmanFilter.h>

#define D_Code Mvd
#define VERSION 3
String Mod_ID = "";

#define _pinoAddr0 36
#define _pinoAddr1 39

int _pinModbusRX = 16;
int _pinModbusTX = 17;
int _pinModbusMD = 4;

void AtualizarEnderecoModulo() {
  bool A0 = digitalRead(_pinoAddr0) == 1;
  bool A1 = digitalRead(_pinoAddr1) == 1;

  if (!A0 && !A1) {
    Mod_ID = "ET";
  }
  else if (A0 && !A1) {
    Mod_ID = "EF";
  }
  else if (!A0 && A1) {
    Mod_ID = "DT";
  }
  else if (A0 && A1) {
    Mod_ID = "DF";
  }
}

long _baudRate = 115200;
volatile bool Conectado = false;
volatile int _TaxaAmostragem = 500;
volatile bool RequisitarDados = false;


class MotorBLDC {
  public:
    String _ID = "Mov";
    int _canal = 4;

    volatile bool Iniciado = false;
    bool Testando = false;

    PinoModel _pinoEN;
    PinoModel _pinoFR;
    PinoModel _pinoSV;
    PinoModel _pinoBK;

    PinoModel _pinoTMP;
    SimpleKalmanFilter tempKalman = SimpleKalmanFilter(2, 2, 0.01);

    byte _endereco;
    int _numPolos;
    int _rampa;
    int _rpmMax;

    // Consumo
    double RPM;
    double RPM_SP;
    double Temperatura;
    double TemperaturaDriver;
    double Tensao;
    double CorrenteMax;
    bool Ligado;
    bool Freio;
    Sentido _Sentido = Parado;
    int CodAlarme;
    
    // Entrada de dados
    double _RPM_SP;
    bool _Ligado = false;
    bool _Freio = false;
    Sentido _SentidoSP = Parado;

    volatile bool ComandoEnviado = false;
    
    
    void Inicializar() {
      if (Iniciado) {
        EnviarDadosSerial(Mod_ID, MontarProtocoloSensor(sCFG, _ID, Iniciado ? "1" : "0"));
        return;
      }

      Iniciado = ConfigurarDriver();

      if (_pinoEN.Definido()) {
        _pinoEN.Conectar(true);
      }
      if (_pinoFR.Definido()) {
        _pinoFR.Conectar();
      }
      if (_pinoBK.Definido()) {
        _pinoBK.Conectar(true);
      }
      if (_pinoSV.Definido()) {
        AtualizarPWM();
      }

      if (_pinoTMP.Definido()) {
        _pinoTMP.Conectar();
      }

      if (Iniciado) {
        AtualizarDadosControle(true);
      }

      EnviarDadosSerial(Mod_ID, MontarProtocoloSensor(sCFG, _ID, Iniciado ? "1" : "0"));
    }

    void Desligar() {
      if (!Iniciado) {
        //EnviarDadosSerial(Mod_ID, _ID + " nao esta inicializado");
        EnviarDadosSerial(Mod_ID, MontarProtocoloSensor(sCFG, _ID, Iniciado ? "1" : "0"));
        return;
      }

      // Parar a execução das tarefas
      //vTaskDelete(MovTaskHandle);

      // Redefinir as configurações para os valores iniciais
      _pinoEN.Desconectar();
      _pinoFR.Desconectar();
      _pinoSV.Desconectar();
      _pinoBK.Desconectar();
      _pinoTMP.Desconectar();

      //EnviarDadosSerial(Mod_ID, _ID + " Desligado");

      Iniciado = false;

      EnviarDadosSerial(Mod_ID, MontarProtocoloSensor(sCFG, _ID, Iniciado ? "1" : "0"));

      AtualizarDadosControle(true);
    }

    void AtualizarDadosControle(bool reset = false) {
      if (!Iniciado) {
        return;
      }

      if (reset) {
        _Ligado = false;
        _Freio = false;
        _SentidoSP = Parado;
        _RPM_SP = 0;
      }

      int val = map(_RPM_SP, 0, _rpmMax, 0, 1023);
      ledcWrite(_canal, val);
      _pinoFR.set(_SentidoSP == Horario ? false : true);
      _pinoBK.set(!_Freio);
      _pinoEN.set(!_Ligado);
      

      /*ComandoEnviado = true;
      EnviarComandoModbus(_endereco, 0x06, 0x006D, _SentidoSP == Horario ? 1 : 0, false);
      EnviarComandoModbus(_endereco, 0x06, 0x0056, _RPM_SP, false);
      EnviarComandoModbus(_endereco, 0x06, 0x006A, _Freio ? 1 : 0, false);
      EnviarComandoModbus(_endereco, 0x06, 0x0066, _Ligado ? 1 : 0, false);
      vTaskDelay(100);
      ComandoEnviado = false;*/
    }

    void RequisitarDados() {
      AferirDadosDriver();
      AferirTemperatura();

      //EnviarDadosSerial(Mod_ID, MontarProtocoloSensor(sBLD, _ID, MontarDadosProtocoloBLD(RPM, Temperatura, Tensao, CorrenteMax, Ligado, _Sentido, Freio, CodAlarme, TemperaturaDriver)));
    }

  private:
    ModbusGetResponse EnviarComandoModbus(byte endereco, byte funcao, byte registro, int valor, bool aguardarResposta = true) {
      const int maxTentativas = 2;
      int tentativas = 0;
      bool sucesso = false;
      ModbusGetResponse resposta;

      while (tentativas < maxTentativas && !sucesso) {
        uint8_t response[256];
        uint8_t length = 0;

        sendModbusCommand(endereco, funcao, registro, valor, response, length, aguardarResposta);

        if (aguardarResposta) {
          if (funcao == 0x03) {
            if (length >= 5) {
              resposta._success = true;
              resposta._address = response[0];
              resposta._command_code = response[1];
              resposta._num_registers = response[2];

              if (resposta._num_registers == 2) {
                uint16_t value = (response[3] << 8) | response[4];
                resposta._value = static_cast<double>(value);
                sucesso = true;
              } else if (resposta._num_registers == 4 && length >= 7) {
                uint16_t highBytes = (response[3] << 8) | response[4];
                uint16_t lowBytes = (response[5] << 8) | response[6];
                
                // Combinar os bytes de alta e baixa ordem em um valor de 32 bits
                uint32_t combined = (static_cast<uint32_t>(highBytes) << 16) | lowBytes;

                // Reinterpretar o valor combinado como um float
                float floatValue;
                memcpy(&floatValue, &combined, sizeof(floatValue));

                resposta._value = static_cast<double>(floatValue);
                
                sucesso = true;
              } else {
                resposta._success = false;
              }
            } else {
              resposta._success = false;
            }
          } else if (funcao == 0x06) {
            if (length >= 8) {
              resposta._success = true;
              resposta._address = response[0];
              resposta._command_code = response[1];
              resposta._num_registers = 0;
              resposta._value = (response[4] << 8) | response[5];
              sucesso = true;
            } else {
              Serial.println("Erro ao enviar comando");
              resposta._success = false;
            }
          }

          tentativas++;
        } else {
          sucesso = true;
          resposta._success = sucesso;
        }
      }

      return resposta;
    }

    bool ConfigurarDriver() {
      ModbusGetResponse a = EnviarComandoModbus(_endereco, 0x06, 0x00B6, 1);
      if (a._success) {
        ModbusGetResponse b = EnviarComandoModbus(_endereco, 0x06, 0x0076, 0);
        if (b._success) {
          ModbusGetResponse c = EnviarComandoModbus(_endereco, 0x06, 0x0086, _numPolos);
          if (c._success) {
            ModbusGetResponse d = EnviarComandoModbus(_endereco, 0x06, 0x008A, _rampa);
            if (d._success) {
              ModbusGetResponse e = EnviarComandoModbus(_endereco, 0x06, 0x0092, _rpmMax);
              if (e._success) {
                ModbusGetResponse f = EnviarComandoModbus(_endereco, 0x06, 0x00B6, 0);
                return f._success;
              }
            }
          }
        }
      }
      return false;
    }

    void AferirDadosDriver() {
      AguardaEnvioComando();
      RPM_SP = EnviarComandoModbus(_endereco, 0x03, 0x0056, 1)._value;
      AguardaEnvioComando();
      RPM = EnviarComandoModbus(_endereco, 0x03, 0x005F, 1)._value;
      AguardaEnvioComando();
      Ligado = EnviarComandoModbus(_endereco, 0x03, 0x0066, 1)._value == 1;
      AguardaEnvioComando();
      Freio = EnviarComandoModbus(_endereco, 0x03, 0x006A, 1)._value == 1;
      AguardaEnvioComando();
      _Sentido = Ligado ? EnviarComandoModbus(_endereco, 0x03, 0x006D, 1)._value == 1 ? Horario : Antihorario : Parado;
      AguardaEnvioComando();
      CodAlarme = EnviarComandoModbus(_endereco, 0x03, 0x0076, 1)._value;
      AguardaEnvioComando();
      Tensao = EnviarComandoModbus(_endereco, 0x03, 0x00C8, 2)._value;
      AguardaEnvioComando();
      CorrenteMax = EnviarComandoModbus(_endereco, 0x03, 0x0096, 2)._value;
      AguardaEnvioComando();
      TemperaturaDriver = EnviarComandoModbus(_endereco, 0x03, 0x00D2, 2)._value;
    }

    void AferirTemperatura() {
      int leitura = _pinoTMP.get();
      float leituraNormalizada = tempKalman.updateEstimate(leitura);
      Temperatura = leituraNormalizada;
    }

    void AguardaEnvioComando() {
      while (ComandoEnviado) {
        vTaskDelay(50);
      }
    }

};

class MotorPasso {
  public:
    String _ID = "Dir";
    int _canal = 0;

    volatile bool Iniciado = false;
    bool Testando = false;

    // Definições
    PinoModel _pinoENA;
    PinoModel _pinoDIR;
    PinoModel _pinoPUL;
    PinoModel _pinoEncoder;

    SimpleKalmanFilter encoderKalman = SimpleKalmanFilter(1, 2, 0.01);
    
    // Consumo
    double _Angulo;
    Sentido _Sentido;
    
    // Entrada de dados
    double _Angulo_SP;
    int _Angulo_Offset = 0;
    Sentido _Sentido_SP = Parado;
    bool _MotorLigado = false;
    bool _Estabilizar = true;
    double _frequencia = 4000;

    int FrequenciaMaxima = 10000;
    int dutyCycle = 512;


    void Inicializar() {
      if (Iniciado) {
        //EnviarDadosSerial(Mod_ID, _ID + " ja inicializado");
        EnviarDadosSerial(Mod_ID, MontarProtocoloSensor(sCFG, _ID, Iniciado ? "1" : "0"));
        return;
      }

      Iniciado = true;

      if (_pinoPUL.Definido()) {
        AtualizarPWM();
      }
      _pinoENA.Conectar(true);
      _pinoDIR.Conectar(false);
      _pinoEncoder.Conectar();

      _Angulo_SP = 0;
      _MotorLigado = false;
      VerificarAnguloSPparado(true);

      xTaskCreatePinnedToCore(&MotorPasso::DirTaskWrapper, "DirTask", 5000, this, 18, &DirTaskHandle, tskNO_AFFINITY);

      //EnviarDadosSerial(Mod_ID, _ID + " Iniciado");

      EnviarDadosSerial(Mod_ID, MontarProtocoloSensor(sCFG, _ID, Iniciado ? "1" : "0"));
    }

    void Desligar() {
      if (!Iniciado) {
        //EnviarDadosSerial(Mod_ID, _ID + " nao esta inicializado");
        EnviarDadosSerial(Mod_ID, MontarProtocoloSensor(sCFG, _ID, Iniciado ? "1" : "0"));
        return;
      }

      // Parar a execução das tarefas
      vTaskDelete(DirTaskHandle);

      // Redefinir as configurações para os valores iniciais
      _pinoENA.Desconectar();
      _pinoDIR.Desconectar();
      _pinoPUL.Desconectar();
      _pinoEncoder.Desconectar();
      
      
      //EnviarDadosSerial(Mod_ID, _ID + " Desligado");

      Iniciado = false;

      EnviarDadosSerial(Mod_ID, MontarProtocoloSensor(sCFG, _ID, Iniciado ? "1" : "0"));
    }

    void RequisitarDados() {
      //EnviarDadosSerial(Mod_ID, MontarProtocoloSensor(sENC, _ID, MontarDadosProtocoloEncoderAbsoluto(_Sentido, _Angulo)));
    }

  
  private:

    double _Margem = 0.5;
    double _AgMin = 95;
    double _AgMax = 855;
    int RefMin = -90;
    int RefMax = 90;

    double _anguloAnterior = 0;
    double _anguloAtual = 0;
    double _taxaMudancaAngulo = 0;

    TaskHandle_t DirTaskHandle = NULL;
    static void DirTaskWrapper(void *pvParameters) {
      MotorPasso* motor = static_cast<MotorPasso*>(pvParameters);
      motor->DirTask();
    }

    void DirTask() {
      unsigned long previousMillisRampa = millis();
    
      while (true) {
        if (!Conectado) {
          vTaskDelay(1000);
          continue;
        }

        AferirPosicao();
        DefinirSentidoDeGiro();
        VerificarAnguloSP();
        Atualizar();

        /*if (millis() - previousMillisRampa >= _TaxaAmostragem) {
          previousMillisRampa = millis();

          RequisitarDados();
        }*/

        vTaskDelay(1);
      }
    }

    void AferirPosicao() {
      if (_pinoEncoder.Definido()) {
        int leitura = _pinoEncoder.get();
        float leituraNormalizada = encoderKalman.updateEstimate(leitura);
        double _angulo = fmap(leituraNormalizada, _AgMin, _AgMax, RefMin, RefMax);

        double angulo_com_offset = _angulo - _Angulo_Offset;

        // Normaliza o ângulo para o intervalo de -90 a 90 graus
        while (angulo_com_offset > RefMax) angulo_com_offset -= 180;
        while (angulo_com_offset < RefMin) angulo_com_offset += 180;

        _Angulo = angulo_com_offset;
      }
    }

    void DefinirSentidoDeGiro() {
      _anguloAtual = _Angulo;

      // Calcular a taxa de mudança do ângulo
      _taxaMudancaAngulo = (_anguloAtual - _anguloAnterior) / (1.0 / 1000.0); // Supondo que a leitura é feita a cada 1 ms

      if (_MotorLigado) {
        if (_taxaMudancaAngulo > 0) {
          _Sentido = Horario;
        } else if (_taxaMudancaAngulo < 0) {
          _Sentido = Antihorario;
        } else {
          _Sentido = Parado;
        }
      } else {
        _Sentido = Parado;
      }

      _anguloAnterior = _anguloAtual;
    }

    unsigned long previousMillisCorrecao = millis();
    void VerificarAnguloSP() {
      bool ModoSP = (_Angulo_SP > -1);
      if (_MotorLigado) {
        double AnguloComparar = ModoSP ? _Angulo_SP : RefMax;
        if (_Sentido_SP == Horario && _Angulo >= AnguloComparar) {
          _MotorLigado = false;
        }
        else if (_Sentido_SP == Antihorario && _Angulo <= (AnguloComparar * -1)) {
          _MotorLigado = false;
        }
      }
      else if (ModoSP) {
        if (millis() - previousMillisCorrecao >= _TaxaAmostragem) {
          previousMillisCorrecao = millis();

          VerificarAnguloSPparado(_Estabilizar);
        }
      }
    }

    void VerificarAnguloSPparado(bool _estabilizar) {
      bool NoRangeZero = (_Angulo >= -_Margem && _Angulo <= _Margem);
      bool NoRangeSP = (_Angulo >= (_Angulo_SP - _Margem) && _Angulo <= (_Angulo_SP + _Margem));
      if (_Angulo_SP == 0) {
        if (!NoRangeZero && _estabilizar) {
          _Sentido_SP = _Angulo > 0 ? Antihorario : Horario;
          _MotorLigado = true;
        }
      }
      else if (!NoRangeSP) {
        _MotorLigado = true;
      }
    }

    void Atualizar() {
      _pinoENA.set(!_MotorLigado);
      _pinoDIR.set(_Sentido_SP != Antihorario);
    }


};

MotorBLDC MotorMOV;
MotorPasso MotorDIR;

void AtualizarPWM() {
  if (MotorDIR.Iniciado && MotorDIR._pinoPUL.Definido()) {
    ledcSetup(MotorDIR._canal, MotorDIR._frequencia, 10);
    ledcAttachPin(MotorDIR._pinoPUL.num, MotorDIR._canal);
    ledcWrite(MotorDIR._canal, MotorDIR.dutyCycle);
  }
  if (MotorMOV.Iniciado && MotorMOV._pinoSV.Definido()) {
    ledcSetup(MotorMOV._canal, 1000, 10);
    ledcAttachPin(MotorMOV._pinoSV.num, MotorMOV._canal);
    ledcWrite(MotorMOV._canal, MotorMOV._RPM_SP);
  }
}

void SensoriamentoTask(void *pvParameters);
TaskHandle_t SensoriamentoTaskHandle = NULL;

void setup() {
  pinMode(_pinoAddr0, INPUT);
  pinMode(_pinoAddr1, INPUT);

  AtualizarEnderecoModulo();

  Serial.begin(_baudRate, SERIAL_8N1, 1, 3);

  analogReadResolution(10);

  delay(10);

  ConfigurarModbus(true, _pinModbusRX, _pinModbusTX, _pinModbusMD);

  delay(10);

  if (xTaskCreatePinnedToCore(SensoriamentoTask, "SensoriamentoTask", 8000, NULL, 5, &SensoriamentoTaskHandle, tskNO_AFFINITY) != pdPASS) {
    Serial.println("Erro ao criar tarefa SensoriamentoTask");
  }
}

void SensoriamentoTask(void *pvParameters) {
  while (1)
  {
    Conectado = MotorMOV.Iniciado || MotorDIR.Iniciado;

    if (!RequisitarDados && Conectado) {
      
      if (MotorMOV.Iniciado) {
        MotorMOV.RequisitarDados();
      }
      if (MotorDIR.Iniciado) {
        MotorDIR.RequisitarDados();
      }

      EnviarDadosSerial(Mod_ID, MontarProtocoloSensor(sMVD, Mod_ID, MontarDadosProtocoloMVD(
        MotorMOV.Iniciado, 
        MotorMOV.RPM, 
        MotorMOV.Temperatura, 
        MotorMOV.Tensao, 
        MotorMOV.CorrenteMax, 
        MotorMOV.Ligado, 
        MotorMOV._Sentido, 
        MotorMOV.Freio, 
        MotorMOV.CodAlarme, 
        MotorMOV.TemperaturaDriver, 
        MotorDIR.Iniciado, 
        MotorDIR._Sentido, 
        MotorDIR._Angulo
      )));
    }
    vTaskDelay(_TaxaAmostragem);
  }
}

void loop() {
  std::vector<ProtocoloSerial> protocolos = LerBufferSerialFila(Mod_ID);

  for (int i = 0; i < protocolos.size(); i++) {
    ProcessarProtocolo(protocolos[i]);
  }

  delay(1);
}

void ProcessarProtocolo(ProtocoloSerial Mensagem) {
  switch (Mensagem.funcao) {
    case Chk:
      ProcessarChk(Mensagem);
      break;
    case Cfg:
      ProcessarCfg(Mensagem);
      break;
    case Cmd:
      ProcessarCmd(Mensagem);
      break;
    case Req:
      ProcessarReq(Mensagem);
      break;
  }

  if (Mensagem.idMensagem > 0) {
    EnviarDadosSerial(Mod_ID, MontarProtocoloMensagemRecebida(Mensagem.idMensagem));
  }
}

void ProcessarChk(ProtocoloSerial Mensagem) {
  Conectado = MotorMOV.Iniciado || MotorDIR.Iniciado;

  EnviarDadosSerial(Mod_ID, MontarProtocoloVerificacao(D_Code, VERSION, Conectado));
}

void ProcessarCfg(ProtocoloSerial Mensagem) {
  std::vector<String> Partes = SplitString(Mensagem.protocolo, SplitParams);

  String ID = Partes[0];
  bool Conectar = Partes[1] == "1";

  if (ID == Mod_ID) {
    _TaxaAmostragem = Partes[2].toInt();
    RequisitarDados = Partes[3].toInt();
    TiposBarramentos txB = (TiposBarramentos)Partes[4].toInt();
    int txP = Partes[5].toInt();
    TiposBarramentos rxB = (TiposBarramentos)Partes[6].toInt();
    int rxP = Partes[7].toInt();
    TiposBarramentos MtxB = (TiposBarramentos)Partes[8].toInt();
    int MtxP = Partes[9].toInt();
    TiposBarramentos MrxB = (TiposBarramentos)Partes[10].toInt();
    int MrxP = Partes[11].toInt();
    TiposBarramentos MmdB = (TiposBarramentos)Partes[12].toInt();
    int MmdP = Partes[13].toInt();
    _pinModbusMD = MmdP;
    _pinModbusRX = MrxP;
    _pinModbusTX = MtxP;

    //ConfigurarModbus(Conectar, _pinModbusRX, _pinModbusTX, _pinModbusMD);
  }
  else if (ID == MotorMOV._ID) {
    if (Conectar) {
      MotorMOV._endereco = stringToByte(Partes[2]);
      MotorMOV._numPolos = Partes[3].toInt();
      MotorMOV._rampa = Partes[4].toInt();
      MotorMOV._rpmMax = Partes[5].toInt();
      MotorMOV._pinoTMP = PinoModel(Partes[7].toInt(), (TiposBarramentos)Partes[6].toInt(), _INPUT, ANALOGICO);
      MotorMOV._pinoEN = PinoModel(Partes[9].toInt(), (TiposBarramentos)Partes[8].toInt(), _OUTPUT);
      MotorMOV._pinoFR = PinoModel(Partes[11].toInt(), (TiposBarramentos)Partes[10].toInt(), _OUTPUT);
      MotorMOV._pinoSV = PinoModel(Partes[13].toInt(), (TiposBarramentos)Partes[12].toInt(), _PWM);
      MotorMOV._pinoBK = PinoModel(Partes[15].toInt(), (TiposBarramentos)Partes[14].toInt(), _OUTPUT);
            
      MotorMOV.Inicializar();
    }
    else {
      MotorMOV.Desligar();
    }
  }
  else if (ID == MotorDIR._ID) {
    if (Conectar) {
      MotorDIR._Estabilizar = Partes[2] == "1";
      MotorDIR._Angulo_Offset = Partes[3].toInt();
      MotorDIR._pinoPUL = PinoModel(Partes[5].toInt(), (TiposBarramentos)Partes[4].toInt(), _PWM);
      MotorDIR._pinoDIR = PinoModel(Partes[7].toInt(), (TiposBarramentos)Partes[6].toInt(), _OUTPUT);
      MotorDIR._pinoENA = PinoModel(Partes[9].toInt(), (TiposBarramentos)Partes[8].toInt(), _OUTPUT);
      MotorDIR._pinoEncoder = PinoModel(Partes[11].toInt(), (TiposBarramentos)Partes[10].toInt(), _INPUT, ANALOGICO);

      MotorDIR.Inicializar();
    }
    else {
      MotorDIR.Desligar();
    }
  }
}

void ProcessarCmd(ProtocoloSerial Mensagem) {
  std::vector<String> Partes = SplitString(Mensagem.protocolo, SplitParams);

  String ID = Partes[0];

  if (ID == MotorMOV._ID && MotorMOV.Iniciado) {
    MotorMOV._SentidoSP = (Sentido)Partes[1].toInt();
    MotorMOV._Ligado = MotorMOV._SentidoSP != Parado;
    MotorMOV._RPM_SP = Partes[2].toInt();
    MotorMOV._Freio = Partes[3] == "1";

    MotorMOV.AtualizarDadosControle();
  }
  else if (ID == MotorDIR._ID && MotorDIR.Iniciado) {
    MotorDIR._Sentido_SP = (Sentido)Partes[1].toInt();
    MotorDIR._Angulo_SP = Partes[2].toInt();
    MotorDIR._frequencia = (Partes[3].toInt() / 100.0) * MotorDIR.FrequenciaMaxima;

    AtualizarPWM();

    MotorDIR._MotorLigado = MotorDIR._Sentido_SP != Parado;
  }
}

void ProcessarReq(ProtocoloSerial Mensagem) {
  //std::vector<String> Partes = SplitString(Mensagem.protocolo, SplitParams);
  //Serial.println("Processando req...");
  if (MotorMOV.Iniciado) {
    MotorMOV.RequisitarDados();
  }
  if (MotorDIR.Iniciado) {
    MotorDIR.RequisitarDados();
  }

  if (MotorMOV.Iniciado || MotorDIR.Iniciado) {
    //Serial.println("Enviando dados...");
    EnviarDadosSerial(Mod_ID, MontarProtocoloSensor(sMVD, Mod_ID, MontarDadosProtocoloMVD(
      MotorMOV.Iniciado, 
      MotorMOV.RPM, 
      MotorMOV.Temperatura, 
      MotorMOV.Tensao, 
      MotorMOV.CorrenteMax, 
      MotorMOV.Ligado, 
      MotorMOV._Sentido, 
      MotorMOV.Freio, 
      MotorMOV.CodAlarme, 
      MotorMOV.TemperaturaDriver, 
      MotorDIR.Iniciado, 
      MotorDIR._Sentido, 
      MotorDIR._Angulo
    )));
  }
}