422 lines
13 KiB
C++
422 lines
13 KiB
C++
// Dispositivo: Movimentação
|
|
// Versão Firmware: 19
|
|
// Ultima atualização: 03/06/2024
|
|
// Atualização: Drivers BLD-300R
|
|
|
|
#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 Mov
|
|
#define VERSION 19
|
|
|
|
#define _pinoLED RGB_BUILTIN
|
|
|
|
long _baudRate = 115200;
|
|
bool Conectado = false;
|
|
int _TaxaAmostragem = 500;
|
|
|
|
|
|
class Motor {
|
|
public:
|
|
// Construtor
|
|
Motor(String desc) {
|
|
_ID = desc;
|
|
}
|
|
|
|
// Definições
|
|
String _ID;
|
|
|
|
bool Iniciado = false;
|
|
bool Testando = false;
|
|
|
|
PinoModel _pinoTMP;
|
|
SimpleKalmanFilter* tempKalman;
|
|
|
|
int _canal;
|
|
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(_ID + " ja inicializado");
|
|
EnviarDadosSerial(MontarProtocoloSensor(sCFG, _ID, Iniciado ? "1" : "0"));
|
|
return;
|
|
}
|
|
|
|
Iniciado = ConfigurarDriver();
|
|
|
|
if (Iniciado) {
|
|
if (_pinoTMP.Definido()) {
|
|
_pinoTMP.Conectar();
|
|
tempKalman = new SimpleKalmanFilter(2, 2, 0.01);
|
|
}
|
|
|
|
xTaskCreatePinnedToCore(&Motor::CheckTaskWrapper, "CheckTask", 5000, this, 21 - _canal + 4, &CheckTaskHandle, tskNO_AFFINITY);
|
|
|
|
AtualizarDadosControle(true);
|
|
|
|
EnviarDadosSerial(_ID + " Iniciado");
|
|
}
|
|
|
|
EnviarDadosSerial(MontarProtocoloSensor(sCFG, _ID, Iniciado ? "1" : "0"));
|
|
}
|
|
|
|
void Desligar() {
|
|
if (!Iniciado) {
|
|
EnviarDadosSerial(_ID + " nao esta inicializado");
|
|
EnviarDadosSerial(MontarProtocoloSensor(sCFG, _ID, Iniciado ? "1" : "0"));
|
|
return;
|
|
}
|
|
|
|
// Parar a execução das tarefas
|
|
vTaskDelete(CheckTaskHandle);
|
|
|
|
// Redefinir as configurações para os valores iniciais
|
|
_pinoTMP.Desconectar();
|
|
|
|
// Outras redefinições de variáveis de estado, se necessário
|
|
delete tempKalman;
|
|
|
|
EnviarDadosSerial(_ID + " Desligado");
|
|
|
|
Iniciado = false;
|
|
|
|
EnviarDadosSerial(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++;
|
|
if (!sucesso) {
|
|
Serial.print("Tentativa ");
|
|
Serial.print(tentativas);
|
|
Serial.println(" falhou, tentando novamente...");
|
|
}
|
|
}
|
|
else {
|
|
sucesso = true;
|
|
resposta._success = sucesso;
|
|
}
|
|
|
|
}
|
|
|
|
if (!sucesso) {
|
|
Serial.println("Todas as tentativas falharam.");
|
|
}
|
|
|
|
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 CheckTaskHandle = NULL;
|
|
static void CheckTaskWrapper(void *pvParameters) {
|
|
Motor *motor = static_cast<Motor*>(pvParameters);
|
|
motor->CheckTask();
|
|
}
|
|
|
|
void CheckTask() {
|
|
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(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;
|
|
}
|
|
|
|
};
|
|
|
|
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);
|
|
|
|
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(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();
|
|
|
|
Motor* _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];
|
|
|
|
Motor* _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();
|
|
}
|
|
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));
|
|
|
|
}
|
|
|
|
|
|
|