498 lines
15 KiB
C++
498 lines
15 KiB
C++
#include "C:\Zendion Inc\agrobot_base\Firmware\Modulos\SerialService.h"
|
|
#include "C:\Zendion Inc\agrobot_base\Firmware\Modulos\MemoryService.h"
|
|
#include "C:\Zendion Inc\agrobot_base\Firmware\Modulos\utils.h"
|
|
#include <PID_v1.h>
|
|
#define D_Code Mov
|
|
|
|
long _baudRate = 115200;
|
|
|
|
bool Conectado = false;
|
|
|
|
int _TaxaAmostragem = 500;
|
|
const int PPR = 22; // Pulsos por revolução
|
|
bool M1_Ativado = false;
|
|
bool M2_Ativado = false;
|
|
bool M3_Ativado = false;
|
|
bool M4_Ativado = false;
|
|
|
|
int RampaMin = 0;
|
|
int ZeroRampa = 0;
|
|
int RampaMax = 1023;
|
|
|
|
class Motor {
|
|
public:
|
|
// Construtor
|
|
Motor(String desc, int SentidoAddr) {
|
|
_ID = desc;
|
|
_US_Addr = SentidoAddr;
|
|
}
|
|
|
|
// Definições
|
|
String _ID;
|
|
int _canal;
|
|
int _pinoPWM;
|
|
int _pinoVEL;
|
|
int _pinoDIR;
|
|
int _pinoBRK;
|
|
int _pinoSTP;
|
|
|
|
double _Kp;
|
|
double _Ki;
|
|
double _Kd;
|
|
PID* _PID = nullptr;
|
|
|
|
bool Iniciado = false;
|
|
bool Testando = false;
|
|
bool Revertendo = false;
|
|
StatusMotor Aceleracao = Estavel;
|
|
|
|
// Consumo
|
|
const int leituras = 5;
|
|
volatile float RPM_arr[5];
|
|
double RPM;
|
|
double PotenciaAtual;
|
|
volatile int ultimaLeituraHall = 0;
|
|
volatile unsigned int tempoAnterior = 0;
|
|
volatile float periodo = 0;
|
|
|
|
// Entrada de dados
|
|
bool _FOC;
|
|
int _Rampa;
|
|
int _Precisao;
|
|
int _FatorDivisor = 1;
|
|
double _Potencia_SP;
|
|
int _PotMap;
|
|
double _RPM_SP;
|
|
Sentido _Sentido = Parado;
|
|
Sentido _SentidoA = Parado;
|
|
Sentido _SentidoM = Parado;
|
|
int _US_Addr;
|
|
bool _Freio = false;
|
|
bool _RampaAtivada = 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);
|
|
ledcWrite(_canal, ZeroRampa);
|
|
}
|
|
if (_pinoVEL > -1) {
|
|
pinMode(_pinoVEL, INPUT);
|
|
ultimaLeituraHall = digitalRead(_pinoVEL);
|
|
RPM = 0;
|
|
periodo = 0;
|
|
tempoAnterior = 0;
|
|
|
|
xTaskCreatePinnedToCore(&Motor::RPMTaskWrapper, "RPMTask", 5000, this, _canal, &RPMTaskHandle, 0);
|
|
|
|
ReiniciarAceleracaoArr();
|
|
|
|
_PID = new PID(&RPM, &PotenciaAtual, &_RPM_SP, _Kp, _Ki, _Kd, DIRECT);
|
|
_PID->SetOutputLimits(RampaMin, RampaMax);
|
|
_PID->SetMode(AUTOMATIC);
|
|
}
|
|
if (_pinoDIR > -1) {
|
|
pinMode(_pinoDIR, OUTPUT);
|
|
digitalWrite(_pinoDIR, LOW);
|
|
|
|
_SentidoM = (Sentido)LerMemoria(_US_Addr);
|
|
}
|
|
if (_pinoBRK > -1) {
|
|
pinMode(_pinoBRK, OUTPUT);
|
|
digitalWrite(_pinoBRK, LOW);
|
|
}
|
|
if (_pinoSTP > -1) {
|
|
pinMode(_pinoSTP, OUTPUT);
|
|
digitalWrite(_pinoSTP, HIGH);
|
|
}
|
|
|
|
xTaskCreatePinnedToCore(&Motor::RampaTaskWrapper, "RampaTask", 5000, this, 18 - _canal, &RampaTaskHandle, 0);
|
|
|
|
EnviarDadosSerial(_ID + " Iniciado");
|
|
|
|
Iniciado = true;
|
|
|
|
EnviarDadosSerial(MontarProtocoloSensor(sCFG, _ID, Iniciado ? "1" : "0"));
|
|
}
|
|
|
|
void Desligar() {
|
|
if (!Iniciado) {
|
|
EnviarDadosSerial(_ID + " nao esta inicializado");
|
|
return;
|
|
}
|
|
|
|
// Parar a execução das tarefas
|
|
vTaskDelete(RPMTaskHandle);
|
|
vTaskDelete(RampaTaskHandle);
|
|
|
|
// Desanexar o canal PWM
|
|
ledcDetachPin(_pinoPWM);
|
|
|
|
// Redefinir as configurações para os valores iniciais
|
|
pinMode(_pinoPWM, INPUT);
|
|
pinMode(_pinoVEL, INPUT);
|
|
pinMode(_pinoDIR, INPUT);
|
|
pinMode(_pinoBRK, INPUT);
|
|
pinMode(_pinoSTP, INPUT);
|
|
|
|
// Outras redefinições de variáveis de estado, se necessário
|
|
RPM = 0;
|
|
periodo = 0;
|
|
tempoAnterior = 0;
|
|
|
|
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;
|
|
}
|
|
}
|
|
|
|
private:
|
|
|
|
void Atualizar() {
|
|
if (_pinoDIR > -1) {
|
|
if (_Sentido != Parado && _Sentido != _SentidoM) {
|
|
Revertendo = true;
|
|
|
|
digitalWrite(_pinoDIR, HIGH);
|
|
vTaskDelay(1500);
|
|
digitalWrite(_pinoDIR, LOW);
|
|
|
|
_SentidoM = _Sentido;
|
|
GravarMemoria(_US_Addr, (int)_SentidoM);
|
|
|
|
Revertendo = false;
|
|
}
|
|
}
|
|
if (_pinoBRK > -1) {
|
|
digitalWrite(_pinoBRK, _Freio);
|
|
}
|
|
if (_pinoSTP > -1) {
|
|
//digitalWrite(_pinoSTP, HIGH);
|
|
}
|
|
if (_pinoPWM > -1) {
|
|
if (_Sentido == Parado) {
|
|
_PotMap = ZeroRampa;
|
|
}
|
|
else {
|
|
_PotMap = PotenciaAtual;
|
|
}
|
|
|
|
float RPM_total = RPM;
|
|
for (int i = 1; i < leituras; i++) {
|
|
RPM_total += RPM_arr[i - 1];
|
|
}
|
|
|
|
float RPM_medio = RPM_total / leituras;
|
|
|
|
if (RPM == 0 && _PotMap > ZeroRampa && RPM_medio > 0) {
|
|
_PotMap = ZeroRampa;
|
|
ledcWrite(_canal, _PotMap);
|
|
vTaskDelay(300);
|
|
_PotMap = PotenciaAtual;
|
|
}
|
|
|
|
ledcWrite(_canal, _PotMap);
|
|
}
|
|
}
|
|
|
|
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 Leitura = digitalRead(_pinoVEL);
|
|
if (Leitura != ultimaLeituraHall) {
|
|
ultimaLeituraHall = Leitura;
|
|
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;
|
|
}
|
|
|
|
CalculaAceleracao(RPM);
|
|
|
|
// Reiniciar variáveis
|
|
tempoAnterior = tempoAtual;
|
|
}
|
|
|
|
long intMillis = (millis() - firstMilis);
|
|
|
|
// A cada _TaxaAmostragem, enviar os dados para o software
|
|
if (intMillis >= _TaxaAmostragem) {
|
|
if (pulsos == 0) {
|
|
RPM = 0;
|
|
periodo = 0;
|
|
CalculaAceleracao(RPM);
|
|
}
|
|
pulsos = 0;
|
|
|
|
double PotAtual = fmap(PotenciaAtual, RampaMin, RampaMax, 0.0, 100.0);
|
|
EnviarDadosSerial(MontarProtocoloSensor(sRPM, _ID, MontarDadosProtocoloRPM(RPM, PotAtual, _Potencia_SP, periodo, Aceleracao)));
|
|
firstMilis = millis();
|
|
}
|
|
|
|
// Aguarda metade do menor período possível entre leituras
|
|
vTaskDelay(1);
|
|
}
|
|
}
|
|
|
|
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);
|
|
float RPM_anterior = RPM_arr[leituras - 2];
|
|
int mg = 5;
|
|
|
|
if (_SentidoA == Parado && _RPM == 0) {
|
|
Aceleracao = Estavel;
|
|
} else if ((_RPM + mg) < RPM_anterior) {
|
|
Aceleracao = Desacelerando;
|
|
} else if ((_RPM - mg) > RPM_anterior) {
|
|
Aceleracao = Acelerando;
|
|
} else {
|
|
Aceleracao = Estavel;
|
|
}
|
|
}
|
|
|
|
void CorrigePotenciaMotor() {
|
|
if (_FOC && !Revertendo && _Sentido != Parado) {
|
|
|
|
_PID->Compute();
|
|
}
|
|
}
|
|
|
|
TaskHandle_t RampaTaskHandle = NULL;
|
|
static void RampaTaskWrapper(void *pvParameters) {
|
|
Motor *motor = static_cast<Motor*>(pvParameters);
|
|
motor->RampaTask();
|
|
}
|
|
|
|
void RampaTask() {
|
|
unsigned long previousMillisRampa = 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;
|
|
}
|
|
unsigned long currentMillis = millis();
|
|
if (currentMillis - previousMillisRampa >= _Rampa) {
|
|
previousMillisRampa = currentMillis;
|
|
|
|
Atualizar();
|
|
|
|
CorrigePotenciaMotor();
|
|
}
|
|
|
|
// Aguarda metade do tempo de rampa para realizar a próxima verificação
|
|
int wait = (_Rampa / 2) + 1;
|
|
vTaskDelay(wait);
|
|
}
|
|
}
|
|
|
|
};
|
|
|
|
Motor M1("ET", 25);
|
|
Motor M2("EF", 35);
|
|
Motor M3("DT", 45);
|
|
Motor M4("DF", 55);
|
|
|
|
void setup() {
|
|
Serial.begin(_baudRate);
|
|
|
|
IniciarMemoria();
|
|
delay(10);
|
|
//LimparMemoria();
|
|
/*GravarMemoria(M1_US_Addr, (int)Horario);
|
|
GravarMemoria(M2_US_Addr, (int)Antihorario);
|
|
GravarMemoria(M3_US_Addr, (int)Antihorario);
|
|
GravarMemoria(M4_US_Addr, (int)Horario);*/
|
|
|
|
TaskHandle_t SerialTaskHandle = NULL;
|
|
xTaskCreatePinnedToCore(SerialTask, "SerialTask", 4000, NULL, 20, &SerialTaskHandle, 1);
|
|
}
|
|
|
|
void loop() {
|
|
float x = 1509 / 300;
|
|
}
|
|
|
|
|
|
void SerialTask(void *pvParameters) {
|
|
unsigned long previousMillisSerial = millis();
|
|
const int TempoLeitura = 100;
|
|
while (1)
|
|
{
|
|
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;
|
|
|
|
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";
|
|
int _fd = ((String)Protocolo[11] + (String)Protocolo[12]).toInt();
|
|
double _Kp = ((String)Protocolo[14] + (String)Protocolo[15] + (String)Protocolo[16] + (String)Protocolo[17] + (String)Protocolo[18]).toDouble();
|
|
double _Ki = ((String)Protocolo[20] + (String)Protocolo[21] + (String)Protocolo[22] + (String)Protocolo[23] + (String)Protocolo[24]).toDouble();
|
|
double _Kd = ((String)Protocolo[26] + (String)Protocolo[27] + (String)Protocolo[28] + (String)Protocolo[29] + (String)Protocolo[30]).toDouble();
|
|
int pwm = ((String)Protocolo[32] + (String)Protocolo[33]).toInt();
|
|
int vel = ((String)Protocolo[35] + (String)Protocolo[36]).toInt();
|
|
int dir = ((String)Protocolo[38] + (String)Protocolo[39]).toInt();
|
|
int brk = ((String)Protocolo[41] + (String)Protocolo[42]).toInt();
|
|
int stp = ((String)Protocolo[44] + (String)Protocolo[45]).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->_pinoVEL = vel;
|
|
_motor->_pinoDIR = dir;
|
|
_motor->_pinoBRK = brk;
|
|
_motor->_pinoSTP = stp;
|
|
_motor->_FOC = _foc;
|
|
_motor->_FatorDivisor = _fd;
|
|
_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->PotenciaAtual = ((String)Protocolo[3] + (String)Protocolo[4] + (String)Protocolo[5]).toInt();
|
|
//_motor->_Potencia_SP = ((String)Protocolo[3] + (String)Protocolo[4] + (String)Protocolo[5]).toInt();
|
|
_motor->_Sentido = (Sentido)((String)Protocolo[7]).toInt();
|
|
_motor->_Rampa = ((String)Protocolo[9] + (String)Protocolo[10] + (String)Protocolo[11]).toInt();
|
|
_motor->_Precisao = ((String)Protocolo[13] + (String)Protocolo[14] + (String)Protocolo[15]).toInt();
|
|
_motor->_RPM_SP = ((String)Protocolo[17] + (String)Protocolo[18] + (String)Protocolo[19]).toInt();
|
|
_motor->_FOC = (String)Protocolo[21] == "1";
|
|
_motor->ReiniciarAceleracaoArr();
|
|
_motor->_RampaAtivada = true;
|
|
}
|
|
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();
|
|
}
|
|
|
|
}
|
|
}
|
|
|
|
vTaskDelay(pdMS_TO_TICKS(TempoLeitura));
|
|
}
|
|
}
|