402 lines
12 KiB
Arduino
402 lines
12 KiB
Arduino
|
// 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 <HardwareSerial.h>
|
||
|
|
#include <SimpleKalmanFilter.h>
|
||
|
|
|
||
|
|
#define D_Code Mvd
|
||
|
|
#define VERSION 1
|
||
|
|
String Mod_ID = "";
|
||
|
|
int Cod_Alarme = 0;
|
||
|
|
|
||
|
|
#define _pinoLED RGB_BUILTIN
|
||
|
|
#define _pinoAddr0 36
|
||
|
|
#define _pinoAddr1 39
|
||
|
|
|
||
|
|
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;
|
||
|
|
bool Conectado = false;
|
||
|
|
int _TaxaAmostragem = 500;
|
||
|
|
|
||
|
|
|
||
|
|
class MotorBLDC {
|
||
|
|
public:
|
||
|
|
String _ID = (String)Mov;
|
||
|
|
|
||
|
|
bool Iniciado = false;
|
||
|
|
bool Testando = false;
|
||
|
|
|
||
|
|
PinoModel _pinoTMP;
|
||
|
|
SimpleKalmanFilter* tempKalman;
|
||
|
|
|
||
|
|
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;
|
||
|
|
|
||
|
|
|
||
|
|
void Inicializar() {
|
||
|
|
if (Iniciado) {
|
||
|
|
EnviarDadosSerial(Mod_ID, _ID + " ja inicializado");
|
||
|
|
EnviarDadosSerial(Mod_ID, MontarProtocoloSensor(sCFG, _ID, Iniciado ? "1" : "0"));
|
||
|
|
return;
|
||
|
|
}
|
||
|
|
|
||
|
|
Iniciado = ConfigurarDriver();
|
||
|
|
|
||
|
|
if (Iniciado) {
|
||
|
|
if (_pinoTMP.Definido()) {
|
||
|
|
_pinoTMP.Conectar();
|
||
|
|
tempKalman = new SimpleKalmanFilter(2, 2, 0.01);
|
||
|
|
}
|
||
|
|
|
||
|
|
xTaskCreatePinnedToCore(&MotorBLDC::MovTaskWrapper, "MovTask", 5000, this, 20, &MovTaskHandle, tskNO_AFFINITY);
|
||
|
|
|
||
|
|
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;
|
||
|
|
|
||
|
|
EnviarDadosSerial(Mod_ID, _ID + " Desligado");
|
||
|
|
|
||
|
|
Iniciado = false;
|
||
|
|
|
||
|
|
EnviarDadosSerial(Mod_ID, MontarProtocoloSensor(sCFG, _ID, Iniciado ? "1" : "0"));
|
||
|
|
|
||
|
|
AtualizarDadosControle(true);
|
||
|
|
}
|
||
|
|
|
||
|
|
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 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);
|
||
|
|
}
|
||
|
|
|
||
|
|
/*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);*/
|
||
|
|
|
||
|
|
}
|
||
|
|
|
||
|
|
private:
|
||
|
|
|
||
|
|
TaskHandle_t MovTaskHandle = NULL;
|
||
|
|
static void MovTaskWrapper(void *pvParameters) {
|
||
|
|
MotorBLDC *motor = static_cast<MotorBLDC*>(pvParameters);
|
||
|
|
motor->MovTask();
|
||
|
|
}
|
||
|
|
|
||
|
|
void MovTask() {
|
||
|
|
unsigned long firstMilis = millis();
|
||
|
|
while (1)
|
||
|
|
{
|
||
|
|
if (!Conectado) {
|
||
|
|
// Caso não esteja conectado, aguarda 1 segundo até a próxima verificação, liberando uso de CPU
|
||
|
|
vTaskDelay(1000);
|
||
|
|
continue;
|
||
|
|
}
|
||
|
|
|
||
|
|
long intMillis = (millis() - firstMilis);
|
||
|
|
|
||
|
|
// A cada _TaxaAmostragem, enviar os dados para o software
|
||
|
|
if (intMillis >= _TaxaAmostragem) {
|
||
|
|
|
||
|
|
AferirTemperatura();
|
||
|
|
AferirDadosDriver();
|
||
|
|
|
||
|
|
EnviarDadosSerial(Mod_ID, MontarProtocoloSensor(sBLD, _ID, MontarDadosProtocoloBLD(RPM, Temperatura, Tensao, CorrenteMax, Ligado, _Sentido, Freio, CodAlarme, TemperaturaDriver)));
|
||
|
|
firstMilis = millis();
|
||
|
|
}
|
||
|
|
|
||
|
|
// Aguarda metade do menor período possível entre leituras
|
||
|
|
vTaskDelay(1);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
void AferirDadosDriver() {
|
||
|
|
RPM_SP = EnviarComandoModbus(_endereco, 0x03, 0x0056, 1)._value;
|
||
|
|
RPM = EnviarComandoModbus(_endereco, 0x03, 0x005F, 1)._value;
|
||
|
|
Ligado = EnviarComandoModbus(_endereco, 0x03, 0x0066, 1)._value == 1;
|
||
|
|
Freio = EnviarComandoModbus(_endereco, 0x03, 0x006A, 1)._value == 1;
|
||
|
|
_Sentido = Ligado ? EnviarComandoModbus(_endereco, 0x03, 0x006D, 1)._value == 1 ? Horario : Antihorario : Parado;
|
||
|
|
CodAlarme = EnviarComandoModbus(_endereco, 0x03, 0x0076, 1)._value;
|
||
|
|
Tensao = EnviarComandoModbus(_endereco, 0x03, 0x00C8, 2)._value;
|
||
|
|
CorrenteMax = EnviarComandoModbus(_endereco, 0x03, 0x0096, 2)._value;
|
||
|
|
TemperaturaDriver = EnviarComandoModbus(_endereco, 0x03, 0x00D2, 2)._value;
|
||
|
|
}
|
||
|
|
|
||
|
|
void AferirTemperatura() {
|
||
|
|
int leitura = _pinoTMP.get();
|
||
|
|
float leituraNormalizada = tempKalman->updateEstimate(leitura);
|
||
|
|
Temperatura = leituraNormalizada;
|
||
|
|
}
|
||
|
|
|
||
|
|
};
|
||
|
|
|
||
|
|
MotorBLDC MotorMOV;
|
||
|
|
|
||
|
|
void setup() {
|
||
|
|
pinMode(_pinoAddr0, INPUT);
|
||
|
|
pinMode(_pinoAddr1, INPUT);
|
||
|
|
|
||
|
|
AtualizarEnderecoModulo();
|
||
|
|
|
||
|
|
Serial.begin(_baudRate);
|
||
|
|
|
||
|
|
delay(10);
|
||
|
|
|
||
|
|
InicializarModbus();
|
||
|
|
}
|
||
|
|
|
||
|
|
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(Mod_ID, 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();
|
||
|
|
|
||
|
|
EnviarDadosSerial(MontarProtocoloSensor(sCFG, "MD", Conectar ? "1" : "0"));
|
||
|
|
|
||
|
|
vTaskDelay(pdMS_TO_TICKS(100));
|
||
|
|
|
||
|
|
Conectado = Conectar;
|
||
|
|
}
|
||
|
|
else {
|
||
|
|
int canal = Partes[2].toInt();
|
||
|
|
String addr = Partes[3];
|
||
|
|
int num_polos = Partes[4].toInt();
|
||
|
|
int rampa = Partes[5].toInt();
|
||
|
|
int rpm_max = Partes[6].toInt();
|
||
|
|
TiposBarramentos tmpB = (TiposBarramentos)Partes[7].toInt();
|
||
|
|
int tmp = Partes[8].toInt();
|
||
|
|
|
||
|
|
MotorBLDC* _motor = MotorPorID(ID);
|
||
|
|
if (Conectar) {
|
||
|
|
_motor->_canal = canal;
|
||
|
|
_motor->_endereco = stringToByte(addr);
|
||
|
|
_motor->_numPolos = num_polos;
|
||
|
|
_motor->_rampa = rampa;
|
||
|
|
_motor->_rpmMax = rpm_max;
|
||
|
|
_motor->_pinoTMP = PinoModel(tmp, tmpB, _INPUT, ANALOGICO);
|
||
|
|
|
||
|
|
_motor->Inicializar();
|
||
|
|
}
|
||
|
|
else {
|
||
|
|
_motor->Desligar();
|
||
|
|
}
|
||
|
|
vTaskDelay(pdMS_TO_TICKS(500));
|
||
|
|
}
|
||
|
|
}
|
||
|
|
else if (_funcao == Cmd) {
|
||
|
|
std::vector<String> Partes = SplitString(Protocolo, (char*)",");
|
||
|
|
|
||
|
|
String ID = Partes[0];
|
||
|
|
|
||
|
|
MotorBLDC* _motor = MotorPorID(ID);
|
||
|
|
|
||
|
|
_motor->_SentidoSP = (Sentido)Partes[1].toInt();
|
||
|
|
_motor->_Ligado = _motor->_SentidoSP != Parado;
|
||
|
|
_motor->_RPM_SP = Partes[2].toInt();
|
||
|
|
_motor->_Freio = Partes[3] == "1";
|
||
|
|
|
||
|
|
_motor->AtualizarDadosControle();
|
||
|
|
}*/
|
||
|
|
|
||
|
|
EnviarDadosSerial(Mod_ID, MontarProtocoloMensagemRecebida(idMensagem));
|
||
|
|
|
||
|
|
}
|