574 lines
15 KiB
Arduino
574 lines
15 KiB
Arduino
|
enum Funcoes {
|
||
|
|
Limpar,
|
||
|
|
Ler,
|
||
|
|
Definir,
|
||
|
|
Incrementar
|
||
|
|
};
|
||
|
|
|
||
|
|
enum T_Code {
|
||
|
|
Vzo = 200,
|
||
|
|
Mov = 100,
|
||
|
|
Dir = 101,
|
||
|
|
Sen = 102,
|
||
|
|
Atu = 103,
|
||
|
|
};
|
||
|
|
|
||
|
|
enum F_Code {
|
||
|
|
Nda = 200,
|
||
|
|
Chk = 244,
|
||
|
|
Cfg = 258,
|
||
|
|
Tst = 288,
|
||
|
|
Cmd = 293,
|
||
|
|
};
|
||
|
|
|
||
|
|
enum S_Code {
|
||
|
|
sRPM,
|
||
|
|
sTST,
|
||
|
|
sCfg
|
||
|
|
};
|
||
|
|
|
||
|
|
enum Sentido {
|
||
|
|
Parado,
|
||
|
|
Horario,
|
||
|
|
Antihorario
|
||
|
|
};
|
||
|
|
|
||
|
|
#define D_Code Mov
|
||
|
|
|
||
|
|
int PinoReleGeral = 23;
|
||
|
|
bool Conectado = false;
|
||
|
|
|
||
|
|
const char EndLine = '$';
|
||
|
|
const char BeginSensor = '#';
|
||
|
|
int _TaxaAmostragem;
|
||
|
|
bool M1_Ativado = false;
|
||
|
|
bool M2_Ativado = false;
|
||
|
|
bool M3_Ativado = false;
|
||
|
|
bool M4_Ativado = false;
|
||
|
|
|
||
|
|
int ZeroRampa = 0;
|
||
|
|
int PotenciaMax = 1023;
|
||
|
|
|
||
|
|
class Motor {
|
||
|
|
public:
|
||
|
|
// Construtor
|
||
|
|
Motor(String desc, int canal, int _pwm, int _vel, int _dir, int _brk, int _stp, double kp = 0.5, double ki = 0.3, double kd = 0.09) {
|
||
|
|
_descricao = desc;
|
||
|
|
_canal = canal;
|
||
|
|
_pinoPWM = _pwm;
|
||
|
|
_pinoVEL = _vel;
|
||
|
|
_pinoDIR = _dir;
|
||
|
|
_pinoBRK = _brk;
|
||
|
|
_pinoSTP = _stp;
|
||
|
|
_Kp = kp;
|
||
|
|
_Ki = ki;
|
||
|
|
_Kd = kd;
|
||
|
|
}
|
||
|
|
|
||
|
|
// Definições
|
||
|
|
String _descricao;
|
||
|
|
int _canal;
|
||
|
|
int _pinoPWM;
|
||
|
|
int _pinoVEL;
|
||
|
|
int _pinoDIR;
|
||
|
|
int _pinoBRK;
|
||
|
|
int _pinoSTP;
|
||
|
|
double _Kp;
|
||
|
|
double _Ki;
|
||
|
|
double _Kd;
|
||
|
|
|
||
|
|
bool Iniciado = false;
|
||
|
|
bool Testando = false;
|
||
|
|
|
||
|
|
// Consumo
|
||
|
|
double RPM;
|
||
|
|
int PotenciaAtual;
|
||
|
|
volatile bool pulso_hall = false;
|
||
|
|
volatile unsigned long tempoAnterior = 0;
|
||
|
|
volatile unsigned long periodo = 0;
|
||
|
|
|
||
|
|
// Entrada de dados
|
||
|
|
int _Rampa;
|
||
|
|
int _Precisao;
|
||
|
|
double _Potencia;
|
||
|
|
double _RPM_SP;
|
||
|
|
Sentido _Sentido = Parado;
|
||
|
|
Sentido _SentidoA = Parado;
|
||
|
|
bool _RampaAtivada = false;
|
||
|
|
int _Margem = 2;
|
||
|
|
|
||
|
|
void Inicializar() {
|
||
|
|
RPM = 0;
|
||
|
|
pulso_hall = false;
|
||
|
|
periodo = 0;
|
||
|
|
tempoAnterior = 0;
|
||
|
|
|
||
|
|
if (Iniciado) {
|
||
|
|
EnviarDadosSerial(_descricao + " ja inicializado");
|
||
|
|
return;
|
||
|
|
}
|
||
|
|
|
||
|
|
pinMode(_pinoPWM, OUTPUT);
|
||
|
|
pinMode(_pinoVEL, INPUT);
|
||
|
|
pinMode(_pinoDIR, OUTPUT);
|
||
|
|
pinMode(_pinoBRK, OUTPUT);
|
||
|
|
pinMode(_pinoSTP, OUTPUT);
|
||
|
|
|
||
|
|
digitalWrite(_pinoPWM, LOW);
|
||
|
|
digitalWrite(_pinoDIR, HIGH);
|
||
|
|
digitalWrite(_pinoBRK, LOW);
|
||
|
|
digitalWrite(_pinoSTP, LOW);
|
||
|
|
|
||
|
|
ledcSetup(_canal, 1000, 10);
|
||
|
|
ledcAttachPin(_pinoPWM, _canal);
|
||
|
|
if (_canal == 0) {
|
||
|
|
attachInterrupt(digitalPinToInterrupt(_pinoVEL), contarPulsoM1, RISING);
|
||
|
|
}
|
||
|
|
else if (_canal == 1) {
|
||
|
|
attachInterrupt(digitalPinToInterrupt(_pinoVEL), contarPulsoM2, RISING);
|
||
|
|
}
|
||
|
|
else if (_canal == 2) {
|
||
|
|
attachInterrupt(digitalPinToInterrupt(_pinoVEL), contarPulsoM3, RISING);
|
||
|
|
}
|
||
|
|
else if (_canal == 3) {
|
||
|
|
attachInterrupt(digitalPinToInterrupt(_pinoVEL), contarPulsoM4, RISING);
|
||
|
|
}
|
||
|
|
xTaskCreatePinnedToCore(&Motor::RPMTaskWrapper, "RPMTask", 4000, this, _canal, &RPMTaskHandle, 0);
|
||
|
|
xTaskCreatePinnedToCore(&Motor::RampaTaskWrapper, "RampaTask", 4000, this, 20 - _canal, &RampaTaskHandle, 0);
|
||
|
|
|
||
|
|
EnviarDadosSerial(_descricao + " Iniciado");
|
||
|
|
|
||
|
|
Iniciado = true;
|
||
|
|
}
|
||
|
|
|
||
|
|
void Desligar() {
|
||
|
|
if (!Iniciado) {
|
||
|
|
EnviarDadosSerial(_descricao + " 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;
|
||
|
|
Iniciado = false;
|
||
|
|
periodo = 0;
|
||
|
|
tempoAnterior = 0;
|
||
|
|
pulso_hall = false;
|
||
|
|
|
||
|
|
EnviarDadosSerial(_descricao + " Desligado");
|
||
|
|
}
|
||
|
|
|
||
|
|
void Testar() {
|
||
|
|
Testando = true;
|
||
|
|
EnviarDadosSerial(MontarProtocoloSensor(sTST, _descricao, "Testando motor " + _descricao + "..."));
|
||
|
|
TestePino("DIR", _pinoDIR);
|
||
|
|
TestePino("BRK", _pinoBRK);
|
||
|
|
TestePino("STP", _pinoSTP);
|
||
|
|
TestePWM();
|
||
|
|
//TesteAcionamentoMotores();
|
||
|
|
Testando = false;
|
||
|
|
EnviarDadosSerial(MontarProtocoloSensor(sTST, _descricao, "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, _descricao, MontarDadosProtocoloTeste(Descricao, ((String)((Estado == 1) ? "SUCESSO" : "FALHA")), "1", ((String)Estado))));
|
||
|
|
vTaskDelay(1000);
|
||
|
|
digitalWrite(Pino, LOW);
|
||
|
|
vTaskDelay(100);
|
||
|
|
Estado = digitalRead(Pino);
|
||
|
|
EnviarDadosSerial(MontarProtocoloSensor(sTST, _descricao, MontarDadosProtocoloTeste(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, _descricao, MontarDadosProtocoloTeste("PWM", (Leitura == Comando ? "SUCESSO" : "FALHA"), (String)Comando, (String)Leitura)));
|
||
|
|
vTaskDelay(500);
|
||
|
|
}
|
||
|
|
|
||
|
|
void TesteAcionamentoMotores() {
|
||
|
|
EnviarDadosSerial("Sentido Horario");
|
||
|
|
PotenciaAtual = 20;
|
||
|
|
_Sentido = Horario;
|
||
|
|
Atualizar();
|
||
|
|
vTaskDelay(2000);
|
||
|
|
EnviarDadosSerial("Sentido Parado");
|
||
|
|
_Sentido = Parado;
|
||
|
|
PotenciaAtual = 0;
|
||
|
|
Atualizar();
|
||
|
|
vTaskDelay(2000);
|
||
|
|
EnviarDadosSerial("Sentido Antihorario");
|
||
|
|
_Sentido = Antihorario;
|
||
|
|
PotenciaAtual = 20;
|
||
|
|
Atualizar();
|
||
|
|
vTaskDelay(2000);
|
||
|
|
EnviarDadosSerial("Sentido Parado");
|
||
|
|
_Sentido = Parado;
|
||
|
|
PotenciaAtual = 0;
|
||
|
|
Atualizar();
|
||
|
|
vTaskDelay(2000);
|
||
|
|
}
|
||
|
|
|
||
|
|
void Atualizar() {
|
||
|
|
digitalWrite(_pinoDIR, _SentidoA == Horario);
|
||
|
|
digitalWrite(_pinoSTP, _SentidoA != Parado);
|
||
|
|
|
||
|
|
ledcWrite(_canal, PotenciaAtual);
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
void EnviarDadosSerial(String Mensagem) {
|
||
|
|
Serial.println(Mensagem);
|
||
|
|
Serial.flush();
|
||
|
|
vTaskDelay(10);
|
||
|
|
}
|
||
|
|
|
||
|
|
String MontarProtocoloSensor(S_Code TipoSensor, String _motor, String Dados) {
|
||
|
|
String Protocolo =
|
||
|
|
BeginSensor +
|
||
|
|
(String)TipoSensor +
|
||
|
|
_motor + ";" +
|
||
|
|
Dados;
|
||
|
|
|
||
|
|
return Protocolo;
|
||
|
|
}
|
||
|
|
|
||
|
|
String MontarDadosProtocoloTeste(String Componente, String Resultado, String Comando, String Leitura) {
|
||
|
|
String Protocolo =
|
||
|
|
(Testando ? "1;" : "0;") +
|
||
|
|
Componente + ";" +
|
||
|
|
Resultado + ";" +
|
||
|
|
Comando + ";" +
|
||
|
|
Leitura;
|
||
|
|
|
||
|
|
return Protocolo;
|
||
|
|
}
|
||
|
|
|
||
|
|
String MontarDadosProtocoloRPM(float rpm, double potencia) {
|
||
|
|
String Protocolo =
|
||
|
|
(String)"0;" +
|
||
|
|
(String)"0;" +
|
||
|
|
(String)rpm + ";" +
|
||
|
|
(String)potencia;
|
||
|
|
|
||
|
|
return Protocolo;
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
private:
|
||
|
|
static Motor* instanceM1;
|
||
|
|
static Motor* instanceM2;
|
||
|
|
static Motor* instanceM3;
|
||
|
|
static Motor* instanceM4;
|
||
|
|
static void contarPulsoM1() {
|
||
|
|
instanceM1->pulso_hall = true;
|
||
|
|
}
|
||
|
|
static void contarPulsoM2() {
|
||
|
|
instanceM2->pulso_hall = true;
|
||
|
|
}
|
||
|
|
static void contarPulsoM3() {
|
||
|
|
instanceM3->pulso_hall = true;
|
||
|
|
}
|
||
|
|
static void contarPulsoM4() {
|
||
|
|
instanceM4->pulso_hall = true;
|
||
|
|
}
|
||
|
|
|
||
|
|
TaskHandle_t RPMTaskHandle = NULL;
|
||
|
|
static void RPMTaskWrapper(void *pvParameters) {
|
||
|
|
Motor *motor = static_cast<Motor*>(pvParameters);
|
||
|
|
motor->RPMTask();
|
||
|
|
}
|
||
|
|
|
||
|
|
void RPMTask() {
|
||
|
|
unsigned long firstMilis = millis();
|
||
|
|
while (1)
|
||
|
|
{
|
||
|
|
// Medir RPM apenas quando ocorrer um pulso
|
||
|
|
if (pulso_hall) {
|
||
|
|
// Calcular o período em microssegundos
|
||
|
|
unsigned long tempoAtual = micros();
|
||
|
|
periodo = tempoAtual - tempoAnterior;
|
||
|
|
|
||
|
|
// Calcular RPM
|
||
|
|
if (periodo != 0) {
|
||
|
|
RPM = 60000000 / (40 * periodo); // 40 pulsos por revolução
|
||
|
|
} else {
|
||
|
|
RPM = 0; // Lidar com divisão por zero
|
||
|
|
}
|
||
|
|
|
||
|
|
// Reiniciar variáveis
|
||
|
|
tempoAnterior = tempoAtual;
|
||
|
|
pulso_hall = false;
|
||
|
|
}
|
||
|
|
|
||
|
|
long intMillis = (millis() - firstMilis);
|
||
|
|
|
||
|
|
if (intMillis >= _TaxaAmostragem) {
|
||
|
|
EnviarDadosSerial(MontarProtocoloSensor(sRPM, _descricao, MontarDadosProtocoloRPM(RPM, _Potencia)));
|
||
|
|
firstMilis = millis();
|
||
|
|
CorrigePotenciaMotor();
|
||
|
|
}
|
||
|
|
|
||
|
|
vTaskDelay(1);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
void CorrigePotenciaMotor() {
|
||
|
|
if (((RPM + _Margem) < _RPM_SP) || ((RPM - _Margem) > _RPM_SP)) {
|
||
|
|
int PA = PotenciaAcrescentar();
|
||
|
|
_Potencia += PA;
|
||
|
|
if (_Potencia > 100) {
|
||
|
|
_Potencia = 100;
|
||
|
|
}
|
||
|
|
else if (_Potencia < 1) {
|
||
|
|
_Potencia = 1;
|
||
|
|
}
|
||
|
|
_RampaAtivada = true;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
int PotenciaAcrescentar() {
|
||
|
|
if (((RPM + _Margem) > _RPM_SP) && ((RPM - _Margem) < _RPM_SP)) {
|
||
|
|
return 0;
|
||
|
|
}
|
||
|
|
|
||
|
|
float PercentualDistancia = (_RPM_SP / (RPM == 0 ? 1 : RPM));
|
||
|
|
float FatorDivisor = 1; // (_TaxaAmostragem / 1000) < 1 ? 1 : (_TaxaAmostragem / 1000);
|
||
|
|
float AcrescimoPotencia = (PercentualDistancia * _Potencia) - _Potencia;
|
||
|
|
float AcrescimoPotenciaCorrigido = round(AcrescimoPotencia / FatorDivisor);
|
||
|
|
return AcrescimoPotenciaCorrigido;
|
||
|
|
}
|
||
|
|
|
||
|
|
//1000;010;2;070;004;005
|
||
|
|
//1000;010;0;070;004;005
|
||
|
|
|
||
|
|
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 = PotenciaMax / 100;
|
||
|
|
while (1)
|
||
|
|
{
|
||
|
|
unsigned long currentMillis = millis();
|
||
|
|
if (currentMillis - previousMillisRampa >= _Rampa && _RampaAtivada) {
|
||
|
|
previousMillisRampa = currentMillis;
|
||
|
|
|
||
|
|
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 < (_Potencia * MultiplicadorPotencia)) { // abaixo do set point
|
||
|
|
PotenciaAtual += _Precisao; // Incrementar para atingir o set point
|
||
|
|
if (PotenciaAtual > PotenciaMax)
|
||
|
|
PotenciaAtual = PotenciaMax;
|
||
|
|
}
|
||
|
|
else { // Estabiliza a potência do motor
|
||
|
|
if (_Sentido == Parado) {
|
||
|
|
PotenciaAtual = ZeroRampa;
|
||
|
|
_Potencia = PotenciaAtual;
|
||
|
|
}
|
||
|
|
else {
|
||
|
|
PotenciaAtual = (PotenciaAtual > PotenciaMax) ? PotenciaMax : (_Potencia * MultiplicadorPotencia);
|
||
|
|
}
|
||
|
|
_SentidoA = _Sentido;
|
||
|
|
Limite = true;
|
||
|
|
}
|
||
|
|
|
||
|
|
Atualizar();
|
||
|
|
|
||
|
|
if (Limite) {
|
||
|
|
_RampaAtivada = false;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
vTaskDelay(1);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
};
|
||
|
|
|
||
|
|
Motor M1("ET", 0, 13, 35, 12, 27, 22);
|
||
|
|
Motor M2("EF", 1, 14, 34, 33, 25, 32);
|
||
|
|
Motor M3("DT", 2, 15, 39, 02, 00, 04);
|
||
|
|
Motor M4("DF", 3, 05, 36, 18, 19, 21);
|
||
|
|
|
||
|
|
Motor* Motor::instanceM1 = &M1;
|
||
|
|
Motor* Motor::instanceM2 = &M2;
|
||
|
|
Motor* Motor::instanceM3 = &M3;
|
||
|
|
Motor* Motor::instanceM4 = &M4;
|
||
|
|
|
||
|
|
void setup() {
|
||
|
|
Serial.begin(115200);
|
||
|
|
pinMode(PinoReleGeral, OUTPUT);
|
||
|
|
digitalWrite(PinoReleGeral, LOW);
|
||
|
|
}
|
||
|
|
|
||
|
|
void loop() {
|
||
|
|
if (Serial.available() > 3) {
|
||
|
|
//Serial.println();
|
||
|
|
String Protocolo = "";
|
||
|
|
//1000;080;1;020;001
|
||
|
|
F_Code _funcao = Nda;
|
||
|
|
while (Serial.available()) {
|
||
|
|
char Entrada = (char)Serial.read();
|
||
|
|
//Serial.print(Entrada);
|
||
|
|
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 = "";
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
if (_funcao == Chk) {
|
||
|
|
Serial.println((String)D_Code + (String)_funcao);
|
||
|
|
Serial.flush();
|
||
|
|
}
|
||
|
|
else if (_funcao == Cfg) {
|
||
|
|
Conectado = (String)Protocolo[0] == "1";
|
||
|
|
_TaxaAmostragem = ((String)Protocolo[2] + (String)Protocolo[3] + (String)Protocolo[4] + (String)Protocolo[5] + (String)Protocolo[6]).toInt();
|
||
|
|
M1_Ativado = (String)Protocolo[8] == "1";
|
||
|
|
M2_Ativado = (String)Protocolo[10] == "1";
|
||
|
|
M3_Ativado = (String)Protocolo[12] == "1";
|
||
|
|
M4_Ativado = (String)Protocolo[14] == "1";
|
||
|
|
double Kp = ((String)Protocolo[16] + (String)Protocolo[17] + (String)Protocolo[18] + (String)Protocolo[19] + (String)Protocolo[20]).toDouble();
|
||
|
|
double Ki = ((String)Protocolo[22] + (String)Protocolo[23] + (String)Protocolo[24] + (String)Protocolo[25] + (String)Protocolo[26]).toDouble();
|
||
|
|
double Kd = ((String)Protocolo[28] + (String)Protocolo[29] + (String)Protocolo[30] + (String)Protocolo[31] + (String)Protocolo[32]).toDouble();
|
||
|
|
if (M1_Ativado) {
|
||
|
|
if (Conectado) {
|
||
|
|
M1._Kp = Kp;
|
||
|
|
M1._Ki = Ki;
|
||
|
|
M1._Kd = Kd;
|
||
|
|
M1.Inicializar();
|
||
|
|
}
|
||
|
|
else {
|
||
|
|
M1.Desligar();
|
||
|
|
}
|
||
|
|
vTaskDelay(500);
|
||
|
|
}
|
||
|
|
if (M2_Ativado) {
|
||
|
|
if (Conectado) {
|
||
|
|
M2._Kp = Kp;
|
||
|
|
M2._Ki = Ki;
|
||
|
|
M2._Kd = Kd;
|
||
|
|
M2.Inicializar();
|
||
|
|
}
|
||
|
|
else {
|
||
|
|
M2.Desligar();
|
||
|
|
}
|
||
|
|
vTaskDelay(500);
|
||
|
|
}
|
||
|
|
if (M3_Ativado) {
|
||
|
|
if (Conectado) {
|
||
|
|
M3._Kp = Kp;
|
||
|
|
M3._Ki = Ki;
|
||
|
|
M3._Kd = Kd;
|
||
|
|
M3.Inicializar();
|
||
|
|
}
|
||
|
|
else {
|
||
|
|
M3.Desligar();
|
||
|
|
}
|
||
|
|
vTaskDelay(500);
|
||
|
|
}
|
||
|
|
if (M4_Ativado) {
|
||
|
|
if (Conectado) {
|
||
|
|
M4._Kp = Kp;
|
||
|
|
M4._Ki = Ki;
|
||
|
|
M4._Kd = Kd;
|
||
|
|
M4.Inicializar();
|
||
|
|
}
|
||
|
|
else {
|
||
|
|
M4.Desligar();
|
||
|
|
}
|
||
|
|
vTaskDelay(500);
|
||
|
|
}
|
||
|
|
|
||
|
|
digitalWrite(PinoReleGeral, Conectado);
|
||
|
|
|
||
|
|
vTaskDelay(100);
|
||
|
|
|
||
|
|
String Protocolo = BeginSensor + ((String)sCfg) + "ET;" + (String)Conectado;
|
||
|
|
Serial.println(Protocolo);
|
||
|
|
Serial.flush();
|
||
|
|
}
|
||
|
|
else if (_funcao == Cmd) {
|
||
|
|
Serial.println(Protocolo);
|
||
|
|
//canal;potencia;sentido;rampa;precisao;rpm
|
||
|
|
//0;000;0;000;000;000
|
||
|
|
int Canal = ((String)Protocolo[0]).toInt();
|
||
|
|
|
||
|
|
Motor* _motor = Canal == 0 ? &M1 : Canal == 1 ? &M2 : Canal == 2 ? &M3 : &M4;
|
||
|
|
|
||
|
|
_motor->_Potencia = ((String)Protocolo[2] + (String)Protocolo[3] + (String)Protocolo[4]).toInt();
|
||
|
|
_motor->_Sentido = (Sentido)((String)Protocolo[6]).toInt();
|
||
|
|
_motor->_Rampa = ((String)Protocolo[8] + (String)Protocolo[9] + (String)Protocolo[10]).toInt();
|
||
|
|
_motor->_Precisao = ((String)Protocolo[12] + (String)Protocolo[13] + (String)Protocolo[14]).toInt();
|
||
|
|
_motor->_RPM_SP = ((String)Protocolo[16] + (String)Protocolo[17] + (String)Protocolo[18]).toInt();
|
||
|
|
|
||
|
|
_motor->_Kp = ((String)Protocolo[20] + (String)Protocolo[21] + (String)Protocolo[22] + (String)Protocolo[23] + (String)Protocolo[24]).toDouble();
|
||
|
|
_motor->_Ki = ((String)Protocolo[26] + (String)Protocolo[27] + (String)Protocolo[28] + (String)Protocolo[29] + (String)Protocolo[30]).toDouble();
|
||
|
|
_motor->_Kd = ((String)Protocolo[32] + (String)Protocolo[33] + (String)Protocolo[34] + (String)Protocolo[35] + (String)Protocolo[36]).toDouble();
|
||
|
|
|
||
|
|
_motor->_RampaAtivada = true;
|
||
|
|
}
|
||
|
|
else if (_funcao == Tst) {
|
||
|
|
int Canal = ((String)Protocolo[0]).toInt();
|
||
|
|
|
||
|
|
Motor* _motor = Canal == 0 ? &M1 : Canal == 1 ? &M2 : Canal == 2 ? &M3 : &M4;
|
||
|
|
|
||
|
|
bool EmTeste = _motor->Testando;
|
||
|
|
if (!EmTeste) {
|
||
|
|
_motor->Testar();
|
||
|
|
}
|
||
|
|
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
delay(10);
|
||
|
|
}
|