880 lines
26 KiB
Arduino
880 lines
26 KiB
Arduino
|
// Dispositivo: Movimentação e Direcional Unificados
|
||
|
|
// Versão Firmware: 5
|
||
|
|
// Ultima atualização: 24/06/2024
|
||
|
|
// Atualização: Adicionado driver motor de passo RS485
|
||
|
|
|
||
|
|
#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 5
|
||
|
|
String Mod_ID = "";
|
||
|
|
|
||
|
|
#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;
|
||
|
|
volatile bool Conectado = false;
|
||
|
|
volatile int _TaxaAmostragem = 500;
|
||
|
|
volatile bool RequisitarDados = false;
|
||
|
|
|
||
|
|
|
||
|
|
class MotorBLDC {
|
||
|
|
public:
|
||
|
|
String _ID = "Mov";
|
||
|
|
int _canal = 4;
|
||
|
|
|
||
|
|
volatile bool Iniciado = false;
|
||
|
|
bool Testando = false;
|
||
|
|
|
||
|
|
PinoModel _pinoEN;
|
||
|
|
PinoModel _pinoFR;
|
||
|
|
PinoModel _pinoSV;
|
||
|
|
PinoModel _pinoBK;
|
||
|
|
|
||
|
|
PinoModel _pinoTMP;
|
||
|
|
SimpleKalmanFilter tempKalman = SimpleKalmanFilter(2, 2, 0.01);
|
||
|
|
|
||
|
|
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;
|
||
|
|
|
||
|
|
volatile bool ComandoEnviado = false;
|
||
|
|
|
||
|
|
|
||
|
|
void Inicializar() {
|
||
|
|
if (Iniciado) {
|
||
|
|
EnviarDadosSerial(Mod_ID, MontarProtocoloSensor(sCFG, _ID, Iniciado ? "1" : "0"));
|
||
|
|
return;
|
||
|
|
}
|
||
|
|
|
||
|
|
Iniciado = ConfigurarDriver();
|
||
|
|
|
||
|
|
if (_pinoEN.Definido()) {
|
||
|
|
_pinoEN.Conectar(true);
|
||
|
|
}
|
||
|
|
if (_pinoFR.Definido()) {
|
||
|
|
_pinoFR.Conectar();
|
||
|
|
}
|
||
|
|
if (_pinoBK.Definido()) {
|
||
|
|
_pinoBK.Conectar(true);
|
||
|
|
}
|
||
|
|
if (_pinoSV.Definido()) {
|
||
|
|
AtualizarPWM();
|
||
|
|
}
|
||
|
|
|
||
|
|
if (_pinoTMP.Definido()) {
|
||
|
|
_pinoTMP.Conectar();
|
||
|
|
}
|
||
|
|
|
||
|
|
if (Iniciado) {
|
||
|
|
AtualizarDadosControle(true);
|
||
|
|
}
|
||
|
|
|
||
|
|
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;
|
||
|
|
}
|
||
|
|
|
||
|
|
// Parar a execução das tarefas
|
||
|
|
//vTaskDelete(MovTaskHandle);
|
||
|
|
|
||
|
|
// Redefinir as configurações para os valores iniciais
|
||
|
|
_pinoEN.Desconectar();
|
||
|
|
_pinoFR.Desconectar();
|
||
|
|
_pinoSV.Desconectar();
|
||
|
|
_pinoBK.Desconectar();
|
||
|
|
_pinoTMP.Desconectar();
|
||
|
|
|
||
|
|
//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;
|
||
|
|
}
|
||
|
|
|
||
|
|
int val = map(_RPM_SP, 0, _rpmMax, 0, 1023);
|
||
|
|
ledcWrite(_canal, val);
|
||
|
|
_pinoFR.set(_SentidoSP == Horario ? false : true);
|
||
|
|
_pinoBK.set(!_Freio);
|
||
|
|
_pinoEN.set(!_Ligado);
|
||
|
|
|
||
|
|
|
||
|
|
/*ComandoEnviado = true;
|
||
|
|
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);
|
||
|
|
vTaskDelay(100);
|
||
|
|
ComandoEnviado = 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) {
|
||
|
|
if (funcao == 0x03) {
|
||
|
|
if (length >= 5) {
|
||
|
|
resposta._success = true;
|
||
|
|
resposta._address = response[0];
|
||
|
|
resposta._command_code = response[1];
|
||
|
|
resposta._num_registers = response[2];
|
||
|
|
|
||
|
|
if (resposta._num_registers == 2) {
|
||
|
|
uint16_t value = (response[3] << 8) | response[4];
|
||
|
|
resposta._value = static_cast<double>(value);
|
||
|
|
sucesso = true;
|
||
|
|
} else if (resposta._num_registers == 4 && length >= 7) {
|
||
|
|
uint16_t highBytes = (response[3] << 8) | response[4];
|
||
|
|
uint16_t lowBytes = (response[5] << 8) | response[6];
|
||
|
|
|
||
|
|
// Combinar os bytes de alta e baixa ordem em um valor de 32 bits
|
||
|
|
uint32_t combined = (static_cast<uint32_t>(highBytes) << 16) | lowBytes;
|
||
|
|
|
||
|
|
// Reinterpretar o valor combinado como um float
|
||
|
|
float floatValue;
|
||
|
|
memcpy(&floatValue, &combined, sizeof(floatValue));
|
||
|
|
|
||
|
|
resposta._value = static_cast<double>(floatValue);
|
||
|
|
|
||
|
|
sucesso = true;
|
||
|
|
} else {
|
||
|
|
resposta._success = false;
|
||
|
|
}
|
||
|
|
} else {
|
||
|
|
resposta._success = false;
|
||
|
|
}
|
||
|
|
} 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);
|
||
|
|
if (e._success) {
|
||
|
|
ModbusGetResponse f = EnviarComandoModbus(_endereco, 0x06, 0x00B6, 0);
|
||
|
|
return f._success;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
return false;
|
||
|
|
}
|
||
|
|
|
||
|
|
void AferirDadosDriver() {
|
||
|
|
AguardaEnvioComando();
|
||
|
|
RPM_SP = EnviarComandoModbus(_endereco, 0x03, 0x0056, 1)._value;
|
||
|
|
AguardaEnvioComando();
|
||
|
|
RPM = EnviarComandoModbus(_endereco, 0x03, 0x005F, 1)._value;
|
||
|
|
AguardaEnvioComando();
|
||
|
|
Ligado = EnviarComandoModbus(_endereco, 0x03, 0x0066, 1)._value == 1;
|
||
|
|
AguardaEnvioComando();
|
||
|
|
Freio = EnviarComandoModbus(_endereco, 0x03, 0x006A, 1)._value == 1;
|
||
|
|
AguardaEnvioComando();
|
||
|
|
_Sentido = Ligado ? EnviarComandoModbus(_endereco, 0x03, 0x006D, 1)._value == 1 ? Horario : Antihorario : Parado;
|
||
|
|
AguardaEnvioComando();
|
||
|
|
CodAlarme = EnviarComandoModbus(_endereco, 0x03, 0x0076, 1)._value;
|
||
|
|
AguardaEnvioComando();
|
||
|
|
Tensao = EnviarComandoModbus(_endereco, 0x03, 0x00C8, 2)._value;
|
||
|
|
AguardaEnvioComando();
|
||
|
|
CorrenteMax = EnviarComandoModbus(_endereco, 0x03, 0x0096, 2)._value;
|
||
|
|
AguardaEnvioComando();
|
||
|
|
TemperaturaDriver = EnviarComandoModbus(_endereco, 0x03, 0x00D2, 2)._value;
|
||
|
|
}
|
||
|
|
|
||
|
|
void AferirTemperatura() {
|
||
|
|
int leitura = _pinoTMP.get();
|
||
|
|
float leituraNormalizada = tempKalman.updateEstimate(leitura);
|
||
|
|
Temperatura = leituraNormalizada;
|
||
|
|
}
|
||
|
|
|
||
|
|
void AguardaEnvioComando() {
|
||
|
|
while (ComandoEnviado) {
|
||
|
|
vTaskDelay(50);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
};
|
||
|
|
|
||
|
|
class MotorPasso {
|
||
|
|
public:
|
||
|
|
String _ID = "Dir";
|
||
|
|
int _canal = 0;
|
||
|
|
|
||
|
|
volatile bool Iniciado = false;
|
||
|
|
bool Testando = false;
|
||
|
|
|
||
|
|
// Definições
|
||
|
|
byte _endereco = 0x01;
|
||
|
|
PinoModel _pinoENA;
|
||
|
|
PinoModel _pinoDIR;
|
||
|
|
PinoModel _pinoPUL;
|
||
|
|
PinoModel _pinoEncoder;
|
||
|
|
|
||
|
|
SimpleKalmanFilter encoderKalman = SimpleKalmanFilter(1, 2, 0.01);
|
||
|
|
|
||
|
|
// 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;
|
||
|
|
}
|
||
|
|
|
||
|
|
Iniciado = true;
|
||
|
|
|
||
|
|
if (_pinoPUL.Definido()) {
|
||
|
|
AtualizarPWM();
|
||
|
|
}
|
||
|
|
_pinoENA.Conectar(true);
|
||
|
|
_pinoDIR.Conectar(false);
|
||
|
|
_pinoEncoder.Conectar();
|
||
|
|
|
||
|
|
_Angulo_SP = 0;
|
||
|
|
_MotorLigado = false;
|
||
|
|
VerificarAnguloSPparado(true);
|
||
|
|
|
||
|
|
xTaskCreatePinnedToCore(&MotorPasso::DirTaskWrapper, "DirTask", 5000, this, 18, &DirTaskHandle, tskNO_AFFINITY);
|
||
|
|
|
||
|
|
//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;
|
||
|
|
}
|
||
|
|
|
||
|
|
// Parar a execução das tarefas
|
||
|
|
vTaskDelete(DirTaskHandle);
|
||
|
|
|
||
|
|
// 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() {
|
||
|
|
_Angulo = EnviarComandoModbus(_endereco, 0x31, 0)._value;
|
||
|
|
//EnviarDadosSerial(Mod_ID, MontarProtocoloSensor(sENC, _ID, MontarDadosProtocoloEncoderAbsoluto(_Sentido, _Angulo)));
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
private:
|
||
|
|
|
||
|
|
void sendModbusCommandStepper(ModbusMessage& message) {
|
||
|
|
uint8_t command[5];
|
||
|
|
|
||
|
|
command[0] = 0xFA;
|
||
|
|
command[1] = message.slaveAddress;
|
||
|
|
command[2] = message.functionCode;
|
||
|
|
|
||
|
|
uint16_t crc = calculateCRC(command, 3);
|
||
|
|
command[3] = crc & 0xFF;
|
||
|
|
command[4] = (crc >> 8) & 0xFF;
|
||
|
|
|
||
|
|
// Alternar para modo de transmissão
|
||
|
|
digitalWrite(DE_RE_PIN, HIGH);
|
||
|
|
vTaskDelay(pdMS_TO_TICKS(1)); // Pequeno atraso para garantir que o modo de transmissão esteja ativo
|
||
|
|
|
||
|
|
SerialDriver.write(command, 5);
|
||
|
|
if (Debug) {
|
||
|
|
// Imprimir o comando em formato hexadecimal
|
||
|
|
Serial.print("Comando enviado: ");
|
||
|
|
for (int i = 0; i < 8; i++) {
|
||
|
|
if (command[i] < 0x10) {
|
||
|
|
Serial.print("0"); // Adiciona um zero à esquerda para bytes menores que 0x10
|
||
|
|
}
|
||
|
|
Serial.print(command[i], HEX);
|
||
|
|
Serial.print(" ");
|
||
|
|
}
|
||
|
|
Serial.println();
|
||
|
|
}
|
||
|
|
|
||
|
|
// Alternar para modo de recepção
|
||
|
|
SerialDriver.flush(); // Garantir que todos os dados foram enviados
|
||
|
|
digitalWrite(DE_RE_PIN, LOW);
|
||
|
|
vTaskDelay(pdMS_TO_TICKS(1)); // Pequeno atraso para garantir que o modo de recepção esteja ativo
|
||
|
|
|
||
|
|
*message.length = 0;
|
||
|
|
|
||
|
|
if (message.waitCallback) {
|
||
|
|
// Aguardar a resposta
|
||
|
|
unsigned long startTime = millis();
|
||
|
|
unsigned long timeout = 1000;
|
||
|
|
while (SerialDriver.available() < 5 && (millis() - startTime) < timeout) {
|
||
|
|
vTaskDelay(pdMS_TO_TICKS(50));
|
||
|
|
}
|
||
|
|
|
||
|
|
// Ler a resposta
|
||
|
|
if (SerialDriver.available()) {
|
||
|
|
*message.length = SerialDriver.readBytes(message.response, SerialDriver.available());
|
||
|
|
|
||
|
|
if (Debug) {
|
||
|
|
// Imprimir a resposta em formato hexadecimal
|
||
|
|
Serial.print("Resposta recebida: ");
|
||
|
|
for (int i = 0; i < *message.length; i++) {
|
||
|
|
if (message.response[i] < 0x10) {
|
||
|
|
Serial.print("0"); // Adiciona um zero à esquerda para bytes menores que 0x10
|
||
|
|
}
|
||
|
|
Serial.print(message.response[i], HEX);
|
||
|
|
Serial.print(" ");
|
||
|
|
}
|
||
|
|
Serial.println();
|
||
|
|
}
|
||
|
|
} else {
|
||
|
|
Serial.println("Timeout ao aguardar resposta Modbus");
|
||
|
|
}
|
||
|
|
} else {
|
||
|
|
vTaskDelay(pdMS_TO_TICKS(50));
|
||
|
|
}
|
||
|
|
|
||
|
|
// Sinalizar que a resposta está disponível
|
||
|
|
xSemaphoreGive(message.responseSemaphore);
|
||
|
|
}
|
||
|
|
|
||
|
|
ModbusGetResponse EnviarComandoModbus(byte endereco, byte funcao, 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;
|
||
|
|
|
||
|
|
ModbusMessage message;
|
||
|
|
message.slaveAddress = endereco;
|
||
|
|
message.functionCode = funcao;
|
||
|
|
message.value = valor;
|
||
|
|
message.response = response;
|
||
|
|
message.length = &length;
|
||
|
|
message.priority = PRIORITY_HIGH;
|
||
|
|
message.waitCallback = aguardarResposta;
|
||
|
|
message.responseSemaphore = xSemaphoreCreateBinary();
|
||
|
|
|
||
|
|
sendModbusCommandStepper(message);
|
||
|
|
|
||
|
|
if (aguardarResposta) {
|
||
|
|
if (length >= 5) {
|
||
|
|
resposta._success = true;
|
||
|
|
resposta._address = response[1];
|
||
|
|
resposta._command_code = response[2];
|
||
|
|
|
||
|
|
if (length <= 7) {
|
||
|
|
uint16_t value = (response[3] << 8) | response[4];
|
||
|
|
resposta._value = static_cast<double>(value);
|
||
|
|
sucesso = true;
|
||
|
|
} else if (length > 7) {
|
||
|
|
uint16_t highBytes = (response[3] << 8) | response[4];
|
||
|
|
uint16_t lowBytes = (response[5] << 8) | response[6];
|
||
|
|
|
||
|
|
// Combinar os bytes de alta e baixa ordem em um valor de 32 bits
|
||
|
|
uint32_t combined = (static_cast<uint32_t>(highBytes) << 16) | lowBytes;
|
||
|
|
|
||
|
|
// Reinterpretar o valor combinado como um float
|
||
|
|
float floatValue;
|
||
|
|
memcpy(&floatValue, &combined, sizeof(floatValue));
|
||
|
|
|
||
|
|
resposta._value = static_cast<double>(floatValue);
|
||
|
|
|
||
|
|
sucesso = true;
|
||
|
|
} else {
|
||
|
|
resposta._success = false;
|
||
|
|
}
|
||
|
|
} else {
|
||
|
|
resposta._success = false;
|
||
|
|
}
|
||
|
|
|
||
|
|
tentativas++;
|
||
|
|
} else {
|
||
|
|
sucesso = true;
|
||
|
|
resposta._success = sucesso;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
return resposta;
|
||
|
|
}
|
||
|
|
|
||
|
|
double _Margem = 0.5;
|
||
|
|
double _AgMin = 95;
|
||
|
|
double _AgMax = 855;
|
||
|
|
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();*/
|
||
|
|
|
||
|
|
/*if (millis() - previousMillisRampa >= _TaxaAmostragem) {
|
||
|
|
previousMillisRampa = millis();
|
||
|
|
|
||
|
|
RequisitarDados();
|
||
|
|
}*/
|
||
|
|
|
||
|
|
vTaskDelay(100);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
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();
|
||
|
|
|
||
|
|
VerificarAnguloSPparado(_Estabilizar);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
void VerificarAnguloSPparado(bool _estabilizar) {
|
||
|
|
bool NoRangeZero = (_Angulo >= -_Margem && _Angulo <= _Margem);
|
||
|
|
bool NoRangeSP = (_Angulo >= (_Angulo_SP - _Margem) && _Angulo <= (_Angulo_SP + _Margem));
|
||
|
|
if (_Angulo_SP == 0) {
|
||
|
|
if (!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 AtualizarPWM() {
|
||
|
|
if (MotorDIR.Iniciado && MotorDIR._pinoPUL.Definido()) {
|
||
|
|
ledcSetup(MotorDIR._canal, MotorDIR._frequencia, 10);
|
||
|
|
ledcAttachPin(MotorDIR._pinoPUL.num, MotorDIR._canal);
|
||
|
|
ledcWrite(MotorDIR._canal, MotorDIR.dutyCycle);
|
||
|
|
}
|
||
|
|
if (MotorMOV.Iniciado && MotorMOV._pinoSV.Definido()) {
|
||
|
|
ledcSetup(MotorMOV._canal, 1000, 10);
|
||
|
|
ledcAttachPin(MotorMOV._pinoSV.num, MotorMOV._canal);
|
||
|
|
ledcWrite(MotorMOV._canal, MotorMOV._RPM_SP);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
void SensoriamentoTask(void *pvParameters);
|
||
|
|
TaskHandle_t SensoriamentoTaskHandle = NULL;
|
||
|
|
|
||
|
|
void setup() {
|
||
|
|
pinMode(_pinoAddr0, INPUT);
|
||
|
|
pinMode(_pinoAddr1, INPUT);
|
||
|
|
|
||
|
|
AtualizarEnderecoModulo();
|
||
|
|
|
||
|
|
Serial.begin(_baudRate, SERIAL_8N1, 1, 3);
|
||
|
|
|
||
|
|
analogReadResolution(10);
|
||
|
|
|
||
|
|
delay(10);
|
||
|
|
|
||
|
|
ConfigurarModbus(true, _pinModbusRX, _pinModbusTX, _pinModbusMD, 9600, true);
|
||
|
|
|
||
|
|
delay(10);
|
||
|
|
|
||
|
|
if (xTaskCreatePinnedToCore(SensoriamentoTask, "SensoriamentoTask", 8000, NULL, 5, &SensoriamentoTaskHandle, tskNO_AFFINITY) != pdPASS) {
|
||
|
|
Serial.println("Erro ao criar tarefa SensoriamentoTask");
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
void SensoriamentoTask(void *pvParameters) {
|
||
|
|
while (1)
|
||
|
|
{
|
||
|
|
Conectado = MotorMOV.Iniciado || MotorDIR.Iniciado;
|
||
|
|
|
||
|
|
if (!RequisitarDados && Conectado) {
|
||
|
|
|
||
|
|
if (MotorMOV.Iniciado) {
|
||
|
|
MotorMOV.RequisitarDados();
|
||
|
|
}
|
||
|
|
if (MotorDIR.Iniciado) {
|
||
|
|
MotorDIR.RequisitarDados();
|
||
|
|
}
|
||
|
|
|
||
|
|
EnviarDadosSerial(Mod_ID, MontarProtocoloSensor(sMVD, Mod_ID, MontarDadosProtocoloMVD(
|
||
|
|
MotorMOV.Iniciado,
|
||
|
|
MotorMOV.RPM,
|
||
|
|
MotorMOV.Temperatura,
|
||
|
|
MotorMOV.Tensao,
|
||
|
|
MotorMOV.CorrenteMax,
|
||
|
|
MotorMOV.Ligado,
|
||
|
|
MotorMOV._Sentido,
|
||
|
|
MotorMOV.Freio,
|
||
|
|
MotorMOV.CodAlarme,
|
||
|
|
MotorMOV.TemperaturaDriver,
|
||
|
|
MotorDIR.Iniciado,
|
||
|
|
MotorDIR._Sentido,
|
||
|
|
MotorDIR._Angulo
|
||
|
|
)));
|
||
|
|
}
|
||
|
|
vTaskDelay(_TaxaAmostragem);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
void loop() {
|
||
|
|
std::vector<ProtocoloSerial> protocolos = LerBufferSerialFila(Mod_ID);
|
||
|
|
|
||
|
|
for (int i = 0; i < protocolos.size(); i++) {
|
||
|
|
ProcessarProtocolo(protocolos[i]);
|
||
|
|
}
|
||
|
|
|
||
|
|
delay(1);
|
||
|
|
}
|
||
|
|
|
||
|
|
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;
|
||
|
|
}
|
||
|
|
|
||
|
|
if (Mensagem.idMensagem > 0) {
|
||
|
|
EnviarDadosSerial(Mod_ID, MontarProtocoloMensagemRecebida(Mensagem.idMensagem));
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
void ProcessarChk(ProtocoloSerial Mensagem) {
|
||
|
|
Conectado = MotorMOV.Iniciado || MotorDIR.Iniciado;
|
||
|
|
|
||
|
|
EnviarDadosSerial(Mod_ID, MontarProtocoloVerificacao(D_Code, VERSION, Conectado));
|
||
|
|
}
|
||
|
|
|
||
|
|
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();
|
||
|
|
RequisitarDados = Partes[3].toInt();
|
||
|
|
TiposBarramentos txB = (TiposBarramentos)Partes[4].toInt();
|
||
|
|
int txP = Partes[5].toInt();
|
||
|
|
TiposBarramentos rxB = (TiposBarramentos)Partes[6].toInt();
|
||
|
|
int rxP = Partes[7].toInt();
|
||
|
|
TiposBarramentos MtxB = (TiposBarramentos)Partes[8].toInt();
|
||
|
|
int MtxP = Partes[9].toInt();
|
||
|
|
TiposBarramentos MrxB = (TiposBarramentos)Partes[10].toInt();
|
||
|
|
int MrxP = Partes[11].toInt();
|
||
|
|
TiposBarramentos MmdB = (TiposBarramentos)Partes[12].toInt();
|
||
|
|
int MmdP = Partes[13].toInt();
|
||
|
|
_pinModbusMD = MmdP;
|
||
|
|
_pinModbusRX = MrxP;
|
||
|
|
_pinModbusTX = MtxP;
|
||
|
|
|
||
|
|
//ConfigurarModbus(Conectar, _pinModbusRX, _pinModbusTX, _pinModbusMD);
|
||
|
|
}
|
||
|
|
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._pinoEN = PinoModel(Partes[9].toInt(), (TiposBarramentos)Partes[8].toInt(), _OUTPUT);
|
||
|
|
MotorMOV._pinoFR = PinoModel(Partes[11].toInt(), (TiposBarramentos)Partes[10].toInt(), _OUTPUT);
|
||
|
|
MotorMOV._pinoSV = PinoModel(Partes[13].toInt(), (TiposBarramentos)Partes[12].toInt(), _PWM);
|
||
|
|
MotorMOV._pinoBK = PinoModel(Partes[15].toInt(), (TiposBarramentos)Partes[14].toInt(), _OUTPUT);
|
||
|
|
|
||
|
|
MotorMOV.Inicializar();
|
||
|
|
}
|
||
|
|
else {
|
||
|
|
MotorMOV.Desligar();
|
||
|
|
}
|
||
|
|
}
|
||
|
|
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();
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
void ProcessarCmd(ProtocoloSerial Mensagem) {
|
||
|
|
std::vector<String> Partes = SplitString(Mensagem.protocolo, SplitParams);
|
||
|
|
|
||
|
|
String ID = Partes[0];
|
||
|
|
|
||
|
|
if (ID == MotorMOV._ID && MotorMOV.Iniciado) {
|
||
|
|
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.Iniciado) {
|
||
|
|
MotorDIR._Sentido_SP = (Sentido)Partes[1].toInt();
|
||
|
|
MotorDIR._Angulo_SP = Partes[2].toInt();
|
||
|
|
MotorDIR._frequencia = (Partes[3].toInt() / 100.0) * MotorDIR.FrequenciaMaxima;
|
||
|
|
|
||
|
|
AtualizarPWM();
|
||
|
|
|
||
|
|
MotorDIR._MotorLigado = MotorDIR._Sentido_SP != Parado;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
void ProcessarReq(ProtocoloSerial Mensagem) {
|
||
|
|
//std::vector<String> Partes = SplitString(Mensagem.protocolo, SplitParams);
|
||
|
|
//Serial.println("Processando req...");
|
||
|
|
if (MotorMOV.Iniciado) {
|
||
|
|
MotorMOV.RequisitarDados();
|
||
|
|
}
|
||
|
|
if (MotorDIR.Iniciado) {
|
||
|
|
MotorDIR.RequisitarDados();
|
||
|
|
}
|
||
|
|
|
||
|
|
if (MotorMOV.Iniciado || MotorDIR.Iniciado) {
|
||
|
|
//Serial.println("Enviando dados...");
|
||
|
|
EnviarDadosSerial(Mod_ID, MontarProtocoloSensor(sMVD, Mod_ID, MontarDadosProtocoloMVD(
|
||
|
|
MotorMOV.Iniciado,
|
||
|
|
MotorMOV.RPM,
|
||
|
|
MotorMOV.Temperatura,
|
||
|
|
MotorMOV.Tensao,
|
||
|
|
MotorMOV.CorrenteMax,
|
||
|
|
MotorMOV.Ligado,
|
||
|
|
MotorMOV._Sentido,
|
||
|
|
MotorMOV.Freio,
|
||
|
|
MotorMOV.CodAlarme,
|
||
|
|
MotorMOV.TemperaturaDriver,
|
||
|
|
MotorDIR.Iniciado,
|
||
|
|
MotorDIR._Sentido,
|
||
|
|
MotorDIR._Angulo
|
||
|
|
)));
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
|