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

//#include "EEPROM.h"

enum Funcoes {
  Limpar, 
  Ler, 
  Definir, 
  Incrementar
};

enum T_Code {
  Vzo = 200,
  Mov = 100,
  Dir = 101, 
  Sen = 102, 
  Atu = 103,
};

enum F_Code {
  Nda = 200,
  Chk = 244,
  Cfg = 258,
  Tst = 288,
  Cmd = 293,
};

enum S_Code {
  sRPM,
  sTST,
  sCfg
};

enum Sentido {
  Parado,
  Horario,
  Antihorario
};

int PinoReleGeral = 23;
bool Conectado = false;

const char EndLine = '$';
const char BeginSensor = '#';
int _TaxaAmostragem;
bool M1_Ativado = false;
bool M2_Ativado = false;
bool M3_Ativado = false;
bool M4_Ativado = false;

class Motor {
public:
  // Definições
  String _descricao;
  int _canal;
  int _pinoPWM;
  int _pinoVEL;
  int _pinoDIR;
  int _pinoBRK;
  int _pinoSTP;

  bool Iniciado = false;
  bool Testando = false;

  // Consumo
  float RPM;
  int nPulsos;
  long intervalo;
  int PotenciaAtual;

  
  volatile bool pulso_hall = false;
  volatile unsigned long tempoAnterior = 0;
  volatile unsigned long periodo = 0;

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

  Motor(String desc, int canal, int _pwm, int _vel, int _dir, int _brk, int _stp) {
    _descricao = desc;
    _canal = canal;
    _pinoPWM = _pwm;
    _pinoVEL = _vel;
    _pinoDIR = _dir;
    _pinoBRK = _brk;
    _pinoSTP = _stp;
  }

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

    if (Iniciado) {
      EnviarDadosSerial(_descricao + " ja inicializado");
      return;
    }

    pinMode(_pinoPWM, OUTPUT);
    pinMode(_pinoVEL, INPUT);
    pinMode(_pinoDIR, OUTPUT);
    pinMode(_pinoBRK, OUTPUT);
    pinMode(_pinoSTP, OUTPUT);

    digitalWrite(_pinoPWM, LOW);
    digitalWrite(_pinoDIR, HIGH);
    digitalWrite(_pinoBRK, LOW);
    digitalWrite(_pinoSTP, LOW);

    ledcSetup(_canal, 1000, 10);
    ledcAttachPin(_pinoPWM, _canal);
    attachInterrupt(digitalPinToInterrupt(_pinoVEL), contarPulso, RISING);
    xTaskCreatePinnedToCore(&Motor::RPMTaskWrapper, "RPMTask", 4000, this, _canal, &RPMTaskHandle, 0);
    xTaskCreatePinnedToCore(&Motor::RampaTaskWrapper, "RampaTask", 4000, this, 20 - _canal, &RampaTaskHandle, 0);

    EnviarDadosSerial(_descricao + " Iniciado");
    
    Iniciado = true;
  }

  void Desligar() {
    if (!Iniciado) {
      EnviarDadosSerial(_descricao + " não está inicializado");
      return;
    }

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

    // 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
    nPulsos = 0;
    intervalo = 0;
    RPM = 0;
    Iniciado = false;

    EnviarDadosSerial(_descricao + " Desligado");
  }

  void Testar() {
    Testando = true;
    EnviarDadosSerial(MontarProtocoloSensor(sTST, _descricao, "Testando motor " + _descricao + "..."));
    TestePino("DIR", _pinoDIR);
    TestePino("BRK", _pinoBRK);
    TestePino("STP", _pinoSTP);
    TestePWM();
    //TesteAcionamentoMotores();
    Testando = false;
    EnviarDadosSerial(MontarProtocoloSensor(sTST, _descricao, "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, _descricao, MontarDadosProtocoloTeste(Descricao, ((String)((Estado == 1) ? "SUCESSO" : "FALHA")), "1", ((String)Estado))));
    vTaskDelay(1000);
    digitalWrite(Pino, LOW);
    vTaskDelay(100);
    Estado = digitalRead(Pino);
    EnviarDadosSerial(MontarProtocoloSensor(sTST, _descricao, MontarDadosProtocoloTeste(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, _descricao, MontarDadosProtocoloTeste("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);

    //Serial.print(_canal);
    //Serial.print(" ");
    //Serial.println(PotenciaAtual);
  }


  void EnviarDadosSerial(String Mensagem) {
    Serial.println(Mensagem);
    Serial.flush();
    vTaskDelay(10);
  }

  String MontarProtocoloSensor(S_Code TipoSensor, String _motor, String Dados) {
    String Protocolo = 
      BeginSensor + 
      (String)TipoSensor + 
      _motor + ";" + 
      Dados;

    return Protocolo;
  }

  String MontarDadosProtocoloTeste(String Componente, String Resultado, String Comando, String Leitura) {
    String Protocolo = 
      (Testando ? "1;" : "0;") + 
      Componente + ";" + 
      Resultado + ";" + 
      Comando + ";" + 
      Leitura;

    return Protocolo;
  }

  String MontarDadosProtocoloRPM(int nPulsos, int intervalo, float rpm, int potencia) {
    String Protocolo = 
      (String)nPulsos + ";" + 
      (String)intervalo + ";" + 
      (String)rpm + ";" + 
      (String)potencia;

    return Protocolo;
  }



private:

  static Motor* instance;
  static void contarPulso() {
    if (instance) {
      instance->pulso_hall = true;
    }
  }

  int ZeroRampa = 0;
  int PotenciaMax = 1023;

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

  void RPMTask() {
    bool uStatus = digitalRead(_pinoVEL);
    unsigned long firstMilis = millis();
    unsigned long firstMilisCalc = millis();
    while (1)
    {
      if (_SentidoA == Parado && _Sentido == Parado) {
        ReiniciaVelocimetro();
        firstMilis = millis();
        firstMilisCalc = millis();
        RPM = 0;
        uStatus = digitalRead(_pinoVEL);
        vTaskDelay(100);
        continue;
      }
      
      bool aStatus = digitalRead(_pinoVEL);
      if (uStatus != aStatus) {
        uStatus = aStatus;
        if (aStatus == true) {
          nPulsos++;
        }
      }

      long intMillis = (millis() - firstMilis);
      long intMillisCalc = (millis() - firstMilisCalc);
      intervalo = intMillis / 1000;

      if (intMillisCalc >= _TaxaAmostragem) {
        RPM = intervalo == 0 ? RPM : (4 * nPulsos) / (3 * intervalo);
        EnviarDadosSerial(MontarProtocoloSensor(sRPM, _descricao, MontarDadosProtocoloRPM(nPulsos, intervalo, RPM, _Potencia)));
        CorrigePotenciaMotorPID();
        firstMilisCalc = millis();

        ReiniciaVelocimetro();
        firstMilis = millis();
        vTaskDelay(50);
      }

      vTaskDelay(1);
    }
  }

  void ReiniciaVelocimetro() {
    nPulsos = 0;
    intervalo = 0;
  }

  void CorrigePotenciaMotor() {
    if (((RPM + _Margem) < _RPM_SP) || ((RPM - _Margem) > _RPM_SP)) {
      int PA = PotenciaAcrescentar();
      _Potencia += PA;
      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 = 1; // (_TaxaAmostragem / 1000) < 1 ? 1 : (_TaxaAmostragem / 1000);
    float AcrescimoPotencia = (PercentualDistancia * _Potencia) - _Potencia;
    float AcrescimoPotenciaCorrigido = round(AcrescimoPotencia / FatorDivisor);
    return AcrescimoPotenciaCorrigido;
  }



  void RPMTaskInterrupcao() {
    while (1)
    {
      // Medir RPM apenas quando ocorrer um pulso
      if (pulso_hall) {
        // Calcular o período em microssegundos
        unsigned long tempoAtual = micros();
        periodo = tempoAtual - tempoAnterior;
        
        // Calcular RPM
        if (periodo != 0) {
          RPM = 60000000 / (40 * periodo); // 40 pulsos por revolução
        } else {
          RPM = 0; // Lidar com divisão por zero
        }
        
        // Reiniciar variáveis
        tempoAnterior = tempoAtual;
        pulso_hall = false;
      }
      vTaskDelay(1);
    }
  }


  void CorrigePotenciaMotorPID() {
    if (((RPM + _Margem) < _RPM_SP) || ((RPM - _Margem) > _RPM_SP)) {
      int NovaPotencia = CorrigirErroPID();
      _Potencia = map(NovaPotencia, ZeroRampa, PotenciaMax, 0, 100);
      _RampaAtivada = true;
    }
  }

  // Defina os parâmetros do PID
  double Kp = 1.0;         // Ganho proporcional
  double Ki = 0.1;         // Ganho integral
  double Kd = 0.01;        // Ganho derivativo

  // Variáveis de controle
  double erro_anterior = 0;
  double integral = 0;
  int CorrigirErroPID() {
    // Calcular o erro
    double erro = _RPM_SP - RPM;

    // Calcular as componentes PID
    double componente_proporcional = Kp * erro;
    integral += Ki * erro;
    double componente_derivativo = Kd * (erro - erro_anterior);

    // Calcular a saída do PID
    double saida_pid = componente_proporcional + integral + componente_derivativo;

    // Limitar a saída do PID dentro dos limites do PWM (0 a 1023)
    if (saida_pid < ZeroRampa) {
      saida_pid = ZeroRampa;
    } else if (saida_pid > PotenciaMax) {
      saida_pid = PotenciaMax;
    }

    // Atualizar o erro anterior para o próximo ciclo
    erro_anterior = erro;

  }

  //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 = PotenciaMax / 100;
    while (1)
    {
      unsigned long currentMillis = millis();
      if (currentMillis - previousMillisRampa >= _Rampa && _RampaAtivada) {
        previousMillisRampa = currentMillis;

        bool Limite = false;

        if ((_Sentido == Parado || _Sentido != _SentidoA) && PotenciaAtual > ZeroRampa) { // M1 estiver parando ou invertendo, e pwm for maior zero rampa
          PotenciaAtual -= _Precisao; // Decrementar para atingir ponto zero rampa
          if (PotenciaAtual < ZeroRampa)
            PotenciaAtual = ZeroRampa;
        }
        else if (_Sentido != _SentidoA && PotenciaAtual == ZeroRampa) { // 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 > PotenciaMax)
            PotenciaAtual = PotenciaMax;
        }
        else { // Estabiliza a potência do motor
          if (_Sentido == Parado) {
            PotenciaAtual = ZeroRampa;
          }
          else {
            PotenciaAtual = (PotenciaAtual > PotenciaMax) ? PotenciaMax : (_Potencia * MultiplicadorPotencia);
          }
          _SentidoA = _Sentido;
          Limite = true;
        }

        Atualizar();

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

};
Motor* Motor::instance = nullptr;


int addr_code = 25;
#define EEPROM_SIZE 512
#define D_Code Mov

Motor M1("ET", 0, 13, 35, 12, 27, 22);
Motor M2("EF", 1, 14, 34, 33, 25, 32);
Motor M3("DT", 2, 15, 39, 02, 00, 04);
Motor M4("DF", 3, 05, 36, 18, 19, 21);

void setup() {
  Serial.begin(115200);
  pinMode(PinoReleGeral, OUTPUT);
  digitalWrite(PinoReleGeral, LOW);
  //EEPROM.begin(EEPROM_SIZE);
  //AtualizarMemoria(Definir, addr_code, D_Code);
}

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

    if (_funcao == Chk) {
      Serial.println((String)D_Code + (String)_funcao);
      Serial.flush();
    }
    else if (_funcao == Cfg) {
      Conectado = (String)Protocolo[0] == "1";
      _TaxaAmostragem = ((String)Protocolo[2] + (String)Protocolo[3] + (String)Protocolo[4] + (String)Protocolo[5] + (String)Protocolo[6]).toInt();
      M1_Ativado = (String)Protocolo[8] == "1";
      M2_Ativado = (String)Protocolo[10] == "1";
      M3_Ativado = (String)Protocolo[12] == "1";
      M4_Ativado = (String)Protocolo[14] == "1";
      if (M1_Ativado) {
        if (Conectado) {
          M1.Inicializar();
        }
        else {
          M1.Desligar();
        }
        vTaskDelay(500);
      }
      if (M2_Ativado) {
        if (Conectado) {
          M2.Inicializar();
        }
        else {
          M2.Desligar();
        }
        vTaskDelay(500);
      }
      if (M3_Ativado) {
        if (Conectado) {
          M3.Inicializar();
        }
        else {
          M3.Desligar();
        }
        vTaskDelay(500);
      }
      if (M4_Ativado) {
        if (Conectado) {
          M4.Inicializar();
        }
        else {
          M4.Desligar();
        }
        vTaskDelay(500);
      }

      digitalWrite(PinoReleGeral, Conectado);

      vTaskDelay(100);

      String Protocolo = BeginSensor + ((String)sCfg) + "ET;" + (String)Conectado;
      Serial.println(Protocolo);
    }
    else if (_funcao == Cmd) {
      Serial.println(Protocolo);
      //canal;potencia;sentido;rampa;precisao;rpm
      //0;000;0;000;000;000
      int Canal = ((String)Protocolo[0]).toInt();

      Motor* _motor = Canal == 0 ? &M1 : Canal == 1 ? &M2 : Canal == 2 ? &M3 : &M4;

      _motor->_Potencia = ((String)Protocolo[2] + (String)Protocolo[3] + (String)Protocolo[4]).toInt();
      _motor->_Sentido = (Sentido)((String)Protocolo[6]).toInt();
      _motor->_Rampa = ((String)Protocolo[8] + (String)Protocolo[9] + (String)Protocolo[10]).toInt();
      _motor->_Precisao = ((String)Protocolo[12] + (String)Protocolo[13] + (String)Protocolo[14]).toInt();
      _motor->_RPM_SP = ((String)Protocolo[16] + (String)Protocolo[17] + (String)Protocolo[18]).toInt();
      _motor->_RampaAtivada = true;
    }
    else if (_funcao == Tst) {
      int Canal = ((String)Protocolo[0]).toInt();
      
      Motor* _motor = Canal == 0 ? &M1 : Canal == 1 ? &M2 : Canal == 2 ? &M3 : &M4;

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


/*int AtualizarMemoria(Funcoes Funcao, int Endereco, int Valor) {
  int MaxMemoria = 3;
  int ValorLido = 0;

  if (Funcao == Limpar) {
    for (int i = 0; i < 254; i++) {
      EEPROM.write(i, '0');
      EEPROM.commit();
    }
  }
  else {
    String ValorLidoStr = "";
    for (int i = Endereco; i < (Endereco + MaxMemoria); i++) {
      ValorLidoStr += (char)EEPROM.read(i);
    }

    ValorLido = ValorLidoStr.toInt();

    if (Funcao != Ler) {
      if (Funcao == Definir) {
        ValorLido = Valor;
      }
      else if (Funcao == Incrementar) {
        ValorLido += Valor;
      }
      ValorLidoStr = (String)ValorLido;

      int idx = 0;
      for (int i = Endereco; i < (Endereco + MaxMemoria); i++) {
        char V = '0';
        int PIndex = (MaxMemoria - ValorLidoStr.length());
        if (PIndex <= idx)
          V = ValorLidoStr[idx - PIndex];
        EEPROM.write(i, V);
        idx++;
      }

      EEPROM.commit();
    }
  }

  return ValorLido;
}*/