From 940d2351e819f5791aa3fd097a60317cfa273eb8 Mon Sep 17 00:00:00 2001 From: Diego Freitas Date: Wed, 8 May 2024 10:04:25 -0300 Subject: [PATCH] Otimizado firmware direcional --- .../Direcional_v7/.theia/launch.json | 8 + .../Direcional_v7/Direcional_v7.ino | 548 ++++++++++++++++++ Firmware/Direcional/Direcional_v7/launch.json | 8 + 3 files changed, 564 insertions(+) create mode 100644 Firmware/Direcional/Direcional_v7/.theia/launch.json create mode 100644 Firmware/Direcional/Direcional_v7/Direcional_v7.ino create mode 100644 Firmware/Direcional/Direcional_v7/launch.json diff --git a/Firmware/Direcional/Direcional_v7/.theia/launch.json b/Firmware/Direcional/Direcional_v7/.theia/launch.json new file mode 100644 index 000000000..7e4253b33 --- /dev/null +++ b/Firmware/Direcional/Direcional_v7/.theia/launch.json @@ -0,0 +1,8 @@ +{ + // Use IntelliSense to learn about possible attributes. + // Hover to view descriptions of existing attributes. + "version": "0.2.0", + "configurations": [ + + ] +} diff --git a/Firmware/Direcional/Direcional_v7/Direcional_v7.ino b/Firmware/Direcional/Direcional_v7/Direcional_v7.ino new file mode 100644 index 000000000..70a146348 --- /dev/null +++ b/Firmware/Direcional/Direcional_v7/Direcional_v7.ino @@ -0,0 +1,548 @@ +// Dispositivo: Direcional +// Versão Firmware: 7 +// Ultima atualização: 08/05/2024 +// Atualização: Otimização de leitura de sensores + +#include "C:\ZendionInc\agrobot_base\Firmware\Modulos\SerialService.h" +#include "C:\ZendionInc\agrobot_base\Firmware\Modulos\Pinout.h" +#define D_Code Dir +#define VERSION 7 + +#define _pinoLED RGB_BUILTIN + +long _baudRate = 115200; + +bool Conectado = false; +bool AA_Ref000 = false; +bool AA_Ref090 = false; +bool AA_Ref270 = false; + +int _TaxaAmostragem; +int FrequenciaMaxima = 10000; +double _Margem = 0.5; + +int dutyCycle = 512; +double _frequencia = 4000; + +PinoModel _pinoGeral; + +class Motor { + public: + // Construtor + Motor(String desc) { + _ID = desc; + } + + // Definições + String _ID; + int _canal; + PinoModel _pinoENA; + PinoModel _pinoDIR; + PinoModel _pinoPUL; + PinoModel _pinoEncoderA; + PinoModel _pinoEncoderB; + PinoModel _pinoRef000; + PinoModel _pinoRef090; + PinoModel _pinoRef270; + + bool Iniciado = false; + bool Testando = false; + bool Referenciando = false; + + // Consumo + double _Angulo; + double _AgRef; + Sentido _Sentido; + + // Entrada de dados + double _Angulo_SP; + double _Angulo_Offset = 0; + Sentido _Sentido_SP = Parado; + bool _MotorLigado = false; + bool _Estabilizar = true; + bool _Referenciar = true; + + bool E0 = false; + bool Ag0 = false; + bool _Ag0 = false; + bool E90 = false; + bool Ag90 = false; + bool _Ag90 = false; + bool E270 = false; + bool Ag270 = false; + bool _Ag270 = false; + bool Referenciado = false; + int RefMax = 90; + int RefMin = -90; + int ulEncoderA = 0; + int ulEncoderB = 0; + double GrausPino = 0; + bool EA_M = false; + bool EB_M = false; + + + void Inicializar() { + if (Iniciado) { + EnviarDadosSerial(_ID + " ja inicializado"); + EnviarDadosSerial(MontarProtocoloSensor(sCFG, _ID, Iniciado ? "1" : "0")); + return; + } + + if (_pinoPUL.Definido()) { + AtualizarPWM(); + } + _pinoENA.Conectar(true); + _pinoDIR.Conectar(false); + _pinoEncoderA.Conectar(); + ulEncoderA = _pinoEncoderA.get(); + EA_M = false; + _pinoEncoderB.Conectar(); + ulEncoderB = _pinoEncoderB.get(); + EB_M = false; + _pinoRef000.Conectar(); + Ag0 = _pinoRef000.get() == 1; + _Ag0 = Ag0; + _pinoRef090.Conectar(); + Ag90 = _pinoRef090.get() == 1; + _Ag90 = Ag90; + _pinoRef270.Conectar(); + Ag270 = _pinoRef270.get() == 1; + _Ag270 = Ag270; + + xTaskCreatePinnedToCore(&Motor::RampaTaskWrapper, "RampaTask", 5000, this, 20 - _canal, &RampaTaskHandle, tskNO_AFFINITY); + + + E0 = false; + E90 = false; + E270 = false; + + ResetRef(_Referenciar); + + EnviarDadosSerial(_ID + " Iniciado"); + + Iniciado = true; + + EnviarDadosSerial(MontarProtocoloSensor(sCFG, _ID, Iniciado ? "1" : "0")); + } + + void ResetRef(bool _Ref) { + _Angulo = 0; + GrausPino = 0; + _Sentido_SP = Parado; + _Sentido = Parado; + Referenciado = !_Ref; + Referenciando = _Ref; + _MotorLigado = _Ref; + } + + void AtualizarPWM() { + ledcSetup(_canal, _frequencia, 10); + ledcAttachPin(_pinoPUL.num, _canal); + ledcWrite(_canal, dutyCycle); + } + + void Desligar() { + if (!Iniciado) { + EnviarDadosSerial(_ID + " nao esta inicializado"); + EnviarDadosSerial(MontarProtocoloSensor(sCFG, _ID, Iniciado ? "1" : "0")); + return; + } + + // Parar a execução das tarefas + vTaskDelete(RampaTaskHandle); + + // Redefinir as configurações para os valores iniciais + _pinoENA.Desconectar(); + _pinoDIR.Desconectar(); + _pinoPUL.Desconectar(); + _pinoEncoderA.Desconectar(); + _pinoEncoderB.Desconectar(); + _pinoRef000.Desconectar(); + _pinoRef090.Desconectar(); + _pinoRef270.Desconectar(); + + + EnviarDadosSerial(_ID + " Desligado"); + + Iniciado = false; + + EnviarDadosSerial(MontarProtocoloSensor(sCFG, _ID, Iniciado ? "1" : "0")); + } + + void Testar() { + Testando = true; + + Testando = false; + } + + + + private: + + TaskHandle_t RampaTaskHandle = NULL; + static void RampaTaskWrapper(void *pvParameters) { + Motor *motor = static_cast(pvParameters); + motor->RampaTask(); + } + + void RampaTask() { + unsigned long previousMillisRampa = millis(); + + while (true) { // Loop infinito mais claro + // Verifica a conexão antes de prosseguir + if (!Conectado) { + // Espera por 1 segundo para reduzir o uso da CPU se não estiver conectado + vTaskDelay(1000 / portTICK_PERIOD_MS); + continue; // Pula para a próxima iteração do loop + } + + // Executa as funções de controle e monitoramento do motor + AferirPosicao(); + if (Referenciando) { + ReferenciarMotor(); + } else { + VerificarAnguloSP(); + } + Atualizar(); + AferirAnguloAbsoluto(); + + // Envia dados via serial em intervalos definidos por _TaxaAmostragem + if (millis() - previousMillisRampa >= _TaxaAmostragem) { + previousMillisRampa = millis(); // Atualiza o tempo para o próximo envio + EnviarDadosSerial(MontarProtocoloSensor(sENC, _ID, MontarDadosProtocoloEncoder(_Sentido, _Angulo, Ag0, Ag90, Ag270, Referenciando, Referenciado))); + } + + // Libera a CPU para outras tarefas por um curto período + vTaskDelay(1 / portTICK_PERIOD_MS); // Torna o delay mais explícito e ajustado ao tick do sistema + } + } + + void Atualizar() { + _pinoENA.set(!_MotorLigado); + _pinoDIR.set(_Sentido_SP != Horario); + } + + void AferirPosicao() { + if (_pinoEncoderA.Definido() && _pinoEncoderB.Definido()) { + int leituraEncoderA = _pinoEncoderA.get(); + int leituraEncoderB = _pinoEncoderB.get(); + + if (!EA_M) { + EA_M = leituraEncoderA != ulEncoderA; + if (EA_M) { + _Sentido = leituraEncoderA != leituraEncoderB ? Horario : Antihorario; + } + } + if (!EB_M) { + EB_M = leituraEncoderB != ulEncoderB; + } + + + if (EA_M && EB_M) { + + if (_Sentido == Horario) { + _Angulo = _Angulo + _Margem; + _AgRef = _AgRef + _Margem; + } else if (_Sentido == Antihorario) { + _Angulo = _Angulo - _Margem; + _AgRef = _AgRef - _Margem; + } + + EA_M = false; + EB_M = false; + } + + ulEncoderA = leituraEncoderA; + ulEncoderB = leituraEncoderB; + } + } + + + void AferirAnguloAbsoluto() { + AtualizarEstadoSensores(); + if (GrausPino == 0) { + VerificarReferenciaInicial(); + } else { + AtualizarAnguloComReferencia(); + } + AtualizarEstadosAnteriores(); + } + + void AtualizarEstadoSensores() { + _Ag0 = _pinoRef000.get() == 1; + _Ag90 = _pinoRef090.get() == 1; + _Ag270 = _pinoRef270.get() == 1; + } + + void VerificarReferenciaInicial() { + VerificarSensor(&_pinoRef000, &_Ag0, &Ag0, &E0, 0); + VerificarSensor(&_pinoRef090, &_Ag90, &Ag90, &E90, 90); + VerificarSensor(&_pinoRef270, &_Ag270, &Ag270, &E270, -90); + } + + void VerificarSensor(PinoModel* pino, bool* estadoAtual, bool* estadoAnterior, bool* evento, double anguloReferencia) { + if (!pino->Definido()) return; + + if (*estadoAtual && !*estadoAnterior && !*evento) { // Entrou no sensor + _AgRef = 0; + *evento = true; + } else if (!*estadoAtual && *estadoAnterior && *evento) { // Saiu do sensor + GrausPino = (_AgRef < 0 ? -1.0 : 1.0) * _AgRef; + Referenciado = true; + *evento = false; + } + } + + void AtualizarAnguloComReferencia() { + if (Referenciando || AA_Ref000) { + AtualizarAnguloNoSensor(&_Ag0, &Ag0, 0); + } + if (Referenciando || AA_Ref090) { + AtualizarAnguloNoSensor(&_Ag90, &Ag90, 90); + } + if (Referenciando || AA_Ref270) { + AtualizarAnguloNoSensor(&_Ag270, &Ag270, -90); + } + } + + void AtualizarAnguloNoSensor(bool* estadoAtual, bool* estadoAnterior, double anguloReferencia) { + if (*estadoAtual != *estadoAnterior) { + _Angulo = CalculaAnguloPino(anguloReferencia, *estadoAtual); + } + } + + void AtualizarEstadosAnteriores() { + Ag0 = _Ag0; + Ag90 = _Ag90; + Ag270 = _Ag270; + } + + double CalculaAnguloPino(double Inicio, bool Entrando) { + double MeioPino = (GrausPino / 2.0); + double Multiplicador = 1.0; + if (Entrando) { + Multiplicador = (_Sentido == Horario ? -1.0 : 1.0); + } + else { + Multiplicador = (_Sentido == Horario ? 1.0 : -1.0); + } + double Angulo = Inicio + (MeioPino * Multiplicador) + _Angulo_Offset; + return Angulo; + } + + + void VerificarAnguloSP() { + if (_MotorLigado) { + if (_Angulo_SP > -1) { + if (_Sentido_SP == Horario && _Angulo >= _Angulo_SP) { + _MotorLigado = false; + } + else if (_Sentido_SP == Antihorario && _Angulo <= (_Angulo_SP * -1)) { + _MotorLigado = false; + } + } + else { + if (Referenciado && (_Sentido_SP == Horario && _Angulo >= RefMax)) { + _MotorLigado = false; + } + else if (Referenciado && (_Sentido_SP == Antihorario && _Angulo <= RefMin)) { + _MotorLigado = false; + } + } + } + else { + if (_Estabilizar && _Sentido_SP == Parado && _Angulo != _Angulo_SP) { + _Sentido_SP = _Angulo > 0 ? Antihorario : Horario; + _MotorLigado = true; + } + } + } + + void ReferenciarMotor() { + int AgP1 = RefMax / 2; + + // Verifica se o motor já está na posição de referência (dentro da margem de 0º). + if (Referenciado && (_Angulo >= -_Margem && _Angulo <= _Margem)) { + Referenciando = false; + _MotorLigado = false; + } else { + // Inicia o processo de referenciamento se o motor está parado. + if (_Sentido_SP == Parado) { + _Angulo_SP = AgP1; + _Sentido_SP = Horario; + } + // Verifica se o motor excedeu a margem de referência enquanto se move no sentido horário. + else if (_Sentido_SP == Horario && _Angulo > _Margem) { + _Angulo_SP = -AgP1; // Muda para antihorário para tentar encontrar o ponto de referência. + _Sentido_SP = Antihorario; + } + // Verifica se o motor excedeu a margem de referência enquanto se move no sentido antihorário. + else if (_Sentido_SP == Antihorario && _Angulo < -_Margem) { + _Angulo_SP = AgP1; // Muda para horário para tentar encontrar o ponto de referência. + _Sentido_SP = Horario; + } + // Continua o processo de referenciamento conforme a lógica original. + else if (_Sentido_SP == Horario && _Angulo >= AgP1) { + _Angulo_SP = -AgP1; + _Sentido_SP = Antihorario; + } + else if (_Sentido_SP == Antihorario && _Angulo <= -AgP1) { + _Angulo_SP = AgP1 * 2; + _Sentido_SP = Horario; + } + else if (_Sentido_SP == Horario && _Angulo >= AgP1 * 2) { + _Angulo_SP = -AgP1 * 2; + _Sentido_SP = Antihorario; + } + // Adiciona mais condições conforme necessário para cobrir o processo completo de referenciamento. + } + } + +}; + +Motor M1("ET"); +Motor M2("EF"); +Motor M3("DT"); +Motor M4("DF"); + +Motor* MotorPorID(String ID) { + Motor* _motor = + ID == "ET" ? &M1 : + ID == "EF" ? &M2 : + ID == "DT" ? &M3 : + ID == "DF" ? &M4 : + nullptr; + return _motor; +} + +void setup() { + Serial.begin(_baudRate); + //iniciarMCP(D_Code, 8, 18, 0x20, false); +} + +void loop() { + std::vector protocolos = LerBufferSerialFila(); + + for (int i = 0; i < protocolos.size(); i++) { + ProcessarProtocolo(protocolos[i].protocolo, protocolos[i].funcao, protocolos[i].idMensagem); + } +} + +void ProcessarProtocolo(String Protocolo, F_Code _funcao, int idMensagem) { + + if (_funcao == Chk) { + EnviarDadosSerial(MontarProtocoloVerificacao(D_Code, VERSION)); + } + else if (_funcao == Cfg) { + std::vector Partes = SplitString(Protocolo, (char*)","); + String ID = Partes[0]; + bool Conectar = Partes[1] == "1"; + if (ID == "MD") { + _TaxaAmostragem = Partes[2].toInt(); + AA_Ref000 = Partes[3] == "1"; + AA_Ref090 = Partes[4] == "1"; + AA_Ref270 = Partes[5] == "1"; + TiposBarramentos _barramento = (TiposBarramentos)Partes[6].toInt(); + int _num = Partes[7].toInt(); + _pinoGeral = PinoModel(_num, _barramento, _OUTPUT); + + if (_pinoGeral.Definido()) { + _pinoGeral.set(true); + } + + EnviarDadosSerial(MontarProtocoloSensor(sCFG, "MD", Conectar ? "1" : "0")); + vTaskDelay(pdMS_TO_TICKS(100)); + Conectado = Conectar; + } + else { + Motor* _motor = MotorPorID(ID); + + int canal = Partes[2].toInt(); + bool motor_ativado = Partes[3] == "1"; + bool estabilizar = Partes[4] == "1"; + bool referenciar = Partes[5] == "1"; + int _offset = Partes[6].toInt(); + TiposBarramentos pulB = (TiposBarramentos)Partes[7].toInt(); + int pul = Partes[8].toInt(); + TiposBarramentos dirB = (TiposBarramentos)Partes[9].toInt(); + int dir = Partes[10].toInt(); + TiposBarramentos enaB = (TiposBarramentos)Partes[11].toInt(); + int ena = Partes[12].toInt(); + TiposBarramentos encAB = (TiposBarramentos)Partes[13].toInt(); + int encoderA = Partes[14].toInt(); + TiposBarramentos encBB = (TiposBarramentos)Partes[15].toInt(); + int encoderB = Partes[16].toInt(); + TiposBarramentos ref0B = (TiposBarramentos)Partes[17].toInt(); + int ref0 = Partes[18].toInt(); + TiposBarramentos ref90B = (TiposBarramentos)Partes[19].toInt(); + int ref90 = Partes[20].toInt(); + TiposBarramentos ref270B = (TiposBarramentos)Partes[21].toInt(); + int ref270 = Partes[22].toInt(); + + if (motor_ativado) { + if (Conectar) { + _motor->_canal = canal; + _motor->_Estabilizar = estabilizar; + _motor->_Referenciar = referenciar; + _motor->_Angulo_Offset = _offset; + _motor->_pinoPUL = PinoModel(pul, pulB, _PWM); + _motor->_pinoDIR = PinoModel(dir, dirB, _OUTPUT); + _motor->_pinoENA = PinoModel(ena, enaB, _OUTPUT); + _motor->_pinoEncoderA = PinoModel(encoderA, encAB, _INPUT, DIGITAL); + _motor->_pinoEncoderB = PinoModel(encoderB, encBB, _INPUT, DIGITAL); + _motor->_pinoRef000 = PinoModel(ref0, ref0B, _INPUT, DIGITAL); + _motor->_pinoRef090 = PinoModel(ref90, ref90B, _INPUT, DIGITAL); + _motor->_pinoRef270 = PinoModel(ref270, ref270B, _INPUT, DIGITAL); + _motor->Inicializar(); + } + else { + _motor->Desligar(); + } + vTaskDelay(500); + } + } + } + else if (_funcao == Cmd) { + std::vector Partes = SplitString(Protocolo, (char*)","); + String ID = Partes[0]; + + Motor* _motor = MotorPorID(ID); + + Sentido sentido = (Sentido)Partes[1].toInt(); + int angulo = Partes[2].toInt(); + int porcentagem = Partes[3].toInt(); + double frequencia = (porcentagem / 100.0) * FrequenciaMaxima; + bool referenciar = Partes[4] == "1"; + int _offset = Partes[5].toInt(); + + _frequencia = frequencia; + + _motor->_Sentido_SP = sentido; + _motor->_Angulo_SP = angulo; + _motor->_Angulo_Offset = _offset; + + _motor->AtualizarPWM(); + + if (referenciar) { + _motor->ResetRef(referenciar); + } + else { + _motor->_MotorLigado = sentido != Parado; + } + } + else if (_funcao == Tst) { + std::vector Partes = SplitString(Protocolo, (char*)","); + String ID = Partes[0]; + + Motor* _motor = MotorPorID(ID); + + bool EmTeste = _motor->Testando; + if (!EmTeste) { + _motor->Testar(); + } + + } + + EnviarDadosSerial(MontarProtocoloMensagemRecebida(idMensagem)); + +} diff --git a/Firmware/Direcional/Direcional_v7/launch.json b/Firmware/Direcional/Direcional_v7/launch.json new file mode 100644 index 000000000..7e4253b33 --- /dev/null +++ b/Firmware/Direcional/Direcional_v7/launch.json @@ -0,0 +1,8 @@ +{ + // Use IntelliSense to learn about possible attributes. + // Hover to view descriptions of existing attributes. + "version": "0.2.0", + "configurations": [ + + ] +}