Otimizado firmware direcional

This commit is contained in:
Diego Freitas 2024-05-08 10:04:25 -03:00
parent 813890bea3
commit 5de9ae8ffd
3 changed files with 564 additions and 0 deletions

View File

@ -0,0 +1,8 @@
{
// Use IntelliSense to learn about possible attributes.
// Hover to view descriptions of existing attributes.
"version": "0.2.0",
"configurations": [
]
}

View File

@ -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<Motor*>(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<ProtocoloSerial> 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<String> 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<String> 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<String> Partes = SplitString(Protocolo, (char*)",");
String ID = Partes[0];
Motor* _motor = MotorPorID(ID);
bool EmTeste = _motor->Testando;
if (!EmTeste) {
_motor->Testar();
}
}
EnviarDadosSerial(MontarProtocoloMensagemRecebida(idMensagem));
}

View File

@ -0,0 +1,8 @@
{
// Use IntelliSense to learn about possible attributes.
// Hover to view descriptions of existing attributes.
"version": "0.2.0",
"configurations": [
]
}