598 lines
17 KiB
Arduino
598 lines
17 KiB
Arduino
|
// Dispositivo: Direcional
|
||
|
|
// Versão Firmware: 6
|
||
|
|
// Ultima atualização: 23/04/2024
|
||
|
|
// Atualização: Adicionado enfileiramento de mensageria
|
||
|
|
|
||
|
|
#include "C:\ZendionInc\agrobot_base\Firmware\Modulos\SerialService.h"
|
||
|
|
#include "C:\ZendionInc\agrobot_base\Firmware\Modulos\Pinout.h"
|
||
|
|
#define D_Code Dir
|
||
|
|
#define VERSION 6
|
||
|
|
|
||
|
|
#define _pinoLED RGB_BUILTIN
|
||
|
|
|
||
|
|
long _baudRate = 115200;
|
||
|
|
|
||
|
|
bool Conectado = false;
|
||
|
|
bool AA_Ref000 = false;
|
||
|
|
bool AA_Ref090 = false;
|
||
|
|
bool AA_Ref270 = false;
|
||
|
|
|
||
|
|
int _TaxaAmostragem;
|
||
|
|
int FrequenciaMaxima = 10000;
|
||
|
|
double _Margem = 0.5;
|
||
|
|
|
||
|
|
int dutyCycle = 512;
|
||
|
|
double _frequencia = 4000;
|
||
|
|
|
||
|
|
PinoModel _pinoGeral;
|
||
|
|
|
||
|
|
class Motor {
|
||
|
|
public:
|
||
|
|
// Construtor
|
||
|
|
Motor(String desc) {
|
||
|
|
_ID = desc;
|
||
|
|
}
|
||
|
|
|
||
|
|
// Definições
|
||
|
|
String _ID;
|
||
|
|
int _canal;
|
||
|
|
PinoModel _pinoENA;
|
||
|
|
PinoModel _pinoDIR;
|
||
|
|
PinoModel _pinoPUL;
|
||
|
|
PinoModel _pinoEncoderA;
|
||
|
|
PinoModel _pinoEncoderB;
|
||
|
|
PinoModel _pinoRef000;
|
||
|
|
PinoModel _pinoRef090;
|
||
|
|
PinoModel _pinoRef270;
|
||
|
|
|
||
|
|
bool Iniciado = false;
|
||
|
|
bool Testando = false;
|
||
|
|
bool Referenciando = false;
|
||
|
|
|
||
|
|
// Consumo
|
||
|
|
double _Angulo;
|
||
|
|
double _AgRef;
|
||
|
|
Sentido _Sentido;
|
||
|
|
|
||
|
|
// Entrada de dados
|
||
|
|
double _Angulo_SP;
|
||
|
|
double _Angulo_Offset = 0;
|
||
|
|
Sentido _Sentido_SP = Parado;
|
||
|
|
bool _MotorLigado = false;
|
||
|
|
bool _Estabilizar = true;
|
||
|
|
bool _Referenciar = true;
|
||
|
|
|
||
|
|
bool E0 = false;
|
||
|
|
bool Ag0 = false;
|
||
|
|
bool _Ag0 = false;
|
||
|
|
bool E90 = false;
|
||
|
|
bool Ag90 = false;
|
||
|
|
bool _Ag90 = false;
|
||
|
|
bool E270 = false;
|
||
|
|
bool Ag270 = false;
|
||
|
|
bool _Ag270 = false;
|
||
|
|
bool Referenciado = false;
|
||
|
|
int RefMax = 90;
|
||
|
|
int RefMin = -90;
|
||
|
|
int ulEncoderA = 0;
|
||
|
|
int ulEncoderB = 0;
|
||
|
|
double GrausPino = 0;
|
||
|
|
bool EA_M = false;
|
||
|
|
bool EB_M = false;
|
||
|
|
|
||
|
|
|
||
|
|
void Inicializar() {
|
||
|
|
if (Iniciado) {
|
||
|
|
EnviarDadosSerial(_ID + " ja inicializado");
|
||
|
|
EnviarDadosSerial(MontarProtocoloSensor(sCFG, _ID, Iniciado ? "1" : "0"));
|
||
|
|
return;
|
||
|
|
}
|
||
|
|
|
||
|
|
if (_pinoPUL.Definido()) {
|
||
|
|
AtualizarPWM();
|
||
|
|
}
|
||
|
|
_pinoENA.Conectar(true);
|
||
|
|
_pinoDIR.Conectar(false);
|
||
|
|
_pinoEncoderA.Conectar();
|
||
|
|
ulEncoderA = _pinoEncoderA.get();
|
||
|
|
EA_M = false;
|
||
|
|
_pinoEncoderB.Conectar();
|
||
|
|
ulEncoderB = _pinoEncoderB.get();
|
||
|
|
EB_M = false;
|
||
|
|
_pinoRef000.Conectar();
|
||
|
|
Ag0 = _pinoRef000.get() == 1;
|
||
|
|
_Ag0 = Ag0;
|
||
|
|
_pinoRef090.Conectar();
|
||
|
|
Ag90 = _pinoRef090.get() == 1;
|
||
|
|
_Ag90 = Ag90;
|
||
|
|
_pinoRef270.Conectar();
|
||
|
|
Ag270 = _pinoRef270.get() == 1;
|
||
|
|
_Ag270 = Ag270;
|
||
|
|
|
||
|
|
xTaskCreatePinnedToCore(&Motor::RampaTaskWrapper, "RampaTask", 5000, this, 20 - _canal, &RampaTaskHandle, tskNO_AFFINITY);
|
||
|
|
|
||
|
|
|
||
|
|
E0 = false;
|
||
|
|
E90 = false;
|
||
|
|
E270 = false;
|
||
|
|
|
||
|
|
ResetRef(_Referenciar);
|
||
|
|
|
||
|
|
EnviarDadosSerial(_ID + " Iniciado");
|
||
|
|
|
||
|
|
Iniciado = true;
|
||
|
|
|
||
|
|
EnviarDadosSerial(MontarProtocoloSensor(sCFG, _ID, Iniciado ? "1" : "0"));
|
||
|
|
}
|
||
|
|
|
||
|
|
void ResetRef(bool _Ref) {
|
||
|
|
_Angulo = 0;
|
||
|
|
GrausPino = 0;
|
||
|
|
_Sentido_SP = Parado;
|
||
|
|
_Sentido = Parado;
|
||
|
|
Referenciado = !_Ref;
|
||
|
|
Referenciando = _Ref;
|
||
|
|
_MotorLigado = _Ref;
|
||
|
|
}
|
||
|
|
|
||
|
|
void AtualizarPWM() {
|
||
|
|
ledcSetup(_canal, _frequencia, 10);
|
||
|
|
ledcAttachPin(_pinoPUL.num, _canal);
|
||
|
|
ledcWrite(_canal, dutyCycle);
|
||
|
|
}
|
||
|
|
|
||
|
|
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);
|
||
|
|
|
||
|
|
// Redefinir as configurações para os valores iniciais
|
||
|
|
_pinoENA.Desconectar();
|
||
|
|
_pinoDIR.Desconectar();
|
||
|
|
_pinoPUL.Desconectar();
|
||
|
|
_pinoEncoderA.Desconectar();
|
||
|
|
_pinoEncoderB.Desconectar();
|
||
|
|
_pinoRef000.Desconectar();
|
||
|
|
_pinoRef090.Desconectar();
|
||
|
|
_pinoRef270.Desconectar();
|
||
|
|
|
||
|
|
|
||
|
|
EnviarDadosSerial(_ID + " Desligado");
|
||
|
|
|
||
|
|
Iniciado = false;
|
||
|
|
|
||
|
|
EnviarDadosSerial(MontarProtocoloSensor(sCFG, _ID, Iniciado ? "1" : "0"));
|
||
|
|
}
|
||
|
|
|
||
|
|
void Testar() {
|
||
|
|
Testando = true;
|
||
|
|
|
||
|
|
Testando = false;
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
private:
|
||
|
|
|
||
|
|
TaskHandle_t RampaTaskHandle = NULL;
|
||
|
|
static void RampaTaskWrapper(void *pvParameters) {
|
||
|
|
Motor *motor = static_cast<Motor*>(pvParameters);
|
||
|
|
motor->RampaTask();
|
||
|
|
}
|
||
|
|
|
||
|
|
void RampaTask() {
|
||
|
|
unsigned long previousMillisRampa = millis();
|
||
|
|
unsigned long previousMillisAngulo = 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;
|
||
|
|
}
|
||
|
|
|
||
|
|
AferirPosicao();
|
||
|
|
|
||
|
|
if (Referenciando) {
|
||
|
|
ReferenciarMotor();
|
||
|
|
}
|
||
|
|
else {
|
||
|
|
VerificarAnguloSP();
|
||
|
|
}
|
||
|
|
|
||
|
|
Atualizar();
|
||
|
|
|
||
|
|
AferirAnguloAbsoluto();
|
||
|
|
|
||
|
|
unsigned long currentMillis = millis();
|
||
|
|
/*if (currentMillis - previousMillisAngulo >= 100) {
|
||
|
|
previousMillisAngulo = currentMillis;
|
||
|
|
|
||
|
|
AferirAnguloAbsoluto();
|
||
|
|
}*/
|
||
|
|
|
||
|
|
if (currentMillis - previousMillisRampa >= _TaxaAmostragem) {
|
||
|
|
previousMillisRampa = currentMillis;
|
||
|
|
|
||
|
|
EnviarDadosSerial(MontarProtocoloSensor(sENC, _ID, MontarDadosProtocoloEncoder(_Sentido, _Angulo, Ag0, Ag90, Ag270, Referenciando, Referenciado)));
|
||
|
|
}
|
||
|
|
|
||
|
|
vTaskDelay(1);
|
||
|
|
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
void Atualizar() {
|
||
|
|
_pinoENA.set(!_MotorLigado);
|
||
|
|
_pinoDIR.set(_Sentido_SP != Horario);
|
||
|
|
}
|
||
|
|
|
||
|
|
void AferirPosicao() {
|
||
|
|
if (_pinoEncoderA.Definido() && _pinoEncoderB.Definido()) {
|
||
|
|
int leituraEncoderA = _pinoEncoderA.get();
|
||
|
|
int leituraEncoderB = _pinoEncoderB.get();
|
||
|
|
|
||
|
|
if (!EA_M) {
|
||
|
|
EA_M = leituraEncoderA != ulEncoderA;
|
||
|
|
if (EA_M) {
|
||
|
|
_Sentido = leituraEncoderA != leituraEncoderB ? Horario : Antihorario;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
if (!EB_M) {
|
||
|
|
EB_M = leituraEncoderB != ulEncoderB;
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
if (EA_M && EB_M) {
|
||
|
|
|
||
|
|
if (_Sentido == Horario) {
|
||
|
|
_Angulo = _Angulo + _Margem;
|
||
|
|
_AgRef = _AgRef + _Margem;
|
||
|
|
} else if (_Sentido == Antihorario) {
|
||
|
|
_Angulo = _Angulo - _Margem;
|
||
|
|
_AgRef = _AgRef - _Margem;
|
||
|
|
}
|
||
|
|
|
||
|
|
EA_M = false;
|
||
|
|
EB_M = false;
|
||
|
|
}
|
||
|
|
|
||
|
|
ulEncoderA = leituraEncoderA;
|
||
|
|
ulEncoderB = leituraEncoderB;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
void AferirAnguloAbsoluto() {
|
||
|
|
_Ag0 = _pinoRef000.get() == 1;
|
||
|
|
_Ag90 = _pinoRef090.get() == 1;
|
||
|
|
_Ag270 = _pinoRef270.get() == 1;
|
||
|
|
|
||
|
|
if (GrausPino == 0) {
|
||
|
|
if (_pinoRef000.Definido()) {
|
||
|
|
if (_Ag0 == true && Ag0 == false && E0 == false) { // entrou no pino 0
|
||
|
|
_AgRef = 0;
|
||
|
|
//Serial.println("Entrou 0");
|
||
|
|
E0 = true;
|
||
|
|
}
|
||
|
|
else if (_Ag0 == false && Ag0 == true && _AgRef != 0 && E0 == true) { // saiu do pino 0
|
||
|
|
GrausPino = (_AgRef < 0 ? -1.0 : 1.0) * _AgRef;
|
||
|
|
Referenciado = true;
|
||
|
|
//Serial.println("Saiu 0");
|
||
|
|
//Serial.println(GrausPino);
|
||
|
|
E0 = false;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
if (_pinoRef090.Definido()) {
|
||
|
|
if (_Ag90 == true && Ag90 == false && E90 == false) { // entrou no pino 90
|
||
|
|
_AgRef = 0;
|
||
|
|
//Serial.println("Entrou 90");
|
||
|
|
E90 = true;
|
||
|
|
}
|
||
|
|
else if (_Ag90 == false && Ag90 == true && E90 == true) { // saiu do pino 90
|
||
|
|
GrausPino = (_AgRef < 0 ? -1.0 : 1.0) * _AgRef;
|
||
|
|
Referenciado = true;
|
||
|
|
//Serial.println("Saiu 90");
|
||
|
|
E90 = false;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
if (_pinoRef270.Definido()) {
|
||
|
|
if (_Ag270 == true && Ag270 == false && E270 == false) { // entrou no pino 270
|
||
|
|
_AgRef = 0;
|
||
|
|
//Serial.println("Entrou 270");
|
||
|
|
E270 = true;
|
||
|
|
}
|
||
|
|
else if (_Ag270 == false && Ag270 == true) { // saiu do pino 270
|
||
|
|
GrausPino = (_AgRef < 0 ? -1.0 : 1.0) * _AgRef;
|
||
|
|
Referenciado = true;
|
||
|
|
//Serial.println("Saiu 270");
|
||
|
|
E270 = false;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
if (GrausPino > 0) {
|
||
|
|
if (Referenciando || AA_Ref000) {
|
||
|
|
if (_Ag0 == true && Ag0 == false) { // entrou no pino 0
|
||
|
|
_Angulo = CalculaAnguloPino(0, true);
|
||
|
|
}
|
||
|
|
else if (_Ag0 == false && Ag0 == true) { // saiu do pino 0
|
||
|
|
_Angulo = CalculaAnguloPino(0, false);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
else if (Referenciando || AA_Ref090) {
|
||
|
|
if (_Ag90 == true && Ag90 == false) { // entrou no pino 90
|
||
|
|
_Angulo = CalculaAnguloPino(90, true);
|
||
|
|
}
|
||
|
|
else if (_Ag90 == false && Ag90 == true) { // saiu do pino 90
|
||
|
|
_Angulo = CalculaAnguloPino(90, false);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
else if (Referenciando || AA_Ref270) {
|
||
|
|
if (_Ag270 == true && Ag270 == false) { // entrou no pino 270
|
||
|
|
_Angulo = CalculaAnguloPino(-90, true);
|
||
|
|
}
|
||
|
|
else if (_Ag270 == false && Ag270 == true) { // saiu do pino 270
|
||
|
|
_Angulo = CalculaAnguloPino(-90, false);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
if (_pinoRef000.Definido()) {
|
||
|
|
Ag0 = _Ag0;
|
||
|
|
}
|
||
|
|
if (_pinoRef090.Definido()) {
|
||
|
|
Ag90 = _Ag90;
|
||
|
|
}
|
||
|
|
if (_pinoRef270.Definido()) {
|
||
|
|
Ag270 = _Ag270;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
double CalculaAnguloPino(double Inicio, bool Entrando) {
|
||
|
|
double MeioPino = (GrausPino / 2.0);
|
||
|
|
double Multiplicador = 1.0;
|
||
|
|
if (Entrando) {
|
||
|
|
Multiplicador = (_Sentido == Horario ? -1.0 : 1.0);
|
||
|
|
}
|
||
|
|
else {
|
||
|
|
Multiplicador = (_Sentido == Horario ? 1.0 : -1.0);
|
||
|
|
}
|
||
|
|
double Angulo = Inicio + (MeioPino * Multiplicador) + _Angulo_Offset;
|
||
|
|
return Angulo;
|
||
|
|
}
|
||
|
|
|
||
|
|
void VerificarAnguloSP() {
|
||
|
|
if (_MotorLigado) {
|
||
|
|
if (_Angulo_SP > -1) {
|
||
|
|
if (_Sentido_SP == Horario && _Angulo >= _Angulo_SP) {
|
||
|
|
_MotorLigado = false;
|
||
|
|
}
|
||
|
|
else if (_Sentido_SP == Antihorario && _Angulo <= (_Angulo_SP * -1)) {
|
||
|
|
_MotorLigado = false;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
else {
|
||
|
|
if (Referenciado && (_Sentido_SP == Horario && _Angulo >= RefMax)) {
|
||
|
|
_MotorLigado = false;
|
||
|
|
}
|
||
|
|
else if (Referenciado && (_Sentido_SP == Antihorario && _Angulo <= RefMin)) {
|
||
|
|
_MotorLigado = false;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
else {
|
||
|
|
if (_Estabilizar && _Sentido_SP == Parado && _Angulo != _Angulo_SP) {
|
||
|
|
_Sentido_SP = _Angulo > 0 ? Antihorario : Horario;
|
||
|
|
_MotorLigado = true;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
void ReferenciarMotor() {
|
||
|
|
int AgP1 = RefMax / 2;
|
||
|
|
int AgP2 = RefMax;
|
||
|
|
|
||
|
|
if (Referenciado && (_Angulo >= -_Margem && _Angulo <= _Margem)) {
|
||
|
|
Referenciando = false;
|
||
|
|
_MotorLigado = false;
|
||
|
|
}
|
||
|
|
else {
|
||
|
|
if (_Sentido_SP == Parado && _Angulo == 0) {
|
||
|
|
_Angulo_SP = AgP1;
|
||
|
|
_Sentido_SP = Horario;
|
||
|
|
}
|
||
|
|
else if (_Sentido_SP == Horario) {
|
||
|
|
if (_Angulo > _Margem && Referenciado) {
|
||
|
|
_Angulo_SP = AgP1;
|
||
|
|
_Sentido_SP = Antihorario;
|
||
|
|
}
|
||
|
|
else if (_Angulo >= AgP1) {
|
||
|
|
_Angulo_SP = AgP1;
|
||
|
|
_Sentido_SP = Antihorario;
|
||
|
|
}
|
||
|
|
else if (_Angulo >= RefMax) {
|
||
|
|
_Angulo_SP = AgP2;
|
||
|
|
_Sentido_SP = Antihorario;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
else if (_Sentido_SP == Antihorario) {
|
||
|
|
if (_Angulo < -_Margem && Referenciado) {
|
||
|
|
_Angulo_SP = AgP1;
|
||
|
|
_Sentido_SP = Horario;
|
||
|
|
}
|
||
|
|
else if (_Angulo <= -AgP1) {
|
||
|
|
_Angulo_SP = AgP2;
|
||
|
|
_Sentido_SP = Antihorario;
|
||
|
|
}
|
||
|
|
else if (_Angulo <= -AgP2) {
|
||
|
|
_Angulo_SP = AgP2;
|
||
|
|
_Sentido_SP = Horario;
|
||
|
|
}
|
||
|
|
else if (_Angulo <= RefMin) {
|
||
|
|
_Angulo_SP = AgP2;
|
||
|
|
_Sentido_SP = Horario;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
};
|
||
|
|
|
||
|
|
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 setup() {
|
||
|
|
Serial.begin(_baudRate);
|
||
|
|
//iniciarMCP(D_Code, 8, 18, 0x20, false);
|
||
|
|
}
|
||
|
|
|
||
|
|
void loop() {
|
||
|
|
ProtocoloSerial protocolos[MAX_PROTOCOLS];
|
||
|
|
int numProtocolos = LerBufferSerialID(protocolos);
|
||
|
|
|
||
|
|
// Processa cada protocolo após a leitura completa
|
||
|
|
for (int i = 0; i < numProtocolos; i++) {
|
||
|
|
ProcessarProtocolo(protocolos[i].protocolo, protocolos[i].funcao, protocolos[i].idMensagem);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
void ProcessarProtocolo(String Protocolo, F_Code _funcao, int idMensagem) {
|
||
|
|
|
||
|
|
if (_funcao == Chk) {
|
||
|
|
EnviarDadosSerial(MontarProtocoloVerificacao(D_Code, VERSION));
|
||
|
|
}
|
||
|
|
|
||
|
|
EnviarDadosSerial(MontarProtocoloMensagemRecebida(idMensagem));
|
||
|
|
|
||
|
|
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();
|
||
|
|
AA_Ref000 = (String)Protocolo[11] == "1";
|
||
|
|
AA_Ref090 = (String)Protocolo[13] == "1";
|
||
|
|
AA_Ref270 = (String)Protocolo[15] == "1";
|
||
|
|
TiposBarramentos _barramento = (TiposBarramentos)((String)Protocolo[17]).toInt();
|
||
|
|
int _num = ((String)Protocolo[18] + (String)Protocolo[19]).toInt();
|
||
|
|
_pinoGeral = PinoModel(_num, _barramento, _OUTPUT);
|
||
|
|
|
||
|
|
if (_pinoGeral.Definido()) {
|
||
|
|
_pinoGeral.set(true);
|
||
|
|
}
|
||
|
|
|
||
|
|
vTaskDelay(pdMS_TO_TICKS(100));
|
||
|
|
|
||
|
|
Conectado = Conectar;
|
||
|
|
|
||
|
|
EnviarDadosSerial(MontarProtocoloSensor(sCFG, "MD", Conectado ? "1" : "0"));
|
||
|
|
}
|
||
|
|
else {
|
||
|
|
Motor* _motor = MotorPorID(ID);
|
||
|
|
|
||
|
|
int canal = ((String)Protocolo[5]).toInt();
|
||
|
|
bool motor_ativado = ((String)Protocolo[7]) == "1";
|
||
|
|
bool estabilizar = ((String)Protocolo[9]) == "1";
|
||
|
|
bool referenciar = ((String)Protocolo[11]) == "1";
|
||
|
|
int _offset = ((String)Protocolo[13] + (String)Protocolo[14] + (String)Protocolo[15] + (String)Protocolo[16]).toInt();
|
||
|
|
TiposBarramentos pulB = (TiposBarramentos)((String)Protocolo[18]).toInt();
|
||
|
|
int pul = ((String)Protocolo[19] + (String)Protocolo[20]).toInt();
|
||
|
|
TiposBarramentos dirB = (TiposBarramentos)((String)Protocolo[22]).toInt();
|
||
|
|
int dir = ((String)Protocolo[23] + (String)Protocolo[24]).toInt();
|
||
|
|
TiposBarramentos enaB = (TiposBarramentos)((String)Protocolo[26]).toInt();
|
||
|
|
int ena = ((String)Protocolo[27] + (String)Protocolo[28]).toInt();
|
||
|
|
TiposBarramentos encAB = (TiposBarramentos)((String)Protocolo[30]).toInt();
|
||
|
|
int encoderA = ((String)Protocolo[31] + (String)Protocolo[32]).toInt();
|
||
|
|
TiposBarramentos encBB = (TiposBarramentos)((String)Protocolo[34]).toInt();
|
||
|
|
int encoderB = ((String)Protocolo[35] + (String)Protocolo[36]).toInt();
|
||
|
|
TiposBarramentos ref0B = (TiposBarramentos)((String)Protocolo[38]).toInt();
|
||
|
|
int ref0 = ((String)Protocolo[39] + (String)Protocolo[40]).toInt();
|
||
|
|
TiposBarramentos ref90B = (TiposBarramentos)((String)Protocolo[42]).toInt();
|
||
|
|
int ref90 = ((String)Protocolo[43] + (String)Protocolo[44]).toInt();
|
||
|
|
TiposBarramentos ref270B = (TiposBarramentos)((String)Protocolo[46]).toInt();
|
||
|
|
int ref270 = ((String)Protocolo[47] + (String)Protocolo[48]).toInt();
|
||
|
|
|
||
|
|
if (motor_ativado) {
|
||
|
|
if (Conectar) {
|
||
|
|
_motor->_canal = canal;
|
||
|
|
_motor->_Estabilizar = estabilizar;
|
||
|
|
_motor->_Referenciar = referenciar;
|
||
|
|
_motor->_Angulo_Offset = _offset;
|
||
|
|
_motor->_pinoPUL = PinoModel(pul, pulB, _PWM);
|
||
|
|
_motor->_pinoDIR = PinoModel(dir, dirB, _OUTPUT);
|
||
|
|
_motor->_pinoENA = PinoModel(ena, enaB, _OUTPUT);
|
||
|
|
_motor->_pinoEncoderA = PinoModel(encoderA, encAB, _INPUT, DIGITAL);
|
||
|
|
_motor->_pinoEncoderB = PinoModel(encoderB, encBB, _INPUT, DIGITAL);
|
||
|
|
_motor->_pinoRef000 = PinoModel(ref0, ref0B, _INPUT, DIGITAL);
|
||
|
|
_motor->_pinoRef090 = PinoModel(ref90, ref90B, _INPUT, DIGITAL);
|
||
|
|
_motor->_pinoRef270 = PinoModel(ref270, ref270B, _INPUT, DIGITAL);
|
||
|
|
_motor->Inicializar();
|
||
|
|
}
|
||
|
|
else {
|
||
|
|
_motor->Desligar();
|
||
|
|
}
|
||
|
|
vTaskDelay(500);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
else if (_funcao == Cmd) {
|
||
|
|
//canal;sentido;angulo
|
||
|
|
//0;0;000.00
|
||
|
|
String ID = ((String)Protocolo[0] + (String)Protocolo[1]);
|
||
|
|
|
||
|
|
Motor* _motor = MotorPorID(ID);
|
||
|
|
|
||
|
|
Sentido sentido = (Sentido)((String)Protocolo[3]).toInt();
|
||
|
|
int angulo = ((String)Protocolo[5] + (String)Protocolo[6] + (String)Protocolo[7]).toInt();
|
||
|
|
int porcentagem = ((String)Protocolo[9] + (String)Protocolo[10] + (String)Protocolo[11]).toInt();
|
||
|
|
double frequencia = (porcentagem / 100.0) * FrequenciaMaxima;
|
||
|
|
bool referenciar = ((String)Protocolo[13]) == "1";
|
||
|
|
int _offset = ((String)Protocolo[15] + (String)Protocolo[16] + (String)Protocolo[17] + (String)Protocolo[18]).toInt();
|
||
|
|
|
||
|
|
_frequencia = frequencia;
|
||
|
|
|
||
|
|
_motor->_Sentido_SP = sentido;
|
||
|
|
_motor->_Angulo_SP = angulo;
|
||
|
|
_motor->_Angulo_Offset = _offset;
|
||
|
|
|
||
|
|
_motor->AtualizarPWM();
|
||
|
|
|
||
|
|
if (referenciar) {
|
||
|
|
_motor->ResetRef(referenciar);
|
||
|
|
}
|
||
|
|
else {
|
||
|
|
_motor->_MotorLigado = sentido != Parado;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
else if (_funcao == Tst) {
|
||
|
|
String ID = ((String)Protocolo[0] + (String)Protocolo[1]);
|
||
|
|
|
||
|
|
Motor* _motor = MotorPorID(ID);
|
||
|
|
|
||
|
|
bool EmTeste = _motor->Testando;
|
||
|
|
if (!EmTeste) {
|
||
|
|
_motor->Testar();
|
||
|
|
}
|
||
|
|
|
||
|
|
}
|
||
|
|
|
||
|
|
}
|