849 lines
33 KiB
C#
849 lines
33 KiB
C#
using AgroBase.Services;
|
|
using System;
|
|
using System.Collections.Generic;
|
|
using System.Linq;
|
|
using static AgroBase.Models.Enums;
|
|
using static AgroBase.Services.OIDCanService.OidHandler;
|
|
|
|
namespace AgroBase.Models.Modules
|
|
{
|
|
public class MovimentacaoModel
|
|
{
|
|
public string ID { get; set; } = T_Code.Mov.ToString();
|
|
|
|
// SETs
|
|
public byte _EnderecoCAN_Tx { get; set; }
|
|
public byte _EnderecoCAN_Rx { get; set; }
|
|
public int heartBeatValue { get; set; } = 0;
|
|
public string Mod_ID
|
|
{
|
|
get
|
|
{
|
|
return Variaveis.OperacaoEmAndamento.DispMvd?.Dados?.Modulos?.FirstOrDefault(x => x.MovMotor._EnderecoCAN_Tx == _EnderecoCAN_Tx)?.Modulo_ID ?? "";
|
|
}
|
|
}
|
|
public bool LeituraLigado
|
|
{
|
|
get
|
|
{
|
|
return Leitura?.CorrenteMotor?.valor != 0;
|
|
}
|
|
}
|
|
public double LeituraRPM { get; set; } = 0;
|
|
public bool ComandoLigado
|
|
{
|
|
get
|
|
{
|
|
return RPM_SP > 0;
|
|
}
|
|
}
|
|
//public bool Freio { get; set; } = false;
|
|
public OidCodigoErro CodigoAlarme
|
|
{
|
|
get
|
|
{
|
|
return Leitura?.CodigoAlarme?.valor ?? 0;
|
|
}
|
|
}
|
|
public bool AlarmeMotor
|
|
{
|
|
get
|
|
{
|
|
return CodigoAlarme != OidCodigoErro.NenhumErro;
|
|
}
|
|
}
|
|
public int RPM_SP { get; set; } = 0;
|
|
public int RPM_SP_Controle { get; set; } = 0;
|
|
public int RPM_SP_Leitura
|
|
{
|
|
get
|
|
{
|
|
return RPM_SP / Convert.ToInt32(Variaveis.OperacaoEmAndamento.DispMvd?.Dados?.ReducaoMotorBLDC ?? 1);
|
|
}
|
|
}
|
|
public Sentido Sentido_SP { get; set; } = Sentido.Parado;
|
|
public int NumeroPolos { get; set; } = 10;
|
|
public int Rampa { get; set; } = 0;
|
|
public int CorrenteTorqueMaximo { get; set; } = 40;
|
|
public double OffsetLeitura { get; set; } = 1.0;
|
|
private DateTime UltimoAlarmeComando { get; set; } = DateTime.MinValue;
|
|
public bool AlarmeComando
|
|
{
|
|
get
|
|
{
|
|
bool _alarme = ComandoLigado && !LeituraLigado;
|
|
|
|
if (_alarme)
|
|
{
|
|
if (UltimoAlarmeComando == DateTime.MinValue)
|
|
{
|
|
UltimoAlarmeComando = DateTime.Now;
|
|
}
|
|
|
|
if (UltimoAlarmeComando.AddSeconds(5) < DateTime.Now)
|
|
{
|
|
return true;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
UltimoAlarmeComando = DateTime.MinValue;
|
|
}
|
|
|
|
return false;
|
|
}
|
|
}
|
|
|
|
|
|
// GETs
|
|
public double RPM_Roda
|
|
{
|
|
get
|
|
{
|
|
//return (Leitura?.RPM ?? 0) * OffsetLeitura;
|
|
return RPM_Motor / VariaveisEquipamento.RelacaoRPM;
|
|
}
|
|
}
|
|
public Sentido Sentido
|
|
{
|
|
get
|
|
{
|
|
return Leitura?.Direcao ?? Sentido.Parado;
|
|
}
|
|
}
|
|
public double Temperatura { get; set; } = 0;
|
|
public double TemperaturaDriver
|
|
{
|
|
get
|
|
{
|
|
return Math.Round(Leitura?.Temperatura?.valor ?? 0, 2);
|
|
}
|
|
}
|
|
public double Tensao
|
|
{
|
|
get
|
|
{
|
|
return Math.Round(Leitura?.Tensao?.valor ?? 0, 2);
|
|
}
|
|
}
|
|
public double Corrente_Barramento
|
|
{
|
|
get
|
|
{
|
|
return Math.Round(Leitura?.CorrenteBarramento?.valor ?? 0, 2);
|
|
}
|
|
}
|
|
public double Corrente_Motor
|
|
{
|
|
get
|
|
{
|
|
return Math.Round(Leitura?.CorrenteMotor?.valor ?? 0, 2);
|
|
}
|
|
}
|
|
public double Potencia
|
|
{
|
|
get
|
|
{
|
|
return Math.Round(Leitura?.Potencia?.valor ?? 0, 2);
|
|
}
|
|
}
|
|
public double CicloTrabalho
|
|
{
|
|
get
|
|
{
|
|
return Math.Round(Leitura?.CicloTrabalho?.valor ?? 0, 2);
|
|
}
|
|
}
|
|
public double RPM_Motor
|
|
{
|
|
get
|
|
{
|
|
//return Convert.ToInt32(RPM_Motor / VariaveisEquipamento.RelacaoRPM);
|
|
//return RPM_Roda * VariaveisEquipamento.RelacaoRPM;
|
|
//return RPM_Roda * 0.74;
|
|
|
|
return (Leitura?.RPM ?? 0);
|
|
}
|
|
}
|
|
public double VelocidadeInstantanea
|
|
{
|
|
get
|
|
{
|
|
return FuncoesMatematicas.CalculaVelocidadeRPM(RPM_Roda);
|
|
}
|
|
}
|
|
|
|
public SensorModel SensorTemperaturaMotor
|
|
{
|
|
get
|
|
{
|
|
var sNTC = Variaveis.OperacaoEmAndamento.DispSen?.Dados?.Sensores?.FirstOrDefault(x => x.Componente == S_Code.sNTC && x.ID.Contains(Mod_ID));
|
|
return sNTC;
|
|
}
|
|
}
|
|
|
|
public bool Comandar { get; set; } = false;
|
|
public bool AlarmeSensores { get; set; } = false;
|
|
public bool Inicializado
|
|
{
|
|
get
|
|
{
|
|
return Leitura?.Iniciado ?? false;
|
|
}
|
|
}
|
|
private OIDModel _leitura = null;
|
|
public OIDModel Leitura
|
|
{
|
|
get
|
|
{
|
|
_leitura = OIDCanService.DadosLeitura?.FirstOrDefault(x => x.EnderecoTx == _EnderecoCAN_Tx);
|
|
return _leitura;
|
|
}
|
|
set
|
|
{
|
|
_leitura = value;
|
|
}
|
|
}
|
|
|
|
public bool CompensacaoNecessaria
|
|
{
|
|
get
|
|
{
|
|
if (ComandoLigado)
|
|
{
|
|
var _Controle = Variaveis.OperacaoEmAndamento.Controle;
|
|
var _ControleDir = _Controle.TiposControle.FirstOrDefault(x => x.Tipo == T_Code.Dir);
|
|
|
|
bool movimentoCompensado =
|
|
(_Controle.TipoMovimento == TipoMovimentoDirecional.MovimentoArco) ||
|
|
(_Controle.TipoMovimento == TipoMovimentoDirecional.RodasDianteiras && Mod_ID.Contains("F")) ||
|
|
(_Controle.TipoMovimento == TipoMovimentoDirecional.RodasTraseiras && Mod_ID.Contains("T"));
|
|
bool emCurva = _ControleDir.UltimaDirecao != Direcao.Parado;
|
|
double anguloDir = Math.Abs(Variaveis.OperacaoEmAndamento.DispMvd?.Dados?.Modulos?.FirstOrDefault(x => x.Modulo_ID == Mod_ID)?.DirMotor?.Angulo_SP ?? 0);
|
|
bool direcionalAtuado = anguloDir > 0.0;
|
|
|
|
if (movimentoCompensado && emCurva && direcionalAtuado)
|
|
{
|
|
if (
|
|
(_ControleDir.UltimaDirecao == Direcao.Esquerda && Mod_ID.Contains("D")) ||
|
|
(_ControleDir.UltimaDirecao == Direcao.Direita && Mod_ID.Contains("E"))
|
|
)
|
|
{
|
|
return true;
|
|
}
|
|
}
|
|
}
|
|
|
|
return false;
|
|
}
|
|
}
|
|
|
|
private float FatorCompensacaoCurvas
|
|
{
|
|
get
|
|
{
|
|
if (CompensacaoNecessaria)
|
|
{
|
|
//double compensacao = 1 + FuncoesMatematicas.Map(Math.Abs(Variaveis.OperacaoEmAndamento.Controle.Angulo), 0.0, Variaveis.OperacaoEmAndamento.Controle.Angulo_Max, 0.0, 0.3);
|
|
double compensacao = MovimentacaoMovimentoCompensadoModel.compensacoes[Mod_ID];
|
|
return (float)compensacao;
|
|
}
|
|
else
|
|
{
|
|
return 1;
|
|
}
|
|
}
|
|
}
|
|
private float FatorCompensacaoCurvasAnterior { get; set; } = 1.0f;
|
|
public bool NovoRPMNecessario
|
|
{
|
|
get
|
|
{
|
|
return FatorCompensacaoCurvas != FatorCompensacaoCurvasAnterior;
|
|
}
|
|
}
|
|
|
|
public Dictionary<TipoMovimentoDirecional, ConfiguracaoSentidoMotor> ConfigSentidos { get; set; }
|
|
public List<FuncoesPinout> Funcoes { get; set; }
|
|
|
|
public List<MvdMotorMOVSensorimanetoGrafico> LogGrafico { get; set; } = new List<MvdMotorMOVSensorimanetoGrafico>();
|
|
public List<AlarmeModel> Alarmes { get; set; } = new List<AlarmeModel>();
|
|
|
|
public MvdServoFreioModel ConfigFreio { get; set; }
|
|
|
|
|
|
public static MovimentacaoModel CarregarParametrosIniciais(string Mod_ID, byte _EnderecoTx, byte _EnderecoRx)
|
|
{
|
|
return new MovimentacaoModel()
|
|
{
|
|
ID = T_Code.Mov.ToString(),
|
|
_EnderecoCAN_Tx = _EnderecoTx,
|
|
_EnderecoCAN_Rx = _EnderecoRx,
|
|
Funcoes = new List<FuncoesPinout>()
|
|
{
|
|
FuncoesPinout.TMP,
|
|
FuncoesPinout.ENA,
|
|
FuncoesPinout.DIR,
|
|
FuncoesPinout.PWM,
|
|
FuncoesPinout.BRK,
|
|
},
|
|
ConfigFreio = new MvdServoFreioModel(Mod_ID)
|
|
{
|
|
AnguloInicial = Mod_ID.Contains("E") ? 10 : 170,
|
|
Incremental = Mod_ID.Contains("E"),
|
|
Leitura = false,
|
|
},
|
|
ConfigSentidos = new Dictionary<TipoMovimentoDirecional, ConfiguracaoSentidoMotor>()
|
|
{
|
|
{
|
|
TipoMovimentoDirecional.RodasDianteiras,
|
|
new ConfiguracaoSentidoMotor()
|
|
{
|
|
Frente = Mod_ID.Contains("E") ? Sentido.Horario : Sentido.Antihorario,
|
|
Tras = Mod_ID.Contains("E") ? Sentido.Antihorario : Sentido.Horario,
|
|
Direita = Sentido.Parado,
|
|
Esquerda = Sentido.Parado,
|
|
Parado = Sentido.Parado,
|
|
}
|
|
},
|
|
{
|
|
TipoMovimentoDirecional.RodasTraseiras,
|
|
new ConfiguracaoSentidoMotor()
|
|
{
|
|
Frente = Mod_ID.Contains("E") ? Sentido.Horario : Sentido.Antihorario,
|
|
Tras = Mod_ID.Contains("E") ? Sentido.Antihorario : Sentido.Horario,
|
|
Direita = Sentido.Parado,
|
|
Esquerda = Sentido.Parado,
|
|
Parado = Sentido.Parado,
|
|
}
|
|
},
|
|
{
|
|
TipoMovimentoDirecional.MovimentoDiagonal,
|
|
new ConfiguracaoSentidoMotor()
|
|
{
|
|
Frente = Mod_ID.Contains("E") ? Sentido.Horario : Sentido.Antihorario,
|
|
Tras = Mod_ID.Contains("E") ? Sentido.Antihorario : Sentido.Horario,
|
|
Direita = Sentido.Parado,
|
|
Esquerda = Sentido.Parado,
|
|
Parado = Sentido.Parado,
|
|
}
|
|
},
|
|
{
|
|
TipoMovimentoDirecional.MovimentoArco,
|
|
new ConfiguracaoSentidoMotor()
|
|
{
|
|
Frente = Mod_ID.Contains("E") ? Sentido.Horario : Sentido.Antihorario,
|
|
Tras = Mod_ID.Contains("E") ? Sentido.Antihorario : Sentido.Horario,
|
|
Direita = Sentido.Parado,
|
|
Esquerda = Sentido.Parado,
|
|
Parado = Sentido.Parado,
|
|
}
|
|
},
|
|
{
|
|
TipoMovimentoDirecional.RotacionarNoEixo,
|
|
new ConfiguracaoSentidoMotor()
|
|
{
|
|
Frente = Sentido.Horario,
|
|
Tras = Sentido.Antihorario,
|
|
Direita = Sentido.Parado,
|
|
Esquerda = Sentido.Parado,
|
|
Parado = Sentido.Parado,
|
|
}
|
|
},
|
|
{
|
|
TipoMovimentoDirecional.MovimentoLateral,
|
|
new ConfiguracaoSentidoMotor()
|
|
{
|
|
Frente = Mod_ID.Contains("T") ? Sentido.Horario : Sentido.Antihorario,
|
|
Tras = Mod_ID.Contains("T") ? Sentido.Antihorario : Sentido.Horario,
|
|
Direita = Sentido.Parado,
|
|
Esquerda = Sentido.Parado,
|
|
Parado = Sentido.Parado,
|
|
}
|
|
},
|
|
{
|
|
TipoMovimentoDirecional.Diagnostico,
|
|
new ConfiguracaoSentidoMotor()
|
|
{
|
|
Frente = Mod_ID.Contains("E") ? Sentido.Horario : Sentido.Antihorario,
|
|
Tras = Mod_ID.Contains("E") ? Sentido.Antihorario : Sentido.Horario,
|
|
Direita = Sentido.Parado,
|
|
Esquerda = Sentido.Parado,
|
|
Parado = Sentido.Parado,
|
|
}
|
|
},
|
|
},
|
|
};
|
|
}
|
|
|
|
|
|
public void EnviarComandoControle(Direcao DirecaoAtual)
|
|
{
|
|
var simulando = Variaveis.OperacaoEmAndamento.Simulando || Variaveis.OperacaoEmAndamento.Treinando;
|
|
|
|
bool Freado = DirecaoAtual == Direcao.EmFreio;
|
|
|
|
if ((Comandar && Inicializado) || simulando)
|
|
{
|
|
var _Controle = Variaveis.OperacaoEmAndamento.Controle;
|
|
|
|
if (AlarmeSensores || AlarmeMotor)
|
|
{
|
|
DirecaoAtual = Direcao.Parado;
|
|
Freado = false;
|
|
}
|
|
|
|
RPM_SP_Controle = _Controle.RPM_Motor;
|
|
RPM_SP =
|
|
DirecaoAtual == Direcao.Parado || Freado ? 0 :
|
|
RPM_SP_Controle;
|
|
|
|
int RpmCorrigido = Math.Min((int)(VariaveisEquipamento.RPM_Max_Roda * VariaveisEquipamento.RelacaoRPM), Math.Max(RPM_SP != 0 ? (int)(VariaveisEquipamento.RPM_Min_Roda * VariaveisEquipamento.RelacaoRPM) : 0, Convert.ToInt32(RPM_SP * FatorCompensacaoCurvas)));
|
|
//Console.WriteLine($"[{Mod_ID}] Direcao: {DirecaoAtual}, RPM_SP: {RPM_SP / VariaveisEquipamento.RelacaoRPM}, FC: {FatorCompensacaoCurvas.ToString("0.00")}, Novo RPM: {RpmCorrigido / VariaveisEquipamento.RelacaoRPM}");
|
|
RPM_SP = RpmCorrigido;
|
|
FatorCompensacaoCurvasAnterior = FatorCompensacaoCurvas;
|
|
|
|
Variaveis.OperacaoEmAndamento.SimulacaoRpmControle = Variaveis.OperacaoEmAndamento.Controle.PercentualVelocidadeSP;
|
|
//Console.WriteLine($"RPM Atualizado para {Variaveis.OperacaoEmAndamento.SimulacaoRpmControle}");
|
|
|
|
switch (DirecaoAtual)
|
|
{
|
|
case Direcao.Frente:
|
|
Sentido_SP = ConfigSentidos[_Controle.TipoMovimento].Frente;
|
|
break;
|
|
case Direcao.Tras:
|
|
Sentido_SP = ConfigSentidos[_Controle.TipoMovimento].Tras;
|
|
break;
|
|
default:
|
|
Sentido_SP = Sentido.Parado;
|
|
break;
|
|
}
|
|
|
|
if (simulando)
|
|
{
|
|
return;
|
|
}
|
|
|
|
bool PrimeiraLeitura = Leitura == null;
|
|
bool SentidoAlterado = (Leitura?.Direcao ?? Sentido.Parado) != Sentido_SP;
|
|
bool RPMAlterado = LeituraRPM != RPM_SP;
|
|
bool LigadoAlterado = LeituraLigado != ComandoLigado;
|
|
bool FreioAlterado = (Leitura?.Freado ?? false) != Freado;
|
|
bool EnviaComando = PrimeiraLeitura || SentidoAlterado || RPMAlterado || LigadoAlterado || FreioAlterado;
|
|
|
|
OidFuncCode modoControle =
|
|
Freado ? OidFuncCode.ComandoCorrenteFreio :
|
|
ComandoLigado ? OidFuncCode.ComandoVelocidade :
|
|
OidFuncCode.ComandoCorrente;
|
|
|
|
if (EnviaComando) // && !Freado
|
|
{
|
|
int mx =
|
|
Sentido_SP == Sentido.Horario ? 1 :
|
|
Sentido_SP == Sentido.Antihorario ? -1 :
|
|
0;
|
|
float velocidade = RPM_SP * mx;
|
|
//velocidade = (Mod_ID.Contains("T") ? velocidade : velocidade * 0.7f);
|
|
|
|
OIDCanService.ComandoControle(_EnderecoCAN_Tx, _EnderecoCAN_Rx, modoControle, velocidade);
|
|
|
|
LeituraRPM = RPM_SP;
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
|
|
public void RequisitarDadosSensoresExternos()
|
|
{
|
|
if (SensorTemperaturaMotor != null)
|
|
{
|
|
Variaveis.OperacaoEmAndamento.DispSen.Dados.RequisitarDadosModulo(SensorTemperaturaMotor.ID_Num, CanMessagePosicaoDados.Dados1, CanMessagePosicaoDados.Dados1, true);
|
|
}
|
|
}
|
|
|
|
public void AtualizarDadosSensoresExternos()
|
|
{
|
|
if (SensorTemperaturaMotor != null)
|
|
{
|
|
Temperatura = SensorTemperaturaMotor.ValoresLeituras.FirstOrDefault(x => x.funcao == FuncoesPinout.Temperatura)?.atual?.valor ?? double.NaN;
|
|
}
|
|
}
|
|
|
|
|
|
|
|
}
|
|
|
|
public class MvdServoFreioModel
|
|
{
|
|
public MvdServoFreioModel(string mod_id) { Mod_ID = mod_id; }
|
|
|
|
public string Mod_ID { get; set; }
|
|
|
|
// ---- Tunáveis ----
|
|
private const int TolAng = 2; // tolerância angular (graus)
|
|
private const int TolVelIn = 2; // histerese interna de velocidade (%)
|
|
private const int TolVelOut = 4; // histerese externa de velocidade (%)
|
|
private const int FatorEscala = 10; // proporcional p/ erro de velocidade
|
|
private const int AnguloMin = 0;
|
|
private const int AnguloMax = 180;
|
|
private const int AnguloControleMaxDefault = 20;
|
|
|
|
// ---- Estado ----
|
|
public bool Incremental { get; set; }
|
|
public int AnguloInicial { get; set; }
|
|
public bool Leitura { get; set; }
|
|
public int AnguloControle { get; set; } = -1;
|
|
private int AnguloControleMax { get; set; } = AnguloControleMaxDefault;
|
|
|
|
private int VelocidadePercentual_SP { get; set; }
|
|
private bool VelOkMemo { get; set; } = true; // memorização p/ histerese
|
|
private bool UltimoControle { get; set; } = false; // último EmFreio?
|
|
public DateTime UltimoEnvio { get; set; }
|
|
public int DelayEnvio { get; set; } = 500;
|
|
|
|
public SensorServoModel Servo
|
|
{
|
|
get
|
|
{
|
|
return Variaveis.OperacaoEmAndamento.DispSen?.Dados?.Servos?
|
|
.FirstOrDefault(x => x.ID.Contains(Mod_ID));
|
|
}
|
|
}
|
|
|
|
private double VelocidadeAtual =>
|
|
FuncoesMatematicas.CalculaVelocidadePercentualMs(Variaveis.OperacaoEmAndamento.Parametros.DadosLeitura?.Movimentacao?.VelocidadeMedia ?? 0);
|
|
|
|
private bool StatusControleEmFreio =>
|
|
(Variaveis.OperacaoEmAndamento.Controle?.TiposControle?
|
|
.FirstOrDefault(x => x.Tipo == T_Code.Mov)?.UltimaDirecao
|
|
?? Direcao.Parado) == Direcao.EmFreio;
|
|
|
|
// leitura atual do ângulo (ou null)
|
|
private int AnguloLido
|
|
{
|
|
get
|
|
{
|
|
var v = Servo?.ValoresLeituras?
|
|
.FirstOrDefault(x => x.funcao == FuncoesPinout.ServoAnguloLeitura)?
|
|
.atual?.valor ?? 0;
|
|
return (int)v;
|
|
}
|
|
}
|
|
|
|
// histerese de velocidade
|
|
private bool VelocidadeOkHisterese(int sp, double atual)
|
|
{
|
|
var erro = Math.Abs(sp - atual);
|
|
if (VelOkMemo)
|
|
{
|
|
// estava OK → só sai se passar da banda externa
|
|
if (erro > TolVelOut) VelOkMemo = false;
|
|
}
|
|
else
|
|
{
|
|
// estava NOK → só entra se ficar dentro da banda interna
|
|
if (erro <= TolVelIn) VelOkMemo = true;
|
|
}
|
|
return VelOkMemo;
|
|
}
|
|
|
|
// condição “atingiu SP” com tolerância angular
|
|
private bool AtingiuSPComTol(int? lido, int alvo)
|
|
{
|
|
if (!lido.HasValue) return false; // sem leitura → considerar não atingido
|
|
return Math.Abs(lido.Value - alvo) <= TolAng;
|
|
}
|
|
|
|
public bool EnviarComando { get; private set; } = false;
|
|
|
|
public void AtualizarDados(int velocidade_sp = 0, int anguloInicial = -1, int anguloSP = -1)
|
|
{
|
|
VelocidadePercentual_SP = velocidade_sp;
|
|
|
|
if (!(Servo?.Controlar ?? false))
|
|
{
|
|
EnviarComando = false;
|
|
return;
|
|
}
|
|
|
|
bool emFreio = StatusControleEmFreio;
|
|
bool transicao = (UltimoControle != emFreio);
|
|
|
|
if (anguloInicial == -1) anguloInicial = AnguloInicial;
|
|
if (AnguloControle == -1) AnguloControle = Incremental ? AnguloMin : AnguloMax;
|
|
|
|
// ---- Calcula alvo de ângulo ----
|
|
int alvo;
|
|
if (!emFreio)
|
|
{
|
|
// Soltar freio: vai para o mínimo (liberado) conforme o sentido do seu hardware.
|
|
alvo = Incremental ? AnguloMin : AnguloMax;
|
|
}
|
|
else
|
|
{
|
|
// Em freio: definir alvo em torno do AnguloInicial +/- janela
|
|
int limiteLo, limiteHi;
|
|
if (Incremental)
|
|
{
|
|
limiteLo = AnguloMin;
|
|
limiteHi = Clamp(anguloInicial + AnguloControleMax, AnguloMin, AnguloMax);
|
|
}
|
|
else
|
|
{
|
|
limiteLo = Clamp(anguloInicial - AnguloControleMax, AnguloMin, AnguloMax);
|
|
limiteHi = AnguloMax;
|
|
}
|
|
|
|
if (anguloSP != -1)
|
|
{
|
|
// alvo explícito
|
|
alvo = Clamp(anguloSP, limiteLo, limiteHi);
|
|
}
|
|
else
|
|
{
|
|
// proporcional ao erro de velocidade (só para DEFINIR alvo, não para “reenviar”)
|
|
int erro = VelocidadePercentual_SP - (int)VelocidadeAtual; // pode ser + ou -
|
|
int ajuste = erro / FatorEscala;
|
|
ajuste = AnguloControleMax;
|
|
ajuste = Math.Abs(ajuste);
|
|
|
|
// direção do ajuste
|
|
if (Incremental)
|
|
{
|
|
alvo = Clamp(AnguloControle + ajuste, limiteLo, limiteHi);
|
|
}
|
|
else
|
|
{
|
|
alvo = Clamp(AnguloControle - ajuste, limiteLo, limiteHi);
|
|
}
|
|
}
|
|
}
|
|
|
|
// aplica alvo
|
|
int anguloAntigo = AnguloControle;
|
|
AnguloControle = alvo;
|
|
|
|
// ---- Decidir se deve enviar ----
|
|
// Regra: envia quando (a) houve transição de modo, (b) alvo mudou significativamente,
|
|
// ou (c) não atingiu o alvo atual (com tolerância).
|
|
bool alvoMudou = Math.Abs(AnguloControle - anguloAntigo) > 0;
|
|
bool atingiu = AtingiuSPComTol(AnguloLido, AnguloControle);
|
|
|
|
// histerese de velocidade apenas para DEFINIR o alvo (já considerado acima).
|
|
// Não use VelocidadeOk para forçar reenvio — isso causa “chatters” de comando.
|
|
bool vel_ok = VelocidadeOkHisterese(VelocidadePercentual_SP, VelocidadeAtual); // atualiza memo, se quiser expor depois
|
|
|
|
EnviarComando = transicao || alvoMudou || !atingiu;
|
|
|
|
UltimoControle = emFreio;
|
|
}
|
|
|
|
private static int Clamp(int v, int lo, int hi) => (v < lo) ? lo : (v > hi ? hi : v);
|
|
}
|
|
|
|
public class MvdMotorMOVSensorimanetoGrafico
|
|
{
|
|
public DateTime Momento { get; set; }
|
|
public string Modulo_ID { get; set; }
|
|
public int RPM_SP { get; set; }
|
|
public double RPM_Motor { get; set; }
|
|
public double RPM_Roda { get; set; }
|
|
public double Temperatura { get; set; }
|
|
public double TemperaturaDriver { get; set; }
|
|
public Sentido Sentido { get; set; }
|
|
public double Velocidade { get; set; }
|
|
public double Tensao { get; set; }
|
|
public double Corrente { get; set; }
|
|
public double Potencia { get; set; }
|
|
public double CicloTrabalho { get; set; }
|
|
}
|
|
|
|
public class MovimentacaoMovimentoCompensadoModel
|
|
{
|
|
static List<TipoMovimentoDirecional> MovimentosCompensar = new List<TipoMovimentoDirecional>() { TipoMovimentoDirecional.RodasDianteiras, TipoMovimentoDirecional.RodasTraseiras, TipoMovimentoDirecional.MovimentoArco };
|
|
static GeometriaRobo g = new GeometriaRobo { L = VariaveisEquipamento.DistanciaEntreEixos / 100.0, W = VariaveisEquipamento.LarguraEquipamentoMm / 1000.0 };
|
|
public static Dictionary<string, double> compensacoes { get; set; } = new Dictionary<string, double>();
|
|
public static Dictionary<string, double> _compAnt { get; set; }
|
|
|
|
public static void AtualizarDados()
|
|
{
|
|
var ctrl = Variaveis.OperacaoEmAndamento?.Controle;
|
|
var disp = Variaveis.OperacaoEmAndamento?.DispMvd?.Dados?.Modulos;
|
|
if (Variaveis.OperacaoEmAndamento.StatusAtual != StatusOperacao.EmAndamento || ctrl == null || disp == null || !MovimentosCompensar.Contains(ctrl.TipoMovimento))
|
|
{
|
|
compensacoes = (disp ?? Enumerable.Empty<ModuloMvdModel>()).ToDictionary(x => x.Modulo_ID, x => 1.0);
|
|
return;
|
|
}
|
|
|
|
// Coleta ângulos dos módulos
|
|
var frente = disp.Where(m => m.Modulo_ID?.Contains("F") == true).Select(m => (double)(m.DirMotor?.Angulo_SP ?? 0) * (m.DirMotor.Sentido_SP == Sentido.Antihorario ? -1 : 1));
|
|
var tras = disp.Where(m => m.Modulo_ID?.Contains("T") == true).Select(m => (double)(m.DirMotor?.Angulo_SP ?? 0) * (m.DirMotor.Sentido_SP == Sentido.Antihorario ? 1 : -1));
|
|
|
|
// Ângulo equivalente do eixo (média em tan-space)
|
|
double angFrenteDeg = EqAngleEixo(frente);
|
|
double angTrasDeg = EqAngleEixo(tras);
|
|
|
|
compensacoes = FatoresVelocidade(
|
|
ctrl.TipoMovimento,
|
|
angFrenteDeg,
|
|
angTrasDeg,
|
|
g,
|
|
PoliticaFator.ReferenciaNoCentro,
|
|
fatorMaximo: 1.6
|
|
);
|
|
|
|
const double alpha = 0.2;
|
|
if (_compAnt != null && _compAnt.Count == compensacoes.Count)
|
|
compensacoes = compensacoes.ToDictionary(kv => kv.Key,
|
|
kv => _compAnt.TryGetValue(kv.Key, out var prev) ? prev * (1 - alpha) + kv.Value * alpha : kv.Value);
|
|
_compAnt = new Dictionary<string, double>(compensacoes);
|
|
}
|
|
|
|
public struct GeometriaRobo
|
|
{
|
|
public double L; // entre-eixos (m)
|
|
public double W; // bitola (m)
|
|
}
|
|
|
|
// assuma ângulos em graus no seu sistema
|
|
static double Deg2Rad(double deg) => deg * Math.PI / 180.0;
|
|
|
|
static double EqAngleEixo(IEnumerable<double> angsDeg)
|
|
{
|
|
// Trata sequência vazia
|
|
if (angsDeg == null) return 0;
|
|
var list = angsDeg.ToList();
|
|
if (list.Count == 0) return 0;
|
|
|
|
// média de tan e volta pra graus
|
|
double mediaTan = list.Select(a => Math.Tan(a * Math.PI / 180.0)).Average();
|
|
return Math.Atan(mediaTan) * 180.0 / Math.PI;
|
|
}
|
|
|
|
static bool TryCalcularRaioCentro(TipoMovimentoDirecional modo, double deltaFrontDeg, double deltaRearDeg, GeometriaRobo g, out double Rc)
|
|
{
|
|
double tf = Math.Tan(Deg2Rad(deltaFrontDeg));
|
|
double tr = Math.Tan(Deg2Rad(deltaRearDeg));
|
|
|
|
Rc = double.PositiveInfinity;
|
|
|
|
switch (modo)
|
|
{
|
|
case TipoMovimentoDirecional.RodasDianteiras:
|
|
if (Math.Abs(tf) < 1e-6) return false; // reta
|
|
Rc = g.L / tf;
|
|
return true;
|
|
|
|
case TipoMovimentoDirecional.RodasTraseiras:
|
|
if (Math.Abs(tr) < 1e-6) return false;
|
|
Rc = g.L / tr;
|
|
return true;
|
|
|
|
case TipoMovimentoDirecional.MovimentoArco:
|
|
// uso geral: Rc = L / (tan(df) - tan(dr))
|
|
double denom = (tf - tr);
|
|
if (Math.Abs(denom) < 1e-6) return false;
|
|
Rc = g.L / denom;
|
|
return true;
|
|
|
|
default:
|
|
return false;
|
|
}
|
|
}
|
|
|
|
// Raio de trajetória por roda para curva à esquerda (Rc > 0) ou direita (Rc < 0)
|
|
// Posições das rodas em relação ao centro do chassi: x = ±L/2, y = ±W/2
|
|
static void CalcularRaiosPorRoda(TipoMovimentoDirecional modo, double Rc, GeometriaRobo g, out double rFL, out double rFR, out double rRL, out double rRR)
|
|
{
|
|
// Por simplicidade, tratamos três casos:
|
|
// 1) Dianteiras: ICC está sobre a linha do eixo traseiro
|
|
// 2) Traseiras: ICC está sobre a linha do eixo dianteiro
|
|
// 3) ArcoOposto (simétrico): ICC está próximo ao centro; distância axial = L/2 para ambos
|
|
double ay = Math.Abs(Rc); // magnitude do Rc; sinal define esquerda/direita
|
|
|
|
if (modo == TipoMovimentoDirecional.RodasDianteiras)
|
|
{
|
|
// Traseiro: raio = Rc ± W/2
|
|
rRL = Math.Max(1e-6, ay - g.W / 2.0);
|
|
rRR = Math.Max(1e-6, ay + g.W / 2.0);
|
|
// Dianteiro: distância diagonal até ICC (pitágoras)
|
|
rFL = Math.Sqrt(Math.Pow(ay - g.W / 2.0, 2) + Math.Pow(g.L, 2));
|
|
rFR = Math.Sqrt(Math.Pow(ay + g.W / 2.0, 2) + Math.Pow(g.L, 2));
|
|
}
|
|
else if (modo == TipoMovimentoDirecional.RodasTraseiras)
|
|
{
|
|
// Dianteiro: raio = Rc ± W/2
|
|
rFL = Math.Max(1e-6, ay - g.W / 2.0);
|
|
rFR = Math.Max(1e-6, ay + g.W / 2.0);
|
|
// Traseiro: distância diagonal
|
|
rRL = Math.Sqrt(Math.Pow(ay - g.W / 2.0, 2) + Math.Pow(g.L, 2));
|
|
rRR = Math.Sqrt(Math.Pow(ay + g.W / 2.0, 2) + Math.Pow(g.L, 2));
|
|
}
|
|
else // Arco (simétrico)
|
|
{
|
|
// Ambos eixos a L/2 do centro
|
|
double a = g.L / 2.0;
|
|
rFL = Math.Sqrt(Math.Pow(ay - g.W / 2.0, 2) + a * a);
|
|
rFR = Math.Sqrt(Math.Pow(ay + g.W / 2.0, 2) + a * a);
|
|
rRL = Math.Sqrt(Math.Pow(ay - g.W / 2.0, 2) + a * a);
|
|
rRR = Math.Sqrt(Math.Pow(ay + g.W / 2.0, 2) + a * a);
|
|
}
|
|
|
|
// Observação: sinal de Rc indica sentido da curva:
|
|
// Rc > 0 -> curva à esquerda (rodas esquerdas são internas)
|
|
// Rc < 0 -> curva à direita (troque "interna"↔"externa")
|
|
// Aqui usamos |Rc|; para decidir quem é interna/externa, use o sinal fora daqui.
|
|
}
|
|
|
|
// Retorna fatores por roda. Modo "SomenteAumentarExterna" mantém a interna em 1.0
|
|
public enum PoliticaFator { ReferenciaNoCentro, SomenteAumentarExterna }
|
|
|
|
public static Dictionary<string, double> FatoresVelocidade(TipoMovimentoDirecional modo, double deltaFrontDeg, double deltaRearDeg, GeometriaRobo g, PoliticaFator politica, double fatorMaximo = 1.5)
|
|
{
|
|
if (!TryCalcularRaioCentro(modo, deltaFrontDeg, deltaRearDeg, g, out double Rc))
|
|
return Variaveis.OperacaoEmAndamento.DispMvd.Dados.Modulos.ToDictionary(x => x.Modulo_ID, x => 1.0);
|
|
|
|
CalcularRaiosPorRoda(modo, Rc, g, out var rFL, out var rFR, out var rRL, out var rRR);
|
|
|
|
// Centro do robô percorre raio |Rc|
|
|
double RcAbs = Math.Abs(Rc);
|
|
|
|
// Descobrir lado interno pela curva (sinal de Rc)
|
|
bool esquerda = Variaveis.OperacaoEmAndamento.Controle.TiposControle.FirstOrDefault(x => x.Tipo == T_Code.Dir).UltimaDirecao == Direcao.Esquerda; // Rc > 0;
|
|
|
|
double rIntFront = esquerda ? rFL : rFR;
|
|
double rExtFront = esquerda ? rFR : rFL;
|
|
double rIntRear = esquerda ? rRL : rRR;
|
|
double rExtRear = esquerda ? rRR : rRL;
|
|
|
|
double fFL, fFR, fRL, fRR;
|
|
|
|
if (politica == PoliticaFator.ReferenciaNoCentro)
|
|
{
|
|
fFL = rIntFront / RcAbs;
|
|
fFR = rExtFront / RcAbs;
|
|
fRL = rIntRear / RcAbs;
|
|
fRR = rExtRear / RcAbs;
|
|
}
|
|
else // SomenteAumentarExterna
|
|
{
|
|
fFL = 1.0;
|
|
fFR = rExtFront / rIntFront;
|
|
fRL = 1.0;
|
|
fRR = rExtRear / rIntRear;
|
|
}
|
|
|
|
// Saturação e leve suavização (ex: EMA) podem ser aplicadas aqui
|
|
fFL = Math.Min(fatorMaximo, fFL);
|
|
fFR = Math.Min(fatorMaximo, fFR);
|
|
fRL = Math.Min(fatorMaximo, fRL);
|
|
fRR = Math.Min(fatorMaximo, fRR);
|
|
|
|
return Variaveis.OperacaoEmAndamento.DispMvd.Dados.Modulos.ToDictionary(x => x.Modulo_ID, x => (double)(x.Modulo_ID == "ET" ? fRL : x.Modulo_ID == "DT" ? fRR : x.Modulo_ID == "EF" ? fFL : x.Modulo_ID == "DF" ? fFR : 1.0));
|
|
}
|
|
|
|
}
|
|
|
|
}
|