401 lines
10 KiB
Arduino
401 lines
10 KiB
Arduino
|
//#include "EEPROM.h"
|
||
|
|
|
||
|
|
enum Funcoes {
|
||
|
|
Limpar,
|
||
|
|
Ler,
|
||
|
|
Definir,
|
||
|
|
Incrementar
|
||
|
|
};
|
||
|
|
|
||
|
|
enum T_Code {
|
||
|
|
Vzo = 200,
|
||
|
|
Chk = 244,
|
||
|
|
Cfg = 258,
|
||
|
|
Mov = 100,
|
||
|
|
Dir = 101,
|
||
|
|
Sen = 102,
|
||
|
|
Atu = 103
|
||
|
|
};
|
||
|
|
|
||
|
|
enum Sentido {
|
||
|
|
Parado,
|
||
|
|
Horario,
|
||
|
|
Antihorario
|
||
|
|
};
|
||
|
|
|
||
|
|
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;
|
||
|
|
|
||
|
|
class Motor {
|
||
|
|
public:
|
||
|
|
// Definições
|
||
|
|
String _descricao;
|
||
|
|
int _canal;
|
||
|
|
int _pinoPWM;
|
||
|
|
int _pinoVEL;
|
||
|
|
int _pinoDIR;
|
||
|
|
int _pinoBRK;
|
||
|
|
int _pinoSTP;
|
||
|
|
|
||
|
|
bool Iniciado = false;
|
||
|
|
|
||
|
|
// Consumo
|
||
|
|
float RPM;
|
||
|
|
int nPulsos;
|
||
|
|
long intervalo;
|
||
|
|
int PotenciaAtual;
|
||
|
|
|
||
|
|
// Entrada de dados
|
||
|
|
int _Rampa;
|
||
|
|
int _Precisao;
|
||
|
|
int _Potencia;
|
||
|
|
int _RPM_SP;
|
||
|
|
Sentido _Sentido = Parado;
|
||
|
|
Sentido _SentidoA = Parado;
|
||
|
|
bool _RampaAtivada = false;
|
||
|
|
int _Margem = 2;
|
||
|
|
|
||
|
|
Motor(String desc, int canal, int _pwm, int _vel, int _dir, int _brk, int _stp) {
|
||
|
|
_descricao = desc;
|
||
|
|
_canal = canal;
|
||
|
|
_pinoPWM = _pwm;
|
||
|
|
_pinoVEL = _vel;
|
||
|
|
_pinoDIR = _dir;
|
||
|
|
_pinoBRK = _brk;
|
||
|
|
_pinoSTP = _stp;
|
||
|
|
}
|
||
|
|
|
||
|
|
void Inicializar() {
|
||
|
|
nPulsos = 0;
|
||
|
|
intervalo = 0;
|
||
|
|
RPM = 0;
|
||
|
|
|
||
|
|
if (Iniciado) {
|
||
|
|
Serial.print(_descricao);
|
||
|
|
Serial.println(" 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);
|
||
|
|
xTaskCreatePinnedToCore(&Motor::RPMTaskWrapper, "RPMTask", 4000, this, _canal, &RPMTaskHandle, 0);
|
||
|
|
xTaskCreatePinnedToCore(&Motor::RampaTaskWrapper, "RampaTask", 4000, this, 20 - _canal, &RampaTaskHandle, 0);
|
||
|
|
|
||
|
|
Serial.println(_descricao + " Iniciado");
|
||
|
|
|
||
|
|
Iniciado = true;
|
||
|
|
}
|
||
|
|
|
||
|
|
void Atualizar() {
|
||
|
|
digitalWrite(_pinoDIR, _SentidoA == Horario);
|
||
|
|
digitalWrite(_pinoSTP, _SentidoA != Parado);
|
||
|
|
|
||
|
|
ledcWrite(_canal, PotenciaAtual);
|
||
|
|
|
||
|
|
//Serial.print(_canal);
|
||
|
|
//Serial.print(" ");
|
||
|
|
//Serial.println(PotenciaAtual);
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
private:
|
||
|
|
TaskHandle_t RPMTaskHandle = NULL;
|
||
|
|
static void RPMTaskWrapper(void *pvParameters) {
|
||
|
|
Motor *motor = static_cast<Motor*>(pvParameters);
|
||
|
|
motor->RPMTask();
|
||
|
|
}
|
||
|
|
|
||
|
|
void RPMTask() {
|
||
|
|
bool uStatus = digitalRead(_pinoVEL);
|
||
|
|
unsigned long firstMilis = millis();
|
||
|
|
unsigned long firstMilisCalc = millis();
|
||
|
|
while (1)
|
||
|
|
{
|
||
|
|
if (_SentidoA == Parado && _Sentido == Parado) {
|
||
|
|
ReiniciaVelocimetro();
|
||
|
|
firstMilis = millis();
|
||
|
|
firstMilisCalc = millis();
|
||
|
|
RPM = 0;
|
||
|
|
uStatus = digitalRead(_pinoVEL);
|
||
|
|
vTaskDelay(100);
|
||
|
|
continue;
|
||
|
|
}
|
||
|
|
|
||
|
|
bool aStatus = digitalRead(_pinoVEL);
|
||
|
|
if (uStatus != aStatus) {
|
||
|
|
uStatus = aStatus;
|
||
|
|
if (aStatus == true) {
|
||
|
|
nPulsos++;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
long intMillis = (millis() - firstMilis);
|
||
|
|
long intMillisCalc = (millis() - firstMilisCalc);
|
||
|
|
intervalo = intMillis / 1000;
|
||
|
|
|
||
|
|
if (intMillisCalc >= _TaxaAmostragem) {
|
||
|
|
RPM = intervalo == 0 ? RPM : (4 * nPulsos) / (3 * intervalo);
|
||
|
|
String protSensor = BeginSensor + _descricao + ";" + (String)nPulsos + ";" + (String)intervalo + ";" + (String)RPM + ";" + (String)_Potencia;
|
||
|
|
Serial.println(protSensor);
|
||
|
|
Serial.flush();
|
||
|
|
CorrigePotenciaMotor();
|
||
|
|
vTaskDelay(100);
|
||
|
|
firstMilisCalc = millis();
|
||
|
|
|
||
|
|
ReiniciaVelocimetro();
|
||
|
|
firstMilis = millis();
|
||
|
|
}
|
||
|
|
|
||
|
|
vTaskDelay(1);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
void ReiniciaVelocimetro() {
|
||
|
|
nPulsos = 0;
|
||
|
|
intervalo = 0;
|
||
|
|
}
|
||
|
|
|
||
|
|
void CorrigePotenciaMotor() {
|
||
|
|
if (((RPM + _Margem) < _RPM_SP) || ((RPM - _Margem) > _RPM_SP)) {
|
||
|
|
int PA = PotenciaAcrescentar();
|
||
|
|
_Potencia = _Potencia + PA;
|
||
|
|
if (_Potencia > 100) {
|
||
|
|
_Potencia = 100;
|
||
|
|
}
|
||
|
|
else if (_Potencia < 0) {
|
||
|
|
_Potencia = 0;
|
||
|
|
}
|
||
|
|
if (PA != 0) {
|
||
|
|
_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 = (_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();
|
||
|
|
int ZeroRampa = 0;
|
||
|
|
int PotenciaMax = 1023;
|
||
|
|
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;
|
||
|
|
}
|
||
|
|
else {
|
||
|
|
PotenciaAtual = (PotenciaAtual > PotenciaMax) ? PotenciaMax : (_Potencia * MultiplicadorPotencia);
|
||
|
|
}
|
||
|
|
_SentidoA = _Sentido;
|
||
|
|
Limite = true;
|
||
|
|
}
|
||
|
|
|
||
|
|
Atualizar();
|
||
|
|
|
||
|
|
if (Limite) {
|
||
|
|
_RampaAtivada = false;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
vTaskDelay(1);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
};
|
||
|
|
|
||
|
|
|
||
|
|
int addr_code = 25;
|
||
|
|
#define EEPROM_SIZE 512
|
||
|
|
#define D_Code Mov
|
||
|
|
|
||
|
|
Motor M1("DF", 0, 05, 36, 18, 19, 21);
|
||
|
|
Motor M2("DT", 1, 15, 39, 02, 00, 04);
|
||
|
|
Motor M3("ET", 2, 13, 35, 12, 26, 27);
|
||
|
|
Motor M4("EF", 3, 14, 34, 25, 33, 32);
|
||
|
|
|
||
|
|
void setup() {
|
||
|
|
Serial.begin(115200);
|
||
|
|
pinMode(PinoReleGeral, OUTPUT);
|
||
|
|
digitalWrite(PinoReleGeral, LOW);
|
||
|
|
//EEPROM.begin(EEPROM_SIZE);
|
||
|
|
//AtualizarMemoria(Definir, addr_code, D_Code);
|
||
|
|
}
|
||
|
|
|
||
|
|
void loop() {
|
||
|
|
if (Serial.available() > 3) {
|
||
|
|
//Serial.println();
|
||
|
|
String Protocolo = "";
|
||
|
|
//1000;080;1;020;001
|
||
|
|
T_Code _funcao = Vzo;
|
||
|
|
while (Serial.available()) {
|
||
|
|
char Entrada = (char)Serial.read();
|
||
|
|
//Serial.print(Entrada);
|
||
|
|
if (Entrada == EndLine) {
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
Protocolo += Entrada;
|
||
|
|
if (Protocolo.length() == 3) {
|
||
|
|
if (_funcao == Vzo) {
|
||
|
|
_funcao = (T_Code)((String)Protocolo[0] + (String)Protocolo[1] + (String)Protocolo[2]).toInt();
|
||
|
|
if (_funcao == D_Code) {
|
||
|
|
//Serial.print("Protocolo iniciado, D_Code: ");
|
||
|
|
//Serial.println(D_Code);
|
||
|
|
Protocolo = "";
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
if (_funcao == Chk) {
|
||
|
|
Serial.println((String)D_Code);
|
||
|
|
Serial.flush();
|
||
|
|
}
|
||
|
|
if (_funcao == Cfg) {
|
||
|
|
Conectado = (String)Protocolo[4] == "1";
|
||
|
|
_TaxaAmostragem = ((String)Protocolo[6] + (String)Protocolo[7] + (String)Protocolo[8] + (String)Protocolo[9] + (String)Protocolo[10]).toInt();
|
||
|
|
M1_Ativado = (String)Protocolo[12] == "1";
|
||
|
|
M2_Ativado = (String)Protocolo[14] == "1";
|
||
|
|
M3_Ativado = (String)Protocolo[16] == "1";
|
||
|
|
M4_Ativado = (String)Protocolo[18] == "1";
|
||
|
|
if (M1_Ativado) {
|
||
|
|
M1.Inicializar();
|
||
|
|
vTaskDelay(500);
|
||
|
|
}
|
||
|
|
if (M2_Ativado) {
|
||
|
|
M2.Inicializar();
|
||
|
|
vTaskDelay(500);
|
||
|
|
}
|
||
|
|
if (M3_Ativado) {
|
||
|
|
M3.Inicializar();
|
||
|
|
vTaskDelay(500);
|
||
|
|
}
|
||
|
|
if (M4_Ativado) {
|
||
|
|
M4.Inicializar();
|
||
|
|
vTaskDelay(500);
|
||
|
|
}
|
||
|
|
|
||
|
|
digitalWrite(PinoReleGeral, Conectado);
|
||
|
|
}
|
||
|
|
else if (_funcao == D_Code) {
|
||
|
|
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->_RampaAtivada = true;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
delay(10);
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
/*int AtualizarMemoria(Funcoes Funcao, int Endereco, int Valor) {
|
||
|
|
int MaxMemoria = 3;
|
||
|
|
int ValorLido = 0;
|
||
|
|
|
||
|
|
if (Funcao == Limpar) {
|
||
|
|
for (int i = 0; i < 254; i++) {
|
||
|
|
EEPROM.write(i, '0');
|
||
|
|
EEPROM.commit();
|
||
|
|
}
|
||
|
|
}
|
||
|
|
else {
|
||
|
|
String ValorLidoStr = "";
|
||
|
|
for (int i = Endereco; i < (Endereco + MaxMemoria); i++) {
|
||
|
|
ValorLidoStr += (char)EEPROM.read(i);
|
||
|
|
}
|
||
|
|
|
||
|
|
ValorLido = ValorLidoStr.toInt();
|
||
|
|
|
||
|
|
if (Funcao != Ler) {
|
||
|
|
if (Funcao == Definir) {
|
||
|
|
ValorLido = Valor;
|
||
|
|
}
|
||
|
|
else if (Funcao == Incrementar) {
|
||
|
|
ValorLido += Valor;
|
||
|
|
}
|
||
|
|
ValorLidoStr = (String)ValorLido;
|
||
|
|
|
||
|
|
int idx = 0;
|
||
|
|
for (int i = Endereco; i < (Endereco + MaxMemoria); i++) {
|
||
|
|
char V = '0';
|
||
|
|
int PIndex = (MaxMemoria - ValorLidoStr.length());
|
||
|
|
if (PIndex <= idx)
|
||
|
|
V = ValorLidoStr[idx - PIndex];
|
||
|
|
EEPROM.write(i, V);
|
||
|
|
idx++;
|
||
|
|
}
|
||
|
|
|
||
|
|
EEPROM.commit();
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
return ValorLido;
|
||
|
|
}*/
|