483 lines
13 KiB
Arduino
483 lines
13 KiB
Arduino
|
#include "C:\Zendion Inc\agrobot_base\Firmware\Modulos\SerialService.h"
|
||
|
|
#define D_Code Dir
|
||
|
|
|
||
|
|
#define _pinoLED RGB_BUILTIN
|
||
|
|
int _pinoReleGeral = 8;
|
||
|
|
|
||
|
|
long _baudRate = 115200;
|
||
|
|
|
||
|
|
bool Conectado = false;
|
||
|
|
|
||
|
|
int _TaxaAmostragem;
|
||
|
|
int PPR = 12800; // Pulsos por revolução
|
||
|
|
int FrequenciaMaxima = 10000;
|
||
|
|
double _Margem = 0.5;
|
||
|
|
|
||
|
|
class Motor {
|
||
|
|
public:
|
||
|
|
// Construtor
|
||
|
|
Motor(String desc) {
|
||
|
|
_ID = desc;
|
||
|
|
}
|
||
|
|
|
||
|
|
// Definições
|
||
|
|
String _ID;
|
||
|
|
int _canal;
|
||
|
|
int _pinoENA;
|
||
|
|
int _pinoDIR;
|
||
|
|
int _pinoPUL;
|
||
|
|
int _pinoEncoderA;
|
||
|
|
int _pinoEncoderB;
|
||
|
|
int _pinoRef000;
|
||
|
|
int _pinoRef090;
|
||
|
|
int _pinoRef270;
|
||
|
|
|
||
|
|
bool Iniciado = false;
|
||
|
|
bool Testando = false;
|
||
|
|
bool Referenciando = false;
|
||
|
|
|
||
|
|
// Consumo
|
||
|
|
double _Angulo;
|
||
|
|
Sentido _Sentido;
|
||
|
|
|
||
|
|
// Entrada de dados
|
||
|
|
double _Angulo_SP;
|
||
|
|
double _Angulo_Offset = 0;
|
||
|
|
Sentido _Sentido_SP = Parado;
|
||
|
|
bool _RampaAtivada = false;
|
||
|
|
int dutyCycle = 512;
|
||
|
|
double _frequencia = 4000;
|
||
|
|
|
||
|
|
bool _Estabilizar = true;
|
||
|
|
|
||
|
|
bool Ag0 = false;
|
||
|
|
bool Ag0Ref = false;
|
||
|
|
bool Ag90 = false;
|
||
|
|
bool Ag270 = false;
|
||
|
|
bool Referenciado = false;
|
||
|
|
int RefMax = 90;
|
||
|
|
int RefMin = -90;
|
||
|
|
int ulEncoderA = 0;
|
||
|
|
int ulEncoderB = 0;
|
||
|
|
double GrausPino = 0;
|
||
|
|
|
||
|
|
void Inicializar() {
|
||
|
|
if (Iniciado) {
|
||
|
|
EnviarDadosSerial(_ID + " ja inicializado");
|
||
|
|
EnviarDadosSerial(MontarProtocoloSensor(sCFG, _ID, Iniciado ? "1" : "0"));
|
||
|
|
return;
|
||
|
|
}
|
||
|
|
|
||
|
|
if (_pinoPUL > -1) {
|
||
|
|
AtualizarPWM();
|
||
|
|
}
|
||
|
|
if (_pinoENA > -1) {
|
||
|
|
pinMode(_pinoENA, OUTPUT);
|
||
|
|
digitalWrite(_pinoENA, HIGH);
|
||
|
|
}
|
||
|
|
if (_pinoDIR > -1) {
|
||
|
|
pinMode(_pinoDIR, OUTPUT);
|
||
|
|
digitalWrite(_pinoDIR, LOW);
|
||
|
|
}
|
||
|
|
if (_pinoEncoderA > -1) {
|
||
|
|
pinMode(_pinoEncoderA, INPUT);
|
||
|
|
ulEncoderA = digitalRead(_pinoEncoderA);
|
||
|
|
}
|
||
|
|
if (_pinoEncoderB > -1) {
|
||
|
|
pinMode(_pinoEncoderB, INPUT);
|
||
|
|
ulEncoderB = digitalRead(_pinoEncoderB);
|
||
|
|
}
|
||
|
|
if (_pinoRef000 > -1) {
|
||
|
|
pinMode(_pinoRef000, INPUT);
|
||
|
|
Ag0 = digitalRead(_pinoRef000) == 1;
|
||
|
|
}
|
||
|
|
if (_pinoRef090 > -1) {
|
||
|
|
pinMode(_pinoRef090, INPUT);
|
||
|
|
Ag90 = digitalRead(_pinoRef090) == 1;
|
||
|
|
}
|
||
|
|
if (_pinoRef270 > -1) {
|
||
|
|
pinMode(_pinoRef270, INPUT);
|
||
|
|
Ag270 = digitalRead(_pinoRef270) == 1;
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
xTaskCreatePinnedToCore(&Motor::RampaTaskWrapper, "RampaTask", 5000, this, _canal, &RampaTaskHandle, 0);
|
||
|
|
|
||
|
|
|
||
|
|
_Angulo = 0;
|
||
|
|
GrausPino = 0;
|
||
|
|
_Sentido_SP = Parado;
|
||
|
|
_Sentido = Parado;
|
||
|
|
Referenciado = false;
|
||
|
|
Referenciando = true;
|
||
|
|
_RampaAtivada = true;
|
||
|
|
|
||
|
|
Iniciado = true;
|
||
|
|
|
||
|
|
EnviarDadosSerial(MontarProtocoloSensor(sCFG, _ID, Iniciado ? "1" : "0"));
|
||
|
|
}
|
||
|
|
|
||
|
|
void AtualizarPWM() {
|
||
|
|
ledcSetup(_canal, _frequencia, 10);
|
||
|
|
ledcAttachPin(_pinoPUL, _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
|
||
|
|
if (_pinoENA > -1) {
|
||
|
|
pinMode(_pinoENA, INPUT);
|
||
|
|
}
|
||
|
|
if (_pinoDIR > -1) {
|
||
|
|
pinMode(_pinoDIR, INPUT);
|
||
|
|
}
|
||
|
|
if (_pinoPUL > -1) {
|
||
|
|
ledcDetachPin(_pinoPUL);
|
||
|
|
pinMode(_pinoPUL, INPUT);
|
||
|
|
}
|
||
|
|
if (_pinoEncoderA > -1) {
|
||
|
|
pinMode(_pinoEncoderA, INPUT);
|
||
|
|
}
|
||
|
|
if (_pinoEncoderB > -1) {
|
||
|
|
pinMode(_pinoEncoderB, INPUT);
|
||
|
|
}
|
||
|
|
if (_pinoRef000 > -1) {
|
||
|
|
pinMode(_pinoRef000, INPUT);
|
||
|
|
}
|
||
|
|
if (_pinoRef090 > -1) {
|
||
|
|
pinMode(_pinoRef090, INPUT);
|
||
|
|
}
|
||
|
|
if (_pinoRef270 > -1) {
|
||
|
|
pinMode(_pinoRef270, INPUT);
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
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();
|
||
|
|
|
||
|
|
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();
|
||
|
|
|
||
|
|
unsigned long currentMillis = millis();
|
||
|
|
if (currentMillis - previousMillisRampa >= _TaxaAmostragem) {
|
||
|
|
previousMillisRampa = currentMillis;
|
||
|
|
|
||
|
|
EnviarDadosSerial(MontarProtocoloSensor(sENC, _ID, MontarDadosProtocoloEncoder(_Sentido, _Angulo, Ag0, Ag90, Ag270, GrausPino)));
|
||
|
|
}
|
||
|
|
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
void Atualizar() {
|
||
|
|
if (_pinoENA > -1) {
|
||
|
|
digitalWrite(_pinoENA, !_RampaAtivada);
|
||
|
|
}
|
||
|
|
if (_pinoDIR > -1) {
|
||
|
|
digitalWrite(_pinoDIR, _Sentido_SP != Horario);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
void AferirPosicao() {
|
||
|
|
if (_pinoRef000 > -1) {
|
||
|
|
bool _Ag0 = digitalRead(_pinoRef000) == 1;
|
||
|
|
|
||
|
|
if (GrausPino == 0) {
|
||
|
|
if (_Ag0 && !Ag0) {
|
||
|
|
_Angulo = 0;
|
||
|
|
}
|
||
|
|
else if (!_Ag0 && Ag0) {
|
||
|
|
GrausPino = (_Angulo < 0 ? -1.0 : 1.0) * _Angulo;
|
||
|
|
_Angulo = ((GrausPino / 2.0) * (_Sentido_SP == Horario ? 1.0 : -1.0)) + _Angulo_Offset;
|
||
|
|
Referenciado = true;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
/*else {
|
||
|
|
if ((!Ag0 && _Ag0) || (Ag0 && !_Ag0)) { // Entrou no pino ou saiu do pino
|
||
|
|
_Angulo = (((GrausPino / 2.0) + _Angulo_Offset) * (_Sentido_SP == Horario ? -1.0 : 1.0));
|
||
|
|
Referenciado = true;
|
||
|
|
}
|
||
|
|
}*/
|
||
|
|
|
||
|
|
Ag0 = _Ag0;
|
||
|
|
}
|
||
|
|
if (_pinoRef090 > -1) {
|
||
|
|
Ag90 = digitalRead(_pinoRef090) == 1;
|
||
|
|
}
|
||
|
|
if (_pinoRef270 > -1) {
|
||
|
|
Ag270 = digitalRead(_pinoRef270) == 1;
|
||
|
|
}
|
||
|
|
|
||
|
|
if (_pinoEncoderA > -1 && _pinoEncoderB > -1) {
|
||
|
|
int leituraEncoderA = digitalRead(_pinoEncoderA);
|
||
|
|
int leituraEncoderB = digitalRead(_pinoEncoderB);
|
||
|
|
|
||
|
|
// Verifica o sentido do giro
|
||
|
|
if (leituraEncoderA != ulEncoderA && leituraEncoderA == 1) {
|
||
|
|
if (leituraEncoderB != leituraEncoderA) {
|
||
|
|
// Sentido horário
|
||
|
|
_Angulo++;
|
||
|
|
_Sentido = Horario;
|
||
|
|
} else {
|
||
|
|
// Sentido anti-horário
|
||
|
|
_Angulo--;
|
||
|
|
_Sentido = Antihorario;
|
||
|
|
}
|
||
|
|
|
||
|
|
if (_Angulo_SP == 0 && (_Angulo >= -_Margem && _Angulo <= _Margem) && Referenciado) {
|
||
|
|
_Sentido_SP = Parado;
|
||
|
|
_Sentido = Parado;
|
||
|
|
_RampaAtivada = false;
|
||
|
|
}
|
||
|
|
|
||
|
|
}
|
||
|
|
|
||
|
|
if (leituraEncoderA != ulEncoderA) {
|
||
|
|
ulEncoderA = leituraEncoderA;
|
||
|
|
}
|
||
|
|
if (leituraEncoderB != ulEncoderB) {
|
||
|
|
ulEncoderB = leituraEncoderB;
|
||
|
|
}
|
||
|
|
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
void VerificarAnguloSP() {
|
||
|
|
if (_RampaAtivada) {
|
||
|
|
if (_Angulo_SP > -1) {
|
||
|
|
if (_Sentido_SP == Horario && ((_Angulo >= _Angulo_SP) || (Referenciado && _Angulo > RefMax))) {
|
||
|
|
_RampaAtivada = false;
|
||
|
|
}
|
||
|
|
else if (_Sentido_SP == Antihorario && ((_Angulo <= (_Angulo_SP * -1)) || (Referenciado && _Angulo < RefMin))) {
|
||
|
|
_RampaAtivada = false;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
else {
|
||
|
|
if (_Estabilizar && _Sentido_SP == Parado && _Angulo != _Angulo_SP) {
|
||
|
|
_Sentido_SP = _Angulo > 0 ? Antihorario : Horario;
|
||
|
|
_RampaAtivada = true;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
void ReferenciarMotor() {
|
||
|
|
int AgP1 = 45;
|
||
|
|
int AgP2 = 90;
|
||
|
|
|
||
|
|
if (Referenciado && (_Angulo >= -_Margem && _Angulo <= _Margem)) {
|
||
|
|
Referenciando = false;
|
||
|
|
_RampaAtivada = false;
|
||
|
|
}
|
||
|
|
else {
|
||
|
|
if (Ag90) {
|
||
|
|
_Angulo = 90 + (((GrausPino / 2.0) * (_Sentido_SP == Horario ? 1.0 : -1.0)) + _Angulo_Offset);
|
||
|
|
_Angulo_SP = AgP2;
|
||
|
|
_Sentido_SP = Antihorario;
|
||
|
|
}
|
||
|
|
else if (Ag270) {
|
||
|
|
_Angulo = -90 + (((GrausPino / 2.0) * (_Sentido_SP == Horario ? 1.0 : -1.0)) + _Angulo_Offset);
|
||
|
|
_Angulo_SP = AgP2;
|
||
|
|
_Sentido_SP = Horario;
|
||
|
|
}
|
||
|
|
else if (_Sentido_SP == Parado && (_Angulo >= -_Margem && _Angulo <= _Margem)) {
|
||
|
|
_Angulo_SP = AgP1;
|
||
|
|
_Sentido_SP = Horario;
|
||
|
|
}
|
||
|
|
else if (_Sentido_SP == Horario) {
|
||
|
|
if (_Angulo >= AgP1) {
|
||
|
|
_Angulo_SP = AgP1;
|
||
|
|
_Sentido_SP = Antihorario;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
else if (_Sentido_SP == Antihorario) {
|
||
|
|
if (_Angulo <= -AgP1) {
|
||
|
|
_Angulo_SP = AgP2;
|
||
|
|
_Sentido_SP = Antihorario;
|
||
|
|
}
|
||
|
|
else if (_Angulo <= -AgP2) {
|
||
|
|
_Angulo_SP = AgP2;
|
||
|
|
_Sentido_SP = Horario;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
};
|
||
|
|
|
||
|
|
Motor M1("ET");
|
||
|
|
Motor M2("EF");
|
||
|
|
Motor M3("DT");
|
||
|
|
Motor M4("DF");
|
||
|
|
|
||
|
|
void setup() {
|
||
|
|
Serial.begin(_baudRate);
|
||
|
|
|
||
|
|
pinMode(_pinoReleGeral, OUTPUT);
|
||
|
|
digitalWrite(_pinoReleGeral, LOW);
|
||
|
|
}
|
||
|
|
|
||
|
|
void loop() {
|
||
|
|
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 _PPR = ((String)Protocolo[14] + (String)Protocolo[15] + (String)Protocolo[16] + (String)Protocolo[17] + (String)Protocolo[18]).toInt();
|
||
|
|
|
||
|
|
PPR = _PPR;
|
||
|
|
|
||
|
|
if (pinoReleGeral > -1) {
|
||
|
|
pinMode(_pinoReleGeral, INPUT);
|
||
|
|
_pinoReleGeral = pinoReleGeral;
|
||
|
|
pinMode(_pinoReleGeral, OUTPUT);
|
||
|
|
digitalWrite(_pinoReleGeral, Conectar);
|
||
|
|
}
|
||
|
|
|
||
|
|
vTaskDelay(pdMS_TO_TICKS(100));
|
||
|
|
Conectado = Conectar;
|
||
|
|
|
||
|
|
EnviarDadosSerial(MontarProtocoloSensor(sCFG, "MD", Conectado ? "1" : "0"));
|
||
|
|
}
|
||
|
|
else {
|
||
|
|
Motor* _motor = ID == "ET" ? &M1 : ID == "EF" ? &M2 : ID == "DT" ? &M3 : &M4;
|
||
|
|
|
||
|
|
int canal = ((String)Protocolo[5]).toInt();
|
||
|
|
bool motor_ativado = (String)Protocolo[7] == "1";
|
||
|
|
bool estabilizar = ((String)Protocolo[9]) == "1";
|
||
|
|
int pul = ((String)Protocolo[11] + (String)Protocolo[12]).toInt();
|
||
|
|
int dir = ((String)Protocolo[14] + (String)Protocolo[15]).toInt();
|
||
|
|
int ena = ((String)Protocolo[17] + (String)Protocolo[18]).toInt();
|
||
|
|
int encoderA = ((String)Protocolo[20] + (String)Protocolo[21]).toInt();
|
||
|
|
int encoderB = ((String)Protocolo[23] + (String)Protocolo[24]).toInt();
|
||
|
|
int ref0 = ((String)Protocolo[26] + (String)Protocolo[27]).toInt();
|
||
|
|
int ref90 = ((String)Protocolo[29] + (String)Protocolo[30]).toInt();
|
||
|
|
int ref270 = ((String)Protocolo[32] + (String)Protocolo[33]).toInt();
|
||
|
|
|
||
|
|
if (motor_ativado) {
|
||
|
|
if (Conectar) {
|
||
|
|
_motor->_canal = canal;
|
||
|
|
_motor->_Estabilizar = estabilizar;
|
||
|
|
_motor->_pinoPUL = pul;
|
||
|
|
_motor->_pinoDIR = dir;
|
||
|
|
_motor->_pinoENA = ena;
|
||
|
|
_motor->_pinoEncoderA = encoderA;
|
||
|
|
_motor->_pinoEncoderB = encoderB;
|
||
|
|
_motor->_pinoRef000 = ref0;
|
||
|
|
_motor->_pinoRef090 = ref90;
|
||
|
|
_motor->_pinoRef270 = ref270;
|
||
|
|
_motor->Inicializar();
|
||
|
|
}
|
||
|
|
else {
|
||
|
|
_motor->Desligar();
|
||
|
|
}
|
||
|
|
vTaskDelay(pdMS_TO_TICKS(500));
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
else if (_funcao == Cmd) {
|
||
|
|
//canal;sentido;angulo
|
||
|
|
//0;0;000.00
|
||
|
|
String ID = ((String)Protocolo[0] + (String)Protocolo[1]);
|
||
|
|
|
||
|
|
Motor* _motor = ID == "ET" ? &M1 : ID == "EF" ? &M2 : ID == "DT" ? &M3 : &M4;
|
||
|
|
|
||
|
|
int porcentagem = ((String)Protocolo[9] + (String)Protocolo[10] + (String)Protocolo[11]).toInt();
|
||
|
|
double frequencia = (porcentagem / 100.0) * FrequenciaMaxima;
|
||
|
|
|
||
|
|
_motor->_Sentido_SP = (Sentido)((String)Protocolo[3]).toInt();
|
||
|
|
_motor->_Angulo_SP = ((String)Protocolo[5] + (String)Protocolo[6] + (String)Protocolo[7]).toInt();
|
||
|
|
_motor->_frequencia = frequencia;
|
||
|
|
_motor->_Angulo_Offset = ((String)Protocolo[13] + (String)Protocolo[14] + (String)Protocolo[15] + (String)Protocolo[16]).toInt();
|
||
|
|
|
||
|
|
_motor->AtualizarPWM();
|
||
|
|
|
||
|
|
_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();
|
||
|
|
}
|
||
|
|
|
||
|
|
}
|
||
|
|
|
||
|
|
}
|
||
|
|
}
|