615 lines
19 KiB
Arduino
615 lines
19 KiB
Arduino
|
#include "C:\Zendion Inc\agrobot_base\Firmware\Modulos\SerialService.h"
|
||
|
|
#define D_Code Mov
|
||
|
|
|
||
|
|
long _baudRate = 115200;
|
||
|
|
|
||
|
|
bool Conectado = false;
|
||
|
|
|
||
|
|
int _TaxaAmostragem;
|
||
|
|
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 RampaMax = 1023;
|
||
|
|
|
||
|
|
class Motor {
|
||
|
|
public:
|
||
|
|
// Construtor
|
||
|
|
Motor(String desc) {
|
||
|
|
_ID = desc;
|
||
|
|
}
|
||
|
|
|
||
|
|
// Definições
|
||
|
|
String _ID;
|
||
|
|
int _canal;
|
||
|
|
int _pinoPWM;
|
||
|
|
int _pinoVEL;
|
||
|
|
int _pinoDIR;
|
||
|
|
int _pinoBRK;
|
||
|
|
int _pinoSTP;
|
||
|
|
|
||
|
|
bool Iniciado = false;
|
||
|
|
bool Testando = false;
|
||
|
|
|
||
|
|
// Consumo
|
||
|
|
volatile float RPM;
|
||
|
|
int PotenciaAtual;
|
||
|
|
volatile int ultimaLeituraHall = 0;
|
||
|
|
volatile unsigned int tempoAnterior = 0;
|
||
|
|
volatile float periodo = 0;
|
||
|
|
|
||
|
|
// Entrada de dados
|
||
|
|
bool _FOC;
|
||
|
|
int _Rampa;
|
||
|
|
int _Precisao;
|
||
|
|
double _Potencia;
|
||
|
|
double _RPM_SP;
|
||
|
|
Sentido _Sentido = Parado;
|
||
|
|
Sentido _SentidoA = Parado;
|
||
|
|
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, RampaMin);
|
||
|
|
}
|
||
|
|
if (_pinoVEL > -1) {
|
||
|
|
pinMode(_pinoVEL, INPUT);
|
||
|
|
ultimaLeituraHall = digitalRead(_pinoVEL);
|
||
|
|
RPM = 0;
|
||
|
|
periodo = 0;
|
||
|
|
tempoAnterior = 0;
|
||
|
|
|
||
|
|
xTaskCreatePinnedToCore(&Motor::RPMTaskWrapper, "RPMTask", 5000, this, _canal, &RPMTaskHandle, 0);
|
||
|
|
}
|
||
|
|
if (_pinoDIR > -1) {
|
||
|
|
pinMode(_pinoDIR, OUTPUT);
|
||
|
|
digitalWrite(_pinoDIR, HIGH);
|
||
|
|
}
|
||
|
|
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;
|
||
|
|
_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() {
|
||
|
|
if (_pinoDIR > -1) {
|
||
|
|
digitalWrite(_pinoDIR, _SentidoA == Horario);
|
||
|
|
}
|
||
|
|
if (_pinoBRK > -1) {
|
||
|
|
digitalWrite(_pinoBRK, _Freio);
|
||
|
|
}
|
||
|
|
if (_pinoSTP > -1) {
|
||
|
|
digitalWrite(_pinoSTP, _SentidoA != Parado);
|
||
|
|
}
|
||
|
|
if (_pinoPWM > -1) {
|
||
|
|
ledcWrite(_canal, PotenciaAtual);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
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 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;
|
||
|
|
}
|
||
|
|
|
||
|
|
// 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;
|
||
|
|
}
|
||
|
|
pulsos = 0;
|
||
|
|
|
||
|
|
EnviarDadosSerial(MontarProtocoloSensor(sRPM, _ID, MontarDadosProtocoloRPM(RPM, _Potencia, periodo)));
|
||
|
|
firstMilis = millis();
|
||
|
|
CorrigePotenciaMotor();
|
||
|
|
}
|
||
|
|
|
||
|
|
// Aguarda metade do menor período possível entre leituras
|
||
|
|
vTaskDelay(1);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
void CorrigePotenciaMotor() {
|
||
|
|
if (_FOC && (((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 = _Precisao; // (_TaxaAmostragem / 1000) < 1 ? 1 : (_TaxaAmostragem / 1000);
|
||
|
|
float AcrescimoPotencia = (PercentualDistancia * _Potencia) - _Potencia;
|
||
|
|
if (AcrescimoPotencia > 100) {
|
||
|
|
AcrescimoPotencia = 100;
|
||
|
|
}
|
||
|
|
else if (AcrescimoPotencia < -100) {
|
||
|
|
AcrescimoPotencia = -100;
|
||
|
|
}
|
||
|
|
float AcrescimoPotenciaCorrigido = round(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 && _RampaAtivada) {
|
||
|
|
previousMillisRampa = currentMillis;
|
||
|
|
|
||
|
|
bool Limite = false;
|
||
|
|
|
||
|
|
if ((_Sentido == Parado || _Sentido != _SentidoA) && PotenciaAtual > RampaMin) { // M1 estiver parando ou invertendo, e pwm for maior zero rampa
|
||
|
|
PotenciaAtual -= _Precisao; // Decrementar para atingir ponto zero rampa
|
||
|
|
if (PotenciaAtual < RampaMin)
|
||
|
|
PotenciaAtual = RampaMin;
|
||
|
|
}
|
||
|
|
else if (_Sentido != _SentidoA && PotenciaAtual == RampaMin) { // 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 > RampaMax)
|
||
|
|
PotenciaAtual = RampaMax;
|
||
|
|
}
|
||
|
|
else { // Estabiliza a potência do motor
|
||
|
|
if (_Sentido == Parado) {
|
||
|
|
PotenciaAtual = RampaMin;
|
||
|
|
_Potencia = PotenciaAtual;
|
||
|
|
}
|
||
|
|
else {
|
||
|
|
PotenciaAtual = (PotenciaAtual > RampaMax) ? RampaMax : (_Potencia * MultiplicadorPotencia);
|
||
|
|
}
|
||
|
|
_SentidoA = _Sentido;
|
||
|
|
Limite = true;
|
||
|
|
}
|
||
|
|
|
||
|
|
Atualizar();
|
||
|
|
|
||
|
|
if (Limite) {
|
||
|
|
_RampaAtivada = false;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
// Aguarda metade do tempo de rampa para realizar a próxima verificação
|
||
|
|
int wait = 10; // (_Rampa / 2.0);
|
||
|
|
vTaskDelay(wait);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
};
|
||
|
|
|
||
|
|
Motor M1("ET");
|
||
|
|
Motor M2("EF");
|
||
|
|
Motor M3("DT");
|
||
|
|
Motor M4("DF");
|
||
|
|
|
||
|
|
void setup() {
|
||
|
|
Serial.begin(_baudRate);
|
||
|
|
|
||
|
|
TaskHandle_t SerialTaskHandle = NULL;
|
||
|
|
xTaskCreatePinnedToCore(SerialTask, "SerialTask", 4000, NULL, 20, &SerialTaskHandle, 1);
|
||
|
|
}
|
||
|
|
|
||
|
|
void loop() {
|
||
|
|
float x = 1509 / 300;
|
||
|
|
/*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") {
|
||
|
|
int _RampaMin = ((String)Protocolo[8] + (String)Protocolo[9] + (String)Protocolo[10] + (String)Protocolo[11]).toInt();
|
||
|
|
int _RampaMax = ((String)Protocolo[13] + (String)Protocolo[14] + (String)Protocolo[15] + (String)Protocolo[16]).toInt();
|
||
|
|
|
||
|
|
RampaMin = _RampaMin;
|
||
|
|
RampaMax = _RampaMax;
|
||
|
|
|
||
|
|
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";
|
||
|
|
_TaxaAmostragem = ((String)Protocolo[9] + (String)Protocolo[10] + (String)Protocolo[11] + (String)Protocolo[12] + (String)Protocolo[13]).toInt();
|
||
|
|
bool motor_ativado = (String)Protocolo[15] == "1";
|
||
|
|
int pwm = ((String)Protocolo[17] + (String)Protocolo[18]).toInt();
|
||
|
|
int vel = ((String)Protocolo[20] + (String)Protocolo[21]).toInt();
|
||
|
|
int dir = ((String)Protocolo[23] + (String)Protocolo[24]).toInt();
|
||
|
|
int brk = ((String)Protocolo[26] + (String)Protocolo[27]).toInt();
|
||
|
|
int stp = ((String)Protocolo[29] + (String)Protocolo[30]).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->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 = ((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();
|
||
|
|
bool _foc = (String)Protocolo[21] == "1";
|
||
|
|
_motor->_FOC = _foc;
|
||
|
|
_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();
|
||
|
|
}
|
||
|
|
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
delay(10);*/
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
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") {
|
||
|
|
int _RampaMin = ((String)Protocolo[8] + (String)Protocolo[9] + (String)Protocolo[10] + (String)Protocolo[11]).toInt();
|
||
|
|
int _RampaMax = ((String)Protocolo[13] + (String)Protocolo[14] + (String)Protocolo[15] + (String)Protocolo[16]).toInt();
|
||
|
|
int _PPR = ((String)Protocolo[18] + (String)Protocolo[19] + (String)Protocolo[20]).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";
|
||
|
|
_TaxaAmostragem = ((String)Protocolo[9] + (String)Protocolo[10] + (String)Protocolo[11] + (String)Protocolo[12] + (String)Protocolo[13]).toInt();
|
||
|
|
bool motor_ativado = (String)Protocolo[15] == "1";
|
||
|
|
int pwm = ((String)Protocolo[17] + (String)Protocolo[18]).toInt();
|
||
|
|
int vel = ((String)Protocolo[20] + (String)Protocolo[21]).toInt();
|
||
|
|
int dir = ((String)Protocolo[23] + (String)Protocolo[24]).toInt();
|
||
|
|
int brk = ((String)Protocolo[26] + (String)Protocolo[27]).toInt();
|
||
|
|
int stp = ((String)Protocolo[29] + (String)Protocolo[30]).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->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 = ((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();
|
||
|
|
bool _foc = (String)Protocolo[21] == "1";
|
||
|
|
_motor->_FOC = _foc;
|
||
|
|
_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));
|
||
|
|
}
|
||
|
|
}
|