640 lines
20 KiB
C++
640 lines
20 KiB
C++
#include "C:\Zendion Inc\agrobot_base\Firmware\Modulos\SerialService.h"
|
|
#include "C:\Zendion Inc\agrobot_base\Firmware\Modulos\MemoryService.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;
|
|
|
|
bool Iniciado = false;
|
|
bool Testando = false;
|
|
bool Revertendo = false;
|
|
StatusMotor Aceleracao = Estavel;
|
|
|
|
// Consumo
|
|
const int leituras = 5;
|
|
volatile float RPM_arr[5];
|
|
volatile float 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;
|
|
double _Potencia_SP_A;
|
|
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();
|
|
}
|
|
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, LOW);
|
|
}
|
|
|
|
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;
|
|
|
|
EnviarDadosSerial(_ID + " Desligado");
|
|
|
|
Iniciado = false;
|
|
|
|
EnviarDadosSerial(MontarProtocoloSensor(sCFG, _ID, Iniciado ? "1" : "0"));
|
|
}
|
|
|
|
void Testar() {
|
|
Testando = true;
|
|
EnviarDadosSerial(MontarProtocoloSensor(sTST, _ID, "Testando motor " + _ID + "..."));
|
|
TestePino("DIR", _pinoDIR);
|
|
TestePino("BRK", _pinoBRK);
|
|
TestePino("STP", _pinoSTP);
|
|
TestePWM();
|
|
//TesteAcionamentoMotores();
|
|
Testando = false;
|
|
EnviarDadosSerial(MontarProtocoloSensor(sTST, _ID, "Teste finalizado"));
|
|
}
|
|
|
|
void TestePino(String Descricao, int Pino) {
|
|
EnviarDadosSerial("Pino " + ((String)Pino) + ": " + Descricao);
|
|
digitalWrite(Pino, LOW);
|
|
vTaskDelay(100);
|
|
digitalWrite(Pino, HIGH);
|
|
vTaskDelay(100);
|
|
int Estado = digitalRead(Pino);
|
|
EnviarDadosSerial(MontarProtocoloSensor(sTST, _ID, MontarDadosProtocoloTeste(Testando, Descricao, ((String)((Estado == 1) ? "SUCESSO" : "FALHA")), "1", ((String)Estado))));
|
|
vTaskDelay(1000);
|
|
digitalWrite(Pino, LOW);
|
|
vTaskDelay(100);
|
|
Estado = digitalRead(Pino);
|
|
EnviarDadosSerial(MontarProtocoloSensor(sTST, _ID, MontarDadosProtocoloTeste(Testando, Descricao, ((String)((Estado == 0) ? "SUCESSO" : "FALHA")), "0", ((String)Estado))));
|
|
vTaskDelay(1000);
|
|
}
|
|
|
|
void TestePWM() {
|
|
EnviarDadosSerial("Pino " + ((String)_pinoPWM) + ": Canal " + ((String)_canal));
|
|
AtualizaTestePWM(0);
|
|
AtualizaTestePWM(255);
|
|
AtualizaTestePWM(512);
|
|
AtualizaTestePWM(768);
|
|
AtualizaTestePWM(1024);
|
|
}
|
|
|
|
void AtualizaTestePWM(int Comando) {
|
|
ledcWrite(_canal, Comando);
|
|
vTaskDelay(100);
|
|
int Leitura = analogRead(_pinoPWM);
|
|
EnviarDadosSerial(MontarProtocoloSensor(sTST, _ID, MontarDadosProtocoloTeste(Testando, "PWM", (Leitura == Comando ? "SUCESSO" : "FALHA"), (String)Comando, (String)Leitura)));
|
|
vTaskDelay(500);
|
|
}
|
|
|
|
void TesteAcionamentoMotores() {
|
|
EnviarDadosSerial("Sentido Horario");
|
|
PotenciaAtual = 20.0;
|
|
_Sentido = Horario;
|
|
Atualizar();
|
|
vTaskDelay(2000);
|
|
EnviarDadosSerial("Sentido Parado");
|
|
_Sentido = Parado;
|
|
PotenciaAtual = 0.0;
|
|
Atualizar();
|
|
vTaskDelay(2000);
|
|
EnviarDadosSerial("Sentido Antihorario");
|
|
_Sentido = Antihorario;
|
|
PotenciaAtual = 20.0;
|
|
Atualizar();
|
|
vTaskDelay(2000);
|
|
EnviarDadosSerial("Sentido Parado");
|
|
_Sentido = Parado;
|
|
PotenciaAtual = 0.0;
|
|
Atualizar();
|
|
vTaskDelay(2000);
|
|
}
|
|
|
|
void ReiniciarAceleracaoArr() {
|
|
for (int i = 0; i < leituras; i++) {
|
|
RPM_arr[i] = -1.0;
|
|
}
|
|
}
|
|
|
|
private:
|
|
|
|
void Atualizar() {
|
|
if (_pinoDIR > -1) {
|
|
if (_SentidoA != Parado && _SentidoA != _SentidoM) {
|
|
Revertendo = true;
|
|
_SentidoM = _SentidoA;
|
|
GravarMemoria(_US_Addr, (int)_SentidoM);
|
|
|
|
digitalWrite(_pinoSTP, HIGH);
|
|
digitalWrite(_pinoDIR, HIGH);
|
|
vTaskDelay(1500);
|
|
digitalWrite(_pinoSTP, LOW);
|
|
digitalWrite(_pinoDIR, LOW);
|
|
Revertendo = false;
|
|
}
|
|
}
|
|
if (_pinoBRK > -1) {
|
|
digitalWrite(_pinoBRK, _Freio);
|
|
}
|
|
if (_pinoSTP > -1) {
|
|
digitalWrite(_pinoSTP, _SentidoA != Parado);
|
|
}
|
|
if (_pinoPWM > -1) {
|
|
EnviarDadosSerial((String)PotenciaAtual);
|
|
_PotMap = map(PotenciaAtual, ZeroRampa, RampaMax, RampaMin, RampaMax);
|
|
if (PotenciaAtual == ZeroRampa && _Sentido != Parado) {
|
|
ledcWrite(_canal, ZeroRampa);
|
|
vTaskDelay(500);
|
|
}
|
|
else {
|
|
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 = map(_PotMap, RampaMin, RampaMax, 0, 100);
|
|
EnviarDadosSerial(MontarProtocoloSensor(sRPM, _ID, MontarDadosProtocoloRPM(RPM, PotAtual, _Potencia_SP, periodo, Aceleracao)));
|
|
firstMilis = millis();
|
|
|
|
//CorrigePotenciaMotor();
|
|
}
|
|
|
|
// 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;
|
|
|
|
if (Desconsiderar > 0 || _RampaAtivada) {
|
|
if (_Potencia_SP < _Potencia_SP_A) {
|
|
Aceleracao = Desacelerando;
|
|
}
|
|
else {
|
|
Aceleracao = Acelerando;
|
|
}
|
|
return;
|
|
}
|
|
|
|
//float RPM_medio = RPM_total / (leituras - Desconsiderar);
|
|
float RPM_medio = RPM_arr[leituras - 2];
|
|
int mg = 5;
|
|
|
|
if (_SentidoA == Parado && _RPM == 0) {
|
|
Aceleracao = Estavel;
|
|
_Potencia_SP_A = 0;
|
|
} else if ((_RPM + mg) < RPM_medio) {
|
|
Aceleracao = Desacelerando;
|
|
} else if ((_RPM - mg) > RPM_medio) {
|
|
Aceleracao = Acelerando;
|
|
} else {
|
|
Aceleracao = Estavel;
|
|
}
|
|
}
|
|
|
|
void CorrigePotenciaMotor() {
|
|
if (_FOC && !Revertendo && !_RampaAtivada && Aceleracao == Estavel && (((RPM + _Margem) < _RPM_SP) || ((RPM - _Margem) > _RPM_SP))) {
|
|
_Potencia_SP_A = _Potencia_SP;
|
|
float PA = PotenciaAcrescentar();
|
|
_Potencia_SP += PA;
|
|
|
|
if (_Potencia_SP > 100) {
|
|
_Potencia_SP = 100;
|
|
}
|
|
else if (_Potencia_SP < 1) {
|
|
_Potencia_SP = 1;
|
|
}
|
|
|
|
if (_Potencia_SP != _Potencia_SP_A) {
|
|
ReiniciarAceleracaoArr();
|
|
_RampaAtivada = true;
|
|
}
|
|
}
|
|
}
|
|
|
|
float PotenciaAcrescentar() {
|
|
if (((RPM + _Margem) > _RPM_SP) && ((RPM - _Margem) < _RPM_SP)) {
|
|
return 0.0;
|
|
}
|
|
|
|
float PercentualDistancia = (_RPM_SP / (RPM == 0 ? 1 : RPM));
|
|
float AcrescimoPotencia = (PercentualDistancia * _Potencia_SP) - _Potencia_SP;
|
|
if (AcrescimoPotencia > 100.0) {
|
|
AcrescimoPotencia = 100.0;
|
|
}
|
|
else if (AcrescimoPotencia < -100.0) {
|
|
AcrescimoPotencia = -100.0;
|
|
}
|
|
float AcrescimoPotenciaCorrigido = (AcrescimoPotencia / _FatorDivisor);
|
|
return AcrescimoPotenciaCorrigido;
|
|
}
|
|
|
|
int newPotenciaAcrescentar() {
|
|
if (((RPM + _Margem) > _RPM_SP) && ((RPM - _Margem) < _RPM_SP)) {
|
|
return 0;
|
|
}
|
|
|
|
int Acrescentar = ((RPM + _Margem) > _RPM_SP) ? -_Precisao : _Precisao;
|
|
return Acrescentar;
|
|
}
|
|
|
|
|
|
TaskHandle_t RampaTaskHandle = NULL;
|
|
static void RampaTaskWrapper(void *pvParameters) {
|
|
Motor *motor = static_cast<Motor*>(pvParameters);
|
|
motor->RampaTask();
|
|
}
|
|
|
|
void RampaTask() {
|
|
unsigned long previousMillisRampa = millis();
|
|
double MultiplicadorPotencia = RampaMax / 100;
|
|
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;
|
|
|
|
if (_RampaAtivada) {
|
|
bool Limite = false;
|
|
|
|
if ((_Sentido == Parado || _Sentido != _SentidoA) && PotenciaAtual > ZeroRampa) { // M1 estiver parando ou invertendo, e pwm for maior zero rampa
|
|
PotenciaAtual -= _Precisao; // Decrementar para atingir ponto zero rampa
|
|
if (PotenciaAtual < ZeroRampa)
|
|
PotenciaAtual = ZeroRampa;
|
|
}
|
|
else if (_Sentido != _SentidoA && PotenciaAtual <= ZeroRampa) { // ZEROU AO MUDAR SENTIDO DE GIRO ANTES DE AUMENTAR
|
|
_SentidoA = _Sentido;
|
|
}
|
|
else if (_Sentido != Parado && (PotenciaAtual + 1) > (_Potencia_SP * MultiplicadorPotencia) && (PotenciaAtual - 1) < (_Potencia_SP * MultiplicadorPotencia)) { // Estabiliza a potência do motor
|
|
if (_Sentido == Parado) {
|
|
PotenciaAtual = ZeroRampa;
|
|
_Potencia_SP = ZeroRampa; //(RampaMax / RampaMin);
|
|
}
|
|
else {
|
|
PotenciaAtual = (PotenciaAtual > RampaMax) ? RampaMax : (_Potencia_SP * MultiplicadorPotencia);
|
|
}
|
|
_SentidoA = _Sentido;
|
|
Limite = true;
|
|
}
|
|
else if (_Sentido != Parado && PotenciaAtual < (_Potencia_SP * MultiplicadorPotencia)) { // abaixo do set point
|
|
PotenciaAtual += _Precisao; // Incrementar para atingir o set point
|
|
if (PotenciaAtual > RampaMax)
|
|
PotenciaAtual = RampaMax;
|
|
}
|
|
else if (_Sentido != Parado && PotenciaAtual > (_Potencia_SP * MultiplicadorPotencia)) { // acima do set point
|
|
PotenciaAtual -= _Precisao; // Decrementar para atingir o set point
|
|
if (PotenciaAtual < ZeroRampa)
|
|
PotenciaAtual = ZeroRampa;
|
|
}
|
|
else { // Estabiliza a potência do motor
|
|
if (_Sentido == Parado) {
|
|
PotenciaAtual = ZeroRampa;
|
|
_Potencia_SP = ZeroRampa; //(RampaMax / RampaMin);
|
|
}
|
|
else {
|
|
PotenciaAtual = (PotenciaAtual > RampaMax) ? RampaMax : (_Potencia_SP * MultiplicadorPotencia);
|
|
}
|
|
_SentidoA = _Sentido;
|
|
Limite = true;
|
|
}
|
|
|
|
Atualizar();
|
|
|
|
if (Limite) {
|
|
_RampaAtivada = false;
|
|
}
|
|
}
|
|
else {
|
|
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();
|
|
int pwm = ((String)Protocolo[14] + (String)Protocolo[15]).toInt();
|
|
int vel = ((String)Protocolo[17] + (String)Protocolo[18]).toInt();
|
|
int dir = ((String)Protocolo[20] + (String)Protocolo[21]).toInt();
|
|
int brk = ((String)Protocolo[23] + (String)Protocolo[24]).toInt();
|
|
int stp = ((String)Protocolo[26] + (String)Protocolo[27]).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->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->_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));
|
|
}
|
|
}
|