753 lines
21 KiB
C++
753 lines
21 KiB
C++
// Dispositivo: Movimentação e Direcional Unificados
|
|
// Versão Firmware: 1
|
|
// Ultima atualização: 06/06/2024
|
|
// Atualização: Unificação dos módulos direcional e movimentação
|
|
|
|
#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 1
|
|
String Mod_ID = "";
|
|
int Cod_Alarme = 0;
|
|
|
|
#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;
|
|
|
|
|
|
class MotorBLDC {
|
|
public:
|
|
String _ID = "Mov";
|
|
|
|
bool Iniciado = false;
|
|
bool Testando = false;
|
|
|
|
PinoModel _pinoTMP;
|
|
SimpleKalmanFilter* tempKalman = nullptr;
|
|
|
|
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, _ID + " ja inicializado");
|
|
EnviarDadosSerial(Mod_ID, MontarProtocoloSensor(sCFG, _ID, Iniciado ? "1" : "0"));
|
|
return;
|
|
}
|
|
|
|
Iniciado = ConfigurarDriver();
|
|
|
|
if (_pinoTMP.Definido()) {
|
|
_pinoTMP.Conectar();
|
|
tempKalman = new (std::nothrow) SimpleKalmanFilter(2, 2, 0.01);
|
|
}
|
|
|
|
//xTaskCreatePinnedToCore(&MotorBLDC::MovTaskWrapper, "MovTask", 5000, this, 18, &MovTaskHandle, tskNO_AFFINITY);
|
|
|
|
if (Iniciado) {
|
|
AtualizarDadosControle(true);
|
|
|
|
//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(MovTaskHandle);
|
|
|
|
// Redefinir as configurações para os valores iniciais
|
|
_pinoTMP.Desconectar();
|
|
|
|
// Outras redefinições de variáveis de estado, se necessário
|
|
delete tempKalman;
|
|
tempKalman = nullptr;
|
|
|
|
//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;
|
|
}
|
|
|
|
/*if (_SentidoSP != Parado && _SentidoSP != _Sentido) {
|
|
int std = EnviarComandoModbus(_endereco, 0x06, 0x006D, _SentidoSP == Horario ? 1 : 0)._value;
|
|
_Sentido = !Ligado ? Parado : std == 0 ? Horario : Antihorario;
|
|
}
|
|
if (_RPM_SP != RPM_SP) {
|
|
RPM_SP = EnviarComandoModbus(_endereco, 0x06, 0x0056, _RPM_SP)._value;
|
|
}
|
|
if (_Freio != Freio) {
|
|
Freio = EnviarComandoModbus(_endereco, 0x06, 0x006A, _Freio ? 1 : 0)._value == 1;
|
|
}
|
|
if (_Ligado != Ligado) {
|
|
Ligado = EnviarComandoModbus(_endereco, 0x06, 0x0066, _Ligado ? 1 : 0)._value == 1;
|
|
}*/
|
|
|
|
/*if (_SentidoSP != Parado && _SentidoSP != _Sentido) {
|
|
EnviarComandoModbus(_endereco, 0x06, 0x006D, _SentidoSP == Horario ? 1 : 0, false);
|
|
}
|
|
if (_RPM_SP != RPM_SP) {
|
|
EnviarComandoModbus(_endereco, 0x06, 0x0056, _RPM_SP, false);
|
|
}
|
|
if (_Freio != Freio) {
|
|
EnviarComandoModbus(_endereco, 0x06, 0x006A, _Freio ? 1 : 0, false);
|
|
}
|
|
if (_Ligado != Ligado) {
|
|
EnviarComandoModbus(_endereco, 0x06, 0x0066, _Ligado ? 1 : 0, false);
|
|
}*/
|
|
|
|
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:
|
|
|
|
/*TaskHandle_t MovTaskHandle = NULL;
|
|
static void MovTaskWrapper(void *pvParameters) {
|
|
MotorBLDC* motor = static_cast<MotorBLDC*>(pvParameters);
|
|
motor->MovTask();
|
|
}
|
|
|
|
void MovTask() {
|
|
unsigned long previousMillisRampa = millis();
|
|
|
|
while (true) {
|
|
if (!Conectado) {
|
|
vTaskDelay(1000);
|
|
continue;
|
|
}
|
|
|
|
if (millis() - previousMillisRampa >= _TaxaAmostragem) {
|
|
previousMillisRampa = millis();
|
|
|
|
RequisitarDados();
|
|
}
|
|
|
|
vTaskDelay(10);
|
|
}
|
|
}*/
|
|
|
|
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) {
|
|
// GET
|
|
if (funcao == 0x03) {
|
|
if (length >= 5) { // Verifica se a resposta tem pelo menos 5 bytes (endereco, codigo_comando, numero_registros, 2 bytes de dados, 2 bytes de CRC)
|
|
resposta._success = true;
|
|
resposta._address = response[0];
|
|
resposta._command_code = response[1];
|
|
resposta._num_registers = response[2];
|
|
|
|
if (resposta._num_registers == 2) { // Caso de 2 bytes (sem ponto flutuante)
|
|
uint16_t value = (response[3] << 8) | response[4]; // Combina os dois bytes
|
|
resposta._value = static_cast<double>(value); // Converte para double
|
|
sucesso = true;
|
|
} else if (resposta._num_registers == 4 && length >= 7) { // Caso de 4 bytes (com ponto flutuante)
|
|
uint16_t highBytes = (response[3] << 8) | response[4]; // Combina os dois primeiros bytes
|
|
uint16_t lowBytes = (response[5] << 8) | response[6]; // Combina os dois últimos bytes
|
|
|
|
// Converte para double
|
|
resposta._value = highBytes + (lowBytes / 10000.0); // Supondo que os valores após a vírgula são representados em 4 dígitos decimais
|
|
sucesso = true;
|
|
} else {
|
|
//Serial.println("Numero de registros inesperado ou resposta incompleta.");
|
|
resposta._success = false;
|
|
}
|
|
} else {
|
|
//Serial.println("Resposta incompleta.");
|
|
resposta._success = false;
|
|
}
|
|
}
|
|
// SET
|
|
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);
|
|
return e._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;
|
|
|
|
bool Iniciado = false;
|
|
bool Testando = false;
|
|
|
|
// Definições
|
|
PinoModel _pinoENA;
|
|
PinoModel _pinoDIR;
|
|
PinoModel _pinoPUL;
|
|
PinoModel _pinoEncoder;
|
|
|
|
SimpleKalmanFilter* encoderKalman = nullptr;
|
|
|
|
// 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;
|
|
}
|
|
|
|
if (_pinoPUL.Definido()) {
|
|
AtualizarPWM();
|
|
}
|
|
_pinoENA.Conectar(true);
|
|
_pinoDIR.Conectar(false);
|
|
_pinoEncoder.Conectar();
|
|
|
|
_Angulo_SP = 0;
|
|
_MotorLigado = false;
|
|
VerificarAnguloSPparado(true);
|
|
|
|
encoderKalman = new (std::nothrow) SimpleKalmanFilter(1, 2, 0.01);
|
|
|
|
xTaskCreatePinnedToCore(&MotorPasso::DirTaskWrapper, "DirTask", 5000, this, 18, &DirTaskHandle, tskNO_AFFINITY);
|
|
|
|
//EnviarDadosSerial(Mod_ID, _ID + " Iniciado");
|
|
|
|
Iniciado = true;
|
|
|
|
EnviarDadosSerial(Mod_ID, MontarProtocoloSensor(sCFG, _ID, Iniciado ? "1" : "0"));
|
|
}
|
|
|
|
void AtualizarPWM() {
|
|
ledcSetup(_canal, _frequencia, 10);
|
|
ledcAttachPin(_pinoPUL.num, _canal);
|
|
ledcWrite(_canal, dutyCycle);
|
|
}
|
|
|
|
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);
|
|
|
|
delete encoderKalman;
|
|
encoderKalman = nullptr;
|
|
|
|
// 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() {
|
|
//EnviarDadosSerial(Mod_ID, MontarProtocoloSensor(sENC, _ID, MontarDadosProtocoloEncoderAbsoluto(_Sentido, _Angulo)));
|
|
}
|
|
|
|
|
|
private:
|
|
|
|
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(1);
|
|
}
|
|
}
|
|
|
|
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 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);
|
|
|
|
xTaskCreatePinnedToCore(SensoriamentoTask, "SensoriamentoTask", 8000, NULL, 5, &SensoriamentoTaskHandle, tskNO_AFFINITY);
|
|
}
|
|
|
|
void SensoriamentoTask(void *pvParameters) {
|
|
unsigned long firstMilis = millis();
|
|
while (1)
|
|
{
|
|
long intMillis = (millis() - firstMilis);
|
|
|
|
if (Conectado && intMillis >= _TaxaAmostragem) {
|
|
|
|
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
|
|
)));
|
|
|
|
|
|
firstMilis = millis();
|
|
}
|
|
|
|
vTaskDelay(1);
|
|
}
|
|
}
|
|
|
|
void loop() {
|
|
std::vector<ProtocoloSerial> protocolos = LerBufferSerialFila(Mod_ID);
|
|
|
|
for (int i = 0; i < protocolos.size(); i++) {
|
|
ProcessarProtocolo(protocolos[i]);
|
|
}
|
|
}
|
|
|
|
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();
|
|
TiposBarramentos txB = (TiposBarramentos)Partes[3].toInt();
|
|
int txP = Partes[4].toInt();
|
|
TiposBarramentos rxB = (TiposBarramentos)Partes[5].toInt();
|
|
int rxP = Partes[6].toInt();
|
|
TiposBarramentos MtxB = (TiposBarramentos)Partes[7].toInt();
|
|
int MtxP = Partes[8].toInt();
|
|
TiposBarramentos MrxB = (TiposBarramentos)Partes[9].toInt();
|
|
int MrxP = Partes[10].toInt();
|
|
TiposBarramentos MmdB = (TiposBarramentos)Partes[11].toInt();
|
|
int MmdP = Partes[12].toInt();
|
|
_pinModbusMD = MmdP;
|
|
_pinModbusRX = MrxP;
|
|
_pinModbusTX = MtxP;
|
|
|
|
InicializarModbus(_pinModbusRX, _pinModbusTX, _pinModbusMD);
|
|
|
|
vTaskDelay(100);
|
|
}
|
|
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.Inicializar();
|
|
}
|
|
else {
|
|
MotorMOV.Desligar();
|
|
}
|
|
|
|
vTaskDelay(500);
|
|
}
|
|
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();
|
|
}
|
|
|
|
vTaskDelay(500);
|
|
}
|
|
}
|
|
|
|
void ProcessarCmd(ProtocoloSerial Mensagem) {
|
|
std::vector<String> Partes = SplitString(Mensagem.protocolo, SplitParams);
|
|
|
|
String ID = Partes[0];
|
|
|
|
if (ID == MotorMOV._ID) {
|
|
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._Sentido_SP = (Sentido)Partes[1].toInt();
|
|
MotorDIR._Angulo_SP = Partes[2].toInt();
|
|
MotorDIR._frequencia = (Partes[3].toInt() / 100.0) * MotorDIR.FrequenciaMaxima;
|
|
|
|
MotorDIR.AtualizarPWM();
|
|
|
|
MotorDIR._MotorLigado = MotorDIR._Sentido_SP != Parado;
|
|
}
|
|
}
|
|
|
|
void ProcessarReq(ProtocoloSerial Mensagem) {
|
|
std::vector<String> Partes = SplitString(Mensagem.protocolo, SplitParams);
|
|
|
|
String ID = Partes[0];
|
|
|
|
if (ID == MotorMOV._ID) {
|
|
MotorMOV.RequisitarDados();
|
|
}
|
|
else if (ID == MotorDIR._ID) {
|
|
MotorDIR.RequisitarDados();
|
|
}
|
|
}
|
|
|
|
|
|
|