511 lines
11 KiB
C++
511 lines
11 KiB
C++
#ifndef ServoAgroModel_v3
|
|
#define ServoAgroModel_v3
|
|
|
|
#include <Arduino.h>
|
|
|
|
#include "SerialService.h"
|
|
#include "Pinout.h"
|
|
#include "Utils.h"
|
|
#include "IComponenteCAN.h"
|
|
|
|
/*
|
|
* Controle de servo diretamente pelo periférico LEDC do ESP32-S3.
|
|
*
|
|
* Não depende da biblioteca ESP32Servo.
|
|
*
|
|
* Cada servo precisa utilizar um canal LEDC exclusivo.
|
|
*
|
|
* ESP32-S3:
|
|
* canais LEDC válidos: 0 a 7
|
|
* resolução máxima: 14 bits
|
|
*/
|
|
class ServoAgro_v3 : public ComponenteCAN {
|
|
public:
|
|
String ID;
|
|
uint8_t ID_Num = 0;
|
|
bool Incremental = false;
|
|
bool Iniciado = false;
|
|
PinoModel _pino;
|
|
|
|
ServoAgro_v3() {}
|
|
|
|
ServoAgro_v3(
|
|
const String& id,
|
|
uint8_t idNum,
|
|
bool incremental,
|
|
int pino,
|
|
TiposBarramentos tipoBarramento,
|
|
uint8_t canalPwm,
|
|
uint16_t pulsoMinUs = 544,
|
|
uint16_t pulsoMaxUs = 2400
|
|
) :
|
|
ID(id),
|
|
ID_Num(idNum),
|
|
Incremental(incremental),
|
|
_pino(PinoModel(pino, tipoBarramento, _OUTPUT)),
|
|
_canalPwm(canalPwm),
|
|
_pulsoMinUs(pulsoMinUs),
|
|
_pulsoMaxUs(pulsoMaxUs)
|
|
{
|
|
}
|
|
|
|
void Inicializar() {
|
|
MostrarLog(
|
|
"Entrou no Inicializar"
|
|
" | pino=" + String(_pino.num) +
|
|
" | canalPWM=" + String(_canalPwm) +
|
|
" | incremental=" + String(Incremental ? 1 : 0)
|
|
);
|
|
|
|
if (Iniciado) {
|
|
MostrarLog("Servo ja inicializado");
|
|
return;
|
|
}
|
|
|
|
if (!_pino.Definido()) {
|
|
MostrarLog("Servo nao iniciado | pino nao definido");
|
|
return;
|
|
}
|
|
|
|
if (_canalPwm >= QuantidadeCanaisLedc) {
|
|
MostrarLog(
|
|
"Servo nao iniciado | canal LEDC invalido: " +
|
|
String(_canalPwm)
|
|
);
|
|
return;
|
|
}
|
|
|
|
if (_pulsoMinUs >= _pulsoMaxUs) {
|
|
MostrarLog(
|
|
"Servo nao iniciado | intervalo de pulso invalido"
|
|
);
|
|
return;
|
|
}
|
|
|
|
if (!ReservarCanal(_canalPwm)) {
|
|
MostrarLog(
|
|
"Servo nao iniciado | canal LEDC ja reservado: " +
|
|
String(_canalPwm)
|
|
);
|
|
return;
|
|
}
|
|
|
|
_canalReservado = true;
|
|
|
|
MostrarLog(
|
|
"Configurando LEDC"
|
|
" | frequencia=" + String(FrequenciaHz) +
|
|
"Hz resolucao=" + String(ResolucaoBits) +
|
|
"bits"
|
|
);
|
|
|
|
double frequenciaReal = ledcSetup(
|
|
_canalPwm,
|
|
FrequenciaHz,
|
|
ResolucaoBits
|
|
);
|
|
|
|
if (frequenciaReal <= 0.0) {
|
|
MostrarLog(
|
|
"Falha no ledcSetup"
|
|
" | canal=" + String(_canalPwm)
|
|
);
|
|
|
|
LiberarRecursos();
|
|
return;
|
|
}
|
|
|
|
_frequenciaRealHz = static_cast<uint32_t>(frequenciaReal);
|
|
|
|
MostrarLog(
|
|
"LEDC configurado"
|
|
" | frequencia_real=" + String(_frequenciaRealHz) +
|
|
"Hz"
|
|
);
|
|
|
|
ledcAttachPin(
|
|
static_cast<uint8_t>(_pino.num),
|
|
_canalPwm
|
|
);
|
|
|
|
_pinoAnexado = true;
|
|
|
|
MostrarLog(
|
|
"Pino anexado ao LEDC"
|
|
" | pino=" + String(_pino.num) +
|
|
" canal=" + String(_canalPwm)
|
|
);
|
|
|
|
/*
|
|
* Primeiro aplica um duty válido.
|
|
*
|
|
* No Arduino-ESP32 2.x, ledcReadFreq() retorna zero
|
|
* quando o duty atual do canal é zero.
|
|
*/
|
|
int anguloInicial = Incremental ? 0 : 180;
|
|
|
|
if (!AplicarAngulo(anguloInicial, true)) {
|
|
MostrarLog("Falha ao aplicar angulo inicial");
|
|
LiberarRecursos();
|
|
return;
|
|
}
|
|
|
|
/*
|
|
* Agora o canal já possui duty diferente de zero,
|
|
* então ledcReadFreq() consegue retornar a frequência.
|
|
*/
|
|
uint32_t frequenciaConfirmada = ledcReadFreq(_canalPwm);
|
|
if (frequenciaConfirmada != FrequenciaHz) {
|
|
LiberarRecursos();
|
|
return;
|
|
}
|
|
|
|
MostrarLog(
|
|
"PWM confirmado"
|
|
" | frequencia=" + String(frequenciaConfirmada) + "Hz" +
|
|
" duty=" + String(ledcRead(_canalPwm)) +
|
|
" angulo=" + String(_Angulo)
|
|
);
|
|
|
|
if (frequenciaConfirmada == 0) {
|
|
MostrarLog(
|
|
"Aviso: nao foi possivel ler a frequencia, "
|
|
"mas o ledcSetup retornou " +
|
|
String(_frequenciaRealHz) + "Hz"
|
|
);
|
|
}
|
|
|
|
Iniciado = true;
|
|
|
|
MostrarLog(
|
|
"Servo iniciado"
|
|
" | angulo=" + String(_Angulo) +
|
|
" pulso=" + String(_pulsoAtualUs) + "us" +
|
|
" duty=" + String(_dutyAtual)
|
|
);
|
|
}
|
|
|
|
void Desligar() {
|
|
if (!Iniciado && !_pinoAnexado && !_canalReservado) {
|
|
MostrarLog("Servo nao esta inicializado");
|
|
return;
|
|
}
|
|
|
|
/*
|
|
* Duty zero interrompe os pulsos antes de desconectar o pino.
|
|
*/
|
|
if (_canalReservado) {
|
|
ledcWrite(_canalPwm, 0);
|
|
}
|
|
|
|
delay(2);
|
|
|
|
LiberarRecursos();
|
|
|
|
Iniciado = false;
|
|
|
|
MostrarLog("Servo desligado");
|
|
}
|
|
|
|
void RequisitarDados() {
|
|
if (!Iniciado) {
|
|
return;
|
|
}
|
|
}
|
|
|
|
void ProcessarComando(std::vector<uint8_t>& data) {
|
|
if (!Iniciado) {
|
|
return;
|
|
}
|
|
|
|
if (data.size() < 3) {
|
|
return;
|
|
}
|
|
|
|
CanMessagePosicaoDados posicao =
|
|
static_cast<CanMessagePosicaoDados>(data[0]);
|
|
|
|
uint8_t idNum = data[1];
|
|
|
|
if (idNum != ID_Num) {
|
|
return;
|
|
}
|
|
|
|
switch (posicao) {
|
|
case CanMessagePosicaoDados::Command1: {
|
|
int novoAngulo = static_cast<int>(data[2]);
|
|
AtualizarAngulo(novoAngulo);
|
|
break;
|
|
}
|
|
|
|
default:
|
|
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>(
|
|
constrain(_Angulo, 0, 180)
|
|
)
|
|
);
|
|
break;
|
|
}
|
|
|
|
default:
|
|
break;
|
|
}
|
|
|
|
return data;
|
|
}
|
|
|
|
int ObterAngulo() const {
|
|
return _Angulo;
|
|
}
|
|
|
|
uint16_t ObterPulsoUs() const {
|
|
return _pulsoAtualUs;
|
|
}
|
|
|
|
uint32_t ObterDuty() const {
|
|
return _dutyAtual;
|
|
}
|
|
|
|
uint8_t ObterCanalPwm() const {
|
|
return _canalPwm;
|
|
}
|
|
|
|
private:
|
|
/*
|
|
* O ESP32-S3 possui 8 canais LEDC.
|
|
*/
|
|
static constexpr uint8_t QuantidadeCanaisLedc = 8;
|
|
|
|
/*
|
|
* O ESP32-S3 suporta no máximo 14 bits no LEDC.
|
|
*/
|
|
static constexpr uint8_t ResolucaoBits = 14;
|
|
|
|
/*
|
|
* Servos convencionais utilizam período de 20 ms.
|
|
*/
|
|
static constexpr uint32_t FrequenciaHz = 50;
|
|
|
|
bool DebugMode = false;
|
|
|
|
uint8_t _canalPwm = 0xFF;
|
|
|
|
uint16_t _pulsoMinUs = 544;
|
|
uint16_t _pulsoMaxUs = 2400;
|
|
|
|
uint32_t _frequenciaRealHz = 0;
|
|
|
|
bool _canalReservado = false;
|
|
bool _pinoAnexado = false;
|
|
|
|
int _Angulo = 0;
|
|
|
|
uint16_t _pulsoAtualUs = 0;
|
|
uint32_t _dutyAtual = 0;
|
|
bool _sinalAplicado = false;
|
|
|
|
/*
|
|
* Um único mapa é compartilhado entre todas as instâncias
|
|
* de ServoAgro_v3.
|
|
*/
|
|
static bool* CanaisReservados() {
|
|
static bool canais[QuantidadeCanaisLedc] = {
|
|
false, false, false, false,
|
|
false, false, false, false
|
|
};
|
|
|
|
return canais;
|
|
}
|
|
|
|
static bool ReservarCanal(uint8_t canal) {
|
|
if (canal >= QuantidadeCanaisLedc) {
|
|
return false;
|
|
}
|
|
|
|
bool* canais = CanaisReservados();
|
|
|
|
if (canais[canal]) {
|
|
return false;
|
|
}
|
|
|
|
canais[canal] = true;
|
|
return true;
|
|
}
|
|
|
|
static void LiberarCanal(uint8_t canal) {
|
|
if (canal >= QuantidadeCanaisLedc) {
|
|
return;
|
|
}
|
|
|
|
CanaisReservados()[canal] = false;
|
|
}
|
|
|
|
static uint32_t DutyMaximo() {
|
|
return (1UL << ResolucaoBits) - 1UL;
|
|
}
|
|
|
|
static uint32_t PeriodoUs() {
|
|
return 1000000UL / FrequenciaHz;
|
|
}
|
|
|
|
uint16_t AnguloParaPulsoUs(int angulo) const {
|
|
angulo = constrain(angulo, 0, 180);
|
|
|
|
uint32_t intervalo =
|
|
static_cast<uint32_t>(_pulsoMaxUs - _pulsoMinUs);
|
|
|
|
uint32_t pulso =
|
|
static_cast<uint32_t>(_pulsoMinUs) +
|
|
(
|
|
static_cast<uint32_t>(angulo) *
|
|
intervalo
|
|
) / 180UL;
|
|
|
|
return static_cast<uint16_t>(pulso);
|
|
}
|
|
|
|
uint32_t PulsoUsParaDuty(uint16_t pulsoUs) const {
|
|
/*
|
|
* duty = pulso / período * duty máximo
|
|
*
|
|
* A soma de PeriodoUs()/2 realiza arredondamento
|
|
* inteiro em vez de simplesmente truncar.
|
|
*/
|
|
uint64_t numerador =
|
|
static_cast<uint64_t>(pulsoUs) *
|
|
static_cast<uint64_t>(DutyMaximo());
|
|
|
|
numerador += PeriodoUs() / 2UL;
|
|
|
|
return static_cast<uint32_t>(
|
|
numerador / PeriodoUs()
|
|
);
|
|
}
|
|
|
|
bool AplicarAngulo(int novoAngulo, bool confirmar = false) {
|
|
if (!_canalReservado || !_pinoAnexado) {
|
|
MostrarLog(
|
|
"Nao foi possivel atualizar angulo"
|
|
" | PWM nao inicializado"
|
|
);
|
|
|
|
return false;
|
|
}
|
|
|
|
novoAngulo = constrain(novoAngulo, 0, 180);
|
|
|
|
/*
|
|
* Só ignora uma posição repetida se um sinal já tiver
|
|
* sido realmente aplicado ao canal LEDC.
|
|
*/
|
|
if (_sinalAplicado && novoAngulo == _Angulo) {
|
|
return true;
|
|
}
|
|
|
|
uint16_t pulsoUs =
|
|
AnguloParaPulsoUs(novoAngulo);
|
|
|
|
uint32_t duty =
|
|
PulsoUsParaDuty(pulsoUs);
|
|
|
|
ledcWrite(_canalPwm, duty);
|
|
|
|
if (confirmar) {
|
|
/*
|
|
* Frequência de 50 Hz: cada período possui 20 ms.
|
|
*/
|
|
vTaskDelay(pdMS_TO_TICKS(25));
|
|
|
|
uint32_t dutyConfirmado =
|
|
ledcRead(_canalPwm);
|
|
|
|
if (dutyConfirmado != duty) {
|
|
MostrarLog(
|
|
"Falha ao confirmar duty"
|
|
" | esperado=" + String(duty) +
|
|
" lido=" + String(dutyConfirmado)
|
|
);
|
|
|
|
return false;
|
|
}
|
|
}
|
|
|
|
_Angulo = novoAngulo;
|
|
_pulsoAtualUs = pulsoUs;
|
|
_dutyAtual = duty;
|
|
_sinalAplicado = true;
|
|
|
|
return true;
|
|
}
|
|
|
|
void AtualizarAngulo(int novoAngulo) {
|
|
if (!Iniciado) {
|
|
MostrarLog(
|
|
"Comando de angulo ignorado"
|
|
" | servo nao iniciado"
|
|
);
|
|
|
|
return;
|
|
}
|
|
|
|
AplicarAngulo(novoAngulo);
|
|
}
|
|
|
|
void LiberarRecursos() {
|
|
if (_pinoAnexado && _pino.Definido()) {
|
|
ledcDetachPin(
|
|
static_cast<uint8_t>(_pino.num)
|
|
);
|
|
|
|
/*
|
|
* Depois do detach deixa o sinal explicitamente baixo.
|
|
*/
|
|
pinMode(_pino.num, OUTPUT);
|
|
digitalWrite(_pino.num, LOW);
|
|
|
|
_pinoAnexado = false;
|
|
}
|
|
|
|
if (_canalReservado) {
|
|
LiberarCanal(_canalPwm);
|
|
_canalReservado = false;
|
|
}
|
|
|
|
_frequenciaRealHz = 0;
|
|
_pulsoAtualUs = 0;
|
|
_dutyAtual = 0;
|
|
_sinalAplicado = false;
|
|
}
|
|
|
|
void MostrarLog(const String& mensagem) {
|
|
if (DebugMode) {
|
|
PrintTela(
|
|
"[SRV " + ID + "] " + mensagem
|
|
);
|
|
}
|
|
}
|
|
};
|
|
|
|
#endif |