509 lines
14 KiB
C++
509 lines
14 KiB
C++
// Dispositivo: Sensoriamento
|
|
// Versão Firmware: 3
|
|
// Ultima atualização: 30/01/2024
|
|
|
|
#include "C:\ZendionInc\agrobot_base\Firmware\Modulos\SerialService.h"
|
|
|
|
#include <SimpleKalmanFilter.h>
|
|
|
|
#define D_Code Sen
|
|
|
|
#define _pinoLED RGB_BUILTIN
|
|
|
|
long _baudRate = 115200;
|
|
|
|
bool Conectado = false;
|
|
|
|
int _TaxaAmostragem;
|
|
int FrequenciaMaxima = 10000;
|
|
|
|
|
|
class SensorTemperatura {
|
|
public:
|
|
SensorTemperatura(String desc) {
|
|
_ID = desc;
|
|
}
|
|
|
|
// Definições
|
|
String _ID;
|
|
S_Code _componente = sTMP;
|
|
int _pino;
|
|
int _delayAmostragem;
|
|
int _prioridade;
|
|
float Voffset = 0.0;
|
|
|
|
bool Iniciado = false;
|
|
|
|
SimpleKalmanFilter* tempKalman;
|
|
|
|
void Inicializar() {
|
|
if (Iniciado) {
|
|
EnviarDadosSerial(_ID + " ja inicializado");
|
|
EnviarDadosSerial(MontarProtocoloSensor(sCFG, _ID, Iniciado ? "1" : "0"));
|
|
return;
|
|
}
|
|
|
|
if (_pino > -1) {
|
|
pinMode(_pino, INPUT);
|
|
}
|
|
|
|
xTaskCreatePinnedToCore(&SensorTemperatura::STTaskWrapper, "STTask", 5000, this, _prioridade, &STTaskHandle, tskNO_AFFINITY);
|
|
|
|
EnviarDadosSerial(_ID + " Iniciado");
|
|
|
|
Iniciado = true;
|
|
|
|
tempKalman = new SimpleKalmanFilter(2, 2, 0.01);
|
|
|
|
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(STTaskHandle);
|
|
|
|
delete tempKalman;
|
|
|
|
// Redefinir as configurações para os valores iniciais
|
|
pinMode(_pino, INPUT);
|
|
|
|
EnviarDadosSerial(_ID + " Desligado");
|
|
|
|
Iniciado = false;
|
|
|
|
EnviarDadosSerial(MontarProtocoloSensor(sCFG, _ID, Iniciado ? "1" : "0"));
|
|
}
|
|
|
|
private:
|
|
|
|
// Variáveis
|
|
double Vs = 3.3; // TENSÃO DE SAIDA DO ESP32
|
|
double R1 = 10000; //RESISTOR UTILIZADO NO DIVISOR DE TENSÃO
|
|
double Beta = 3950; // VALOR DE BETA
|
|
double To = 298.15; // VALOR EM KELVIN REFERENTE A 25° CELSIUS
|
|
double Ro = 10000;
|
|
double adcMax = 4095.0;
|
|
|
|
TaskHandle_t STTaskHandle = NULL;
|
|
static void STTaskWrapper(void *pvParameters) {
|
|
SensorTemperatura *sensor = static_cast<SensorTemperatura*>(pvParameters);
|
|
sensor->STTask();
|
|
}
|
|
|
|
void STTask() {
|
|
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 + _delayAmostragem)) {
|
|
AferirTemperatura();
|
|
|
|
firstMilis = millis();
|
|
}
|
|
|
|
// Aguarda metade do menor período possível entre leituras
|
|
vTaskDelay(1);
|
|
}
|
|
}
|
|
|
|
void AferirTemperatura() {
|
|
double Vout, Rt = 0;
|
|
double T, Tc, Tf, adc = 0;
|
|
adc = analogRead(_pino); // VARIÁVEL QUE RECEBE A LEITURA DO NTC10K
|
|
float adcValue = tempKalman->updateEstimate(adc);
|
|
//CALCULOS PARA CONVERSÃO DA LEITURA RECEBIDA PELO ESP32 EM TEMPERATURA EM °C
|
|
Vout = (adcValue * Vs / adcMax) + Voffset;
|
|
Rt = R1 * Vout / (Vs - Vout);
|
|
T = 1 / (1 / To + log(Rt / Ro) / Beta);
|
|
Tc = T - 273.15;
|
|
Tf = Tc * 9 / 5 + 32;
|
|
|
|
EnviarDadosSerial(MontarProtocoloSensor(sTMP, _ID, MontarDadosProtocoloNTC10K(Tc)));
|
|
}
|
|
};
|
|
|
|
class SensorTensao {
|
|
public:
|
|
SensorTensao(String desc) {
|
|
_ID = desc;
|
|
}
|
|
|
|
// Variáveis
|
|
const float Vcc = 3.3; // Tensão de alimentação
|
|
float Voffset = 0.85;
|
|
int R1 = 47000;
|
|
int R2 = 10000;
|
|
|
|
// Definições
|
|
String _ID;
|
|
S_Code _componente = sTEN;
|
|
int _pino;
|
|
int _delayAmostragem;
|
|
int _prioridade;
|
|
|
|
SimpleKalmanFilter* kalmanTEN;
|
|
|
|
bool Iniciado = false;
|
|
|
|
void Inicializar() {
|
|
if (Iniciado) {
|
|
EnviarDadosSerial(_ID + " ja inicializado");
|
|
EnviarDadosSerial(MontarProtocoloSensor(sCFG, _ID, Iniciado ? "1" : "0"));
|
|
return;
|
|
}
|
|
|
|
if (_pino > -1) {
|
|
pinMode(_pino, INPUT);
|
|
}
|
|
|
|
kalmanTEN = new SimpleKalmanFilter(2, 2, 0.01);
|
|
|
|
xTaskCreatePinnedToCore(&SensorTensao::SVTaskWrapper, "SVTask", 5000, this, _prioridade, &SVTaskHandle, tskNO_AFFINITY);
|
|
|
|
EnviarDadosSerial(_ID + " Iniciado");
|
|
|
|
Iniciado = true;
|
|
|
|
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(SVTaskHandle);
|
|
|
|
// Redefinir as configurações para os valores iniciais
|
|
pinMode(_pino, INPUT);
|
|
|
|
delete kalmanTEN;
|
|
|
|
EnviarDadosSerial(_ID + " Desligado");
|
|
|
|
Iniciado = false;
|
|
|
|
EnviarDadosSerial(MontarProtocoloSensor(sCFG, _ID, Iniciado ? "1" : "0"));
|
|
}
|
|
|
|
private:
|
|
TaskHandle_t SVTaskHandle = NULL;
|
|
static void SVTaskWrapper(void *pvParameters) {
|
|
SensorTensao *sensor = static_cast<SensorTensao*>(pvParameters);
|
|
sensor->SVTask();
|
|
}
|
|
|
|
void SVTask() {
|
|
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 + _delayAmostragem)) {
|
|
AferirTensao();
|
|
|
|
firstMilis = millis();
|
|
}
|
|
|
|
// Aguarda metade do menor período possível entre leituras
|
|
vTaskDelay(1);
|
|
}
|
|
}
|
|
|
|
void AferirTensao() {
|
|
float Razao = (R1 + R2) / (float)R2;
|
|
int adcV = analogRead(_pino);
|
|
float adcValueV = adcV; //float adcValueV = kalmanTEN->updateEstimate(adcV);
|
|
|
|
float voltageV = (adcValueV * (Vcc / 4095.0)) + (Voffset / Razao); // Convertendo o valor ADC para tensão
|
|
float realVoltage = voltageV * Razao;
|
|
|
|
EnviarDadosSerial(MontarProtocoloSensor(sTEN, _ID, MontarDadosProtocoloTensao(adcValueV, voltageV, realVoltage)));
|
|
}
|
|
|
|
};
|
|
|
|
class SensorCorrente {
|
|
public:
|
|
SensorCorrente(String desc) {
|
|
_ID = desc;
|
|
}
|
|
|
|
// Variáveis
|
|
const float Vcc = 3.3; // Tensão de alimentação
|
|
float Voffset = 0.413;
|
|
float sensibilidade = 0.35;
|
|
|
|
// Definições
|
|
String _ID;
|
|
S_Code _componente = sCOR;
|
|
int _pino;
|
|
int _delayAmostragem;
|
|
int _prioridade;
|
|
|
|
SimpleKalmanFilter* kalmanCOR;
|
|
|
|
bool Iniciado = false;
|
|
|
|
void Inicializar() {
|
|
if (Iniciado) {
|
|
EnviarDadosSerial(_ID + " ja inicializado");
|
|
EnviarDadosSerial(MontarProtocoloSensor(sCFG, _ID, Iniciado ? "1" : "0"));
|
|
return;
|
|
}
|
|
|
|
if (_pino > -1) {
|
|
pinMode(_pino, INPUT);
|
|
}
|
|
|
|
kalmanCOR = new SimpleKalmanFilter(2, 2, 0.01);
|
|
|
|
xTaskCreatePinnedToCore(&SensorCorrente::SATaskWrapper, "SATask", 5000, this, _prioridade, &SATaskHandle, tskNO_AFFINITY);
|
|
|
|
EnviarDadosSerial(_ID + " Iniciado");
|
|
|
|
Iniciado = true;
|
|
|
|
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(SATaskHandle);
|
|
|
|
// Redefinir as configurações para os valores iniciais
|
|
pinMode(_pino, INPUT);
|
|
|
|
delete kalmanCOR;
|
|
|
|
EnviarDadosSerial(_ID + " Desligado");
|
|
|
|
Iniciado = false;
|
|
|
|
EnviarDadosSerial(MontarProtocoloSensor(sCFG, _ID, Iniciado ? "1" : "0"));
|
|
}
|
|
|
|
private:
|
|
TaskHandle_t SATaskHandle = NULL;
|
|
static void SATaskWrapper(void *pvParameters) {
|
|
SensorCorrente *sensor = static_cast<SensorCorrente*>(pvParameters);
|
|
sensor->SATask();
|
|
}
|
|
|
|
void SATask() {
|
|
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 + _delayAmostragem)) {
|
|
AferirCorrente();
|
|
|
|
firstMilis = millis();
|
|
}
|
|
|
|
// Aguarda metade do menor período possível entre leituras
|
|
vTaskDelay(1);
|
|
}
|
|
}
|
|
|
|
void AferirCorrente() {
|
|
int adcV = analogRead(_pino);
|
|
float adcValue = adcV; //float adcValue = kalmanCOR->updateEstimate(adcV);
|
|
|
|
float voltage = adcValue * (Vcc / 4095.0); // Convertendo o valor ADC para tensão
|
|
float current = (voltage - Voffset) / sensibilidade; // Calculando a corrente em A
|
|
|
|
EnviarDadosSerial(MontarProtocoloSensor(sCOR, _ID, MontarDadosProtocoloCorrente(adcValue, voltage, current)));
|
|
}
|
|
|
|
};
|
|
|
|
SensorTemperatura TM1("TMVET");
|
|
SensorTemperatura TM2("TMVEF");
|
|
SensorTemperatura TM3("TMVDT");
|
|
SensorTemperatura TM4("TMVDF");
|
|
SensorTemperatura TBAT("TBAT ");
|
|
SensorTensao VBAT("VBAT ");
|
|
SensorCorrente ABAT("ABAT ");
|
|
|
|
|
|
SensorTemperatura* STPorID(String ID) {
|
|
SensorTemperatura* _sensor =
|
|
ID == "TMVET" ? &TM1 :
|
|
ID == "TMVEF" ? &TM2 :
|
|
ID == "TMVDT" ? &TM3 :
|
|
ID == "TMVDF" ? &TM4 :
|
|
ID == "TBAT " ? &TBAT :
|
|
nullptr;
|
|
return _sensor;
|
|
}
|
|
|
|
SensorTensao* SVPorID(String ID) {
|
|
SensorTensao* _sensor =
|
|
ID == "VBAT " ? &VBAT :
|
|
nullptr;
|
|
return _sensor;
|
|
}
|
|
|
|
SensorCorrente* SAPorID(String ID) {
|
|
SensorCorrente* _sensor =
|
|
ID == "ABAT " ? &ABAT :
|
|
nullptr;
|
|
return _sensor;
|
|
}
|
|
|
|
void setup() {
|
|
Serial.begin(_baudRate);
|
|
}
|
|
|
|
void loop() {
|
|
if (Serial.available() > 3) {
|
|
String Protocolo = "";
|
|
F_Code _funcao = Nda;
|
|
while (Serial.available()) {
|
|
char Entrada = (char)Serial.read();
|
|
if (Entrada == EndLine) {
|
|
break;
|
|
}
|
|
Protocolo += Entrada;
|
|
if (Protocolo.length() == 3) {
|
|
if (_funcao == Nda) {
|
|
_funcao = (F_Code)((String)Protocolo[0] + (String)Protocolo[1] + (String)Protocolo[2]).toInt();
|
|
Protocolo = "";
|
|
}
|
|
}
|
|
}
|
|
|
|
EnviarDadosSerial("OK");
|
|
|
|
if (_funcao == Chk) {
|
|
EnviarDadosSerial(MontarProtocoloVerificacao(D_Code));
|
|
} else if (_funcao == Cfg) {
|
|
String ID = ((String)Protocolo[0] + (String)Protocolo[1] + (String)Protocolo[2] + (String)Protocolo[3] + (String)Protocolo[4]);
|
|
bool Conectar = (String)Protocolo[6] == "1";
|
|
if (ID == "MD ") {
|
|
_TaxaAmostragem = ((String)Protocolo[8] + (String)Protocolo[9] + (String)Protocolo[10] + (String)Protocolo[11] + (String)Protocolo[12]).toInt();
|
|
|
|
vTaskDelay(pdMS_TO_TICKS(100));
|
|
Conectado = Conectar;
|
|
EnviarDadosSerial(MontarProtocoloSensor(sCFG, "MD", Conectado ? "1" : "0"));
|
|
}
|
|
else {
|
|
S_Code componente = (S_Code)((String)Protocolo[8] + (String)Protocolo[9] + (String)Protocolo[10]).toInt();
|
|
bool sensor_ativado = (String)Protocolo[12] == "1";
|
|
int delayAmostragem = ((String)Protocolo[14] + (String)Protocolo[15] + (String)Protocolo[16] + (String)Protocolo[17] + (String)Protocolo[18]).toInt();
|
|
int prioridade = ((String)Protocolo[20] + (String)Protocolo[21]).toInt();
|
|
int pino = ((String)Protocolo[23] + (String)Protocolo[24]).toInt();
|
|
|
|
switch (componente) {
|
|
case sTMP: {
|
|
SensorTemperatura* _sensor = STPorID(ID);
|
|
if (sensor_ativado) {
|
|
double _voffset = ((String)Protocolo[26] + (String)Protocolo[27] + (String)Protocolo[28] + (String)Protocolo[29] + (String)Protocolo[30] + (String)Protocolo[31]).toDouble();
|
|
_sensor->Voffset = (float)_voffset;
|
|
if (Conectar) {
|
|
_sensor->_pino = pino;
|
|
_sensor->_prioridade = prioridade;
|
|
_sensor->_delayAmostragem = delayAmostragem;
|
|
|
|
_sensor->Inicializar();
|
|
}
|
|
else {
|
|
_sensor->Desligar();
|
|
}
|
|
}
|
|
break;
|
|
}
|
|
case sTEN: {
|
|
SensorTensao* _sensor = SVPorID(ID);
|
|
if (sensor_ativado) {
|
|
double _voffset = ((String)Protocolo[26] + (String)Protocolo[27] + (String)Protocolo[28] + (String)Protocolo[29] + (String)Protocolo[30] + (String)Protocolo[31]).toDouble();
|
|
double _r1 = ((String)Protocolo[33] + (String)Protocolo[34] + (String)Protocolo[35] + (String)Protocolo[36] + (String)Protocolo[37] + (String)Protocolo[38]).toDouble();
|
|
double _r2 = ((String)Protocolo[40] + (String)Protocolo[41] + (String)Protocolo[42] + (String)Protocolo[43] + (String)Protocolo[44] + (String)Protocolo[45]).toDouble();
|
|
_sensor->Voffset = (float)_voffset;
|
|
_sensor->R1 = (int)_r1;
|
|
_sensor->R2 = (int)_r2;
|
|
if (Conectar) {
|
|
_sensor->_pino = pino;
|
|
_sensor->_prioridade = prioridade;
|
|
_sensor->_delayAmostragem = delayAmostragem;
|
|
|
|
_sensor->Inicializar();
|
|
}
|
|
else {
|
|
_sensor->Desligar();
|
|
}
|
|
}
|
|
break;
|
|
}
|
|
case sCOR: {
|
|
SensorCorrente* _sensor = SAPorID(ID);
|
|
if (sensor_ativado) {
|
|
double _voffset = ((String)Protocolo[26] + (String)Protocolo[27] + (String)Protocolo[28] + (String)Protocolo[29] + (String)Protocolo[30] + (String)Protocolo[31]).toDouble();
|
|
double _sensibilidade = ((String)Protocolo[33] + (String)Protocolo[34] + (String)Protocolo[35] + (String)Protocolo[36] + (String)Protocolo[37] + (String)Protocolo[38]).toDouble();
|
|
_sensor->Voffset = (float)_voffset;
|
|
_sensor->sensibilidade = (float)_sensibilidade;
|
|
if (Conectar) {
|
|
_sensor->_pino = pino;
|
|
_sensor->_prioridade = prioridade;
|
|
_sensor->_delayAmostragem = delayAmostragem;
|
|
|
|
_sensor->Inicializar();
|
|
}
|
|
else {
|
|
_sensor->Desligar();
|
|
}
|
|
}
|
|
break;
|
|
}
|
|
|
|
}
|
|
|
|
|
|
}
|
|
|
|
} else if (_funcao == Cmd) {
|
|
|
|
} else if (_funcao == Tst) {
|
|
}
|
|
}
|
|
}
|