agrobot_base/Firmware/Movimentação/Movimentacao_v2/Movimentacao_v2.ino

#include "DHT.h"
#include <SimpleKalmanFilter.h>
#include "C:\Zendion Inc\agrobot_base\Firmware\Modulos\SerialService.h"
#define D_Code Mov

long _baudRate = 115200;

int PinoReleGeral = -1;
bool Conectado = false;

int _TaxaAmostragem;
bool M1_Ativado = false;
bool M2_Ativado = false;
bool M3_Ativado = false;
bool M4_Ativado = false;

int RampaMin = 0;
int RampaMax = 1023;

class Motor {
public:
  // Construtor
  Motor(String desc) {
    _ID = desc;
  }

  // Definições
  String _ID;
  int _canal;
  int _pinoPWM;
  int _pinoVEL;
  int _pinoDIR;
  int _pinoBRK;
  int _pinoSTP;
  int _pinoDHT;

  bool Iniciado = false;
  bool Testando = false;

  // Consumo
  volatile float RPM;
  int PotenciaAtual;
  volatile bool pulso_hall = false;
  volatile unsigned long tempoAnterior = 0;
  volatile unsigned long periodo = 0;

  // Entrada de dados
  bool _FOC;
  int _Rampa;
  int _Precisao;
  double _Potencia;
  double _RPM_SP;
  Sentido _Sentido = Parado;
  Sentido _SentidoA = Parado;
  bool _RampaAtivada = false;
  int _Margem = 2;

  // Sensores
  DHT* dht;

  SimpleKalmanFilter* rpmKalmanFilter;
  SimpleKalmanFilter* potKalmanFilter;

  void Inicializar() {
    RPM = 0;
    pulso_hall = false;
    periodo = 0;
    tempoAnterior = 0;

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

    if (_pinoPWM > -1) {
      ledcSetup(_canal, 1000, 10);
      ledcAttachPin(_pinoPWM, _canal);
      ledcWrite(_canal, RampaMin);
    }
    if (_pinoVEL > -1) {
      pinMode(_pinoVEL, INPUT);
      if (_ID == "ET") {
        attachInterrupt(digitalPinToInterrupt(_pinoVEL), contarPulsoM1, FALLING);
      }
      else if (_ID == "EF") {
        attachInterrupt(digitalPinToInterrupt(_pinoVEL), contarPulsoM2, FALLING);
      }
      else if (_ID == "DT") {
        attachInterrupt(digitalPinToInterrupt(_pinoVEL), contarPulsoM3, FALLING);
      }
      else if (_ID == "DF") {
        attachInterrupt(digitalPinToInterrupt(_pinoVEL), contarPulsoM4, FALLING);
      }
      xTaskCreatePinnedToCore(&Motor::RPMTaskWrapper, "RPMTask", 5000, this, _canal, &RPMTaskHandle, 0);
      rpmKalmanFilter = new SimpleKalmanFilter(1, 1, 0.01);
      potKalmanFilter = new SimpleKalmanFilter(1, 1, 0.01);
    }
    if (_pinoDIR > -1) {
      pinMode(_pinoDIR, OUTPUT);
      digitalWrite(_pinoDIR, HIGH);
    }
    if (_pinoBRK > -1) {
      pinMode(_pinoBRK, OUTPUT);
      digitalWrite(_pinoBRK, LOW);
    }
    if (_pinoSTP > -1) {
      pinMode(_pinoSTP, OUTPUT);
      digitalWrite(_pinoSTP, LOW);
    }
    if (_pinoDHT > -1) {
      dht = new DHT(_pinoDHT, DHT22);
      dht->begin();
      xTaskCreatePinnedToCore(&Motor::DHT22TaskWrapper, "DHT22Task", 5000, this, 10, &DHT22TaskHandle, 1);
    }

    xTaskCreatePinnedToCore(&Motor::RampaTaskWrapper, "RampaTask", 5000, this, 20 - _canal, &RampaTaskHandle, 0);

    EnviarDadosSerial(_ID + " Iniciado");
    
    Iniciado = true;

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

  void Desligar() {
    if (!Iniciado) {
      EnviarDadosSerial(_ID + " nao esta inicializado");
      return;
    }

    // Parar a execução das tarefas
    vTaskDelete(RPMTaskHandle);
    vTaskDelete(RampaTaskHandle);
    vTaskDelete(DHT22TaskHandle);

    // Desanexar o canal PWM
    ledcDetachPin(_pinoPWM);
    
    // Redefinir as configurações para os valores iniciais
    pinMode(_pinoPWM, INPUT);
    pinMode(_pinoVEL, INPUT);
    pinMode(_pinoDIR, INPUT);
    pinMode(_pinoBRK, INPUT);
    pinMode(_pinoSTP, INPUT);

    // Outras redefinições de variáveis de estado, se necessário
    RPM = 0;
    periodo = 0;
    tempoAnterior = 0;
    pulso_hall = false;

    EnviarDadosSerial(_ID + " Desligado");

    Iniciado = false;

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

  void Testar() {
    Testando = true;
    EnviarDadosSerial(MontarProtocoloSensor(sTST, _ID, "Testando motor " + _ID + "..."));
    TestePino("DIR", _pinoDIR);
    TestePino("BRK", _pinoBRK);
    TestePino("STP", _pinoSTP);
    TestePWM();
    //TesteAcionamentoMotores();
    Testando = false;
    EnviarDadosSerial(MontarProtocoloSensor(sTST, _ID, "Teste finalizado"));
  }

  void TestePino(String Descricao, int Pino) {
    EnviarDadosSerial("Pino " + ((String)Pino) + ": " + Descricao);
    digitalWrite(Pino, LOW);
    vTaskDelay(100);
    digitalWrite(Pino, HIGH);
    vTaskDelay(100);
    int Estado = digitalRead(Pino);
    EnviarDadosSerial(MontarProtocoloSensor(sTST, _ID, MontarDadosProtocoloTeste(Testando, Descricao, ((String)((Estado == 1) ? "SUCESSO" : "FALHA")), "1", ((String)Estado))));
    vTaskDelay(1000);
    digitalWrite(Pino, LOW);
    vTaskDelay(100);
    Estado = digitalRead(Pino);
    EnviarDadosSerial(MontarProtocoloSensor(sTST, _ID, MontarDadosProtocoloTeste(Testando, Descricao, ((String)((Estado == 0) ? "SUCESSO" : "FALHA")), "0", ((String)Estado))));
    vTaskDelay(1000);
  }

  void TestePWM() {
    EnviarDadosSerial("Pino " + ((String)_pinoPWM) + ": Canal " + ((String)_canal));
    AtualizaTestePWM(0);
    AtualizaTestePWM(255);
    AtualizaTestePWM(512);
    AtualizaTestePWM(768);
    AtualizaTestePWM(1024);
  }

  void AtualizaTestePWM(int Comando) {
    ledcWrite(_canal, Comando);
    vTaskDelay(100);
    int Leitura = analogRead(_pinoPWM);
    EnviarDadosSerial(MontarProtocoloSensor(sTST, _ID, MontarDadosProtocoloTeste(Testando, "PWM", (Leitura == Comando ? "SUCESSO" : "FALHA"), (String)Comando, (String)Leitura)));
    vTaskDelay(500);
  }

  void TesteAcionamentoMotores() {
    EnviarDadosSerial("Sentido Horario");
    PotenciaAtual = 20;
    _Sentido = Horario;
    Atualizar();
    vTaskDelay(2000);
    EnviarDadosSerial("Sentido Parado");
    _Sentido = Parado;
    PotenciaAtual = 0;
    Atualizar();
    vTaskDelay(2000);
    EnviarDadosSerial("Sentido Antihorario");
    _Sentido = Antihorario;
    PotenciaAtual = 20;
    Atualizar();
    vTaskDelay(2000);
    EnviarDadosSerial("Sentido Parado");
    _Sentido = Parado;
    PotenciaAtual = 0;
    Atualizar();
    vTaskDelay(2000);
  }

  void Atualizar() {
    digitalWrite(_pinoDIR, _SentidoA == Horario);
    digitalWrite(_pinoSTP, _SentidoA != Parado);
    
    ledcWrite(_canal, PotenciaAtual);
  }

private:
  static Motor* instanceM1;
  static Motor* instanceM2;
  static Motor* instanceM3;
  static Motor* instanceM4;
  static void contarPulsoM1() {
    instanceM1->pulso_hall = true;
  }
  static void contarPulsoM2() {
    instanceM2->pulso_hall = true;
  }
  static void contarPulsoM3() {
    instanceM3->pulso_hall = true;
  }
  static void contarPulsoM4() {
    instanceM4->pulso_hall = true;
  }

  TaskHandle_t RPMTaskHandle = NULL;
  static void RPMTaskWrapper(void *pvParameters) {
    Motor *motor = static_cast<Motor*>(pvParameters);
    motor->RPMTask();
  }

  void RPMTask() {
    unsigned long firstMilis = millis();
    int pulsos = 0;
    float estimated_rpm = 0;
    while (1)
    {
      if (!Conectado) {
        vTaskDelay(100);
        continue;
      }
      // Medir RPM apenas quando ocorrer um pulso
      if (pulso_hall) {
        pulsos++;
        // Calcular o período em microssegundos
        unsigned long tempoAtual = micros();
        periodo = tempoAtual - tempoAnterior;
        
        double RPM_A = RPM;

        // Calcular RPM
        if (periodo <= 3052) {
          RPM = RPM_A;
        }
        else if (periodo != 0) {
          //RPM = 60000000 / (40 * periodo); // 40 pulsos por revolução
          RPM = 2000000 / (periodo); // 15 pulsos por revolução
        } else {
          RPM = 0; // Lidar com divisão por zero
        }

        if (RPM >= 650) { // Tratar ponto de parada
          RPM = RPM_A;
        }

        RPM = rpmKalmanFilter->updateEstimate(RPM);

        // Reiniciar variáveis
        tempoAnterior = tempoAtual;
        pulso_hall = false;
      }

      long intMillis = (millis() - firstMilis);

      if (intMillis >= _TaxaAmostragem) {
        if (pulsos == 0) {
          RPM = 0;
          periodo = 0;
        }
        pulsos = 0;

        EnviarDadosSerial(MontarProtocoloSensor(sRPM, _ID, MontarDadosProtocoloRPM(RPM, _Potencia, periodo)));
        firstMilis = millis();
        CorrigePotenciaMotor();
      }

      vTaskDelay(1);
    }
  }

  void CorrigePotenciaMotor() {
    if (_FOC && (((RPM + _Margem) < _RPM_SP) || ((RPM - _Margem) > _RPM_SP))) {
      int PA = PotenciaAcrescentar();
      _Potencia += PA;

      //_Potencia = potKalmanFilter->updateEstimate(_Potencia);

      if (_Potencia > 100) {
        _Potencia = 100;
      }
      else if (_Potencia < 1) {
        _Potencia = 1;
      }
      _RampaAtivada = true;
    }
  }

  int PotenciaAcrescentar() {
    if (((RPM + _Margem) > _RPM_SP) && ((RPM - _Margem) < _RPM_SP)) {
      return 0;
    }

    float PercentualDistancia = (_RPM_SP / (RPM == 0 ? 1 : RPM));
    float FatorDivisor = _Precisao; // (_TaxaAmostragem / 1000) < 1 ? 1 : (_TaxaAmostragem / 1000);
    float AcrescimoPotencia = (PercentualDistancia * _Potencia) - _Potencia;
    float AcrescimoPotenciaCorrigido = round(AcrescimoPotencia / FatorDivisor);
    return AcrescimoPotenciaCorrigido;
  }

  int newPotenciaAcrescentar() {
    if (((RPM + _Margem) > _RPM_SP) && ((RPM - _Margem) < _RPM_SP)) {
      return 0;
    }

    int Acrescentar = ((RPM + _Margem) > _RPM_SP) ? -_Precisao : _Precisao;
    return Acrescentar;
  }

  //1000;010;2;070;004;005
  //1000;010;0;070;004;005

  TaskHandle_t RampaTaskHandle = NULL;
  static void RampaTaskWrapper(void *pvParameters) {
    Motor *motor = static_cast<Motor*>(pvParameters);
    motor->RampaTask();
  }

  void RampaTask() {
    unsigned long previousMillisRampa = millis();
    double MultiplicadorPotencia = RampaMax / 100;
    while (1)
    {
      if (!Conectado) {
        vTaskDelay(100);
        continue;
      }
      unsigned long currentMillis = millis();
      if (currentMillis - previousMillisRampa >= _Rampa && _RampaAtivada) {
        previousMillisRampa = currentMillis;

        bool Limite = false;

        if ((_Sentido == Parado || _Sentido != _SentidoA) && PotenciaAtual > RampaMin) { // M1 estiver parando ou invertendo, e pwm for maior zero rampa
          PotenciaAtual -= _Precisao; // Decrementar para atingir ponto zero rampa
          if (PotenciaAtual < RampaMin)
            PotenciaAtual = RampaMin;
        }
        else if (_Sentido != _SentidoA && PotenciaAtual == RampaMin) { // ZEROU AO MUDAR SENTIDO DE GIRO ANTES DE AUMENTAR
          _SentidoA = _Sentido;
        }
        else if (_Sentido != Parado && PotenciaAtual < (_Potencia * MultiplicadorPotencia)) { // abaixo do set point
          PotenciaAtual += _Precisao; // Incrementar para atingir o set point
          if (PotenciaAtual > RampaMax)
            PotenciaAtual = RampaMax;
        }
        else { // Estabiliza a potência do motor
          if (_Sentido == Parado) {
            PotenciaAtual = RampaMin;
            _Potencia = PotenciaAtual;
          }
          else {
            PotenciaAtual = (PotenciaAtual > RampaMax) ? RampaMax : (_Potencia * MultiplicadorPotencia);
          }
          _SentidoA = _Sentido;
          Limite = true;
        }

        Atualizar();

        if (Limite) {
          _RampaAtivada = false;
        }
      }
      
      vTaskDelay(1);
    }
  }

  TaskHandle_t DHT22TaskHandle = NULL;
  static void DHT22TaskWrapper(void *pvParameters) {
    Motor *motor = static_cast<Motor*>(pvParameters);
    motor->DHT22Task();
  }

  void DHT22Task() {
    unsigned long firstMilis = millis();
    while (1)
    {
      if (!Conectado) {
        vTaskDelay(100);
        continue;
      }

      long intMillis = (millis() - firstMilis);

      if (intMillis >= _TaxaAmostragem + 2000) {

        float t = dht->readTemperature();
        float h = dht->readHumidity();
        float hic = dht->computeHeatIndex(t, h, false);

        EnviarDadosSerial(MontarProtocoloSensor(sTMP, _ID, MontarDadosProtocoloDHT22(t, h, hic)));
        firstMilis = millis();
      }

      vTaskDelay(100);
    }
  }

};

Motor M1("ET");
Motor M2("EF");
Motor M3("DT");
Motor M4("DF");

Motor* Motor::instanceM1 = &M1;
Motor* Motor::instanceM2 = &M2;
Motor* Motor::instanceM3 = &M3;
Motor* Motor::instanceM4 = &M4;

void setup() {
  Serial.begin(_baudRate);
}

void loop() {
  if (Serial.available() > 3) {
    //Serial.println();
    String Protocolo = "";
    //1000;080;1;020;001
    F_Code _funcao = Nda;
    while (Serial.available()) {
      char Entrada = (char)Serial.read();
      //Serial.print(Entrada);
      if (Entrada == EndLine) {
        break;
      }
      Protocolo += Entrada;
      if (Protocolo.length() == 3) {
        if (_funcao == Nda) {
          _funcao = (F_Code)((String)Protocolo[0] + (String)Protocolo[1] + (String)Protocolo[2]).toInt();
          Protocolo = "";
        }
      }
    }

    EnviarDadosSerial(Protocolo);

    if (_funcao == Chk) {
      EnviarDadosSerial(MontarProtocoloVerificacao(D_Code));
    }
    else if (_funcao == Cfg) {
      String ID = ((String)Protocolo[0] + (String)Protocolo[1]);
      bool Conectar = (String)Protocolo[3] == "1";
      if (ID == "MD") {
        int _pinoGeral = ((String)Protocolo[5] + (String)Protocolo[6]).toInt();
        int _RampaMin = ((String)Protocolo[8] + (String)Protocolo[9] + (String)Protocolo[10] + (String)Protocolo[11]).toInt();
        int _RampaMax = ((String)Protocolo[13] + (String)Protocolo[14] + (String)Protocolo[15] + (String)Protocolo[16]).toInt();

        PinoReleGeral = _pinoGeral;
        if (PinoReleGeral > -1) {
          pinMode(PinoReleGeral, OUTPUT);
          digitalWrite(PinoReleGeral, Conectar);
        }

        RampaMin = _RampaMin;
        RampaMax = _RampaMax;

        vTaskDelay(100);
        Conectado = Conectar;
        EnviarDadosSerial(MontarProtocoloSensor(sCFG, "MD", Conectado ? "1" : "0"));
      }
      else {
        int canal = ((String)Protocolo[5]).toInt();
        bool _foc = (String)Protocolo[7] == "1";
        _TaxaAmostragem = ((String)Protocolo[9] + (String)Protocolo[10] + (String)Protocolo[11] + (String)Protocolo[12] + (String)Protocolo[13]).toInt();
        bool motor_ativado = (String)Protocolo[15] == "1";
        int pwm = ((String)Protocolo[17] + (String)Protocolo[18]).toInt();
        int vel = ((String)Protocolo[20] + (String)Protocolo[21]).toInt();
        int dir = ((String)Protocolo[23] + (String)Protocolo[24]).toInt();
        int brk = ((String)Protocolo[26] + (String)Protocolo[27]).toInt();
        int stp = ((String)Protocolo[29] + (String)Protocolo[30]).toInt();
        int dht = ((String)Protocolo[32] + (String)Protocolo[33]).toInt();

        Motor* _motor = ID == "ET" ? &M1 : ID == "EF" ? &M2 : ID == "DT" ? &M3 : &M4;
        if (motor_ativado) {
          if (Conectar) {
            _motor->_canal = canal;
            _motor->_pinoPWM = pwm;
            _motor->_pinoVEL = vel;
            _motor->_pinoDIR = dir;
            _motor->_pinoBRK = brk;
            _motor->_pinoSTP = stp;
            _motor->_pinoDHT = dht;
            _motor->_FOC = _foc;
            
            _motor->Inicializar();
          }
          else {
            _motor->Desligar();
          }
          vTaskDelay(500);
        }
      }
    }
    else if (_funcao == Cmd) {
      //canal;potencia;sentido;rampa;precisao;rpm
      //0;000;0;000;000;000
      String ID = ((String)Protocolo[0] + (String)Protocolo[1]);

      Motor* _motor = ID == "ET" ? &M1 : ID == "EF" ? &M2 : ID == "DT" ? &M3 : &M4;

      _motor->_Potencia = ((String)Protocolo[3] + (String)Protocolo[4] + (String)Protocolo[5]).toInt();
      _motor->_Sentido = (Sentido)((String)Protocolo[7]).toInt();
      _motor->_Rampa = ((String)Protocolo[9] + (String)Protocolo[10] + (String)Protocolo[11]).toInt();
      _motor->_Precisao = ((String)Protocolo[13] + (String)Protocolo[14] + (String)Protocolo[15]).toInt();
      _motor->_RPM_SP = ((String)Protocolo[17] + (String)Protocolo[18] + (String)Protocolo[19]).toInt();
      bool _foc = (String)Protocolo[21] == "1";
      _motor->_FOC = _foc;
      _motor->_RampaAtivada = true;
    }
    else if (_funcao == Tst) {
      String ID = ((String)Protocolo[0] +(String)Protocolo[1]);
      
      Motor* _motor = ID == "ET" ? &M1 : ID == "EF" ? &M2 : ID == "DT" ? &M3 : &M4;

      bool EmTeste = _motor->Testando;
      if (!EmTeste) {
        _motor->Testar();
      }
      
    }
  }
      
  delay(10);
}