agrobot_base/Firmware/MovUnificado/MovUnificado_v1/MovUnificado_v1.ino

// Dispositivo: Movimentação e Direcional Unificados
// Versão Firmware: 1
// Ultima atualização: 06/06/2024
// Atualização: Unificação dos módulos direcional e movimentação 

#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 1
String Mod_ID = "";
int Cod_Alarme = 0;

#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;
bool Conectado = false;
int _TaxaAmostragem = 500;


class MotorBLDC {
  public:
    String _ID = "Mov";

    bool Iniciado = false;
    bool Testando = false;

    PinoModel _pinoTMP;
    SimpleKalmanFilter* tempKalman = nullptr;

    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;
    
    
    void Inicializar() {
      if (Iniciado) {
        EnviarDadosSerial(Mod_ID, _ID + " ja inicializado");
        EnviarDadosSerial(Mod_ID, MontarProtocoloSensor(sCFG, _ID, Iniciado ? "1" : "0"));
        return;
      }

      Iniciado = ConfigurarDriver();

      if (_pinoTMP.Definido()) {
        _pinoTMP.Conectar();
        tempKalman = new (std::nothrow) SimpleKalmanFilter(2, 2, 0.01);
      }

      if (Iniciado) {
        AtualizarDadosControle(true);

        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;
      }

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

      // Outras redefinições de variáveis de estado, se necessário
      delete tempKalman;
      tempKalman = nullptr;

      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;
      }

      if (_SentidoSP != Parado && _SentidoSP != _Sentido) {
        int std = EnviarComandoModbus(_endereco, 0x06, 0x006D, _SentidoSP == Horario ? 1 : 0)._value;
        _Sentido = !Ligado ? Parado : std == 0 ? Horario : Antihorario;
      }
      if (_RPM_SP != RPM_SP) {
        RPM_SP = EnviarComandoModbus(_endereco, 0x06, 0x0056, _RPM_SP)._value;
      }
      if (_Freio != Freio) {
        Freio = EnviarComandoModbus(_endereco, 0x06, 0x006A, _Freio ? 1 : 0)._value == 1;
      }
      if (_Ligado != Ligado) {
        Ligado = EnviarComandoModbus(_endereco, 0x06, 0x0066, _Ligado ? 1 : 0)._value == 1;
      }

      /*if (_SentidoSP != Parado && _SentidoSP != _Sentido) {
        EnviarComandoModbus(_endereco, 0x06, 0x006D, _SentidoSP == Horario ? 1 : 0, false);
      }
      if (_RPM_SP != RPM_SP) {
        EnviarComandoModbus(_endereco, 0x06, 0x0056, _RPM_SP, false);
      }
      if (_Freio != Freio) {
        EnviarComandoModbus(_endereco, 0x06, 0x006A, _Freio ? 1 : 0, false);
      }
      if (_Ligado != Ligado) {
        EnviarComandoModbus(_endereco, 0x06, 0x0066, _Ligado ? 1 : 0, false);
      }*/

      /*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);*/
      
    }

    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) {
            // GET
            if (funcao == 0x03) {
                if (length >= 5) { // Verifica se a resposta tem pelo menos 5 bytes (endereco, codigo_comando, numero_registros, 2 bytes de dados, 2 bytes de CRC)
                    resposta._success = true;
                    resposta._address = response[0];
                    resposta._command_code = response[1];
                    resposta._num_registers = response[2];

                    if (resposta._num_registers == 2) { // Caso de 2 bytes (sem ponto flutuante)
                        uint16_t value = (response[3] << 8) | response[4]; // Combina os dois bytes
                        resposta._value = static_cast<double>(value); // Converte para double
                        sucesso = true;
                    } else if (resposta._num_registers == 4 && length >= 7) { // Caso de 4 bytes (com ponto flutuante)
                        uint16_t highBytes = (response[3] << 8) | response[4]; // Combina os dois primeiros bytes
                        uint16_t lowBytes = (response[5] << 8) | response[6];  // Combina os dois últimos bytes

                        // Converte para double
                        resposta._value = highBytes + (lowBytes / 10000.0); // Supondo que os valores após a vírgula são representados em 4 dígitos decimais
                        sucesso = true;
                    } else {
                        Serial.println("Numero de registros inesperado ou resposta incompleta.");
                        resposta._success = false;
                    }
                } else {
                    Serial.println("Resposta incompleta.");
                    resposta._success = false;
                }
            }
            // SET
            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);
              return e._success;
            }
          }
        }
      }
      return false;
    }

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

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

};

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

    bool Iniciado = false;
    bool Testando = false;

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

    SimpleKalmanFilter* encoderKalman = nullptr;
    
    // 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;
      }

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

      _Angulo_SP = 0;
      _MotorLigado = false;

      encoderKalman = new (std::nothrow) SimpleKalmanFilter(1, 2, 0.01);
      
      xTaskCreatePinnedToCore(&MotorPasso::DirTaskWrapper, "DirTask", 5000, this, 18, &DirTaskHandle, tskNO_AFFINITY);

      EnviarDadosSerial(Mod_ID, _ID + " Iniciado");

      Iniciado = true;

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

    void AtualizarPWM() {
      ledcSetup(_canal, _frequencia, 10);
      ledcAttachPin(_pinoPUL.num, _canal);
      ledcWrite(_canal, dutyCycle);
    }

    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);

      delete encoderKalman;
      encoderKalman = nullptr;

      // 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 = 380;
    double _AgMax = 3430;
    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();

        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();

          bool NoRangeZero = (_Angulo >= -_Margem && _Angulo <= _Margem);
          bool NoRangeSP = (_Angulo >= (_Angulo_SP - _Margem) && _Angulo <= (_Angulo_SP + _Margem));
          if (_Angulo_SP == 0 && !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 setup() {
  pinMode(_pinoAddr0, INPUT);
  pinMode(_pinoAddr1, INPUT);

  AtualizarEnderecoModulo();

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

  delay(10);
}

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

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

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;
  }

  EnviarDadosSerial(Mod_ID, MontarProtocoloMensagemRecebida(Mensagem.idMensagem));
}

void ProcessarChk(ProtocoloSerial Mensagem) {
  EnviarDadosSerial(Mod_ID, MontarProtocoloVerificacao(D_Code, VERSION));
}

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();
    TiposBarramentos txB = (TiposBarramentos)Partes[3].toInt();
    int txP = Partes[4].toInt();
    TiposBarramentos rxB = (TiposBarramentos)Partes[5].toInt();
    int rxP = Partes[6].toInt();
    TiposBarramentos MtxB = (TiposBarramentos)Partes[7].toInt();
    int MtxP = Partes[8].toInt();
    TiposBarramentos MrxB = (TiposBarramentos)Partes[9].toInt();
    int MrxP = Partes[10].toInt();
    TiposBarramentos MmdB = (TiposBarramentos)Partes[11].toInt();
    int MmdP = Partes[12].toInt();
    _pinModbusMD = MmdP;
    _pinModbusRX = MrxP;
    _pinModbusTX = MtxP;

    InicializarModbus(Conectar, _pinModbusRX, _pinModbusTX, _pinModbusMD);
    
    EnviarDadosSerial(Mod_ID, MontarProtocoloSensor(sCFG, Mod_ID, Conectar ? "1" : "0"));

    vTaskDelay(100);

    Conectado = Conectar;
  }
  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.Inicializar();
    }
    else {
      MotorMOV.Desligar();
    }

    vTaskDelay(500);
  }
  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();
    }

    vTaskDelay(500);
  }
}

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

  String ID = Partes[0];

  if (ID == MotorMOV._ID) {
    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._Sentido_SP = (Sentido)Partes[1].toInt();
    MotorDIR._Angulo_SP = Partes[2].toInt();
    MotorDIR._frequencia = (Partes[3].toInt() / 100.0) * MotorDIR.FrequenciaMaxima;

    MotorDIR.AtualizarPWM();

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

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

  String ID = Partes[0];

  if (ID == MotorMOV._ID) {
    MotorMOV.RequisitarDados();
  }
  else if (ID == MotorDIR._ID) {
    MotorDIR.RequisitarDados();
  }
}