285 lines
11 KiB
C#
285 lines
11 KiB
C#
using AgroBase.Models;
|
|
using AgroBase.Models.Modules;
|
|
using System;
|
|
using System.Collections.Generic;
|
|
using System.Linq;
|
|
|
|
namespace AgroBase.Services
|
|
{
|
|
public class CoulombCounterService
|
|
{
|
|
public CoulombCounterService(double _max_Amp, double w_m_base = 0.1167) // 3 km/h ~ 350 wh
|
|
{
|
|
double coulombsMax = _max_Amp * 3600;
|
|
CargaMaxima = coulombsMax;
|
|
WhPorMetroBase = w_m_base;
|
|
//double consumo = (Variaveis.OperacaoEmAndamento.DispSen?.Dados?.BateriaCargaConsumida ?? 0);
|
|
//Consumido = consumo < CargaMaxima ? consumo : 0;
|
|
Consumido = 0;
|
|
CargaAtual = CargaMaxima - Consumido;
|
|
//double consumoW = (Variaveis.OperacaoEmAndamento.DispSen?.Dados?.BateriaCargaConsumidaWatts ?? 0);
|
|
//ConsumidoWatts = consumoW;
|
|
|
|
// Autonomia
|
|
MargemSegurancaAutonomia = 0.30; // 30% de margem padrão
|
|
EnergiaConsumida_Wh = 0;
|
|
_energiaConsumidaAnterior_Wh = 0;
|
|
_distanciaTotalMetrosAnterior = 0;
|
|
}
|
|
|
|
public double TempoEstimadoMinutos { get; private set; }
|
|
private const int BufferSize = 30; // últimos 3 segundos se chamada a cada 100ms
|
|
private readonly Queue<double> _historicoCorrente = new Queue<double>();
|
|
|
|
public bool Iniciado { get; private set; }
|
|
private double CargaMaxima { get; set; }
|
|
private double CargaAtual { get; set; }
|
|
public double CargaAtualAmp
|
|
{
|
|
get
|
|
{
|
|
return CargaAtual / 3600.0;
|
|
}
|
|
}
|
|
private double Consumido { get; set; }
|
|
public double _Consumido
|
|
{
|
|
get
|
|
{
|
|
return Consumido;
|
|
}
|
|
}
|
|
public double ConsumidoAmp
|
|
{
|
|
get
|
|
{
|
|
return Consumido / 3600.0;
|
|
}
|
|
}
|
|
public double PercentualAtual
|
|
{
|
|
get
|
|
{
|
|
return (CargaAtual / CargaMaxima) * 100;
|
|
}
|
|
}
|
|
public double PercentualConsumido
|
|
{
|
|
get
|
|
{
|
|
double calc = (Consumido / CargaMaxima) * 100;
|
|
calc = FuncoesMatematicas.Clamp(calc, 0, 100);
|
|
return calc;
|
|
}
|
|
}
|
|
public double PercentualBateria { get; private set; }
|
|
public double EnergiaConsumida_Wh { get; set; }
|
|
public double _dt { get; private set; }
|
|
public double CorrenteInstantanea { get; private set; }
|
|
public double TensaoInstantanea { get; private set; }
|
|
public double PotenciaInstantanea { get; private set; }
|
|
public double WhRestante
|
|
{
|
|
get
|
|
{
|
|
// Exemplo: usar uma tensão nominal do pack
|
|
double tensaoNominal = VariaveisEquipamento.TensaoMaximaBateria;
|
|
return CargaAtualAmp * tensaoNominal; // Ah * V = Wh
|
|
}
|
|
}
|
|
|
|
public double WhPorMetro { get; private set; }
|
|
public double WhPorMetroBase { get; set; } // configurado externamente
|
|
public double WhPorMetroHistorico { get; private set; }
|
|
public double WhPorMetroReferencia
|
|
{
|
|
get
|
|
{
|
|
if (WhPorMetro > 0) return WhPorMetro;
|
|
if (WhPorMetroHistorico > 0) return WhPorMetroHistorico;
|
|
return WhPorMetroBase; // pode ser 0 se você não configurar
|
|
}
|
|
}
|
|
public double DistanciaMaximaTeorica_m { get; private set; }
|
|
public double DistanciaMaximaSegura_m { get; private set; }
|
|
public double DistanciaEstimadaRestante_m { get; private set; }
|
|
public double MargemSegurancaAutonomia { get; set; }
|
|
private const double JanelaAutonomia_m = 50.0; // metros analisados na média
|
|
private readonly Queue<double> _historicoDeltaDist_m = new Queue<double>();
|
|
private readonly Queue<double> _historicoDeltaWh = new Queue<double>();
|
|
private double _somaDistanciaJanela_m;
|
|
private double _somaEnergiaJanela_Wh;
|
|
private double _distanciaTotalMetrosAnterior;
|
|
private double _energiaConsumidaAnterior_Wh;
|
|
|
|
|
|
public void AtualizarConsumo(double distanciaTotalMetrosAtual)
|
|
{
|
|
var Sensor = Variaveis.OperacaoEmAndamento.DispSen?.Dados?.Sensores?.FirstOrDefault(x => x.ID == "AB36V");
|
|
|
|
Iniciado = Sensor?.Inicializado ?? false;
|
|
|
|
if (!Iniciado)
|
|
return;
|
|
|
|
_dt = Sensor.ValoresLeituras.FirstOrDefault(x => x.funcao == FuncoesPinout.Corrente)?.atual?.dt ?? 0;
|
|
TensaoInstantanea = Sensor.ValoresLeituras?.FirstOrDefault(x => x.funcao == FuncoesPinout.Tensao)?.atual?.valor ?? 0;
|
|
CorrenteInstantanea = (Sensor.ValoresLeituras?.FirstOrDefault(x => x.funcao == FuncoesPinout.Corrente)?.atual?.valor ?? 0) / 1000.0;
|
|
PotenciaInstantanea = Sensor.ValoresLeituras?.FirstOrDefault(x => x.funcao == FuncoesPinout.Potencia)?.atual?.valor ?? 0;
|
|
|
|
PercentualBateria = Math.Round(FuncoesMatematicas.Map(TensaoInstantanea, VariaveisEquipamento.TensaoMinimaBateria, VariaveisEquipamento.TensaoMaximaBateria, 0.0, 100.0), 2);
|
|
PercentualBateria = FuncoesMatematicas.Clamp(PercentualBateria, 0, 100);
|
|
|
|
// Verifique se as datas são válidas (não MinValue, MaxValue ou valores anômalos)
|
|
if (_dt == 0 || _dt > 10)
|
|
{
|
|
Console.WriteLine("[COULOMB COUNTER] Leitura inválida detectada.");
|
|
return;
|
|
}
|
|
|
|
// --- COULOMB COUNTING (igual ao que já tinha, só com pequenos cuidados) ---
|
|
double deltaCoulombs = CorrenteInstantanea * _dt; // A * s
|
|
|
|
Consumido += deltaCoulombs;
|
|
if (Consumido < 0) Consumido = 0;
|
|
if (Consumido > CargaMaxima) Consumido = CargaMaxima;
|
|
|
|
CargaAtual = CargaMaxima - Consumido;
|
|
|
|
// --- ENERGIA ACUMULADA (Wh) ---
|
|
double potenciaInst_W = TensaoInstantanea * CorrenteInstantanea; // W
|
|
double deltaEnergia_Wh = potenciaInst_W * _dt / 3600.0; // Wh
|
|
|
|
EnergiaConsumida_Wh += deltaEnergia_Wh;
|
|
//EnergiaConsumida_Wh = Sensor.ValoresLeituras?.FirstOrDefault(x => x.funcao == FuncoesPinout.Energia)?.atual?.valor ?? 0;
|
|
|
|
// --- HISTÓRICO P/ TEMPO ESTIMADO ---
|
|
_historicoCorrente.Enqueue(CorrenteInstantanea);
|
|
if (_historicoCorrente.Count > BufferSize)
|
|
_historicoCorrente.Dequeue();
|
|
|
|
double correnteMedia = _historicoCorrente.Average();
|
|
|
|
if (TensaoInstantanea > VariaveisEquipamento.TensaoMinimaBateria && correnteMedia > 0)
|
|
{
|
|
TempoEstimadoMinutos = (CargaAtualAmp / correnteMedia) * 60;
|
|
}
|
|
else
|
|
{
|
|
TempoEstimadoMinutos = 0;
|
|
}
|
|
|
|
AtualizarAutonomia(distanciaTotalMetrosAtual);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Atualiza os cálculos de autonomia em função da distância total percorrida.
|
|
///
|
|
/// IMPORTANTE:
|
|
/// - distanciaTotalMetrosAtual deve ser o "total rodado" acumulado em metros (odometria / MPC / motores).
|
|
/// - Aqui dentro calculamos o delta de distância e o delta de energia desde a última chamada.
|
|
/// </summary>
|
|
/// <param name="distanciaTotalMetrosAtual">Distância total percorrida até o momento (em metros).</param>
|
|
private void AtualizarAutonomia(double distanciaTotalMetrosAtual)
|
|
{
|
|
if (!Iniciado)
|
|
return;
|
|
|
|
// Cálculo dos deltas
|
|
double deltaDist_m = distanciaTotalMetrosAtual - _distanciaTotalMetrosAnterior;
|
|
double deltaWh = EnergiaConsumida_Wh - _energiaConsumidaAnterior_Wh;
|
|
|
|
// Atualiza referências para próxima chamada
|
|
_distanciaTotalMetrosAnterior = distanciaTotalMetrosAtual;
|
|
_energiaConsumidaAnterior_Wh = EnergiaConsumida_Wh;
|
|
|
|
// ---------- 1) ATUALIZAÇÃO DA JANELA (SÓ SE ANDOU) ----------
|
|
if (deltaDist_m > 0 && deltaWh >= 0)
|
|
{
|
|
// Alimenta a janela móvel
|
|
_historicoDeltaDist_m.Enqueue(deltaDist_m);
|
|
_historicoDeltaWh.Enqueue(deltaWh);
|
|
|
|
_somaDistanciaJanela_m += deltaDist_m;
|
|
_somaEnergiaJanela_Wh += deltaWh;
|
|
|
|
// Mantém a janela com no máximo JanelaAutonomia_m metros
|
|
while (_somaDistanciaJanela_m > JanelaAutonomia_m && _historicoDeltaDist_m.Count > 0)
|
|
{
|
|
double dOld = _historicoDeltaDist_m.Dequeue();
|
|
double whOld = _historicoDeltaWh.Dequeue();
|
|
|
|
_somaDistanciaJanela_m -= dOld;
|
|
_somaEnergiaJanela_Wh -= whOld;
|
|
}
|
|
|
|
// Calcula Wh/m médio da janela
|
|
if (_somaDistanciaJanela_m > 0 && _somaEnergiaJanela_Wh > 0)
|
|
{
|
|
WhPorMetro = _somaEnergiaJanela_Wh / _somaDistanciaJanela_m;
|
|
}
|
|
else
|
|
{
|
|
WhPorMetro = 0;
|
|
}
|
|
|
|
if (WhPorMetro > 0)
|
|
{
|
|
// EMA simples
|
|
double alpha = 0.2; // suavização
|
|
if (WhPorMetroHistorico <= 0)
|
|
WhPorMetroHistorico = WhPorMetro;
|
|
else
|
|
WhPorMetroHistorico = WhPorMetroHistorico * (1 - alpha) + WhPorMetro * alpha;
|
|
}
|
|
}
|
|
// se deltaDist_m <= 0, só não atualiza a janela, mas continua
|
|
|
|
// ---------- 2) CÁLCULO DA AUTONOMIA (SEMPRE QUE POSSÍVEL) ----------
|
|
double consumoRef_WhPorMetro = WhPorMetroReferencia;
|
|
|
|
if (consumoRef_WhPorMetro > 0 && WhRestante > 0)
|
|
{
|
|
DistanciaMaximaTeorica_m = WhRestante / consumoRef_WhPorMetro;
|
|
|
|
double margem = FuncoesMatematicas.Clamp(MargemSegurancaAutonomia, 0.0, 0.9);
|
|
DistanciaMaximaSegura_m = DistanciaMaximaTeorica_m * (1.0 - margem);
|
|
if (DistanciaMaximaSegura_m < 0)
|
|
DistanciaMaximaSegura_m = 0;
|
|
}
|
|
else
|
|
{
|
|
DistanciaMaximaTeorica_m = 0;
|
|
DistanciaMaximaSegura_m = 0;
|
|
}
|
|
|
|
// Distância estimada restante = distância segura
|
|
DistanciaEstimadaRestante_m = DistanciaMaximaSegura_m;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Permite restaurar o valor consumido (em Ah) de um log salvo, por exemplo.
|
|
/// Atualiza também CargaAtual.
|
|
/// </summary>
|
|
public void AtualizarValorConsumido(double _consumo_Ah)
|
|
{
|
|
double coulombsConsumidos = _consumo_Ah * 3600;
|
|
Consumido = FuncoesMatematicas.Clamp(coulombsConsumidos, 0, CargaMaxima);
|
|
CargaAtual = CargaMaxima - Consumido;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Helper para o Manager/Health: indica se, com a situação atual,
|
|
/// o robô consegue percorrer uma certa distância em metros.
|
|
/// </summary>
|
|
public bool ConseguePercorrer(double distanciaRequerida_m)
|
|
{
|
|
if (distanciaRequerida_m <= 0)
|
|
return true;
|
|
|
|
return DistanciaMaximaSegura_m >= distanciaRequerida_m;
|
|
}
|
|
|
|
}
|
|
}
|