128 lines
2.7 KiB
C++
128 lines
2.7 KiB
C++
#include "SerialService.h"
|
|
#include "Pinout.h"
|
|
#include "Utils.h"
|
|
#include "IComponenteCAN.h"
|
|
#include <ESP32Servo.h>
|
|
|
|
#ifndef ServoAgroModel_v2
|
|
#define ServoAgroModel_v2
|
|
|
|
class ServoAgro_v2 : public ComponenteCAN {
|
|
public:
|
|
String ID;
|
|
uint8_t ID_Num = 0;
|
|
bool Incremental = false;
|
|
bool Iniciado = false;
|
|
PinoModel _pino;
|
|
|
|
ServoAgro_v2() {}
|
|
|
|
ServoAgro_v2(
|
|
const String& id,
|
|
uint8_t idNum,
|
|
bool incremental,
|
|
int pino,
|
|
TiposBarramentos tipoBarramento
|
|
) :
|
|
ID(id),
|
|
ID_Num(idNum),
|
|
Incremental(incremental),
|
|
_pino(PinoModel(pino, tipoBarramento, _OUTPUT))
|
|
{
|
|
}
|
|
|
|
void Inicializar() {
|
|
if (Iniciado) {
|
|
MostrarLog("Servo ja inicializado");
|
|
return;
|
|
}
|
|
if (_pino.Definido()) {
|
|
_Servo.attach(_pino.num);
|
|
AtualizarAngulo(Incremental ? 0 : 180);
|
|
Iniciado = true;
|
|
MostrarLog("Servo iniciado");
|
|
}
|
|
else {
|
|
MostrarLog("Servo nao iniciado");
|
|
}
|
|
}
|
|
|
|
void Desligar() {
|
|
if (!Iniciado) {
|
|
MostrarLog("Servo nao esta inicializado");
|
|
return;
|
|
}
|
|
|
|
if (_pino.Definido()) {
|
|
_Servo.detach();
|
|
}
|
|
|
|
MostrarLog("Servo Desligado");
|
|
|
|
Iniciado = false;
|
|
}
|
|
|
|
void RequisitarDados() {
|
|
if (!Iniciado) return;
|
|
}
|
|
|
|
void ProcessarComando(std::vector<uint8_t>& data) {
|
|
if (!Iniciado) return;
|
|
if (data.size() < 3) return;
|
|
CanMessagePosicaoDados posicao = (CanMessagePosicaoDados)data[0];
|
|
uint8_t idNum = data[1];
|
|
if (idNum != ID_Num) return;
|
|
switch (posicao) {
|
|
case CanMessagePosicaoDados::Command1: {
|
|
int novoAngulo = (int)data[2];
|
|
AtualizarAngulo(novoAngulo);
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
|
|
std::vector<uint8_t> MontarMensagemCAN(CanMessagePosicaoDados posicao) {
|
|
std::vector<uint8_t> data;
|
|
data.push_back(static_cast<uint8_t>(posicao));
|
|
data.push_back(ID_Num);
|
|
switch (posicao) {
|
|
case CanMessagePosicaoDados::Status: {
|
|
data.push_back(Iniciado ? 1 : 0);
|
|
break;
|
|
}
|
|
case CanMessagePosicaoDados::Dados1: {
|
|
data.push_back(static_cast<uint8_t>(_Angulo));
|
|
break;
|
|
}
|
|
}
|
|
return data;
|
|
}
|
|
|
|
|
|
private:
|
|
bool DebugMode = false;
|
|
|
|
Servo _Servo;
|
|
int _Angulo = 0;
|
|
|
|
void AtualizarAngulo(int novoAngulo) {
|
|
novoAngulo = constrain(novoAngulo, 0, 180);
|
|
MostrarLog("Angulo atualizado de " + String(_Angulo) + " para " + String(novoAngulo));
|
|
_Angulo = novoAngulo;
|
|
_Servo.write(_Angulo);
|
|
}
|
|
|
|
void MostrarLog(String mensagem) {
|
|
if (DebugMode) {
|
|
PrintTela("[SRV " + ID + "] " + mensagem);
|
|
}
|
|
}
|
|
|
|
};
|
|
|
|
|
|
|
|
|
|
|
|
|
|
#endif |