Zendion/agrobot_base
Zendion
/
agrobot_base
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity
agrobot_base/Firmware/Movimentação/Movimentacao_v13/Movimentacao_v13.ino

604 lines
17 KiB
C++
Raw Blame History

#include "C:\ZendionInc\agrobot_base\Firmware\Modulos\SerialService.h"
#include "C:\ZendionInc\agrobot_base\Firmware\Modulos\utils.h"
#include <PID_v1.h>
#include <SimpleKalmanFilter.h>
#define D_Code Mov
#define _pinoLED RGB_BUILTIN
int _pinoReleGeral = -1;
long _baudRate = 115200;
bool Conectado = false;
int _TaxaAmostragem = 500;
const int PPR = 22; // Pulsos por revolução
int RampaMin = 0;
int ZeroRampa = 0;
int RampaMax = 1023;
// Temperatura NTC 10K
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;
class Motor {
public:
// Construtor
Motor(String desc) {
_ID = desc;
}
// Definições
String _ID;
int _canal;
int _pinoPWM;
int _pinoDIR;
int _pinoBRK;
int _pinoSTP;
int _pinoHLA;
int _pinoHLB;
int _pinoHLC;
int _pinoTMP;
double _Kp;
double _Ki;
double _Kd;
PID* _PID = nullptr;
bool Iniciado = false;
bool Testando = false;
bool Revertendo = false;
bool SentidoContrario = false;
StatusMotor Aceleracao = Estavel;
StatusMotor AceleracaoA = Estavel;
// Consumo
const int leituras = 5;
volatile float RPM_arr[5];
volatile bool Hall_arr[5][3];
double RPM;
double PotenciaAtual;
volatile int ultimaLeituraHallA = 0;
volatile unsigned int tempoAnterior = 0;
volatile float periodo = 0;
double temperatura = 0.0;
SimpleKalmanFilter* tempKalman;
// Entrada de dados
bool _MalhaFechada;
int _PotMap;
double _RPM_SP;
Sentido _SentidoSP = Parado;
Sentido _SentidoM = Parado;
Sentido _Sentido = Parado;
int _US_Addr;
bool _Freio = false;
int _Margem = 2;
void Inicializar() {
if (Iniciado) {
EnviarDadosSerial(_ID + " ja inicializado");
EnviarDadosSerial(MontarProtocoloSensor(sCFG, _ID, Iniciado ? "1" : "0"));
return;
}
if (_pinoPWM > -1) {
ledcSetup(_canal, 1000, 10);
ledcAttachPin(_pinoPWM, _canal);
PotenciaAtual = ZeroRampa;
_PotMap = PotenciaAtual;
_RPM_SP = 0;
ledcWrite(_canal, _PotMap);
}
if (_pinoDIR > -1) {
pinMode(_pinoDIR, OUTPUT);
digitalWrite(_pinoDIR, LOW);
_SentidoM = Parado;
}
if (_pinoBRK > -1) {
pinMode(_pinoBRK, OUTPUT);
digitalWrite(_pinoBRK, LOW);
}
if (_pinoSTP > -1) {
pinMode(_pinoSTP, OUTPUT);
digitalWrite(_pinoSTP, HIGH);
}
if (_pinoHLA > -1) {
pinMode(_pinoHLA, INPUT);
ultimaLeituraHallA = digitalRead(_pinoHLA);
RPM = 0;
periodo = 0;
tempoAnterior = 0;
xTaskCreatePinnedToCore(&Motor::RPMTaskWrapper, "RPMTask", 15000, this, 25 - _canal, &RPMTaskHandle, tskNO_AFFINITY);
ReiniciarAceleracaoArr();
ReiniciarHallArr();
_PID = new PID(&RPM, &PotenciaAtual, &_RPM_SP, _Kp, _Ki, _Kd, DIRECT);
_PID->SetOutputLimits(RampaMin, RampaMax);
_PID->SetMode(AUTOMATIC);
}
if (_pinoHLB > -1) {
pinMode(_pinoHLB, INPUT);
}
if (_pinoHLC > -1) {
pinMode(_pinoHLC, INPUT);
}
if (_pinoTMP > -1) {
pinMode(_pinoTMP, INPUT);
tempKalman = new SimpleKalmanFilter(2, 2, 0.01);
}
xTaskCreatePinnedToCore(&Motor::RampaTaskWrapper, "RampaTask", 5000, this, 21 - _canal + 4, &RampaTaskHandle, 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(RampaTaskHandle);
if (_pinoHLA > -1) {
vTaskDelete(RPMTaskHandle);
}
if (_pinoTMP > -1) {
delete tempKalman;
}
// Desanexar o canal PWM
if (_pinoPWM > -1) {
ledcDetachPin(_pinoPWM);
}
// Redefinir as configurações para os valores iniciais
pinMode(_pinoPWM, INPUT);
pinMode(_pinoDIR, INPUT);
pinMode(_pinoBRK, INPUT);
pinMode(_pinoSTP, INPUT);
pinMode(_pinoHLA, INPUT);
pinMode(_pinoHLB, INPUT);
pinMode(_pinoHLC, INPUT);
pinMode(_pinoTMP, INPUT);
// Outras redefinições de variáveis de estado, se necessário
delete _PID;
EnviarDadosSerial(_ID + " Desligado");
Iniciado = false;
EnviarDadosSerial(MontarProtocoloSensor(sCFG, _ID, Iniciado ? "1" : "0"));
}
void ReiniciarAceleracaoArr() {
for (int i = 0; i < leituras; i++) {
RPM_arr[i] = -1.0;
}
}
void ReiniciarHallArr() {
for (int i = 0; i < leituras; i++) {
Hall_arr[i][0] = false;
Hall_arr[i][1] = false;
Hall_arr[i][2] = false;
}
}
void ReverterSentidoGiro() {
_SentidoM = _SentidoM == Horario ? Antihorario : Horario;
}
private:
TaskHandle_t RPMTaskHandle = NULL;
static void RPMTaskWrapper(void *pvParameters) {
Motor *motor = static_cast<Motor*>(pvParameters);
motor->RPMTask();
}
void RPMTask() {
const int RpmMax = 650; // RPM Máximo aferir
const float RelacaoPPR = (60 / PPR) * 1000; // Multiplicador do cálculo de RPM (período em ms)
const float MenorPeriodo = RelacaoPPR / RpmMax; // Tempo mínimo de leitura
unsigned long firstMilis = millis();
int pulsos = 0;
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;
}
// Medir RPM apenas quando ocorrer um pulso no sensor HALL
int LeituraHa = digitalRead(_pinoHLA);
if (LeituraHa != ultimaLeituraHallA) {
ultimaLeituraHallA = LeituraHa;
pulsos++;
// Calcular o período em microssegundos
unsigned long tempoAtual = micros();
unsigned long periodoUs = tempoAtual - tempoAnterior;
periodo = periodoUs / 1000.0; // Converte de us para ms (2300 us para 2,3 ms)
// Salva o valor do RPM anterior
double RPM_A = RPM;
// Se o período entre pulsos for menor que o tempo mínimo entre pulsos em ms, significa que o sensor está em uma posição em que existe oscilação de leitura,
// pois o RPM estaria acima do máximo, logo, assumir o valor de RPM aferido anteriormente
if (periodo < MenorPeriodo) {
RPM = RPM_A;
}
// Calcular RPM se houve variação de tempo entre o pulso atual e o pulso anterior
else if (periodo != 0) {
// RPM = 60 / (Pulsos por Revolução * Período em segundos)
// A fórmula foi adaptada para otimizar processamento
RPM = RelacaoPPR / periodo;
}
// Se não houve alteração no período, então o motor não se moveu
else {
RPM = 0;
}
CalcularSentidoGiro(RPM);
// Reiniciar variáveis
tempoAnterior = tempoAtual;
}
CorrigirPotenciaMotor();
long intMillis = (millis() - firstMilis);
// A cada _TaxaAmostragem, enviar os dados para o software
if (intMillis >= _TaxaAmostragem) {
if (pulsos == 0) {
RPM = 0;
periodo = 0;
CalcularSentidoGiro(RPM);
}
pulsos = 0;
double PotAtual = fmap(PotenciaAtual, RampaMin, RampaMax, 0.0, 100.0);
EnviarDadosSerial(MontarProtocoloSensor(sRPM, _ID, MontarDadosProtocoloRPM(RPM, PotAtual, Aceleracao, _Sentido, Revertendo)));
firstMilis = millis();
}
// Aguarda metade do menor período possível entre leituras
vTaskDelay(1);
}
}
void CalcularSentidoGiro(float _RPM) {
if (_pinoHLA == -1 || _pinoHLB == -1 || _pinoHLC == -1) {
CalculaAceleracao(_RPM);
return;
}
bool LeituraHa = ultimaLeituraHallA == 1;
bool LeituraHb = digitalRead(_pinoHLB) == 1;
bool LeituraHc = digitalRead(_pinoHLC) == 1;
for (int i = 1; i < leituras; i++) {
Hall_arr[i - 1][0] = Hall_arr[i][0];
Hall_arr[i - 1][1] = Hall_arr[i][1];
Hall_arr[i - 1][2] = Hall_arr[i][2];
}
Hall_arr[leituras - 1][0] = LeituraHa;
Hall_arr[leituras - 1][1] = LeituraHb;
Hall_arr[leituras - 1][2] = LeituraHc;
// Leituras dos sensores Hall nas leituras anteriores
bool LeituraAnteriorHa = Hall_arr[leituras - 2][0];
bool LeituraAnteriorHb = Hall_arr[leituras - 2][1];
bool LeituraAnteriorHc = Hall_arr[leituras - 2][2];
// Comparação das leituras atuais com as leituras anteriores
if (LeituraHa == LeituraAnteriorHc && LeituraHc == LeituraAnteriorHa) {
_Sentido = Antihorario;
}
else if (LeituraHa == LeituraAnteriorHb && LeituraHb == LeituraAnteriorHa) {
_Sentido = Horario;
}
else {
// Outros casos (não determinados)
}
CalculaAceleracao(_RPM);
}
void CalculaAceleracao(float _RPM) {
float RPM_total = _RPM;
int Desconsiderar = 0;
// Salvar as ultimas leituras
for (int i = 1; i < leituras; i++) {
RPM_arr[i - 1] = RPM_arr[i];
RPM_total += RPM_arr[i - 1] < 0 ? 0 : RPM_arr[i - 1];
Desconsiderar += RPM_arr[i - 1] < 0 ? 1 : 0;
}
RPM_arr[leituras - 1] = _RPM;
float RPM_medio = RPM_total / (leituras - Desconsiderar);
AceleracaoA = Aceleracao;
if (_SentidoSP == Parado && _RPM == 0) {
Aceleracao = Estavel;
} else if (RPM_arr[leituras - 3] < (RPM_arr[leituras - 2] - _Margem) && RPM_arr[leituras - 2] < (_RPM - _Margem)) {
Aceleracao = Acelerando;
} else if (RPM_arr[leituras - 3] > (RPM_arr[leituras - 2] + _Margem) && RPM_arr[leituras - 2] > (_RPM + _Margem)) {
Aceleracao = Desacelerando;
} else {
Aceleracao = Estavel;
}
}
void CorrigirPotenciaMotor() {
if (_MalhaFechada && !Revertendo && _SentidoSP != Parado) {
if (_pinoHLA > -1 && _pinoHLB > -1 && _pinoHLC > -1) {
SentidoContrario = _Sentido != _SentidoSP;
}
float _rpmA = RPM;
if (SentidoContrario) {
RPM = RPM * -1;
}
/*else if (Aceleracao == Desacelerando && AceleracaoA == Estavel) {
RPM = 0;
}*/
_PID->Compute();
//RPM = _rpmA;
}
}
TaskHandle_t RampaTaskHandle = NULL;
static void RampaTaskWrapper(void *pvParameters) {
Motor *motor = static_cast<Motor*>(pvParameters);
motor->RampaTask();
}
void RampaTask() {
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;
}
Atualizar();
long intMillis = (millis() - firstMilis);
if (intMillis >= _TaxaAmostragem) {
AferirTemperatura();
firstMilis = millis();
}
vTaskDelay(1);
}
}
void Atualizar() {
bool Travado = _PotMap > ZeroRampa && RPM_arr[leituras - 1] == 0 && RPM_arr[leituras - 2] > 0 && RPM_arr[leituras - 3] > 0 && _Sentido != Parado;
if (_pinoPWM > -1 && Travado) {
_PotMap = ZeroRampa;
ledcWrite(_canal, _PotMap);
vTaskDelay(400);
_PotMap = PotenciaAtual;
}
if (_pinoDIR > -1) {
Revertendo = _SentidoSP != Parado && _SentidoSP != _SentidoM;
if (Revertendo) {
ledcWrite(_canal, ZeroRampa);
vTaskDelay(400);
_SentidoM = _SentidoSP;
Revertendo = false;
}
digitalWrite(_pinoDIR, _SentidoSP == Horario);
}
if (_pinoBRK > -1) {
digitalWrite(_pinoBRK, _Freio);
}
if (_pinoSTP > -1) {
//digitalWrite(_pinoSTP, HIGH);
}
if (_pinoPWM > -1) {
if (_SentidoSP == Parado) {
_PotMap = ZeroRampa;
}
else {
_PotMap = PotenciaAtual;
}
ledcWrite(_canal, _PotMap);
}
}
// TEMPERATURA E UMIDADE
void AferirTemperatura() {
if (_pinoTMP > -1) {
double Vout, Rt = 0;
double T, Tc, Tf, adc = 0;
adc = analogRead(_pinoTMP);// 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;
Rt = R1 * Vout / (Vs - Vout);
T = 1/(1/To + log(Rt/Ro)/Beta);
Tc = T - 273.15;
Tf = Tc * 9 / 5 + 32;
EnviarDadosSerial(MontarProtocoloSensor(sNTC, _ID, MontarDadosProtocoloNTC10K(Tc)));
}
}
};
Motor M1("ET");
Motor M2("EF");
Motor M3("DT");
Motor M4("DF");
void setup() {
Serial.begin(_baudRate);
delay(10);
if (_pinoReleGeral > -1) {
pinMode(_pinoReleGeral, OUTPUT);
digitalWrite(_pinoReleGeral, LOW);
}
}
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]);
bool Conectar = (String)Protocolo[3] == "1";
if (ID == "MD") {
_TaxaAmostragem = ((String)Protocolo[5] + (String)Protocolo[6] + (String)Protocolo[7] + (String)Protocolo[8] + (String)Protocolo[9]).toInt();
int pinoReleGeral = ((String)Protocolo[11] + (String)Protocolo[12]).toInt();
int _RampaMin = ((String)Protocolo[14] + (String)Protocolo[15] + (String)Protocolo[16] + (String)Protocolo[17]).toInt();
int _RampaMax = ((String)Protocolo[19] + (String)Protocolo[20] + (String)Protocolo[21] + (String)Protocolo[22]).toInt();
int _PPR = ((String)Protocolo[24] + (String)Protocolo[25] + (String)Protocolo[26]).toInt();
RampaMin = _RampaMin;
RampaMax = _RampaMax;
//PPR = _PPR;
if (pinoReleGeral > -1) {
pinMode(_pinoReleGeral, INPUT);
_pinoReleGeral = pinoReleGeral;
pinMode(_pinoReleGeral, OUTPUT);
digitalWrite(_pinoReleGeral, Conectar);
}
vTaskDelay(pdMS_TO_TICKS(100));
Conectado = Conectar;
EnviarDadosSerial(MontarProtocoloSensor(sCFG, "MD", Conectado ? "1" : "0"));
}
else {
int canal = ((String)Protocolo[5]).toInt();
bool _foc = (String)Protocolo[7] == "1";
bool motor_ativado = (String)Protocolo[9] == "1";
double _Kp = ((String)Protocolo[11] + (String)Protocolo[12] + (String)Protocolo[13] + (String)Protocolo[14] + (String)Protocolo[15]).toDouble();
double _Ki = ((String)Protocolo[17] + (String)Protocolo[18] + (String)Protocolo[19] + (String)Protocolo[20] + (String)Protocolo[21]).toDouble();
double _Kd = ((String)Protocolo[23] + (String)Protocolo[24] + (String)Protocolo[25] + (String)Protocolo[26] + (String)Protocolo[27]).toDouble();
int pwm = ((String)Protocolo[29] + (String)Protocolo[30]).toInt();
int dir = ((String)Protocolo[32] + (String)Protocolo[33]).toInt();
int brk = ((String)Protocolo[35] + (String)Protocolo[36]).toInt();
int stp = ((String)Protocolo[38] + (String)Protocolo[39]).toInt();
int hallA = ((String)Protocolo[41] + (String)Protocolo[42]).toInt();
int hallB = ((String)Protocolo[44] + (String)Protocolo[45]).toInt();
int hallC = ((String)Protocolo[47] + (String)Protocolo[48]).toInt();
int tmp = ((String)Protocolo[50] + (String)Protocolo[51]).toInt();
Motor* _motor = ID == "ET" ? &M1 : ID == "EF" ? &M2 : ID == "DT" ? &M3 : &M4;
if (motor_ativado) {
if (Conectar) {
_motor->_canal = canal;
_motor->_pinoPWM = pwm;
_motor->_pinoDIR = dir;
_motor->_pinoBRK = brk;
_motor->_pinoSTP = stp;
_motor->_pinoHLA = hallA;
_motor->_pinoHLB = hallB;
_motor->_pinoHLC = hallC;
_motor->_pinoTMP = tmp;
_motor->_MalhaFechada = _foc;
_motor->_Kp = _Kp;
_motor->_Ki = _Ki;
_motor->_Kd = _Kd;
_motor->Inicializar();
}
else {
_motor->Desligar();
}
vTaskDelay(pdMS_TO_TICKS(500));
}
}
}
else if (_funcao == Cmd) {
//canal;potencia;sentido;rampa;precisao;rpm
//0;000;0;000;000;000
String ID = ((String)Protocolo[0] + (String)Protocolo[1]);
Motor* _motor = ID == "ET" ? &M1 : ID == "EF" ? &M2 : ID == "DT" ? &M3 : &M4;
_motor->_SentidoSP = (Sentido)((String)Protocolo[3]).toInt();
_motor->_RPM_SP = ((String)Protocolo[5] + (String)Protocolo[6] + (String)Protocolo[7]).toInt();
_motor->_MalhaFechada = (String)Protocolo[9] == "1";
_motor->ReiniciarAceleracaoArr();
bool reverter = ((String)Protocolo[11]) == "1";
if (reverter) {
_motor->ReverterSentidoGiro();
}
}
else if (_funcao == Tst) {
String ID = ((String)Protocolo[0] +(String)Protocolo[1]);
Motor* _motor = ID == "ET" ? &M1 : ID == "EF" ? &M2 : ID == "DT" ? &M3 : &M4;
bool EmTeste = _motor->Testando;
if (!EmTeste) {
//_motor->Testar();
}
}
}
}
Reference in New Issue View Git Blame Copy Permalink