Zendion/agrobot_base
Zendion
/
agrobot_base
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Ajustes no I2CService e ServoAgro

This commit is contained in:
Diego Freitas 2026-06-15 17:00:41 -03:00
parent c9b96c7861
commit 769cf2a4d5
4 changed files with 634 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
AgroBase/AgroBase/Forms/Operacoes/frmOperacaoManual.cs
View File

@ -83,8 +83,8 @@ namespace AgroBase.Forms.Operacoes
}
chbPulverizador.Checked = pC.PulverizadorAutomatico;
chbSonarAtivado.Checked = pC.MovAuxilioSonar;
chbSonarAtivado.Checked = pC.DirAuxilioSonar;
chbSonarAtivado.Checked = pC.MovAuxilioSonar || pC.DirAuxilioSonar;
chbImuAtivado.Checked = pC.ImuAuxilioMovimento || pC.ImuAuxilioDirecional;
InicializarComandosDirecional();
PopularInformacoesGerais();
@ -204,6 +204,13 @@ namespace AgroBase.Forms.Operacoes
chbSonarAtivado.Checked = false;
}
bool imuAtivado = Variaveis.OperacaoEmAndamento.Sensoriamento.IMU.saude.status == StatusModulo.Operante;
chbImuAtivado.Enabled = imuAtivado;
if (!imuAtivado)
{
chbImuAtivado.Checked = false;
}
if (!ReqFrameCam)
_ = AtualizarCameraSolo();

106
Firmware/Modulos/I2CService.h
View File

@ -144,8 +144,14 @@ class I2CService {
MostrarLog("I2C", "Iniciando Wire...");
I2CIniciado = Wire.begin(_pinoSDA, _pinoSCL);
MostrarLog(
"I2C",
"Wire configurado | clock=" + String(Wire.getClock()) +
" timeout=" + String(Wire.getTimeOut()) + "ms"
);
if (I2CIniciado) {
Wire.setClock(100000);
Wire.setTimeOut(TimeoutWireMs);
}
@ -465,6 +471,19 @@ class I2CService {
bool precisaRecuperar = _recoveryPendente;
String motivo = _motivoRecoveryPendente;
ContextoBarramento contexto = _contextoAtual;
if (
contexto.valido &&
contexto.canalMuxSolicitado >= 0 &&
!_recoveryPendente
) {
DesabilitarMuxAtualSemMutex();
precisaRecuperar = _recoveryPendente;
motivo = _motivoRecoveryPendente;
}
if (precisaRecuperar) {
CapturarContextoFalha();
i2cMutexReiniciando = true;
@ -473,6 +492,7 @@ class I2CService {
LimparContextoAtual();
idAtualUsandoI2C = -1;
tempoEntradaI2C = 0;
xSemaphoreGive(i2cMutex);
if (precisaRecuperar) {
@ -668,6 +688,12 @@ class I2CService {
}
uint8_t canalGlobal = (indiceMux * 10) + canalMux;
int canalAnterior = CanalGlobalAtivoAtual();
int sdaAntes = digitalRead(_pinoSDA);
int sclAntes = digitalRead(_pinoSCL);
int rdyAntes = TCAReadyOk() ? 1 : 0;
uint32_t inicio = millis();
if (CanalMuxEmQuarentena(canalGlobal)) {
MostrarLog("MUX", "Canal bloqueado por quarentena | canalGlobal=" + String(canalGlobal));
@ -715,6 +741,26 @@ class I2CService {
Wire.write((uint8_t)(1U << canalMux));
int erro = Wire.endTransmission();
uint32_t duracao = (uint32_t)(millis() - inicio);
int sdaDepois = digitalRead(_pinoSDA);
int sclDepois = digitalRead(_pinoSCL);
int rdyDepois = TCAReadyOk() ? 1 : 0;
MostrarLog(
"MUX",
"Troca"
" | anterior=" + String(canalAnterior) +
" solicitado=" + String(canalGlobal) +
" erro=" + String(erro) +
" duracao=" + String(duracao) + "ms" +
" antes[SDA=" + String(sdaAntes) +
" SCL=" + String(sclAntes) +
" RDY=" + String(rdyAntes) + "]" +
" depois[SDA=" + String(sdaDepois) +
" SCL=" + String(sclDepois) +
" RDY=" + String(rdyDepois) + "]"
);
if (erro != 0) {
MostrarLog(
"MUX",
@ -785,7 +831,7 @@ class I2CService {
int leituraADC = (int)ads.readADC_SingleEnded(canal);
uint32_t duracaoLeitura = (uint32_t)(millis() - inicioLeitura);
if (!TCAReadyOk() || duracaoLeitura > LimiteTempoI2C) {
if (!TCAReadyOk() || duracaoLeitura > LimiteLeituraADS_MS) {
RegistrarFalhaTransacao(
!TCAReadyOk()
? "READY caiu durante leitura ADS"
@ -1015,6 +1061,7 @@ class I2CService {
static constexpr uint32_t TimeoutWireMs = 80;
static constexpr uint32_t LimiteReadyLowMs = 200;
static constexpr uint32_t CooldownRecoveryMs = 1000;
static constexpr uint32_t LimiteLeituraADS_MS = 50;
static constexpr uint32_t LimiteFalhasTransacaoRecovery = 3;
static constexpr uint32_t MaxFalhasRecoveryAntesRestart = 3;
static constexpr uint32_t JanelaQuarentenaMs = 120000;
@ -1205,10 +1252,18 @@ class I2CService {
static void PrepararContextoAtual(int idSensor, int canalMuxSolicitado) {
ContextoBarramento contexto;
contexto.valido = true;
contexto.idSensor = idSensor;
contexto.canalMuxSolicitado = canalMuxSolicitado;
contexto.canalMuxAtivo = CanalGlobalAtivoAtual();
// Uma transacao direta no barramento principal nao deve herdar
// o ultimo canal que ficou selecionado no MUX.
contexto.canalMuxAtivo =
canalMuxSolicitado >= 0
? CanalGlobalAtivoAtual()
: -1;
contexto.enderecoADS = 0xFF;
contexto.inicioMs = millis();
@ -1540,8 +1595,14 @@ class I2CService {
if (barramentoLivre) {
I2CIniciado = Wire.begin(_pinoSDA, _pinoSCL);
MostrarLog(
"I2C",
"Wire configurado | clock=" + String(Wire.getClock()) +
" timeout=" + String(Wire.getTimeOut()) + "ms"
);
if (I2CIniciado) {
Wire.setClock(100000);
Wire.setTimeOut(TimeoutWireMs);
wireReiniciado = true;
}
@ -1685,6 +1746,47 @@ class I2CService {
DelayMs(50);
}
}
static bool DesabilitarMuxAtualSemMutex() {
if (enderecoMuxAtivo == 0xFF) {
MostrarLog("MUX", "Nenhum MUX marcado como ativo");
canalAtivo = -1;
return true;
}
const uint8_t endereco = enderecoMuxAtivo;
const int canalAnterior = canalAtivo;
uint32_t inicio = millis();
Wire.beginTransmission(endereco);
Wire.write((uint8_t)0x00);
int erro = Wire.endTransmission();
uint32_t duracao = (uint32_t)(millis() - inicio);
MostrarLog(
"MUX",
"Desabilitar canais"
" | anterior=" + String(canalAnterior) +
" erro=" + String(erro) +
" duracao=" + String(duracao) + "ms" +
" SDA=" + String(digitalRead(_pinoSDA)) +
" SCL=" + String(digitalRead(_pinoSCL)) +
" RDY=" + String(TCAReadyOk() ? 1 : 0)
);
if (erro != 0) {
RegistrarFalhaTransacao("Falha ao desabilitar canais do MUX");
return false;
}
enderecoMuxAtivo = 0xFF;
canalAtivo = -1;
RegistrarSucessoTransacao();
return true;
}
};
bool I2CService::DebugMode = false;

511
Firmware/Modulos/ServoAgroModel_v3.h Normal file
View File

@ -0,0 +1,511 @@
#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

18
Firmware/Sensoriamento/Sensoriamento_v23/Sensoriamento_v23.ino
View File

@ -9,7 +9,7 @@
#include "C:\ZendionInc\agrobot_base\Firmware\Modulos\I2CService.h"
#include "C:\ZendionInc\agrobot_base\Firmware\Modulos\Utils.h"
#include "C:\ZendionInc\agrobot_base\Firmware\Modulos\Pinout.h"
#include "C:\ZendionInc\agrobot_base\Firmware\Modulos\ServoAgroModel_v2.h"
#include "C:\ZendionInc\agrobot_base\Firmware\Modulos\ServoAgroModel_v3.h"
#include "C:\ZendionInc\agrobot_base\Firmware\Modulos\ReleModel_v2.h"
#include "C:\ZendionInc\agrobot_base\Firmware\Modulos\SinaleiroModel_v2.h"
#include "C:\ZendionInc\agrobot_base\Firmware\Modulos\SensorTemperaturaNtcModel.h"
@ -62,10 +62,10 @@ void LedTask(void* pv) {
#pragma endregion
#pragma region SERVOS
ServoAgro_v2 sSRV_FreioET("FROET", 41, true, 45, TiposBarramentos::CONTROLADOR);
ServoAgro_v2 sSRV_FreioDT("FRODT", 43, false, 47, TiposBarramentos::CONTROLADOR);
ServoAgro_v3 sSRV_FreioET("FROET", 41, true, 45, TiposBarramentos::CONTROLADOR, 0);
ServoAgro_v3 sSRV_FreioDT("FRODT", 43, false, 47, TiposBarramentos::CONTROLADOR, 1);
ServoAgro_v2* ObterServoPorId(uint8_t idNum) {
ServoAgro_v3* ObterServoPorId(uint8_t idNum) {
switch (idNum) {
case 41: return &sSRV_FreioET;
case 43: return &sSRV_FreioDT;
@ -75,8 +75,8 @@ ServoAgro_v2* ObterServoPorId(uint8_t idNum) {
#pragma endregion
#pragma region RELES
Rele_v2 sRLE_CoolerIn("RLCIN", 81, true, 19, TiposBarramentos::CONTROLADOR);
Rele_v2 sRLE_CoolerOut("RLCOUT", 82, true, 20, TiposBarramentos::CONTROLADOR);
Rele_v2 sRLE_CoolerIn("RLCIN", 81, true, 13, TiposBarramentos::CONTROLADOR);
Rele_v2 sRLE_CoolerOut("RLCOUT", 82, true, 14, TiposBarramentos::CONTROLADOR);
Rele_v2* ObterRelePorId(uint8_t idNum) {
switch (idNum) {
@ -152,7 +152,7 @@ void MostrarLog(String mensagem) {
}
void inicializarDependenciasI2C() {
I2CService::DefinirPinos(1, 2, 41, 42, 39, 40);
I2CService::DefinirPinos(1, 2, -1, -1, 39, 40);
I2CService::IniciarI2C();
I2CService::IniciarADS();
I2CService::IniciarMUX();
@ -193,6 +193,7 @@ void setup() {
xTaskCreatePinnedToCore(LedTask, "LedTask", 3000, NULL, 1, NULL, tskNO_AFFINITY);
// Registrar callback para processar mensagens CAN recebidas
MostrarLog("Registrando callback CAN...");
canService.setReceiveCallback([](int packetSize, int senderId, CanMessagePosicaoDados posicao, byte* data, int dataLength) {
std::vector<uint8_t> fullData;
fullData.push_back(static_cast<uint8_t>(posicao)); // Primeiro byte é sempre a posicao
@ -203,6 +204,7 @@ void setup() {
ProcessarFrameCAN(posicao, fullData);
});
MostrarLog("Iniciando comunicação CAN...");
canService.begin();
}
@ -378,7 +380,7 @@ void ProcessarCmd(std::vector<uint8_t> data) {
break;
}
case sSRV: {
ServoAgro_v2* servo = ObterServoPorId(id);
ServoAgro_v3* servo = ObterServoPorId(id);
if (servo == nullptr)
return;
servo->ProcessarComando(data);