agrobot_base/Firmware/MovUnificado/MovUnificado_v6/MovUnificado_v6.ino

477 lines
13 KiB
Arduino
Raw Permalink Normal View History

2024-07-05 18:59:05 +00:00
// Dispositivo: Movimentação e Direcional Unificados
// Versão Firmware: 6
// Ultima atualização: 01/07/2024
// Atualização: Removido dados do motor de passo
#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 6
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);
}
}
};
MotorBLDC MotorMOV;
void AtualizarPWM() {
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);
//Serial.begin(_baudRate);
analogReadResolution(10);
delay(10);
ConfigurarModbus(true, _pinModbusRX, _pinModbusTX, _pinModbusMD);
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;
if (!RequisitarDados && Conectado) {
ProcessarReq();
}
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();
break;
}
if (Mensagem.idMensagem > 0) {
EnviarDadosSerial(Mod_ID, MontarProtocoloMensagemRecebida(Mensagem.idMensagem));
}
}
void ProcessarChk(ProtocoloSerial Mensagem) {
Conectado = MotorMOV.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();
}
}
}
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();
}
}
void ProcessarReq() {
//std::vector<String> Partes = SplitString(Mensagem.protocolo, SplitParams);
//Serial.println("Processando req...");
if (MotorMOV.Iniciado) {
MotorMOV.RequisitarDados();
}
if (MotorMOV.Iniciado) {
//Serial.println("Enviando dados...");
EnviarDadosSerial(Mod_ID, MontarProtocoloSensor(sBLD, Mod_ID, MontarDadosProtocoloBLD(
MotorMOV.Iniciado,
MotorMOV.RPM,
MotorMOV.Temperatura,
MotorMOV.Tensao,
MotorMOV.CorrenteMax,
MotorMOV.Ligado,
MotorMOV._Sentido,
MotorMOV.Freio,
MotorMOV.CodAlarme,
MotorMOV.TemperaturaDriver
)));
}
}