603 lines
19 KiB
Arduino
603 lines
19 KiB
Arduino
|
// Dispositivo: Movimentação
|
||
|
|
// Versão Firmware: 17
|
||
|
|
// Ultima atualização: 04/04/2024
|
||
|
|
// Atualizações: Atualização do modelo pinout
|
||
|
|
|
||
|
|
#include "C:\ZendionInc\agrobot_base\Firmware\Modulos\SerialService.h"
|
||
|
|
#include "C:\ZendionInc\agrobot_base\Firmware\Modulos\utils.h"
|
||
|
|
#include "C:\ZendionInc\agrobot_base\Firmware\Modulos\Pinout.h"
|
||
|
|
#include <PID_v1.h>
|
||
|
|
#define D_Code Mov
|
||
|
|
#define VERSION 17
|
||
|
|
|
||
|
|
#define _pinoLED RGB_BUILTIN
|
||
|
|
|
||
|
|
long _baudRate = 115200;
|
||
|
|
bool Conectado = false;
|
||
|
|
int _TaxaAmostragem = 500;
|
||
|
|
|
||
|
|
int PPR = 22; // Pulsos por revolução
|
||
|
|
int RampaMin = 0;
|
||
|
|
int ZeroRampa = 0;
|
||
|
|
int RampaMax = 1023;
|
||
|
|
const int RpmMax = 650; // RPM Máximo aferir
|
||
|
|
float RelacaoPPR = 0;
|
||
|
|
float MenorPeriodo = 0;
|
||
|
|
|
||
|
|
int delayBalanceamento = 5000;
|
||
|
|
float RPM_Offset_Max = 25.0;
|
||
|
|
float RPM_Margem = 5.0;
|
||
|
|
float RPM_SetPoint = 0.0;
|
||
|
|
float RPM_Medio = 0.0;
|
||
|
|
bool RPM_Estavel = true;
|
||
|
|
|
||
|
|
class Motor {
|
||
|
|
public:
|
||
|
|
// Construtor
|
||
|
|
Motor(String desc) {
|
||
|
|
_ID = desc;
|
||
|
|
}
|
||
|
|
|
||
|
|
// Definições
|
||
|
|
String _ID;
|
||
|
|
int _canal;
|
||
|
|
PinoModel _pinoPWM;
|
||
|
|
PinoModel _pinoDIR;
|
||
|
|
PinoModel _pinoBRK;
|
||
|
|
PinoModel _pinoSTP;
|
||
|
|
PinoModel _pinoHLA;
|
||
|
|
PinoModel _pinoHLB;
|
||
|
|
PinoModel _pinoHLC;
|
||
|
|
|
||
|
|
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 int ultimaLeituraHallB = 0;
|
||
|
|
volatile int ultimaLeituraHallC = 0;
|
||
|
|
volatile unsigned int tempoAnterior = 0;
|
||
|
|
volatile float periodo = 0;
|
||
|
|
|
||
|
|
// Entrada de dados
|
||
|
|
bool _MalhaFechada = true;
|
||
|
|
int _PotMap;
|
||
|
|
double _RPM_SP;
|
||
|
|
Sentido _SentidoSP = Parado;
|
||
|
|
Sentido _Sentido = Parado;
|
||
|
|
Sentido _SentidoU = Parado;
|
||
|
|
int _US_Addr;
|
||
|
|
bool _Alarme = false;
|
||
|
|
bool _Freio = false;
|
||
|
|
int _Margem = 2;
|
||
|
|
double _RPM_Offset = 0;
|
||
|
|
|
||
|
|
void Inicializar() {
|
||
|
|
if (Iniciado) {
|
||
|
|
EnviarDadosSerial(_ID + " ja inicializado");
|
||
|
|
EnviarDadosSerial(MontarProtocoloSensor(sCFG, _ID, Iniciado ? "1" : "0"));
|
||
|
|
return;
|
||
|
|
}
|
||
|
|
|
||
|
|
if (_pinoPWM.Definido()) {
|
||
|
|
ledcSetup(_canal, 1000, 10);
|
||
|
|
ledcAttachPin(_pinoPWM.num, _canal);
|
||
|
|
PotenciaAtual = ZeroRampa;
|
||
|
|
_PotMap = PotenciaAtual;
|
||
|
|
_RPM_SP = 0;
|
||
|
|
ledcWrite(_canal, _PotMap);
|
||
|
|
}
|
||
|
|
_pinoDIR.Conectar(false);
|
||
|
|
_SentidoU = Parado;
|
||
|
|
_pinoBRK.Conectar(_Freio);
|
||
|
|
_pinoSTP.Conectar(true);
|
||
|
|
if (_pinoHLA.Definido()) {
|
||
|
|
_pinoHLA.Conectar();
|
||
|
|
ultimaLeituraHallA = _pinoHLA.get();
|
||
|
|
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);
|
||
|
|
}
|
||
|
|
_pinoHLB.Conectar();
|
||
|
|
ultimaLeituraHallB = _pinoHLB.get();
|
||
|
|
_pinoHLC.Conectar();
|
||
|
|
ultimaLeituraHallC = _pinoHLC.get();
|
||
|
|
|
||
|
|
|
||
|
|
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.Definido()) {
|
||
|
|
vTaskDelete(RPMTaskHandle);
|
||
|
|
}
|
||
|
|
|
||
|
|
// Redefinir as configurações para os valores iniciais
|
||
|
|
_pinoPWM.Desconectar();
|
||
|
|
_pinoPWM.Desconectar();
|
||
|
|
_pinoDIR.Desconectar();
|
||
|
|
_pinoBRK.Desconectar();
|
||
|
|
_pinoSTP.Desconectar();
|
||
|
|
_pinoHLA.Desconectar();
|
||
|
|
_pinoHLB.Desconectar();
|
||
|
|
_pinoHLC.Desconectar();
|
||
|
|
|
||
|
|
// 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;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
private:
|
||
|
|
|
||
|
|
TaskHandle_t RPMTaskHandle = NULL;
|
||
|
|
static void RPMTaskWrapper(void *pvParameters) {
|
||
|
|
Motor *motor = static_cast<Motor*>(pvParameters);
|
||
|
|
motor->RPMTask();
|
||
|
|
}
|
||
|
|
|
||
|
|
void RPMTask() {
|
||
|
|
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 = _pinoHLA.get();
|
||
|
|
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, _RPM_Offset)));
|
||
|
|
firstMilis = millis();
|
||
|
|
}
|
||
|
|
|
||
|
|
// Aguarda metade do menor período possível entre leituras
|
||
|
|
vTaskDelay(1);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
void CalcularSentidoGiro(float _RPM) {
|
||
|
|
if (!_pinoHLA.Definido() || !_pinoHLB.Definido() || !_pinoHLC.Definido()) {
|
||
|
|
CalculaAceleracao(_RPM);
|
||
|
|
return;
|
||
|
|
}
|
||
|
|
|
||
|
|
bool LeituraHa = ultimaLeituraHallA == 1;
|
||
|
|
bool LeituraHb = _pinoHLB.get() == 1;
|
||
|
|
bool LeituraHc = _pinoHLC.get() == 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.Definido() && _pinoHLB.Definido() && _pinoHLC.Definido()) {
|
||
|
|
SentidoContrario = _Sentido != _SentidoSP;
|
||
|
|
}
|
||
|
|
|
||
|
|
if (SentidoContrario) {
|
||
|
|
RPM = RPM * -1;
|
||
|
|
}
|
||
|
|
/*else if (Aceleracao == Desacelerando && AceleracaoA == Estavel) {
|
||
|
|
RPM = 0;
|
||
|
|
}*/
|
||
|
|
|
||
|
|
float _rpmA = RPM;
|
||
|
|
if (RPM_Estavel) {
|
||
|
|
RPM = RPM + _RPM_Offset;
|
||
|
|
}
|
||
|
|
|
||
|
|
_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();
|
||
|
|
|
||
|
|
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.Definido() && Travado) {
|
||
|
|
_PotMap = ZeroRampa;
|
||
|
|
ledcWrite(_canal, _PotMap);
|
||
|
|
vTaskDelay(400);
|
||
|
|
_PotMap = PotenciaAtual;
|
||
|
|
}
|
||
|
|
|
||
|
|
if (_pinoDIR.Definido()) {
|
||
|
|
bool ReleAtuado = _SentidoSP == Horario;
|
||
|
|
|
||
|
|
if (_SentidoSP != _SentidoU && ReleAtuado) {
|
||
|
|
_pinoDIR.set(ReleAtuado);
|
||
|
|
vTaskDelay(1000);
|
||
|
|
}
|
||
|
|
|
||
|
|
_pinoDIR.set(ReleAtuado);
|
||
|
|
|
||
|
|
_SentidoU = _SentidoSP;
|
||
|
|
}
|
||
|
|
if (_pinoBRK.Definido()) {
|
||
|
|
_pinoBRK.set(_Freio);
|
||
|
|
}
|
||
|
|
if (_pinoSTP.Definido()) {
|
||
|
|
_pinoSTP.set(HIGH);
|
||
|
|
}
|
||
|
|
if (_pinoPWM.Definido()) {
|
||
|
|
if (_SentidoSP == Parado) {
|
||
|
|
_PotMap = ZeroRampa;
|
||
|
|
}
|
||
|
|
else {
|
||
|
|
_PotMap = PotenciaAtual;
|
||
|
|
}
|
||
|
|
ledcWrite(_canal, _PotMap);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
};
|
||
|
|
|
||
|
|
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 BalanceamentoTask(void *pvParameters);
|
||
|
|
TaskHandle_t BalanceamentoTaskHandle = NULL;
|
||
|
|
|
||
|
|
void setup() {
|
||
|
|
Serial.begin(_baudRate);
|
||
|
|
|
||
|
|
delay(10);
|
||
|
|
|
||
|
|
xTaskCreatePinnedToCore(BalanceamentoTask, "BalanceamentoTask", 8000, NULL, 5, &BalanceamentoTaskHandle, tskNO_AFFINITY);
|
||
|
|
|
||
|
|
RecalcularRelacaoPPR();
|
||
|
|
}
|
||
|
|
|
||
|
|
void loop() {
|
||
|
|
ProtocoloSerial protocolos[MAX_PROTOCOLS];
|
||
|
|
int numProtocolos = LerBufferSerial(protocolos);
|
||
|
|
|
||
|
|
// Processa cada protocolo após a leitura completa
|
||
|
|
for (int i = 0; i < numProtocolos; i++) {
|
||
|
|
ProcessarProtocolo(protocolos[i].protocolo, protocolos[i].funcao);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
void ProcessarProtocolo(String Protocolo, F_Code _funcao) {
|
||
|
|
EnviarDadosSerial("OK");
|
||
|
|
|
||
|
|
if (_funcao == Chk) {
|
||
|
|
EnviarDadosSerial(MontarProtocoloVerificacao(D_Code, VERSION));
|
||
|
|
}
|
||
|
|
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 _RampaMin = ((String)Protocolo[11] + (String)Protocolo[12] + (String)Protocolo[13] + (String)Protocolo[14]).toInt();
|
||
|
|
int _RampaMax = ((String)Protocolo[16] + (String)Protocolo[17] + (String)Protocolo[18] + (String)Protocolo[19]).toInt();
|
||
|
|
int _PPR = ((String)Protocolo[21] + (String)Protocolo[22] + (String)Protocolo[23]).toInt();
|
||
|
|
int _delayBalanceamento = ((String)Protocolo[25] + (String)Protocolo[26] + (String)Protocolo[27] + (String)Protocolo[28] + (String)Protocolo[29]).toInt();
|
||
|
|
int _margemRPM = ((String)Protocolo[31] + (String)Protocolo[32]).toInt();
|
||
|
|
int _maxOffsetRPM = ((String)Protocolo[34] + (String)Protocolo[35]).toInt();
|
||
|
|
|
||
|
|
RampaMin = _RampaMin;
|
||
|
|
RampaMax = _RampaMax;
|
||
|
|
PPR = _PPR;
|
||
|
|
delayBalanceamento = _delayBalanceamento;
|
||
|
|
RPM_Margem = _margemRPM;
|
||
|
|
RPM_Offset_Max = _maxOffsetRPM;
|
||
|
|
|
||
|
|
RecalcularRelacaoPPR();
|
||
|
|
|
||
|
|
vTaskDelay(pdMS_TO_TICKS(100));
|
||
|
|
Conectado = Conectar;
|
||
|
|
EnviarDadosSerial(MontarProtocoloSensor(sCFG, "MD", Conectado ? "1" : "0"));
|
||
|
|
}
|
||
|
|
else {
|
||
|
|
int canal = ((String)Protocolo[5]).toInt();
|
||
|
|
bool _alarme = (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();
|
||
|
|
TiposBarramentos pwmB = (TiposBarramentos)((String)Protocolo[29]).toInt();
|
||
|
|
int pwm = ((String)Protocolo[30] + (String)Protocolo[31]).toInt();
|
||
|
|
TiposBarramentos dirB = (TiposBarramentos)((String)Protocolo[33]).toInt();
|
||
|
|
int dir = ((String)Protocolo[34] + (String)Protocolo[35]).toInt();
|
||
|
|
TiposBarramentos brkB = (TiposBarramentos)((String)Protocolo[37]).toInt();
|
||
|
|
int brk = ((String)Protocolo[38] + (String)Protocolo[39]).toInt();
|
||
|
|
TiposBarramentos stpB = (TiposBarramentos)((String)Protocolo[41]).toInt();
|
||
|
|
int stp = ((String)Protocolo[42] + (String)Protocolo[43]).toInt();
|
||
|
|
TiposBarramentos hlAB = (TiposBarramentos)((String)Protocolo[45]).toInt();
|
||
|
|
int hallA = ((String)Protocolo[46] + (String)Protocolo[47]).toInt();
|
||
|
|
TiposBarramentos hlBB = (TiposBarramentos)((String)Protocolo[49]).toInt();
|
||
|
|
int hallB = ((String)Protocolo[50] + (String)Protocolo[51]).toInt();
|
||
|
|
TiposBarramentos hlCB = (TiposBarramentos)((String)Protocolo[53]).toInt();
|
||
|
|
int hallC = ((String)Protocolo[54] + (String)Protocolo[55]).toInt();
|
||
|
|
|
||
|
|
Motor* _motor = MotorPorID(ID);
|
||
|
|
if (motor_ativado) {
|
||
|
|
if (Conectar) {
|
||
|
|
_motor->_canal = canal;
|
||
|
|
_motor->_pinoPWM = PinoModel(pwm, pwmB, _PWM);
|
||
|
|
_motor->_pinoDIR = PinoModel(dir, dirB, _OUTPUT);
|
||
|
|
_motor->_pinoBRK = PinoModel(brk, brkB, _OUTPUT);
|
||
|
|
_motor->_pinoSTP = PinoModel(stp, stpB, _OUTPUT);
|
||
|
|
_motor->_pinoHLA = PinoModel(hallA, hlAB, _INPUT, DIGITAL);
|
||
|
|
_motor->_pinoHLB = PinoModel(hallB, hlBB, _INPUT, DIGITAL);
|
||
|
|
_motor->_pinoHLC = PinoModel(hallC, hlCB, _INPUT, DIGITAL);
|
||
|
|
_motor->_Alarme = _alarme;
|
||
|
|
_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 = MotorPorID(ID);
|
||
|
|
|
||
|
|
_motor->_SentidoSP = (Sentido)((String)Protocolo[3]).toInt();
|
||
|
|
_motor->_RPM_SP = ((String)Protocolo[5] + (String)Protocolo[6] + (String)Protocolo[7]).toInt();
|
||
|
|
_motor->_Alarme = ((String)Protocolo[9]) == "1";
|
||
|
|
_motor->ReiniciarAceleracaoArr();
|
||
|
|
}
|
||
|
|
else if (_funcao == Tst) {
|
||
|
|
String ID = ((String)Protocolo[0] +(String)Protocolo[1]);
|
||
|
|
|
||
|
|
Motor* _motor = MotorPorID(ID);
|
||
|
|
|
||
|
|
bool EmTeste = _motor->Testando;
|
||
|
|
if (!EmTeste) {
|
||
|
|
//_motor->Testar();
|
||
|
|
}
|
||
|
|
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
void RecalcularRelacaoPPR() {
|
||
|
|
RelacaoPPR = (60 / PPR) * 1000; // Multiplicador do cálculo de RPM (período em ms)
|
||
|
|
MenorPeriodo = RelacaoPPR / RpmMax; // Tempo mínimo de leitura
|
||
|
|
}
|
||
|
|
|
||
|
|
void BalanceamentoTask(void *pvParameters) {
|
||
|
|
while (1)
|
||
|
|
{
|
||
|
|
int _delay = delayBalanceamento > 0 ? delayBalanceamento : 5000;
|
||
|
|
|
||
|
|
if (Conectado && delayBalanceamento > 0) {
|
||
|
|
BalancearCargasMotores();
|
||
|
|
}
|
||
|
|
|
||
|
|
vTaskDelay(_delay);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
void BalancearCargasMotores() {
|
||
|
|
RPM_Medio = (M1.RPM + M2.RPM + M3.RPM + M4.RPM) / 4.0;
|
||
|
|
RPM_SetPoint = (M1._RPM_SP + M2._RPM_SP + M3._RPM_SP + M4._RPM_SP) / 4.0;
|
||
|
|
RPM_Estavel = (RPM_Medio >= (RPM_SetPoint - RPM_Margem)) && (RPM_Medio <= (RPM_SetPoint + RPM_Margem));
|
||
|
|
|
||
|
|
double M1_Pot = M1.PotenciaAtual;
|
||
|
|
double M2_Pot = M2.PotenciaAtual;
|
||
|
|
double M3_Pot = M3.PotenciaAtual;
|
||
|
|
double M4_Pot = M4.PotenciaAtual;
|
||
|
|
|
||
|
|
double mediaPotencia = (M1_Pot + M2_Pot + M3_Pot + M4_Pot) / 4.0;
|
||
|
|
|
||
|
|
// Certifique-se de verificar se a potência atual não é zero antes de dividir
|
||
|
|
double offsetM1_RPM = (M1_Pot != 0 ? (M1.RPM * (mediaPotencia - M1_Pot)) / M1_Pot : 0) * -1;
|
||
|
|
double offsetM2_RPM = (M2_Pot != 0 ? (M2.RPM * (mediaPotencia - M2_Pot)) / M2_Pot : 0) * -1;
|
||
|
|
double offsetM3_RPM = (M3_Pot != 0 ? (M3.RPM * (mediaPotencia - M3_Pot)) / M3_Pot : 0) * -1;
|
||
|
|
double offsetM4_RPM = (M4_Pot != 0 ? (M4.RPM * (mediaPotencia - M4_Pot)) / M4_Pot : 0) * -1;
|
||
|
|
|
||
|
|
// Aplica o offset com limitação
|
||
|
|
M1._RPM_Offset = fmin(fmax(offsetM1_RPM, -RPM_Offset_Max), RPM_Offset_Max);
|
||
|
|
M2._RPM_Offset = fmin(fmax(offsetM2_RPM, -RPM_Offset_Max), RPM_Offset_Max);
|
||
|
|
M3._RPM_Offset = fmin(fmax(offsetM3_RPM, -RPM_Offset_Max), RPM_Offset_Max);
|
||
|
|
M4._RPM_Offset = fmin(fmax(offsetM4_RPM, -RPM_Offset_Max), RPM_Offset_Max);
|
||
|
|
}
|